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(tm: &v->tcf_tm); |
33 | tcf_action_update_bstats(a: &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, len: 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, vlan_proto: p->tcfv_push_proto, vlan_tci: 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, vlan_tci: &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, vlan_proto: p->tcfv_push_proto, vlan_tci: 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, dst: p->tcfv_push_dst, src: 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, len: skb->mac_len); |
98 | |
99 | return action; |
100 | |
101 | drop: |
102 | tcf_action_inc_drop_qstats(a: &v->common); |
103 | return TC_ACT_SHOT; |
104 | } |
105 | |
106 | static const struct nla_policy vlan_policy[TCA_VLAN_MAX + 1] = { |
107 | [TCA_VLAN_UNSPEC] = { .strict_start_type = TCA_VLAN_PUSH_ETH_DST }, |
108 | [TCA_VLAN_PARMS] = { .len = sizeof(struct tc_vlan) }, |
109 | [TCA_VLAN_PUSH_VLAN_ID] = { .type = NLA_U16 }, |
110 | [TCA_VLAN_PUSH_VLAN_PROTOCOL] = { .type = NLA_U16 }, |
111 | [TCA_VLAN_PUSH_VLAN_PRIORITY] = { .type = NLA_U8 }, |
112 | [TCA_VLAN_PUSH_ETH_DST] = NLA_POLICY_ETH_ADDR, |
113 | [TCA_VLAN_PUSH_ETH_SRC] = NLA_POLICY_ETH_ADDR, |
114 | }; |
115 | |
116 | static int tcf_vlan_init(struct net *net, struct nlattr *nla, |
117 | struct nlattr *est, struct tc_action **a, |
118 | struct tcf_proto *tp, u32 flags, |
119 | struct netlink_ext_ack *extack) |
120 | { |
121 | struct tc_action_net *tn = net_generic(net, id: act_vlan_ops.net_id); |
122 | bool bind = flags & TCA_ACT_FLAGS_BIND; |
123 | struct nlattr *tb[TCA_VLAN_MAX + 1]; |
124 | struct tcf_chain *goto_ch = NULL; |
125 | bool push_prio_exists = false; |
126 | struct tcf_vlan_params *p; |
127 | struct tc_vlan *parm; |
128 | struct tcf_vlan *v; |
129 | int action; |
130 | u16 push_vid = 0; |
131 | __be16 push_proto = 0; |
132 | u8 push_prio = 0; |
133 | bool exists = false; |
134 | int ret = 0, err; |
135 | u32 index; |
136 | |
137 | if (!nla) |
138 | return -EINVAL; |
139 | |
140 | err = nla_parse_nested_deprecated(tb, TCA_VLAN_MAX, nla, policy: vlan_policy, |
141 | NULL); |
142 | if (err < 0) |
143 | return err; |
144 | |
145 | if (!tb[TCA_VLAN_PARMS]) |
146 | return -EINVAL; |
147 | parm = nla_data(nla: tb[TCA_VLAN_PARMS]); |
148 | index = parm->index; |
149 | err = tcf_idr_check_alloc(tn, index: &index, a, bind); |
150 | if (err < 0) |
151 | return err; |
152 | exists = err; |
153 | if (exists && bind) |
154 | return 0; |
155 | |
156 | switch (parm->v_action) { |
157 | case TCA_VLAN_ACT_POP: |
158 | break; |
159 | case TCA_VLAN_ACT_PUSH: |
160 | case TCA_VLAN_ACT_MODIFY: |
161 | if (!tb[TCA_VLAN_PUSH_VLAN_ID]) { |
162 | if (exists) |
163 | tcf_idr_release(a: *a, bind); |
164 | else |
165 | tcf_idr_cleanup(tn, index); |
166 | return -EINVAL; |
167 | } |
168 | push_vid = nla_get_u16(nla: tb[TCA_VLAN_PUSH_VLAN_ID]); |
169 | if (push_vid >= VLAN_VID_MASK) { |
170 | if (exists) |
171 | tcf_idr_release(a: *a, bind); |
172 | else |
173 | tcf_idr_cleanup(tn, index); |
174 | return -ERANGE; |
175 | } |
176 | |
177 | if (tb[TCA_VLAN_PUSH_VLAN_PROTOCOL]) { |
178 | push_proto = nla_get_be16(nla: tb[TCA_VLAN_PUSH_VLAN_PROTOCOL]); |
179 | switch (push_proto) { |
180 | case htons(ETH_P_8021Q): |
181 | case htons(ETH_P_8021AD): |
182 | break; |
183 | default: |
184 | if (exists) |
185 | tcf_idr_release(a: *a, bind); |
186 | else |
187 | tcf_idr_cleanup(tn, index); |
188 | return -EPROTONOSUPPORT; |
189 | } |
190 | } else { |
191 | push_proto = htons(ETH_P_8021Q); |
192 | } |
193 | |
194 | push_prio_exists = !!tb[TCA_VLAN_PUSH_VLAN_PRIORITY]; |
195 | if (push_prio_exists) |
196 | push_prio = nla_get_u8(nla: tb[TCA_VLAN_PUSH_VLAN_PRIORITY]); |
197 | break; |
198 | case TCA_VLAN_ACT_POP_ETH: |
199 | break; |
200 | case TCA_VLAN_ACT_PUSH_ETH: |
201 | if (!tb[TCA_VLAN_PUSH_ETH_DST] || !tb[TCA_VLAN_PUSH_ETH_SRC]) { |
202 | if (exists) |
203 | tcf_idr_release(a: *a, bind); |
204 | else |
205 | tcf_idr_cleanup(tn, index); |
206 | return -EINVAL; |
207 | } |
208 | break; |
209 | default: |
210 | if (exists) |
211 | tcf_idr_release(a: *a, bind); |
212 | else |
213 | tcf_idr_cleanup(tn, index); |
214 | return -EINVAL; |
215 | } |
216 | action = parm->v_action; |
217 | |
218 | if (!exists) { |
219 | ret = tcf_idr_create_from_flags(tn, index, est, a, |
220 | ops: &act_vlan_ops, bind, flags); |
221 | if (ret) { |
222 | tcf_idr_cleanup(tn, index); |
223 | return ret; |
224 | } |
225 | |
226 | ret = ACT_P_CREATED; |
227 | } else if (!(flags & TCA_ACT_FLAGS_REPLACE)) { |
228 | tcf_idr_release(a: *a, bind); |
229 | return -EEXIST; |
230 | } |
231 | |
232 | err = tcf_action_check_ctrlact(action: parm->action, tp, handle: &goto_ch, newchain: extack); |
233 | if (err < 0) |
234 | goto release_idr; |
235 | |
236 | v = to_vlan(*a); |
237 | |
238 | p = kzalloc(size: sizeof(*p), GFP_KERNEL); |
239 | if (!p) { |
240 | err = -ENOMEM; |
241 | goto put_chain; |
242 | } |
243 | |
244 | p->tcfv_action = action; |
245 | p->tcfv_push_vid = push_vid; |
246 | p->tcfv_push_prio = push_prio; |
247 | p->tcfv_push_prio_exists = push_prio_exists || action == TCA_VLAN_ACT_PUSH; |
248 | p->tcfv_push_proto = push_proto; |
249 | |
250 | if (action == TCA_VLAN_ACT_PUSH_ETH) { |
251 | nla_memcpy(dest: &p->tcfv_push_dst, src: tb[TCA_VLAN_PUSH_ETH_DST], |
252 | ETH_ALEN); |
253 | nla_memcpy(dest: &p->tcfv_push_src, src: tb[TCA_VLAN_PUSH_ETH_SRC], |
254 | ETH_ALEN); |
255 | } |
256 | |
257 | spin_lock_bh(lock: &v->tcf_lock); |
258 | goto_ch = tcf_action_set_ctrlact(a: *a, action: parm->action, newchain: goto_ch); |
259 | p = rcu_replace_pointer(v->vlan_p, p, lockdep_is_held(&v->tcf_lock)); |
260 | spin_unlock_bh(lock: &v->tcf_lock); |
261 | |
262 | if (goto_ch) |
263 | tcf_chain_put_by_act(chain: goto_ch); |
264 | if (p) |
265 | kfree_rcu(p, rcu); |
266 | |
267 | return ret; |
268 | put_chain: |
269 | if (goto_ch) |
270 | tcf_chain_put_by_act(chain: goto_ch); |
271 | release_idr: |
272 | tcf_idr_release(a: *a, bind); |
273 | return err; |
274 | } |
275 | |
276 | static void tcf_vlan_cleanup(struct tc_action *a) |
277 | { |
278 | struct tcf_vlan *v = to_vlan(a); |
279 | struct tcf_vlan_params *p; |
280 | |
281 | p = rcu_dereference_protected(v->vlan_p, 1); |
282 | if (p) |
283 | kfree_rcu(p, rcu); |
284 | } |
285 | |
286 | static int tcf_vlan_dump(struct sk_buff *skb, struct tc_action *a, |
287 | int bind, int ref) |
288 | { |
289 | unsigned char *b = skb_tail_pointer(skb); |
290 | struct tcf_vlan *v = to_vlan(a); |
291 | struct tcf_vlan_params *p; |
292 | struct tc_vlan opt = { |
293 | .index = v->tcf_index, |
294 | .refcnt = refcount_read(r: &v->tcf_refcnt) - ref, |
295 | .bindcnt = atomic_read(v: &v->tcf_bindcnt) - bind, |
296 | }; |
297 | struct tcf_t t; |
298 | |
299 | spin_lock_bh(lock: &v->tcf_lock); |
300 | opt.action = v->tcf_action; |
301 | p = rcu_dereference_protected(v->vlan_p, lockdep_is_held(&v->tcf_lock)); |
302 | opt.v_action = p->tcfv_action; |
303 | if (nla_put(skb, attrtype: TCA_VLAN_PARMS, attrlen: sizeof(opt), data: &opt)) |
304 | goto nla_put_failure; |
305 | |
306 | if ((p->tcfv_action == TCA_VLAN_ACT_PUSH || |
307 | p->tcfv_action == TCA_VLAN_ACT_MODIFY) && |
308 | (nla_put_u16(skb, attrtype: TCA_VLAN_PUSH_VLAN_ID, value: p->tcfv_push_vid) || |
309 | nla_put_be16(skb, attrtype: TCA_VLAN_PUSH_VLAN_PROTOCOL, |
310 | value: p->tcfv_push_proto) || |
311 | (p->tcfv_push_prio_exists && |
312 | nla_put_u8(skb, attrtype: TCA_VLAN_PUSH_VLAN_PRIORITY, value: p->tcfv_push_prio)))) |
313 | goto nla_put_failure; |
314 | |
315 | if (p->tcfv_action == TCA_VLAN_ACT_PUSH_ETH) { |
316 | if (nla_put(skb, attrtype: TCA_VLAN_PUSH_ETH_DST, ETH_ALEN, |
317 | data: p->tcfv_push_dst)) |
318 | goto nla_put_failure; |
319 | if (nla_put(skb, attrtype: TCA_VLAN_PUSH_ETH_SRC, ETH_ALEN, |
320 | data: p->tcfv_push_src)) |
321 | goto nla_put_failure; |
322 | } |
323 | |
324 | tcf_tm_dump(dtm: &t, stm: &v->tcf_tm); |
325 | if (nla_put_64bit(skb, attrtype: TCA_VLAN_TM, attrlen: sizeof(t), data: &t, padattr: TCA_VLAN_PAD)) |
326 | goto nla_put_failure; |
327 | spin_unlock_bh(lock: &v->tcf_lock); |
328 | |
329 | return skb->len; |
330 | |
331 | nla_put_failure: |
332 | spin_unlock_bh(lock: &v->tcf_lock); |
333 | nlmsg_trim(skb, mark: b); |
334 | return -1; |
335 | } |
336 | |
337 | static void tcf_vlan_stats_update(struct tc_action *a, u64 bytes, u64 packets, |
338 | u64 drops, u64 lastuse, bool hw) |
339 | { |
340 | struct tcf_vlan *v = to_vlan(a); |
341 | struct tcf_t *tm = &v->tcf_tm; |
342 | |
343 | tcf_action_update_stats(a, bytes, packets, drops, hw); |
344 | tm->lastuse = max_t(u64, tm->lastuse, lastuse); |
345 | } |
346 | |
347 | static size_t tcf_vlan_get_fill_size(const struct tc_action *act) |
348 | { |
349 | return nla_total_size(payload: sizeof(struct tc_vlan)) |
350 | + nla_total_size(payload: sizeof(u16)) /* TCA_VLAN_PUSH_VLAN_ID */ |
351 | + nla_total_size(payload: sizeof(u16)) /* TCA_VLAN_PUSH_VLAN_PROTOCOL */ |
352 | + nla_total_size(payload: sizeof(u8)); /* TCA_VLAN_PUSH_VLAN_PRIORITY */ |
353 | } |
354 | |
355 | static int tcf_vlan_offload_act_setup(struct tc_action *act, void *entry_data, |
356 | u32 *index_inc, bool bind, |
357 | struct netlink_ext_ack *extack) |
358 | { |
359 | if (bind) { |
360 | struct flow_action_entry *entry = entry_data; |
361 | |
362 | switch (tcf_vlan_action(a: act)) { |
363 | case TCA_VLAN_ACT_PUSH: |
364 | entry->id = FLOW_ACTION_VLAN_PUSH; |
365 | entry->vlan.vid = tcf_vlan_push_vid(a: act); |
366 | entry->vlan.proto = tcf_vlan_push_proto(a: act); |
367 | entry->vlan.prio = tcf_vlan_push_prio(a: act); |
368 | break; |
369 | case TCA_VLAN_ACT_POP: |
370 | entry->id = FLOW_ACTION_VLAN_POP; |
371 | break; |
372 | case TCA_VLAN_ACT_MODIFY: |
373 | entry->id = FLOW_ACTION_VLAN_MANGLE; |
374 | entry->vlan.vid = tcf_vlan_push_vid(a: act); |
375 | entry->vlan.proto = tcf_vlan_push_proto(a: act); |
376 | entry->vlan.prio = tcf_vlan_push_prio(a: act); |
377 | break; |
378 | case TCA_VLAN_ACT_POP_ETH: |
379 | entry->id = FLOW_ACTION_VLAN_POP_ETH; |
380 | break; |
381 | case TCA_VLAN_ACT_PUSH_ETH: |
382 | entry->id = FLOW_ACTION_VLAN_PUSH_ETH; |
383 | tcf_vlan_push_eth(src: entry->vlan_push_eth.src, dest: entry->vlan_push_eth.dst, a: act); |
384 | break; |
385 | default: |
386 | NL_SET_ERR_MSG_MOD(extack, "Unsupported vlan action mode offload" ); |
387 | return -EOPNOTSUPP; |
388 | } |
389 | *index_inc = 1; |
390 | } else { |
391 | struct flow_offload_action *fl_action = entry_data; |
392 | |
393 | switch (tcf_vlan_action(a: act)) { |
394 | case TCA_VLAN_ACT_PUSH: |
395 | fl_action->id = FLOW_ACTION_VLAN_PUSH; |
396 | break; |
397 | case TCA_VLAN_ACT_POP: |
398 | fl_action->id = FLOW_ACTION_VLAN_POP; |
399 | break; |
400 | case TCA_VLAN_ACT_MODIFY: |
401 | fl_action->id = FLOW_ACTION_VLAN_MANGLE; |
402 | break; |
403 | case TCA_VLAN_ACT_POP_ETH: |
404 | fl_action->id = FLOW_ACTION_VLAN_POP_ETH; |
405 | break; |
406 | case TCA_VLAN_ACT_PUSH_ETH: |
407 | fl_action->id = FLOW_ACTION_VLAN_PUSH_ETH; |
408 | break; |
409 | default: |
410 | return -EOPNOTSUPP; |
411 | } |
412 | } |
413 | |
414 | return 0; |
415 | } |
416 | |
417 | static struct tc_action_ops act_vlan_ops = { |
418 | .kind = "vlan" , |
419 | .id = TCA_ID_VLAN, |
420 | .owner = THIS_MODULE, |
421 | .act = tcf_vlan_act, |
422 | .dump = tcf_vlan_dump, |
423 | .init = tcf_vlan_init, |
424 | .cleanup = tcf_vlan_cleanup, |
425 | .stats_update = tcf_vlan_stats_update, |
426 | .get_fill_size = tcf_vlan_get_fill_size, |
427 | .offload_act_setup = tcf_vlan_offload_act_setup, |
428 | .size = sizeof(struct tcf_vlan), |
429 | }; |
430 | |
431 | static __net_init int vlan_init_net(struct net *net) |
432 | { |
433 | struct tc_action_net *tn = net_generic(net, id: act_vlan_ops.net_id); |
434 | |
435 | return tc_action_net_init(net, tn, ops: &act_vlan_ops); |
436 | } |
437 | |
438 | static void __net_exit vlan_exit_net(struct list_head *net_list) |
439 | { |
440 | tc_action_net_exit(net_list, id: act_vlan_ops.net_id); |
441 | } |
442 | |
443 | static struct pernet_operations vlan_net_ops = { |
444 | .init = vlan_init_net, |
445 | .exit_batch = vlan_exit_net, |
446 | .id = &act_vlan_ops.net_id, |
447 | .size = sizeof(struct tc_action_net), |
448 | }; |
449 | |
450 | static int __init vlan_init_module(void) |
451 | { |
452 | return tcf_register_action(a: &act_vlan_ops, ops: &vlan_net_ops); |
453 | } |
454 | |
455 | static void __exit vlan_cleanup_module(void) |
456 | { |
457 | tcf_unregister_action(a: &act_vlan_ops, ops: &vlan_net_ops); |
458 | } |
459 | |
460 | module_init(vlan_init_module); |
461 | module_exit(vlan_cleanup_module); |
462 | |
463 | MODULE_AUTHOR("Jiri Pirko <jiri@resnulli.us>" ); |
464 | MODULE_DESCRIPTION("vlan manipulation actions" ); |
465 | MODULE_LICENSE("GPL v2" ); |
466 | |