serial_tun.c

tinyos2.0版本驱动

/*
 * Copyright (c) 2007 Matus Harvan
 * All rights reserved
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * 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.
 *     * The name of the author may not be used to endorse or promote
 *       products derived from this software without specific prior
 *       written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS 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 COPYRIGHT
 * OWNER 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.
 */

#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <sys/types.h>
#include <sys/time.h>
#include <sys/socket.h>
#include <netinet/in_systm.h>
#include <netinet/in.h>
#include <netinet/ip.h>
#include <arpa/inet.h>
#include <netdb.h>

#include <stdarg.h>

#include "tun_dev.h"
#include "serialsource.h"
#include "serialpacket.h"
#include "6lowpan.h"

#define min(a,b) ( (a>b) ? b : a )
#define max(a,b) ( (a>b) ? a : b )

static char *msgs[] = {
  "unknown_packet_type",
  "ack_timeout"	,
  "sync"	,
  "too_long"	,
  "too_short"	,
  "bad_sync"	,
  "bad_crc"	,
  "closed"	,
  "no_memory"	,
  "unix_error"
};

/* global variables */
lowpan_pkt_t *fragments = NULL; /* fragment reassembly of received frames */
//lowpan_pkt_t *send_queue = NULL;
int g_send_pending = 0;

hw_addr_t hw_addr;

serial_source ser_src;
int tun_fd = 0; /* tunnel device */
//int ser_fd = 0; /* serial device */

uint16_t g_dgram_tag = 0; /* datagram_tag for sending fragmented packets */

/* ------------------------------------------------------------------------- */
/* function pre-declarations */
int serial_output_am_payload(uint8_t *buf, int len,
			     const hw_addr_t *hw_src_addr,
			     const hw_addr_t *hw_dst_addr);

int serial_input_layer3(uint8_t *buf, int len,
			const hw_addr_t *hw_src_addr,
			const hw_addr_t *hw_dst_addr);

int serial_input_ipv6_uncompressed(uint8_t *buf, int len,
				   const hw_addr_t *hw_src_addr,
				   const hw_addr_t *hw_dst_addr);

int serial_input_ipv6_compressed(uint8_t *buf, int len,
				 const hw_addr_t *hw_src_addr,
				 const hw_addr_t *hw_dst_addr);

/* ------------------------------------------------------------------------- */
/* utility functions */
int get_ser_fd()
{
    return serial_source_fd(ser_src);
}

void increment_g_dgram_tag()
{
    uint16_t tmp = ntohs(g_dgram_tag);
    if (tmp == 0xFFFF) {
	tmp = 0;
    } else {
	tmp++;
    }
    g_dgram_tag = htons(tmp);
}

void stderr_msg(serial_source_msg problem)
{
  fprintf(stderr, "Note: %s\n", msgs[problem]);
}

int
debug(const char *fmt, ...)
{
    int result;
    va_list ap;
    va_start(ap, fmt);
    result = vfprintf(stderr, fmt, ap);
    va_end(ap);
    return result;
}

/* from contiki-2.x/tools/tunslip.c */
int
ssystem(const char *fmt, ...)
{
  char cmd[128];
  va_list ap;
  va_start(ap, fmt);
  vsnprintf(cmd, sizeof(cmd), fmt, ap);
  va_end(ap);
  printf("%s\n", cmd);
  fflush(stdout);
  return system(cmd);
}

/* print char* in hex format */
void dump_serial_packet(const unsigned char *packet, const int len) {
    int i;
    printf("len: %d\n", len);
    if (!packet)
	return;
    for (i = 0; i < len; i++) {
	printf("%02x ", packet[i]);
	//printf("%02x(%c) ", packet[i], packet[i]);
	//printf("%c", packet[i]);
    }
    putchar('\n');
/*     printf("---\n"); */
/*     for (i = 0; i < len; i++) { */
/* 	printf("%c", packet[i]); */
/*     } */
/*     putchar('\n'); */
/*     printf("---\n"); */
}

/* ------------------------------------------------------------------------- */
/* ip6_addr_t and hw_addr_t utility functions */
int ipv6_addr_is_zero(const ip6_addr_t *addr)
{
    int i;
    for (i=0;i<16;i++) {
	if (addr->addr[i]) {
	    return 0;
	}
    }
    return 1;
}

int ipv6_addr_is_linklocal_unicast(const ip6_addr_t *addr)
{
    if (   addr->addr[0] == 0xFE
	   && addr->addr[1] == 0x80
	   && addr->addr[2] == 0
	   && addr->addr[3] == 0
	   && addr->addr[4] == 0
	   && addr->addr[5] == 0
	   && addr->addr[6] == 0
	   && addr->addr[7] == 0
	   )
	return 1;
    else
	return 0;
}

int cmp_ipv6_addr(const ip6_addr_t *addr1, const ip6_addr_t *addr2)
{
    return memcmp(addr1, addr2, sizeof(ip6_addr_t));
}

int cmp_hw_addr(const hw_addr_t *addr1, const hw_addr_t *addr2)
{
    // for short addresses compare only the first two bytes
    if (addr1->type == HW_ADDR_SHORT && addr2->type == HW_ADDR_SHORT) {
	return memcmp(addr1->addr_short, addr2->addr_short,
		      sizeof(addr1->addr_short));
    } else {
	return memcmp(addr1, addr2, sizeof(hw_addr_t));
    }
}

int hw_addr_is_broadcat(const hw_addr_t *hw_addr)
{
    if (hw_addr->type == HW_ADDR_SHORT
	&& hw_addr->addr_short[0] == 0xFF  && hw_addr->addr_short[1] == 0xFF)
	return 1;
    // TODO: long address
    else return 0;
}

/* ------------------------------------------------------------------------- */
/* more utility functions */
void clear_pkt(lowpan_pkt_t *pkt)
{
    memset(pkt, 0, sizeof(*pkt));
    pkt->buf_begin = pkt->buf + LOWPAN_OVERHEAD;
}

void free_frag_list(frag_info_t *p)
{
    frag_info_t *q;
    while (p) {
	q = p->next;
	free(p);
	p = q;
    }
}

void free_lowpan_pkt(lowpan_pkt_t *pkt)
{
    lowpan_pkt_t *p;
    lowpan_pkt_t **q;

    if (!fragments) return;
    for(q=&fragments; *q; q=&(*q)->next) {
	if (*q == pkt) {
	    p = *q;
	    *q = p->next;
	    free_frag_list(p->frag_list);
	    free(p);
	    return;
	}
    }
}

lowpan_pkt_t * find_fragment(hw_addr_t *hw_src_addr, hw_addr_t *hw_dst_addr,
			     uint16_t dgram_size, uint16_t dgram_tag)
{
    lowpan_pkt_t *p;
    for(p=fragments; p; p=p->next) {
	if ((p->dgram_tag == dgram_tag)
	    && (p->dgram_size == dgram_size)
	    && cmp_hw_addr(&p->hw_src_addr, hw_src_addr) == 0
	    && cmp_hw_addr(&p->hw_dst_addr, hw_dst_addr) == 0
	    ) {
	    return p;
	}
    }
    return NULL;
}
/* ------------------------------------------------------------------------- */
/* HC1 and HC2 compression and decompresstion functions */

/* the caller has to free() new_buf */
int lowpan_decompress(uint8_t *buf, int len,
		       const hw_addr_t *hw_src_addr,
		       const hw_addr_t *hw_dst_addr,
		       uint8_t **new_buf, int *new_len)
{
    uint8_t hc1_enc;
    uint8_t hc2_enc;
    struct ip6_hdr *ip_hdr = NULL;
    struct udp_hdr *udp_hdr = NULL;
    
    *new_buf = malloc(len + sizeof(*ip_hdr) + sizeof(*udp_hdr));
    if (!*new_buf) {
	fprintf(stderr, "%s: out of memory\n", __func__);
	*new_len = 0;
	return 1;
    }
    hc1_enc = *buf;
    buf += sizeof(hc1_enc);
    len -= sizeof(hc1_enc);

    /* HC2 encoding follows HC1 encoding */
    if ((hc1_enc & HC1_HC2_MASK) == HC1_HC2_PRESENT) {
	hc2_enc = *buf;
	buf += sizeof(hc2_enc);
	len -= sizeof(hc2_enc);
    }

    /* IP header fields */
    ip_hdr = (struct ip6_hdr *) *new_buf;
    memset(ip_hdr, 0, sizeof(struct ip6_hdr));

    ip_hdr->vtc |= IPV6_VERSION;

    ip_hdr->hlim = *buf;
    buf += sizeof(ip_hdr->hlim);
    len -= sizeof(ip_hdr->hlim);

    /* source IP address */
    if ((hc1_enc & HC1_SRC_PREFIX_MASK) == HC1_SRC_PREFIX_INLINE) {
	memcpy(&ip_hdr->src_addr, buf, sizeof(ip_hdr->src_addr)/2);
	buf += sizeof(ip_hdr->src_addr)/2;
	len -= sizeof(ip_hdr->src_addr)/2;
    } else {
	ip_hdr->src_addr.addr[0] = 0xFE;
	ip_hdr->src_addr.addr[1] = 0x80;
    }
     
    if ((hc1_enc & HC1_SRC_IFACEID_MASK) == HC1_SRC_IFACEID_INLINE) {
	memcpy(((void*)&ip_hdr->src_addr) + sizeof(ip_hdr->src_addr)/2,
	       buf, sizeof(ip_hdr->src_addr)/2);
	buf += sizeof(ip_hdr->src_addr)/2;
	len -= sizeof(ip_hdr->src_addr)/2;
    }

    /* destination IP address */
    if ((hc1_enc & HC1_DST_PREFIX_MASK) == HC1_DST_PREFIX_INLINE) {
	memcpy(&ip_hdr->dst_addr, buf, sizeof(ip_hdr->dst_addr)/2);
	buf += sizeof(ip_hdr->dst_addr)/2;
	len -= sizeof(ip_hdr->dst_addr)/2;
    } else {
	ip_hdr->dst_addr.addr[0] = 0xFE;
	ip_hdr->dst_addr.addr[1] = 0x80;
    }
     
    if ((hc1_enc & HC1_DST_IFACEID_MASK) == HC1_DST_IFACEID_INLINE) {
	memcpy(((void*)&ip_hdr->dst_addr) + sizeof(ip_hdr->dst_addr)/2,
	       buf, sizeof(ip_hdr->dst_addr)/2);
	buf += sizeof(ip_hdr->dst_addr)/2;
	len -= sizeof(ip_hdr->dst_addr)/2;
    }

    /* Traffic Class and Flow Label */
    if ((hc1_enc & HC1_TCFL_MASK) == HC1_TCFL_INLINE) {
	//TODO
    }

    /* Next Header */
    switch (hc1_enc & HC1_NEXTHDR_MASK) {
    case HC1_NEXTHDR_INLINE:
	ip_hdr->nxt_hdr = *buf;
	buf += sizeof(ip_hdr->nxt_hdr);
	len -= sizeof(ip_hdr->nxt_hdr);
	break;
    case HC1_NEXTHDR_UDP:
	ip_hdr->nxt_hdr = NEXT_HEADER_UDP;
	break;
    case HC1_NEXTHDR_ICMP:
	ip_hdr->nxt_hdr = NEXT_HEADER_ICMP6;
	break;
    case HC1_NEXTHDR_TCP:
	ip_hdr->nxt_hdr = NEXT_HEADER_TCP;
	break;
    default:
	fprintf(stderr, "unknown next header HC1 encoding\n");
	break;
    }
    
    /* HC_UDP compression */
    if ((hc1_enc & HC1_HC2_MASK) == HC1_HC2_PRESENT
	&& (hc1_enc & HC1_NEXTHDR_MASK) == HC1_NEXTHDR_UDP) {
	
	udp_hdr = (struct udp_hdr *) ((*new_buf) + sizeof(struct ip6_hdr));
	//udp_hdr = (struct udp_hdr *) (ip_hdr + 1);
	memset(udp_hdr, 0, sizeof(struct udp_hdr));

	/* UDP Source Port */
	if ((hc2_enc & HC2_UDP_SRC_PORT_MASK) == HC2_UDP_SRC_PORT_INLINE) {
	    memcpy(&udp_hdr->src_port, buf, sizeof(udp_hdr->src_port));
	    buf += sizeof(udp_hdr->src_port);
	    len -= sizeof(udp_hdr->src_port);
	} else {
	    //TODO
	}

	/* UDP Destination Port */
	if ((hc2_enc & HC2_UDP_DST_PORT_MASK) == HC2_UDP_DST_PORT_INLINE) {
	    memcpy(&udp_hdr->dst_port, buf, sizeof(udp_hdr->dst_port));
	    buf += sizeof(udp_hdr->dst_port);
	    len -= sizeof(udp_hdr->dst_port);
	} else {
	    //TODO
	}

	/* UDP Length */
	if ((hc2_enc & HC2_UDP_LEN_MASK) == HC2_UDP_LEN_INLINE) {
	    memcpy(&udp_hdr->len, buf, sizeof(udp_hdr->len));
	    buf += sizeof(udp_hdr->len);
	    len -= sizeof(udp_hdr->len);
	} else {
	    udp_hdr->len = len - sizeof(udp_hdr->chksum)
		+ sizeof(struct udp_hdr);
	}
	
	/* Checksum */
	memcpy(&udp_hdr->chksum, buf, sizeof(udp_hdr->chksum));
	buf += sizeof(udp_hdr->chksum);
	len -= sizeof(udp_hdr->chksum);
	
	/* IPv6 Payload Length */
	ip_hdr->plen = htons(len + sizeof(struct udp_hdr));
	
	memcpy((*new_buf) + sizeof(struct ip6_hdr) + sizeof(struct udp_hdr),