net/sched/act_vlan.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 /*
   3  * Copyright (c) 2014 Jiri Pirko <jiri@resnulli.us>
   4  */
   5
   6 #include <linux/module.h>
   7 #include <linux/init.h>
   8 #include <linux/kernel.h>
   9 #include <linux/skbuff.h>
  10 #include <linux/rtnetlink.h>
  11 #include <linux/if_vlan.h>
  12 #include <net/netlink.h>
  13 #include <net/pkt_sched.h>
  14 #include <net/pkt_cls.h>
  15
  16 #include <linux/tc_act/tc_vlan.h>
  17 #include <net/tc_act/tc_vlan.h>
  18
  19 static struct tc_action_ops act_vlan_ops;
  20
  21 static int tcf_vlan_act(struct sk_buff *skb, const struct tc_action *a,
  22                         struct tcf_result *res)
  23 {
  24         struct tcf_vlan *v = to_vlan(a);
  25         struct tcf_vlan_params *p;
  26         int action;
  27         int err;
  28         u16 tci;
  29
  30         tcf_lastuse_update(&v->tcf_tm);
  31         tcf_action_update_bstats(&v->common, skb);
  32
  33         /* Ensure 'data' points at mac_header prior calling vlan manipulating
  34          * functions.
  35          */
  36         if (skb_at_tc_ingress(skb))
  37                 skb_push_rcsum(skb, skb->mac_len);
  38
  39         action = READ_ONCE(v->tcf_action);
  40
  41         p = rcu_dereference_bh(v->vlan_p);
  42
  43         switch (p->tcfv_action) {
  44         case TCA_VLAN_ACT_POP:
  45                 err = skb_vlan_pop(skb);
  46                 if (err)
  47                         goto drop;
  48                 break;
  49         case TCA_VLAN_ACT_PUSH:
  50                 err = skb_vlan_push(skb, p->tcfv_push_proto, p->tcfv_push_vid |
  51                                     (p->tcfv_push_prio << VLAN_PRIO_SHIFT));
  52                 if (err)
  53                         goto drop;
  54                 break;
  55         case TCA_VLAN_ACT_MODIFY:
  56                 /* No-op if no vlan tag (either hw-accel or in-payload) */
  57                 if (!skb_vlan_tagged(skb))
  58                         goto out;
  59                 /* extract existing tag (and guarantee no hw-accel tag) */
  60                 if (skb_vlan_tag_present(skb)) {
  61                         tci = skb_vlan_tag_get(skb);
  62                         __vlan_hwaccel_clear_tag(skb);
  63                 } else {
  64                         /* in-payload vlan tag, pop it */
  65                         err = __skb_vlan_pop(skb, &tci);
  66                         if (err)
  67                                 goto drop;
  68                 }
  69                 /* replace the vid */
  70                 tci = (tci & ~VLAN_VID_MASK) | p->tcfv_push_vid;
  71                 /* replace prio bits, if tcfv_push_prio specified */
  72                 if (p->tcfv_push_prio_exists) {
  73                         tci &= ~VLAN_PRIO_MASK;
  74                         tci |= p->tcfv_push_prio << VLAN_PRIO_SHIFT;
  75                 }
  76                 /* put updated tci as hwaccel tag */
  77                 __vlan_hwaccel_put_tag(skb, p->tcfv_push_proto, tci);
  78                 break;
  79         case TCA_VLAN_ACT_POP_ETH:
  80                 err = skb_eth_pop(skb);
  81                 if (err)
  82                         goto drop;
  83                 break;
  84         case TCA_VLAN_ACT_PUSH_ETH:
  85                 err = skb_eth_push(skb, p->tcfv_push_dst, p->tcfv_push_src);
  86                 if (err)
  87                         goto drop;
  88                 break;
  89         default:
  90                 BUG();
  91         }
  92
  93 out:
  94         if (skb_at_tc_ingress(skb))
  95                 skb_pull_rcsum(skb, skb->mac_len);
  96
  97         return action;
  98
  99 drop:
 100         tcf_action_inc_drop_qstats(&v->common);
 101         return TC_ACT_SHOT;
 102 }
 103
 104 static const struct nla_policy vlan_policy[TCA_VLAN_MAX + 1] = {
 105         [TCA_VLAN_UNSPEC]               = { .strict_start_type = TCA_VLAN_PUSH_ETH_DST },
 106         [TCA_VLAN_PARMS]                = { .len = sizeof(struct tc_vlan) },
 107         [TCA_VLAN_PUSH_VLAN_ID]         = { .type = NLA_U16 },
 108         [TCA_VLAN_PUSH_VLAN_PROTOCOL]   = { .type = NLA_U16 },
 109         [TCA_VLAN_PUSH_VLAN_PRIORITY]   = { .type = NLA_U8 },
 110         [TCA_VLAN_PUSH_ETH_DST]         = NLA_POLICY_ETH_ADDR,
 111         [TCA_VLAN_PUSH_ETH_SRC]         = NLA_POLICY_ETH_ADDR,
 112 };
 113
 114 static int tcf_vlan_init(struct net *net, struct nlattr *nla,
 115                          struct nlattr *est, struct tc_action **a,
 116                          struct tcf_proto *tp, u32 flags,
 117                          struct netlink_ext_ack *extack)
 118 {
 119         struct tc_action_net *tn = net_generic(net, act_vlan_ops.net_id);
 120         bool bind = flags & TCA_ACT_FLAGS_BIND;
 121         struct nlattr *tb[TCA_VLAN_MAX + 1];
 122         struct tcf_chain *goto_ch = NULL;
 123         bool push_prio_exists = false;
 124         struct tcf_vlan_params *p;
 125         struct tc_vlan *parm;
 126         struct tcf_vlan *v;
 127         int action;
 128         u16 push_vid = 0;
 129         __be16 push_proto = 0;
 130         u8 push_prio = 0;
 131         bool exists = false;
 132         int ret = 0, err;
 133         u32 index;
 134
 135         if (!nla)
 136                 return -EINVAL;
 137
 138         err = nla_parse_nested_deprecated(tb, TCA_VLAN_MAX, nla, vlan_policy,
 139                                           NULL);
 140         if (err < 0)
 141                 return err;
 142
 143         if (!tb[TCA_VLAN_PARMS])
 144                 return -EINVAL;
 145         parm = nla_data(tb[TCA_VLAN_PARMS]);
 146         index = parm->index;
 147         err = tcf_idr_check_alloc(tn, &index, a, bind);
 148         if (err < 0)
 149                 return err;
 150         exists = err;
 151         if (exists && bind)
 152                 return 0;
 153
 154         switch (parm->v_action) {
 155         case TCA_VLAN_ACT_POP:
 156                 break;
 157         case TCA_VLAN_ACT_PUSH:
 158         case TCA_VLAN_ACT_MODIFY:
 159                 if (!tb[TCA_VLAN_PUSH_VLAN_ID]) {
 160                         if (exists)
 161                                 tcf_idr_release(*a, bind);
 162                         else
 163                                 tcf_idr_cleanup(tn, index);
 164                         return -EINVAL;
 165                 }
 166                 push_vid = nla_get_u16(tb[TCA_VLAN_PUSH_VLAN_ID]);
 167                 if (push_vid >= VLAN_VID_MASK) {
 168                         if (exists)
 169                                 tcf_idr_release(*a, bind);
 170                         else
 171                                 tcf_idr_cleanup(tn, index);
 172                         return -ERANGE;
 173                 }
 174
 175                 if (tb[TCA_VLAN_PUSH_VLAN_PROTOCOL]) {
 176                         push_proto = nla_get_be16(tb[TCA_VLAN_PUSH_VLAN_PROTOCOL]);
 177                         switch (push_proto) {
 178                         case htons(ETH_P_8021Q):
 179                         case htons(ETH_P_8021AD):
 180                                 break;
 181                         default:
 182                                 if (exists)
 183                                         tcf_idr_release(*a, bind);
 184                                 else
 185                                         tcf_idr_cleanup(tn, index);
 186                                 return -EPROTONOSUPPORT;
 187                         }
 188                 } else {
 189                         push_proto = htons(ETH_P_8021Q);
 190                 }
 191
 192                 push_prio_exists = !!tb[TCA_VLAN_PUSH_VLAN_PRIORITY];
 193                 if (push_prio_exists)
 194                         push_prio = nla_get_u8(tb[TCA_VLAN_PUSH_VLAN_PRIORITY]);
 195                 break;
 196         case TCA_VLAN_ACT_POP_ETH:
 197                 break;
 198         case TCA_VLAN_ACT_PUSH_ETH:
 199                 if (!tb[TCA_VLAN_PUSH_ETH_DST] || !tb[TCA_VLAN_PUSH_ETH_SRC]) {
 200                         if (exists)
 201                                 tcf_idr_release(*a, bind);
 202                         else
 203                                 tcf_idr_cleanup(tn, index);
 204                         return -EINVAL;
 205                 }
 206                 break;
 207         default:
 208                 if (exists)
 209                         tcf_idr_release(*a, bind);
 210                 else
 211                         tcf_idr_cleanup(tn, index);
 212                 return -EINVAL;
 213         }
 214         action = parm->v_action;
 215
 216         if (!exists) {
 217                 ret = tcf_idr_create_from_flags(tn, index, est, a,
 218                                                 &act_vlan_ops, bind, flags);
 219                 if (ret) {
 220                         tcf_idr_cleanup(tn, index);
 221                         return ret;
 222                 }
 223
 224                 ret = ACT_P_CREATED;
 225         } else if (!(flags & TCA_ACT_FLAGS_REPLACE)) {
 226                 tcf_idr_release(*a, bind);
 227                 return -EEXIST;
 228         }
 229
 230         err = tcf_action_check_ctrlact(parm->action, tp, &goto_ch, extack);
 231         if (err < 0)
 232                 goto release_idr;
 233
 234         v = to_vlan(*a);
 235
 236         p = kzalloc(sizeof(*p), GFP_KERNEL);
 237         if (!p) {
 238                 err = -ENOMEM;
 239                 goto put_chain;
 240         }
 241
 242         p->tcfv_action = action;
 243         p->tcfv_push_vid = push_vid;
 244         p->tcfv_push_prio = push_prio;
 245         p->tcfv_push_prio_exists = push_prio_exists || action == TCA_VLAN_ACT_PUSH;
 246         p->tcfv_push_proto = push_proto;
 247
 248         if (action == TCA_VLAN_ACT_PUSH_ETH) {
 249                 nla_memcpy(&p->tcfv_push_dst, tb[TCA_VLAN_PUSH_ETH_DST],
 250                            ETH_ALEN);
 251                 nla_memcpy(&p->tcfv_push_src, tb[TCA_VLAN_PUSH_ETH_SRC],
 252                            ETH_ALEN);
 253         }
 254
 255         spin_lock_bh(&v->tcf_lock);
 256         goto_ch = tcf_action_set_ctrlact(*a, parm->action, goto_ch);
 257         p = rcu_replace_pointer(v->vlan_p, p, lockdep_is_held(&v->tcf_lock));
 258         spin_unlock_bh(&v->tcf_lock);
 259
 260         if (goto_ch)
 261                 tcf_chain_put_by_act(goto_ch);
 262         if (p)
 263                 kfree_rcu(p, rcu);
 264
 265         return ret;
 266 put_chain:
 267         if (goto_ch)
 268                 tcf_chain_put_by_act(goto_ch);
 269 release_idr:
 270         tcf_idr_release(*a, bind);
 271         return err;
 272 }
 273
 274 static void tcf_vlan_cleanup(struct tc_action *a)
 275 {
 276         struct tcf_vlan *v = to_vlan(a);
 277         struct tcf_vlan_params *p;
 278
 279         p = rcu_dereference_protected(v->vlan_p, 1);
 280         if (p)
 281                 kfree_rcu(p, rcu);
 282 }
 283
 284 static int tcf_vlan_dump(struct sk_buff *skb, struct tc_action *a,
 285                          int bind, int ref)
 286 {
 287         unsigned char *b = skb_tail_pointer(skb);
 288         struct tcf_vlan *v = to_vlan(a);
 289         struct tcf_vlan_params *p;
 290         struct tc_vlan opt = {
 291                 .index    = v->tcf_index,
 292                 .refcnt   = refcount_read(&v->tcf_refcnt) - ref,
 293                 .bindcnt  = atomic_read(&v->tcf_bindcnt) - bind,
 294         };
 295         struct tcf_t t;
 296
 297         spin_lock_bh(&v->tcf_lock);
 298         opt.action = v->tcf_action;
 299         p = rcu_dereference_protected(v->vlan_p, lockdep_is_held(&v->tcf_lock));
 300         opt.v_action = p->tcfv_action;
 301         if (nla_put(skb, TCA_VLAN_PARMS, sizeof(opt), &opt))
 302                 goto nla_put_failure;
 303
 304         if ((p->tcfv_action == TCA_VLAN_ACT_PUSH ||
 305              p->tcfv_action == TCA_VLAN_ACT_MODIFY) &&
 306             (nla_put_u16(skb, TCA_VLAN_PUSH_VLAN_ID, p->tcfv_push_vid) ||
 307              nla_put_be16(skb, TCA_VLAN_PUSH_VLAN_PROTOCOL,
 308                           p->tcfv_push_proto) ||
 309              (p->tcfv_push_prio_exists &&
 310               nla_put_u8(skb, TCA_VLAN_PUSH_VLAN_PRIORITY, p->tcfv_push_prio))))
 311                 goto nla_put_failure;
 312
 313         if (p->tcfv_action == TCA_VLAN_ACT_PUSH_ETH) {
 314                 if (nla_put(skb, TCA_VLAN_PUSH_ETH_DST, ETH_ALEN,
 315                             p->tcfv_push_dst))
 316                         goto nla_put_failure;
 317                 if (nla_put(skb, TCA_VLAN_PUSH_ETH_SRC, ETH_ALEN,
 318                             p->tcfv_push_src))
 319                         goto nla_put_failure;
 320         }
 321
 322         tcf_tm_dump(&t, &v->tcf_tm);
 323         if (nla_put_64bit(skb, TCA_VLAN_TM, sizeof(t), &t, TCA_VLAN_PAD))
 324                 goto nla_put_failure;
 325         spin_unlock_bh(&v->tcf_lock);
 326
 327         return skb->len;
 328
 329 nla_put_failure:
 330         spin_unlock_bh(&v->tcf_lock);
 331         nlmsg_trim(skb, b);
 332         return -1;
 333 }
 334
 335 static void tcf_vlan_stats_update(struct tc_action *a, u64 bytes, u64 packets,
 336                                   u64 drops, u64 lastuse, bool hw)
 337 {
 338         struct tcf_vlan *v = to_vlan(a);
 339         struct tcf_t *tm = &v->tcf_tm;
 340
 341         tcf_action_update_stats(a, bytes, packets, drops, hw);
 342         tm->lastuse = max_t(u64, tm->lastuse, lastuse);
 343 }
 344
 345 static size_t tcf_vlan_get_fill_size(const struct tc_action *act)
 346 {
 347         return nla_total_size(sizeof(struct tc_vlan))
 348                 + nla_total_size(sizeof(u16)) /* TCA_VLAN_PUSH_VLAN_ID */
 349                 + nla_total_size(sizeof(u16)) /* TCA_VLAN_PUSH_VLAN_PROTOCOL */
 350                 + nla_total_size(sizeof(u8)); /* TCA_VLAN_PUSH_VLAN_PRIORITY */
 351 }
 352
 353 static int tcf_vlan_offload_act_setup(struct tc_action *act, void *entry_data,
 354                                       u32 *index_inc, bool bind,
 355                                       struct netlink_ext_ack *extack)
 356 {
 357         if (bind) {
 358                 struct flow_action_entry *entry = entry_data;
 359
 360                 switch (tcf_vlan_action(act)) {
 361                 case TCA_VLAN_ACT_PUSH:
 362                         entry->id = FLOW_ACTION_VLAN_PUSH;
 363                         entry->vlan.vid = tcf_vlan_push_vid(act);
 364                         entry->vlan.proto = tcf_vlan_push_proto(act);
 365                         entry->vlan.prio = tcf_vlan_push_prio(act);
 366                         break;
 367                 case TCA_VLAN_ACT_POP:
 368                         entry->id = FLOW_ACTION_VLAN_POP;
 369                         break;
 370                 case TCA_VLAN_ACT_MODIFY:
 371                         entry->id = FLOW_ACTION_VLAN_MANGLE;
 372                         entry->vlan.vid = tcf_vlan_push_vid(act);
 373                         entry->vlan.proto = tcf_vlan_push_proto(act);
 374                         entry->vlan.prio = tcf_vlan_push_prio(act);
 375                         break;
 376                 case TCA_VLAN_ACT_POP_ETH:
 377                         entry->id = FLOW_ACTION_VLAN_POP_ETH;
 378                         break;
 379                 case TCA_VLAN_ACT_PUSH_ETH:
 380                         entry->id = FLOW_ACTION_VLAN_PUSH_ETH;
 381                         tcf_vlan_push_eth(entry->vlan_push_eth.src, entry->vlan_push_eth.dst, act);
 382                         break;
 383                 default:
 384                         NL_SET_ERR_MSG_MOD(extack, "Unsupported vlan action mode offload");
 385                         return -EOPNOTSUPP;
 386                 }
 387                 *index_inc = 1;
 388         } else {
 389                 struct flow_offload_action *fl_action = entry_data;
 390
 391                 switch (tcf_vlan_action(act)) {
 392                 case TCA_VLAN_ACT_PUSH:
 393                         fl_action->id = FLOW_ACTION_VLAN_PUSH;
 394                         break;
 395                 case TCA_VLAN_ACT_POP:
 396                         fl_action->id = FLOW_ACTION_VLAN_POP;
 397                         break;
 398                 case TCA_VLAN_ACT_MODIFY:
 399                         fl_action->id = FLOW_ACTION_VLAN_MANGLE;
 400                         break;
 401                 case TCA_VLAN_ACT_POP_ETH:
 402                         fl_action->id = FLOW_ACTION_VLAN_POP_ETH;
 403                         break;
 404                 case TCA_VLAN_ACT_PUSH_ETH:
 405                         fl_action->id = FLOW_ACTION_VLAN_PUSH_ETH;
 406                         break;
 407                 default:
 408                         return -EOPNOTSUPP;
 409                 }
 410         }
 411
 412         return 0;
 413 }
 414
 415 static struct tc_action_ops act_vlan_ops = {
 416         .kind           =       "vlan",
 417         .id             =       TCA_ID_VLAN,
 418         .owner          =       THIS_MODULE,
 419         .act            =       tcf_vlan_act,
 420         .dump           =       tcf_vlan_dump,
 421         .init           =       tcf_vlan_init,
 422         .cleanup        =       tcf_vlan_cleanup,
 423         .stats_update   =       tcf_vlan_stats_update,
 424         .get_fill_size  =       tcf_vlan_get_fill_size,
 425         .offload_act_setup =    tcf_vlan_offload_act_setup,
 426         .size           =       sizeof(struct tcf_vlan),
 427 };
 428
 429 static __net_init int vlan_init_net(struct net *net)
 430 {
 431         struct tc_action_net *tn = net_generic(net, act_vlan_ops.net_id);
 432
 433         return tc_action_net_init(net, tn, &act_vlan_ops);
 434 }
 435
 436 static void __net_exit vlan_exit_net(struct list_head *net_list)
 437 {
 438         tc_action_net_exit(net_list, act_vlan_ops.net_id);
 439 }
 440
 441 static struct pernet_operations vlan_net_ops = {
 442         .init = vlan_init_net,
 443         .exit_batch = vlan_exit_net,
 444         .id   = &act_vlan_ops.net_id,
 445         .size = sizeof(struct tc_action_net),
 446 };
 447
 448 static int __init vlan_init_module(void)
 449 {
 450         return tcf_register_action(&act_vlan_ops, &vlan_net_ops);
 451 }
 452
 453 static void __exit vlan_cleanup_module(void)
 454 {
 455         tcf_unregister_action(&act_vlan_ops, &vlan_net_ops);
 456 }
 457
 458 module_init(vlan_init_module);
 459 module_exit(vlan_cleanup_module);
 460
 461 MODULE_AUTHOR("Jiri Pirko <jiri@resnulli.us>");
 462 MODULE_DESCRIPTION("vlan manipulation actions");
 463 MODULE_LICENSE("GPL v2");