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 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 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 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 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 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 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 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 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 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 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 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 (q->offload) 32788cab771SJesus Sanchez-Palencia return 0; 32888cab771SJesus Sanchez-Palencia 32988cab771SJesus Sanchez-Palencia if (!ops->ndo_setup_tc) { 33088cab771SJesus Sanchez-Palencia NL_SET_ERR_MSG(extack, "Specified device does not support ETF offload"); 33188cab771SJesus Sanchez-Palencia return -EOPNOTSUPP; 33288cab771SJesus Sanchez-Palencia } 33388cab771SJesus Sanchez-Palencia 33488cab771SJesus Sanchez-Palencia etf.queue = q->queue; 33588cab771SJesus Sanchez-Palencia etf.enable = 1; 33688cab771SJesus Sanchez-Palencia 33788cab771SJesus Sanchez-Palencia err = ops->ndo_setup_tc(dev, TC_SETUP_QDISC_ETF, &etf); 33888cab771SJesus Sanchez-Palencia if (err < 0) { 33988cab771SJesus Sanchez-Palencia NL_SET_ERR_MSG(extack, "Specified device failed to setup ETF hardware offload"); 34088cab771SJesus Sanchez-Palencia return err; 34188cab771SJesus Sanchez-Palencia } 34288cab771SJesus Sanchez-Palencia 34388cab771SJesus Sanchez-Palencia return 0; 34488cab771SJesus Sanchez-Palencia } 34588cab771SJesus Sanchez-Palencia 34625db26a9SVinicius Costa Gomes static int etf_init(struct Qdisc *sch, struct nlattr *opt, 34725db26a9SVinicius Costa Gomes struct netlink_ext_ack *extack) 34825db26a9SVinicius Costa Gomes { 34925db26a9SVinicius Costa Gomes struct etf_sched_data *q = qdisc_priv(sch); 35025db26a9SVinicius Costa Gomes struct net_device *dev = qdisc_dev(sch); 35125db26a9SVinicius Costa Gomes struct nlattr *tb[TCA_ETF_MAX + 1]; 35225db26a9SVinicius Costa Gomes struct tc_etf_qopt *qopt; 35325db26a9SVinicius Costa Gomes int err; 35425db26a9SVinicius Costa Gomes 35525db26a9SVinicius Costa Gomes if (!opt) { 35625db26a9SVinicius Costa Gomes NL_SET_ERR_MSG(extack, 35725db26a9SVinicius Costa Gomes "Missing ETF qdisc options which are mandatory"); 35825db26a9SVinicius Costa Gomes return -EINVAL; 35925db26a9SVinicius Costa Gomes } 36025db26a9SVinicius Costa Gomes 3618cb08174SJohannes Berg err = nla_parse_nested_deprecated(tb, TCA_ETF_MAX, opt, etf_policy, 3628cb08174SJohannes Berg extack); 36325db26a9SVinicius Costa Gomes if (err < 0) 36425db26a9SVinicius Costa Gomes return err; 36525db26a9SVinicius Costa Gomes 36625db26a9SVinicius Costa Gomes if (!tb[TCA_ETF_PARMS]) { 36725db26a9SVinicius Costa Gomes NL_SET_ERR_MSG(extack, "Missing mandatory ETF parameters"); 36825db26a9SVinicius Costa Gomes return -EINVAL; 36925db26a9SVinicius Costa Gomes } 37025db26a9SVinicius Costa Gomes 37125db26a9SVinicius Costa Gomes qopt = nla_data(tb[TCA_ETF_PARMS]); 37225db26a9SVinicius Costa Gomes 37388cab771SJesus Sanchez-Palencia pr_debug("delta %d clockid %d offload %s deadline %s\n", 37425db26a9SVinicius Costa Gomes qopt->delta, qopt->clockid, 37588cab771SJesus Sanchez-Palencia OFFLOAD_IS_ON(qopt) ? "on" : "off", 37625db26a9SVinicius Costa Gomes DEADLINE_MODE_IS_ON(qopt) ? "on" : "off"); 37725db26a9SVinicius Costa Gomes 37825db26a9SVinicius Costa Gomes err = validate_input_params(qopt, extack); 37925db26a9SVinicius Costa Gomes if (err < 0) 38025db26a9SVinicius Costa Gomes return err; 38125db26a9SVinicius Costa Gomes 38225db26a9SVinicius Costa Gomes q->queue = sch->dev_queue - netdev_get_tx_queue(dev, 0); 38325db26a9SVinicius Costa Gomes 38488cab771SJesus Sanchez-Palencia if (OFFLOAD_IS_ON(qopt)) { 38588cab771SJesus Sanchez-Palencia err = etf_enable_offload(dev, q, extack); 38688cab771SJesus Sanchez-Palencia if (err < 0) 38788cab771SJesus Sanchez-Palencia return err; 38888cab771SJesus Sanchez-Palencia } 38988cab771SJesus Sanchez-Palencia 39025db26a9SVinicius Costa Gomes /* Everything went OK, save the parameters used. */ 39125db26a9SVinicius Costa Gomes q->delta = qopt->delta; 39225db26a9SVinicius Costa Gomes q->clockid = qopt->clockid; 39388cab771SJesus Sanchez-Palencia q->offload = OFFLOAD_IS_ON(qopt); 39425db26a9SVinicius Costa Gomes q->deadline_mode = DEADLINE_MODE_IS_ON(qopt); 395d14d2b20SVedang Patel q->skip_sock_check = SKIP_SOCK_CHECK_IS_SET(qopt); 39625db26a9SVinicius Costa Gomes 39725db26a9SVinicius Costa Gomes switch (q->clockid) { 39825db26a9SVinicius Costa Gomes case CLOCK_REALTIME: 39925db26a9SVinicius Costa Gomes q->get_time = ktime_get_real; 40025db26a9SVinicius Costa Gomes break; 40125db26a9SVinicius Costa Gomes case CLOCK_MONOTONIC: 40225db26a9SVinicius Costa Gomes q->get_time = ktime_get; 40325db26a9SVinicius Costa Gomes break; 40425db26a9SVinicius Costa Gomes case CLOCK_BOOTTIME: 40525db26a9SVinicius Costa Gomes q->get_time = ktime_get_boottime; 40625db26a9SVinicius Costa Gomes break; 40725db26a9SVinicius Costa Gomes case CLOCK_TAI: 40825db26a9SVinicius Costa Gomes q->get_time = ktime_get_clocktai; 40925db26a9SVinicius Costa Gomes break; 41025db26a9SVinicius Costa Gomes default: 41125db26a9SVinicius Costa Gomes NL_SET_ERR_MSG(extack, "Clockid is not supported"); 41225db26a9SVinicius Costa Gomes return -ENOTSUPP; 41325db26a9SVinicius Costa Gomes } 41425db26a9SVinicius Costa Gomes 41525db26a9SVinicius Costa Gomes qdisc_watchdog_init_clockid(&q->watchdog, sch, q->clockid); 41625db26a9SVinicius Costa Gomes 41725db26a9SVinicius Costa Gomes return 0; 41825db26a9SVinicius Costa Gomes } 41925db26a9SVinicius Costa Gomes 42025db26a9SVinicius Costa Gomes static void timesortedlist_clear(struct Qdisc *sch) 42125db26a9SVinicius Costa Gomes { 42225db26a9SVinicius Costa Gomes struct etf_sched_data *q = qdisc_priv(sch); 42309fd4860SJesus Sanchez-Palencia struct rb_node *p = rb_first_cached(&q->head); 42425db26a9SVinicius Costa Gomes 42525db26a9SVinicius Costa Gomes while (p) { 42625db26a9SVinicius Costa Gomes struct sk_buff *skb = rb_to_skb(p); 42725db26a9SVinicius Costa Gomes 42825db26a9SVinicius Costa Gomes p = rb_next(p); 42925db26a9SVinicius Costa Gomes 43009fd4860SJesus Sanchez-Palencia rb_erase_cached(&skb->rbnode, &q->head); 43125db26a9SVinicius Costa Gomes rtnl_kfree_skbs(skb, skb); 43225db26a9SVinicius Costa Gomes sch->q.qlen--; 43325db26a9SVinicius Costa Gomes } 43425db26a9SVinicius Costa Gomes } 43525db26a9SVinicius Costa Gomes 43625db26a9SVinicius Costa Gomes static void etf_reset(struct Qdisc *sch) 43725db26a9SVinicius Costa Gomes { 43825db26a9SVinicius Costa Gomes struct etf_sched_data *q = qdisc_priv(sch); 43925db26a9SVinicius Costa Gomes 44025db26a9SVinicius Costa Gomes /* Only cancel watchdog if it's been initialized. */ 44125db26a9SVinicius Costa Gomes if (q->watchdog.qdisc == sch) 44225db26a9SVinicius Costa Gomes qdisc_watchdog_cancel(&q->watchdog); 44325db26a9SVinicius Costa Gomes 44425db26a9SVinicius Costa Gomes /* No matter which mode we are on, it's safe to clear both lists. */ 44525db26a9SVinicius Costa Gomes timesortedlist_clear(sch); 44625db26a9SVinicius Costa Gomes __qdisc_reset_queue(&sch->q); 44725db26a9SVinicius Costa Gomes 44825db26a9SVinicius Costa Gomes sch->qstats.backlog = 0; 44925db26a9SVinicius Costa Gomes sch->q.qlen = 0; 45025db26a9SVinicius Costa Gomes 45125db26a9SVinicius Costa Gomes q->last = 0; 45225db26a9SVinicius Costa Gomes } 45325db26a9SVinicius Costa Gomes 45425db26a9SVinicius Costa Gomes static void etf_destroy(struct Qdisc *sch) 45525db26a9SVinicius Costa Gomes { 45625db26a9SVinicius Costa Gomes struct etf_sched_data *q = qdisc_priv(sch); 45788cab771SJesus Sanchez-Palencia struct net_device *dev = qdisc_dev(sch); 45825db26a9SVinicius Costa Gomes 45925db26a9SVinicius Costa Gomes /* Only cancel watchdog if it's been initialized. */ 46025db26a9SVinicius Costa Gomes if (q->watchdog.qdisc == sch) 46125db26a9SVinicius Costa Gomes qdisc_watchdog_cancel(&q->watchdog); 46288cab771SJesus Sanchez-Palencia 46388cab771SJesus Sanchez-Palencia etf_disable_offload(dev, q); 46425db26a9SVinicius Costa Gomes } 46525db26a9SVinicius Costa Gomes 46625db26a9SVinicius Costa Gomes static int etf_dump(struct Qdisc *sch, struct sk_buff *skb) 46725db26a9SVinicius Costa Gomes { 46825db26a9SVinicius Costa Gomes struct etf_sched_data *q = qdisc_priv(sch); 46925db26a9SVinicius Costa Gomes struct tc_etf_qopt opt = { }; 47025db26a9SVinicius Costa Gomes struct nlattr *nest; 47125db26a9SVinicius Costa Gomes 472ae0be8deSMichal Kubecek nest = nla_nest_start_noflag(skb, TCA_OPTIONS); 47325db26a9SVinicius Costa Gomes if (!nest) 47425db26a9SVinicius Costa Gomes goto nla_put_failure; 47525db26a9SVinicius Costa Gomes 47625db26a9SVinicius Costa Gomes opt.delta = q->delta; 47725db26a9SVinicius Costa Gomes opt.clockid = q->clockid; 47888cab771SJesus Sanchez-Palencia if (q->offload) 47988cab771SJesus Sanchez-Palencia opt.flags |= TC_ETF_OFFLOAD_ON; 48088cab771SJesus Sanchez-Palencia 48125db26a9SVinicius Costa Gomes if (q->deadline_mode) 48225db26a9SVinicius Costa Gomes opt.flags |= TC_ETF_DEADLINE_MODE_ON; 48325db26a9SVinicius Costa Gomes 484d14d2b20SVedang Patel if (q->skip_sock_check) 485d14d2b20SVedang Patel opt.flags |= TC_ETF_SKIP_SOCK_CHECK; 486d14d2b20SVedang Patel 48725db26a9SVinicius Costa Gomes if (nla_put(skb, TCA_ETF_PARMS, sizeof(opt), &opt)) 48825db26a9SVinicius Costa Gomes goto nla_put_failure; 48925db26a9SVinicius Costa Gomes 49025db26a9SVinicius Costa Gomes return nla_nest_end(skb, nest); 49125db26a9SVinicius Costa Gomes 49225db26a9SVinicius Costa Gomes nla_put_failure: 49325db26a9SVinicius Costa Gomes nla_nest_cancel(skb, nest); 49425db26a9SVinicius Costa Gomes return -1; 49525db26a9SVinicius Costa Gomes } 49625db26a9SVinicius Costa Gomes 49725db26a9SVinicius Costa Gomes static struct Qdisc_ops etf_qdisc_ops __read_mostly = { 49825db26a9SVinicius Costa Gomes .id = "etf", 49925db26a9SVinicius Costa Gomes .priv_size = sizeof(struct etf_sched_data), 50025db26a9SVinicius Costa Gomes .enqueue = etf_enqueue_timesortedlist, 50125db26a9SVinicius Costa Gomes .dequeue = etf_dequeue_timesortedlist, 50225db26a9SVinicius Costa Gomes .peek = etf_peek_timesortedlist, 50325db26a9SVinicius Costa Gomes .init = etf_init, 50425db26a9SVinicius Costa Gomes .reset = etf_reset, 50525db26a9SVinicius Costa Gomes .destroy = etf_destroy, 50625db26a9SVinicius Costa Gomes .dump = etf_dump, 50725db26a9SVinicius Costa Gomes .owner = THIS_MODULE, 50825db26a9SVinicius Costa Gomes }; 50925db26a9SVinicius Costa Gomes 51025db26a9SVinicius Costa Gomes static int __init etf_module_init(void) 51125db26a9SVinicius Costa Gomes { 51225db26a9SVinicius Costa Gomes return register_qdisc(&etf_qdisc_ops); 51325db26a9SVinicius Costa Gomes } 51425db26a9SVinicius Costa Gomes 51525db26a9SVinicius Costa Gomes static void __exit etf_module_exit(void) 51625db26a9SVinicius Costa Gomes { 51725db26a9SVinicius Costa Gomes unregister_qdisc(&etf_qdisc_ops); 51825db26a9SVinicius Costa Gomes } 51925db26a9SVinicius Costa Gomes module_init(etf_module_init) 52025db26a9SVinicius Costa Gomes module_exit(etf_module_exit) 52125db26a9SVinicius Costa Gomes MODULE_LICENSE("GPL"); 522