Commit | Line | Data |
---|---|---|
cc6ed268 AA |
1 | /* |
2 | * 6LoWPAN IPv6 UDP compression according to RFC6282 | |
3 | * | |
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 | * This program is free software; you can redistribute it and/or | |
13 | * modify it under the terms of the GNU General Public License | |
14 | * as published by the Free Software Foundation; either version | |
15 | * 2 of the License, or (at your option) any later version. | |
16 | */ | |
17 | ||
18 | #include "nhc.h" | |
19 | ||
20 | #define LOWPAN_NHC_UDP_IDLEN 1 | |
21 | ||
22 | static int udp_uncompress(struct sk_buff *skb, size_t needed) | |
23 | { | |
24 | u8 tmp = 0, val = 0; | |
25 | struct udphdr uh; | |
26 | bool fail; | |
27 | int err; | |
28 | ||
29 | fail = lowpan_fetch_skb(skb, &tmp, sizeof(tmp)); | |
30 | ||
31 | pr_debug("UDP header uncompression\n"); | |
32 | switch (tmp & LOWPAN_NHC_UDP_CS_P_11) { | |
33 | case LOWPAN_NHC_UDP_CS_P_00: | |
34 | fail |= lowpan_fetch_skb(skb, &uh.source, sizeof(uh.source)); | |
35 | fail |= lowpan_fetch_skb(skb, &uh.dest, sizeof(uh.dest)); | |
36 | break; | |
37 | case LOWPAN_NHC_UDP_CS_P_01: | |
38 | fail |= lowpan_fetch_skb(skb, &uh.source, sizeof(uh.source)); | |
39 | fail |= lowpan_fetch_skb(skb, &val, sizeof(val)); | |
40 | uh.dest = htons(val + LOWPAN_NHC_UDP_8BIT_PORT); | |
41 | break; | |
42 | case LOWPAN_NHC_UDP_CS_P_10: | |
43 | fail |= lowpan_fetch_skb(skb, &val, sizeof(val)); | |
44 | uh.source = htons(val + LOWPAN_NHC_UDP_8BIT_PORT); | |
45 | fail |= lowpan_fetch_skb(skb, &uh.dest, sizeof(uh.dest)); | |
46 | break; | |
47 | case LOWPAN_NHC_UDP_CS_P_11: | |
48 | fail |= lowpan_fetch_skb(skb, &val, sizeof(val)); | |
49 | uh.source = htons(LOWPAN_NHC_UDP_4BIT_PORT + (val >> 4)); | |
50 | uh.dest = htons(LOWPAN_NHC_UDP_4BIT_PORT + (val & 0x0f)); | |
51 | break; | |
52 | default: | |
53 | BUG(); | |
54 | } | |
55 | ||
56 | pr_debug("uncompressed UDP ports: src = %d, dst = %d\n", | |
57 | ntohs(uh.source), ntohs(uh.dest)); | |
58 | ||
59 | /* checksum */ | |
60 | if (tmp & LOWPAN_NHC_UDP_CS_C) { | |
61 | pr_debug_ratelimited("checksum elided currently not supported\n"); | |
62 | fail = true; | |
63 | } else { | |
64 | fail |= lowpan_fetch_skb(skb, &uh.check, sizeof(uh.check)); | |
65 | } | |
66 | ||
67 | if (fail) | |
68 | return -EINVAL; | |
69 | ||
70 | /* UDP length needs to be infered from the lower layers | |
71 | * here, we obtain the hint from the remaining size of the | |
72 | * frame | |
73 | */ | |
72a5e6bb AA |
74 | switch (lowpan_priv(skb->dev)->lltype) { |
75 | case LOWPAN_LLTYPE_IEEE802154: | |
76 | if (lowpan_802154_cb(skb)->d_size) | |
77 | uh.len = htons(lowpan_802154_cb(skb)->d_size - | |
78 | sizeof(struct ipv6hdr)); | |
79 | else | |
80 | uh.len = htons(skb->len + sizeof(struct udphdr)); | |
81 | break; | |
82 | default: | |
83 | uh.len = htons(skb->len + sizeof(struct udphdr)); | |
84 | break; | |
85 | } | |
cc6ed268 AA |
86 | pr_debug("uncompressed UDP length: src = %d", ntohs(uh.len)); |
87 | ||
88 | /* replace the compressed UDP head by the uncompressed UDP | |
89 | * header | |
90 | */ | |
91 | err = skb_cow(skb, needed); | |
92 | if (unlikely(err)) | |
93 | return err; | |
94 | ||
95 | skb_push(skb, sizeof(struct udphdr)); | |
96 | skb_copy_to_linear_data(skb, &uh, sizeof(struct udphdr)); | |
97 | ||
98 | return 0; | |
99 | } | |
100 | ||
101 | static int udp_compress(struct sk_buff *skb, u8 **hc_ptr) | |
102 | { | |
103 | const struct udphdr *uh = udp_hdr(skb); | |
104 | u8 tmp; | |
105 | ||
106 | if (((ntohs(uh->source) & LOWPAN_NHC_UDP_4BIT_MASK) == | |
107 | LOWPAN_NHC_UDP_4BIT_PORT) && | |
108 | ((ntohs(uh->dest) & LOWPAN_NHC_UDP_4BIT_MASK) == | |
109 | LOWPAN_NHC_UDP_4BIT_PORT)) { | |
110 | pr_debug("UDP header: both ports compression to 4 bits\n"); | |
111 | /* compression value */ | |
112 | tmp = LOWPAN_NHC_UDP_CS_P_11; | |
113 | lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp)); | |
114 | /* source and destination port */ | |
115 | tmp = ntohs(uh->dest) - LOWPAN_NHC_UDP_4BIT_PORT + | |
116 | ((ntohs(uh->source) - LOWPAN_NHC_UDP_4BIT_PORT) << 4); | |
117 | lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp)); | |
118 | } else if ((ntohs(uh->dest) & LOWPAN_NHC_UDP_8BIT_MASK) == | |
119 | LOWPAN_NHC_UDP_8BIT_PORT) { | |
120 | pr_debug("UDP header: remove 8 bits of dest\n"); | |
121 | /* compression value */ | |
122 | tmp = LOWPAN_NHC_UDP_CS_P_01; | |
123 | lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp)); | |
124 | /* source port */ | |
125 | lowpan_push_hc_data(hc_ptr, &uh->source, sizeof(uh->source)); | |
126 | /* destination port */ | |
127 | tmp = ntohs(uh->dest) - LOWPAN_NHC_UDP_8BIT_PORT; | |
128 | lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp)); | |
129 | } else if ((ntohs(uh->source) & LOWPAN_NHC_UDP_8BIT_MASK) == | |
130 | LOWPAN_NHC_UDP_8BIT_PORT) { | |
131 | pr_debug("UDP header: remove 8 bits of source\n"); | |
132 | /* compression value */ | |
133 | tmp = LOWPAN_NHC_UDP_CS_P_10; | |
134 | lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp)); | |
135 | /* source port */ | |
136 | tmp = ntohs(uh->source) - LOWPAN_NHC_UDP_8BIT_PORT; | |
137 | lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp)); | |
138 | /* destination port */ | |
139 | lowpan_push_hc_data(hc_ptr, &uh->dest, sizeof(uh->dest)); | |
140 | } else { | |
141 | pr_debug("UDP header: can't compress\n"); | |
142 | /* compression value */ | |
143 | tmp = LOWPAN_NHC_UDP_CS_P_00; | |
144 | lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp)); | |
145 | /* source port */ | |
146 | lowpan_push_hc_data(hc_ptr, &uh->source, sizeof(uh->source)); | |
147 | /* destination port */ | |
148 | lowpan_push_hc_data(hc_ptr, &uh->dest, sizeof(uh->dest)); | |
149 | } | |
150 | ||
151 | /* checksum is always inline */ | |
152 | lowpan_push_hc_data(hc_ptr, &uh->check, sizeof(uh->check)); | |
153 | ||
154 | return 0; | |
155 | } | |
156 | ||
157 | static void udp_nhid_setup(struct lowpan_nhc *nhc) | |
158 | { | |
159 | nhc->id[0] = LOWPAN_NHC_UDP_ID; | |
160 | nhc->idmask[0] = LOWPAN_NHC_UDP_MASK; | |
161 | } | |
162 | ||
163 | LOWPAN_NHC(nhc_udp, "RFC6282 UDP", NEXTHDR_UDP, sizeof(struct udphdr), | |
164 | udp_nhid_setup, LOWPAN_NHC_UDP_IDLEN, udp_uncompress, udp_compress); | |
165 | ||
166 | module_lowpan_nhc(nhc_udp); | |
167 | MODULE_DESCRIPTION("6LoWPAN next header RFC6282 UDP compression"); | |
168 | MODULE_LICENSE("GPL"); |