rrqnet.c

   1 // This program is a UDP based tunneling of stdin/out Ethernet packets.
   2 //
   3 // A rrqnet program is a bi-directional networking plug that channels
   4 // packets between a UDP port and stdin/out. It is configured on the
   5 // command line with channel rules that declares which remotes it may
   6 // communicate with. Allowed remotes are specified in the format
   7 // "ip[/n][:port][=key]", to indicate which subnet and port to accept,
   8 // and nominating the associated keyfile to use for channel
   9 // encryption.
  10 //
  11 // The program maintains a table of actualized connections, as an
  12 // association between MAC addresses and IP:port addresses. This table
  13 // is used for resolving destination for outgoing packets, including
  14 // the forwarding of broadcasts.
  15 //
  16
  17 #include <errno.h>
  18 #include <fcntl.h>
  19 #include <linux/if.h>
  20 #include <linux/if_tun.h>
  21 #include <stddef.h>
  22 #include <stdio.h>
  23 #include <stdlib.h>
  24 #include <string.h>
  25 #include <sys/ioctl.h>
  26 #include <sys/stat.h>
  27 #include <sys/time.h>
  28 #include <sys/types.h>
  29 #include <time.h>
  30 #include <unistd.h>
  31
  32 #include "htable.h"
  33 #include "queue.h"
  34
  35 //// Data structures
  36
  37 // "Private Shared Key" details.
  38 struct PSK {
  39     char *keyfile;
  40     unsigned int seed;          // Encryption seed
  41     unsigned char *key;         // Encryption key
  42     unsigned int key_length;    // Encryption key length
  43 };
  44
  45 // Compacted IP address ipv4/ipv6
  46 struct CharAddr {
  47     int width; // 4=ipv4 and 16=ipv6
  48     union {
  49         unsigned char bytes[16];
  50         struct in_addr in4;
  51         struct in6_addr in6;
  52     };
  53 };
  54
  55 // Details of channel rules.
  56 struct Allowed {
  57     char *source;               // Orginal rule
  58     struct CharAddr addr;
  59     unsigned int bits;          // Bits of IP prefix
  60     unsigned short port;        // Port (0=any)
  61     struct PSK psk;             // Associated key
  62     htable ignored_mac;         // MAC to ignore by this spec
  63 };
  64
  65 // Details of actualized connections.
  66 struct Remote {
  67     struct SockAddr uaddr;
  68     struct Allowed *spec;       // Rule being instantiated
  69     struct timeval rec_when;    // Last received packet time, in seconds
  70 };
  71
  72 // Details of an interface at a remote.
  73 struct Interface  {
  74     unsigned char mac[6];       // MAC address used last (key for by_mac table)
  75     struct timeval rec_when;    // Last packet time, in seconds
  76     struct Remote *remote;
  77 };
  78
  79 // Maximal packet size .. allow for jumbo frames (9000)
  80 #define BUFSIZE 10000
  81
  82 typedef struct _PacketItem {
  83     QueueItem base;
  84     int fd;
  85     struct SockAddr src;
  86     ssize_t len;
  87     unsigned char buffer[ BUFSIZE ];
  88 } PacketItem;
  89
  90 typedef struct _ReaderData {
  91     int fd;
  92 } ReaderData;
  93
  94 // heartbeat interval, in seconds
  95 #define HEARTBEAT 30
  96 #define HEARTBEAT_MICROS ( HEARTBEAT * 1000000 )
  97
  98 // Macros for timing, for struct timeval variables
  99 #define TIME_MICROS(TM) (((int64_t) (TM)->tv_sec * 1000000) + (TM)->tv_usec )
 100 #define DIFF_MICROS(TM1,TM2) ( TIME_MICROS(TM1) - TIME_MICROS(TM2) )
 101
 102 // RECENT_MICROS(T,M) is the time logic for requiring a gap time (in
 103 // milliseconds) before shifting a MAC to a new remote. The limit is
 104 // 6s for broadcast and 20s for unicast.
 105 #define RECENT_MICROS(T,M) ((M) < ((T)? 6000000 : 20000000 ))
 106
 107 // VERYOLD_MICROSS is used for discarding downlink remotes whose latest
 108 // activity is older than this.
 109 #define VERYOLD_MICROS 180000000
 110
 111 ////////// Variables
 112
 113 // Allowed remote specs are held in a table sorted by IP prefix.
 114 static struct {
 115     struct Allowed **table;
 116     unsigned int count;
 117 } allowed;
 118
 119 // Actual remotes are kept in a hash table keyed by their +uaddr+
 120 // field, and another hash table keps Interface records for all MAC
 121 // addresses sourced from some remote, keyed by their +mac+ field. The
 122 // latter is used both for resolving destinations for outgoing
 123 // packets, and for limiting broadcast cycles. The former table is
 124 // used for limiting incoming packets to allowed sources, and then
 125 // decrypt the payload accordingly.
 126 static int hashcode_uaddr(struct _htable *table,unsigned char *key);
 127 static int hashcode_mac(struct _htable *table,unsigned char *key);
 128 static struct {
 129     htable by_mac; // struct Interface hash table
 130     htable by_addr; // struct Remote hash table
 131 } remotes = {
 132     .by_mac = HTABLEINIT( struct Interface, mac, hashcode_mac ),
 133     .by_addr = HTABLEINIT( struct Remote, uaddr, hashcode_uaddr )
 134 };
 135
 136 #define Interface_LOCK if ( pthread_mutex_lock( &remotes.by_mac.lock ) ) { \
 137         perror( "FATAL" ); exit( 1 ); }
 138
 139 #define Interface_UNLOCK if (pthread_mutex_unlock( &remotes.by_mac.lock ) ) { \
 140         perror( "FATAL" ); exit( 1 ); }
 141
 142 #define Interface_FIND(m,r) \
 143     htfind( &remotes.by_mac, m, (unsigned char **)&r )
 144
 145 #define Interface_ADD(r) \
 146     htadd( &remotes.by_mac, (unsigned char *)r )
 147
 148 #define Interface_DEL(r) \
 149     htdelete( &remotes.by_mac, (unsigned char *) r )
 150
 151 #define Remote_LOCK if ( pthread_mutex_lock( &remotes.by_addr.lock ) ) { \
 152         perror( "FATAL" ); exit( 1 ); }
 153
 154 #define Remote_UNLOCK if ( pthread_mutex_unlock( &remotes.by_addr.lock ) ) { \
 155         perror( "FATAL" ); exit( 1 ); }
 156
 157 #define Remote_FIND(a,r) \
 158     htfind( &remotes.by_addr, (unsigned char *)a, (unsigned char **) &r )
 159
 160 #define Remote_ADD(r) \
 161     htadd( &remotes.by_addr, (unsigned char *) r )
 162
 163 #define Remote_DEL(r) \
 164     htdelete( &remotes.by_addr, (unsigned char *) r )
 165
 166 #define Ignored_FIND(a,m,x)                             \
 167     htfind( &a->ignored_mac, m, (unsigned char **)&x )
 168
 169 #define Ignored_ADD(a,x) \
 170     htadd( &a->ignored_mac, (unsigned char *)x )
 171
 172 // Input channels
 173 static int stdio = 0; // Default is neither stdio nor tap
 174 static char *tap = 0; // Name of tap, if any, or "-" for stdio
 175 static int tap_fd = 0; // Also used for stdin in stdio mode
 176 static int udp_fd;
 177 static int threads_count = 0;
 178 static int buffers_count = 0;
 179
 180 // Setup for multicast channel
 181 static struct {
 182     struct ip_mreqn group;
 183     struct SockAddr sock;
 184     int fd;
 185     struct PSK psk;
 186 } mcast;
 187
 188 // Flag to signal the UDP socket as being ipv6 or not (forced ipv4)
 189 static int udp6 = 1;
 190
 191 // The given UDP source address, if any
 192 static struct {
 193     int family;
 194     unsigned char address[16];
 195 } udp_source;
 196
 197 // Flag to indicate tpg transport patch = avoid UDP payload of 1470
 198 // bytes by adding 2 tag-along bytes
 199 static int tpg_quirk = 0;
 200
 201 // Flag whether to make some stderr outputs or not.
 202 // 1 = normal verbosity, 2 = more output, 3 = source debug level stuff
 203 static int verbose;
 204
 205 // Note: allows a thread to lock/unlock recursively
 206 static pthread_mutex_t crypting = PTHREAD_MUTEX_INITIALIZER;
 207
 208 // Note: allows a thread to lock/unlock recursively
 209 static pthread_mutex_t printing = PTHREAD_RECURSIVE_MUTEX_INITIALIZER_NP;
 210
 211 #define PRINTLOCK \
 212     if ( pthread_mutex_lock( &printing ) ) { perror( "FATAL" ); exit(1); }
 213
 214 #define PRINTUNLOCK \
 215     if ( pthread_mutex_unlock( &printing ) ) { perror( "FATAL" ); exit(1); }
 216
 217 #define PRINT( X ) { PRINTLOCK; X; PRINTUNLOCK; }
 218
 219 #define VERBOSEOUT(fmt, ...) \
 220     if ( verbose >= 1 ) PRINT( fprintf( stderr, fmt, ##__VA_ARGS__ ) )
 221
 222 #define VERBOSE2OUT(fmt, ...) \
 223     if ( verbose >= 2 ) PRINT( fprintf( stderr, fmt, ##__VA_ARGS__ ) )
 224
 225 #define VERBOSE3OUT(fmt, ...) \
 226     if ( verbose >= 3 ) PRINT( fprintf( stderr, fmt, ##__VA_ARGS__ ) )
 227
 228 // The actual name of this program (argv[0])
 229 static unsigned char *progname;
 230
 231 // Compute a hashcode for the given SockAddr key
 232 static int hashcode_uaddr(
 233     __attribute__((unused)) struct _htable *table,unsigned char *key)
 234 {
 235     struct SockAddr *s = (struct SockAddr *) key;
 236     key = (unsigned char*) &s->in;
 237     unsigned char *e = key + ( ( s->in.sa_family == AF_INET )?
 238                                sizeof( struct sockaddr_in ) :
 239                                sizeof( struct sockaddr_in6 ) );
 240     int x = 0;
 241     while ( key < e ) {
 242         x += *(key++);
 243     }
 244     return x;
 245 }
 246
 247 // Compute a hashcode for the given MAC addr key
 248 static int hashcode_mac(struct _htable *table,unsigned char *key) {
 249     int x = 0;
 250     int i = 0;
 251     if ( table->size == 256 ) {
 252         for ( ; i < 6; i++ ) {
 253             x += *(key++);
 254         }
 255         return x;
 256     }
 257     uint16_t *p = (uint16_t *) key;
 258     for ( ; i < 3; i++ ) {
 259         x += *( p++ );
 260     }
 261     return x;
 262 }
 263
 264 // Make a text representation of bytes as ipv4 or ipv6
 265 static char *inet_nmtoa(unsigned char *b,int w) {
 266     static char buffer[20000];
 267     int i = 0;
 268     char * p = buffer;
 269     if ( w == 4 ) {
 270         sprintf( p,"%d.%d.%d.%d", b[0], b[1], b[2], b[3] );
 271     } else if ( w == 16 ){
 272         sprintf( p,
 273   "%02x%02x:%02x%02x:%02x%02x:%02x%02x:%02x%02x:%02x%02x:%02x%02x:%02x%02x",
 274                  b[0], b[1], b[2], b[3],
 275                  b[4], b[5], b[6], b[7],
 276                  b[8], b[9], b[10], b[11],
 277                  b[12], b[13], b[14], b[15] );
 278     } else {
 279         VERBOSE3OUT( "HEX data of %d bytes\n", w );
 280         for ( ; i < w && i < 19000; i++, p += 3 ) {
 281             sprintf( p, "%02x:", b[i] );
 282         }
 283         if ( w > 0 ) {
 284             *(--p) = 0;
 285         }
 286     }
 287     return buffer;
 288 }
 289
 290 // Form a MAC address string from 6 MAC address bytes, into one of the
 291 // 4 static buffer, whose use are cycled.
 292 static char *inet_mtoa(unsigned char *mac) {
 293     static char buffer[4][30];
 294     static int i = 0;
 295     if ( i > 3 ) {
 296         i = 0;
 297     }
 298     sprintf( buffer[i], "%02x:%02x:%02x:%02x:%02x:%02x",
 299              mac[0], mac[1], mac[2], mac[3], mac[4], mac[5] );
 300     return buffer[i++];
 301 }
 302
 303 // Form a socket address string from Sockaddr, into one of the
 304 // 4 static buffer, whose use are cycled.
 305 static char *inet_stoa(struct SockAddr *a) {
 306     static char buffer[1000];
 307     static char out[4][1000];
 308     static int i = 0;
 309     if ( i > 3 ) {
 310         i = 0;
 311     }
 312     if ( a->in.sa_family == AF_INET ) {
 313         sprintf( out[i], "%s:%d",
 314                  inet_ntop( AF_INET, &a->in4.sin_addr, buffer, 100 ),
 315                  ntohs( a->in4.sin_port ) );
 316     } else if ( a->in.sa_family == AF_INET6 ) {
 317         sprintf( out[i], "[%s]:%d",
 318                  inet_ntop( AF_INET6, &a->in6.sin6_addr, buffer, 100 ),
 319                  ntohs( a->in6.sin6_port ) );
 320     } else {
 321         sprintf( out[i], "<tap/stdio>" );
 322     }
 323     return out[i++];
 324 }
 325
 326 // Debugging: string representation of an Allowed record.
 327 static char *show_allowed(struct Allowed *a) {
 328     static char buffer[20000];
 329     if ( a == 0 ) {
 330         sprintf( buffer, "{tap/stdio}" );
 331     } else {
 332         sprintf( buffer, "%hd (%d) %s %p",
 333                  a->port, a->bits, inet_nmtoa( a->addr.bytes, a->addr.width ),
 334                  a->psk.key );
 335     }
 336     return buffer;
 337 }
 338
 339 // Recognize uplink specification
 340 static int is_uplink(struct Allowed *a) {
 341     return a->bits == (unsigned int) ( a->addr.width * 8 ) && a->port != 0;
 342 }
 343
 344 // Add a new Interface for a Remote. If non-null, the interface is
 345 // also added to the interface table.
 346 static struct Interface *add_interface(unsigned char *mac,struct Remote *r) {
 347     struct Interface *x = calloc( 1, sizeof( struct Interface ) );
 348     memcpy( x->mac, mac, sizeof( x->mac ) );
 349     x->remote = r;
 350     if ( r ) {
 351         Interface_ADD( x );
 352     }
 353     return x;
 354 }
 355
 356 // Add a new remote for a given address and spec.
 357 static struct Remote *add_remote(struct SockAddr *a,struct Allowed *s) {
 358     struct Remote *r = calloc( 1, sizeof( struct Remote ) );
 359     if ( a != 0 ) {
 360         memcpy( &r->uaddr, a, sizeof( r->uaddr ) );
 361     }
 362     r->spec = s;
 363     VERBOSE2OUT( "add_remote %s from spec: %s\n",
 364                  inet_stoa( &r->uaddr ),
 365                  ( s == 0 )? ( (a == 0)? "{tap/stdio}" : "{multicast}" )
 366                  : show_allowed( s ) );
 367     Remote_ADD( r );
 368     return r;
 369 }
 370
 371 // Add a new ignored interface on a channel
 372 static int add_ignored(struct Allowed *link,unsigned char *mac) {
 373     struct Interface *x = add_interface( mac, 0 );
 374     if ( x == 0 ) {
 375         return 1; // error: out of memory
 376     }
 377     Ignored_ADD( link, x );
 378     return 0;
 379 }
 380
 381 // Parse ignored interfaces
 382 // Comma separated list of MAC addresses
 383 static int parse_ignored_interfaces(char *arg,struct Allowed *link) {
 384     int a, b, c, d, e, f, g;
 385     while ( *arg ) {
 386         if ( sscanf( arg,"%x:%x:%x:%x:%x:%x%n",&a,&b,&c,&d,&e,&f,&g ) != 6 ) {
 387             // Not a mac addr
 388             return 1;
 389         }
 390         if ( (a|b|c|d|e|f) & ~0xff ) {
 391             return 1; // some %x is not hex
 392         }
 393         unsigned char mac[6] = { a, b, c, d, e, f };
 394         if ( add_ignored( link, mac ) ) {
 395             // Out of memory ??
 396             return 1;
 397         }
 398         VERBOSEOUT( "Ignoring: %s on channel %s\n",
 399                     inet_mtoa( mac ), link->source );
 400         arg += g;
 401         if ( *arg == 0 ) {
 402             break;
 403         }
 404         if ( *(arg++) != ',' ) {
 405             return 1; // Not comma separated
 406         }
 407     }
 408     return 0;
 409 }
 410
 411 //** IP address parsing utility
 412 // Clear bits after <bits>
 413 static void clearbitsafter(struct CharAddr *a,unsigned int bits) {
 414     unsigned int max = a->width * 8;
 415     int i;
 416     for ( i = a->width; i < 16; i++ ) {
 417         a->bytes[ i ] = 0;
 418     }
 419     for ( i = a->width - 1; i >= 0; i--, max -= 8 ) {
 420         if ( max - 8 < bits ) {
 421             break;
 422         }
 423         a->bytes[ i ] = 0;
 424     }
 425     if ( i >= 0 && max >= bits ) {
 426         a->bytes[ i ] &= ( 0xFF << ( bits - max ) );
 427     }
 428 }
 429
 430 //** IP address parsing utility
 431 // Find the PSK for the given +file+ in the +loaded+ table (of +count+ size)
 432 static struct PSK *findLoadedKeyfile(char *file,struct PSK *loaded,int count) {
 433     VERBOSE3OUT( "find %s\n", file );
 434     for ( count--; count >= 0; count-- ) {
 435         if ( strcmp( file, loaded[ count ].keyfile ) ) {
 436             VERBOSE3OUT( "found %d\n", count );
 437             return &loaded[ count ];
 438         }
 439     }
 440     VERBOSE3OUT( "found nothing\n" );
 441     return 0;
 442 }
 443
 444 //** IP address parsing utility
 445 // Load a key file into dynamically allocated memory, and update the
 446 // given PSK header for it.
 447 static void loadkey(struct PSK *psk) {
 448     static struct PSK *loaded = 0;
 449     static int count = 0;
 450     if ( psk->keyfile == 0 ) {
 451         return;
 452     }
 453     struct PSK *old = findLoadedKeyfile( psk->keyfile, loaded, count );
 454     if ( old ) {
 455         memcpy( psk, old, sizeof( struct PSK ) );
 456         return;
 457     }
 458     int e;
 459     unsigned char *p;
 460     int n;
 461     struct stat filestat;
 462     psk->keyfile = strdup( psk->keyfile );
 463     int fd = open( (char*) psk->keyfile, O_RDONLY );
 464     psk->seed = 0;
 465     if ( fd < 0 ) {
 466         perror( "open key file" );
 467         exit( 1 );
 468     }
 469     if ( fstat( fd, &filestat ) ) {
 470         perror( "stat of key file" );
 471         exit( 1 );
 472     }
 473     psk->key_length = filestat.st_size;
 474     if ( psk->key_length < 256 ) {
 475         fprintf( stderr, "Too small key file: %d %s\n", psk->key_length,
 476                  psk->keyfile );
 477         exit( 1 );
 478     }
 479     psk->key = malloc( psk->key_length );
 480     if ( psk->key == 0 ) {
 481         fprintf( stderr, "Cannot allocate %d bytes for %s\n",
 482                  psk->key_length, psk->keyfile );
 483         exit( 1 );
 484     }
 485     e = psk->key_length;
 486     p = psk->key;
 487     while ( ( n = read( fd, p, e ) ) > 0 ) {
 488         e -= n;
 489         p += n;
 490     }
 491     close( fd );
 492     if ( e != 0 ) {
 493         fprintf( stderr, "Failed loading key %s\n", psk->keyfile );
 494         exit( 1 );
 495     }
 496     for ( e = 0; (unsigned) e < psk->key_length; e++ ) {
 497         psk->seed += psk->key[ e ];
 498     }
 499     if ( psk->seed == 0 ) {
 500         fprintf( stderr, "Bad key %s; adds up to 0\n", psk->keyfile );
 501         exit( 1 );
 502     }
 503     count++;
 504     if ( loaded ) {
 505         loaded = realloc( loaded, ( count * sizeof( struct PSK ) ) );
 506     } else {
 507         loaded = malloc( sizeof( struct PSK ) );
 508     }
 509     memcpy( &loaded[ count-1 ], psk, sizeof( struct PSK ) );
 510     VERBOSE3OUT( "%d: %s %d %p %d\n", count-1, psk->keyfile, psk->seed,
 511                  psk->key, psk->key_length );
 512 }
 513
 514 //** IP address parsing utility
 515 // Fill out a CharAddr and *port from a SockAddr
 516 static void set_charaddrport(
 517     struct CharAddr *ca,unsigned short *port,struct SockAddr *sa)
 518 {
 519     memset( ca, 0, sizeof( struct CharAddr ) );
 520     ca->width = ( sa->in.sa_family == AF_INET )? 4 : 16;
 521     if ( ca->width == 4 ) {
 522         memcpy( &ca->in4, &sa->in4.sin_addr, 4 );
 523         *port = ntohs( sa->in4.sin_port );
 524     } else {
 525         memcpy( &ca->in6, &sa->in6.sin6_addr, 16 );
 526         *port = ntohs( sa->in6.sin6_port );
 527     }
 528 }
 529
 530 //** IP address parsing utility
 531 // Fill out a SockAddr from a CharAddr and port
 532 static void set_sockaddr(struct SockAddr *sa,struct CharAddr *ca,int port) {
 533     memset( sa, 0, sizeof( struct SockAddr ) );
 534     if ( ca->width == 4 ) {
 535         sa->in4.sin_family = AF_INET;
 536         sa->in4.sin_port = htons( port );
 537         memcpy( &sa->in4.sin_addr, &ca->in4, 4 );
 538     } else {
 539         sa->in6.sin6_family = AF_INET6;
 540         sa->in6.sin6_port = htons( port );
 541         memcpy( &sa->in6.sin6_addr, &ca->in6, 16 );
 542     }
 543 }
 544
 545 //** IP address parsing utility
 546 // Capture an optional port sub phrase [:<port>]
 547 static int parse_port(char *port,struct Allowed *into) {
 548     into->port = 0;
 549     if ( port ) {
 550         *(port++) = 0;
 551         int p;
 552         if ( sscanf( port, "%d", &p ) != 1 || p < 1 || p > 65535 ) {
 553             // Bad port number
 554             return 1;
 555         }
 556         into->port = p;
 557     }
 558     return 0;
 559 }
 560
 561 //** IP address parsing utility
 562 // Capture an optional bits sub phrase [/<bits>]
 563 static int parse_bits(char *bits,int max,struct Allowed *into) {
 564     into->bits = max;
 565     if ( bits ) {
 566         *(bits++) = 0;
 567         int b;
 568         if ( sscanf( bits, "%d", &b ) != 1 || b < 0 || b > max ) {
 569             return 1;
 570         }
 571         into->bits = b;
 572     }
 573     return 0;
 574 }
 575
 576 //** IP address parsing utility
 577 // Parse a command line argument as a declaration of an allowed
 578 // remote into the given <addr>.
 579 // Return 0 if ok and 1 otherwise
 580 // Formats: <ipv4-address>[/<bits>][:<port>][=keyfile]
 581 // Formats: <ipv6-address>[/<bits>][=keyfile]
 582 // Formats: \[<ipv6-address>[/<bits>]\][:<port>][=keyfile]
 583 // Formats: hostname:port[=keyfile]
 584 static int parse_allowed(char *arg,struct Allowed *into) {
 585     static char buffer[10000];
 586     int n = strlen( arg );
 587     if ( n > 9000 ) {
 588         return 1; // excessively large argument
 589     }
 590     strcpy( buffer, arg );
 591     into->source = arg;
 592     char * keyfile = strchr( buffer, '=' );
 593     if ( keyfile ) {
 594         *(keyfile++) = 0;
 595         into->psk.keyfile = keyfile;
 596     }
 597 #define B(b) b, b+1, b+2, b+3
 598     if ( sscanf( buffer, "%hhu.%hhu.%hhu.%hhu", B(into->addr.bytes) ) == 4 ) {
 599 #undef B
 600         // ipv4 address
 601         into->addr.width = 4;
 602         if ( parse_port( strchr( buffer, ':' ), into ) ) {
 603             fprintf( stderr, "bad port\n" );
 604             return 1;
 605         }
 606         if ( parse_bits( strchr( buffer, '/' ), 32, into ) ) {
 607             fprintf( stderr, "bad bits\n" );
 608             return 1;
 609         }
 610         return 0;
 611     }
 612     // ipv6 address
 613     char * address = buffer;
 614     into->port = 0;
 615     if ( *buffer == '[' ) {
 616         // bracketed form, necessary for port
 617         char *end = strchr( buffer, ']' );
 618         if ( end == 0 ) {
 619             return 1; // bad argument
 620         }
 621         address++;
 622         *(end++) = 0;
 623         if ( *end == ':' && parse_port( end, into ) ) {
 624             return 1;
 625         }
 626     }
 627     into->addr.width = 16;
 628     if ( parse_bits( strchr( address, '/' ), 128, into ) ) {
 629         return 1;
 630     }
 631     if ( inet_pton( AF_INET6, address, into->addr.bytes ) != 1 ) {
 632         return 1; // Bad IPv6
 633     }
 634     return 0;
 635 }
 636
 637 //** IP address parsing utility
 638 // Add a new channel spec into the <allowed> table
 639 // spec == 0 for the tap/stdio channel
 640 static struct Allowed *add_allowed(char *spec) {
 641     struct Allowed *into = calloc( 1, sizeof(struct Allowed) );
 642     htable x = HTABLEINIT( struct Interface, mac, hashcode_mac );
 643     into->ignored_mac = x;
 644     if ( spec != 0 ) {
 645         if ( parse_allowed( spec, into ) ) {
 646             fprintf( stderr, "Bad remote spec: %s\n", spec );
 647             return 0;
 648         }
 649     }
 650     int i;
 651     if ( allowed.table == 0 ) {
 652         // First entry.
 653         allowed.table = calloc( 1, sizeof(struct Allowed*) );
 654         allowed.count = 1;
 655         i = 0;
 656     } else {
 657         i = allowed.count++;
 658         allowed.table = realloc( allowed.table,
 659                                  allowed.count * sizeof(struct Allowed*) );
 660         if ( allowed.table == 0 ) {
 661             fprintf( stderr, "OUT OF MEMORY\n" );
 662             exit( 1 );
 663         }
 664     }
 665     allowed.table[i] = into;
 666
 667     loadkey( &into->psk );
 668     VERBOSE3OUT( "Allowed %s { %s }\n", into->source, show_allowed( into ) );
 669     if ( is_uplink( into ) ) {
 670         struct SockAddr addr;
 671         set_sockaddr( &addr, &into->addr, into->port );
 672         VERBOSEOUT( "Add uplink %s\n", show_allowed( into ) );
 673         (void) add_remote( &addr, into );
 674     }
 675     return into;
 676 }
 677
 678 static int parse_threads_count(char *arg) {
 679     if ( ( sscanf( arg, "%u", &threads_count ) != 1 ) || threads_count < 1 ) {
 680         return 1;
 681     }
 682     VERBOSEOUT( "** Threads count = %d\n", threads_count );
 683     return 0;
 684 }
 685
 686 static int parse_buffers_count(char *arg) {
 687     if ( ( sscanf( arg, "%u", &buffers_count ) != 1 ) || buffers_count < 1 ) {
 688         return 1;
 689     }
 690     VERBOSEOUT( "** Buffers count = %d\n", buffers_count );
 691     return 0;
 692 }
 693
 694 //** IP address parsing utility for multicast phrase
 695 // Return 0 if ok and 1 otherwise
 696 // Formats: <ipv4-address>:<port>[=keyfile]
 697 // The ipv4 address should be a multicast address in ranges
 698 // 224.0.0.0/22, 232.0.0.0/7, 234.0.0.0/8 or 239.0.0.0/8
 699 // though it's not checked here.
 700 static int parse_mcast(char *arg) {
 701     static char buffer[10000];
 702     int n = strlen( arg );
 703     if ( n > 9000 ) {
 704         return 1; // excessively large argument
 705     }
 706     memcpy( buffer, arg, n );
 707     char *p = buffer + n - 1;
 708     for ( ; p > buffer && *p != ':' && *p != '='; p-- ) { }
 709     if ( *p == '=' ) {
 710         mcast.psk.keyfile = p+1;
 711         *p = 0;
 712         loadkey( &mcast.psk );
 713         for ( ; p > buffer && *p != ':' ; p-- ) { }
 714     }
 715     if ( *p != ':' ) {
 716         fprintf( stderr, "Multicast port is required\n" );
 717         return 1; // Port number is required
 718     }
 719     *(p++) = 0;
 720     if ( inet_pton( AF_INET, buffer, &mcast.group.imr_multiaddr.s_addr )==0 ) {
 721         fprintf( stderr, "Multicast address required\n" );
 722         return 1;
 723     }
 724     char *e;
 725     long int port = strtol( p, &e, 10 );
 726     if ( *e != 0 || port < 1 || port > 65535 ) {
 727         fprintf( stderr, "Bad multicast port\n" );
 728         return 1;
 729     }
 730     mcast.group.imr_address.s_addr = htonl(INADDR_ANY);
 731     mcast.sock.in4.sin_family = AF_INET;
 732     mcast.sock.in4.sin_addr.s_addr = htonl(INADDR_ANY);
 733     mcast.sock.in4.sin_port = htons( atoi( p ) );
 734     return 0;
 735 }
 736
 737 //** IP address parsing utility for UDP source address
 738 // Return 0 if ok and 1 otherwise
 739 // Formats: <ipv4-address> or <ipv6-address>
 740 // The ipv4 address should be a multicast address in ranges
 741 // 224.0.0.0/22, 232.0.0.0/7, 234.0.0.0/8 or 239.0.0.0/8
 742 // though it's not checked here.
 743 static int parse_udp_source(char *arg) {
 744     if ( inet_pton( AF_INET6, arg, udp_source.address ) ) {
 745         // An ipv6 address is given.
 746         if ( udp6 ) {
 747             udp_source.family = AF_INET6;
 748             return 0;
 749         }
 750         return 1;
 751     }
 752     if ( ! inet_pton( AF_INET, arg, udp_source.address ) ) {
 753         return 1;
 754     }
 755
 756     // An ipv4 address is given.
 757     if ( udp6 ) {
 758         // Translate into ipv6-encoded ipv4
 759         memmove( udp_source.address + 12, udp_source.address, 4 );
 760         memset( udp_source.address, 0, 10 );
 761         memset( udp_source.address + 10, -1, 2 );
 762         udp_source.family = AF_INET6;
 763     } else {
 764         udp_source.family = AF_INET;
 765     }
 766     return 0;
 767 }
 768
 769 // Utility that sets upt the multicast socket, which is used for
 770 // receiving multicast packets.
 771 static void setup_mcast() {
 772     // set up ipv4 socket
 773     if ( ( mcast.fd = socket( AF_INET, SOCK_DGRAM, 0 ) ) == 0 ) {
 774         perror( "creating socket");
 775         exit(1);
 776     }
 777     if ( setsockopt( mcast.fd, IPPROTO_IP, IP_ADD_MEMBERSHIP,
 778                      (char *) &mcast.group, sizeof( mcast.group ) ) < 0) {
 779         perror( "Joining multicast group" );
 780         exit( 1 );
 781     }
 782     int reuse = 1;
 783     if ( setsockopt( mcast.fd, SOL_SOCKET, SO_REUSEADDR,
 784                      &reuse, sizeof( int ) ) < 0 ) {
 785         perror( "SO_REUSEADDR" );
 786         exit( 1 );
 787     }
 788     if ( bind( mcast.fd, (struct sockaddr*) &mcast.sock.in,
 789                sizeof( struct sockaddr ) ) ) {
 790         fprintf( stderr, "Error binding socket!\n");
 791         exit(1);
 792     }
 793     // Change mcast address to be the group multiaddress, and add
 794     // a persistent "remote" for it.
 795     mcast.sock.in4.sin_addr.s_addr = mcast.group.imr_multiaddr.s_addr;
 796     add_remote( &mcast.sock, 0 );
 797 }
 798
 799 // Find the applicable channel rule for a given ip:port address
 800 static struct Allowed *is_allowed_remote(struct SockAddr *addr) {
 801     struct CharAddr ca;
 802     int width = ( addr->in.sa_family == AF_INET )? 4 : 16;
 803     unsigned short port;
 804     int i = 0;
 805     for ( ; (unsigned) i < allowed.count; i++ ) {
 806         struct Allowed *a = allowed.table[i];
 807         if ( a->addr.width != width ) {
 808             continue;
 809         }
 810         set_charaddrport( &ca, &port, addr );
 811         if ( a->port != 0 && a->port != port ) {
 812             continue;
 813         }
 814         clearbitsafter( &ca, a->bits );
 815         if ( memcmp( &ca, &a->addr, sizeof( struct CharAddr ) ) == 0 ) {
 816             return a;
 817         }
 818     }
 819     return 0; // Disallowed
 820 }
 821
 822 // Simple PSK encryption:
 823 //
 824 // First, xor each byte with a key byte that is picked from the key
 825 // by means of an index that includes the prior encoding. Also,
 826 // compute the sum of encrypted bytes into a "magic" that is added the
 827 // "seed" for seeding the random number generator. Secondly reorder
 828 // the bytes using successive rand number picks from the seeded
 829 // generator.
 830 //
 831 static void encrypt(unsigned char *buf,unsigned int n,struct PSK *psk) {
 832     unsigned int k;
 833     unsigned int r;
 834     unsigned char b;
 835     unsigned int magic;
 836     VERBOSE3OUT( "encrypt by %s %p\n", psk->keyfile, psk->key );
 837     for ( k = 0, r = 0, magic = 0; k < n; k++ ) {
 838         r = ( r + magic + k ) % psk->key_length;
 839         buf[k] ^= psk->key[ r ];
 840         magic += buf[k];
 841     }
 842     pthread_mutex_lock( &crypting );
 843     srand( psk->seed + magic );
 844     for ( k = 0; k < n; k++ ) {
 845         r = rand() % n;
 846         b = buf[k];
 847         buf[k] = buf[r];
 848         buf[r] = b;
 849     }
 850     pthread_mutex_unlock( &crypting );
 851 }
 852
 853 // Corresponding decryption procedure .
 854 static void decrypt(unsigned char *buf,unsigned int n,struct PSK *psk) {
 855     unsigned int randoms[ BUFSIZE ];
 856     unsigned int k;
 857     unsigned int r;
 858     unsigned char b;
 859     unsigned int magic = 0;
 860     for ( k = 0; k < n; k++ ) {
 861         magic += buf[k];
 862     }
 863     pthread_mutex_lock( &crypting );
 864     srand( psk->seed + magic );
 865     for ( k = 0; k < n; k++ ) {
 866         randoms[k] = rand() % n;
 867     }
 868     pthread_mutex_unlock( &crypting );
 869     for ( k = n; k > 0; ) {
 870         r = randoms[ --k ];
 871         b = buf[k];
 872         buf[k] = buf[r];
 873         buf[r] = b;
 874     }
 875     for ( k = 0, r = 0, magic = 0; k < n; k++ ) {
 876         r = ( r + magic + k ) % psk->key_length;
 877         magic += buf[k];
 878         buf[k] ^= psk->key[r];
 879     }
 880 }
 881
 882 // Write a buffer data to given file descriptor (basically tap_fd in
 883 // this program). This is never fragmented.
 884 static int dowrite(int fd, unsigned char *buf, int n) {
 885     int w;
 886     if ( ( w = write( fd, buf, n ) ) < 0){
 887         perror( "Writing data" );
 888         w = -1;
 889     }
 890     return w;
 891 }
 892
 893 // Write to the tap/stdio; adding length prefix for stdio
 894 static int write_tap(unsigned char *buf, int n) {
 895     uint8_t tag0 = *( buf + 12 );
 896     if ( tag0 == 8 ) {
 897         uint16_t size = ntohs( *(uint16_t*)(buf + 16) );
 898         if ( size <= 1500 ) {
 899             if ( ( verbose >= 2 ) && ( n != size + 14 ) ) {
 900                 VERBOSEOUT( "clip %d to %d\n", n, size + 14 );
 901             }
 902             n = size + 14; // Clip of any tail
 903         }
 904     }
 905     if ( stdio ) {
 906         uint16_t plength = htons( n );
 907         if ( dowrite( 1, (unsigned char *) &plength,
 908                       sizeof( plength ) ) < 0 ) {
 909             return -11;
 910         }
 911         return dowrite( 1, buf, n );
 912     }
 913     return dowrite( tap_fd, buf, n );
 914 }
 915
 916 // Write a packet via the given Interface with encryption as specified.
 917 static void write_remote(unsigned char *buf, int n,struct Remote *r) {
 918     // A packet buffer
 919     unsigned char output[ BUFSIZE ];
 920     if ( n < 12 ) {
 921         VERBOSE2OUT( "SEND %d bytes to %s\n", n, inet_stoa( &r->uaddr ) );
 922     } else {
 923         VERBOSE2OUT( "SEND %d bytes %s -> %s to %s\n", n,
 924                      inet_mtoa( buf+6 ), inet_mtoa( buf ),
 925                      inet_stoa( &r->uaddr ) );
 926     }
 927     memcpy( output, buf, n ); // Use the private buffer for delivery
 928     // Apply the TPG quirk
 929     if ( tpg_quirk && ( n > 1460 ) && ( n < 1478 ) ) {
 930         VERBOSE2OUT( "tpg quirk applied\n" );
 931         n = 1478; // Add some "random" tag-along bytes
 932     }
 933     if ( r->spec == 0 ) {
 934         if ( r->uaddr.in.sa_family == 0 ) {
 935             // Output to tap/stdio
 936             if ( write_tap( buf, n ) < 0 ) {
 937                 // panic error
 938                 fprintf( stderr, "Cannot write to tap/stdio: exiting!\n" );
 939                 exit( 1 );
 940             }
 941             return;
 942         }
 943         // Fall through for multicast
 944         if ( mcast.psk.keyfile ) {
 945             encrypt( output, n, &mcast.psk );
 946         }
 947     } else if ( r->spec->psk.keyfile ) {
 948         encrypt( output, n, &r->spec->psk );
 949     }
 950     struct sockaddr *sock = &r->uaddr.in;
 951     size_t size;
 952     if ( sock->sa_family == AF_INET6 ) {
 953         // Note that the size of +struct sockaddr_in6+ is actually
 954         // larger than the size of +struct sockaddr+ (due to the
 955         // addition of the +sin6_flowinfo+ field). It results in the
 956         // following cuteness for passing arguments to +sendto+.
 957         size = sizeof( struct sockaddr_in6 );
 958         VERBOSE2OUT( "IPv6 UDP %d %s\n",
 959                      udp_fd, inet_stoa( (struct SockAddr*) sock ) );
 960     } else {
 961         size = sizeof( struct sockaddr_in );
 962         VERBOSE2OUT( "IPv4 UDP %d %s\n",
 963                      udp_fd, inet_stoa( (struct SockAddr*) sock ) );
 964     }
 965     VERBOSE2OUT( "SEND %d bytes to %s [%s -> %s]\n",
 966                  n, inet_stoa( (struct SockAddr*) sock ),
 967                  ( n < 12 )? "" : inet_mtoa( buf+6 ),
 968                  ( n < 12 )? "" : inet_mtoa( buf )
 969               );
 970     // IS sendto thread safe??
 971     if ( sendto( udp_fd, output, n, 0, sock, size ) < n ) {
 972         perror( "Writing socket" );
 973         // Invalidate remote temporarily instead? But if it's an
 974         // "uplink" it should be retried eventually...
 975         // For now: just ignore the error.
 976         // exit( 1 );
 977     }
 978 }
 979
 980 // Delete a Remote and all its interfaces
 981 static void delete_remote(struct Remote *r) {
 982     VERBOSE2OUT( "DELETE Remote and all its interfaces %s\n",
 983                  inet_stoa( &r->uaddr ) );
 984     unsigned int i = 0;
 985     struct Interface *x;
 986     Interface_LOCK;
 987     for ( ; i < remotes.by_mac.size; i++ ) {
 988         unsigned char *tmp = remotes.by_mac.data[i];
 989         if ( tmp == 0 || tmp == (unsigned char *)1 ) {
 990             continue;
 991         }
 992         x = (struct Interface *) tmp;
 993         if ( x->remote == r ) {
 994             Interface_DEL( x );
 995             free( x );
 996         }
 997     }
 998     Interface_UNLOCK;
 999     Remote_DEL( r );
1000     free( r );
1001 }
1002
1003 // Unmap an ipv4-mapped ipv6 address
1004 static void unmap_if_mapped(struct SockAddr *s) {
1005     if ( s->in.sa_family != AF_INET6 ||
1006          memcmp( "\000\000\000\000\000\000\000\000\000\000\377\377",
1007                  &s->in6.sin6_addr, 12 ) ) {
1008         return;
1009     }
1010     VERBOSE2OUT( "unmap %s\n",
1011                  inet_nmtoa( (unsigned char*) s, sizeof( struct SockAddr ) ) );
1012     s->in.sa_family = AF_INET;
1013     memcpy( &s->in4.sin_addr, s->in6.sin6_addr.s6_addr + 12, 4 );
1014     memset( s->in6.sin6_addr.s6_addr + 4, 0, 12 );
1015     VERBOSE2OUT( "becomes %s\n",
1016                  inet_nmtoa( (unsigned char*) s, sizeof( struct SockAddr ) ) );
1017 }
1018
1019 // Route the packet from the given src
1020 static struct Interface *input_check(
1021     unsigned char *buf,ssize_t len,struct SockAddr *src )
1022 {
1023     VERBOSE2OUT( "RECV %ld bytes from %s\n", len, inet_stoa( src ) );
1024     struct Remote *r = 0;
1025     struct timeval now = { 0 };
1026     if ( gettimeofday( &now, 0 ) ) {
1027         perror( "RECV time" );
1028         now.tv_sec = time( 0 );
1029     }
1030     Remote_FIND( src, r );
1031     if ( r == 0 ) {
1032         struct Allowed *a = is_allowed_remote( src );
1033         if ( a == 0 ) {
1034             VERBOSEOUT( "Ignoring %s\n", inet_stoa( src ) );
1035             return 0; // Disallowed
1036         }
1037         VERBOSEOUT( "New remote %s by %s\n", inet_stoa( src ), a->source );
1038         r = add_remote( src, a );
1039         //r->rec_when = now; // Set activity stamp of new remote
1040     }
1041     if ( len < 12 ) {
1042         // Ignore short data, but maintain channel
1043         r->rec_when = now; // Update activity stamp touched remote
1044         if ( len > 0 ) {
1045             VERBOSEOUT( "Ignoring %ld bytes from %s\n",
1046                         len, inet_stoa( src ) );
1047         }
1048         return 0;
1049     }
1050     // Now decrypt the data as needed
1051     if ( r->spec ) {
1052         if ( r->spec->psk.seed ) {
1053             decrypt( buf, len, &r->spec->psk );
1054         }
1055     } else if ( r->uaddr.in.sa_family == 0 && mcast.psk.keyfile ) {
1056         decrypt( buf, len, &mcast.psk );
1057     }
1058     VERBOSE2OUT( "RECV %s -> %s from %s\n",
1059                  inet_mtoa( buf+6 ), inet_mtoa( buf ),
1060                  inet_stoa( &r->uaddr ) );
1061     // Note: the payload is now decrypted, and known to be from +r+
1062     struct Interface *x = 0;
1063     // Packets concerning an ignored interface should be ignored.
1064     if ( r->spec && r->spec->ignored_mac.data ) {
1065         Ignored_FIND( r->spec, buf+6, x );
1066         if ( x ) {
1067             VERBOSE2OUT( "Dropped MAC %s from %s on %s\n",
1068                          inet_mtoa( buf+6 ), inet_stoa( &r->uaddr ),
1069                          r->spec->source );
1070             return 0;
1071         }
1072         Ignored_FIND( r->spec, buf, x );
1073         if ( x ) {
1074             VERBOSE2OUT( "Dropped MAC %s to %s on %s\n",
1075                          inet_mtoa( buf ), inet_stoa( &r->uaddr ),
1076                          r->spec->source );
1077             return 0;
1078         }
1079     }
1080     Interface_FIND( buf+6, x );
1081     if ( x == 0 ) {
1082         // Totally new MAC. Should bind it to the remote
1083         VERBOSEOUT( "New MAC %s from %s\n",
1084                     inet_mtoa( buf+6 ), inet_stoa( src ) );
1085         x = add_interface( buf+6, r );
1086         r->rec_when = now; // Update activity stamp for remote
1087         x->rec_when = now;
1088         return x;
1089     }
1090     // Seen that MAC already
1091     if ( x->remote == r ) {
1092         VERBOSE2OUT( "RECV %s from %s again\n",
1093                      inet_mtoa( buf+6 ), inet_stoa( &x->remote->uaddr ) );
1094         r->rec_when = now; // Update activity stamp
1095         x->rec_when = now; // Update activity stamp
1096         return x;
1097     }
1098     // MAC clash from two different connections
1099     // r = current
1100     // x->remote = previous
1101     VERBOSE2OUT( "RECV %s from %s previously from %s\n",
1102               inet_mtoa( buf+6 ),
1103               inet_stoa( &r->uaddr ),
1104               inet_stoa( &x->remote->uaddr ) );
1105     if ( r->spec ) {
1106         // The packet source MAC has arrived on other than its
1107         // previous channel. It thus gets dropped if tap/stdin is the
1108         // primary channel, or the time since the last packet for that
1109         // interface is less than RECENT_MICROS, with different limits
1110         // for broadcast and unicast.
1111         int64_t dmac = DIFF_MICROS( &now, &x->rec_when);
1112         if ( x->remote->spec == 0 || RECENT_MICROS( *buf & 1, dmac ) ) {
1113             if ( verbose >= 2 ) {
1114                 fprintf(
1115                     stderr,
1116                     "Dropped. MAC %s (%ld) from %s, should be %s\n",
1117                     inet_mtoa( buf+6 ), dmac,
1118                     inet_stoa( src ), inet_stoa( &x->remote->uaddr ) );
1119             }
1120             return 0;
1121         }
1122         // Check if previous package on the interface was recent
1123     } else if ( r->uaddr.in.sa_family ) {
1124         // Multicast incoming clashing with tap/stdio
1125         VERBOSE3OUT( "Dropped multicast loopback\n" );
1126         return 0;
1127     }
1128
1129     // New remote takes over the MAC
1130     VERBOSEOUT( "MAC %s from %s cancels previous %s\n",
1131                 inet_mtoa( buf+6 ), inet_stoa( src ),
1132                 inet_stoa( &x->remote->uaddr ) );
1133     x->remote = r; // Change remote for MAC
1134     // Note that this may leave the old x->remote without any interface
1135     r->rec_when = now; // Update activity stamp
1136     x->rec_when = now; // Update activity stamp
1137     return x;
1138 }
1139
1140 // Check packet and deliver out
1141 static void route_packet(unsigned char *buf,int len,struct SockAddr *src) {
1142     struct Interface *x = input_check( buf, len, src );
1143     if ( x == 0 ) {
1144         return; // not a nice packet
1145     }
1146     if ( ( *buf & 1 ) == 0 ) {
1147         // unicast
1148         struct Interface *y = 0; // reuse for destination interface
1149         Interface_FIND( buf, y );
1150         if ( y == 0 ) {
1151             VERBOSE2OUT( "RECV %s -> %s from %s without channel and dropped\n",
1152                         inet_mtoa( buf+6 ), inet_mtoa( buf ),
1153                         inet_stoa( &x->remote->uaddr ) );
1154             return;
1155         }
1156         if ( x->remote == y->remote ) {
1157             VERBOSEOUT( "RECV loop for %s -> %s from %s to %s\n",
1158                         inet_mtoa( buf+6 ), inet_mtoa( buf ),
1159                         inet_stoa( &x->remote->uaddr ),
1160                         inet_stoa( &y->remote->uaddr ) );
1161             Interface_DEL( y ); // Need to see this interface again
1162             return;
1163         }
1164         VERBOSE2OUT( "RECV route %s -> %s to %s\n",
1165                      inet_mtoa( buf+6 ), inet_mtoa( buf ),
1166                      inet_stoa( &y->remote->uaddr ) );
1167         write_remote( buf, len, y->remote );
1168         return;
1169     }
1170     // broadcast. +x+ is source interface
1171     // x->rec_when is not updated
1172     struct timeval now = { 0 };
1173     if ( gettimeofday( &now, 0 ) ) {
1174         perror( "RECV time" );
1175         now.tv_sec = time( 0 );
1176     }
1177     VERBOSE2OUT( "BC %s -> %s from %s\n",
1178                  inet_mtoa( buf+6 ), inet_mtoa( buf ),
1179                  inet_stoa( &x->remote->uaddr ) );
1180     struct Remote *r;
1181     unsigned int i = 0;
1182     Remote_LOCK;
1183     for ( ; i < remotes.by_addr.size; i++ ) {
1184         unsigned char *tmp = remotes.by_addr.data[i];
1185         if ( tmp == 0 || tmp == (unsigned char *)1 ) {
1186             continue;
1187         }
1188         r = (struct Remote *) tmp;
1189         VERBOSE3OUT( "BC check %s\n", inet_stoa( &r->uaddr ) );
1190         if ( r == x->remote ) {
1191             VERBOSE3OUT( "BC r == x->remote\n" );
1192             continue;
1193         }
1194         if ( r->spec && ! is_uplink( r->spec ) &&
1195              DIFF_MICROS( &now, &r->rec_when ) > VERYOLD_MICROS ) {
1196             // remove old downlink connection
1197             VERBOSEOUT( "Old remote discarded %s (%ld)\n",
1198                         inet_stoa( &r->uaddr ),
1199                         TIME_MICROS( &r->rec_when ) );
1200             // Removing a downlink might have threading implications
1201             delete_remote( r );
1202             continue;
1203         }
1204         // Send packet to the remote
1205         // Only no-clash or to the tap/stdin
1206         write_remote( buf, len, r );
1207     }
1208     Remote_UNLOCK;
1209 }
1210
1211 // The packet handling queues
1212 static struct {
1213     Queue full;
1214     Queue free;
1215     sem_t reading;
1216 } todolist;
1217
1218 // The threadcontrol program for handling packets.
1219 static void *packet_handler(void *data) {
1220     (void) data;
1221     for ( ;; ) {
1222         PacketItem *todo = (PacketItem *) Queue_getItem( &todolist.full );
1223         if ( todo->fd == mcast.fd ) {
1224             // Patch multicast address as source for multicast packet
1225             route_packet( todo->buffer, todo->len, &mcast.sock );
1226         } else {
1227             if ( udp6 ) {
1228                 unmap_if_mapped( &todo->src );
1229             }
1230             route_packet( todo->buffer, todo->len, &todo->src );
1231         }
1232         Queue_addItem( &todolist.free, (QueueItem*) todo );
1233     }
1234     return 0;
1235 }
1236
1237 void todolist_initialize(int nbuf,int nthr) {
1238     if ( pthread_mutex_init( &todolist.full.mutex, 0 ) ||
1239          sem_init( &todolist.full.count, 0, 0 ) ) {
1240         perror( "FATAL" );
1241         exit( 1 );
1242     }
1243     if ( pthread_mutex_init( &todolist.free.mutex, 0 ) ||
1244          sem_init( &todolist.free.count, 0, 0 ) ) {
1245         perror( "FATAL" );
1246         exit( 1 );
1247     }
1248     if ( sem_init( &todolist.reading, 0, 1 ) ) {
1249         perror( "FATAL" );
1250         exit( 1 );
1251     }
1252     Queue_initialize( &todolist.free, nbuf, sizeof( PacketItem ) );
1253     for ( ; nthr > 0; nthr-- ) {
1254         pthread_t thread; // Temporary thread id
1255         pthread_create( &thread, 0, packet_handler, 0 );
1256     }
1257 }
1258
1259 // Read a full UDP packet into the given buffer, associate with a
1260 // connection, or create a new connection, the decrypt the as
1261 // specified, and capture the sender MAC address. The connection table
1262 // is updated for the new MAC address, However, if there is then a MAC
1263 // address clash in the connection table, then the associated remote
1264 // is removed, and the packet is dropped.
1265 static void *doreadUDP(void *data) {
1266     int fd = ((ReaderData *) data)->fd;
1267     while ( 1 ) {
1268         PacketItem *todo = (PacketItem *) Queue_getItem( &todolist.free );
1269         socklen_t addrlen =
1270             udp6? sizeof( todo->src.in6 ) : sizeof( todo->src.in4 );
1271         memset( &todo->src, 0, sizeof( todo->src ) );
1272         todo->fd = fd;
1273         todo->len = recvfrom(
1274             fd, todo->buffer, BUFSIZE, 0, &todo->src.in, &addrlen );
1275         if ( todo->len == -1) {
1276             perror( "Receiving UDP" );
1277             exit( 1 );
1278         }
1279 #ifdef GPROF
1280         if ( todo->len == 17 &&
1281              memcmp( todo->buffer, "STOPSTOPSTOPSTOP", 16 ) == 0 ) {
1282             exit( 0 );
1283         }
1284 #endif
1285         Queue_addItem( &todolist.full, (QueueItem*) todo );
1286     }
1287     return 0;
1288 }
1289
1290 // Read up to n bytes from the given file descriptor into the buffer
1291 static int doread(int fd, unsigned char *buf, int n) {
1292     ssize_t len;
1293     if ( ( len = read( fd, buf, n ) ) < 0 ) {
1294         perror( "Reading stdin" );
1295         exit( 1 );
1296     }
1297     return len;
1298 }
1299
1300 // Read n bytes from the given file descriptor into the buffer.
1301 // If partial is allowed, then return amount read, otherwise keep
1302 // reading until full.
1303 static int read_into(int fd, unsigned char *buf, int n,int partial) {
1304     int r, x = n;
1305     while( x > 0 ) {
1306         if ( (r = doread( fd, buf, x ) ) == 0 ) {
1307             return 0 ;
1308         }
1309         x -= r;
1310         buf += r;
1311         if ( partial ) {
1312             return n - x;
1313         }
1314     }
1315     return n;
1316 }
1317
1318 // Go through all uplinks and issue a "heart beat"
1319 static void heartbeat(int fd) {
1320     static unsigned char data[10];
1321     VERBOSE3OUT( "heartbeat fd=%d\n", fd );
1322     struct Remote *r;
1323     unsigned int i = 0;
1324     struct timeval now;
1325     if ( gettimeofday( &now, 0 ) ) {
1326         perror( "HEARTBEAT time" );
1327         now.tv_sec = time( 0 );
1328         now.tv_usec = 0;
1329     }
1330     Remote_LOCK;
1331     for ( ; i < remotes.by_addr.size; i++ ) {
1332         unsigned char *tmp = remotes.by_addr.data[i];
1333         if ( tmp == 0 || tmp == (unsigned char *)1 ) {
1334             continue;
1335         }
1336         r = (struct Remote *) tmp;
1337         VERBOSE3OUT( "heartbeat check %s\n", inet_stoa( &r->uaddr ) );
1338         if ( r->spec && is_uplink( r->spec ) ) {
1339             if ( DIFF_MICROS( &now, &r->rec_when ) > HEARTBEAT_MICROS ) {
1340                 VERBOSE3OUT( "heartbeat %s\n", inet_stoa( &r->uaddr ) );
1341                 write_remote( data, 0, r );
1342             }
1343         }
1344     }
1345     Remote_UNLOCK;
1346 }
1347
1348 // Tell how to use this program and exit with failure.
1349 static void usage(void) {
1350     fprintf( stderr, "Packet tunneling over UDP, multiple channels, " );
1351     fprintf( stderr, "version 1.5.3\n" );
1352     fprintf( stderr, "Usage: " );
1353     fprintf( stderr, "%s [options] port [remote]+ \n", progname );
1354     fprintf( stderr, "** options must be given or omitted in order!!\n" );
1355     fprintf( stderr, " -v        = verbose log, -vv or -vvv for more logs\n" );
1356     fprintf( stderr, " -tpg      = UDP transport quirk: avoid bad sizes\n" );
1357     fprintf( stderr, " -4        = use an ipv4 UDP socket\n" );
1358     fprintf( stderr, " -B n      = use n buffers (2*threads) by default\n");
1359     fprintf( stderr, " -T n      = use n delivery threads (5 bu default)\n" );
1360     fprintf( stderr, " -m mcast  = allow remotes on multicast address\n" );
1361     fprintf( stderr, " -t tap    = use the nominated tap (or - for stdio)\n" );
1362     fprintf( stderr, " -S source = use given source address for UDP\n" );
1363     exit( 1 );
1364 }
1365
1366 // Open the given tap
1367 static int tun_alloc(char *dev, int flags) {
1368     struct ifreq ifr;
1369     int fd, err;
1370     if ( ( fd = open( "/dev/net/tun", O_RDWR ) ) < 0 ) {
1371         perror( "Opening /dev/net/tun" );
1372         return fd;
1373     }
1374     memset( &ifr, 0, sizeof( ifr ) );
1375     ifr.ifr_flags = flags;
1376     if ( *dev ) {
1377         strcpy( ifr.ifr_name, dev );
1378     }
1379     if ( ( err = ioctl( fd, TUNSETIFF, (void *) &ifr ) ) < 0 ) {
1380         perror( "ioctl(TUNSETIFF)" );
1381         close( fd );
1382         return err;
1383     }
1384     strcpy( dev, ifr.ifr_name );
1385     return fd;
1386 }
1387
1388 // Handle packet received on the tap/stdio channel
1389 static void initialize_tap() {
1390     // Ensure there is a Remote for this
1391     static struct Remote *tap_remote = 0;
1392     if ( tap_remote == 0 ) {
1393         Remote_LOCK;
1394         if ( tap_remote == 0 ) {
1395             tap_remote = add_remote( 0, 0 );
1396         }
1397         Remote_UNLOCK;
1398     }
1399 }
1400
1401 // Thread to handle tap/stdio input
1402 static void *doreadTap(void *data) {
1403     int fd = ((ReaderData*) data)->fd;
1404     unsigned int end = 0; // Packet size
1405     unsigned int cur = 0; // Amount read so far
1406     size_t e;
1407     PacketItem *todo = (PacketItem*) Queue_getItem( &todolist.free );
1408     while ( 1 ) {
1409         if ( stdio ) {
1410             uint16_t plength;
1411             int n = read_into( 0, (unsigned char *) &plength,
1412                                sizeof( plength ), 0 );
1413             if ( n == 0 ) {
1414                 // Tap/stdio closed => exit silently
1415                 exit( 0 );
1416             }
1417             end = ntohs( plength );
1418             cur = 0;
1419             while ( ( e = ( end - cur ) ) != 0 ) {
1420                 unsigned char *p = todo->buffer + cur;
1421                 if ( end > BUFSIZE ) {
1422                     // Oversize packets should be read and discarded
1423                     if ( e > BUFSIZE ) {
1424                         e = BUFSIZE;
1425                     }
1426                     p = todo->buffer;
1427                 }
1428                 cur += read_into( 0, p, e, 1 );
1429             }
1430         } else {
1431             end = doread( fd, todo->buffer, BUFSIZE );
1432             cur = end;
1433         }
1434         VERBOSE3OUT( "TAP/stdio input %d bytes\n", end );
1435         if ( end <= BUFSIZE ) {
1436             todo->fd = 0;
1437             todo->len = end;
1438             Queue_addItem( &todolist.full, (QueueItem*) todo );
1439             todo = (PacketItem*) Queue_getItem( &todolist.free );
1440         }
1441         // End handling tap
1442     }
1443     return 0;
1444 }
1445
1446 // Application main function
1447 // Parentheses mark optional
1448 // $* = (-v) (-4) (-B n) (-T n) (-m mcast) (-t port) (ip:)port (remote)+
1449 // remote = ipv4(/maskwidth)(:port)(=key)
1450 // remote = ipv6(/maskwidth)(=key)
1451 // remote = [ipv6(/maskwidth)](:port)(=key)
1452 // ip = ipv4 | [ipv6]
1453 int main(int argc, char *argv[]) {
1454     pthread_t thread; // Temporary thread id
1455     int port, i;
1456     progname = (unsigned char *) argv[0];
1457     ///// Parse command line arguments
1458     i = 1;
1459 #define ENSUREARGS(n) if ( argc < i + n ) usage()
1460     ENSUREARGS( 1 );
1461     // First: optional -v, -vv or -vvv
1462     if ( strncmp( "-v", argv[i], 2 ) == 0 ) {
1463         if ( strncmp( "-v", argv[i], 3 ) == 0 ) {
1464             verbose = 1;
1465         } else if ( strncmp( "-vv", argv[i], 4 ) == 0 ) {
1466             verbose = 2;
1467         } else if ( strncmp( "-vvv", argv[i], 5 ) == 0 ) {
1468             verbose = 3;
1469         } else {
1470             usage();
1471         }
1472         i++;
1473         ENSUREARGS( 1 );
1474     }
1475     if ( strncmp( "-tpg", argv[i], 4 ) == 0 ) {
1476         tpg_quirk = 1;
1477         i++;
1478         ENSUREARGS( 1 );
1479     }
1480     // then: optional -4
1481     if ( strncmp( "-4", argv[i], 2 ) == 0 ) {
1482         udp6 = 0;
1483         i++;
1484         ENSUREARGS( 1 );
1485     }
1486     // then: optional -B buffers
1487     if ( strncmp( "-B", argv[i], 2 ) == 0 ) {
1488         ENSUREARGS( 2 );
1489         if ( parse_buffers_count( argv[i+1] ) ) {
1490             usage();
1491         }
1492         i += 2;
1493         ENSUREARGS( 1 );
1494     }
1495     // then: optional -T threads
1496     if ( strncmp( "-T", argv[i], 2 ) == 0 ) {
1497         ENSUREARGS( 2 );
1498         if ( parse_threads_count( argv[i+1] ) ) {
1499             usage();
1500         }
1501         i += 2;
1502         ENSUREARGS( 1 );
1503     }
1504     // then: optional -m mcast
1505     if ( strncmp( "-m", argv[i], 2 ) == 0 ) {
1506         ENSUREARGS( 2 );
1507         if ( parse_mcast( argv[i+1] ) ) {
1508             usage();
1509         }
1510         i += 2;
1511         ENSUREARGS( 1 );
1512     }
1513     // then: optional -t tap
1514     if ( strncmp( "-t", argv[i], 2 ) == 0 ) {
1515         ENSUREARGS( 2 );
1516         tap = argv[i+1];
1517         i += 2;
1518         ENSUREARGS( 1 );
1519     }
1520     // Then optional source address for UDP
1521     if ( strncmp( "-S", argv[i], 2 ) == 0 ) {
1522         ENSUREARGS( 2 );
1523         if ( parse_udp_source( argv[i+1] ) ) {
1524             usage();
1525         }
1526         i += 2;
1527         ENSUREARGS( 1 );
1528     }
1529     // then: required port
1530     if ( sscanf( argv[i++], "%d", &port ) != 1 ) {
1531         fprintf( stderr, "Bad local port: %s\n", argv[i-1] );
1532         usage();
1533     }
1534     // then: any number of allowed remotes
1535     struct Allowed *last_allowed = 0;
1536     for ( ; i < argc; i++ ) {
1537         if ( last_allowed ) {
1538             // optionally adding ignored interfaces
1539             if ( strncmp( "-i", argv[i], 2 ) == 0 ) {
1540                 ENSUREARGS( 2 );
1541                 if ( parse_ignored_interfaces( argv[i+1], last_allowed ) ) {
1542                     usage();
1543                 }
1544                 i += 1;
1545                 continue;
1546             }
1547         }
1548         if ( ( last_allowed = add_allowed( argv[i] ) ) == 0 ) {
1549             fprintf( stderr, "Cannot load remote %s. Exiting.\n", argv[i] );
1550             exit( 1 );
1551         }
1552     }
1553     // end of command line parsing
1554
1555     // Initialize buffers and threads
1556     if ( threads_count == 0 ) {
1557         threads_count = 5;
1558     }
1559     if ( buffers_count < threads_count ) {
1560         buffers_count = 2 * threads_count;
1561     }
1562     todolist_initialize( buffers_count, threads_count );
1563
1564     // Set up the tap/stdio channel
1565     if ( tap ) {
1566         // set up the nominated tap
1567         if ( strcmp( "-", tap ) ) { // Unless "-"
1568             tap_fd = tun_alloc( tap, IFF_TAP | IFF_NO_PI );
1569             if ( tap_fd < 0 ) {
1570                 fprintf( stderr, "Error connecting to interface %s!\n", tap);
1571                 exit(1);
1572             }
1573             VERBOSEOUT( "Using tap %s at %d\n", tap, tap_fd );
1574             stdio = 0;
1575             // pretend a zero packet on the tap, for initializing.
1576             initialize_tap();
1577         } else {
1578             // set up for stdin/stdout local traffix
1579             setbuf( stdout, NULL ); // No buffering on stdout.
1580             tap_fd = 0; // actually stdin
1581             stdio = 1;
1582         }
1583     } else {
1584         stdio = 0;
1585     }
1586     // Set up the multicast UDP channel (all interfaces)
1587     if ( mcast.group.imr_multiaddr.s_addr ) {
1588         setup_mcast();
1589         unsigned char *x = (unsigned char *) &mcast.group.imr_multiaddr.s_addr;
1590         VERBOSEOUT( "Using multicast %s:%d at %d\n",
1591                     inet_nmtoa( x, 4 ), ntohs( mcast.sock.in4.sin_port ),
1592                     mcast.fd );
1593     }
1594     // Set up the unicast UPD channel (all interfaces)
1595     if ( udp6 == 0 ) {
1596         // set up ipv4 socket
1597         if ( ( udp_fd = socket( AF_INET, SOCK_DGRAM, 0 ) ) == 0 ) {
1598             perror( "creating socket");
1599             exit(1);
1600         }
1601         struct sockaddr_in udp_addr = {
1602             .sin_family = AF_INET,
1603             .sin_port = htons( port ),
1604         };
1605         if ( udp_source.family == 0 ) {
1606             udp_addr.sin_addr.s_addr = htonl( INADDR_ANY );
1607         } else {
1608             udp_addr.sin_addr.s_addr = *((uint32_t*) udp_source.address);
1609         }
1610         if ( bind( udp_fd, (struct sockaddr*) &udp_addr, sizeof(udp_addr))) {
1611             fprintf( stderr, "Error binding socket!\n");
1612             exit(1);
1613         }
1614         VERBOSEOUT( "Using ipv4 UDP at %d\n", udp_fd );
1615     } else {
1616         // set up ipv6 socket
1617         if ( ( udp_fd = socket( AF_INET6, SOCK_DGRAM, 0 ) ) == 0 ) {
1618             perror( "creating socket");
1619             exit(1);
1620         }
1621         struct sockaddr_in6 udp6_addr = {
1622             .sin6_family = AF_INET6,
1623             .sin6_port = htons( port ),
1624         };
1625         memcpy( udp6_addr.sin6_addr.s6_addr, udp_source.address, 16 );
1626         if ( bind( udp_fd, (struct sockaddr*) &udp6_addr, sizeof(udp6_addr))) {
1627             fprintf( stderr, "Error binding socket!\n");
1628             exit(1);
1629         }
1630         VERBOSEOUT( "Using ipv6 UDP at %d\n", udp_fd );
1631     }
1632     // If not using stdio for local traffic, then stdin and stdout are
1633     // closed here, so as to avoid that any other traffic channel gets
1634     // 0 or 1 as its file descriptor. Note: stderr (2) is left open.
1635     if ( ! stdio ) {
1636         close( 0 );
1637         close( 1 );
1638     }
1639     VERBOSE2OUT( "Socket loop tap=%d mcast=%d udp=%d\n",
1640                  tap_fd, mcast.fd, udp_fd );
1641
1642     // Handle packets
1643     ReaderData udp_reader = { .fd = udp_fd };
1644     pthread_create( &thread, 0, doreadUDP, &udp_reader );
1645
1646     if ( mcast.group.imr_multiaddr.s_addr ) {
1647         ReaderData mcast_reader = { .fd = mcast.fd };
1648         pthread_create( &thread, 0, doreadUDP, &mcast_reader );
1649     }
1650
1651     if ( tap_fd || stdio ) {
1652         ReaderData tap_reader = { .fd = tap_fd };
1653         pthread_create( &thread, 0, doreadTap, &tap_reader );
1654     }
1655
1656     // Start heartbeating to uplinks
1657     for ( ;; ) {
1658         sleep( HEARTBEAT );
1659         heartbeat( udp_fd );
1660     }
1661     return 0;
1662 }