nhc_udp.c 5.5 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183
  1. // SPDX-License-Identifier: GPL-2.0-or-later
  2. /*
  3. * 6LoWPAN IPv6 UDP compression according to RFC6282
  4. *
  5. * Authors:
  6. * Alexander Aring <aar@pengutronix.de>
  7. *
  8. * Orignal written by:
  9. * Alexander Smirnov <alex.bluesman.smirnov@gmail.com>
  10. * Jon Smirl <jonsmirl@gmail.com>
  11. */
  12. #include "nhc.h"
  13. #define LOWPAN_NHC_UDP_MASK 0xF8
  14. #define LOWPAN_NHC_UDP_ID 0xF0
  15. #define LOWPAN_NHC_UDP_IDLEN 1
  16. #define LOWPAN_NHC_UDP_4BIT_PORT 0xF0B0
  17. #define LOWPAN_NHC_UDP_4BIT_MASK 0xFFF0
  18. #define LOWPAN_NHC_UDP_8BIT_PORT 0xF000
  19. #define LOWPAN_NHC_UDP_8BIT_MASK 0xFF00
  20. /* values for port compression, _with checksum_ ie bit 5 set to 0 */
  21. /* all inline */
  22. #define LOWPAN_NHC_UDP_CS_P_00 0xF0
  23. /* source 16bit inline, dest = 0xF0 + 8 bit inline */
  24. #define LOWPAN_NHC_UDP_CS_P_01 0xF1
  25. /* source = 0xF0 + 8bit inline, dest = 16 bit inline */
  26. #define LOWPAN_NHC_UDP_CS_P_10 0xF2
  27. /* source & dest = 0xF0B + 4bit inline */
  28. #define LOWPAN_NHC_UDP_CS_P_11 0xF3
  29. /* checksum elided */
  30. #define LOWPAN_NHC_UDP_CS_C 0x04
  31. static int udp_uncompress(struct sk_buff *skb, size_t needed)
  32. {
  33. u8 tmp = 0, val = 0;
  34. struct udphdr uh;
  35. bool fail;
  36. int err;
  37. fail = lowpan_fetch_skb(skb, &tmp, sizeof(tmp));
  38. pr_debug("UDP header uncompression\n");
  39. switch (tmp & LOWPAN_NHC_UDP_CS_P_11) {
  40. case LOWPAN_NHC_UDP_CS_P_00:
  41. fail |= lowpan_fetch_skb(skb, &uh.source, sizeof(uh.source));
  42. fail |= lowpan_fetch_skb(skb, &uh.dest, sizeof(uh.dest));
  43. break;
  44. case LOWPAN_NHC_UDP_CS_P_01:
  45. fail |= lowpan_fetch_skb(skb, &uh.source, sizeof(uh.source));
  46. fail |= lowpan_fetch_skb(skb, &val, sizeof(val));
  47. uh.dest = htons(val + LOWPAN_NHC_UDP_8BIT_PORT);
  48. break;
  49. case LOWPAN_NHC_UDP_CS_P_10:
  50. fail |= lowpan_fetch_skb(skb, &val, sizeof(val));
  51. uh.source = htons(val + LOWPAN_NHC_UDP_8BIT_PORT);
  52. fail |= lowpan_fetch_skb(skb, &uh.dest, sizeof(uh.dest));
  53. break;
  54. case LOWPAN_NHC_UDP_CS_P_11:
  55. fail |= lowpan_fetch_skb(skb, &val, sizeof(val));
  56. uh.source = htons(LOWPAN_NHC_UDP_4BIT_PORT + (val >> 4));
  57. uh.dest = htons(LOWPAN_NHC_UDP_4BIT_PORT + (val & 0x0f));
  58. break;
  59. default:
  60. BUG();
  61. }
  62. pr_debug("uncompressed UDP ports: src = %d, dst = %d\n",
  63. ntohs(uh.source), ntohs(uh.dest));
  64. /* checksum */
  65. if (tmp & LOWPAN_NHC_UDP_CS_C) {
  66. pr_debug_ratelimited("checksum elided currently not supported\n");
  67. fail = true;
  68. } else {
  69. fail |= lowpan_fetch_skb(skb, &uh.check, sizeof(uh.check));
  70. }
  71. if (fail)
  72. return -EINVAL;
  73. /* UDP length needs to be infered from the lower layers
  74. * here, we obtain the hint from the remaining size of the
  75. * frame
  76. */
  77. switch (lowpan_dev(skb->dev)->lltype) {
  78. case LOWPAN_LLTYPE_IEEE802154:
  79. if (lowpan_802154_cb(skb)->d_size)
  80. uh.len = htons(lowpan_802154_cb(skb)->d_size -
  81. sizeof(struct ipv6hdr));
  82. else
  83. uh.len = htons(skb->len + sizeof(struct udphdr));
  84. break;
  85. default:
  86. uh.len = htons(skb->len + sizeof(struct udphdr));
  87. break;
  88. }
  89. pr_debug("uncompressed UDP length: src = %d", ntohs(uh.len));
  90. /* replace the compressed UDP head by the uncompressed UDP
  91. * header
  92. */
  93. err = skb_cow(skb, needed);
  94. if (unlikely(err))
  95. return err;
  96. skb_push(skb, sizeof(struct udphdr));
  97. skb_copy_to_linear_data(skb, &uh, sizeof(struct udphdr));
  98. return 0;
  99. }
  100. static int udp_compress(struct sk_buff *skb, u8 **hc_ptr)
  101. {
  102. const struct udphdr *uh = udp_hdr(skb);
  103. u8 tmp;
  104. if (((ntohs(uh->source) & LOWPAN_NHC_UDP_4BIT_MASK) ==
  105. LOWPAN_NHC_UDP_4BIT_PORT) &&
  106. ((ntohs(uh->dest) & LOWPAN_NHC_UDP_4BIT_MASK) ==
  107. LOWPAN_NHC_UDP_4BIT_PORT)) {
  108. pr_debug("UDP header: both ports compression to 4 bits\n");
  109. /* compression value */
  110. tmp = LOWPAN_NHC_UDP_CS_P_11;
  111. lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp));
  112. /* source and destination port */
  113. tmp = ntohs(uh->dest) - LOWPAN_NHC_UDP_4BIT_PORT +
  114. ((ntohs(uh->source) - LOWPAN_NHC_UDP_4BIT_PORT) << 4);
  115. lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp));
  116. } else if ((ntohs(uh->dest) & LOWPAN_NHC_UDP_8BIT_MASK) ==
  117. LOWPAN_NHC_UDP_8BIT_PORT) {
  118. pr_debug("UDP header: remove 8 bits of dest\n");
  119. /* compression value */
  120. tmp = LOWPAN_NHC_UDP_CS_P_01;
  121. lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp));
  122. /* source port */
  123. lowpan_push_hc_data(hc_ptr, &uh->source, sizeof(uh->source));
  124. /* destination port */
  125. tmp = ntohs(uh->dest) - LOWPAN_NHC_UDP_8BIT_PORT;
  126. lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp));
  127. } else if ((ntohs(uh->source) & LOWPAN_NHC_UDP_8BIT_MASK) ==
  128. LOWPAN_NHC_UDP_8BIT_PORT) {
  129. pr_debug("UDP header: remove 8 bits of source\n");
  130. /* compression value */
  131. tmp = LOWPAN_NHC_UDP_CS_P_10;
  132. lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp));
  133. /* source port */
  134. tmp = ntohs(uh->source) - LOWPAN_NHC_UDP_8BIT_PORT;
  135. lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp));
  136. /* destination port */
  137. lowpan_push_hc_data(hc_ptr, &uh->dest, sizeof(uh->dest));
  138. } else {
  139. pr_debug("UDP header: can't compress\n");
  140. /* compression value */
  141. tmp = LOWPAN_NHC_UDP_CS_P_00;
  142. lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp));
  143. /* source port */
  144. lowpan_push_hc_data(hc_ptr, &uh->source, sizeof(uh->source));
  145. /* destination port */
  146. lowpan_push_hc_data(hc_ptr, &uh->dest, sizeof(uh->dest));
  147. }
  148. /* checksum is always inline */
  149. lowpan_push_hc_data(hc_ptr, &uh->check, sizeof(uh->check));
  150. return 0;
  151. }
  152. static void udp_nhid_setup(struct lowpan_nhc *nhc)
  153. {
  154. nhc->id[0] = LOWPAN_NHC_UDP_ID;
  155. nhc->idmask[0] = LOWPAN_NHC_UDP_MASK;
  156. }
  157. LOWPAN_NHC(nhc_udp, "RFC6282 UDP", NEXTHDR_UDP, sizeof(struct udphdr),
  158. udp_nhid_setup, LOWPAN_NHC_UDP_IDLEN, udp_uncompress, udp_compress);
  159. module_lowpan_nhc(nhc_udp);
  160. MODULE_DESCRIPTION("6LoWPAN next header RFC6282 UDP compression");
  161. MODULE_LICENSE("GPL");