int lowpan_recon_add(struct lowpan_reconstruct *recon,
uint8_t *pkt, size_t len) {
struct packed_lowmsg msg;
uint8_t *buf;
msg.data = pkt;
msg.len = len;
msg.headers = getHeaderBitmap(&msg);
if (msg.headers == LOWMSG_NALP) return -1;
if (!hasFragNHeader(&msg)) {
return -2;
}
buf = getLowpanPayload(&msg);
len -= (buf - pkt);
if (recon->r_size < recon->r_bytes_rcvd + len) return -3;
/* just need to copy the new payload in and return */
memcpy(recon->r_buf + recon->r_bytes_rcvd, buf, len);
recon->r_bytes_rcvd += len;
return 0;
}
int lowpan_recon_start(struct ieee154_frame_addr *frame_addr,
struct lowpan_reconstruct *recon,
uint8_t *pkt, size_t len) {
uint8_t *unpack_point, *unpack_end;
struct packed_lowmsg msg;
msg.data = pkt;
msg.len = len;
msg.headers = getHeaderBitmap(&msg);
if (msg.headers == LOWMSG_NALP) return -1;
/* remove the 6lowpan headers from the payload */
unpack_point = getLowpanPayload(&msg);
len -= (unpack_point - pkt);
/* set up the reconstruction, or just fill in the packet length */
if (hasFrag1Header(&msg)) {
getFragDgramTag(&msg, &recon->r_tag);
getFragDgramSize(&msg, &recon->r_size);
} else {
recon->r_size = LIB6LOWPAN_MAX_LEN + LOWPAN_LINK_MTU;
}
recon->r_buf = malloc(recon->r_size);
if (!recon->r_buf) return -2;
memset(recon->r_buf, 0, recon->r_size);
recon->r_app_len = NULL;
if (*unpack_point == LOWPAN_IPV6_PATTERN) {
/* uncompressed header... no need to un-hc */
unpack_point++; len --;
memcpy(recon->r_buf, unpack_point, len);
unpack_end = recon->r_buf + len;
} else {
/* unpack the first fragment */
unpack_end = lowpan_unpack_headers(recon,
frame_addr,
unpack_point, len);
}
if (!unpack_end) {
free(recon->r_buf);
return -3;
}
if (!hasFrag1Header(&msg)) {
recon->r_size = (unpack_end - recon->r_buf);
}
recon->r_bytes_rcvd = unpack_end - recon->r_buf;
((struct ip6_hdr *)(recon->r_buf))->ip6_plen =
htons(recon->r_size - sizeof(struct ip6_hdr));
/* fill in any elided app data length fields */
if (recon->r_app_len) {
*recon->r_app_len =
htons(recon->r_size - (recon->r_transport_header - recon->r_buf));
}
/* done, updated all the fields */
/* reconstruction is complete if r_bytes_rcvd == r_size */
return 0;
}
/*
* return the length of the compressed fields in buf
*/
int getCompressedLen(packed_lowmsg_t *pkt) {
// min lenght is DISPATCH + ENCODING
uint8_t encoding, len = 2;
uint8_t *buf = getLowpanPayload(pkt);
if (!(*buf == LOWPAN_HC_LOCAL_PATTERN || *buf == LOWPAN_HC_CRP_PATTERN))
return 0;
encoding = *(buf + 1);
if ((encoding & LOWPAN_IPHC_VTF_MASK) == LOWPAN_IPHC_VTF_INLINE)
len += 4;
if ((encoding & LOWPAN_IPHC_NH_MASK) == LOWPAN_IPHC_NH_INLINE)
len += 1;
if ((encoding &LOWPAN_IPHC_HLIM_MASK) == LOWPAN_IPHC_HLIM_INLINE)
len += 1;
switch ((encoding >> LOWPAN_IPHC_SC_OFFSET) & LOWPAN_IPHC_ADDRFLAGS_MASK) {
case LOWPAN_IPHC_ADDR_128: len += 16; break;
case LOWPAN_IPHC_ADDR_64: len += 8; break;
case LOWPAN_IPHC_ADDR_16: len += 2; break;
case LOWPAN_IPHC_ADDR_0: len += 0; break;
}
switch ((encoding >> LOWPAN_IPHC_DST_OFFSET) & LOWPAN_IPHC_ADDRFLAGS_MASK) {
case LOWPAN_IPHC_ADDR_128: len += 16; break;
case LOWPAN_IPHC_ADDR_64: len += 8; break;
case LOWPAN_IPHC_ADDR_16: len += 2; break;
case LOWPAN_IPHC_ADDR_0: len += 0; break;
}
if ((encoding & LOWPAN_IPHC_NH_MASK) != LOWPAN_IPHC_NH_INLINE) {
// figure out how long the next header encoding is
uint8_t *nh = buf + len;
if ((*nh & LOWPAN_UDP_DISPATCH) == LOWPAN_UDP_DISPATCH) {
// are at a udp packet
len += 1; // add LOWPAN_HCNH
uint8_t udp_enc = *nh;
//printf("udp_enc: 0x%x\n", udp_enc);
if ((udp_enc & LOWPAN_UDP_S_MASK) && (udp_enc & LOWPAN_UDP_D_MASK))
len += 1;
else if ((udp_enc & LOWPAN_UDP_S_MASK) || (udp_enc & LOWPAN_UDP_D_MASK))
len += 3;
else
len += 4;
if ((udp_enc & LOWPAN_UDP_C_MASK) == 0)
len += 2;
}
}
len += (buf - pkt->data);
return len;
}
void *pan_read(void * arg) {
while (run_state == S_RUNNING) {
struct packed_lowmsg lowmsg;
struct ieee154_frame_addr frame_address;
int length;
uint8_t *frame = read_pan_packet(&length);
uint8_t *buf = frame + 1; /* skip the dispatch byte */
if (!frame)
continue;
if (frame[0] != TOS_SERIAL_802_15_4_ID) {
warn("invalid frame received!\n");
goto done;
}
info("serial packet arrived! (len: %i)\n", length);
print_buffer(buf, length);
buf = unpack_ieee154_hdr(buf, &frame_address);
length -= buf - frame;
if (!buf) {
warn("unpacking IEEE154 header failed!\n");
goto done;
}
lowmsg.data = buf;
lowmsg.len = length;
lowmsg.headers = getHeaderBitmap(&lowmsg);
if (lowmsg.headers == LOWMSG_NALP) {
warn("lowmsg NALP!\n");
goto done;
}
if (hasFrag1Header(&lowmsg) || hasFragNHeader(&lowmsg)) {
struct lowpan_reconstruct *recon;
uint16_t tag, source_key;
int rv;
pthread_mutex_lock(&reconstruct_lock);
source_key = ieee154_hashaddr(&frame_address.ieee_src);
getFragDgramTag(&lowmsg, &tag);
recon = get_reconstruct(source_key, tag);
if (!recon) {
pthread_mutex_unlock(&reconstruct_lock);
goto done;
}
if (hasFrag1Header(&lowmsg))
rv = lowpan_recon_start(&frame_address, recon, buf, length);
else rv = lowpan_recon_add(recon, buf, length);
if (rv < 0) {
recon->r_timeout = T_FAILED1;
pthread_mutex_unlock(&reconstruct_lock);
goto done;
} else {
recon->r_timeout = T_ACTIVE;
recon->r_source_key = source_key;
recon->r_tag = tag;
}
if (recon->r_size == recon->r_bytes_rcvd) {
deliver_to_kernel(recon);
}
pthread_mutex_unlock(&reconstruct_lock);
} else {
int rv;
struct lowpan_reconstruct recon;
buf = getLowpanPayload(&lowmsg);
if ((rv = lowpan_recon_start(&frame_address, &recon, buf, length)) < 0) {
warn("reconstruction failed!\n");
goto done;
}
if (recon.r_size == recon.r_bytes_rcvd) {
deliver_to_kernel(&recon);
} else {
free(recon.r_buf);
}
}
done:
free(frame);
}
}
/*
* Unpacks all headers, including any compressed transport headers if
* there is a compression scheme defined for them.
*
* @pkt - the wrapped struct pointing to the compressed headers
* @dest - buffer to unpack the headers into
* @len - the len of 'dest'
* @return the number of bytes written to dest, or zero if decompression failed.
* should be >= sizeof(struct ip6_hdr)
*/
uint8_t *unpackHeaders(packed_lowmsg_t *pkt, unpack_info_t *u_info,
uint8_t *dest, uint16_t len) {
uint8_t dispatch, encoding;
uint16_t size, extra_header_length = 0;
uint8_t *buf = (uint8_t *)getLowpanPayload(pkt);
// pointers to fields we may come back to fill in later
uint8_t *plen, *prot_len, *nxt_hdr;
ip_memclr((void *)u_info, sizeof(unpack_info_t));
// a buffer we can write addresses prefixes and suffexes into.
// now we don't need to check sizes until we get to next headers
if (buf == NULL || len < sizeof(struct ip6_hdr)) return NULL;
len -= sizeof(struct ip6_hdr);
dispatch = *buf; buf++;
encoding = *buf; buf++;
if (dispatch != LOWPAN_HC_LOCAL_PATTERN && dispatch != LOWPAN_HC_CRP_PATTERN)
return NULL;
if ((encoding & LOWPAN_IPHC_VTF_MASK) == LOWPAN_IPHC_VTF_INLINE) {
// copy the inline 4 bytes of fields.
ip_memcpy(dest, buf, 4);
buf += 4;
} else {
// cler the traffic class and flow label fields, and write the version.
ip_memclr(dest, 4);
*dest = IPV6_VERSION << 4;
}
dest += 4;
plen = dest;
prot_len = dest;
// payload length field requires some computation...
dest += 2;
if ((encoding & LOWPAN_IPHC_NH_MASK) == LOWPAN_IPHC_NH_INLINE) {
*dest = *buf;
buf++;
}
nxt_hdr = dest;
dest += 1;
// otherwise, decompress IPNH compression once we reach the end of
// the packed data.
u_info->hlim = NULL;
if ((encoding & LOWPAN_IPHC_HLIM_MASK) == LOWPAN_IPHC_HLIM_INLINE) {
*dest = *buf;
u_info->hlim = buf;
buf++;
}
dest += 1;
// otherwise, follow instructions for reconstructing hop limit once
// destination address is known.
// dest points at the start of the source address IP header field.
decompressAddress(dispatch, pkt->src,
(encoding >> LOWPAN_IPHC_SC_OFFSET) & LOWPAN_IPHC_ADDRFLAGS_MASK,
&buf, dest);
dest += 16;
decompressAddress(dispatch, pkt->src,
(encoding >> LOWPAN_IPHC_DST_OFFSET) & LOWPAN_IPHC_ADDRFLAGS_MASK,
&buf, dest);
dest += 16;
// we're done with the IP headers; time to decompress any compressed
// headers which follow... We need to re-check that there's enough
// buffer to do this.
if ((encoding & LOWPAN_IPHC_NH_MASK) != LOWPAN_IPHC_NH_INLINE) {
// time to decode some next header fields
// we ought to be pointing at the HCNH encoding byte now.
if ((*buf & LOWPAN_UDP_DISPATCH) == LOWPAN_UDP_DISPATCH) {
pkt->headers |= LOWMSG_IPNH_HDR;
if (len < sizeof(struct udp_hdr)) return NULL;
len -= sizeof(struct udp_hdr);
struct udp_hdr *udp = (struct udp_hdr *)dest;
uint8_t udp_enc = *buf;
uint8_t dst_shift = 4;
extra_header_length = sizeof(struct udp_hdr);
buf += 1;
// UDP
*nxt_hdr = IANA_UDP;
if (udp_enc & LOWPAN_UDP_S_MASK) {
// recover from 4 bit packing
udp->srcport = hton16((*buf >> 4) + LOWPAN_UDP_PORT_BASE);
dst_shift = 0;
} else {
