rrqnet/rrqnet.c

// This program is a UDP based tunneling of stdin/out Ethernet packets.
//
// A rrqnet program is a bi-directional networking plug that channels
// packets between a UDP port and stdin/out. It is configured on the
// command line with channel rules that declares which remotes it may
// communicate with. Allowed remotes are specified in the format
// "ip[/n][:port][=key]", to indicate which subnet and port to accept,
// and nominating the associated keyfile to use for channel
// encryption.
//
// The program maintains a table of actualized connections, as an
// association between MAC addresses and IP:port addresses. This table
// is used for resolving destination for outgoing packets, including
// the forwarding of broadcasts.
//

#include <errno.h>
#include <fcntl.h>
#include <linux/if.h>
#include <linux/if_tun.h>
#include <stddef.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/stat.h>
#include <sys/time.h>
#include <sys/types.h>
#include <time.h>
#include <unistd.h>

#include "htable.h"
#include "queue.h"

//// Data structures

// "Private Shared Key" details.
struct PSK {
    char *keyfile;
    unsigned int seed;		// Encryption seed
    unsigned char *key;		// Encryption key
    unsigned int key_length;	// Encryption key length
};

// Compacted IP address ipv4/ipv6
struct CharAddr {
    int width; // 4=ipv4 and 16=ipv6
    union {
	unsigned char bytes[16];
	struct in_addr in4; 
	struct in6_addr in6;
    };
};

// Details of channel rules.
struct Allowed {
    char *source;		// Orginal rule
    struct CharAddr addr;
    unsigned int bits;		// Bits of IP prefix
    unsigned short port;	// Port (0=any)
    struct PSK psk;		// Associated key
    htable ignored_mac;		// MAC to ignore by this spec
};

// Details of actualized connections.
struct Remote {
    struct SockAddr uaddr;
    struct Allowed *spec;	// Rule being instantiated
    struct timeval rec_when;	// Last received packet time, in seconds
};

// Details of an interface at a remote.
struct Interface  {
    unsigned char mac[6];	// MAC address used last (key for by_mac table)
    struct timeval rec_when; 	// Last packet time, in seconds
    struct Remote *remote;
};

// Maximal packet size .. allow for jumbo frames (9000)
#define BUFSIZE 10000

typedef struct _PacketItem {
    QueueItem base;
    int fd;
    struct SockAddr src;
    ssize_t len;
    unsigned char buffer[ BUFSIZE ];
} PacketItem;

typedef struct _ReaderData {
    int fd;
} ReaderData;

// heartbeat interval, in seconds
#define HEARTBEAT 30
#define HEARTBEAT_MICROS ( HEARTBEAT * 1000000 )

// Macros for timing, for struct timeval variables
#define TIME_MICROS(TM) (((int64_t) (TM)->tv_sec * 1000000) + (TM)->tv_usec )
#define DIFF_MICROS(TM1,TM2) ( TIME_MICROS(TM1) - TIME_MICROS(TM2) )

// RECENT_MICROS(T,M) is the time logic for requiring a gap time (in
// milliseconds) before shifting a MAC to a new remote. The limit is
// 6s for broadcast and 20s for unicast.
#define RECENT_MICROS(T,M) ((M) < ((T)? 6000000 : 20000000 ))

// VERYOLD_MICROSS is used for discarding downlink remotes whose latest
// activity is older than this.
#define VERYOLD_MICROS 180000000

////////// Variables

// Allowed remote specs are held in a table sorted by IP prefix.
static struct {
    struct Allowed **table;
    unsigned int count;
} allowed;

// Actual remotes are kept in a hash table keyed by their +uaddr+
// field, and another hash table keps Interface records for all MAC
// addresses sourced from some remote, keyed by their +mac+ field. The
// latter is used both for resolving destinations for outgoing
// packets, and for limiting broadcast cycles. The former table is
// used for limiting incoming packets to allowed sources, and then
// decrypt the payload accordingly.
static int hashcode_uaddr(struct _htable *table,unsigned char *key);
static int hashcode_mac(struct _htable *table,unsigned char *key);
static struct {
    htable by_mac; // struct Interface hash table
    htable by_addr; // struct Remote hash table
} remotes = {
    .by_mac = HTABLEINIT( struct Interface, mac, hashcode_mac ),
    .by_addr = HTABLEINIT( struct Remote, uaddr, hashcode_uaddr )
};

#define Interface_LOCK if ( pthread_mutex_lock( &remotes.by_mac.lock ) ) { \
	perror( "FATAL" ); exit( 1 ); }

#define Interface_UNLOCK if (pthread_mutex_unlock( &remotes.by_mac.lock ) ) { \
	perror( "FATAL" ); exit( 1 ); }

#define Interface_FIND(m,r) \
    htfind( &remotes.by_mac, m, (unsigned char **)&r )

#define Interface_ADD(r) \
    htadd( &remotes.by_mac, (unsigned char *)r )

#define Interface_DEL(r) \
    htdelete( &remotes.by_mac, (unsigned char *) r )

#define Remote_LOCK if ( pthread_mutex_lock( &remotes.by_addr.lock ) ) { \
	perror( "FATAL" ); exit( 1 ); }

#define Remote_UNLOCK if ( pthread_mutex_unlock( &remotes.by_addr.lock ) ) { \
	perror( "FATAL" ); exit( 1 ); }

#define Remote_FIND(a,r) \
    htfind( &remotes.by_addr, (unsigned char *)a, (unsigned char **) &r )

#define Remote_ADD(r) \
    htadd( &remotes.by_addr, (unsigned char *) r )

#define Remote_DEL(r) \
    htdelete( &remotes.by_addr, (unsigned char *) r )

#define Ignored_FIND(a,m,x)				\
    htfind( &a->ignored_mac, m, (unsigned char **)&x )

#define Ignored_ADD(a,x) \
    htadd( &a->ignored_mac, (unsigned char *)x )

// Input channels
static int stdio = 0; // Default is neither stdio nor tap
static char *tap = 0; // Name of tap, if any, or "-" for stdio
static int tap_fd = 0; // Also used for stdin in stdio mode
static int udp_fd;
static int threads_count = 0;
static int buffers_count = 0;

// Setup for multicast channel
static struct {
    struct ip_mreqn group;
    struct SockAddr sock;
    int fd;
    struct PSK psk;
} mcast;

// Flag to signal the UDP socket as being ipv6 or not (forced ipv4)
static int udp6 = 1;

// Flag whether to make some stderr outputs or not.
// 1 = normal verbosity, 2 = more output, 3 = source debug level stuff
static int verbose;

// Note: allows a thread to lock/unlock recursively
static pthread_mutex_t crypting = PTHREAD_MUTEX_INITIALIZER;

// Note: allows a thread to lock/unlock recursively
static pthread_mutex_t printing = PTHREAD_RECURSIVE_MUTEX_INITIALIZER_NP;

#define PRINTLOCK \
    if ( pthread_mutex_lock( &printing ) ) { perror( "FATAL" ); exit(1); }

#define PRINTUNLOCK \
    if ( pthread_mutex_unlock( &printing ) ) { perror( "FATAL" ); exit(1); }

#define PRINT( X ) { PRINTLOCK; X; PRINTUNLOCK; }

#define VERBOSEOUT(fmt, ...) \
    if ( verbose >= 1 ) PRINT( fprintf( stderr, fmt, ##__VA_ARGS__ ) )

#define VERBOSE2OUT(fmt, ...) \
    if ( verbose >= 2 ) PRINT( fprintf( stderr, fmt, ##__VA_ARGS__ ) )

#define VERBOSE3OUT(fmt, ...) \
    if ( verbose >= 3 ) PRINT( fprintf( stderr, fmt, ##__VA_ARGS__ ) )

// The actual name of this program (argv[0])
static unsigned char *progname;

// Compute a hashcode for the given SockAddr key
static int hashcode_uaddr(
    __attribute__((unused)) struct _htable *table,unsigned char *key)
{
    struct SockAddr *s = (struct SockAddr *) key;
    key = (unsigned char*) &s->in;
    unsigned char *e = key + ( ( s->in.sa_family == AF_INET )?
			       sizeof( struct sockaddr_in ) :
			       sizeof( struct sockaddr_in6 ) );
    int x = 0;
    while ( key < e ) {
	x += *(key++);
    }
    return x;
}

// Compute a hashcode for the given MAC addr key
static int hashcode_mac(struct _htable *table,unsigned char *key) {
    int x = 0;
    int i = 0;
    if ( table->size == 256 ) {
	for ( ; i < 6; i++ ) {
	    x += *(key++);
	}
	return x;
    }
    uint16_t *p = (uint16_t *) key;
    for ( ; i < 3; i++ ) {
	x += *( p++ );
    }
    return x;
}

// Make a text representation of bytes as ipv4 or ipv6
static char *inet_nmtoa(unsigned char *b,int w) {
    static char buffer[20000];
    int i = 0;
    char * p = buffer;
    if ( w == 4 ) {
	sprintf( p,"%d.%d.%d.%d", b[0], b[1], b[2], b[3] );
    } else if ( w == 16 ){
	sprintf( p,
  "%02x%02x:%02x%02x:%02x%02x:%02x%02x:%02x%02x:%02x%02x:%02x%02x:%02x%02x",
		 b[0], b[1], b[2], b[3],
		 b[4], b[5], b[6], b[7],
		 b[8], b[9], b[10], b[11],
		 b[12], b[13], b[14], b[15] );
    } else {
	VERBOSE3OUT( "HEX data of %d bytes\n", w );
	for ( ; i < w && i < 19000; i++, p += 3 ) {
	    sprintf( p, "%02x:", b[i] );
	}
	if ( w > 0 ) {
	    *(--p) = 0;
	}
    }
    return buffer;
}

// Form a MAC address string from 6 MAC address bytes, into one of the
// 4 static buffer, whose use are cycled.
static char *inet_mtoa(unsigned char *mac) {
    static char buffer[4][30];
    static int i = 0;
    if ( i > 3 ) {
	i = 0;
    }
    sprintf( buffer[i], "%02x:%02x:%02x:%02x:%02x:%02x",
	     mac[0], mac[1], mac[2], mac[3], mac[4], mac[5] );
    return buffer[i++];
}

// Form a socket address string from Sockaddr, into one of the
// 4 static buffer, whose use are cycled.
static char *inet_stoa(struct SockAddr *a) {
    static char buffer[1000];
    static char out[4][1000];
    static int i = 0;
    if ( i > 3 ) {
	i = 0;
    }
    if ( a->in.sa_family == AF_INET ) {
	sprintf( out[i], "%s:%d",
		 inet_ntop( AF_INET, &a->in4.sin_addr, buffer, 100 ),
		 ntohs( a->in4.sin_port ) );
    } else if ( a->in.sa_family == AF_INET6 ) {
	sprintf( out[i], "[%s]:%d",
		 inet_ntop( AF_INET6, &a->in6.sin6_addr, buffer, 100 ),
		 ntohs( a->in6.sin6_port ) );
    } else {
	sprintf( out[i], "<tap/stdio>" );
    }
    return out[i++];
}

// Debugging: string representation of an Allowed record.
static char *show_allowed(struct Allowed *a) {
    static char buffer[20000];
    if ( a == 0 ) {
	sprintf( buffer, "{tap/stdio}" );
    } else {
	sprintf( buffer, "%hd (%d) %s %p",
		 a->port, a->bits, inet_nmtoa( a->addr.bytes, a->addr.width ),
		 a->psk.key );
    }
    return buffer;
}

// Recognize uplink specification
static int is_uplink(struct Allowed *a) {
    return a->bits == (unsigned int) ( a->addr.width * 8 ) && a->port != 0;
}

// Add a new Interface for a Remote. If non-null, the interface is
// also added to the interface table.
static struct Interface *add_interface(unsigned char *mac,struct Remote *r) {
    struct Interface *x = calloc( 1, sizeof( struct Interface ) );
    memcpy( x->mac, mac, sizeof( x->mac ) );
    x->remote = r;
    if ( r ) {
	Interface_ADD( x );
    }
    return x;
}

// Add a new remote for a given address and spec.
static struct Remote *add_remote(struct SockAddr *a,struct Allowed *s) {
    struct Remote *r = calloc( 1, sizeof( struct Remote ) );
    if ( a != 0 ) {
	memcpy( &r->uaddr, a, sizeof( r->uaddr ) );
    }
    r->spec = s;
    VERBOSE2OUT( "add_remote %s from spec: %s\n",
		 inet_stoa( &r->uaddr ),
		 ( s == 0 )? ( (a == 0)? "{tap/stdio}" : "{multicast}" )
		 : show_allowed( s ) );
    Remote_ADD( r );
    return r;
}

// Add a new ignored interface on a channel
static int add_ignored(struct Allowed *link,unsigned char *mac) {
    struct Interface *x = add_interface( mac, 0 );
    if ( x == 0 ) {
	return 1; // error: out of memory
    }
    Ignored_ADD( link, x );
    return 0;
}

// Parse ignored interfaces
// Comma separated list of MAC addresses
static int parse_ignored_interfaces(char *arg,struct Allowed *link) {
    int a, b, c, d, e, f, g;
    while ( *arg ) {
	if ( sscanf( arg,"%x:%x:%x:%x:%x:%x%n",&a,&b,&c,&d,&e,&f,&g ) != 6 ) {
	    // Not a mac addr
	    return 1;
	}
	if ( (a|b|c|d|e|f) & ~0xff ) {
	    return 1; // some %x is not hex
	}
	unsigned char mac[6] = { a, b, c, d, e, f };
	if ( add_ignored( link, mac ) ) {
	    // Out of memory ??
	    return 1;
	}
	VERBOSEOUT( "Ignoring: %s on channel %s\n",
		    inet_mtoa( mac ), link->source );
	arg += g;
	if ( *arg == 0 ) {
	    break;
	}
	if ( *(arg++) != ',' ) {
	    return 1; // Not comma separated
	}
    }
    return 0;
}

//** IP address parsing utility
// Clear bits after <bits>
static void clearbitsafter(struct CharAddr *a,unsigned int bits) {
    unsigned int max = a->width * 8;
    int i;
    for ( i = a->width; i < 16; i++ ) {
	a->bytes[ i ] = 0;
    }
    for ( i = a->width - 1; i >= 0; i--, max -= 8 ) {
	if ( max - 8 < bits ) {
	    break;
	}
	a->bytes[ i ] = 0;
    }
    if ( i >= 0 && max >= bits ) {
	a->bytes[ i ] &= ( 0xFF << ( bits - max ) );
    }
}

//** IP address parsing utility
// Find the PSK for the given +file+ in the +loaded+ table (of +count+ size)
static struct PSK *findLoadedKeyfile(char *file,struct PSK *loaded,int count) {
    VERBOSE3OUT( "find %s\n", file );
    for ( count--; count >= 0; count-- ) {
	if ( strcmp( file, loaded[ count ].keyfile ) ) {
	    VERBOSE3OUT( "found %d\n", count );
	    return &loaded[ count ];
	}
    }
    VERBOSE3OUT( "found nothing\n" );
    return 0;
}

//** IP address parsing utility
// Load a key file into dynamically allocated memory, and update the
// given PSK header for it.
static void loadkey(struct PSK *psk) {
    static struct PSK *loaded = 0;
    static int count = 0;
    if ( psk->keyfile == 0 ) {
	return;
    }
    struct PSK *old = findLoadedKeyfile( psk->keyfile, loaded, count );
    if ( old ) {
	memcpy( psk, old, sizeof( struct PSK ) );
	return;
    }
    int e;
    unsigned char *p;
    int n;
    struct stat filestat;
    psk->keyfile = strdup( psk->keyfile );
    int fd = open( (char*) psk->keyfile, O_RDONLY );
    psk->seed = 0;
    if ( fd < 0 ) {
	perror( "open key file" );
	exit( 1 );
    }
    if ( fstat( fd, &filestat ) ) {
	perror( "stat of key file" );
	exit( 1 );
    }
    psk->key_length = filestat.st_size;
    if ( psk->key_length < 256 ) {
	fprintf( stderr, "Too small key file: %d %s\n", psk->key_length,
		 psk->keyfile );
	exit( 1 );
    }
    psk->key = malloc( psk->key_length );
    if ( psk->key == 0 ) {
	fprintf( stderr, "Cannot allocate %d bytes for %s\n",
		 psk->key_length, psk->keyfile );
	exit( 1 );
    }
    e = psk->key_length;
    p = psk->key;
    while ( ( n = read( fd, p, e ) ) > 0 ) {
	e -= n;
	p += n;
    }
    close( fd );
    if ( e != 0 ) {
	fprintf( stderr, "Failed loading key %s\n", psk->keyfile );
	exit( 1 );
    }
    for ( e = 0; (unsigned) e < psk->key_length; e++ ) {
	psk->seed += psk->key[ e ];
    }
    if ( psk->seed == 0 ) {
	fprintf( stderr, "Bad key %s; adds up to 0\n", psk->keyfile );
	exit( 1 );
    }
    count++;
    if ( loaded ) {
	loaded = realloc( loaded, ( count * sizeof( struct PSK ) ) );
    } else {
	loaded = malloc( sizeof( struct PSK ) );
    }
    memcpy( &loaded[ count-1 ], psk, sizeof( struct PSK ) );
    VERBOSE3OUT( "%d: %s %d %p %d\n", count-1, psk->keyfile, psk->seed,
		 psk->key, psk->key_length );
}

//** IP address parsing utility
// Fill out a CharAddr and *port from a SockAddr
static void set_charaddrport(
    struct CharAddr *ca,unsigned short *port,struct SockAddr *sa)
{
    memset( ca, 0, sizeof( struct CharAddr ) );
    ca->width = ( sa->in.sa_family == AF_INET )? 4 : 16;
    if ( ca->width == 4 ) {
	memcpy( &ca->in4, &sa->in4.sin_addr, 4 );
	*port = ntohs( sa->in4.sin_port );
    } else {
        memcpy( &ca->in6, &sa->in6.sin6_addr, 16 );
	*port = ntohs( sa->in6.sin6_port );
    }
}

//** IP address parsing utility
// Fill out a SockAddr from a CharAddr and port
static void set_sockaddr(struct SockAddr *sa,struct CharAddr *ca,int port) {
    memset( sa, 0, sizeof( struct SockAddr ) );
    if ( ca->width == 4 ) {
	sa->in4.sin_family = AF_INET;
        sa->in4.sin_port = htons( port );
        memcpy( &sa->in4.sin_addr, &ca->in4, 4 );
    } else {
	sa->in6.sin6_family = AF_INET6;
        sa->in6.sin6_port = htons( port );
        memcpy( &sa->in6.sin6_addr, &ca->in6, 16 );
    }
}

//** IP address parsing utility
// Capture an optional port sub phrase [:<port>]
static int parse_port(char *port,struct Allowed *into) {
    into->port = 0;
    if ( port ) {
	*(port++) = 0;
	int p;
	if ( sscanf( port, "%d", &p ) != 1 || p < 1 || p > 65535 ) {
	    // Bad port number
	    return 1;
	}
	into->port = p;
    }
    return 0;
}

//** IP address parsing utility
// Capture an optional bits sub phrase [/<bits>]
static int parse_bits(char *bits,int max,struct Allowed *into) {
    into->bits = max;
    if ( bits ) {
	*(bits++) = 0;
	int b;
	if ( sscanf( bits, "%d", &b ) != 1 || b < 0 || b > max ) {
	    return 1;
	}
	into->bits = b;
    }
    return 0;
}

//** IP address parsing utility
// Parse a command line argument as a declaration of an allowed
// remote into the given <addr>.
// Return 0 if ok and 1 otherwise
// Formats: <ipv4-address>[/<bits>][:<port>][=keyfile]
// Formats: <ipv6-address>[/<bits>][=keyfile]
// Formats: \[<ipv6-address>[/<bits>]\][:<port>][=keyfile]
static int parse_allowed(char *arg,struct Allowed *into) {
    static char buffer[10000];
    int n = strlen( arg );
    if ( n > 9000 ) {
	return 1; // excessively large argument
    }
    strcpy( buffer, arg );
    into->source = arg;
    char * keyfile = strchr( buffer, '=' );
    if ( keyfile ) {
	*(keyfile++) = 0;
	into->psk.keyfile = keyfile;
    }
#define B(b) b, b+1, b+2, b+3
    if ( sscanf( buffer, "%hhu.%hhu.%hhu.%hhu", B(into->addr.bytes) ) == 4 ) {
#undef B
	// ipv4 address
	into->addr.width = 4;
	if ( parse_port( strchr( buffer, ':' ), into ) ) {
	    fprintf( stderr, "bad port\n" );
	    return 1;
	}
	if ( parse_bits( strchr( buffer, '/' ), 32, into ) ) {
	    fprintf( stderr, "bad bits\n" );
	    return 1;
	}
	return 0;
    }
    // ipv6 address
    char * address = buffer;
    into->port = 0;
    if ( *buffer == '[' ) {
	// bracketed form, necessary for port
	char *end = strchr( buffer, ']' );
	if ( end == 0 ) {
	    return 1; // bad argument
	}
	address++;
	*(end++) = 0;
	if ( *end == ':' && parse_port( end, into ) ) {
	    return 1;
	}
    }
    into->addr.width = 16;
    if ( parse_bits( strchr( address, '/' ), 128, into ) ) {
	return 1;
    }
    if ( inet_pton( AF_INET6, address, into->addr.bytes ) != 1 ) {
	return 1; // Bad IPv6
    }
    return 0;
}

//** IP address parsing utility
// Add a new channel spec into the <allowed> table
// spec == 0 for the tap/stdio channel
static struct Allowed *add_allowed(char *spec) {
    struct Allowed *into = calloc( 1, sizeof(struct Allowed) );
    htable x = HTABLEINIT( struct Interface, mac, hashcode_mac );
    into->ignored_mac = x;
    if ( spec != 0 ) {
	if ( parse_allowed( spec, into ) ) {
	    fprintf( stderr, "Bad remote spec: %s\n", spec );
	    return 0;
	}
    }
    int i;
    if ( allowed.table == 0 ) {
	// First entry.
	allowed.table = calloc( 1, sizeof(struct Allowed*) );
	allowed.count = 1;
	i = 0;
    } else {
	i = allowed.count++;
	allowed.table = realloc( allowed.table,
				 allowed.count * sizeof(struct Allowed*) );
	if ( allowed.table == 0 ) {
	    fprintf( stderr, "OUT OF MEMORY\n" );
	    exit( 1 );
	}
    }
    allowed.table[i] = into;

    loadkey( &into->psk );
    VERBOSE3OUT( "Allowed %s { %s }\n", into->source, show_allowed( into ) );
    if ( is_uplink( into ) ) {
	struct SockAddr addr;
	set_sockaddr( &addr, &into->addr, into->port );
	VERBOSEOUT( "Add uplink %s\n", show_allowed( into ) );
	(void) add_remote( &addr, into );
    }
    return into;
}

static int parse_threads_count(char *arg) {
    if ( ( sscanf( arg, "%u", &threads_count ) != 1 ) || threads_count < 1 ) {
	return 1;
    }
    VERBOSEOUT( "** Threads count = %d\n", threads_count );
    return 0;
}

static int parse_buffers_count(char *arg) {
    if ( ( sscanf( arg, "%u", &buffers_count ) != 1 ) || buffers_count < 1 ) {
	return 1;
    }
    VERBOSEOUT( "** Buffers count = %d\n", buffers_count );
    return 0;
}

//** IP address parsing utility for multicast phrase
// Return 0 if ok and 1 otherwise
// Formats: <ipv4-address>:<port>[=keyfile]
// The ipv4 address should be a multicast address in ranges
// 224.0.0.0/22, 232.0.0.0/7, 234.0.0.0/8 or 239.0.0.0/8
// though it's not checked here.
static int parse_mcast(char *arg) {
    static char buffer[10000];
    int n = strlen( arg );
    if ( n > 9000 ) {
	return 1; // excessively large argument
    }
    memcpy( buffer, arg, n );
    char *p = buffer + n - 1;
    for ( ; p > buffer && *p != ':' && *p != '='; p-- ) { }
    if ( *p == '=' ) {
	mcast.psk.keyfile = p+1;
	*p = 0;
	loadkey( &mcast.psk );
	for ( ; p > buffer && *p != ':' ; p-- ) { }
    }
    if ( *p != ':' ) {
	fprintf( stderr, "Multicast port is required\n" );
	return 1; // Port number is required
    }
    *(p++) = 0;
    if ( inet_pton( AF_INET, buffer, &mcast.group.imr_multiaddr.s_addr )==0 ) {
	fprintf( stderr, "Multicast address required\n" );
	return 1;
    }
    char *e;
    long int port = strtol( p, &e, 10 );
    if ( *e != 0 || port < 1 || port > 65535 ) {
	fprintf( stderr, "Bad multicast port\n" );
	return 1;
    }
    mcast.group.imr_address.s_addr = htonl(INADDR_ANY);
    mcast.sock.in4.sin_family = AF_INET;
    mcast.sock.in4.sin_addr.s_addr = htonl(INADDR_ANY);
    mcast.sock.in4.sin_port = htons( atoi( p ) );
    return 0;
}

// Utility that sets upt the multicast socket, which is used for
// receiving multicast packets.
static void setup_mcast() {
    // set up ipv4 socket
    if ( ( mcast.fd = socket( AF_INET, SOCK_DGRAM, 0 ) ) == 0 ) {
	perror( "creating socket");
	exit(1);
    }
    if ( setsockopt( mcast.fd, IPPROTO_IP, IP_ADD_MEMBERSHIP,
		     (char *) &mcast.group, sizeof( mcast.group ) ) < 0) {
	perror( "Joining multicast group" );
	exit( 1 );
    }
    int reuse = 1;
    if ( setsockopt( mcast.fd, SOL_SOCKET, SO_REUSEADDR,
		     &reuse, sizeof( int ) ) < 0 ) {
	perror( "SO_REUSEADDR" );
	exit( 1 );
    }
    if ( bind( mcast.fd, (struct sockaddr*) &mcast.sock.in,
	       sizeof( struct sockaddr ) ) ) {
	fprintf( stderr, "Error binding socket!\n");
	exit(1);
    }
    // Change mcast address to be the group multiaddress, and add
    // a persistent "remote" for it.
    mcast.sock.in4.sin_addr.s_addr = mcast.group.imr_multiaddr.s_addr;
    add_remote( &mcast.sock, 0 );
}

// Find the applicable channel rule for a given ip:port address
static struct Allowed *is_allowed_remote(struct SockAddr *addr) {
    struct CharAddr ca;
    int width = ( addr->in.sa_family == AF_INET )? 4 : 16;
    unsigned short port;
    int i = 0;
    for ( ; (unsigned) i < allowed.count; i++ ) {
	struct Allowed *a = allowed.table[i];
	if ( a->addr.width != width ) {
	    continue;
	}
	set_charaddrport( &ca, &port, addr );
	if ( a->port != 0 && a->port != port ) {
	    continue;
	}
	clearbitsafter( &ca, a->bits );
	if ( memcmp( &ca, &a->addr, sizeof( struct CharAddr ) ) == 0 ) {
	    return a;
	}
    }
    return 0; // Disallowed
}

// Simple PSK encryption:
//
// First, xor each byte with a key byte that is picked from the key
// by means of an index that includes the prior encoding. Also,
// compute the sum of encrypted bytes into a "magic" that is added the
// "seed" for seeding the random number generator. Secondly reorder
// the bytes using successive rand number picks from the seeded
// generator.
//
static void encrypt(unsigned char *buf,unsigned int n,struct PSK *psk) {
    unsigned int k;
    unsigned int r;
    unsigned char b;
    unsigned int magic;
    VERBOSE3OUT( "encrypt by %s %p\n", psk->keyfile, psk->key );
    for ( k = 0, r = 0, magic = 0; k < n; k++ ) {
	r = ( r + magic + k ) % psk->key_length;
	buf[k] ^= psk->key[ r ];
	magic += buf[k];
    }
    pthread_mutex_lock( &crypting );
    srand( psk->seed + magic );
    for ( k = 0; k < n; k++ ) {
	r = rand() % n;
	b = buf[k];
	buf[k] = buf[r];
	buf[r] = b;
    }
    pthread_mutex_unlock( &crypting );
}

// Corresponding decryption procedure .
static void decrypt(unsigned char *buf,unsigned int n,struct PSK *psk) {
    unsigned int randoms[ BUFSIZE ];
    unsigned int k;
    unsigned int r;
    unsigned char b;
    unsigned int magic = 0;
    for ( k = 0; k < n; k++ ) {
	magic += buf[k];
    }
    pthread_mutex_lock( &crypting );
    srand( psk->seed + magic );
    for ( k = 0; k < n; k++ ) {
	randoms[k] = rand() % n;
    }
    pthread_mutex_unlock( &crypting );
    for ( k = n; k > 0; ) {
	r = randoms[ --k ];
	b = buf[k];
	buf[k] = buf[r];
	buf[r] = b;
    }
    for ( k = 0, r = 0, magic = 0; k < n; k++ ) {
	r = ( r + magic + k ) % psk->key_length;
	magic += buf[k];
	buf[k] ^= psk->key[r];
    }
}

// Write a buffer data to given file descriptor (basically tap_fd in
// this program). This is never fragmented.
static int dowrite(int fd, unsigned char *buf, int n) {
    int w;
    if ( ( w = write( fd, buf, n ) ) < 0){
	perror( "Writing data" );
	w = -1;
    }
    return w;
}

// Write to the tap/stdio; adding length prefix for stdio
static int write_tap(unsigned char *buf, int n) {
    uint8_t tag0 = *( buf + 12 );
    if ( tag0 == 8 ) {
	uint16_t size = ntohs( *(uint16_t*)(buf + 16) );
	if ( size <= 1500 ) {
	    if ( ( verbose >= 2 ) && ( n != size + 14 ) ) {
		VERBOSEOUT( "clip %d to %d\n", n, size + 14 );
	    }
	    n = size + 14; // Clip of any tail
	}
    }
    if ( stdio ) {
	uint16_t plength = htons( n );
	if ( dowrite( 1, (unsigned char *) &plength,
		      sizeof( plength ) ) < 0 ) {
	    return -11;
	}
	return dowrite( 1, buf, n );
    }
    return dowrite( tap_fd, buf, n );
}

// Write a packet via the given Interface with encryption as specified.
static void write_remote(unsigned char *buf, int n,struct Remote *r) {
    // A packet buffer
    unsigned char output[ BUFSIZE ];
    if ( n < 12 ) {
	VERBOSE2OUT( "SEND %d bytes to %s\n", n, inet_stoa( &r->uaddr ) );
    } else {
	VERBOSE2OUT( "SEND %s -> %s to %s\n",
		     inet_mtoa( buf+6 ), inet_mtoa( buf ),
		     inet_stoa( &r->uaddr ) );
    }
    memcpy( output, buf, n ); // Use the private buffer for delivery
    if ( r->spec == 0 ) {
	if ( r->uaddr.in.sa_family == 0 ) {
	    // Output to tap/stdio
	    if ( write_tap( buf, n ) < 0 ) {
		// panic error
		fprintf( stderr, "Cannot write to tap/stdio: exiting!\n" );
		exit( 1 );
	    }
	    return;
	}
	// Fall through for multicast
	if ( mcast.psk.keyfile ) {
	    encrypt( output, n, &mcast.psk );
	}
    } else if ( r->spec->psk.keyfile ) {
	encrypt( output, n, &r->spec->psk );
    }
    struct sockaddr *sock = &r->uaddr.in;
    size_t size;
    if ( sock->sa_family == AF_INET6 ) {
	// Note that the size of +struct sockaddr_in6+ is actually
	// larger than the size of +struct sockaddr+ (due to the
	// addition of the +sin6_flowinfo+ field). It results in the
	// following cuteness for passing arguments to +sendto+.
	size = sizeof( struct sockaddr_in6 );
	VERBOSE2OUT( "IPv6 UDP %d %s\n",
		     udp_fd, inet_stoa( (struct SockAddr*) sock ) );
    } else {
	size = sizeof( struct sockaddr_in );
	VERBOSE2OUT( "IPv4 UDP %d %s\n",
		     udp_fd, inet_stoa( (struct SockAddr*) sock ) );
    }
    VERBOSE2OUT( "SEND %d bytes to %s [%s -> %s]\n",
		 n, inet_stoa( (struct SockAddr*) sock ),
		 ( n < 12 )? "" : inet_mtoa( buf+6 ),
		 ( n < 12 )? "" : inet_mtoa( buf )
	      );
    // IS sendto thread safe??
    if ( sendto( udp_fd, output, n, 0, sock, size ) < n ) {
	perror( "Writing socket" );
	// Invalidate remote temporarily instead? But if it's an
	// "uplink" it should be retried eventually...
	// For now: just ignore the error.
	// exit( 1 );
    }
}

// Delete a Remote and all its interfaces
static void delete_remote(struct Remote *r) {
    VERBOSE2OUT( "DELETE Remote and all its interfaces %s\n",
		 inet_stoa( &r->uaddr ) );
    unsigned int i = 0;
    struct Interface *x;
    Interface_LOCK;
    for ( ; i < remotes.by_mac.size; i++ ) {
	unsigned char *tmp = remotes.by_mac.data[i];
	if ( tmp == 0 || tmp == (unsigned char *)1 ) {
	    continue;
	}
	x = (struct Interface *) tmp;
	if ( x->remote == r ) {
	    Interface_DEL( x );
	    free( x );
	}
    }
    Interface_UNLOCK;
    Remote_DEL( r );
    free( r );
}

// Unmap an ipv4-mapped ipv6 address
static void unmap_if_mapped(struct SockAddr *s) {
    if ( s->in.sa_family != AF_INET6 ||
	 memcmp( "\000\000\000\000\000\000\000\000\000\000\377\377",
		 &s->in6.sin6_addr, 12 ) ) {
	return;
    }
    VERBOSE2OUT( "unmap %s\n",
		 inet_nmtoa( (unsigned char*) s, sizeof( struct SockAddr ) ) );
    s->in.sa_family = AF_INET;
    memcpy( &s->in4.sin_addr, s->in6.sin6_addr.s6_addr + 12, 4 );
    memset( s->in6.sin6_addr.s6_addr + 4, 0, 12 );
    VERBOSE2OUT( "becomes %s\n",
		 inet_nmtoa( (unsigned char*) s, sizeof( struct SockAddr ) ) );
}

// Route the packet from the given src
static struct Interface *input_check(
    unsigned char *buf,ssize_t len,struct SockAddr *src )
{
    VERBOSE2OUT( "RECV %ld bytes from %s\n", len, inet_stoa( src ) );
    struct Remote *r = 0;
    struct timeval now = { 0 };
    if ( gettimeofday( &now, 0 ) ) {
	perror( "RECV time" );
	now.tv_sec = time( 0 );
    }
    Remote_FIND( src, r );
    if ( r == 0 ) {
	struct Allowed *a = is_allowed_remote( src );
	if ( a == 0 ) {
	    VERBOSEOUT( "Ignoring %s\n", inet_stoa( src ) );
	    return 0; // Disallowed
	}
	VERBOSEOUT( "New remote %s by %s\n", inet_stoa( src ), a->source );
	r = add_remote( src, a );
	//r->rec_when = now; // Set activity stamp of new remote
    }
    if ( len < 12 ) {
	// Ignore short data, but maintain channel
	r->rec_when = now; // Update activity stamp touched remote
	if ( len > 0 ) {
	    VERBOSEOUT( "Ignoring %ld bytes from %s\n",
			len, inet_stoa( src ) );
	}
	return 0;
    }
    // Now decrypt the data as needed
    if ( r->spec ) {
	if ( r->spec->psk.seed ) {
	    decrypt( buf, len, &r->spec->psk );
	}
    } else if ( r->uaddr.in.sa_family == 0 && mcast.psk.keyfile ) {
	decrypt( buf, len, &mcast.psk );
    }
    VERBOSE2OUT( "RECV %s -> %s from %s\n",
		 inet_mtoa( buf+6 ), inet_mtoa( buf ),
		 inet_stoa( &r->uaddr ) );
    // Note: the payload is now decrypted, and known to be from +r+
    struct Interface *x = 0;
    // Packets concerning an ignored interface should be ignored.
    if ( r->spec && r->spec->ignored_mac.data ) {
	Ignored_FIND( r->spec, buf+6, x );
	if ( x ) {
	    VERBOSE2OUT( "Dropped MAC %s from %s on %s\n",
			 inet_mtoa( buf+6 ), inet_stoa( &r->uaddr ),
			 r->spec->source );
	    return 0;
	}
	Ignored_FIND( r->spec, buf, x );
	if ( x ) {
	    VERBOSE2OUT( "Dropped MAC %s to %s on %s\n",
			 inet_mtoa( buf ), inet_stoa( &r->uaddr ),
			 r->spec->source );
	    return 0;
	}
    }
    Interface_FIND( buf+6, x );
    // x is the previous interface for the source MAC, or null.
    if ( x == 0 ) {
	// Totally new MAC, so create a new interface for it and bind
	// that to the channel.
	VERBOSEOUT( "New MAC %s from %s\n",
		    inet_mtoa( buf+6 ), inet_stoa( src ) );
	x = add_interface( buf+6, r );
	r->rec_when = now; // Update channel activity stamp
	x->rec_when = now; // Update interface activity stamp
	return x;
    }
    // Seen that MAC already.
    if ( x->remote == r ) {
	VERBOSE2OUT( "RECV %s from %s again\n",
		     inet_mtoa( buf+6 ), inet_stoa( &x->remote->uaddr ) );
	r->rec_when = now; // Update channel activity stamp
	x->rec_when = now; // Update interface activity stamp
	return x;
    }
    // There is a MAC "clash" from two different channels.
    // r = current channel
    // x->remote = previous channel
    VERBOSE2OUT( "RECV %s from %s previously from %s\n",
	      inet_mtoa( buf+6 ),
	      inet_stoa( &r->uaddr ),
	      inet_stoa( &x->remote->uaddr ) );
    // Consider source fallover:
    // treat tap/stdio differently from other channels
    if ( r->spec ) {
	// The incoming packet is not from tap/stdin.
	int64_t time_since_last = DIFF_MICROS( &now, &x->rec_when);
	// Fallover happens if the previous is sufficiently idle
	if ( x->remote->spec == 0 ) {
	    // Fallover limits are 4 times larger for tap/stdio
	    time_since_last /= 4;
	}
	// The effective idle time (i.e. the time since last packet)
	// of the previous interface must be more than RECENT_MICROS,
	// which has different limits for incoming broadcast and
	// unicast.
	if ( RECENT_MICROS( *buf & 1, time_since_last ) ) {
	    if ( verbose >= 2 ) {
		fprintf(
		    stderr,
		    "Dropped. MAC %s (%ld) from %s, should be %s\n",
		    inet_mtoa( buf+6 ), time_since_last,
		    inet_stoa( src ), inet_stoa( &x->remote->uaddr ) );
	    }
	    return 0;
	}
	// falling through when old channel is deemed inactive
	//
    } else if ( r->uaddr.in.sa_family ) {
	// Multicast incoming clashing with tap/stdio
	VERBOSE3OUT( "Dropped multicast loopback\n" );
	return 0;
    }
    // New remote takes over the MAC
    VERBOSEOUT( "MAC %s from %s cancels previous %s\n",
		inet_mtoa( buf+6 ), inet_stoa( src ),
		inet_stoa( &x->remote->uaddr ) );
    x->remote = r; // Change remote for MAC
    // Note that this may leave the old x->remote without any interface
    r->rec_when = now; // Update activity stamp
    x->rec_when = now; // Update activity stamp
    return x;
}

// Check packet and deliver out
static void route_packet(unsigned char *buf,int len,struct SockAddr *src) {
    struct Interface *x = input_check( buf, len, src );
    if ( x == 0 ) {
	return; // not a nice packet
    }
    if ( ( *buf & 1 ) == 0 ) {
	// unicast
	struct Interface *y = 0; // reuse for destination interface
	Interface_FIND( buf, y );
	if ( y == 0 ) {
	    VERBOSE2OUT( "RECV %s -> %s from %s without channel and dropped\n",
			inet_mtoa( buf+6 ), inet_mtoa( buf ),
			inet_stoa( &x->remote->uaddr ) );
	    return;
	}
	if ( x->remote == y->remote ) {
	    VERBOSEOUT( "RECV loop for %s -> %s from %s to %s\n",
			inet_mtoa( buf+6 ), inet_mtoa( buf ),
			inet_stoa( &x->remote->uaddr ),
			inet_stoa( &y->remote->uaddr ) );
	    Interface_DEL( y ); // Need to see this interface again
	    return;
	}
	VERBOSE2OUT( "RECV route %s -> %s to %s\n",
		     inet_mtoa( buf+6 ), inet_mtoa( buf ),
		     inet_stoa( &y->remote->uaddr ) );
	write_remote( buf, len, y->remote );
	return;
    }
    // broadcast. +x+ is source interface
    // x->rec_when is not updated 
    struct timeval now = { 0 };
    if ( gettimeofday( &now, 0 ) ) {
	perror( "RECV time" );
	now.tv_sec = time( 0 );
    }
    VERBOSE2OUT( "BC %s -> %s from %s\n",
		 inet_mtoa( buf+6 ), inet_mtoa( buf ),
		 inet_stoa( &x->remote->uaddr ) );
    struct Remote *r;
    unsigned int i = 0;
    Remote_LOCK;
    for ( ; i < remotes.by_addr.size; i++ ) {
	unsigned char *tmp = remotes.by_addr.data[i];
	if ( tmp == 0 || tmp == (unsigned char *)1 ) {
	    continue;
	}
	r = (struct Remote *) tmp;
	VERBOSE3OUT( "BC check %s\n", inet_stoa( &r->uaddr ) );
	if ( r == x->remote ) {
	    VERBOSE3OUT( "BC r == x->remote\n" );
	    continue;
	}
	if ( r->spec && ! is_uplink( r->spec ) &&
	     DIFF_MICROS( &now, &r->rec_when ) > VERYOLD_MICROS ) {
	    // remove old downlink connection
	    VERBOSEOUT( "Old remote discarded %s (%ld)\n",
			inet_stoa( &r->uaddr ),
			TIME_MICROS( &r->rec_when ) );
	    // Removing a downlink might have threading implications
	    delete_remote( r );
	    continue;
	}
	// Send packet to the remote
	// Only no-clash or to the tap/stdin
	write_remote( buf, len, r );
    }
    Remote_UNLOCK;
}

// The packet handling queues
static struct {
    Queue full;
    Queue free;
    sem_t reading;
} todolist;

// The threadcontrol program for handling packets.
static void *packet_handler(void *data) {
    (void) data;
    for ( ;; ) {
	PacketItem *todo = (PacketItem *) Queue_getItem( &todolist.full );
	if ( todo->fd == mcast.fd ) {
	    // Patch multicast address as source for multicast packet
	    route_packet( todo->buffer, todo->len, &mcast.sock );
	} else {
	    if ( udp6 ) {
		unmap_if_mapped( &todo->src );
	    }
	    route_packet( todo->buffer, todo->len, &todo->src );
	}
	Queue_addItem( &todolist.free, (QueueItem*) todo );
    }
    return 0;
}

void todolist_initialize(int nbuf,int nthr) {
    if ( pthread_mutex_init( &todolist.full.mutex, 0 ) ||
	 sem_init( &todolist.full.count, 0, 0 ) ) {
	perror( "FATAL" );
	exit( 1 );
    }
    if ( pthread_mutex_init( &todolist.free.mutex, 0 ) ||
	 sem_init( &todolist.free.count, 0, 0 ) ) {
	perror( "FATAL" );
	exit( 1 );
    }
    if ( sem_init( &todolist.reading, 0, 1 ) ) {
	perror( "FATAL" );
	exit( 1 );
    }
    Queue_initialize( &todolist.free, nbuf, sizeof( PacketItem ) );
    for ( ; nthr > 0; nthr-- ) {
	pthread_t thread; // Temporary thread id
	pthread_create( &thread, 0, packet_handler, 0 );
    }
}

// Read a full UDP packet into the given buffer, associate with a
// connection, or create a new connection, the decrypt the as
// specified, and capture the sender MAC address. The connection table
// is updated for the new MAC address, However, if there is then a MAC
// address clash in the connection table, then the associated remote
// is removed, and the packet is dropped.
static void *doreadUDP(void *data) {
    int fd = ((ReaderData *) data)->fd;
    while ( 1 ) {
	PacketItem *todo = (PacketItem *) Queue_getItem( &todolist.free );
	socklen_t addrlen =
	    udp6? sizeof( todo->src.in6 ) : sizeof( todo->src.in4 );
	memset( &todo->src, 0, sizeof( todo->src ) );
	todo->fd = fd;
	todo->len = recvfrom(
	    fd, todo->buffer, BUFSIZE, 0, &todo->src.in, &addrlen );
	if ( todo->len == -1) {
	    perror( "Receiving UDP" );
	    exit( 1 );
	}
#ifdef GPROF
	if ( todo->len == 17 &&
	     memcmp( todo->buffer, "STOPSTOPSTOPSTOP", 16 ) == 0 ) {
	    exit( 0 );
	}
#endif
	Queue_addItem( &todolist.full, (QueueItem*) todo );
    }
    return 0;
}

// Read up to n bytes from the given file descriptor into the buffer
static int doread(int fd, unsigned char *buf, int n) {
    ssize_t len;
    if ( ( len = read( fd, buf, n ) ) < 0 ) {
	perror( "Reading stdin" );
	exit( 1 );
    }
    return len;
}

// Read n bytes from the given file descriptor into the buffer.
// If partial is allowed, then return amount read, otherwise keep
// reading until full.
static int read_into(int fd, unsigned char *buf, int n,int partial) {
    int r, x = n;
    while( x > 0 ) {
	if ( (r = doread( fd, buf, x ) ) == 0 ) {
	    return 0 ;
	}
	x -= r;
	buf += r;
	if ( partial ) {
	    return n - x;
	}
    }
    return n;
}

// Go through all uplinks and issue a "heart beat"
static void heartbeat(int fd) {
    static unsigned char data[10];
    VERBOSE3OUT( "heartbeat fd=%d\n", fd );
    struct Remote *r;
    unsigned int i = 0;
    struct timeval now;
    if ( gettimeofday( &now, 0 ) ) {
	perror( "HEARTBEAT time" );
	now.tv_sec = time( 0 );
	now.tv_usec = 0;
    }
    Remote_LOCK;
    for ( ; i < remotes.by_addr.size; i++ ) {
	unsigned char *tmp = remotes.by_addr.data[i];
	if ( tmp == 0 || tmp == (unsigned char *)1 ) {
	    continue;
	}
	r = (struct Remote *) tmp;
	VERBOSE3OUT( "heartbeat check %s\n", inet_stoa( &r->uaddr ) );
	if ( r->spec && is_uplink( r->spec ) ) {
	    if ( DIFF_MICROS( &now, &r->rec_when ) > HEARTBEAT_MICROS ) {
		VERBOSE3OUT( "heartbeat %s\n", inet_stoa( &r->uaddr ) );
		write_remote( data, 0, r );
	    }
	}
    }
    Remote_UNLOCK;
}

// Tell how to use this program and exit with failure.
static void usage(void) {
    fprintf( stderr, "Packet tunneling over UDP, multiple channels, " );
    fprintf( stderr, "version 0.2.5\n" );
    fprintf( stderr, "Usage: " );
    fprintf( stderr,
 "%s [-v] [-4] [-B n] [-T n] [-m mcast] [-t tap] port [remote]+ \n",
	     progname );
    exit( 1 );
}

// Open the given tap
static int tun_alloc(char *dev, int flags) {
    struct ifreq ifr;
    int fd, err;
    if ( ( fd = open( "/dev/net/tun", O_RDWR ) ) < 0 ) {
	perror( "Opening /dev/net/tun" );
	return fd;
    }
    memset( &ifr, 0, sizeof( ifr ) );
    ifr.ifr_flags = flags;
    if ( *dev ) {
	strcpy( ifr.ifr_name, dev );
    }
    if ( ( err = ioctl( fd, TUNSETIFF, (void *) &ifr ) ) < 0 ) {
	perror( "ioctl(TUNSETIFF)" );
	close( fd );
	return err;
    }
    strcpy( dev, ifr.ifr_name );
    return fd;
}

// Handle packet received on the tap/stdio channel
static void initialize_tap() {
    // Ensure there is a Remote for this
    static struct Remote *tap_remote = 0;
    if ( tap_remote == 0 ) {
	Remote_LOCK;
	if ( tap_remote == 0 ) {
	    tap_remote = add_remote( 0, 0 );
	}
	Remote_UNLOCK;
    }
}

// Thread to handle tap/stdio input
static void *doreadTap(void *data) {
    int fd = ((ReaderData*) data)->fd;
    unsigned int end = 0; // Packet size
    unsigned int cur = 0; // Amount read so far
    size_t e;
    PacketItem *todo = (PacketItem*) Queue_getItem( &todolist.free );
    while ( 1 ) {
    	if ( stdio ) {
	    uint16_t plength;
	    int n = read_into( 0, (unsigned char *) &plength,
			       sizeof( plength ), 0 );
	    if ( n == 0 ) {
		// Tap/stdio closed => exit silently
		exit( 0 );
	    }
	    end = ntohs( plength );
	    cur = 0;
	    while ( ( e = ( end - cur ) ) != 0 ) {
		unsigned char *p = todo->buffer + cur;
		if ( end > BUFSIZE ) {
		    // Oversize packets should be read and discarded
		    if ( e > BUFSIZE ) {
			e = BUFSIZE;
		    }
		    p = todo->buffer;
		}
		cur += read_into( 0, p, e, 1 );
	    }
	} else {
	    end = doread( fd, todo->buffer, BUFSIZE );
	    cur = end;
	}
	VERBOSE3OUT( "TAP/stdio input %d bytes\n", end );
	if ( end <= BUFSIZE ) {
	    todo->fd = 0;
	    todo->len = end;
	    Queue_addItem( &todolist.full, (QueueItem*) todo );
	    todo = (PacketItem*) Queue_getItem( &todolist.free );
	}
	// End handling tap
    }
    return 0;
}

// Application main function
// Parentheses mark optional
// $* = (-v) (-4) (-B n) (-T n) (-m mcast) (-t port) (ip:)port (remote)+
// remote = ipv4(/maskwidth)(:port)(=key)
// remote = ipv6(/maskwidth)(=key)
// remote = [ipv6(/maskwidth)](:port)(=key)
// ip = ipv4 | [ipv6]
int main(int argc, char *argv[]) {
    pthread_t thread; // Temporary thread id
    int port, i;
    progname = (unsigned char *) argv[0];
    ///// Parse command line arguments
    i = 1;
#define ENSUREARGS(n) if ( argc < i + n ) usage()
    ENSUREARGS( 1 );
    // First: optional -v, -vv or -vvv
    if ( strncmp( "-v", argv[i], 2 ) == 0 ) {
	if ( strncmp( "-v", argv[i], 3 ) == 0 ) {
	    verbose = 1;
	} else if ( strncmp( "-vv", argv[i], 4 ) == 0 ) {
	    verbose = 2;
	} else if ( strncmp( "-vvv", argv[i], 5 ) == 0 ) {
	    verbose = 3;
	} else {
	    usage();
	}
	i++;
	ENSUREARGS( 1 );
    }
    // then: optional -4
    if ( strncmp( "-4", argv[i], 2 ) == 0 ) {
	udp6 = 0;
	i++;
	ENSUREARGS( 1 );
    }
    // then: optional -B buffers
    if ( strncmp( "-B", argv[i], 2 ) == 0 ) {
	ENSUREARGS( 2 );
	if ( parse_buffers_count( argv[i+1] ) ) {
	    usage();
	}
	i += 2;
	ENSUREARGS( 1 );
    }
    // then: optional -T threads
    if ( strncmp( "-T", argv[i], 2 ) == 0 ) {
	ENSUREARGS( 2 );
	if ( parse_threads_count( argv[i+1] ) ) {
	    usage();
	}
	i += 2;
	ENSUREARGS( 1 );
    }
    // then: optional -m mcast
    if ( strncmp( "-m", argv[i], 2 ) == 0 ) {
	ENSUREARGS( 2 );
	if ( parse_mcast( argv[i+1] ) ) {
	    usage();
	}
	i += 2;
	ENSUREARGS( 1 );
    }
    // then: optional -t tap
    if ( strncmp( "-t", argv[i], 2 ) == 0 ) {
	ENSUREARGS( 2 );
	tap = argv[i+1];
	i += 2;
	ENSUREARGS( 1 );
    }
    // then: required port
    if ( sscanf( argv[i++], "%d", &port ) != 1 ) {
	fprintf( stderr, "Bad local port" );
	usage();
    }
    // then: any number of allowed remotes
    struct Allowed *last_allowed = 0;
    for ( ; i < argc; i++ ) {
	if ( last_allowed ) {
	    // optionally adding ignored interfaces
	    if ( strncmp( "-i", argv[i], 2 ) == 0 ) {
		ENSUREARGS( 2 );
		if ( parse_ignored_interfaces( argv[i+1], last_allowed ) ) {
		    usage();
		}
		i += 1;
		continue;
	    }
	}
	if ( ( last_allowed = add_allowed( argv[i] ) ) == 0 ) {
	    fprintf( stderr, "Cannot load remote %s. Exiting.\n", argv[i] );
	    exit( 1 );
	}
    }
    // end of command line parsing

    // Initialize buffers and threads
    if ( threads_count == 0 ) {
	threads_count = 5;
    }
    if ( buffers_count < threads_count ) {
	buffers_count = 2 * threads_count;
    }
    todolist_initialize( buffers_count, threads_count );

    // Set up the tap/stdio channel
    if ( tap ) {
	// set up the nominated tap
	if ( strcmp( "-", tap ) ) { // Unless "-"
	    tap_fd = tun_alloc( tap, IFF_TAP | IFF_NO_PI );
	    if ( tap_fd < 0 ) {
		fprintf( stderr, "Error connecting to interface %s!\n", tap);
		exit(1);
	    }
	    VERBOSEOUT( "Using tap %s at %d\n", tap, tap_fd );
	    stdio = 0;
	    // pretend a zero packet on the tap, for initializing.
	    initialize_tap(); 
	} else {
	    // set up for stdin/stdout local traffix
	    setbuf( stdout, NULL ); // No buffering on stdout.
	    tap_fd = 0; // actually stdin
	    stdio = 1;
	}
    } else {
	stdio = 0;
    }
    // Set up the multicast UDP channel (all interfaces)
    if ( mcast.group.imr_multiaddr.s_addr ) {
	setup_mcast();
	unsigned char *x = (unsigned char *) &mcast.group.imr_multiaddr.s_addr;
        VERBOSEOUT( "Using multicast %s:%d at %d\n",
		    inet_nmtoa( x, 4 ), ntohs( mcast.sock.in4.sin_port ),
		    mcast.fd );
    }
    // Set up the unicast UPD channel (all interfaces)
    if ( udp6 == 0 ) {
	// set up ipv4 socket
	if ( ( udp_fd = socket( AF_INET, SOCK_DGRAM, 0 ) ) == 0 ) {
	    perror( "creating socket");
	    exit(1);
	}
	struct sockaddr_in udp_addr = {
	    .sin_family = AF_INET,
	    .sin_port = htons( port ),
	    .sin_addr.s_addr = htonl(INADDR_ANY),
	};
	if ( bind( udp_fd, (struct sockaddr*) &udp_addr, sizeof(udp_addr))) {
	    fprintf( stderr, "Error binding socket!\n");
	    exit(1);
	}
	VERBOSEOUT( "Using ipv4 UDP at %d\n", udp_fd );
    } else {
	// set up ipv6 socket
	if ( ( udp_fd = socket( AF_INET6, SOCK_DGRAM, 0 ) ) == 0 ) {
	    perror( "creating socket");
	    exit(1);
	}
	struct sockaddr_in6 udp6_addr = {
	    .sin6_family = AF_INET6,
	    .sin6_port = htons( port ),
	    .sin6_addr = IN6ADDR_ANY_INIT,
	};
	if ( bind( udp_fd, (struct sockaddr*) &udp6_addr, sizeof(udp6_addr))) {
	    fprintf( stderr, "Error binding socket!\n");
	    exit(1);
	}
	VERBOSEOUT( "Using ipv6 UDP at %d\n", udp_fd );
    }
    // If not using stdio for local traffic, then stdin and stdout are
    // closed here, so as to avoid that any other traffic channel gets
    // 0 or 1 as its file descriptor. Note: stderr (2) is left open.
    if ( ! stdio ) {
	close( 0 );
	close( 1 );
    }
    VERBOSE2OUT( "Socket loop tap=%d mcast=%d udp=%d\n",
		 tap_fd, mcast.fd, udp_fd );

    // Handle packets
    ReaderData udp_reader = { .fd = udp_fd };
    pthread_create( &thread, 0, doreadUDP, &udp_reader );

    if ( mcast.group.imr_multiaddr.s_addr ) {
	ReaderData mcast_reader = { .fd = mcast.fd };
	pthread_create( &thread, 0, doreadUDP, &mcast_reader );
    }

    if ( tap_fd || stdio ) {
	ReaderData tap_reader = { .fd = tap_fd };
	pthread_create( &thread, 0, doreadTap, &tap_reader );
    }

    // Start heartbeating to uplinks
    for ( ;; ) {
	sleep( HEARTBEAT );
	heartbeat( udp_fd );
    }
    return 0;
}