net/sched/sch_prio.c

0001 // SPDX-License-Identifier: GPL-2.0-or-later
0002 /*
0003  * net/sched/sch_prio.c Simple 3-band priority "scheduler".
0004  *
0005  * Authors: Alexey Kuznetsov, <kuznet@ms2.inr.ac.ru>
0006  * Fixes:       19990609: J Hadi Salim <hadi@nortelnetworks.com>:
0007  *              Init --  EINVAL when opt undefined
0008  */
0009
0010 #include <linux/module.h>
0011 #include <linux/slab.h>
0012 #include <linux/types.h>
0013 #include <linux/kernel.h>
0014 #include <linux/string.h>
0015 #include <linux/errno.h>
0016 #include <linux/skbuff.h>
0017 #include <net/netlink.h>
0018 #include <net/pkt_sched.h>
0019 #include <net/pkt_cls.h>
0020
0021 struct prio_sched_data {
0022     int bands;
0023     struct tcf_proto __rcu *filter_list;
0024     struct tcf_block *block;
0025     u8  prio2band[TC_PRIO_MAX+1];
0026     struct Qdisc *queues[TCQ_PRIO_BANDS];
0027 };
0028
0029
0030 static struct Qdisc *
0031 prio_classify(struct sk_buff *skb, struct Qdisc *sch, int *qerr)
0032 {
0033     struct prio_sched_data *q = qdisc_priv(sch);
0034     u32 band = skb->priority;
0035     struct tcf_result res;
0036     struct tcf_proto *fl;
0037     int err;
0038
0039     *qerr = NET_XMIT_SUCCESS | __NET_XMIT_BYPASS;
0040     if (TC_H_MAJ(skb->priority) != sch->handle) {
0041         fl = rcu_dereference_bh(q->filter_list);
0042         err = tcf_classify(skb, NULL, fl, &res, false);
0043 #ifdef CONFIG_NET_CLS_ACT
0044         switch (err) {
0045         case TC_ACT_STOLEN:
0046         case TC_ACT_QUEUED:
0047         case TC_ACT_TRAP:
0048             *qerr = NET_XMIT_SUCCESS | __NET_XMIT_STOLEN;
0049             fallthrough;
0050         case TC_ACT_SHOT:
0051             return NULL;
0052         }
0053 #endif
0054         if (!fl || err < 0) {
0055             if (TC_H_MAJ(band))
0056                 band = 0;
0057             return q->queues[q->prio2band[band & TC_PRIO_MAX]];
0058         }
0059         band = res.classid;
0060     }
0061     band = TC_H_MIN(band) - 1;
0062     if (band >= q->bands)
0063         return q->queues[q->prio2band[0]];
0064
0065     return q->queues[band];
0066 }
0067
0068 static int
0069 prio_enqueue(struct sk_buff *skb, struct Qdisc *sch, struct sk_buff **to_free)
0070 {
0071     unsigned int len = qdisc_pkt_len(skb);
0072     struct Qdisc *qdisc;
0073     int ret;
0074
0075     qdisc = prio_classify(skb, sch, &ret);
0076 #ifdef CONFIG_NET_CLS_ACT
0077     if (qdisc == NULL) {
0078
0079         if (ret & __NET_XMIT_BYPASS)
0080             qdisc_qstats_drop(sch);
0081         __qdisc_drop(skb, to_free);
0082         return ret;
0083     }
0084 #endif
0085
0086     ret = qdisc_enqueue(skb, qdisc, to_free);
0087     if (ret == NET_XMIT_SUCCESS) {
0088         sch->qstats.backlog += len;
0089         sch->q.qlen++;
0090         return NET_XMIT_SUCCESS;
0091     }
0092     if (net_xmit_drop_count(ret))
0093         qdisc_qstats_drop(sch);
0094     return ret;
0095 }
0096
0097 static struct sk_buff *prio_peek(struct Qdisc *sch)
0098 {
0099     struct prio_sched_data *q = qdisc_priv(sch);
0100     int prio;
0101
0102     for (prio = 0; prio < q->bands; prio++) {
0103         struct Qdisc *qdisc = q->queues[prio];
0104         struct sk_buff *skb = qdisc->ops->peek(qdisc);
0105         if (skb)
0106             return skb;
0107     }
0108     return NULL;
0109 }
0110
0111 static struct sk_buff *prio_dequeue(struct Qdisc *sch)
0112 {
0113     struct prio_sched_data *q = qdisc_priv(sch);
0114     int prio;
0115
0116     for (prio = 0; prio < q->bands; prio++) {
0117         struct Qdisc *qdisc = q->queues[prio];
0118         struct sk_buff *skb = qdisc_dequeue_peeked(qdisc);
0119         if (skb) {
0120             qdisc_bstats_update(sch, skb);
0121             qdisc_qstats_backlog_dec(sch, skb);
0122             sch->q.qlen--;
0123             return skb;
0124         }
0125     }
0126     return NULL;
0127
0128 }
0129
0130 static void
0131 prio_reset(struct Qdisc *sch)
0132 {
0133     int prio;
0134     struct prio_sched_data *q = qdisc_priv(sch);
0135
0136     for (prio = 0; prio < q->bands; prio++)
0137         qdisc_reset(q->queues[prio]);
0138     sch->qstats.backlog = 0;
0139     sch->q.qlen = 0;
0140 }
0141
0142 static int prio_offload(struct Qdisc *sch, struct tc_prio_qopt *qopt)
0143 {
0144     struct net_device *dev = qdisc_dev(sch);
0145     struct tc_prio_qopt_offload opt = {
0146         .handle = sch->handle,
0147         .parent = sch->parent,
0148     };
0149
0150     if (!tc_can_offload(dev) || !dev->netdev_ops->ndo_setup_tc)
0151         return -EOPNOTSUPP;
0152
0153     if (qopt) {
0154         opt.command = TC_PRIO_REPLACE;
0155         opt.replace_params.bands = qopt->bands;
0156         memcpy(&opt.replace_params.priomap, qopt->priomap,
0157                TC_PRIO_MAX + 1);
0158         opt.replace_params.qstats = &sch->qstats;
0159     } else {
0160         opt.command = TC_PRIO_DESTROY;
0161     }
0162
0163     return dev->netdev_ops->ndo_setup_tc(dev, TC_SETUP_QDISC_PRIO, &opt);
0164 }
0165
0166 static void
0167 prio_destroy(struct Qdisc *sch)
0168 {
0169     int prio;
0170     struct prio_sched_data *q = qdisc_priv(sch);
0171
0172     tcf_block_put(q->block);
0173     prio_offload(sch, NULL);
0174     for (prio = 0; prio < q->bands; prio++)
0175         qdisc_put(q->queues[prio]);
0176 }
0177
0178 static int prio_tune(struct Qdisc *sch, struct nlattr *opt,
0179              struct netlink_ext_ack *extack)
0180 {
0181     struct prio_sched_data *q = qdisc_priv(sch);
0182     struct Qdisc *queues[TCQ_PRIO_BANDS];
0183     int oldbands = q->bands, i;
0184     struct tc_prio_qopt *qopt;
0185
0186     if (nla_len(opt) < sizeof(*qopt))
0187         return -EINVAL;
0188     qopt = nla_data(opt);
0189
0190     if (qopt->bands > TCQ_PRIO_BANDS || qopt->bands < 2)
0191         return -EINVAL;
0192
0193     for (i = 0; i <= TC_PRIO_MAX; i++) {
0194         if (qopt->priomap[i] >= qopt->bands)
0195             return -EINVAL;
0196     }
0197
0198     /* Before commit, make sure we can allocate all new qdiscs */
0199     for (i = oldbands; i < qopt->bands; i++) {
0200         queues[i] = qdisc_create_dflt(sch->dev_queue, &pfifo_qdisc_ops,
0201                           TC_H_MAKE(sch->handle, i + 1),
0202                           extack);
0203         if (!queues[i]) {
0204             while (i > oldbands)
0205                 qdisc_put(queues[--i]);
0206             return -ENOMEM;
0207         }
0208     }
0209
0210     prio_offload(sch, qopt);
0211     sch_tree_lock(sch);
0212     q->bands = qopt->bands;
0213     memcpy(q->prio2band, qopt->priomap, TC_PRIO_MAX+1);
0214
0215     for (i = q->bands; i < oldbands; i++)
0216         qdisc_tree_flush_backlog(q->queues[i]);
0217
0218     for (i = oldbands; i < q->bands; i++) {
0219         q->queues[i] = queues[i];
0220         if (q->queues[i] != &noop_qdisc)
0221             qdisc_hash_add(q->queues[i], true);
0222     }
0223
0224     sch_tree_unlock(sch);
0225
0226     for (i = q->bands; i < oldbands; i++)
0227         qdisc_put(q->queues[i]);
0228     return 0;
0229 }
0230
0231 static int prio_init(struct Qdisc *sch, struct nlattr *opt,
0232              struct netlink_ext_ack *extack)
0233 {
0234     struct prio_sched_data *q = qdisc_priv(sch);
0235     int err;
0236
0237     if (!opt)
0238         return -EINVAL;
0239
0240     err = tcf_block_get(&q->block, &q->filter_list, sch, extack);
0241     if (err)
0242         return err;
0243
0244     return prio_tune(sch, opt, extack);
0245 }
0246
0247 static int prio_dump_offload(struct Qdisc *sch)
0248 {
0249     struct tc_prio_qopt_offload hw_stats = {
0250         .command = TC_PRIO_STATS,
0251         .handle = sch->handle,
0252         .parent = sch->parent,
0253         {
0254             .stats = {
0255                 .bstats = &sch->bstats,
0256                 .qstats = &sch->qstats,
0257             },
0258         },
0259     };
0260
0261     return qdisc_offload_dump_helper(sch, TC_SETUP_QDISC_PRIO, &hw_stats);
0262 }
0263
0264 static int prio_dump(struct Qdisc *sch, struct sk_buff *skb)
0265 {
0266     struct prio_sched_data *q = qdisc_priv(sch);
0267     unsigned char *b = skb_tail_pointer(skb);
0268     struct tc_prio_qopt opt;
0269     int err;
0270
0271     opt.bands = q->bands;
0272     memcpy(&opt.priomap, q->prio2band, TC_PRIO_MAX + 1);
0273
0274     err = prio_dump_offload(sch);
0275     if (err)
0276         goto nla_put_failure;
0277
0278     if (nla_put(skb, TCA_OPTIONS, sizeof(opt), &opt))
0279         goto nla_put_failure;
0280
0281     return skb->len;
0282
0283 nla_put_failure:
0284     nlmsg_trim(skb, b);
0285     return -1;
0286 }
0287
0288 static int prio_graft(struct Qdisc *sch, unsigned long arg, struct Qdisc *new,
0289               struct Qdisc **old, struct netlink_ext_ack *extack)
0290 {
0291     struct prio_sched_data *q = qdisc_priv(sch);
0292     struct tc_prio_qopt_offload graft_offload;
0293     unsigned long band = arg - 1;
0294
0295     if (!new) {
0296         new = qdisc_create_dflt(sch->dev_queue, &pfifo_qdisc_ops,
0297                     TC_H_MAKE(sch->handle, arg), extack);
0298         if (!new)
0299             new = &noop_qdisc;
0300         else
0301             qdisc_hash_add(new, true);
0302     }
0303
0304     *old = qdisc_replace(sch, new, &q->queues[band]);
0305
0306     graft_offload.handle = sch->handle;
0307     graft_offload.parent = sch->parent;
0308     graft_offload.graft_params.band = band;
0309     graft_offload.graft_params.child_handle = new->handle;
0310     graft_offload.command = TC_PRIO_GRAFT;
0311
0312     qdisc_offload_graft_helper(qdisc_dev(sch), sch, new, *old,
0313                    TC_SETUP_QDISC_PRIO, &graft_offload,
0314                    extack);
0315     return 0;
0316 }
0317
0318 static struct Qdisc *
0319 prio_leaf(struct Qdisc *sch, unsigned long arg)
0320 {
0321     struct prio_sched_data *q = qdisc_priv(sch);
0322     unsigned long band = arg - 1;
0323
0324     return q->queues[band];
0325 }
0326
0327 static unsigned long prio_find(struct Qdisc *sch, u32 classid)
0328 {
0329     struct prio_sched_data *q = qdisc_priv(sch);
0330     unsigned long band = TC_H_MIN(classid);
0331
0332     if (band - 1 >= q->bands)
0333         return 0;
0334     return band;
0335 }
0336
0337 static unsigned long prio_bind(struct Qdisc *sch, unsigned long parent, u32 classid)
0338 {
0339     return prio_find(sch, classid);
0340 }
0341
0342
0343 static void prio_unbind(struct Qdisc *q, unsigned long cl)
0344 {
0345 }
0346
0347 static int prio_dump_class(struct Qdisc *sch, unsigned long cl, struct sk_buff *skb,
0348                struct tcmsg *tcm)
0349 {
0350     struct prio_sched_data *q = qdisc_priv(sch);
0351
0352     tcm->tcm_handle |= TC_H_MIN(cl);
0353     tcm->tcm_info = q->queues[cl-1]->handle;
0354     return 0;
0355 }
0356
0357 static int prio_dump_class_stats(struct Qdisc *sch, unsigned long cl,
0358                  struct gnet_dump *d)
0359 {
0360     struct prio_sched_data *q = qdisc_priv(sch);
0361     struct Qdisc *cl_q;
0362
0363     cl_q = q->queues[cl - 1];
0364     if (gnet_stats_copy_basic(d, cl_q->cpu_bstats,
0365                   &cl_q->bstats, true) < 0 ||
0366         qdisc_qstats_copy(d, cl_q) < 0)
0367         return -1;
0368
0369     return 0;
0370 }
0371
0372 static void prio_walk(struct Qdisc *sch, struct qdisc_walker *arg)
0373 {
0374     struct prio_sched_data *q = qdisc_priv(sch);
0375     int prio;
0376
0377     if (arg->stop)
0378         return;
0379
0380     for (prio = 0; prio < q->bands; prio++) {
0381         if (arg->count < arg->skip) {
0382             arg->count++;
0383             continue;
0384         }
0385         if (arg->fn(sch, prio + 1, arg) < 0) {
0386             arg->stop = 1;
0387             break;
0388         }
0389         arg->count++;
0390     }
0391 }
0392
0393 static struct tcf_block *prio_tcf_block(struct Qdisc *sch, unsigned long cl,
0394                     struct netlink_ext_ack *extack)
0395 {
0396     struct prio_sched_data *q = qdisc_priv(sch);
0397
0398     if (cl)
0399         return NULL;
0400     return q->block;
0401 }
0402
0403 static const struct Qdisc_class_ops prio_class_ops = {
0404     .graft      =   prio_graft,
0405     .leaf       =   prio_leaf,
0406     .find       =   prio_find,
0407     .walk       =   prio_walk,
0408     .tcf_block  =   prio_tcf_block,
0409     .bind_tcf   =   prio_bind,
0410     .unbind_tcf =   prio_unbind,
0411     .dump       =   prio_dump_class,
0412     .dump_stats =   prio_dump_class_stats,
0413 };
0414
0415 static struct Qdisc_ops prio_qdisc_ops __read_mostly = {
0416     .next       =   NULL,
0417     .cl_ops     =   &prio_class_ops,
0418     .id     =   "prio",
0419     .priv_size  =   sizeof(struct prio_sched_data),
0420     .enqueue    =   prio_enqueue,
0421     .dequeue    =   prio_dequeue,
0422     .peek       =   prio_peek,
0423     .init       =   prio_init,
0424     .reset      =   prio_reset,
0425     .destroy    =   prio_destroy,
0426     .change     =   prio_tune,
0427     .dump       =   prio_dump,
0428     .owner      =   THIS_MODULE,
0429 };
0430
0431 static int __init prio_module_init(void)
0432 {
0433     return register_qdisc(&prio_qdisc_ops);
0434 }
0435
0436 static void __exit prio_module_exit(void)
0437 {
0438     unregister_qdisc(&prio_qdisc_ops);
0439 }
0440
0441 module_init(prio_module_init)
0442 module_exit(prio_module_exit)
0443
0444 MODULE_LICENSE("GPL");