mctap.c

   1 #include <stdlib.h>
   2 #include <stdio.h>
   3 #include <string.h>
   4 #include <unistd.h>
   5 #include <sys/socket.h>
   6 #include <netinet/in.h>
   7 #include <arpa/inet.h>
   8 #include <errno.h>
   9 #include <net/if.h>
  10 #include <linux/if_tun.h>
  11 #include <fcntl.h>
  12 #include <sys/ioctl.h>
  13 #include <sys/poll.h>
  14 #include <syslog.h>
  15 #include <signal.h>
  16
  17 #include "utils.h"
  18
  19 static int quit = 0;
  20 static unsigned char macaddr[6];
  21
  22 static void usage(FILE *out)
  23 {
  24     fprintf(out, "usage: mctap [-hdpk] [-P PIDFILE] [-D TAPNAME] MCASTGROUP PORT\n");
  25 }
  26
  27 static __attribute__ ((unused)) char *formataddress(struct sockaddr *arg, socklen_t arglen)
  28 {
  29     struct sockaddr_in *ipv4;
  30     struct sockaddr_in6 *ipv6;
  31     static char *ret = NULL;
  32     char buf[1024];
  33
  34     if(ret != NULL)
  35         free(ret);
  36     ret = NULL;
  37     switch(arg->sa_family)
  38     {
  39     case AF_UNIX:
  40         ret = sstrdup("Unix socket");
  41         break;
  42     case AF_INET:
  43         ipv4 = (struct sockaddr_in *)arg;
  44         if(inet_ntop(AF_INET, &ipv4->sin_addr, buf, sizeof(buf)) == NULL)
  45             return(NULL);
  46         ret = sprintf2("%s:%i", buf, (int)ntohs(ipv4->sin_port));
  47         break;
  48     case AF_INET6:
  49         ipv6 = (struct sockaddr_in6 *)arg;
  50         if(inet_ntop(AF_INET6, &ipv6->sin6_addr, buf, sizeof(buf)) == NULL)
  51             return(NULL);
  52         ret = sprintf2("[%s]:%i", buf, (int)ntohs(ipv6->sin6_port));
  53         break;
  54     default:
  55         errno = EPFNOSUPPORT;
  56         break;
  57     }
  58     return(ret);
  59 }
  60
  61 static int mkmcastsk4(struct in_addr group, int port)
  62 {
  63     int fd;
  64     int soval;
  65     struct sockaddr_in nm;
  66     struct ip_mreqn mreq;
  67
  68     fd = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
  69     soval = 1;
  70     if(setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &soval, sizeof(soval)))
  71         return(-1);
  72     memset(&nm, 0, sizeof(nm));
  73     nm.sin_family = AF_INET;
  74     nm.sin_port = htons(port);
  75     if(bind(fd, (struct sockaddr *)&nm, sizeof(nm)))
  76         return(-1);
  77     memset(&mreq, 0, sizeof(mreq));
  78     mreq.imr_multiaddr = group;
  79     if(setsockopt(fd, SOL_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq)))
  80         return(-1);
  81     soval = 1;
  82     if(setsockopt(fd, SOL_IP, IP_MULTICAST_LOOP, &soval, sizeof(soval)))
  83         return(-1);
  84     return(fd);
  85 }
  86
  87 static __attribute__ ((unused)) void test(int fd)
  88 {
  89     char buf[65536];
  90     int i, ret;
  91     struct pollfd pfd;
  92
  93     while(1) {
  94         pfd.fd = fd;
  95         pfd.events = POLLIN;
  96         ret = poll(&pfd, 1, -1);
  97         if(ret < 0) {
  98             fprintf(stderr, "mctap: poll: %s\n", strerror(errno));
  99             exit(1);
 100         }
 101         if(pfd.revents) {
 102             ret = read(fd, buf, sizeof(buf));
 103             if(ret < 0) {
 104                 fprintf(stderr, "mctap: read: %s\n", strerror(errno));
 105                 exit(1);
 106             }
 107             for(i = 0; i < ret; i++) {
 108                 printf("%02x ", (unsigned char)buf[i]);
 109                 if(i % 20 == 19)
 110                     putchar(10);
 111             }
 112             putchar(10);
 113         }
 114     }
 115 }
 116
 117 static void bridge(int sock, int tap, struct sockaddr *dst, socklen_t dstlen)
 118 {
 119     char buf[65536];
 120     int ret;
 121     struct pollfd pfds[2];
 122     struct tun_pi pi;
 123
 124     fcntl(sock, F_SETFL, fcntl(sock, F_GETFL) | O_NONBLOCK);
 125     fcntl(tap, F_SETFL, fcntl(tap, F_GETFL) | O_NONBLOCK);
 126     while(!quit) {
 127         pfds[0].fd = sock;
 128         pfds[0].events = POLLIN;
 129         pfds[1].fd = tap;
 130         pfds[1].events = POLLIN;
 131         ret = poll(pfds, 2, -1);
 132         if(ret < 0) {
 133             if(errno != EINTR) {
 134                 syslog(LOG_ERR, "mctap: poll: %s", strerror(errno));
 135                 exit(1);
 136             }
 137             continue;
 138         }
 139         if(pfds[0].revents) {
 140             ret = read(sock, buf, sizeof(buf));
 141             if(ret < 0) {
 142                 if((errno != EINTR) && (errno != EAGAIN)) {
 143                     syslog(LOG_ERR, "mctap: mcast packet: %s", strerror(errno));
 144                     exit(1);
 145                 }
 146             } else {
 147                 if(sizeof(buf) - ret < sizeof(pi)) {
 148                     /* Drop */
 149                 } else if((ret < 12) || !memcmp(macaddr, buf + 6, 6)) {
 150                     /* Drop looped back */
 151                 } else {
 152                     memmove(buf + sizeof(pi), buf, ret);
 153                     pi.flags = 0;
 154                     pi.proto = 0;
 155                     memcpy(buf, &pi, sizeof(pi));
 156                     write(tap, buf, sizeof(pi) + ret);
 157                 }
 158             }
 159         }
 160         if(pfds[1].revents) {
 161             ret = read(tap, buf, sizeof(buf));
 162             if(ret < 0) {
 163                 if((errno != EINTR) && (errno != EAGAIN)) {
 164                     syslog(LOG_ERR, "mctap: mcast packet: %s", strerror(errno));
 165                     exit(1);
 166                 }
 167             } else {
 168                 if(ret < sizeof(pi)) {
 169                     /* Drop */
 170                 } else {
 171                     memcpy(&pi, buf, sizeof(pi));
 172                     if(pi.flags & TUN_PKT_STRIP) {
 173                         /* Drop */
 174                     } else {
 175                         sendto(sock, buf + sizeof(pi), ret - sizeof(pi), 0, dst, dstlen);
 176                     }
 177                 }
 178             }
 179         }
 180     }
 181 }
 182
 183 static int maketap(char *name)
 184 {
 185     int fd;
 186     struct ifreq rb;
 187
 188     if((fd = open("/dev/net/tun", O_RDWR)) < 0)
 189         return(-1);
 190     memset(&rb, 0, sizeof(rb));
 191     rb.ifr_flags = IFF_TAP;
 192     strncpy(rb.ifr_name, name, IFNAMSIZ);
 193     if(ioctl(fd, TUNSETIFF, &rb))
 194         return(-1);
 195     if(ioctl(fd, SIOCGIFHWADDR, &rb))
 196         return(-1);
 197     memcpy(macaddr, rb.ifr_hwaddr.sa_data, 6);
 198     return(fd);
 199 }
 200
 201 static void sighand(int sig)
 202 {
 203     switch(sig) {
 204     case SIGINT:
 205     case SIGTERM:
 206         quit = 1;
 207         break;
 208     case SIGHUP:
 209         break;
 210     }
 211 }
 212
 213 static void killrunning(char *pidfile)
 214 {
 215     FILE *pidfd;
 216     int pid;
 217
 218     if((pidfd = fopen(pidfile, "r")) == NULL) {
 219         fprintf(stderr, "mctab -k: could not read PID file %s: %s\n", pidfile, strerror(errno));
 220         exit(1);
 221     }
 222     fscanf(pidfd, "%i", &pid);
 223     if(kill(pid, SIGTERM)) {
 224         fprintf(stderr, "mctab -k: could not kill %i: %s\n", pid, strerror(errno));
 225         exit(1);
 226     }
 227     fclose(pidfd);
 228 }
 229
 230 int main(int argc, char **argv)
 231 {
 232     int c;
 233     int sock, tap;
 234     struct in_addr group;
 235     int port;
 236     char *tapname;
 237     char *pidfile;
 238     int daemonize, killold;
 239     struct sockaddr_in dst;
 240     FILE *pidfd;
 241
 242     tapname = "mctap";
 243     daemonize = killold = 0;
 244     pidfile = NULL;
 245     while((c = getopt(argc, argv, "hD:dpP:k")) >= 0) {
 246         switch(c) {
 247         case 'D':
 248             tapname = optarg;
 249             break;
 250         case 'd':
 251             daemonize = 1;
 252             break;
 253         case 'p':
 254             pidfile = (void *)-1;
 255             break;
 256         case 'P':
 257             pidfile = optarg;
 258             break;
 259         case 'k':
 260             killold = 1;
 261             if(pidfile == NULL)
 262                 pidfile = (void *)-1;
 263             break;
 264         case 'h':
 265             usage(stdout);
 266             return(0);
 267         default:
 268             usage(stderr);
 269             exit(1);
 270         }
 271     }
 272     if(pidfile == (void *)-1)
 273         pidfile = sprintf2("/var/run/mctap.%s.pid", tapname);
 274     if(killold) {
 275         killrunning(pidfile);
 276         return(0);
 277     }
 278     if(argc - optind < 2) {
 279         usage(stderr);
 280         exit(1);
 281     }
 282     if(!inet_aton(argv[optind], &group)) {
 283         fprintf(stderr, "mctap: invalid group address: %s\n", argv[optind]);
 284         exit(1);
 285     }
 286     port = atoi(argv[optind + 1]);
 287     if((sock = mkmcastsk4(group, port)) < 0) {
 288         fprintf(stderr, "mctap: could not create multicast socket: %s\n", strerror(errno));
 289         exit(1);
 290     }
 291     if((tap = maketap(tapname)) < 0) {
 292         fprintf(stderr, "mctap: could not create TAP device: %s\n", strerror(errno));
 293         exit(1);
 294     }
 295     openlog(sprintf2("mctap-%s", tapname), LOG_PID, LOG_DAEMON);
 296
 297     pidfd = NULL;
 298     if((pidfile != NULL) && ((pidfd = fopen(pidfile, "w")) == NULL)) {
 299         fprintf(stderr, "mctap: could not create PID file %s: %s\n", pidfile, strerror(errno));
 300         exit(1);
 301     }
 302     if(daemonize)
 303         daemon(0, 0);
 304     if(pidfd != NULL) {
 305         fprintf(pidfd, "%i\n", getpid());
 306         fclose(pidfd);
 307     }
 308
 309     signal(SIGTERM, sighand);
 310     signal(SIGINT, sighand);
 311     signal(SIGHUP, sighand);
 312
 313     memset(&dst, 0, sizeof(dst));
 314     dst.sin_family = AF_INET;
 315     dst.sin_addr = group;
 316     dst.sin_port = htons(port);
 317     syslog(LOG_INFO, "bridge created with MAC %02x:%02x:%02x:%02x:%02x:%02x", macaddr[0], macaddr[1], macaddr[2], macaddr[3], macaddr[4], macaddr[5]);
 318     bridge(sock, tap, (struct sockaddr *)&dst, sizeof(dst));
 319     syslog(LOG_INFO, "exiting");
 320
 321     if(pidfile != NULL)
 322         unlink(pidfile);
 323     return(0);
 324 }