act_mpls.c 11 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435
  1. // SPDX-License-Identifier: (GPL-2.0-only OR BSD-2-Clause)
  2. /* Copyright (C) 2019 Netronome Systems, Inc. */
  3. #include <linux/if_arp.h>
  4. #include <linux/init.h>
  5. #include <linux/kernel.h>
  6. #include <linux/module.h>
  7. #include <linux/mpls.h>
  8. #include <linux/rtnetlink.h>
  9. #include <linux/skbuff.h>
  10. #include <linux/tc_act/tc_mpls.h>
  11. #include <net/mpls.h>
  12. #include <net/netlink.h>
  13. #include <net/pkt_sched.h>
  14. #include <net/pkt_cls.h>
  15. #include <net/tc_act/tc_mpls.h>
  16. static unsigned int mpls_net_id;
  17. static struct tc_action_ops act_mpls_ops;
  18. #define ACT_MPLS_TTL_DEFAULT 255
  19. static __be32 tcf_mpls_get_lse(struct mpls_shim_hdr *lse,
  20. struct tcf_mpls_params *p, bool set_bos)
  21. {
  22. u32 new_lse = 0;
  23. if (lse)
  24. new_lse = be32_to_cpu(lse->label_stack_entry);
  25. if (p->tcfm_label != ACT_MPLS_LABEL_NOT_SET) {
  26. new_lse &= ~MPLS_LS_LABEL_MASK;
  27. new_lse |= p->tcfm_label << MPLS_LS_LABEL_SHIFT;
  28. }
  29. if (p->tcfm_ttl) {
  30. new_lse &= ~MPLS_LS_TTL_MASK;
  31. new_lse |= p->tcfm_ttl << MPLS_LS_TTL_SHIFT;
  32. }
  33. if (p->tcfm_tc != ACT_MPLS_TC_NOT_SET) {
  34. new_lse &= ~MPLS_LS_TC_MASK;
  35. new_lse |= p->tcfm_tc << MPLS_LS_TC_SHIFT;
  36. }
  37. if (p->tcfm_bos != ACT_MPLS_BOS_NOT_SET) {
  38. new_lse &= ~MPLS_LS_S_MASK;
  39. new_lse |= p->tcfm_bos << MPLS_LS_S_SHIFT;
  40. } else if (set_bos) {
  41. new_lse |= 1 << MPLS_LS_S_SHIFT;
  42. }
  43. return cpu_to_be32(new_lse);
  44. }
  45. static int tcf_mpls_act(struct sk_buff *skb, const struct tc_action *a,
  46. struct tcf_result *res)
  47. {
  48. struct tcf_mpls *m = to_mpls(a);
  49. struct tcf_mpls_params *p;
  50. __be32 new_lse;
  51. int ret, mac_len;
  52. tcf_lastuse_update(&m->tcf_tm);
  53. bstats_cpu_update(this_cpu_ptr(m->common.cpu_bstats), skb);
  54. /* Ensure 'data' points at mac_header prior calling mpls manipulating
  55. * functions.
  56. */
  57. if (skb_at_tc_ingress(skb)) {
  58. skb_push_rcsum(skb, skb->mac_len);
  59. mac_len = skb->mac_len;
  60. } else {
  61. mac_len = skb_network_header(skb) - skb_mac_header(skb);
  62. }
  63. ret = READ_ONCE(m->tcf_action);
  64. p = rcu_dereference_bh(m->mpls_p);
  65. switch (p->tcfm_action) {
  66. case TCA_MPLS_ACT_POP:
  67. if (skb_mpls_pop(skb, p->tcfm_proto, mac_len,
  68. skb->dev && skb->dev->type == ARPHRD_ETHER))
  69. goto drop;
  70. break;
  71. case TCA_MPLS_ACT_PUSH:
  72. new_lse = tcf_mpls_get_lse(NULL, p, !eth_p_mpls(skb_protocol(skb, true)));
  73. if (skb_mpls_push(skb, new_lse, p->tcfm_proto, mac_len,
  74. skb->dev && skb->dev->type == ARPHRD_ETHER))
  75. goto drop;
  76. break;
  77. case TCA_MPLS_ACT_MAC_PUSH:
  78. if (skb_vlan_tag_present(skb)) {
  79. if (__vlan_insert_inner_tag(skb, skb->vlan_proto,
  80. skb_vlan_tag_get(skb),
  81. ETH_HLEN) < 0)
  82. goto drop;
  83. skb->protocol = skb->vlan_proto;
  84. __vlan_hwaccel_clear_tag(skb);
  85. }
  86. new_lse = tcf_mpls_get_lse(NULL, p, mac_len ||
  87. !eth_p_mpls(skb->protocol));
  88. if (skb_mpls_push(skb, new_lse, p->tcfm_proto, 0, false))
  89. goto drop;
  90. break;
  91. case TCA_MPLS_ACT_MODIFY:
  92. if (!pskb_may_pull(skb,
  93. skb_network_offset(skb) + MPLS_HLEN))
  94. goto drop;
  95. new_lse = tcf_mpls_get_lse(mpls_hdr(skb), p, false);
  96. if (skb_mpls_update_lse(skb, new_lse))
  97. goto drop;
  98. break;
  99. case TCA_MPLS_ACT_DEC_TTL:
  100. if (skb_mpls_dec_ttl(skb))
  101. goto drop;
  102. break;
  103. }
  104. if (skb_at_tc_ingress(skb))
  105. skb_pull_rcsum(skb, skb->mac_len);
  106. return ret;
  107. drop:
  108. qstats_drop_inc(this_cpu_ptr(m->common.cpu_qstats));
  109. return TC_ACT_SHOT;
  110. }
  111. static int valid_label(const struct nlattr *attr,
  112. struct netlink_ext_ack *extack)
  113. {
  114. const u32 *label = nla_data(attr);
  115. if (*label & ~MPLS_LABEL_MASK || *label == MPLS_LABEL_IMPLNULL) {
  116. NL_SET_ERR_MSG_MOD(extack, "MPLS label out of range");
  117. return -EINVAL;
  118. }
  119. return 0;
  120. }
  121. static const struct nla_policy mpls_policy[TCA_MPLS_MAX + 1] = {
  122. [TCA_MPLS_PARMS] = NLA_POLICY_EXACT_LEN(sizeof(struct tc_mpls)),
  123. [TCA_MPLS_PROTO] = { .type = NLA_U16 },
  124. [TCA_MPLS_LABEL] = NLA_POLICY_VALIDATE_FN(NLA_U32, valid_label),
  125. [TCA_MPLS_TC] = NLA_POLICY_RANGE(NLA_U8, 0, 7),
  126. [TCA_MPLS_TTL] = NLA_POLICY_MIN(NLA_U8, 1),
  127. [TCA_MPLS_BOS] = NLA_POLICY_RANGE(NLA_U8, 0, 1),
  128. };
  129. static int tcf_mpls_init(struct net *net, struct nlattr *nla,
  130. struct nlattr *est, struct tc_action **a,
  131. int ovr, int bind, bool rtnl_held,
  132. struct tcf_proto *tp, u32 flags,
  133. struct netlink_ext_ack *extack)
  134. {
  135. struct tc_action_net *tn = net_generic(net, mpls_net_id);
  136. struct nlattr *tb[TCA_MPLS_MAX + 1];
  137. struct tcf_chain *goto_ch = NULL;
  138. struct tcf_mpls_params *p;
  139. struct tc_mpls *parm;
  140. bool exists = false;
  141. struct tcf_mpls *m;
  142. int ret = 0, err;
  143. u8 mpls_ttl = 0;
  144. u32 index;
  145. if (!nla) {
  146. NL_SET_ERR_MSG_MOD(extack, "Missing netlink attributes");
  147. return -EINVAL;
  148. }
  149. err = nla_parse_nested(tb, TCA_MPLS_MAX, nla, mpls_policy, extack);
  150. if (err < 0)
  151. return err;
  152. if (!tb[TCA_MPLS_PARMS]) {
  153. NL_SET_ERR_MSG_MOD(extack, "No MPLS params");
  154. return -EINVAL;
  155. }
  156. parm = nla_data(tb[TCA_MPLS_PARMS]);
  157. index = parm->index;
  158. /* Verify parameters against action type. */
  159. switch (parm->m_action) {
  160. case TCA_MPLS_ACT_POP:
  161. if (!tb[TCA_MPLS_PROTO]) {
  162. NL_SET_ERR_MSG_MOD(extack, "Protocol must be set for MPLS pop");
  163. return -EINVAL;
  164. }
  165. if (!eth_proto_is_802_3(nla_get_be16(tb[TCA_MPLS_PROTO]))) {
  166. NL_SET_ERR_MSG_MOD(extack, "Invalid protocol type for MPLS pop");
  167. return -EINVAL;
  168. }
  169. if (tb[TCA_MPLS_LABEL] || tb[TCA_MPLS_TTL] || tb[TCA_MPLS_TC] ||
  170. tb[TCA_MPLS_BOS]) {
  171. NL_SET_ERR_MSG_MOD(extack, "Label, TTL, TC or BOS cannot be used with MPLS pop");
  172. return -EINVAL;
  173. }
  174. break;
  175. case TCA_MPLS_ACT_DEC_TTL:
  176. if (tb[TCA_MPLS_PROTO] || tb[TCA_MPLS_LABEL] ||
  177. tb[TCA_MPLS_TTL] || tb[TCA_MPLS_TC] || tb[TCA_MPLS_BOS]) {
  178. NL_SET_ERR_MSG_MOD(extack, "Label, TTL, TC, BOS or protocol cannot be used with MPLS dec_ttl");
  179. return -EINVAL;
  180. }
  181. break;
  182. case TCA_MPLS_ACT_PUSH:
  183. case TCA_MPLS_ACT_MAC_PUSH:
  184. if (!tb[TCA_MPLS_LABEL]) {
  185. NL_SET_ERR_MSG_MOD(extack, "Label is required for MPLS push");
  186. return -EINVAL;
  187. }
  188. if (tb[TCA_MPLS_PROTO] &&
  189. !eth_p_mpls(nla_get_be16(tb[TCA_MPLS_PROTO]))) {
  190. NL_SET_ERR_MSG_MOD(extack, "Protocol must be an MPLS type for MPLS push");
  191. return -EPROTONOSUPPORT;
  192. }
  193. /* Push needs a TTL - if not specified, set a default value. */
  194. if (!tb[TCA_MPLS_TTL]) {
  195. #if IS_ENABLED(CONFIG_MPLS)
  196. mpls_ttl = net->mpls.default_ttl ?
  197. net->mpls.default_ttl : ACT_MPLS_TTL_DEFAULT;
  198. #else
  199. mpls_ttl = ACT_MPLS_TTL_DEFAULT;
  200. #endif
  201. }
  202. break;
  203. case TCA_MPLS_ACT_MODIFY:
  204. if (tb[TCA_MPLS_PROTO]) {
  205. NL_SET_ERR_MSG_MOD(extack, "Protocol cannot be used with MPLS modify");
  206. return -EINVAL;
  207. }
  208. break;
  209. default:
  210. NL_SET_ERR_MSG_MOD(extack, "Unknown MPLS action");
  211. return -EINVAL;
  212. }
  213. err = tcf_idr_check_alloc(tn, &index, a, bind);
  214. if (err < 0)
  215. return err;
  216. exists = err;
  217. if (exists && bind)
  218. return 0;
  219. if (!exists) {
  220. ret = tcf_idr_create(tn, index, est, a,
  221. &act_mpls_ops, bind, true, 0);
  222. if (ret) {
  223. tcf_idr_cleanup(tn, index);
  224. return ret;
  225. }
  226. ret = ACT_P_CREATED;
  227. } else if (!ovr) {
  228. tcf_idr_release(*a, bind);
  229. return -EEXIST;
  230. }
  231. err = tcf_action_check_ctrlact(parm->action, tp, &goto_ch, extack);
  232. if (err < 0)
  233. goto release_idr;
  234. m = to_mpls(*a);
  235. p = kzalloc(sizeof(*p), GFP_KERNEL);
  236. if (!p) {
  237. err = -ENOMEM;
  238. goto put_chain;
  239. }
  240. p->tcfm_action = parm->m_action;
  241. p->tcfm_label = tb[TCA_MPLS_LABEL] ? nla_get_u32(tb[TCA_MPLS_LABEL]) :
  242. ACT_MPLS_LABEL_NOT_SET;
  243. p->tcfm_tc = tb[TCA_MPLS_TC] ? nla_get_u8(tb[TCA_MPLS_TC]) :
  244. ACT_MPLS_TC_NOT_SET;
  245. p->tcfm_ttl = tb[TCA_MPLS_TTL] ? nla_get_u8(tb[TCA_MPLS_TTL]) :
  246. mpls_ttl;
  247. p->tcfm_bos = tb[TCA_MPLS_BOS] ? nla_get_u8(tb[TCA_MPLS_BOS]) :
  248. ACT_MPLS_BOS_NOT_SET;
  249. p->tcfm_proto = tb[TCA_MPLS_PROTO] ? nla_get_be16(tb[TCA_MPLS_PROTO]) :
  250. htons(ETH_P_MPLS_UC);
  251. spin_lock_bh(&m->tcf_lock);
  252. goto_ch = tcf_action_set_ctrlact(*a, parm->action, goto_ch);
  253. p = rcu_replace_pointer(m->mpls_p, p, lockdep_is_held(&m->tcf_lock));
  254. spin_unlock_bh(&m->tcf_lock);
  255. if (goto_ch)
  256. tcf_chain_put_by_act(goto_ch);
  257. if (p)
  258. kfree_rcu(p, rcu);
  259. return ret;
  260. put_chain:
  261. if (goto_ch)
  262. tcf_chain_put_by_act(goto_ch);
  263. release_idr:
  264. tcf_idr_release(*a, bind);
  265. return err;
  266. }
  267. static void tcf_mpls_cleanup(struct tc_action *a)
  268. {
  269. struct tcf_mpls *m = to_mpls(a);
  270. struct tcf_mpls_params *p;
  271. p = rcu_dereference_protected(m->mpls_p, 1);
  272. if (p)
  273. kfree_rcu(p, rcu);
  274. }
  275. static int tcf_mpls_dump(struct sk_buff *skb, struct tc_action *a,
  276. int bind, int ref)
  277. {
  278. unsigned char *b = skb_tail_pointer(skb);
  279. struct tcf_mpls *m = to_mpls(a);
  280. struct tcf_mpls_params *p;
  281. struct tc_mpls opt = {
  282. .index = m->tcf_index,
  283. .refcnt = refcount_read(&m->tcf_refcnt) - ref,
  284. .bindcnt = atomic_read(&m->tcf_bindcnt) - bind,
  285. };
  286. struct tcf_t t;
  287. spin_lock_bh(&m->tcf_lock);
  288. opt.action = m->tcf_action;
  289. p = rcu_dereference_protected(m->mpls_p, lockdep_is_held(&m->tcf_lock));
  290. opt.m_action = p->tcfm_action;
  291. if (nla_put(skb, TCA_MPLS_PARMS, sizeof(opt), &opt))
  292. goto nla_put_failure;
  293. if (p->tcfm_label != ACT_MPLS_LABEL_NOT_SET &&
  294. nla_put_u32(skb, TCA_MPLS_LABEL, p->tcfm_label))
  295. goto nla_put_failure;
  296. if (p->tcfm_tc != ACT_MPLS_TC_NOT_SET &&
  297. nla_put_u8(skb, TCA_MPLS_TC, p->tcfm_tc))
  298. goto nla_put_failure;
  299. if (p->tcfm_ttl && nla_put_u8(skb, TCA_MPLS_TTL, p->tcfm_ttl))
  300. goto nla_put_failure;
  301. if (p->tcfm_bos != ACT_MPLS_BOS_NOT_SET &&
  302. nla_put_u8(skb, TCA_MPLS_BOS, p->tcfm_bos))
  303. goto nla_put_failure;
  304. if (nla_put_be16(skb, TCA_MPLS_PROTO, p->tcfm_proto))
  305. goto nla_put_failure;
  306. tcf_tm_dump(&t, &m->tcf_tm);
  307. if (nla_put_64bit(skb, TCA_MPLS_TM, sizeof(t), &t, TCA_MPLS_PAD))
  308. goto nla_put_failure;
  309. spin_unlock_bh(&m->tcf_lock);
  310. return skb->len;
  311. nla_put_failure:
  312. spin_unlock_bh(&m->tcf_lock);
  313. nlmsg_trim(skb, b);
  314. return -EMSGSIZE;
  315. }
  316. static int tcf_mpls_walker(struct net *net, struct sk_buff *skb,
  317. struct netlink_callback *cb, int type,
  318. const struct tc_action_ops *ops,
  319. struct netlink_ext_ack *extack)
  320. {
  321. struct tc_action_net *tn = net_generic(net, mpls_net_id);
  322. return tcf_generic_walker(tn, skb, cb, type, ops, extack);
  323. }
  324. static int tcf_mpls_search(struct net *net, struct tc_action **a, u32 index)
  325. {
  326. struct tc_action_net *tn = net_generic(net, mpls_net_id);
  327. return tcf_idr_search(tn, a, index);
  328. }
  329. static struct tc_action_ops act_mpls_ops = {
  330. .kind = "mpls",
  331. .id = TCA_ID_MPLS,
  332. .owner = THIS_MODULE,
  333. .act = tcf_mpls_act,
  334. .dump = tcf_mpls_dump,
  335. .init = tcf_mpls_init,
  336. .cleanup = tcf_mpls_cleanup,
  337. .walk = tcf_mpls_walker,
  338. .lookup = tcf_mpls_search,
  339. .size = sizeof(struct tcf_mpls),
  340. };
  341. static __net_init int mpls_init_net(struct net *net)
  342. {
  343. struct tc_action_net *tn = net_generic(net, mpls_net_id);
  344. return tc_action_net_init(net, tn, &act_mpls_ops);
  345. }
  346. static void __net_exit mpls_exit_net(struct list_head *net_list)
  347. {
  348. tc_action_net_exit(net_list, mpls_net_id);
  349. }
  350. static struct pernet_operations mpls_net_ops = {
  351. .init = mpls_init_net,
  352. .exit_batch = mpls_exit_net,
  353. .id = &mpls_net_id,
  354. .size = sizeof(struct tc_action_net),
  355. };
  356. static int __init mpls_init_module(void)
  357. {
  358. return tcf_register_action(&act_mpls_ops, &mpls_net_ops);
  359. }
  360. static void __exit mpls_cleanup_module(void)
  361. {
  362. tcf_unregister_action(&act_mpls_ops, &mpls_net_ops);
  363. }
  364. module_init(mpls_init_module);
  365. module_exit(mpls_cleanup_module);
  366. MODULE_SOFTDEP("post: mpls_gso");
  367. MODULE_AUTHOR("Netronome Systems <oss-drivers@netronome.com>");
  368. MODULE_LICENSE("GPL");
  369. MODULE_DESCRIPTION("MPLS manipulation actions");