[linux-block.git] / net / sched / sch_etf.c

// SPDX-License-Identifier: GPL-2.0

/* net/sched/sch_etf.c  Earliest TxTime First queueing discipline.
 *
 * Authors:	Jesus Sanchez-Palencia <jesus.sanchez-palencia@intel.com>
 *		Vinicius Costa Gomes <vinicius.gomes@intel.com>
 */

#include <linux/module.h>
#include <linux/types.h>
#include <linux/kernel.h>
#include <linux/string.h>
#include <linux/errno.h>
#include <linux/errqueue.h>
#include <linux/rbtree.h>
#include <linux/skbuff.h>
#include <linux/posix-timers.h>
#include <net/netlink.h>
#include <net/sch_generic.h>
#include <net/pkt_sched.h>
#include <net/sock.h>

#define DEADLINE_MODE_IS_ON(x) ((x)->flags & TC_ETF_DEADLINE_MODE_ON)
#define OFFLOAD_IS_ON(x) ((x)->flags & TC_ETF_OFFLOAD_ON)
#define SKIP_SOCK_CHECK_IS_SET(x) ((x)->flags & TC_ETF_SKIP_SOCK_CHECK)

struct etf_sched_data {
	bool offload;
	bool deadline_mode;
	bool skip_sock_check;
	int clockid;
	int queue;
	s32 delta; /* in ns */
	ktime_t last; /* The txtime of the last skb sent to the netdevice. */
	struct rb_root_cached head;
	struct qdisc_watchdog watchdog;
	ktime_t (*get_time)(void);
};

static const struct nla_policy etf_policy[TCA_ETF_MAX + 1] = {
	[TCA_ETF_PARMS]	= { .len = sizeof(struct tc_etf_qopt) },
};

static inline int validate_input_params(struct tc_etf_qopt *qopt,
					struct netlink_ext_ack *extack)
{
	/* Check if params comply to the following rules:
	 *	* Clockid and delta must be valid.
	 *
	 *	* Dynamic clockids are not supported.
	 *
	 *	* Delta must be a positive integer.
	 *
	 * Also note that for the HW offload case, we must
	 * expect that system clocks have been synchronized to PHC.
	 */
	if (qopt->clockid < 0) {
		NL_SET_ERR_MSG(extack, "Dynamic clockids are not supported");
		return -ENOTSUPP;
	}

	if (qopt->clockid != CLOCK_TAI) {
		NL_SET_ERR_MSG(extack, "Invalid clockid. CLOCK_TAI must be used");
		return -EINVAL;
	}

	if (qopt->delta < 0) {
		NL_SET_ERR_MSG(extack, "Delta must be positive");
		return -EINVAL;
	}

	return 0;
}

static bool is_packet_valid(struct Qdisc *sch, struct sk_buff *nskb)
{
	struct etf_sched_data *q = qdisc_priv(sch);
	ktime_t txtime = nskb->tstamp;
	struct sock *sk = nskb->sk;
	ktime_t now;

	if (q->skip_sock_check)
		goto skip;

	if (!sk || !sk_fullsock(sk))
		return false;

	if (!sock_flag(sk, SOCK_TXTIME))
		return false;

	/* We don't perform crosstimestamping.
	 * Drop if packet's clockid differs from qdisc's.
	 */
	if (sk->sk_clockid != q->clockid)
		return false;

	if (sk->sk_txtime_deadline_mode != q->deadline_mode)
		return false;

skip:
	now = q->get_time();
	if (ktime_before(txtime, now) || ktime_before(txtime, q->last))
		return false;

	return true;
}

static struct sk_buff *etf_peek_timesortedlist(struct Qdisc *sch)
{
	struct etf_sched_data *q = qdisc_priv(sch);
	struct rb_node *p;

	p = rb_first_cached(&q->head);
	if (!p)
		return NULL;

	return rb_to_skb(p);
}

static void reset_watchdog(struct Qdisc *sch)
{
	struct etf_sched_data *q = qdisc_priv(sch);
	struct sk_buff *skb = etf_peek_timesortedlist(sch);
	ktime_t next;

	if (!skb) {
		qdisc_watchdog_cancel(&q->watchdog);
		return;
	}

	next = ktime_sub_ns(skb->tstamp, q->delta);
	qdisc_watchdog_schedule_ns(&q->watchdog, ktime_to_ns(next));
}

static void report_sock_error(struct sk_buff *skb, u32 err, u8 code)
{
	struct sock_exterr_skb *serr;
	struct sk_buff *clone;
	ktime_t txtime = skb->tstamp;
	struct sock *sk = skb->sk;

	if (!sk || !sk_fullsock(sk) || !(sk->sk_txtime_report_errors))
		return;

	clone = skb_clone(skb, GFP_ATOMIC);
	if (!clone)
		return;

	serr = SKB_EXT_ERR(clone);
	serr->ee.ee_errno = err;
	serr->ee.ee_origin = SO_EE_ORIGIN_TXTIME;
	serr->ee.ee_type = 0;
	serr->ee.ee_code = code;
	serr->ee.ee_pad = 0;
	serr->ee.ee_data = (txtime >> 32); /* high part of tstamp */
	serr->ee.ee_info = txtime; /* low part of tstamp */

	if (sock_queue_err_skb(sk, clone))
		kfree_skb(clone);
}

static int etf_enqueue_timesortedlist(struct sk_buff *nskb, struct Qdisc *sch,
				      struct sk_buff **to_free)
{
	struct etf_sched_data *q = qdisc_priv(sch);
	struct rb_node **p = &q->head.rb_root.rb_node, *parent = NULL;
	ktime_t txtime = nskb->tstamp;
	bool leftmost = true;

	if (!is_packet_valid(sch, nskb)) {
		report_sock_error(nskb, EINVAL,
				  SO_EE_CODE_TXTIME_INVALID_PARAM);
		return qdisc_drop(nskb, sch, to_free);
	}

	while (*p) {
		struct sk_buff *skb;

		parent = *p;
		skb = rb_to_skb(parent);
		if (ktime_compare(txtime, skb->tstamp) >= 0) {
			p = &parent->rb_right;
			leftmost = false;
		} else {
			p = &parent->rb_left;
		}
	}
	rb_link_node(&nskb->rbnode, parent, p);
	rb_insert_color_cached(&nskb->rbnode, &q->head, leftmost);

	qdisc_qstats_backlog_inc(sch, nskb);
	sch->q.qlen++;

	/* Now we may need to re-arm the qdisc watchdog for the next packet. */
	reset_watchdog(sch);

	return NET_XMIT_SUCCESS;
}

static void timesortedlist_drop(struct Qdisc *sch, struct sk_buff *skb,
				ktime_t now)
{
	struct etf_sched_data *q = qdisc_priv(sch);
	struct sk_buff *to_free = NULL;
	struct sk_buff *tmp = NULL;

	skb_rbtree_walk_from_safe(skb, tmp) {
		if (ktime_after(skb->tstamp, now))
			break;

		rb_erase_cached(&skb->rbnode, &q->head);

		/* The rbnode field in the skb re-uses these fields, now that
		 * we are done with the rbnode, reset them.
		 */
		skb->next = NULL;
		skb->prev = NULL;
		skb->dev = qdisc_dev(sch);

		report_sock_error(skb, ECANCELED, SO_EE_CODE_TXTIME_MISSED);

		qdisc_qstats_backlog_dec(sch, skb);
		qdisc_drop(skb, sch, &to_free);
		qdisc_qstats_overlimit(sch);
		sch->q.qlen--;
	}

	kfree_skb_list(to_free);
}

static void timesortedlist_remove(struct Qdisc *sch, struct sk_buff *skb)
{
	struct etf_sched_data *q = qdisc_priv(sch);

	rb_erase_cached(&skb->rbnode, &q->head);

	/* The rbnode field in the skb re-uses these fields, now that
	 * we are done with the rbnode, reset them.
	 */
	skb->next = NULL;
	skb->prev = NULL;
	skb->dev = qdisc_dev(sch);

	qdisc_qstats_backlog_dec(sch, skb);

	qdisc_bstats_update(sch, skb);

	q->last = skb->tstamp;

	sch->q.qlen--;
}

static struct sk_buff *etf_dequeue_timesortedlist(struct Qdisc *sch)
{
	struct etf_sched_data *q = qdisc_priv(sch);
	struct sk_buff *skb;
	ktime_t now, next;

	skb = etf_peek_timesortedlist(sch);
	if (!skb)
		return NULL;

	now = q->get_time();

	/* Drop if packet has expired while in queue. */
	if (ktime_before(skb->tstamp, now)) {
		timesortedlist_drop(sch, skb, now);
		skb = NULL;
		goto out;
	}

	/* When in deadline mode, dequeue as soon as possible and change the
	 * txtime from deadline to (now + delta).
	 */
	if (q->deadline_mode) {
		timesortedlist_remove(sch, skb);
		skb->tstamp = now;
		goto out;
	}

	next = ktime_sub_ns(skb->tstamp, q->delta);

	/* Dequeue only if now is within the [txtime - delta, txtime] range. */
	if (ktime_after(now, next))
		timesortedlist_remove(sch, skb);
	else
		skb = NULL;

out:
	/* Now we may need to re-arm the qdisc watchdog for the next packet. */
	reset_watchdog(sch);

	return skb;
}

static void etf_disable_offload(struct net_device *dev,
				struct etf_sched_data *q)
{
	struct tc_etf_qopt_offload etf = { };
	const struct net_device_ops *ops;
	int err;

	if (!q->offload)
		return;

	ops = dev->netdev_ops;
	if (!ops->ndo_setup_tc)
		return;

	etf.queue = q->queue;
	etf.enable = 0;

	err = ops->ndo_setup_tc(dev, TC_SETUP_QDISC_ETF, &etf);
	if (err < 0)
		pr_warn("Couldn't disable ETF offload for queue %d\n",
			etf.queue);
}

static int etf_enable_offload(struct net_device *dev, struct etf_sched_data *q,
			      struct netlink_ext_ack *extack)
{
	const struct net_device_ops *ops = dev->netdev_ops;
	struct tc_etf_qopt_offload etf = { };
	int err;

	if (!ops->ndo_setup_tc) {
		NL_SET_ERR_MSG(extack, "Specified device does not support ETF offload");
		return -EOPNOTSUPP;
	}

	etf.queue = q->queue;
	etf.enable = 1;

	err = ops->ndo_setup_tc(dev, TC_SETUP_QDISC_ETF, &etf);
	if (err < 0) {
		NL_SET_ERR_MSG(extack, "Specified device failed to setup ETF hardware offload");
		return err;
	}

	return 0;
}

static int etf_init(struct Qdisc *sch, struct nlattr *opt,
		    struct netlink_ext_ack *extack)
{
	struct etf_sched_data *q = qdisc_priv(sch);
	struct net_device *dev = qdisc_dev(sch);
	struct nlattr *tb[TCA_ETF_MAX + 1];
	struct tc_etf_qopt *qopt;
	int err;

	if (!opt) {
		NL_SET_ERR_MSG(extack,
			       "Missing ETF qdisc options which are mandatory");
		return -EINVAL;
	}

	err = nla_parse_nested_deprecated(tb, TCA_ETF_MAX, opt, etf_policy,
					  extack);
	if (err < 0)
		return err;

	if (!tb[TCA_ETF_PARMS]) {
		NL_SET_ERR_MSG(extack, "Missing mandatory ETF parameters");
		return -EINVAL;
	}

	qopt = nla_data(tb[TCA_ETF_PARMS]);

	pr_debug("delta %d clockid %d offload %s deadline %s\n",
		 qopt->delta, qopt->clockid,
		 OFFLOAD_IS_ON(qopt) ? "on" : "off",
		 DEADLINE_MODE_IS_ON(qopt) ? "on" : "off");

	err = validate_input_params(qopt, extack);
	if (err < 0)
		return err;

	q->queue = sch->dev_queue - netdev_get_tx_queue(dev, 0);

	if (OFFLOAD_IS_ON(qopt)) {
		err = etf_enable_offload(dev, q, extack);
		if (err < 0)
			return err;
	}

	/* Everything went OK, save the parameters used. */
	q->delta = qopt->delta;
	q->clockid = qopt->clockid;
	q->offload = OFFLOAD_IS_ON(qopt);
	q->deadline_mode = DEADLINE_MODE_IS_ON(qopt);
	q->skip_sock_check = SKIP_SOCK_CHECK_IS_SET(qopt);

	switch (q->clockid) {
	case CLOCK_REALTIME:
		q->get_time = ktime_get_real;
		break;
	case CLOCK_MONOTONIC:
		q->get_time = ktime_get;
		break;
	case CLOCK_BOOTTIME:
		q->get_time = ktime_get_boottime;
		break;
	case CLOCK_TAI:
		q->get_time = ktime_get_clocktai;
		break;
	default:
		NL_SET_ERR_MSG(extack, "Clockid is not supported");
		return -ENOTSUPP;
	}

	qdisc_watchdog_init_clockid(&q->watchdog, sch, q->clockid);

	return 0;
}

static void timesortedlist_clear(struct Qdisc *sch)
{
	struct etf_sched_data *q = qdisc_priv(sch);
	struct rb_node *p = rb_first_cached(&q->head);

	while (p) {
		struct sk_buff *skb = rb_to_skb(p);

		p = rb_next(p);

		rb_erase_cached(&skb->rbnode, &q->head);
		rtnl_kfree_skbs(skb, skb);
		sch->q.qlen--;
	}
}

static void etf_reset(struct Qdisc *sch)
{
	struct etf_sched_data *q = qdisc_priv(sch);

	/* Only cancel watchdog if it's been initialized. */
	if (q->watchdog.qdisc == sch)
		qdisc_watchdog_cancel(&q->watchdog);

	/* No matter which mode we are on, it's safe to clear both lists. */
	timesortedlist_clear(sch);
	__qdisc_reset_queue(&sch->q);

	q->last = 0;
}

static void etf_destroy(struct Qdisc *sch)
{
	struct etf_sched_data *q = qdisc_priv(sch);
	struct net_device *dev = qdisc_dev(sch);

	/* Only cancel watchdog if it's been initialized. */
	if (q->watchdog.qdisc == sch)
		qdisc_watchdog_cancel(&q->watchdog);

	etf_disable_offload(dev, q);
}

static int etf_dump(struct Qdisc *sch, struct sk_buff *skb)
{
	struct etf_sched_data *q = qdisc_priv(sch);
	struct tc_etf_qopt opt = { };
	struct nlattr *nest;

	nest = nla_nest_start_noflag(skb, TCA_OPTIONS);
	if (!nest)
		goto nla_put_failure;

	opt.delta = q->delta;
	opt.clockid = q->clockid;
	if (q->offload)
		opt.flags |= TC_ETF_OFFLOAD_ON;

	if (q->deadline_mode)
		opt.flags |= TC_ETF_DEADLINE_MODE_ON;

	if (q->skip_sock_check)
		opt.flags |= TC_ETF_SKIP_SOCK_CHECK;

	if (nla_put(skb, TCA_ETF_PARMS, sizeof(opt), &opt))
		goto nla_put_failure;

	return nla_nest_end(skb, nest);

nla_put_failure:
	nla_nest_cancel(skb, nest);
	return -1;
}

static struct Qdisc_ops etf_qdisc_ops __read_mostly = {
	.id		=	"etf",
	.priv_size	=	sizeof(struct etf_sched_data),
	.enqueue	=	etf_enqueue_timesortedlist,
	.dequeue	=	etf_dequeue_timesortedlist,
	.peek		=	etf_peek_timesortedlist,
	.init		=	etf_init,
	.reset		=	etf_reset,
	.destroy	=	etf_destroy,
	.dump		=	etf_dump,
	.owner		=	THIS_MODULE,
};

static int __init etf_module_init(void)
{
	return register_qdisc(&etf_qdisc_ops);
}

static void __exit etf_module_exit(void)
{
	unregister_qdisc(&etf_qdisc_ops);
}
module_init(etf_module_init)
module_exit(etf_module_exit)
MODULE_LICENSE("GPL");
Commit	Line	Data
25db26a9 VCG	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>
4b15c707	14	#include <linux/errqueue.h>
25db26a9 VCG	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)
88cab771	24	#define OFFLOAD_IS_ON(x) ((x)->flags & TC_ETF_OFFLOAD_ON)
d14d2b20	25	#define SKIP_SOCK_CHECK_IS_SET(x) ((x)->flags & TC_ETF_SKIP_SOCK_CHECK)
25db26a9 VCG	26
25db26a9 VCG	27	struct etf_sched_data {
88cab771	28	bool offload;
25db26a9	29	bool deadline_mode;
d14d2b20	30	bool skip_sock_check;
25db26a9 VCG	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. */
09fd4860	35	struct rb_root_cached head;
25db26a9 VCG	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.
88cab771 JSP	53	*
	54	* Also note that for the HW offload case, we must
	55	* expect that system clocks have been synchronized to PHC.
25db26a9 VCG	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
d14d2b20 VP	82	if (q->skip_sock_check)
	83	goto skip;
	84
a1211bf9	85	if (!sk \|\| !sk_fullsock(sk))
25db26a9 VCG	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
d14d2b20	100	skip:
25db26a9 VCG	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
09fd4860	113	p = rb_first_cached(&q->head);
25db26a9 VCG	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
3fcbdaee JSP	126	if (!skb) {
3fcbdaee JSP	127	qdisc_watchdog_cancel(&q->watchdog);
25db26a9	128	return;
3fcbdaee	129	}
25db26a9 VCG	130
	131	next = ktime_sub_ns(skb->tstamp, q->delta);
	132	qdisc_watchdog_schedule_ns(&q->watchdog, ktime_to_ns(next));
	133	}
	134
4b15c707 JSP	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;
a1211bf9	140	struct sock *sk = skb->sk;
4b15c707	141
a1211bf9	142	if (!sk \|\| !sk_fullsock(sk) \|\| !(sk->sk_txtime_report_errors))
4b15c707 JSP	143	return;
	144
	145	clone = skb_clone(skb, GFP_ATOMIC);
	146	if (!clone)
	147	return;
	148
	149	serr = SKB_EXT_ERR(clone);
	150	serr->ee.ee_errno = err;
	151	serr->ee.ee_origin = SO_EE_ORIGIN_TXTIME;
	152	serr->ee.ee_type = 0;
	153	serr->ee.ee_code = code;
	154	serr->ee.ee_pad = 0;
	155	serr->ee.ee_data = (txtime >> 32); /* high part of tstamp */
	156	serr->ee.ee_info = txtime; /* low part of tstamp */
	157
a1211bf9	158	if (sock_queue_err_skb(sk, clone))
4b15c707 JSP	159	kfree_skb(clone);
	160	}
	161
25db26a9 VCG	162	static int etf_enqueue_timesortedlist(struct sk_buff nskb, struct Qdisc sch,
	163	struct sk_buff **to_free)
	164	{
	165	struct etf_sched_data *q = qdisc_priv(sch);
09fd4860	166	struct rb_node *p = &q->head.rb_root.rb_node, parent = NULL;
25db26a9	167	ktime_t txtime = nskb->tstamp;
09fd4860	168	bool leftmost = true;
25db26a9	169
4b15c707 JSP	170	if (!is_packet_valid(sch, nskb)) {
	171	report_sock_error(nskb, EINVAL,
	172	SO_EE_CODE_TXTIME_INVALID_PARAM);
25db26a9	173	return qdisc_drop(nskb, sch, to_free);
4b15c707	174	}
25db26a9 VCG	175
	176	while (*p) {
	177	struct sk_buff *skb;
	178
	179	parent = *p;
	180	skb = rb_to_skb(parent);
28aa7c86	181	if (ktime_compare(txtime, skb->tstamp) >= 0) {
25db26a9	182	p = &parent->rb_right;
09fd4860 JSP	183	leftmost = false;
09fd4860 JSP	184	} else {
25db26a9	185	p = &parent->rb_left;
09fd4860	186	}
25db26a9 VCG	187	}
25db26a9 VCG	188	rb_link_node(&nskb->rbnode, parent, p);
09fd4860	189	rb_insert_color_cached(&nskb->rbnode, &q->head, leftmost);
25db26a9 VCG	190
	191	qdisc_qstats_backlog_inc(sch, nskb);
	192	sch->q.qlen++;
	193
	194	/* Now we may need to re-arm the qdisc watchdog for the next packet. */
	195	reset_watchdog(sch);
	196
	197	return NET_XMIT_SUCCESS;
	198	}
	199
37342bda JSP	200	static void timesortedlist_drop(struct Qdisc sch, struct sk_buff skb,
37342bda JSP	201	ktime_t now)
25db26a9 VCG	202	{
25db26a9 VCG	203	struct etf_sched_data *q = qdisc_priv(sch);
cbeeb8ef	204	struct sk_buff *to_free = NULL;
37342bda	205	struct sk_buff *tmp = NULL;
25db26a9	206
37342bda JSP	207	skb_rbtree_walk_from_safe(skb, tmp) {
	208	if (ktime_after(skb->tstamp, now))
	209	break;
25db26a9	210
37342bda	211	rb_erase_cached(&skb->rbnode, &q->head);
25db26a9	212
37342bda JSP	213	/* The rbnode field in the skb re-uses these fields, now that
	214	* we are done with the rbnode, reset them.
	215	*/
	216	skb->next = NULL;
	217	skb->prev = NULL;
	218	skb->dev = qdisc_dev(sch);
25db26a9	219
37342bda	220	report_sock_error(skb, ECANCELED, SO_EE_CODE_TXTIME_MISSED);
25db26a9	221
37342bda JSP	222	qdisc_qstats_backlog_dec(sch, skb);
	223	qdisc_drop(skb, sch, &to_free);
	224	qdisc_qstats_overlimit(sch);
	225	sch->q.qlen--;
	226	}
4b15c707	227
37342bda	228	kfree_skb_list(to_free);
cbeeb8ef	229	}
25db26a9	230
cbeeb8ef JSP	231	static void timesortedlist_remove(struct Qdisc sch, struct sk_buff skb)
	232	{
	233	struct etf_sched_data *q = qdisc_priv(sch);
	234
	235	rb_erase_cached(&skb->rbnode, &q->head);
	236
	237	/* The rbnode field in the skb re-uses these fields, now that
	238	* we are done with the rbnode, reset them.
	239	*/
	240	skb->next = NULL;
	241	skb->prev = NULL;
	242	skb->dev = qdisc_dev(sch);
	243
	244	qdisc_qstats_backlog_dec(sch, skb);
	245
	246	qdisc_bstats_update(sch, skb);
	247
	248	q->last = skb->tstamp;
25db26a9 VCG	249
	250	sch->q.qlen--;
	251	}
	252
	253	static struct sk_buff etf_dequeue_timesortedlist(struct Qdisc sch)
	254	{
	255	struct etf_sched_data *q = qdisc_priv(sch);
	256	struct sk_buff *skb;
	257	ktime_t now, next;
	258
	259	skb = etf_peek_timesortedlist(sch);
	260	if (!skb)
	261	return NULL;
	262
	263	now = q->get_time();
	264
	265	/* Drop if packet has expired while in queue. */
25db26a9	266	if (ktime_before(skb->tstamp, now)) {
37342bda	267	timesortedlist_drop(sch, skb, now);
25db26a9 VCG	268	skb = NULL;
	269	goto out;
	270	}
	271
	272	/* When in deadline mode, dequeue as soon as possible and change the
	273	* txtime from deadline to (now + delta).
	274	*/
	275	if (q->deadline_mode) {
cbeeb8ef	276	timesortedlist_remove(sch, skb);
25db26a9 VCG	277	skb->tstamp = now;
	278	goto out;
	279	}
	280
	281	next = ktime_sub_ns(skb->tstamp, q->delta);
	282
	283	/* Dequeue only if now is within the [txtime - delta, txtime] range. */
	284	if (ktime_after(now, next))
cbeeb8ef	285	timesortedlist_remove(sch, skb);
25db26a9 VCG	286	else
	287	skb = NULL;
	288
	289	out:
	290	/* Now we may need to re-arm the qdisc watchdog for the next packet. */
	291	reset_watchdog(sch);
	292
	293	return skb;
	294	}
	295
88cab771 JSP	296	static void etf_disable_offload(struct net_device *dev,
	297	struct etf_sched_data *q)
	298	{
	299	struct tc_etf_qopt_offload etf = { };
	300	const struct net_device_ops *ops;
	301	int err;
	302
	303	if (!q->offload)
	304	return;
	305
	306	ops = dev->netdev_ops;
	307	if (!ops->ndo_setup_tc)
	308	return;
	309
	310	etf.queue = q->queue;
	311	etf.enable = 0;
	312
	313	err = ops->ndo_setup_tc(dev, TC_SETUP_QDISC_ETF, &etf);
	314	if (err < 0)
	315	pr_warn("Couldn't disable ETF offload for queue %d\n",
	316	etf.queue);
	317	}
	318
	319	static int etf_enable_offload(struct net_device dev, struct etf_sched_data q,
	320	struct netlink_ext_ack *extack)
	321	{
	322	const struct net_device_ops *ops = dev->netdev_ops;
	323	struct tc_etf_qopt_offload etf = { };
	324	int err;
	325
88cab771 JSP	326	if (!ops->ndo_setup_tc) {
	327	NL_SET_ERR_MSG(extack, "Specified device does not support ETF offload");
	328	return -EOPNOTSUPP;
	329	}
	330
	331	etf.queue = q->queue;
	332	etf.enable = 1;
	333
	334	err = ops->ndo_setup_tc(dev, TC_SETUP_QDISC_ETF, &etf);
	335	if (err < 0) {
	336	NL_SET_ERR_MSG(extack, "Specified device failed to setup ETF hardware offload");
	337	return err;
	338	}
	339
	340	return 0;
	341	}
	342
25db26a9 VCG	343	static int etf_init(struct Qdisc sch, struct nlattr opt,
	344	struct netlink_ext_ack *extack)
	345	{
	346	struct etf_sched_data *q = qdisc_priv(sch);
	347	struct net_device *dev = qdisc_dev(sch);
	348	struct nlattr *tb[TCA_ETF_MAX + 1];
	349	struct tc_etf_qopt *qopt;
	350	int err;
	351
	352	if (!opt) {
	353	NL_SET_ERR_MSG(extack,
	354	"Missing ETF qdisc options which are mandatory");
	355	return -EINVAL;
	356	}
	357
8cb08174 JB	358	err = nla_parse_nested_deprecated(tb, TCA_ETF_MAX, opt, etf_policy,
8cb08174 JB	359	extack);
25db26a9 VCG	360	if (err < 0)
	361	return err;
	362
	363	if (!tb[TCA_ETF_PARMS]) {
	364	NL_SET_ERR_MSG(extack, "Missing mandatory ETF parameters");
	365	return -EINVAL;
	366	}
	367
	368	qopt = nla_data(tb[TCA_ETF_PARMS]);
	369
88cab771	370	pr_debug("delta %d clockid %d offload %s deadline %s\n",
25db26a9	371	qopt->delta, qopt->clockid,
88cab771	372	OFFLOAD_IS_ON(qopt) ? "on" : "off",
25db26a9 VCG	373	DEADLINE_MODE_IS_ON(qopt) ? "on" : "off");
	374
	375	err = validate_input_params(qopt, extack);
	376	if (err < 0)
	377	return err;
	378
	379	q->queue = sch->dev_queue - netdev_get_tx_queue(dev, 0);
	380
88cab771 JSP	381	if (OFFLOAD_IS_ON(qopt)) {
	382	err = etf_enable_offload(dev, q, extack);
	383	if (err < 0)
	384	return err;
	385	}
	386
25db26a9 VCG	387	/* Everything went OK, save the parameters used. */
	388	q->delta = qopt->delta;
	389	q->clockid = qopt->clockid;
88cab771	390	q->offload = OFFLOAD_IS_ON(qopt);
25db26a9	391	q->deadline_mode = DEADLINE_MODE_IS_ON(qopt);
d14d2b20	392	q->skip_sock_check = SKIP_SOCK_CHECK_IS_SET(qopt);
25db26a9 VCG	393
	394	switch (q->clockid) {
	395	case CLOCK_REALTIME:
	396	q->get_time = ktime_get_real;
	397	break;
	398	case CLOCK_MONOTONIC:
	399	q->get_time = ktime_get;
	400	break;
	401	case CLOCK_BOOTTIME:
	402	q->get_time = ktime_get_boottime;
	403	break;
	404	case CLOCK_TAI:
	405	q->get_time = ktime_get_clocktai;
	406	break;
	407	default:
	408	NL_SET_ERR_MSG(extack, "Clockid is not supported");
	409	return -ENOTSUPP;
	410	}
	411
	412	qdisc_watchdog_init_clockid(&q->watchdog, sch, q->clockid);
	413
	414	return 0;
	415	}
	416
	417	static void timesortedlist_clear(struct Qdisc *sch)
	418	{
	419	struct etf_sched_data *q = qdisc_priv(sch);
09fd4860	420	struct rb_node *p = rb_first_cached(&q->head);
25db26a9 VCG	421
	422	while (p) {
	423	struct sk_buff *skb = rb_to_skb(p);
	424
	425	p = rb_next(p);
	426
09fd4860	427	rb_erase_cached(&skb->rbnode, &q->head);
25db26a9 VCG	428	rtnl_kfree_skbs(skb, skb);
	429	sch->q.qlen--;
	430	}
	431	}
	432
	433	static void etf_reset(struct Qdisc *sch)
	434	{
	435	struct etf_sched_data *q = qdisc_priv(sch);
	436
	437	/* Only cancel watchdog if it's been initialized. */
	438	if (q->watchdog.qdisc == sch)
	439	qdisc_watchdog_cancel(&q->watchdog);
	440
	441	/* No matter which mode we are on, it's safe to clear both lists. */
	442	timesortedlist_clear(sch);
	443	__qdisc_reset_queue(&sch->q);
	444
25db26a9 VCG	445	q->last = 0;
	446	}
	447
	448	static void etf_destroy(struct Qdisc *sch)
	449	{
	450	struct etf_sched_data *q = qdisc_priv(sch);
88cab771	451	struct net_device *dev = qdisc_dev(sch);
25db26a9 VCG	452
	453	/* Only cancel watchdog if it's been initialized. */
	454	if (q->watchdog.qdisc == sch)
	455	qdisc_watchdog_cancel(&q->watchdog);
88cab771 JSP	456
88cab771 JSP	457	etf_disable_offload(dev, q);
25db26a9 VCG	458	}
	459
	460	static int etf_dump(struct Qdisc sch, struct sk_buff skb)
	461	{
	462	struct etf_sched_data *q = qdisc_priv(sch);
	463	struct tc_etf_qopt opt = { };
	464	struct nlattr *nest;
	465
ae0be8de	466	nest = nla_nest_start_noflag(skb, TCA_OPTIONS);
25db26a9 VCG	467	if (!nest)
	468	goto nla_put_failure;
	469
	470	opt.delta = q->delta;
	471	opt.clockid = q->clockid;
88cab771 JSP	472	if (q->offload)
	473	opt.flags \|= TC_ETF_OFFLOAD_ON;
	474
25db26a9 VCG	475	if (q->deadline_mode)
	476	opt.flags \|= TC_ETF_DEADLINE_MODE_ON;
	477
d14d2b20 VP	478	if (q->skip_sock_check)
	479	opt.flags \|= TC_ETF_SKIP_SOCK_CHECK;
	480
25db26a9 VCG	481	if (nla_put(skb, TCA_ETF_PARMS, sizeof(opt), &opt))
	482	goto nla_put_failure;
	483
	484	return nla_nest_end(skb, nest);
	485
	486	nla_put_failure:
	487	nla_nest_cancel(skb, nest);
	488	return -1;
	489	}
	490
	491	static struct Qdisc_ops etf_qdisc_ops __read_mostly = {
	492	.id = "etf",
	493	.priv_size = sizeof(struct etf_sched_data),
	494	.enqueue = etf_enqueue_timesortedlist,
	495	.dequeue = etf_dequeue_timesortedlist,
	496	.peek = etf_peek_timesortedlist,
	497	.init = etf_init,
	498	.reset = etf_reset,
	499	.destroy = etf_destroy,
	500	.dump = etf_dump,
	501	.owner = THIS_MODULE,
	502	};
	503
	504	static int __init etf_module_init(void)
	505	{
	506	return register_qdisc(&etf_qdisc_ops);
	507	}
	508
	509	static void __exit etf_module_exit(void)
	510	{
	511	unregister_qdisc(&etf_qdisc_ops);
	512	}
	513	module_init(etf_module_init)
	514	module_exit(etf_module_exit)
	515	MODULE_LICENSE("GPL");