rrqnet.c

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