2011-05-11 13:39:23 +03:00
|
|
|
/*
|
|
|
|
* dirtpan/dirtpan.c - Quick and dirty IPv4 over 802.15.4 tunnel
|
|
|
|
*
|
|
|
|
* Written 2011 by Werner Almesberger
|
|
|
|
* Copyright 2011 Werner Almesberger
|
|
|
|
*
|
|
|
|
* This program is free software; you can redistribute it and/or modify
|
|
|
|
* it under the terms of the GNU General Public License as published by
|
|
|
|
* the Free Software Foundation; either version 2 of the License, or
|
|
|
|
* (at your option) any later version.
|
|
|
|
*/
|
|
|
|
|
|
|
|
|
|
|
|
#include <stdint.h>
|
|
|
|
#include <stdlib.h>
|
|
|
|
#include <stdio.h>
|
|
|
|
#include <unistd.h>
|
|
|
|
#include <string.h>
|
|
|
|
#include <fcntl.h>
|
|
|
|
#include <assert.h>
|
|
|
|
#include <sys/types.h>
|
|
|
|
#include <sys/time.h>
|
|
|
|
#include <sys/select.h>
|
|
|
|
#include <sys/ioctl.h>
|
|
|
|
#include <sys/socket.h>
|
|
|
|
#include <linux/if.h>
|
|
|
|
#include <linux/if_tun.h>
|
|
|
|
|
|
|
|
#include <ieee802154.h>
|
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
* Control byte structure:
|
|
|
|
*
|
|
|
|
* +--7--+--6--+--5--+--4--+--3--+--2--+--1--+
|
|
|
|
* | 0 | 0 | 0 | 0 | seq | pck_type |
|
|
|
|
* +-----+-----+-----+-----+-----+-----+-----+
|
|
|
|
*/
|
|
|
|
|
|
|
|
#define SEQ 4
|
|
|
|
#define PT_MASK 3
|
|
|
|
|
|
|
|
enum packet_type {
|
|
|
|
pt_first = 0,
|
|
|
|
pt_next = 1,
|
|
|
|
pt_ack = 2,
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
|
|
#define TUN_DEV "/dev/net/tun"
|
|
|
|
|
|
|
|
#define MAX_FRAG (127-11-2-1) /* MHDR, FCS, control byte */
|
|
|
|
#define MAX_PACKET 2000
|
|
|
|
#define MAX_TRIES 5
|
|
|
|
#define T_REASS_MS 200
|
|
|
|
#define T_ACK_MS 50
|
|
|
|
|
|
|
|
|
|
|
|
static enum state {
|
|
|
|
s_idle,
|
|
|
|
s_tx,
|
|
|
|
s_rx,
|
|
|
|
} state = s_idle;
|
|
|
|
|
|
|
|
|
|
|
|
static int tun, net;
|
|
|
|
static uint8_t packet[MAX_PACKET+1];
|
|
|
|
static void *pos;
|
|
|
|
static int left;
|
|
|
|
static int my_seq = 0, peer_seq;
|
|
|
|
static int retries;
|
|
|
|
static int debug = 0;
|
|
|
|
|
|
|
|
|
|
|
|
/* ----- Debugging --------------------------------------------------------- */
|
|
|
|
|
|
|
|
|
|
|
|
static void debug_label(const char *label)
|
|
|
|
{
|
|
|
|
const char *t;
|
|
|
|
|
|
|
|
switch (state) {
|
|
|
|
case s_idle:
|
|
|
|
t = "--";
|
|
|
|
break;
|
|
|
|
case s_tx:
|
|
|
|
t = "tx";
|
|
|
|
break;
|
|
|
|
case s_rx:
|
|
|
|
t = "rx";
|
|
|
|
break;
|
|
|
|
default:
|
|
|
|
t = "???";
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
fprintf(stderr, "%s(%s)", label, t);
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
static void dump(const void *buf, int size)
|
|
|
|
{
|
|
|
|
const uint8_t *p = buf;
|
|
|
|
|
|
|
|
while (size--) {
|
|
|
|
fprintf(stderr, "%s%02x", p == buf ? "" : " ", *p);
|
|
|
|
p++;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
static void debug_ip(const char *label, void *buf, int size)
|
|
|
|
{
|
2011-05-11 20:15:45 +03:00
|
|
|
if (debug < 2)
|
2011-05-11 13:39:23 +03:00
|
|
|
return;
|
|
|
|
debug_label(label);
|
|
|
|
fprintf(stderr, ", %d: ", size);
|
|
|
|
dump(buf, size);
|
|
|
|
fprintf(stderr, "\n");
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
static void debug_dirt(const char *label, void *buf, int size)
|
|
|
|
{
|
|
|
|
const uint8_t *p = buf;
|
|
|
|
|
|
|
|
if (!debug)
|
|
|
|
return;
|
2011-05-11 20:15:45 +03:00
|
|
|
if (debug == 1) {
|
|
|
|
if (size) {
|
|
|
|
fprintf(stderr, "%c%d",
|
|
|
|
(label[1] == '>' ? "FNA?" : "fna?")[*p & PT_MASK],
|
|
|
|
*p & SEQ ? 0 : 1);
|
|
|
|
}
|
|
|
|
return;
|
|
|
|
}
|
2011-05-11 13:39:23 +03:00
|
|
|
debug_label(label);
|
2011-05-11 20:15:45 +03:00
|
|
|
fprintf(stderr, ", %d+1: ", size-1);
|
2011-05-11 13:39:23 +03:00
|
|
|
if (size) {
|
2011-05-11 20:15:45 +03:00
|
|
|
fprintf(stderr, "%02x(%c%d) | ",
|
2011-05-11 13:39:23 +03:00
|
|
|
*p, "FNA?"[*p & PT_MASK], *p & SEQ ? 0 : 1);
|
|
|
|
dump(buf+1, size-1);
|
|
|
|
}
|
|
|
|
fprintf(stderr, "\n");
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
2011-05-11 20:30:54 +03:00
|
|
|
static void debug_timeout(const char *label)
|
2011-05-11 13:39:23 +03:00
|
|
|
{
|
|
|
|
if (!debug)
|
|
|
|
return;
|
2011-05-11 20:30:54 +03:00
|
|
|
if (debug == 1) {
|
|
|
|
fprintf(stderr, "*%c", *label);
|
|
|
|
return;
|
|
|
|
}
|
2011-05-11 13:39:23 +03:00
|
|
|
debug_label(label);
|
|
|
|
fprintf(stderr, "\n");
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/* ----- Timers ------------------------------------------------------------ */
|
|
|
|
|
|
|
|
|
|
|
|
static struct timeval t0;
|
|
|
|
|
|
|
|
|
|
|
|
static void start_timer(void)
|
|
|
|
{
|
|
|
|
gettimeofday(&t0, NULL);
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
static struct timeval *timeout(int ms)
|
|
|
|
{
|
|
|
|
static struct timeval t;
|
|
|
|
|
|
|
|
gettimeofday(&t, NULL);
|
|
|
|
t.tv_sec -= t0.tv_sec;
|
|
|
|
t.tv_usec -= t0.tv_usec;
|
|
|
|
t.tv_usec += 1000*ms;
|
|
|
|
|
|
|
|
while (t.tv_usec < 0) {
|
|
|
|
t.tv_sec--;
|
|
|
|
t.tv_usec += 1000000;
|
|
|
|
}
|
2011-05-11 21:43:07 +03:00
|
|
|
while (t.tv_usec >= 1000000) {
|
2011-05-11 13:39:23 +03:00
|
|
|
t.tv_sec++;
|
|
|
|
t.tv_usec -= 1000000;
|
|
|
|
}
|
|
|
|
if (t.tv_sec < 0)
|
|
|
|
t.tv_sec = t.tv_usec = 0;
|
|
|
|
|
|
|
|
return &t;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/* ----- Packet/frame delivery --------------------------------------------- */
|
|
|
|
|
|
|
|
|
|
|
|
static inline int send_size(void)
|
|
|
|
{
|
|
|
|
return left > MAX_FRAG ? MAX_FRAG : left;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
static void write_buf(int fd, void *buf, int size)
|
|
|
|
{
|
|
|
|
ssize_t wrote;
|
|
|
|
|
|
|
|
wrote = write(fd, buf, size);
|
|
|
|
if (wrote < 0) {
|
|
|
|
perror("write");
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
if (wrote != size)
|
|
|
|
fprintf(stderr, "short write: %d < %d\n", (int) wrote, size);
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
static void send_frame(void *buf, int size)
|
|
|
|
{
|
|
|
|
debug_dirt("->", buf, size);
|
|
|
|
write_buf(net, buf, size);
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
static void send_more(void)
|
|
|
|
{
|
|
|
|
uint8_t *p = pos-1;
|
|
|
|
|
|
|
|
*p = (pos == packet+1 ? pt_first : pt_next) | (my_seq ? SEQ : 0);
|
|
|
|
send_frame(p, send_size()+1);
|
|
|
|
start_timer();
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
static void send_ack(int seq)
|
|
|
|
{
|
|
|
|
uint8_t ack = pt_ack | (seq ? SEQ : 0);
|
|
|
|
|
|
|
|
send_frame(&ack, 1);
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/* ----- Main events ------------------------------------------------------- */
|
|
|
|
|
|
|
|
|
|
|
|
static void rx_pck(void *buf, int size)
|
|
|
|
{
|
|
|
|
const uint8_t *p = buf;
|
|
|
|
uint8_t ctrl, type, seq;
|
|
|
|
|
|
|
|
debug_dirt("-<", buf, size);
|
|
|
|
|
|
|
|
if (size < 1)
|
|
|
|
return;
|
|
|
|
|
|
|
|
ctrl = *p;
|
|
|
|
type = ctrl & PT_MASK;
|
|
|
|
seq = !!(ctrl & SEQ);
|
|
|
|
|
2011-05-11 22:24:12 +03:00
|
|
|
if (type == pt_first || type == pt_next)
|
|
|
|
send_ack(seq);
|
2011-05-11 13:39:23 +03:00
|
|
|
switch (state) {
|
|
|
|
case s_tx:
|
|
|
|
if (type != pt_ack)
|
|
|
|
return;
|
|
|
|
if (seq != my_seq)
|
|
|
|
return;
|
|
|
|
pos += send_size();
|
|
|
|
left -= send_size();
|
|
|
|
if (!left) {
|
|
|
|
state = s_idle;
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
my_seq = !my_seq;
|
|
|
|
retries = 0;
|
|
|
|
send_more();
|
|
|
|
return;
|
|
|
|
case s_rx:
|
|
|
|
if (type == pt_first) {
|
|
|
|
start_timer();
|
|
|
|
state = s_idle;
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
if (type != pt_next)
|
|
|
|
return;
|
2011-05-11 22:24:12 +03:00
|
|
|
if (seq == peer_seq)
|
|
|
|
return; /* retransmission */
|
2011-05-11 13:39:23 +03:00
|
|
|
goto recv_more;
|
|
|
|
case s_idle:
|
|
|
|
if (type == pt_first)
|
|
|
|
break;
|
2011-05-11 22:24:12 +03:00
|
|
|
if (type == pt_next)
|
2011-05-11 13:39:23 +03:00
|
|
|
return;
|
|
|
|
return;
|
|
|
|
default:
|
|
|
|
abort();
|
|
|
|
}
|
|
|
|
|
|
|
|
if (size < 5)
|
|
|
|
return;
|
|
|
|
left = p[3] << 8 | p[4];
|
|
|
|
if (left > MAX_PACKET+1)
|
|
|
|
return;
|
|
|
|
state = s_rx;
|
|
|
|
pos = packet;
|
|
|
|
|
|
|
|
recv_more:
|
|
|
|
if (left < size-1) {
|
|
|
|
state = s_idle;
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
memcpy(pos, buf+1, size-1);
|
|
|
|
pos += size-1;
|
|
|
|
left -= size-1;
|
|
|
|
peer_seq = seq;
|
|
|
|
|
|
|
|
if (!left) {
|
|
|
|
debug_ip("<-", packet, pos-(void *) packet);
|
|
|
|
write_buf(tun, packet, pos-(void *) packet);
|
|
|
|
state = s_idle;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
static void tx_pck(void *buf, int size)
|
|
|
|
{
|
|
|
|
const uint8_t *p = buf;
|
|
|
|
|
|
|
|
debug_ip(">-", buf, size);
|
|
|
|
assert(state == s_idle);
|
|
|
|
state = s_tx;
|
|
|
|
pos = packet+1;
|
|
|
|
left = p[2] << 8 | p[3];
|
|
|
|
assert(left <= MAX_PACKET);
|
|
|
|
assert(left == size);
|
|
|
|
/*
|
|
|
|
* We could avoid the memcpy by reading directly into "packet"
|
|
|
|
*/
|
|
|
|
memcpy(pos, buf, size);
|
|
|
|
my_seq = !my_seq;
|
|
|
|
retries = 0;
|
|
|
|
send_more();
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
static void ack_timeout(void)
|
|
|
|
{
|
2011-05-11 20:30:54 +03:00
|
|
|
debug_timeout("ACK-TO");
|
2011-05-11 13:39:23 +03:00
|
|
|
if (++retries == MAX_TRIES)
|
|
|
|
state = s_idle;
|
|
|
|
else
|
|
|
|
send_more();
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
static void reass_timeout(void)
|
|
|
|
{
|
2011-05-11 20:30:54 +03:00
|
|
|
debug_timeout("REASS-TO");
|
2011-05-11 13:39:23 +03:00
|
|
|
state = s_idle;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/* ----- Event dispatcher -------------------------------------------------- */
|
|
|
|
|
|
|
|
|
|
|
|
static void event(void)
|
|
|
|
{
|
|
|
|
uint8_t buf[MAX_PACKET];
|
|
|
|
struct timeval *to;
|
|
|
|
fd_set rset;
|
|
|
|
int res;
|
|
|
|
ssize_t got;
|
|
|
|
|
|
|
|
FD_ZERO(&rset);
|
|
|
|
FD_SET(net, &rset);
|
|
|
|
switch (state) {
|
|
|
|
case s_idle:
|
|
|
|
FD_SET(tun, &rset);
|
|
|
|
to = NULL;
|
|
|
|
break;
|
|
|
|
case s_rx:
|
|
|
|
to = timeout(T_REASS_MS);
|
|
|
|
break;
|
|
|
|
case s_tx:
|
|
|
|
to = timeout(T_ACK_MS);
|
|
|
|
break;
|
|
|
|
default:
|
|
|
|
abort();
|
|
|
|
}
|
|
|
|
res = select(net > tun ? net+1 : tun+1, &rset, NULL, NULL, to);
|
|
|
|
if (res < 0) {
|
|
|
|
perror("select");
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
if (!res) {
|
|
|
|
switch (state) {
|
|
|
|
case s_rx:
|
|
|
|
reass_timeout();
|
|
|
|
break;
|
|
|
|
case s_tx:
|
|
|
|
ack_timeout();
|
|
|
|
break;
|
|
|
|
default:
|
|
|
|
abort();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
if (FD_ISSET(tun, &rset)) {
|
|
|
|
got = read(tun, buf, sizeof(buf));
|
|
|
|
if (got < 0) {
|
|
|
|
perror("read tun");
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
tx_pck(buf, got);
|
|
|
|
}
|
|
|
|
if (FD_ISSET(net, &rset)) {
|
|
|
|
got = read(net, buf, sizeof(buf));
|
|
|
|
if (got < 0) {
|
|
|
|
perror("read net");
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
rx_pck(buf, got);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/* ----- Setup ------------------------------------------------------------- */
|
|
|
|
|
|
|
|
|
|
|
|
static int open_net(uint16_t pan, uint16_t me, uint16_t peer)
|
|
|
|
{
|
|
|
|
struct sockaddr_ieee802154 addr;
|
2011-05-11 20:41:11 +03:00
|
|
|
int zero = 0;
|
2011-05-11 13:39:23 +03:00
|
|
|
int s;
|
|
|
|
|
|
|
|
s = socket(PF_IEEE802154, SOCK_DGRAM, 0);
|
|
|
|
if (s < 0) {
|
|
|
|
perror("socket 802.15.4");
|
|
|
|
exit(1);
|
|
|
|
}
|
|
|
|
|
|
|
|
addr.family = AF_IEEE802154;
|
|
|
|
addr.addr.addr_type = IEEE802154_ADDR_SHORT;
|
|
|
|
addr.addr.pan_id = pan;
|
|
|
|
addr.addr.short_addr = me;
|
|
|
|
|
|
|
|
if (bind(s, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
|
|
|
|
perror("bind 802.15.4");
|
|
|
|
exit(1);
|
|
|
|
}
|
|
|
|
|
|
|
|
addr.addr.short_addr = peer;
|
|
|
|
if (connect(s, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
|
|
|
|
perror("connect 802.15.4");
|
|
|
|
exit(1);
|
|
|
|
}
|
|
|
|
|
2011-05-11 20:41:11 +03:00
|
|
|
if (setsockopt(s, SOL_IEEE802154, WPAN_WANTACK, &zero, sizeof(zero))
|
|
|
|
< 0) {
|
|
|
|
perror("setsockopt SOL_IEEE802154 WPAN_WANTACK");
|
|
|
|
exit(1);
|
|
|
|
}
|
|
|
|
|
2011-05-11 13:39:23 +03:00
|
|
|
return s;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
static int open_tun(void)
|
|
|
|
{
|
|
|
|
struct ifreq ifr;
|
|
|
|
int fd;
|
|
|
|
|
|
|
|
fd = open(TUN_DEV, O_RDWR);
|
|
|
|
if (fd < 0) {
|
|
|
|
perror(TUN_DEV);
|
|
|
|
exit(1);
|
|
|
|
}
|
|
|
|
|
|
|
|
memset(&ifr, 0, sizeof(ifr));
|
|
|
|
ifr.ifr_flags = IFF_TUN | IFF_NO_PI;
|
|
|
|
|
|
|
|
if (ioctl(fd, TUNSETIFF, (void *) &ifr) < 0) {
|
|
|
|
perror("ioctl TUNSETIFF");
|
|
|
|
exit(1);
|
|
|
|
}
|
|
|
|
|
|
|
|
fprintf(stderr, "%s\n", ifr.ifr_name);
|
|
|
|
|
|
|
|
return fd;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/* ----- Command-line processing ------------------------------------------- */
|
|
|
|
|
|
|
|
|
|
|
|
static void usage(const char *name)
|
|
|
|
{
|
2011-05-11 20:15:45 +03:00
|
|
|
fprintf(stderr, "usage: %s [-d [-d]] pan_id src_addr dst_addr\n",
|
|
|
|
name);
|
2011-05-11 13:39:23 +03:00
|
|
|
exit(1);
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
static uint16_t addr(const char *name, const char *s)
|
|
|
|
{
|
|
|
|
char *end;
|
|
|
|
unsigned long n;
|
|
|
|
|
|
|
|
n = strtoul(s, &end, 16);
|
|
|
|
if (*end)
|
|
|
|
usage(name);
|
|
|
|
if (n > 0xffff)
|
|
|
|
usage(name);
|
|
|
|
return n;
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
int main(int argc, char **argv)
|
|
|
|
{
|
|
|
|
uint16_t pan, src, dst;
|
|
|
|
int c;
|
|
|
|
|
|
|
|
while ((c = getopt(argc, argv, "d")) != EOF)
|
|
|
|
switch (c) {
|
|
|
|
case 'd':
|
|
|
|
debug++;
|
|
|
|
break;
|
|
|
|
default:
|
|
|
|
usage(*argv);
|
|
|
|
}
|
|
|
|
|
|
|
|
switch (argc-optind) {
|
|
|
|
case 3:
|
|
|
|
pan = addr(*argv, argv[optind]);
|
|
|
|
src = addr(*argv, argv[optind+1]);
|
|
|
|
dst = addr(*argv, argv[optind+2]);
|
|
|
|
break;
|
|
|
|
default:
|
|
|
|
usage(*argv);
|
|
|
|
}
|
|
|
|
|
|
|
|
net = open_net(pan, src, dst);
|
|
|
|
tun = open_tun();
|
|
|
|
while (1)
|
|
|
|
event();
|
|
|
|
|
|
|
|
return 0;
|
|
|
|
}
|