qemu/roms/ipxe/src/util/hijack.c

   1 #include <stdio.h>
   2 #include <stdlib.h>
   3 #include <unistd.h>
   4 #include <string.h>
   5 #include <stdarg.h>
   6 #include <errno.h>
   7 #include <fcntl.h>
   8 #include <libgen.h>
   9 #include <signal.h>
  10 #include <net/if.h>
  11 #include <net/ethernet.h>
  12 #include <sys/select.h>
  13 #include <sys/socket.h>
  14 #include <sys/stat.h>
  15 #include <sys/un.h>
  16 #include <syslog.h>
  17 #include <getopt.h>
  18 #include <pcap.h>
  19
  20 #define SNAPLEN 1600
  21
  22 /*
  23  * FIXME: is there a way to detect the version of the libpcap library?
  24  * Version 0.9 has pcap_inject; version 0.8 doesn't, but both report
  25  * their version number as 2.4.
  26  */
  27 #define HAVE_PCAP_INJECT 0
  28
  29 struct hijack {
  30         pcap_t *pcap;
  31         int fd;
  32         int datalink;
  33         int filtered;
  34         unsigned long rx_count;
  35         unsigned long tx_count;
  36 };
  37
  38 struct hijack_listener {
  39         struct sockaddr_un sun;
  40         int fd;
  41 };
  42
  43 struct hijack_options {
  44         char interface[IF_NAMESIZE];
  45         int daemonise;
  46 };
  47
  48 static int daemonised = 0;
  49
  50 static int signalled = 0;
  51
  52 static void flag_signalled ( int signal __attribute__ (( unused )) ) {
  53         signalled = 1;
  54 }
  55
  56 #if ! HAVE_PCAP_INJECT
  57 /**
  58  * Substitute for pcap_inject(), if this version of libpcap doesn't
  59  * have it.  Will almost certainly only work under Linux.
  60  *
  61  */
  62 int pcap_inject ( pcap_t *pcap, const void *data, size_t len ) {
  63         int fd;
  64         char *errbuf = pcap_geterr ( pcap );
  65
  66         fd = pcap_get_selectable_fd ( pcap );
  67         if ( fd < 0 ) {
  68                 snprintf ( errbuf, PCAP_ERRBUF_SIZE,
  69                            "could not get file descriptor" );
  70                 return -1;
  71         }
  72         if ( write ( fd, data, len ) != len ) {
  73                 snprintf ( errbuf, PCAP_ERRBUF_SIZE,
  74                            "could not write data: %s", strerror ( errno ) );
  75                 return -1;
  76         }
  77         return len;
  78 }
  79 #endif /* ! HAVE_PCAP_INJECT */
  80
  81 /**
  82  * Log error message
  83  *
  84  */
  85 static __attribute__ (( format ( printf, 2, 3 ) )) void
  86 logmsg ( int level, const char *format, ... ) {
  87         va_list ap;
  88
  89         va_start ( ap, format );
  90         if ( daemonised ) {
  91                 vsyslog ( ( LOG_DAEMON | level ), format, ap );
  92         } else {
  93                 vfprintf ( stderr, format, ap );
  94         }
  95         va_end ( ap );
  96 }
  97
  98 /**
  99  * Open pcap device
 100  *
 101  */
 102 static int hijack_open ( const char *interface, struct hijack *hijack ) {
 103         char errbuf[PCAP_ERRBUF_SIZE];
 104
 105         /* Open interface via pcap */
 106         errbuf[0] = '\0';
 107         hijack->pcap = pcap_open_live ( interface, SNAPLEN, 1, 0, errbuf );
 108         if ( ! hijack->pcap ) {
 109                 logmsg ( LOG_ERR, "Failed to open %s: %s\n",
 110                          interface, errbuf );
 111                 goto err;
 112         }
 113         if ( errbuf[0] )
 114                 logmsg ( LOG_WARNING, "Warning: %s\n", errbuf );
 115
 116         /* Set capture interface to non-blocking mode */
 117         if ( pcap_setnonblock ( hijack->pcap, 1, errbuf ) < 0 ) {
 118                 logmsg ( LOG_ERR, "Could not make %s non-blocking: %s\n",
 119                          interface, errbuf );
 120                 goto err;
 121         }
 122
 123         /* Get file descriptor for select() */
 124         hijack->fd = pcap_get_selectable_fd ( hijack->pcap );
 125         if ( hijack->fd < 0 ) {
 126                 logmsg ( LOG_ERR, "Cannot get selectable file descriptor "
 127                          "for %s\n", interface );
 128                 goto err;
 129         }
 130
 131         /* Get link layer type */
 132         hijack->datalink = pcap_datalink ( hijack->pcap );
 133
 134         return 0;
 135
 136  err:
 137         if ( hijack->pcap )
 138                 pcap_close ( hijack->pcap );
 139         return -1;
 140 }
 141
 142 /**
 143  * Close pcap device
 144  *
 145  */
 146 static void hijack_close ( struct hijack *hijack ) {
 147         pcap_close ( hijack->pcap );
 148 }
 149
 150 /**
 151  * Install filter for hijacked connection
 152  *
 153  */
 154 static int hijack_install_filter ( struct hijack *hijack,
 155                                    char *filter ) {
 156         struct bpf_program program;
 157
 158         /* Compile filter */
 159         if ( pcap_compile ( hijack->pcap, &program, filter, 1, 0 ) < 0 ) {
 160                 logmsg ( LOG_ERR, "could not compile filter \"%s\": %s\n",
 161                          filter, pcap_geterr ( hijack->pcap ) );
 162                 goto err_nofree;
 163         }
 164
 165         /* Install filter */
 166         if ( pcap_setfilter ( hijack->pcap, &program ) < 0 ) {
 167                 logmsg ( LOG_ERR, "could not install filter \"%s\": %s\n",
 168                          filter, pcap_geterr ( hijack->pcap ) );
 169                 goto err;
 170         }
 171
 172         logmsg ( LOG_INFO, "using filter \"%s\"\n", filter );
 173
 174         pcap_freecode ( &program );
 175         return 0;
 176
 177  err:
 178         pcap_freecode ( &program );
 179  err_nofree:
 180         return -1;
 181 }
 182
 183 /**
 184  * Set up filter for hijacked ethernet connection
 185  *
 186  */
 187 static int hijack_filter_ethernet ( struct hijack *hijack, const char *buf,
 188                                     size_t len ) {
 189         char filter[55]; /* see format string */
 190         struct ether_header *ether_header = ( struct ether_header * ) buf;
 191         unsigned char *hwaddr = ether_header->ether_shost;
 192
 193         if ( len < sizeof ( *ether_header ) )
 194                 return -1;
 195
 196         snprintf ( filter, sizeof ( filter ), "broadcast or multicast or "
 197                    "ether host %02x:%02x:%02x:%02x:%02x:%02x", hwaddr[0],
 198                    hwaddr[1], hwaddr[2], hwaddr[3], hwaddr[4], hwaddr[5] );
 199
 200         return hijack_install_filter ( hijack, filter );
 201 }
 202
 203 /**
 204  * Set up filter for hijacked connection
 205  *
 206  */
 207 static int hijack_filter ( struct hijack *hijack, const char *buf,
 208                            size_t len ) {
 209         switch ( hijack->datalink ) {
 210         case DLT_EN10MB:
 211                 return hijack_filter_ethernet ( hijack, buf, len );
 212         default:
 213                 logmsg ( LOG_ERR, "unsupported protocol %s: cannot filter\n",
 214                          ( pcap_datalink_val_to_name ( hijack->datalink ) ?
 215                            pcap_datalink_val_to_name ( hijack->datalink ) :
 216                            "UNKNOWN" ) );
 217                 /* Return success so we don't get called again */
 218                 return 0;
 219         }
 220 }
 221
 222 /**
 223  * Forward data from hijacker
 224  *
 225  */
 226 static ssize_t forward_from_hijacker ( struct hijack *hijack, int fd ) {
 227         char buf[SNAPLEN];
 228         ssize_t len;
 229
 230         /* Read packet from hijacker */
 231         len = read ( fd, buf, sizeof ( buf ) );
 232         if ( len < 0 ) {
 233                 logmsg ( LOG_ERR, "read from hijacker failed: %s\n",
 234                          strerror ( errno ) );
 235                 return -1;
 236         }
 237         if ( len == 0 )
 238                 return 0;
 239
 240         /* Set up filter if not already in place */
 241         if ( ! hijack->filtered ) {
 242                 if ( hijack_filter ( hijack, buf, len ) == 0 )
 243                         hijack->filtered = 1;
 244         }
 245
 246         /* Transmit packet to network */
 247         if ( pcap_inject ( hijack->pcap, buf, len ) != len ) {
 248                 logmsg ( LOG_ERR, "write to hijacked port failed: %s\n",
 249                          pcap_geterr ( hijack->pcap ) );
 250                 return -1;
 251         }
 252
 253         hijack->tx_count++;
 254         return len;
 255 };
 256
 257 /**
 258  * Forward data to hijacker
 259  *
 260  */
 261 static ssize_t forward_to_hijacker ( int fd, struct hijack *hijack ) {
 262         struct pcap_pkthdr *pkt_header;
 263         const unsigned char *pkt_data;
 264         ssize_t len;
 265
 266         /* Receive packet from network */
 267         if ( pcap_next_ex ( hijack->pcap, &pkt_header, &pkt_data ) < 0 ) {
 268                 logmsg ( LOG_ERR, "read from hijacked port failed: %s\n",
 269                          pcap_geterr ( hijack->pcap ) );
 270                 return -1;
 271         }
 272         if ( pkt_header->caplen != pkt_header->len ) {
 273                 logmsg ( LOG_ERR, "read partial packet (%d of %d bytes)\n",
 274                          pkt_header->caplen, pkt_header->len );
 275                 return -1;
 276         }
 277         if ( pkt_header->caplen == 0 )
 278                 return 0;
 279         len = pkt_header->caplen;
 280
 281         /* Write packet to hijacker */
 282         if ( write ( fd, pkt_data, len ) != len ) {
 283                 logmsg ( LOG_ERR, "write to hijacker failed: %s\n",
 284                          strerror ( errno ) );
 285                 return -1;
 286         }
 287
 288         hijack->rx_count++;
 289         return len;
 290 };
 291
 292
 293 /**
 294  * Run hijacker
 295  *
 296  */
 297 static int run_hijacker ( const char *interface, int fd ) {
 298         struct hijack hijack;
 299         fd_set fdset;
 300         int max_fd;
 301         ssize_t len;
 302
 303         logmsg ( LOG_INFO, "new connection for %s\n", interface );
 304
 305         /* Open connection to network */
 306         memset ( &hijack, 0, sizeof ( hijack ) );
 307         if ( hijack_open ( interface, &hijack ) < 0 )
 308                 goto err;
 309
 310         /* Do the forwarding */
 311         max_fd = ( ( fd > hijack.fd ) ? fd : hijack.fd );
 312         while ( 1 ) {
 313                 /* Wait for available data */
 314                 FD_ZERO ( &fdset );
 315                 FD_SET ( fd, &fdset );
 316                 FD_SET ( hijack.fd, &fdset );
 317                 if ( select ( ( max_fd + 1 ), &fdset, NULL, NULL, 0 ) < 0 ) {
 318                         logmsg ( LOG_ERR, "select failed: %s\n",
 319                                  strerror ( errno ) );
 320                         goto err;
 321                 }
 322                 if ( FD_ISSET ( fd, &fdset ) ) {
 323                         len = forward_from_hijacker ( &hijack, fd );
 324                         if ( len < 0 )
 325                                 goto err;
 326                         if ( len == 0 )
 327                                 break;
 328                 }
 329                 if ( FD_ISSET ( hijack.fd, &fdset ) ) {
 330                         len = forward_to_hijacker ( fd, &hijack );
 331                         if ( len < 0 )
 332                                 goto err;
 333                         if ( len == 0 )
 334                                 break;
 335                 }
 336         }
 337
 338         hijack_close ( &hijack );
 339         logmsg ( LOG_INFO, "closed connection for %s\n", interface );
 340         logmsg ( LOG_INFO, "received %ld packets, sent %ld packets\n",
 341                  hijack.rx_count, hijack.tx_count );
 342
 343         return 0;
 344
 345  err:
 346         if ( hijack.pcap )
 347                 hijack_close ( &hijack );
 348         return -1;
 349 }
 350
 351 /**
 352  * Open listener socket
 353  *
 354  */
 355 static int open_listener ( const char *interface,
 356                            struct hijack_listener *listener ) {
 357
 358         /* Create socket */
 359         listener->fd = socket ( PF_UNIX, SOCK_SEQPACKET, 0 );
 360         if ( listener->fd < 0 ) {
 361                 logmsg ( LOG_ERR, "Could not create socket: %s\n",
 362                          strerror ( errno ) );
 363                 goto err;
 364         }
 365
 366         /* Bind to local filename */
 367         listener->sun.sun_family = AF_UNIX,
 368         snprintf ( listener->sun.sun_path, sizeof ( listener->sun.sun_path ),
 369                    "/var/run/hijack-%s", interface );
 370         if ( bind ( listener->fd, ( struct sockaddr * ) &listener->sun,
 371                     sizeof ( listener->sun ) ) < 0 ) {
 372                 logmsg ( LOG_ERR, "Could not bind socket to %s: %s\n",
 373                          listener->sun.sun_path, strerror ( errno ) );
 374                 goto err;
 375         }
 376
 377         /* Set as a listening socket */
 378         if ( listen ( listener->fd, 0 ) < 0 ) {
 379                 logmsg ( LOG_ERR, "Could not listen to %s: %s\n",
 380                          listener->sun.sun_path, strerror ( errno ) );
 381                 goto err;
 382         }
 383
 384         return 0;
 385
 386  err:
 387         if ( listener->fd >= 0 )
 388                 close ( listener->fd );
 389         return -1;
 390 }
 391
 392 /**
 393  * Listen on listener socket
 394  *
 395  */
 396 static int listen_for_hijackers ( struct hijack_listener *listener,
 397                                   const char *interface ) {
 398         int fd;
 399         pid_t child;
 400         int rc;
 401
 402         logmsg ( LOG_INFO, "Listening on %s\n", listener->sun.sun_path );
 403
 404         while ( ! signalled ) {
 405                 /* Accept new connection, interruptibly */
 406                 siginterrupt ( SIGINT, 1 );
 407                 siginterrupt ( SIGHUP, 1 );
 408                 fd = accept ( listener->fd, NULL, 0 );
 409                 siginterrupt ( SIGINT, 0 );
 410                 siginterrupt ( SIGHUP, 0 );
 411                 if ( fd < 0 ) {
 412                         if ( errno == EINTR ) {
 413                                 continue;
 414                         } else {
 415                                 logmsg ( LOG_ERR, "accept failed: %s\n",
 416                                          strerror ( errno ) );
 417                                 goto err;
 418                         }
 419                 }
 420
 421                 /* Fork child process */
 422                 child = fork();
 423                 if ( child < 0 ) {
 424                         logmsg ( LOG_ERR, "fork failed: %s\n",
 425                                  strerror ( errno ) );
 426                         goto err;
 427                 }
 428                 if ( child == 0 ) {
 429                         /* I am the child; run the hijacker */
 430                         rc = run_hijacker ( interface, fd );
 431                         close ( fd );
 432                         exit ( rc );
 433                 }
 434
 435                 close ( fd );
 436         }
 437
 438         logmsg ( LOG_INFO, "Stopped listening on %s\n",
 439                  listener->sun.sun_path );
 440         return 0;
 441
 442  err:
 443         if ( fd >= 0 )
 444                 close ( fd );
 445         return -1;
 446 }
 447
 448 /**
 449  * Close listener socket
 450  *
 451  */
 452 static void close_listener ( struct hijack_listener *listener ) {
 453         close ( listener->fd );
 454         unlink ( listener->sun.sun_path );
 455 }
 456
 457 /**
 458  * Print usage
 459  *
 460  */
 461 static void usage ( char **argv ) {
 462         logmsg ( LOG_ERR,
 463                  "Usage: %s [options]\n"
 464                  "\n"
 465                  "Options:\n"
 466                  "  -h|--help               Print this help message\n"
 467                  "  -i|--interface intf     Use specified network interface\n"
 468                  "  -n|--nodaemon           Run in foreground\n",
 469                  argv[0] );
 470 }
 471
 472 /**
 473  * Parse command-line options
 474  *
 475  */
 476 static int parse_options ( int argc, char **argv,
 477                            struct hijack_options *options ) {
 478         static struct option long_options[] = {
 479                 { "interface", 1, NULL, 'i' },
 480                 { "nodaemon", 0, NULL, 'n' },
 481                 { "help", 0, NULL, 'h' },
 482                 { },
 483         };
 484         int c;
 485
 486         /* Set default options */
 487         memset ( options, 0, sizeof ( *options ) );
 488         strncpy ( options->interface, "eth0", sizeof ( options->interface ) );
 489         options->daemonise = 1;
 490
 491         /* Parse command-line options */
 492         while ( 1 ) {
 493                 int option_index = 0;
 494
 495                 c = getopt_long ( argc, argv, "i:hn", long_options,
 496                                   &option_index );
 497                 if ( c < 0 )
 498                         break;
 499
 500                 switch ( c ) {
 501                 case 'i':
 502                         strncpy ( options->interface, optarg,
 503                                   sizeof ( options->interface ) );
 504                         break;
 505                 case 'n':
 506                         options->daemonise = 0;
 507                         break;
 508                 case 'h':
 509                         usage( argv );
 510                         return -1;
 511                 case '?':
 512                         /* Unrecognised option */
 513                         return -1;
 514                 default:
 515                         logmsg ( LOG_ERR, "Unrecognised option '-%c'\n", c );
 516                         return -1;
 517                 }
 518         }
 519
 520         /* Check there's nothing left over on the command line */
 521         if ( optind != argc ) {
 522                 usage ( argv );
 523                 return -1;
 524         }
 525
 526         return 0;
 527 }
 528
 529 /**
 530  * Daemonise
 531  *
 532  */
 533 static int daemonise ( const char *interface ) {
 534         char pidfile[16 + IF_NAMESIZE + 4]; /* "/var/run/hijack-<intf>.pid" */
 535         char pid[16];
 536         int pidlen;
 537         int fd = -1;
 538
 539         /* Daemonise */
 540         if ( daemon ( 0, 0 ) < 0 ) {
 541                 logmsg ( LOG_ERR, "Could not daemonise: %s\n",
 542                          strerror ( errno ) );
 543                 goto err;
 544         }
 545         daemonised = 1; /* Direct messages to syslog now */
 546
 547         /* Open pid file */
 548         snprintf ( pidfile, sizeof ( pidfile ), "/var/run/hijack-%s.pid",
 549                    interface );
 550         fd = open ( pidfile, ( O_WRONLY | O_CREAT | O_TRUNC ),
 551                     ( S_IRUSR | S_IWUSR | S_IRGRP | S_IROTH ) );
 552         if ( fd < 0 ) {
 553                 logmsg ( LOG_ERR, "Could not open %s for writing: %s\n",
 554                          pidfile, strerror ( errno ) );
 555                 goto err;
 556         }
 557
 558         /* Write pid to file */
 559         pidlen = snprintf ( pid, sizeof ( pid ), "%d\n", getpid() );
 560         if ( write ( fd, pid, pidlen ) != pidlen ) {
 561                 logmsg ( LOG_ERR, "Could not write %s: %s\n",
 562                          pidfile, strerror ( errno ) );
 563                 goto err;
 564         }
 565
 566         close ( fd );
 567         return 0;
 568
 569  err:
 570         if ( fd >= 0 )
 571                 close ( fd );
 572         return -1;
 573 }
 574
 575 int main ( int argc, char **argv ) {
 576         struct hijack_options options;
 577         struct hijack_listener listener;
 578         struct sigaction sa;
 579
 580         /* Parse command-line options */
 581         if ( parse_options ( argc, argv, &options ) < 0 )
 582                 exit ( 1 );
 583
 584         /* Set up syslog connection */
 585         openlog ( basename ( argv[0] ), LOG_PID, LOG_DAEMON );
 586
 587         /* Set up listening socket */
 588         if ( open_listener ( options.interface, &listener ) < 0 )
 589                 exit ( 1 );
 590
 591         /* Daemonise on demand */
 592         if ( options.daemonise ) {
 593                 if ( daemonise ( options.interface ) < 0 )
 594                         exit ( 1 );
 595         }
 596
 597         /* Avoid creating zombies */
 598         memset ( &sa, 0, sizeof ( sa ) );
 599         sa.sa_handler = SIG_IGN;
 600         sa.sa_flags = SA_RESTART | SA_NOCLDWAIT;
 601         if ( sigaction ( SIGCHLD, &sa, NULL ) < 0 ) {
 602                 logmsg ( LOG_ERR, "Could not set SIGCHLD handler: %s",
 603                          strerror ( errno ) );
 604                 exit ( 1 );
 605         }
 606
 607         /* Set 'signalled' flag on SIGINT or SIGHUP */
 608         sa.sa_handler = flag_signalled;
 609         sa.sa_flags = SA_RESTART | SA_RESETHAND;
 610         if ( sigaction ( SIGINT, &sa, NULL ) < 0 ) {
 611                 logmsg ( LOG_ERR, "Could not set SIGINT handler: %s",
 612                          strerror ( errno ) );
 613                 exit ( 1 );
 614         }
 615         if ( sigaction ( SIGHUP, &sa, NULL ) < 0 ) {
 616                 logmsg ( LOG_ERR, "Could not set SIGHUP handler: %s",
 617                          strerror ( errno ) );
 618                 exit ( 1 );
 619         }
 620
 621         /* Listen for hijackers */
 622         if ( listen_for_hijackers ( &listener, options.interface ) < 0 )
 623                 exit ( 1 );
 624
 625         close_listener ( &listener );
 626
 627         return 0;
 628 }