1204591Sluigi/* 2204591Sluigi * Copyright (c) 2010 Fabio Checconi, Luigi Rizzo, Paolo Valente 3204591Sluigi * All rights reserved 4204591Sluigi * 5204591Sluigi * Redistribution and use in source and binary forms, with or without 6204591Sluigi * modification, are permitted provided that the following conditions 7204591Sluigi * are met: 8204591Sluigi * 1. Redistributions of source code must retain the above copyright 9204591Sluigi * notice, this list of conditions and the following disclaimer. 10204591Sluigi * 2. Redistributions in binary form must reproduce the above copyright 11204591Sluigi * notice, this list of conditions and the following disclaimer in the 12204591Sluigi * documentation and/or other materials provided with the distribution. 13204591Sluigi * 14204591Sluigi * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND 15204591Sluigi * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 16204591Sluigi * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 17204591Sluigi * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE 18204591Sluigi * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 19204591Sluigi * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 20204591Sluigi * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 21204591Sluigi * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 22204591Sluigi * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 23204591Sluigi * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 24204591Sluigi * SUCH DAMAGE. 25204591Sluigi */ 26204591Sluigi 27204591Sluigi/* 28204591Sluigi * $FreeBSD: releng/11.0/sys/netpfil/ipfw/dn_sched_qfq.c 300779 2016-05-26 21:40:13Z truckman $ 29204591Sluigi */ 30204591Sluigi 31204591Sluigi#ifdef _KERNEL 32204591Sluigi#include <sys/malloc.h> 33204591Sluigi#include <sys/socket.h> 34204591Sluigi#include <sys/socketvar.h> 35204591Sluigi#include <sys/kernel.h> 36204591Sluigi#include <sys/mbuf.h> 37204591Sluigi#include <sys/module.h> 38204591Sluigi#include <net/if.h> /* IFNAMSIZ */ 39204591Sluigi#include <netinet/in.h> 40204591Sluigi#include <netinet/ip_var.h> /* ipfw_rule_ref */ 41204591Sluigi#include <netinet/ip_fw.h> /* flow_id */ 42204591Sluigi#include <netinet/ip_dummynet.h> 43240494Sglebius#include <netpfil/ipfw/dn_heap.h> 44240494Sglebius#include <netpfil/ipfw/ip_dn_private.h> 45300779Struckman#ifdef NEW_AQM 46300779Struckman#include <netpfil/ipfw/dn_aqm.h> 47300779Struckman#endif 48240494Sglebius#include <netpfil/ipfw/dn_sched.h> 49204591Sluigi#else 50204591Sluigi#include <dn_test.h> 51204591Sluigi#endif 52204591Sluigi 53204591Sluigi#ifdef QFQ_DEBUG 54294858Sluigi#define _P64 unsigned long long /* cast for printing uint64_t */ 55204591Sluigistruct qfq_sched; 56204591Sluigistatic void dump_sched(struct qfq_sched *q, const char *msg); 57204591Sluigi#define NO(x) x 58204591Sluigi#else 59204591Sluigi#define NO(x) 60204591Sluigi#endif 61204591Sluigi#define DN_SCHED_QFQ 4 // XXX Where? 62204591Sluigitypedef unsigned long bitmap; 63204591Sluigi 64204591Sluigi/* 65204591Sluigi * bitmaps ops are critical. Some linux versions have __fls 66204591Sluigi * and the bitmap ops. Some machines have ffs 67294761Sluigi * NOTE: fls() returns 1 for the least significant bit, 68294761Sluigi * __fls() returns 0 for the same case. 69294761Sluigi * We use the base-0 version __fls() to match the description in 70294761Sluigi * the ToN QFQ paper 71204591Sluigi */ 72213253Sluigi#if defined(_WIN32) || (defined(__MIPSEL__) && defined(LINUX_24)) 73204591Sluigiint fls(unsigned int n) 74204591Sluigi{ 75204591Sluigi int i = 0; 76204591Sluigi for (i = 0; n > 0; n >>= 1, i++) 77204591Sluigi ; 78204591Sluigi return i; 79204591Sluigi} 80204591Sluigi#endif 81204591Sluigi 82213253Sluigi#if !defined(_KERNEL) || defined( __FreeBSD__ ) || defined(_WIN32) || (defined(__MIPSEL__) && defined(LINUX_24)) 83204591Sluigistatic inline unsigned long __fls(unsigned long word) 84204591Sluigi{ 85204591Sluigi return fls(word) - 1; 86204591Sluigi} 87204591Sluigi#endif 88204591Sluigi 89204591Sluigi#if !defined(_KERNEL) || !defined(__linux__) 90204591Sluigi#ifdef QFQ_DEBUG 91294858Sluigistatic int test_bit(int ix, bitmap *p) 92204591Sluigi{ 93204591Sluigi if (ix < 0 || ix > 31) 94204591Sluigi D("bad index %d", ix); 95204591Sluigi return *p & (1<<ix); 96204591Sluigi} 97294858Sluigistatic void __set_bit(int ix, bitmap *p) 98204591Sluigi{ 99204591Sluigi if (ix < 0 || ix > 31) 100204591Sluigi D("bad index %d", ix); 101204591Sluigi *p |= (1<<ix); 102204591Sluigi} 103294858Sluigistatic void __clear_bit(int ix, bitmap *p) 104204591Sluigi{ 105204591Sluigi if (ix < 0 || ix > 31) 106204591Sluigi D("bad index %d", ix); 107204591Sluigi *p &= ~(1<<ix); 108204591Sluigi} 109204591Sluigi#else /* !QFQ_DEBUG */ 110204591Sluigi/* XXX do we have fast version, or leave it to the compiler ? */ 111204591Sluigi#define test_bit(ix, pData) ((*pData) & (1<<(ix))) 112204591Sluigi#define __set_bit(ix, pData) (*pData) |= (1<<(ix)) 113204591Sluigi#define __clear_bit(ix, pData) (*pData) &= ~(1<<(ix)) 114204591Sluigi#endif /* !QFQ_DEBUG */ 115204591Sluigi#endif /* !__linux__ */ 116204591Sluigi 117204591Sluigi#ifdef __MIPSEL__ 118213253Sluigi#define __clear_bit(ix, pData) (*pData) &= ~(1<<(ix)) 119204591Sluigi#endif 120204591Sluigi 121204591Sluigi/*-------------------------------------------*/ 122204591Sluigi/* 123204591Sluigi 124204591SluigiVirtual time computations. 125204591Sluigi 126204591SluigiS, F and V are all computed in fixed point arithmetic with 127204591SluigiFRAC_BITS decimal bits. 128204591Sluigi 129204591Sluigi QFQ_MAX_INDEX is the maximum index allowed for a group. We need 130204591Sluigi one bit per index. 131204591Sluigi QFQ_MAX_WSHIFT is the maximum power of two supported as a weight. 132204591Sluigi The layout of the bits is as below: 133204591Sluigi 134204591Sluigi [ MTU_SHIFT ][ FRAC_BITS ] 135204591Sluigi [ MAX_INDEX ][ MIN_SLOT_SHIFT ] 136204591Sluigi ^.__grp->index = 0 137204591Sluigi *.__grp->slot_shift 138204591Sluigi 139204591Sluigi where MIN_SLOT_SHIFT is derived by difference from the others. 140204591Sluigi 141204591SluigiThe max group index corresponds to Lmax/w_min, where 142204591SluigiLmax=1<<MTU_SHIFT, w_min = 1 . 143204591SluigiFrom this, and knowing how many groups (MAX_INDEX) we want, 144204591Sluigiwe can derive the shift corresponding to each group. 145204591Sluigi 146204591SluigiBecause we often need to compute 147204591Sluigi F = S + len/w_i and V = V + len/wsum 148204591Sluigiinstead of storing w_i store the value 149204591Sluigi inv_w = (1<<FRAC_BITS)/w_i 150204591Sluigiso we can do F = S + len * inv_w * wsum. 151204591SluigiWe use W_TOT in the formulas so we can easily move between 152204591Sluigistatic and adaptive weight sum. 153204591Sluigi 154204591SluigiThe per-scheduler-instance data contain all the data structures 155204591Sluigifor the scheduler: bitmaps and bucket lists. 156204591Sluigi 157204591Sluigi */ 158204591Sluigi/* 159204591Sluigi * Maximum number of consecutive slots occupied by backlogged classes 160204591Sluigi * inside a group. This is approx lmax/lmin + 5. 161204591Sluigi * XXX check because it poses constraints on MAX_INDEX 162204591Sluigi */ 163204591Sluigi#define QFQ_MAX_SLOTS 32 164204591Sluigi/* 165204591Sluigi * Shifts used for class<->group mapping. Class weights are 166204591Sluigi * in the range [1, QFQ_MAX_WEIGHT], we to map each class i to the 167204591Sluigi * group with the smallest index that can support the L_i / r_i 168204591Sluigi * configured for the class. 169204591Sluigi * 170204591Sluigi * grp->index is the index of the group; and grp->slot_shift 171204591Sluigi * is the shift for the corresponding (scaled) sigma_i. 172204591Sluigi * 173204591Sluigi * When computing the group index, we do (len<<FP_SHIFT)/weight, 174204591Sluigi * then compute an FLS (which is like a log2()), and if the result 175204591Sluigi * is below the MAX_INDEX region we use 0 (which is the same as 176204591Sluigi * using a larger len). 177204591Sluigi */ 178204591Sluigi#define QFQ_MAX_INDEX 19 179204591Sluigi#define QFQ_MAX_WSHIFT 16 /* log2(max_weight) */ 180204591Sluigi 181204591Sluigi#define QFQ_MAX_WEIGHT (1<<QFQ_MAX_WSHIFT) 182204591Sluigi#define QFQ_MAX_WSUM (2*QFQ_MAX_WEIGHT) 183204591Sluigi 184204591Sluigi#define FRAC_BITS 30 /* fixed point arithmetic */ 185204591Sluigi#define ONE_FP (1UL << FRAC_BITS) 186204591Sluigi 187204591Sluigi#define QFQ_MTU_SHIFT 11 /* log2(max_len) */ 188204591Sluigi#define QFQ_MIN_SLOT_SHIFT (FRAC_BITS + QFQ_MTU_SHIFT - QFQ_MAX_INDEX) 189204591Sluigi 190204591Sluigi/* 191204591Sluigi * Possible group states, also indexes for the bitmaps array in 192204591Sluigi * struct qfq_queue. We rely on ER, IR, EB, IB being numbered 0..3 193204591Sluigi */ 194204591Sluigienum qfq_state { ER, IR, EB, IB, QFQ_MAX_STATE }; 195204591Sluigi 196204591Sluigistruct qfq_group; 197204591Sluigi/* 198204591Sluigi * additional queue info. Some of this info should come from 199204591Sluigi * the flowset, we copy them here for faster processing. 200204591Sluigi * This is an overlay of the struct dn_queue 201204591Sluigi */ 202204591Sluigistruct qfq_class { 203204591Sluigi struct dn_queue _q; 204204591Sluigi uint64_t S, F; /* flow timestamps (exact) */ 205204591Sluigi struct qfq_class *next; /* Link for the slot list. */ 206204591Sluigi 207204591Sluigi /* group we belong to. In principle we would need the index, 208204591Sluigi * which is log_2(lmax/weight), but we never reference it 209204591Sluigi * directly, only the group. 210204591Sluigi */ 211204591Sluigi struct qfq_group *grp; 212204591Sluigi 213204591Sluigi /* these are copied from the flowset. */ 214204591Sluigi uint32_t inv_w; /* ONE_FP/weight */ 215204591Sluigi uint32_t lmax; /* Max packet size for this flow. */ 216204591Sluigi}; 217204591Sluigi 218204591Sluigi/* Group descriptor, see the paper for details. 219204591Sluigi * Basically this contains the bucket lists 220204591Sluigi */ 221204591Sluigistruct qfq_group { 222204591Sluigi uint64_t S, F; /* group timestamps (approx). */ 223204591Sluigi unsigned int slot_shift; /* Slot shift. */ 224204591Sluigi unsigned int index; /* Group index. */ 225204591Sluigi unsigned int front; /* Index of the front slot. */ 226204591Sluigi bitmap full_slots; /* non-empty slots */ 227204591Sluigi 228204591Sluigi /* Array of lists of active classes. */ 229204591Sluigi struct qfq_class *slots[QFQ_MAX_SLOTS]; 230204591Sluigi}; 231204591Sluigi 232204591Sluigi/* scheduler instance descriptor. */ 233204591Sluigistruct qfq_sched { 234204591Sluigi uint64_t V; /* Precise virtual time. */ 235204591Sluigi uint32_t wsum; /* weight sum */ 236285362Sluigi uint32_t iwsum; /* inverse weight sum */ 237294858Sluigi NO(uint32_t i_wsum;) /* ONE_FP/w_sum */ 238294858Sluigi NO(uint32_t queued;) /* debugging */ 239294858Sluigi NO(uint32_t loops;) /* debugging */ 240204591Sluigi bitmap bitmaps[QFQ_MAX_STATE]; /* Group bitmaps. */ 241204591Sluigi struct qfq_group groups[QFQ_MAX_INDEX + 1]; /* The groups. */ 242204591Sluigi}; 243204591Sluigi 244204591Sluigi/*---- support functions ----------------------------*/ 245204591Sluigi 246204591Sluigi/* Generic comparison function, handling wraparound. */ 247204591Sluigistatic inline int qfq_gt(uint64_t a, uint64_t b) 248204591Sluigi{ 249204591Sluigi return (int64_t)(a - b) > 0; 250204591Sluigi} 251204591Sluigi 252204591Sluigi/* Round a precise timestamp to its slotted value. */ 253204591Sluigistatic inline uint64_t qfq_round_down(uint64_t ts, unsigned int shift) 254204591Sluigi{ 255204591Sluigi return ts & ~((1ULL << shift) - 1); 256204591Sluigi} 257204591Sluigi 258204591Sluigi/* return the pointer to the group with lowest index in the bitmap */ 259204591Sluigistatic inline struct qfq_group *qfq_ffs(struct qfq_sched *q, 260204591Sluigi unsigned long bitmap) 261204591Sluigi{ 262204591Sluigi int index = ffs(bitmap) - 1; // zero-based 263204591Sluigi return &q->groups[index]; 264204591Sluigi} 265204591Sluigi 266204591Sluigi/* 267204591Sluigi * Calculate a flow index, given its weight and maximum packet length. 268204591Sluigi * index = log_2(maxlen/weight) but we need to apply the scaling. 269204591Sluigi * This is used only once at flow creation. 270204591Sluigi */ 271204591Sluigistatic int qfq_calc_index(uint32_t inv_w, unsigned int maxlen) 272204591Sluigi{ 273204591Sluigi uint64_t slot_size = (uint64_t)maxlen *inv_w; 274204591Sluigi unsigned long size_map; 275204591Sluigi int index = 0; 276204591Sluigi 277204591Sluigi size_map = (unsigned long)(slot_size >> QFQ_MIN_SLOT_SHIFT); 278204591Sluigi if (!size_map) 279204591Sluigi goto out; 280204591Sluigi 281204591Sluigi index = __fls(size_map) + 1; // basically a log_2() 282204591Sluigi index -= !(slot_size - (1ULL << (index + QFQ_MIN_SLOT_SHIFT - 1))); 283204591Sluigi 284204591Sluigi if (index < 0) 285204591Sluigi index = 0; 286204591Sluigi 287204591Sluigiout: 288204591Sluigi ND("W = %d, L = %d, I = %d\n", ONE_FP/inv_w, maxlen, index); 289204591Sluigi return index; 290204591Sluigi} 291204591Sluigi/*---- end support functions ----*/ 292204591Sluigi 293204591Sluigi/*-------- API calls --------------------------------*/ 294204591Sluigi/* 295204591Sluigi * Validate and copy parameters from flowset. 296204591Sluigi */ 297204591Sluigistatic int 298204591Sluigiqfq_new_queue(struct dn_queue *_q) 299204591Sluigi{ 300204591Sluigi struct qfq_sched *q = (struct qfq_sched *)(_q->_si + 1); 301204591Sluigi struct qfq_class *cl = (struct qfq_class *)_q; 302204591Sluigi int i; 303204591Sluigi uint32_t w; /* approximated weight */ 304204591Sluigi 305204591Sluigi /* import parameters from the flowset. They should be correct 306204591Sluigi * already. 307204591Sluigi */ 308204591Sluigi w = _q->fs->fs.par[0]; 309204591Sluigi cl->lmax = _q->fs->fs.par[1]; 310204591Sluigi if (!w || w > QFQ_MAX_WEIGHT) { 311204591Sluigi w = 1; 312204591Sluigi D("rounding weight to 1"); 313204591Sluigi } 314204591Sluigi cl->inv_w = ONE_FP/w; 315204591Sluigi w = ONE_FP/cl->inv_w; 316204591Sluigi if (q->wsum + w > QFQ_MAX_WSUM) 317204591Sluigi return EINVAL; 318204591Sluigi 319204591Sluigi i = qfq_calc_index(cl->inv_w, cl->lmax); 320204591Sluigi cl->grp = &q->groups[i]; 321204591Sluigi q->wsum += w; 322285362Sluigi q->iwsum = ONE_FP / q->wsum; /* XXX note theory */ 323204591Sluigi // XXX cl->S = q->V; ? 324204591Sluigi return 0; 325204591Sluigi} 326204591Sluigi 327204591Sluigi/* remove an empty queue */ 328204591Sluigistatic int 329204591Sluigiqfq_free_queue(struct dn_queue *_q) 330204591Sluigi{ 331204591Sluigi struct qfq_sched *q = (struct qfq_sched *)(_q->_si + 1); 332204591Sluigi struct qfq_class *cl = (struct qfq_class *)_q; 333204591Sluigi if (cl->inv_w) { 334204591Sluigi q->wsum -= ONE_FP/cl->inv_w; 335285362Sluigi if (q->wsum != 0) 336285362Sluigi q->iwsum = ONE_FP / q->wsum; 337204591Sluigi cl->inv_w = 0; /* reset weight to avoid run twice */ 338204591Sluigi } 339204591Sluigi return 0; 340204591Sluigi} 341204591Sluigi 342204591Sluigi/* Calculate a mask to mimic what would be ffs_from(). */ 343204591Sluigistatic inline unsigned long 344204591Sluigimask_from(unsigned long bitmap, int from) 345204591Sluigi{ 346204591Sluigi return bitmap & ~((1UL << from) - 1); 347204591Sluigi} 348204591Sluigi 349204591Sluigi/* 350204591Sluigi * The state computation relies on ER=0, IR=1, EB=2, IB=3 351204591Sluigi * First compute eligibility comparing grp->S, q->V, 352204591Sluigi * then check if someone is blocking us and possibly add EB 353204591Sluigi */ 354204591Sluigistatic inline unsigned int 355204591Sluigiqfq_calc_state(struct qfq_sched *q, struct qfq_group *grp) 356204591Sluigi{ 357204591Sluigi /* if S > V we are not eligible */ 358204591Sluigi unsigned int state = qfq_gt(grp->S, q->V); 359204591Sluigi unsigned long mask = mask_from(q->bitmaps[ER], grp->index); 360204591Sluigi struct qfq_group *next; 361204591Sluigi 362204591Sluigi if (mask) { 363204591Sluigi next = qfq_ffs(q, mask); 364204591Sluigi if (qfq_gt(grp->F, next->F)) 365204591Sluigi state |= EB; 366204591Sluigi } 367204591Sluigi 368204591Sluigi return state; 369204591Sluigi} 370204591Sluigi 371204591Sluigi/* 372204591Sluigi * In principle 373204591Sluigi * q->bitmaps[dst] |= q->bitmaps[src] & mask; 374204591Sluigi * q->bitmaps[src] &= ~mask; 375204591Sluigi * but we should make sure that src != dst 376204591Sluigi */ 377204591Sluigistatic inline void 378204591Sluigiqfq_move_groups(struct qfq_sched *q, unsigned long mask, int src, int dst) 379204591Sluigi{ 380204591Sluigi q->bitmaps[dst] |= q->bitmaps[src] & mask; 381204591Sluigi q->bitmaps[src] &= ~mask; 382204591Sluigi} 383204591Sluigi 384204591Sluigistatic inline void 385204591Sluigiqfq_unblock_groups(struct qfq_sched *q, int index, uint64_t old_finish) 386204591Sluigi{ 387204591Sluigi unsigned long mask = mask_from(q->bitmaps[ER], index + 1); 388204591Sluigi struct qfq_group *next; 389204591Sluigi 390204591Sluigi if (mask) { 391204591Sluigi next = qfq_ffs(q, mask); 392204591Sluigi if (!qfq_gt(next->F, old_finish)) 393204591Sluigi return; 394204591Sluigi } 395204591Sluigi 396204591Sluigi mask = (1UL << index) - 1; 397204591Sluigi qfq_move_groups(q, mask, EB, ER); 398204591Sluigi qfq_move_groups(q, mask, IB, IR); 399204591Sluigi} 400204591Sluigi 401204591Sluigi/* 402204591Sluigi * perhaps 403204591Sluigi * 404204591Sluigi old_V ^= q->V; 405204591Sluigi old_V >>= QFQ_MIN_SLOT_SHIFT; 406204591Sluigi if (old_V) { 407204591Sluigi ... 408204591Sluigi } 409204591Sluigi * 410204591Sluigi */ 411204591Sluigistatic inline void 412204591Sluigiqfq_make_eligible(struct qfq_sched *q, uint64_t old_V) 413204591Sluigi{ 414204591Sluigi unsigned long mask, vslot, old_vslot; 415204591Sluigi 416204591Sluigi vslot = q->V >> QFQ_MIN_SLOT_SHIFT; 417204591Sluigi old_vslot = old_V >> QFQ_MIN_SLOT_SHIFT; 418204591Sluigi 419204591Sluigi if (vslot != old_vslot) { 420294761Sluigi /* must be 2ULL, see ToN QFQ article fig.5, we use base-0 fls */ 421294761Sluigi mask = (2ULL << (__fls(vslot ^ old_vslot))) - 1; 422204591Sluigi qfq_move_groups(q, mask, IR, ER); 423204591Sluigi qfq_move_groups(q, mask, IB, EB); 424204591Sluigi } 425204591Sluigi} 426204591Sluigi 427204591Sluigi/* 428204591Sluigi * XXX we should make sure that slot becomes less than 32. 429204591Sluigi * This is guaranteed by the input values. 430204591Sluigi * roundedS is always cl->S rounded on grp->slot_shift bits. 431204591Sluigi */ 432204591Sluigistatic inline void 433204591Sluigiqfq_slot_insert(struct qfq_group *grp, struct qfq_class *cl, uint64_t roundedS) 434204591Sluigi{ 435204591Sluigi uint64_t slot = (roundedS - grp->S) >> grp->slot_shift; 436204591Sluigi unsigned int i = (grp->front + slot) % QFQ_MAX_SLOTS; 437204591Sluigi 438204591Sluigi cl->next = grp->slots[i]; 439204591Sluigi grp->slots[i] = cl; 440204591Sluigi __set_bit(slot, &grp->full_slots); 441204591Sluigi} 442204591Sluigi 443204591Sluigi/* 444204591Sluigi * remove the entry from the slot 445204591Sluigi */ 446204591Sluigistatic inline void 447204591Sluigiqfq_front_slot_remove(struct qfq_group *grp) 448204591Sluigi{ 449204591Sluigi struct qfq_class **h = &grp->slots[grp->front]; 450204591Sluigi 451204591Sluigi *h = (*h)->next; 452204591Sluigi if (!*h) 453204591Sluigi __clear_bit(0, &grp->full_slots); 454204591Sluigi} 455204591Sluigi 456204591Sluigi/* 457204591Sluigi * Returns the first full queue in a group. As a side effect, 458204591Sluigi * adjust the bucket list so the first non-empty bucket is at 459204591Sluigi * position 0 in full_slots. 460204591Sluigi */ 461204591Sluigistatic inline struct qfq_class * 462204591Sluigiqfq_slot_scan(struct qfq_group *grp) 463204591Sluigi{ 464204591Sluigi int i; 465204591Sluigi 466204591Sluigi ND("grp %d full %x", grp->index, grp->full_slots); 467204591Sluigi if (!grp->full_slots) 468204591Sluigi return NULL; 469204591Sluigi 470204591Sluigi i = ffs(grp->full_slots) - 1; // zero-based 471204591Sluigi if (i > 0) { 472204591Sluigi grp->front = (grp->front + i) % QFQ_MAX_SLOTS; 473204591Sluigi grp->full_slots >>= i; 474204591Sluigi } 475204591Sluigi 476204591Sluigi return grp->slots[grp->front]; 477204591Sluigi} 478204591Sluigi 479204591Sluigi/* 480204591Sluigi * adjust the bucket list. When the start time of a group decreases, 481204591Sluigi * we move the index down (modulo QFQ_MAX_SLOTS) so we don't need to 482204591Sluigi * move the objects. The mask of occupied slots must be shifted 483204591Sluigi * because we use ffs() to find the first non-empty slot. 484204591Sluigi * This covers decreases in the group's start time, but what about 485204591Sluigi * increases of the start time ? 486204591Sluigi * Here too we should make sure that i is less than 32 487204591Sluigi */ 488204591Sluigistatic inline void 489204591Sluigiqfq_slot_rotate(struct qfq_sched *q, struct qfq_group *grp, uint64_t roundedS) 490204591Sluigi{ 491204591Sluigi unsigned int i = (grp->S - roundedS) >> grp->slot_shift; 492204591Sluigi 493294858Sluigi (void)q; 494204591Sluigi grp->full_slots <<= i; 495204591Sluigi grp->front = (grp->front - i) % QFQ_MAX_SLOTS; 496204591Sluigi} 497204591Sluigi 498204591Sluigi 499204591Sluigistatic inline void 500204591Sluigiqfq_update_eligible(struct qfq_sched *q, uint64_t old_V) 501204591Sluigi{ 502204591Sluigi bitmap ineligible; 503204591Sluigi 504204591Sluigi ineligible = q->bitmaps[IR] | q->bitmaps[IB]; 505204591Sluigi if (ineligible) { 506204591Sluigi if (!q->bitmaps[ER]) { 507204591Sluigi struct qfq_group *grp; 508204591Sluigi grp = qfq_ffs(q, ineligible); 509204591Sluigi if (qfq_gt(grp->S, q->V)) 510204591Sluigi q->V = grp->S; 511204591Sluigi } 512204591Sluigi qfq_make_eligible(q, old_V); 513204591Sluigi } 514204591Sluigi} 515204591Sluigi 516204591Sluigi/* 517204591Sluigi * Updates the class, returns true if also the group needs to be updated. 518204591Sluigi */ 519204591Sluigistatic inline int 520204591Sluigiqfq_update_class(struct qfq_sched *q, struct qfq_group *grp, 521204591Sluigi struct qfq_class *cl) 522204591Sluigi{ 523204591Sluigi 524294858Sluigi (void)q; 525204591Sluigi cl->S = cl->F; 526204591Sluigi if (cl->_q.mq.head == NULL) { 527204591Sluigi qfq_front_slot_remove(grp); 528204591Sluigi } else { 529204591Sluigi unsigned int len; 530204591Sluigi uint64_t roundedS; 531204591Sluigi 532204591Sluigi len = cl->_q.mq.head->m_pkthdr.len; 533204591Sluigi cl->F = cl->S + (uint64_t)len * cl->inv_w; 534204591Sluigi roundedS = qfq_round_down(cl->S, grp->slot_shift); 535204591Sluigi if (roundedS == grp->S) 536204591Sluigi return 0; 537204591Sluigi 538204591Sluigi qfq_front_slot_remove(grp); 539204591Sluigi qfq_slot_insert(grp, cl, roundedS); 540204591Sluigi } 541204591Sluigi return 1; 542204591Sluigi} 543204591Sluigi 544204591Sluigistatic struct mbuf * 545204591Sluigiqfq_dequeue(struct dn_sch_inst *si) 546204591Sluigi{ 547204591Sluigi struct qfq_sched *q = (struct qfq_sched *)(si + 1); 548204591Sluigi struct qfq_group *grp; 549204591Sluigi struct qfq_class *cl; 550204591Sluigi struct mbuf *m; 551204591Sluigi uint64_t old_V; 552204591Sluigi 553204591Sluigi NO(q->loops++;) 554204591Sluigi if (!q->bitmaps[ER]) { 555204591Sluigi NO(if (q->queued) 556204591Sluigi dump_sched(q, "start dequeue");) 557204591Sluigi return NULL; 558204591Sluigi } 559204591Sluigi 560204591Sluigi grp = qfq_ffs(q, q->bitmaps[ER]); 561204591Sluigi 562204591Sluigi cl = grp->slots[grp->front]; 563204591Sluigi /* extract from the first bucket in the bucket list */ 564204591Sluigi m = dn_dequeue(&cl->_q); 565204591Sluigi 566204591Sluigi if (!m) { 567204591Sluigi D("BUG/* non-workconserving leaf */"); 568204591Sluigi return NULL; 569204591Sluigi } 570204591Sluigi NO(q->queued--;) 571204591Sluigi old_V = q->V; 572285362Sluigi q->V += (uint64_t)m->m_pkthdr.len * q->iwsum; 573204591Sluigi ND("m is %p F 0x%llx V now 0x%llx", m, cl->F, q->V); 574204591Sluigi 575204591Sluigi if (qfq_update_class(q, grp, cl)) { 576204591Sluigi uint64_t old_F = grp->F; 577204591Sluigi cl = qfq_slot_scan(grp); 578204591Sluigi if (!cl) { /* group gone, remove from ER */ 579204591Sluigi __clear_bit(grp->index, &q->bitmaps[ER]); 580204591Sluigi // grp->S = grp->F + 1; // XXX debugging only 581204591Sluigi } else { 582204591Sluigi uint64_t roundedS = qfq_round_down(cl->S, grp->slot_shift); 583204591Sluigi unsigned int s; 584204591Sluigi 585204591Sluigi if (grp->S == roundedS) 586204591Sluigi goto skip_unblock; 587204591Sluigi grp->S = roundedS; 588204591Sluigi grp->F = roundedS + (2ULL << grp->slot_shift); 589204591Sluigi /* remove from ER and put in the new set */ 590204591Sluigi __clear_bit(grp->index, &q->bitmaps[ER]); 591204591Sluigi s = qfq_calc_state(q, grp); 592204591Sluigi __set_bit(grp->index, &q->bitmaps[s]); 593204591Sluigi } 594204591Sluigi /* we need to unblock even if the group has gone away */ 595204591Sluigi qfq_unblock_groups(q, grp->index, old_F); 596204591Sluigi } 597204591Sluigi 598204591Sluigiskip_unblock: 599204591Sluigi qfq_update_eligible(q, old_V); 600204591Sluigi NO(if (!q->bitmaps[ER] && q->queued) 601204591Sluigi dump_sched(q, "end dequeue");) 602204591Sluigi 603204591Sluigi return m; 604204591Sluigi} 605204591Sluigi 606204591Sluigi/* 607204591Sluigi * Assign a reasonable start time for a new flow k in group i. 608204591Sluigi * Admissible values for \hat(F) are multiples of \sigma_i 609204591Sluigi * no greater than V+\sigma_i . Larger values mean that 610204591Sluigi * we had a wraparound so we consider the timestamp to be stale. 611204591Sluigi * 612204591Sluigi * If F is not stale and F >= V then we set S = F. 613204591Sluigi * Otherwise we should assign S = V, but this may violate 614204591Sluigi * the ordering in ER. So, if we have groups in ER, set S to 615204591Sluigi * the F_j of the first group j which would be blocking us. 616204591Sluigi * We are guaranteed not to move S backward because 617204591Sluigi * otherwise our group i would still be blocked. 618204591Sluigi */ 619204591Sluigistatic inline void 620204591Sluigiqfq_update_start(struct qfq_sched *q, struct qfq_class *cl) 621204591Sluigi{ 622204591Sluigi unsigned long mask; 623230614Sluigi uint64_t limit, roundedF; 624204591Sluigi int slot_shift = cl->grp->slot_shift; 625204591Sluigi 626204591Sluigi roundedF = qfq_round_down(cl->F, slot_shift); 627285362Sluigi limit = qfq_round_down(q->V, slot_shift) + (1ULL << slot_shift); 628204591Sluigi 629204591Sluigi if (!qfq_gt(cl->F, q->V) || qfq_gt(roundedF, limit)) { 630204591Sluigi /* timestamp was stale */ 631204591Sluigi mask = mask_from(q->bitmaps[ER], cl->grp->index); 632204591Sluigi if (mask) { 633204591Sluigi struct qfq_group *next = qfq_ffs(q, mask); 634204591Sluigi if (qfq_gt(roundedF, next->F)) { 635285362Sluigi /* from pv 71261956973ba9e0637848a5adb4a5819b4bae83 */ 636285362Sluigi if (qfq_gt(limit, next->F)) 637285362Sluigi cl->S = next->F; 638285362Sluigi else /* preserve timestamp correctness */ 639285362Sluigi cl->S = limit; 640204591Sluigi return; 641204591Sluigi } 642204591Sluigi } 643204591Sluigi cl->S = q->V; 644204591Sluigi } else { /* timestamp is not stale */ 645204591Sluigi cl->S = cl->F; 646204591Sluigi } 647204591Sluigi} 648204591Sluigi 649204591Sluigistatic int 650204591Sluigiqfq_enqueue(struct dn_sch_inst *si, struct dn_queue *_q, struct mbuf *m) 651204591Sluigi{ 652204591Sluigi struct qfq_sched *q = (struct qfq_sched *)(si + 1); 653204591Sluigi struct qfq_group *grp; 654204591Sluigi struct qfq_class *cl = (struct qfq_class *)_q; 655204591Sluigi uint64_t roundedS; 656204591Sluigi int s; 657204591Sluigi 658204591Sluigi NO(q->loops++;) 659204591Sluigi DX(4, "len %d flow %p inv_w 0x%x grp %d", m->m_pkthdr.len, 660204591Sluigi _q, cl->inv_w, cl->grp->index); 661204591Sluigi /* XXX verify that the packet obeys the parameters */ 662204591Sluigi if (m != _q->mq.head) { 663204591Sluigi if (dn_enqueue(_q, m, 0)) /* packet was dropped */ 664204591Sluigi return 1; 665204591Sluigi NO(q->queued++;) 666204591Sluigi if (m != _q->mq.head) 667204591Sluigi return 0; 668204591Sluigi } 669204591Sluigi /* If reach this point, queue q was idle */ 670204591Sluigi grp = cl->grp; 671204591Sluigi qfq_update_start(q, cl); /* adjust start time */ 672204591Sluigi /* compute new finish time and rounded start. */ 673204591Sluigi cl->F = cl->S + (uint64_t)(m->m_pkthdr.len) * cl->inv_w; 674204591Sluigi roundedS = qfq_round_down(cl->S, grp->slot_shift); 675204591Sluigi 676204591Sluigi /* 677204591Sluigi * insert cl in the correct bucket. 678204591Sluigi * If cl->S >= grp->S we don't need to adjust the 679204591Sluigi * bucket list and simply go to the insertion phase. 680204591Sluigi * Otherwise grp->S is decreasing, we must make room 681204591Sluigi * in the bucket list, and also recompute the group state. 682204591Sluigi * Finally, if there were no flows in this group and nobody 683204591Sluigi * was in ER make sure to adjust V. 684204591Sluigi */ 685204591Sluigi if (grp->full_slots) { 686204591Sluigi if (!qfq_gt(grp->S, cl->S)) 687204591Sluigi goto skip_update; 688204591Sluigi /* create a slot for this cl->S */ 689204591Sluigi qfq_slot_rotate(q, grp, roundedS); 690204591Sluigi /* group was surely ineligible, remove */ 691204591Sluigi __clear_bit(grp->index, &q->bitmaps[IR]); 692204591Sluigi __clear_bit(grp->index, &q->bitmaps[IB]); 693204591Sluigi } else if (!q->bitmaps[ER] && qfq_gt(roundedS, q->V)) 694204591Sluigi q->V = roundedS; 695204591Sluigi 696204591Sluigi grp->S = roundedS; 697204591Sluigi grp->F = roundedS + (2ULL << grp->slot_shift); // i.e. 2\sigma_i 698204591Sluigi s = qfq_calc_state(q, grp); 699204591Sluigi __set_bit(grp->index, &q->bitmaps[s]); 700204591Sluigi ND("new state %d 0x%x", s, q->bitmaps[s]); 701204591Sluigi ND("S %llx F %llx V %llx", cl->S, cl->F, q->V); 702204591Sluigiskip_update: 703204591Sluigi qfq_slot_insert(grp, cl, roundedS); 704204591Sluigi 705204591Sluigi return 0; 706204591Sluigi} 707204591Sluigi 708204591Sluigi 709204591Sluigi#if 0 710204591Sluigistatic inline void 711204591Sluigiqfq_slot_remove(struct qfq_sched *q, struct qfq_group *grp, 712204591Sluigi struct qfq_class *cl, struct qfq_class **pprev) 713204591Sluigi{ 714204591Sluigi unsigned int i, offset; 715204591Sluigi uint64_t roundedS; 716204591Sluigi 717204591Sluigi roundedS = qfq_round_down(cl->S, grp->slot_shift); 718204591Sluigi offset = (roundedS - grp->S) >> grp->slot_shift; 719204591Sluigi i = (grp->front + offset) % QFQ_MAX_SLOTS; 720204591Sluigi 721204591Sluigi#ifdef notyet 722204591Sluigi if (!pprev) { 723204591Sluigi pprev = &grp->slots[i]; 724204591Sluigi while (*pprev && *pprev != cl) 725204591Sluigi pprev = &(*pprev)->next; 726204591Sluigi } 727204591Sluigi#endif 728204591Sluigi 729204591Sluigi *pprev = cl->next; 730204591Sluigi if (!grp->slots[i]) 731204591Sluigi __clear_bit(offset, &grp->full_slots); 732204591Sluigi} 733204591Sluigi 734204591Sluigi/* 735204591Sluigi * called to forcibly destroy a queue. 736204591Sluigi * If the queue is not in the front bucket, or if it has 737204591Sluigi * other queues in the front bucket, we can simply remove 738204591Sluigi * the queue with no other side effects. 739204591Sluigi * Otherwise we must propagate the event up. 740204591Sluigi * XXX description to be completed. 741204591Sluigi */ 742204591Sluigistatic void 743204591Sluigiqfq_deactivate_class(struct qfq_sched *q, struct qfq_class *cl, 744204591Sluigi struct qfq_class **pprev) 745204591Sluigi{ 746204591Sluigi struct qfq_group *grp = &q->groups[cl->index]; 747204591Sluigi unsigned long mask; 748204591Sluigi uint64_t roundedS; 749204591Sluigi int s; 750204591Sluigi 751204591Sluigi cl->F = cl->S; // not needed if the class goes away. 752204591Sluigi qfq_slot_remove(q, grp, cl, pprev); 753204591Sluigi 754204591Sluigi if (!grp->full_slots) { 755204591Sluigi /* nothing left in the group, remove from all sets. 756204591Sluigi * Do ER last because if we were blocking other groups 757204591Sluigi * we must unblock them. 758204591Sluigi */ 759204591Sluigi __clear_bit(grp->index, &q->bitmaps[IR]); 760204591Sluigi __clear_bit(grp->index, &q->bitmaps[EB]); 761204591Sluigi __clear_bit(grp->index, &q->bitmaps[IB]); 762204591Sluigi 763204591Sluigi if (test_bit(grp->index, &q->bitmaps[ER]) && 764204591Sluigi !(q->bitmaps[ER] & ~((1UL << grp->index) - 1))) { 765204591Sluigi mask = q->bitmaps[ER] & ((1UL << grp->index) - 1); 766204591Sluigi if (mask) 767204591Sluigi mask = ~((1UL << __fls(mask)) - 1); 768204591Sluigi else 769204591Sluigi mask = ~0UL; 770204591Sluigi qfq_move_groups(q, mask, EB, ER); 771204591Sluigi qfq_move_groups(q, mask, IB, IR); 772204591Sluigi } 773204591Sluigi __clear_bit(grp->index, &q->bitmaps[ER]); 774204591Sluigi } else if (!grp->slots[grp->front]) { 775204591Sluigi cl = qfq_slot_scan(grp); 776204591Sluigi roundedS = qfq_round_down(cl->S, grp->slot_shift); 777204591Sluigi if (grp->S != roundedS) { 778204591Sluigi __clear_bit(grp->index, &q->bitmaps[ER]); 779204591Sluigi __clear_bit(grp->index, &q->bitmaps[IR]); 780204591Sluigi __clear_bit(grp->index, &q->bitmaps[EB]); 781204591Sluigi __clear_bit(grp->index, &q->bitmaps[IB]); 782204591Sluigi grp->S = roundedS; 783204591Sluigi grp->F = roundedS + (2ULL << grp->slot_shift); 784204591Sluigi s = qfq_calc_state(q, grp); 785204591Sluigi __set_bit(grp->index, &q->bitmaps[s]); 786204591Sluigi } 787204591Sluigi } 788204591Sluigi qfq_update_eligible(q, q->V); 789204591Sluigi} 790204591Sluigi#endif 791204591Sluigi 792204591Sluigistatic int 793204591Sluigiqfq_new_fsk(struct dn_fsk *f) 794204591Sluigi{ 795204591Sluigi ipdn_bound_var(&f->fs.par[0], 1, 1, QFQ_MAX_WEIGHT, "qfq weight"); 796204591Sluigi ipdn_bound_var(&f->fs.par[1], 1500, 1, 2000, "qfq maxlen"); 797204591Sluigi ND("weight %d len %d\n", f->fs.par[0], f->fs.par[1]); 798204591Sluigi return 0; 799204591Sluigi} 800204591Sluigi 801204591Sluigi/* 802204591Sluigi * initialize a new scheduler instance 803204591Sluigi */ 804204591Sluigistatic int 805204591Sluigiqfq_new_sched(struct dn_sch_inst *si) 806204591Sluigi{ 807204591Sluigi struct qfq_sched *q = (struct qfq_sched *)(si + 1); 808204591Sluigi struct qfq_group *grp; 809204591Sluigi int i; 810204591Sluigi 811204591Sluigi for (i = 0; i <= QFQ_MAX_INDEX; i++) { 812204591Sluigi grp = &q->groups[i]; 813204591Sluigi grp->index = i; 814204591Sluigi grp->slot_shift = QFQ_MTU_SHIFT + FRAC_BITS - 815204591Sluigi (QFQ_MAX_INDEX - i); 816204591Sluigi } 817204591Sluigi return 0; 818204591Sluigi} 819204591Sluigi 820204591Sluigi/* 821204591Sluigi * QFQ scheduler descriptor 822204591Sluigi */ 823204591Sluigistatic struct dn_alg qfq_desc = { 824204591Sluigi _SI( .type = ) DN_SCHED_QFQ, 825204591Sluigi _SI( .name = ) "QFQ", 826204591Sluigi _SI( .flags = ) DN_MULTIQUEUE, 827204591Sluigi 828204591Sluigi _SI( .schk_datalen = ) 0, 829204591Sluigi _SI( .si_datalen = ) sizeof(struct qfq_sched), 830204591Sluigi _SI( .q_datalen = ) sizeof(struct qfq_class) - sizeof(struct dn_queue), 831204591Sluigi 832204591Sluigi _SI( .enqueue = ) qfq_enqueue, 833204591Sluigi _SI( .dequeue = ) qfq_dequeue, 834204591Sluigi 835204591Sluigi _SI( .config = ) NULL, 836204591Sluigi _SI( .destroy = ) NULL, 837204591Sluigi _SI( .new_sched = ) qfq_new_sched, 838204591Sluigi _SI( .free_sched = ) NULL, 839204591Sluigi _SI( .new_fsk = ) qfq_new_fsk, 840204591Sluigi _SI( .free_fsk = ) NULL, 841204591Sluigi _SI( .new_queue = ) qfq_new_queue, 842204591Sluigi _SI( .free_queue = ) qfq_free_queue, 843300779Struckman#ifdef NEW_AQM 844300779Struckman _SI( .getconfig = ) NULL, 845300779Struckman#endif 846204591Sluigi}; 847204591Sluigi 848204591SluigiDECLARE_DNSCHED_MODULE(dn_qfq, &qfq_desc); 849204591Sluigi 850204591Sluigi#ifdef QFQ_DEBUG 851204591Sluigistatic void 852204591Sluigidump_groups(struct qfq_sched *q, uint32_t mask) 853204591Sluigi{ 854204591Sluigi int i, j; 855204591Sluigi 856204591Sluigi for (i = 0; i < QFQ_MAX_INDEX + 1; i++) { 857204591Sluigi struct qfq_group *g = &q->groups[i]; 858204591Sluigi 859204591Sluigi if (0 == (mask & (1<<i))) 860204591Sluigi continue; 861204591Sluigi for (j = 0; j < QFQ_MAX_SLOTS; j++) { 862204591Sluigi if (g->slots[j]) 863204591Sluigi D(" bucket %d %p", j, g->slots[j]); 864204591Sluigi } 865294858Sluigi D("full_slots 0x%llx", (_P64)g->full_slots); 866204591Sluigi D(" %2d S 0x%20llx F 0x%llx %c", i, 867294858Sluigi (_P64)g->S, (_P64)g->F, 868204591Sluigi mask & (1<<i) ? '1' : '0'); 869204591Sluigi } 870204591Sluigi} 871204591Sluigi 872204591Sluigistatic void 873204591Sluigidump_sched(struct qfq_sched *q, const char *msg) 874204591Sluigi{ 875204591Sluigi D("--- in %s: ---", msg); 876294858Sluigi D("loops %d queued %d V 0x%llx", q->loops, q->queued, (_P64)q->V); 877294858Sluigi D(" ER 0x%08x", (unsigned)q->bitmaps[ER]); 878294858Sluigi D(" EB 0x%08x", (unsigned)q->bitmaps[EB]); 879294858Sluigi D(" IR 0x%08x", (unsigned)q->bitmaps[IR]); 880294858Sluigi D(" IB 0x%08x", (unsigned)q->bitmaps[IB]); 881204591Sluigi dump_groups(q, 0xffffffff); 882204591Sluigi}; 883204591Sluigi#endif /* QFQ_DEBUG */ 884