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