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