#include #include #include #include #include #include #include #include #include #include #include #include #include /* Remove later */ #include #include "main.h" #include "uring.h" #include "igmp.h" struct igmp { struct uring_task task; struct uring_task_buf tbuf; }; #define ETH_HDR_LEN 14 #define IPV4_MIN_HDR_LEN 20 #define IGMP_MIN_LEN 8 struct __attribute__((packed, scalar_storage_order("big-endian"))) ipv4_hdr { unsigned version:4; unsigned ihl:4; unsigned dscp:6; unsigned ecn:2; unsigned length:16; unsigned id:16; unsigned flags:3; unsigned fragment_offset:13; unsigned ttl:8; unsigned protocol:8; unsigned checksum:16; unsigned src:32; unsigned dst:32; unsigned options[]; }; enum igmp_type { IGMP_MEMBERSHIP_QUERY = 0x11, IGMP_V1_MEMBERSHIP_REPORT = 0x12, IGMP_V2_MEMBERSHIP_REPORT = 0x16, IGMP_V3_MEMBERSHIP_REPORT = 0x22, IGMP_V2_LEAVE_GROUP = 0x17 }; union igmp_msg { struct __attribute__((packed, scalar_storage_order("big-endian"))) { enum igmp_type type:8; unsigned unknown:8; unsigned checksum:16; } common; struct __attribute__((packed, scalar_storage_order("big-endian"))) { enum igmp_type type:8; unsigned resptime:8; unsigned checksum:16; unsigned addr:32; } v2; struct __attribute__((packed, scalar_storage_order("big-endian"))) { enum igmp_type type:8; unsigned reserved1:8; unsigned checksum:16; unsigned reserved2:16; unsigned nrecs:16; uint8_t records[]; } v3; }; enum igmp_v3_record_type { IGMP_V3_REC_MODE_IS_INCL = 1, IGMP_V3_REC_MODE_IS_EXCL = 2, IGMP_V3_REC_MODE_CH_INCL = 3, IGMP_V3_REC_MODE_CH_EXCL = 4 }; union igmp_v3_record { struct __attribute__((packed, scalar_storage_order("big-endian"))) { enum igmp_v3_record_type type:8; unsigned auxlen:8; unsigned nsrcs:16; unsigned addr:32; uint32_t saddr[]; }; }; static inline unsigned short from32to16(unsigned int x) { /* add up 16-bit and 16-bit for 16+c bit */ x = (x & 0xffff) + (x >> 16); /* add up carry.. */ x = (x & 0xffff) + (x >> 16); return x; } static unsigned int do_csum(const unsigned char *buf, int len) { int odd; unsigned int result = 0; assert_return(buf && len > 0, 0); odd = 1 & (unsigned long)buf; if (odd) { #if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__ result += (*buf << 8); #else result = *buf; #endif len--; buf++; } if (len >= 2) { if (2 & (unsigned long)buf) { result += *(unsigned short *)buf; len -= 2; buf += 2; } if (len >= 4) { const unsigned char *end = buf + ((unsigned)len & ~3); unsigned int carry = 0; do { unsigned int w = *(unsigned int *)buf; buf += 4; result += carry; result += w; carry = (w > result); } while (buf < end); result += carry; result = (result & 0xffff) + (result >> 16); } if (len & 2) { result += *(unsigned short *)buf; buf += 2; } } if (len & 1) #if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__ result += *buf; #else result += (*buf << 8); #endif result = from32to16(result); if (odd) result = ((result >> 8) & 0xff) | ((result & 0xff) << 8); return result; } static inline bool csum_valid(const char *buf, size_t len) { assert_return(buf && len > 0, false); return do_csum((unsigned const char *)buf, len) == 0xffff; } static void igmp_match() { /* Sent with approx 120-130 sec intervals */ time_t t = time(NULL); struct tm *tm = localtime(&t); char s[64]; strftime(s, sizeof(s), "%c", tm); debug(DBG_IGMP, "multicast request discovered at: %s", s); //start announce } static void igmp_parse(struct cfg *cfg, struct igmp *igmp) { char *buf; size_t len; struct ipv4_hdr *hdr; size_t body_len; union igmp_msg *igmp_msg; assert_return(cfg && igmp); buf = igmp->task.tbuf->buf; len = igmp->task.tbuf->len; hdr = (struct ipv4_hdr *)buf; if (len <= IPV4_MIN_HDR_LEN) return; if (hdr->version != 4) return; if (hdr->ihl * 4 < IPV4_MIN_HDR_LEN) return; if (hdr->length < hdr->ihl * 4) return; if (hdr->length != len) return; if (hdr->fragment_offset > 0) return; if (hdr->protocol != IPPROTO_IGMP) return; if (!csum_valid(buf, hdr->ihl * 4)) return; body_len = hdr->length - hdr->ihl * 4; igmp_msg = (union igmp_msg *)(buf + hdr->ihl * 4); if (body_len < IGMP_MIN_LEN) return; switch (igmp_msg->common.type) { case IGMP_V1_MEMBERSHIP_REPORT: debug(DBG_IGMP, "igmp_v1_membership_report"); /* fall through */ case IGMP_V2_MEMBERSHIP_REPORT: { struct in_addr src; char src_str[INET_ADDRSTRLEN]; struct in_addr dst; char dst_str[INET_ADDRSTRLEN]; struct in_addr grp; char grp_str[INET_ADDRSTRLEN]; src.s_addr = htonl(hdr->src); inet_ntop(AF_INET, &src, src_str, sizeof(src_str)); dst.s_addr = htonl(hdr->dst); inet_ntop(AF_INET, &dst, dst_str, sizeof(dst_str)); grp.s_addr = htonl(igmp_msg->v2.addr); inet_ntop(AF_INET, &grp, grp_str, sizeof(grp_str)); debug(DBG_IGMP, "igmp_v2_membership_report %s -> %s (%s)", src_str, dst_str, grp_str); if (body_len != IGMP_MIN_LEN) { error("IGMPv2 invalid size"); break; } if (!csum_valid((char *)igmp_msg, body_len)) { error("IGMPv2 invalid checksum"); break; } debug(DBG_IGMP, "Inet addr: 0x%x", inet_addr("224.0.2.60")); debug(DBG_IGMP, "Inet addr: 0x%x", cinet_addr(224,0,2,60)); debug(DBG_IGMP, "Inet addr: 0x%x", chtobe32(cinet_addr(224,0,2,60))); if (htonl(hdr->dst) != cinet_addr(224,0,2,60)) { debug(DBG_IGMP, "IGMPv2 invalid dst addr"); break; } if (htonl(igmp_msg->v2.addr) != cinet_addr(224,0,2,60)) { debug(DBG_IGMP, "IGMPv2 invalid grp addr"); break; } igmp_match(); break; } case IGMP_V3_MEMBERSHIP_REPORT: { char *pos = (char *)igmp_msg; struct in_addr src; char src_str[INET_ADDRSTRLEN]; struct in_addr dst; char dst_str[INET_ADDRSTRLEN]; src.s_addr = htonl(hdr->src); inet_ntop(AF_INET, &src, src_str, sizeof(src_str)); dst.s_addr = htonl(hdr->dst); inet_ntop(AF_INET, &dst, dst_str, sizeof(dst_str)); debug(DBG_IGMP, "igmp_v3_membership_report %s -> %s", src_str, dst_str); debug(DBG_IGMP, "IGMPv3\n" " * Type: %x\n" " * Reserved: %u\n" " * Csum: %u\n" " * Reserved: %u\n" " * NRecs: %u\n" " * Size: %zu bytes", igmp_msg->v3.type, igmp_msg->v3.reserved1, igmp_msg->v3.checksum, igmp_msg->v3.reserved2, igmp_msg->v3.nrecs, sizeof(igmp_msg->v3)); if (!csum_valid(pos, body_len)) { error("IGMPv3 csum invalid"); break; } if (htonl(hdr->dst) != cinet_addr(224,0,0,22)) { debug(DBG_IGMP, "IGMPv2 invalid dst addr"); break; } body_len -= sizeof(igmp_msg->v3); pos += sizeof(igmp_msg->v3); for (unsigned rec = 0; rec < igmp_msg->v3.nrecs; rec++) { union igmp_v3_record *record = (union igmp_v3_record *)pos; struct in_addr grp; char grp_str[INET_ADDRSTRLEN]; if (body_len < sizeof(*record)) { error("IGMPv3 too short"); break; } grp.s_addr = htonl(record->addr); inet_ntop(AF_INET, &grp, grp_str, sizeof(grp_str)); debug(DBG_IGMP, "received IGMPv3 record to %s", grp_str); debug(DBG_IGMP, "IGMPv3 rec" " * Type: %un" " * Auxlen: %u" " * NSrcs: %u" " * Addr: %s" " * Size: %zu bytes", record->type, record->auxlen, record->nsrcs, grp_str, sizeof(*record)); body_len -= sizeof(*record); pos += sizeof(*record); if (body_len < record->nsrcs * sizeof(uint32_t) + record->auxlen) { error("IGMPv3 too short"); break; } for (unsigned addr = 0; addr < record->nsrcs; addr++) { struct in_addr grp_src; char grp_src_str[INET_ADDRSTRLEN]; grp_src.s_addr = htonl(record->saddr[addr]); inet_ntop(AF_INET, &grp_src, grp_src_str, sizeof(grp_src_str)); debug(DBG_IGMP, "received IGMPv3 record src %s", grp_src_str); body_len -= sizeof(record->saddr[addr]); pos += sizeof(record->saddr[addr]); } if ((htonl(record->addr) == cinet_addr(224,0,2,60)) && ((record->type == IGMP_V3_REC_MODE_IS_INCL) || (record->type == IGMP_V3_REC_MODE_CH_INCL))) igmp_match(); body_len -= record->auxlen; pos += record->auxlen; } break; } case IGMP_MEMBERSHIP_QUERY: debug(DBG_IGMP, "igmp_membership_query"); break; case IGMP_V2_LEAVE_GROUP: debug(DBG_IGMP, "igmp_v2_leave_group"); break; default: debug(DBG_IGMP, "IGMP msg type %02hhx", igmp_msg->common.type); break; } buf += hdr->length; len -= hdr->length; } static void igmp_read_cb(struct cfg *cfg, struct uring_task *task, int res) { struct igmp *igmp = container_of(task, struct igmp, task); assert_return(cfg && task); assert_task_alive(DBG_IGMP, task); debug(DBG_IGMP, "task %p, igmp %p, res %i", task, igmp, res); if (res < 0) { error("res: %i", res); return; } task->tbuf->len = res; if (task->saddr.storage.ss_family == AF_PACKET || task->saddr.ll.sll_protocol == htons(ETH_P_IP)) igmp_parse(cfg, igmp); else debug(DBG_IGMP, "invalid packet type received"); uring_tbuf_read(cfg, &igmp->task, igmp_read_cb); } static void igmp_free(struct uring_task *task) { struct igmp *igmp = container_of(task, struct igmp, task); assert_return(task); debug(DBG_IGMP, "task %p, igmp %p", task, igmp); xfree(igmp); } void igmp_refdump(struct igmp *igmp) { assert_return_silent(igmp); uring_task_refdump(&igmp->task); } void igmp_delete(struct cfg *cfg) { assert_return(cfg); assert_return_silent(cfg->igmp); debug(DBG_IGMP, "closing fd %i", cfg->igmp->task.fd); uring_task_destroy(cfg, &cfg->igmp->task); cfg->igmp = NULL; } void igmp_init(struct cfg *cfg) { static const struct sock_filter filter[] = { BPF_STMT(BPF_LD + BPF_W + BPF_LEN, 0), /* A <- packet length */ BPF_JUMP(BPF_JMP + BPF_JGE + BPF_K, sizeof(struct iphdr), 1, 0), /* A < sizeof(iphdr) */ BPF_STMT(BPF_RET + BPF_K, 0), /* drop packet */ BPF_STMT(BPF_LD + BPF_B + BPF_ABS, 0 /* iphdr[0] */), /* A <- version + ihl */ BPF_STMT(BPF_ALU + BPF_RSH + BPF_K, 4), /* A <- A >> 4 (version) */ BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, 0x04, 1, 0), /* A != 4 */ BPF_STMT(BPF_RET + BPF_K, 0), /* drop packet */ BPF_STMT(BPF_LD + BPF_B + BPF_ABS, offsetof(struct iphdr, protocol)), /* A <- ip protocol */ BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, IPPROTO_IGMP, 1, 0), /* A != IPPROTO_IGMP */ BPF_STMT(BPF_RET + BPF_K, 0), /* drop packet */ BPF_STMT(BPF_LD + BPF_W + BPF_ABS, offsetof(struct iphdr, daddr)), /* A <- ip dst addr */ BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, chtobe32(cinet_addr(224,0,2,60)), 2, 0), /* A != 224.0.2.60 */ //BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, 0xe000023c, 2, 0), /* A != 224.0.2.60 */ BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, chtobe32(cinet_addr(224,0,0,22)), 1, 0), /* A != 224.0.0.22 */ //BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, 0xe0000016, 1, 0), /* A != 224.0.0.22 */ BPF_STMT(BPF_RET + BPF_K, 0), /* drop packet */ BPF_STMT(BPF_LDX + BPF_B + BPF_MSH, 0 /* iphdr[0] */), /* X <- pkt->ihl * 4 */ BPF_STMT(BPF_LD + BPF_IMM, 20), /* A <- 20 */ BPF_JUMP(BPF_JMP + BPF_JGT + BPF_X, 0, 0, 1), /* A > X */ BPF_STMT(BPF_RET + BPF_K, 0), /* drop packet */ BPF_STMT(BPF_LD + BPF_H + BPF_ABS, offsetof(struct iphdr, tot_len)), /* A <- ip tot_len */ BPF_JUMP(BPF_JMP + BPF_JGT + BPF_X, 0, 1, 0), /* A <= ip->ihl * 4 */ BPF_STMT(BPF_RET + BPF_K, 0), /* drop packet */ BPF_STMT(BPF_ALU + BPF_SUB + BPF_X, 0), /* A <- A - X (bodylen) */ BPF_JUMP(BPF_JMP + BPF_JGE + BPF_K, 8, 1, 0), /* A < 8 */ BPF_STMT(BPF_RET + BPF_K, 0), /* drop packet */ BPF_STMT(BPF_LD + BPF_H + BPF_ABS, offsetof(struct iphdr, tot_len)), /* A <- ip->tot_len */ BPF_STMT(BPF_MISC + BPF_TAX, 0), /* X <- A */ BPF_STMT(BPF_LD + BPF_W + BPF_LEN, 0), /* A <- packet length */ BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_X, 0, 1, 0), /* A != ip->tot_len */ BPF_STMT(BPF_RET + BPF_K, 0), /* drop packet */ BPF_STMT(BPF_RET + BPF_K, (uint32_t) -1), /* accept packet */ }; static const struct sock_fprog fprog = { .len = ARRAY_SIZE(filter), .filter = (struct sock_filter*) filter, }; struct sockaddr_ll addr = { .sll_family = AF_PACKET, .sll_ifindex = 0, .sll_pkttype = PACKET_MULTICAST, }; struct igmp *igmp; int sfd; int opt; assert_return(cfg); if (!cfg->do_igmp) { debug(DBG_IGMP, "igmp snooping disabled"); return; } igmp = zmalloc(sizeof(*igmp)); if (!igmp) return; /* * Kernel limitation, must be ETH_P_ALL, not ETH_P_IP or we won't get * outgoing packets, https://lkml.org/lkml/1999/12/23/112 */ sfd = socket(AF_PACKET, SOCK_DGRAM | SOCK_CLOEXEC, htons(ETH_P_ALL)); if (sfd < 0) { if (errno == EACCES || errno == EPERM) info("Unable to do IGMP snooping, permission denied"); else error("socket: %m"); goto error; } if (setsockopt(sfd, SOL_SOCKET, SO_ATTACH_FILTER, &fprog, sizeof(fprog)) < 0) { error("setsockopt(SO_ATTACH_FILTER): %m"); goto error; } if (setsockopt(sfd, SOL_SOCKET, SO_LOCK_FILTER, &opt, sizeof(opt)) < 0) { error("setsockopt(SO_LOCK_FILTER): %m"); goto error; } if (cfg->igmp_iface) { struct ifreq ifreq; int r; r = snprintf(ifreq.ifr_name, sizeof(ifreq.ifr_name), "%s", cfg->igmp_iface); if (r < 0 || r >= sizeof(ifreq.ifr_name)) { error("invalid interface name"); goto error; } if (ioctl(sfd, SIOCGIFINDEX, &ifreq) < 0) { error("ioctl: %m"); goto error; } debug(DBG_IGMP, "using interface %s (%i)", cfg->igmp_iface, ifreq.ifr_ifindex); struct packet_mreq mreq = { .mr_ifindex = ifreq.ifr_ifindex, .mr_type = PACKET_MR_ALLMULTI }; if (setsockopt(sfd, SOL_PACKET, PACKET_ADD_MEMBERSHIP, &mreq, sizeof(mreq)) < 0) { error("setsockopt(PACKET_ADD_MEMBERSHIP): %m"); goto error; } } /* can't set .sll_protocol to htons(ETH_P_IP), see comment above */ if (bind(sfd, (struct sockaddr *)&addr, sizeof(addr)) < 0) { error("bind: %m"); goto error; } debug(DBG_IGMP, "init successful, using fd %i", sfd); uring_task_init(cfg, &igmp->task, "igmp", uring_parent(cfg), igmp_free); uring_task_set_fd(&igmp->task, sfd); uring_task_set_buf(&igmp->task, &igmp->tbuf); igmp->task.saddr.addrlen = sizeof(igmp->task.saddr.ll); uring_tbuf_recvmsg(cfg, &igmp->task, igmp_read_cb); cfg->igmp = igmp; return; error: close(sfd); xfree(igmp); }