lib6lowpanip.c

tinyos-2.x.rar

/*
 * "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 <[e-mail]>
 */

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;