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