net/sched/sch_dsmark.c

0001 // SPDX-License-Identifier: GPL-2.0-only
0002 /* net/sched/sch_dsmark.c - Differentiated Services field marker */
0003
0004 /* Written 1998-2000 by Werner Almesberger, EPFL ICA */
0005
0006
0007 #include <linux/module.h>
0008 #include <linux/init.h>
0009 #include <linux/slab.h>
0010 #include <linux/types.h>
0011 #include <linux/string.h>
0012 #include <linux/errno.h>
0013 #include <linux/skbuff.h>
0014 #include <linux/rtnetlink.h>
0015 #include <linux/bitops.h>
0016 #include <net/pkt_sched.h>
0017 #include <net/pkt_cls.h>
0018 #include <net/dsfield.h>
0019 #include <net/inet_ecn.h>
0020 #include <asm/byteorder.h>
0021
0022 /*
0023  * classid  class       marking
0024  * -------  -----       -------
0025  *   n/a      0     n/a
0026  *   x:0      1     use entry [0]
0027  *   ...     ...        ...
0028  *   x:y y>0     y+1        use entry [y]
0029  *   ...     ...        ...
0030  * x:indices-1  indices     use entry [indices-1]
0031  *   ...     ...        ...
0032  *   x:y     y+1        use entry [y & (indices-1)]
0033  *   ...     ...        ...
0034  * 0xffff   0x10000     use entry [indices-1]
0035  */
0036
0037
0038 #define NO_DEFAULT_INDEX    (1 << 16)
0039
0040 struct mask_value {
0041     u8          mask;
0042     u8          value;
0043 };
0044
0045 struct dsmark_qdisc_data {
0046     struct Qdisc        *q;
0047     struct tcf_proto __rcu  *filter_list;
0048     struct tcf_block    *block;
0049     struct mask_value   *mv;
0050     u16         indices;
0051     u8          set_tc_index;
0052     u32         default_index;  /* index range is 0...0xffff */
0053 #define DSMARK_EMBEDDED_SZ  16
0054     struct mask_value   embedded[DSMARK_EMBEDDED_SZ];
0055 };
0056
0057 static inline int dsmark_valid_index(struct dsmark_qdisc_data *p, u16 index)
0058 {
0059     return index <= p->indices && index > 0;
0060 }
0061
0062 /* ------------------------- Class/flow operations ------------------------- */
0063
0064 static int dsmark_graft(struct Qdisc *sch, unsigned long arg,
0065             struct Qdisc *new, struct Qdisc **old,
0066             struct netlink_ext_ack *extack)
0067 {
0068     struct dsmark_qdisc_data *p = qdisc_priv(sch);
0069
0070     pr_debug("%s(sch %p,[qdisc %p],new %p,old %p)\n",
0071          __func__, sch, p, new, old);
0072
0073     if (new == NULL) {
0074         new = qdisc_create_dflt(sch->dev_queue, &pfifo_qdisc_ops,
0075                     sch->handle, NULL);
0076         if (new == NULL)
0077             new = &noop_qdisc;
0078     }
0079
0080     *old = qdisc_replace(sch, new, &p->q);
0081     return 0;
0082 }
0083
0084 static struct Qdisc *dsmark_leaf(struct Qdisc *sch, unsigned long arg)
0085 {
0086     struct dsmark_qdisc_data *p = qdisc_priv(sch);
0087     return p->q;
0088 }
0089
0090 static unsigned long dsmark_find(struct Qdisc *sch, u32 classid)
0091 {
0092     return TC_H_MIN(classid) + 1;
0093 }
0094
0095 static unsigned long dsmark_bind_filter(struct Qdisc *sch,
0096                     unsigned long parent, u32 classid)
0097 {
0098     pr_debug("%s(sch %p,[qdisc %p],classid %x)\n",
0099          __func__, sch, qdisc_priv(sch), classid);
0100
0101     return dsmark_find(sch, classid);
0102 }
0103
0104 static void dsmark_unbind_filter(struct Qdisc *sch, unsigned long cl)
0105 {
0106 }
0107
0108 static const struct nla_policy dsmark_policy[TCA_DSMARK_MAX + 1] = {
0109     [TCA_DSMARK_INDICES]        = { .type = NLA_U16 },
0110     [TCA_DSMARK_DEFAULT_INDEX]  = { .type = NLA_U16 },
0111     [TCA_DSMARK_SET_TC_INDEX]   = { .type = NLA_FLAG },
0112     [TCA_DSMARK_MASK]       = { .type = NLA_U8 },
0113     [TCA_DSMARK_VALUE]      = { .type = NLA_U8 },
0114 };
0115
0116 static int dsmark_change(struct Qdisc *sch, u32 classid, u32 parent,
0117              struct nlattr **tca, unsigned long *arg,
0118              struct netlink_ext_ack *extack)
0119 {
0120     struct dsmark_qdisc_data *p = qdisc_priv(sch);
0121     struct nlattr *opt = tca[TCA_OPTIONS];
0122     struct nlattr *tb[TCA_DSMARK_MAX + 1];
0123     int err = -EINVAL;
0124
0125     pr_debug("%s(sch %p,[qdisc %p],classid %x,parent %x), arg 0x%lx\n",
0126          __func__, sch, p, classid, parent, *arg);
0127
0128     if (!dsmark_valid_index(p, *arg)) {
0129         err = -ENOENT;
0130         goto errout;
0131     }
0132
0133     if (!opt)
0134         goto errout;
0135
0136     err = nla_parse_nested_deprecated(tb, TCA_DSMARK_MAX, opt,
0137                       dsmark_policy, NULL);
0138     if (err < 0)
0139         goto errout;
0140
0141     if (tb[TCA_DSMARK_VALUE])
0142         p->mv[*arg - 1].value = nla_get_u8(tb[TCA_DSMARK_VALUE]);
0143
0144     if (tb[TCA_DSMARK_MASK])
0145         p->mv[*arg - 1].mask = nla_get_u8(tb[TCA_DSMARK_MASK]);
0146
0147     err = 0;
0148
0149 errout:
0150     return err;
0151 }
0152
0153 static int dsmark_delete(struct Qdisc *sch, unsigned long arg,
0154              struct netlink_ext_ack *extack)
0155 {
0156     struct dsmark_qdisc_data *p = qdisc_priv(sch);
0157
0158     if (!dsmark_valid_index(p, arg))
0159         return -EINVAL;
0160
0161     p->mv[arg - 1].mask = 0xff;
0162     p->mv[arg - 1].value = 0;
0163
0164     return 0;
0165 }
0166
0167 static void dsmark_walk(struct Qdisc *sch, struct qdisc_walker *walker)
0168 {
0169     struct dsmark_qdisc_data *p = qdisc_priv(sch);
0170     int i;
0171
0172     pr_debug("%s(sch %p,[qdisc %p],walker %p)\n",
0173          __func__, sch, p, walker);
0174
0175     if (walker->stop)
0176         return;
0177
0178     for (i = 0; i < p->indices; i++) {
0179         if (p->mv[i].mask == 0xff && !p->mv[i].value)
0180             goto ignore;
0181         if (walker->count >= walker->skip) {
0182             if (walker->fn(sch, i + 1, walker) < 0) {
0183                 walker->stop = 1;
0184                 break;
0185             }
0186         }
0187 ignore:
0188         walker->count++;
0189     }
0190 }
0191
0192 static struct tcf_block *dsmark_tcf_block(struct Qdisc *sch, unsigned long cl,
0193                       struct netlink_ext_ack *extack)
0194 {
0195     struct dsmark_qdisc_data *p = qdisc_priv(sch);
0196
0197     return p->block;
0198 }
0199
0200 /* --------------------------- Qdisc operations ---------------------------- */
0201
0202 static int dsmark_enqueue(struct sk_buff *skb, struct Qdisc *sch,
0203               struct sk_buff **to_free)
0204 {
0205     unsigned int len = qdisc_pkt_len(skb);
0206     struct dsmark_qdisc_data *p = qdisc_priv(sch);
0207     int err;
0208
0209     pr_debug("%s(skb %p,sch %p,[qdisc %p])\n", __func__, skb, sch, p);
0210
0211     if (p->set_tc_index) {
0212         int wlen = skb_network_offset(skb);
0213
0214         switch (skb_protocol(skb, true)) {
0215         case htons(ETH_P_IP):
0216             wlen += sizeof(struct iphdr);
0217             if (!pskb_may_pull(skb, wlen) ||
0218                 skb_try_make_writable(skb, wlen))
0219                 goto drop;
0220
0221             skb->tc_index = ipv4_get_dsfield(ip_hdr(skb))
0222                 & ~INET_ECN_MASK;
0223             break;
0224
0225         case htons(ETH_P_IPV6):
0226             wlen += sizeof(struct ipv6hdr);
0227             if (!pskb_may_pull(skb, wlen) ||
0228                 skb_try_make_writable(skb, wlen))
0229                 goto drop;
0230
0231             skb->tc_index = ipv6_get_dsfield(ipv6_hdr(skb))
0232                 & ~INET_ECN_MASK;
0233             break;
0234         default:
0235             skb->tc_index = 0;
0236             break;
0237         }
0238     }
0239
0240     if (TC_H_MAJ(skb->priority) == sch->handle)
0241         skb->tc_index = TC_H_MIN(skb->priority);
0242     else {
0243         struct tcf_result res;
0244         struct tcf_proto *fl = rcu_dereference_bh(p->filter_list);
0245         int result = tcf_classify(skb, NULL, fl, &res, false);
0246
0247         pr_debug("result %d class 0x%04x\n", result, res.classid);
0248
0249         switch (result) {
0250 #ifdef CONFIG_NET_CLS_ACT
0251         case TC_ACT_QUEUED:
0252         case TC_ACT_STOLEN:
0253         case TC_ACT_TRAP:
0254             __qdisc_drop(skb, to_free);
0255             return NET_XMIT_SUCCESS | __NET_XMIT_STOLEN;
0256
0257         case TC_ACT_SHOT:
0258             goto drop;
0259 #endif
0260         case TC_ACT_OK:
0261             skb->tc_index = TC_H_MIN(res.classid);
0262             break;
0263
0264         default:
0265             if (p->default_index != NO_DEFAULT_INDEX)
0266                 skb->tc_index = p->default_index;
0267             break;
0268         }
0269     }
0270
0271     err = qdisc_enqueue(skb, p->q, to_free);
0272     if (err != NET_XMIT_SUCCESS) {
0273         if (net_xmit_drop_count(err))
0274             qdisc_qstats_drop(sch);
0275         return err;
0276     }
0277
0278     sch->qstats.backlog += len;
0279     sch->q.qlen++;
0280
0281     return NET_XMIT_SUCCESS;
0282
0283 drop:
0284     qdisc_drop(skb, sch, to_free);
0285     return NET_XMIT_SUCCESS | __NET_XMIT_BYPASS;
0286 }
0287
0288 static struct sk_buff *dsmark_dequeue(struct Qdisc *sch)
0289 {
0290     struct dsmark_qdisc_data *p = qdisc_priv(sch);
0291     struct sk_buff *skb;
0292     u32 index;
0293
0294     pr_debug("%s(sch %p,[qdisc %p])\n", __func__, sch, p);
0295
0296     skb = qdisc_dequeue_peeked(p->q);
0297     if (skb == NULL)
0298         return NULL;
0299
0300     qdisc_bstats_update(sch, skb);
0301     qdisc_qstats_backlog_dec(sch, skb);
0302     sch->q.qlen--;
0303
0304     index = skb->tc_index & (p->indices - 1);
0305     pr_debug("index %d->%d\n", skb->tc_index, index);
0306
0307     switch (skb_protocol(skb, true)) {
0308     case htons(ETH_P_IP):
0309         ipv4_change_dsfield(ip_hdr(skb), p->mv[index].mask,
0310                     p->mv[index].value);
0311             break;
0312     case htons(ETH_P_IPV6):
0313         ipv6_change_dsfield(ipv6_hdr(skb), p->mv[index].mask,
0314                     p->mv[index].value);
0315             break;
0316     default:
0317         /*
0318          * Only complain if a change was actually attempted.
0319          * This way, we can send non-IP traffic through dsmark
0320          * and don't need yet another qdisc as a bypass.
0321          */
0322         if (p->mv[index].mask != 0xff || p->mv[index].value)
0323             pr_warn("%s: unsupported protocol %d\n",
0324                 __func__, ntohs(skb_protocol(skb, true)));
0325         break;
0326     }
0327
0328     return skb;
0329 }
0330
0331 static struct sk_buff *dsmark_peek(struct Qdisc *sch)
0332 {
0333     struct dsmark_qdisc_data *p = qdisc_priv(sch);
0334
0335     pr_debug("%s(sch %p,[qdisc %p])\n", __func__, sch, p);
0336
0337     return p->q->ops->peek(p->q);
0338 }
0339
0340 static int dsmark_init(struct Qdisc *sch, struct nlattr *opt,
0341                struct netlink_ext_ack *extack)
0342 {
0343     struct dsmark_qdisc_data *p = qdisc_priv(sch);
0344     struct nlattr *tb[TCA_DSMARK_MAX + 1];
0345     int err = -EINVAL;
0346     u32 default_index = NO_DEFAULT_INDEX;
0347     u16 indices;
0348     int i;
0349
0350     pr_debug("%s(sch %p,[qdisc %p],opt %p)\n", __func__, sch, p, opt);
0351
0352     if (!opt)
0353         goto errout;
0354
0355     err = tcf_block_get(&p->block, &p->filter_list, sch, extack);
0356     if (err)
0357         return err;
0358
0359     err = nla_parse_nested_deprecated(tb, TCA_DSMARK_MAX, opt,
0360                       dsmark_policy, NULL);
0361     if (err < 0)
0362         goto errout;
0363
0364     err = -EINVAL;
0365     if (!tb[TCA_DSMARK_INDICES])
0366         goto errout;
0367     indices = nla_get_u16(tb[TCA_DSMARK_INDICES]);
0368
0369     if (hweight32(indices) != 1)
0370         goto errout;
0371
0372     if (tb[TCA_DSMARK_DEFAULT_INDEX])
0373         default_index = nla_get_u16(tb[TCA_DSMARK_DEFAULT_INDEX]);
0374
0375     if (indices <= DSMARK_EMBEDDED_SZ)
0376         p->mv = p->embedded;
0377     else
0378         p->mv = kmalloc_array(indices, sizeof(*p->mv), GFP_KERNEL);
0379     if (!p->mv) {
0380         err = -ENOMEM;
0381         goto errout;
0382     }
0383     for (i = 0; i < indices; i++) {
0384         p->mv[i].mask = 0xff;
0385         p->mv[i].value = 0;
0386     }
0387     p->indices = indices;
0388     p->default_index = default_index;
0389     p->set_tc_index = nla_get_flag(tb[TCA_DSMARK_SET_TC_INDEX]);
0390
0391     p->q = qdisc_create_dflt(sch->dev_queue, &pfifo_qdisc_ops, sch->handle,
0392                  NULL);
0393     if (p->q == NULL)
0394         p->q = &noop_qdisc;
0395     else
0396         qdisc_hash_add(p->q, true);
0397
0398     pr_debug("%s: qdisc %p\n", __func__, p->q);
0399
0400     err = 0;
0401 errout:
0402     return err;
0403 }
0404
0405 static void dsmark_reset(struct Qdisc *sch)
0406 {
0407     struct dsmark_qdisc_data *p = qdisc_priv(sch);
0408
0409     pr_debug("%s(sch %p,[qdisc %p])\n", __func__, sch, p);
0410     if (p->q)
0411         qdisc_reset(p->q);
0412     sch->qstats.backlog = 0;
0413     sch->q.qlen = 0;
0414 }
0415
0416 static void dsmark_destroy(struct Qdisc *sch)
0417 {
0418     struct dsmark_qdisc_data *p = qdisc_priv(sch);
0419
0420     pr_debug("%s(sch %p,[qdisc %p])\n", __func__, sch, p);
0421
0422     tcf_block_put(p->block);
0423     qdisc_put(p->q);
0424     if (p->mv != p->embedded)
0425         kfree(p->mv);
0426 }
0427
0428 static int dsmark_dump_class(struct Qdisc *sch, unsigned long cl,
0429                  struct sk_buff *skb, struct tcmsg *tcm)
0430 {
0431     struct dsmark_qdisc_data *p = qdisc_priv(sch);
0432     struct nlattr *opts = NULL;
0433
0434     pr_debug("%s(sch %p,[qdisc %p],class %ld\n", __func__, sch, p, cl);
0435
0436     if (!dsmark_valid_index(p, cl))
0437         return -EINVAL;
0438
0439     tcm->tcm_handle = TC_H_MAKE(TC_H_MAJ(sch->handle), cl - 1);
0440     tcm->tcm_info = p->q->handle;
0441
0442     opts = nla_nest_start_noflag(skb, TCA_OPTIONS);
0443     if (opts == NULL)
0444         goto nla_put_failure;
0445     if (nla_put_u8(skb, TCA_DSMARK_MASK, p->mv[cl - 1].mask) ||
0446         nla_put_u8(skb, TCA_DSMARK_VALUE, p->mv[cl - 1].value))
0447         goto nla_put_failure;
0448
0449     return nla_nest_end(skb, opts);
0450
0451 nla_put_failure:
0452     nla_nest_cancel(skb, opts);
0453     return -EMSGSIZE;
0454 }
0455
0456 static int dsmark_dump(struct Qdisc *sch, struct sk_buff *skb)
0457 {
0458     struct dsmark_qdisc_data *p = qdisc_priv(sch);
0459     struct nlattr *opts = NULL;
0460
0461     opts = nla_nest_start_noflag(skb, TCA_OPTIONS);
0462     if (opts == NULL)
0463         goto nla_put_failure;
0464     if (nla_put_u16(skb, TCA_DSMARK_INDICES, p->indices))
0465         goto nla_put_failure;
0466
0467     if (p->default_index != NO_DEFAULT_INDEX &&
0468         nla_put_u16(skb, TCA_DSMARK_DEFAULT_INDEX, p->default_index))
0469         goto nla_put_failure;
0470
0471     if (p->set_tc_index &&
0472         nla_put_flag(skb, TCA_DSMARK_SET_TC_INDEX))
0473         goto nla_put_failure;
0474
0475     return nla_nest_end(skb, opts);
0476
0477 nla_put_failure:
0478     nla_nest_cancel(skb, opts);
0479     return -EMSGSIZE;
0480 }
0481
0482 static const struct Qdisc_class_ops dsmark_class_ops = {
0483     .graft      =   dsmark_graft,
0484     .leaf       =   dsmark_leaf,
0485     .find       =   dsmark_find,
0486     .change     =   dsmark_change,
0487     .delete     =   dsmark_delete,
0488     .walk       =   dsmark_walk,
0489     .tcf_block  =   dsmark_tcf_block,
0490     .bind_tcf   =   dsmark_bind_filter,
0491     .unbind_tcf =   dsmark_unbind_filter,
0492     .dump       =   dsmark_dump_class,
0493 };
0494
0495 static struct Qdisc_ops dsmark_qdisc_ops __read_mostly = {
0496     .next       =   NULL,
0497     .cl_ops     =   &dsmark_class_ops,
0498     .id     =   "dsmark",
0499     .priv_size  =   sizeof(struct dsmark_qdisc_data),
0500     .enqueue    =   dsmark_enqueue,
0501     .dequeue    =   dsmark_dequeue,
0502     .peek       =   dsmark_peek,
0503     .init       =   dsmark_init,
0504     .reset      =   dsmark_reset,
0505     .destroy    =   dsmark_destroy,
0506     .change     =   NULL,
0507     .dump       =   dsmark_dump,
0508     .owner      =   THIS_MODULE,
0509 };
0510
0511 static int __init dsmark_module_init(void)
0512 {
0513     return register_qdisc(&dsmark_qdisc_ops);
0514 }
0515
0516 static void __exit dsmark_module_exit(void)
0517 {
0518     unregister_qdisc(&dsmark_qdisc_ops);
0519 }
0520
0521 module_init(dsmark_module_init)
0522 module_exit(dsmark_module_exit)
0523
0524 MODULE_LICENSE("GPL");