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 18static unsigned int mpls_net_id; 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 53static int tcf_mpls_act(struct sk_buff *skb, const struct tc_action *a, 54 struct tcf_result *res) 55{ 56 struct tcf_mpls *m = to_mpls(a); 57 struct tcf_mpls_params *p; 58 __be32 new_lse; 59 int ret, mac_len; 60 61 tcf_lastuse_update(&m->tcf_tm); 62 bstats_cpu_update(this_cpu_ptr(m->common.cpu_bstats), skb); 63 64 /* Ensure 'data' points at mac_header prior calling mpls manipulating 65 * functions. 66 */ 67 if (skb_at_tc_ingress(skb)) { 68 skb_push_rcsum(skb, skb->mac_len); 69 mac_len = skb->mac_len; 70 } else { 71 mac_len = skb_network_header(skb) - skb_mac_header(skb); 72 } 73 74 ret = READ_ONCE(m->tcf_action); 75 76 p = rcu_dereference_bh(m->mpls_p); 77 78 switch (p->tcfm_action) { 79 case TCA_MPLS_ACT_POP: 80 if (skb_mpls_pop(skb, p->tcfm_proto, mac_len, 81 skb->dev && skb->dev->type == ARPHRD_ETHER)) 82 goto drop; 83 break; 84 case TCA_MPLS_ACT_PUSH: 85 new_lse = tcf_mpls_get_lse(NULL, p, !eth_p_mpls(skb_protocol(skb, true))); 86 if (skb_mpls_push(skb, new_lse, p->tcfm_proto, mac_len, 87 skb->dev && skb->dev->type == ARPHRD_ETHER)) 88 goto drop; 89 break; 90 case TCA_MPLS_ACT_MAC_PUSH: 91 if (skb_vlan_tag_present(skb)) { 92 if (__vlan_insert_inner_tag(skb, skb->vlan_proto, 93 skb_vlan_tag_get(skb), 94 ETH_HLEN) < 0) 95 goto drop; 96 97 skb->protocol = skb->vlan_proto; 98 __vlan_hwaccel_clear_tag(skb); 99 } 100 101 new_lse = tcf_mpls_get_lse(NULL, p, mac_len || 102 !eth_p_mpls(skb->protocol)); 103 104 if (skb_mpls_push(skb, new_lse, p->tcfm_proto, 0, false)) 105 goto drop; 106 break; 107 case TCA_MPLS_ACT_MODIFY: 108 if (!pskb_may_pull(skb, 109 skb_network_offset(skb) + MPLS_HLEN)) 110 goto drop; 111 new_lse = tcf_mpls_get_lse(mpls_hdr(skb), p, false); 112 if (skb_mpls_update_lse(skb, new_lse)) 113 goto drop; 114 break; 115 case TCA_MPLS_ACT_DEC_TTL: 116 if (skb_mpls_dec_ttl(skb)) 117 goto drop; 118 break; 119 } 120 121 if (skb_at_tc_ingress(skb)) 122 skb_pull_rcsum(skb, skb->mac_len); 123 124 return ret; 125 126drop: 127 qstats_drop_inc(this_cpu_ptr(m->common.cpu_qstats)); 128 return TC_ACT_SHOT; 129} 130 131static int valid_label(const struct nlattr *attr, 132 struct netlink_ext_ack *extack) 133{ 134 const u32 *label = nla_data(attr); 135 136 if (nla_len(attr) != sizeof(*label)) { 137 NL_SET_ERR_MSG_MOD(extack, "Invalid MPLS label length"); 138 return -EINVAL; 139 } 140 141 if (*label & ~MPLS_LABEL_MASK || *label == MPLS_LABEL_IMPLNULL) { 142 NL_SET_ERR_MSG_MOD(extack, "MPLS label out of range"); 143 return -EINVAL; 144 } 145 146 return 0; 147} 148 149static const struct nla_policy mpls_policy[TCA_MPLS_MAX + 1] = { 150 [TCA_MPLS_PARMS] = NLA_POLICY_EXACT_LEN(sizeof(struct tc_mpls)), 151 [TCA_MPLS_PROTO] = { .type = NLA_U16 }, 152 [TCA_MPLS_LABEL] = NLA_POLICY_VALIDATE_FN(NLA_BINARY, 153 valid_label), 154 [TCA_MPLS_TC] = NLA_POLICY_RANGE(NLA_U8, 0, 7), 155 [TCA_MPLS_TTL] = NLA_POLICY_MIN(NLA_U8, 1), 156 [TCA_MPLS_BOS] = NLA_POLICY_RANGE(NLA_U8, 0, 1), 157}; 158 159static int tcf_mpls_init(struct net *net, struct nlattr *nla, 160 struct nlattr *est, struct tc_action **a, 161 int ovr, int bind, bool rtnl_held, 162 struct tcf_proto *tp, u32 flags, 163 struct netlink_ext_ack *extack) 164{ 165 struct tc_action_net *tn = net_generic(net, mpls_net_id); 166 struct nlattr *tb[TCA_MPLS_MAX + 1]; 167 struct tcf_chain *goto_ch = NULL; 168 struct tcf_mpls_params *p; 169 struct tc_mpls *parm; 170 bool exists = false; 171 struct tcf_mpls *m; 172 int ret = 0, err; 173 u8 mpls_ttl = 0; 174 u32 index; 175 176 if (!nla) { 177 NL_SET_ERR_MSG_MOD(extack, "Missing netlink attributes"); 178 return -EINVAL; 179 } 180 181 err = nla_parse_nested(tb, TCA_MPLS_MAX, nla, mpls_policy, extack); 182 if (err < 0) 183 return err; 184 185 if (!tb[TCA_MPLS_PARMS]) { 186 NL_SET_ERR_MSG_MOD(extack, "No MPLS params"); 187 return -EINVAL; 188 } 189 parm = nla_data(tb[TCA_MPLS_PARMS]); 190 index = parm->index; 191 192 /* Verify parameters against action type. */ 193 switch (parm->m_action) { 194 case TCA_MPLS_ACT_POP: 195 if (!tb[TCA_MPLS_PROTO]) { 196 NL_SET_ERR_MSG_MOD(extack, "Protocol must be set for MPLS pop"); 197 return -EINVAL; 198 } 199 if (!eth_proto_is_802_3(nla_get_be16(tb[TCA_MPLS_PROTO]))) { 200 NL_SET_ERR_MSG_MOD(extack, "Invalid protocol type for MPLS pop"); 201 return -EINVAL; 202 } 203 if (tb[TCA_MPLS_LABEL] || tb[TCA_MPLS_TTL] || tb[TCA_MPLS_TC] || 204 tb[TCA_MPLS_BOS]) { 205 NL_SET_ERR_MSG_MOD(extack, "Label, TTL, TC or BOS cannot be used with MPLS pop"); 206 return -EINVAL; 207 } 208 break; 209 case TCA_MPLS_ACT_DEC_TTL: 210 if (tb[TCA_MPLS_PROTO] || tb[TCA_MPLS_LABEL] || 211 tb[TCA_MPLS_TTL] || tb[TCA_MPLS_TC] || tb[TCA_MPLS_BOS]) { 212 NL_SET_ERR_MSG_MOD(extack, "Label, TTL, TC, BOS or protocol cannot be used with MPLS dec_ttl"); 213 return -EINVAL; 214 } 215 break; 216 case TCA_MPLS_ACT_PUSH: 217 case TCA_MPLS_ACT_MAC_PUSH: 218 if (!tb[TCA_MPLS_LABEL]) { 219 NL_SET_ERR_MSG_MOD(extack, "Label is required for MPLS push"); 220 return -EINVAL; 221 } 222 if (tb[TCA_MPLS_PROTO] && 223 !eth_p_mpls(nla_get_be16(tb[TCA_MPLS_PROTO]))) { 224 NL_SET_ERR_MSG_MOD(extack, "Protocol must be an MPLS type for MPLS push"); 225 return -EPROTONOSUPPORT; 226 } 227 /* Push needs a TTL - if not specified, set a default value. */ 228 if (!tb[TCA_MPLS_TTL]) { 229#if IS_ENABLED(CONFIG_MPLS) 230 mpls_ttl = net->mpls.default_ttl ? 231 net->mpls.default_ttl : ACT_MPLS_TTL_DEFAULT; 232#else 233 mpls_ttl = ACT_MPLS_TTL_DEFAULT; 234#endif 235 } 236 break; 237 case TCA_MPLS_ACT_MODIFY: 238 if (tb[TCA_MPLS_PROTO]) { 239 NL_SET_ERR_MSG_MOD(extack, "Protocol cannot be used with MPLS modify"); 240 return -EINVAL; 241 } 242 break; 243 default: 244 NL_SET_ERR_MSG_MOD(extack, "Unknown MPLS action"); 245 return -EINVAL; 246 } 247 248 err = tcf_idr_check_alloc(tn, &index, a, bind); 249 if (err < 0) 250 return err; 251 exists = err; 252 if (exists && bind) 253 return 0; 254 255 if (!exists) { 256 ret = tcf_idr_create(tn, index, est, a, 257 &act_mpls_ops, bind, true, flags); 258 if (ret) { 259 tcf_idr_cleanup(tn, index); 260 return ret; 261 } 262 263 ret = ACT_P_CREATED; 264 } else if (!ovr) { 265 tcf_idr_release(*a, bind); 266 return -EEXIST; 267 } 268 269 err = tcf_action_check_ctrlact(parm->action, tp, &goto_ch, extack); 270 if (err < 0) 271 goto release_idr; 272 273 m = to_mpls(*a); 274 275 p = kzalloc(sizeof(*p), GFP_KERNEL); 276 if (!p) { 277 err = -ENOMEM; 278 goto put_chain; 279 } 280 281 p->tcfm_action = parm->m_action; 282 p->tcfm_label = tb[TCA_MPLS_LABEL] ? nla_get_u32(tb[TCA_MPLS_LABEL]) : 283 ACT_MPLS_LABEL_NOT_SET; 284 p->tcfm_tc = tb[TCA_MPLS_TC] ? nla_get_u8(tb[TCA_MPLS_TC]) : 285 ACT_MPLS_TC_NOT_SET; 286 p->tcfm_ttl = tb[TCA_MPLS_TTL] ? nla_get_u8(tb[TCA_MPLS_TTL]) : 287 mpls_ttl; 288 p->tcfm_bos = tb[TCA_MPLS_BOS] ? nla_get_u8(tb[TCA_MPLS_BOS]) : 289 ACT_MPLS_BOS_NOT_SET; 290 p->tcfm_proto = tb[TCA_MPLS_PROTO] ? nla_get_be16(tb[TCA_MPLS_PROTO]) : 291 htons(ETH_P_MPLS_UC); 292 293 spin_lock_bh(&m->tcf_lock); 294 goto_ch = tcf_action_set_ctrlact(*a, parm->action, goto_ch); 295 p = rcu_replace_pointer(m->mpls_p, p, lockdep_is_held(&m->tcf_lock)); 296 spin_unlock_bh(&m->tcf_lock); 297 298 if (goto_ch) 299 tcf_chain_put_by_act(goto_ch); 300 if (p) 301 kfree_rcu(p, rcu); 302 303 return ret; 304put_chain: 305 if (goto_ch) 306 tcf_chain_put_by_act(goto_ch); 307release_idr: 308 tcf_idr_release(*a, bind); 309 return err; 310} 311 312static void tcf_mpls_cleanup(struct tc_action *a) 313{ 314 struct tcf_mpls *m = to_mpls(a); 315 struct tcf_mpls_params *p; 316 317 p = rcu_dereference_protected(m->mpls_p, 1); 318 if (p) 319 kfree_rcu(p, rcu); 320} 321 322static int tcf_mpls_dump(struct sk_buff *skb, struct tc_action *a, 323 int bind, int ref) 324{ 325 unsigned char *b = skb_tail_pointer(skb); 326 struct tcf_mpls *m = to_mpls(a); 327 struct tcf_mpls_params *p; 328 struct tc_mpls opt = { 329 .index = m->tcf_index, 330 .refcnt = refcount_read(&m->tcf_refcnt) - ref, 331 .bindcnt = atomic_read(&m->tcf_bindcnt) - bind, 332 }; 333 struct tcf_t t; 334 335 spin_lock_bh(&m->tcf_lock); 336 opt.action = m->tcf_action; 337 p = rcu_dereference_protected(m->mpls_p, lockdep_is_held(&m->tcf_lock)); 338 opt.m_action = p->tcfm_action; 339 340 if (nla_put(skb, TCA_MPLS_PARMS, sizeof(opt), &opt)) 341 goto nla_put_failure; 342 343 if (p->tcfm_label != ACT_MPLS_LABEL_NOT_SET && 344 nla_put_u32(skb, TCA_MPLS_LABEL, p->tcfm_label)) 345 goto nla_put_failure; 346 347 if (p->tcfm_tc != ACT_MPLS_TC_NOT_SET && 348 nla_put_u8(skb, TCA_MPLS_TC, p->tcfm_tc)) 349 goto nla_put_failure; 350 351 if (p->tcfm_ttl && nla_put_u8(skb, TCA_MPLS_TTL, p->tcfm_ttl)) 352 goto nla_put_failure; 353 354 if (p->tcfm_bos != ACT_MPLS_BOS_NOT_SET && 355 nla_put_u8(skb, TCA_MPLS_BOS, p->tcfm_bos)) 356 goto nla_put_failure; 357 358 if (nla_put_be16(skb, TCA_MPLS_PROTO, p->tcfm_proto)) 359 goto nla_put_failure; 360 361 tcf_tm_dump(&t, &m->tcf_tm); 362 363 if (nla_put_64bit(skb, TCA_MPLS_TM, sizeof(t), &t, TCA_MPLS_PAD)) 364 goto nla_put_failure; 365 366 spin_unlock_bh(&m->tcf_lock); 367 368 return skb->len; 369 370nla_put_failure: 371 spin_unlock_bh(&m->tcf_lock); 372 nlmsg_trim(skb, b); 373 return -EMSGSIZE; 374} 375 376static int tcf_mpls_walker(struct net *net, struct sk_buff *skb, 377 struct netlink_callback *cb, int type, 378 const struct tc_action_ops *ops, 379 struct netlink_ext_ack *extack) 380{ 381 struct tc_action_net *tn = net_generic(net, mpls_net_id); 382 383 return tcf_generic_walker(tn, skb, cb, type, ops, extack); 384} 385 386static int tcf_mpls_search(struct net *net, struct tc_action **a, u32 index) 387{ 388 struct tc_action_net *tn = net_generic(net, mpls_net_id); 389 390 return tcf_idr_search(tn, a, index); 391} 392 393static struct tc_action_ops act_mpls_ops = { 394 .kind = "mpls", 395 .id = TCA_ID_MPLS, 396 .owner = THIS_MODULE, 397 .act = tcf_mpls_act, 398 .dump = tcf_mpls_dump, 399 .init = tcf_mpls_init, 400 .cleanup = tcf_mpls_cleanup, 401 .walk = tcf_mpls_walker, 402 .lookup = tcf_mpls_search, 403 .size = sizeof(struct tcf_mpls), 404}; 405 406static __net_init int mpls_init_net(struct net *net) 407{ 408 struct tc_action_net *tn = net_generic(net, mpls_net_id); 409 410 return tc_action_net_init(net, tn, &act_mpls_ops); 411} 412 413static void __net_exit mpls_exit_net(struct list_head *net_list) 414{ 415 tc_action_net_exit(net_list, mpls_net_id); 416} 417 418static struct pernet_operations mpls_net_ops = { 419 .init = mpls_init_net, 420 .exit_batch = mpls_exit_net, 421 .id = &mpls_net_id, 422 .size = sizeof(struct tc_action_net), 423}; 424 425static int __init mpls_init_module(void) 426{ 427 return tcf_register_action(&act_mpls_ops, &mpls_net_ops); 428} 429 430static void __exit mpls_cleanup_module(void) 431{ 432 tcf_unregister_action(&act_mpls_ops, &mpls_net_ops); 433} 434 435module_init(mpls_init_module); 436module_exit(mpls_cleanup_module); 437 438MODULE_SOFTDEP("post: mpls_gso"); 439MODULE_AUTHOR("Netronome Systems <oss-drivers@netronome.com>"); 440MODULE_LICENSE("GPL"); 441MODULE_DESCRIPTION("MPLS manipulation actions"); 442