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