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