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;
}
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);
}
}
