linux/net/sched/sch_dsmark.c
<<
>>
Prefs
   1// SPDX-License-Identifier: GPL-2.0-only
   2/* net/sched/sch_dsmark.c - Differentiated Services field marker */
   3
   4/* Written 1998-2000 by Werner Almesberger, EPFL ICA */
   5
   6
   7#include <linux/module.h>
   8#include <linux/init.h>
   9#include <linux/slab.h>
  10#include <linux/types.h>
  11#include <linux/string.h>
  12#include <linux/errno.h>
  13#include <linux/skbuff.h>
  14#include <linux/rtnetlink.h>
  15#include <linux/bitops.h>
  16#include <net/pkt_sched.h>
  17#include <net/pkt_cls.h>
  18#include <net/dsfield.h>
  19#include <net/inet_ecn.h>
  20#include <asm/byteorder.h>
  21
  22/*
  23 * classid      class           marking
  24 * -------      -----           -------
  25 *   n/a          0             n/a
  26 *   x:0          1             use entry [0]
  27 *   ...         ...            ...
  28 *   x:y y>0     y+1            use entry [y]
  29 *   ...         ...            ...
  30 * x:indices-1  indices         use entry [indices-1]
  31 *   ...         ...            ...
  32 *   x:y         y+1            use entry [y & (indices-1)]
  33 *   ...         ...            ...
  34 * 0xffff       0x10000         use entry [indices-1]
  35 */
  36
  37
  38#define NO_DEFAULT_INDEX        (1 << 16)
  39
  40struct mask_value {
  41        u8                      mask;
  42        u8                      value;
  43};
  44
  45struct dsmark_qdisc_data {
  46        struct Qdisc            *q;
  47        struct tcf_proto __rcu  *filter_list;
  48        struct tcf_block        *block;
  49        struct mask_value       *mv;
  50        u16                     indices;
  51        u8                      set_tc_index;
  52        u32                     default_index;  /* index range is 0...0xffff */
  53#define DSMARK_EMBEDDED_SZ      16
  54        struct mask_value       embedded[DSMARK_EMBEDDED_SZ];
  55};
  56
  57static inline int dsmark_valid_index(struct dsmark_qdisc_data *p, u16 index)
  58{
  59        return index <= p->indices && index > 0;
  60}
  61
  62/* ------------------------- Class/flow operations ------------------------- */
  63
  64static int dsmark_graft(struct Qdisc *sch, unsigned long arg,
  65                        struct Qdisc *new, struct Qdisc **old,
  66                        struct netlink_ext_ack *extack)
  67{
  68        struct dsmark_qdisc_data *p = qdisc_priv(sch);
  69
  70        pr_debug("%s(sch %p,[qdisc %p],new %p,old %p)\n",
  71                 __func__, sch, p, new, old);
  72
  73        if (new == NULL) {
  74                new = qdisc_create_dflt(sch->dev_queue, &pfifo_qdisc_ops,
  75                                        sch->handle, NULL);
  76                if (new == NULL)
  77                        new = &noop_qdisc;
  78        }
  79
  80        *old = qdisc_replace(sch, new, &p->q);
  81        return 0;
  82}
  83
  84static struct Qdisc *dsmark_leaf(struct Qdisc *sch, unsigned long arg)
  85{
  86        struct dsmark_qdisc_data *p = qdisc_priv(sch);
  87        return p->q;
  88}
  89
  90static unsigned long dsmark_find(struct Qdisc *sch, u32 classid)
  91{
  92        return TC_H_MIN(classid) + 1;
  93}
  94
  95static unsigned long dsmark_bind_filter(struct Qdisc *sch,
  96                                        unsigned long parent, u32 classid)
  97{
  98        pr_debug("%s(sch %p,[qdisc %p],classid %x)\n",
  99                 __func__, sch, qdisc_priv(sch), classid);
 100
 101        return dsmark_find(sch, classid);
 102}
 103
 104static void dsmark_unbind_filter(struct Qdisc *sch, unsigned long cl)
 105{
 106}
 107
 108static const struct nla_policy dsmark_policy[TCA_DSMARK_MAX + 1] = {
 109        [TCA_DSMARK_INDICES]            = { .type = NLA_U16 },
 110        [TCA_DSMARK_DEFAULT_INDEX]      = { .type = NLA_U16 },
 111        [TCA_DSMARK_SET_TC_INDEX]       = { .type = NLA_FLAG },
 112        [TCA_DSMARK_MASK]               = { .type = NLA_U8 },
 113        [TCA_DSMARK_VALUE]              = { .type = NLA_U8 },
 114};
 115
 116static int dsmark_change(struct Qdisc *sch, u32 classid, u32 parent,
 117                         struct nlattr **tca, unsigned long *arg,
 118                         struct netlink_ext_ack *extack)
 119{
 120        struct dsmark_qdisc_data *p = qdisc_priv(sch);
 121        struct nlattr *opt = tca[TCA_OPTIONS];
 122        struct nlattr *tb[TCA_DSMARK_MAX + 1];
 123        int err = -EINVAL;
 124
 125        pr_debug("%s(sch %p,[qdisc %p],classid %x,parent %x), arg 0x%lx\n",
 126                 __func__, sch, p, classid, parent, *arg);
 127
 128        if (!dsmark_valid_index(p, *arg)) {
 129                err = -ENOENT;
 130                goto errout;
 131        }
 132
 133        if (!opt)
 134                goto errout;
 135
 136        err = nla_parse_nested_deprecated(tb, TCA_DSMARK_MAX, opt,
 137                                          dsmark_policy, NULL);
 138        if (err < 0)
 139                goto errout;
 140
 141        if (tb[TCA_DSMARK_VALUE])
 142                p->mv[*arg - 1].value = nla_get_u8(tb[TCA_DSMARK_VALUE]);
 143
 144        if (tb[TCA_DSMARK_MASK])
 145                p->mv[*arg - 1].mask = nla_get_u8(tb[TCA_DSMARK_MASK]);
 146
 147        err = 0;
 148
 149errout:
 150        return err;
 151}
 152
 153static int dsmark_delete(struct Qdisc *sch, unsigned long arg,
 154                         struct netlink_ext_ack *extack)
 155{
 156        struct dsmark_qdisc_data *p = qdisc_priv(sch);
 157
 158        if (!dsmark_valid_index(p, arg))
 159                return -EINVAL;
 160
 161        p->mv[arg - 1].mask = 0xff;
 162        p->mv[arg - 1].value = 0;
 163
 164        return 0;
 165}
 166
 167static void dsmark_walk(struct Qdisc *sch, struct qdisc_walker *walker)
 168{
 169        struct dsmark_qdisc_data *p = qdisc_priv(sch);
 170        int i;
 171
 172        pr_debug("%s(sch %p,[qdisc %p],walker %p)\n",
 173                 __func__, sch, p, walker);
 174
 175        if (walker->stop)
 176                return;
 177
 178        for (i = 0; i < p->indices; i++) {
 179                if (p->mv[i].mask == 0xff && !p->mv[i].value)
 180                        goto ignore;
 181                if (walker->count >= walker->skip) {
 182                        if (walker->fn(sch, i + 1, walker) < 0) {
 183                                walker->stop = 1;
 184                                break;
 185                        }
 186                }
 187ignore:
 188                walker->count++;
 189        }
 190}
 191
 192static struct tcf_block *dsmark_tcf_block(struct Qdisc *sch, unsigned long cl,
 193                                          struct netlink_ext_ack *extack)
 194{
 195        struct dsmark_qdisc_data *p = qdisc_priv(sch);
 196
 197        return p->block;
 198}
 199
 200/* --------------------------- Qdisc operations ---------------------------- */
 201
 202static int dsmark_enqueue(struct sk_buff *skb, struct Qdisc *sch,
 203                          struct sk_buff **to_free)
 204{
 205        unsigned int len = qdisc_pkt_len(skb);
 206        struct dsmark_qdisc_data *p = qdisc_priv(sch);
 207        int err;
 208
 209        pr_debug("%s(skb %p,sch %p,[qdisc %p])\n", __func__, skb, sch, p);
 210
 211        if (p->set_tc_index) {
 212                int wlen = skb_network_offset(skb);
 213
 214                switch (skb_protocol(skb, true)) {
 215                case htons(ETH_P_IP):
 216                        wlen += sizeof(struct iphdr);
 217                        if (!pskb_may_pull(skb, wlen) ||
 218                            skb_try_make_writable(skb, wlen))
 219                                goto drop;
 220
 221                        skb->tc_index = ipv4_get_dsfield(ip_hdr(skb))
 222                                & ~INET_ECN_MASK;
 223                        break;
 224
 225                case htons(ETH_P_IPV6):
 226                        wlen += sizeof(struct ipv6hdr);
 227                        if (!pskb_may_pull(skb, wlen) ||
 228                            skb_try_make_writable(skb, wlen))
 229                                goto drop;
 230
 231                        skb->tc_index = ipv6_get_dsfield(ipv6_hdr(skb))
 232                                & ~INET_ECN_MASK;
 233                        break;
 234                default:
 235                        skb->tc_index = 0;
 236                        break;
 237                }
 238        }
 239
 240        if (TC_H_MAJ(skb->priority) == sch->handle)
 241                skb->tc_index = TC_H_MIN(skb->priority);
 242        else {
 243                struct tcf_result res;
 244                struct tcf_proto *fl = rcu_dereference_bh(p->filter_list);
 245                int result = tcf_classify(skb, fl, &res, false);
 246
 247                pr_debug("result %d class 0x%04x\n", result, res.classid);
 248
 249                switch (result) {
 250#ifdef CONFIG_NET_CLS_ACT
 251                case TC_ACT_QUEUED:
 252                case TC_ACT_STOLEN:
 253                case TC_ACT_TRAP:
 254                        __qdisc_drop(skb, to_free);
 255                        return NET_XMIT_SUCCESS | __NET_XMIT_STOLEN;
 256
 257                case TC_ACT_SHOT:
 258                        goto drop;
 259#endif
 260                case TC_ACT_OK:
 261                        skb->tc_index = TC_H_MIN(res.classid);
 262                        break;
 263
 264                default:
 265                        if (p->default_index != NO_DEFAULT_INDEX)
 266                                skb->tc_index = p->default_index;
 267                        break;
 268                }
 269        }
 270
 271        err = qdisc_enqueue(skb, p->q, to_free);
 272        if (err != NET_XMIT_SUCCESS) {
 273                if (net_xmit_drop_count(err))
 274                        qdisc_qstats_drop(sch);
 275                return err;
 276        }
 277
 278        sch->qstats.backlog += len;
 279        sch->q.qlen++;
 280
 281        return NET_XMIT_SUCCESS;
 282
 283drop:
 284        qdisc_drop(skb, sch, to_free);
 285        return NET_XMIT_SUCCESS | __NET_XMIT_BYPASS;
 286}
 287
 288static struct sk_buff *dsmark_dequeue(struct Qdisc *sch)
 289{
 290        struct dsmark_qdisc_data *p = qdisc_priv(sch);
 291        struct sk_buff *skb;
 292        u32 index;
 293
 294        pr_debug("%s(sch %p,[qdisc %p])\n", __func__, sch, p);
 295
 296        skb = qdisc_dequeue_peeked(p->q);
 297        if (skb == NULL)
 298                return NULL;
 299
 300        qdisc_bstats_update(sch, skb);
 301        qdisc_qstats_backlog_dec(sch, skb);
 302        sch->q.qlen--;
 303
 304        index = skb->tc_index & (p->indices - 1);
 305        pr_debug("index %d->%d\n", skb->tc_index, index);
 306
 307        switch (skb_protocol(skb, true)) {
 308        case htons(ETH_P_IP):
 309                ipv4_change_dsfield(ip_hdr(skb), p->mv[index].mask,
 310                                    p->mv[index].value);
 311                        break;
 312        case htons(ETH_P_IPV6):
 313                ipv6_change_dsfield(ipv6_hdr(skb), p->mv[index].mask,
 314                                    p->mv[index].value);
 315                        break;
 316        default:
 317                /*
 318                 * Only complain if a change was actually attempted.
 319                 * This way, we can send non-IP traffic through dsmark
 320                 * and don't need yet another qdisc as a bypass.
 321                 */
 322                if (p->mv[index].mask != 0xff || p->mv[index].value)
 323                        pr_warn("%s: unsupported protocol %d\n",
 324                                __func__, ntohs(skb_protocol(skb, true)));
 325                break;
 326        }
 327
 328        return skb;
 329}
 330
 331static struct sk_buff *dsmark_peek(struct Qdisc *sch)
 332{
 333        struct dsmark_qdisc_data *p = qdisc_priv(sch);
 334
 335        pr_debug("%s(sch %p,[qdisc %p])\n", __func__, sch, p);
 336
 337        return p->q->ops->peek(p->q);
 338}
 339
 340static int dsmark_init(struct Qdisc *sch, struct nlattr *opt,
 341                       struct netlink_ext_ack *extack)
 342{
 343        struct dsmark_qdisc_data *p = qdisc_priv(sch);
 344        struct nlattr *tb[TCA_DSMARK_MAX + 1];
 345        int err = -EINVAL;
 346        u32 default_index = NO_DEFAULT_INDEX;
 347        u16 indices;
 348        int i;
 349
 350        pr_debug("%s(sch %p,[qdisc %p],opt %p)\n", __func__, sch, p, opt);
 351
 352        if (!opt)
 353                goto errout;
 354
 355        err = tcf_block_get(&p->block, &p->filter_list, sch, extack);
 356        if (err)
 357                return err;
 358
 359        err = nla_parse_nested_deprecated(tb, TCA_DSMARK_MAX, opt,
 360                                          dsmark_policy, NULL);
 361        if (err < 0)
 362                goto errout;
 363
 364        err = -EINVAL;
 365        if (!tb[TCA_DSMARK_INDICES])
 366                goto errout;
 367        indices = nla_get_u16(tb[TCA_DSMARK_INDICES]);
 368
 369        if (hweight32(indices) != 1)
 370                goto errout;
 371
 372        if (tb[TCA_DSMARK_DEFAULT_INDEX])
 373                default_index = nla_get_u16(tb[TCA_DSMARK_DEFAULT_INDEX]);
 374
 375        if (indices <= DSMARK_EMBEDDED_SZ)
 376                p->mv = p->embedded;
 377        else
 378                p->mv = kmalloc_array(indices, sizeof(*p->mv), GFP_KERNEL);
 379        if (!p->mv) {
 380                err = -ENOMEM;
 381                goto errout;
 382        }
 383        for (i = 0; i < indices; i++) {
 384                p->mv[i].mask = 0xff;
 385                p->mv[i].value = 0;
 386        }
 387        p->indices = indices;
 388        p->default_index = default_index;
 389        p->set_tc_index = nla_get_flag(tb[TCA_DSMARK_SET_TC_INDEX]);
 390
 391        p->q = qdisc_create_dflt(sch->dev_queue, &pfifo_qdisc_ops, sch->handle,
 392                                 NULL);
 393        if (p->q == NULL)
 394                p->q = &noop_qdisc;
 395        else
 396                qdisc_hash_add(p->q, true);
 397
 398        pr_debug("%s: qdisc %p\n", __func__, p->q);
 399
 400        err = 0;
 401errout:
 402        return err;
 403}
 404
 405static void dsmark_reset(struct Qdisc *sch)
 406{
 407        struct dsmark_qdisc_data *p = qdisc_priv(sch);
 408
 409        pr_debug("%s(sch %p,[qdisc %p])\n", __func__, sch, p);
 410        if (p->q)
 411                qdisc_reset(p->q);
 412        sch->qstats.backlog = 0;
 413        sch->q.qlen = 0;
 414}
 415
 416static void dsmark_destroy(struct Qdisc *sch)
 417{
 418        struct dsmark_qdisc_data *p = qdisc_priv(sch);
 419
 420        pr_debug("%s(sch %p,[qdisc %p])\n", __func__, sch, p);
 421
 422        tcf_block_put(p->block);
 423        qdisc_put(p->q);
 424        if (p->mv != p->embedded)
 425                kfree(p->mv);
 426}
 427
 428static int dsmark_dump_class(struct Qdisc *sch, unsigned long cl,
 429                             struct sk_buff *skb, struct tcmsg *tcm)
 430{
 431        struct dsmark_qdisc_data *p = qdisc_priv(sch);
 432        struct nlattr *opts = NULL;
 433
 434        pr_debug("%s(sch %p,[qdisc %p],class %ld\n", __func__, sch, p, cl);
 435
 436        if (!dsmark_valid_index(p, cl))
 437                return -EINVAL;
 438
 439        tcm->tcm_handle = TC_H_MAKE(TC_H_MAJ(sch->handle), cl - 1);
 440        tcm->tcm_info = p->q->handle;
 441
 442        opts = nla_nest_start_noflag(skb, TCA_OPTIONS);
 443        if (opts == NULL)
 444                goto nla_put_failure;
 445        if (nla_put_u8(skb, TCA_DSMARK_MASK, p->mv[cl - 1].mask) ||
 446            nla_put_u8(skb, TCA_DSMARK_VALUE, p->mv[cl - 1].value))
 447                goto nla_put_failure;
 448
 449        return nla_nest_end(skb, opts);
 450
 451nla_put_failure:
 452        nla_nest_cancel(skb, opts);
 453        return -EMSGSIZE;
 454}
 455
 456static int dsmark_dump(struct Qdisc *sch, struct sk_buff *skb)
 457{
 458        struct dsmark_qdisc_data *p = qdisc_priv(sch);
 459        struct nlattr *opts = NULL;
 460
 461        opts = nla_nest_start_noflag(skb, TCA_OPTIONS);
 462        if (opts == NULL)
 463                goto nla_put_failure;
 464        if (nla_put_u16(skb, TCA_DSMARK_INDICES, p->indices))
 465                goto nla_put_failure;
 466
 467        if (p->default_index != NO_DEFAULT_INDEX &&
 468            nla_put_u16(skb, TCA_DSMARK_DEFAULT_INDEX, p->default_index))
 469                goto nla_put_failure;
 470
 471        if (p->set_tc_index &&
 472            nla_put_flag(skb, TCA_DSMARK_SET_TC_INDEX))
 473                goto nla_put_failure;
 474
 475        return nla_nest_end(skb, opts);
 476
 477nla_put_failure:
 478        nla_nest_cancel(skb, opts);
 479        return -EMSGSIZE;
 480}
 481
 482static const struct Qdisc_class_ops dsmark_class_ops = {
 483        .graft          =       dsmark_graft,
 484        .leaf           =       dsmark_leaf,
 485        .find           =       dsmark_find,
 486        .change         =       dsmark_change,
 487        .delete         =       dsmark_delete,
 488        .walk           =       dsmark_walk,
 489        .tcf_block      =       dsmark_tcf_block,
 490        .bind_tcf       =       dsmark_bind_filter,
 491        .unbind_tcf     =       dsmark_unbind_filter,
 492        .dump           =       dsmark_dump_class,
 493};
 494
 495static struct Qdisc_ops dsmark_qdisc_ops __read_mostly = {
 496        .next           =       NULL,
 497        .cl_ops         =       &dsmark_class_ops,
 498        .id             =       "dsmark",
 499        .priv_size      =       sizeof(struct dsmark_qdisc_data),
 500        .enqueue        =       dsmark_enqueue,
 501        .dequeue        =       dsmark_dequeue,
 502        .peek           =       dsmark_peek,
 503        .init           =       dsmark_init,
 504        .reset          =       dsmark_reset,
 505        .destroy        =       dsmark_destroy,
 506        .change         =       NULL,
 507        .dump           =       dsmark_dump,
 508        .owner          =       THIS_MODULE,
 509};
 510
 511static int __init dsmark_module_init(void)
 512{
 513        return register_qdisc(&dsmark_qdisc_ops);
 514}
 515
 516static void __exit dsmark_module_exit(void)
 517{
 518        unregister_qdisc(&dsmark_qdisc_ops);
 519}
 520
 521module_init(dsmark_module_init)
 522module_exit(dsmark_module_exit)
 523
 524MODULE_LICENSE("GPL");
 525