net/sched/sch_etf.c

0001 // SPDX-License-Identifier: GPL-2.0
0002
0003 /* net/sched/sch_etf.c  Earliest TxTime First queueing discipline.
0004  *
0005  * Authors: Jesus Sanchez-Palencia <jesus.sanchez-palencia@intel.com>
0006  *      Vinicius Costa Gomes <vinicius.gomes@intel.com>
0007  */
0008
0009 #include <linux/module.h>
0010 #include <linux/types.h>
0011 #include <linux/kernel.h>
0012 #include <linux/string.h>
0013 #include <linux/errno.h>
0014 #include <linux/errqueue.h>
0015 #include <linux/rbtree.h>
0016 #include <linux/skbuff.h>
0017 #include <linux/posix-timers.h>
0018 #include <net/netlink.h>
0019 #include <net/sch_generic.h>
0020 #include <net/pkt_sched.h>
0021 #include <net/sock.h>
0022
0023 #define DEADLINE_MODE_IS_ON(x) ((x)->flags & TC_ETF_DEADLINE_MODE_ON)
0024 #define OFFLOAD_IS_ON(x) ((x)->flags & TC_ETF_OFFLOAD_ON)
0025 #define SKIP_SOCK_CHECK_IS_SET(x) ((x)->flags & TC_ETF_SKIP_SOCK_CHECK)
0026
0027 struct etf_sched_data {
0028     bool offload;
0029     bool deadline_mode;
0030     bool skip_sock_check;
0031     int clockid;
0032     int queue;
0033     s32 delta; /* in ns */
0034     ktime_t last; /* The txtime of the last skb sent to the netdevice. */
0035     struct rb_root_cached head;
0036     struct qdisc_watchdog watchdog;
0037     ktime_t (*get_time)(void);
0038 };
0039
0040 static const struct nla_policy etf_policy[TCA_ETF_MAX + 1] = {
0041     [TCA_ETF_PARMS] = { .len = sizeof(struct tc_etf_qopt) },
0042 };
0043
0044 static inline int validate_input_params(struct tc_etf_qopt *qopt,
0045                     struct netlink_ext_ack *extack)
0046 {
0047     /* Check if params comply to the following rules:
0048      *  * Clockid and delta must be valid.
0049      *
0050      *  * Dynamic clockids are not supported.
0051      *
0052      *  * Delta must be a positive integer.
0053      *
0054      * Also note that for the HW offload case, we must
0055      * expect that system clocks have been synchronized to PHC.
0056      */
0057     if (qopt->clockid < 0) {
0058         NL_SET_ERR_MSG(extack, "Dynamic clockids are not supported");
0059         return -ENOTSUPP;
0060     }
0061
0062     if (qopt->clockid != CLOCK_TAI) {
0063         NL_SET_ERR_MSG(extack, "Invalid clockid. CLOCK_TAI must be used");
0064         return -EINVAL;
0065     }
0066
0067     if (qopt->delta < 0) {
0068         NL_SET_ERR_MSG(extack, "Delta must be positive");
0069         return -EINVAL;
0070     }
0071
0072     return 0;
0073 }
0074
0075 static bool is_packet_valid(struct Qdisc *sch, struct sk_buff *nskb)
0076 {
0077     struct etf_sched_data *q = qdisc_priv(sch);
0078     ktime_t txtime = nskb->tstamp;
0079     struct sock *sk = nskb->sk;
0080     ktime_t now;
0081
0082     if (q->skip_sock_check)
0083         goto skip;
0084
0085     if (!sk || !sk_fullsock(sk))
0086         return false;
0087
0088     if (!sock_flag(sk, SOCK_TXTIME))
0089         return false;
0090
0091     /* We don't perform crosstimestamping.
0092      * Drop if packet's clockid differs from qdisc's.
0093      */
0094     if (sk->sk_clockid != q->clockid)
0095         return false;
0096
0097     if (sk->sk_txtime_deadline_mode != q->deadline_mode)
0098         return false;
0099
0100 skip:
0101     now = q->get_time();
0102     if (ktime_before(txtime, now) || ktime_before(txtime, q->last))
0103         return false;
0104
0105     return true;
0106 }
0107
0108 static struct sk_buff *etf_peek_timesortedlist(struct Qdisc *sch)
0109 {
0110     struct etf_sched_data *q = qdisc_priv(sch);
0111     struct rb_node *p;
0112
0113     p = rb_first_cached(&q->head);
0114     if (!p)
0115         return NULL;
0116
0117     return rb_to_skb(p);
0118 }
0119
0120 static void reset_watchdog(struct Qdisc *sch)
0121 {
0122     struct etf_sched_data *q = qdisc_priv(sch);
0123     struct sk_buff *skb = etf_peek_timesortedlist(sch);
0124     ktime_t next;
0125
0126     if (!skb) {
0127         qdisc_watchdog_cancel(&q->watchdog);
0128         return;
0129     }
0130
0131     next = ktime_sub_ns(skb->tstamp, q->delta);
0132     qdisc_watchdog_schedule_ns(&q->watchdog, ktime_to_ns(next));
0133 }
0134
0135 static void report_sock_error(struct sk_buff *skb, u32 err, u8 code)
0136 {
0137     struct sock_exterr_skb *serr;
0138     struct sk_buff *clone;
0139     ktime_t txtime = skb->tstamp;
0140     struct sock *sk = skb->sk;
0141
0142     if (!sk || !sk_fullsock(sk) || !(sk->sk_txtime_report_errors))
0143         return;
0144
0145     clone = skb_clone(skb, GFP_ATOMIC);
0146     if (!clone)
0147         return;
0148
0149     serr = SKB_EXT_ERR(clone);
0150     serr->ee.ee_errno = err;
0151     serr->ee.ee_origin = SO_EE_ORIGIN_TXTIME;
0152     serr->ee.ee_type = 0;
0153     serr->ee.ee_code = code;
0154     serr->ee.ee_pad = 0;
0155     serr->ee.ee_data = (txtime >> 32); /* high part of tstamp */
0156     serr->ee.ee_info = txtime; /* low part of tstamp */
0157
0158     if (sock_queue_err_skb(sk, clone))
0159         kfree_skb(clone);
0160 }
0161
0162 static int etf_enqueue_timesortedlist(struct sk_buff *nskb, struct Qdisc *sch,
0163                       struct sk_buff **to_free)
0164 {
0165     struct etf_sched_data *q = qdisc_priv(sch);
0166     struct rb_node **p = &q->head.rb_root.rb_node, *parent = NULL;
0167     ktime_t txtime = nskb->tstamp;
0168     bool leftmost = true;
0169
0170     if (!is_packet_valid(sch, nskb)) {
0171         report_sock_error(nskb, EINVAL,
0172                   SO_EE_CODE_TXTIME_INVALID_PARAM);
0173         return qdisc_drop(nskb, sch, to_free);
0174     }
0175
0176     while (*p) {
0177         struct sk_buff *skb;
0178
0179         parent = *p;
0180         skb = rb_to_skb(parent);
0181         if (ktime_compare(txtime, skb->tstamp) >= 0) {
0182             p = &parent->rb_right;
0183             leftmost = false;
0184         } else {
0185             p = &parent->rb_left;
0186         }
0187     }
0188     rb_link_node(&nskb->rbnode, parent, p);
0189     rb_insert_color_cached(&nskb->rbnode, &q->head, leftmost);
0190
0191     qdisc_qstats_backlog_inc(sch, nskb);
0192     sch->q.qlen++;
0193
0194     /* Now we may need to re-arm the qdisc watchdog for the next packet. */
0195     reset_watchdog(sch);
0196
0197     return NET_XMIT_SUCCESS;
0198 }
0199
0200 static void timesortedlist_drop(struct Qdisc *sch, struct sk_buff *skb,
0201                 ktime_t now)
0202 {
0203     struct etf_sched_data *q = qdisc_priv(sch);
0204     struct sk_buff *to_free = NULL;
0205     struct sk_buff *tmp = NULL;
0206
0207     skb_rbtree_walk_from_safe(skb, tmp) {
0208         if (ktime_after(skb->tstamp, now))
0209             break;
0210
0211         rb_erase_cached(&skb->rbnode, &q->head);
0212
0213         /* The rbnode field in the skb re-uses these fields, now that
0214          * we are done with the rbnode, reset them.
0215          */
0216         skb->next = NULL;
0217         skb->prev = NULL;
0218         skb->dev = qdisc_dev(sch);
0219
0220         report_sock_error(skb, ECANCELED, SO_EE_CODE_TXTIME_MISSED);
0221
0222         qdisc_qstats_backlog_dec(sch, skb);
0223         qdisc_drop(skb, sch, &to_free);
0224         qdisc_qstats_overlimit(sch);
0225         sch->q.qlen--;
0226     }
0227
0228     kfree_skb_list(to_free);
0229 }
0230
0231 static void timesortedlist_remove(struct Qdisc *sch, struct sk_buff *skb)
0232 {
0233     struct etf_sched_data *q = qdisc_priv(sch);
0234
0235     rb_erase_cached(&skb->rbnode, &q->head);
0236
0237     /* The rbnode field in the skb re-uses these fields, now that
0238      * we are done with the rbnode, reset them.
0239      */
0240     skb->next = NULL;
0241     skb->prev = NULL;
0242     skb->dev = qdisc_dev(sch);
0243
0244     qdisc_qstats_backlog_dec(sch, skb);
0245
0246     qdisc_bstats_update(sch, skb);
0247
0248     q->last = skb->tstamp;
0249
0250     sch->q.qlen--;
0251 }
0252
0253 static struct sk_buff *etf_dequeue_timesortedlist(struct Qdisc *sch)
0254 {
0255     struct etf_sched_data *q = qdisc_priv(sch);
0256     struct sk_buff *skb;
0257     ktime_t now, next;
0258
0259     skb = etf_peek_timesortedlist(sch);
0260     if (!skb)
0261         return NULL;
0262
0263     now = q->get_time();
0264
0265     /* Drop if packet has expired while in queue. */
0266     if (ktime_before(skb->tstamp, now)) {
0267         timesortedlist_drop(sch, skb, now);
0268         skb = NULL;
0269         goto out;
0270     }
0271
0272     /* When in deadline mode, dequeue as soon as possible and change the
0273      * txtime from deadline to (now + delta).
0274      */
0275     if (q->deadline_mode) {
0276         timesortedlist_remove(sch, skb);
0277         skb->tstamp = now;
0278         goto out;
0279     }
0280
0281     next = ktime_sub_ns(skb->tstamp, q->delta);
0282
0283     /* Dequeue only if now is within the [txtime - delta, txtime] range. */
0284     if (ktime_after(now, next))
0285         timesortedlist_remove(sch, skb);
0286     else
0287         skb = NULL;
0288
0289 out:
0290     /* Now we may need to re-arm the qdisc watchdog for the next packet. */
0291     reset_watchdog(sch);
0292
0293     return skb;
0294 }
0295
0296 static void etf_disable_offload(struct net_device *dev,
0297                 struct etf_sched_data *q)
0298 {
0299     struct tc_etf_qopt_offload etf = { };
0300     const struct net_device_ops *ops;
0301     int err;
0302
0303     if (!q->offload)
0304         return;
0305
0306     ops = dev->netdev_ops;
0307     if (!ops->ndo_setup_tc)
0308         return;
0309
0310     etf.queue = q->queue;
0311     etf.enable = 0;
0312
0313     err = ops->ndo_setup_tc(dev, TC_SETUP_QDISC_ETF, &etf);
0314     if (err < 0)
0315         pr_warn("Couldn't disable ETF offload for queue %d\n",
0316             etf.queue);
0317 }
0318
0319 static int etf_enable_offload(struct net_device *dev, struct etf_sched_data *q,
0320                   struct netlink_ext_ack *extack)
0321 {
0322     const struct net_device_ops *ops = dev->netdev_ops;
0323     struct tc_etf_qopt_offload etf = { };
0324     int err;
0325
0326     if (q->offload)
0327         return 0;
0328
0329     if (!ops->ndo_setup_tc) {
0330         NL_SET_ERR_MSG(extack, "Specified device does not support ETF offload");
0331         return -EOPNOTSUPP;
0332     }
0333
0334     etf.queue = q->queue;
0335     etf.enable = 1;
0336
0337     err = ops->ndo_setup_tc(dev, TC_SETUP_QDISC_ETF, &etf);
0338     if (err < 0) {
0339         NL_SET_ERR_MSG(extack, "Specified device failed to setup ETF hardware offload");
0340         return err;
0341     }
0342
0343     return 0;
0344 }
0345
0346 static int etf_init(struct Qdisc *sch, struct nlattr *opt,
0347             struct netlink_ext_ack *extack)
0348 {
0349     struct etf_sched_data *q = qdisc_priv(sch);
0350     struct net_device *dev = qdisc_dev(sch);
0351     struct nlattr *tb[TCA_ETF_MAX + 1];
0352     struct tc_etf_qopt *qopt;
0353     int err;
0354
0355     if (!opt) {
0356         NL_SET_ERR_MSG(extack,
0357                    "Missing ETF qdisc options which are mandatory");
0358         return -EINVAL;
0359     }
0360
0361     err = nla_parse_nested_deprecated(tb, TCA_ETF_MAX, opt, etf_policy,
0362                       extack);
0363     if (err < 0)
0364         return err;
0365
0366     if (!tb[TCA_ETF_PARMS]) {
0367         NL_SET_ERR_MSG(extack, "Missing mandatory ETF parameters");
0368         return -EINVAL;
0369     }
0370
0371     qopt = nla_data(tb[TCA_ETF_PARMS]);
0372
0373     pr_debug("delta %d clockid %d offload %s deadline %s\n",
0374          qopt->delta, qopt->clockid,
0375          OFFLOAD_IS_ON(qopt) ? "on" : "off",
0376          DEADLINE_MODE_IS_ON(qopt) ? "on" : "off");
0377
0378     err = validate_input_params(qopt, extack);
0379     if (err < 0)
0380         return err;
0381
0382     q->queue = sch->dev_queue - netdev_get_tx_queue(dev, 0);
0383
0384     if (OFFLOAD_IS_ON(qopt)) {
0385         err = etf_enable_offload(dev, q, extack);
0386         if (err < 0)
0387             return err;
0388     }
0389
0390     /* Everything went OK, save the parameters used. */
0391     q->delta = qopt->delta;
0392     q->clockid = qopt->clockid;
0393     q->offload = OFFLOAD_IS_ON(qopt);
0394     q->deadline_mode = DEADLINE_MODE_IS_ON(qopt);
0395     q->skip_sock_check = SKIP_SOCK_CHECK_IS_SET(qopt);
0396
0397     switch (q->clockid) {
0398     case CLOCK_REALTIME:
0399         q->get_time = ktime_get_real;
0400         break;
0401     case CLOCK_MONOTONIC:
0402         q->get_time = ktime_get;
0403         break;
0404     case CLOCK_BOOTTIME:
0405         q->get_time = ktime_get_boottime;
0406         break;
0407     case CLOCK_TAI:
0408         q->get_time = ktime_get_clocktai;
0409         break;
0410     default:
0411         NL_SET_ERR_MSG(extack, "Clockid is not supported");
0412         return -ENOTSUPP;
0413     }
0414
0415     qdisc_watchdog_init_clockid(&q->watchdog, sch, q->clockid);
0416
0417     return 0;
0418 }
0419
0420 static void timesortedlist_clear(struct Qdisc *sch)
0421 {
0422     struct etf_sched_data *q = qdisc_priv(sch);
0423     struct rb_node *p = rb_first_cached(&q->head);
0424
0425     while (p) {
0426         struct sk_buff *skb = rb_to_skb(p);
0427
0428         p = rb_next(p);
0429
0430         rb_erase_cached(&skb->rbnode, &q->head);
0431         rtnl_kfree_skbs(skb, skb);
0432         sch->q.qlen--;
0433     }
0434 }
0435
0436 static void etf_reset(struct Qdisc *sch)
0437 {
0438     struct etf_sched_data *q = qdisc_priv(sch);
0439
0440     /* Only cancel watchdog if it's been initialized. */
0441     if (q->watchdog.qdisc == sch)
0442         qdisc_watchdog_cancel(&q->watchdog);
0443
0444     /* No matter which mode we are on, it's safe to clear both lists. */
0445     timesortedlist_clear(sch);
0446     __qdisc_reset_queue(&sch->q);
0447
0448     sch->qstats.backlog = 0;
0449     sch->q.qlen = 0;
0450
0451     q->last = 0;
0452 }
0453
0454 static void etf_destroy(struct Qdisc *sch)
0455 {
0456     struct etf_sched_data *q = qdisc_priv(sch);
0457     struct net_device *dev = qdisc_dev(sch);
0458
0459     /* Only cancel watchdog if it's been initialized. */
0460     if (q->watchdog.qdisc == sch)
0461         qdisc_watchdog_cancel(&q->watchdog);
0462
0463     etf_disable_offload(dev, q);
0464 }
0465
0466 static int etf_dump(struct Qdisc *sch, struct sk_buff *skb)
0467 {
0468     struct etf_sched_data *q = qdisc_priv(sch);
0469     struct tc_etf_qopt opt = { };
0470     struct nlattr *nest;
0471
0472     nest = nla_nest_start_noflag(skb, TCA_OPTIONS);
0473     if (!nest)
0474         goto nla_put_failure;
0475
0476     opt.delta = q->delta;
0477     opt.clockid = q->clockid;
0478     if (q->offload)
0479         opt.flags |= TC_ETF_OFFLOAD_ON;
0480
0481     if (q->deadline_mode)
0482         opt.flags |= TC_ETF_DEADLINE_MODE_ON;
0483
0484     if (q->skip_sock_check)
0485         opt.flags |= TC_ETF_SKIP_SOCK_CHECK;
0486
0487     if (nla_put(skb, TCA_ETF_PARMS, sizeof(opt), &opt))
0488         goto nla_put_failure;
0489
0490     return nla_nest_end(skb, nest);
0491
0492 nla_put_failure:
0493     nla_nest_cancel(skb, nest);
0494     return -1;
0495 }
0496
0497 static struct Qdisc_ops etf_qdisc_ops __read_mostly = {
0498     .id     =   "etf",
0499     .priv_size  =   sizeof(struct etf_sched_data),
0500     .enqueue    =   etf_enqueue_timesortedlist,
0501     .dequeue    =   etf_dequeue_timesortedlist,
0502     .peek       =   etf_peek_timesortedlist,
0503     .init       =   etf_init,
0504     .reset      =   etf_reset,
0505     .destroy    =   etf_destroy,
0506     .dump       =   etf_dump,
0507     .owner      =   THIS_MODULE,
0508 };
0509
0510 static int __init etf_module_init(void)
0511 {
0512     return register_qdisc(&etf_qdisc_ops);
0513 }
0514
0515 static void __exit etf_module_exit(void)
0516 {
0517     unregister_qdisc(&etf_qdisc_ops);
0518 }
0519 module_init(etf_module_init)
0520 module_exit(etf_module_exit)
0521 MODULE_LICENSE("GPL");