f504e4fe4efb1941de2770cec36f42ea764d89de
[people/xl0/gpxe.git] / 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 static 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 }