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