forked from Qortal/Brooklyn
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
234 lines
5.1 KiB
234 lines
5.1 KiB
// SPDX-License-Identifier: GPL-2.0-or-later |
|
/* |
|
* 6LoWPAN next header compression |
|
* |
|
* Authors: |
|
* Alexander Aring <[email protected]> |
|
*/ |
|
|
|
#include <linux/netdevice.h> |
|
|
|
#include <net/ipv6.h> |
|
|
|
#include "nhc.h" |
|
|
|
static struct rb_root rb_root = RB_ROOT; |
|
static struct lowpan_nhc *lowpan_nexthdr_nhcs[NEXTHDR_MAX + 1]; |
|
static DEFINE_SPINLOCK(lowpan_nhc_lock); |
|
|
|
static int lowpan_nhc_insert(struct lowpan_nhc *nhc) |
|
{ |
|
struct rb_node **new = &rb_root.rb_node, *parent = NULL; |
|
|
|
/* Figure out where to put new node */ |
|
while (*new) { |
|
struct lowpan_nhc *this = rb_entry(*new, struct lowpan_nhc, |
|
node); |
|
int result, len_dif, len; |
|
|
|
len_dif = nhc->idlen - this->idlen; |
|
|
|
if (nhc->idlen < this->idlen) |
|
len = nhc->idlen; |
|
else |
|
len = this->idlen; |
|
|
|
result = memcmp(nhc->id, this->id, len); |
|
if (!result) |
|
result = len_dif; |
|
|
|
parent = *new; |
|
if (result < 0) |
|
new = &((*new)->rb_left); |
|
else if (result > 0) |
|
new = &((*new)->rb_right); |
|
else |
|
return -EEXIST; |
|
} |
|
|
|
/* Add new node and rebalance tree. */ |
|
rb_link_node(&nhc->node, parent, new); |
|
rb_insert_color(&nhc->node, &rb_root); |
|
|
|
return 0; |
|
} |
|
|
|
static void lowpan_nhc_remove(struct lowpan_nhc *nhc) |
|
{ |
|
rb_erase(&nhc->node, &rb_root); |
|
} |
|
|
|
static struct lowpan_nhc *lowpan_nhc_by_nhcid(const struct sk_buff *skb) |
|
{ |
|
struct rb_node *node = rb_root.rb_node; |
|
const u8 *nhcid_skb_ptr = skb->data; |
|
|
|
while (node) { |
|
struct lowpan_nhc *nhc = rb_entry(node, struct lowpan_nhc, |
|
node); |
|
u8 nhcid_skb_ptr_masked[LOWPAN_NHC_MAX_ID_LEN]; |
|
int result, i; |
|
|
|
if (nhcid_skb_ptr + nhc->idlen > skb->data + skb->len) |
|
return NULL; |
|
|
|
/* copy and mask afterwards the nhid value from skb */ |
|
memcpy(nhcid_skb_ptr_masked, nhcid_skb_ptr, nhc->idlen); |
|
for (i = 0; i < nhc->idlen; i++) |
|
nhcid_skb_ptr_masked[i] &= nhc->idmask[i]; |
|
|
|
result = memcmp(nhcid_skb_ptr_masked, nhc->id, nhc->idlen); |
|
if (result < 0) |
|
node = node->rb_left; |
|
else if (result > 0) |
|
node = node->rb_right; |
|
else |
|
return nhc; |
|
} |
|
|
|
return NULL; |
|
} |
|
|
|
int lowpan_nhc_check_compression(struct sk_buff *skb, |
|
const struct ipv6hdr *hdr, u8 **hc_ptr) |
|
{ |
|
struct lowpan_nhc *nhc; |
|
int ret = 0; |
|
|
|
spin_lock_bh(&lowpan_nhc_lock); |
|
|
|
nhc = lowpan_nexthdr_nhcs[hdr->nexthdr]; |
|
if (!(nhc && nhc->compress)) |
|
ret = -ENOENT; |
|
|
|
spin_unlock_bh(&lowpan_nhc_lock); |
|
|
|
return ret; |
|
} |
|
|
|
int lowpan_nhc_do_compression(struct sk_buff *skb, const struct ipv6hdr *hdr, |
|
u8 **hc_ptr) |
|
{ |
|
int ret; |
|
struct lowpan_nhc *nhc; |
|
|
|
spin_lock_bh(&lowpan_nhc_lock); |
|
|
|
nhc = lowpan_nexthdr_nhcs[hdr->nexthdr]; |
|
/* check if the nhc module was removed in unlocked part. |
|
* TODO: this is a workaround we should prevent unloading |
|
* of nhc modules while unlocked part, this will always drop |
|
* the lowpan packet but it's very unlikely. |
|
* |
|
* Solution isn't easy because we need to decide at |
|
* lowpan_nhc_check_compression if we do a compression or not. |
|
* Because the inline data which is added to skb, we can't move this |
|
* handling. |
|
*/ |
|
if (unlikely(!nhc || !nhc->compress)) { |
|
ret = -EINVAL; |
|
goto out; |
|
} |
|
|
|
/* In the case of RAW sockets the transport header is not set by |
|
* the ip6 stack so we must set it ourselves |
|
*/ |
|
if (skb->transport_header == skb->network_header) |
|
skb_set_transport_header(skb, sizeof(struct ipv6hdr)); |
|
|
|
ret = nhc->compress(skb, hc_ptr); |
|
if (ret < 0) |
|
goto out; |
|
|
|
/* skip the transport header */ |
|
skb_pull(skb, nhc->nexthdrlen); |
|
|
|
out: |
|
spin_unlock_bh(&lowpan_nhc_lock); |
|
|
|
return ret; |
|
} |
|
|
|
int lowpan_nhc_do_uncompression(struct sk_buff *skb, |
|
const struct net_device *dev, |
|
struct ipv6hdr *hdr) |
|
{ |
|
struct lowpan_nhc *nhc; |
|
int ret; |
|
|
|
spin_lock_bh(&lowpan_nhc_lock); |
|
|
|
nhc = lowpan_nhc_by_nhcid(skb); |
|
if (nhc) { |
|
if (nhc->uncompress) { |
|
ret = nhc->uncompress(skb, sizeof(struct ipv6hdr) + |
|
nhc->nexthdrlen); |
|
if (ret < 0) { |
|
spin_unlock_bh(&lowpan_nhc_lock); |
|
return ret; |
|
} |
|
} else { |
|
spin_unlock_bh(&lowpan_nhc_lock); |
|
netdev_warn(dev, "received nhc id for %s which is not implemented.\n", |
|
nhc->name); |
|
return -ENOTSUPP; |
|
} |
|
} else { |
|
spin_unlock_bh(&lowpan_nhc_lock); |
|
netdev_warn(dev, "received unknown nhc id which was not found.\n"); |
|
return -ENOENT; |
|
} |
|
|
|
hdr->nexthdr = nhc->nexthdr; |
|
skb_reset_transport_header(skb); |
|
raw_dump_table(__func__, "raw transport header dump", |
|
skb_transport_header(skb), nhc->nexthdrlen); |
|
|
|
spin_unlock_bh(&lowpan_nhc_lock); |
|
|
|
return 0; |
|
} |
|
|
|
int lowpan_nhc_add(struct lowpan_nhc *nhc) |
|
{ |
|
int ret; |
|
|
|
if (!nhc->idlen || !nhc->idsetup) |
|
return -EINVAL; |
|
|
|
WARN_ONCE(nhc->idlen > LOWPAN_NHC_MAX_ID_LEN, |
|
"LOWPAN_NHC_MAX_ID_LEN should be updated to %zd.\n", |
|
nhc->idlen); |
|
|
|
nhc->idsetup(nhc); |
|
|
|
spin_lock_bh(&lowpan_nhc_lock); |
|
|
|
if (lowpan_nexthdr_nhcs[nhc->nexthdr]) { |
|
ret = -EEXIST; |
|
goto out; |
|
} |
|
|
|
ret = lowpan_nhc_insert(nhc); |
|
if (ret < 0) |
|
goto out; |
|
|
|
lowpan_nexthdr_nhcs[nhc->nexthdr] = nhc; |
|
out: |
|
spin_unlock_bh(&lowpan_nhc_lock); |
|
return ret; |
|
} |
|
EXPORT_SYMBOL(lowpan_nhc_add); |
|
|
|
void lowpan_nhc_del(struct lowpan_nhc *nhc) |
|
{ |
|
spin_lock_bh(&lowpan_nhc_lock); |
|
|
|
lowpan_nhc_remove(nhc); |
|
lowpan_nexthdr_nhcs[nhc->nexthdr] = NULL; |
|
|
|
spin_unlock_bh(&lowpan_nhc_lock); |
|
|
|
synchronize_net(); |
|
} |
|
EXPORT_SYMBOL(lowpan_nhc_del);
|
|
|