[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 <syslog.h>

#include "utils.h"

static void usage(FILE *out)
{
    fprintf(out, "usage: mctap [-hd] [-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) {
		syslog(LOG_ERR, "mctap: poll: %s", strerror(errno));
		exit(1);
	    }
	    continue;
	}
	if(pfds[0].revents) {
	    ret = read(sock, buf, sizeof(buf));
	    if(ret < 0) {
		if((errno != EINTR) && (errno != EAGAIN)) {
		    syslog(LOG_ERR, "mctap: mcast packet: %s", 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, sizeof(pi) + ret);
		}
	    }
	}
	if(pfds[1].revents) {
	    ret = read(tap, buf, sizeof(buf));
	    if(ret < 0) {
		if((errno != EINTR) && (errno != EAGAIN)) {
		    syslog(LOG_ERR, "mctap: mcast packet: %s", 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;
    int daemonize;
    struct sockaddr_in dst;
    
    tapname = "mctap";
    daemonize = 0;
    while((c = getopt(argc, argv, "hD:d")) >= 0) {
	switch(c) {
	case 'D':
	    tapname = optarg;
	    break;
	case 'd':
	    daemonize = 1;
	    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);
    }
    openlog(sprintf2("mctap-%s", tapname), LOG_PID, LOG_DAEMON);
    
    if(daemonize)
	daemon(0, 0);
    
    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>
a13b233b	14	#include <syslog.h>
b5038e15 FT	15
b5038e15 FT	16	#include "utils.h"
3415540f FT	17
	18	static void usage(FILE *out)
	19	{
a13b233b	20	fprintf(out, "usage: mctap [-hd] [-D TAPNAME] MCASTGROUP PORT\n");
b5038e15 FT	21	}
b5038e15 FT	22
49697920	23	static __attribute__ ((unused)) char formataddress(struct sockaddr arg, socklen_t arglen)
b5038e15 FT	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:
49697920	36	ret = sstrdup("Unix socket");
b5038e15 FT	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;
aeddd223	60	int soval;
b5038e15 FT	61	struct sockaddr_in nm;
	62	struct ip_mreqn mreq;
	63
	64	fd = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
aeddd223 FT	65	soval = 1;
	66	if(setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &soval, sizeof(soval)))
	67	return(-1);
b5038e15 FT	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;
aeddd223	75	if(setsockopt(fd, SOL_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq)))
b5038e15	76	return(-1);
af3fc4a5 FT	77	soval = 1;
	78	if(setsockopt(fd, SOL_IP, IP_MULTICAST_LOOP, &soval, sizeof(soval)))
	79	return(-1);
b5038e15 FT	80	return(fd);
	81	}
	82
49697920	83	static __attribute__ ((unused)) void test(int fd)
b5038e15 FT	84	{
b5038e15 FT	85	char buf[65536];
4f6c7408	86	int i, ret;
49697920	87	struct pollfd pfd;
b5038e15 FT	88
b5038e15 FT	89	while(1) {
49697920 FT	90	pfd.fd = fd;
	91	pfd.events = POLLIN;
	92	ret = poll(&pfd, 1, -1);
b5038e15	93	if(ret < 0) {
49697920	94	fprintf(stderr, "mctap: poll: %s\n", strerror(errno));
b5038e15 FT	95	exit(1);
b5038e15 FT	96	}
49697920 FT	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);
4f6c7408	109	}
b5038e15	110	}
3415540f FT	111	}
3415540f FT	112
49697920 FT	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) {
a13b233b	130	syslog(LOG_ERR, "mctap: poll: %s", strerror(errno));
49697920 FT	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)) {
a13b233b	139	syslog(LOG_ERR, "mctap: mcast packet: %s", strerror(errno));
49697920 FT	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));
3cb771e8	150	write(tap, buf, sizeof(pi) + ret);
49697920 FT	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)) {
a13b233b	158	syslog(LOG_ERR, "mctap: mcast packet: %s", strerror(errno));
49697920 FT	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
3415540f FT	192	int main(int argc, char **argv)
	193	{
	194	int c;
49697920	195	int sock, tap;
b5038e15 FT	196	struct in_addr group;
b5038e15 FT	197	int port;
49697920	198	char *tapname;
a13b233b	199	int daemonize;
49697920	200	struct sockaddr_in dst;
3415540f	201
49697920	202	tapname = "mctap";
a13b233b FT	203	daemonize = 0;
a13b233b FT	204	while((c = getopt(argc, argv, "hD:d")) >= 0) {
3415540f	205	switch(c) {
49697920 FT	206	case 'D':
	207	tapname = optarg;
	208	break;
a13b233b FT	209	case 'd':
	210	daemonize = 1;
	211	break;
3415540f FT	212	case 'h':
	213	usage(stdout);
	214	return(0);
	215	default:
	216	usage(stderr);
	217	exit(1);
	218	}
	219	}
b5038e15 FT	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	}
49697920 FT	233	if((tap = maketap(tapname)) < 0) {
	234	fprintf(stderr, "mctap: could not create TAP device: %s\n", strerror(errno));
	235	exit(1);
	236	}
a13b233b FT	237	openlog(sprintf2("mctap-%s", tapname), LOG_PID, LOG_DAEMON);
	238
	239	if(daemonize)
	240	daemon(0, 0);
3415540f	241
49697920 FT	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));
3415540f FT	247	return(0);
3415540f FT	248	}