RIOT-OS/RIOT
RIOT-OS/RIOT
1
0
Fork 0
mirror of https://github.com/RIOT-OS/RIOT.git synced 2024-12-29 04:50:03 +01:00
Code Releases Activity
d9ff08b23a
RIOT/sys/net/sixlowpan/sixlowpan.c

1011 lines
38 KiB
C
Raw Blame History

#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <vtimer.h>
#include <timex.h>
#include <debug.h>
#include <thread.h>
#include <mutex.h>
#include "msg.h"
#include "sixlowmac.h"
#include "sixlowpan.h"
#include "sixlowborder.h"
#include "sixlowip.h"
#include "sixlownd.h"
#include "transceiver.h"
#include "ieee802154_frame.h"
uint16_t packet_length;
uint8_t packet_dispatch;
uint16_t tag;
uint8_t header_size = 0;
uint8_t max_frame = 0;
uint8_t max_frag_initial = 0;
uint8_t position;
uint8_t max_frag;
struct ipv6_hdr_t *ipv6_buf;
/* length of compressed packet */
uint16_t comp_len;
uint8_t frag_size;
uint8_t reas_buf[512];
uint8_t comp_buf[512];
uint8_t byte_offset;
uint8_t first_frag = 0;
unsigned int ip_process_pid;
unsigned int nd_nbr_cache_rem_pid = 0;
unsigned int contexts_rem_pid = 0;
iface_t iface;
ipv6_addr_t lladdr;
ieee_802154_long_t laddr;
mutex_t buf_mutex;
mutex_t lowpan_context_mutex;
char ip_process_buf[IP_PROCESS_STACKSIZE];
char nc_buf[NC_STACKSIZE];
char con_buf[CON_STACKSIZE];
lowpan_context_t contexts[LOWPAN_CONTEXT_MAX];
uint8_t context_len = 0;
void lowpan_context_auto_remove(void);
/* deliver packet to mac*/
void lowpan_init(ieee_802154_long_t *addr, uint8_t *data){
ipv6_buf = get_ipv6_buf();
uint8_t mcast = 0;
memcpy(&laddr.uint8[0], &addr->uint8[0], 8);
if(ipv6_prefix_mcast_match(&ipv6_buf->destaddr)){
/* send broadcast */
mcast = 1;
}
//#ifdef LOWPAN_IPHC
lowpan_iphc_encoding(&laddr);
data = &comp_buf[0];
packet_length = comp_len;
//#endif
//#ifndef LOWPAN_IPHC
// lowpan_ipv6_set_dispatch(data);
//#endif
/* check if packet needs to be fragmented */
if(packet_length + header_size > PAYLOAD_SIZE - IEEE_802154_MAX_HDR_LEN){
uint8_t fragbuf[packet_length + header_size];
uint8_t remaining;
uint8_t i = 2;
/* first fragment */
max_frame = PAYLOAD_SIZE - IEEE_802154_MAX_HDR_LEN;
max_frag_initial = ((max_frame - 4 - header_size) / 8) * 8;
memcpy(fragbuf + 4, data, max_frag_initial);
fragbuf[0] = (((0xc0 << 8) | packet_length) >> 8) & 0xff;
fragbuf[1] = ((0xc0 << 8) | packet_length) & 0xff;
fragbuf[2] = (tag >> 8) & 0xff;
fragbuf[3] = tag & 0xff;
send_ieee802154_frame(&laddr,(uint8_t*)&fragbuf,
max_frag_initial + header_size + 4, mcast);
/* subsequent fragments */
position = max_frag_initial;
max_frag = ((max_frame - 5) / 8) * 8;
data += position;
while(packet_length - position > max_frame - 5){
memset(&fragbuf,0,packet_length + header_size);
memcpy(fragbuf + 5, data, max_frag);
fragbuf[0] = (((0xe0 << 8) | packet_length) >> 8) & 0xff;
fragbuf[1] = ((0xe0 << 8) | packet_length) & 0xff;
fragbuf[2] = (tag >> 8) & 0xff;
fragbuf[3] = tag & 0xff;
fragbuf[4] = position / 8;
send_ieee802154_frame(&laddr,(uint8_t*)&fragbuf, max_frag + 5,
mcast);
data += max_frag;
position += max_frag;
i++;
}
remaining = packet_length - position;
memset(&fragbuf,0,packet_length + header_size);
memcpy(fragbuf + 5, data, remaining);
fragbuf[0] = (((0xe0 << 8) | packet_length) >> 8) & 0xff;
fragbuf[1] = ((0xe0 << 8) | packet_length) & 0xff;
fragbuf[2] = (tag >> 8) & 0xff;
fragbuf[3] = tag & 0xff;
fragbuf[4] = position / 8;
send_ieee802154_frame(&laddr, (uint8_t*)&fragbuf, remaining + 5, mcast);
} else {
send_ieee802154_frame(&laddr, data, packet_length, mcast);
}
tag++;
}
void lowpan_read(uint8_t *data, uint8_t length, ieee_802154_long_t *s_laddr,
ieee_802154_long_t *d_laddr){
/* check if packet is fragmented */
msg_t m;
uint8_t hdr_length = 0;
uint8_t datagram_offset = 0;
uint16_t datagram_size = 0;
uint16_t datagram_tag = 0;
/* check first 5-bit*/
switch(data[0] & 0xf8) {
/* first fragment */
case(0xc0):{
first_frag = 1;
datagram_offset = 0;
/* get 11-bit from first 2 byte*/
datagram_size = (((uint16_t)(data[0] << 8)) | data[1]) & 0x07ff;
/* get 16-bit datagram tag */
datagram_tag = (((uint16_t)(data[2] << 8)) | data[3]);
/* discard fragment header */
hdr_length += 4;
/* set global variable frag_size to size of decompr. packet*/
frag_size = datagram_size;
/* check dispatch byte */
switch(data[hdr_length]) {
case(LOWPAN_IPV6_DISPATCH):{
break;
}
default:
break;
}
memcpy(reas_buf, data + hdr_length, length - 4);
break;
}
/* subsequent fragment */
case(0xe0):{
if(first_frag == 0){
printf("ERROR: first fragment not received\n");
break;
}
/* get 11-bit from first 2 byte*/
datagram_size = (((uint16_t)(data[0] << 8)) | data[1]) & 0x07ff;
/* get 16-bit datagram tag */
datagram_tag = (((uint16_t)(data[2] << 8)) | data[3]);
/* get 8-bit datagram offset */
datagram_offset = data[4];
/* discard framentation header */
hdr_length += 5;
frag_size = length - hdr_length;
byte_offset = datagram_offset * 8;
if((frag_size % 8) != 0){
if((byte_offset + frag_size) != datagram_size){
printf("ERROR: received invalid fragment\n");
return;
}
}
memcpy(reas_buf + byte_offset, data + hdr_length, frag_size);
if((byte_offset + frag_size) == datagram_size){
if(reas_buf[0] == LOWPAN_IPV6_DISPATCH) {
/* mutex lock here */
// mutex_lock(&buf_mutex);
ipv6_buf = get_ipv6_buf();
memcpy(ipv6_buf, reas_buf + 1, datagram_size - 1);
m.content.ptr = (char*) ipv6_buf;
packet_length = datagram_size - 1;
msg_send(&m,ip_process_pid, 1);
} else if((reas_buf[0] & 0xe0) == LOWPAN_IPHC_DISPATCH) {
/* mutex lock */
// mutex_lock(&buf_mutex);
lowpan_iphc_decoding(reas_buf, datagram_size, s_laddr, d_laddr);
ipv6_buf = get_ipv6_buf();
m.content.ptr = (char*) ipv6_buf;
msg_send(&m,ip_process_pid, 1);
} else {
printf("ERROR: packet with unknown dispatch received\n");
}
}
break;
}
default:{
if(data[0] == LOWPAN_IPV6_DISPATCH){
/* mutex lock here */
// mutex_lock(&buf_mutex);
ipv6_buf = get_ipv6_buf();
memcpy(ipv6_buf, data + 1, length - 1);
m.content.ptr = (char*) ipv6_buf;
packet_length = length - 1;
msg_send(&m,ip_process_pid, 1);
break;
} else if((data[0] & 0xe0) == LOWPAN_IPHC_DISPATCH){
/* mutex lock here */
// mutex_lock(&buf_mutex);
lowpan_iphc_decoding(data, length, s_laddr, d_laddr);
ipv6_buf = get_ipv6_buf();
m.content.ptr = (char*) ipv6_buf;
msg_send(&m,ip_process_pid, 1);
break;
} else {
printf("ERROR: packet with unknown dispatch received\n");
break;
}
}
}
}
void lowpan_ipv6_set_dispatch(uint8_t *data){
memmove(data + 1, data, packet_length);
data[0] = LOWPAN_IPV6_DISPATCH;
packet_length++;
}
/* draft-ietf-6lowpan-hc-13#section-3.1 */
void lowpan_iphc_encoding(ieee_802154_long_t *dest){
ipv6_buf = get_ipv6_buf();
uint16_t payload_length = ipv6_buf->length;
uint8_t lowpan_iphc[2];
uint8_t *ipv6_hdr_fields = &comp_buf[2];
lowpan_context_t *con = NULL;
uint16_t hdr_pos = 0;
uint8_t tc;
memset(&lowpan_iphc, 0, 2);
/* set iphc dispatch */
lowpan_iphc[0] = LOWPAN_IPHC_DISPATCH;
/* TF: Traffic Class, Flow Label:
* first we need to change DSCP and ECN because in 6lowpan-nd-13 these
* fields are reverse, the original order is DSCP/ECN (rfc 3168) */
tc = (ipv6_buf->version_trafficclass << 4) | (ipv6_buf->trafficclass_flowlabel >> 4);
tc = (tc >> 2) | (tc << 6);
if((ipv6_buf->flowlabel == 0) &&
(ipv6_buf->trafficclass_flowlabel & 0x0f) == 0){
/* flowlabel is elided */
lowpan_iphc[0] |= LOWPAN_IPHC_FL_C;
if(((ipv6_buf->version_trafficclass & 0x0f) == 0) &&
((ipv6_buf->trafficclass_flowlabel & 0xf0) == 0)){
/* traffic class is elided */
lowpan_iphc[0] |= LOWPAN_IPHC_TC_C;
} else {
/* ECN + DSCP (1 byte), Flow Label is elided */
ipv6_hdr_fields[hdr_pos] = tc;
hdr_pos++;
}
} else {
/* flowlabel not compressible */
if(((ipv6_buf->version_trafficclass & 0x0f) == 0) &&
((ipv6_buf->trafficclass_flowlabel & 0xf0) == 0)){
/* traffic class is elided */
lowpan_iphc[0] |= LOWPAN_IPHC_TC_C;
/* ECN + 2-bit Pad + Flow Label (3 bytes), DSCP is elided */
ipv6_hdr_fields[hdr_pos] = ((tc & 0xc0) |
(ipv6_buf->trafficclass_flowlabel & 0x0f));
memcpy(&(ipv6_hdr_fields[hdr_pos]), &ipv6_buf->flowlabel , 2);
hdr_pos += 3;
} else {
/* ECN + DSCP + 4-bit Pad + Flow Label (4 bytes) */
memcpy(&ipv6_hdr_fields[hdr_pos], &ipv6_buf->version_trafficclass, 4);
ipv6_hdr_fields[hdr_pos] = tc;
hdr_pos += 4;
}
}
/* NH: Next Header:
* TODO: NHC */
ipv6_hdr_fields[hdr_pos] = ipv6_buf->nextheader;
hdr_pos++;
/* HLIM: Hop Limit: */
switch(ipv6_buf->hoplimit){
case(1):{
/* 01: The Hop Limit field is compressed and the hop limit is 1. */
lowpan_iphc[0] |= 0x01;
break;
}
case(64):{
/* 10: The Hop Limit field is compressed and the hop limit is 64. */
lowpan_iphc[0] |= 0x02;
break;
}
case(255):{
/* 11: The Hop Limit field is compressed and the hop limit is 255. */
lowpan_iphc[0] |= 0x03;
break;
}
default:{
ipv6_hdr_fields[hdr_pos] = ipv6_buf->hoplimit;
hdr_pos++;
break;
}
}
mutex_lock(&lowpan_context_mutex);
/* CID: Context Identifier Extension: */
if((lowpan_context_lookup(&ipv6_buf->srcaddr) != NULL ) ||
(lowpan_context_lookup(&ipv6_buf->destaddr) != NULL)){
lowpan_iphc[1] |= LOWPAN_IPHC_CID;
memmove(&ipv6_hdr_fields[1],&ipv6_hdr_fields[0], hdr_pos);
hdr_pos++;
}
/* SAC: Source Address Compression */
if(ipv6_addr_unspec_match(&(ipv6_buf->srcaddr))){
/* SAC = 1 and SAM = 00 */
lowpan_iphc[1] |= LOWPAN_IPHC_SAC;
} else if((con = lowpan_context_lookup(&ipv6_buf->srcaddr)) != NULL) {
/* 1: Source address compression uses stateful, context-based
* compression. */
lowpan_iphc[1] |= LOWPAN_IPHC_SAC;
ipv6_hdr_fields[0] |= (con->num << 4);
if(memcmp(&(ipv6_buf->srcaddr.uint8[8]),&(iface.laddr.uint8[0]), 8) == 0){
/* 0 bits. The address is derived using context information
* and possibly the link-layer addresses.*/
lowpan_iphc[1] |= 0x30;
} else if((ipv6_buf->srcaddr.uint16[4] == 0) &&
(ipv6_buf->srcaddr.uint16[5] == 0) &&
(ipv6_buf->srcaddr.uint16[6] == 0) &&
((ipv6_buf->srcaddr.uint8[14]) & 0x80) == 0){
/* 49-bit of interface identifier are 0, so we can compress
* source address-iid to 16-bit */
memcpy(&ipv6_hdr_fields[hdr_pos], &ipv6_buf->srcaddr.uint16[7], 2);
hdr_pos += 2;
/* 16 bits. The address is derived using context information
* and the 16 bits carried inline. */
lowpan_iphc[1] |= 0x20;
} else {
memcpy(&ipv6_hdr_fields[hdr_pos], &(ipv6_buf->srcaddr.uint16[4]),8);
hdr_pos += 8;
/* 64 bits. The address is derived using context information
* and the 64 bits carried inline. */
lowpan_iphc[1] |= 0x10;
}
} else if(ipv6_prefix_ll_match(&ipv6_buf->srcaddr)){
/* 0: Source address compression uses stateless compression.*/
if(memcmp(&(ipv6_buf->srcaddr.uint8[8]),&(iface.laddr.uint8[0]), 8) == 0){
/* 0 bits. The address is derived using context information
* and possibly the link-layer addresses.*/
lowpan_iphc[1] |= 0x30;
} else if((ipv6_buf->srcaddr.uint16[4] == 0) &&
(ipv6_buf->srcaddr.uint16[5] == 0) &&
(ipv6_buf->srcaddr.uint16[6] == 0) &&
((ipv6_buf->srcaddr.uint8[14]) & 0x80) == 0){
/* 49-bit of interface identifier are 0, so we can compress
* source address-iid to 16-bit */
memcpy(&ipv6_hdr_fields[hdr_pos], &ipv6_buf->srcaddr.uint16[7], 2);
hdr_pos += 2;
/* 16 bits. The address is derived using context information
* and the 16 bits carried inline. */
lowpan_iphc[1] |= 0x20;
} else {
memcpy(&ipv6_hdr_fields[hdr_pos], &(ipv6_buf->srcaddr.uint16[4]),8);
hdr_pos += 8;
/* 64 bits. The address is derived using context information
* and the 64 bits carried inline. */
lowpan_iphc[1] |= 0x10;
}
} else {
/* full address carried inline */
memcpy(&ipv6_hdr_fields[hdr_pos], &(ipv6_buf->srcaddr.uint8[0]), 16);
hdr_pos += 16;
}
/* M: Multicast Compression */
if(ipv6_prefix_mcast_match(&ipv6_buf->destaddr)){
/* 1: Destination address is a multicast address. */
lowpan_iphc[1] |= LOWPAN_IPHC_M;
/* just another cool if condition */
if((ipv6_buf->destaddr.uint8[1] == 2) &&
(ipv6_buf->destaddr.uint16[1] == 0) &&
(ipv6_buf->destaddr.uint16[2] == 0) &&
(ipv6_buf->destaddr.uint16[3] == 0) &&
(ipv6_buf->destaddr.uint16[4] == 0) &&
(ipv6_buf->destaddr.uint16[5] == 0) &&
(ipv6_buf->destaddr.uint16[6] == 0) &&
(ipv6_buf->destaddr.uint8[14] == 0)){
/* 11: 8 bits. The address takes the form FF02::00XX. */
lowpan_iphc[1] |= 0x03;
ipv6_hdr_fields[hdr_pos] = ipv6_buf->destaddr.uint8[15];
hdr_pos++;
} else if((ipv6_buf->destaddr.uint16[1] == 0) &&
(ipv6_buf->destaddr.uint16[2] == 0) &&
(ipv6_buf->destaddr.uint16[3] == 0) &&
(ipv6_buf->destaddr.uint16[4] == 0) &&
(ipv6_buf->destaddr.uint16[5] == 0) &&
(ipv6_buf->destaddr.uint8[12] == 0)){
/* 10: 32 bits. The address takes the form FFXX::00XX:XXXX. */
lowpan_iphc[1] |= 0x02;
/* copy second and last 3 byte */
ipv6_hdr_fields[hdr_pos] = ipv6_buf->destaddr.uint8[1];
hdr_pos++;
memcpy(&ipv6_hdr_fields[hdr_pos], &ipv6_buf->destaddr.uint8[13], 3);
hdr_pos += 3;
} else if((ipv6_buf->destaddr.uint16[1] == 0) &&
(ipv6_buf->destaddr.uint16[2] == 0) &&
(ipv6_buf->destaddr.uint16[3] == 0) &&
(ipv6_buf->destaddr.uint16[4] == 0) &&
(ipv6_buf->destaddr.uint8[10] == 0)){
/* 01: 48 bits. The address takes the form FFXX::00XX:XXXX:XXXX */
lowpan_iphc[1] |= 0x01;
/* copy second and last 5 byte */
ipv6_hdr_fields[hdr_pos] = ipv6_buf->destaddr.uint8[1];
hdr_pos++;
memcpy(&ipv6_hdr_fields[hdr_pos], &ipv6_buf->destaddr.uint8[11], 5);
hdr_pos += 5;
} else {
memcpy(&ipv6_hdr_fields[hdr_pos], &ipv6_buf->destaddr.uint8[0], 16);
hdr_pos += 16;
}
} else {
/* 0: Destination address is not a multicast address. */
if((con = lowpan_context_lookup(&ipv6_buf->destaddr)) != NULL){
/* 1: Destination address compression uses stateful, context-based
* compression. */
lowpan_iphc[1] |= LOWPAN_IPHC_DAC;
ipv6_hdr_fields[0] = con->num;
if(memcmp(&(ipv6_buf->destaddr.uint8[8]),&(dest->uint8[0]), 8) == 0){
/* 0 bits. The address is derived using context information
* and possibly the link-layer addresses.*/
lowpan_iphc[1] |= 0x03;
} else if((ipv6_buf->destaddr.uint16[4] == 0) &&
(ipv6_buf->destaddr.uint16[5] == 0) &&
(ipv6_buf->destaddr.uint16[6] == 0) &&
((ipv6_buf->destaddr.uint8[14]) & 0x80) == 0){
/* 49-bit of interface identifier are 0, so we can compress
* source address-iid to 16-bit */
memcpy(&ipv6_hdr_fields[hdr_pos], &ipv6_buf->destaddr.uint16[7], 2);
hdr_pos += 2;
/* 16 bits. The address is derived using context information
* and the 16 bits carried inline. */
lowpan_iphc[1] |= 0x02;
} else {
memcpy(&ipv6_hdr_fields[hdr_pos], &(ipv6_buf->destaddr.uint16[4]),8);
hdr_pos += 8;
/* 64 bits. The address is derived using context information
* and the 64 bits carried inline. */
lowpan_iphc[1] |= 0x01;
}
} else if(ipv6_prefix_ll_match(&ipv6_buf->destaddr)){
if(memcmp(&(ipv6_buf->destaddr.uint8[8]),&(dest->uint8[0]), 8) == 0){
/* 0 bits. The address is derived using context information
* and possibly the link-layer addresses.*/
lowpan_iphc[1] |= 0x03;
} else if((ipv6_buf->destaddr.uint16[4] == 0) &&
(ipv6_buf->destaddr.uint16[5] == 0) &&
(ipv6_buf->destaddr.uint16[6] == 0) &&
((ipv6_buf->destaddr.uint8[14]) & 0x80) == 0){
/* 49-bit of interface identifier are 0, so we can compress
* source address-iid to 16-bit */
memcpy(&ipv6_hdr_fields[hdr_pos], &ipv6_buf->destaddr.uint16[7], 2);
hdr_pos += 2;
/* 16 bits. The address is derived using context information
* and the 16 bits carried inline. */
lowpan_iphc[1] |= 0x02;
} else {
memcpy(&ipv6_hdr_fields[hdr_pos], &(ipv6_buf->destaddr.uint16[4]),8);
hdr_pos += 8;
/* 64 bits. The address is derived using context information
* and the 64 bits carried inline. */
lowpan_iphc[1] |= 0x01;
}
} else {
memcpy(&ipv6_hdr_fields[hdr_pos],&(ipv6_buf->destaddr.uint8[0]), 16);
hdr_pos += 16;
}
}
mutex_unlock(&lowpan_context_mutex,0);
comp_buf[0] = lowpan_iphc[0];
comp_buf[1] = lowpan_iphc[1];
uint8_t *ptr = get_payload_buf(ipv6_ext_hdr_len);
memcpy(&ipv6_hdr_fields[hdr_pos],ptr,ipv6_buf->length);
comp_len = 2 + hdr_pos + payload_length;
}
void lowpan_iphc_decoding(uint8_t *data, uint8_t length,
ieee_802154_long_t *s_laddr,
ieee_802154_long_t *d_laddr){
uint8_t hdr_pos = 0;
uint8_t *ipv6_hdr_fields = data;
uint8_t lowpan_iphc[2];
uint8_t cid = 0;
uint8_t sci = 0;
uint8_t dci = 0;
uint8_t ll_prefix[2] = {0xfe, 0x80};
uint8_t m_prefix[2] = {0xff, 0x02};
lowpan_context_t *con = NULL;
ipv6_buf = get_ipv6_buf();
lowpan_iphc[0] = ipv6_hdr_fields[0];
lowpan_iphc[1] = ipv6_hdr_fields[1];
hdr_pos += 2;
/* first check if CID flag is set */
if(lowpan_iphc[1] & LOWPAN_IPHC_CID){
hdr_pos++;
cid = 1;
}
/* TF: Traffic Class, Flow Label: */
if(lowpan_iphc[0] & LOWPAN_IPHC_FL_C){
/* flowlabel is elided */
if(lowpan_iphc[0] & LOWPAN_IPHC_TC_C){
/* traffic class is elided */
ipv6_buf->version_trafficclass = 0x60;
ipv6_buf->trafficclass_flowlabel = 0;
ipv6_buf->flowlabel = 0;
} else {
/* toogle ecn/dscp order */
ipv6_buf->version_trafficclass = 0x60 | (0x0f &
(ipv6_hdr_fields[hdr_pos] >> 2));
ipv6_buf->trafficclass_flowlabel = ((ipv6_hdr_fields[hdr_pos] >> 2) & 0x30) |
((ipv6_hdr_fields[hdr_pos] << 6) & 0xc0);
ipv6_buf->flowlabel = 0;
hdr_pos += 3;
}
} else {
/* flowlabel carried inline */
if(lowpan_iphc[0] & LOWPAN_IPHC_TC_C){
/* traffic class is elided */
ipv6_buf->version_trafficclass = 0x60;
/* ecn + 4 bit flowlabel*/
ipv6_buf->trafficclass_flowlabel = ((ipv6_hdr_fields[hdr_pos] >> 2) & 0x30) |
(ipv6_hdr_fields[hdr_pos] & 0x0f);
hdr_pos++;
/* copy 2byte flowlabel */
memcpy(&ipv6_buf->flowlabel, &ipv6_hdr_fields[hdr_pos], 2);
hdr_pos += 2;
} else {
ipv6_buf->version_trafficclass = 0x60 | (0x0f &
(ipv6_hdr_fields[hdr_pos] >> 2));
ipv6_buf->trafficclass_flowlabel = ((ipv6_hdr_fields[hdr_pos] >> 2) & 0x30) |
(ipv6_hdr_fields[hdr_pos] & 0x0f) |
(ipv6_hdr_fields[hdr_pos + 1] & 0x0f);
hdr_pos += 2;
memcpy(&ipv6_buf->trafficclass_flowlabel,
&ipv6_hdr_fields[hdr_pos], 2);
hdr_pos += 2;
}
}
/* NH: Next Header: */
if(lowpan_iphc[0] & LOWPAN_IPHC_NH){
// TODO: next header decompression
} else {
ipv6_buf->nextheader = ipv6_hdr_fields[hdr_pos];
hdr_pos++;
}
/* HLIM: Hop Limit: */
if(lowpan_iphc[0] & 0x03){
switch(lowpan_iphc[0] & 0x03){
case(0x01):{
ipv6_buf->hoplimit = 1;
break;
}
case(0x02):{
ipv6_buf->hoplimit = 64;
break;
}
case(0x03):{
ipv6_buf->hoplimit = 255;
break;
}
default:
break;
}
} else {
ipv6_buf->hoplimit = ipv6_hdr_fields[hdr_pos];
hdr_pos++;
}
/* CID: Context Identifier Extension: + SAC: Source Address Compression */
if(lowpan_iphc[1] & LOWPAN_IPHC_SAC){
/* 1: Source address compression uses stateful, context-based
* compression.*/
if(cid){
sci = ipv6_hdr_fields[3] >> 4;
}
mutex_lock(&lowpan_context_mutex);
/* check context number */
if(((lowpan_iphc[1] & LOWPAN_IPHC_SAM) >> 4) & 0x03){
con = lowpan_context_num_lookup(sci);
}
if(con == NULL){
printf("ERROR: context not found\n");
return;
}
switch(((lowpan_iphc[1] & LOWPAN_IPHC_SAM) >> 4) & 0x03){
case(0x01):{
/* 64-bits */
memcpy(&(ipv6_buf->srcaddr.uint8[8]), &ipv6_hdr_fields[hdr_pos], 8);
/* By draft-ietf-6lowpan-hc-15 3.1.1. Bits covered by context information are always used. */
memcpy(&(ipv6_buf->srcaddr.uint8[0]), &con->prefix, con->length);
hdr_pos += 8;
break;
}
case(0x02):{
/* 16-bits */
memset(&(ipv6_buf->srcaddr.uint8[8]), 0, 6);
memcpy(&(ipv6_buf->srcaddr.uint8[14]), &ipv6_hdr_fields[hdr_pos], 2);
/* By draft-ietf-6lowpan-hc-15 3.1.1. Bits covered by context information are always used. */
memcpy(&(ipv6_buf->srcaddr.uint8[0]), &con->prefix, con->length);
hdr_pos += 2;
break;
}
case(0x03):{
/* 0-bits */
memset(&(ipv6_buf->srcaddr.uint8[8]), 0, 8);
memcpy(&(ipv6_buf->srcaddr.uint8[8]), &s_laddr->uint8[0], 8);
/* By draft-ietf-6lowpan-hc-15 3.1.1. Bits covered by context information are always used. */
memcpy(&(ipv6_buf->srcaddr.uint8[0]), &con->prefix, con->length);
break;
}
default:{
/* unspecified address */
memset(&(ipv6_buf->srcaddr.uint8[0]), 0, 16);
break;
}
}
mutex_unlock(&lowpan_context_mutex,0);
} else {
switch(((lowpan_iphc[1] & LOWPAN_IPHC_SAM) >> 4) & 0x03){
case(0x01):{
/* 64-bits */
memcpy(&(ipv6_buf->srcaddr.uint8[0]), &ll_prefix[0], 2);
memset(&(ipv6_buf->srcaddr.uint8[2]), 0, 6);
memcpy(&(ipv6_buf->srcaddr.uint8[8]), &ipv6_hdr_fields[hdr_pos], 8);
hdr_pos += 8;
break;
}
case(0x02):{
/* 16-bits */
memcpy(&(ipv6_buf->srcaddr.uint8[0]), &ll_prefix[0], 2);
memset(&(ipv6_buf->srcaddr.uint8[2]), 0, 12);
memcpy(&(ipv6_buf->srcaddr.uint8[14]), &ipv6_hdr_fields[hdr_pos], 2);
hdr_pos += 2;
break;
}
case(0x03):{
/* 0-bits */
memcpy(&(ipv6_buf->srcaddr.uint8[0]), &ll_prefix[0], 2);
memset(&(ipv6_buf->srcaddr.uint8[8]), 0, 14);
memcpy(&(ipv6_buf->srcaddr.uint8[8]), &s_laddr->uint8[0], 8);
break;
}
default:{
/* full address carried inline */
memcpy(&(ipv6_buf->srcaddr.uint8[0]),
&ipv6_hdr_fields[hdr_pos], 16);
hdr_pos += 16;
break;
}
}
}
/* M: Multicast Compression + DAC: Destination Address Compression */
if(lowpan_iphc[1] & LOWPAN_IPHC_M){
/* 1: Destination address is a multicast address. */
if(lowpan_iphc[1] & LOWPAN_IPHC_DAC){
/* 1: Destination address compression uses stateful, context-based
* compression.
* If M=1 and DAC=1: */
if(cid){
dci = ipv6_hdr_fields[3] & 0x0f;
}
mutex_lock(&lowpan_context_mutex);
if((lowpan_iphc[1] & LOWPAN_IPHC_DAM) & 0x03){
con = lowpan_context_num_lookup(dci);
}
if(con == NULL){
printf("ERROR: context not found\n");
return;
}
// TODO:
mutex_unlock(&lowpan_context_mutex,0);
} else {
/* If M=1 and DAC=0: */
switch(lowpan_iphc[1] & 0x03){
case(0x01):{
m_prefix[1] = ipv6_hdr_fields[hdr_pos];
hdr_pos++;
break;
}
case(0x02):{
m_prefix[1] = ipv6_hdr_fields[hdr_pos];
hdr_pos++;
break;
}
default:
break;
}
switch(lowpan_iphc[1] & 0x03){
case(0x01):{
memcpy(&(ipv6_buf->destaddr.uint8[0]), &m_prefix[0], 2);
memset(&(ipv6_buf->destaddr.uint8[2]), 0, 9);
memcpy(&(ipv6_buf->destaddr.uint8[11]), &ipv6_hdr_fields[hdr_pos], 5);
hdr_pos += 5;
break;
}
case(0x02):{
memcpy(&(ipv6_buf->destaddr.uint8[0]), &m_prefix[0], 2);
memset(&(ipv6_buf->destaddr.uint8[2]), 0, 11);
memcpy(&(ipv6_buf->destaddr.uint8[13]), &ipv6_hdr_fields[hdr_pos], 3);
hdr_pos += 3;
break;
}
case(0x03):{
memcpy(&(ipv6_buf->destaddr.uint8[0]), &m_prefix[0], 2);
memset(&(ipv6_buf->destaddr.uint8[2]), 0, 13);
memcpy(&(ipv6_buf->destaddr.uint8[15]), &ipv6_hdr_fields[hdr_pos], 1);
hdr_pos++;
break;
}
default:{
memcpy(&(ipv6_buf->destaddr.uint8[0]), &ipv6_hdr_fields[hdr_pos], 16);
break;
}
}
}
} else {
if(lowpan_iphc[1] & LOWPAN_IPHC_DAC){
/* 1: Destination address compression uses stateful, context-based
* compression.
* If M=1 and DAC=1: */
if(cid){
dci = ipv6_hdr_fields[3] & 0x0f;
}
mutex_lock(&lowpan_context_mutex);
if((lowpan_iphc[1] & LOWPAN_IPHC_DAM) & 0x03){
con = lowpan_context_num_lookup(dci);
}
if(con == NULL){
printf("ERROR: context not found\n");
return;
}
switch((lowpan_iphc[1] & LOWPAN_IPHC_DAM) & 0x03){
case(0x01):{
memcpy(&(ipv6_buf->destaddr.uint8[8]), &ipv6_hdr_fields[hdr_pos], 8);
/* By draft-ietf-6lowpan-hc-15 3.1.1. Bits covered by context information are always used. */
memcpy(&(ipv6_buf->srcaddr.uint8[0]), &con->prefix, con->length);
hdr_pos += 8;
break;
}
case(0x02):{
memset(&(ipv6_buf->destaddr.uint8[8]), 0, 6);
memcpy(&(ipv6_buf->destaddr.uint8[14]), &ipv6_hdr_fields[hdr_pos], 2);
/* By draft-ietf-6lowpan-hc-15 3.1.1. Bits covered by context information are always used. */
memcpy(&(ipv6_buf->srcaddr.uint8[0]), &con->prefix, con->length);
hdr_pos += 2;
break;
}
case(0x03):{
memset(&(ipv6_buf->destaddr.uint8[0]), 0, 8);
memcpy(&(ipv6_buf->destaddr.uint8[8]), &d_laddr->uint8[0], 8);
/* By draft-ietf-6lowpan-hc-15 3.1.1. Bits covered by context information are always used. */
memcpy(&(ipv6_buf->srcaddr.uint8[0]), &con->prefix, con->length);
break;
}
default:
break;
}
mutex_unlock(&lowpan_context_mutex,0);
} else {
switch((lowpan_iphc[1] & LOWPAN_IPHC_DAM) & 0x03){
case(0x01):{
memcpy(&(ipv6_buf->destaddr.uint8[0]), &ll_prefix[0], 2);
memset(&(ipv6_buf->destaddr.uint8[2]), 0, 6);
memcpy(&(ipv6_buf->destaddr.uint8[8]),
&ipv6_hdr_fields[hdr_pos], 8);
hdr_pos += 8;
break;
}
case(0x02):{
memcpy(&(ipv6_buf->destaddr.uint8[0]), &ll_prefix[0], 2);
memset(&(ipv6_buf->destaddr.uint8[2]), 0, 12);
memcpy(&(ipv6_buf->destaddr.uint8[14]),
&ipv6_hdr_fields[hdr_pos], 2);
hdr_pos += 2;
break;
}
case(0x03):{
memcpy(&(ipv6_buf->destaddr.uint8[0]), &ll_prefix, 2);
memset(&(ipv6_buf->destaddr.uint8[2]), 0, 14);
memcpy(&(ipv6_buf->destaddr.uint8[8]), &d_laddr->uint8[0], 8);
break;
}
default:{
memcpy(&(ipv6_buf->destaddr.uint8[0]),
&ipv6_hdr_fields[hdr_pos], 16);
hdr_pos += 16;
break;
}
}
}
}
uint8_t *ptr = get_payload_buf(ipv6_ext_hdr_len);
memcpy(ptr, &ipv6_hdr_fields[hdr_pos], length - hdr_pos);
/* ipv6 length */
ipv6_buf->length = length - hdr_pos;
packet_length = IPV6_HDR_LEN + ipv6_buf->length;
}
uint8_t lowpan_context_len(){
return context_len;
}
void lowpan_context_remove(uint8_t num) {
int i,j;
for(i = 0; i < LOWPAN_CONTEXT_MAX; i++){
if(contexts[i].num == num){
context_len--;
break;
}
}
abr_remove_context(num);
for(j = i; j < LOWPAN_CONTEXT_MAX; j++) {
contexts[j] = contexts[j+1];
}
}
lowpan_context_t *lowpan_context_update(uint8_t num, const ipv6_addr_t *prefix,
uint8_t length, uint8_t comp,
uint16_t lifetime) {
lowpan_context_t *context;
if (lifetime == 0){
lowpan_context_remove(num);
return NULL;
}
if (context_len == LOWPAN_CONTEXT_MAX)
return NULL;
context = lowpan_context_num_lookup(num);
if (context == NULL) {
context = &(contexts[context_len++]);
}
context->num = num;
memset((void*)(&context->prefix),0,16);
// length in bits
memcpy((void*)(&context->prefix),(void*)prefix,length/8);
context->length = length;
context->comp = comp;
context->lifetime = lifetime;
return context;
}
lowpan_context_t *lowpan_context_get() {
return contexts;
}
lowpan_context_t * lowpan_context_lookup(ipv6_addr_t *addr){
int i;
lowpan_context_t *context = NULL;
for(i = 0; i < lowpan_context_len(); i++){
if(contexts[i].length > 0 && memcmp((void*)addr,&(contexts[i].prefix),contexts[i].length) == 0){
if (context == NULL || context->length < contexts[i].length) { // longer prefixes are always prefered
context = &contexts[i];
}
}
}
return context;
}
lowpan_context_t * lowpan_context_num_lookup(uint8_t num){
int i;
for(i = 0; i < lowpan_context_len(); i++){
if(contexts[i].num == num){
return &contexts[i];
}
}
return NULL;
}
void lowpan_context_auto_remove(void) {
timex_t minute = timex_set(60,0);
int i;
int8_t to_remove[LOWPAN_CONTEXT_MAX];
int8_t to_remove_size;
while(1){
vtimer_sleep(minute);
to_remove_size = 0;
mutex_lock(&lowpan_context_mutex);
for(i = 0; i < lowpan_context_len(); i++) {
if (--(contexts[i].lifetime) == 0) {
to_remove[to_remove_size++] = contexts[i].num;
}
}
for(i = 0; i < to_remove_size; i++) {
lowpan_context_remove(to_remove[i]);
}
mutex_unlock(&lowpan_context_mutex,0);
}
}
void sixlowpan_init(transceiver_type_t trans, uint8_t r_addr, int as_border){
ipv6_addr_t tmp;
/* init mac-layer and radio transceiver */
vtimer_init();
sixlowmac_init(trans);
/* init interface addresses */
memset(&iface,0,sizeof(iface_t));
set_radio_address(r_addr);
init_802154_short_addr(&(iface.saddr));
init_802154_long_addr(&(iface.laddr));
/* init global buffer mutex */
mutex_init(&buf_mutex);
/* init lowpan context mutex */
mutex_init(&lowpan_context_mutex);
/* init link-local address */
ipv6_set_ll_prefix(&lladdr);
memcpy(&(lladdr.uint8[8]), &(iface.laddr.uint8[0]), 8);
ipv6_iface_add_addr(&lladdr, ADDR_STATE_PREFERRED, 0, 0,
ADDR_TYPE_LINK_LOCAL);
ipv6_set_loaddr(&tmp);
ipv6_iface_add_addr(&tmp, ADDR_STATE_PREFERRED, 0, 0,
ADDR_TYPE_LOOPBACK);
ipv6_set_all_nds_mcast_addr(&tmp);
ipv6_iface_add_addr(&tmp, ADDR_STATE_PREFERRED, 0, 0,
ADDR_TYPE_LOOPBACK);
ipv6_iface_add_addr(&lladdr, ADDR_STATE_PREFERRED, 0, 0,
ADDR_CONFIGURED_AUTO);
if (as_border) {
ip_process_pid = thread_create(ip_process_buf, IP_PROCESS_STACKSIZE,
PRIORITY_MAIN-1, CREATE_STACKTEST,
border_process_lowpan, "border_process_lowpan");
} else {
ip_process_pid = thread_create(ip_process_buf, IP_PROCESS_STACKSIZE,
PRIORITY_MAIN-1, CREATE_STACKTEST,
ipv6_process, "ip_process");
}
nd_nbr_cache_rem_pid = thread_create(nc_buf, NC_STACKSIZE,
PRIORITY_MAIN-1, CREATE_STACKTEST,
nbr_cache_auto_rem, "nbr_cache_rem");
contexts_rem_pid = thread_create(con_buf, CON_STACKSIZE,
PRIORITY_MAIN+1, CREATE_STACKTEST,
lowpan_context_auto_remove, "lowpan_context_rem");
}
void sixlowpan_adhoc_init(transceiver_type_t trans, ipv6_addr_t *prefix, uint8_t r_addr){
/* init network prefix */
ipv6_set_prefix(prefix, prefix);
plist_add(prefix, 64, OPT_PI_VLIFETIME_INFINITE,0,1,OPT_PI_FLAG_A);
ipv6_init_iface_as_router();
sixlowpan_init(trans, r_addr,0);
}
View Git Blame Copy Permalink