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