125db26a9SVinicius Costa Gomes // SPDX-License-Identifier: GPL-2.0
225db26a9SVinicius Costa Gomes
325db26a9SVinicius Costa Gomes /* net/sched/sch_etf.c Earliest TxTime First queueing discipline.
425db26a9SVinicius Costa Gomes *
525db26a9SVinicius Costa Gomes * Authors: Jesus Sanchez-Palencia <jesus.sanchez-palencia@intel.com>
625db26a9SVinicius Costa Gomes * Vinicius Costa Gomes <vinicius.gomes@intel.com>
725db26a9SVinicius Costa Gomes */
825db26a9SVinicius Costa Gomes
925db26a9SVinicius Costa Gomes #include <linux/module.h>
1025db26a9SVinicius Costa Gomes #include <linux/types.h>
1125db26a9SVinicius Costa Gomes #include <linux/kernel.h>
1225db26a9SVinicius Costa Gomes #include <linux/string.h>
1325db26a9SVinicius Costa Gomes #include <linux/errno.h>
144b15c707SJesus Sanchez-Palencia #include <linux/errqueue.h>
1525db26a9SVinicius Costa Gomes #include <linux/rbtree.h>
1625db26a9SVinicius Costa Gomes #include <linux/skbuff.h>
1725db26a9SVinicius Costa Gomes #include <linux/posix-timers.h>
1825db26a9SVinicius Costa Gomes #include <net/netlink.h>
1925db26a9SVinicius Costa Gomes #include <net/sch_generic.h>
2025db26a9SVinicius Costa Gomes #include <net/pkt_sched.h>
2125db26a9SVinicius Costa Gomes #include <net/sock.h>
2225db26a9SVinicius Costa Gomes
2325db26a9SVinicius Costa Gomes #define DEADLINE_MODE_IS_ON(x) ((x)->flags & TC_ETF_DEADLINE_MODE_ON)
2488cab771SJesus Sanchez-Palencia #define OFFLOAD_IS_ON(x) ((x)->flags & TC_ETF_OFFLOAD_ON)
25d14d2b20SVedang Patel #define SKIP_SOCK_CHECK_IS_SET(x) ((x)->flags & TC_ETF_SKIP_SOCK_CHECK)
2625db26a9SVinicius Costa Gomes
2725db26a9SVinicius Costa Gomes struct etf_sched_data {
2888cab771SJesus Sanchez-Palencia bool offload;
2925db26a9SVinicius Costa Gomes bool deadline_mode;
30d14d2b20SVedang Patel bool skip_sock_check;
3125db26a9SVinicius Costa Gomes int clockid;
3225db26a9SVinicius Costa Gomes int queue;
3325db26a9SVinicius Costa Gomes s32 delta; /* in ns */
3425db26a9SVinicius Costa Gomes ktime_t last; /* The txtime of the last skb sent to the netdevice. */
3509fd4860SJesus Sanchez-Palencia struct rb_root_cached head;
3625db26a9SVinicius Costa Gomes struct qdisc_watchdog watchdog;
3725db26a9SVinicius Costa Gomes ktime_t (*get_time)(void);
3825db26a9SVinicius Costa Gomes };
3925db26a9SVinicius Costa Gomes
4025db26a9SVinicius Costa Gomes static const struct nla_policy etf_policy[TCA_ETF_MAX + 1] = {
4125db26a9SVinicius Costa Gomes [TCA_ETF_PARMS] = { .len = sizeof(struct tc_etf_qopt) },
4225db26a9SVinicius Costa Gomes };
4325db26a9SVinicius Costa Gomes
validate_input_params(struct tc_etf_qopt * qopt,struct netlink_ext_ack * extack)4425db26a9SVinicius Costa Gomes static inline int validate_input_params(struct tc_etf_qopt *qopt,
4525db26a9SVinicius Costa Gomes struct netlink_ext_ack *extack)
4625db26a9SVinicius Costa Gomes {
4725db26a9SVinicius Costa Gomes /* Check if params comply to the following rules:
4825db26a9SVinicius Costa Gomes * * Clockid and delta must be valid.
4925db26a9SVinicius Costa Gomes *
5025db26a9SVinicius Costa Gomes * * Dynamic clockids are not supported.
5125db26a9SVinicius Costa Gomes *
5225db26a9SVinicius Costa Gomes * * Delta must be a positive integer.
5388cab771SJesus Sanchez-Palencia *
5488cab771SJesus Sanchez-Palencia * Also note that for the HW offload case, we must
5588cab771SJesus Sanchez-Palencia * expect that system clocks have been synchronized to PHC.
5625db26a9SVinicius Costa Gomes */
5725db26a9SVinicius Costa Gomes if (qopt->clockid < 0) {
5825db26a9SVinicius Costa Gomes NL_SET_ERR_MSG(extack, "Dynamic clockids are not supported");
5925db26a9SVinicius Costa Gomes return -ENOTSUPP;
6025db26a9SVinicius Costa Gomes }
6125db26a9SVinicius Costa Gomes
6225db26a9SVinicius Costa Gomes if (qopt->clockid != CLOCK_TAI) {
6325db26a9SVinicius Costa Gomes NL_SET_ERR_MSG(extack, "Invalid clockid. CLOCK_TAI must be used");
6425db26a9SVinicius Costa Gomes return -EINVAL;
6525db26a9SVinicius Costa Gomes }
6625db26a9SVinicius Costa Gomes
6725db26a9SVinicius Costa Gomes if (qopt->delta < 0) {
6825db26a9SVinicius Costa Gomes NL_SET_ERR_MSG(extack, "Delta must be positive");
6925db26a9SVinicius Costa Gomes return -EINVAL;
7025db26a9SVinicius Costa Gomes }
7125db26a9SVinicius Costa Gomes
7225db26a9SVinicius Costa Gomes return 0;
7325db26a9SVinicius Costa Gomes }
7425db26a9SVinicius Costa Gomes
is_packet_valid(struct Qdisc * sch,struct sk_buff * nskb)7525db26a9SVinicius Costa Gomes static bool is_packet_valid(struct Qdisc *sch, struct sk_buff *nskb)
7625db26a9SVinicius Costa Gomes {
7725db26a9SVinicius Costa Gomes struct etf_sched_data *q = qdisc_priv(sch);
7825db26a9SVinicius Costa Gomes ktime_t txtime = nskb->tstamp;
7925db26a9SVinicius Costa Gomes struct sock *sk = nskb->sk;
8025db26a9SVinicius Costa Gomes ktime_t now;
8125db26a9SVinicius Costa Gomes
82d14d2b20SVedang Patel if (q->skip_sock_check)
83d14d2b20SVedang Patel goto skip;
84d14d2b20SVedang Patel
85a1211bf9SEric Dumazet if (!sk || !sk_fullsock(sk))
8625db26a9SVinicius Costa Gomes return false;
8725db26a9SVinicius Costa Gomes
8825db26a9SVinicius Costa Gomes if (!sock_flag(sk, SOCK_TXTIME))
8925db26a9SVinicius Costa Gomes return false;
9025db26a9SVinicius Costa Gomes
9125db26a9SVinicius Costa Gomes /* We don't perform crosstimestamping.
9225db26a9SVinicius Costa Gomes * Drop if packet's clockid differs from qdisc's.
9325db26a9SVinicius Costa Gomes */
9425db26a9SVinicius Costa Gomes if (sk->sk_clockid != q->clockid)
9525db26a9SVinicius Costa Gomes return false;
9625db26a9SVinicius Costa Gomes
9725db26a9SVinicius Costa Gomes if (sk->sk_txtime_deadline_mode != q->deadline_mode)
9825db26a9SVinicius Costa Gomes return false;
9925db26a9SVinicius Costa Gomes
100d14d2b20SVedang Patel skip:
10125db26a9SVinicius Costa Gomes now = q->get_time();
10225db26a9SVinicius Costa Gomes if (ktime_before(txtime, now) || ktime_before(txtime, q->last))
10325db26a9SVinicius Costa Gomes return false;
10425db26a9SVinicius Costa Gomes
10525db26a9SVinicius Costa Gomes return true;
10625db26a9SVinicius Costa Gomes }
10725db26a9SVinicius Costa Gomes
etf_peek_timesortedlist(struct Qdisc * sch)10825db26a9SVinicius Costa Gomes static struct sk_buff *etf_peek_timesortedlist(struct Qdisc *sch)
10925db26a9SVinicius Costa Gomes {
11025db26a9SVinicius Costa Gomes struct etf_sched_data *q = qdisc_priv(sch);
11125db26a9SVinicius Costa Gomes struct rb_node *p;
11225db26a9SVinicius Costa Gomes
11309fd4860SJesus Sanchez-Palencia p = rb_first_cached(&q->head);
11425db26a9SVinicius Costa Gomes if (!p)
11525db26a9SVinicius Costa Gomes return NULL;
11625db26a9SVinicius Costa Gomes
11725db26a9SVinicius Costa Gomes return rb_to_skb(p);
11825db26a9SVinicius Costa Gomes }
11925db26a9SVinicius Costa Gomes
reset_watchdog(struct Qdisc * sch)12025db26a9SVinicius Costa Gomes static void reset_watchdog(struct Qdisc *sch)
12125db26a9SVinicius Costa Gomes {
12225db26a9SVinicius Costa Gomes struct etf_sched_data *q = qdisc_priv(sch);
12325db26a9SVinicius Costa Gomes struct sk_buff *skb = etf_peek_timesortedlist(sch);
12425db26a9SVinicius Costa Gomes ktime_t next;
12525db26a9SVinicius Costa Gomes
1263fcbdaeeSJesus Sanchez-Palencia if (!skb) {
1273fcbdaeeSJesus Sanchez-Palencia qdisc_watchdog_cancel(&q->watchdog);
12825db26a9SVinicius Costa Gomes return;
1293fcbdaeeSJesus Sanchez-Palencia }
13025db26a9SVinicius Costa Gomes
13125db26a9SVinicius Costa Gomes next = ktime_sub_ns(skb->tstamp, q->delta);
13225db26a9SVinicius Costa Gomes qdisc_watchdog_schedule_ns(&q->watchdog, ktime_to_ns(next));
13325db26a9SVinicius Costa Gomes }
13425db26a9SVinicius Costa Gomes
report_sock_error(struct sk_buff * skb,u32 err,u8 code)1354b15c707SJesus Sanchez-Palencia static void report_sock_error(struct sk_buff *skb, u32 err, u8 code)
1364b15c707SJesus Sanchez-Palencia {
1374b15c707SJesus Sanchez-Palencia struct sock_exterr_skb *serr;
1384b15c707SJesus Sanchez-Palencia struct sk_buff *clone;
1394b15c707SJesus Sanchez-Palencia ktime_t txtime = skb->tstamp;
140a1211bf9SEric Dumazet struct sock *sk = skb->sk;
1414b15c707SJesus Sanchez-Palencia
142a1211bf9SEric Dumazet if (!sk || !sk_fullsock(sk) || !(sk->sk_txtime_report_errors))
1434b15c707SJesus Sanchez-Palencia return;
1444b15c707SJesus Sanchez-Palencia
1454b15c707SJesus Sanchez-Palencia clone = skb_clone(skb, GFP_ATOMIC);
1464b15c707SJesus Sanchez-Palencia if (!clone)
1474b15c707SJesus Sanchez-Palencia return;
1484b15c707SJesus Sanchez-Palencia
1494b15c707SJesus Sanchez-Palencia serr = SKB_EXT_ERR(clone);
1504b15c707SJesus Sanchez-Palencia serr->ee.ee_errno = err;
1514b15c707SJesus Sanchez-Palencia serr->ee.ee_origin = SO_EE_ORIGIN_TXTIME;
1524b15c707SJesus Sanchez-Palencia serr->ee.ee_type = 0;
1534b15c707SJesus Sanchez-Palencia serr->ee.ee_code = code;
1544b15c707SJesus Sanchez-Palencia serr->ee.ee_pad = 0;
1554b15c707SJesus Sanchez-Palencia serr->ee.ee_data = (txtime >> 32); /* high part of tstamp */
1564b15c707SJesus Sanchez-Palencia serr->ee.ee_info = txtime; /* low part of tstamp */
1574b15c707SJesus Sanchez-Palencia
158a1211bf9SEric Dumazet if (sock_queue_err_skb(sk, clone))
1594b15c707SJesus Sanchez-Palencia kfree_skb(clone);
1604b15c707SJesus Sanchez-Palencia }
1614b15c707SJesus Sanchez-Palencia
etf_enqueue_timesortedlist(struct sk_buff * nskb,struct Qdisc * sch,struct sk_buff ** to_free)16225db26a9SVinicius Costa Gomes static int etf_enqueue_timesortedlist(struct sk_buff *nskb, struct Qdisc *sch,
16325db26a9SVinicius Costa Gomes struct sk_buff **to_free)
16425db26a9SVinicius Costa Gomes {
16525db26a9SVinicius Costa Gomes struct etf_sched_data *q = qdisc_priv(sch);
16609fd4860SJesus Sanchez-Palencia struct rb_node **p = &q->head.rb_root.rb_node, *parent = NULL;
16725db26a9SVinicius Costa Gomes ktime_t txtime = nskb->tstamp;
16809fd4860SJesus Sanchez-Palencia bool leftmost = true;
16925db26a9SVinicius Costa Gomes
1704b15c707SJesus Sanchez-Palencia if (!is_packet_valid(sch, nskb)) {
1714b15c707SJesus Sanchez-Palencia report_sock_error(nskb, EINVAL,
1724b15c707SJesus Sanchez-Palencia SO_EE_CODE_TXTIME_INVALID_PARAM);
17325db26a9SVinicius Costa Gomes return qdisc_drop(nskb, sch, to_free);
1744b15c707SJesus Sanchez-Palencia }
17525db26a9SVinicius Costa Gomes
17625db26a9SVinicius Costa Gomes while (*p) {
17725db26a9SVinicius Costa Gomes struct sk_buff *skb;
17825db26a9SVinicius Costa Gomes
17925db26a9SVinicius Costa Gomes parent = *p;
18025db26a9SVinicius Costa Gomes skb = rb_to_skb(parent);
18128aa7c86SVinicius Costa Gomes if (ktime_compare(txtime, skb->tstamp) >= 0) {
18225db26a9SVinicius Costa Gomes p = &parent->rb_right;
18309fd4860SJesus Sanchez-Palencia leftmost = false;
18409fd4860SJesus Sanchez-Palencia } else {
18525db26a9SVinicius Costa Gomes p = &parent->rb_left;
18625db26a9SVinicius Costa Gomes }
18709fd4860SJesus Sanchez-Palencia }
18825db26a9SVinicius Costa Gomes rb_link_node(&nskb->rbnode, parent, p);
18909fd4860SJesus Sanchez-Palencia rb_insert_color_cached(&nskb->rbnode, &q->head, leftmost);
19025db26a9SVinicius Costa Gomes
19125db26a9SVinicius Costa Gomes qdisc_qstats_backlog_inc(sch, nskb);
19225db26a9SVinicius Costa Gomes sch->q.qlen++;
19325db26a9SVinicius Costa Gomes
19425db26a9SVinicius Costa Gomes /* Now we may need to re-arm the qdisc watchdog for the next packet. */
19525db26a9SVinicius Costa Gomes reset_watchdog(sch);
19625db26a9SVinicius Costa Gomes
19725db26a9SVinicius Costa Gomes return NET_XMIT_SUCCESS;
19825db26a9SVinicius Costa Gomes }
19925db26a9SVinicius Costa Gomes
timesortedlist_drop(struct Qdisc * sch,struct sk_buff * skb,ktime_t now)20037342bdaSJesus Sanchez-Palencia static void timesortedlist_drop(struct Qdisc *sch, struct sk_buff *skb,
20137342bdaSJesus Sanchez-Palencia ktime_t now)
202cbeeb8efSJesus Sanchez-Palencia {
203cbeeb8efSJesus Sanchez-Palencia struct etf_sched_data *q = qdisc_priv(sch);
204cbeeb8efSJesus Sanchez-Palencia struct sk_buff *to_free = NULL;
20537342bdaSJesus Sanchez-Palencia struct sk_buff *tmp = NULL;
20637342bdaSJesus Sanchez-Palencia
20737342bdaSJesus Sanchez-Palencia skb_rbtree_walk_from_safe(skb, tmp) {
20837342bdaSJesus Sanchez-Palencia if (ktime_after(skb->tstamp, now))
20937342bdaSJesus Sanchez-Palencia break;
210cbeeb8efSJesus Sanchez-Palencia
211cbeeb8efSJesus Sanchez-Palencia rb_erase_cached(&skb->rbnode, &q->head);
212cbeeb8efSJesus Sanchez-Palencia
213cbeeb8efSJesus Sanchez-Palencia /* The rbnode field in the skb re-uses these fields, now that
214cbeeb8efSJesus Sanchez-Palencia * we are done with the rbnode, reset them.
215cbeeb8efSJesus Sanchez-Palencia */
216cbeeb8efSJesus Sanchez-Palencia skb->next = NULL;
217cbeeb8efSJesus Sanchez-Palencia skb->prev = NULL;
218cbeeb8efSJesus Sanchez-Palencia skb->dev = qdisc_dev(sch);
219cbeeb8efSJesus Sanchez-Palencia
220cbeeb8efSJesus Sanchez-Palencia report_sock_error(skb, ECANCELED, SO_EE_CODE_TXTIME_MISSED);
221cbeeb8efSJesus Sanchez-Palencia
22237342bdaSJesus Sanchez-Palencia qdisc_qstats_backlog_dec(sch, skb);
223cbeeb8efSJesus Sanchez-Palencia qdisc_drop(skb, sch, &to_free);
224cbeeb8efSJesus Sanchez-Palencia qdisc_qstats_overlimit(sch);
225cbeeb8efSJesus Sanchez-Palencia sch->q.qlen--;
226cbeeb8efSJesus Sanchez-Palencia }
227cbeeb8efSJesus Sanchez-Palencia
22837342bdaSJesus Sanchez-Palencia kfree_skb_list(to_free);
22937342bdaSJesus Sanchez-Palencia }
23037342bdaSJesus Sanchez-Palencia
timesortedlist_remove(struct Qdisc * sch,struct sk_buff * skb)231cbeeb8efSJesus Sanchez-Palencia static void timesortedlist_remove(struct Qdisc *sch, struct sk_buff *skb)
23225db26a9SVinicius Costa Gomes {
23325db26a9SVinicius Costa Gomes struct etf_sched_data *q = qdisc_priv(sch);
23425db26a9SVinicius Costa Gomes
23509fd4860SJesus Sanchez-Palencia rb_erase_cached(&skb->rbnode, &q->head);
23625db26a9SVinicius Costa Gomes
23725db26a9SVinicius Costa Gomes /* The rbnode field in the skb re-uses these fields, now that
23825db26a9SVinicius Costa Gomes * we are done with the rbnode, reset them.
23925db26a9SVinicius Costa Gomes */
24025db26a9SVinicius Costa Gomes skb->next = NULL;
24125db26a9SVinicius Costa Gomes skb->prev = NULL;
24225db26a9SVinicius Costa Gomes skb->dev = qdisc_dev(sch);
24325db26a9SVinicius Costa Gomes
24425db26a9SVinicius Costa Gomes qdisc_qstats_backlog_dec(sch, skb);
24525db26a9SVinicius Costa Gomes
24625db26a9SVinicius Costa Gomes qdisc_bstats_update(sch, skb);
24725db26a9SVinicius Costa Gomes
24825db26a9SVinicius Costa Gomes q->last = skb->tstamp;
24925db26a9SVinicius Costa Gomes
25025db26a9SVinicius Costa Gomes sch->q.qlen--;
25125db26a9SVinicius Costa Gomes }
25225db26a9SVinicius Costa Gomes
etf_dequeue_timesortedlist(struct Qdisc * sch)25325db26a9SVinicius Costa Gomes static struct sk_buff *etf_dequeue_timesortedlist(struct Qdisc *sch)
25425db26a9SVinicius Costa Gomes {
25525db26a9SVinicius Costa Gomes struct etf_sched_data *q = qdisc_priv(sch);
25625db26a9SVinicius Costa Gomes struct sk_buff *skb;
25725db26a9SVinicius Costa Gomes ktime_t now, next;
25825db26a9SVinicius Costa Gomes
25925db26a9SVinicius Costa Gomes skb = etf_peek_timesortedlist(sch);
26025db26a9SVinicius Costa Gomes if (!skb)
26125db26a9SVinicius Costa Gomes return NULL;
26225db26a9SVinicius Costa Gomes
26325db26a9SVinicius Costa Gomes now = q->get_time();
26425db26a9SVinicius Costa Gomes
26525db26a9SVinicius Costa Gomes /* Drop if packet has expired while in queue. */
26625db26a9SVinicius Costa Gomes if (ktime_before(skb->tstamp, now)) {
26737342bdaSJesus Sanchez-Palencia timesortedlist_drop(sch, skb, now);
26825db26a9SVinicius Costa Gomes skb = NULL;
26925db26a9SVinicius Costa Gomes goto out;
27025db26a9SVinicius Costa Gomes }
27125db26a9SVinicius Costa Gomes
27225db26a9SVinicius Costa Gomes /* When in deadline mode, dequeue as soon as possible and change the
27325db26a9SVinicius Costa Gomes * txtime from deadline to (now + delta).
27425db26a9SVinicius Costa Gomes */
27525db26a9SVinicius Costa Gomes if (q->deadline_mode) {
276cbeeb8efSJesus Sanchez-Palencia timesortedlist_remove(sch, skb);
27725db26a9SVinicius Costa Gomes skb->tstamp = now;
27825db26a9SVinicius Costa Gomes goto out;
27925db26a9SVinicius Costa Gomes }
28025db26a9SVinicius Costa Gomes
28125db26a9SVinicius Costa Gomes next = ktime_sub_ns(skb->tstamp, q->delta);
28225db26a9SVinicius Costa Gomes
28325db26a9SVinicius Costa Gomes /* Dequeue only if now is within the [txtime - delta, txtime] range. */
28425db26a9SVinicius Costa Gomes if (ktime_after(now, next))
285cbeeb8efSJesus Sanchez-Palencia timesortedlist_remove(sch, skb);
28625db26a9SVinicius Costa Gomes else
28725db26a9SVinicius Costa Gomes skb = NULL;
28825db26a9SVinicius Costa Gomes
28925db26a9SVinicius Costa Gomes out:
29025db26a9SVinicius Costa Gomes /* Now we may need to re-arm the qdisc watchdog for the next packet. */
29125db26a9SVinicius Costa Gomes reset_watchdog(sch);
29225db26a9SVinicius Costa Gomes
29325db26a9SVinicius Costa Gomes return skb;
29425db26a9SVinicius Costa Gomes }
29525db26a9SVinicius Costa Gomes
etf_disable_offload(struct net_device * dev,struct etf_sched_data * q)29688cab771SJesus Sanchez-Palencia static void etf_disable_offload(struct net_device *dev,
29788cab771SJesus Sanchez-Palencia struct etf_sched_data *q)
29888cab771SJesus Sanchez-Palencia {
29988cab771SJesus Sanchez-Palencia struct tc_etf_qopt_offload etf = { };
30088cab771SJesus Sanchez-Palencia const struct net_device_ops *ops;
30188cab771SJesus Sanchez-Palencia int err;
30288cab771SJesus Sanchez-Palencia
30388cab771SJesus Sanchez-Palencia if (!q->offload)
30488cab771SJesus Sanchez-Palencia return;
30588cab771SJesus Sanchez-Palencia
30688cab771SJesus Sanchez-Palencia ops = dev->netdev_ops;
30788cab771SJesus Sanchez-Palencia if (!ops->ndo_setup_tc)
30888cab771SJesus Sanchez-Palencia return;
30988cab771SJesus Sanchez-Palencia
31088cab771SJesus Sanchez-Palencia etf.queue = q->queue;
31188cab771SJesus Sanchez-Palencia etf.enable = 0;
31288cab771SJesus Sanchez-Palencia
31388cab771SJesus Sanchez-Palencia err = ops->ndo_setup_tc(dev, TC_SETUP_QDISC_ETF, &etf);
31488cab771SJesus Sanchez-Palencia if (err < 0)
31588cab771SJesus Sanchez-Palencia pr_warn("Couldn't disable ETF offload for queue %d\n",
31688cab771SJesus Sanchez-Palencia etf.queue);
31788cab771SJesus Sanchez-Palencia }
31888cab771SJesus Sanchez-Palencia
etf_enable_offload(struct net_device * dev,struct etf_sched_data * q,struct netlink_ext_ack * extack)31988cab771SJesus Sanchez-Palencia static int etf_enable_offload(struct net_device *dev, struct etf_sched_data *q,
32088cab771SJesus Sanchez-Palencia struct netlink_ext_ack *extack)
32188cab771SJesus Sanchez-Palencia {
32288cab771SJesus Sanchez-Palencia const struct net_device_ops *ops = dev->netdev_ops;
32388cab771SJesus Sanchez-Palencia struct tc_etf_qopt_offload etf = { };
32488cab771SJesus Sanchez-Palencia int err;
32588cab771SJesus Sanchez-Palencia
32688cab771SJesus Sanchez-Palencia if (!ops->ndo_setup_tc) {
32788cab771SJesus Sanchez-Palencia NL_SET_ERR_MSG(extack, "Specified device does not support ETF offload");
32888cab771SJesus Sanchez-Palencia return -EOPNOTSUPP;
32988cab771SJesus Sanchez-Palencia }
33088cab771SJesus Sanchez-Palencia
33188cab771SJesus Sanchez-Palencia etf.queue = q->queue;
33288cab771SJesus Sanchez-Palencia etf.enable = 1;
33388cab771SJesus Sanchez-Palencia
33488cab771SJesus Sanchez-Palencia err = ops->ndo_setup_tc(dev, TC_SETUP_QDISC_ETF, &etf);
33588cab771SJesus Sanchez-Palencia if (err < 0) {
33688cab771SJesus Sanchez-Palencia NL_SET_ERR_MSG(extack, "Specified device failed to setup ETF hardware offload");
33788cab771SJesus Sanchez-Palencia return err;
33888cab771SJesus Sanchez-Palencia }
33988cab771SJesus Sanchez-Palencia
34088cab771SJesus Sanchez-Palencia return 0;
34188cab771SJesus Sanchez-Palencia }
34288cab771SJesus Sanchez-Palencia
etf_init(struct Qdisc * sch,struct nlattr * opt,struct netlink_ext_ack * extack)34325db26a9SVinicius Costa Gomes static int etf_init(struct Qdisc *sch, struct nlattr *opt,
34425db26a9SVinicius Costa Gomes struct netlink_ext_ack *extack)
34525db26a9SVinicius Costa Gomes {
34625db26a9SVinicius Costa Gomes struct etf_sched_data *q = qdisc_priv(sch);
34725db26a9SVinicius Costa Gomes struct net_device *dev = qdisc_dev(sch);
34825db26a9SVinicius Costa Gomes struct nlattr *tb[TCA_ETF_MAX + 1];
34925db26a9SVinicius Costa Gomes struct tc_etf_qopt *qopt;
35025db26a9SVinicius Costa Gomes int err;
35125db26a9SVinicius Costa Gomes
35225db26a9SVinicius Costa Gomes if (!opt) {
35325db26a9SVinicius Costa Gomes NL_SET_ERR_MSG(extack,
35425db26a9SVinicius Costa Gomes "Missing ETF qdisc options which are mandatory");
35525db26a9SVinicius Costa Gomes return -EINVAL;
35625db26a9SVinicius Costa Gomes }
35725db26a9SVinicius Costa Gomes
3588cb08174SJohannes Berg err = nla_parse_nested_deprecated(tb, TCA_ETF_MAX, opt, etf_policy,
3598cb08174SJohannes Berg extack);
36025db26a9SVinicius Costa Gomes if (err < 0)
36125db26a9SVinicius Costa Gomes return err;
36225db26a9SVinicius Costa Gomes
36325db26a9SVinicius Costa Gomes if (!tb[TCA_ETF_PARMS]) {
36425db26a9SVinicius Costa Gomes NL_SET_ERR_MSG(extack, "Missing mandatory ETF parameters");
36525db26a9SVinicius Costa Gomes return -EINVAL;
36625db26a9SVinicius Costa Gomes }
36725db26a9SVinicius Costa Gomes
36825db26a9SVinicius Costa Gomes qopt = nla_data(tb[TCA_ETF_PARMS]);
36925db26a9SVinicius Costa Gomes
37088cab771SJesus Sanchez-Palencia pr_debug("delta %d clockid %d offload %s deadline %s\n",
37125db26a9SVinicius Costa Gomes qopt->delta, qopt->clockid,
37288cab771SJesus Sanchez-Palencia OFFLOAD_IS_ON(qopt) ? "on" : "off",
37325db26a9SVinicius Costa Gomes DEADLINE_MODE_IS_ON(qopt) ? "on" : "off");
37425db26a9SVinicius Costa Gomes
37525db26a9SVinicius Costa Gomes err = validate_input_params(qopt, extack);
37625db26a9SVinicius Costa Gomes if (err < 0)
37725db26a9SVinicius Costa Gomes return err;
37825db26a9SVinicius Costa Gomes
37925db26a9SVinicius Costa Gomes q->queue = sch->dev_queue - netdev_get_tx_queue(dev, 0);
38025db26a9SVinicius Costa Gomes
38188cab771SJesus Sanchez-Palencia if (OFFLOAD_IS_ON(qopt)) {
38288cab771SJesus Sanchez-Palencia err = etf_enable_offload(dev, q, extack);
38388cab771SJesus Sanchez-Palencia if (err < 0)
38488cab771SJesus Sanchez-Palencia return err;
38588cab771SJesus Sanchez-Palencia }
38688cab771SJesus Sanchez-Palencia
38725db26a9SVinicius Costa Gomes /* Everything went OK, save the parameters used. */
38825db26a9SVinicius Costa Gomes q->delta = qopt->delta;
38925db26a9SVinicius Costa Gomes q->clockid = qopt->clockid;
39088cab771SJesus Sanchez-Palencia q->offload = OFFLOAD_IS_ON(qopt);
39125db26a9SVinicius Costa Gomes q->deadline_mode = DEADLINE_MODE_IS_ON(qopt);
392d14d2b20SVedang Patel q->skip_sock_check = SKIP_SOCK_CHECK_IS_SET(qopt);
39325db26a9SVinicius Costa Gomes
39425db26a9SVinicius Costa Gomes switch (q->clockid) {
39525db26a9SVinicius Costa Gomes case CLOCK_REALTIME:
39625db26a9SVinicius Costa Gomes q->get_time = ktime_get_real;
39725db26a9SVinicius Costa Gomes break;
39825db26a9SVinicius Costa Gomes case CLOCK_MONOTONIC:
39925db26a9SVinicius Costa Gomes q->get_time = ktime_get;
40025db26a9SVinicius Costa Gomes break;
40125db26a9SVinicius Costa Gomes case CLOCK_BOOTTIME:
40225db26a9SVinicius Costa Gomes q->get_time = ktime_get_boottime;
40325db26a9SVinicius Costa Gomes break;
40425db26a9SVinicius Costa Gomes case CLOCK_TAI:
40525db26a9SVinicius Costa Gomes q->get_time = ktime_get_clocktai;
40625db26a9SVinicius Costa Gomes break;
40725db26a9SVinicius Costa Gomes default:
40825db26a9SVinicius Costa Gomes NL_SET_ERR_MSG(extack, "Clockid is not supported");
40925db26a9SVinicius Costa Gomes return -ENOTSUPP;
41025db26a9SVinicius Costa Gomes }
41125db26a9SVinicius Costa Gomes
41225db26a9SVinicius Costa Gomes qdisc_watchdog_init_clockid(&q->watchdog, sch, q->clockid);
41325db26a9SVinicius Costa Gomes
41425db26a9SVinicius Costa Gomes return 0;
41525db26a9SVinicius Costa Gomes }
41625db26a9SVinicius Costa Gomes
timesortedlist_clear(struct Qdisc * sch)41725db26a9SVinicius Costa Gomes static void timesortedlist_clear(struct Qdisc *sch)
41825db26a9SVinicius Costa Gomes {
41925db26a9SVinicius Costa Gomes struct etf_sched_data *q = qdisc_priv(sch);
42009fd4860SJesus Sanchez-Palencia struct rb_node *p = rb_first_cached(&q->head);
42125db26a9SVinicius Costa Gomes
42225db26a9SVinicius Costa Gomes while (p) {
42325db26a9SVinicius Costa Gomes struct sk_buff *skb = rb_to_skb(p);
42425db26a9SVinicius Costa Gomes
42525db26a9SVinicius Costa Gomes p = rb_next(p);
42625db26a9SVinicius Costa Gomes
42709fd4860SJesus Sanchez-Palencia rb_erase_cached(&skb->rbnode, &q->head);
42825db26a9SVinicius Costa Gomes rtnl_kfree_skbs(skb, skb);
42925db26a9SVinicius Costa Gomes sch->q.qlen--;
43025db26a9SVinicius Costa Gomes }
43125db26a9SVinicius Costa Gomes }
43225db26a9SVinicius Costa Gomes
etf_reset(struct Qdisc * sch)43325db26a9SVinicius Costa Gomes static void etf_reset(struct Qdisc *sch)
43425db26a9SVinicius Costa Gomes {
43525db26a9SVinicius Costa Gomes struct etf_sched_data *q = qdisc_priv(sch);
43625db26a9SVinicius Costa Gomes
43725db26a9SVinicius Costa Gomes /* Only cancel watchdog if it's been initialized. */
43825db26a9SVinicius Costa Gomes if (q->watchdog.qdisc == sch)
43925db26a9SVinicius Costa Gomes qdisc_watchdog_cancel(&q->watchdog);
44025db26a9SVinicius Costa Gomes
44125db26a9SVinicius Costa Gomes /* No matter which mode we are on, it's safe to clear both lists. */
44225db26a9SVinicius Costa Gomes timesortedlist_clear(sch);
44325db26a9SVinicius Costa Gomes __qdisc_reset_queue(&sch->q);
44425db26a9SVinicius Costa Gomes
44525db26a9SVinicius Costa Gomes q->last = 0;
44625db26a9SVinicius Costa Gomes }
44725db26a9SVinicius Costa Gomes
etf_destroy(struct Qdisc * sch)44825db26a9SVinicius Costa Gomes static void etf_destroy(struct Qdisc *sch)
44925db26a9SVinicius Costa Gomes {
45025db26a9SVinicius Costa Gomes struct etf_sched_data *q = qdisc_priv(sch);
45188cab771SJesus Sanchez-Palencia struct net_device *dev = qdisc_dev(sch);
45225db26a9SVinicius Costa Gomes
45325db26a9SVinicius Costa Gomes /* Only cancel watchdog if it's been initialized. */
45425db26a9SVinicius Costa Gomes if (q->watchdog.qdisc == sch)
45525db26a9SVinicius Costa Gomes qdisc_watchdog_cancel(&q->watchdog);
45688cab771SJesus Sanchez-Palencia
45788cab771SJesus Sanchez-Palencia etf_disable_offload(dev, q);
45825db26a9SVinicius Costa Gomes }
45925db26a9SVinicius Costa Gomes
etf_dump(struct Qdisc * sch,struct sk_buff * skb)46025db26a9SVinicius Costa Gomes static int etf_dump(struct Qdisc *sch, struct sk_buff *skb)
46125db26a9SVinicius Costa Gomes {
46225db26a9SVinicius Costa Gomes struct etf_sched_data *q = qdisc_priv(sch);
46325db26a9SVinicius Costa Gomes struct tc_etf_qopt opt = { };
46425db26a9SVinicius Costa Gomes struct nlattr *nest;
46525db26a9SVinicius Costa Gomes
466ae0be8deSMichal Kubecek nest = nla_nest_start_noflag(skb, TCA_OPTIONS);
46725db26a9SVinicius Costa Gomes if (!nest)
46825db26a9SVinicius Costa Gomes goto nla_put_failure;
46925db26a9SVinicius Costa Gomes
47025db26a9SVinicius Costa Gomes opt.delta = q->delta;
47125db26a9SVinicius Costa Gomes opt.clockid = q->clockid;
47288cab771SJesus Sanchez-Palencia if (q->offload)
47388cab771SJesus Sanchez-Palencia opt.flags |= TC_ETF_OFFLOAD_ON;
47488cab771SJesus Sanchez-Palencia
47525db26a9SVinicius Costa Gomes if (q->deadline_mode)
47625db26a9SVinicius Costa Gomes opt.flags |= TC_ETF_DEADLINE_MODE_ON;
47725db26a9SVinicius Costa Gomes
478d14d2b20SVedang Patel if (q->skip_sock_check)
479d14d2b20SVedang Patel opt.flags |= TC_ETF_SKIP_SOCK_CHECK;
480d14d2b20SVedang Patel
48125db26a9SVinicius Costa Gomes if (nla_put(skb, TCA_ETF_PARMS, sizeof(opt), &opt))
48225db26a9SVinicius Costa Gomes goto nla_put_failure;
48325db26a9SVinicius Costa Gomes
48425db26a9SVinicius Costa Gomes return nla_nest_end(skb, nest);
48525db26a9SVinicius Costa Gomes
48625db26a9SVinicius Costa Gomes nla_put_failure:
48725db26a9SVinicius Costa Gomes nla_nest_cancel(skb, nest);
48825db26a9SVinicius Costa Gomes return -1;
48925db26a9SVinicius Costa Gomes }
49025db26a9SVinicius Costa Gomes
49125db26a9SVinicius Costa Gomes static struct Qdisc_ops etf_qdisc_ops __read_mostly = {
49225db26a9SVinicius Costa Gomes .id = "etf",
49325db26a9SVinicius Costa Gomes .priv_size = sizeof(struct etf_sched_data),
49425db26a9SVinicius Costa Gomes .enqueue = etf_enqueue_timesortedlist,
49525db26a9SVinicius Costa Gomes .dequeue = etf_dequeue_timesortedlist,
49625db26a9SVinicius Costa Gomes .peek = etf_peek_timesortedlist,
49725db26a9SVinicius Costa Gomes .init = etf_init,
49825db26a9SVinicius Costa Gomes .reset = etf_reset,
49925db26a9SVinicius Costa Gomes .destroy = etf_destroy,
50025db26a9SVinicius Costa Gomes .dump = etf_dump,
50125db26a9SVinicius Costa Gomes .owner = THIS_MODULE,
50225db26a9SVinicius Costa Gomes };
50325db26a9SVinicius Costa Gomes
etf_module_init(void)50425db26a9SVinicius Costa Gomes static int __init etf_module_init(void)
50525db26a9SVinicius Costa Gomes {
50625db26a9SVinicius Costa Gomes return register_qdisc(&etf_qdisc_ops);
50725db26a9SVinicius Costa Gomes }
50825db26a9SVinicius Costa Gomes
etf_module_exit(void)50925db26a9SVinicius Costa Gomes static void __exit etf_module_exit(void)
51025db26a9SVinicius Costa Gomes {
51125db26a9SVinicius Costa Gomes unregister_qdisc(&etf_qdisc_ops);
51225db26a9SVinicius Costa Gomes }
51325db26a9SVinicius Costa Gomes module_init(etf_module_init)
51425db26a9SVinicius Costa Gomes module_exit(etf_module_exit)
51525db26a9SVinicius Costa Gomes MODULE_LICENSE("GPL");
516