10  * 1. Redistributions of source code must retain the above copyright
60 static void dump_sched(struct qfq_sched *q, const char *msg);
71  * NOTE: fls() returns 1 for the least significant bit,
80 	for (i = 0; n > 0; n >>= 1, i++)  in fls()
89 	return fls(word) - 1;  in __fls()
99 	return *p & (1<<ix);  in test_bit()
105 	*p |= (1<<ix);  in __set_bit()
111 	*p &= ~(1<<ix);  in __clear_bit()
115 #define test_bit(ix, pData)	((*pData) & (1<<(ix)))
116 #define __set_bit(ix, pData)	(*pData) |= (1<<(ix))
117 #define __clear_bit(ix, pData)	(*pData) &= ~(1<<(ix))
122 #define __clear_bit(ix, pData)	(*pData) &= ~(1<<(ix))
146 Lmax=1<<MTU_SHIFT, w_min = 1 .
153 	inv_w = (1<<FRAC_BITS)/w_i
170  * in the range [1, QFQ_MAX_WEIGHT], we to map each class i to the
185 #define	QFQ_MAX_WEIGHT		(1<<QFQ_MAX_WSHIFT)
189 #define ONE_FP			(1UL << FRAC_BITS)
245 	struct qfq_group groups[QFQ_MAX_INDEX + 1]; /* The groups. */
259 	return ts & ~((1ULL << shift) - 1);  in qfq_round_down()
263 static inline struct qfq_group *qfq_ffs(struct qfq_sched *q,  in qfq_ffs()  argument
266 	int index = ffs(bitmap) - 1; // zero-based  in qfq_ffs()
267 	return &q->groups[index];  in qfq_ffs()
285 	index = __fls(size_map) + 1;	// basically a log_2()  in qfq_calc_index()
286 	index -= !(slot_size - (1ULL << (index + QFQ_MIN_SLOT_SHIFT - 1)));  in qfq_calc_index()
304 	struct qfq_sched *q = (struct qfq_sched *)(_q->_si + 1);  in qfq_new_queue()  local
313 	cl->lmax = _q->fs->fs.par[1];  in qfq_new_queue()
315 		w = 1;  in qfq_new_queue()
316 		D("rounding weight to 1");  in qfq_new_queue()
320 	if (q->wsum + w > QFQ_MAX_WSUM)  in qfq_new_queue()
324 	cl->grp = &q->groups[i];  in qfq_new_queue()
325 	q->wsum += w;  in qfq_new_queue()
326 	q->iwsum = ONE_FP / q->wsum; /* XXX note theory */  in qfq_new_queue()
327 	// XXX cl->S = q->V; ?  in qfq_new_queue()
335 	struct qfq_sched *q = (struct qfq_sched *)(_q->_si + 1);  in qfq_free_queue()  local
338 		q->wsum -= ONE_FP/cl->inv_w;  in qfq_free_queue()
339 		if (q->wsum != 0)  in qfq_free_queue()
340 			q->iwsum = ONE_FP / q->wsum;  in qfq_free_queue()
350 	return bitmap & ~((1UL << from) - 1);  in mask_from()
354  * The state computation relies on ER=0, IR=1, EB=2, IB=3
355  * First compute eligibility comparing grp->S, q->V,
359 qfq_calc_state(struct qfq_sched *q, struct qfq_group *grp)  in qfq_calc_state()  argument
362 	unsigned int state = qfq_gt(grp->S, q->V);  in qfq_calc_state()
363 	unsigned long mask = mask_from(q->bitmaps[ER], grp->index);  in qfq_calc_state()
367 		next = qfq_ffs(q, mask);  in qfq_calc_state()
377  *	q->bitmaps[dst] |= q->bitmaps[src] & mask;
378  *	q->bitmaps[src] &= ~mask;
382 qfq_move_groups(struct qfq_sched *q, unsigned long mask, int src, int dst)  in qfq_move_groups()  argument
384 	q->bitmaps[dst] |= q->bitmaps[src] & mask;  in qfq_move_groups()
385 	q->bitmaps[src] &= ~mask;  in qfq_move_groups()
389 qfq_unblock_groups(struct qfq_sched *q, int index, uint64_t old_finish)  in qfq_unblock_groups()  argument
391 	unsigned long mask = mask_from(q->bitmaps[ER], index + 1);  in qfq_unblock_groups()
395 		next = qfq_ffs(q, mask);  in qfq_unblock_groups()
400 	mask = (1UL << index) - 1;  in qfq_unblock_groups()
401 	qfq_move_groups(q, mask, EB, ER);  in qfq_unblock_groups()
402 	qfq_move_groups(q, mask, IB, IR);  in qfq_unblock_groups()
408 	old_V ^= q->V;
416 qfq_make_eligible(struct qfq_sched *q, uint64_t old_V)  in qfq_make_eligible()  argument
420 	vslot = q->V >> QFQ_MIN_SLOT_SHIFT;  in qfq_make_eligible()
425 		mask = (2ULL << (__fls(vslot ^ old_vslot))) - 1;  in qfq_make_eligible()
426 		qfq_move_groups(q, mask, IR, ER);  in qfq_make_eligible()
427 		qfq_move_groups(q, mask, IB, EB);  in qfq_make_eligible()
474 	i = ffs(grp->full_slots) - 1; // zero-based  in qfq_slot_scan()
493 qfq_slot_rotate(struct qfq_sched *q, struct qfq_group *grp, uint64_t roundedS)  in qfq_slot_rotate()  argument
497 	(void)q;  in qfq_slot_rotate()
503 qfq_update_eligible(struct qfq_sched *q, uint64_t old_V)  in qfq_update_eligible()  argument
507 	ineligible = q->bitmaps[IR] | q->bitmaps[IB];  in qfq_update_eligible()
509 		if (!q->bitmaps[ER]) {  in qfq_update_eligible()
511 			grp = qfq_ffs(q, ineligible);  in qfq_update_eligible()
512 			if (qfq_gt(grp->S, q->V))  in qfq_update_eligible()
513 				q->V = grp->S;  in qfq_update_eligible()
515 		qfq_make_eligible(q, old_V);  in qfq_update_eligible()
523 qfq_update_class(struct qfq_sched *q, struct qfq_group *grp,  in qfq_update_class()  argument
527 	(void)q;  in qfq_update_class()
544 	return 1;  in qfq_update_class()
550 	struct qfq_sched *q = (struct qfq_sched *)(si + 1);  in qfq_dequeue()  local
556 	NO(q->loops++;)  in qfq_dequeue()
557 	if (!q->bitmaps[ER]) {  in qfq_dequeue()
558 		NO(if (q->queued)  in qfq_dequeue()
559 			dump_sched(q, "start dequeue");)  in qfq_dequeue()
563 	grp = qfq_ffs(q, q->bitmaps[ER]);  in qfq_dequeue()
573 	NO(q->queued--;)  in qfq_dequeue()
574 	old_V = q->V;  in qfq_dequeue()
575 	q->V += (uint64_t)m->m_pkthdr.len * q->iwsum;  in qfq_dequeue()
576 	ND("m is %p F 0x%llx V now 0x%llx", m, cl->F, q->V);  in qfq_dequeue()
578 	if (qfq_update_class(q, grp, cl)) {  in qfq_dequeue()
582 			__clear_bit(grp->index, &q->bitmaps[ER]);  in qfq_dequeue()
583 			// grp->S = grp->F + 1; // XXX debugging only  in qfq_dequeue()
593 			__clear_bit(grp->index, &q->bitmaps[ER]);  in qfq_dequeue()
594 			s = qfq_calc_state(q, grp);  in qfq_dequeue()
595 			__set_bit(grp->index, &q->bitmaps[s]);  in qfq_dequeue()
598 		qfq_unblock_groups(q, grp->index, old_F);  in qfq_dequeue()
602 	qfq_update_eligible(q, old_V);  in qfq_dequeue()
603 	NO(if (!q->bitmaps[ER] && q->queued)  in qfq_dequeue()
604 		dump_sched(q, "end dequeue");)  in qfq_dequeue()
623 qfq_update_start(struct qfq_sched *q, struct qfq_class *cl)  in qfq_update_start()  argument
630 	limit = qfq_round_down(q->V, slot_shift) + (1ULL << slot_shift);  in qfq_update_start()
632 	if (!qfq_gt(cl->F, q->V) || qfq_gt(roundedF, limit)) {  in qfq_update_start()
634 		mask = mask_from(q->bitmaps[ER], cl->grp->index);  in qfq_update_start()
636 			struct qfq_group *next = qfq_ffs(q, mask);  in qfq_update_start()
646 		cl->S = q->V;  in qfq_update_start()
655 	struct qfq_sched *q = (struct qfq_sched *)(si + 1);  in qfq_enqueue()  local
661 	NO(q->loops++;)  in qfq_enqueue()
667 			return 1;  in qfq_enqueue()
668 		NO(q->queued++;)  in qfq_enqueue()
672 	/* If reach this point, queue q was idle */  in qfq_enqueue()
674 	qfq_update_start(q, cl); /* adjust start time */  in qfq_enqueue()
692 		qfq_slot_rotate(q, grp, roundedS);  in qfq_enqueue()
694 		__clear_bit(grp->index, &q->bitmaps[IR]);  in qfq_enqueue()
695 		__clear_bit(grp->index, &q->bitmaps[IB]);  in qfq_enqueue()
696 	} else if (!q->bitmaps[ER] && qfq_gt(roundedS, q->V))  in qfq_enqueue()
697 		q->V = roundedS;  in qfq_enqueue()
701 	s = qfq_calc_state(q, grp);  in qfq_enqueue()
702 	__set_bit(grp->index, &q->bitmaps[s]);  in qfq_enqueue()
703 	ND("new state %d 0x%x", s, q->bitmaps[s]);  in qfq_enqueue()
704 	ND("S %llx F %llx V %llx", cl->S, cl->F, q->V);  in qfq_enqueue()
713 qfq_slot_remove(struct qfq_sched *q, struct qfq_group *grp,
745 qfq_deactivate_class(struct qfq_sched *q, struct qfq_class *cl,
748 	struct qfq_group *grp = &q->groups[cl->index];
754 	qfq_slot_remove(q, grp, cl, pprev);
761 		__clear_bit(grp->index, &q->bitmaps[IR]);
762 		__clear_bit(grp->index, &q->bitmaps[EB]);
763 		__clear_bit(grp->index, &q->bitmaps[IB]);
765 		if (test_bit(grp->index, &q->bitmaps[ER]) &&
766 		    !(q->bitmaps[ER] & ~((1UL << grp->index) - 1))) {
767 			mask = q->bitmaps[ER] & ((1UL << grp->index) - 1);
769 				mask = ~((1UL << __fls(mask)) - 1);
772 			qfq_move_groups(q, mask, EB, ER);
773 			qfq_move_groups(q, mask, IB, IR);
775 		__clear_bit(grp->index, &q->bitmaps[ER]);
780 			__clear_bit(grp->index, &q->bitmaps[ER]);
781 			__clear_bit(grp->index, &q->bitmaps[IR]);
782 			__clear_bit(grp->index, &q->bitmaps[EB]);
783 			__clear_bit(grp->index, &q->bitmaps[IB]);
786 			s = qfq_calc_state(q, grp);
787 			__set_bit(grp->index, &q->bitmaps[s]);
790 	qfq_update_eligible(q, q->V);
797 	ipdn_bound_var(&f->fs.par[0], 1, 1, QFQ_MAX_WEIGHT, "qfq weight");  in qfq_new_fsk()
798 	ipdn_bound_var(&f->fs.par[1], 1500, 1, 2000, "qfq maxlen");  in qfq_new_fsk()
799 	ND("weight %d len %d\n", f->fs.par[0], f->fs.par[1]);  in qfq_new_fsk()
809 	struct qfq_sched *q = (struct qfq_sched *)(si + 1);  in qfq_new_sched()  local
814 		grp = &q->groups[i];  in qfq_new_sched()
854 dump_groups(struct qfq_sched *q, uint32_t mask)  in dump_groups()  argument
858 	for (i = 0; i < QFQ_MAX_INDEX + 1; i++) {  in dump_groups()
859 		struct qfq_group *g = &q->groups[i];  in dump_groups()
861 		if (0 == (mask & (1<<i)))  in dump_groups()
870 			mask & (1<<i) ? '1' : '0');  in dump_groups()
875 dump_sched(struct qfq_sched *q, const char *msg)  in dump_sched()  argument
878 	D("loops %d queued %d V 0x%llx", q->loops, q->queued, (_P64)q->V);  in dump_sched()
879 	D("    ER 0x%08x", (unsigned)q->bitmaps[ER]);  in dump_sched()
880 	D("    EB 0x%08x", (unsigned)q->bitmaps[EB]);  in dump_sched()
881 	D("    IR 0x%08x", (unsigned)q->bitmaps[IR]);  in dump_sched()
882 	D("    IB 0x%08x", (unsigned)q->bitmaps[IB]);  in dump_sched()
883 	dump_groups(q, 0xffffffff);  in dump_sched()