net/sched/sch_etf.c

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