rrqnet.c

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