1 files changed, 404 insertions, 0 deletions
diff --git a/nhrpd/netlink.c b/nhrpd/netlink.c
new file mode 100644
index 00000000..300e1f4b
--- /dev/null
+++ b/nhrpd/netlink.c
@@ -0,0 +1,404 @@
+/* NHRP netlink/neighbor table arpd code
+ * Copyright (c) 2014-2015 Timo Teräs
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <fcntl.h>
+#include <net/if.h>
+#include <netinet/ip.h>
+#include <netinet/if_ether.h>
+#include <linux/netlink.h>
+#include <linux/neighbour.h>
+#include <linux/netfilter/nfnetlink_log.h>
+#include <linux/if_tunnel.h>
+
+#include "thread.h"
+#include "nhrpd.h"
+#include "netlink.h"
+#include "znl.h"
+
+int netlink_nflog_group;
+static int netlink_log_fd = -1;
+static struct thread *netlink_log_thread;
+static int netlink_req_fd = -1;
+static int netlink_listen_fd = -1;
+
+typedef void (*netlink_dispatch_f)(struct nlmsghdr *msg, struct zbuf *zb);
+
+void netlink_update_binding(struct interface *ifp, union sockunion *proto, union sockunion *nbma)
+{
+	struct nlmsghdr *n;
+	struct ndmsg *ndm;
+	struct zbuf *zb = zbuf_alloc(512);
+
+	n = znl_nlmsg_push(zb, nbma ? RTM_NEWNEIGH : RTM_DELNEIGH, NLM_F_REQUEST | NLM_F_REPLACE | NLM_F_CREATE);
+	ndm = znl_push(zb, sizeof(*ndm));
+	*ndm = (struct ndmsg) {
+		.ndm_family = sockunion_family(proto),
+		.ndm_ifindex = ifp->ifindex,
+		.ndm_type = RTN_UNICAST,
+		.ndm_state = nbma ? NUD_REACHABLE : NUD_FAILED,
+	};
+	znl_rta_push(zb, NDA_DST, sockunion_get_addr(proto), family2addrsize(sockunion_family(proto)));
+	if (nbma)
+		znl_rta_push(zb, NDA_LLADDR, sockunion_get_addr(nbma), family2addrsize(sockunion_family(nbma)));
+	znl_nlmsg_complete(zb, n);
+	zbuf_send(zb, netlink_req_fd);
+	zbuf_recv(zb, netlink_req_fd);
+	zbuf_free(zb);
+}
+
+static void netlink_neigh_msg(struct nlmsghdr *msg, struct zbuf *zb)
+{
+	struct ndmsg *ndm;
+	struct rtattr *rta;
+	struct nhrp_cache *c;
+	struct interface *ifp;
+	struct zbuf payload;
+	union sockunion addr;
+	size_t len;
+	char buf[SU_ADDRSTRLEN];
+	int state;
+
+	ndm = znl_pull(zb, sizeof(*ndm));
+	if (!ndm) return;
+
+	sockunion_family(&addr) = AF_UNSPEC;
+	while ((rta = znl_rta_pull(zb, &payload)) != NULL) {
+		len = zbuf_used(&payload);
+		switch (rta->rta_type) {
+		case NDA_DST:
+			sockunion_set(&addr, ndm->ndm_family, zbuf_pulln(&payload, len), len);
+			break;
+		}
+	}
+
+	ifp = if_lookup_by_index(ndm->ndm_ifindex);
+	if (!ifp || sockunion_family(&addr) == AF_UNSPEC)
+		return;
+
+	c = nhrp_cache_get(ifp, &addr, 0);
+	if (!c)
+		return;
+
+	if (msg->nlmsg_type == RTM_GETNEIGH) {
+		debugf(NHRP_DEBUG_KERNEL, "Netlink: who-has %s dev %s",
+			sockunion2str(&addr, buf, sizeof buf),
+			ifp->name);
+
+		if (c->cur.type >= NHRP_CACHE_CACHED) {
+			nhrp_cache_set_used(c, 1);
+			netlink_update_binding(ifp, &addr, &c->cur.peer->vc->remote.nbma);
+		}
+	} else {
+		debugf(NHRP_DEBUG_KERNEL, "Netlink: update %s dev %s nud %x",
+			sockunion2str(&addr, buf, sizeof buf),
+			ifp->name, ndm->ndm_state);
+
+		state = (msg->nlmsg_type == RTM_NEWNEIGH) ? ndm->ndm_state : NUD_FAILED;
+		nhrp_cache_set_used(c, state == NUD_REACHABLE);
+	}
+}
+
+static const netlink_dispatch_f nldispatch[RTM_MAX] = {
+	[RTM_GETNEIGH] = netlink_neigh_msg,
+	[RTM_NEWNEIGH] = netlink_neigh_msg,
+	[RTM_DELNEIGH] = netlink_neigh_msg,
+};
+
+static int netlink_route_recv(struct thread *t)
+{
+	uint8_t buf[ZNL_BUFFER_SIZE];
+	int fd = THREAD_FD(t);
+	struct zbuf payload, zb;
+	struct nlmsghdr *n;
+
+	zbuf_init(&zb, buf, sizeof(buf), 0);
+	while (zbuf_recv(&zb, fd) > 0) {
+		while ((n = znl_nlmsg_pull(&zb, &payload)) != 0) {
+			debugf(NHRP_DEBUG_KERNEL, "Netlink: Received msg_type %u, msg_flags %u",
+				n->nlmsg_type, n->nlmsg_flags);
+			switch (n->nlmsg_type) {
+			case RTM_GETNEIGH:
+			case RTM_NEWNEIGH:
+			case RTM_DELNEIGH:
+				netlink_neigh_msg(n, &payload);
+				break;
+			}
+		}
+	}
+
+	thread_add_read(master, netlink_route_recv, 0, fd);
+
+	return 0;
+}
+
+static void netlink_log_register(int fd, int group)
+{
+	struct nlmsghdr *n;
+	struct nfgenmsg *nf;
+	struct nfulnl_msg_config_cmd cmd;
+	struct zbuf *zb = zbuf_alloc(512);
+
+	n = znl_nlmsg_push(zb, (NFNL_SUBSYS_ULOG<<8) | NFULNL_MSG_CONFIG, NLM_F_REQUEST | NLM_F_ACK);
+	nf = znl_push(zb, sizeof(*nf));
+	*nf = (struct nfgenmsg) {
+		.nfgen_family = AF_UNSPEC,
+		.version = NFNETLINK_V0,
+		.res_id = htons(group),
+	};
+	cmd.command = NFULNL_CFG_CMD_BIND;
+	znl_rta_push(zb, NFULA_CFG_CMD, &cmd, sizeof(cmd));
+	znl_nlmsg_complete(zb, n);
+
+	zbuf_send(zb, fd);
+	zbuf_free(zb);
+}
+
+static void netlink_log_indication(struct nlmsghdr *msg, struct zbuf *zb)
+{
+	struct nfgenmsg *nf;
+	struct rtattr *rta;
+	struct zbuf rtapl, pktpl;
+	struct interface *ifp;
+	struct nfulnl_msg_packet_hdr *pkthdr = NULL;
+	uint32_t *in_ndx = NULL;
+
+	nf = znl_pull(zb, sizeof(*nf));
+	if (!nf) return;
+
+	memset(&pktpl, 0, sizeof(pktpl));
+	while ((rta = znl_rta_pull(zb, &rtapl)) != NULL) {
+		switch (rta->rta_type) {
+		case NFULA_PACKET_HDR:
+			pkthdr = znl_pull(&rtapl, sizeof(*pkthdr));
+			break;
+		case NFULA_IFINDEX_INDEV:
+			in_ndx = znl_pull(&rtapl, sizeof(*in_ndx));
+			break;
+		case NFULA_PAYLOAD:
+			pktpl = rtapl;
+			break;
+		/* NFULA_HWHDR exists and is supposed to contain source
+		 * hardware address. However, for ip_gre it seems to be
+		 * the nexthop destination address if the packet matches
+		 * route. */
+		}
+	}
+
+	if (!pkthdr || !in_ndx || !zbuf_used(&pktpl))
+		return;
+
+	ifp = if_lookup_by_index(htonl(*in_ndx));
+	if (!ifp)
+		return;
+
+	nhrp_peer_send_indication(ifp, htons(pkthdr->hw_protocol), &pktpl);
+}
+
+static int netlink_log_recv(struct thread *t)
+{
+	uint8_t buf[ZNL_BUFFER_SIZE];
+	int fd = THREAD_FD(t);
+	struct zbuf payload, zb;
+	struct nlmsghdr *n;
+
+	netlink_log_thread = NULL;
+
+	zbuf_init(&zb, buf, sizeof(buf), 0);
+	while (zbuf_recv(&zb, fd) > 0) {
+		while ((n = znl_nlmsg_pull(&zb, &payload)) != 0) {
+			debugf(NHRP_DEBUG_KERNEL, "Netlink-log: Received msg_type %u, msg_flags %u",
+				n->nlmsg_type, n->nlmsg_flags);
+			switch (n->nlmsg_type) {
+			case (NFNL_SUBSYS_ULOG<<8) | NFULNL_MSG_PACKET:
+				netlink_log_indication(n, &payload);
+				break;
+			}
+		}
+	}
+
+	THREAD_READ_ON(master, netlink_log_thread, netlink_log_recv, 0, netlink_log_fd);
+
+	return 0;
+}
+
+void netlink_set_nflog_group(int nlgroup)
+{
+	if (netlink_log_fd >= 0) {
+		THREAD_OFF(netlink_log_thread);
+		close(netlink_log_fd);
+		netlink_log_fd = -1;
+	}
+	netlink_nflog_group = nlgroup;
+	if (nlgroup) {
+		netlink_log_fd = znl_open(NETLINK_NETFILTER,  0);
+		netlink_log_register(netlink_log_fd, nlgroup);
+		THREAD_READ_ON(master, netlink_log_thread, netlink_log_recv, 0, netlink_log_fd);
+	}
+}
+
+int netlink_init(void)
+{
+	netlink_req_fd = znl_open(NETLINK_ROUTE, 0);
+	netlink_listen_fd = znl_open(NETLINK_ROUTE, RTMGRP_NEIGH);
+	thread_add_read(master, netlink_route_recv, 0, netlink_listen_fd);
+
+	return 0;
+}
+
+int netlink_configure_arp(unsigned int ifindex, int pf)
+{
+	struct nlmsghdr *n;
+	struct ndtmsg *ndtm;
+	struct rtattr *rta;
+	struct zbuf *zb = zbuf_alloc(512);
+	int r;
+
+	n = znl_nlmsg_push(zb, RTM_SETNEIGHTBL, NLM_F_REQUEST | NLM_F_REPLACE);
+	ndtm = znl_push(zb, sizeof(*ndtm));
+	*ndtm = (struct ndtmsg) {
+		.ndtm_family = pf,
+	};
+
+	znl_rta_push(zb, NDTA_NAME, pf == AF_INET ? "arp_cache" : "ndisc_cache", 10);
+
+	rta = znl_rta_nested_push(zb, NDTA_PARMS);
+	znl_rta_push_u32(zb, NDTPA_IFINDEX, ifindex);
+	znl_rta_push_u32(zb, NDTPA_APP_PROBES, 1);
+	znl_rta_push_u32(zb, NDTPA_MCAST_PROBES, 0);
+	znl_rta_push_u32(zb, NDTPA_UCAST_PROBES, 0);
+	znl_rta_nested_complete(zb, rta);
+
+	znl_nlmsg_complete(zb, n);
+	r = zbuf_send(zb, netlink_req_fd);
+	zbuf_recv(zb, netlink_req_fd);
+	zbuf_free(zb);
+
+	return r;
+}
+
+static int __netlink_gre_get_data(struct zbuf *zb, struct zbuf *data, int ifindex)
+{
+	struct nlmsghdr *n;
+	struct ifinfomsg *ifi;
+	struct zbuf payload, rtapayload;
+	struct rtattr *rta;
+
+	debugf(NHRP_DEBUG_KERNEL, "netlink-link-gre: get-info %u", ifindex);
+
+	n = znl_nlmsg_push(zb, RTM_GETLINK, NLM_F_REQUEST);
+	ifi = znl_push(zb, sizeof(*ifi));
+	*ifi = (struct ifinfomsg) {
+		.ifi_index = ifindex,
+	};
+	znl_nlmsg_complete(zb, n);
+
+	if (zbuf_send(zb, netlink_req_fd) < 0 ||
+	    zbuf_recv(zb, netlink_req_fd) < 0)
+		return -1;
+
+	n = znl_nlmsg_pull(zb, &payload);
+	if (!n) return -1;
+
+	if (n->nlmsg_type != RTM_NEWLINK)
+		return -1;
+
+	ifi = znl_pull(&payload, sizeof(struct ifinfomsg));
+	if (!ifi)
+		return -1;
+
+	debugf(NHRP_DEBUG_KERNEL, "netlink-link-gre: ifindex %u, receive msg_type %u, msg_flags %u",
+		ifi->ifi_index, n->nlmsg_type, n->nlmsg_flags);
+
+	if (ifi->ifi_index != ifindex)
+		return -1;
+
+	while ((rta = znl_rta_pull(&payload, &rtapayload)) != NULL)
+		if (rta->rta_type == IFLA_LINKINFO)
+			break;
+	if (!rta) return -1;
+
+	payload = rtapayload;
+	while ((rta = znl_rta_pull(&payload, &rtapayload)) != NULL)
+		if (rta->rta_type == IFLA_INFO_DATA)
+			break;
+	if (!rta) return -1;
+
+	*data = rtapayload;
+	return 0;
+}
+
+void netlink_gre_get_info(unsigned int ifindex, uint32_t *gre_key, unsigned int *link_index, struct in_addr *saddr)
+{
+	struct zbuf *zb = zbuf_alloc(8192), data, rtapl;
+	struct rtattr *rta;
+
+	*link_index = 0;
+	*gre_key = 0;
+	saddr->s_addr = 0;
+
+	if (__netlink_gre_get_data(zb, &data, ifindex) < 0)
+		goto err;
+
+	while ((rta = znl_rta_pull(&data, &rtapl)) != NULL) {
+		switch (rta->rta_type) {
+		case IFLA_GRE_LINK:
+			*link_index = zbuf_get32(&rtapl);
+			break;
+		case IFLA_GRE_IKEY:
+		case IFLA_GRE_OKEY:
+			*gre_key = zbuf_get32(&rtapl);
+			break;
+		case IFLA_GRE_LOCAL:
+			saddr->s_addr = zbuf_get32(&rtapl);
+			break;
+		}
+	}
+err:
+	zbuf_free(zb);
+}
+
+void netlink_gre_set_link(unsigned int ifindex, unsigned int link_index)
+{
+	struct nlmsghdr *n;
+	struct ifinfomsg *ifi;
+	struct rtattr *rta_info, *rta_data, *rta;
+	struct zbuf *zr = zbuf_alloc(8192), data, rtapl;
+	struct zbuf *zb = zbuf_alloc(8192);
+	size_t len;
+
+	if (__netlink_gre_get_data(zr, &data, ifindex) < 0)
+		goto err;
+
+	n = znl_nlmsg_push(zb, RTM_NEWLINK, NLM_F_REQUEST);
+	ifi = znl_push(zb, sizeof(*ifi));
+	*ifi = (struct ifinfomsg) {
+		.ifi_index = ifindex,
+	};
+	rta_info = znl_rta_nested_push(zb, IFLA_LINKINFO);
+	znl_rta_push(zb, IFLA_INFO_KIND, "gre", 3);
+	rta_data = znl_rta_nested_push(zb, IFLA_INFO_DATA);
+
+	znl_rta_push_u32(zb, IFLA_GRE_LINK, link_index);
+	while ((rta = znl_rta_pull(&data, &rtapl)) != NULL) {
+		if (rta->rta_type == IFLA_GRE_LINK)
+			continue;
+		len = zbuf_used(&rtapl);
+		znl_rta_push(zb, rta->rta_type, zbuf_pulln(&rtapl, len), len);
+	}
+
+	znl_rta_nested_complete(zb, rta_data);
+	znl_rta_nested_complete(zb, rta_info);
+
+	znl_nlmsg_complete(zb, n);
+	zbuf_send(zb, netlink_req_fd);
+	zbuf_recv(zb, netlink_req_fd);
+err:
+	zbuf_free(zb);
+	zbuf_free(zr);
+}