blob: fd20fc51a7c49ffb9c2f0782973df08e1f8535c6 [file] [log] [blame]
Alexander Aring92aa7c62015-01-09 16:42:57 +01001/*
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
20static struct rb_root rb_root = RB_ROOT;
21static struct lowpan_nhc *lowpan_nexthdr_nhcs[NEXTHDR_MAX];
22static DEFINE_SPINLOCK(lowpan_nhc_lock);
23
24static 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
61static void lowpan_nhc_remove(struct lowpan_nhc *nhc)
62{
63 rb_erase(&nhc->node, &rb_root);
64}
65
66static 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
97int 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
117int 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
154out:
155 spin_unlock_bh(&lowpan_nhc_lock);
156
157 return ret;
158}
159
160int 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
199int 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;
224out:
225 spin_unlock_bh(&lowpan_nhc_lock);
226 return ret;
227}
228EXPORT_SYMBOL(lowpan_nhc_add);
229
230void 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}
241EXPORT_SYMBOL(lowpan_nhc_del);