Linux Audio

Check our new training course

Loading...
v5.14.15
  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);
v5.9
  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);