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"); 464