xref: /openbmc/linux/net/sched/sch_sfq.c (revision d636fc5d)
1 // SPDX-License-Identifier: GPL-2.0-or-later
2 /*
3  * net/sched/sch_sfq.c	Stochastic Fairness Queueing discipline.
4  *
5  * Authors:	Alexey Kuznetsov, <kuznet@ms2.inr.ac.ru>
6  */
7 
8 #include <linux/module.h>
9 #include <linux/types.h>
10 #include <linux/kernel.h>
11 #include <linux/jiffies.h>
12 #include <linux/string.h>
13 #include <linux/in.h>
14 #include <linux/errno.h>
15 #include <linux/init.h>
16 #include <linux/skbuff.h>
17 #include <linux/siphash.h>
18 #include <linux/slab.h>
19 #include <linux/vmalloc.h>
20 #include <net/netlink.h>
21 #include <net/pkt_sched.h>
22 #include <net/pkt_cls.h>
23 #include <net/red.h>
24 
25 
26 /*	Stochastic Fairness Queuing algorithm.
27 	=======================================
28 
29 	Source:
30 	Paul E. McKenney "Stochastic Fairness Queuing",
31 	IEEE INFOCOMM'90 Proceedings, San Francisco, 1990.
32 
33 	Paul E. McKenney "Stochastic Fairness Queuing",
34 	"Interworking: Research and Experience", v.2, 1991, p.113-131.
35 
36 
37 	See also:
38 	M. Shreedhar and George Varghese "Efficient Fair
39 	Queuing using Deficit Round Robin", Proc. SIGCOMM 95.
40 
41 
42 	This is not the thing that is usually called (W)FQ nowadays.
43 	It does not use any timestamp mechanism, but instead
44 	processes queues in round-robin order.
45 
46 	ADVANTAGE:
47 
48 	- It is very cheap. Both CPU and memory requirements are minimal.
49 
50 	DRAWBACKS:
51 
52 	- "Stochastic" -> It is not 100% fair.
53 	When hash collisions occur, several flows are considered as one.
54 
55 	- "Round-robin" -> It introduces larger delays than virtual clock
56 	based schemes, and should not be used for isolating interactive
57 	traffic	from non-interactive. It means, that this scheduler
58 	should be used as leaf of CBQ or P3, which put interactive traffic
59 	to higher priority band.
60 
61 	We still need true WFQ for top level CSZ, but using WFQ
62 	for the best effort traffic is absolutely pointless:
63 	SFQ is superior for this purpose.
64 
65 	IMPLEMENTATION:
66 	This implementation limits :
67 	- maximal queue length per flow to 127 packets.
68 	- max mtu to 2^18-1;
69 	- max 65408 flows,
70 	- number of hash buckets to 65536.
71 
72 	It is easy to increase these values, but not in flight.  */
73 
74 #define SFQ_MAX_DEPTH		127 /* max number of packets per flow */
75 #define SFQ_DEFAULT_FLOWS	128
76 #define SFQ_MAX_FLOWS		(0x10000 - SFQ_MAX_DEPTH - 1) /* max number of flows */
77 #define SFQ_EMPTY_SLOT		0xffff
78 #define SFQ_DEFAULT_HASH_DIVISOR 1024
79 
80 /* We use 16 bits to store allot, and want to handle packets up to 64K
81  * Scale allot by 8 (1<<3) so that no overflow occurs.
82  */
83 #define SFQ_ALLOT_SHIFT		3
84 #define SFQ_ALLOT_SIZE(X)	DIV_ROUND_UP(X, 1 << SFQ_ALLOT_SHIFT)
85 
86 /* This type should contain at least SFQ_MAX_DEPTH + 1 + SFQ_MAX_FLOWS values */
87 typedef u16 sfq_index;
88 
89 /*
90  * We dont use pointers to save space.
91  * Small indexes [0 ... SFQ_MAX_FLOWS - 1] are 'pointers' to slots[] array
92  * while following values [SFQ_MAX_FLOWS ... SFQ_MAX_FLOWS + SFQ_MAX_DEPTH]
93  * are 'pointers' to dep[] array
94  */
95 struct sfq_head {
96 	sfq_index	next;
97 	sfq_index	prev;
98 };
99 
100 struct sfq_slot {
101 	struct sk_buff	*skblist_next;
102 	struct sk_buff	*skblist_prev;
103 	sfq_index	qlen; /* number of skbs in skblist */
104 	sfq_index	next; /* next slot in sfq RR chain */
105 	struct sfq_head dep; /* anchor in dep[] chains */
106 	unsigned short	hash; /* hash value (index in ht[]) */
107 	short		allot; /* credit for this slot */
108 
109 	unsigned int    backlog;
110 	struct red_vars vars;
111 };
112 
113 struct sfq_sched_data {
114 /* frequently used fields */
115 	int		limit;		/* limit of total number of packets in this qdisc */
116 	unsigned int	divisor;	/* number of slots in hash table */
117 	u8		headdrop;
118 	u8		maxdepth;	/* limit of packets per flow */
119 
120 	siphash_key_t 	perturbation;
121 	u8		cur_depth;	/* depth of longest slot */
122 	u8		flags;
123 	unsigned short  scaled_quantum; /* SFQ_ALLOT_SIZE(quantum) */
124 	struct tcf_proto __rcu *filter_list;
125 	struct tcf_block *block;
126 	sfq_index	*ht;		/* Hash table ('divisor' slots) */
127 	struct sfq_slot	*slots;		/* Flows table ('maxflows' entries) */
128 
129 	struct red_parms *red_parms;
130 	struct tc_sfqred_stats stats;
131 	struct sfq_slot *tail;		/* current slot in round */
132 
133 	struct sfq_head	dep[SFQ_MAX_DEPTH + 1];
134 					/* Linked lists of slots, indexed by depth
135 					 * dep[0] : list of unused flows
136 					 * dep[1] : list of flows with 1 packet
137 					 * dep[X] : list of flows with X packets
138 					 */
139 
140 	unsigned int	maxflows;	/* number of flows in flows array */
141 	int		perturb_period;
142 	unsigned int	quantum;	/* Allotment per round: MUST BE >= MTU */
143 	struct timer_list perturb_timer;
144 	struct Qdisc	*sch;
145 };
146 
147 /*
148  * sfq_head are either in a sfq_slot or in dep[] array
149  */
sfq_dep_head(struct sfq_sched_data * q,sfq_index val)150 static inline struct sfq_head *sfq_dep_head(struct sfq_sched_data *q, sfq_index val)
151 {
152 	if (val < SFQ_MAX_FLOWS)
153 		return &q->slots[val].dep;
154 	return &q->dep[val - SFQ_MAX_FLOWS];
155 }
156 
sfq_hash(const struct sfq_sched_data * q,const struct sk_buff * skb)157 static unsigned int sfq_hash(const struct sfq_sched_data *q,
158 			     const struct sk_buff *skb)
159 {
160 	return skb_get_hash_perturb(skb, &q->perturbation) & (q->divisor - 1);
161 }
162 
sfq_classify(struct sk_buff * skb,struct Qdisc * sch,int * qerr)163 static unsigned int sfq_classify(struct sk_buff *skb, struct Qdisc *sch,
164 				 int *qerr)
165 {
166 	struct sfq_sched_data *q = qdisc_priv(sch);
167 	struct tcf_result res;
168 	struct tcf_proto *fl;
169 	int result;
170 
171 	if (TC_H_MAJ(skb->priority) == sch->handle &&
172 	    TC_H_MIN(skb->priority) > 0 &&
173 	    TC_H_MIN(skb->priority) <= q->divisor)
174 		return TC_H_MIN(skb->priority);
175 
176 	fl = rcu_dereference_bh(q->filter_list);
177 	if (!fl)
178 		return sfq_hash(q, skb) + 1;
179 
180 	*qerr = NET_XMIT_SUCCESS | __NET_XMIT_BYPASS;
181 	result = tcf_classify(skb, NULL, fl, &res, false);
182 	if (result >= 0) {
183 #ifdef CONFIG_NET_CLS_ACT
184 		switch (result) {
185 		case TC_ACT_STOLEN:
186 		case TC_ACT_QUEUED:
187 		case TC_ACT_TRAP:
188 			*qerr = NET_XMIT_SUCCESS | __NET_XMIT_STOLEN;
189 			fallthrough;
190 		case TC_ACT_SHOT:
191 			return 0;
192 		}
193 #endif
194 		if (TC_H_MIN(res.classid) <= q->divisor)
195 			return TC_H_MIN(res.classid);
196 	}
197 	return 0;
198 }
199 
200 /*
201  * x : slot number [0 .. SFQ_MAX_FLOWS - 1]
202  */
sfq_link(struct sfq_sched_data * q,sfq_index x)203 static inline void sfq_link(struct sfq_sched_data *q, sfq_index x)
204 {
205 	sfq_index p, n;
206 	struct sfq_slot *slot = &q->slots[x];
207 	int qlen = slot->qlen;
208 
209 	p = qlen + SFQ_MAX_FLOWS;
210 	n = q->dep[qlen].next;
211 
212 	slot->dep.next = n;
213 	slot->dep.prev = p;
214 
215 	q->dep[qlen].next = x;		/* sfq_dep_head(q, p)->next = x */
216 	sfq_dep_head(q, n)->prev = x;
217 }
218 
219 #define sfq_unlink(q, x, n, p)			\
220 	do {					\
221 		n = q->slots[x].dep.next;	\
222 		p = q->slots[x].dep.prev;	\
223 		sfq_dep_head(q, p)->next = n;	\
224 		sfq_dep_head(q, n)->prev = p;	\
225 	} while (0)
226 
227 
sfq_dec(struct sfq_sched_data * q,sfq_index x)228 static inline void sfq_dec(struct sfq_sched_data *q, sfq_index x)
229 {
230 	sfq_index p, n;
231 	int d;
232 
233 	sfq_unlink(q, x, n, p);
234 
235 	d = q->slots[x].qlen--;
236 	if (n == p && q->cur_depth == d)
237 		q->cur_depth--;
238 	sfq_link(q, x);
239 }
240 
sfq_inc(struct sfq_sched_data * q,sfq_index x)241 static inline void sfq_inc(struct sfq_sched_data *q, sfq_index x)
242 {
243 	sfq_index p, n;
244 	int d;
245 
246 	sfq_unlink(q, x, n, p);
247 
248 	d = ++q->slots[x].qlen;
249 	if (q->cur_depth < d)
250 		q->cur_depth = d;
251 	sfq_link(q, x);
252 }
253 
254 /* helper functions : might be changed when/if skb use a standard list_head */
255 
256 /* remove one skb from tail of slot queue */
slot_dequeue_tail(struct sfq_slot * slot)257 static inline struct sk_buff *slot_dequeue_tail(struct sfq_slot *slot)
258 {
259 	struct sk_buff *skb = slot->skblist_prev;
260 
261 	slot->skblist_prev = skb->prev;
262 	skb->prev->next = (struct sk_buff *)slot;
263 	skb->next = skb->prev = NULL;
264 	return skb;
265 }
266 
267 /* remove one skb from head of slot queue */
slot_dequeue_head(struct sfq_slot * slot)268 static inline struct sk_buff *slot_dequeue_head(struct sfq_slot *slot)
269 {
270 	struct sk_buff *skb = slot->skblist_next;
271 
272 	slot->skblist_next = skb->next;
273 	skb->next->prev = (struct sk_buff *)slot;
274 	skb->next = skb->prev = NULL;
275 	return skb;
276 }
277 
slot_queue_init(struct sfq_slot * slot)278 static inline void slot_queue_init(struct sfq_slot *slot)
279 {
280 	memset(slot, 0, sizeof(*slot));
281 	slot->skblist_prev = slot->skblist_next = (struct sk_buff *)slot;
282 }
283 
284 /* add skb to slot queue (tail add) */
slot_queue_add(struct sfq_slot * slot,struct sk_buff * skb)285 static inline void slot_queue_add(struct sfq_slot *slot, struct sk_buff *skb)
286 {
287 	skb->prev = slot->skblist_prev;
288 	skb->next = (struct sk_buff *)slot;
289 	slot->skblist_prev->next = skb;
290 	slot->skblist_prev = skb;
291 }
292 
sfq_drop(struct Qdisc * sch,struct sk_buff ** to_free)293 static unsigned int sfq_drop(struct Qdisc *sch, struct sk_buff **to_free)
294 {
295 	struct sfq_sched_data *q = qdisc_priv(sch);
296 	sfq_index x, d = q->cur_depth;
297 	struct sk_buff *skb;
298 	unsigned int len;
299 	struct sfq_slot *slot;
300 
301 	/* Queue is full! Find the longest slot and drop tail packet from it */
302 	if (d > 1) {
303 		x = q->dep[d].next;
304 		slot = &q->slots[x];
305 drop:
306 		skb = q->headdrop ? slot_dequeue_head(slot) : slot_dequeue_tail(slot);
307 		len = qdisc_pkt_len(skb);
308 		slot->backlog -= len;
309 		sfq_dec(q, x);
310 		sch->q.qlen--;
311 		qdisc_qstats_backlog_dec(sch, skb);
312 		qdisc_drop(skb, sch, to_free);
313 		return len;
314 	}
315 
316 	if (d == 1) {
317 		/* It is difficult to believe, but ALL THE SLOTS HAVE LENGTH 1. */
318 		x = q->tail->next;
319 		slot = &q->slots[x];
320 		q->tail->next = slot->next;
321 		q->ht[slot->hash] = SFQ_EMPTY_SLOT;
322 		goto drop;
323 	}
324 
325 	return 0;
326 }
327 
328 /* Is ECN parameter configured */
sfq_prob_mark(const struct sfq_sched_data * q)329 static int sfq_prob_mark(const struct sfq_sched_data *q)
330 {
331 	return q->flags & TC_RED_ECN;
332 }
333 
334 /* Should packets over max threshold just be marked */
sfq_hard_mark(const struct sfq_sched_data * q)335 static int sfq_hard_mark(const struct sfq_sched_data *q)
336 {
337 	return (q->flags & (TC_RED_ECN | TC_RED_HARDDROP)) == TC_RED_ECN;
338 }
339 
sfq_headdrop(const struct sfq_sched_data * q)340 static int sfq_headdrop(const struct sfq_sched_data *q)
341 {
342 	return q->headdrop;
343 }
344 
345 static int
sfq_enqueue(struct sk_buff * skb,struct Qdisc * sch,struct sk_buff ** to_free)346 sfq_enqueue(struct sk_buff *skb, struct Qdisc *sch, struct sk_buff **to_free)
347 {
348 	struct sfq_sched_data *q = qdisc_priv(sch);
349 	unsigned int hash, dropped;
350 	sfq_index x, qlen;
351 	struct sfq_slot *slot;
352 	int ret;
353 	struct sk_buff *head;
354 	int delta;
355 
356 	hash = sfq_classify(skb, sch, &ret);
357 	if (hash == 0) {
358 		if (ret & __NET_XMIT_BYPASS)
359 			qdisc_qstats_drop(sch);
360 		__qdisc_drop(skb, to_free);
361 		return ret;
362 	}
363 	hash--;
364 
365 	x = q->ht[hash];
366 	slot = &q->slots[x];
367 	if (x == SFQ_EMPTY_SLOT) {
368 		x = q->dep[0].next; /* get a free slot */
369 		if (x >= SFQ_MAX_FLOWS)
370 			return qdisc_drop(skb, sch, to_free);
371 		q->ht[hash] = x;
372 		slot = &q->slots[x];
373 		slot->hash = hash;
374 		slot->backlog = 0; /* should already be 0 anyway... */
375 		red_set_vars(&slot->vars);
376 		goto enqueue;
377 	}
378 	if (q->red_parms) {
379 		slot->vars.qavg = red_calc_qavg_no_idle_time(q->red_parms,
380 							&slot->vars,
381 							slot->backlog);
382 		switch (red_action(q->red_parms,
383 				   &slot->vars,
384 				   slot->vars.qavg)) {
385 		case RED_DONT_MARK:
386 			break;
387 
388 		case RED_PROB_MARK:
389 			qdisc_qstats_overlimit(sch);
390 			if (sfq_prob_mark(q)) {
391 				/* We know we have at least one packet in queue */
392 				if (sfq_headdrop(q) &&
393 				    INET_ECN_set_ce(slot->skblist_next)) {
394 					q->stats.prob_mark_head++;
395 					break;
396 				}
397 				if (INET_ECN_set_ce(skb)) {
398 					q->stats.prob_mark++;
399 					break;
400 				}
401 			}
402 			q->stats.prob_drop++;
403 			goto congestion_drop;
404 
405 		case RED_HARD_MARK:
406 			qdisc_qstats_overlimit(sch);
407 			if (sfq_hard_mark(q)) {
408 				/* We know we have at least one packet in queue */
409 				if (sfq_headdrop(q) &&
410 				    INET_ECN_set_ce(slot->skblist_next)) {
411 					q->stats.forced_mark_head++;
412 					break;
413 				}
414 				if (INET_ECN_set_ce(skb)) {
415 					q->stats.forced_mark++;
416 					break;
417 				}
418 			}
419 			q->stats.forced_drop++;
420 			goto congestion_drop;
421 		}
422 	}
423 
424 	if (slot->qlen >= q->maxdepth) {
425 congestion_drop:
426 		if (!sfq_headdrop(q))
427 			return qdisc_drop(skb, sch, to_free);
428 
429 		/* We know we have at least one packet in queue */
430 		head = slot_dequeue_head(slot);
431 		delta = qdisc_pkt_len(head) - qdisc_pkt_len(skb);
432 		sch->qstats.backlog -= delta;
433 		slot->backlog -= delta;
434 		qdisc_drop(head, sch, to_free);
435 
436 		slot_queue_add(slot, skb);
437 		qdisc_tree_reduce_backlog(sch, 0, delta);
438 		return NET_XMIT_CN;
439 	}
440 
441 enqueue:
442 	qdisc_qstats_backlog_inc(sch, skb);
443 	slot->backlog += qdisc_pkt_len(skb);
444 	slot_queue_add(slot, skb);
445 	sfq_inc(q, x);
446 	if (slot->qlen == 1) {		/* The flow is new */
447 		if (q->tail == NULL) {	/* It is the first flow */
448 			slot->next = x;
449 		} else {
450 			slot->next = q->tail->next;
451 			q->tail->next = x;
452 		}
453 		/* We put this flow at the end of our flow list.
454 		 * This might sound unfair for a new flow to wait after old ones,
455 		 * but we could endup servicing new flows only, and freeze old ones.
456 		 */
457 		q->tail = slot;
458 		/* We could use a bigger initial quantum for new flows */
459 		slot->allot = q->scaled_quantum;
460 	}
461 	if (++sch->q.qlen <= q->limit)
462 		return NET_XMIT_SUCCESS;
463 
464 	qlen = slot->qlen;
465 	dropped = sfq_drop(sch, to_free);
466 	/* Return Congestion Notification only if we dropped a packet
467 	 * from this flow.
468 	 */
469 	if (qlen != slot->qlen) {
470 		qdisc_tree_reduce_backlog(sch, 0, dropped - qdisc_pkt_len(skb));
471 		return NET_XMIT_CN;
472 	}
473 
474 	/* As we dropped a packet, better let upper stack know this */
475 	qdisc_tree_reduce_backlog(sch, 1, dropped);
476 	return NET_XMIT_SUCCESS;
477 }
478 
479 static struct sk_buff *
sfq_dequeue(struct Qdisc * sch)480 sfq_dequeue(struct Qdisc *sch)
481 {
482 	struct sfq_sched_data *q = qdisc_priv(sch);
483 	struct sk_buff *skb;
484 	sfq_index a, next_a;
485 	struct sfq_slot *slot;
486 
487 	/* No active slots */
488 	if (q->tail == NULL)
489 		return NULL;
490 
491 next_slot:
492 	a = q->tail->next;
493 	slot = &q->slots[a];
494 	if (slot->allot <= 0) {
495 		q->tail = slot;
496 		slot->allot += q->scaled_quantum;
497 		goto next_slot;
498 	}
499 	skb = slot_dequeue_head(slot);
500 	sfq_dec(q, a);
501 	qdisc_bstats_update(sch, skb);
502 	sch->q.qlen--;
503 	qdisc_qstats_backlog_dec(sch, skb);
504 	slot->backlog -= qdisc_pkt_len(skb);
505 	/* Is the slot empty? */
506 	if (slot->qlen == 0) {
507 		q->ht[slot->hash] = SFQ_EMPTY_SLOT;
508 		next_a = slot->next;
509 		if (a == next_a) {
510 			q->tail = NULL; /* no more active slots */
511 			return skb;
512 		}
513 		q->tail->next = next_a;
514 	} else {
515 		slot->allot -= SFQ_ALLOT_SIZE(qdisc_pkt_len(skb));
516 	}
517 	return skb;
518 }
519 
520 static void
sfq_reset(struct Qdisc * sch)521 sfq_reset(struct Qdisc *sch)
522 {
523 	struct sk_buff *skb;
524 
525 	while ((skb = sfq_dequeue(sch)) != NULL)
526 		rtnl_kfree_skbs(skb, skb);
527 }
528 
529 /*
530  * When q->perturbation is changed, we rehash all queued skbs
531  * to avoid OOO (Out Of Order) effects.
532  * We dont use sfq_dequeue()/sfq_enqueue() because we dont want to change
533  * counters.
534  */
sfq_rehash(struct Qdisc * sch)535 static void sfq_rehash(struct Qdisc *sch)
536 {
537 	struct sfq_sched_data *q = qdisc_priv(sch);
538 	struct sk_buff *skb;
539 	int i;
540 	struct sfq_slot *slot;
541 	struct sk_buff_head list;
542 	int dropped = 0;
543 	unsigned int drop_len = 0;
544 
545 	__skb_queue_head_init(&list);
546 
547 	for (i = 0; i < q->maxflows; i++) {
548 		slot = &q->slots[i];
549 		if (!slot->qlen)
550 			continue;
551 		while (slot->qlen) {
552 			skb = slot_dequeue_head(slot);
553 			sfq_dec(q, i);
554 			__skb_queue_tail(&list, skb);
555 		}
556 		slot->backlog = 0;
557 		red_set_vars(&slot->vars);
558 		q->ht[slot->hash] = SFQ_EMPTY_SLOT;
559 	}
560 	q->tail = NULL;
561 
562 	while ((skb = __skb_dequeue(&list)) != NULL) {
563 		unsigned int hash = sfq_hash(q, skb);
564 		sfq_index x = q->ht[hash];
565 
566 		slot = &q->slots[x];
567 		if (x == SFQ_EMPTY_SLOT) {
568 			x = q->dep[0].next; /* get a free slot */
569 			if (x >= SFQ_MAX_FLOWS) {
570 drop:
571 				qdisc_qstats_backlog_dec(sch, skb);
572 				drop_len += qdisc_pkt_len(skb);
573 				kfree_skb(skb);
574 				dropped++;
575 				continue;
576 			}
577 			q->ht[hash] = x;
578 			slot = &q->slots[x];
579 			slot->hash = hash;
580 		}
581 		if (slot->qlen >= q->maxdepth)
582 			goto drop;
583 		slot_queue_add(slot, skb);
584 		if (q->red_parms)
585 			slot->vars.qavg = red_calc_qavg(q->red_parms,
586 							&slot->vars,
587 							slot->backlog);
588 		slot->backlog += qdisc_pkt_len(skb);
589 		sfq_inc(q, x);
590 		if (slot->qlen == 1) {		/* The flow is new */
591 			if (q->tail == NULL) {	/* It is the first flow */
592 				slot->next = x;
593 			} else {
594 				slot->next = q->tail->next;
595 				q->tail->next = x;
596 			}
597 			q->tail = slot;
598 			slot->allot = q->scaled_quantum;
599 		}
600 	}
601 	sch->q.qlen -= dropped;
602 	qdisc_tree_reduce_backlog(sch, dropped, drop_len);
603 }
604 
sfq_perturbation(struct timer_list * t)605 static void sfq_perturbation(struct timer_list *t)
606 {
607 	struct sfq_sched_data *q = from_timer(q, t, perturb_timer);
608 	struct Qdisc *sch = q->sch;
609 	spinlock_t *root_lock;
610 	siphash_key_t nkey;
611 
612 	get_random_bytes(&nkey, sizeof(nkey));
613 	rcu_read_lock();
614 	root_lock = qdisc_lock(qdisc_root_sleeping(sch));
615 	spin_lock(root_lock);
616 	q->perturbation = nkey;
617 	if (!q->filter_list && q->tail)
618 		sfq_rehash(sch);
619 	spin_unlock(root_lock);
620 
621 	if (q->perturb_period)
622 		mod_timer(&q->perturb_timer, jiffies + q->perturb_period);
623 	rcu_read_unlock();
624 }
625 
sfq_change(struct Qdisc * sch,struct nlattr * opt)626 static int sfq_change(struct Qdisc *sch, struct nlattr *opt)
627 {
628 	struct sfq_sched_data *q = qdisc_priv(sch);
629 	struct tc_sfq_qopt *ctl = nla_data(opt);
630 	struct tc_sfq_qopt_v1 *ctl_v1 = NULL;
631 	unsigned int qlen, dropped = 0;
632 	struct red_parms *p = NULL;
633 	struct sk_buff *to_free = NULL;
634 	struct sk_buff *tail = NULL;
635 
636 	if (opt->nla_len < nla_attr_size(sizeof(*ctl)))
637 		return -EINVAL;
638 	if (opt->nla_len >= nla_attr_size(sizeof(*ctl_v1)))
639 		ctl_v1 = nla_data(opt);
640 	if (ctl->divisor &&
641 	    (!is_power_of_2(ctl->divisor) || ctl->divisor > 65536))
642 		return -EINVAL;
643 
644 	/* slot->allot is a short, make sure quantum is not too big. */
645 	if (ctl->quantum) {
646 		unsigned int scaled = SFQ_ALLOT_SIZE(ctl->quantum);
647 
648 		if (scaled <= 0 || scaled > SHRT_MAX)
649 			return -EINVAL;
650 	}
651 
652 	if (ctl_v1 && !red_check_params(ctl_v1->qth_min, ctl_v1->qth_max,
653 					ctl_v1->Wlog, ctl_v1->Scell_log, NULL))
654 		return -EINVAL;
655 	if (ctl_v1 && ctl_v1->qth_min) {
656 		p = kmalloc(sizeof(*p), GFP_KERNEL);
657 		if (!p)
658 			return -ENOMEM;
659 	}
660 	sch_tree_lock(sch);
661 	if (ctl->quantum) {
662 		q->quantum = ctl->quantum;
663 		q->scaled_quantum = SFQ_ALLOT_SIZE(q->quantum);
664 	}
665 	q->perturb_period = ctl->perturb_period * HZ;
666 	if (ctl->flows)
667 		q->maxflows = min_t(u32, ctl->flows, SFQ_MAX_FLOWS);
668 	if (ctl->divisor) {
669 		q->divisor = ctl->divisor;
670 		q->maxflows = min_t(u32, q->maxflows, q->divisor);
671 	}
672 	if (ctl_v1) {
673 		if (ctl_v1->depth)
674 			q->maxdepth = min_t(u32, ctl_v1->depth, SFQ_MAX_DEPTH);
675 		if (p) {
676 			swap(q->red_parms, p);
677 			red_set_parms(q->red_parms,
678 				      ctl_v1->qth_min, ctl_v1->qth_max,
679 				      ctl_v1->Wlog,
680 				      ctl_v1->Plog, ctl_v1->Scell_log,
681 				      NULL,
682 				      ctl_v1->max_P);
683 		}
684 		q->flags = ctl_v1->flags;
685 		q->headdrop = ctl_v1->headdrop;
686 	}
687 	if (ctl->limit) {
688 		q->limit = min_t(u32, ctl->limit, q->maxdepth * q->maxflows);
689 		q->maxflows = min_t(u32, q->maxflows, q->limit);
690 	}
691 
692 	qlen = sch->q.qlen;
693 	while (sch->q.qlen > q->limit) {
694 		dropped += sfq_drop(sch, &to_free);
695 		if (!tail)
696 			tail = to_free;
697 	}
698 
699 	rtnl_kfree_skbs(to_free, tail);
700 	qdisc_tree_reduce_backlog(sch, qlen - sch->q.qlen, dropped);
701 
702 	del_timer(&q->perturb_timer);
703 	if (q->perturb_period) {
704 		mod_timer(&q->perturb_timer, jiffies + q->perturb_period);
705 		get_random_bytes(&q->perturbation, sizeof(q->perturbation));
706 	}
707 	sch_tree_unlock(sch);
708 	kfree(p);
709 	return 0;
710 }
711 
sfq_alloc(size_t sz)712 static void *sfq_alloc(size_t sz)
713 {
714 	return  kvmalloc(sz, GFP_KERNEL);
715 }
716 
sfq_free(void * addr)717 static void sfq_free(void *addr)
718 {
719 	kvfree(addr);
720 }
721 
sfq_destroy(struct Qdisc * sch)722 static void sfq_destroy(struct Qdisc *sch)
723 {
724 	struct sfq_sched_data *q = qdisc_priv(sch);
725 
726 	tcf_block_put(q->block);
727 	q->perturb_period = 0;
728 	del_timer_sync(&q->perturb_timer);
729 	sfq_free(q->ht);
730 	sfq_free(q->slots);
731 	kfree(q->red_parms);
732 }
733 
sfq_init(struct Qdisc * sch,struct nlattr * opt,struct netlink_ext_ack * extack)734 static int sfq_init(struct Qdisc *sch, struct nlattr *opt,
735 		    struct netlink_ext_ack *extack)
736 {
737 	struct sfq_sched_data *q = qdisc_priv(sch);
738 	int i;
739 	int err;
740 
741 	q->sch = sch;
742 	timer_setup(&q->perturb_timer, sfq_perturbation, TIMER_DEFERRABLE);
743 
744 	err = tcf_block_get(&q->block, &q->filter_list, sch, extack);
745 	if (err)
746 		return err;
747 
748 	for (i = 0; i < SFQ_MAX_DEPTH + 1; i++) {
749 		q->dep[i].next = i + SFQ_MAX_FLOWS;
750 		q->dep[i].prev = i + SFQ_MAX_FLOWS;
751 	}
752 
753 	q->limit = SFQ_MAX_DEPTH;
754 	q->maxdepth = SFQ_MAX_DEPTH;
755 	q->cur_depth = 0;
756 	q->tail = NULL;
757 	q->divisor = SFQ_DEFAULT_HASH_DIVISOR;
758 	q->maxflows = SFQ_DEFAULT_FLOWS;
759 	q->quantum = psched_mtu(qdisc_dev(sch));
760 	q->scaled_quantum = SFQ_ALLOT_SIZE(q->quantum);
761 	q->perturb_period = 0;
762 	get_random_bytes(&q->perturbation, sizeof(q->perturbation));
763 
764 	if (opt) {
765 		int err = sfq_change(sch, opt);
766 		if (err)
767 			return err;
768 	}
769 
770 	q->ht = sfq_alloc(sizeof(q->ht[0]) * q->divisor);
771 	q->slots = sfq_alloc(sizeof(q->slots[0]) * q->maxflows);
772 	if (!q->ht || !q->slots) {
773 		/* Note: sfq_destroy() will be called by our caller */
774 		return -ENOMEM;
775 	}
776 
777 	for (i = 0; i < q->divisor; i++)
778 		q->ht[i] = SFQ_EMPTY_SLOT;
779 
780 	for (i = 0; i < q->maxflows; i++) {
781 		slot_queue_init(&q->slots[i]);
782 		sfq_link(q, i);
783 	}
784 	if (q->limit >= 1)
785 		sch->flags |= TCQ_F_CAN_BYPASS;
786 	else
787 		sch->flags &= ~TCQ_F_CAN_BYPASS;
788 	return 0;
789 }
790 
sfq_dump(struct Qdisc * sch,struct sk_buff * skb)791 static int sfq_dump(struct Qdisc *sch, struct sk_buff *skb)
792 {
793 	struct sfq_sched_data *q = qdisc_priv(sch);
794 	unsigned char *b = skb_tail_pointer(skb);
795 	struct tc_sfq_qopt_v1 opt;
796 	struct red_parms *p = q->red_parms;
797 
798 	memset(&opt, 0, sizeof(opt));
799 	opt.v0.quantum	= q->quantum;
800 	opt.v0.perturb_period = q->perturb_period / HZ;
801 	opt.v0.limit	= q->limit;
802 	opt.v0.divisor	= q->divisor;
803 	opt.v0.flows	= q->maxflows;
804 	opt.depth	= q->maxdepth;
805 	opt.headdrop	= q->headdrop;
806 
807 	if (p) {
808 		opt.qth_min	= p->qth_min >> p->Wlog;
809 		opt.qth_max	= p->qth_max >> p->Wlog;
810 		opt.Wlog	= p->Wlog;
811 		opt.Plog	= p->Plog;
812 		opt.Scell_log	= p->Scell_log;
813 		opt.max_P	= p->max_P;
814 	}
815 	memcpy(&opt.stats, &q->stats, sizeof(opt.stats));
816 	opt.flags	= q->flags;
817 
818 	if (nla_put(skb, TCA_OPTIONS, sizeof(opt), &opt))
819 		goto nla_put_failure;
820 
821 	return skb->len;
822 
823 nla_put_failure:
824 	nlmsg_trim(skb, b);
825 	return -1;
826 }
827 
sfq_leaf(struct Qdisc * sch,unsigned long arg)828 static struct Qdisc *sfq_leaf(struct Qdisc *sch, unsigned long arg)
829 {
830 	return NULL;
831 }
832 
sfq_find(struct Qdisc * sch,u32 classid)833 static unsigned long sfq_find(struct Qdisc *sch, u32 classid)
834 {
835 	return 0;
836 }
837 
sfq_bind(struct Qdisc * sch,unsigned long parent,u32 classid)838 static unsigned long sfq_bind(struct Qdisc *sch, unsigned long parent,
839 			      u32 classid)
840 {
841 	return 0;
842 }
843 
sfq_unbind(struct Qdisc * q,unsigned long cl)844 static void sfq_unbind(struct Qdisc *q, unsigned long cl)
845 {
846 }
847 
sfq_tcf_block(struct Qdisc * sch,unsigned long cl,struct netlink_ext_ack * extack)848 static struct tcf_block *sfq_tcf_block(struct Qdisc *sch, unsigned long cl,
849 				       struct netlink_ext_ack *extack)
850 {
851 	struct sfq_sched_data *q = qdisc_priv(sch);
852 
853 	if (cl)
854 		return NULL;
855 	return q->block;
856 }
857 
sfq_dump_class(struct Qdisc * sch,unsigned long cl,struct sk_buff * skb,struct tcmsg * tcm)858 static int sfq_dump_class(struct Qdisc *sch, unsigned long cl,
859 			  struct sk_buff *skb, struct tcmsg *tcm)
860 {
861 	tcm->tcm_handle |= TC_H_MIN(cl);
862 	return 0;
863 }
864 
sfq_dump_class_stats(struct Qdisc * sch,unsigned long cl,struct gnet_dump * d)865 static int sfq_dump_class_stats(struct Qdisc *sch, unsigned long cl,
866 				struct gnet_dump *d)
867 {
868 	struct sfq_sched_data *q = qdisc_priv(sch);
869 	sfq_index idx = q->ht[cl - 1];
870 	struct gnet_stats_queue qs = { 0 };
871 	struct tc_sfq_xstats xstats = { 0 };
872 
873 	if (idx != SFQ_EMPTY_SLOT) {
874 		const struct sfq_slot *slot = &q->slots[idx];
875 
876 		xstats.allot = slot->allot << SFQ_ALLOT_SHIFT;
877 		qs.qlen = slot->qlen;
878 		qs.backlog = slot->backlog;
879 	}
880 	if (gnet_stats_copy_queue(d, NULL, &qs, qs.qlen) < 0)
881 		return -1;
882 	return gnet_stats_copy_app(d, &xstats, sizeof(xstats));
883 }
884 
sfq_walk(struct Qdisc * sch,struct qdisc_walker * arg)885 static void sfq_walk(struct Qdisc *sch, struct qdisc_walker *arg)
886 {
887 	struct sfq_sched_data *q = qdisc_priv(sch);
888 	unsigned int i;
889 
890 	if (arg->stop)
891 		return;
892 
893 	for (i = 0; i < q->divisor; i++) {
894 		if (q->ht[i] == SFQ_EMPTY_SLOT) {
895 			arg->count++;
896 			continue;
897 		}
898 		if (!tc_qdisc_stats_dump(sch, i + 1, arg))
899 			break;
900 	}
901 }
902 
903 static const struct Qdisc_class_ops sfq_class_ops = {
904 	.leaf		=	sfq_leaf,
905 	.find		=	sfq_find,
906 	.tcf_block	=	sfq_tcf_block,
907 	.bind_tcf	=	sfq_bind,
908 	.unbind_tcf	=	sfq_unbind,
909 	.dump		=	sfq_dump_class,
910 	.dump_stats	=	sfq_dump_class_stats,
911 	.walk		=	sfq_walk,
912 };
913 
914 static struct Qdisc_ops sfq_qdisc_ops __read_mostly = {
915 	.cl_ops		=	&sfq_class_ops,
916 	.id		=	"sfq",
917 	.priv_size	=	sizeof(struct sfq_sched_data),
918 	.enqueue	=	sfq_enqueue,
919 	.dequeue	=	sfq_dequeue,
920 	.peek		=	qdisc_peek_dequeued,
921 	.init		=	sfq_init,
922 	.reset		=	sfq_reset,
923 	.destroy	=	sfq_destroy,
924 	.change		=	NULL,
925 	.dump		=	sfq_dump,
926 	.owner		=	THIS_MODULE,
927 };
928 
sfq_module_init(void)929 static int __init sfq_module_init(void)
930 {
931 	return register_qdisc(&sfq_qdisc_ops);
932 }
sfq_module_exit(void)933 static void __exit sfq_module_exit(void)
934 {
935 	unregister_qdisc(&sfq_qdisc_ops);
936 }
937 module_init(sfq_module_init)
938 module_exit(sfq_module_exit)
939 MODULE_LICENSE("GPL");
940