[mctap.git] / src / mctap.c

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <errno.h>
#include <net/if.h>
#include <linux/if_tun.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <sys/poll.h>

#include "utils.h"

static void usage(FILE *out)
{
    fprintf(out, "usage: mctap [-h] [-D TAPNAME] MCASTGROUP PORT\n");
}

static __attribute__ ((unused)) char *formataddress(struct sockaddr *arg, socklen_t arglen)
{
    struct sockaddr_in *ipv4;
    struct sockaddr_in6 *ipv6;
    static char *ret = NULL;
    char buf[1024];
    
    if(ret != NULL)
	free(ret);
    ret = NULL;
    switch(arg->sa_family)
    {
    case AF_UNIX:
	ret = sstrdup("Unix socket");
	break;
    case AF_INET:
	ipv4 = (struct sockaddr_in *)arg;
	if(inet_ntop(AF_INET, &ipv4->sin_addr, buf, sizeof(buf)) == NULL)
	    return(NULL);
	ret = sprintf2("%s:%i", buf, (int)ntohs(ipv4->sin_port));
	break;
    case AF_INET6:
	ipv6 = (struct sockaddr_in6 *)arg;
	if(inet_ntop(AF_INET6, &ipv6->sin6_addr, buf, sizeof(buf)) == NULL)
	    return(NULL);
	ret = sprintf2("[%s]:%i", buf, (int)ntohs(ipv6->sin6_port));
	break;
    default:
	errno = EPFNOSUPPORT;
	break;
    }
    return(ret);
}

static int mkmcastsk4(struct in_addr group, int port)
{
    int fd;
    int soval;
    struct sockaddr_in nm;
    struct ip_mreqn mreq;
    
    fd = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
    soval = 1;
    if(setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &soval, sizeof(soval)))
	return(-1);
    memset(&nm, 0, sizeof(nm));
    nm.sin_family = AF_INET;
    nm.sin_port = htons(port);
    if(bind(fd, (struct sockaddr *)&nm, sizeof(nm)))
	return(-1);
    memset(&mreq, 0, sizeof(mreq));
    mreq.imr_multiaddr = group;
    if(setsockopt(fd, SOL_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq)))
	return(-1);
    soval = 1;
    if(setsockopt(fd, SOL_IP, IP_MULTICAST_LOOP, &soval, sizeof(soval)))
	return(-1);
    return(fd);
}

static __attribute__ ((unused)) void test(int fd)
{
    char buf[65536];
    int i, ret;
    struct pollfd pfd;
    
    while(1) {
	pfd.fd = fd;
	pfd.events = POLLIN;
	ret = poll(&pfd, 1, -1);
	if(ret < 0) {
	    fprintf(stderr, "mctap: poll: %s\n", strerror(errno));
	    exit(1);
	}
	if(pfd.revents) {
	    ret = read(fd, buf, sizeof(buf));
	    if(ret < 0) {
		fprintf(stderr, "mctap: read: %s\n", strerror(errno));
		exit(1);
	    }
	    for(i = 0; i < ret; i++) {
		printf("%02x ", (unsigned char)buf[i]);
		if(i % 20 == 19)
		    putchar(10);
	    }
	    putchar(10);
	}
    }
}

static void bridge(int sock, int tap, struct sockaddr *dst, socklen_t dstlen)
{
    char buf[65536];
    int ret;
    struct pollfd pfds[2];
    struct tun_pi pi;
    
    fcntl(sock, F_SETFL, fcntl(sock, F_GETFL) | O_NONBLOCK);
    fcntl(tap, F_SETFL, fcntl(tap, F_GETFL) | O_NONBLOCK);
    while(1) {
	pfds[0].fd = sock;
	pfds[0].events = POLLIN;
	pfds[1].fd = tap;
	pfds[1].events = POLLIN;
	ret = poll(pfds, 2, -1);
	if(ret < 0) {
	    if(errno != EINTR) {
		fprintf(stderr, "mctap: poll: %s\n", strerror(errno));
		exit(1);
	    }
	    continue;
	}
	if(pfds[0].revents) {
	    ret = read(sock, buf, sizeof(buf));
	    if(ret < 0) {
		if((errno != EINTR) && (errno != EAGAIN)) {
		    fprintf(stderr, "mctap: mcast packet: %s\n", strerror(errno));
		    exit(1);
		}
	    } else {
		if(sizeof(buf) - ret < sizeof(pi)) {
		    /* Drop */
		} else {
		    memmove(buf + sizeof(pi), buf, ret);
		    pi.flags = 0;
		    pi.proto = 0;
		    memcpy(buf, &pi, sizeof(pi));
		    write(tap, buf, ret);
		}
	    }
	}
	if(pfds[1].revents) {
	    ret = read(tap, buf, sizeof(buf));
	    if(ret < 0) {
		if((errno != EINTR) && (errno != EAGAIN)) {
		    fprintf(stderr, "mctap: mcast packet: %s\n", strerror(errno));
		    exit(1);
		}
	    } else {
		if(ret < sizeof(pi)) {
		    /* Drop */
		} else {
		    memcpy(&pi, buf, sizeof(pi));
		    if(pi.flags & TUN_PKT_STRIP) {
			/* Drop */
		    } else {
			sendto(sock, buf + sizeof(pi), ret - sizeof(pi), 0, dst, dstlen);
		    }
		}
	    }
	}
    }
}

static int maketap(char *name)
{
    int fd;
    struct ifreq rb;
    
    if((fd = open("/dev/net/tun", O_RDWR)) < 0)
	return(-1);
    memset(&rb, 0, sizeof(rb));
    rb.ifr_flags = IFF_TAP;
    strncpy(rb.ifr_name, name, IFNAMSIZ);
    if(ioctl(fd, TUNSETIFF, &rb))
	return(-1);
    return(fd);
}

int main(int argc, char **argv)
{
    int c;
    int sock, tap;
    struct in_addr group;
    int port;
    char *tapname;
    struct sockaddr_in dst;
    
    tapname = "mctap";
    while((c = getopt(argc, argv, "hD:")) >= 0) {
	switch(c) {
	case 'D':
	    tapname = optarg;
	    break;
	case 'h':
	    usage(stdout);
	    return(0);
	default:
	    usage(stderr);
	    exit(1);
	}
    }
    if(argc - optind < 2) {
	usage(stderr);
	exit(1);
    }
    if(!inet_aton(argv[optind], &group)) {
	fprintf(stderr, "mctap: invalid group address: %s\n", argv[optind]);
	exit(1);
    }
    port = atoi(argv[optind + 1]);
    if((sock = mkmcastsk4(group, port)) < 0) {
	fprintf(stderr, "mctap: could not create multicast socket: %s\n", strerror(errno));
	exit(1);
    }
    if((tap = maketap(tapname)) < 0) {
	fprintf(stderr, "mctap: could not create TAP device: %s\n", strerror(errno));
	exit(1);
    }
    
    memset(&dst, 0, sizeof(dst));
    dst.sin_family = AF_INET;
    dst.sin_addr = group;
    dst.sin_port = htons(port);
    bridge(sock, tap, (struct sockaddr *)&dst, sizeof(dst));
    return(0);
}
Commit	Line	Data
3415540f FT	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>
b5038e15 FT	7	#include <arpa/inet.h>
b5038e15 FT	8	#include <errno.h>
49697920 FT	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>
b5038e15 FT	14
b5038e15 FT	15	#include "utils.h"
3415540f FT	16
	17	static void usage(FILE *out)
	18	{
49697920	19	fprintf(out, "usage: mctap [-h] [-D TAPNAME] MCASTGROUP PORT\n");
b5038e15 FT	20	}
b5038e15 FT	21
49697920	22	static __attribute__ ((unused)) char formataddress(struct sockaddr arg, socklen_t arglen)
b5038e15 FT	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:
49697920	35	ret = sstrdup("Unix socket");
b5038e15 FT	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;
aeddd223	59	int soval;
b5038e15 FT	60	struct sockaddr_in nm;
	61	struct ip_mreqn mreq;
	62
	63	fd = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
aeddd223 FT	64	soval = 1;
	65	if(setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &soval, sizeof(soval)))
	66	return(-1);
b5038e15 FT	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;
aeddd223	74	if(setsockopt(fd, SOL_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq)))
b5038e15	75	return(-1);
af3fc4a5 FT	76	soval = 1;
	77	if(setsockopt(fd, SOL_IP, IP_MULTICAST_LOOP, &soval, sizeof(soval)))
	78	return(-1);
b5038e15 FT	79	return(fd);
	80	}
	81
49697920	82	static __attribute__ ((unused)) void test(int fd)
b5038e15 FT	83	{
b5038e15 FT	84	char buf[65536];
4f6c7408	85	int i, ret;
49697920	86	struct pollfd pfd;
b5038e15 FT	87
b5038e15 FT	88	while(1) {
49697920 FT	89	pfd.fd = fd;
	90	pfd.events = POLLIN;
	91	ret = poll(&pfd, 1, -1);
b5038e15	92	if(ret < 0) {
49697920	93	fprintf(stderr, "mctap: poll: %s\n", strerror(errno));
b5038e15 FT	94	exit(1);
b5038e15 FT	95	}
49697920 FT	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);
4f6c7408	108	}
b5038e15	109	}
3415540f FT	110	}
3415540f FT	111
49697920 FT	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, 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
3415540f FT	191	int main(int argc, char **argv)
	192	{
	193	int c;
49697920	194	int sock, tap;
b5038e15 FT	195	struct in_addr group;
b5038e15 FT	196	int port;
49697920 FT	197	char *tapname;
49697920 FT	198	struct sockaddr_in dst;
3415540f	199
49697920 FT	200	tapname = "mctap";
49697920 FT	201	while((c = getopt(argc, argv, "hD:")) >= 0) {
3415540f	202	switch(c) {
49697920 FT	203	case 'D':
	204	tapname = optarg;
	205	break;
3415540f FT	206	case 'h':
	207	usage(stdout);
	208	return(0);
	209	default:
	210	usage(stderr);
	211	exit(1);
	212	}
	213	}
b5038e15 FT	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	}
49697920 FT	227	if((tap = maketap(tapname)) < 0) {
	228	fprintf(stderr, "mctap: could not create TAP device: %s\n", strerror(errno));
	229	exit(1);
	230	}
3415540f	231
49697920 FT	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));
3415540f FT	237	return(0);
3415540f FT	238	}