1/*
2 * Ethernet Switch IGMP Snooper
3 * Copyright (C) 2014 ASUSTeK Inc.
4 * All Rights Reserved.
5
6 * This program is free software: you can redistribute it and/or modify
7 * it under the terms of the GNU Affero General Public License as
8 * published by the Free Software Foundation, either version 3 of the
9 * License, or (at your option) any later version.
10
11 * This program is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14 * GNU Affero General Public License for more details.
15
16 * You should have received a copy of the GNU Affero General Public License
17 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
18 */
19
20#include <stdio.h>
21#include <stdint.h>
22#include <stdlib.h>
23#include <string.h>
24#include <unistd.h>
25#include <fcntl.h>
26#include <errno.h>
27#include <signal.h>
28#include <sys/ioctl.h>
29#include <sys/socket.h>
30#include <netinet/in.h>
31#include <netinet/ip.h>
32#include <netinet/ether.h>
33#include <net/if.h>
34#include <arpa/inet.h>
35#include <netpacket/packet.h>
36#include <linux/types.h>
37#include <linux/filter.h>
38#include <syslog.h>
39
40#include "snooper.h"
41#include "queue.h"
42
43#define INADDR_IGMPV3_GROUP ((in_addr_t) 0xe0000016)
44
45static int terminated = 0;
46static int fd;
47static int ifindex;
48unsigned char ifhwaddr[ETHER_ADDR_LEN];
49in_addr_t ifaddr;
50
51static int snoop_init(char *ifswitch, char *ifbridge, int rcvsize, unsigned char *ifhwaddr, in_addr_t *ifaddr)
52{
53	static const struct sock_filter filter[] = {
54		BPF_STMT(BPF_LD|BPF_W|BPF_ABS, SKF_AD_OFF + SKF_AD_PROTOCOL),
55		BPF_JUMP(BPF_JMP|BPF_JEQ|BPF_K, ETHERTYPE_IP, 0, 5),
56		BPF_STMT(BPF_LD|BPF_H|BPF_ABS, 12),
57		BPF_JUMP(BPF_JMP|BPF_JEQ|BPF_K, ETHERTYPE_IP, 0, 3),
58		BPF_STMT(BPF_LD|BPF_B|BPF_ABS, 23),
59		BPF_JUMP(BPF_JMP|BPF_JEQ|BPF_K, IPPROTO_IGMP, 0, 1),
60		BPF_STMT(BPF_RET|BPF_K, 0x7fffffff),
61		BPF_STMT(BPF_RET|BPF_K, 0),
62	};
63	static const struct sock_fprog fprog = {
64		.len = sizeof(filter)/sizeof(filter[0]),
65		.filter = (struct sock_filter *) filter,
66	};
67	struct ifreq ifr;
68	struct sockaddr_ll sll;
69	struct packet_mreq mreq;
70	int fd;
71
72	if (init_timers() < 0 || init_cache() < 0)
73		return -1;
74
75#ifdef SOCK_CLOEXEC
76	fd = socket(PF_PACKET, SOCK_RAW | SOCK_CLOEXEC | SOCK_NONBLOCK, htons(ETH_P_IP));
77	if (fd < 0) {
78		log_error("socket: %s", strerror(errno));
79		return -1;
80	}
81#else
82	int value;
83
84	fd = socket(PF_PACKET, SOCK_RAW, htons(ETHERTYPE_IP));
85	if (fd < 0) {
86		log_error("socket: %s", strerror(errno));
87		return -1;
88	}
89
90	value = fcntl(fd, F_GETFD, 0);
91	if (value < 0 || fcntl(fd, F_SETFD, value | FD_CLOEXEC) < 0) {
92		log_error("fcntl::FD_CLOEXEC: %s", strerror(errno));
93		goto error;
94	}
95
96	value = fcntl(fd, F_GETFL, 0);
97	if (value < 0 || fcntl(fd, F_SETFL, value | O_NONBLOCK) < 0) {
98		log_error("fcntl::O_NONBLOCK: %s", strerror(errno));
99		goto error;
100	}
101#endif
102
103	if (setsockopt(fd, SOL_SOCKET, SO_ATTACH_FILTER, &fprog, sizeof(fprog)) < 0) {
104		log_error("setsockopt::SO_ATTACH_FILTER: %s", strerror(errno));
105		goto error;
106	}
107
108	memset(&sll, 0, sizeof(sll));
109	sll.sll_family = AF_PACKET;
110	sll.sll_ifindex = ifindex;
111	sll.sll_protocol = ifbridge ? htons(ETH_P_ALL) : htons(ETH_P_IP);
112	if (bind(fd, (struct sockaddr *) &sll , sizeof(sll)) < 0) {
113		log_error("bind: %s", strerror(errno));
114		goto error;
115	}
116
117	memset(&mreq, 0, sizeof(mreq));
118	mreq.mr_ifindex = ifindex;
119	mreq.mr_type = PACKET_MR_ALLMULTI;
120	if (setsockopt(fd, SOL_PACKET, PACKET_ADD_MEMBERSHIP, &mreq, sizeof(mreq)) < 0) {
121		log_error("setsockopt::PACKET_ADD_MEMBERSHIP: %s", strerror(errno));
122		goto error;
123	}
124
125	if (setsockopt(fd, SOL_SOCKET, SO_RCVBUF, &rcvsize, sizeof(rcvsize)) < 0) {
126		log_error("setsockopt::SO_RCVBUF: %s", strerror(errno));
127		goto error;
128	}
129
130	memset(&ifr, 0, sizeof(ifr));
131	strncpy(ifr.ifr_name, ifbridge ? : ifswitch, sizeof(ifr.ifr_name));
132	if (ioctl(fd, SIOCGIFHWADDR, &ifr) < 0) {
133		log_error("ioctl::SIOCGIFHWADDR: %s", strerror(errno));
134		goto error;
135	}
136	if (ifhwaddr)
137		memcpy(ifhwaddr, ifr.ifr_hwaddr.sa_data, ETHER_ADDR_LEN);
138
139	if (ifaddr) {
140		memset(&ifr, 0, sizeof(ifr));
141		strncpy(ifr.ifr_name, ifbridge ? : ifswitch, sizeof(ifr.ifr_name));
142		*ifaddr = (ioctl(fd, SIOCGIFADDR, &ifr) < 0) ?
143		    INADDR_ANY : ((struct sockaddr_in*) &ifr.ifr_addr)->sin_addr.s_addr;
144	}
145
146	if (switch_init(ifswitch) < 0)
147		goto error;
148
149	return fd;
150
151error:
152	close(fd);
153	return -1;
154}
155
156static void snoop_done(void)
157{
158	purge_timers();
159	purge_cache();
160	if (fd >= 0)
161		close(fd);
162	switch_done();
163}
164
165int send_query(in_addr_t group)
166{
167	unsigned char packet[1500];
168	unsigned char ea[ETHER_ADDR_LEN];
169	struct sockaddr_ll sll;
170	in_addr_t dst = group ? : htonl(INADDR_ALLHOSTS_GROUP);
171	int ret, len;
172
173	ether_mtoe(dst, ea);
174	len = build_query(packet, sizeof(packet), group, dst, ea);
175	if (len < 0)
176		return -1;
177
178	memset(&sll, 0, sizeof(sll));
179	sll.sll_family = AF_PACKET;
180	sll.sll_ifindex = ifindex;
181	sll.sll_halen = sizeof(ea);
182	memcpy(sll.sll_addr, ea, sizeof(ea));
183
184	do {
185		ret = sendto(fd, packet, len, 0, (struct sockaddr *) &sll, sizeof(sll));
186	} while (ret < 0 && errno == EINTR);
187	if (ret < 0 && errno != ENETDOWN)
188		log_error("sendto: %s", strerror(errno));
189
190	if (ret > 0)
191		accept_igmp(packet, ret, ifhwaddr, 1);
192
193	return ret;
194}
195
196static void sighandler(int sig)
197{
198	switch (sig) {
199	case SIGINT:
200	case SIGTERM:
201	case SIGALRM:
202		terminated = sig ? : -1;
203		break;
204	}
205}
206
207static void usage(char *name, int service, int querier)
208{
209	printf("Usage: %s [-d|-D] [-q|-Q] [-s <switch>] [-b <bridge>]\n", name);
210	printf(" -d or -D	run in background%s or in foreground%s\n",
211	    service ? " (default)" : "", service ? "" : " (default)");
212	printf(" -q or -Q	enable%s or disable%s querier\n",
213	    querier ? " (default)" : "", querier ? "" : " (default)");
214	printf(" -s <switch>	switch interface\n");
215	printf(" -b <bridge>	bridge interface, if switch interface is bridged\n");
216}
217
218int main(int argc, char *argv[])
219{
220	unsigned char packet[1500];
221	struct sockaddr_ll sll;
222        struct sigaction sa;
223	char *name, *ifswitch, *ifbridge;
224	int opt, querier, service, ret = -1;
225
226	name = strrchr(argv[0], '/');
227	name = name ? name + 1 : argv[0];
228
229	ifswitch = ifbridge = NULL;
230	querier = 1;
231	service = 1;
232
233	while ((opt = getopt(argc, argv, "s:b:qQdDh")) > 0) {
234		switch (opt) {
235		case 's':
236			ifswitch = optarg;
237			break;
238		case 'b':
239			ifbridge = optarg;
240			break;
241		case 'd':
242		case 'D':
243			service = (opt == 'd');
244			break;
245		case 'q':
246		case 'Q':
247			querier = (opt == 'q');
248			break;
249		default:
250			usage(name, service, querier);
251			return 1;
252		}
253	}
254
255	if (!ifswitch) {
256		usage(name, service, querier);
257		return 1;
258	} else if (ifbridge && strcmp(ifbridge, ifswitch) == 0)
259		ifbridge = NULL;
260
261	openlog(name, 0, LOG_USER);
262	log_info("started on %s%s%s", ifswitch, ifbridge ? "@" : "", ifbridge ? : "");
263
264	ifindex = if_nametoindex(ifswitch);
265	if (ifindex == 0) {
266		log_error("%s: %s", ifswitch, strerror(errno));
267		return 1;
268	}
269
270	fd = snoop_init(ifswitch, ifbridge, sizeof(packet) * 16, ifhwaddr, &ifaddr);
271	if (fd < 0)
272		goto error;
273
274	if (service && daemon(0, 0) < 0)
275		log_error("daemon: %s", strerror(errno));
276
277	sa.sa_handler = sighandler;
278	sa.sa_flags = 0;
279	sigemptyset(&sa.sa_mask);
280	sigaction(SIGTERM, &sa, NULL);
281	sigaction(SIGINT, &sa, NULL);
282	sigaction(SIGALRM, &sa, NULL);
283
284	init_querier(querier);
285
286	while (!terminated) {
287		fd_set rfds;
288		struct timeval tv, *timeout;
289
290		FD_ZERO(&rfds);
291		FD_SET(fd, &rfds);
292		timeout = (next_timer(&tv) < 0) ? NULL : &tv;
293
294		ret = select(fd + 1, &rfds, NULL, NULL, timeout);
295		if (ret < 0 && errno == EINTR)
296			continue;
297		if (ret < 0) {
298			log_error("select: %s", strerror(errno));
299			goto error;
300		}
301
302		if (ret > 0 && FD_ISSET(fd, &rfds)) {
303			socklen_t socklen = sizeof(sll);
304			do {
305				ret = recvfrom(fd, packet, sizeof(packet), 0, (struct sockaddr *) &sll, &socklen);
306			} while (ret < 0 && errno == EINTR);
307			if (ret < 0 && errno != ENETDOWN) {
308				log_error("recvfrom: %s", strerror(errno));
309				goto error;
310			}
311			if (sll.sll_hatype == ARPHRD_ETHER && sll.sll_halen == ETHER_ADDR_LEN) {
312				ret = accept_igmp(packet, ret, sll.sll_addr, 0);
313				if (ret < 0)
314					goto error;
315			}
316		}
317
318		run_timers();
319	}
320
321	log_info("terminated with signal %d", terminated);
322	ret = 0;
323
324error:
325	snoop_done();
326
327	return ret;
328}
329