[Tinyos-2-commits] CVS: tinyos-2.x/support/sdk/c/blip/lib6lowpan Makefile, NONE, 1.1 in_cksum.c, NONE, 1.1 ip_malloc.c, NONE, 1.1 lib6lowpan.c, NONE, 1.1 lib6lowpanFrag.c, NONE, 1.1 lib6lowpanIP.c, NONE, 1.1 printpacket.c, NONE, 1.1
sdhsdh
sdhsdh at users.sourceforge.net
Mon Jan 19 16:28:17 PST 2009
- Previous message: [Tinyos-2-commits] CVS: tinyos-2.x/support/sdk/c/blip/include 6lowpan.h, NONE, 1.1 IEEE154.h, NONE, 1.1 devconf.h, NONE, 1.1 in_cksum.h, NONE, 1.1 ip.h, NONE, 1.1 ip_malloc.h, NONE, 1.1 lib6lowpan.h, NONE, 1.1
- Next message: [Tinyos-2-commits] CVS: tinyos-2.x/support/sdk/c/blip/libtcp Makefile, NONE, 1.1 circ.c, NONE, 1.1 circ.h, NONE, 1.1 tcplib.c, NONE, 1.1 tcplib.h, NONE, 1.1 test_circ.c, NONE, 1.1 test_server.c, NONE, 1.1
- Messages sorted by:
[ date ]
[ thread ]
[ subject ]
[ author ]
Update of /cvsroot/tinyos/tinyos-2.x/support/sdk/c/blip/lib6lowpan
In directory ddv4jf1.ch3.sourceforge.com:/tmp/cvs-serv6583/support/sdk/c/blip/lib6lowpan
Added Files:
Makefile in_cksum.c ip_malloc.c lib6lowpan.c lib6lowpanFrag.c
lib6lowpanIP.c printpacket.c
Log Message:
- initial commit of blip (berkeley low-power ip) stack
--- NEW FILE: Makefile ---
SOURCES=lib6lowpan.c lib6lowpanIP.c lib6lowpanFrag.c
INCLUDE=../include/
###############
##
## Use these for PC
ifndef GCC
GCC=gcc
endif
ifndef AR
AR=ar
endif
CFLAGS=-DPC -g -I$(INCLUDE)
TEST=testhc.c
###############
##
## These for msp430
# GCC=msp430-gcc
# AR=msp430-ar
# TEST=msp_test.c
# CFLAGS=-DMSP
LIB=lib6lowpan.a
OBJS=$(SOURCES:.c=.o)
$(LIB): $(OBJS)
$(AR) crv $(LIB) $(OBJS)
test: $(SOURCES) $(TEST) $(LIB)
$(GCC) -o $@ $(TEST) $(CFLAGS) $(LIB) printpacket.c
%.o: %.c
$(GCC) -o $@ $< $(CFLAGS) -c
clean:
rm -f test $(OBJS) $(LIB)
lib6lowpan.o: lib6lowpan.c ip-hdrs
lib6lowpanIP.o: lib6lowpanIP.c ip-hdrs
lib6lowpanFrag.o: lib6lowpanFrag.c ip-hdrs
ip-hdrs: $(INCLUDE)/6lowpan.h $(INCLUDE)/ip.h $(INCLUDE)/lib6lowpan.h
--- NEW FILE: in_cksum.c ---
/*
* "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."
*
*/
/* in_cksum.c
* 4.4-Lite-2 Internet checksum routine, modified to take a vector of
* pointers/lengths giving the pieces to be checksummed.
*
* $Id: in_cksum.c,v 1.1 2009/01/20 00:28:15 sdhsdh Exp $
*/
/*
* Copyright (c) 1988, 1992, 1993
* The Regents of the University of California. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* 3. Neither the name of the University nor the names of its contributors
* may be used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
* OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
* SUCH DAMAGE.
*
* @(#)in_cksum.c 8.1 (Berkeley) 6/10/93
*/
#include <stdlib.h>
#include "in_cksum.h"
#include "lib6lowpan.h"
#define ADDCARRY(x) (x > 65535 ? x -= 65535 : x)
#define REDUCE {l_util.l = sum; sum = l_util.s[0] + l_util.s[1]; ADDCARRY(sum);}
int
in_cksum(const vec_t *vec, int veclen) {
uint32_t sum = 0;
uint16_t res = 0;
uint16_t cur = 0;
int i;
uint8_t *w;
for (; veclen != 0; vec++, veclen--) {
if (vec->len == 0)
continue;
w = (uint8_t *)vec->ptr;
for (i = 0; i < vec->len; i++) {
if (i % 2 == 0) {
cur |= ((uint16_t)w[i]) << 8;
if (i + 1 == vec->len) {
goto finish;
}
} else {
cur |= w[i];
finish:
sum += cur;
res = (sum & 0xffff) + (sum >> 16);
cur = 0;
}
}
}
return ~res ;
#if 0
register const uint16_t *w;
register uint32_t sum = 0;
register uint32_t mlen = 0;
int byte_swapped = 0;
union {
uint8_t c[2];
uint16_t s;
} s_util;
union {
uint16_t s[2];
uint32_t l;
} l_util;
for (; veclen != 0; vec++, veclen--) {
if (vec->len == 0)
continue;
w = (const uint16_t *)vec->ptr;
if (mlen == -1) {
/*
* The first byte of this chunk is the continuation
* of a word spanning between this chunk and the
* last chunk.
*
* s_util.c[0] is already saved when scanning previous
* chunk.
*/
s_util.c[1] = *(const uint8_t *)w;
sum += s_util.s;
w = (const uint16_t *)((const uint8_t *)w + 1);
mlen = vec->len - 1;
} else
mlen = vec->len;
/*
* Force to even boundary.
*/
if ((1 & (int) w) && (mlen > 0)) {
REDUCE;
sum <<= 8;
s_util.c[0] = *(const uint8_t *)w;
w = (const uint16_t *)((const uint8_t *)w + 1);
mlen--;
byte_swapped = 1;
}
/*
* Unroll the loop to make overhead from
* branches &c small.
*/
while ((mlen -= 32) >= 0) {
sum += w[0]; sum += w[1]; sum += w[2]; sum += w[3];
sum += w[4]; sum += w[5]; sum += w[6]; sum += w[7];
sum += w[8]; sum += w[9]; sum += w[10]; sum += w[11];
sum += w[12]; sum += w[13]; sum += w[14]; sum += w[15];
w += 16;
}
mlen += 32;
while ((mlen -= 8) >= 0) {
sum += w[0]; sum += w[1]; sum += w[2]; sum += w[3];
w += 4;
}
mlen += 8;
if (mlen == 0 && byte_swapped == 0)
continue;
REDUCE;
while ((mlen -= 2) >= 0) {
sum += *w++;
}
if (byte_swapped) {
REDUCE;
sum <<= 8;
byte_swapped = 0;
if (mlen == -1) {
s_util.c[1] = *(const uint8_t *)w;
sum += s_util.s;
mlen = 0;
} else
mlen = -1;
} else if (mlen == -1)
s_util.c[0] = *(const uint8_t *)w;
}
if (mlen == -1) {
/* The last mbuf has odd # of bytes. Follow the
standard (the odd byte may be shifted left by 8 bits
or not as determined by endian-ness of the machine) */
s_util.c[1] = 0;
sum += s_util.s;
}
REDUCE;
return (~sum & 0xffff);
#endif
}
/* SDH : Added to allow for friendly message checksumming */
uint16_t msg_cksum(struct split_ip_msg *msg, uint8_t nxt_hdr) {
struct generic_header *cur;
int n_headers = 4;
vec_t cksum_vec[7];
uint32_t hdr[2];
cksum_vec[0].ptr = (uint8_t *)(msg->hdr.ip6_src.s6_addr);
cksum_vec[0].len = 16;
cksum_vec[1].ptr = (uint8_t *)(msg->hdr.ip6_dst.s6_addr);
cksum_vec[1].len = 16;
cksum_vec[2].ptr = (uint8_t *)hdr;
cksum_vec[2].len = 8;
hdr[0] = msg->data_len;
hdr[1] = htonl(nxt_hdr);
cksum_vec[3].ptr = msg->data;
cksum_vec[3].len = msg->data_len;
cur = msg->headers;
while (cur != NULL) {
cksum_vec[n_headers].ptr = cur->hdr.data;
cksum_vec[n_headers].len = cur->len;
hdr[0] += cur->len;
n_headers++;
cur = cur->next;
}
hdr[0] = htonl(hdr[0]);
return in_cksum(cksum_vec, n_headers);
}
--- NEW FILE: ip_malloc.c ---
#ifndef NO_IP_MALLOC
#include <stdint.h>
#include <stdio.h>
#include "ip_malloc.h"
uint8_t heap[IP_MALLOC_HEAP_SIZE];
void ip_malloc_init() {
bndrt_t *b = (bndrt_t *)heap;
*b = IP_MALLOC_HEAP_SIZE & IP_MALLOC_LEN;
}
void *ip_malloc(uint16_t sz) {
bndrt_t *cur = (bndrt_t *)heap;
sz += sizeof(bndrt_t) * 2;
sz += (sz % IP_MALLOC_ALIGN);
while (((*cur & IP_MALLOC_LEN) < sz || (*cur & IP_MALLOC_INUSE) != 0)
&& (uint8_t *)cur - heap < IP_MALLOC_HEAP_SIZE) {
cur = (bndrt_t *)(((uint8_t *)cur) + ((*cur) & IP_MALLOC_LEN));
}
if ((uint8_t *)cur < heap + IP_MALLOC_HEAP_SIZE) {
uint16_t oldsize = *cur & IP_MALLOC_LEN;
bndrt_t *next;
sz -= sizeof(bndrt_t);
next = ((bndrt_t *)(((uint8_t *)cur) + sz));
*cur = (sz & IP_MALLOC_LEN) | IP_MALLOC_INUSE;
*next = (oldsize - sz) & IP_MALLOC_LEN;
return cur + 1;
} else return NULL;
}
void ip_free(void *ptr) {
bndrt_t *prev = NULL, *cur, *next = NULL;
cur = (bndrt_t *)heap;
while (cur + 1 != ptr && (uint8_t *)cur - heap < IP_MALLOC_HEAP_SIZE) {
prev = cur;
cur = (bndrt_t *)(((uint8_t *)cur) + ((*cur) & IP_MALLOC_LEN));
}
if (cur + 1 == ptr) {
next = (bndrt_t *)((*cur & IP_MALLOC_LEN) + ((uint8_t *)cur));
*cur &= ~IP_MALLOC_INUSE;
if ((((uint8_t *)next) - heap) < IP_MALLOC_HEAP_SIZE &&
(*next & IP_MALLOC_INUSE) == 0) {
*cur = (*cur & IP_MALLOC_LEN) + (*next & IP_MALLOC_LEN);
}
if (prev != NULL && (*prev & IP_MALLOC_INUSE) == 0) {
*prev = (*prev & IP_MALLOC_LEN) + (*cur & IP_MALLOC_LEN);
}
}
}
uint16_t ip_malloc_freespace() {
uint16_t ret = 0;
bndrt_t *cur = (bndrt_t *)heap;
while ((uint8_t *)cur - heap < IP_MALLOC_HEAP_SIZE) {
if ((*cur & IP_MALLOC_INUSE) == 0)
ret += *cur & IP_MALLOC_LEN;
cur = (bndrt_t *)(((uint8_t *)cur) + ((*cur) & IP_MALLOC_LEN));
}
return ret;
}
#ifdef PC
void dump_heap() {
int i;
for (i = 0; i < IP_MALLOC_HEAP_SIZE; i++) {
printf("0x%x ", heap[i]);
if (i % 8 == 7) printf(" ");
if (i % 16 == 15) printf ("\n");
if (i > 64) break;
}
printf("\n");
}
void ip_print_heap() {
bndrt_t *cur = (bndrt_t *)heap;
while (((uint8_t *)cur) - heap < IP_MALLOC_HEAP_SIZE) {
printf ("heap region start: 0x%x length: %i used: %i\n",
cur, (*cur & IP_MALLOC_LEN), (*cur & IP_MALLOC_INUSE) >> 15);
if ((*cur & IP_MALLOC_LEN) == 0) {
printf("ERROR: zero length cell detected!\n");
dump_heap();
exit(1);
}
cur = (bndrt_t *)(((uint8_t *)cur) + ((*cur) & IP_MALLOC_LEN));
}
}
#endif
#endif
--- NEW FILE: lib6lowpan.c ---
/*
* "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.
*/
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.
*
*/
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
*/
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, hw_addr_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, hw_addr_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, hw_addr_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, hw_addr_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;
}
--- NEW FILE: lib6lowpanFrag.c ---
/*
* "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."
*
*/
/*
* @author Stephen Dawson-Haggerty <stevedh at cs.berkeley.edu>
*/
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include "ip.h"
#include "ip_malloc.h"
#include "6lowpan.h"
#include "lib6lowpan.h"
#define min(a,b) ( ((a)>(b)) ? (b) : (a) )
#define max(a,b) ( ((a)<(b)) ? (b) : (a) )
uint16_t lib6lowpan_frag_tag = 0;
void ip_memclr(uint8_t *buf, uint16_t len) {
for (; len > 0; len--)
*buf++ = 0;
}
void *ip_memcpy(void *dst0, const void *src0, uint16_t len) {
uint8_t *dst = (uint8_t *) dst0;
uint8_t *src = (uint8_t *) src0;
void *ret = dst0;
for (; len > 0; len--)
*dst++ = *src++;
return ret;
}
/*
* this function writes the next fragment which needs to be sent into
* the buffer passed in. It updates the structures in process to
* reflect how much of the packet has been sent so far.
*
* if the packet does not require fragmentation, this function will
* not insert a fragmentation header and will merely compress the
* headers into the packet.
*
* returns the number of bytes used in buf, or zero if there was no
* fragment to be sent.
*
*/
uint8_t getNextFrag(struct split_ip_msg *msg, fragment_t *progress,
uint8_t *buf, uint16_t len) {
if (msg == NULL || progress == NULL || buf == NULL) return 0;
packed_lowmsg_t pkt;
uint16_t frag_length = 0;
pkt.headers = 0;
pkt.data = buf;
pkt.len = len;
// if this is the first fragment, we will compress the headers in
// the ip message, and only insert a fragmentation header if
// necessary to pack it into buffer
if (progress->offset == 0) {
uint8_t *compressed_headers;
uint8_t lowpan_len = 0;
uint8_t cmpr_header_len = 0, header_length = 0;
compressed_headers = malloc(LIB6LOWPAN_MAX_LEN);
if (compressed_headers == NULL) return 0;
// pack the headers into a temporary buffer
cmpr_header_len = packHeaders(msg, compressed_headers, LIB6LOWPAN_MAX_LEN);
{
struct generic_header *cur = msg->headers;
while (cur != NULL) {
header_length += cur->len;
cur = cur->next;
}
}
// printBuf(compressed_headers, compressed_len);
// printf("payload: %p\n", payload);
// maybe add a fragmentation header
if (cmpr_header_len + msg->data_len > len) {
pkt.headers |= LOWMSG_FRAG1_HDR;
if (setupHeaders(&pkt, pkt.headers)) goto free_fail;
if (setFragDgramTag(&pkt, ++lib6lowpan_frag_tag)) goto free_fail;
if (setFragDgramSize(&pkt, ntoh16(msg->hdr.plen) + sizeof(struct ip6_hdr))) goto free_fail;
lowpan_len += LOWMSG_FRAG1_LEN;
} else {
if (setupHeaders(&pkt, pkt.headers)) goto free_fail;
}
ip_memcpy(getLowpanPayload(&pkt), compressed_headers, cmpr_header_len);
/*
* calculate how much to put into this fragment
*
* if the whole packet fits in the buffer, this is easy; it's the
* compressed headers plus the payload length.
*
* given fragmentation, we need to do a little more work.
*/
if (pkt.headers & LOWMSG_FRAG1_HDR) {
frag_length = len - cmpr_header_len - LOWMSG_FRAG1_LEN + sizeof(struct ip6_hdr) + header_length;
frag_length -= (frag_length % 8);
frag_length -= (sizeof(struct ip6_hdr) + header_length);
} else {
frag_length = ntoh16(msg->hdr.plen) - header_length;
}
// frag_length contains the number of bytes in uncompressed headers.
ip_memcpy(getLowpanPayload(&pkt) + cmpr_header_len,
msg->data, frag_length);
progress->tag = lib6lowpan_frag_tag;
progress->offset = frag_length + sizeof(struct ip6_hdr) + header_length;
// printfUART("frag: 0x%x 0x%x 0x%x\n", header_len, frag_length, compressed_len);
// return the length we wrote, which comes in three pieces;
free(compressed_headers);
return lowpan_len + cmpr_header_len + frag_length;
free_fail:
free(compressed_headers);
goto fail;
} else {
/*
* we've already sent the first fragment; we only need to send a
* subsequent one or return zero if we're at the end of the buffer;
*
*/
//printf("offset: 0x%x plen: 0x%x\n", progress->offset, ntoh16(ip->plen) + sizeof(struct ip6_hdr));
// NOTE : this should be a >= to detect runaway lengths. However,
// it is useful for debugging to require equality.
if (progress->offset == ntoh16(msg->hdr.plen) + sizeof(struct ip6_hdr)) return 0;
// the only headers we need now is the fragn header.
pkt.headers |= LOWMSG_FRAGN_HDR;
// send out the fragments some frasgments.
//printf ("--- sending fragment\n");
// now we're pointing at the start of the 6loWPAN frame in the packet.
pkt.data = buf;
pkt.len = len;
// setup the fragmentation headers
if (setupHeaders(&pkt, pkt.headers)) goto fail;
if (setFragDgramTag(&pkt, progress->tag)) goto fail;
//printf ("frag dgram size 0x%x progress: 0x%x\n", ntoh16(ip->plen) + sizeof(struct ip6_hdr),
/* progress->offset); */
if (setFragDgramSize(&pkt, ntoh16(msg->hdr.plen) + sizeof(struct ip6_hdr))) goto fail;
if (setFragDgramOffset(&pkt, (progress->offset) / 8)) goto fail;
frag_length = min(len - LOWMSG_FRAGN_LEN,
ntoh16(msg->hdr.plen) + sizeof(struct ip6_hdr) - progress->offset);
// unless this is the last fragment, we must sent a multiple of 8 bytes;
if (frag_length + progress->offset != ntoh16(msg->hdr.plen) + sizeof(struct ip6_hdr))
frag_length -= (frag_length % 8);
pkt.len = frag_length + LOWMSG_FRAGN_LEN;
{
uint8_t header_length = sizeof(struct ip6_hdr);
struct generic_header *cur = msg->headers;
while (cur != NULL) {
header_length += cur->len;
cur = cur->next;
}
ip_memcpy(buf + LOWMSG_FRAGN_LEN, msg->data + progress->offset - header_length, frag_length);
}
progress->offset += frag_length;
//printf("frag length is: 0x%x offset: 0x%x max: 0x%x\n", frag_length, progress->offset, LOWPAN_MTU);
return frag_length + LOWMSG_FRAGN_LEN;
}
fail:
return 0;
}
--- NEW FILE: lib6lowpanIP.c ---
/*l
* "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 at cs.berkeley.edu>
*/
uint8_t linklocal_prefix [] = {0xfe, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
uint8_t multicast_prefix [] = {0xff, 0x02, 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 (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]);
}
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));
}
/*
* 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;
}
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 + 8, 8);
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;
}
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;
}
/*
* 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;
u_info->payload_offset = 0;
u_info->rih = NULL;
u_info->sh = NULL;
u_info->transport_ptr = NULL;
// 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;
while (KNOWN_HEADER(nhdr)) {
hdr = (struct ip6_ext *)buf;
u_info->nxt_hdr = nhdr;
switch (nhdr) {
case IANA_UDP:
nhdr = NXTHDR_UNKNOWN;
nhdr_len = sizeof(struct udp_hdr);
u_info->transport_ptr = dest;
break;
case NXTHDR_SOURCE:
u_info->sh = (struct source_header *)buf;
nhdr = hdr->nxt_hdr;
nhdr_len = hdr->len;
u_info->nxt_hdr = nhdr;
break;
case NXTHDR_INSTALL:
// this is how to handle all ipv6 options headers: should we
// use "default" here?
u_info->rih = (struct rinstall_header *)buf;
default:
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->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
*/
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... */) {
// 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; */
/* } */
/* } */
#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')
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;
}
}
--- NEW FILE: printpacket.c ---
/*
* "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 <stdint.h>
#include <stdio.h>
// #include <arpa/inet.h>
#include "6lowpan.h"
#include "lib6lowpan.h"
#include "lib6lowpanIP.h"
void printBuf(uint8_t *buf, uint16_t len) {
int i;
for (i = 1; i <= len; i++) {
printf(" 0x%02x", buf[i-1]);
if (i % 16 == 0) printf("\n");
}
printf("\n\n");
}
void printPacket(uint8_t *buf, int len) {
uint8_t val;
uint16_t origin, final;
packed_lowmsg_t pkt;
// used for autoconfiguration; would be provided by lower layers.
pkt.src = 0xaa;
pkt.dst = 0xbb;
printBuf(buf, len);
pkt.data = buf;
pkt.len = len;
pkt.headers = getHeaderBitmap(&pkt);
printf("6loWPAN Packet (headers: 0x%x)\n", pkt.headers);
if (hasBcastHeader(&pkt)) {
getBcastSeqno(&pkt, &val);
printf(" BCast seqno: 0x%x\n", val);
}
if (hasMeshHeader(&pkt)) {
getMeshHopsLeft(&pkt, &val);
getMeshOriginAddr(&pkt, &origin);
getMeshFinalAddr(&pkt, &final);
printf(" Mesh hops: 0x%x origin: 0x%x final: 0x%x\n", val, origin, final);
}
if (hasFrag1Header(&pkt) || hasFragNHeader(&pkt)) {
getFragDgramSize(&pkt, &origin);
getFragDgramTag(&pkt, &final);
printf(" Frag size: 0x%x tag: 0x%x\n", origin, final);
}
if (hasFragNHeader(&pkt)) {
getFragDgramOffset(&pkt, &val);
printf(" Frag offset: 0x%x\n", val);
}
uint8_t data[100];
struct ip6_hdr *h = (struct ip6_hdr *)data;
unpackHeaders(&pkt, (uint8_t *)h, 100);
printf(" ip first bytes: 0x%x\n", ntohl(*((uint32_t *)h->vlfc)));
printf(" plen: 0x%x next: 0x%x hlim: 0x%x\n", ntohs(h->plen), h->nxt_hdr, h->hlim);
printf(" src: ");
for (val = 0; val < 16; val++)
printf("0x%x ", h->src_addr[val]);
printf("\n dst: ");
for (val = 0; val < 16; val++)
printf("0x%x ", h->dst_addr[val]);
printf("\n");
if (h->nxt_hdr == IANA_UDP) {
struct udp_hdr *udp = (struct udp_hdr *)&data[sizeof(struct ip6_hdr)];
printf("udp src: 0x%x dst: 0x%x len: 0x%x cksum: 0x%x\n",
ntoh16(udp->srcport), ntoh16(udp->dstport),
ntoh16(udp->len), ntoh16(udp->chksum));
}
printf("\n\n");
}
- Previous message: [Tinyos-2-commits] CVS: tinyos-2.x/support/sdk/c/blip/include 6lowpan.h, NONE, 1.1 IEEE154.h, NONE, 1.1 devconf.h, NONE, 1.1 in_cksum.h, NONE, 1.1 ip.h, NONE, 1.1 ip_malloc.h, NONE, 1.1 lib6lowpan.h, NONE, 1.1
- Next message: [Tinyos-2-commits] CVS: tinyos-2.x/support/sdk/c/blip/libtcp Makefile, NONE, 1.1 circ.c, NONE, 1.1 circ.h, NONE, 1.1 tcplib.c, NONE, 1.1 tcplib.h, NONE, 1.1 test_circ.c, NONE, 1.1 test_server.c, NONE, 1.1
- Messages sorted by:
[ date ]
[ thread ]
[ subject ]
[ author ]
More information about the Tinyos-2-commits
mailing list