1 | // SPDX-License-Identifier: (GPL-2.0-only OR BSD-2-Clause) |
2 | /* Copyright (C) 2019 Netronome Systems, Inc. */ |
3 | |
4 | #include <linux/if_arp.h> |
5 | #include <linux/init.h> |
6 | #include <linux/kernel.h> |
7 | #include <linux/module.h> |
8 | #include <linux/mpls.h> |
9 | #include <linux/rtnetlink.h> |
10 | #include <linux/skbuff.h> |
11 | #include <linux/tc_act/tc_mpls.h> |
12 | #include <net/mpls.h> |
13 | #include <net/netlink.h> |
14 | #include <net/pkt_sched.h> |
15 | #include <net/pkt_cls.h> |
16 | #include <net/tc_act/tc_mpls.h> |
17 | #include <net/tc_wrapper.h> |
18 | |
19 | static struct tc_action_ops act_mpls_ops; |
20 | |
21 | #define ACT_MPLS_TTL_DEFAULT 255 |
22 | |
23 | static __be32 tcf_mpls_get_lse(struct mpls_shim_hdr *lse, |
24 | struct tcf_mpls_params *p, bool set_bos) |
25 | { |
26 | u32 new_lse = 0; |
27 | |
28 | if (lse) |
29 | new_lse = be32_to_cpu(lse->label_stack_entry); |
30 | |
31 | if (p->tcfm_label != ACT_MPLS_LABEL_NOT_SET) { |
32 | new_lse &= ~MPLS_LS_LABEL_MASK; |
33 | new_lse |= p->tcfm_label << MPLS_LS_LABEL_SHIFT; |
34 | } |
35 | if (p->tcfm_ttl) { |
36 | new_lse &= ~MPLS_LS_TTL_MASK; |
37 | new_lse |= p->tcfm_ttl << MPLS_LS_TTL_SHIFT; |
38 | } |
39 | if (p->tcfm_tc != ACT_MPLS_TC_NOT_SET) { |
40 | new_lse &= ~MPLS_LS_TC_MASK; |
41 | new_lse |= p->tcfm_tc << MPLS_LS_TC_SHIFT; |
42 | } |
43 | if (p->tcfm_bos != ACT_MPLS_BOS_NOT_SET) { |
44 | new_lse &= ~MPLS_LS_S_MASK; |
45 | new_lse |= p->tcfm_bos << MPLS_LS_S_SHIFT; |
46 | } else if (set_bos) { |
47 | new_lse |= 1 << MPLS_LS_S_SHIFT; |
48 | } |
49 | |
50 | return cpu_to_be32(new_lse); |
51 | } |
52 | |
53 | TC_INDIRECT_SCOPE int tcf_mpls_act(struct sk_buff *skb, |
54 | const struct tc_action *a, |
55 | struct tcf_result *res) |
56 | { |
57 | struct tcf_mpls *m = to_mpls(a); |
58 | struct tcf_mpls_params *p; |
59 | __be32 new_lse; |
60 | int ret, mac_len; |
61 | |
62 | tcf_lastuse_update(tm: &m->tcf_tm); |
63 | bstats_update(this_cpu_ptr(m->common.cpu_bstats), skb); |
64 | |
65 | /* Ensure 'data' points at mac_header prior calling mpls manipulating |
66 | * functions. |
67 | */ |
68 | if (skb_at_tc_ingress(skb)) { |
69 | skb_push_rcsum(skb, len: skb->mac_len); |
70 | mac_len = skb->mac_len; |
71 | } else { |
72 | mac_len = skb_network_offset(skb); |
73 | } |
74 | |
75 | ret = READ_ONCE(m->tcf_action); |
76 | |
77 | p = rcu_dereference_bh(m->mpls_p); |
78 | |
79 | switch (p->tcfm_action) { |
80 | case TCA_MPLS_ACT_POP: |
81 | if (skb_mpls_pop(skb, next_proto: p->tcfm_proto, mac_len, |
82 | ethernet: skb->dev && skb->dev->type == ARPHRD_ETHER)) |
83 | goto drop; |
84 | break; |
85 | case TCA_MPLS_ACT_PUSH: |
86 | new_lse = tcf_mpls_get_lse(NULL, p, set_bos: !eth_p_mpls(eth_type: skb_protocol(skb, skip_vlan: true))); |
87 | if (skb_mpls_push(skb, mpls_lse: new_lse, mpls_proto: p->tcfm_proto, mac_len, |
88 | ethernet: skb->dev && skb->dev->type == ARPHRD_ETHER)) |
89 | goto drop; |
90 | break; |
91 | case TCA_MPLS_ACT_MAC_PUSH: |
92 | if (skb_vlan_tag_present(skb)) { |
93 | if (__vlan_insert_inner_tag(skb, vlan_proto: skb->vlan_proto, |
94 | skb_vlan_tag_get(skb), |
95 | ETH_HLEN) < 0) |
96 | goto drop; |
97 | |
98 | skb->protocol = skb->vlan_proto; |
99 | __vlan_hwaccel_clear_tag(skb); |
100 | } |
101 | |
102 | new_lse = tcf_mpls_get_lse(NULL, p, set_bos: mac_len || |
103 | !eth_p_mpls(eth_type: skb->protocol)); |
104 | |
105 | if (skb_mpls_push(skb, mpls_lse: new_lse, mpls_proto: p->tcfm_proto, mac_len: 0, ethernet: false)) |
106 | goto drop; |
107 | break; |
108 | case TCA_MPLS_ACT_MODIFY: |
109 | if (!pskb_may_pull(skb, |
110 | len: skb_network_offset(skb) + MPLS_HLEN)) |
111 | goto drop; |
112 | new_lse = tcf_mpls_get_lse(lse: mpls_hdr(skb), p, set_bos: false); |
113 | if (skb_mpls_update_lse(skb, mpls_lse: new_lse)) |
114 | goto drop; |
115 | break; |
116 | case TCA_MPLS_ACT_DEC_TTL: |
117 | if (skb_mpls_dec_ttl(skb)) |
118 | goto drop; |
119 | break; |
120 | } |
121 | |
122 | if (skb_at_tc_ingress(skb)) |
123 | skb_pull_rcsum(skb, len: skb->mac_len); |
124 | |
125 | return ret; |
126 | |
127 | drop: |
128 | qstats_drop_inc(this_cpu_ptr(m->common.cpu_qstats)); |
129 | return TC_ACT_SHOT; |
130 | } |
131 | |
132 | static int valid_label(const struct nlattr *attr, |
133 | struct netlink_ext_ack *extack) |
134 | { |
135 | const u32 *label = nla_data(nla: attr); |
136 | |
137 | if (nla_len(nla: attr) != sizeof(*label)) { |
138 | NL_SET_ERR_MSG_MOD(extack, "Invalid MPLS label length" ); |
139 | return -EINVAL; |
140 | } |
141 | |
142 | if (*label & ~MPLS_LABEL_MASK || *label == MPLS_LABEL_IMPLNULL) { |
143 | NL_SET_ERR_MSG_MOD(extack, "MPLS label out of range" ); |
144 | return -EINVAL; |
145 | } |
146 | |
147 | return 0; |
148 | } |
149 | |
150 | static const struct nla_policy mpls_policy[TCA_MPLS_MAX + 1] = { |
151 | [TCA_MPLS_PARMS] = NLA_POLICY_EXACT_LEN(sizeof(struct tc_mpls)), |
152 | [TCA_MPLS_PROTO] = { .type = NLA_U16 }, |
153 | [TCA_MPLS_LABEL] = NLA_POLICY_VALIDATE_FN(NLA_BINARY, |
154 | valid_label), |
155 | [TCA_MPLS_TC] = NLA_POLICY_RANGE(NLA_U8, 0, 7), |
156 | [TCA_MPLS_TTL] = NLA_POLICY_MIN(NLA_U8, 1), |
157 | [TCA_MPLS_BOS] = NLA_POLICY_RANGE(NLA_U8, 0, 1), |
158 | }; |
159 | |
160 | static int tcf_mpls_init(struct net *net, struct nlattr *nla, |
161 | struct nlattr *est, struct tc_action **a, |
162 | struct tcf_proto *tp, u32 flags, |
163 | struct netlink_ext_ack *extack) |
164 | { |
165 | struct tc_action_net *tn = net_generic(net, id: act_mpls_ops.net_id); |
166 | bool bind = flags & TCA_ACT_FLAGS_BIND; |
167 | struct nlattr *tb[TCA_MPLS_MAX + 1]; |
168 | struct tcf_chain *goto_ch = NULL; |
169 | struct tcf_mpls_params *p; |
170 | struct tc_mpls *parm; |
171 | bool exists = false; |
172 | struct tcf_mpls *m; |
173 | int ret = 0, err; |
174 | u8 mpls_ttl = 0; |
175 | u32 index; |
176 | |
177 | if (!nla) { |
178 | NL_SET_ERR_MSG_MOD(extack, "Missing netlink attributes" ); |
179 | return -EINVAL; |
180 | } |
181 | |
182 | err = nla_parse_nested(tb, TCA_MPLS_MAX, nla, policy: mpls_policy, extack); |
183 | if (err < 0) |
184 | return err; |
185 | |
186 | if (!tb[TCA_MPLS_PARMS]) { |
187 | NL_SET_ERR_MSG_MOD(extack, "No MPLS params" ); |
188 | return -EINVAL; |
189 | } |
190 | parm = nla_data(nla: tb[TCA_MPLS_PARMS]); |
191 | index = parm->index; |
192 | |
193 | err = tcf_idr_check_alloc(tn, index: &index, a, bind); |
194 | if (err < 0) |
195 | return err; |
196 | exists = err; |
197 | if (exists && bind) |
198 | return ACT_P_BOUND; |
199 | |
200 | if (!exists) { |
201 | ret = tcf_idr_create(tn, index, est, a, ops: &act_mpls_ops, bind, |
202 | cpustats: true, flags); |
203 | if (ret) { |
204 | tcf_idr_cleanup(tn, index); |
205 | return ret; |
206 | } |
207 | |
208 | ret = ACT_P_CREATED; |
209 | } else if (!(flags & TCA_ACT_FLAGS_REPLACE)) { |
210 | tcf_idr_release(a: *a, bind); |
211 | return -EEXIST; |
212 | } |
213 | |
214 | /* Verify parameters against action type. */ |
215 | switch (parm->m_action) { |
216 | case TCA_MPLS_ACT_POP: |
217 | if (!tb[TCA_MPLS_PROTO]) { |
218 | NL_SET_ERR_MSG_MOD(extack, "Protocol must be set for MPLS pop" ); |
219 | err = -EINVAL; |
220 | goto release_idr; |
221 | } |
222 | if (!eth_proto_is_802_3(proto: nla_get_be16(nla: tb[TCA_MPLS_PROTO]))) { |
223 | NL_SET_ERR_MSG_MOD(extack, "Invalid protocol type for MPLS pop" ); |
224 | err = -EINVAL; |
225 | goto release_idr; |
226 | } |
227 | if (tb[TCA_MPLS_LABEL] || tb[TCA_MPLS_TTL] || tb[TCA_MPLS_TC] || |
228 | tb[TCA_MPLS_BOS]) { |
229 | NL_SET_ERR_MSG_MOD(extack, "Label, TTL, TC or BOS cannot be used with MPLS pop" ); |
230 | err = -EINVAL; |
231 | goto release_idr; |
232 | } |
233 | break; |
234 | case TCA_MPLS_ACT_DEC_TTL: |
235 | if (tb[TCA_MPLS_PROTO] || tb[TCA_MPLS_LABEL] || |
236 | tb[TCA_MPLS_TTL] || tb[TCA_MPLS_TC] || tb[TCA_MPLS_BOS]) { |
237 | NL_SET_ERR_MSG_MOD(extack, "Label, TTL, TC, BOS or protocol cannot be used with MPLS dec_ttl" ); |
238 | err = -EINVAL; |
239 | goto release_idr; |
240 | } |
241 | break; |
242 | case TCA_MPLS_ACT_PUSH: |
243 | case TCA_MPLS_ACT_MAC_PUSH: |
244 | if (!tb[TCA_MPLS_LABEL]) { |
245 | NL_SET_ERR_MSG_MOD(extack, "Label is required for MPLS push" ); |
246 | err = -EINVAL; |
247 | goto release_idr; |
248 | } |
249 | if (tb[TCA_MPLS_PROTO] && |
250 | !eth_p_mpls(eth_type: nla_get_be16(nla: tb[TCA_MPLS_PROTO]))) { |
251 | NL_SET_ERR_MSG_MOD(extack, "Protocol must be an MPLS type for MPLS push" ); |
252 | err = -EPROTONOSUPPORT; |
253 | goto release_idr; |
254 | } |
255 | /* Push needs a TTL - if not specified, set a default value. */ |
256 | if (!tb[TCA_MPLS_TTL]) { |
257 | #if IS_ENABLED(CONFIG_MPLS) |
258 | mpls_ttl = net->mpls.default_ttl ? |
259 | net->mpls.default_ttl : ACT_MPLS_TTL_DEFAULT; |
260 | #else |
261 | mpls_ttl = ACT_MPLS_TTL_DEFAULT; |
262 | #endif |
263 | } |
264 | break; |
265 | case TCA_MPLS_ACT_MODIFY: |
266 | if (tb[TCA_MPLS_PROTO]) { |
267 | NL_SET_ERR_MSG_MOD(extack, "Protocol cannot be used with MPLS modify" ); |
268 | err = -EINVAL; |
269 | goto release_idr; |
270 | } |
271 | break; |
272 | default: |
273 | NL_SET_ERR_MSG_MOD(extack, "Unknown MPLS action" ); |
274 | err = -EINVAL; |
275 | goto release_idr; |
276 | } |
277 | |
278 | err = tcf_action_check_ctrlact(action: parm->action, tp, handle: &goto_ch, newchain: extack); |
279 | if (err < 0) |
280 | goto release_idr; |
281 | |
282 | m = to_mpls(*a); |
283 | |
284 | p = kzalloc(size: sizeof(*p), GFP_KERNEL); |
285 | if (!p) { |
286 | err = -ENOMEM; |
287 | goto put_chain; |
288 | } |
289 | |
290 | p->tcfm_action = parm->m_action; |
291 | p->tcfm_label = tb[TCA_MPLS_LABEL] ? nla_get_u32(nla: tb[TCA_MPLS_LABEL]) : |
292 | ACT_MPLS_LABEL_NOT_SET; |
293 | p->tcfm_tc = tb[TCA_MPLS_TC] ? nla_get_u8(nla: tb[TCA_MPLS_TC]) : |
294 | ACT_MPLS_TC_NOT_SET; |
295 | p->tcfm_ttl = tb[TCA_MPLS_TTL] ? nla_get_u8(nla: tb[TCA_MPLS_TTL]) : |
296 | mpls_ttl; |
297 | p->tcfm_bos = tb[TCA_MPLS_BOS] ? nla_get_u8(nla: tb[TCA_MPLS_BOS]) : |
298 | ACT_MPLS_BOS_NOT_SET; |
299 | p->tcfm_proto = tb[TCA_MPLS_PROTO] ? nla_get_be16(nla: tb[TCA_MPLS_PROTO]) : |
300 | htons(ETH_P_MPLS_UC); |
301 | |
302 | spin_lock_bh(lock: &m->tcf_lock); |
303 | goto_ch = tcf_action_set_ctrlact(a: *a, action: parm->action, newchain: goto_ch); |
304 | p = rcu_replace_pointer(m->mpls_p, p, lockdep_is_held(&m->tcf_lock)); |
305 | spin_unlock_bh(lock: &m->tcf_lock); |
306 | |
307 | if (goto_ch) |
308 | tcf_chain_put_by_act(chain: goto_ch); |
309 | if (p) |
310 | kfree_rcu(p, rcu); |
311 | |
312 | return ret; |
313 | put_chain: |
314 | if (goto_ch) |
315 | tcf_chain_put_by_act(chain: goto_ch); |
316 | release_idr: |
317 | tcf_idr_release(a: *a, bind); |
318 | return err; |
319 | } |
320 | |
321 | static void tcf_mpls_cleanup(struct tc_action *a) |
322 | { |
323 | struct tcf_mpls *m = to_mpls(a); |
324 | struct tcf_mpls_params *p; |
325 | |
326 | p = rcu_dereference_protected(m->mpls_p, 1); |
327 | if (p) |
328 | kfree_rcu(p, rcu); |
329 | } |
330 | |
331 | static int tcf_mpls_dump(struct sk_buff *skb, struct tc_action *a, |
332 | int bind, int ref) |
333 | { |
334 | unsigned char *b = skb_tail_pointer(skb); |
335 | struct tcf_mpls *m = to_mpls(a); |
336 | struct tcf_mpls_params *p; |
337 | struct tc_mpls opt = { |
338 | .index = m->tcf_index, |
339 | .refcnt = refcount_read(r: &m->tcf_refcnt) - ref, |
340 | .bindcnt = atomic_read(v: &m->tcf_bindcnt) - bind, |
341 | }; |
342 | struct tcf_t t; |
343 | |
344 | spin_lock_bh(lock: &m->tcf_lock); |
345 | opt.action = m->tcf_action; |
346 | p = rcu_dereference_protected(m->mpls_p, lockdep_is_held(&m->tcf_lock)); |
347 | opt.m_action = p->tcfm_action; |
348 | |
349 | if (nla_put(skb, attrtype: TCA_MPLS_PARMS, attrlen: sizeof(opt), data: &opt)) |
350 | goto nla_put_failure; |
351 | |
352 | if (p->tcfm_label != ACT_MPLS_LABEL_NOT_SET && |
353 | nla_put_u32(skb, attrtype: TCA_MPLS_LABEL, value: p->tcfm_label)) |
354 | goto nla_put_failure; |
355 | |
356 | if (p->tcfm_tc != ACT_MPLS_TC_NOT_SET && |
357 | nla_put_u8(skb, attrtype: TCA_MPLS_TC, value: p->tcfm_tc)) |
358 | goto nla_put_failure; |
359 | |
360 | if (p->tcfm_ttl && nla_put_u8(skb, attrtype: TCA_MPLS_TTL, value: p->tcfm_ttl)) |
361 | goto nla_put_failure; |
362 | |
363 | if (p->tcfm_bos != ACT_MPLS_BOS_NOT_SET && |
364 | nla_put_u8(skb, attrtype: TCA_MPLS_BOS, value: p->tcfm_bos)) |
365 | goto nla_put_failure; |
366 | |
367 | if (nla_put_be16(skb, attrtype: TCA_MPLS_PROTO, value: p->tcfm_proto)) |
368 | goto nla_put_failure; |
369 | |
370 | tcf_tm_dump(dtm: &t, stm: &m->tcf_tm); |
371 | |
372 | if (nla_put_64bit(skb, attrtype: TCA_MPLS_TM, attrlen: sizeof(t), data: &t, padattr: TCA_MPLS_PAD)) |
373 | goto nla_put_failure; |
374 | |
375 | spin_unlock_bh(lock: &m->tcf_lock); |
376 | |
377 | return skb->len; |
378 | |
379 | nla_put_failure: |
380 | spin_unlock_bh(lock: &m->tcf_lock); |
381 | nlmsg_trim(skb, mark: b); |
382 | return -EMSGSIZE; |
383 | } |
384 | |
385 | static int tcf_mpls_offload_act_setup(struct tc_action *act, void *entry_data, |
386 | u32 *index_inc, bool bind, |
387 | struct netlink_ext_ack *extack) |
388 | { |
389 | if (bind) { |
390 | struct flow_action_entry *entry = entry_data; |
391 | |
392 | switch (tcf_mpls_action(a: act)) { |
393 | case TCA_MPLS_ACT_PUSH: |
394 | entry->id = FLOW_ACTION_MPLS_PUSH; |
395 | entry->mpls_push.proto = tcf_mpls_proto(a: act); |
396 | entry->mpls_push.label = tcf_mpls_label(a: act); |
397 | entry->mpls_push.tc = tcf_mpls_tc(a: act); |
398 | entry->mpls_push.bos = tcf_mpls_bos(a: act); |
399 | entry->mpls_push.ttl = tcf_mpls_ttl(a: act); |
400 | break; |
401 | case TCA_MPLS_ACT_POP: |
402 | entry->id = FLOW_ACTION_MPLS_POP; |
403 | entry->mpls_pop.proto = tcf_mpls_proto(a: act); |
404 | break; |
405 | case TCA_MPLS_ACT_MODIFY: |
406 | entry->id = FLOW_ACTION_MPLS_MANGLE; |
407 | entry->mpls_mangle.label = tcf_mpls_label(a: act); |
408 | entry->mpls_mangle.tc = tcf_mpls_tc(a: act); |
409 | entry->mpls_mangle.bos = tcf_mpls_bos(a: act); |
410 | entry->mpls_mangle.ttl = tcf_mpls_ttl(a: act); |
411 | break; |
412 | case TCA_MPLS_ACT_DEC_TTL: |
413 | NL_SET_ERR_MSG_MOD(extack, "Offload not supported when \"dec_ttl\" option is used" ); |
414 | return -EOPNOTSUPP; |
415 | case TCA_MPLS_ACT_MAC_PUSH: |
416 | NL_SET_ERR_MSG_MOD(extack, "Offload not supported when \"mac_push\" option is used" ); |
417 | return -EOPNOTSUPP; |
418 | default: |
419 | NL_SET_ERR_MSG_MOD(extack, "Unsupported MPLS mode offload" ); |
420 | return -EOPNOTSUPP; |
421 | } |
422 | *index_inc = 1; |
423 | } else { |
424 | struct flow_offload_action *fl_action = entry_data; |
425 | |
426 | switch (tcf_mpls_action(a: act)) { |
427 | case TCA_MPLS_ACT_PUSH: |
428 | fl_action->id = FLOW_ACTION_MPLS_PUSH; |
429 | break; |
430 | case TCA_MPLS_ACT_POP: |
431 | fl_action->id = FLOW_ACTION_MPLS_POP; |
432 | break; |
433 | case TCA_MPLS_ACT_MODIFY: |
434 | fl_action->id = FLOW_ACTION_MPLS_MANGLE; |
435 | break; |
436 | default: |
437 | return -EOPNOTSUPP; |
438 | } |
439 | } |
440 | |
441 | return 0; |
442 | } |
443 | |
444 | static struct tc_action_ops act_mpls_ops = { |
445 | .kind = "mpls" , |
446 | .id = TCA_ID_MPLS, |
447 | .owner = THIS_MODULE, |
448 | .act = tcf_mpls_act, |
449 | .dump = tcf_mpls_dump, |
450 | .init = tcf_mpls_init, |
451 | .cleanup = tcf_mpls_cleanup, |
452 | .offload_act_setup = tcf_mpls_offload_act_setup, |
453 | .size = sizeof(struct tcf_mpls), |
454 | }; |
455 | MODULE_ALIAS_NET_ACT("mpls" ); |
456 | |
457 | static __net_init int mpls_init_net(struct net *net) |
458 | { |
459 | struct tc_action_net *tn = net_generic(net, id: act_mpls_ops.net_id); |
460 | |
461 | return tc_action_net_init(net, tn, ops: &act_mpls_ops); |
462 | } |
463 | |
464 | static void __net_exit mpls_exit_net(struct list_head *net_list) |
465 | { |
466 | tc_action_net_exit(net_list, id: act_mpls_ops.net_id); |
467 | } |
468 | |
469 | static struct pernet_operations mpls_net_ops = { |
470 | .init = mpls_init_net, |
471 | .exit_batch = mpls_exit_net, |
472 | .id = &act_mpls_ops.net_id, |
473 | .size = sizeof(struct tc_action_net), |
474 | }; |
475 | |
476 | static int __init mpls_init_module(void) |
477 | { |
478 | return tcf_register_action(a: &act_mpls_ops, ops: &mpls_net_ops); |
479 | } |
480 | |
481 | static void __exit mpls_cleanup_module(void) |
482 | { |
483 | tcf_unregister_action(a: &act_mpls_ops, ops: &mpls_net_ops); |
484 | } |
485 | |
486 | module_init(mpls_init_module); |
487 | module_exit(mpls_cleanup_module); |
488 | |
489 | MODULE_SOFTDEP("post: mpls_gso" ); |
490 | MODULE_AUTHOR("Netronome Systems <oss-drivers@netronome.com>" ); |
491 | MODULE_LICENSE("GPL" ); |
492 | MODULE_DESCRIPTION("MPLS manipulation actions" ); |
493 | |