src/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
  16 #include "utils.h"
  17
  18 static void usage(FILE *out)
  19 {
  20     fprintf(out, "usage: mctap [-hd] [-D TAPNAME] MCASTGROUP PORT\n");
  21 }
  22
  23 static __attribute__ ((unused)) char *formataddress(struct sockaddr *arg, socklen_t arglen)
  24 {
  25     struct sockaddr_in *ipv4;
  26     struct sockaddr_in6 *ipv6;
  27     static char *ret = NULL;
  28     char buf[1024];
  29
  30     if(ret != NULL)
  31         free(ret);
  32     ret = NULL;
  33     switch(arg->sa_family)
  34     {
  35     case AF_UNIX:
  36         ret = sstrdup("Unix socket");
  37         break;
  38     case AF_INET:
  39         ipv4 = (struct sockaddr_in *)arg;
  40         if(inet_ntop(AF_INET, &ipv4->sin_addr, buf, sizeof(buf)) == NULL)
  41             return(NULL);
  42         ret = sprintf2("%s:%i", buf, (int)ntohs(ipv4->sin_port));
  43         break;
  44     case AF_INET6:
  45         ipv6 = (struct sockaddr_in6 *)arg;
  46         if(inet_ntop(AF_INET6, &ipv6->sin6_addr, buf, sizeof(buf)) == NULL)
  47             return(NULL);
  48         ret = sprintf2("[%s]:%i", buf, (int)ntohs(ipv6->sin6_port));
  49         break;
  50     default:
  51         errno = EPFNOSUPPORT;
  52         break;
  53     }
  54     return(ret);
  55 }
  56
  57 static int mkmcastsk4(struct in_addr group, int port)
  58 {
  59     int fd;
  60     int soval;
  61     struct sockaddr_in nm;
  62     struct ip_mreqn mreq;
  63
  64     fd = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
  65     soval = 1;
  66     if(setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &soval, sizeof(soval)))
  67         return(-1);
  68     memset(&nm, 0, sizeof(nm));
  69     nm.sin_family = AF_INET;
  70     nm.sin_port = htons(port);
  71     if(bind(fd, (struct sockaddr *)&nm, sizeof(nm)))
  72         return(-1);
  73     memset(&mreq, 0, sizeof(mreq));
  74     mreq.imr_multiaddr = group;
  75     if(setsockopt(fd, SOL_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq)))
  76         return(-1);
  77     soval = 1;
  78     if(setsockopt(fd, SOL_IP, IP_MULTICAST_LOOP, &soval, sizeof(soval)))
  79         return(-1);
  80     return(fd);
  81 }
  82
  83 static __attribute__ ((unused)) void test(int fd)
  84 {
  85     char buf[65536];
  86     int i, ret;
  87     struct pollfd pfd;
  88
  89     while(1) {
  90         pfd.fd = fd;
  91         pfd.events = POLLIN;
  92         ret = poll(&pfd, 1, -1);
  93         if(ret < 0) {
  94             fprintf(stderr, "mctap: poll: %s\n", strerror(errno));
  95             exit(1);
  96         }
  97         if(pfd.revents) {
  98             ret = read(fd, buf, sizeof(buf));
  99             if(ret < 0) {
 100                 fprintf(stderr, "mctap: read: %s\n", strerror(errno));
 101                 exit(1);
 102             }
 103             for(i = 0; i < ret; i++) {
 104                 printf("%02x ", (unsigned char)buf[i]);
 105                 if(i % 20 == 19)
 106                     putchar(10);
 107             }
 108             putchar(10);
 109         }
 110     }
 111 }
 112
 113 static void bridge(int sock, int tap, struct sockaddr *dst, socklen_t dstlen)
 114 {
 115     char buf[65536];
 116     int ret;
 117     struct pollfd pfds[2];
 118     struct tun_pi pi;
 119
 120     fcntl(sock, F_SETFL, fcntl(sock, F_GETFL) | O_NONBLOCK);
 121     fcntl(tap, F_SETFL, fcntl(tap, F_GETFL) | O_NONBLOCK);
 122     while(1) {
 123         pfds[0].fd = sock;
 124         pfds[0].events = POLLIN;
 125         pfds[1].fd = tap;
 126         pfds[1].events = POLLIN;
 127         ret = poll(pfds, 2, -1);
 128         if(ret < 0) {
 129             if(errno != EINTR) {
 130                 syslog(LOG_ERR, "mctap: poll: %s", strerror(errno));
 131                 exit(1);
 132             }
 133             continue;
 134         }
 135         if(pfds[0].revents) {
 136             ret = read(sock, buf, sizeof(buf));
 137             if(ret < 0) {
 138                 if((errno != EINTR) && (errno != EAGAIN)) {
 139                     syslog(LOG_ERR, "mctap: mcast packet: %s", strerror(errno));
 140                     exit(1);
 141                 }
 142             } else {
 143                 if(sizeof(buf) - ret < sizeof(pi)) {
 144                     /* Drop */
 145                 } else {
 146                     memmove(buf + sizeof(pi), buf, ret);
 147                     pi.flags = 0;
 148                     pi.proto = 0;
 149                     memcpy(buf, &pi, sizeof(pi));
 150                     write(tap, buf, sizeof(pi) + ret);
 151                 }
 152             }
 153         }
 154         if(pfds[1].revents) {
 155             ret = read(tap, buf, sizeof(buf));
 156             if(ret < 0) {
 157                 if((errno != EINTR) && (errno != EAGAIN)) {
 158                     syslog(LOG_ERR, "mctap: mcast packet: %s", strerror(errno));
 159                     exit(1);
 160                 }
 161             } else {
 162                 if(ret < sizeof(pi)) {
 163                     /* Drop */
 164                 } else {
 165                     memcpy(&pi, buf, sizeof(pi));
 166                     if(pi.flags & TUN_PKT_STRIP) {
 167                         /* Drop */
 168                     } else {
 169                         sendto(sock, buf + sizeof(pi), ret - sizeof(pi), 0, dst, dstlen);
 170                     }
 171                 }
 172             }
 173         }
 174     }
 175 }
 176
 177 static int maketap(char *name)
 178 {
 179     int fd;
 180     struct ifreq rb;
 181
 182     if((fd = open("/dev/net/tun", O_RDWR)) < 0)
 183         return(-1);
 184     memset(&rb, 0, sizeof(rb));
 185     rb.ifr_flags = IFF_TAP;
 186     strncpy(rb.ifr_name, name, IFNAMSIZ);
 187     if(ioctl(fd, TUNSETIFF, &rb))
 188         return(-1);
 189     return(fd);
 190 }
 191
 192 int main(int argc, char **argv)
 193 {
 194     int c;
 195     int sock, tap;
 196     struct in_addr group;
 197     int port;
 198     char *tapname;
 199     int daemonize;
 200     struct sockaddr_in dst;
 201
 202     tapname = "mctap";
 203     daemonize = 0;
 204     while((c = getopt(argc, argv, "hD:d")) >= 0) {
 205         switch(c) {
 206         case 'D':
 207             tapname = optarg;
 208             break;
 209         case 'd':
 210             daemonize = 1;
 211             break;
 212         case 'h':
 213             usage(stdout);
 214             return(0);
 215         default:
 216             usage(stderr);
 217             exit(1);
 218         }
 219     }
 220     if(argc - optind < 2) {
 221         usage(stderr);
 222         exit(1);
 223     }
 224     if(!inet_aton(argv[optind], &group)) {
 225         fprintf(stderr, "mctap: invalid group address: %s\n", argv[optind]);
 226         exit(1);
 227     }
 228     port = atoi(argv[optind + 1]);
 229     if((sock = mkmcastsk4(group, port)) < 0) {
 230         fprintf(stderr, "mctap: could not create multicast socket: %s\n", strerror(errno));
 231         exit(1);
 232     }
 233     if((tap = maketap(tapname)) < 0) {
 234         fprintf(stderr, "mctap: could not create TAP device: %s\n", strerror(errno));
 235         exit(1);
 236     }
 237     openlog(sprintf2("mctap-%s", tapname), LOG_PID, LOG_DAEMON);
 238
 239     if(daemonize)
 240         daemon(0, 0);
 241
 242     memset(&dst, 0, sizeof(dst));
 243     dst.sin_family = AF_INET;
 244     dst.sin_addr = group;
 245     dst.sin_port = htons(port);
 246     bridge(sock, tap, (struct sockaddr *)&dst, sizeof(dst));
 247     return(0);
 248 }