xref: /linux/net/sched/sch_fq.c (revision c4f796c4f16ba375b43c608d6bd0f72e20168312)
1 // SPDX-License-Identifier: GPL-2.0-or-later
2 /*
3  * net/sched/sch_fq.c Fair Queue Packet Scheduler (per flow pacing)
4  *
5  *  Copyright (C) 2013-2023 Eric Dumazet <edumazet@google.com>
6  *
7  *  Meant to be mostly used for locally generated traffic :
8  *  Fast classification depends on skb->sk being set before reaching us.
9  *  If not, (router workload), we use rxhash as fallback, with 32 bits wide hash.
10  *  All packets belonging to a socket are considered as a 'flow'.
11  *
12  *  Flows are dynamically allocated and stored in a hash table of RB trees
13  *  They are also part of one Round Robin 'queues' (new or old flows)
14  *
15  *  Burst avoidance (aka pacing) capability :
16  *
17  *  Transport (eg TCP) can set in sk->sk_pacing_rate a rate, enqueue a
18  *  bunch of packets, and this packet scheduler adds delay between
19  *  packets to respect rate limitation.
20  *
21  *  enqueue() :
22  *   - lookup one RB tree (out of 1024 or more) to find the flow.
23  *     If non existent flow, create it, add it to the tree.
24  *     Add skb to the per flow list of skb (fifo).
25  *   - Use a special fifo for high prio packets
26  *
27  *  dequeue() : serves flows in Round Robin
28  *  Note : When a flow becomes empty, we do not immediately remove it from
29  *  rb trees, for performance reasons (its expected to send additional packets,
30  *  or SLAB cache will reuse socket for another flow)
31  */
32 
33 #include <linux/module.h>
34 #include <linux/types.h>
35 #include <linux/kernel.h>
36 #include <linux/jiffies.h>
37 #include <linux/string.h>
38 #include <linux/in.h>
39 #include <linux/errno.h>
40 #include <linux/init.h>
41 #include <linux/skbuff.h>
42 #include <linux/slab.h>
43 #include <linux/rbtree.h>
44 #include <linux/hash.h>
45 #include <linux/prefetch.h>
46 #include <linux/vmalloc.h>
47 #include <net/netlink.h>
48 #include <net/pkt_sched.h>
49 #include <net/sock.h>
50 #include <net/tcp_states.h>
51 #include <net/tcp.h>
52 
53 struct fq_skb_cb {
54 	u64	time_to_send;
55 	u8	band;
56 };
57 
58 static inline struct fq_skb_cb *fq_skb_cb(struct sk_buff *skb)
59 {
60 	qdisc_cb_private_validate(skb, sizeof(struct fq_skb_cb));
61 	return (struct fq_skb_cb *)qdisc_skb_cb(skb)->data;
62 }
63 
64 /*
65  * Per flow structure, dynamically allocated.
66  * If packets have monotically increasing time_to_send, they are placed in O(1)
67  * in linear list (head,tail), otherwise are placed in a rbtree (t_root).
68  */
69 struct fq_flow {
70 /* First cache line : used in fq_gc(), fq_enqueue(), fq_dequeue() */
71 	struct rb_root	t_root;
72 	struct sk_buff	*head;		/* list of skbs for this flow : first skb */
73 	union {
74 		struct sk_buff *tail;	/* last skb in the list */
75 		unsigned long  age;	/* (jiffies | 1UL) when flow was emptied, for gc */
76 	};
77 	union {
78 		struct rb_node	fq_node;	/* anchor in fq_root[] trees */
79 		/* Following field is only used for q->internal,
80 		 * because q->internal is not hashed in fq_root[]
81 		 */
82 		u64		stat_fastpath_packets;
83 	};
84 	struct sock	*sk;
85 	u32		socket_hash;	/* sk_hash */
86 	int		qlen;		/* number of packets in flow queue */
87 
88 /* Second cache line */
89 	int		credit;
90 	int		band;
91 	struct fq_flow *next;		/* next pointer in RR lists */
92 
93 	struct rb_node  rate_node;	/* anchor in q->delayed tree */
94 	u64		time_next_packet;
95 };
96 
97 struct fq_flow_head {
98 	struct fq_flow *first;
99 	struct fq_flow *last;
100 };
101 
102 struct fq_perband_flows {
103 	struct fq_flow_head new_flows;
104 	struct fq_flow_head old_flows;
105 	int		    credit;
106 	int		    quantum; /* based on band nr : 576KB, 192KB, 64KB */
107 };
108 
109 #define FQ_PRIO2BAND_CRUMB_SIZE ((TC_PRIO_MAX + 1) >> 2)
110 
111 struct fq_sched_data {
112 /* Read mostly cache line */
113 
114 	u64		offload_horizon;
115 	u32		quantum;
116 	u32		initial_quantum;
117 	u32		flow_refill_delay;
118 	u32		flow_plimit;	/* max packets per flow */
119 	unsigned long	flow_max_rate;	/* optional max rate per flow */
120 	u64		ce_threshold;
121 	u64		horizon;	/* horizon in ns */
122 	u32		orphan_mask;	/* mask for orphaned skb */
123 	u32		low_rate_threshold;
124 	struct rb_root	*fq_root;
125 	u8		rate_enable;
126 	u8		fq_trees_log;
127 	u8		horizon_drop;
128 	u8		prio2band[FQ_PRIO2BAND_CRUMB_SIZE];
129 	u32		timer_slack; /* hrtimer slack in ns */
130 
131 /* Read/Write fields. */
132 
133 	unsigned int band_nr; /* band being serviced in fq_dequeue() */
134 
135 	struct fq_perband_flows band_flows[FQ_BANDS];
136 
137 	struct fq_flow	internal;	/* fastpath queue. */
138 	struct rb_root	delayed;	/* for rate limited flows */
139 	u64		time_next_delayed_flow;
140 	unsigned long	unthrottle_latency_ns;
141 
142 	u32		band_pkt_count[FQ_BANDS];
143 	u32		flows;
144 	u32		inactive_flows; /* Flows with no packet to send. */
145 	u32		throttled_flows;
146 
147 	u64		stat_throttled;
148 	struct qdisc_watchdog watchdog;
149 	u64		stat_gc_flows;
150 
151 /* Seldom used fields. */
152 
153 	u64		stat_band_drops[FQ_BANDS];
154 	u64		stat_ce_mark;
155 	u64		stat_horizon_drops;
156 	u64		stat_horizon_caps;
157 	u64		stat_flows_plimit;
158 	u64		stat_pkts_too_long;
159 	u64		stat_allocation_errors;
160 };
161 
162 /* return the i-th 2-bit value ("crumb") */
163 static u8 fq_prio2band(const u8 *prio2band, unsigned int prio)
164 {
165 	return (READ_ONCE(prio2band[prio / 4]) >> (2 * (prio & 0x3))) & 0x3;
166 }
167 
168 /*
169  * f->tail and f->age share the same location.
170  * We can use the low order bit to differentiate if this location points
171  * to a sk_buff or contains a jiffies value, if we force this value to be odd.
172  * This assumes f->tail low order bit must be 0 since alignof(struct sk_buff) >= 2
173  */
174 static void fq_flow_set_detached(struct fq_flow *f)
175 {
176 	f->age = jiffies | 1UL;
177 }
178 
179 static bool fq_flow_is_detached(const struct fq_flow *f)
180 {
181 	return !!(f->age & 1UL);
182 }
183 
184 /* special value to mark a throttled flow (not on old/new list) */
185 static struct fq_flow throttled;
186 
187 static bool fq_flow_is_throttled(const struct fq_flow *f)
188 {
189 	return f->next == &throttled;
190 }
191 
192 enum new_flow {
193 	NEW_FLOW,
194 	OLD_FLOW
195 };
196 
197 static void fq_flow_add_tail(struct fq_sched_data *q, struct fq_flow *flow,
198 			     enum new_flow list_sel)
199 {
200 	struct fq_perband_flows *pband = &q->band_flows[flow->band];
201 	struct fq_flow_head *head = (list_sel == NEW_FLOW) ?
202 					&pband->new_flows :
203 					&pband->old_flows;
204 
205 	if (head->first)
206 		head->last->next = flow;
207 	else
208 		head->first = flow;
209 	head->last = flow;
210 	flow->next = NULL;
211 }
212 
213 static void fq_flow_unset_throttled(struct fq_sched_data *q, struct fq_flow *f)
214 {
215 	rb_erase(&f->rate_node, &q->delayed);
216 	q->throttled_flows--;
217 	fq_flow_add_tail(q, f, OLD_FLOW);
218 }
219 
220 static void fq_flow_rb_insert(struct fq_sched_data *q, struct fq_flow *f)
221 {
222 	struct rb_node **p = &q->delayed.rb_node, *parent = NULL;
223 
224 	while (*p) {
225 		struct fq_flow *aux;
226 
227 		parent = *p;
228 		aux = rb_entry(parent, struct fq_flow, rate_node);
229 		if (f->time_next_packet >= aux->time_next_packet)
230 			p = &parent->rb_right;
231 		else
232 			p = &parent->rb_left;
233 	}
234 	rb_link_node(&f->rate_node, parent, p);
235 	rb_insert_color(&f->rate_node, &q->delayed);
236 
237 	if (q->time_next_delayed_flow > f->time_next_packet)
238 		q->time_next_delayed_flow = f->time_next_packet;
239 }
240 
241 static void fq_flow_set_throttled(struct fq_sched_data *q, struct fq_flow *f)
242 {
243 	fq_flow_rb_insert(q, f);
244 	q->throttled_flows++;
245 	q->stat_throttled++;
246 	f->next = &throttled;
247 }
248 
249 static struct kmem_cache *fq_flow_cachep __read_mostly;
250 
251 
252 #define FQ_GC_AGE (3*HZ)
253 
254 static bool fq_gc_candidate(const struct fq_flow *f)
255 {
256 	return fq_flow_is_detached(f) &&
257 	       time_after(jiffies, f->age + FQ_GC_AGE);
258 }
259 
260 static void fq_gc(struct fq_sched_data *q,
261 		  struct rb_root *root,
262 		  struct sock *sk)
263 {
264 	struct fq_flow *f, *tofree = NULL;
265 	struct rb_node **p, *parent;
266 	int fcnt;
267 
268 	p = &root->rb_node;
269 	parent = NULL;
270 	while (*p) {
271 		parent = *p;
272 
273 		f = rb_entry(parent, struct fq_flow, fq_node);
274 		if (f->sk == sk)
275 			break;
276 
277 		if (fq_gc_candidate(f)) {
278 			f->next = tofree;
279 			tofree = f;
280 		}
281 
282 		if (f->sk > sk)
283 			p = &parent->rb_right;
284 		else
285 			p = &parent->rb_left;
286 	}
287 
288 	if (!tofree)
289 		return;
290 
291 	fcnt = 0;
292 	while (tofree) {
293 		f = tofree;
294 		tofree = f->next;
295 		rb_erase(&f->fq_node, root);
296 		kmem_cache_free(fq_flow_cachep, f);
297 		fcnt++;
298 	}
299 	q->flows -= fcnt;
300 	q->inactive_flows -= fcnt;
301 	q->stat_gc_flows += fcnt;
302 }
303 
304 /* Fast path can be used if :
305  * 1) Packet tstamp is in the past, or within the pacing offload horizon.
306  * 2) FQ qlen == 0   OR
307  *   (no flow is currently eligible for transmit,
308  *    AND fast path queue has less than 8 packets)
309  * 3) No SO_MAX_PACING_RATE on the socket (if any).
310  * 4) No @maxrate attribute on this qdisc,
311  *
312  * FQ can not use generic TCQ_F_CAN_BYPASS infrastructure.
313  */
314 static bool fq_fastpath_check(const struct Qdisc *sch, struct sk_buff *skb,
315 			      u64 now)
316 {
317 	const struct fq_sched_data *q = qdisc_priv(sch);
318 	const struct sock *sk;
319 
320 	if (fq_skb_cb(skb)->time_to_send > now + q->offload_horizon)
321 		return false;
322 
323 	if (sch->q.qlen != 0) {
324 		/* Even if some packets are stored in this qdisc,
325 		 * we can still enable fast path if all of them are
326 		 * scheduled in the future (ie no flows are eligible)
327 		 * or in the fast path queue.
328 		 */
329 		if (q->flows != q->inactive_flows + q->throttled_flows)
330 			return false;
331 
332 		/* Do not allow fast path queue to explode, we want Fair Queue mode
333 		 * under pressure.
334 		 */
335 		if (q->internal.qlen >= 8)
336 			return false;
337 
338 		/* Ordering invariants fall apart if some delayed flows
339 		 * are ready but we haven't serviced them, yet.
340 		 */
341 		if (q->time_next_delayed_flow <= now + q->offload_horizon)
342 			return false;
343 	}
344 
345 	sk = skb->sk;
346 	if (sk && sk_fullsock(sk) && !sk_is_tcp(sk) &&
347 	    sk->sk_max_pacing_rate != ~0UL)
348 		return false;
349 
350 	if (q->flow_max_rate != ~0UL)
351 		return false;
352 
353 	return true;
354 }
355 
356 static struct fq_flow *fq_classify(struct Qdisc *sch, struct sk_buff *skb,
357 				   u64 now)
358 {
359 	struct fq_sched_data *q = qdisc_priv(sch);
360 	struct rb_node **p, *parent;
361 	struct sock *sk = skb->sk;
362 	struct rb_root *root;
363 	struct fq_flow *f;
364 
365 	/* SYNACK messages are attached to a TCP_NEW_SYN_RECV request socket
366 	 * or a listener (SYNCOOKIE mode)
367 	 * 1) request sockets are not full blown,
368 	 *    they do not contain sk_pacing_rate
369 	 * 2) They are not part of a 'flow' yet
370 	 * 3) We do not want to rate limit them (eg SYNFLOOD attack),
371 	 *    especially if the listener set SO_MAX_PACING_RATE
372 	 * 4) We pretend they are orphaned
373 	 * TCP can also associate TIME_WAIT sockets with RST or ACK packets.
374 	 */
375 	if (!sk || sk_listener_or_tw(sk)) {
376 		unsigned long hash = skb_get_hash(skb) & q->orphan_mask;
377 
378 		/* By forcing low order bit to 1, we make sure to not
379 		 * collide with a local flow (socket pointers are word aligned)
380 		 */
381 		sk = (struct sock *)((hash << 1) | 1UL);
382 		skb_orphan(skb);
383 	} else if (sk->sk_state == TCP_CLOSE) {
384 		unsigned long hash = skb_get_hash(skb) & q->orphan_mask;
385 		/*
386 		 * Sockets in TCP_CLOSE are non connected.
387 		 * Typical use case is UDP sockets, they can send packets
388 		 * with sendto() to many different destinations.
389 		 * We probably could use a generic bit advertising
390 		 * non connected sockets, instead of sk_state == TCP_CLOSE,
391 		 * if we care enough.
392 		 */
393 		sk = (struct sock *)((hash << 1) | 1UL);
394 	}
395 
396 	if (fq_fastpath_check(sch, skb, now)) {
397 		q->internal.stat_fastpath_packets++;
398 		if (skb->sk == sk && q->rate_enable &&
399 		    READ_ONCE(sk->sk_pacing_status) != SK_PACING_FQ)
400 			smp_store_release(&sk->sk_pacing_status,
401 					  SK_PACING_FQ);
402 		return &q->internal;
403 	}
404 
405 	root = &q->fq_root[hash_ptr(sk, q->fq_trees_log)];
406 
407 	fq_gc(q, root, sk);
408 
409 	p = &root->rb_node;
410 	parent = NULL;
411 	while (*p) {
412 		parent = *p;
413 
414 		f = rb_entry(parent, struct fq_flow, fq_node);
415 		if (f->sk == sk) {
416 			/* socket might have been reallocated, so check
417 			 * if its sk_hash is the same.
418 			 * It not, we need to refill credit with
419 			 * initial quantum
420 			 */
421 			if (unlikely(skb->sk == sk &&
422 				     f->socket_hash != sk->sk_hash)) {
423 				f->credit = q->initial_quantum;
424 				f->socket_hash = sk->sk_hash;
425 				if (q->rate_enable)
426 					smp_store_release(&sk->sk_pacing_status,
427 							  SK_PACING_FQ);
428 				if (fq_flow_is_throttled(f))
429 					fq_flow_unset_throttled(q, f);
430 				f->time_next_packet = 0ULL;
431 			}
432 			return f;
433 		}
434 		if (f->sk > sk)
435 			p = &parent->rb_right;
436 		else
437 			p = &parent->rb_left;
438 	}
439 
440 	f = kmem_cache_zalloc(fq_flow_cachep, GFP_ATOMIC | __GFP_NOWARN);
441 	if (unlikely(!f)) {
442 		q->stat_allocation_errors++;
443 		return &q->internal;
444 	}
445 	/* f->t_root is already zeroed after kmem_cache_zalloc() */
446 
447 	fq_flow_set_detached(f);
448 	f->sk = sk;
449 	if (skb->sk == sk) {
450 		f->socket_hash = sk->sk_hash;
451 		if (q->rate_enable)
452 			smp_store_release(&sk->sk_pacing_status,
453 					  SK_PACING_FQ);
454 	}
455 	f->credit = q->initial_quantum;
456 
457 	rb_link_node(&f->fq_node, parent, p);
458 	rb_insert_color(&f->fq_node, root);
459 
460 	q->flows++;
461 	q->inactive_flows++;
462 	return f;
463 }
464 
465 static struct sk_buff *fq_peek(struct fq_flow *flow)
466 {
467 	struct sk_buff *skb = skb_rb_first(&flow->t_root);
468 	struct sk_buff *head = flow->head;
469 
470 	if (!skb)
471 		return head;
472 
473 	if (!head)
474 		return skb;
475 
476 	if (fq_skb_cb(skb)->time_to_send < fq_skb_cb(head)->time_to_send)
477 		return skb;
478 	return head;
479 }
480 
481 static void fq_erase_head(struct Qdisc *sch, struct fq_flow *flow,
482 			  struct sk_buff *skb)
483 {
484 	if (skb == flow->head) {
485 		struct sk_buff *next = skb->next;
486 
487 		prefetch(next);
488 		flow->head = next;
489 	} else {
490 		rb_erase(&skb->rbnode, &flow->t_root);
491 		skb->dev = qdisc_dev(sch);
492 	}
493 }
494 
495 /* Remove one skb from flow queue.
496  * This skb must be the return value of prior fq_peek().
497  */
498 static void fq_dequeue_skb(struct Qdisc *sch, struct fq_flow *flow,
499 			   struct sk_buff *skb)
500 {
501 	fq_erase_head(sch, flow, skb);
502 	skb_mark_not_on_list(skb);
503 	qdisc_qstats_backlog_dec(sch, skb);
504 	qdisc_qlen_dec(sch);
505 	qdisc_bstats_update(sch, skb);
506 }
507 
508 static void flow_queue_add(struct fq_flow *flow, struct sk_buff *skb)
509 {
510 	struct rb_node **p, *parent;
511 	struct sk_buff *head, *aux;
512 
513 	head = flow->head;
514 	if (!head ||
515 	    fq_skb_cb(skb)->time_to_send >= fq_skb_cb(flow->tail)->time_to_send) {
516 		if (!head)
517 			flow->head = skb;
518 		else
519 			flow->tail->next = skb;
520 		flow->tail = skb;
521 		skb->next = NULL;
522 		return;
523 	}
524 
525 	p = &flow->t_root.rb_node;
526 	parent = NULL;
527 
528 	while (*p) {
529 		parent = *p;
530 		aux = rb_to_skb(parent);
531 		if (fq_skb_cb(skb)->time_to_send >= fq_skb_cb(aux)->time_to_send)
532 			p = &parent->rb_right;
533 		else
534 			p = &parent->rb_left;
535 	}
536 	rb_link_node(&skb->rbnode, parent, p);
537 	rb_insert_color(&skb->rbnode, &flow->t_root);
538 }
539 
540 static bool fq_packet_beyond_horizon(ktime_t time_to_send,
541 				     const struct fq_sched_data *q, u64 now)
542 {
543 	return unlikely((s64)time_to_send > (s64)(now + q->horizon));
544 }
545 
546 static void fq_flow_adjust_timer(struct fq_sched_data *q, struct fq_flow *flow,
547 				 u64 time_to_send, u64 now)
548 {
549 	if (time_to_send <= now) {
550 		fq_flow_unset_throttled(q, flow);
551 		if (q->time_next_delayed_flow == flow->time_next_packet) {
552 			struct rb_node *p = rb_first(&q->delayed);
553 
554 			q->time_next_delayed_flow = p ? rb_entry(p, struct fq_flow, rate_node)->time_next_packet : ~0ULL;
555 		}
556 		flow->time_next_packet = time_to_send;
557 	} else {
558 		rb_erase(&flow->rate_node, &q->delayed);
559 		flow->time_next_packet = time_to_send;
560 		fq_flow_rb_insert(q, flow);
561 	}
562 }
563 
564 static ktime_t fq_skb_tstamp_to_mono(struct sk_buff *skb)
565 {
566 	const ktime_t mono_max = NSEC_PER_SEC * TIME_UPTIME_SEC_MAX;
567 
568 	if (likely(skb->tstamp_type == SKB_CLOCK_MONOTONIC))
569 		return max(skb->tstamp, 1);
570 
571 	if (skb->tstamp_type == SKB_CLOCK_TAI)
572 		return max(ktime_sub(skb->tstamp, ktime_mono_to_any(0, TK_OFFS_TAI)), 1);
573 
574 	if (likely(skb->tstamp > mono_max))
575 		return max(ktime_sub(skb->tstamp, ktime_mono_to_real(0)), 1);
576 
577 	/* Handle BPF programs setting skb->stamp but not tstamp_type */
578 	net_warn_ratelimited("fq: likely mono tstamp with tstamp_type 0\n");
579 
580 	skb->tstamp_type = SKB_CLOCK_MONOTONIC;
581 	return max(skb->tstamp, 1);
582 }
583 
584 static void fq_mono_to_skb_tstamp(struct sk_buff *skb, ktime_t time_to_send)
585 {
586 	if (skb->tstamp_type == SKB_CLOCK_MONOTONIC)
587 		skb->tstamp = time_to_send;
588 	else if (skb->tstamp_type == SKB_CLOCK_REALTIME)
589 		skb->tstamp = ktime_mono_to_real(time_to_send);
590 	else
591 		skb->tstamp = ktime_mono_to_any(time_to_send, TK_OFFS_TAI);
592 }
593 
594 static int fq_enqueue(struct sk_buff *skb, struct Qdisc *sch,
595 		      struct sk_buff **to_free)
596 {
597 	struct fq_sched_data *q = qdisc_priv(sch);
598 	struct fq_flow *f;
599 	u64 now;
600 	u8 band;
601 
602 	band = fq_prio2band(q->prio2band, skb->priority & TC_PRIO_MAX);
603 	if (unlikely(q->band_pkt_count[band] >= sch->limit)) {
604 		q->stat_band_drops[band]++;
605 		return qdisc_drop_reason(skb, sch, to_free, QDISC_DROP_BAND_LIMIT);
606 	}
607 
608 	now = ktime_get_ns();
609 	if (!skb->tstamp) {
610 		fq_skb_cb(skb)->time_to_send = now;
611 	} else {
612 		ktime_t time_to_send = fq_skb_tstamp_to_mono(skb);
613 
614 		/* Check if packet timestamp is too far in the future. */
615 		if (fq_packet_beyond_horizon(time_to_send, q, now)) {
616 			if (q->horizon_drop) {
617 				q->stat_horizon_drops++;
618 				return qdisc_drop_reason(skb, sch, to_free,
619 							 QDISC_DROP_HORIZON_LIMIT);
620 			}
621 			q->stat_horizon_caps++;
622 			time_to_send = now + q->horizon;
623 			fq_mono_to_skb_tstamp(skb, time_to_send);
624 		}
625 		fq_skb_cb(skb)->time_to_send = (u64)time_to_send;
626 	}
627 
628 	f = fq_classify(sch, skb, now);
629 
630 	if (f != &q->internal) {
631 		if (unlikely(f->qlen >= q->flow_plimit)) {
632 			q->stat_flows_plimit++;
633 			return qdisc_drop_reason(skb, sch, to_free,
634 						 QDISC_DROP_FLOW_LIMIT);
635 		}
636 
637 		if (fq_flow_is_detached(f)) {
638 			fq_flow_add_tail(q, f, NEW_FLOW);
639 			if (time_after(jiffies, f->age + q->flow_refill_delay))
640 				f->credit = max_t(u32, f->credit, q->quantum);
641 		}
642 
643 		f->band = band;
644 		q->band_pkt_count[band]++;
645 		fq_skb_cb(skb)->band = band;
646 		if (f->qlen == 0)
647 			q->inactive_flows--;
648 	}
649 
650 	f->qlen++;
651 	/* Note: this overwrites f->age */
652 	flow_queue_add(f, skb);
653 
654 	if (fq_skb_cb(skb)->time_to_send < f->time_next_packet && skb->tstamp &&
655 	    fq_flow_is_throttled(f) && q->flow_max_rate == ~0UL)
656 		fq_flow_adjust_timer(q, f, fq_skb_cb(skb)->time_to_send, now);
657 
658 	qdisc_qstats_backlog_inc(sch, skb);
659 	qdisc_qlen_inc(sch);
660 
661 	return NET_XMIT_SUCCESS;
662 }
663 
664 static void fq_check_throttled(struct fq_sched_data *q, u64 now)
665 {
666 	unsigned long sample;
667 	struct rb_node *p;
668 
669 	if (q->time_next_delayed_flow > now + q->offload_horizon)
670 		return;
671 
672 	/* Update unthrottle latency EWMA.
673 	 * This is cheap and can help diagnosing timer/latency problems.
674 	 */
675 	sample = (unsigned long)(now - q->time_next_delayed_flow);
676 	if ((long)sample > 0) {
677 		q->unthrottle_latency_ns -= q->unthrottle_latency_ns >> 3;
678 		q->unthrottle_latency_ns += sample >> 3;
679 	}
680 	now += q->offload_horizon;
681 
682 	q->time_next_delayed_flow = ~0ULL;
683 	while ((p = rb_first(&q->delayed)) != NULL) {
684 		struct fq_flow *f = rb_entry(p, struct fq_flow, rate_node);
685 
686 		if (f->time_next_packet > now) {
687 			q->time_next_delayed_flow = f->time_next_packet;
688 			break;
689 		}
690 		fq_flow_unset_throttled(q, f);
691 	}
692 }
693 
694 static struct fq_flow_head *fq_pband_head_select(struct fq_perband_flows *pband)
695 {
696 	if (pband->credit <= 0)
697 		return NULL;
698 
699 	if (pband->new_flows.first)
700 		return &pband->new_flows;
701 
702 	return pband->old_flows.first ? &pband->old_flows : NULL;
703 }
704 
705 static struct sk_buff *fq_dequeue(struct Qdisc *sch)
706 {
707 	struct fq_sched_data *q = qdisc_priv(sch);
708 	struct fq_perband_flows *pband;
709 	struct fq_flow_head *head;
710 	struct sk_buff *skb;
711 	struct fq_flow *f;
712 	unsigned long rate;
713 	int retry;
714 	u32 plen;
715 	u64 now;
716 
717 	if (!sch->q.qlen)
718 		return NULL;
719 
720 	skb = fq_peek(&q->internal);
721 	if (skb) {
722 		q->internal.qlen--;
723 		fq_dequeue_skb(sch, &q->internal, skb);
724 		goto out;
725 	}
726 
727 	now = ktime_get_ns();
728 	fq_check_throttled(q, now);
729 	retry = 0;
730 	pband = &q->band_flows[q->band_nr];
731 begin:
732 	head = fq_pband_head_select(pband);
733 	if (!head) {
734 		while (++retry <= FQ_BANDS) {
735 			if (++q->band_nr == FQ_BANDS)
736 				q->band_nr = 0;
737 			pband = &q->band_flows[q->band_nr];
738 			pband->credit = min(pband->credit + pband->quantum,
739 					    pband->quantum);
740 			if (pband->credit > 0)
741 				goto begin;
742 			retry = 0;
743 		}
744 		if (q->time_next_delayed_flow != ~0ULL)
745 			qdisc_watchdog_schedule_range_ns(&q->watchdog,
746 							q->time_next_delayed_flow,
747 							q->timer_slack);
748 		return NULL;
749 	}
750 	f = head->first;
751 	retry = 0;
752 	if (f->credit <= 0) {
753 		f->credit += q->quantum;
754 		head->first = f->next;
755 		fq_flow_add_tail(q, f, OLD_FLOW);
756 		goto begin;
757 	}
758 
759 	skb = fq_peek(f);
760 	if (skb) {
761 		u64 time_next_packet = max_t(u64, fq_skb_cb(skb)->time_to_send,
762 					     f->time_next_packet);
763 
764 		if (now + q->offload_horizon < time_next_packet) {
765 			head->first = f->next;
766 			f->time_next_packet = time_next_packet;
767 			fq_flow_set_throttled(q, f);
768 			goto begin;
769 		}
770 		prefetch(&skb->end);
771 		fq_dequeue_skb(sch, f, skb);
772 		if (unlikely((s64)(now - time_next_packet - q->ce_threshold) > 0)) {
773 			INET_ECN_set_ce(skb);
774 			q->stat_ce_mark++;
775 		}
776 		if (--f->qlen == 0)
777 			q->inactive_flows++;
778 		q->band_pkt_count[fq_skb_cb(skb)->band]--;
779 	} else {
780 		head->first = f->next;
781 		/* force a pass through old_flows to prevent starvation */
782 		if (head == &pband->new_flows) {
783 			fq_flow_add_tail(q, f, OLD_FLOW);
784 		} else {
785 			fq_flow_set_detached(f);
786 		}
787 		goto begin;
788 	}
789 	plen = qdisc_pkt_len(skb);
790 	f->credit -= plen;
791 	pband->credit -= plen;
792 
793 	if (!q->rate_enable)
794 		goto out;
795 
796 	rate = q->flow_max_rate;
797 
798 	/* If EDT time was provided for this skb, we need to
799 	 * update f->time_next_packet only if this qdisc enforces
800 	 * a flow max rate.
801 	 */
802 	if (!skb->tstamp) {
803 		if (skb->sk)
804 			rate = min(READ_ONCE(skb->sk->sk_pacing_rate), rate);
805 
806 		if (rate <= q->low_rate_threshold) {
807 			f->credit = 0;
808 		} else {
809 			plen = max(plen, q->quantum);
810 			if (f->credit > 0)
811 				goto out;
812 		}
813 	}
814 	if (rate != ~0UL) {
815 		u64 len = (u64)plen * NSEC_PER_SEC;
816 
817 		if (likely(rate))
818 			len = div64_ul(len, rate);
819 		/* Since socket rate can change later,
820 		 * clamp the delay to 1 second.
821 		 * Really, providers of too big packets should be fixed !
822 		 */
823 		if (unlikely(len > NSEC_PER_SEC)) {
824 			len = NSEC_PER_SEC;
825 			q->stat_pkts_too_long++;
826 		}
827 		/* Account for schedule/timers drifts.
828 		 * f->time_next_packet was set when prior packet was sent,
829 		 * and current time (@now) can be too late by tens of us.
830 		 */
831 		if (f->time_next_packet)
832 			len -= min(len/2, now - f->time_next_packet);
833 		f->time_next_packet = now + len;
834 	}
835 out:
836 	return skb;
837 }
838 
839 static void fq_flow_purge(struct fq_flow *flow)
840 {
841 	struct rb_node *p = rb_first(&flow->t_root);
842 
843 	while (p) {
844 		struct sk_buff *skb = rb_to_skb(p);
845 
846 		p = rb_next(p);
847 		rb_erase(&skb->rbnode, &flow->t_root);
848 		rtnl_kfree_skbs(skb, skb);
849 	}
850 	rtnl_kfree_skbs(flow->head, flow->tail);
851 	flow->head = NULL;
852 	flow->qlen = 0;
853 }
854 
855 static void fq_reset(struct Qdisc *sch)
856 {
857 	struct fq_sched_data *q = qdisc_priv(sch);
858 	struct rb_root *root;
859 	struct rb_node *p;
860 	struct fq_flow *f;
861 	unsigned int idx;
862 
863 	WRITE_ONCE(sch->q.qlen, 0);
864 	WRITE_ONCE(sch->qstats.backlog, 0);
865 
866 	fq_flow_purge(&q->internal);
867 
868 	if (!q->fq_root)
869 		return;
870 
871 	for (idx = 0; idx < (1U << q->fq_trees_log); idx++) {
872 		root = &q->fq_root[idx];
873 		while ((p = rb_first(root)) != NULL) {
874 			f = rb_entry(p, struct fq_flow, fq_node);
875 			rb_erase(p, root);
876 
877 			fq_flow_purge(f);
878 
879 			kmem_cache_free(fq_flow_cachep, f);
880 		}
881 	}
882 	for (idx = 0; idx < FQ_BANDS; idx++) {
883 		q->band_flows[idx].new_flows.first = NULL;
884 		q->band_flows[idx].old_flows.first = NULL;
885 		q->band_pkt_count[idx] = 0;
886 	}
887 	q->delayed		= RB_ROOT;
888 	q->flows		= 0;
889 	q->inactive_flows	= 0;
890 	q->throttled_flows	= 0;
891 }
892 
893 static void fq_rehash(struct fq_sched_data *q,
894 		      struct rb_root *old_array, u32 old_log,
895 		      struct rb_root *new_array, u32 new_log)
896 {
897 	struct rb_node *op, **np, *parent;
898 	struct rb_root *oroot, *nroot;
899 	struct fq_flow *of, *nf;
900 	int fcnt = 0;
901 	u32 idx;
902 
903 	for (idx = 0; idx < (1U << old_log); idx++) {
904 		oroot = &old_array[idx];
905 		while ((op = rb_first(oroot)) != NULL) {
906 			rb_erase(op, oroot);
907 			of = rb_entry(op, struct fq_flow, fq_node);
908 			if (fq_gc_candidate(of)) {
909 				fcnt++;
910 				kmem_cache_free(fq_flow_cachep, of);
911 				continue;
912 			}
913 			nroot = &new_array[hash_ptr(of->sk, new_log)];
914 
915 			np = &nroot->rb_node;
916 			parent = NULL;
917 			while (*np) {
918 				parent = *np;
919 
920 				nf = rb_entry(parent, struct fq_flow, fq_node);
921 				BUG_ON(nf->sk == of->sk);
922 
923 				if (nf->sk > of->sk)
924 					np = &parent->rb_right;
925 				else
926 					np = &parent->rb_left;
927 			}
928 
929 			rb_link_node(&of->fq_node, parent, np);
930 			rb_insert_color(&of->fq_node, nroot);
931 		}
932 	}
933 	q->flows -= fcnt;
934 	q->inactive_flows -= fcnt;
935 	q->stat_gc_flows += fcnt;
936 }
937 
938 static void fq_free(void *addr)
939 {
940 	kvfree(addr);
941 }
942 
943 static int fq_resize(struct Qdisc *sch, u32 log)
944 {
945 	struct fq_sched_data *q = qdisc_priv(sch);
946 	struct rb_root *array;
947 	void *old_fq_root;
948 	u32 idx;
949 
950 	if (q->fq_root && log == q->fq_trees_log)
951 		return 0;
952 
953 	/* If XPS was setup, we can allocate memory on right NUMA node */
954 	array = kvmalloc_node(sizeof(struct rb_root) << log, GFP_KERNEL | __GFP_RETRY_MAYFAIL,
955 			      netdev_queue_numa_node_read(sch->dev_queue));
956 	if (!array)
957 		return -ENOMEM;
958 
959 	for (idx = 0; idx < (1U << log); idx++)
960 		array[idx] = RB_ROOT;
961 
962 	sch_tree_lock(sch);
963 
964 	old_fq_root = q->fq_root;
965 	if (old_fq_root)
966 		fq_rehash(q, old_fq_root, q->fq_trees_log, array, log);
967 
968 	q->fq_root = array;
969 	WRITE_ONCE(q->fq_trees_log, log);
970 
971 	sch_tree_unlock(sch);
972 
973 	fq_free(old_fq_root);
974 
975 	return 0;
976 }
977 
978 static const struct netlink_range_validation iq_range = {
979 	.max = INT_MAX,
980 };
981 
982 static const struct nla_policy fq_policy[TCA_FQ_MAX + 1] = {
983 	[TCA_FQ_UNSPEC]			= { .strict_start_type = TCA_FQ_TIMER_SLACK },
984 
985 	[TCA_FQ_PLIMIT]			= { .type = NLA_U32 },
986 	[TCA_FQ_FLOW_PLIMIT]		= { .type = NLA_U32 },
987 	[TCA_FQ_QUANTUM]		= { .type = NLA_U32 },
988 	[TCA_FQ_INITIAL_QUANTUM]	= NLA_POLICY_FULL_RANGE(NLA_U32, &iq_range),
989 	[TCA_FQ_RATE_ENABLE]		= { .type = NLA_U32 },
990 	[TCA_FQ_FLOW_DEFAULT_RATE]	= { .type = NLA_U32 },
991 	[TCA_FQ_FLOW_MAX_RATE]		= { .type = NLA_U32 },
992 	[TCA_FQ_BUCKETS_LOG]		= { .type = NLA_U32 },
993 	[TCA_FQ_FLOW_REFILL_DELAY]	= { .type = NLA_U32 },
994 	[TCA_FQ_ORPHAN_MASK]		= { .type = NLA_U32 },
995 	[TCA_FQ_LOW_RATE_THRESHOLD]	= { .type = NLA_U32 },
996 	[TCA_FQ_CE_THRESHOLD]		= { .type = NLA_U32 },
997 	[TCA_FQ_TIMER_SLACK]		= { .type = NLA_U32 },
998 	[TCA_FQ_HORIZON]		= { .type = NLA_U32 },
999 	[TCA_FQ_HORIZON_DROP]		= { .type = NLA_U8 },
1000 	[TCA_FQ_PRIOMAP]		= NLA_POLICY_EXACT_LEN(sizeof(struct tc_prio_qopt)),
1001 	[TCA_FQ_WEIGHTS]		= NLA_POLICY_EXACT_LEN(FQ_BANDS * sizeof(s32)),
1002 	[TCA_FQ_OFFLOAD_HORIZON]	= { .type = NLA_U32 },
1003 };
1004 
1005 /* compress a u8 array with all elems <= 3 to an array of 2-bit fields */
1006 static void fq_prio2band_compress_crumb(const u8 *in, u8 *out)
1007 {
1008 	const int num_elems = TC_PRIO_MAX + 1;
1009 	u8 tmp[FQ_PRIO2BAND_CRUMB_SIZE];
1010 	int i;
1011 
1012 	memset(tmp, 0, sizeof(tmp));
1013 	for (i = 0; i < num_elems; i++)
1014 		tmp[i / 4] |= in[i] << (2 * (i & 0x3));
1015 
1016 	for (i = 0; i < FQ_PRIO2BAND_CRUMB_SIZE; i++)
1017 		WRITE_ONCE(out[i], tmp[i]);
1018 }
1019 
1020 static void fq_prio2band_decompress_crumb(const u8 *in, u8 *out)
1021 {
1022 	const int num_elems = TC_PRIO_MAX + 1;
1023 	int i;
1024 
1025 	for (i = 0; i < num_elems; i++)
1026 		out[i] = fq_prio2band(in, i);
1027 }
1028 
1029 static int fq_load_weights(struct fq_sched_data *q,
1030 			   const struct nlattr *attr,
1031 			   struct netlink_ext_ack *extack)
1032 {
1033 	s32 *weights = nla_data(attr);
1034 	int i;
1035 
1036 	for (i = 0; i < FQ_BANDS; i++) {
1037 		if (weights[i] < FQ_MIN_WEIGHT) {
1038 			NL_SET_ERR_MSG_FMT_MOD(extack, "Weight %d less that minimum allowed %d",
1039 					       weights[i], FQ_MIN_WEIGHT);
1040 			return -EINVAL;
1041 		}
1042 	}
1043 	for (i = 0; i < FQ_BANDS; i++)
1044 		WRITE_ONCE(q->band_flows[i].quantum, weights[i]);
1045 	return 0;
1046 }
1047 
1048 static int fq_load_priomap(struct fq_sched_data *q,
1049 			   const struct nlattr *attr,
1050 			   struct netlink_ext_ack *extack)
1051 {
1052 	const struct tc_prio_qopt *map = nla_data(attr);
1053 	int i;
1054 
1055 	if (map->bands != FQ_BANDS) {
1056 		NL_SET_ERR_MSG_MOD(extack, "FQ only supports 3 bands");
1057 		return -EINVAL;
1058 	}
1059 	for (i = 0; i < TC_PRIO_MAX + 1; i++) {
1060 		if (map->priomap[i] >= FQ_BANDS) {
1061 			NL_SET_ERR_MSG_FMT_MOD(extack, "FQ priomap field %d maps to a too high band %d",
1062 					       i, map->priomap[i]);
1063 			return -EINVAL;
1064 		}
1065 	}
1066 	fq_prio2band_compress_crumb(map->priomap, q->prio2band);
1067 	return 0;
1068 }
1069 
1070 static int fq_change(struct Qdisc *sch, struct nlattr *opt,
1071 		     struct netlink_ext_ack *extack)
1072 {
1073 	unsigned int dropped_pkts = 0, dropped_bytes = 0;
1074 	struct fq_sched_data *q = qdisc_priv(sch);
1075 	struct nlattr *tb[TCA_FQ_MAX + 1];
1076 	u32 fq_log;
1077 	int err;
1078 
1079 	err = nla_parse_nested_deprecated(tb, TCA_FQ_MAX, opt, fq_policy,
1080 					  NULL);
1081 	if (err < 0)
1082 		return err;
1083 
1084 	sch_tree_lock(sch);
1085 
1086 	fq_log = q->fq_trees_log;
1087 
1088 	if (tb[TCA_FQ_BUCKETS_LOG]) {
1089 		u32 nval = nla_get_u32(tb[TCA_FQ_BUCKETS_LOG]);
1090 
1091 		if (nval >= 1 && nval <= ilog2(256*1024))
1092 			fq_log = nval;
1093 		else
1094 			err = -EINVAL;
1095 	}
1096 	if (tb[TCA_FQ_PLIMIT])
1097 		WRITE_ONCE(sch->limit,
1098 			   nla_get_u32(tb[TCA_FQ_PLIMIT]));
1099 
1100 	if (tb[TCA_FQ_FLOW_PLIMIT])
1101 		WRITE_ONCE(q->flow_plimit,
1102 			   nla_get_u32(tb[TCA_FQ_FLOW_PLIMIT]));
1103 
1104 	if (tb[TCA_FQ_QUANTUM]) {
1105 		u32 quantum = nla_get_u32(tb[TCA_FQ_QUANTUM]);
1106 
1107 		if (quantum > 0 && quantum <= (1 << 20)) {
1108 			WRITE_ONCE(q->quantum, quantum);
1109 		} else {
1110 			NL_SET_ERR_MSG_MOD(extack, "invalid quantum");
1111 			err = -EINVAL;
1112 		}
1113 	}
1114 
1115 	if (tb[TCA_FQ_INITIAL_QUANTUM])
1116 		WRITE_ONCE(q->initial_quantum,
1117 			   nla_get_u32(tb[TCA_FQ_INITIAL_QUANTUM]));
1118 
1119 	if (tb[TCA_FQ_FLOW_DEFAULT_RATE])
1120 		pr_warn_ratelimited("sch_fq: defrate %u ignored.\n",
1121 				    nla_get_u32(tb[TCA_FQ_FLOW_DEFAULT_RATE]));
1122 
1123 	if (tb[TCA_FQ_FLOW_MAX_RATE]) {
1124 		u32 rate = nla_get_u32(tb[TCA_FQ_FLOW_MAX_RATE]);
1125 
1126 		WRITE_ONCE(q->flow_max_rate,
1127 			   (rate == ~0U) ? ~0UL : rate);
1128 	}
1129 	if (tb[TCA_FQ_LOW_RATE_THRESHOLD])
1130 		WRITE_ONCE(q->low_rate_threshold,
1131 			   nla_get_u32(tb[TCA_FQ_LOW_RATE_THRESHOLD]));
1132 
1133 	if (tb[TCA_FQ_RATE_ENABLE]) {
1134 		u32 enable = nla_get_u32(tb[TCA_FQ_RATE_ENABLE]);
1135 
1136 		if (enable <= 1)
1137 			WRITE_ONCE(q->rate_enable,
1138 				   enable);
1139 		else
1140 			err = -EINVAL;
1141 	}
1142 
1143 	if (tb[TCA_FQ_FLOW_REFILL_DELAY]) {
1144 		u32 usecs_delay = nla_get_u32(tb[TCA_FQ_FLOW_REFILL_DELAY]) ;
1145 
1146 		WRITE_ONCE(q->flow_refill_delay,
1147 			   usecs_to_jiffies(usecs_delay));
1148 	}
1149 
1150 	if (!err && tb[TCA_FQ_PRIOMAP])
1151 		err = fq_load_priomap(q, tb[TCA_FQ_PRIOMAP], extack);
1152 
1153 	if (!err && tb[TCA_FQ_WEIGHTS])
1154 		err = fq_load_weights(q, tb[TCA_FQ_WEIGHTS], extack);
1155 
1156 	if (tb[TCA_FQ_ORPHAN_MASK])
1157 		WRITE_ONCE(q->orphan_mask,
1158 			   nla_get_u32(tb[TCA_FQ_ORPHAN_MASK]));
1159 
1160 	if (tb[TCA_FQ_CE_THRESHOLD])
1161 		WRITE_ONCE(q->ce_threshold,
1162 			   (u64)NSEC_PER_USEC *
1163 			   nla_get_u32(tb[TCA_FQ_CE_THRESHOLD]));
1164 
1165 	if (tb[TCA_FQ_TIMER_SLACK])
1166 		WRITE_ONCE(q->timer_slack,
1167 			   nla_get_u32(tb[TCA_FQ_TIMER_SLACK]));
1168 
1169 	if (tb[TCA_FQ_HORIZON])
1170 		WRITE_ONCE(q->horizon,
1171 			   (u64)NSEC_PER_USEC *
1172 			   nla_get_u32(tb[TCA_FQ_HORIZON]));
1173 
1174 	if (tb[TCA_FQ_HORIZON_DROP])
1175 		WRITE_ONCE(q->horizon_drop,
1176 			   nla_get_u8(tb[TCA_FQ_HORIZON_DROP]));
1177 
1178 	if (tb[TCA_FQ_OFFLOAD_HORIZON]) {
1179 		u64 offload_horizon = (u64)NSEC_PER_USEC *
1180 				      nla_get_u32(tb[TCA_FQ_OFFLOAD_HORIZON]);
1181 
1182 		if (offload_horizon <= qdisc_dev(sch)->max_pacing_offload_horizon) {
1183 			WRITE_ONCE(q->offload_horizon, offload_horizon);
1184 		} else {
1185 			NL_SET_ERR_MSG_MOD(extack, "invalid offload_horizon");
1186 			err = -EINVAL;
1187 		}
1188 	}
1189 	if (!err) {
1190 
1191 		sch_tree_unlock(sch);
1192 		err = fq_resize(sch, fq_log);
1193 		sch_tree_lock(sch);
1194 	}
1195 
1196 	while (sch->q.qlen > sch->limit) {
1197 		struct sk_buff *skb = qdisc_dequeue_internal(sch, false);
1198 
1199 		if (!skb)
1200 			break;
1201 
1202 		dropped_pkts++;
1203 		dropped_bytes += qdisc_pkt_len(skb);
1204 		rtnl_kfree_skbs(skb, skb);
1205 	}
1206 	qdisc_tree_reduce_backlog(sch, dropped_pkts, dropped_bytes);
1207 
1208 	sch_tree_unlock(sch);
1209 	return err;
1210 }
1211 
1212 static void fq_destroy(struct Qdisc *sch)
1213 {
1214 	struct fq_sched_data *q = qdisc_priv(sch);
1215 
1216 	fq_reset(sch);
1217 	fq_free(q->fq_root);
1218 	qdisc_watchdog_cancel(&q->watchdog);
1219 }
1220 
1221 static int fq_init(struct Qdisc *sch, struct nlattr *opt,
1222 		   struct netlink_ext_ack *extack)
1223 {
1224 	struct fq_sched_data *q = qdisc_priv(sch);
1225 	int i, err;
1226 
1227 	sch->limit		= 10000;
1228 	q->flow_plimit		= 100;
1229 	q->quantum		= 2 * psched_mtu(qdisc_dev(sch));
1230 	q->initial_quantum	= 10 * psched_mtu(qdisc_dev(sch));
1231 	q->flow_refill_delay	= msecs_to_jiffies(40);
1232 	q->flow_max_rate	= ~0UL;
1233 	q->time_next_delayed_flow = ~0ULL;
1234 	q->rate_enable		= 1;
1235 	for (i = 0; i < FQ_BANDS; i++) {
1236 		q->band_flows[i].new_flows.first = NULL;
1237 		q->band_flows[i].old_flows.first = NULL;
1238 	}
1239 	q->band_flows[0].quantum = 9 << 16;
1240 	q->band_flows[1].quantum = 3 << 16;
1241 	q->band_flows[2].quantum = 1 << 16;
1242 	q->delayed		= RB_ROOT;
1243 	q->fq_root		= NULL;
1244 	q->fq_trees_log		= ilog2(1024);
1245 	q->orphan_mask		= 1024 - 1;
1246 	q->low_rate_threshold	= 550000 / 8;
1247 
1248 	q->timer_slack = 10 * NSEC_PER_USEC; /* 10 usec of hrtimer slack */
1249 
1250 	q->horizon = 10ULL * NSEC_PER_SEC; /* 10 seconds */
1251 	q->horizon_drop = 1; /* by default, drop packets beyond horizon */
1252 
1253 	/* Default ce_threshold of 4294 seconds */
1254 	q->ce_threshold		= (u64)NSEC_PER_USEC * ~0U;
1255 
1256 	fq_prio2band_compress_crumb(sch_default_prio2band, q->prio2band);
1257 	qdisc_watchdog_init_clockid(&q->watchdog, sch, CLOCK_MONOTONIC);
1258 
1259 	if (opt)
1260 		err = fq_change(sch, opt, extack);
1261 	else
1262 		err = fq_resize(sch, q->fq_trees_log);
1263 
1264 	return err;
1265 }
1266 
1267 static int fq_dump(struct Qdisc *sch, struct sk_buff *skb)
1268 {
1269 	struct fq_sched_data *q = qdisc_priv(sch);
1270 	struct tc_prio_qopt prio = {
1271 		.bands = FQ_BANDS,
1272 	};
1273 	struct nlattr *opts;
1274 	u64 offload_horizon;
1275 	u64 ce_threshold;
1276 	s32 weights[3];
1277 	u64 horizon;
1278 
1279 	opts = nla_nest_start_noflag(skb, TCA_OPTIONS);
1280 	if (opts == NULL)
1281 		goto nla_put_failure;
1282 
1283 	/* TCA_FQ_FLOW_DEFAULT_RATE is not used anymore */
1284 
1285 	ce_threshold = READ_ONCE(q->ce_threshold);
1286 	do_div(ce_threshold, NSEC_PER_USEC);
1287 
1288 	horizon = READ_ONCE(q->horizon);
1289 	do_div(horizon, NSEC_PER_USEC);
1290 
1291 	offload_horizon = READ_ONCE(q->offload_horizon);
1292 	do_div(offload_horizon, NSEC_PER_USEC);
1293 
1294 	if (nla_put_u32(skb, TCA_FQ_PLIMIT,
1295 			READ_ONCE(sch->limit)) ||
1296 	    nla_put_u32(skb, TCA_FQ_FLOW_PLIMIT,
1297 			READ_ONCE(q->flow_plimit)) ||
1298 	    nla_put_u32(skb, TCA_FQ_QUANTUM,
1299 			READ_ONCE(q->quantum)) ||
1300 	    nla_put_u32(skb, TCA_FQ_INITIAL_QUANTUM,
1301 			READ_ONCE(q->initial_quantum)) ||
1302 	    nla_put_u32(skb, TCA_FQ_RATE_ENABLE,
1303 			READ_ONCE(q->rate_enable)) ||
1304 	    nla_put_u32(skb, TCA_FQ_FLOW_MAX_RATE,
1305 			min_t(unsigned long,
1306 			      READ_ONCE(q->flow_max_rate), ~0U)) ||
1307 	    nla_put_u32(skb, TCA_FQ_FLOW_REFILL_DELAY,
1308 			jiffies_to_usecs(READ_ONCE(q->flow_refill_delay))) ||
1309 	    nla_put_u32(skb, TCA_FQ_ORPHAN_MASK,
1310 			READ_ONCE(q->orphan_mask)) ||
1311 	    nla_put_u32(skb, TCA_FQ_LOW_RATE_THRESHOLD,
1312 			READ_ONCE(q->low_rate_threshold)) ||
1313 	    nla_put_u32(skb, TCA_FQ_CE_THRESHOLD, (u32)ce_threshold) ||
1314 	    nla_put_u32(skb, TCA_FQ_BUCKETS_LOG,
1315 			READ_ONCE(q->fq_trees_log)) ||
1316 	    nla_put_u32(skb, TCA_FQ_TIMER_SLACK,
1317 			READ_ONCE(q->timer_slack)) ||
1318 	    nla_put_u32(skb, TCA_FQ_HORIZON, (u32)horizon) ||
1319 	    nla_put_u32(skb, TCA_FQ_OFFLOAD_HORIZON, (u32)offload_horizon) ||
1320 	    nla_put_u8(skb, TCA_FQ_HORIZON_DROP,
1321 		       READ_ONCE(q->horizon_drop)))
1322 		goto nla_put_failure;
1323 
1324 	fq_prio2band_decompress_crumb(q->prio2band, prio.priomap);
1325 	if (nla_put(skb, TCA_FQ_PRIOMAP, sizeof(prio), &prio))
1326 		goto nla_put_failure;
1327 
1328 	weights[0] = READ_ONCE(q->band_flows[0].quantum);
1329 	weights[1] = READ_ONCE(q->band_flows[1].quantum);
1330 	weights[2] = READ_ONCE(q->band_flows[2].quantum);
1331 	if (nla_put(skb, TCA_FQ_WEIGHTS, sizeof(weights), &weights))
1332 		goto nla_put_failure;
1333 
1334 	return nla_nest_end(skb, opts);
1335 
1336 nla_put_failure:
1337 	return -1;
1338 }
1339 
1340 static int fq_dump_stats(struct Qdisc *sch, struct gnet_dump *d)
1341 {
1342 	struct fq_sched_data *q = qdisc_priv(sch);
1343 	struct tc_fq_qd_stats st;
1344 	int i;
1345 
1346 	st.pad = 0;
1347 
1348 	sch_tree_lock(sch);
1349 
1350 	st.gc_flows		  = q->stat_gc_flows;
1351 	st.highprio_packets	  = 0;
1352 	st.fastpath_packets	  = q->internal.stat_fastpath_packets;
1353 	st.tcp_retrans		  = 0;
1354 	st.throttled		  = q->stat_throttled;
1355 	st.flows_plimit		  = q->stat_flows_plimit;
1356 	st.pkts_too_long	  = q->stat_pkts_too_long;
1357 	st.allocation_errors	  = q->stat_allocation_errors;
1358 	st.time_next_delayed_flow = q->time_next_delayed_flow + q->timer_slack -
1359 				    ktime_get_ns();
1360 	st.flows		  = q->flows;
1361 	st.inactive_flows	  = q->inactive_flows;
1362 	st.throttled_flows	  = q->throttled_flows;
1363 	st.unthrottle_latency_ns  = min_t(unsigned long,
1364 					  q->unthrottle_latency_ns, ~0U);
1365 	st.ce_mark		  = q->stat_ce_mark;
1366 	st.horizon_drops	  = q->stat_horizon_drops;
1367 	st.horizon_caps		  = q->stat_horizon_caps;
1368 	for (i = 0; i < FQ_BANDS; i++) {
1369 		st.band_drops[i]  = q->stat_band_drops[i];
1370 		st.band_pkt_count[i] = q->band_pkt_count[i];
1371 	}
1372 	sch_tree_unlock(sch);
1373 
1374 	return gnet_stats_copy_app(d, &st, sizeof(st));
1375 }
1376 
1377 static struct Qdisc_ops fq_qdisc_ops __read_mostly = {
1378 	.id		=	"fq",
1379 	.priv_size	=	sizeof(struct fq_sched_data),
1380 
1381 	.enqueue	=	fq_enqueue,
1382 	.dequeue	=	fq_dequeue,
1383 	.peek		=	qdisc_peek_dequeued,
1384 	.init		=	fq_init,
1385 	.reset		=	fq_reset,
1386 	.destroy	=	fq_destroy,
1387 	.change		=	fq_change,
1388 	.dump		=	fq_dump,
1389 	.dump_stats	=	fq_dump_stats,
1390 	.owner		=	THIS_MODULE,
1391 };
1392 MODULE_ALIAS_NET_SCH("fq");
1393 
1394 static int __init fq_module_init(void)
1395 {
1396 	int ret;
1397 
1398 	fq_flow_cachep = kmem_cache_create("fq_flow_cache",
1399 					   sizeof(struct fq_flow),
1400 					   0, SLAB_HWCACHE_ALIGN, NULL);
1401 	if (!fq_flow_cachep)
1402 		return -ENOMEM;
1403 
1404 	ret = register_qdisc(&fq_qdisc_ops);
1405 	if (ret)
1406 		kmem_cache_destroy(fq_flow_cachep);
1407 	return ret;
1408 }
1409 
1410 static void __exit fq_module_exit(void)
1411 {
1412 	unregister_qdisc(&fq_qdisc_ops);
1413 	kmem_cache_destroy(fq_flow_cachep);
1414 }
1415 
1416 module_init(fq_module_init)
1417 module_exit(fq_module_exit)
1418 MODULE_AUTHOR("Eric Dumazet");
1419 MODULE_LICENSE("GPL");
1420 MODULE_DESCRIPTION("Fair Queue Packet Scheduler");
1421