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 | */ | |
74 | uh.len = htons(skb->len + sizeof(struct udphdr)); | |
75 | pr_debug("uncompressed UDP length: src = %d", ntohs(uh.len)); | |
76 | ||
77 | /* replace the compressed UDP head by the uncompressed UDP | |
78 | * header | |
79 | */ | |
80 | err = skb_cow(skb, needed); | |
81 | if (unlikely(err)) | |
82 | return err; | |
83 | ||
84 | skb_push(skb, sizeof(struct udphdr)); | |
85 | skb_copy_to_linear_data(skb, &uh, sizeof(struct udphdr)); | |
86 | ||
87 | return 0; | |
88 | } | |
89 | ||
90 | static int udp_compress(struct sk_buff *skb, u8 **hc_ptr) | |
91 | { | |
92 | const struct udphdr *uh = udp_hdr(skb); | |
93 | u8 tmp; | |
94 | ||
95 | if (((ntohs(uh->source) & LOWPAN_NHC_UDP_4BIT_MASK) == | |
96 | LOWPAN_NHC_UDP_4BIT_PORT) && | |
97 | ((ntohs(uh->dest) & LOWPAN_NHC_UDP_4BIT_MASK) == | |
98 | LOWPAN_NHC_UDP_4BIT_PORT)) { | |
99 | pr_debug("UDP header: both ports compression to 4 bits\n"); | |
100 | /* compression value */ | |
101 | tmp = LOWPAN_NHC_UDP_CS_P_11; | |
102 | lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp)); | |
103 | /* source and destination port */ | |
104 | tmp = ntohs(uh->dest) - LOWPAN_NHC_UDP_4BIT_PORT + | |
105 | ((ntohs(uh->source) - LOWPAN_NHC_UDP_4BIT_PORT) << 4); | |
106 | lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp)); | |
107 | } else if ((ntohs(uh->dest) & LOWPAN_NHC_UDP_8BIT_MASK) == | |
108 | LOWPAN_NHC_UDP_8BIT_PORT) { | |
109 | pr_debug("UDP header: remove 8 bits of dest\n"); | |
110 | /* compression value */ | |
111 | tmp = LOWPAN_NHC_UDP_CS_P_01; | |
112 | lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp)); | |
113 | /* source port */ | |
114 | lowpan_push_hc_data(hc_ptr, &uh->source, sizeof(uh->source)); | |
115 | /* destination port */ | |
116 | tmp = ntohs(uh->dest) - LOWPAN_NHC_UDP_8BIT_PORT; | |
117 | lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp)); | |
118 | } else if ((ntohs(uh->source) & LOWPAN_NHC_UDP_8BIT_MASK) == | |
119 | LOWPAN_NHC_UDP_8BIT_PORT) { | |
120 | pr_debug("UDP header: remove 8 bits of source\n"); | |
121 | /* compression value */ | |
122 | tmp = LOWPAN_NHC_UDP_CS_P_10; | |
123 | lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp)); | |
124 | /* source port */ | |
125 | tmp = ntohs(uh->source) - LOWPAN_NHC_UDP_8BIT_PORT; | |
126 | lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp)); | |
127 | /* destination port */ | |
128 | lowpan_push_hc_data(hc_ptr, &uh->dest, sizeof(uh->dest)); | |
129 | } else { | |
130 | pr_debug("UDP header: can't compress\n"); | |
131 | /* compression value */ | |
132 | tmp = LOWPAN_NHC_UDP_CS_P_00; | |
133 | lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp)); | |
134 | /* source port */ | |
135 | lowpan_push_hc_data(hc_ptr, &uh->source, sizeof(uh->source)); | |
136 | /* destination port */ | |
137 | lowpan_push_hc_data(hc_ptr, &uh->dest, sizeof(uh->dest)); | |
138 | } | |
139 | ||
140 | /* checksum is always inline */ | |
141 | lowpan_push_hc_data(hc_ptr, &uh->check, sizeof(uh->check)); | |
142 | ||
143 | return 0; | |
144 | } | |
145 | ||
146 | static void udp_nhid_setup(struct lowpan_nhc *nhc) | |
147 | { | |
148 | nhc->id[0] = LOWPAN_NHC_UDP_ID; | |
149 | nhc->idmask[0] = LOWPAN_NHC_UDP_MASK; | |
150 | } | |
151 | ||
152 | LOWPAN_NHC(nhc_udp, "RFC6282 UDP", NEXTHDR_UDP, sizeof(struct udphdr), | |
153 | udp_nhid_setup, LOWPAN_NHC_UDP_IDLEN, udp_uncompress, udp_compress); | |
154 | ||
155 | module_lowpan_nhc(nhc_udp); | |
156 | MODULE_DESCRIPTION("6LoWPAN next header RFC6282 UDP compression"); | |
157 | MODULE_LICENSE("GPL"); |