xref: /linux/net/sched/sch_etf.c (revision 5d7f3375cd574fc83f0cd30839b21455c0e79ae2) 
1 // SPDX-License-Identifier: GPL-2.0
2 
3 /* net/sched/sch_etf.c  Earliest TxTime First queueing discipline.
4  *
5  * Authors:	Jesus Sanchez-Palencia <jesus.sanchez-palencia@intel.com>
6  *		Vinicius Costa Gomes <vinicius.gomes@intel.com>
7  */
8 
9 #include <linux/module.h>
10 #include <linux/types.h>
11 #include <linux/kernel.h>
12 #include <linux/string.h>
13 #include <linux/errno.h>
14 #include <linux/errqueue.h>
15 #include <linux/rbtree.h>
16 #include <linux/skbuff.h>
17 #include <linux/posix-timers.h>
18 #include <net/netlink.h>
19 #include <net/sch_generic.h>
20 #include <net/pkt_sched.h>
21 #include <net/sock.h>
22 
23 #define DEADLINE_MODE_IS_ON(x) ((x)->flags & TC_ETF_DEADLINE_MODE_ON)
24 #define OFFLOAD_IS_ON(x) ((x)->flags & TC_ETF_OFFLOAD_ON)
25 #define SKIP_SOCK_CHECK_IS_SET(x) ((x)->flags & TC_ETF_SKIP_SOCK_CHECK)
26 
27 struct etf_sched_data {
28 	bool offload;
29 	bool deadline_mode;
30 	bool skip_sock_check;
31 	int clockid;
32 	int queue;
33 	s32 delta; /* in ns */
34 	ktime_t last; /* The txtime of the last skb sent to the netdevice. */
35 	struct rb_root_cached head;
36 	struct qdisc_watchdog watchdog;
37 	ktime_t (*get_time)(void);
38 };
39 
40 static const struct nla_policy etf_policy[TCA_ETF_MAX + 1] = {
41 	[TCA_ETF_PARMS]	= { .len = sizeof(struct tc_etf_qopt) },
42 };
43 
44 static inline int validate_input_params(struct tc_etf_qopt *qopt,
45 					struct netlink_ext_ack *extack)
46 {
47 	/* Check if params comply to the following rules:
48 	 *	* Clockid and delta must be valid.
49 	 *
50 	 *	* Dynamic clockids are not supported.
51 	 *
52 	 *	* Delta must be a positive integer.
53 	 *
54 	 * Also note that for the HW offload case, we must
55 	 * expect that system clocks have been synchronized to PHC.
56 	 */
57 	if (qopt->clockid < 0) {
58 		NL_SET_ERR_MSG(extack, "Dynamic clockids are not supported");
59 		return -ENOTSUPP;
60 	}
61 
62 	if (qopt->clockid != CLOCK_TAI) {
63 		NL_SET_ERR_MSG(extack, "Invalid clockid. CLOCK_TAI must be used");
64 		return -EINVAL;
65 	}
66 
67 	if (qopt->delta < 0) {
68 		NL_SET_ERR_MSG(extack, "Delta must be positive");
69 		return -EINVAL;
70 	}
71 
72 	return 0;
73 }
74 
75 static bool is_packet_valid(struct Qdisc *sch, struct sk_buff *nskb)
76 {
77 	struct etf_sched_data *q = qdisc_priv(sch);
78 	ktime_t txtime = nskb->tstamp;
79 	struct sock *sk = nskb->sk;
80 	ktime_t now;
81 
82 	if (q->skip_sock_check)
83 		goto skip;
84 
85 	if (!sk || !sk_fullsock(sk))
86 		return false;
87 
88 	if (!sock_flag(sk, SOCK_TXTIME))
89 		return false;
90 
91 	/* We don't perform crosstimestamping.
92 	 * Drop if packet's clockid differs from qdisc's.
93 	 */
94 	if (sk->sk_clockid != q->clockid)
95 		return false;
96 
97 	if (sk->sk_txtime_deadline_mode != q->deadline_mode)
98 		return false;
99 
100 skip:
101 	now = q->get_time();
102 	if (ktime_before(txtime, now) || ktime_before(txtime, q->last))
103 		return false;
104 
105 	return true;
106 }
107 
108 static struct sk_buff *etf_peek_timesortedlist(struct Qdisc *sch)
109 {
110 	struct etf_sched_data *q = qdisc_priv(sch);
111 	struct rb_node *p;
112 
113 	p = rb_first_cached(&q->head);
114 	if (!p)
115 		return NULL;
116 
117 	return rb_to_skb(p);
118 }
119 
120 static void reset_watchdog(struct Qdisc *sch)
121 {
122 	struct etf_sched_data *q = qdisc_priv(sch);
123 	struct sk_buff *skb = etf_peek_timesortedlist(sch);
124 	ktime_t next;
125 
126 	if (!skb) {
127 		qdisc_watchdog_cancel(&q->watchdog);
128 		return;
129 	}
130 
131 	next = ktime_sub_ns(skb->tstamp, q->delta);
132 	qdisc_watchdog_schedule_ns(&q->watchdog, ktime_to_ns(next));
133 }
134 
135 static void report_sock_error(struct sk_buff *skb, u32 err, u8 code)
136 {
137 	struct sock_exterr_skb *serr;
138 	struct sk_buff *clone;
139 	ktime_t txtime = skb->tstamp;
140 	struct sock *sk = skb->sk;
141 
142 	if (!sk || !sk_fullsock(sk) || !(sk->sk_txtime_report_errors))
143 		return;
144 
145 	clone = skb_clone(skb, GFP_ATOMIC);
146 	if (!clone)
147 		return;
148 
149 	serr = SKB_EXT_ERR(clone);
150 	memset(serr, 0, sizeof(*serr));
151 	serr->ee.ee_errno = err;
152 	serr->ee.ee_origin = SO_EE_ORIGIN_TXTIME;
153 	serr->ee.ee_type = 0;
154 	serr->ee.ee_code = code;
155 	serr->ee.ee_pad = 0;
156 	serr->ee.ee_data = (txtime >> 32); /* high part of tstamp */
157 	serr->ee.ee_info = txtime; /* low part of tstamp */
158 
159 	if (sock_queue_err_skb(sk, clone))
160 		kfree_skb(clone);
161 }
162 
163 static int etf_enqueue_timesortedlist(struct sk_buff *nskb, struct Qdisc *sch,
164 				      struct sk_buff **to_free)
165 {
166 	struct etf_sched_data *q = qdisc_priv(sch);
167 	struct rb_node **p = &q->head.rb_root.rb_node, *parent = NULL;
168 	ktime_t txtime = nskb->tstamp;
169 	bool leftmost = true;
170 
171 	if (!is_packet_valid(sch, nskb)) {
172 		report_sock_error(nskb, EINVAL,
173 				  SO_EE_CODE_TXTIME_INVALID_PARAM);
174 		return qdisc_drop(nskb, sch, to_free);
175 	}
176 
177 	while (*p) {
178 		struct sk_buff *skb;
179 
180 		parent = *p;
181 		skb = rb_to_skb(parent);
182 		if (ktime_compare(txtime, skb->tstamp) >= 0) {
183 			p = &parent->rb_right;
184 			leftmost = false;
185 		} else {
186 			p = &parent->rb_left;
187 		}
188 	}
189 	rb_link_node(&nskb->rbnode, parent, p);
190 	rb_insert_color_cached(&nskb->rbnode, &q->head, leftmost);
191 
192 	qdisc_qstats_backlog_inc(sch, nskb);
193 	qdisc_qlen_inc(sch);
194 
195 	/* Now we may need to re-arm the qdisc watchdog for the next packet. */
196 	reset_watchdog(sch);
197 
198 	return NET_XMIT_SUCCESS;
199 }
200 
201 static void timesortedlist_drop(struct Qdisc *sch, struct sk_buff *skb,
202 				ktime_t now)
203 {
204 	struct etf_sched_data *q = qdisc_priv(sch);
205 	struct sk_buff *to_free = NULL;
206 	struct sk_buff *tmp = NULL;
207 
208 	skb_rbtree_walk_from_safe(skb, tmp) {
209 		if (ktime_after(skb->tstamp, now))
210 			break;
211 
212 		rb_erase_cached(&skb->rbnode, &q->head);
213 
214 		/* The rbnode field in the skb re-uses these fields, now that
215 		 * we are done with the rbnode, reset them.
216 		 */
217 		skb->next = NULL;
218 		skb->prev = NULL;
219 		skb->dev = qdisc_dev(sch);
220 
221 		report_sock_error(skb, ECANCELED, SO_EE_CODE_TXTIME_MISSED);
222 
223 		qdisc_qstats_backlog_dec(sch, skb);
224 		qdisc_drop(skb, sch, &to_free);
225 		qdisc_qstats_overlimit(sch);
226 		qdisc_qlen_dec(sch);
227 	}
228 
229 	kfree_skb_list(to_free);
230 }
231 
232 static void timesortedlist_remove(struct Qdisc *sch, struct sk_buff *skb)
233 {
234 	struct etf_sched_data *q = qdisc_priv(sch);
235 
236 	rb_erase_cached(&skb->rbnode, &q->head);
237 
238 	/* The rbnode field in the skb re-uses these fields, now that
239 	 * we are done with the rbnode, reset them.
240 	 */
241 	skb->next = NULL;
242 	skb->prev = NULL;
243 	skb->dev = qdisc_dev(sch);
244 
245 	qdisc_qstats_backlog_dec(sch, skb);
246 
247 	qdisc_bstats_update(sch, skb);
248 
249 	q->last = skb->tstamp;
250 
251 	qdisc_qlen_dec(sch);
252 }
253 
254 static struct sk_buff *etf_dequeue_timesortedlist(struct Qdisc *sch)
255 {
256 	struct etf_sched_data *q = qdisc_priv(sch);
257 	struct sk_buff *skb;
258 	ktime_t now, next;
259 
260 	skb = etf_peek_timesortedlist(sch);
261 	if (!skb)
262 		return NULL;
263 
264 	now = q->get_time();
265 
266 	/* Drop if packet has expired while in queue. */
267 	if (ktime_before(skb->tstamp, now)) {
268 		timesortedlist_drop(sch, skb, now);
269 		skb = NULL;
270 		goto out;
271 	}
272 
273 	/* When in deadline mode, dequeue as soon as possible and change the
274 	 * txtime from deadline to (now + delta).
275 	 */
276 	if (q->deadline_mode) {
277 		timesortedlist_remove(sch, skb);
278 		skb->tstamp = now;
279 		goto out;
280 	}
281 
282 	next = ktime_sub_ns(skb->tstamp, q->delta);
283 
284 	/* Dequeue only if now is within the [txtime - delta, txtime] range. */
285 	if (ktime_after(now, next))
286 		timesortedlist_remove(sch, skb);
287 	else
288 		skb = NULL;
289 
290 out:
291 	/* Now we may need to re-arm the qdisc watchdog for the next packet. */
292 	reset_watchdog(sch);
293 
294 	return skb;
295 }
296 
297 static void etf_disable_offload(struct net_device *dev,
298 				struct etf_sched_data *q)
299 {
300 	struct tc_etf_qopt_offload etf = { };
301 	const struct net_device_ops *ops;
302 	int err;
303 
304 	if (!q->offload)
305 		return;
306 
307 	ops = dev->netdev_ops;
308 	if (!ops->ndo_setup_tc)
309 		return;
310 
311 	etf.queue = q->queue;
312 	etf.enable = 0;
313 
314 	err = ops->ndo_setup_tc(dev, TC_SETUP_QDISC_ETF, &etf);
315 	if (err < 0)
316 		pr_warn("Couldn't disable ETF offload for queue %d\n",
317 			etf.queue);
318 }
319 
320 static int etf_enable_offload(struct net_device *dev, struct etf_sched_data *q,
321 			      struct netlink_ext_ack *extack)
322 {
323 	const struct net_device_ops *ops = dev->netdev_ops;
324 	struct tc_etf_qopt_offload etf = { };
325 	int err;
326 
327 	if (!ops->ndo_setup_tc) {
328 		NL_SET_ERR_MSG(extack, "Specified device does not support ETF offload");
329 		return -EOPNOTSUPP;
330 	}
331 
332 	etf.queue = q->queue;
333 	etf.enable = 1;
334 
335 	err = ops->ndo_setup_tc(dev, TC_SETUP_QDISC_ETF, &etf);
336 	if (err < 0) {
337 		NL_SET_ERR_MSG(extack, "Specified device failed to setup ETF hardware offload");
338 		return err;
339 	}
340 
341 	return 0;
342 }
343 
344 static int etf_init(struct Qdisc *sch, struct nlattr *opt,
345 		    struct netlink_ext_ack *extack)
346 {
347 	struct etf_sched_data *q = qdisc_priv(sch);
348 	struct net_device *dev = qdisc_dev(sch);
349 	struct nlattr *tb[TCA_ETF_MAX + 1];
350 	struct tc_etf_qopt *qopt;
351 	int err;
352 
353 	if (!opt) {
354 		NL_SET_ERR_MSG(extack,
355 			       "Missing ETF qdisc options which are mandatory");
356 		return -EINVAL;
357 	}
358 
359 	err = nla_parse_nested_deprecated(tb, TCA_ETF_MAX, opt, etf_policy,
360 					  extack);
361 	if (err < 0)
362 		return err;
363 
364 	if (!tb[TCA_ETF_PARMS]) {
365 		NL_SET_ERR_MSG(extack, "Missing mandatory ETF parameters");
366 		return -EINVAL;
367 	}
368 
369 	qopt = nla_data(tb[TCA_ETF_PARMS]);
370 
371 	pr_debug("delta %d clockid %d offload %s deadline %s\n",
372 		 qopt->delta, qopt->clockid,
373 		 OFFLOAD_IS_ON(qopt) ? "on" : "off",
374 		 DEADLINE_MODE_IS_ON(qopt) ? "on" : "off");
375 
376 	err = validate_input_params(qopt, extack);
377 	if (err < 0)
378 		return err;
379 
380 	q->queue = sch->dev_queue - netdev_get_tx_queue(dev, 0);
381 
382 	if (OFFLOAD_IS_ON(qopt)) {
383 		err = etf_enable_offload(dev, q, extack);
384 		if (err < 0)
385 			return err;
386 	}
387 
388 	/* Everything went OK, save the parameters used. */
389 	q->delta = qopt->delta;
390 	q->clockid = qopt->clockid;
391 	q->offload = OFFLOAD_IS_ON(qopt);
392 	q->deadline_mode = DEADLINE_MODE_IS_ON(qopt);
393 	q->skip_sock_check = SKIP_SOCK_CHECK_IS_SET(qopt);
394 
395 	switch (q->clockid) {
396 	case CLOCK_REALTIME:
397 		q->get_time = ktime_get_real;
398 		break;
399 	case CLOCK_MONOTONIC:
400 		q->get_time = ktime_get;
401 		break;
402 	case CLOCK_BOOTTIME:
403 		q->get_time = ktime_get_boottime;
404 		break;
405 	case CLOCK_TAI:
406 		q->get_time = ktime_get_clocktai;
407 		break;
408 	default:
409 		NL_SET_ERR_MSG(extack, "Clockid is not supported");
410 		return -ENOTSUPP;
411 	}
412 
413 	qdisc_watchdog_init_clockid(&q->watchdog, sch, q->clockid);
414 
415 	return 0;
416 }
417 
418 static void timesortedlist_clear(struct Qdisc *sch)
419 {
420 	struct etf_sched_data *q = qdisc_priv(sch);
421 	struct rb_node *p = rb_first_cached(&q->head);
422 
423 	while (p) {
424 		struct sk_buff *skb = rb_to_skb(p);
425 
426 		p = rb_next(p);
427 
428 		rb_erase_cached(&skb->rbnode, &q->head);
429 		rtnl_kfree_skbs(skb, skb);
430 		qdisc_qlen_dec(sch);
431 	}
432 }
433 
434 static void etf_reset(struct Qdisc *sch)
435 {
436 	struct etf_sched_data *q = qdisc_priv(sch);
437 
438 	/* Only cancel watchdog if it's been initialized. */
439 	if (q->watchdog.qdisc == sch)
440 		qdisc_watchdog_cancel(&q->watchdog);
441 
442 	/* No matter which mode we are on, it's safe to clear both lists. */
443 	timesortedlist_clear(sch);
444 	__qdisc_reset_queue(&sch->q);
445 
446 	q->last = 0;
447 }
448 
449 static void etf_destroy(struct Qdisc *sch)
450 {
451 	struct etf_sched_data *q = qdisc_priv(sch);
452 	struct net_device *dev = qdisc_dev(sch);
453 
454 	/* Only cancel watchdog if it's been initialized. */
455 	if (q->watchdog.qdisc == sch)
456 		qdisc_watchdog_cancel(&q->watchdog);
457 
458 	etf_disable_offload(dev, q);
459 }
460 
461 static int etf_dump(struct Qdisc *sch, struct sk_buff *skb)
462 {
463 	struct etf_sched_data *q = qdisc_priv(sch);
464 	struct tc_etf_qopt opt = { };
465 	struct nlattr *nest;
466 
467 	nest = nla_nest_start_noflag(skb, TCA_OPTIONS);
468 	if (!nest)
469 		goto nla_put_failure;
470 
471 	opt.delta = READ_ONCE(q->delta);
472 	opt.clockid = READ_ONCE(q->clockid);
473 	if (READ_ONCE(q->offload))
474 		opt.flags |= TC_ETF_OFFLOAD_ON;
475 
476 	if (READ_ONCE(q->deadline_mode))
477 		opt.flags |= TC_ETF_DEADLINE_MODE_ON;
478 
479 	if (READ_ONCE(q->skip_sock_check))
480 		opt.flags |= TC_ETF_SKIP_SOCK_CHECK;
481 
482 	if (nla_put(skb, TCA_ETF_PARMS, sizeof(opt), &opt))
483 		goto nla_put_failure;
484 
485 	return nla_nest_end(skb, nest);
486 
487 nla_put_failure:
488 	nla_nest_cancel(skb, nest);
489 	return -1;
490 }
491 
492 static struct Qdisc_ops etf_qdisc_ops __read_mostly = {
493 	.id		=	"etf",
494 	.priv_size	=	sizeof(struct etf_sched_data),
495 	.enqueue	=	etf_enqueue_timesortedlist,
496 	.dequeue	=	etf_dequeue_timesortedlist,
497 	.peek		=	etf_peek_timesortedlist,
498 	.init		=	etf_init,
499 	.reset		=	etf_reset,
500 	.destroy	=	etf_destroy,
501 	.dump		=	etf_dump,
502 	.owner		=	THIS_MODULE,
503 };
504 MODULE_ALIAS_NET_SCH("etf");
505 
506 static int __init etf_module_init(void)
507 {
508 	return register_qdisc(&etf_qdisc_ops);
509 }
510 
511 static void __exit etf_module_exit(void)
512 {
513 	unregister_qdisc(&etf_qdisc_ops);
514 }
515 module_init(etf_module_init)
516 module_exit(etf_module_exit)
517 MODULE_LICENSE("GPL");
518 MODULE_DESCRIPTION("Earliest TxTime First (ETF) qdisc");
519