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
19static struct tc_action_ops act_mpls_ops;
20
21#define ACT_MPLS_TTL_DEFAULT	255
22
23static __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
53TC_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(&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, 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, p->tcfm_proto, mac_len,
82				 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, !eth_p_mpls(skb_protocol(skb, true)));
87		if (skb_mpls_push(skb, new_lse, p->tcfm_proto, mac_len,
88				  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, 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, mac_len ||
103					   !eth_p_mpls(skb->protocol));
104
105		if (skb_mpls_push(skb, new_lse, p->tcfm_proto, 0, false))
106			goto drop;
107		break;
108	case TCA_MPLS_ACT_MODIFY:
109		if (!pskb_may_pull(skb,
110				   skb_network_offset(skb) + MPLS_HLEN))
111			goto drop;
112		new_lse = tcf_mpls_get_lse(mpls_hdr(skb), p, false);
113		if (skb_mpls_update_lse(skb, 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, skb->mac_len);
124
125	return ret;
126
127drop:
128	qstats_drop_inc(this_cpu_ptr(m->common.cpu_qstats));
129	return TC_ACT_SHOT;
130}
131
132static int valid_label(const struct nlattr *attr,
133		       struct netlink_ext_ack *extack)
134{
135	const u32 *label = nla_data(attr);
136
137	if (nla_len(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
150static 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
160static 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, 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, 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(tb[TCA_MPLS_PARMS]);
191	index = parm->index;
192
193	err = tcf_idr_check_alloc(tn, &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, &act_mpls_ops, bind,
202				     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, 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(nla_get_be16(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(nla_get_be16(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(parm->action, tp, &goto_ch, extack);
279	if (err < 0)
280		goto release_idr;
281
282	m = to_mpls(*a);
283
284	p = kzalloc(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(tb[TCA_MPLS_LABEL]) :
292					     ACT_MPLS_LABEL_NOT_SET;
293	p->tcfm_tc = tb[TCA_MPLS_TC] ? nla_get_u8(tb[TCA_MPLS_TC]) :
294				       ACT_MPLS_TC_NOT_SET;
295	p->tcfm_ttl = tb[TCA_MPLS_TTL] ? nla_get_u8(tb[TCA_MPLS_TTL]) :
296					 mpls_ttl;
297	p->tcfm_bos = tb[TCA_MPLS_BOS] ? nla_get_u8(tb[TCA_MPLS_BOS]) :
298					 ACT_MPLS_BOS_NOT_SET;
299	p->tcfm_proto = tb[TCA_MPLS_PROTO] ? nla_get_be16(tb[TCA_MPLS_PROTO]) :
300					     htons(ETH_P_MPLS_UC);
301
302	spin_lock_bh(&m->tcf_lock);
303	goto_ch = tcf_action_set_ctrlact(*a, parm->action, goto_ch);
304	p = rcu_replace_pointer(m->mpls_p, p, lockdep_is_held(&m->tcf_lock));
305	spin_unlock_bh(&m->tcf_lock);
306
307	if (goto_ch)
308		tcf_chain_put_by_act(goto_ch);
309	if (p)
310		kfree_rcu(p, rcu);
311
312	return ret;
313put_chain:
314	if (goto_ch)
315		tcf_chain_put_by_act(goto_ch);
316release_idr:
317	tcf_idr_release(*a, bind);
318	return err;
319}
320
321static 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
331static 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(&m->tcf_refcnt) - ref,
340		.bindcnt  = atomic_read(&m->tcf_bindcnt) - bind,
341	};
342	struct tcf_t t;
343
344	spin_lock_bh(&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, TCA_MPLS_PARMS, sizeof(opt), &opt))
350		goto nla_put_failure;
351
352	if (p->tcfm_label != ACT_MPLS_LABEL_NOT_SET &&
353	    nla_put_u32(skb, TCA_MPLS_LABEL, p->tcfm_label))
354		goto nla_put_failure;
355
356	if (p->tcfm_tc != ACT_MPLS_TC_NOT_SET &&
357	    nla_put_u8(skb, TCA_MPLS_TC, p->tcfm_tc))
358		goto nla_put_failure;
359
360	if (p->tcfm_ttl && nla_put_u8(skb, TCA_MPLS_TTL, p->tcfm_ttl))
361		goto nla_put_failure;
362
363	if (p->tcfm_bos != ACT_MPLS_BOS_NOT_SET &&
364	    nla_put_u8(skb, TCA_MPLS_BOS, p->tcfm_bos))
365		goto nla_put_failure;
366
367	if (nla_put_be16(skb, TCA_MPLS_PROTO, p->tcfm_proto))
368		goto nla_put_failure;
369
370	tcf_tm_dump(&t, &m->tcf_tm);
371
372	if (nla_put_64bit(skb, TCA_MPLS_TM, sizeof(t), &t, TCA_MPLS_PAD))
373		goto nla_put_failure;
374
375	spin_unlock_bh(&m->tcf_lock);
376
377	return skb->len;
378
379nla_put_failure:
380	spin_unlock_bh(&m->tcf_lock);
381	nlmsg_trim(skb, b);
382	return -EMSGSIZE;
383}
384
385static 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(act)) {
393		case TCA_MPLS_ACT_PUSH:
394			entry->id = FLOW_ACTION_MPLS_PUSH;
395			entry->mpls_push.proto = tcf_mpls_proto(act);
396			entry->mpls_push.label = tcf_mpls_label(act);
397			entry->mpls_push.tc = tcf_mpls_tc(act);
398			entry->mpls_push.bos = tcf_mpls_bos(act);
399			entry->mpls_push.ttl = tcf_mpls_ttl(act);
400			break;
401		case TCA_MPLS_ACT_POP:
402			entry->id = FLOW_ACTION_MPLS_POP;
403			entry->mpls_pop.proto = tcf_mpls_proto(act);
404			break;
405		case TCA_MPLS_ACT_MODIFY:
406			entry->id = FLOW_ACTION_MPLS_MANGLE;
407			entry->mpls_mangle.label = tcf_mpls_label(act);
408			entry->mpls_mangle.tc = tcf_mpls_tc(act);
409			entry->mpls_mangle.bos = tcf_mpls_bos(act);
410			entry->mpls_mangle.ttl = tcf_mpls_ttl(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(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
444static 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};
455MODULE_ALIAS_NET_ACT("mpls");
456
457static __net_init int mpls_init_net(struct net *net)
458{
459	struct tc_action_net *tn = net_generic(net, act_mpls_ops.net_id);
460
461	return tc_action_net_init(net, tn, &act_mpls_ops);
462}
463
464static void __net_exit mpls_exit_net(struct list_head *net_list)
465{
466	tc_action_net_exit(net_list, act_mpls_ops.net_id);
467}
468
469static 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
476static int __init mpls_init_module(void)
477{
478	return tcf_register_action(&act_mpls_ops, &mpls_net_ops);
479}
480
481static void __exit mpls_cleanup_module(void)
482{
483	tcf_unregister_action(&act_mpls_ops, &mpls_net_ops);
484}
485
486module_init(mpls_init_module);
487module_exit(mpls_cleanup_module);
488
489MODULE_SOFTDEP("post: mpls_gso");
490MODULE_AUTHOR("Netronome Systems <oss-drivers@netronome.com>");
491MODULE_LICENSE("GPL");
492MODULE_DESCRIPTION("MPLS manipulation actions");
493