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

#include "utils.h"

static int quit = 0;
static unsigned char macaddr[6];

static void usage(FILE *out)
{
    fprintf(out, "usage: mctap [-hdpk] [-P PIDFILE] [-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(!quit) {
	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 if((ret < 12) || !memcmp(macaddr, buf + 6, 6)) {
		    /* Drop looped back */
		} 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);
    if(ioctl(fd, SIOCGIFHWADDR, &rb))
	return(-1);
    memcpy(macaddr, rb.ifr_hwaddr.sa_data, 6);
    return(fd);
}

static void sighand(int sig)
{
    switch(sig) {
    case SIGINT:
    case SIGTERM:
	quit = 1;
	break;
    case SIGHUP:
	break;
    }
}

static void killrunning(char *pidfile)
{
    FILE *pidfd;
    int pid;
    
    if((pidfd = fopen(pidfile, "r")) == NULL) {
	fprintf(stderr, "mctab -k: could not read PID file %s: %s\n", pidfile, strerror(errno));
	exit(1);
    }
    fscanf(pidfd, "%i", &pid);
    if(kill(pid, SIGTERM)) {
	fprintf(stderr, "mctab -k: could not kill %i: %s\n", pid, strerror(errno));
	exit(1);
    }
    fclose(pidfd);
}

int main(int argc, char **argv)
{
    int c;
    int sock, tap;
    struct in_addr group;
    int port;
    char *tapname;
    char *pidfile;
    int daemonize, killold;
    struct sockaddr_in dst;
    FILE *pidfd;
    
    tapname = "mctap";
    daemonize = killold = 0;
    pidfile = NULL;
    while((c = getopt(argc, argv, "hD:dpP:k")) >= 0) {
	switch(c) {
	case 'D':
	    tapname = optarg;
	    break;
	case 'd':
	    daemonize = 1;
	    break;
	case 'p':
	    pidfile = (void *)-1;
	    break;
	case 'P':
	    pidfile = optarg;
	    break;
	case 'k':
	    killold = 1;
	    if(pidfile == NULL)
		pidfile = (void *)-1;
	    break;
	case 'h':
	    usage(stdout);
	    return(0);
	default:
	    usage(stderr);
	    exit(1);
	}
    }
    if(pidfile == (void *)-1)
	pidfile = sprintf2("/var/run/mctap.%s.pid", tapname);
    if(killold) {
	killrunning(pidfile);
	return(0);
    }
    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);
    
    pidfd = NULL;
    if((pidfile != NULL) && ((pidfd = fopen(pidfile, "w")) == NULL)) {
	fprintf(stderr, "mctap: could not create PID file %s: %s\n", pidfile, strerror(errno));
	exit(1);
    }
    if(daemonize)
	daemon(0, 0);
    if(pidfd != NULL) {
	fprintf(pidfd, "%i\n", getpid());
	fclose(pidfd);
    }
    
    signal(SIGTERM, sighand);
    signal(SIGINT, sighand);
    signal(SIGHUP, sighand);
    
    memset(&dst, 0, sizeof(dst));
    dst.sin_family = AF_INET;
    dst.sin_addr = group;
    dst.sin_port = htons(port);
    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]);
    bridge(sock, tap, (struct sockaddr *)&dst, sizeof(dst));
    syslog(LOG_INFO, "exiting");
    
    if(pidfile != NULL)
	unlink(pidfile);
    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>
1fefa40d	15	#include <signal.h>
b5038e15 FT	16
b5038e15 FT	17	#include "utils.h"
3415540f	18
1fefa40d	19	static int quit = 0;
b62b996e	20	static unsigned char macaddr[6];
1fefa40d	21
3415540f FT	22	static void usage(FILE *out)
3415540f FT	23	{
8cac9901	24	fprintf(out, "usage: mctap [-hdpk] [-P PIDFILE] [-D TAPNAME] MCASTGROUP PORT\n");
b5038e15 FT	25	}
b5038e15 FT	26
49697920	27	static __attribute__ ((unused)) char formataddress(struct sockaddr arg, socklen_t arglen)
b5038e15 FT	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:
49697920	40	ret = sstrdup("Unix socket");
b5038e15 FT	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;
aeddd223	64	int soval;
b5038e15 FT	65	struct sockaddr_in nm;
	66	struct ip_mreqn mreq;
	67
	68	fd = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
aeddd223 FT	69	soval = 1;
	70	if(setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &soval, sizeof(soval)))
	71	return(-1);
b5038e15 FT	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;
aeddd223	79	if(setsockopt(fd, SOL_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq)))
b5038e15	80	return(-1);
af3fc4a5 FT	81	soval = 1;
	82	if(setsockopt(fd, SOL_IP, IP_MULTICAST_LOOP, &soval, sizeof(soval)))
	83	return(-1);
b5038e15 FT	84	return(fd);
	85	}
	86
49697920	87	static __attribute__ ((unused)) void test(int fd)
b5038e15 FT	88	{
b5038e15 FT	89	char buf[65536];
4f6c7408	90	int i, ret;
49697920	91	struct pollfd pfd;
b5038e15 FT	92
b5038e15 FT	93	while(1) {
49697920 FT	94	pfd.fd = fd;
	95	pfd.events = POLLIN;
	96	ret = poll(&pfd, 1, -1);
b5038e15	97	if(ret < 0) {
49697920	98	fprintf(stderr, "mctap: poll: %s\n", strerror(errno));
b5038e15 FT	99	exit(1);
b5038e15 FT	100	}
49697920 FT	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);
4f6c7408	113	}
b5038e15	114	}
3415540f FT	115	}
3415540f FT	116
49697920 FT	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);
1fefa40d	126	while(!quit) {
49697920 FT	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) {
a13b233b	134	syslog(LOG_ERR, "mctap: poll: %s", strerror(errno));
49697920 FT	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)) {
a13b233b	143	syslog(LOG_ERR, "mctap: mcast packet: %s", strerror(errno));
49697920 FT	144	exit(1);
	145	}
	146	} else {
	147	if(sizeof(buf) - ret < sizeof(pi)) {
	148	/* Drop */
b62b996e FT	149	} else if((ret < 12) \|\| !memcmp(macaddr, buf + 6, 6)) {
b62b996e FT	150	/* Drop looped back */
49697920 FT	151	} else {
	152	memmove(buf + sizeof(pi), buf, ret);
	153	pi.flags = 0;
	154	pi.proto = 0;
	155	memcpy(buf, &pi, sizeof(pi));
3cb771e8	156	write(tap, buf, sizeof(pi) + ret);
49697920 FT	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)) {
a13b233b	164	syslog(LOG_ERR, "mctap: mcast packet: %s", strerror(errno));
49697920 FT	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);
b62b996e FT	195	if(ioctl(fd, SIOCGIFHWADDR, &rb))
	196	return(-1);
	197	memcpy(macaddr, rb.ifr_hwaddr.sa_data, 6);
49697920 FT	198	return(fd);
	199	}
	200
1fefa40d FT	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
8cac9901 FT	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
3415540f FT	230	int main(int argc, char **argv)
	231	{
	232	int c;
49697920	233	int sock, tap;
b5038e15 FT	234	struct in_addr group;
b5038e15 FT	235	int port;
49697920	236	char *tapname;
1fefa40d	237	char *pidfile;
8cac9901	238	int daemonize, killold;
49697920	239	struct sockaddr_in dst;
1fefa40d	240	FILE *pidfd;
3415540f	241
49697920	242	tapname = "mctap";
8cac9901	243	daemonize = killold = 0;
1fefa40d	244	pidfile = NULL;
8cac9901	245	while((c = getopt(argc, argv, "hD:dpP:k")) >= 0) {
3415540f	246	switch(c) {
49697920 FT	247	case 'D':
	248	tapname = optarg;
	249	break;
a13b233b FT	250	case 'd':
	251	daemonize = 1;
	252	break;
1fefa40d FT	253	case 'p':
	254	pidfile = (void *)-1;
	255	break;
	256	case 'P':
	257	pidfile = optarg;
	258	break;
8cac9901 FT	259	case 'k':
	260	killold = 1;
	261	if(pidfile == NULL)
	262	pidfile = (void *)-1;
	263	break;
3415540f FT	264	case 'h':
	265	usage(stdout);
	266	return(0);
	267	default:
	268	usage(stderr);
	269	exit(1);
	270	}
	271	}
1fefa40d FT	272	if(pidfile == (void *)-1)
1fefa40d FT	273	pidfile = sprintf2("/var/run/mctap.%s.pid", tapname);
8cac9901 FT	274	if(killold) {
	275	killrunning(pidfile);
	276	return(0);
	277	}
b5038e15 FT	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	}
49697920 FT	291	if((tap = maketap(tapname)) < 0) {
	292	fprintf(stderr, "mctap: could not create TAP device: %s\n", strerror(errno));
	293	exit(1);
	294	}
a13b233b FT	295	openlog(sprintf2("mctap-%s", tapname), LOG_PID, LOG_DAEMON);
a13b233b FT	296
1fefa40d FT	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	}
a13b233b FT	302	if(daemonize)
a13b233b FT	303	daemon(0, 0);
1fefa40d FT	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);
3415540f	312
49697920 FT	313	memset(&dst, 0, sizeof(dst));
	314	dst.sin_family = AF_INET;
	315	dst.sin_addr = group;
	316	dst.sin_port = htons(port);
b62b996e	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]);
49697920	318	bridge(sock, tap, (struct sockaddr *)&dst, sizeof(dst));
1fefa40d FT	319	syslog(LOG_INFO, "exiting");
	320
	321	if(pidfile != NULL)
	322	unlink(pidfile);
3415540f FT	323	return(0);
3415540f FT	324	}