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