int lowpan_frag_get(uint8_t *frag, size_t len, struct ip6_packet *packet, struct ieee154_frame_addr *frame, struct lowpan_ctx *ctx) { uint8_t *buf, *lowpan_buf, *ieee_buf = frag; uint16_t extra_payload; /* pack 802.15.4 */ buf = lowpan_buf = pack_ieee154_header(frag, len, frame); if (ctx->offset == 0) { int offset = 0; #if LIB6LOWPAN_HC_VERSION == -1 /* just copy the ipv6 header around... */ *buf++ = LOWPAN_IPV6_PATTERN; memcpy(buf, &packet->ip6_hdr, sizeof(struct ip6_hdr)); buf += sizeof(struct ip6_hdr); #elif !defined(LIB6LOWPAN_HC_VERSION) || LIB6LOWPAN_HC_VERSION == 6 /* pack the IPv6 header */ buf = lowpan_pack_headers(packet, frame, buf, len - (buf - frag)); if (!buf) return -1; /* pack the next headers */ offset = pack_nhc_chain(&buf, len - (buf - ieee_buf), packet); if (offset < 0) return -2; #endif /* copy the rest of the payload into this fragment */ extra_payload = ntohs(packet->ip6_hdr.ip6_plen) - offset; /* may need to fragment -- insert a FRAG1 header if so */ if (extra_payload > len - (buf - ieee_buf)) { struct packed_lowmsg lowmsg; memmove(lowpan_buf + LOWMSG_FRAG1_LEN, lowpan_buf, buf - lowpan_buf); lowmsg.data = lowpan_buf; lowmsg.len = LOWMSG_FRAG1_LEN; lowmsg.headers = 0; setupHeaders(&lowmsg, LOWMSG_FRAG1_HDR); setFragDgramSize(&lowmsg, ntohs(packet->ip6_hdr.ip6_plen) + sizeof(struct ip6_hdr)); setFragDgramTag(&lowmsg, ctx->tag); lowpan_buf += LOWMSG_FRAG1_LEN; buf += LOWMSG_FRAG1_LEN; extra_payload = len - (buf - ieee_buf); extra_payload -= (extra_payload % 8); } if (iov_read(packet->ip6_data, offset, extra_payload, buf) != extra_payload) { return -3; } ctx->offset = offset + extra_payload + sizeof(struct ip6_hdr); return (buf - frag) + extra_payload; } else { struct packed_lowmsg lowmsg; buf = lowpan_buf = pack_ieee154_header(frag, len, frame); /* setup the FRAGN header */ lowmsg.data = lowpan_buf; lowmsg.len = LOWMSG_FRAGN_LEN; lowmsg.headers = 0; setupHeaders(&lowmsg, LOWMSG_FRAGN_HDR); if (setFragDgramSize(&lowmsg, ntohs(packet->ip6_hdr.ip6_plen) + sizeof(struct ip6_hdr))) return -5; if (setFragDgramTag(&lowmsg, ctx->tag)) return -6; if (setFragDgramOffset(&lowmsg, ctx->offset / 8)) return -7; buf += LOWMSG_FRAGN_LEN; extra_payload = ntohs(packet->ip6_hdr.ip6_plen) + sizeof(struct ip6_hdr) - ctx->offset; if (extra_payload > len - (buf - ieee_buf)) { extra_payload = len - (buf - ieee_buf); extra_payload -= (extra_payload % 8); } if (iov_read(packet->ip6_data, ctx->offset - sizeof(struct ip6_hdr), extra_payload, buf) != extra_payload) { return -4; } ctx->offset += extra_payload; if (extra_payload == 0) return 0; else return (lowpan_buf - ieee_buf) + LOWMSG_FRAGN_LEN + extra_payload; } }
int main() { uint8_t nxt; uint8_t buf[512], result[512], *bufp; uint8_t *bptr = buf; uint8_t *bptr2 = buf; uint8_t *rptr = result; size_t rlen = 512; int i, len; size_t blen = 512; size_t blen2 = 0; struct lowpan_reconstruct recon; int ret; struct ip_iovec v[1]; struct ip6_packet pkt2; uint8_t outbuf[512]; uint8_t *outbufptr = outbuf; size_t outbuflen = 512; uint8_t success = 0; uint8_t total = 0; bufp = buf; packet.ip6_hdr.ip6_nxt = IPV6_HOP; iov_prefix(NULL, &vec[2], (uint8_t *)&udppkt, sizeof(udppkt)); iov_prefix(&vec[2], &vec[1], ip_hdrs[1].hdr, 128); iov_prefix(&vec[1], &vec[0], ip_hdrs[0].hdr, 128); packet.ip6_data = vec; for (i=0; i<128; i++) { printf("%02x", vec[0].iov_base[i]); } printf("\n"); len = pack_nhc_chain(&bufp, &blen, &packet); printf("used %i bytes from source\n", len); printf("[%i] ", 512-blen); if (len < 0) { printf("ERROR: packing chain failed\n"); return 1; } printf("packed: 0x"); for (i = 0; i < 512-blen; i++) { printf("%02x", buf[i]); } printf("\n\n"); blen2 = 512-blen; unpack_nhc_chain(&recon, &rptr, &rlen, &nxt, &bptr2, &blen2); printf("unpacked: 0x"); for (i = 0; i < 512-rlen; i++) printf("%02x", result[i]); printf("\n"); v->iov_base = result; v->iov_len = 512-rlen; v->iov_next = NULL; pkt2.ip6_data = v; pkt2.ip6_hdr.ip6_nxt = IPV6_HOP; len = pack_nhc_chain(&outbufptr, &outbuflen, &pkt2); printf("[xx] packed: 0x"); for (i = 0; i < 512-outbuflen; i++) { printf("%02x", outbuf[i]); } printf("\n"); total++; ret = memcmp(outbuf, buf, 512-outbuflen); if (ret != 0) { printf("ERROR: did not unpack what we packed.\n"); return 1; } else { success++; } printf("%s: %i/%i tests succeeded\n", __FILE__, success, total); }