rrqnet.c

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