nhc.c 5.1 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234
  1. // SPDX-License-Identifier: GPL-2.0-or-later
  2. /*
  3. * 6LoWPAN next header compression
  4. *
  5. * Authors:
  6. * Alexander Aring <aar@pengutronix.de>
  7. */
  8. #include <linux/netdevice.h>
  9. #include <net/ipv6.h>
  10. #include "nhc.h"
  11. static struct rb_root rb_root = RB_ROOT;
  12. static struct lowpan_nhc *lowpan_nexthdr_nhcs[NEXTHDR_MAX + 1];
  13. static DEFINE_SPINLOCK(lowpan_nhc_lock);
  14. static int lowpan_nhc_insert(struct lowpan_nhc *nhc)
  15. {
  16. struct rb_node **new = &rb_root.rb_node, *parent = NULL;
  17. /* Figure out where to put new node */
  18. while (*new) {
  19. struct lowpan_nhc *this = rb_entry(*new, struct lowpan_nhc,
  20. node);
  21. int result, len_dif, len;
  22. len_dif = nhc->idlen - this->idlen;
  23. if (nhc->idlen < this->idlen)
  24. len = nhc->idlen;
  25. else
  26. len = this->idlen;
  27. result = memcmp(nhc->id, this->id, len);
  28. if (!result)
  29. result = len_dif;
  30. parent = *new;
  31. if (result < 0)
  32. new = &((*new)->rb_left);
  33. else if (result > 0)
  34. new = &((*new)->rb_right);
  35. else
  36. return -EEXIST;
  37. }
  38. /* Add new node and rebalance tree. */
  39. rb_link_node(&nhc->node, parent, new);
  40. rb_insert_color(&nhc->node, &rb_root);
  41. return 0;
  42. }
  43. static void lowpan_nhc_remove(struct lowpan_nhc *nhc)
  44. {
  45. rb_erase(&nhc->node, &rb_root);
  46. }
  47. static struct lowpan_nhc *lowpan_nhc_by_nhcid(const struct sk_buff *skb)
  48. {
  49. struct rb_node *node = rb_root.rb_node;
  50. const u8 *nhcid_skb_ptr = skb->data;
  51. while (node) {
  52. struct lowpan_nhc *nhc = rb_entry(node, struct lowpan_nhc,
  53. node);
  54. u8 nhcid_skb_ptr_masked[LOWPAN_NHC_MAX_ID_LEN];
  55. int result, i;
  56. if (nhcid_skb_ptr + nhc->idlen > skb->data + skb->len)
  57. return NULL;
  58. /* copy and mask afterwards the nhid value from skb */
  59. memcpy(nhcid_skb_ptr_masked, nhcid_skb_ptr, nhc->idlen);
  60. for (i = 0; i < nhc->idlen; i++)
  61. nhcid_skb_ptr_masked[i] &= nhc->idmask[i];
  62. result = memcmp(nhcid_skb_ptr_masked, nhc->id, nhc->idlen);
  63. if (result < 0)
  64. node = node->rb_left;
  65. else if (result > 0)
  66. node = node->rb_right;
  67. else
  68. return nhc;
  69. }
  70. return NULL;
  71. }
  72. int lowpan_nhc_check_compression(struct sk_buff *skb,
  73. const struct ipv6hdr *hdr, u8 **hc_ptr)
  74. {
  75. struct lowpan_nhc *nhc;
  76. int ret = 0;
  77. spin_lock_bh(&lowpan_nhc_lock);
  78. nhc = lowpan_nexthdr_nhcs[hdr->nexthdr];
  79. if (!(nhc && nhc->compress))
  80. ret = -ENOENT;
  81. spin_unlock_bh(&lowpan_nhc_lock);
  82. return ret;
  83. }
  84. int lowpan_nhc_do_compression(struct sk_buff *skb, const struct ipv6hdr *hdr,
  85. u8 **hc_ptr)
  86. {
  87. int ret;
  88. struct lowpan_nhc *nhc;
  89. spin_lock_bh(&lowpan_nhc_lock);
  90. nhc = lowpan_nexthdr_nhcs[hdr->nexthdr];
  91. /* check if the nhc module was removed in unlocked part.
  92. * TODO: this is a workaround we should prevent unloading
  93. * of nhc modules while unlocked part, this will always drop
  94. * the lowpan packet but it's very unlikely.
  95. *
  96. * Solution isn't easy because we need to decide at
  97. * lowpan_nhc_check_compression if we do a compression or not.
  98. * Because the inline data which is added to skb, we can't move this
  99. * handling.
  100. */
  101. if (unlikely(!nhc || !nhc->compress)) {
  102. ret = -EINVAL;
  103. goto out;
  104. }
  105. /* In the case of RAW sockets the transport header is not set by
  106. * the ip6 stack so we must set it ourselves
  107. */
  108. if (skb->transport_header == skb->network_header)
  109. skb_set_transport_header(skb, sizeof(struct ipv6hdr));
  110. ret = nhc->compress(skb, hc_ptr);
  111. if (ret < 0)
  112. goto out;
  113. /* skip the transport header */
  114. skb_pull(skb, nhc->nexthdrlen);
  115. out:
  116. spin_unlock_bh(&lowpan_nhc_lock);
  117. return ret;
  118. }
  119. int lowpan_nhc_do_uncompression(struct sk_buff *skb,
  120. const struct net_device *dev,
  121. struct ipv6hdr *hdr)
  122. {
  123. struct lowpan_nhc *nhc;
  124. int ret;
  125. spin_lock_bh(&lowpan_nhc_lock);
  126. nhc = lowpan_nhc_by_nhcid(skb);
  127. if (nhc) {
  128. if (nhc->uncompress) {
  129. ret = nhc->uncompress(skb, sizeof(struct ipv6hdr) +
  130. nhc->nexthdrlen);
  131. if (ret < 0) {
  132. spin_unlock_bh(&lowpan_nhc_lock);
  133. return ret;
  134. }
  135. } else {
  136. spin_unlock_bh(&lowpan_nhc_lock);
  137. netdev_warn(dev, "received nhc id for %s which is not implemented.\n",
  138. nhc->name);
  139. return -ENOTSUPP;
  140. }
  141. } else {
  142. spin_unlock_bh(&lowpan_nhc_lock);
  143. netdev_warn(dev, "received unknown nhc id which was not found.\n");
  144. return -ENOENT;
  145. }
  146. hdr->nexthdr = nhc->nexthdr;
  147. skb_reset_transport_header(skb);
  148. raw_dump_table(__func__, "raw transport header dump",
  149. skb_transport_header(skb), nhc->nexthdrlen);
  150. spin_unlock_bh(&lowpan_nhc_lock);
  151. return 0;
  152. }
  153. int lowpan_nhc_add(struct lowpan_nhc *nhc)
  154. {
  155. int ret;
  156. if (!nhc->idlen || !nhc->idsetup)
  157. return -EINVAL;
  158. WARN_ONCE(nhc->idlen > LOWPAN_NHC_MAX_ID_LEN,
  159. "LOWPAN_NHC_MAX_ID_LEN should be updated to %zd.\n",
  160. nhc->idlen);
  161. nhc->idsetup(nhc);
  162. spin_lock_bh(&lowpan_nhc_lock);
  163. if (lowpan_nexthdr_nhcs[nhc->nexthdr]) {
  164. ret = -EEXIST;
  165. goto out;
  166. }
  167. ret = lowpan_nhc_insert(nhc);
  168. if (ret < 0)
  169. goto out;
  170. lowpan_nexthdr_nhcs[nhc->nexthdr] = nhc;
  171. out:
  172. spin_unlock_bh(&lowpan_nhc_lock);
  173. return ret;
  174. }
  175. EXPORT_SYMBOL(lowpan_nhc_add);
  176. void lowpan_nhc_del(struct lowpan_nhc *nhc)
  177. {
  178. spin_lock_bh(&lowpan_nhc_lock);
  179. lowpan_nhc_remove(nhc);
  180. lowpan_nexthdr_nhcs[nhc->nexthdr] = NULL;
  181. spin_unlock_bh(&lowpan_nhc_lock);
  182. synchronize_net();
  183. }
  184. EXPORT_SYMBOL(lowpan_nhc_del);