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