kernel-pfroute: Implement enumeration of local subnets

src/libcharon/plugins/kernel_pfroute/kernel_pfroute_net.c

@@ -15,6 +15,7 @@
 
 #include <sys/types.h>
 #include <sys/socket.h>
+#include <sys/sysctl.h>
 #include <net/if.h>
 #include <net/if_dl.h>
 #include <ifaddrs.h>
@@ -1704,6 +1705,198 @@ METHOD(kernel_net_t, get_nexthop, host_t*,
 	return get_route(this, TRUE, dest, src, iface);
 }
 
+/**
+ * Get the number of set bits in the given netmask
+ */
+static uint8_t sockaddr_to_netmask(sockaddr_t *sockaddr, host_t *dst)
+{
+	uint8_t len = 0, i, byte, mask = 0;
+	struct sockaddr_storage ss;
+	char *addr;
+
+	/* at least some older FreeBSD versions send us shorter sockaddrs
+	 * with the family set to -1 (255) */
+	if (sockaddr->sa_family == 255)
+	{
+		memset(&ss, 0, sizeof(ss));
+		memcpy(&ss, sockaddr, sockaddr->sa_len);
+		/* use the address family and length of the destination as hint */
+		ss.ss_len = *dst->get_sockaddr_len(dst);
+		ss.ss_family = dst->get_family(dst);
+		sockaddr = (sockaddr_t*)&ss;
+	}
+
+	switch (sockaddr->sa_family)
+	{
+		case AF_INET:
+			len = 4;
+			addr = (char*)&((struct sockaddr_in*)sockaddr)->sin_addr;
+			break;
+		case AF_INET6:
+			len = 16;
+			addr = (char*)&((struct sockaddr_in6*)sockaddr)->sin6_addr;
+			break;
+		default:
+			break;
+	}
+
+	for (i = 0; i < len; i++)
+	{
+		byte = addr[i];
+
+		if (byte == 0x00)
+		{
+			break;
+		}
+		if (byte == 0xff)
+		{
+			mask += 8;
+		}
+		else
+		{
+			while (byte & 0x80)
+			{
+				mask++;
+				byte <<= 1;
+			}
+		}
+	}
+	return mask;
+}
+
+/** enumerator over subnets */
+typedef struct {
+	enumerator_t public;
+	/** sysctl result */
+	char *buf;
+	/** length of the complete result */
+	size_t len;
+	/** start of the current route entry */
+	char *current;
+	/** last subnet enumerated */
+	host_t *net;
+	/** interface of current net */
+	char *ifname;
+} subnet_enumerator_t;
+
+METHOD(enumerator_t, destroy_subnet_enumerator, void,
+	subnet_enumerator_t *this)
+{
+	DESTROY_IF(this->net);
+	free(this->ifname);
+	free(this->buf);
+	free(this);
+}
+
+METHOD(enumerator_t, enumerate_subnets, bool,
+	subnet_enumerator_t *this, host_t **net, uint8_t *mask, char **ifname)
+{
+	enumerator_t *enumerator;
+	struct rt_msghdr *rtm;
+	struct sockaddr *addr;
+	int type;
+
+	if (!this->current)
+	{
+		this->current = this->buf;
+	}
+	else
+	{
+		rtm = (struct rt_msghdr*)this->current;
+		this->current += rtm->rtm_msglen;
+		DESTROY_IF(this->net);
+		this->net = NULL;
+		free(this->ifname);
+		this->ifname = NULL;
+	}
+
+	for (; this->current < this->buf + this->len;
+		 this->current += rtm->rtm_msglen)
+	{
+		struct sockaddr *netmask;
+		uint8_t netbits = 0;
+
+		rtm = (struct rt_msghdr*)this->current;
+
+		if (rtm->rtm_version != RTM_VERSION)
+		{
+			continue;
+		}
+		if (rtm->rtm_flags & RTF_GATEWAY ||
+			rtm->rtm_flags & RTF_HOST ||
+			rtm->rtm_flags & RTF_REJECT)
+		{
+			continue;
+		}
+		enumerator = create_rtmsg_enumerator(rtm);
+		while (enumerator->enumerate(enumerator, &type, &addr))
+		{
+			if (type == RTAX_DST)
+			{
+				this->net = this->net ?: host_create_from_sockaddr(addr);
+			}
+			if (type == RTAX_NETMASK)
+			{
+				netmask = addr;
+			}
+			if (type == RTAX_IFP && addr->sa_family == AF_LINK)
+			{
+				struct sockaddr_dl *sdl = (struct sockaddr_dl*)addr;
+				free(this->ifname);
+				this->ifname = strndup(sdl->sdl_data, sdl->sdl_nlen);
+			}
+		}
+		if (this->net)
+		{
+			netbits = sockaddr_to_netmask(netmask, this->net);
+		}
+		enumerator->destroy(enumerator);
+
+		if (this->net && this->ifname)
+		{
+			*net = this->net;
+			*mask = netbits ?: this->net->get_address(this->net).len * 8;
+			*ifname = this->ifname;
+			return TRUE;
+		}
+	}
+	return FALSE;
+}
+
+METHOD(kernel_net_t, create_local_subnet_enumerator, enumerator_t*,
+	private_kernel_pfroute_net_t *this)
+{
+	subnet_enumerator_t *enumerator;
+	char *buf;
+	size_t len;
+	int mib[7] = {
+		CTL_NET, PF_ROUTE, 0, AF_UNSPEC, NET_RT_DUMP, 0, 0
+	};
+
+	if (sysctl(mib, countof(mib), NULL, &len, NULL, 0) < 0)
+	{
+		DBG2(DBG_KNL, "enumerating local subnets failed");
+		return enumerator_create_empty();
+	}
+	buf = malloc(len);
+	if (sysctl(mib, countof(mib), buf, &len, NULL, 0) < 0)
+	{
+		DBG2(DBG_KNL, "enumerating local subnets failed");
+		free(buf);
+		return enumerator_create_empty();
+	}
+
+	INIT(enumerator,
+		.public = {
+			.enumerate = (void*)_enumerate_subnets,
+			.destroy = _destroy_subnet_enumerator,
+		},
+		.buf = buf,
+		.len = len,
+	);
+	return &enumerator->public;
+}
+
 /**
  * Initialize a list of local addresses.
  */
@@ -1849,6 +2042,7 @@ kernel_pfroute_net_t *kernel_pfroute_net_create()
 				.get_features = _get_features,
 				.get_interface = _get_interface_name,
 				.create_address_enumerator = _create_address_enumerator,
+				.create_local_subnet_enumerator = _create_local_subnet_enumerator,
 				.get_source_addr = _get_source_addr,
 				.get_nexthop = _get_nexthop,
 				.add_ip = _add_ip,