From cfb468e44d9d8c46ccfdd80c11a0a523c61c05b4 Mon Sep 17 00:00:00 2001 From: Denis Kenzior Date: Tue, 26 Dec 2023 18:23:47 -0600 Subject: [PATCH] monitor: Move iwmon reading logic into main.c To support multiple nlmon sources, move the logic that reads from iwmon device into main.c instead of nlmon. nlmon.c now becomes agnostic of how the packets are actually obtained. Packets are fed in via high-level APIs such as nlmon_print_rtnl, nlmon_print_genl, nlmon_print_pae. --- monitor/main.c | 175 ++++++++++++++++++++++++++++++++++++++++++++++-- monitor/nlmon.c | 165 +-------------------------------------------- monitor/nlmon.h | 2 +- 3 files changed, 174 insertions(+), 168 deletions(-) diff --git a/monitor/main.c b/monitor/main.c index 0c5f0670..e9384e1b 100644 --- a/monitor/main.c +++ b/monitor/main.c @@ -32,10 +32,13 @@ #include #include #include +#include #include #include #include #include +#include +#include #include #ifndef ARPHRD_NETLINK @@ -68,11 +71,160 @@ static struct nlmon_config config; #define NLMON_TYPE "nlmon" #define NLMON_LEN 5 +static bool nlmon_receive(struct l_io *io, void *user_data) +{ + struct nlmon *nlmon = user_data; + struct msghdr msg; + struct sockaddr_ll sll; + struct iovec iov; + struct cmsghdr *cmsg; + struct timeval copy_tv; + const struct timeval *tv = NULL; + uint16_t proto_type; + unsigned char buf[8192]; + unsigned char control[32]; + ssize_t bytes_read; + int fd; + + fd = l_io_get_fd(io); + if (fd < 0) + return false; + + memset(&sll, 0, sizeof(sll)); + + memset(&iov, 0, sizeof(iov)); + iov.iov_base = buf; + iov.iov_len = sizeof(buf); + + memset(&msg, 0, sizeof(msg)); + msg.msg_name = &sll; + msg.msg_namelen = sizeof(sll); + msg.msg_iov = &iov; + msg.msg_iovlen = 1; + msg.msg_control = control; + msg.msg_controllen = sizeof(control); + + bytes_read = recvmsg(fd, &msg, 0); + if (bytes_read < 0) { + if (errno != EAGAIN && errno != EINTR) + return false; + + return true; + } + + if (sll.sll_hatype != ARPHRD_NETLINK) + return true; + + proto_type = ntohs(sll.sll_protocol); + + for (cmsg = CMSG_FIRSTHDR(&msg); cmsg; + cmsg = CMSG_NXTHDR(&msg, cmsg)) { + if (cmsg->cmsg_level == SOL_SOCKET && + cmsg->cmsg_type == SCM_TIMESTAMP) { + memcpy(©_tv, CMSG_DATA(cmsg), sizeof(copy_tv)); + tv = ©_tv; + } + } + + switch (proto_type) { + case NETLINK_ROUTE: + nlmon_print_rtnl(nlmon, tv, iov.iov_base, bytes_read); + break; + case NETLINK_GENERIC: + nlmon_print_genl(nlmon, tv, iov.iov_base, bytes_read); + break; + } + + return true; +} + +/* + * BPF filter to match skb->dev->type == 824 (ARPHRD_NETLINK) and + * either match skb->protocol == 0x0000 (NETLINK_ROUTE) or match + * skb->protocol == 0x0010 (NETLINK_GENERIC). + */ +static struct sock_filter mon_filter[] = { + { 0x28, 0, 0, 0xfffff01c }, /* ldh #hatype */ + { 0x15, 0, 3, 0x00000338 }, /* jne #824, drop */ + { 0x28, 0, 0, 0xfffff000 }, /* ldh #proto */ + { 0x15, 2, 0, 0000000000 }, /* jeq #0x0000, pass */ + { 0x15, 1, 0, 0x00000010 }, /* jeq #0x0010, pass */ + { 0x06, 0, 0, 0000000000 }, /* drop: ret #0 */ + { 0x06, 0, 0, 0xffffffff }, /* pass: ret #-1 */ +}; + +static const struct sock_fprog mon_fprog = { .len = 7, .filter = mon_filter }; + +static struct l_io *open_packet(const char *name) +{ + struct l_io *io; + struct sockaddr_ll sll; + struct packet_mreq mr; + struct ifreq ifr; + int fd, opt = 1; + + fd = socket(PF_PACKET, SOCK_RAW | SOCK_CLOEXEC | SOCK_NONBLOCK, 0); + if (fd < 0) { + perror("Failed to create packet socket"); + return NULL; + } + + strncpy(ifr.ifr_name, name, IFNAMSIZ - 1); + + if (ioctl(fd, SIOCGIFINDEX, &ifr) < 0) { + perror("Failed to get monitor index"); + close(fd); + return NULL; + } + + memset(&sll, 0, sizeof(sll)); + sll.sll_family = AF_PACKET; + sll.sll_protocol = htons(ETH_P_ALL); + sll.sll_ifindex = ifr.ifr_ifindex; + + if (bind(fd, (struct sockaddr *) &sll, sizeof(sll)) < 0) { + perror("Failed to bind packet socket"); + close(fd); + return NULL; + } + + memset(&mr, 0, sizeof(mr)); + mr.mr_ifindex = ifr.ifr_ifindex; + mr.mr_type = PACKET_MR_ALLMULTI; + + if (setsockopt(fd, SOL_PACKET, PACKET_ADD_MEMBERSHIP, + &mr, sizeof(mr)) < 0) { + perror("Failed to enable all multicast"); + close(fd); + return NULL; + } + + if (setsockopt(fd, SOL_SOCKET, SO_ATTACH_FILTER, + &mon_fprog, sizeof(mon_fprog)) < 0) { + perror("Failed to enable monitor filter"); + close(fd); + return NULL; + } + + if (setsockopt(fd, SOL_SOCKET, SO_TIMESTAMP, &opt, sizeof(opt)) < 0) { + perror("Failed to enable monitor timestamps"); + close(fd); + return NULL; + } + + io = l_io_new(fd); + + l_io_set_close_on_destroy(io, true); + + return io; +} + struct iwmon_interface { char *ifname; bool exists; struct l_netlink *rtnl; struct l_netlink *genl; + struct l_io *io; }; static struct iwmon_interface monitor_interface = { }; @@ -109,11 +261,23 @@ static void genl_parse(uint16_t type, const void *data, uint32_t len, if (id == 0) return; - if (!strcmp(name, NL80211_GENL_NAME)) { - nlmon = nlmon_open(ifname, id, writer_path, &config); - if (!nlmon) - l_main_quit(); - } + if (strcmp(name, NL80211_GENL_NAME)) + return; + + monitor_interface.io = open_packet(ifname); + if (!monitor_interface.io) + goto failed; + + nlmon = nlmon_open(id, writer_path, &config); + if (!nlmon) + goto failed; + + l_io_set_read_handler(monitor_interface.io, nlmon_receive, nlmon, NULL); + + return; + +failed: + l_main_quit(); } static void genl_notify(uint16_t type, const void *data, @@ -790,6 +954,7 @@ int main(int argc, char *argv[]) exit_status = l_main_run_with_signal(signal_handler, NULL); + l_io_destroy(monitor_interface.io); l_netlink_destroy(monitor_interface.rtnl); l_netlink_destroy(monitor_interface.genl); l_free(monitor_interface.ifname); diff --git a/monitor/nlmon.c b/monitor/nlmon.c index 72cd8ca7..5e88cdf1 100644 --- a/monitor/nlmon.c +++ b/monitor/nlmon.c @@ -96,7 +96,6 @@ enum msg_type { struct nlmon { uint16_t id; - struct l_io *io; struct l_io *pae_io; struct l_queue *req_list; struct pcap *pcap; @@ -8185,154 +8184,6 @@ void nlmon_print_genl(struct nlmon *nlmon, const struct timeval *tv, } } -static bool nlmon_receive(struct l_io *io, void *user_data) -{ - struct nlmon *nlmon = user_data; - struct msghdr msg; - struct sockaddr_ll sll; - struct iovec iov; - struct cmsghdr *cmsg; - struct timeval copy_tv; - const struct timeval *tv = NULL; - uint16_t proto_type; - unsigned char buf[8192]; - unsigned char control[32]; - ssize_t bytes_read; - int fd; - - fd = l_io_get_fd(io); - if (fd < 0) - return false; - - memset(&sll, 0, sizeof(sll)); - - memset(&iov, 0, sizeof(iov)); - iov.iov_base = buf; - iov.iov_len = sizeof(buf); - - memset(&msg, 0, sizeof(msg)); - msg.msg_name = &sll; - msg.msg_namelen = sizeof(sll); - msg.msg_iov = &iov; - msg.msg_iovlen = 1; - msg.msg_control = control; - msg.msg_controllen = sizeof(control); - - bytes_read = recvmsg(fd, &msg, 0); - if (bytes_read < 0) { - if (errno != EAGAIN && errno != EINTR) - return false; - - return true; - } - - if (sll.sll_hatype != ARPHRD_NETLINK) - return true; - - proto_type = ntohs(sll.sll_protocol); - - for (cmsg = CMSG_FIRSTHDR(&msg); cmsg; - cmsg = CMSG_NXTHDR(&msg, cmsg)) { - if (cmsg->cmsg_level == SOL_SOCKET && - cmsg->cmsg_type == SCM_TIMESTAMP) { - memcpy(©_tv, CMSG_DATA(cmsg), sizeof(copy_tv)); - tv = ©_tv; - } - } - - switch (proto_type) { - case NETLINK_ROUTE: - nlmon_print_rtnl(nlmon, tv, iov.iov_base, bytes_read); - break; - case NETLINK_GENERIC: - nlmon_print_genl(nlmon, tv, iov.iov_base, bytes_read); - break; - } - - return true; -} - -/* - * BPF filter to match skb->dev->type == 824 (ARPHRD_NETLINK) and - * either match skb->protocol == 0x0000 (NETLINK_ROUTE) or match - * skb->protocol == 0x0010 (NETLINK_GENERIC). - */ -static struct sock_filter mon_filter[] = { - { 0x28, 0, 0, 0xfffff01c }, /* ldh #hatype */ - { 0x15, 0, 3, 0x00000338 }, /* jne #824, drop */ - { 0x28, 0, 0, 0xfffff000 }, /* ldh #proto */ - { 0x15, 2, 0, 0000000000 }, /* jeq #0x0000, pass */ - { 0x15, 1, 0, 0x00000010 }, /* jeq #0x0010, pass */ - { 0x06, 0, 0, 0000000000 }, /* drop: ret #0 */ - { 0x06, 0, 0, 0xffffffff }, /* pass: ret #-1 */ -}; - -static const struct sock_fprog mon_fprog = { .len = 7, .filter = mon_filter }; - -static struct l_io *open_packet(const char *name) -{ - struct l_io *io; - struct sockaddr_ll sll; - struct packet_mreq mr; - struct ifreq ifr; - int fd, opt = 1; - - fd = socket(PF_PACKET, SOCK_RAW | SOCK_CLOEXEC | SOCK_NONBLOCK, 0); - if (fd < 0) { - perror("Failed to create packet socket"); - return NULL; - } - - strncpy(ifr.ifr_name, name, IFNAMSIZ - 1); - - if (ioctl(fd, SIOCGIFINDEX, &ifr) < 0) { - perror("Failed to get monitor index"); - close(fd); - return NULL; - } - - memset(&sll, 0, sizeof(sll)); - sll.sll_family = AF_PACKET; - sll.sll_protocol = htons(ETH_P_ALL); - sll.sll_ifindex = ifr.ifr_ifindex; - - if (bind(fd, (struct sockaddr *) &sll, sizeof(sll)) < 0) { - perror("Failed to bind packet socket"); - close(fd); - return NULL; - } - - memset(&mr, 0, sizeof(mr)); - mr.mr_ifindex = ifr.ifr_ifindex; - mr.mr_type = PACKET_MR_ALLMULTI; - - if (setsockopt(fd, SOL_PACKET, PACKET_ADD_MEMBERSHIP, - &mr, sizeof(mr)) < 0) { - perror("Failed to enable all multicast"); - close(fd); - return NULL; - } - - if (setsockopt(fd, SOL_SOCKET, SO_ATTACH_FILTER, - &mon_fprog, sizeof(mon_fprog)) < 0) { - perror("Failed to enable monitor filter"); - close(fd); - return NULL; - } - - if (setsockopt(fd, SOL_SOCKET, SO_TIMESTAMP, &opt, sizeof(opt)) < 0) { - perror("Failed to enable monitor timestamps"); - close(fd); - return NULL; - } - - io = l_io_new(fd); - - l_io_set_close_on_destroy(io, true); - - return io; -} - void nlmon_print_pae(struct nlmon *nlmon, const struct timeval *tv, uint8_t type, int index, const void *data, uint32_t size) @@ -8460,28 +8311,21 @@ static struct l_io *open_pae(void) return io; } -struct nlmon *nlmon_open(const char *ifname, uint16_t id, const char *pathname, +struct nlmon *nlmon_open(uint16_t id, const char *pathname, const struct nlmon_config *config) { struct nlmon *nlmon; - struct l_io *io, *pae_io; + struct l_io *pae_io; struct pcap *pcap; - io = open_packet(ifname); - if (!io) - return NULL; - pae_io = open_pae(); - if (!pae_io) { - l_io_destroy(io); + if (!pae_io) return NULL; - } if (pathname) { pcap = pcap_create(pathname); if (!pcap) { l_io_destroy(pae_io); - l_io_destroy(io); return NULL; } } else @@ -8490,11 +8334,9 @@ struct nlmon *nlmon_open(const char *ifname, uint16_t id, const char *pathname, nlmon = nlmon_create(id, config); - nlmon->io = io; nlmon->pae_io = pae_io; nlmon->pcap = pcap; - l_io_set_read_handler(nlmon->io, nlmon_receive, nlmon, NULL); l_io_set_read_handler(nlmon->pae_io, pae_receive, nlmon, NULL); wlan_iface_list = l_hashmap_new(); @@ -8507,7 +8349,6 @@ void nlmon_close(struct nlmon *nlmon) if (!nlmon) return; - l_io_destroy(nlmon->io); l_io_destroy(nlmon->pae_io); l_queue_destroy(nlmon->req_list, nlmon_req_free); diff --git a/monitor/nlmon.h b/monitor/nlmon.h index 96958c25..bb1a7c58 100644 --- a/monitor/nlmon.h +++ b/monitor/nlmon.h @@ -33,7 +33,7 @@ struct nlmon_config { bool read_only; }; -struct nlmon *nlmon_open(const char *ifname, uint16_t id, const char *pathname, +struct nlmon *nlmon_open(uint16_t id, const char *pathname, const struct nlmon_config *config); void nlmon_close(struct nlmon *nlmon);