serial_tun.c

tinyos2.0版本驱动

	       buf, len);
	*new_len = len + sizeof(struct ip6_hdr) + sizeof(struct udp_hdr);
    } else {
    /* IPv6 Payload Length */
    ip_hdr->plen = htons(len);

    memcpy((*new_buf) + sizeof(struct ip6_hdr), buf, len);
    *new_len = len + sizeof(struct ip6_hdr);
    }
    
    return 0;
}

/* assuming there is space available in from of buf_begin */
int lowpan_compress(uint8_t **buf_begin, int *len,
		     const hw_addr_t *hw_src_addr,
		     const hw_addr_t *hw_dst_addr)
{
    uint8_t *hc1_enc;
    uint8_t *hc2_enc;
    struct ip6_hdr *ip_hdr = NULL;
    struct udp_hdr *udp_hdr = NULL;
    
    uint8_t new_buf[sizeof(struct ip6_hdr) + sizeof(struct udp_hdr) + 5];
    uint8_t *new_buf_p = new_buf;
    int new_len = 0;

    debug("%s\n", __func__);

    ip_hdr = (struct ip6_hdr *) *buf_begin;
    udp_hdr = (struct udp_hdr *) ((*buf_begin) + sizeof(struct ip6_hdr));

    /* check if this is an IPv6 packet */
    if ((ip_hdr->vtc & IPV6_VERSION_MASK) != IPV6_VERSION) {
	debug("IP version check failed - not an IPv6 packet\n");
	return 0;
    }
    
    /* set 6lowpan dispatch value */
    *new_buf_p = DISPATCH_COMPRESSED_IPV6;
    new_buf_p += sizeof(uint8_t);
    new_len += sizeof(uint8_t);

    /* HC1 encoding field */
    hc1_enc = new_buf_p;
    new_buf_p += sizeof(uint8_t);
    new_len += sizeof(uint8_t);
    *hc1_enc = 0;

    /* does HC2 follow after HC1? */
    if (ip_hdr->nxt_hdr == NEXT_HEADER_UDP) {
	*hc1_enc |= HC1_HC2_PRESENT;

	/* HC2 encoding field */
	hc2_enc = new_buf_p;
	new_buf_p += sizeof(uint8_t);
	new_len += sizeof(uint8_t);
	*hc2_enc = 0;
    } else {    
	*hc1_enc |= HC1_HC2_NONE;
    }

    /* Hop Limit */
    *new_buf_p = ip_hdr->hlim;
    new_buf_p += sizeof(uint8_t);
    new_len += sizeof(uint8_t);

    /* source address prefix */
    //TODO: fails checksum on the mote !!!
    if (ipv6_addr_is_linklocal_unicast(&ip_hdr->src_addr)) {
	*hc1_enc |= HC1_SRC_PREFIX_LINKLOCAL;
    } else {
	*hc1_enc |= HC1_SRC_PREFIX_INLINE;

	memcpy(new_buf_p, &(ip_hdr->src_addr), 8);
	new_buf_p += 8;
	new_len += 8;
    }

    /* source address interface identifier */
    *hc1_enc |= HC1_SRC_IFACEID_INLINE;
    
    memcpy(new_buf_p, ((void*)&(ip_hdr->src_addr)) + 8, 8);
    new_buf_p += 8;
    new_len += 8;

    /* destination address prefix */
    if (ipv6_addr_is_linklocal_unicast(&ip_hdr->dst_addr)) {
	*hc1_enc |= HC1_DST_PREFIX_LINKLOCAL;
    } else {
	*hc1_enc |= HC1_DST_PREFIX_INLINE;

	memcpy(new_buf_p, &(ip_hdr->dst_addr), 8);
	new_buf_p += 8;
	new_len += 8;
    }

    /* destination address interface identifier */
    *hc1_enc |= HC1_DST_IFACEID_INLINE;
    
    memcpy(new_buf_p, ((void*)&(ip_hdr->dst_addr)) + 8, 8);
    new_buf_p += 8;
    new_len += 8;

    /* we're always sending packets with TC anf FL zero */
    *hc1_enc |= HC1_TCFL_ZERO;
    
    /* next header */
    switch (ip_hdr->nxt_hdr) {
    case NEXT_HEADER_UDP:
	*hc1_enc |= HC1_NEXTHDR_UDP;
	break;
    case NEXT_HEADER_ICMP6:
	*hc1_enc |= HC1_NEXTHDR_ICMP;
	break;
    case NEXT_HEADER_TCP:
	*hc1_enc |= HC1_NEXTHDR_TCP;
	break;
    default:
	*hc1_enc |= HC1_NEXTHDR_INLINE;

	*new_buf_p = ip_hdr->nxt_hdr;
	new_buf_p += sizeof(ip_hdr->nxt_hdr);
	new_len += sizeof(ip_hdr->nxt_hdr);
	break;
    }
    
    /* HC_UDP encoding */
    if ((*hc1_enc & HC1_HC2_MASK) == HC1_HC2_PRESENT
	&& (*hc1_enc & HC1_NEXTHDR_MASK) == HC1_NEXTHDR_UDP) {
	
	/* Source Port */
	*hc2_enc |= HC2_UDP_SRC_PORT_INLINE;
	memcpy(new_buf_p, &udp_hdr->src_port, sizeof(udp_hdr->src_port));
	new_buf_p += sizeof(udp_hdr->src_port);
	new_len += sizeof(udp_hdr->src_port);
	
	/* Destination Port */
	*hc2_enc |= HC2_UDP_DST_PORT_INLINE;
	memcpy(new_buf_p, &udp_hdr->dst_port, sizeof(udp_hdr->dst_port));
	new_buf_p += sizeof(udp_hdr->dst_port);
	new_len += sizeof(udp_hdr->dst_port);

	/* Length */
	//*hc2_enc |= HC2_UDP_LEN_COMPR;
	*hc2_enc |= HC2_UDP_LEN_INLINE;
	memcpy(new_buf_p, &udp_hdr->len, sizeof(udp_hdr->len));
	new_buf_p += sizeof(udp_hdr->len);
	new_len += sizeof(udp_hdr->len);

	/* Checksum */
	memcpy(new_buf_p, &udp_hdr->chksum, sizeof(udp_hdr->chksum));
	new_buf_p += sizeof(udp_hdr->chksum);
	new_len += sizeof(udp_hdr->chksum);

	/* replace the IP and UDP headers with the compressed ones */
	*len += new_len;
	*len -= sizeof(struct ip6_hdr);
	*len -= sizeof(struct udp_hdr);
	*buf_begin += sizeof(struct ip6_hdr);
	*buf_begin += sizeof(struct udp_hdr);
	*buf_begin -= new_len;
	memcpy(*buf_begin, new_buf, new_len);
    } else {
	/* replace the IP header with the compressed one */
	*len += new_len;
	*len -= sizeof(struct ip6_hdr);
	*buf_begin += sizeof(struct ip6_hdr);
	*buf_begin -= new_len;
	memcpy(*buf_begin, new_buf, new_len);
    }

    return 0;
}

/* ------------------------------------------------------------------------- */
/* handling of data arriving on the tun interface */

/*
 * encapsulate buf as an Active Message payload
 * fragments packets if needed
 */
int serial_output_am_payload(uint8_t *buf, int len,
			     const hw_addr_t *hw_src_addr,
			     const hw_addr_t *hw_dst_addr)
{
    am_packet_t AMpacket;
    int result;
    
    //debug("%s: dumping buf (len: %d)...\n", __func__, len);
    //dump_serial_packet(buf, len);

    if (len > LINK_DATA_MTU) {
	fprintf(stderr, "%s: requested to send more than LINK_DATA_MTU"\
		"(%d bytes)\n", __func__, len);
	// TODO: maybe we should send the fisr LINK_DATA_MTU bytes
	//       and only print a warning
	return -1;
    }
    memset(&AMpacket, 0, sizeof(AMpacket));
    AMpacket.pkt_type = 0;
    // TODO: make the dst addr handling more general
    //AMpacket.dst = htons(0x14);
    AMpacket.dst = htons(0xFFFF);
    //AMpacket.src = htons(0x12);
    // TODO: make the src addr handling more general
    memcpy(&AMpacket.src, hw_addr.addr_short, 2);
    AMpacket.group = 0;
    AMpacket.type = 0x41;
    AMpacket.length = min(len,LINK_DATA_MTU);
    //AMpacket.data = buf;
    memcpy(AMpacket.data, buf, AMpacket.length);
    
    len = AMpacket.length + 8; // data + header

    debug("sending to serial port...\n");
    dump_serial_packet((unsigned char *)&AMpacket, len);

    result = write_serial_packet(ser_src, &AMpacket, len);
    /*
     * Returns: 0 if packet successfully written, 1 if successfully
     * written but not acknowledged, -1 otherwise
     */
    debug("write_serial_packet returned %d\n", result);

    if (result < 0) {
	perror ("sendto");
	return -1;
    }
    return len;
}

/*
 * read data from the tun device and send it to the serial port
 * does also fragmentation
 */
int tun_input()
{
    uint8_t buf[LOWPAN_MTU + LOWPAN_OVERHEAD];
    uint8_t *buf_begin = buf + LOWPAN_OVERHEAD;
    int len;
    int result;
    
    struct lowpan_frag_hdr *frag_hdr;
    uint8_t dgram_offset = 0;
    uint16_t dgram_size;
    hw_addr_t hw_dst_addr;
    uint8_t frag_len; /* length of the fragment just being sent */

    uint8_t *frame_begin; /* begin of the frame payload */
    uint8_t frame_len; /* length of the frame payload */
    
    len = tun_read (tun_fd, (char*) buf_begin, LOWPAN_MTU);
    if (len <= 0) {
	perror ("read");
	return 0;
    }
    printf("data on tun interface\n");

    /* set 802.15.4 destination address */
    hw_dst_addr.type = HW_ADDR_SHORT;
    hw_dst_addr.addr_short[0] =0xFF;
    hw_dst_addr.addr_short[1] =0xFF;
    
    /* HC compression */
    lowpan_compress(&buf_begin, &len,
		    &hw_addr, &hw_dst_addr);

    /* prepend dispatch */
/*     buf_begin--; */
/*     *buf_begin = DISPATCH_UNCOMPRESSED_IPV6;  */
/*     len++; */

    /* determine if fragmentation is needed */
    if (len > LINK_DATA_MTU) {
	/* fragmentation needed */
	increment_g_dgram_tag();
	dgram_size = htons(len);
	
	/* first fragment */
	debug("first fragment... (len: %d, offset: %d)\n",
	      len, dgram_offset);
	/* fragment heder */
	frame_begin = buf_begin - sizeof(struct lowpan_frag_hdr);
	frag_hdr = (struct lowpan_frag_hdr *) frame_begin;
	frag_hdr->dgram_size = dgram_size;
	frag_hdr->dispatch |= DISPATCH_FIRST_FRAG;
	frag_hdr->dgram_tag = g_dgram_tag;
	/* align fragment length at an 8-byte multiple */
	frag_len = LINK_DATA_MTU - sizeof(struct lowpan_frag_hdr);
	frag_len -= frag_len%8;
	frame_len = frag_len + sizeof(struct lowpan_frag_hdr);
	result = serial_output_am_payload(frame_begin, frame_len,
					  &hw_addr, &hw_dst_addr);
	if (result < 0) {
	    perror("serial_output_am_payload() failed\n");
	    return -1;
	}
	buf_begin += frag_len;
	len -= frag_len;
	dgram_offset += frag_len/8; /* in 8-byte multiples */
    
	/* subseq fragment */
	while (len > 0) {
	    usleep(10000); /* workaround to prevent loosing fragments */
	    debug("subsequent fragment... (len: %d, offset: %d)\n",
		  len, dgram_offset);
	    /* dgram_offset */
	    frame_begin = buf_begin - sizeof(uint8_t);
	    *(frame_begin) = dgram_offset;
	    /* fragment heder */
	    frame_begin -= sizeof(struct lowpan_frag_hdr);
	    frag_hdr = (struct lowpan_frag_hdr *) frame_begin;
	    frag_hdr->dgram_size = dgram_size;
	    frag_hdr->dispatch |= DISPATCH_SUBSEQ_FRAG;
	    frag_hdr->dgram_tag = g_dgram_tag;
	    if (len <= LINK_DATA_MTU  - sizeof(struct lowpan_frag_hdr)
		- sizeof(uint8_t)) {
		/*
		 * last fragment does not have to be aligned
		 * at an 8-byte multiple
		 */
		frag_len = len;
	    } else {
		/* align fragment length at an 8-byte multiple */
		frag_len = LINK_DATA_MTU - sizeof(struct lowpan_frag_hdr)
		           - sizeof(uint8_t);
		frag_len -= frag_len%8;
	    }
	    frame_len = frag_len + sizeof(struct lowpan_frag_hdr)
		                 + sizeof(uint8_t);
	    result = serial_output_am_payload(frame_begin, frame_len,
					      &hw_addr, &hw_dst_addr);
	    if (result < 0) {
		perror("serial_output_am_payload() failed\n");
		//return -1;
	    }
	    buf_begin += frag_len;
	    len -= frag_len;
	    dgram_offset += frag_len/8; /* in 8-byte multiples */
	}
	return 1;

    } else {
	/* no need for fragmentation */
	serial_output_am_payload(buf_begin, len,
				 &hw_addr, &hw_dst_addr);
	return 1;
    }

}

/* ------------------------------------------------------------------------- */
/* handling of data arriving on the serial port */

/* 
 * read data on serial port and send it to the tun interface
 * does fragment reassembly
 */
int serial_input()
{
    int result = 0;

    void *ser_data; /* data read from serial port */
    int ser_len;    /* length of data read from serial port */
    uint8_t *buf;
    int len;

    am_packet_t *AMpacket;
    struct hw_addr hw_src_addr;
    struct hw_addr hw_dst_addr;
    uint8_t *dispatch;
    struct lowpan_broadcast_hdr *bc_hdr;
    struct lowpan_frag_hdr *frag_hdr;

    uint16_t dgram_tag;
    uint16_t dgram_size;
    uint8_t dgram_offset;
    struct timeval tv;
    frag_info_t *p;
    frag_info_t **q;
    int last_frag;
    lowpan_pkt_t *pkt;

    printf("serial_input()\n");
    /* read data from serial port */
    ser_data = read_serial_packet(ser_src, &ser_len);

    /* process the packet we have received */
    if (ser_len && ser_data) {
	printf("dumping data on serial port...\n");
	dump_serial_packet(ser_data, ser_len);
	AMpacket = ser_data;

	/* copy 802.15.4 addresses */
	// TODO: check if I got the byte ordering right
	hw_src_addr.type = HW_ADDR_SHORT;
	memcpy(hw_src_addr.addr_short, &AMpacket->src,
	       sizeof(hw_src_addr.addr_short));
	hw_dst_addr.type = HW_ADDR_SHORT;
	memcpy(hw_dst_addr.addr_short, &AMpacket->dst,
	       sizeof(hw_dst_addr.addr_short));

	/* --- 6lowpan optional headers --- */
	buf = AMpacket->data;
	len = AMpacket->length;
	if (len != ser_len - 8) {
	    fprintf(stderr,
		    "warning: mismatch between AMpacket->length(%d)"\
		    " and ser_len - 8(%d)", AMpacket->length, ser_len - 8);
	}
	// TODO: check if length has a sensible value
	dispatch = AMpacket->data;
	/* Mesh Addressing header */
	if ( (*dispatch & DISPATCH_MESH_MASK) == DISPATCH_MESH) {
	    /* move over the dispatch field */
	    buf += sizeof(*dispatch);