static inline int decompressAddress(uint8_t dispatch, uint16_t src, uint8_t addr_flags, uint8_t **buf, uint8_t *dest) { uint8_t *prefix; uint16_t tmp; int rc = 0; if (dispatch == LOWPAN_HC_LOCAL_PATTERN) prefix = linklocal_prefix; else prefix = __my_address.s6_addr; switch (addr_flags) { case LOWPAN_IPHC_ADDR_128: ip_memcpy(dest, *buf, 16); *buf += 16; break; case LOWPAN_IPHC_ADDR_64: ip_memcpy(dest, prefix, 8); ip_memcpy(dest + 8, *buf, 8); *buf += 8; break; case LOWPAN_IPHC_ADDR_16: rc = decompressShortAddress(dispatch, *buf, dest); *buf += 2; break; case LOWPAN_IPHC_ADDR_0: ip_memcpy(dest, prefix, 8); ip_memclr(dest + 8, 6); tmp = hton16(src); ip_memcpy(dest + 14, (uint8_t *)&tmp, 2); break; } return rc; }
static inline int decompressShortAddress(uint8_t dispatch, uint8_t *s_addr, uint8_t *dest) { if ((*s_addr & LOWPAN_IPHC_SHORT_MASK) == 0) { // simplest case, just use the appropriate prefix. if (dispatch == LOWPAN_HC_LOCAL_PATTERN) ip_memcpy(dest, linklocal_prefix, 8); else ip_memcpy(dest, __my_address.s6_addr, 8); ip_memclr(dest + 8, 8); dest[14] = (*s_addr) & ~LOWPAN_IPHC_SHORT_MASK; dest[15] = *(s_addr + 1); return 0; } // otherwise we either have an invalid compression, or else it's a // multicast address // ip_memcpy(dest, multicast_prefix, 8); ip_memclr(dest, 16); dest[0] = 0xff; dest[1] = 0x02; switch (*s_addr & LOWPAN_IPHC_SHORT_LONG_MASK) { case LOWPAN_IPHC_HC1_MCAST: dest[14] = (*s_addr) & ~LOWPAN_IPHC_SHORT_LONG_MASK; dest[15] = *(s_addr + 1); break; case LOWPAN_IPHC_HC_MCAST: dest[1] = ((*s_addr) & LOWPAN_HC_MCAST_SCOPE_MASK) >> LOWPAN_HC_MCAST_SCOPE_OFFSET; // we'll just direct map the bottom 9 bits in for the moment, // since HC doesn't specify anything that would break this. In // the future, a more complicated mapping is likely. dest[14] = (*s_addr) & 0x1; dest[15] = *(s_addr + 1); break; default: return 1; } return 0; }
/* * 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 {
int main(char **argv, int argc) { uint8_t buf[2000]; uint8_t *payload, *i; struct split_ip_msg msg; uint8_t data[200]; globalPrefix = 1; uint16_t plen = 200; ip_memclr((uint8_t *)&msg.hdr, 40); ip_memcpy(msg.hdr.src_addr, my_address, 16); ip_memcpy(msg.hdr.dst_addr, my_address, 16); msg.hdr.vlfc[0] = 6 << 4; msg.hdr.nxt_hdr = NXTHDR_SOURCE; msg.hdr.hlim = 0x64; msg.hdr.plen = hton16(plen); msg.hdr.dst_addr[0] = 0xff; msg.hdr.dst_addr[1] = 0xfe; msg.hdr.dst_addr[14]= 0x12; msg.hdr.dst_addr[15] = 0xfe; msg.headers = NULL; msg.data = data; msg.data_len = plen - 12; struct udp_hdr udp; udp.srcport = hton16(0xf0d1); udp.dstport = hton16(0xf0e0); udp.len = hton16(plen - 4); udp.chksum = hton16(0x9abc); printBuf(&udp, 8); struct generic_header u_p; u_p.len = sizeof(struct udp_hdr); u_p.hdr.udp = &udp; u_p.next = NULL; struct source_header source; source.nxt_hdr = IANA_UDP; source.len = sizeof(struct source_header); source.current = 0; struct generic_header s_h; s_h.len = sizeof(struct source_header); s_h.hdr.ext = &source; s_h.next = &u_p; msg.headers = &s_h; int j; for (j = 0; j < 200; j++) data[j] = j; fragment_t prog; /* uint8_t r_buf[2000]; */ int rc; /* ip_memclr(r_buf, 1000); */ /* packed_lowmsg_t pkt; */ /* pkt.headers = 0; */ /* pkt.data = buf; */ /* reconstruct_t recon; */ printBuf(&msg.hdr, 40 ); while ((rc =getNextFrag(&msg, &prog, buf, 100)) > 0) { printf("n_frags: %i\n", prog.n_frags); printBuf(buf, rc); } /* printf ("rc: %i\n", rc); */ /* pkt.len = rc; */ /* pkt.headers = getHeaderBitmap(&pkt); */ /* printf("cmprlen: 0x%x\n", getCompressedLen(&pkt)); */ /* printBuf(buf, getCompressedLen(&pkt)); */ /* return; */ /* uint16_t mytag, size; */ /* pkt.headers = getHeaderBitmap(&pkt); */ /* printf("headers: 0x%x\n", pkt.headers); */ /* if (getFragDgramTag(&pkt, &mytag)); */ /* if (getFragDgramSize(&pkt, &size)); */ /* recon.tag = mytag; */ /* recon.size = size; */ /* recon.buf = r_buf; */ /* recon.bytes_rcvd = 0; */ /* addFragment(&pkt, &recon); */ /* printf ("adding fragment len: %i\n", rc); */ /* while (rc > 0) { */ /* ip_memclr(buf, 100); */ /* rc = getNextFrag(msg, &prog, buf, 100); */ /* pkt.len = rc; */ /* pkt.headers = getHeaderBitmap(&pkt); */ /* addFragment(&pkt, &recon); */ /* ip_memclr(buf, 100); */ /* printf ("adding fragment len: %i\n", rc); */ /* } */ /* printf("Reconstructed buffer [%i]:\n", size); */ /* printBuf(&r_buf[10], size); */ /* for (j = 10; j < size; j++) */ /* if (r_buf[j] != buf1[j]) */ /* printf("differ in byte %i (0x%x, 0x%x)\n", j - 10, buf1[j], r_buf[j]); */ return 0; }