serial_tun.c

tinyos-2.x.rar

	    len -= sizeof(*dispatch);

	    /* Hops Left */
	    if ((*dispatch & 0x0F) == 0) {
	      goto discard_packet;
	    }
	    
	    /* Final Destination Address */
	    if (*dispatch & DISPATCH_MESH_F_FLAG) {
		hw_dst_addr.type = HW_ADDR_LONG;
		memcpy(&hw_dst_addr.addr_long, buf,
		       sizeof(hw_dst_addr.addr_long));
		buf += sizeof(hw_dst_addr.addr_long);
		len -= sizeof(hw_dst_addr.addr_long);
	    } else {
		hw_dst_addr.type = HW_ADDR_SHORT;
		memcpy(&hw_dst_addr.addr_short, buf,
		       sizeof(hw_dst_addr.addr_short));
		buf += sizeof(hw_dst_addr.addr_short);
		len -= sizeof(hw_dst_addr.addr_short);
	    }
	    
	    /* check if we're the recipient */
	    if (cmp_hw_addr(&hw_dst_addr, &hw_addr) != 0
		&& !hw_addr_is_broadcat(&hw_dst_addr)) {
		// TODO: if mesh forwarding enabled, then forward
		goto discard_packet;
	    }
	    
	    /* Originator Address */
	    if (*dispatch & DISPATCH_MESH_O_FLAG) {
		hw_src_addr.type = HW_ADDR_LONG;
		memcpy(&hw_src_addr.addr_long, buf,
		       sizeof(hw_src_addr.addr_long));
		buf += sizeof(hw_src_addr.addr_long);
		len -= sizeof(hw_src_addr.addr_long);
	    } else {
		hw_src_addr.type = HW_ADDR_SHORT;
		memcpy(&hw_src_addr.addr_short, buf,
		       sizeof(hw_src_addr.addr_short));
		buf += sizeof(hw_src_addr.addr_short);
		len -= sizeof(hw_src_addr.addr_short);
	    }
	    
	    dispatch = buf;
	}
	/* Broadcast header */
	if (*dispatch == DISPATCH_BC0) {
	    bc_hdr = (struct lowpan_broadcast_hdr *) buf;
	    // do something usefull with bc_hdr->seq_no...
	    
	    buf += (sizeof(struct lowpan_broadcast_hdr));
	    len -= (sizeof(struct lowpan_broadcast_hdr));
	    dispatch = buf;
	}

	/* fragment header */
	if ((*dispatch & DISPATCH_FRAG_MASK)
	    == DISPATCH_FIRST_FRAG
	    || (*dispatch & DISPATCH_FRAG_MASK)
	    == DISPATCH_SUBSEQ_FRAG
	    ) {
	    frag_hdr = (struct lowpan_frag_hdr *) buf;
	    buf += sizeof(struct lowpan_frag_hdr);
	    len -= sizeof(struct lowpan_frag_hdr);

	    /* collect information about the fragment */
	    dgram_tag = frag_hdr->dgram_tag;
	    dgram_size = frag_hdr->dgram_size & htons(0x07FF);
	    //dgram_size = frag_hdr->dgram_size8[1];
	    //dgram_size += ((uint16_t) (frag_hdr->dgram_size8[0] & 0x07)) << 8;
	    if ((*dispatch & DISPATCH_FRAG_MASK) == DISPATCH_SUBSEQ_FRAG) {
		dgram_offset = *buf;
		buf += 1;
		len -= 1;
	    } else {
		dgram_offset = 0;
	    }
	    
	    debug("fragment reassembly: tag: 0x%04X, size: %d, offset: %d"\
		  "(*8=%d)\n",
		  ntohs(dgram_tag), ntohs(dgram_size),
		  dgram_offset, dgram_offset*8);

	    pkt = find_fragment(&hw_src_addr, &hw_dst_addr,
				dgram_size, dgram_tag);
	    if (pkt) {
		debug("found an existing reassembly buffer\n");
		/* fragment reassembly buffer found */
		/* check for overlap */
		for (p = pkt->frag_list; p; p=p->next) {
		    if (dgram_offset == p->offset && len == p->len) {
			/* duplicate - discard it */
			result = 0;
			goto discard_packet;
		    } else if ((dgram_offset == p->offset && len < p->len) ||
			       (dgram_offset > p->offset
				&& dgram_offset < p->offset + p->len/8)
			       ) {
			goto frag_overlap;
		    }
		}
		/* no overlap found */
		goto frag_reassemble;
	    } else {
		debug("starting a new reassembly buffer\n");
		/* fragment reassembly buffer not found - set up a new one */
		pkt = malloc(sizeof(lowpan_pkt_t));
		if (!pkt) {
		    // no free slot for reassembling fragments
		    fprintf(stderr, "out of memory - dropping a fragment\n");
		    result = -1;
		    goto discard_packet;
		}
		pkt->next = fragments;
		fragments = pkt;
		clear_pkt(pkt);
		memcpy(&pkt->hw_src_addr, &hw_src_addr, sizeof(hw_src_addr));
		memcpy(&pkt->hw_dst_addr, &hw_dst_addr, sizeof(hw_dst_addr));
		pkt->dgram_tag = dgram_tag;
		pkt->dgram_size = dgram_size;
		gettimeofday(&tv, NULL);
		pkt->frag_timeout = tv.tv_sec + FRAG_TIMEOUT;
		goto frag_reassemble;
	    }
	    
	frag_overlap:
	    /* overlap - discard previous frags
	     * and restart freagment reassembly
	     */
	    free_frag_list(pkt->frag_list);
	    pkt->frag_list = NULL;
	    /* not sure if we want to clear the whole buf */
	    //memset(&pkt->buf, 0, sizeof(pkt->buf));
	    gettimeofday(&tv, NULL);
	    pkt->frag_timeout = tv.tv_sec + FRAG_TIMEOUT;
	    goto frag_reassemble;
	    
	frag_reassemble:
	    /* copy buf data */
	    debug("dgram_offset: %d\n", dgram_offset);
	    memcpy(pkt->buf_begin + dgram_offset*8, buf, len);
	    //TODO: make sure a large len does not cause a buffer overflow

	    /* update frag_info */
	    p = malloc(sizeof(frag_info_t));
	    if (!p) {
		fprintf(stderr, "out of memory - fragment "\
			"reassembly failing\n");
	    } else {
		p->offset = dgram_offset;
		p->len = len;

		/* insert frag_info into the orderer list */
		if (pkt->frag_list) {
		    for(q = &(pkt->frag_list); (*q)->next; q=&((*q)->next)) {
			if (p->offset > (*q)->offset) {
			    break;
			}
		    }
		    if ((*q)) {
			debug("inserting frag_info before offset %d\n",
			      (*q)->offset);
		    } else {
			debug("inserting frag_info at the beginning/end\n");
		    }

		    p->next = *q;
		    *q = p;
		} else {
		    debug("inserting frag_info to the beginning "
			  "of the list\n");
		    p->next = pkt->frag_list;
		    pkt->frag_list = p;
		}
	    }

	    /* check if this is not the last fragment */
	    if (!dgram_offset) {
		/* the first fragment cannot be the last one */
		last_frag = 0;
	    } else {
		debug("checking last_frag...\n");
		last_frag=1;
		dgram_offset = ntohs(dgram_size)/8;
		for(p=pkt->frag_list; p && dgram_offset; p=p->next) {
		    debug("dgram_offset: %d, p->offset: %d, p->len: %d\n",
			  dgram_offset, p->offset, p->len);
		    if (p->offset + p->len/8 != dgram_offset) {
			debug("offset mismatch - not the last fragment\n");
			last_frag = 0;
			break;
		    }
		    dgram_offset = p->offset;
		}
	    }

	    if (last_frag) {
		debug("last fragment, reassembly done\n");
		pkt->len = ntohs(dgram_size);
		
		debug("dumping reassembled datagram...\n");
		dump_serial_packet(pkt->buf_begin, pkt->len);
		
		/* pass up the complete packet */
		result = serial_input_layer3(pkt->buf_begin, pkt->len,
					     &hw_src_addr, &hw_dst_addr);
		/* deallocate pkt and all fragment info */
		free_lowpan_pkt(pkt);
	    } else {
		result = 0;
	    }
	} else { /* no fragment header present */
	    result =  serial_input_layer3(buf, len,
					  &hw_src_addr, &hw_dst_addr);
	}
	
    } else {
	//printf("no data on serial port, but FD trigerred select\n");
    }
 discard_packet:
    if (ser_data) {
	free(ser_data);
    }
    if (ser_data && ser_len > 0) {
	return 1;
    } else {
	return 0;
    }
    //return result;
}

int serial_input_layer3(uint8_t *buf, int len,
			const hw_addr_t *hw_src_addr,
			const hw_addr_t *hw_dst_addr)
{
    uint8_t *dispatch = buf;
    //debug("%s()\n", __func__);
    //dump_serial_packet(buf, len);

    if (len <= 0) return 1;

    /* uncompressed IPv6 */
    if (*dispatch == 0x41) {
	return serial_input_ipv6_uncompressed(buf+1, len-1,
					      hw_src_addr, hw_dst_addr);

    }
    /* LOWPAN_HC1 compressed IPv6 */
    else if (*dispatch == 0x42) {
	return serial_input_ipv6_compressed(buf+1, len-1,
					    hw_src_addr, hw_dst_addr);
    }
    /* unknown dispatch value if we got here */
    else {
	debug("unknown dispatch value: %X\n", *dispatch);
	return tun_write(tun_fd, (char*) buf+1, len-1);
    }
}

int serial_input_ipv6_uncompressed(uint8_t *buf, int len,
				   const hw_addr_t *hw_src_addr,
				   const hw_addr_t *hw_dst_addr)
{
    debug("%s()\n", __func__);
    //dump_serial_packet(buf, len);
    // TODO: update neighbor table
    return tun_write(tun_fd, (char*) buf, len);
}

int serial_input_ipv6_compressed(uint8_t *buf, int len,
				 const hw_addr_t *hw_src_addr,
				 const hw_addr_t *hw_dst_addr)
{
    int ret=0;
    int new_len;
    uint8_t *new_buf;

    debug("%s()\n", __func__);
    if (0 == lowpan_decompress(buf, len,
			       hw_src_addr, hw_dst_addr,
			       &new_buf, &new_len)
	) {
	// TODO: update neighbor table
	buf = new_buf;
	len = new_len;
	ret =  tun_write(tun_fd, (char*) buf, len);
	
	if (new_buf && new_len) {
	    free(new_buf);
	}
    }
    
    return ret;
}

/* ------------------------------------------------------------------------- */

void timer_fired()
{
    struct timeval tv;
    lowpan_pkt_t *p;
    lowpan_pkt_t **q;

    /* time out old fragments */
    (void) gettimeofday(&tv, NULL);
    for(q = &fragments; *q; ) {
	if ((*q)->frag_timeout > tv.tv_sec) {
	    p = (*q)->next;
	    free(*q);
	    *q = p;
	} else {
	    q = &((*q)->next);
	}
    }
    // TODO: ND retransmission
    // TODO: neighbor table timeouts
}

/* shifts data between the serial port and the tun interface */
int serial_tunnel(serial_source ser_src, int tun_fd) {
    //int result;
    fd_set fs;
    
    while (1) {
	FD_ZERO (&fs);
	FD_SET (tun_fd, &fs);
	FD_SET (serial_source_fd(ser_src), &fs);

	select (tun_fd>serial_source_fd(ser_src)?
		tun_fd+1 : serial_source_fd(ser_src)+1,
		&fs, NULL, NULL, NULL);

	debug("--- select() fired ---\n");

	/* data available on tunnel device */
	if (FD_ISSET (tun_fd, &fs)) {
	    //result = tun_input();
	    while( tun_input() );
	}
	
	/* data available on serial port */
	if (FD_ISSET (serial_source_fd(ser_src), &fs)) {
	    /* more packets may be queued so process them all */
	    while (serial_input());
	    /* using serial_source_empty() seems to make select()
	     * fire way too often, so the above solution is better */
	    //while(! serial_source_empty(ser_src)) {
	    //result = serial_input();
		//}
	}
	/* end of data available */
    }
    /* end of while(1) */
    
    return 0;
}

int main(int argc, char **argv) {
    char dev[16];
    
    if (argc != 3)
	{
	    fprintf(stderr, "Usage: %s <device> <rate>\n", argv[0]);
	    exit(2);
	}
    
    hw_addr.type = HW_ADDR_SHORT;
    hw_addr.addr_short[0] = 0x00; // network byte order
    hw_addr.addr_short[1] = 0x12;

    /* create the tunnel device */
    dev[0] = 0;
    tun_fd = tun_open(dev);
    if (tun_fd < 1) {
	printf("Could not create tunnel device. Fatal.\n");
		return 1;
    }
    else {
	printf("Created tunnel device: %s\n", dev);
    }
    
    /* open the serial port */
    ser_src = open_serial_source(argv[1], platform_baud_rate(argv[2]),
				 1, stderr_msg);
    /* 0 - blocking reads
     * 1 - non-blocking reads
     */
    
    if (!ser_src) {
	debug("Couldn't open serial port at %s:%s\n",
		argv[1], argv[2]);
	exit(1);
    }
    
    /* set up the tun interface */
    printf("\n");
    ssystem("ifconfig tun0 up");
    ssystem("ifconfig tun0 mtu 1280");
    ssystem("ifconfig tun0 inet6 add 2001:0638:0709:1234::fffe:12/64");
    ssystem("ifconfig tun0 inet6 add fe80::fffe:12/64");
    printf("\n");

    printf("try:\n\tsudo ping6 -s 0 2001:0638:0709:1234::fffe:14\n"
	   "\tnc6 -u 2001:0638:0709:1234::fffe:14 1234\n\n");

    /* start tunneling */
    serial_tunnel(ser_src, tun_fd);
    
    /* clean up */
    close_serial_source(ser_src);
    //close(ser_fd);
    tun_close(tun_fd, dev);
    return 0;
}