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