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