lib6lowpan.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 "6lowpan.h"
#include "ip.h"
#include "lib6lowpan.h"


#ifndef __6LOWPAN_16BIT_ADDRESS
#error "Only 16-bit short addresses are supported"
#endif

/*
 *  Library implementation of packing of 6lowpan packets.  
 *
 *  This should allow uniform code treatment between pc and mote code;
 *  the goal is to write ANSI C here...  This means no nx_ types,
 *  unfortunately.
 *
 *  Accessing fields programtically is probably a little less
 *  efficient, but that can be improved.  By precomputing the packet
 *  headers present, we can make the overhead not too bad.  The #1
 *  goal of this library is portability and readability.
 *
 *  The broadcast and mesh headers may or may not be useful, and are
 *  off by default to reduce code size.  Removing them reduces the
 *  library size by about 600 bytes.
 */


/*
 * Return the length (in bytes) of the buffer required to pack lowmsg
 * into a buffer.
 */
inline uint8_t *getLowpanPayload(packed_lowmsg_t *lowmsg) {
  uint8_t len = 0;
#if LIB6LOWPAN_FULL
  if (lowmsg->headers & LOWMSG_MESH_HDR)
    len += LOWMSG_MESH_LEN;
  if (lowmsg->headers & LOWMSG_BCAST_HDR)
    len += LOWMSG_BCAST_LEN;
#endif
  if (lowmsg->headers & LOWMSG_FRAG1_HDR)
    len += LOWMSG_FRAG1_LEN;
  if (lowmsg->headers & LOWMSG_FRAGN_HDR)
    len += LOWMSG_FRAGN_LEN;
  return lowmsg->data + len;
}

/*
 * Return a bitmap indicating which lowpan headers are
 *  present in the message pointed to by lowmsg.
 *
 */
inline uint16_t getHeaderBitmap(packed_lowmsg_t *lowmsg) {
  uint16_t headers = 0;
  uint8_t *buf = lowmsg->data;
  uint16_t len = lowmsg->len;
  if (buf == NULL) return headers;

  if (len > 0 && ((*buf) >> 6) == LOWPAN_NALP_PATTERN) {
    return LOWMSG_NALP;
  }
  
#if LIB6LOWPAN_FULL
  if (len > 0 && ((*buf) >> 6) == LOWPAN_MESH_PATTERN) {
    if (!(*buf & LOWPAN_MESH_V_MASK) ||
        !(*buf & LOWPAN_MESH_F_MASK)) {
      // we will not parse a packet with 64-bit addressing.
      return LOWMSG_NALP;
    }
    headers |= LOWMSG_MESH_HDR;
    buf += LOWMSG_MESH_LEN;
    len -= LOWMSG_MESH_LEN;
  }
  if (len > 0 && (*buf) == LOWPAN_BCAST_PATTERN) {
    headers |= LOWMSG_BCAST_HDR;
    buf += LOWMSG_BCAST_LEN;
    len -= LOWMSG_BCAST_LEN;
  }
#endif 

  if (len > 0 && ((*buf) >> 3) == LOWPAN_FRAG1_PATTERN) {
    headers |= LOWMSG_FRAG1_HDR;
    buf += LOWMSG_FRAG1_LEN;
    len -= LOWMSG_FRAG1_LEN;
  }
  if (len > 0 && ((*buf) >> 3) == LOWPAN_FRAGN_PATTERN) {
    headers |= LOWMSG_FRAGN_HDR;
    buf += LOWMSG_FRAGN_LEN;
    len -= LOWMSG_FRAGN_LEN;
  }
  return headers;
}

/*
 * Fill in dispatch values
 */
inline uint8_t setupHeaders(packed_lowmsg_t *packed, uint16_t headers) {
  uint8_t *buf = packed->data;
  uint16_t len = packed->len;
  if (packed == NULL) return 1;
  if (buf == NULL) return 1;
  packed->headers = 0;
#if LIB6LOWPAN_FULL
  if (headers & LOWMSG_MESH_HDR)  {
    if (len < LOWMSG_MESH_LEN) return 1;
    packed->headers |= LOWMSG_MESH_HDR;
    *buf = LOWPAN_MESH_PATTERN << 6 | LOWPAN_MESH_V_MASK | LOWPAN_MESH_F_MASK;
    buf += LOWMSG_MESH_LEN;
    len -= LOWMSG_MESH_LEN;
  }
  if (headers & LOWMSG_BCAST_HDR) {
    if (len < LOWMSG_BCAST_LEN) return 1;
    packed->headers |= LOWMSG_BCAST_HDR;
    *buf = LOWPAN_BCAST_PATTERN;
    buf += LOWMSG_BCAST_LEN;
    len -= LOWMSG_BCAST_LEN;
  }
#endif
  if (headers & LOWMSG_FRAG1_HDR) {
    if (len < LOWMSG_FRAG1_HDR) return 1;
    packed->headers |= LOWMSG_FRAG1_HDR;
    *buf = LOWPAN_FRAG1_PATTERN << 3;
    buf += LOWMSG_FRAG1_LEN;
    len -= LOWMSG_FRAG1_LEN;
  }
  if (headers & LOWMSG_FRAGN_HDR) {
    if (len < LOWMSG_FRAGN_HDR) return 1;
    packed->headers |= LOWMSG_FRAGN_HDR;
    *buf = LOWPAN_FRAGN_PATTERN << 3;
  }
  return 0;
  
}

/*
 * Test if various headers are present are enabled
 */
#ifdef LIB6LOWPAN_FULL
inline uint8_t hasMeshHeader(packed_lowmsg_t *msg) {
  return (msg->headers & LOWMSG_MESH_HDR);
}
inline uint8_t hasBcastHeader(packed_lowmsg_t *msg) {
  return (msg->headers & LOWMSG_BCAST_HDR);
}
#endif
inline uint8_t hasFrag1Header(packed_lowmsg_t *msg) {
  return (msg->headers & LOWMSG_FRAG1_HDR);
}
inline uint8_t hasFragNHeader(packed_lowmsg_t *msg) {
  return (msg->headers & LOWMSG_FRAGN_HDR);
}
#ifdef LIB6LOWPAN_FULL
/*
 * Mesh header fields
 *
 *  return FAIL if the message doesn't have a mesh header
 */
inline uint8_t getMeshHopsLeft(packed_lowmsg_t *msg, uint8_t *hops) {
  uint8_t *buf = msg->data;
  if (!hasMeshHeader(msg) || msg->data == NULL || hops == NULL) return 1;
  *hops = (*buf) & LOWPAN_MESH_HOPS_MASK;
  return 0;
}
inline uint8_t getMeshOriginAddr(packed_lowmsg_t *msg, ieee154_saddr_t *origin) {
  uint8_t *buf = msg->data;
  if (!hasMeshHeader(msg) || msg->data == NULL || origin == NULL) return 1;
  // skip 64-bit addresses
  if (!(*buf & LOWPAN_MESH_V_MASK)) return 1;
  buf += 1;
  *origin = ntoh16(*((uint16_t *)buf));
  return 0;
}
inline uint8_t getMeshFinalAddr(packed_lowmsg_t *msg, ieee154_saddr_t *final) {
  uint8_t *buf = msg->data;
  if (!hasMeshHeader(msg) || msg->data == NULL || final == NULL) return 1;
  // skip 64-bit addresses
  if (!(*buf & LOWPAN_MESH_F_MASK)) return 1;
  buf += 3;
  *final = ntoh16(*((uint16_t *)buf));
  return 0;
}


inline uint8_t setMeshHopsLeft(packed_lowmsg_t *msg, uint8_t hops) {
  uint8_t *buf = msg->data;
  if (!hasMeshHeader(msg) || msg->data == NULL) return 1;
  
  *buf = 0xb0;
  *buf |= hops & LOWPAN_MESH_HOPS_MASK;
  return 0;
}
inline uint8_t setMeshOriginAddr(packed_lowmsg_t *msg, ieee154_saddr_t origin) {
  uint8_t *buf = msg->data;
  if (!hasMeshHeader(msg) || msg->data == NULL) return 1;
  // skip 64-bit addresses
  if (!(*buf & LOWPAN_MESH_V_MASK)) return 1;
  buf += 1;
  *((uint16_t *)buf) = hton16(origin);
  return 0;
}
inline uint8_t setMeshFinalAddr(packed_lowmsg_t *msg, ieee154_saddr_t final) {
  uint8_t *buf = msg->data;
  if (!hasMeshHeader(msg) || msg->data == NULL) return 1;
  // skip 64-bit addresses
  if (!(*buf & LOWPAN_MESH_F_MASK)) return 1;
  buf += 3;
  *((uint16_t *)buf) = hton16(final);
  return 0;
}
 
/*
 * Broadcast header fields
 */
inline uint8_t getBcastSeqno(packed_lowmsg_t *msg, uint8_t *seqno) {
  uint8_t *buf = msg->data;
  if (buf == NULL || seqno == NULL || !hasBcastHeader(msg)) return 1;
  if (hasMeshHeader(msg)) buf += LOWMSG_MESH_LEN;
  if (*buf != LOWPAN_BCAST_PATTERN) return 2;
  buf += 1;
  *seqno = *buf;
  return 0;
}

inline uint8_t setBcastSeqno(packed_lowmsg_t *msg, uint8_t seqno) {
  uint8_t *buf = msg->data;
  if (buf == NULL || !hasBcastHeader(msg)) return 1;
  if (hasMeshHeader(msg)) buf += LOWMSG_MESH_LEN;
  if (*buf != LOWPAN_BCAST_PATTERN) return 2;
  buf += 1;
  *buf = seqno;
  return 0;
}
#endif

/*
 * Fragmentation header fields
 */
inline uint8_t getFragDgramSize(packed_lowmsg_t *msg, uint16_t *size) {
  uint8_t *buf = msg->data;
  uint8_t s[2];
  if (buf == NULL || size == NULL) return 1;
#ifdef LIB6LOWPAN_FULL
  if (hasMeshHeader(msg)) buf += LOWMSG_MESH_LEN;
  if (hasBcastHeader(msg)) buf += LOWMSG_BCAST_LEN;
#endif
  if ((*buf >> 3) != LOWPAN_FRAG1_PATTERN &&
      (*buf >> 3) != LOWPAN_FRAGN_PATTERN) return 1;

  s[0] = *buf & 0x7;
  buf++;
  s[1] = *buf;
  *size = ntoh16 ( *(uint16_t *)s);
  return 0;
}
inline uint8_t getFragDgramTag(packed_lowmsg_t *msg, uint16_t *tag) {
  uint8_t *buf = msg->data;
  if (buf == NULL || tag == NULL) return 1;
#ifdef LIB6LOWPAN_FULL
  if (hasMeshHeader(msg)) buf += LOWMSG_MESH_LEN;
  if (hasBcastHeader(msg)) buf += LOWMSG_BCAST_LEN;
#endif
  if ((*buf >> 3) != LOWPAN_FRAG1_PATTERN &&
      (*buf >> 3) != LOWPAN_FRAGN_PATTERN) return 1;
  buf += 2;
  //*tag = (*buf << 8) | *(buf + 1);  ;
  *tag = ntoh16( *(uint16_t *)buf);
  return 0;
}
inline uint8_t getFragDgramOffset(packed_lowmsg_t *msg, uint8_t *size) {
  uint8_t *buf = msg->data;
  if (buf == NULL || size == NULL) return 1;
#ifdef LIB6LOWPAN_FULL
  if (hasMeshHeader(msg)) buf += LOWMSG_MESH_LEN;
  if (hasBcastHeader(msg)) buf += LOWMSG_BCAST_LEN;
#endif
  if ((*buf >> 3) != LOWPAN_FRAGN_PATTERN) return 1;
  buf += 4;
  *size = *buf;
  return 0;

}


inline uint8_t setFragDgramSize(packed_lowmsg_t *msg, uint16_t size) {
  uint8_t *buf = msg->data;
  if (buf == NULL) return 1;
#ifdef LIB6LOWPAN_FULL
  if (hasMeshHeader(msg)) buf += LOWMSG_MESH_LEN;
  if (hasBcastHeader(msg)) buf += LOWMSG_BCAST_LEN;
#endif
  if ((*buf >> 3) != LOWPAN_FRAG1_PATTERN &&
      (*buf >> 3) != LOWPAN_FRAGN_PATTERN) return 1;
  // zero out the dgram size first.
  *buf &= 0xf8;
  *(buf + 1) = 0;
  *((uint16_t *)buf) |= hton16(size & 0x7ff);
  return 0;
}

inline uint8_t setFragDgramTag(packed_lowmsg_t *msg, uint16_t tag) {
  uint8_t *buf = msg->data;
  if (buf == NULL) return 1;
#ifdef LIB6LOWPAN_FULL
  if (hasMeshHeader(msg)) buf += LOWMSG_MESH_LEN;
  if (hasBcastHeader(msg)) buf += LOWMSG_BCAST_LEN;
#endif
  
  if ((*buf >> 3) != LOWPAN_FRAG1_PATTERN &&
      (*buf >> 3) != LOWPAN_FRAGN_PATTERN) return 1;
  buf += 2;
  *(uint16_t *)buf = ntoh16(tag);
  return 0;
}
inline uint8_t setFragDgramOffset(packed_lowmsg_t *msg, uint8_t size) {
  uint8_t *buf = msg->data;
  if (buf == NULL) return 1;
#ifdef LIB6LOWPAN_FULL
  if (hasMeshHeader(msg)) buf += LOWMSG_MESH_LEN;
  if (hasBcastHeader(msg)) buf += LOWMSG_BCAST_LEN;
#endif

  if ((*buf >> 3) != LOWPAN_FRAGN_PATTERN) return 1;
  buf += 4;
  *buf = size;
  return 0;
}