1648 lines
56 KiB
C
1648 lines
56 KiB
C
/*********************************************************************
|
|
PicoTCP. Copyright (c) 2012-2017 Altran Intelligent Systems. Some rights reserved.
|
|
See LICENSE and COPYING for usage.
|
|
|
|
Authors: Jelle De Vleeschouwer
|
|
*********************************************************************/
|
|
|
|
#include "pico_udp.h"
|
|
#include "pico_ipv6.h"
|
|
#include "pico_stack.h"
|
|
#include "pico_frame.h"
|
|
#include "pico_6lowpan.h"
|
|
#include "pico_protocol.h"
|
|
#include "pico_addressing.h"
|
|
#include "pico_6lowpan_ll.h"
|
|
|
|
#ifdef PICO_SUPPORT_6LOWPAN
|
|
|
|
/*******************************************************************************
|
|
* Macros
|
|
******************************************************************************/
|
|
|
|
#ifdef DEBUG_6LOWPAN
|
|
#define GRN "\x1b[32m"
|
|
#define ORG "\x1b[33m"
|
|
#define RST "\x1b[0m"
|
|
#define lp_dbg dbg
|
|
#else
|
|
#define lp_dbg(...) do {} while(0)
|
|
#endif
|
|
|
|
#define IPV6_MCAST_48(addr) (!addr[8] && !addr[9] && !addr[10] && (addr[11] || addr[12]))
|
|
#define IPV6_MCAST_32(addr) (!addr[8] && !addr[9] && !addr[10] && !addr[11] && !addr[12] && (addr[13] || addr[14]))
|
|
#define IPV6_MCAST_8(addr) (addr[1] == 0x02 && !addr[14] && addr[15])
|
|
#define PORT_COMP(a, mask, b) (((a) & (mask)) == (b))
|
|
|
|
/*******************************************************************************
|
|
* Constants
|
|
******************************************************************************/
|
|
|
|
#define NUM_IPV6_FIELDS (6)
|
|
#define NUM_UDP_FIELDS (4)
|
|
#define IPV6_DISPATCH (0x41)
|
|
#define IPHC_DISPATCH (0x60)
|
|
#define UDP_DISPATCH (0xF0)
|
|
#define EXT_DISPATCH (0xE0)
|
|
#define EXT_HOPBYHOP (0x00)
|
|
#define EXT_ROUTING (0x02)
|
|
#define EXT_FRAG (0x04)
|
|
#define EXT_DSTOPT (0x06)
|
|
#define EXT_COMPRESSED_NH (0x01)
|
|
#define UDP_COMPRESSED_DST (0x01)
|
|
#define UDP_COMPRESSED_SRC (0x02)
|
|
#define UDP_COMPRESSED_BOTH (0x03)
|
|
#define UDP_COMPRESSED_CHCK (0x04)
|
|
#define TF_INLINE (0x00)
|
|
#define TF_ELIDED_DSCP (0x08)
|
|
#define TF_ELIDED_FL (0x10)
|
|
#define TF_ELIDED (0x18)
|
|
#define NH_COMPRESSED (0x04)
|
|
#define HL_COMPRESSED_1 (0x01)
|
|
#define HL_COMPRESSED_64 (0x02)
|
|
#define HL_COMPRESSED_255 (0x03)
|
|
#define CTX_EXTENSION (0x80)
|
|
#define SRC_SHIFT (0x04)
|
|
#define SRC_STATEFUL (0x40)
|
|
#define SRC_COMPRESSED_64 (0x10)
|
|
#define SRC_COMPRESSED_16 (0x20)
|
|
#define SRC_COMPRESSED (0x30)
|
|
#define DST_STATEFUL (0x04)
|
|
#define DST_COMPRESSED_64 (0x01)
|
|
#define DST_COMPRESSED_16 (0x02)
|
|
#define DST_COMPRESSED (0x03)
|
|
#define DST_MULTICAST (0x08)
|
|
#define DST_MCAST_48 (0x01)
|
|
#define DST_MCAST_32 (0x02)
|
|
#define DST_MCAST_8 (0x03)
|
|
#define COMP_LINKLOCAL (0)
|
|
#define COMP_STATELESS (-1)
|
|
#define COMP_MULTICAST (-2)
|
|
#define COMP_UNSPECIFIED (-3)
|
|
#define FRAG1_SIZE (4)
|
|
#define FRAGN_SIZE (5)
|
|
#define FRAG1_DISPATCH (0xC0)
|
|
#define FRAGN_DISPATCH (0xE0)
|
|
#define FRAG_TIMEOUT (5)
|
|
/*******************************************************************************
|
|
* Type definitions
|
|
******************************************************************************/
|
|
|
|
struct hdr_field
|
|
{
|
|
int8_t ori_size;
|
|
int8_t (* compress)(uint8_t *, uint8_t *, uint8_t *, union pico_ll_addr *, union pico_ll_addr *, struct pico_device *);
|
|
int8_t (* decompress)(uint8_t *, uint8_t *, uint8_t *, union pico_ll_addr *, union pico_ll_addr *, struct pico_device *);
|
|
};
|
|
|
|
struct frag_ctx {
|
|
struct pico_frame *f;
|
|
uint16_t dgram_size;
|
|
uint16_t dgram_tag;
|
|
uint8_t dgram_off;
|
|
uint16_t copied;
|
|
uint32_t hash;
|
|
pico_time timestamp;
|
|
};
|
|
|
|
/*******************************************************************************
|
|
* Global Variables
|
|
******************************************************************************/
|
|
|
|
static struct pico_queue pico_6lowpan_in = {
|
|
0
|
|
};
|
|
static struct pico_queue pico_6lowpan_out = {
|
|
0
|
|
};
|
|
|
|
static uint16_t dgram_tag = 0;
|
|
|
|
/*******************************************************************************
|
|
* Private functions
|
|
******************************************************************************/
|
|
|
|
/* Copies two memory buffers but also considers overlapping buffers */
|
|
static void
|
|
buf_move(void *dst, const void *src, size_t len)
|
|
{
|
|
uint8_t *d = (uint8_t *)dst;
|
|
const uint8_t *s = (const uint8_t *)src;
|
|
if (!dst || !src) {
|
|
return;
|
|
} else {
|
|
if (d < s) {
|
|
while (len--)
|
|
*d++ = *s++;
|
|
} else {
|
|
s = s + len - 1;
|
|
d = d + len - 1;
|
|
while (len--)
|
|
*d-- = *s--;
|
|
}
|
|
}
|
|
}
|
|
|
|
/*******************************************************************************
|
|
* Frags
|
|
******************************************************************************/
|
|
|
|
/* Compares two fragmentation cookies based on the hash */
|
|
static int32_t
|
|
frag_ctx_cmp(void *a, void *b)
|
|
{
|
|
struct frag_ctx *fa = (struct frag_ctx *)a;
|
|
struct frag_ctx *fb = (struct frag_ctx *)b;
|
|
return (int32_t)(fa->hash - fb->hash);
|
|
}
|
|
|
|
/* Compares two fragmentation cookies according to RFC4944 5.3 */
|
|
static int32_t
|
|
frag_cmp(void *a, void *b)
|
|
{
|
|
struct frag_ctx *fa = (struct frag_ctx *)a;
|
|
struct frag_ctx *fb = (struct frag_ctx *)b;
|
|
int32_t ret = 0;
|
|
if (fa->dgram_size != fb->dgram_size) {
|
|
return (int32_t)(fa->dgram_size - fb->dgram_size);
|
|
} else if (fa->dgram_tag != fb->dgram_tag) {
|
|
return (int32_t)(fa->dgram_tag - fb->dgram_tag);
|
|
} else {
|
|
if ((ret = pico_6lowpan_lls[fa->f->dev->mode].addr_cmp(&fa->f->src, &fb->f->src))) {
|
|
return ret;
|
|
} else {
|
|
return pico_6lowpan_lls[fa->f->dev->mode].addr_cmp(&fa->f->dst, &fb->f->dst);
|
|
}
|
|
}
|
|
}
|
|
|
|
PICO_TREE_DECLARE(FragTree, &frag_ctx_cmp);
|
|
PICO_TREE_DECLARE(ReassemblyTree, &frag_cmp);
|
|
|
|
/* Find a fragmentation cookie for transmission of subsequent fragments */
|
|
static struct frag_ctx *
|
|
frag_ctx_find(uint32_t hash)
|
|
{
|
|
struct frag_ctx f = { .hash = hash };
|
|
return pico_tree_findKey(&FragTree, &f);
|
|
}
|
|
|
|
/* Reassembly timeout function, deletes */
|
|
static void
|
|
frag_timeout(pico_time now, void *arg)
|
|
{
|
|
struct pico_tree_node *i = NULL, *next = NULL;
|
|
struct frag_ctx *key = NULL;
|
|
IGNORE_PARAMETER(arg);
|
|
pico_tree_foreach_safe(i, &ReassemblyTree, next) {
|
|
if ((key = i->keyValue)) {
|
|
if ((pico_time)(FRAG_TIMEOUT * 1000) <= (now - key->timestamp)) {
|
|
lp_dbg("Timeout for reassembly: %d\n", key->dgram_tag);
|
|
pico_tree_delete(&ReassemblyTree, key);
|
|
pico_frame_discard(key->f);
|
|
PICO_FREE(key);
|
|
}
|
|
}
|
|
}
|
|
|
|
/* If adding a timer fails, there's not really an easy way to recover, so abort all ongoing
|
|
* reassemblies
|
|
* TODO: Maybe using a global variable allows recovering from this situation */
|
|
if (0 == pico_timer_add(1000, frag_timeout, NULL)) {
|
|
lp_dbg("6LP: Failed to set reassembly timeout! Aborting all ongoing reassemblies...\n");
|
|
pico_tree_foreach_safe(i, &ReassemblyTree, next) {
|
|
if ((key = i->keyValue)) {
|
|
pico_tree_delete(&ReassemblyTree, key);
|
|
pico_frame_discard(key->f);
|
|
PICO_FREE(key);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/* Finds a reassembly cookie in the reassembly-tree */
|
|
static struct frag_ctx *
|
|
frag_find(uint16_t dgram_size, uint16_t tag, struct pico_frame *frame)
|
|
{
|
|
struct frag_ctx f = {.f = frame, .dgram_size = dgram_size, .dgram_tag = tag};
|
|
return pico_tree_findKey(&ReassemblyTree, &f);
|
|
}
|
|
|
|
/* Stores a fragmentation cookie in either the fragmentetion cookie tree or
|
|
* in the reassembly tree */
|
|
static int32_t
|
|
frag_store(struct pico_frame *f, uint16_t dgram_size, uint16_t tag,
|
|
uint8_t dgram_off, uint16_t copied, struct pico_tree *tree)
|
|
{
|
|
struct frag_ctx *fr = PICO_ZALLOC(sizeof(struct frag_ctx));
|
|
if (fr) {
|
|
fr->f = f;
|
|
fr->dgram_size = dgram_size;
|
|
fr->dgram_off = dgram_off;
|
|
fr->dgram_tag = tag;
|
|
fr->copied = copied;
|
|
fr->timestamp = PICO_TIME_MS();
|
|
if (&FragTree == tree) {
|
|
fr->hash = pico_hash((void *)fr, sizeof(struct frag_ctx));
|
|
f->hash = fr->hash; // Also set hash in frame so we can identify it
|
|
lp_dbg("6LP: START: "ORG"fragmentation"RST" with hash '%X' of %u bytes.\n", fr->hash, f->len);
|
|
} else {
|
|
lp_dbg("6LP: START: "GRN"reassembly"RST" with tag '%d' of %u bytes.\n", tag, dgram_size);
|
|
}
|
|
/* Insert the cookie in the appropriate tree (FragTree/ReassemblyTree) */
|
|
if (pico_tree_insert(tree, fr)) {
|
|
PICO_FREE(fr);
|
|
return -1;
|
|
}
|
|
} else {
|
|
return (-1);
|
|
}
|
|
return (1); // Succes for 'proto_loop_out'
|
|
}
|
|
|
|
/*******************************************************************************
|
|
* IPHC
|
|
******************************************************************************/
|
|
|
|
#ifdef PICO_6LOWPAN_IPHC_ENABLED
|
|
|
|
/* Compresses the VTF-field of an IPv6 header */
|
|
static int8_t
|
|
compressor_vtf(uint8_t *ori, uint8_t *comp, uint8_t *iphc, union pico_ll_addr *
|
|
llsrc, union pico_ll_addr *lldst, struct pico_device *dev)
|
|
{
|
|
uint8_t ecn = 0, dscp = 0;
|
|
uint32_t fl = 0;
|
|
*ori &= 0x0F; // Clear version field
|
|
*iphc &= (uint8_t)0x07; // Clear IPHC field
|
|
*iphc |= (uint8_t)IPHC_DISPATCH;
|
|
IGNORE_PARAMETER(llsrc);
|
|
IGNORE_PARAMETER(lldst);
|
|
IGNORE_PARAMETER(dev);
|
|
|
|
/* Don't worry... */
|
|
ecn = (uint8_t)((ori[0] << 4) & 0xC0);
|
|
dscp = (uint8_t)(((ori[0] << 4) & 0x30) | ((ori[1] & 0xF0) >> 4));
|
|
fl = long_be((uint32_t)(ori[1] & 0x0F) << 16);
|
|
fl += long_be((uint32_t)(ori[2] & 0xFF) << 8);
|
|
fl += long_be((uint32_t)(ori[3] & 0xFF));
|
|
|
|
if (fl) {
|
|
if (!dscp) { // Flow label carried in-line
|
|
*iphc |= TF_ELIDED_DSCP;
|
|
comp[0] = (uint8_t)(ecn | (ori[1] & 0x0F));
|
|
comp[1] = ori[2];
|
|
comp[2] = ori[3];
|
|
return 3;
|
|
} else { // Traffic class and flow label carried in-line
|
|
*iphc |= TF_INLINE;
|
|
*comp = ecn | dscp;
|
|
comp[1] = ori[1] & 0x0F;
|
|
comp[2] = ori[2];
|
|
comp[3] = ori[3];
|
|
return 4;
|
|
}
|
|
} else if (ecn || dscp) { // Traffic class carried in-line
|
|
*iphc |= TF_ELIDED_FL;
|
|
*comp = ecn | dscp;
|
|
return 1;
|
|
} else { // Traffic class and flow label elided
|
|
*iphc |= TF_ELIDED;
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
/* Decompresses the VTF-field of a IPHC-header */
|
|
static int8_t
|
|
decompressor_vtf(uint8_t *ori, uint8_t *comp, uint8_t *iphc, union pico_ll_addr
|
|
*llsrc, union pico_ll_addr *lldst, struct pico_device *dev)
|
|
{
|
|
uint8_t tf = *iphc & TF_ELIDED;
|
|
IGNORE_PARAMETER(llsrc);
|
|
IGNORE_PARAMETER(lldst);
|
|
IGNORE_PARAMETER(dev);
|
|
if (TF_INLINE == tf) {
|
|
*ori++ = (0x60 | (*comp >> 4));
|
|
*ori |= (uint8_t)((uint8_t)(*comp++ << 4) & 0xF0);
|
|
*ori++ |= *comp++;
|
|
*ori++ = *comp++;
|
|
*ori++ = *comp++;
|
|
return 4;
|
|
} else if (TF_ELIDED_DSCP == tf) {
|
|
*ori++ = (0x60 | (*comp >> 4)) & 0xFC;
|
|
*ori++ = *comp++ & 0x0F;
|
|
*ori++ = *comp++;
|
|
*ori = *comp;
|
|
return 3;
|
|
} else if (TF_ELIDED_FL == tf) {
|
|
*ori++ = (0x60 | (*comp >> 4));
|
|
*ori = (uint8_t)(*comp << 4) & 0xF0;
|
|
return 1;
|
|
} else {
|
|
*ori = 0x60; // Set version field to IPv6
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
/* Checks whether or not next header is compressible according to NHC scheme */
|
|
static int32_t
|
|
compressible_nh(uint8_t nh)
|
|
{
|
|
switch (nh) {
|
|
case PICO_IPV6_EXTHDR_HOPBYHOP:
|
|
case PICO_IPV6_EXTHDR_ROUTING:
|
|
case PICO_IPV6_EXTHDR_FRAG:
|
|
case PICO_IPV6_EXTHDR_DESTOPT:
|
|
case PICO_PROTO_UDP: return 1;
|
|
default: return 0;
|
|
}
|
|
}
|
|
|
|
/* Checks whether or not the next header can be compressed and sets the IPHC
|
|
* bits accordingly, compression of next header itself happens in NHC-compression
|
|
*/
|
|
static int8_t
|
|
compressor_nh(uint8_t *ori, uint8_t *comp, uint8_t *iphc, union pico_ll_addr *
|
|
llsrc, union pico_ll_addr *lldst, struct pico_device *dev)
|
|
{
|
|
*iphc &= (uint8_t)~NH_COMPRESSED;
|
|
IGNORE_PARAMETER(comp);
|
|
IGNORE_PARAMETER(llsrc);
|
|
IGNORE_PARAMETER(lldst);
|
|
IGNORE_PARAMETER(dev);
|
|
if (compressible_nh(*ori)) {
|
|
*iphc |= NH_COMPRESSED;
|
|
return 0;
|
|
} else {
|
|
*comp = *ori;
|
|
return 1;
|
|
}
|
|
}
|
|
|
|
/* Check whether or no the next header is NHC-compressed, indicates this for the
|
|
* general decompressor so it knows that it has to decompress the next header
|
|
* and fill in the NH-header field in IPv6 header */
|
|
static int8_t
|
|
decompressor_nh(uint8_t *ori, uint8_t *comp, uint8_t *iphc, union pico_ll_addr
|
|
*llsrc, union pico_ll_addr *lldst, struct pico_device *dev)
|
|
{
|
|
IGNORE_PARAMETER(llsrc);
|
|
IGNORE_PARAMETER(lldst);
|
|
IGNORE_PARAMETER(dev);
|
|
IGNORE_PARAMETER(comp);
|
|
if (*iphc & NH_COMPRESSED) {
|
|
*ori = 0; // Indicate that next header needs to be decompressed
|
|
return 0;
|
|
} else {
|
|
*ori = *comp;
|
|
return 1;
|
|
}
|
|
}
|
|
|
|
/* Compressed the HL-field if common hop limit values are used, like 1, 64 and
|
|
* 255 */
|
|
static int8_t
|
|
compressor_hl(uint8_t *ori, uint8_t *comp, uint8_t *iphc, union pico_ll_addr *
|
|
llsrc, union pico_ll_addr *lldst, struct pico_device *dev)
|
|
{
|
|
IGNORE_PARAMETER(llsrc);
|
|
IGNORE_PARAMETER(lldst);
|
|
IGNORE_PARAMETER(dev);
|
|
*iphc &= (uint8_t)~HL_COMPRESSED_255;
|
|
switch (*ori) {
|
|
case 1: *iphc |= (uint8_t)HL_COMPRESSED_1;
|
|
return 0;
|
|
case 64: *iphc |= (uint8_t)HL_COMPRESSED_64;
|
|
return 0;
|
|
case 255: *iphc |= (uint8_t)HL_COMPRESSED_255;
|
|
return 0;
|
|
default: *comp = *ori;
|
|
return 1;
|
|
}
|
|
}
|
|
|
|
/* Decompresses the HL-field to common hop limit values like 1, 64 and 255 */
|
|
static int8_t
|
|
decompressor_hl(uint8_t *ori, uint8_t *comp, uint8_t *iphc, union pico_ll_addr
|
|
*llsrc, union pico_ll_addr *lldst, struct pico_device *dev)
|
|
|
|
{
|
|
uint8_t hl = *iphc & HL_COMPRESSED_255;
|
|
IGNORE_PARAMETER(llsrc);
|
|
IGNORE_PARAMETER(lldst);
|
|
IGNORE_PARAMETER(dev);
|
|
switch(hl) {
|
|
case HL_COMPRESSED_1: *ori = (uint8_t)1;
|
|
return 0;
|
|
case HL_COMPRESSED_64: *ori = (uint8_t)64;
|
|
return 0;
|
|
case HL_COMPRESSED_255: *ori = (uint8_t)255;
|
|
return 0;
|
|
default: *ori = *comp;
|
|
return 1;
|
|
}
|
|
}
|
|
|
|
/* Determines if an address can be statefully or statelessly compressed */
|
|
static int8_t
|
|
addr_comp_prefix(uint8_t *iphc, struct pico_ip6 *addr, int8_t src)
|
|
{
|
|
struct iphc_ctx *ctx = NULL;
|
|
uint8_t state = src ? SRC_STATEFUL : DST_STATEFUL;
|
|
iphc[1] &= (uint8_t)~state; // Clear out compression state for src/dst
|
|
|
|
if (pico_ipv6_is_multicast(addr->addr)) {
|
|
/* TODO: Support stateful multicast compression with Unicast-Prefix-Based
|
|
* IPv6 Multicast Addresses as defined in RFC3956 */
|
|
return COMP_MULTICAST; // AC = 0
|
|
} else if (pico_ipv6_is_linklocal(addr->addr)) {
|
|
return COMP_LINKLOCAL; // AC = 0
|
|
} else if ((ctx = ctx_lookup(*addr))) {
|
|
if (ctx->flags & PICO_IPHC_CTX_COMPRESS) {
|
|
iphc[1] |= state; // AC = 1
|
|
iphc[1] |= CTX_EXTENSION; // SRC or DST is stateful, CID = 1
|
|
return (int8_t)ctx->id;
|
|
}
|
|
}
|
|
return COMP_STATELESS; // AC = 0
|
|
}
|
|
|
|
/* Checks whether or not an IPv6 address is derived from a link layer address */
|
|
static int8_t
|
|
addr_ll_derived(struct pico_ip6 *addr, union pico_ll_addr *lladdr, struct pico_device *dev)
|
|
{
|
|
uint8_t iid[8] = {0};
|
|
if (pico_6lowpan_lls[dev->mode].addr_iid) {
|
|
if (!pico_6lowpan_lls[dev->mode].addr_iid(iid, lladdr))
|
|
return (int8_t)(0 == memcmp(iid, &addr->addr[8], 8));
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
/* Sets the compression mode of either the source address or the destination
|
|
* address, based on the shift parameter. Use SRC_SHIFT for source, 0 for dst */
|
|
static int8_t
|
|
addr_comp_mode(uint8_t *iphc, struct pico_ip6 *addr, union pico_ll_addr lladdr, struct pico_device *dev, int8_t shift)
|
|
{
|
|
int8_t mac = addr_ll_derived(addr, &lladdr, dev);
|
|
iphc[1] &= (uint8_t)((uint8_t)~DST_COMPRESSED << shift); // Clear src/dst mode
|
|
|
|
if (mac > 0) { // Address is mac derived
|
|
iphc[1] |= (uint8_t)(DST_COMPRESSED << shift);
|
|
return 0;
|
|
} else if (!mac && IID_16(&addr->addr[8])) { // Address is 16-bit deriveable
|
|
iphc[1] |= (uint8_t)(DST_COMPRESSED_16 << shift);
|
|
return 2;
|
|
} else if (!mac) { // Copy the entire IID
|
|
iphc[1] |= (uint8_t)(DST_COMPRESSED_64 << shift);
|
|
return 8;
|
|
} else {
|
|
return -1; // Something went wrong, indicate failure
|
|
}
|
|
}
|
|
|
|
/* Compresses a multicast address statelessly */
|
|
static int8_t
|
|
addr_comp_mcast(uint8_t *iphc, uint8_t *comp, struct pico_ip6 *mcast)
|
|
{
|
|
iphc[1] &= (uint8_t)~DST_MCAST_8; // Clear out addressing mode
|
|
iphc[1] |= (uint8_t)DST_MULTICAST; // Set multicast flag
|
|
|
|
if (IPV6_MCAST_48(mcast->addr)) {
|
|
comp[0] = mcast->addr[1]; // Copy flags and scope
|
|
buf_move(&comp[1], &mcast->addr[11], 5); // Copy group identifier
|
|
iphc[1] |= DST_MCAST_48;
|
|
return 6;
|
|
} else if (IPV6_MCAST_32(mcast->addr)) {
|
|
comp[0] = mcast->addr[1]; // Copy flags and scope
|
|
buf_move(&comp[1], &mcast->addr[13], 3); // Copy group identifier
|
|
iphc[1] |= DST_MCAST_32;
|
|
return 4;
|
|
} else if (IPV6_MCAST_8(mcast->addr)) {
|
|
comp[0] = mcast->addr[15]; // Copy group identifier
|
|
iphc[1] |= DST_MCAST_8; // Flags and scope = 0x02
|
|
return 1;
|
|
} else {
|
|
buf_move(comp, mcast->addr, PICO_SIZE_IP6); // Copy entire address
|
|
return PICO_SIZE_IP6;
|
|
}
|
|
}
|
|
|
|
/* Compresses the IID of a IPv6 address into 'comp'. Also has to take link layer
|
|
* address into account and whether it's about source or destination address. */
|
|
static int8_t
|
|
addr_comp_iid(uint8_t *iphc, uint8_t *comp, int8_t state, struct pico_ip6 *addr, union pico_ll_addr ll, struct pico_device *dev, int8_t shift)
|
|
{
|
|
int8_t len = PICO_SIZE_IP6;
|
|
switch (state) {
|
|
case COMP_UNSPECIFIED: // Set stateful bit
|
|
iphc[1] |= SRC_STATEFUL;
|
|
case COMP_STATELESS: // Clear compressed flags
|
|
iphc[1] &= (uint8_t)~SRC_COMPRESSED;
|
|
break;
|
|
case COMP_LINKLOCAL:
|
|
len = addr_comp_mode(iphc, addr, ll, dev, shift);
|
|
break;
|
|
case COMP_MULTICAST: // Multicast, compress statelessly
|
|
return addr_comp_mcast(iphc, comp, addr);
|
|
default: // Context available, extend header, and check for IID
|
|
iphc[2] = (uint8_t)((uint8_t)state << shift);
|
|
len = addr_comp_mode(iphc, addr, ll, dev, shift);
|
|
}
|
|
|
|
if (len >= 0)
|
|
buf_move(comp, addr->addr + PICO_SIZE_IP6 - len, (size_t)len);
|
|
return len;
|
|
}
|
|
|
|
/* Compresses the SOURCE address of the IPv6 frame */
|
|
static int8_t
|
|
compressor_src(uint8_t *ori, uint8_t *comp, uint8_t *iphc, union pico_ll_addr *llsrc, union pico_ll_addr *lldst, struct pico_device *dev)
|
|
{
|
|
struct pico_ip6 src = *(struct pico_ip6 *)ori;
|
|
int8_t ret = addr_comp_prefix(iphc, &src, SRC_SHIFT);
|
|
IGNORE_PARAMETER(lldst);
|
|
|
|
if (pico_ipv6_is_unspecified(src.addr))
|
|
ret = COMP_UNSPECIFIED;
|
|
|
|
return addr_comp_iid(iphc, comp, ret, &src, *llsrc, dev, SRC_SHIFT);
|
|
}
|
|
|
|
/* Copies the appropriate IPv6 prefix in the decompressed address. Based on
|
|
* context, link local address or multicast address */
|
|
static int8_t
|
|
addr_decomp_prefix(uint8_t *prefix, uint8_t *iphc, int8_t shift)
|
|
{
|
|
struct pico_ip6 ll = { .addr = {0xfe,0x80,0,0,0,0,0,0,0,0,0,0xff,0xfe,0,0,0}};
|
|
uint8_t addr_state = (uint8_t)(DST_STATEFUL << shift);
|
|
struct iphc_ctx *ctx = NULL;
|
|
|
|
if (iphc[1] & addr_state) {
|
|
if ((ctx = ctx_lookup_id((uint8_t)(iphc[2] >> shift)))) {
|
|
buf_move(prefix, ctx->prefix.addr, PICO_SIZE_IP6);
|
|
buf_move(&prefix[8], &ll.addr[8], 8); // For 16-bit derived addresses
|
|
} else {
|
|
/* No context available while stateful compression is used... */
|
|
return -1;
|
|
}
|
|
} else {
|
|
buf_move(prefix, ll.addr, PICO_SIZE_IP6);
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
/* Decompresses the IID of the IPv6 address based on addressing mode of the IPHC-
|
|
* header */
|
|
static int8_t
|
|
addr_decomp_iid(struct pico_ip6 *addr, uint8_t *comp, uint8_t am, union pico_ll_addr lladdr, struct pico_device *dev)
|
|
{
|
|
if (addr) {
|
|
switch (am) {
|
|
case DST_COMPRESSED_64: buf_move(&addr->addr[8], comp, 8);
|
|
return 8;
|
|
case DST_COMPRESSED_16: buf_move(&addr->addr[14], comp, 2);
|
|
return 2;
|
|
case DST_COMPRESSED:
|
|
if (dev && pico_6lowpan_lls[dev->mode].addr_iid) {
|
|
pico_6lowpan_lls[dev->mode].addr_iid(&addr->addr[8], &lladdr);
|
|
return 0;
|
|
} else {
|
|
return -1;
|
|
}
|
|
default: buf_move(addr->addr, comp, PICO_SIZE_IP6);
|
|
return 16;
|
|
}
|
|
} else {
|
|
return -1;
|
|
}
|
|
}
|
|
|
|
/* Decompress the SOURCE address of the 6LoWPAN frame */
|
|
static int8_t
|
|
decompressor_src(uint8_t *ori, uint8_t *comp, uint8_t *iphc, union pico_ll_addr
|
|
*llsrc, union pico_ll_addr *lldst, struct pico_device *dev)
|
|
{
|
|
struct pico_ip6 *src = (struct pico_ip6 *)ori;
|
|
uint8_t sam = (uint8_t)((uint8_t)(iphc[1] & SRC_COMPRESSED) >> 4);
|
|
IGNORE_PARAMETER(lldst);
|
|
|
|
/* Get the appropriate IPv6 prefix */
|
|
if (addr_decomp_prefix(ori, iphc, SRC_SHIFT))
|
|
return -1;
|
|
|
|
return addr_decomp_iid(src, comp, sam, *llsrc, dev);
|
|
}
|
|
|
|
/* Compresses the DESTINATION address of IPv6 frame */
|
|
static int8_t
|
|
compressor_dst(uint8_t *ori, uint8_t *comp, uint8_t *iphc, union pico_ll_addr *
|
|
llsrc, union pico_ll_addr *lldst, struct pico_device *dev)
|
|
{
|
|
struct pico_ip6 dst = *(struct pico_ip6 *)ori;
|
|
int8_t ret = addr_comp_prefix(iphc, &dst, 0);
|
|
IGNORE_PARAMETER(llsrc);
|
|
return addr_comp_iid(iphc, comp, ret, &dst, *lldst, dev, 0);
|
|
}
|
|
|
|
/* Decompresses the IPv6 multicast destination address when the IPHC mcast-flag
|
|
* is set */
|
|
static int8_t
|
|
addr_decomp_mcast(uint8_t *comp, struct pico_ip6 *dst, uint8_t am)
|
|
{
|
|
if (dst) {
|
|
memset(dst->addr, 0, PICO_SIZE_IP6);
|
|
dst->addr[0] = 0xff;
|
|
dst->addr[1] = *comp;
|
|
switch (am) {
|
|
case DST_MCAST_48:
|
|
buf_move(dst->addr + 11, comp + 1, 5);
|
|
return 6;
|
|
case DST_MCAST_32:
|
|
buf_move(dst->addr + 13, comp + 1, 3);
|
|
return 4;
|
|
case DST_MCAST_8:
|
|
dst->addr[1] = 0x02;
|
|
dst->addr[15] = *comp;
|
|
return 1;
|
|
default:
|
|
buf_move(dst->addr, comp, PICO_SIZE_IP6);
|
|
return PICO_SIZE_IP6;
|
|
}
|
|
} else {
|
|
return -1;
|
|
}
|
|
}
|
|
|
|
/* Decompresses the DESTINATION address of a 6LoWPAN frame */
|
|
static int8_t
|
|
decompressor_dst(uint8_t *ori, uint8_t *comp, uint8_t *iphc, union pico_ll_addr *llsrc, union pico_ll_addr *lldst, struct pico_device *dev)
|
|
{
|
|
struct pico_ip6 *dst = (struct pico_ip6 *)ori;
|
|
uint8_t dam = iphc[1] & DST_COMPRESSED;
|
|
IGNORE_PARAMETER(llsrc);
|
|
|
|
if (addr_decomp_prefix(ori, iphc, SRC_SHIFT))
|
|
return -1;
|
|
|
|
if (iphc[1] & DST_MULTICAST) {
|
|
return addr_decomp_mcast(comp, dst, dam);
|
|
} else {
|
|
return addr_decomp_iid(dst, comp, dam, *lldst, dev);
|
|
}
|
|
}
|
|
|
|
static const struct hdr_field ip6_fields[] = {
|
|
{4, compressor_vtf, decompressor_vtf},
|
|
{2, NULL, NULL},
|
|
{1, compressor_nh, decompressor_nh},
|
|
{1, compressor_hl, decompressor_hl},
|
|
{16, compressor_src, decompressor_src},
|
|
{16, compressor_dst, decompressor_dst}
|
|
};
|
|
|
|
/* Compresses the IPv6 frame according to the IPHC-compression scheme */
|
|
static uint8_t *
|
|
compressor_iphc(struct pico_frame *f, int32_t *compressed_len, uint8_t *nh)
|
|
{
|
|
uint8_t *inline_buf = PICO_ZALLOC(PICO_SIZE_IP6HDR + 3);
|
|
uint8_t *comp = inline_buf + 3;
|
|
uint8_t *iphc = inline_buf;
|
|
uint8_t *ori = f->net_hdr;
|
|
int32_t i = 0, ret = 0;
|
|
*compressed_len = 0;
|
|
*nh = ((struct pico_ipv6_hdr *)f->net_hdr)->nxthdr;
|
|
|
|
if (!inline_buf) {
|
|
return NULL;
|
|
} else {
|
|
/* Compress fixed IPv6 fields */
|
|
for (i = 0; i < NUM_IPV6_FIELDS; i++) {
|
|
if (ip6_fields[i].compress) {
|
|
ret = ip6_fields[i].compress(ori, comp, iphc, &f->src, &f->dst, f->dev);
|
|
if (ret < 0) { // Something went wrong ...
|
|
PICO_FREE(inline_buf);
|
|
return NULL;
|
|
}
|
|
*compressed_len += ret; // Increase compressed length
|
|
comp += ret; // Move forward compressed length
|
|
}
|
|
ori += ip6_fields[i].ori_size; // Move to next field
|
|
}
|
|
|
|
/* Rearrange IPHC-header if CTX-extension is included */
|
|
if (iphc[1] & CTX_EXTENSION) {
|
|
*compressed_len += 3;
|
|
} else {
|
|
buf_move(inline_buf + 2, inline_buf + 3, (size_t)*compressed_len);
|
|
*compressed_len += 2;
|
|
}
|
|
}
|
|
return inline_buf;
|
|
}
|
|
|
|
/* Decompresses a frame compressed with the IPHC compression scheme, RFC6282 */
|
|
static uint8_t *
|
|
decompressor_iphc(struct pico_frame *f, int32_t *compressed_len)
|
|
{
|
|
uint8_t *ipv6_hdr = PICO_ZALLOC(PICO_SIZE_IP6HDR);
|
|
uint8_t *iphc = f->net_hdr, *ori = ipv6_hdr, *comp = NULL;
|
|
int32_t i = 0, ret = 0, ctx = f->net_hdr[1] & CTX_EXTENSION;
|
|
*compressed_len = ctx ? 3 : 2;
|
|
comp = f->net_hdr + (ctx ? 3 : 2);
|
|
|
|
if (!ipv6_hdr) {
|
|
return NULL;
|
|
} else {
|
|
for (i = 0; i < NUM_IPV6_FIELDS; i++) {
|
|
if (ip6_fields[i].decompress) {
|
|
ret = ip6_fields[i].decompress(ori, comp, iphc, &f->src, &f->dst, f->dev);
|
|
if (ret < 0) { // Something went wrong ...
|
|
PICO_FREE(ipv6_hdr);
|
|
return NULL;
|
|
}
|
|
*compressed_len += ret; // Increase compressed size
|
|
comp += ret; // Move to next compressed chunk
|
|
}
|
|
ori += ip6_fields[i].ori_size; // Move to next IPv6 field
|
|
}
|
|
}
|
|
return ipv6_hdr;
|
|
}
|
|
|
|
/* Compresses a UDP header according to the NHC_UDP compression scheme, RFC6282 */
|
|
static uint8_t *
|
|
compressor_nhc_udp(struct pico_frame *f, int32_t *compressed_len)
|
|
{
|
|
uint8_t *inline_buf = PICO_ZALLOC(PICO_UDPHDR_SIZE);
|
|
struct pico_udp_hdr *hdr = (struct pico_udp_hdr *)f->transport_hdr;
|
|
uint16_t sport = hdr->trans.sport, dport = hdr->trans.dport;
|
|
uint16_t xF0B0 = short_be(0xF0B0), xF000 = short_be(0xF000);
|
|
uint16_t xFF00 = short_be(0xFF00), xFFF0 = short_be(0xFFF0);
|
|
*compressed_len = 0;
|
|
|
|
if (!inline_buf) {
|
|
return NULL;
|
|
} else {
|
|
/* Dispatch header */
|
|
inline_buf[0] = (uint8_t)UDP_DISPATCH;
|
|
/* Port compression */
|
|
if (PORT_COMP(sport, xFFF0, xF0B0) && PORT_COMP(dport, xFFF0, xF0B0)) {
|
|
inline_buf[0] |= UDP_COMPRESSED_BOTH;
|
|
inline_buf[1] = (uint8_t)(short_be(sport) << 4);
|
|
dport = (uint8_t)(short_be(dport) & (uint16_t)0x000F);
|
|
inline_buf[1] = (uint8_t)(inline_buf[1] | (uint8_t)dport);
|
|
*compressed_len = 2;
|
|
} else if (PORT_COMP(sport, xFF00, xF000)) {
|
|
inline_buf[0] |= UDP_COMPRESSED_SRC;
|
|
inline_buf[1] = (uint8_t)short_be(sport);
|
|
buf_move(inline_buf + 2, (uint8_t *)hdr + 2, 2);
|
|
*compressed_len = 4;
|
|
} else if (PORT_COMP(dport, xFF00, xF000)) {
|
|
inline_buf[0] |= UDP_COMPRESSED_DST;
|
|
inline_buf[3] = (uint8_t)short_be(dport);
|
|
buf_move(inline_buf + 1, (uint8_t *)hdr, 2);
|
|
*compressed_len = 4;
|
|
} else {
|
|
inline_buf[0] &= (uint8_t)~UDP_COMPRESSED_BOTH;
|
|
buf_move(inline_buf + 1, (uint8_t *)hdr, 4);
|
|
*compressed_len = 5;
|
|
}
|
|
/* Length MUST be compressed checksum carried inline.
|
|
* RFC6282: .., a compressor in the source transport endpoint MAY elide
|
|
* the UDP checksum if it is autorized by the upper layer. The compressor
|
|
* MUST NOT set the C bit unless it has received such authorization */
|
|
buf_move(inline_buf + *compressed_len, (uint8_t *)hdr + 6, 2);
|
|
*compressed_len += 2;
|
|
return inline_buf;
|
|
}
|
|
}
|
|
|
|
/* Decompresses a NHC_UDP header according to the NHC_UDP compression scheme */
|
|
static uint8_t *
|
|
decompressor_nhc_udp(struct pico_frame *f, int32_t processed_len, int32_t *compressed_len)
|
|
{
|
|
struct pico_udp_hdr *hdr = NULL;
|
|
uint8_t *buf = f->transport_hdr;
|
|
uint8_t compression = buf[0] & UDP_COMPRESSED_BOTH;
|
|
uint16_t xF0B0 = short_be(0xF0B0);
|
|
uint16_t xF000 = short_be(0xF000);
|
|
int32_t payload_len = 0;
|
|
*compressed_len = 0;
|
|
|
|
/* Decompress ports */
|
|
hdr = PICO_ZALLOC(PICO_UDPHDR_SIZE);
|
|
if (hdr) {
|
|
if (UDP_COMPRESSED_BOTH == compression) {
|
|
hdr->trans.sport = xF0B0 | short_be((uint16_t)(buf[1] >> 4));
|
|
hdr->trans.dport = xF0B0 | short_be((uint16_t)(buf[1] & 0xff));
|
|
*compressed_len = 2;
|
|
} else if (UDP_COMPRESSED_SRC == compression) {
|
|
hdr->trans.dport = short_be((uint16_t)(((uint16_t)buf[2] << 8) | (uint16_t)buf[3]));
|
|
hdr->trans.sport = xF000 | short_be((uint16_t)buf[1]);
|
|
*compressed_len = 4;
|
|
} else if (UDP_COMPRESSED_DST == compression) {
|
|
hdr->trans.sport = short_be((uint16_t)(((uint16_t)buf[1] << 8) | (uint16_t)buf[2]));
|
|
hdr->trans.dport = xF000 | short_be((uint16_t)buf[3]);
|
|
*compressed_len = 4;
|
|
} else {
|
|
buf_move((uint8_t *)&hdr->trans, &buf[1], 4);
|
|
*compressed_len = 5;
|
|
}
|
|
if (!(buf[0] & UDP_COMPRESSED_CHCK)) { // Leave empty room for checksum
|
|
buf_move((uint8_t *)&hdr->crc, &buf[*compressed_len],2);
|
|
*compressed_len += 2;
|
|
}
|
|
/* Restore inherently compressed length */
|
|
payload_len = (int32_t)f->len - (processed_len + *compressed_len);
|
|
hdr->len = short_be((uint16_t)(payload_len + PICO_UDPHDR_SIZE));
|
|
return (uint8_t *)hdr;
|
|
}
|
|
return NULL;
|
|
}
|
|
|
|
/* Get's the length of an IPv6 extension header */
|
|
static int32_t
|
|
ext_hdr_len(struct pico_ipv6_exthdr *ext, uint8_t hdr, uint8_t *dispatch)
|
|
{
|
|
int32_t len = 0;
|
|
/* Get length of extension header */
|
|
switch (hdr) {
|
|
case PICO_IPV6_EXTHDR_HOPBYHOP:
|
|
*dispatch |= (uint8_t)EXT_HOPBYHOP;
|
|
len = IPV6_OPTLEN(ext->ext.destopt.len); // Length in bytes
|
|
ext->ext.destopt.len = (uint8_t)(len - 2); // Octets after len-field
|
|
return (int32_t)len;
|
|
case PICO_IPV6_EXTHDR_ROUTING:
|
|
*dispatch |= (uint8_t)EXT_ROUTING;
|
|
len = IPV6_OPTLEN(ext->ext.destopt.len); // Length in bytes
|
|
ext->ext.destopt.len = (uint8_t)(len - 2); // Octets after len-field
|
|
return (int32_t)len;
|
|
case PICO_IPV6_EXTHDR_DESTOPT:
|
|
*dispatch |= (uint8_t)EXT_DSTOPT;
|
|
len = IPV6_OPTLEN(ext->ext.destopt.len); // Length in bytes
|
|
ext->ext.destopt.len = (uint8_t)(len - 2); // Octets after len-field
|
|
return (int32_t)len;
|
|
case PICO_IPV6_EXTHDR_FRAG:
|
|
*dispatch |= (uint8_t)EXT_FRAG;
|
|
return (int32_t)8;
|
|
default: // Somethin went wrong, bail out...
|
|
return -1;
|
|
}
|
|
}
|
|
|
|
/* Compresses an IPv6 extension header according to the NHC_EXT compression
|
|
* scheme */
|
|
static uint8_t *
|
|
compressor_nhc_ext(struct pico_frame *f, int32_t *compressed_len, uint8_t *nh)
|
|
{
|
|
struct pico_ipv6_exthdr *ext = (struct pico_ipv6_exthdr *)f->net_hdr;
|
|
uint8_t dispatch = EXT_DISPATCH;
|
|
int32_t len = 0, lead = 0, ret = 0;
|
|
uint8_t *buf = NULL;
|
|
uint8_t hdr = *nh;
|
|
|
|
/* Determine next header */
|
|
*nh = ext->nxthdr;
|
|
if (!compressible_nh(*nh)) {
|
|
len++; // Dispatch header has to be prepended
|
|
lead++; // Copy right after dispatch
|
|
} else {
|
|
dispatch |= (uint8_t)0x01; // Set NH flag
|
|
}
|
|
|
|
/* Get length of extension header */
|
|
ret = ext_hdr_len(ext, hdr, &dispatch);
|
|
if (ret < 0) {
|
|
return NULL;
|
|
} else {
|
|
/* Provide inline buffer */
|
|
len += ret;
|
|
buf = PICO_ZALLOC((size_t)len);
|
|
if (!buf) {
|
|
return NULL;
|
|
} else {
|
|
/* Copy extension header */
|
|
buf_move(buf + lead, (uint8_t *)ext, (size_t)(len - lead));
|
|
buf[0] = dispatch; // Set the dispatch header
|
|
*compressed_len = len;
|
|
f->net_hdr += *compressed_len; // Move to next header
|
|
return buf;
|
|
}
|
|
}
|
|
}
|
|
|
|
/* Retrieves the next header from the immediately following header */
|
|
static uint8_t
|
|
ext_nh_retrieve(uint8_t *buf, int32_t len)
|
|
{
|
|
uint8_t eid = 0;
|
|
buf += len;
|
|
if ((buf[0] & 0xF0) == EXT_DISPATCH) {
|
|
eid = buf[0] & 0x0E;
|
|
switch (eid) {
|
|
case EXT_HOPBYHOP:
|
|
return (uint8_t)PICO_IPV6_EXTHDR_HOPBYHOP;
|
|
case EXT_ROUTING:
|
|
return (uint8_t)PICO_IPV6_EXTHDR_ROUTING;
|
|
case EXT_FRAG:
|
|
return (uint8_t)PICO_IPV6_EXTHDR_FRAG;
|
|
case EXT_DSTOPT:
|
|
return (uint8_t)PICO_IPV6_EXTHDR_DESTOPT;
|
|
default:
|
|
return 0;
|
|
}
|
|
} else if ((buf[0] & 0xF8) == UDP_DISPATCH) {
|
|
return PICO_PROTO_UDP;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
/* RFC6282: A decompressor MUST ensure that the
|
|
* containing header is padded out to a multiple of 8 octets in length,
|
|
* using a Pad1 or PadN option if necessary. */
|
|
static int32_t
|
|
ext_align(uint8_t *buf, int32_t alloc, int32_t len)
|
|
{
|
|
int32_t padlen = alloc - len;
|
|
buf += len; // Move to padding location
|
|
if (padlen == 1) {
|
|
buf[0] = 0; // Pad1
|
|
} else if (padlen > 1) {
|
|
buf[0] = 1; // PadN
|
|
buf[1] = (uint8_t)(padlen - 2);
|
|
} else {
|
|
return -1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
/* Determines the compressed length (and some other parameters) from NHC_EXT
|
|
* compressed extension header */
|
|
static int32_t
|
|
ext_compressed_length(uint8_t *buf, uint8_t eid, int32_t *compressed_len, int32_t *head)
|
|
{
|
|
int32_t len = 0;
|
|
switch (eid) {
|
|
case EXT_HOPBYHOP: // Intentional fall-through
|
|
case EXT_ROUTING: // Intentional fall-through
|
|
case EXT_DSTOPT: // Intentional fall-through
|
|
if (!(buf[0] & NH_COMPRESSED)) { // [ DIS | NXT | LEN | ... (len)
|
|
len = 2 + buf[2];
|
|
*compressed_len = len + 1;
|
|
} else { // [ DIS | LEN | ... (len)
|
|
len = 2 + buf[1];
|
|
*compressed_len = len;
|
|
*head = 1;
|
|
}
|
|
return len;
|
|
case EXT_FRAG: // [ DIS | FRAG ...
|
|
len = 8;
|
|
*compressed_len = len;
|
|
return len;
|
|
default: // Something went wrong, bail out..
|
|
return -1;
|
|
}
|
|
}
|
|
|
|
/* Decompresses an extension header pointed to by 'f->net_hdr', according to the
|
|
* NHC_EXT compression scheme */
|
|
static uint8_t *
|
|
decompressor_nhc_ext(struct pico_frame *f, int32_t *compressed_len, int32_t *decompressed_len)
|
|
{
|
|
struct pico_ipv6_exthdr *ext = NULL;
|
|
int32_t len = 0, head = 0, alloc = 0;
|
|
uint8_t *buf = f->net_hdr;
|
|
uint8_t eid = buf[0] & 0x0E;
|
|
uint8_t nh = 0;
|
|
|
|
if ((buf[0] & 0xF0) == EXT_DISPATCH) {
|
|
/* Determine compressed header length */
|
|
len = ext_compressed_length(buf, eid, compressed_len, &head);
|
|
if (len >= 0) {
|
|
/* Retrieve next header from following header */
|
|
nh = ext_nh_retrieve(buf, *compressed_len);
|
|
|
|
/* Make sure options are 8 octet aligned */
|
|
alloc = (len % 8) ? (((len / 8) + 1) * 8) : (len);
|
|
ext = (struct pico_ipv6_exthdr *)PICO_ZALLOC((size_t)alloc);
|
|
if (ext) {
|
|
buf_move((uint8_t *)ext + head, buf + 1, (size_t)(len - head));
|
|
ext->nxthdr = nh;
|
|
if (EXT_HOPBYHOP == eid || EXT_DSTOPT == eid || EXT_ROUTING) {
|
|
ext->ext.destopt.len = (uint8_t)((alloc / 8) - 1);
|
|
ext_align((uint8_t *)ext, alloc, len);
|
|
}
|
|
}
|
|
*decompressed_len = alloc;
|
|
return (uint8_t *)ext;
|
|
}
|
|
}
|
|
return NULL;
|
|
}
|
|
|
|
/* Free's memory of a all assembled chunks for 'n' amount */
|
|
static struct pico_frame *
|
|
pico_iphc_bail_out(uint8_t **chunks, int32_t n)
|
|
{
|
|
int32_t i = 0;
|
|
for (i = 0; i < n; i++) {
|
|
PICO_FREE(chunks[i]);
|
|
}
|
|
return NULL;
|
|
}
|
|
|
|
/* Performs reassembly after either compression of decompression */
|
|
static struct pico_frame *
|
|
pico_iphc_reassemble(struct pico_frame *f, uint8_t **chunks, int32_t *chunks_len, int32_t n, int32_t processed_len, int32_t handled_len)
|
|
{
|
|
uint32_t grow = f->buffer_len;
|
|
struct pico_frame *new = NULL;
|
|
int32_t payload_len = 0;
|
|
uint8_t *dst = NULL;
|
|
int32_t ret = 0, i = 0;
|
|
|
|
/* Calculate buffer size including IPv6 payload */
|
|
payload_len = (int32_t)f->len - handled_len;
|
|
processed_len += payload_len; // Length of frame after processing
|
|
|
|
/* Reallocate frame size if there isn't enough room available */
|
|
if (f->len < (uint16_t)processed_len) {
|
|
grow = (uint32_t)(grow + (uint32_t)processed_len - f->len);
|
|
ret = pico_frame_grow(f, grow);
|
|
if (ret)
|
|
return pico_iphc_bail_out(chunks, n);
|
|
}
|
|
|
|
chunks[n] = f->net_hdr + handled_len; // Start of payload_available
|
|
chunks_len[n] = payload_len; // Size of payload
|
|
n++; // Payload is another chunk to copy
|
|
|
|
/* Provide a new frame */
|
|
if (!(new = pico_frame_deepcopy(f)))
|
|
return pico_iphc_bail_out(chunks, n);
|
|
|
|
/* Copy each chunk back in the frame starting at the end of the new
|
|
* frame-buffer so we don't overwrite overlapping memory regions */
|
|
dst = new->buffer + new->buffer_len;
|
|
for (i = n - 1; i >= 0; i--) {
|
|
dst -= chunks_len[i];
|
|
buf_move(dst, chunks[i], (size_t)chunks_len[i]);
|
|
}
|
|
new->net_hdr = dst; // Last destination is net_hdr
|
|
new->start = new->net_hdr; // Start of useful data is at net_hdr
|
|
new->len = (uint32_t)processed_len;
|
|
new->transport_len = 0;
|
|
new->payload_len = 0;
|
|
new->app_len = 0;
|
|
new->transport_hdr = new->net_hdr + new->net_len;
|
|
pico_iphc_bail_out(chunks, n - 1); // Success, discard compressed chunk
|
|
if (new->start < new->buffer) {
|
|
pico_frame_discard(new);
|
|
return NULL;
|
|
}
|
|
return new;
|
|
}
|
|
|
|
/* Compresses a frame according to the IPHC, NHC_EXT and NHC_UDP compression scheme */
|
|
static struct pico_frame *
|
|
pico_iphc_compress(struct pico_frame *f)
|
|
{
|
|
int32_t i = 0, compressed_len = 0, loop = 1, uncompressed = f->net_len;
|
|
uint8_t *old_nethdr = f->net_hdr; // Save net_hdr temporary ...
|
|
uint8_t nh = PICO_PROTO_IPV6;
|
|
uint8_t *chunks[8] = { NULL };
|
|
int32_t chunks_len[8] = { 0 };
|
|
|
|
do {
|
|
switch (nh) {
|
|
/* IPV6 HEADER */
|
|
case PICO_PROTO_IPV6:
|
|
chunks[i] = compressor_iphc(f, &chunks_len[i], &nh);
|
|
f->net_hdr += 40; // Move after IPv6 header
|
|
f->net_len = (uint16_t)chunks_len[i];
|
|
break;
|
|
/* IPV6 EXTENSION HEADERS */
|
|
case PICO_IPV6_EXTHDR_HOPBYHOP:
|
|
case PICO_IPV6_EXTHDR_ROUTING:
|
|
case PICO_IPV6_EXTHDR_FRAG:
|
|
case PICO_IPV6_EXTHDR_DESTOPT:
|
|
chunks[i] = compressor_nhc_ext(f, &chunks_len[i], &nh);
|
|
f->net_len = (uint16_t)(f->net_len + chunks_len[i]);
|
|
/* f->net_hdr is updated in compresor_nhc_ext with original size */
|
|
break;
|
|
/* UDP HEADER */
|
|
case PICO_PROTO_UDP:
|
|
chunks[i] = compressor_nhc_udp(f, &chunks_len[i]);
|
|
uncompressed += PICO_UDPHDR_SIZE;
|
|
f->transport_len = (uint16_t)chunks_len[i];
|
|
default: /* Intentional fall-through */
|
|
loop = 0;
|
|
}
|
|
/* Check if an error occured */
|
|
if (!chunks[i])
|
|
return pico_iphc_bail_out(chunks, i);
|
|
/* Increment total compressed_len and increase iterator */
|
|
compressed_len += chunks_len[i++];
|
|
} while (compressible_nh(nh) && loop && i < 8);
|
|
|
|
f->net_hdr = old_nethdr; // ... Restore old net_hdr
|
|
return pico_iphc_reassemble(f, chunks, chunks_len, i, compressed_len, uncompressed);
|
|
}
|
|
|
|
/* Restore some IPv6 header fields like next header and payload length */
|
|
static struct pico_frame *
|
|
pico_ipv6_finalize(struct pico_frame *f, uint8_t nh)
|
|
{
|
|
struct pico_ipv6_hdr *hdr = NULL;
|
|
if (!f) {
|
|
return NULL;
|
|
} else {
|
|
hdr = (struct pico_ipv6_hdr *)f->net_hdr;
|
|
if (!hdr->nxthdr)
|
|
hdr->nxthdr = nh;
|
|
hdr->len = short_be((uint16_t)(f->len - PICO_SIZE_IP6HDR));
|
|
return f;
|
|
}
|
|
}
|
|
|
|
/* Decompresses a frame according to the IPHC, NHC_EXT and NHC_UDP compression scheme */
|
|
static struct pico_frame *
|
|
pico_iphc_decompress(struct pico_frame *f)
|
|
{
|
|
int32_t i = 0, compressed = 0, loop = 1, uncompressed = 0, ret = 0;
|
|
uint8_t *old_nethdr = f->net_hdr; // Save net_hdr temporary ...
|
|
uint8_t dispatch = PICO_PROTO_IPV6;
|
|
uint8_t *chunks[8] = { NULL };
|
|
struct pico_frame *n = NULL;
|
|
int32_t chunks_len[8] = { 0 };
|
|
uint8_t nh = 0;
|
|
|
|
do {
|
|
switch (dispatch) {
|
|
/* IPV6 HEADER */
|
|
case PICO_PROTO_IPV6:
|
|
chunks[i] = decompressor_iphc(f, &ret);
|
|
chunks_len[i] = PICO_SIZE_IP6HDR;
|
|
f->net_len = PICO_SIZE_IP6HDR;
|
|
nh = ext_nh_retrieve(f->net_hdr, ret);
|
|
break;
|
|
/* IPV6 EXTENSION HEADERS */
|
|
case PICO_IPV6_EXTHDR_HOPBYHOP:
|
|
case PICO_IPV6_EXTHDR_ROUTING:
|
|
case PICO_IPV6_EXTHDR_FRAG:
|
|
case PICO_IPV6_EXTHDR_DESTOPT:
|
|
chunks[i] = decompressor_nhc_ext(f, &ret, &chunks_len[i]);
|
|
f->net_len = (uint16_t)(f->net_len + chunks_len[i]);
|
|
break;
|
|
/* UDP HEADER */
|
|
case PICO_PROTO_UDP:
|
|
f->transport_hdr = f->net_hdr; // Switch to transport header
|
|
chunks[i] = decompressor_nhc_udp(f, compressed, &ret);
|
|
chunks_len[i] = PICO_UDPHDR_SIZE;
|
|
default: /* Intentional fall-through */
|
|
loop = 0;
|
|
}
|
|
/* Check if an error occured */
|
|
if (!chunks[i])
|
|
return pico_iphc_bail_out(chunks, i);
|
|
|
|
/* Increase compressed and uncompressed length */
|
|
compressed += ret;
|
|
uncompressed += chunks_len[i++];
|
|
|
|
/* Get next dispatch header */
|
|
f->net_hdr += ret;
|
|
dispatch = ext_nh_retrieve(f->net_hdr, 0);
|
|
} while (dispatch && loop && i < 8);
|
|
f->net_hdr = old_nethdr; // ... Restore old net_hdr
|
|
|
|
/* Reassemble gathererd decompressed buffers */
|
|
n = pico_iphc_reassemble(f, chunks, chunks_len, i, uncompressed, compressed);
|
|
return pico_ipv6_finalize(n, nh);
|
|
}
|
|
|
|
#endif
|
|
|
|
/* Prepends an uncompressed IPv6 dispatch header */
|
|
static void
|
|
pico_iphc_no_comp(struct pico_frame *f)
|
|
{
|
|
f->net_hdr--; // Only need one bytes
|
|
f->start--;
|
|
f->len++;
|
|
f->net_len++;
|
|
f->net_hdr[0] = IPV6_DISPATCH;
|
|
}
|
|
|
|
/* Removes an uncompressed IPv6 dispatch header */
|
|
static void
|
|
pico_iphc_no_comp_dec(struct pico_frame *f)
|
|
{
|
|
f->net_hdr++;
|
|
f->start++;
|
|
f->len--;
|
|
f->net_len--;
|
|
}
|
|
|
|
/* Updates the fragmentation cookie with how many bytes there are copied and units
|
|
* of 8-octets that are transmitted, if bytes copied equals the size of the datagram
|
|
* the cookie is removed from the cookie-tree and the datagram is discarded */
|
|
static int32_t
|
|
frag_update(struct pico_frame *f, struct frag_ctx *frag, uint8_t units, uint16_t copy)
|
|
{
|
|
frag->dgram_off = (uint8_t)(frag->dgram_off + units);
|
|
frag->copied = (uint16_t)(frag->copied + copy);
|
|
/* Datagram is completely transmitted */
|
|
if (frag->copied >= f->len) {
|
|
lp_dbg("6LP: FIN: "ORG"fragmentation"RST" with hash '%X', sent %u of %u bytes\n", frag->hash, frag->copied, f->len);
|
|
pico_tree_delete(&FragTree, frag);
|
|
PICO_FREE(frag);
|
|
pico_frame_discard(f);
|
|
} else {
|
|
lp_dbg("6LP: UPDATE: "ORG"fragmentation"RST" with hash '%X', sent %u of %u bytes\n", frag->hash, frag->copied, f->len);
|
|
return pico_datalink_send(f);
|
|
}
|
|
return (int32_t)1; // Success
|
|
}
|
|
|
|
static void
|
|
frag_fill(uint8_t *frag, uint8_t dispatch, uint16_t dgram_size, uint16_t tag, uint8_t dgram_off, int32_t offset, uint16_t copy, uint16_t copied, uint8_t *buf)
|
|
{
|
|
frag[0] = (uint8_t)(dispatch | ((uint8_t)short_be(dgram_size) & 0x07));
|
|
frag[1] = (uint8_t)(short_be(dgram_size) >> 8);
|
|
frag[2] = (uint8_t)(short_be(tag));
|
|
frag[3] = (uint8_t)(short_be(tag) >> 8);
|
|
frag[4] = (uint8_t)(dgram_off);
|
|
buf_move(frag + offset, buf + copied, copy);
|
|
}
|
|
|
|
/* Looks for a fragmentation cookie and creates an n-th fragment frame that it
|
|
* tries to push to the datalink layer, if the entire datagram is transmitted,
|
|
* the fragment cookie is removed from the tree and the datagram is free'd */
|
|
static int32_t
|
|
frag_nth(struct pico_frame *f)
|
|
{
|
|
struct frag_ctx *frag = frag_ctx_find(f->hash);
|
|
uint16_t left = 0;
|
|
uint16_t copy = 0, alloc = FRAGN_SIZE;
|
|
struct pico_frame *n = NULL;
|
|
uint8_t units = 0;
|
|
int32_t avail = 0, ret = 0;
|
|
|
|
if (frag) {
|
|
/* Check how many bytes there are available for n-th fragment */
|
|
avail = pico_6lowpan_ll_push(f);
|
|
if (avail > 0) {
|
|
/* Calculate dgram_off and bytes to copy */
|
|
left = (uint16_t)(f->len - frag->copied);
|
|
if (left <= (uint16_t)(avail - FRAGN_SIZE)) {
|
|
copy = left;
|
|
} else {
|
|
units = (uint8_t)((uint16_t)(avail - FRAGN_SIZE) >> 3);
|
|
copy = (uint16_t)(units << 3);
|
|
}
|
|
alloc = (uint16_t)(alloc + copy);
|
|
|
|
n = pico_proto_6lowpan_ll.alloc(&pico_proto_6lowpan_ll, f->dev, alloc);
|
|
if (n) {
|
|
frag_fill(n->net_hdr, FRAGN_DISPATCH, frag->dgram_size,
|
|
frag->dgram_tag, frag->dgram_off, 5, copy,
|
|
frag->copied, f->net_hdr);
|
|
n->net_len = alloc;
|
|
n->len = (uint32_t)n->net_len;
|
|
n->src = frag->f->src;
|
|
n->dst = frag->f->dst;
|
|
|
|
/* Try to push fragment to link layer */
|
|
ret = pico_6lowpan_ll_push(n);
|
|
if (!ret) { // Update frag cookie
|
|
return frag_update(f, frag, units, copy);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
pico_frame_discard(f);
|
|
return -1;
|
|
}
|
|
|
|
/* Makes a first fragment from a frame and tries to push it to the datalink layer
|
|
* Also enqueues the frame back in the outgoing frame-queue of the 6LOWPAN
|
|
* layer for subsequent fragments */
|
|
static int32_t
|
|
frag_1st(struct pico_frame *f, uint16_t dgram_size, uint8_t dgram_off, uint16_t copy)
|
|
{
|
|
uint16_t alloc = (uint16_t)(copy + FRAG1_SIZE);
|
|
struct pico_frame *n = NULL;
|
|
int32_t ret = 0;
|
|
|
|
n = pico_proto_6lowpan_ll.alloc(&pico_proto_6lowpan_ll, f->dev, alloc);
|
|
if (n) {
|
|
frag_fill(n->net_hdr, FRAG1_DISPATCH, dgram_size, dgram_tag, 0, 4, copy, 0,f->net_hdr);
|
|
n->net_len = alloc;
|
|
n->len = (uint32_t)n->net_len;
|
|
n->src = f->src;
|
|
n->dst = f->dst;
|
|
|
|
/* Try to push fragment to link layer */
|
|
ret = pico_6lowpan_ll_push(n);
|
|
if (ret) {
|
|
dgram_tag--;
|
|
return -1;
|
|
}
|
|
|
|
/* Enqueue the frame again for subsequent fragments */
|
|
f->flags |= PICO_FRAME_FLAG_SLP_FRAG;
|
|
if (pico_datalink_send(f) <= 0)
|
|
return -1;
|
|
|
|
/* Everything was a success store a cookie for subsequent fragments */
|
|
return frag_store(f, dgram_size, dgram_tag++, dgram_off, copy, &FragTree);
|
|
} else {
|
|
pico_err = PICO_ERR_ENOMEM;
|
|
return -1;
|
|
}
|
|
}
|
|
|
|
/* Send the first fragment of a uncompressed IPv6 datagram */
|
|
static int32_t
|
|
frag_1st_no_comp(struct pico_frame *f, uint16_t dgram_size, int32_t available)
|
|
{
|
|
/* Available bytes after inserting FRAG1 dispatch and IPv6 dispatch */
|
|
uint16_t rest_size = (uint16_t)(available - FRAG1_SIZE - 1);
|
|
uint8_t dgram_off = (uint8_t)(rest_size >> 3);
|
|
uint16_t copy_size = (uint16_t)(rest_size + 1);
|
|
return frag_1st(f, dgram_size, dgram_off, copy_size);
|
|
}
|
|
|
|
#ifdef PICO_6LOWPAN_IPHC_ENABLED
|
|
/* Determines the length of the compressed header */
|
|
static uint16_t
|
|
frame_comp_hlen(struct pico_frame *f, int32_t udp)
|
|
{
|
|
return (uint16_t)(f->net_len + ((udp) ? (f->transport_len) : (0)));
|
|
}
|
|
|
|
/* Send the first fragment of a compressed datagram */
|
|
static int32_t
|
|
frag_1st_comp(struct pico_frame *f, uint16_t dgram_size, int32_t available, int32_t udp)
|
|
{
|
|
/* Calculate amount of bytes that are elided */
|
|
uint16_t comp_diff = (uint16_t)(dgram_size - f->len);
|
|
uint16_t comp_hlen = frame_comp_hlen(f, udp);
|
|
/* Decompressed header length */
|
|
uint16_t deco_hlen = (uint16_t)(comp_hlen + comp_diff);
|
|
/* Available octects after inserting FRAG1 dispatch and compressed header */
|
|
uint16_t rest_size = (uint16_t)(available - FRAG1_SIZE - comp_hlen);
|
|
/* Offset for subsequent fragments in 8-octect units and in octets */
|
|
uint8_t dgram_off = (uint8_t)((uint16_t)(rest_size + deco_hlen) >> 3);
|
|
uint16_t copy_size = 0;
|
|
/* 8-octet aligned available octets after decompression */
|
|
rest_size = (uint16_t)((uint16_t)(dgram_off << 3) - deco_hlen);
|
|
copy_size = (uint16_t)(rest_size + comp_hlen);
|
|
return frag_1st(f, dgram_size, dgram_off, copy_size);
|
|
}
|
|
#endif
|
|
|
|
static int32_t
|
|
pico_6lowpan_compress(struct pico_frame *f, int32_t avail)
|
|
{
|
|
struct pico_ipv6_hdr *ip = (struct pico_ipv6_hdr *)f->net_hdr;
|
|
uint16_t dgram_size = (uint16_t)(short_be(ip->len) + PICO_SIZE_IP6HDR);
|
|
|
|
#ifdef PICO_6LOWPAN_IPHC_ENABLED
|
|
int32_t udp = (PICO_PROTO_UDP == ip->nxthdr);
|
|
struct pico_frame *try = pico_iphc_compress(f);
|
|
if (try) {
|
|
/* Try to push frame to link layer */
|
|
avail = pico_6lowpan_ll_push(try);
|
|
if (0 < avail && frame_comp_hlen(try, udp) <= (uint16_t)avail) {
|
|
/* RFC6282: any header that cannot fit within the first fragment
|
|
* MUST NOT be compressed. */
|
|
pico_frame_discard(f);
|
|
return frag_1st_comp(try, dgram_size, avail, udp);
|
|
} else if (!avail) {
|
|
pico_frame_discard(f);
|
|
return (int32_t)try->len; // Success, compression was enough
|
|
} else if (0 > avail) {
|
|
pico_frame_discard(try);
|
|
pico_frame_discard(f);
|
|
return -1; // Error pushing compressed frame
|
|
}
|
|
pico_frame_discard(try);
|
|
}
|
|
#endif
|
|
|
|
pico_iphc_no_comp(f); // Add uncompressed dispatch header again
|
|
return frag_1st_no_comp(f, dgram_size, avail);
|
|
}
|
|
|
|
/* General compression function that first tries to compress the frame and sends
|
|
* it through to the link layer, if that doesn't work the frame is fragmented */
|
|
static int32_t
|
|
pico_6lowpan_send(struct pico_frame *f)
|
|
{
|
|
int32_t avail = 0;
|
|
pico_iphc_no_comp(f); // Add uncrompressed dispatch header ...
|
|
|
|
/* Try to push frame to link layer */
|
|
avail = pico_6lowpan_ll_push(f);
|
|
if (avail > 0) {
|
|
pico_iphc_no_comp_dec(f); // ... remove IPv6 Dispatch Header
|
|
return pico_6lowpan_compress(f, avail);
|
|
} else if (!avail) { // Success
|
|
return (int32_t)f->len;
|
|
} else {
|
|
return -1;
|
|
}
|
|
}
|
|
|
|
static int32_t
|
|
pico_6lowpan_process_out(struct pico_protocol *self, struct pico_frame *f)
|
|
{
|
|
IGNORE_PARAMETER(self);
|
|
|
|
/* Check if it's meant for fragmentation */
|
|
if (f->flags & PICO_FRAME_FLAG_SLP_FRAG) {
|
|
return frag_nth(f);
|
|
} else if ((f->net_hdr[0] & 0xF0) != 0x60) {
|
|
lp_dbg("6lowpan - ERROR: not an IPv6 frame\n");
|
|
goto fin;
|
|
} else if (!f->dev || LL_MODE_ETHERNET == f->dev->mode) {
|
|
lp_dbg("6lowpan - ERROR: link layer mode not supported\n");
|
|
goto fin;
|
|
}
|
|
|
|
lp_dbg("6LP: ***NEW***, some stats: ");
|
|
lp_dbg("len: %d net_len: %d transport_len: %d\n", f->len, f->net_len, f->transport_len);
|
|
|
|
/* Retrieve link layer addresses */
|
|
if (pico_6lowpan_lls[f->dev->mode].addr_from_net(&f->src, f, 0) ||
|
|
pico_6lowpan_lls[f->dev->mode].addr_from_net(&f->dst, f, 1)) {
|
|
/* Address mode is unspecified, probably destination ll-address is being resolved */
|
|
return (int32_t)f->len;
|
|
}
|
|
|
|
return pico_6lowpan_send(f);
|
|
fin:
|
|
pico_frame_discard(f);
|
|
return -1;
|
|
}
|
|
|
|
static struct pico_frame *
|
|
pico_6lowpan_decompress(struct pico_frame *f)
|
|
{
|
|
#ifdef PICO_6LOWPAN_IPHC_ENABLED
|
|
struct pico_frame *dec = NULL;
|
|
#endif
|
|
|
|
if (0) {}
|
|
#ifdef PICO_6LOWPAN_IPHC_ENABLED
|
|
else if ((f->net_hdr[0] & 0xE0) == IPHC_DISPATCH) {
|
|
dec = pico_iphc_decompress(f);
|
|
pico_frame_discard(f);
|
|
return dec;
|
|
}
|
|
#endif
|
|
else if (f->net_hdr[0] == IPV6_DISPATCH) {
|
|
pico_iphc_no_comp_dec(f);
|
|
return f;
|
|
} else {
|
|
lp_dbg("6LP: RCVD invalid frame\n");
|
|
pico_frame_discard(f);
|
|
return NULL;
|
|
}
|
|
}
|
|
|
|
static int32_t
|
|
defrag_new(struct pico_frame *f, uint16_t dgram_size, uint16_t tag, uint16_t off)
|
|
{
|
|
struct pico_frame *r = pico_proto_6lowpan_ll.alloc(&pico_proto_6lowpan_ll, f->dev, dgram_size);
|
|
if (r) {
|
|
r->start = r->buffer + (int32_t)(r->buffer_len - (uint32_t)dgram_size);
|
|
r->len = dgram_size;
|
|
r->net_hdr = r->start;
|
|
r->net_len = f->net_len;
|
|
r->transport_len = (uint16_t)(r->len - r->net_len);
|
|
r->src = f->src;
|
|
r->dst = f->dst;
|
|
buf_move(r->net_hdr + off, f->start, f->len);
|
|
if (frag_store(r, dgram_size, tag, 0, (uint16_t)f->len, &ReassemblyTree) < 0) {
|
|
pico_frame_discard(f);
|
|
pico_frame_discard(r);
|
|
return -1;
|
|
}
|
|
}
|
|
pico_frame_discard(f);
|
|
return 1;
|
|
}
|
|
|
|
static int32_t
|
|
defrag_update(struct frag_ctx *frag, uint16_t off, struct pico_frame *f)
|
|
{
|
|
struct pico_frame *r = frag->f;
|
|
buf_move(r->start + (int32_t)off, f->start, f->len); // Copy at start
|
|
frag->copied = (uint16_t)(frag->copied + (uint16_t)f->len);
|
|
pico_frame_discard(f);
|
|
if (frag->copied >= frag->dgram_size) { // Datagram completely reassembled
|
|
lp_dbg("6LP: FIN: "GRN"reassembly"RST" with tag '%u', stats: len: %d net: %d trans: %d\n", frag->dgram_tag, r->len, r->net_len, r->transport_len);
|
|
pico_tree_delete(&ReassemblyTree, frag);
|
|
PICO_FREE(frag);
|
|
#ifdef PICO_6LOWPAN_IPHC_ENABLED
|
|
r = pico_ipv6_finalize(r, 0);
|
|
#endif
|
|
return pico_network_receive(r);
|
|
} else {
|
|
lp_dbg("6LP: UPDATE: "GRN"reassembly"RST" with tag '%u', %u of %u bytes received\n", frag->dgram_tag, frag->copied, frag->dgram_size);
|
|
}
|
|
return (int32_t)r->len;
|
|
}
|
|
|
|
static struct frag_ctx *
|
|
defrag_remove_header(struct pico_frame *f, uint16_t *dgram_size, uint16_t *tag, uint16_t *off, int32_t size)
|
|
{
|
|
*dgram_size = (uint16_t)(((uint16_t)(f->net_hdr[0] & 0x07) << 8) | (uint16_t)f->net_hdr[1]);
|
|
*tag = (uint16_t)(((uint16_t)f->net_hdr[2] << 8) | (uint16_t)f->net_hdr[3]);
|
|
*off = (uint16_t)((uint16_t)f->net_hdr[4] << 3);
|
|
f->net_len = (uint16_t)(f->net_len - (uint16_t)size);
|
|
f->len = (uint32_t)(f->len - (uint32_t)size);
|
|
f->net_hdr += size;
|
|
f->start = f->net_hdr;
|
|
return frag_find(*dgram_size, *tag, f);
|
|
}
|
|
|
|
static int32_t
|
|
defrag(struct pico_frame *f)
|
|
{
|
|
uint16_t size = 0, tag = 0, off = 0;
|
|
struct frag_ctx *frag = NULL;
|
|
|
|
if ((f->net_hdr[0] & 0xF8) == FRAG1_DISPATCH) {
|
|
frag = defrag_remove_header(f, &size, &tag, &off, FRAG1_SIZE);
|
|
if (!(f = pico_6lowpan_decompress(f)))
|
|
return -1;
|
|
off = 0;
|
|
} else if ((f->net_hdr[0] & 0xF8) == FRAGN_DISPATCH) {
|
|
frag = defrag_remove_header(f, &size, &tag, &off, FRAGN_SIZE);
|
|
} else {
|
|
lp_dbg("6LP: RCVD invalid frame\n");
|
|
pico_frame_discard(f);
|
|
return -1;
|
|
}
|
|
|
|
if (frag) {
|
|
return defrag_update(frag, off, f);
|
|
} else {
|
|
return defrag_new(f, size, tag, off);
|
|
}
|
|
}
|
|
|
|
static int32_t
|
|
pico_6lowpan_process_in(struct pico_protocol *self, struct pico_frame *f)
|
|
{
|
|
IGNORE_PARAMETER(self);
|
|
|
|
if (f->net_hdr[0] & 0x80) {
|
|
return defrag(f);
|
|
} else {
|
|
f = pico_6lowpan_decompress(f);
|
|
if (f) {
|
|
lp_dbg("6LP: Decompression finished, stats: len: %d net: %d trans: %d\n", f->len, f->net_len, f->transport_len);
|
|
return pico_network_receive(f);
|
|
}
|
|
return -1;
|
|
}
|
|
}
|
|
|
|
int32_t
|
|
pico_6lowpan_pull(struct pico_frame *f)
|
|
{
|
|
if (pico_enqueue(pico_proto_6lowpan.q_in, f) > 0) {
|
|
return (int32_t)f->len; // Success
|
|
}
|
|
|
|
pico_frame_discard(f);
|
|
return -1;
|
|
}
|
|
|
|
struct pico_protocol pico_proto_6lowpan = {
|
|
.name = "6lowpan",
|
|
.layer = PICO_LAYER_DATALINK,
|
|
.process_in = pico_6lowpan_process_in,
|
|
.process_out = pico_6lowpan_process_out,
|
|
.q_in = &pico_6lowpan_in,
|
|
.q_out = &pico_6lowpan_out
|
|
};
|
|
|
|
int pico_6lowpan_init(void)
|
|
{
|
|
pico_6lowpan_ll_init();
|
|
if (0 == pico_timer_add(1000, frag_timeout, NULL)) {
|
|
return -1; /* We care if timer fails, results in memory leak if frames don't get reassembled */
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
#endif /* PICO_SUPPORT_6LOWPAN */
|