diff options
| -rw-r--r-- | minecproxy/igmp.c | 228 |
1 files changed, 112 insertions, 116 deletions
diff --git a/minecproxy/igmp.c b/minecproxy/igmp.c index f3579f4..ae9ed10 100644 --- a/minecproxy/igmp.c +++ b/minecproxy/igmp.c @@ -22,78 +22,77 @@ struct igmp { struct uring_task_buf tbuf; }; -#define ETH_HDR_LEN 14 -#define IPV4_MIN_HDR_LEN 20 -#define IGMP_MIN_LEN 8 +#define ETH_HDR_LEN 14 +#define IPV4_MIN_HDR_LEN 20 +#define IGMP_MIN_LEN 8 struct _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 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 + 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 _big_endian_ { - enum igmp_type type:8; - unsigned unknown:8; - unsigned checksum:16; + enum igmp_type type : 8; + unsigned unknown : 8; + unsigned checksum : 16; } common; struct _big_endian_ { - enum igmp_type type:8; - unsigned resptime:8; - unsigned checksum:16; - unsigned addr:32; + enum igmp_type type : 8; + unsigned resptime : 8; + unsigned checksum : 16; + unsigned addr : 32; } v2; struct _big_endian_ { - enum igmp_type type:8; - unsigned reserved1:8; - unsigned checksum:16; - unsigned reserved2:16; - unsigned nrecs:16; + 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 + 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 _big_endian_ { - enum igmp_v3_record_type type:8; - unsigned auxlen:8; - unsigned nsrcs:16; - unsigned addr:32; + 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) +static inline unsigned short from32to16(unsigned int x) { /* add up 16-bit and 16-bit for 16+c bit */ x = (x & 0xffff) + (x >> 16); @@ -102,8 +101,7 @@ from32to16(unsigned int x) return x; } -static unsigned int -do_csum(const unsigned char *buf, int len) +static unsigned int do_csum(const unsigned char *buf, int len) { int odd; unsigned int result = 0; @@ -160,16 +158,14 @@ do_csum(const unsigned char *buf, int len) return result; } -static inline bool -csum_valid(const char *buf, size_t len) +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() +static void igmp_match() { debug(DBG_IGMP, "multicast request discovered"); /* @@ -179,8 +175,7 @@ igmp_match() announce_start(5 * 60); } -static void -igmp_parse(struct igmp *igmp) +static void igmp_parse(struct igmp *igmp) { char *buf; size_t len; @@ -259,7 +254,8 @@ igmp_parse(struct igmp *igmp) 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))); + 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; @@ -286,22 +282,20 @@ igmp_parse(struct igmp *igmp) 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, "igmp_v3_membership_report %s -> %s", src_str, + dst_str); - debug(DBG_IGMP, "IGMPv3" + debug(DBG_IGMP, + "IGMPv3" " type: %x," " reserved: %u," " csum: %u," " reserved: %u," " nrecs: %u," " size: %zu\n", - igmp_msg->v3.type, - igmp_msg->v3.reserved1, - igmp_msg->v3.checksum, - igmp_msg->v3.reserved2, - igmp_msg->v3.nrecs, - sizeof(igmp_msg->v3)); + 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"); @@ -317,7 +311,8 @@ igmp_parse(struct igmp *igmp) 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; + union igmp_v3_record *record = + (union igmp_v3_record *)pos; struct in_addr grp; char grp_str[INET_ADDRSTRLEN]; @@ -328,25 +323,24 @@ igmp_parse(struct igmp *igmp) 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, "received IGMPv3 record to %s", + grp_str); - debug(DBG_IGMP, "IGMPv3 rec, " + debug(DBG_IGMP, + "IGMPv3 rec, " " type: %u," " auxlen: %u," " nsrcs: %u," " addr: %s," " size: %zu bytes", - record->type, - record->auxlen, - record->nsrcs, - grp_str, - sizeof(*record)); + 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)) { + if (body_len < (record->nsrcs * sizeof(uint32_t) + + record->auxlen)) { error("IGMPv3 too short"); break; } @@ -356,7 +350,8 @@ igmp_parse(struct igmp *igmp) 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)); + inet_ntop(AF_INET, &grp_src, grp_src_str, + sizeof(grp_src_str)); debug(DBG_IGMP, "received IGMPv3 record src %s", grp_src_str); @@ -396,8 +391,7 @@ igmp_parse(struct igmp *igmp) len -= hdr->length; } -static void -igmp_read_cb(struct uring_task *task, int res) +static void igmp_read_cb(struct uring_task *task, int res) { struct igmp *igmp = container_of(task, struct igmp, task); @@ -421,8 +415,7 @@ igmp_read_cb(struct uring_task *task, int res) uring_tbuf_read(&igmp->task, igmp_read_cb); } -static void -igmp_free(struct uring_task *task) +static void igmp_free(struct uring_task *task) { struct igmp *igmp = container_of(task, struct igmp, task); @@ -431,16 +424,14 @@ igmp_free(struct uring_task *task) xfree(igmp); } -void -igmp_refdump() +void igmp_refdump() { assert_return_silent(cfg->igmp); uring_task_refdump(&cfg->igmp->task); } -void -igmp_delete() +void igmp_delete() { assert_return_silent(cfg->igmp); @@ -449,8 +440,7 @@ igmp_delete() cfg->igmp = NULL; } -void -igmp_init() +void igmp_init() { static const struct sock_filter filter[] = { /* A <- packet length */ @@ -458,32 +448,36 @@ igmp_init() /* A < sizeof(iphdr) */ BPF_JUMP(BPF_JMP + BPF_JGE + BPF_K, sizeof(struct iphdr), 1, 0), /* drop packet */ - BPF_STMT(BPF_RET + BPF_K, 0), + BPF_STMT(BPF_RET + BPF_K, 0), /* A <- version + ihl */ BPF_STMT(BPF_LD + BPF_B + BPF_ABS, 0 /* iphdr[0] */), /* A <- A >> 4 (version) */ BPF_STMT(BPF_ALU + BPF_RSH + BPF_K, 4), /* A != 4 */ - BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, 0x04, 1, 0), + BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, 0x04, 1, 0), /* drop packet */ - BPF_STMT(BPF_RET + BPF_K, 0), + BPF_STMT(BPF_RET + BPF_K, 0), /* A <- ip protocol */ - BPF_STMT(BPF_LD + BPF_B + BPF_ABS, offsetof(struct iphdr, protocol)), + BPF_STMT(BPF_LD + BPF_B + BPF_ABS, + offsetof(struct iphdr, protocol)), /* A != IPPROTO_IGMP */ - BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, IPPROTO_IGMP, 1, 0), + BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, IPPROTO_IGMP, 1, 0), /* drop packet */ - BPF_STMT(BPF_RET + BPF_K, 0), + BPF_STMT(BPF_RET + BPF_K, 0), /* A <- ip dst addr */ - BPF_STMT(BPF_LD + BPF_W + BPF_ABS, offsetof(struct iphdr, daddr)), + BPF_STMT(BPF_LD + BPF_W + BPF_ABS, + offsetof(struct iphdr, daddr)), /* A != 224.0.2.60 */ - BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, chtobe32(cinet_addr(224,0,2,60)), 2, 0), + BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, + chtobe32(cinet_addr(224,0,2,60)), 2, 0), /* A != 224.0.0.22 */ - BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, chtobe32(cinet_addr(224,0,0,22)), 1, 0), + BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_K, + chtobe32(cinet_addr(224,0,0,22)), 1, 0), /* drop packet */ - BPF_STMT(BPF_RET + BPF_K, 0), + BPF_STMT(BPF_RET + BPF_K, 0), /* X <- pkt->ihl * 4 */ BPF_STMT(BPF_LDX + BPF_B + BPF_MSH, 0 /* iphdr[0] */), @@ -492,23 +486,25 @@ igmp_init() /* A > X */ BPF_JUMP(BPF_JMP + BPF_JGT + BPF_X, 0, 0, 1), /* drop packet */ - BPF_STMT(BPF_RET + BPF_K, 0), + BPF_STMT(BPF_RET + BPF_K, 0), /* A <- ip tot_len */ - BPF_STMT(BPF_LD + BPF_H + BPF_ABS, offsetof(struct iphdr, tot_len)), + BPF_STMT(BPF_LD + BPF_H + BPF_ABS, + offsetof(struct iphdr, tot_len)), /* A <= ip->ihl * 4 */ BPF_JUMP(BPF_JMP + BPF_JGT + BPF_X, 0, 1, 0), /* drop packet */ - BPF_STMT(BPF_RET + BPF_K, 0), + BPF_STMT(BPF_RET + BPF_K, 0), /* A <- A - X (bodylen) */ BPF_STMT(BPF_ALU + BPF_SUB + BPF_X, 0), /* A < 8 */ BPF_JUMP(BPF_JMP + BPF_JGE + BPF_K, 8, 1, 0), /* drop packet */ - BPF_STMT(BPF_RET + BPF_K, 0), + BPF_STMT(BPF_RET + BPF_K, 0), /* A <- ip->tot_len */ - BPF_STMT(BPF_LD + BPF_H + BPF_ABS, offsetof(struct iphdr, tot_len)), + BPF_STMT(BPF_LD + BPF_H + BPF_ABS, + offsetof(struct iphdr, tot_len)), /* X <- A */ BPF_STMT(BPF_MISC + BPF_TAX, 0), /* A <- packet length */ @@ -516,15 +512,15 @@ igmp_init() /* A != ip->tot_len */ BPF_JUMP(BPF_JMP + BPF_JEQ + BPF_X, 0, 1, 0), /* drop packet */ - BPF_STMT(BPF_RET + BPF_K, 0), + BPF_STMT(BPF_RET + BPF_K, 0), /* accept packet */ - BPF_STMT(BPF_RET + BPF_K, (uint32_t) -1), + BPF_STMT(BPF_RET + BPF_K, (uint32_t)-1), + }; + static const struct sock_fprog fprog = { + .len = ARRAY_SIZE(filter), + .filter = (struct sock_filter *)filter, }; - 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, @@ -558,12 +554,14 @@ igmp_init() goto error; } - if (setsockopt(sfd, SOL_SOCKET, SO_ATTACH_FILTER, &fprog, sizeof(fprog)) < 0) { + 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) { + if (setsockopt(sfd, SOL_SOCKET, SO_LOCK_FILTER, &opt, sizeof(opt)) < + 0) { error("setsockopt(SO_LOCK_FILTER): %m"); goto error; } @@ -572,24 +570,22 @@ igmp_init() struct ifreq ifreq; int r; - r = snprintf(ifreq.ifr_name, sizeof(ifreq.ifr_name), - "%s", cfg->igmp_iface); + r = snprintf(ifreq.ifr_name, sizeof(ifreq.ifr_name), "%s", + cfg->igmp_iface); if (r < 0 || r >= sizeof(ifreq.ifr_name)) die("invalid interface name: %s", cfg->igmp_iface); if (ioctl(sfd, SIOCGIFINDEX, &ifreq) < 0) die("ioctl: %m"); - debug(DBG_IGMP, "using interface %s (%i)", - cfg->igmp_iface, ifreq.ifr_ifindex); + 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 - }; + 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) { + if (setsockopt(sfd, SOL_PACKET, PACKET_ADD_MEMBERSHIP, &mreq, + sizeof(mreq)) < 0) { error("setsockopt(PACKET_ADD_MEMBERSHIP): %m"); goto error; } @@ -607,7 +603,7 @@ igmp_init() uring_task_set_buf(&igmp->task, &igmp->tbuf); igmp->task.saddr.addrlen = sizeof(igmp->task.saddr.ll); uring_tbuf_recvmsg(&igmp->task, igmp_read_cb); - + cfg->igmp = igmp; return; |