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