Packet tunneling over UDP, multiple channels
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
 
 
 
 

1604 lines
45 KiB

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