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 {