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