--- /dev/null
+/*
+ * "Copyright (c) 2008 The Regents of the University of California.
+ * All rights reserved."
+ *
+ * Permission to use, copy, modify, and distribute this software and its
+ * documentation for any purpose, without fee, and without written agreement is
+ * hereby granted, provided that the above copyright notice, the following
+ * two paragraphs and the author appear in all copies of this software.
+ *
+ * IN NO EVENT SHALL THE UNIVERSITY OF CALIFORNIA BE LIABLE TO ANY PARTY FOR
+ * DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT
+ * OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF
+ * CALIFORNIA HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ * THE UNIVERSITY OF CALIFORNIA SPECIFICALLY DISCLAIMS ANY WARRANTIES,
+ * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
+ * AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS
+ * ON AN "AS IS" BASIS, AND THE UNIVERSITY OF CALIFORNIA HAS NO OBLIGATION TO
+ * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS."
+ *
+ */
+
+#include <string.h>
+#include "lib6lowpan.h"
+/*
+ * This file presents an interface for parsing IP and UDP headers in a
+ * 6loWPAN packet.
+ *
+ * @author Stephen Dawson-Haggerty <stevedh@cs.berkeley.edu>
+ */
+
+uint8_t linklocal_prefix [] = {0xfe, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
+
+struct in6_addr __my_address = {{{0xfe, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x65}}};
+uint8_t globalPrefix = 0;
+
+uint8_t *getLinkLocalPrefix() {
+ return &linklocal_prefix[0];
+}
+
+uint8_t cmpPfx(ip6_addr_t a, uint8_t *pfx) {
+ return (memcmp(a, pfx, 8) == 0);
+#if 0
+ (a[0] == pfx[0] &&
+ a[1] == pfx[1] &&
+ a[2] == pfx[2] &&
+ a[3] == pfx[3] &&
+ a[4] == pfx[4] &&
+ a[5] == pfx[5] &&
+ a[6] == pfx[6] &&
+ a[7] == pfx[7]);
+#endif
+}
+
+static inline int ipv6_addr_suffix_is_long(const ip6_addr_t addr) {
+ return (!(addr[8] == 0 &&
+ addr[9] == 0 &&
+ addr[10] == 0 &&
+ addr[11] == 0 &&
+ addr[12] == 0 &&
+ addr[13] == 0));
+}
+
+#if LIB6LOWPAN_FULL
+/*
+ * 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;
+}
+#endif
+
+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;
+}
+
+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;
+}
+
+void adjustPlen(struct ip6_hdr *ip, unpack_info_t *u_info) {
+ uint16_t adjust_amt = u_info->payload_offset;
+
+ switch (ip->nxt_hdr) {
+ case IANA_UDP:
+ adjust_amt -= sizeof(struct udp_hdr); break;
+ }
+ ip->plen = htons(ntohs(ip->plen) - adjust_amt);
+}
+
+/*
+ * 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 {
+ ip_memcpy((uint8_t *)&udp->srcport, buf, 2);
+ buf += 2;
+ }
+
+ if (udp_enc & LOWPAN_UDP_D_MASK) {
+ udp->dstport = hton16((((*buf >> dst_shift)) & 0x0f) + LOWPAN_UDP_PORT_BASE);
+ buf += 1;
+ } else {
+ if (dst_shift == 0) buf += 1;
+ ip_memcpy((uint8_t *)&udp->dstport, buf, 2);
+ buf += 2;
+ }
+
+ if (udp_enc & LOWPAN_UDP_C_MASK) {
+ // we elided the checksum and so are supposed to recompute it here.
+ // however, we won't do this.
+ } else {
+ ip_memcpy((uint8_t *)&udp->chksum, buf, 2);
+ buf += 2;
+ }
+
+ // still must fill in the length field, but we must first
+ // recompute the IP length, which we couldn't do until now.
+ // lamers...
+ prot_len = (uint8_t *)&udp->len;
+ dest += sizeof(struct udp_hdr);
+
+ u_info->nxt_hdr = IANA_UDP;
+ u_info->payload_offset += sizeof(struct udp_hdr);
+ u_info->transport_ptr = (uint8_t *)udp;
+
+ } else {
+ // otherwise who knows what's here... it's an error because the
+ // NH bit said we were inline but when we got here, we didn't
+ // recognize the NH encoding.
+ return NULL;
+ }
+ } else {
+ // there was no IPNH field, but there might be uncompressed fields
+ // we want to copy out for consistency (since we always unpack all
+ // headers not part of the payload).
+ uint8_t nhdr = *nxt_hdr;
+ uint8_t nhdr_len = 0;
+ struct ip6_ext *hdr;
+ u_info->nxt_hdr = nhdr;
+
+ // copy any IPv6 extension headers out of the packet.
+ // the rule is that the extension headers must fit in the first
+ // fragment so we can route on them after only one fragment.
+ while (EXTENSION_HEADER(nhdr)) {
+ hdr = (struct ip6_ext *)buf;
+
+ switch (nhdr) {
+ case IPV6_HOP:
+ u_info->hdr_hop = (struct ip6_ext *)buf;
+ break;
+ case IPV6_ROUTING:
+ u_info->hdr_route = (struct ip6_route *)buf;
+ break;
+ case IPV6_DEST:
+ u_info->hdr_dest = (struct ip6_ext *)buf;
+ break;
+ }
+ nhdr = hdr->nxt_hdr;
+ nhdr_len = hdr->len;
+ u_info->nxt_hdr = nhdr;
+
+ if (len < nhdr_len) return NULL;
+ ip_memcpy(dest, buf, nhdr_len);
+ dest += nhdr_len;
+ buf += nhdr_len;
+
+ u_info->payload_offset += nhdr_len;
+ extra_header_length += nhdr_len;
+ }
+
+ u_info->transport_ptr = dest;
+ }
+
+ u_info->payload_start = buf;
+ u_info->header_end = dest;
+ if (u_info->transport_ptr == NULL)
+ u_info->transport_ptr = dest;
+
+ // we can go back and figure out the payload length now that we know
+ // how long the compressed headers were
+ if (hasFrag1Header(pkt) || hasFragNHeader(pkt)) {
+ getFragDgramSize(pkt, &size);
+ size -= sizeof(struct ip6_hdr);
+ } else {
+ // it's a one fragment packet
+ size = pkt->len - (buf - pkt->data);
+ size += extra_header_length;
+ size -= getLowpanPayload(pkt) - pkt->data;
+ }
+
+ *plen = size >> 8;
+ *(plen + 1) = size & 0xff;
+
+ // finally fill in the udp length field that we finally can recompute
+ switch (*nxt_hdr) {
+ case IANA_UDP:
+ *prot_len = size >> 8;
+ *(prot_len + 1) = size & 0xff;
+ }
+
+
+ return buf;
+}
+
+/* packs addr into *buf, and updates the pointer relative to the length
+ * that was needed.
+ * @returns the bit flags indicating which length was used
+ */
+static uint8_t packAddress(uint8_t dispatch, uint8_t **buf, ip6_addr_t addr) {
+
+ if ((dispatch == LOWPAN_HC_CRP_PATTERN && globalPrefix &&
+ cmpPfx(addr, __my_address.s6_addr)) ||
+ (dispatch == LOWPAN_HC_LOCAL_PATTERN &&
+ cmpPfx(addr, linklocal_prefix))) {
+ // only choice here are 64-bit and 16-bit addresses
+ if (ipv6_addr_suffix_is_long(addr)) {
+ // use the 64-bit compression
+ ip_memcpy(*buf, &addr[8], 8);
+ *buf += 8;
+ return LOWPAN_IPHC_ADDR_64;
+ } else {
+ // down to 16 bits: we never use the 0-bit compression
+ // (althought we could for link local).
+ ip_memcpy(*buf, &addr[14], 2);
+ *buf += 2;
+ return LOWPAN_IPHC_ADDR_16;
+ }
+ } else if (addr[0] == 0xff && // is multicast
+ addr[1] < 0x0f) { // the scope is small enough
+ // XXX : SDH : Need to check that the group is small enough
+ **buf = LOWPAN_IPHC_HC_MCAST; // set the encoding bits
+ **buf |= (addr[1] << LOWPAN_HC_MCAST_SCOPE_OFFSET); // scope
+
+ // direct mapped group id
+ **buf |= addr[14] & 0x1;
+ *(*buf + 1) = addr[15];
+ *buf += 2;
+ return LOWPAN_IPHC_ADDR_16;
+ } else {
+ // fuck it, send the whole thing
+ ip_memcpy(*buf, addr, 16);
+ *buf += 16;
+ return LOWPAN_IPHC_ADDR_128;
+ }
+}
+
+/*
+ * pack the headers of msg into the buffer pointed to by buf.
+ *
+ * @returns a pointer to where we stopped writing
+ */
+uint8_t packHeaders(struct split_ip_msg *msg,
+ uint8_t *buf, uint8_t len) {
+ uint8_t *dispatch, *encoding, addr_enc, nxt_hdr;
+ struct ip6_hdr *hdr = &msg->hdr;
+ dispatch = buf;
+ buf += 1;
+ encoding = buf;
+ buf += 1;
+ *encoding = 0;
+
+ if (!(hdr->vlfc[0] == (IPV6_VERSION << 4) &&
+ hdr->vlfc[1] == 0 &&
+ hdr->vlfc[2] == 0 &&
+ hdr->vlfc[3] == 0)) {
+ ip_memcpy(buf, &hdr->vlfc, 4);
+ buf += 4;
+ } else {
+ *encoding |= LOWPAN_IPHC_VTF_MASK;
+ }
+
+ nxt_hdr = hdr->nxt_hdr;
+ if (hdr->nxt_hdr == IANA_UDP && /* or other compressed values... */
+ msg->headers != NULL) {
+ // we will add the HCNH encoding at the end of the header
+ *encoding |= LOWPAN_IPHC_NH_MASK;
+ } else {
+ *buf = hdr->nxt_hdr;
+ buf += 1;
+ }
+
+ // always carry hop limit
+ *buf = hdr->hlim;
+ buf += 1;
+
+ if (globalPrefix && cmpPfx(hdr->ip6_src.s6_addr, __my_address.s6_addr)) {
+ *dispatch = LOWPAN_HC_CRP_PATTERN;
+ } else if (globalPrefix && cmpPfx(hdr->ip6_dst.s6_addr, __my_address.s6_addr)) {
+ *dispatch = LOWPAN_HC_CRP_PATTERN;
+ } else if (cmpPfx(hdr->ip6_src.s6_addr, linklocal_prefix)) {
+ *dispatch = LOWPAN_HC_LOCAL_PATTERN;
+ } else {
+ *dispatch = LOWPAN_HC_LOCAL_PATTERN;
+ }
+
+ addr_enc = packAddress(*dispatch, &buf, hdr->ip6_src.s6_addr);
+ *encoding |= addr_enc << LOWPAN_IPHC_SC_OFFSET;
+
+ addr_enc = packAddress(*dispatch, &buf, hdr->ip6_dst.s6_addr);
+ *encoding |= addr_enc << LOWPAN_IPHC_DST_OFFSET;
+
+ len -= (buf - dispatch);
+ // now come the compressions for special next header values.
+ // we pack all the headers in the split message into this fragment, and fail if we cannot;
+ {
+ int i = 0;
+ struct generic_header *cur = msg->headers;
+ while (cur != NULL) {
+ if (nxt_hdr == IANA_UDP && i == 0) {
+ struct udp_hdr *udp = cur->hdr.udp;
+
+ uint8_t *udp_enc = buf;
+ uint8_t *cmpr_port = NULL;
+ // do the LOWPAN_UDP coding
+
+ if (len < sizeof(struct udp_hdr)) return (buf - dispatch);
+
+ *udp_enc = LOWPAN_UDP_DISPATCH;;
+ buf += 1;
+
+
+ if ((ntoh16(udp->srcport) & LOWPAN_UDP_PORT_BASE_MASK) ==
+ LOWPAN_UDP_PORT_BASE) {
+ //printf("compr to 4b\n");
+ cmpr_port = buf;
+ *cmpr_port = (ntoh16(udp->srcport) & ~LOWPAN_UDP_PORT_BASE_MASK) << 4;
+ *udp_enc |= LOWPAN_UDP_S_MASK;
+ buf += 1;
+ } else {
+ ip_memcpy(buf, (uint8_t *)&udp->srcport, 2);
+ buf += 2;
+ }
+
+ if ((ntoh16(udp->dstport) & LOWPAN_UDP_PORT_BASE_MASK) ==
+ LOWPAN_UDP_PORT_BASE) {
+ if (cmpr_port == NULL) {
+ // the source port must not have been compressed, so
+ // allocate a new byte for this guy
+ *buf = ((ntoh16(udp->dstport) & ~LOWPAN_UDP_PORT_BASE_MASK) << 4);
+ buf += 1;
+ } else {
+ // already in the middle of a byte for the port compression,
+ // so fill in the rest of the byte
+ *cmpr_port = *cmpr_port | ((ntoh16(udp->dstport) & ~LOWPAN_UDP_PORT_BASE_MASK));
+ }
+ *udp_enc |= LOWPAN_UDP_D_MASK;
+ } else {
+ ip_memcpy(buf, (uint8_t *)&udp->dstport, 2);
+ buf += 2;
+ }
+
+ // we never elide the checksum
+ ip_memcpy(buf, (uint8_t *)&udp->chksum, 2);
+ buf += 2;
+ } else {
+ // otherwise we just need to copy the extension header
+ if (len < cur->len) return 0;
+ ip_memcpy(buf, (uint8_t *)cur->hdr.ext, cur->len);
+ len -= cur->len;
+ buf += cur->len;
+ }
+ cur = cur->next;
+ i++;
+ }
+ }
+ // I think we're done here...
+ return buf - dispatch;
+}
+
+/*
+ * indicates how much of the packet after the IP header we will pack
+ *
+ */
+/* int packs_header(struct split_ip_msg *msg) { */
+/* switch (hdr->nxt_hdr) { */
+/* case IANA_UDP: */
+/* return sizeof(struct udp_hdr); */
+/* default: */
+/* return 0; */
+/* } */
+/* } */
+
+
+
+#ifndef NO_LIB6LOWPAN_ASCII
+
+#define TO_CHAR(X) (((X) < 10) ? ('0' + (X)) : ('a' + ((X) - 10)))
+#define CHAR_VAL(X) (((X) >= '0' && (X) <= '9') ? ((X) - '0') : \
+ (((X) >= 'A' && (X) <= 'F') ? ((X) - 'A' + 10) : ((X) - 'a' + 10)))
+
+
+void inet_pton6(char *addr, struct in6_addr *dest) {
+ uint16_t cur = 0;
+ char *p = addr;
+ uint8_t block = 0, shift = 0;
+ if (addr == NULL || dest == NULL) return;
+ ip_memclr(dest->s6_addr, 16);
+
+ // first fill in from the front
+ while (*p != '\0') {
+ if (*p != ':') {
+ cur <<= 4;
+ cur |= CHAR_VAL(*p);
+ } else {
+ dest->s6_addr16[block++] = hton16(cur);
+ cur = 0;
+ }
+ p++;
+ if (*p == '\0') {
+ dest->s6_addr16[block++] = hton16(cur);
+ return;
+ }
+ if (*(p - 1) == ':' && *p == ':') {
+ break;
+ }
+ }
+ // we must have hit a "::" which means we need to start filling in from the end.
+ block = 7;
+ cur = 0;
+ while (*p != '\0') p++;
+ p--;
+ // now pointing at the end of the address string
+ while (p > addr) {
+ if (*p != ':') {
+ cur |= (CHAR_VAL(*p) << shift);
+ shift += 4;
+ } else {
+ dest->s6_addr16[block--] = hton16(cur);
+ cur = 0; shift = 0;
+ }
+ p --;
+ if (*(p + 1) == ':' && *p == ':') break;
+ }
+}
+
+
+
+int inet_ntop6(struct in6_addr *addr, char *buf, int cnt) {
+ uint16_t block;
+ char *end = buf + cnt;
+ int i, j, compressed = 0;
+
+ for (j = 0; j < 8; j++) {
+ if (buf > end - 7) return -1;
+
+ block = ntohs(addr->s6_addr16[j]);
+ for (i = 4; i <= 16; i+=4) {
+ if (block > (0xffff >> i) || (compressed == 2 && i == 16)) {
+ *buf++ = TO_CHAR((block >> (16 - i)) & 0xf);
+ }
+ }
+ if (addr->s6_addr16[j] == 0 && compressed == 0) {
+ *buf++ = ':';
+ compressed++;
+ }
+ if (addr->s6_addr16[j] != 0 && compressed == 1) compressed++;
+
+ if (j < 7 && compressed != 1) *buf++ = ':';
+ }
+ *buf++ = '\0';
+ return buf - (end - cnt);
+}
+
+#endif
projects / tinyos-2.x.git / blobdiff