nsh.c 3.2 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150
  1. // SPDX-License-Identifier: GPL-2.0-only
  2. /*
  3. * Network Service Header
  4. *
  5. * Copyright (c) 2017 Red Hat, Inc. -- Jiri Benc <[email protected]>
  6. */
  7. #include <linux/module.h>
  8. #include <linux/netdevice.h>
  9. #include <linux/skbuff.h>
  10. #include <net/nsh.h>
  11. #include <net/tun_proto.h>
  12. int nsh_push(struct sk_buff *skb, const struct nshhdr *pushed_nh)
  13. {
  14. struct nshhdr *nh;
  15. size_t length = nsh_hdr_len(pushed_nh);
  16. u8 next_proto;
  17. if (skb->mac_len) {
  18. next_proto = TUN_P_ETHERNET;
  19. } else {
  20. next_proto = tun_p_from_eth_p(skb->protocol);
  21. if (!next_proto)
  22. return -EAFNOSUPPORT;
  23. }
  24. /* Add the NSH header */
  25. if (skb_cow_head(skb, length) < 0)
  26. return -ENOMEM;
  27. skb_push(skb, length);
  28. nh = (struct nshhdr *)(skb->data);
  29. memcpy(nh, pushed_nh, length);
  30. nh->np = next_proto;
  31. skb_postpush_rcsum(skb, nh, length);
  32. skb->protocol = htons(ETH_P_NSH);
  33. skb_reset_mac_header(skb);
  34. skb_reset_network_header(skb);
  35. skb_reset_mac_len(skb);
  36. return 0;
  37. }
  38. EXPORT_SYMBOL_GPL(nsh_push);
  39. int nsh_pop(struct sk_buff *skb)
  40. {
  41. struct nshhdr *nh;
  42. size_t length;
  43. __be16 inner_proto;
  44. if (!pskb_may_pull(skb, NSH_BASE_HDR_LEN))
  45. return -ENOMEM;
  46. nh = (struct nshhdr *)(skb->data);
  47. length = nsh_hdr_len(nh);
  48. if (length < NSH_BASE_HDR_LEN)
  49. return -EINVAL;
  50. inner_proto = tun_p_to_eth_p(nh->np);
  51. if (!pskb_may_pull(skb, length))
  52. return -ENOMEM;
  53. if (!inner_proto)
  54. return -EAFNOSUPPORT;
  55. skb_pull_rcsum(skb, length);
  56. skb_reset_mac_header(skb);
  57. skb_reset_network_header(skb);
  58. skb_reset_mac_len(skb);
  59. skb->protocol = inner_proto;
  60. return 0;
  61. }
  62. EXPORT_SYMBOL_GPL(nsh_pop);
  63. static struct sk_buff *nsh_gso_segment(struct sk_buff *skb,
  64. netdev_features_t features)
  65. {
  66. struct sk_buff *segs = ERR_PTR(-EINVAL);
  67. u16 mac_offset = skb->mac_header;
  68. unsigned int nsh_len, mac_len;
  69. __be16 proto;
  70. skb_reset_network_header(skb);
  71. mac_len = skb->mac_len;
  72. if (unlikely(!pskb_may_pull(skb, NSH_BASE_HDR_LEN)))
  73. goto out;
  74. nsh_len = nsh_hdr_len(nsh_hdr(skb));
  75. if (nsh_len < NSH_BASE_HDR_LEN)
  76. goto out;
  77. if (unlikely(!pskb_may_pull(skb, nsh_len)))
  78. goto out;
  79. proto = tun_p_to_eth_p(nsh_hdr(skb)->np);
  80. if (!proto)
  81. goto out;
  82. __skb_pull(skb, nsh_len);
  83. skb_reset_mac_header(skb);
  84. skb->mac_len = proto == htons(ETH_P_TEB) ? ETH_HLEN : 0;
  85. skb->protocol = proto;
  86. features &= NETIF_F_SG;
  87. segs = skb_mac_gso_segment(skb, features);
  88. if (IS_ERR_OR_NULL(segs)) {
  89. skb_gso_error_unwind(skb, htons(ETH_P_NSH), nsh_len,
  90. mac_offset, mac_len);
  91. goto out;
  92. }
  93. for (skb = segs; skb; skb = skb->next) {
  94. skb->protocol = htons(ETH_P_NSH);
  95. __skb_push(skb, nsh_len);
  96. skb->mac_header = mac_offset;
  97. skb->network_header = skb->mac_header + mac_len;
  98. skb->mac_len = mac_len;
  99. }
  100. out:
  101. return segs;
  102. }
  103. static struct packet_offload nsh_packet_offload __read_mostly = {
  104. .type = htons(ETH_P_NSH),
  105. .priority = 15,
  106. .callbacks = {
  107. .gso_segment = nsh_gso_segment,
  108. },
  109. };
  110. static int __init nsh_init_module(void)
  111. {
  112. dev_add_offload(&nsh_packet_offload);
  113. return 0;
  114. }
  115. static void __exit nsh_cleanup_module(void)
  116. {
  117. dev_remove_offload(&nsh_packet_offload);
  118. }
  119. module_init(nsh_init_module);
  120. module_exit(nsh_cleanup_module);
  121. MODULE_AUTHOR("Jiri Benc <[email protected]>");
  122. MODULE_DESCRIPTION("NSH protocol");
  123. MODULE_LICENSE("GPL v2");