Initialize pools from routes via netlink

5 files changed, 191 insertions, 29 deletions

diff --git a/lease.c b/lease.c
index fc8eebf..73028aa 100644
--- a/lease.c
+++ b/lease.c
@@ -2,12 +2,16 @@
 
 #include <arpa/inet.h>
 #include <inttypes.h>
+#include <libmnl/libmnl.h>
+#include <linux/rtnetlink.h>
+#include <stdbool.h>
 #include <stdint.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <sys/socket.h>
 #include <time.h>
 
+#include "common.h"
 #include "dbg.h"
 #include "khash.h"
 #include "lease.h"
@@ -19,6 +23,7 @@
 
 static struct ip_pool pool;
 static time_t gexpires = TIME_T_MAX;
+static bool synchronized;
 
 KHASH_MAP_INIT_WGKEY(leaseht, struct wg_dynamic_lease *)
 khash_t(leaseht) * leases_ht;
@@ -42,26 +47,31 @@ static time_t get_monotonic_time()
 	return monotime.tv_sec;
 }
 
-void leases_init(char *fname)
+void leases_init(char *fname, struct mnl_socket *nlsock)
 {
-	UNUSED(fname); /* TODO: open file and initialize from it */
+	struct nlmsghdr *nlh;
+	struct rtmsg *rtm;
+	char buf[MNL_NLMSG_HDRLEN + MNL_ALIGN(sizeof *rtm)];
+	unsigned int seq;
 
+	synchronized = false;
 	leases_ht = kh_init(leaseht);
-
 	ipp_init(&pool);
 
-	/* TODO: initialize pool properly from routes */
-	struct in_addr pool1_v4, pool2_v4;
-	struct in6_addr pool1_v6, pool2_v6;
-	inet_pton(AF_INET, "192.168.4.0", &pool1_v4);
-	inet_pton(AF_INET, "192.168.73.0", &pool2_v4);
-	inet_pton(AF_INET6, "2001:db8:1234::", &pool1_v6);
-	inet_pton(AF_INET6, "2001:db8:7777::", &pool2_v6);
-
-	ipp_addpool_v4(&pool, &pool1_v4, 28);
-	ipp_addpool_v4(&pool, &pool2_v4, 27);
-	ipp_addpool_v6(&pool, &pool1_v6, 124);
-	ipp_addpool_v6(&pool, &pool2_v6, 124);
+	nlh = mnl_nlmsg_put_header(buf);
+	nlh->nlmsg_type = RTM_GETROUTE;
+	nlh->nlmsg_flags = NLM_F_REQUEST | NLM_F_DUMP;
+	nlh->nlmsg_seq = seq = time(NULL);
+	rtm = mnl_nlmsg_put_extra_header(nlh, sizeof(struct rtmsg));
+	rtm->rtm_family = 0; /* both ipv4 and ipv6 */
+
+	if (mnl_socket_sendto(nlsock, nlh, nlh->nlmsg_len) < 0)
+		fatal("mnl_socket_sendto()");
+
+	leases_update_pools(nlsock);
+	synchronized = true;
+
+	UNUSED(fname); /* TODO: open file and initialize from it */
 }
 
 void leases_free()
@@ -221,7 +231,102 @@ int leases_refresh()
 	return MIN(INT_MAX / 1000, gexpires - cur_time);
 }
 
-void leases_update_pools(int fd)
+static int data_ipv4_attr_cb(const struct nlattr *attr, void *data)
+{
+	const struct nlattr **tb = data;
+	int type = mnl_attr_get_type(attr);
+
+	switch (type) {
+	case RTA_DST:
+	case RTA_GATEWAY:
+		if (mnl_attr_validate(attr, MNL_TYPE_U32) < 0) {
+			log_err("mnl_attr_validate: %s\n", strerror(errno));
+			return MNL_CB_ERROR;
+		}
+		break;
+	default:
+		return MNL_CB_OK;
+	}
+	tb[type] = attr;
+	return MNL_CB_OK;
+}
+
+static int data_ipv6_attr_cb(const struct nlattr *attr, void *data)
+{
+	const struct nlattr **tb = data;
+	int type = mnl_attr_get_type(attr);
+
+	switch (type) {
+	case RTA_DST:
+	case RTA_GATEWAY:
+		if (mnl_attr_validate2(attr, MNL_TYPE_BINARY,
+				       sizeof(struct in6_addr)) < 0) {
+			log_err("mnl_attr_validate: %s\n", strerror(errno));
+			return MNL_CB_ERROR;
+		}
+		break;
+	default:
+		return MNL_CB_OK;
+	}
+	tb[type] = attr;
+	return MNL_CB_OK;
+}
+
+static int process_nlpacket_cb(const struct nlmsghdr *nlh, void *data)
 {
-	UNUSED(fd);
+	struct nlattr *tb[RTA_MAX + 1] = {};
+	struct rtmsg *rm = mnl_nlmsg_get_payload(nlh);
+	UNUSED(data);
+
+	if (rm->rtm_family == AF_INET)
+		mnl_attr_parse(nlh, sizeof(*rm), data_ipv4_attr_cb, tb);
+	else if (rm->rtm_family == AF_INET6)
+		mnl_attr_parse(nlh, sizeof(*rm), data_ipv6_attr_cb, tb);
+
+	if (tb[RTA_GATEWAY])
+		return MNL_CB_OK;
+
+	if (!tb[RTA_DST]) {
+		debug("Netlink packet without RTA_DST, ignoring\n");
+		return MNL_CB_OK;
+	}
+
+	void *addr = mnl_attr_get_payload(tb[RTA_DST]);
+	if (rm->rtm_family == AF_INET6 &&
+	    (is_link_local(addr) || IN6_IS_ADDR_MULTICAST(addr)))
+		return MNL_CB_OK;
+
+	if (nlh->nlmsg_type == RTM_NEWROUTE) {
+		if (rm->rtm_family == AF_INET) {
+			if (ipp_addpool_v4(&pool, addr, rm->rtm_dst_len))
+				die("ipp_addpool_v4()\n");
+		} else if (rm->rtm_family == AF_INET6) {
+			if (ipp_addpool_v6(&pool, addr, rm->rtm_dst_len))
+				die("ipp_addpool_v6()\n");
+		}
+	} else if (nlh->nlmsg_type == RTM_DELROUTE) {
+		if (rm->rtm_family == AF_INET) {
+			if (ipp_removepool_v4(&pool, addr) && synchronized)
+				die("ipp_removepool_v4()\n");
+		} else if (rm->rtm_family == AF_INET6) {
+			if (ipp_removepool_v6(&pool, addr) && synchronized)
+				die("ipp_removepool_v6()\n");
+		}
+	}
+
+	return MNL_CB_OK;
+}
+
+void leases_update_pools(struct mnl_socket *nlsock)
+{
+	int ret;
+	char buf[MNL_SOCKET_BUFFER_SIZE];
+
+	while ((ret = mnl_socket_recvfrom(nlsock, buf, sizeof buf)) > 0) {
+		if (mnl_cb_run(buf, ret, 0, 0, process_nlpacket_cb, NULL) == -1)
+			fatal("mnl_cb_run()");
+	}
+
+	if (ret == -1 && errno != EAGAIN && errno != EWOULDBLOCK)
+		fatal("mnl_socket_recvfrom()");
 }
diff --git a/lease.h b/lease.h
index 95a7b58..3e1402d 100644
--- a/lease.h
+++ b/lease.h
@@ -8,7 +8,7 @@
 
 #include <stdint.h>
 #include <sys/socket.h>
-#include <time.h>
+#include <libmnl/libmnl.h>
 
 #include "common.h"
 #include "netlink.h"
@@ -23,9 +23,10 @@ struct wg_dynamic_lease {
 };
 
 /*
- * Initializes internal state, reads leases from fname.
+ * Initializes internal state, retrieves routes from nlsock and reads leases
+ * from fname.
  */
-void leases_init(char *fname);
+void leases_init(char *fname, struct mnl_socket *nlsock);
 
 /*
  * Frees everything, closes file.
@@ -56,8 +57,8 @@ bool extend_lease(struct wg_dynamic_lease *lease, uint32_t leasetime);
 int leases_refresh();
 
 /*
- * Updates all pools with information from the netlink file descriptor fd.
+ * Updates all pools with information from the mnl socket nlsock.
  */
-void leases_update_pools(int fd);
+void leases_update_pools(struct mnl_socket *nlsock);
 
 #endif
diff --git a/radix-trie.c b/radix-trie.c
index 18f656c..25bdd75 100644
--- a/radix-trie.c
+++ b/radix-trie.c
@@ -614,6 +614,18 @@ int ipp_addpool_v6(struct ip_pool *ipp, const struct in6_addr *ip, uint8_t cidr)
 	return ipp_addpool(ipp, &ipp->ip6_pool, &ipp->ip6_root, 128, key, cidr);
 }
 
+/* TODO: implement */
+int ipp_removepool_v4(struct ip_pool *pool, const struct in_addr *ip)
+{
+	return 0;
+}
+
+/* TODO: implement */
+int ipp_removepool_v6(struct ip_pool *pool, const struct in6_addr *ip)
+{
+	return 0;
+}
+
 void ipp_addnth_v4(struct ip_pool *pool, struct in_addr *dest, uint32_t index)
 {
 	struct radix_pool *current = pool->ip4_pool;
diff --git a/tests/netsh.sh b/tests/netsh.sh
index 3d98550..0376c14 100755
--- a/tests/netsh.sh
+++ b/tests/netsh.sh
@@ -73,6 +73,10 @@ configure_peers() {
 	ip2 link set up dev wg0
 
 	ip2 route add fe80::/128 dev wg0
+	ip1 route add 192.168.4.0/28 dev wg0
+	ip1 route add 192.168.73.0/27 dev wg0
+	ip1 route add 2001:db8:1234::/124 dev wg0
+	ip1 route add 2001:db8:7777::/124 dev wg0
 }
 configure_peers
 
diff --git a/wg-dynamic-server.c b/wg-dynamic-server.c
index e952be0..a5e707a 100644
--- a/wg-dynamic-server.c
+++ b/wg-dynamic-server.c
@@ -37,6 +37,7 @@ static struct wg_dynamic_request requests[MAX_CONNECTIONS] = { 0 };
 
 static int sockfd = -1;
 static int epollfd = -1;
+static struct mnl_socket *nlsock = NULL;
 
 KHASH_MAP_INIT_INT64(allowedht, wg_key *)
 khash_t(allowedht) * allowedips_ht;
@@ -372,7 +373,7 @@ static bool send_response(int fd, struct wg_dynamic_request *req)
 	return false;
 }
 
-static void setup_socket()
+static void setup_sockets()
 {
 	int val = 1, res;
 	struct sockaddr_in6 addr = {
@@ -398,6 +399,29 @@ static void setup_socket()
 
 	if (listen(sockfd, SOMAXCONN) == -1)
 		fatal("Listening to socket failed");
+
+	/* netlink route socket */
+	nlsock = mnl_socket_open(NETLINK_ROUTE);
+	if (!nlsock)
+		fatal("mnl_socket_open(NETLINK_ROUTE)");
+
+	res = fcntl(mnl_socket_get_fd(nlsock), F_GETFL, 0);
+	if (res < 0 ||
+	    fcntl(mnl_socket_get_fd(nlsock), F_SETFL, res | O_NONBLOCK) < 0)
+		fatal("Setting netlink socket to nonblocking failed");
+
+	if (mnl_socket_bind(nlsock, 0, MNL_SOCKET_AUTOPID) < 0)
+		fatal("mnl_socket_bind()");
+
+	val = RTNLGRP_IPV4_ROUTE;
+	if (mnl_socket_setsockopt(nlsock, NETLINK_ADD_MEMBERSHIP, &val,
+				  sizeof val) < 0)
+		fatal("mnl_socket_setsockopt()");
+
+	val = RTNLGRP_IPV6_ROUTE;
+	if (mnl_socket_setsockopt(nlsock, NETLINK_ADD_MEMBERSHIP, &val,
+				  sizeof val) < 0)
+		fatal("mnl_socket_setsockopt()");
 }
 
 static void cleanup()
@@ -406,6 +430,9 @@ static void cleanup()
 	kh_destroy(allowedht, allowedips_ht);
 	wg_free_device(device);
 
+	if (nlsock)
+		mnl_socket_close(nlsock);
+
 	if (sockfd >= 0)
 		close(sockfd);
 
@@ -425,7 +452,6 @@ static void setup()
 	if (inet_pton(AF_INET6, WG_DYNAMIC_ADDR, &well_known) != 1)
 		fatal("inet_pton()");
 
-	leases_init("leases_file");
 	allowedips_ht = kh_init(allowedht);
 
 	for (int i = 0; i < MAX_CONNECTIONS; ++i)
@@ -445,7 +471,8 @@ static void setup()
 		die("%s has no peers with link-local allowedips\n",
 		    wg_interface);
 
-	setup_socket(&sockfd);
+	setup_sockets();
+	leases_init("leases_file", nlsock);
 }
 
 static int get_avail_request()
@@ -489,13 +516,21 @@ static void accept_incoming(int sockfd, int epollfd,
 	}
 }
 
-static void handle_event(struct wg_dynamic_request *req, uint32_t events)
+static void handle_event(void *ptr, uint32_t events)
 {
-	if (!req) {
+	struct wg_dynamic_request *req;
+
+	if (ptr == &sockfd) {
 		accept_incoming(sockfd, epollfd, requests);
 		return;
 	}
 
+	if (ptr == nlsock) {
+		leases_update_pools(nlsock);
+		return;
+	}
+
+	req = (struct wg_dynamic_request *)ptr;
 	if (events & EPOLLIN) {
 		if (handle_request(req, send_response, send_error))
 			close_connection(req);
@@ -518,8 +553,13 @@ static void poll_loop()
 		fatal("epoll_create1()");
 
 	ev.events = EPOLLIN | EPOLLET;
-	ev.data.ptr = NULL;
-	if (epoll_ctl(epollfd, EPOLL_CTL_ADD, sockfd, &ev) == -1)
+	ev.data.ptr = &sockfd;
+	if (epoll_ctl(epollfd, EPOLL_CTL_ADD, sockfd, &ev))
+		fatal("epoll_ctl()");
+
+	ev.events = EPOLLIN;
+	ev.data.ptr = nlsock;
+	if (epoll_ctl(epollfd, EPOLL_CTL_ADD, mnl_socket_get_fd(nlsock), &ev))
 		fatal("epoll_ctl()");
 
 	while (1) {