Packet tunneling over UDP, multiple channels
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

1590 lines
45 KiB

2 years ago
// 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>
2 years ago
#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
2 years ago
};
// 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
2 years ago
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;
2 years ago
// heartbeat interval, in seconds
#define HEARTBEAT 30
#define HEARTBEAT_MICROS ( HEARTBEAT * 1000000 )
2 years ago
// 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) )
2 years ago
// RECENT(T,M) is the time logic for requiring a gap time (in
// milliseconds) before shifting a MAC to a new remote. The limit is
// 6000 for broadcast and 20000 for unicast.
#define RECENT(T,M) ((M) < ((T)? 6000 : 20000 ))
// VERYOLD_MICROSS is used for discarding downlink remotes whose latest
2 years ago
// activity is older than this.
#define VERYOLD_MICROS 180000000
2 years ago
////////// 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 );
2 years ago
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 ) ) {
2 years ago
perror( "RECV time" );
now.tv_sec = time( 0 );
2 years ago
}
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 ) {
2 years ago
// 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 ) );
}
2 years ago
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 );
if ( x == 0 ) {
// Totally new MAC. Should bind it to the remote
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 activity stamp for remote
x->rec_when = now;
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 activity stamp
x->rec_when = now; // Update activity stamp
return x;
}
// MAC clash from two different connections
// r = current
// x->remote = previous
VERBOSE2OUT( "RECV %s from %s previously from %s\n",
inet_mtoa( buf+6 ),
inet_stoa( &r->uaddr ),
inet_stoa( &x->remote->uaddr ) );
if ( r->spec ) {
// The packet source MAC has arrived on other than its
// previous channel. It thus gets dropped if tap/stdin is the
// primary channel, or the time since the last packet for that
// interface is less than RECENT, with different limits for
// broadcast and unicast.
int64_t dmac = DIFF_MICROS( &now, &x->rec_when);
2 years ago
if ( x->remote->spec == 0 || RECENT( *buf & 1, dmac ) ) {
if ( verbose >= 2 ) {
fprintf(
stderr,
"Dropped. MAC %s (%ld) from %s, should be %s\n",
inet_mtoa( buf+6 ), dmac,
inet_stoa( src ), inet_stoa( &x->remote->uaddr ) );
}
return 0;
}
// Check if previous package on the interface was recent
} 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 );