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
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);
242