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