int lowpan_frag_rcv(struct sk_buff *skb, u8 frag_type)
{
struct lowpan_frag_queue *fq;
struct net *net = dev_net(skb->dev);
struct lowpan_802154_cb *cb = lowpan_802154_cb(skb);
struct ieee802154_hdr hdr;
int err;
if (ieee802154_hdr_peek_addrs(skb, &hdr) < 0)
goto err;
err = lowpan_get_cb(skb, frag_type, cb);
if (err < 0)
goto err;
if (frag_type == LOWPAN_DISPATCH_FRAG1) {
err = lowpan_invoke_frag_rx_handlers(skb);
if (err == NET_RX_DROP)
goto err;
}
if (cb->d_size > IPV6_MIN_MTU) {
net_warn_ratelimited("lowpan_frag_rcv: datagram size exceeds MTU\n");
goto err;
}
fq = fq_find(net, cb, &hdr.source, &hdr.dest);
if (fq != NULL) {
int ret;
spin_lock(&fq->q.lock);
ret = lowpan_frag_queue(fq, skb, frag_type);
spin_unlock(&fq->q.lock);
inet_frag_put(&fq->q, &lowpan_frags);
return ret;
}
err:
kfree_skb(skb);
return -1;
}
static int udp_uncompress(struct sk_buff *skb, size_t needed)
{
u8 tmp = 0, val = 0;
struct udphdr uh;
bool fail;
int err;
fail = lowpan_fetch_skb(skb, &tmp, sizeof(tmp));
pr_debug("UDP header uncompression\n");
switch (tmp & LOWPAN_NHC_UDP_CS_P_11) {
case LOWPAN_NHC_UDP_CS_P_00:
fail |= lowpan_fetch_skb(skb, &uh.source, sizeof(uh.source));
fail |= lowpan_fetch_skb(skb, &uh.dest, sizeof(uh.dest));
break;
case LOWPAN_NHC_UDP_CS_P_01:
fail |= lowpan_fetch_skb(skb, &uh.source, sizeof(uh.source));
fail |= lowpan_fetch_skb(skb, &val, sizeof(val));
uh.dest = htons(val + LOWPAN_NHC_UDP_8BIT_PORT);
break;
case LOWPAN_NHC_UDP_CS_P_10:
fail |= lowpan_fetch_skb(skb, &val, sizeof(val));
uh.source = htons(val + LOWPAN_NHC_UDP_8BIT_PORT);
fail |= lowpan_fetch_skb(skb, &uh.dest, sizeof(uh.dest));
break;
case LOWPAN_NHC_UDP_CS_P_11:
fail |= lowpan_fetch_skb(skb, &val, sizeof(val));
uh.source = htons(LOWPAN_NHC_UDP_4BIT_PORT + (val >> 4));
uh.dest = htons(LOWPAN_NHC_UDP_4BIT_PORT + (val & 0x0f));
break;
default:
BUG();
}
pr_debug("uncompressed UDP ports: src = %d, dst = %d\n",
ntohs(uh.source), ntohs(uh.dest));
/* checksum */
if (tmp & LOWPAN_NHC_UDP_CS_C) {
pr_debug_ratelimited("checksum elided currently not supported\n");
fail = true;
} else {
fail |= lowpan_fetch_skb(skb, &uh.check, sizeof(uh.check));
}
if (fail)
return -EINVAL;
/* UDP length needs to be infered from the lower layers
* here, we obtain the hint from the remaining size of the
* frame
*/
switch (lowpan_priv(skb->dev)->lltype) {
case LOWPAN_LLTYPE_IEEE802154:
if (lowpan_802154_cb(skb)->d_size)
uh.len = htons(lowpan_802154_cb(skb)->d_size -
sizeof(struct ipv6hdr));
else
uh.len = htons(skb->len + sizeof(struct udphdr));
break;
default:
uh.len = htons(skb->len + sizeof(struct udphdr));
break;
}
pr_debug("uncompressed UDP length: src = %d", ntohs(uh.len));
/* replace the compressed UDP head by the uncompressed UDP
* header
*/
err = skb_cow(skb, needed);
if (unlikely(err))
return err;
skb_push(skb, sizeof(struct udphdr));
skb_copy_to_linear_data(skb, &uh, sizeof(struct udphdr));
return 0;
}
static int lowpan_frag_queue(struct lowpan_frag_queue *fq,
struct sk_buff *skb, u8 frag_type)
{
struct sk_buff *prev, *next;
struct net_device *ldev;
int end, offset;
if (fq->q.flags & INET_FRAG_COMPLETE)
goto err;
offset = lowpan_802154_cb(skb)->d_offset << 3;
end = lowpan_802154_cb(skb)->d_size;
/* Is this the final fragment? */
if (offset + skb->len == end) {
/* If we already have some bits beyond end
* or have different end, the segment is corrupted.
*/
if (end < fq->q.len ||
((fq->q.flags & INET_FRAG_LAST_IN) && end != fq->q.len))
goto err;
fq->q.flags |= INET_FRAG_LAST_IN;
fq->q.len = end;
} else {
if (end > fq->q.len) {
/* Some bits beyond end -> corruption. */
if (fq->q.flags & INET_FRAG_LAST_IN)
goto err;
fq->q.len = end;
}
}
/* Find out which fragments are in front and at the back of us
* in the chain of fragments so far. We must know where to put
* this fragment, right?
*/
prev = fq->q.fragments_tail;
if (!prev ||
lowpan_802154_cb(prev)->d_offset <
lowpan_802154_cb(skb)->d_offset) {
next = NULL;
goto found;
}
prev = NULL;
for (next = fq->q.fragments; next != NULL; next = next->next) {
if (lowpan_802154_cb(next)->d_offset >=
lowpan_802154_cb(skb)->d_offset)
break; /* bingo! */
prev = next;
}
found:
/* Insert this fragment in the chain of fragments. */
skb->next = next;
if (!next)
fq->q.fragments_tail = skb;
if (prev)
prev->next = skb;
else
fq->q.fragments = skb;
ldev = skb->dev;
if (ldev)
skb->dev = NULL;
fq->q.stamp = skb->tstamp;
if (frag_type == LOWPAN_DISPATCH_FRAG1)
fq->q.flags |= INET_FRAG_FIRST_IN;
fq->q.meat += skb->len;
add_frag_mem_limit(fq->q.net, skb->truesize);
if (fq->q.flags == (INET_FRAG_FIRST_IN | INET_FRAG_LAST_IN) &&
fq->q.meat == fq->q.len) {
int res;
unsigned long orefdst = skb->_skb_refdst;
skb->_skb_refdst = 0UL;
res = lowpan_frag_reasm(fq, prev, ldev);
skb->_skb_refdst = orefdst;
return res;
}
return -1;
err:
kfree_skb(skb);
return -1;
}
