Imported Upstream version 0.13

23 files changed, 8541 insertions, 0 deletions

diff --git a/nhrp/Makefile b/nhrp/Makefile
new file mode 100644
index 0000000..7c2560e
--- /dev/null
+++ b/nhrp/Makefile
@@ -0,0 +1,27 @@
+progs-y			+= opennhrp
+opennhrp-objs		+= libev.o opennhrp.o nhrp_address.o nhrp_packet.o \
+			   nhrp_peer.o nhrp_server.o nhrp_interface.o admin.o \
+			   sysdep_netlink.o sysdep_pfpacket.o \
+			   sysdep_syslog.o
+
+CFLAGS_libev.o		+= -Wno-unused -Wno-comment -Wno-parentheses
+CFLAGS_opennhrp.o	+= -DOPENNHRP_VERSION=\"$(FULL_VERSION)\" \
+			   -DOPENNHRP_ADMIN_SOCKET=\"$(STATEDIR)/opennhrp.socket\"
+LIBS_opennhrp		+= -lm
+ifeq ($(shell pkg-config --exists libcares && echo "yes"),yes)
+CFLAGS			+=$(shell pkg-config --cflags libcares)
+LIBS_opennhrp  		+=$(shell pkg-config --libs libcares)
+else
+LIBS_opennhrp  		+= -lcares
+endif
+
+progs-y			+= opennhrpctl
+opennhrpctl-objs	+= opennhrpctl.o
+CFLAGS_opennhrpctl.o	+= $(CFLAGS_opennhrp.o)
+
+CFLAGS_EXTRA		+= -I$(srctree)/include -Wno-strict-aliasing
+
+install:
+	$(INSTALLDIR) $(DESTDIR)$(SBINDIR)
+	$(INSTALL) $(addprefix $(obj)/,$(progs-y)) $(DESTDIR)$(SBINDIR)
+
diff --git a/nhrp/admin.c b/nhrp/admin.c
new file mode 100644
index 0000000..68a3e9e
--- /dev/null
+++ b/nhrp/admin.c
@@ -0,0 +1,609 @@
+/* admin.c - OpenNHRP administrative interface implementation
+ *
+ * Copyright (C) 2007-2009 Timo Teräs <timo.teras@iki.fi>
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 or later as
+ * published by the Free Software Foundation.
+ *
+ * See http://www.gnu.org/ for details.
+ */
+
+#include <ctype.h>
+#include <errno.h>
+#include <stdio.h>
+#include <fcntl.h>
+#include <malloc.h>
+#include <stdarg.h>
+#include <string.h>
+#include <unistd.h>
+#include <sys/un.h>
+#include <sys/socket.h>
+
+#include "nhrp_common.h"
+#include "nhrp_peer.h"
+#include "nhrp_address.h"
+#include "nhrp_interface.h"
+
+static struct ev_io accept_io;
+
+struct admin_remote {
+	struct ev_timer timeout;
+	struct ev_io io;
+	int num_read;
+	char cmd[512];
+};
+
+static int parse_word(const char **bufptr, size_t len, char *word)
+{
+	const char *buf = *bufptr;
+	int i, pos = 0;
+
+	while (isspace(buf[pos]) && buf[pos] != '\n' && buf[pos])
+		pos++;
+
+	if (buf[pos] == '\n' || buf[pos] == 0)
+		return FALSE;
+
+	for (i = 0; i < len-1 && !isspace(buf[pos+i]); i++)
+		word[i] = buf[pos+i];
+	word[i] = 0;
+
+	*bufptr += i + pos;
+	return TRUE;
+}
+
+
+static void admin_write(void *ctx, const char *format, ...)
+{
+	struct admin_remote *rmt = (struct admin_remote *) ctx;
+	char msg[1024];
+	va_list ap;
+	size_t len;
+
+	va_start(ap, format);
+	len = vsnprintf(msg, sizeof(msg), format, ap);
+	va_end(ap);
+
+	if (write(rmt->io.fd, msg, len) != len) {
+	}
+}
+
+static void admin_free_remote(struct admin_remote *rm)
+{
+	int fd = rm->io.fd;
+
+	ev_io_stop(&rm->io);
+	ev_timer_stop(&rm->timeout);
+	shutdown(fd, SHUT_RDWR);
+	close(fd);
+	free(rm);
+}
+
+static int admin_show_peer(void *ctx, struct nhrp_peer *peer)
+{
+	char buf[512], tmp[32];
+	char *str;
+	size_t len = sizeof(buf);
+	int i = 0, rel;
+
+	if (peer->interface != NULL)
+		i += snprintf(&buf[i], len - i,
+			"Interface: %s\n",
+			peer->interface->name);
+
+	i += snprintf(&buf[i], len - i,
+		"Type: %s\n"
+		"Protocol-Address: %s/%d\n",
+		nhrp_peer_type[peer->type],
+		nhrp_address_format(&peer->protocol_address, sizeof(tmp), tmp),
+		peer->prefix_length);
+
+	if (peer->next_hop_address.type != PF_UNSPEC) {
+		switch (peer->type) {
+		case NHRP_PEER_TYPE_SHORTCUT_ROUTE:
+		case NHRP_PEER_TYPE_LOCAL_ROUTE:
+			str = "Next-hop-Address";
+			break;
+		case NHRP_PEER_TYPE_LOCAL_ADDR:
+			str = "Alias-Address";
+			break;
+		default:
+			str = "NBMA-Address";
+			break;
+		}
+		i += snprintf(&buf[i], len - i, "%s: %s\n",
+			str,
+			nhrp_address_format(&peer->next_hop_address,
+					    sizeof(tmp), tmp));
+	}
+	if (peer->nbma_hostname) {
+		i += snprintf(&buf[i], len - i, "Hostname: %s\n",
+			      peer->nbma_hostname);
+	}
+	if (peer->next_hop_nat_oa.type != PF_UNSPEC) {
+		i += snprintf(&buf[i], len - i, "NBMA-NAT-OA-Address: %s\n",
+			nhrp_address_format(&peer->next_hop_nat_oa,
+					    sizeof(tmp), tmp));
+	}
+	if (peer->flags & (NHRP_PEER_FLAG_USED | NHRP_PEER_FLAG_UNIQUE |
+			   NHRP_PEER_FLAG_UP | NHRP_PEER_FLAG_LOWER_UP)) {
+		i += snprintf(&buf[i], len - i, "Flags:");
+		if (peer->flags & NHRP_PEER_FLAG_UNIQUE)
+			i += snprintf(&buf[i], len - i, " unique");
+
+		if (peer->flags & NHRP_PEER_FLAG_USED)
+			i += snprintf(&buf[i], len - i, " used");
+		if (peer->flags & NHRP_PEER_FLAG_UP)
+			i += snprintf(&buf[i], len - i, " up");
+		else if (peer->flags & NHRP_PEER_FLAG_LOWER_UP)
+			i += snprintf(&buf[i], len - i, " lower-up");
+		i += snprintf(&buf[i], len - i, "\n");
+	}
+	if (peer->expire_time) {
+		rel = (int) (peer->expire_time - ev_now());
+		if (rel >= 0) {
+			i += snprintf(&buf[i], len - i, "Expires-In: %d:%02d\n",
+				      rel / 60, rel % 60);
+		}
+	}
+
+	admin_write(ctx, "%s\n", buf);
+	return 0;
+}
+
+static void admin_free_selector(struct nhrp_peer_selector *sel)
+{
+	if (sel->hostname != NULL) {
+		free((void *) sel->hostname);
+		sel->hostname = NULL;
+	}
+}
+
+static int admin_parse_selector(void *ctx, const char *cmd,
+				struct nhrp_peer_selector *sel)
+{
+	char keyword[64], tmp[64];
+	struct nhrp_address address;
+	uint8_t prefix_length;
+
+	while (parse_word(&cmd, sizeof(keyword), keyword)) {
+		if (!parse_word(&cmd, sizeof(tmp), tmp)) {
+			admin_write(ctx,
+				    "Status: failed\n"
+				    "Reason: missing-argument\n"
+				    "Near-Keyword: '%s'\n",
+				    keyword);
+			return FALSE;
+		}
+
+		if (strcmp(keyword, "interface") == 0 ||
+		    strcmp(keyword, "iface") == 0 ||
+		    strcmp(keyword, "dev") == 0) {
+			if (sel->interface != NULL)
+				goto err_conflict;
+			sel->interface = nhrp_interface_get_by_name(tmp, FALSE);
+			if (sel->interface == NULL)
+				goto err_noiface;
+			continue;
+		} else if (strcmp(keyword, "host") == 0 ||
+			   strcmp(keyword, "hostname") == 0) {
+			if (sel->hostname != NULL)
+				goto err_conflict;
+			sel->hostname = strdup(tmp);
+			continue;
+		}
+
+		if (!nhrp_address_parse(tmp, &address, &prefix_length)) {
+			admin_write(ctx,
+				    "Status: failed\n"
+				    "Reason: invalid-address\n"
+				    "Near-Keyword: '%s'\n",
+				   keyword);
+			return FALSE;
+		}
+
+		if (strcmp(keyword, "protocol") == 0) {
+			if (sel->protocol_address.type != AF_UNSPEC)
+				goto err_conflict;
+			sel->protocol_address = address;
+			sel->prefix_length = prefix_length;
+		} else if (strcmp(keyword, "nbma") == 0) {
+			if (sel->next_hop_address.type != AF_UNSPEC)
+				goto err_conflict;
+			sel->type_mask &= ~BIT(NHRP_PEER_TYPE_SHORTCUT_ROUTE);
+			sel->next_hop_address = address;
+		} else if (strcmp(keyword, "local-protocol") == 0) {
+			if (sel->interface != NULL)
+				goto err_conflict;
+			sel->interface = nhrp_interface_get_by_protocol(&address);
+			if (sel->interface == NULL)
+				goto err_noiface;
+		} else if (strcmp(keyword, "local-nbma") == 0) {
+			if (sel->interface != NULL)
+				goto err_conflict;
+			sel->interface = nhrp_interface_get_by_nbma(&address);
+			if (sel->interface == NULL)
+				goto err_noiface;
+		} else {
+			admin_write(ctx,
+				    "Status: failed\n"
+				    "Reason: syntax-error\n"
+				    "Near-Keyword: '%s'\n",
+				    keyword);
+			return FALSE;
+		}
+	}
+	return TRUE;
+
+err_conflict:
+	admin_write(ctx,
+		    "Status: failed\n"
+		    "Reason: conflicting-keyword\n"
+		    "Near-Keyword: '%s'\n",
+		    keyword);
+	goto err;
+err_noiface:
+	admin_write(ctx,
+		    "Status: failed\n"
+		    "Reason: interface-not-found\n"
+		    "Near-Keyword: '%s'\n"
+		    "Argument: '%s'\n",
+		    keyword, tmp);
+err:
+	admin_free_selector(sel);
+	return FALSE;
+}
+
+static void admin_route_show(void *ctx, const char *cmd)
+{
+	struct nhrp_peer_selector sel;
+
+	memset(&sel, 0, sizeof(sel));
+	sel.type_mask = BIT(NHRP_PEER_TYPE_LOCAL_ROUTE);
+	if (!admin_parse_selector(ctx, cmd, &sel))
+		return;
+
+	admin_write(ctx, "Status: ok\n\n");
+	nhrp_peer_foreach(admin_show_peer, ctx, &sel);
+	admin_free_selector(&sel);
+}
+
+static void admin_cache_show(void *ctx, const char *cmd)
+{
+	struct nhrp_peer_selector sel;
+
+	memset(&sel, 0, sizeof(sel));
+	sel.type_mask = NHRP_PEER_TYPEMASK_ALL &
+		~BIT(NHRP_PEER_TYPE_LOCAL_ROUTE);
+	if (!admin_parse_selector(ctx, cmd, &sel))
+		return;
+
+	admin_write(ctx, "Status: ok\n\n");
+	nhrp_peer_foreach(admin_show_peer, ctx, &sel);
+	admin_free_selector(&sel);
+}
+
+static void admin_cache_purge(void *ctx, const char *cmd)
+{
+	struct nhrp_peer_selector sel;
+	int count = 0;
+
+	memset(&sel, 0, sizeof(sel));
+	sel.type_mask = NHRP_PEER_TYPEMASK_PURGEABLE;
+	if (!admin_parse_selector(ctx, cmd, &sel))
+		return;
+
+	nhrp_peer_foreach(nhrp_peer_purge_matching, &count, &sel);
+	admin_free_selector(&sel);
+
+	admin_write(ctx,
+		    "Status: ok\n"
+		    "Entries-Affected: %d\n",
+		    count);
+}
+
+static void admin_cache_lower_down(void *ctx, const char *cmd)
+{
+	struct nhrp_peer_selector sel;
+	int count = 0;
+
+	memset(&sel, 0, sizeof(sel));
+	sel.type_mask = NHRP_PEER_TYPEMASK_PURGEABLE;
+	if (!admin_parse_selector(ctx, cmd, &sel))
+		return;
+
+	nhrp_peer_foreach(nhrp_peer_lowerdown_matching, &count, &sel);
+	admin_free_selector(&sel);
+
+	admin_write(ctx,
+		    "Status: ok\n"
+		    "Entries-Affected: %d\n",
+		    count);
+}
+
+static void admin_cache_flush(void *ctx, const char *cmd)
+{
+	struct nhrp_peer_selector sel;
+	int count = 0;
+
+	memset(&sel, 0, sizeof(sel));
+	sel.type_mask = NHRP_PEER_TYPEMASK_REMOVABLE;
+	if (!admin_parse_selector(ctx, cmd, &sel))
+		return;
+
+	nhrp_peer_foreach(nhrp_peer_remove_matching, &count, &sel);
+	admin_free_selector(&sel);
+
+	admin_write(ctx,
+		    "Status: ok\n"
+		    "Entries-Affected: %d\n",
+		    count);
+}
+
+static int admin_show_interface(void *ctx, struct nhrp_interface *iface)
+{
+	char buf[512], tmp[32];
+	size_t len = sizeof(buf);
+	int i = 0;
+
+	i += snprintf(&buf[i], len - i,
+		"Interface: %s\n"
+		"Index: %d\n",
+		iface->name,
+		iface->index);
+
+	if (iface->protocol_address.addr_len != 0) {
+		i += snprintf(&buf[i], len - i,
+			"Protocol-Address: %s/%d\n",
+			nhrp_address_format(&iface->protocol_address, sizeof(tmp), tmp),
+			iface->protocol_address_prefix);
+	}
+
+	if (iface->flags) {
+		i += snprintf(&buf[i], len - i,
+			"Flags:%s%s%s%s%s\n",
+			(iface->flags & NHRP_INTERFACE_FLAG_NON_CACHING) ? " non-caching" : "",
+			(iface->flags & NHRP_INTERFACE_FLAG_SHORTCUT) ? " shortcut" : "",
+			(iface->flags & NHRP_INTERFACE_FLAG_REDIRECT) ? " redirect" : "",
+			(iface->flags & NHRP_INTERFACE_FLAG_SHORTCUT_DEST) ? " shortcut-dest" : "",
+			(iface->flags & NHRP_INTERFACE_FLAG_CONFIGURED) ? " configured" : "");
+	}
+
+	if (!(iface->flags & NHRP_INTERFACE_FLAG_CONFIGURED))
+		goto done;
+
+	i += snprintf(&buf[i], len - i,
+		"Holding-Time: %u\n"
+		"Route-Table: %u\n"
+		"GRE-Key: %u\n"
+		"MTU: %u\n",
+		iface->holding_time,
+		iface->route_table,
+		iface->gre_key,
+		iface->mtu);
+
+	if (iface->link_index) {
+		struct nhrp_interface *link;
+
+		i += snprintf(&buf[i], len - i, "Link-Index: %d\n", iface->link_index);
+		link = nhrp_interface_get_by_index(iface->link_index, FALSE);
+		if (link != NULL)
+			i += snprintf(&buf[i], len - i, "Link-Name: %s\n", link->name);
+	}
+
+	if (iface->nbma_address.addr_len != 0) {
+		i += snprintf(&buf[i], len - i,
+			"NBMA-MTU: %u\n"
+			"NBMA-Address: %s\n",
+			iface->nbma_mtu,
+			nhrp_address_format(&iface->nbma_address, sizeof(tmp), tmp));
+	}
+	if (iface->nat_cie.nbma_address.addr_len != 0) {
+		i += snprintf(&buf[i], len - i,
+			"NBMA-NAT-OA: %s\n",
+			nhrp_address_format(&iface->nat_cie.nbma_address, sizeof(tmp), tmp));
+	}
+done:
+	admin_write(ctx, "%s\n", buf);
+	return 0;
+}
+
+static void admin_interface_show(void *ctx, const char *cmd)
+{
+	admin_write(ctx, "Status: ok\n\n");
+	nhrp_interface_foreach(admin_show_interface, ctx);
+}
+
+static void admin_redirect_purge(void *ctx, const char *cmd)
+{
+	char keyword[64];
+	struct nhrp_address addr;
+	uint8_t prefix;
+	int count;
+
+	nhrp_address_set_type(&addr, PF_UNSPEC);
+
+	if (parse_word(&cmd, sizeof(keyword), keyword)) {
+		if (!nhrp_address_parse(keyword, &addr, &prefix)) {
+			admin_write(ctx,
+				    "Status: failed\n"
+				    "Reason: invalid-address\n"
+				    "Near-Keyword: '%s'\n",
+				    keyword);
+			return;
+		}
+	}
+
+	count = nhrp_rate_limit_clear(&addr, prefix);
+	admin_write(ctx,
+		    "Status: ok\n"
+		    "Entries-Affected: %d\n",
+		    count);
+}
+
+struct update_nbma {
+	struct nhrp_address addr;
+	int count;
+};
+
+static int update_nbma(void *ctx, struct nhrp_peer *p)
+{
+	struct update_nbma *un = (struct update_nbma *) ctx;
+
+	nhrp_peer_discover_nhs(p, &un->addr);
+	un->count++;
+
+	return 0;
+}
+
+static void admin_update_nbma(void *ctx, const char *cmd)
+{
+	char keyword[64];
+	struct nhrp_peer_selector sel;
+	struct update_nbma un;
+
+	memset(&sel, 0, sizeof(sel));
+	sel.type_mask = BIT(NHRP_PEER_TYPE_DYNAMIC_NHS);
+
+	if (!parse_word(&cmd, sizeof(keyword), keyword))
+		goto err;
+	if (!nhrp_address_parse(keyword, &sel.next_hop_address, NULL))
+		goto err;
+	if (!parse_word(&cmd, sizeof(keyword), keyword))
+		goto err;
+	if (!nhrp_address_parse(keyword, &un.addr, NULL))
+		goto err;
+
+	un.count = 0;
+	nhrp_peer_foreach(update_nbma, &un, &sel);
+
+	admin_write(ctx,
+		    "Status: ok\n"
+		    "Entries-Affected: %d\n",
+		    un.count);
+	return;
+err:
+	admin_write(ctx,
+		    "Status: failed\n"
+		    "Reason: syntax-error\n"
+		    "Near-Keyword: '%s'\n",
+		    keyword);
+	return;
+}
+
+static struct {
+	const char *command;
+	void (*handler)(void *ctx, const char *cmd);
+} admin_handler[] = {
+	{ "route show",		admin_route_show },
+	{ "show",		admin_cache_show },
+	{ "cache show",		admin_cache_show },
+	{ "flush",		admin_cache_flush },
+	{ "cache flush",	admin_cache_flush },
+	{ "purge",		admin_cache_purge },
+	{ "cache purge",	admin_cache_purge },
+	{ "cache lowerdown",	admin_cache_lower_down },
+	{ "interface show",	admin_interface_show },
+	{ "redirect purge",	admin_redirect_purge },
+	{ "update nbma",	admin_update_nbma },
+};
+
+static void admin_receive_cb(struct ev_io *w, int revents)
+{
+	struct admin_remote *rm = container_of(w, struct admin_remote, io);
+	int fd = rm->io.fd;
+	ssize_t len;
+	int i, cmdlen;
+
+	len = recv(fd, rm->cmd, sizeof(rm->cmd) - rm->num_read, MSG_DONTWAIT);
+	if (len < 0 && errno == EAGAIN)
+		return;
+	if (len <= 0)
+		goto err;
+
+	rm->num_read += len;
+	if (rm->num_read >= sizeof(rm->cmd))
+		goto err;
+
+	if (rm->cmd[rm->num_read-1] != '\n')
+		return;
+	rm->cmd[--rm->num_read] = 0;
+
+	for (i = 0; i < ARRAY_SIZE(admin_handler); i++) {
+		cmdlen = strlen(admin_handler[i].command);
+		if (rm->num_read >= cmdlen &&
+		    strncasecmp(rm->cmd, admin_handler[i].command, cmdlen) == 0) {
+			nhrp_debug("Admin: %s", rm->cmd);
+			admin_handler[i].handler(rm, &rm->cmd[cmdlen]);
+			break;
+		}
+	}
+	if (i >= ARRAY_SIZE(admin_handler)) {
+		admin_write(rm,
+			    "Status: error\n"
+			    "Reason: unrecognized command\n");
+	}
+
+err:
+	admin_free_remote(rm);
+}
+
+static void admin_timeout_cb(struct ev_timer *t, int revents)
+{
+	admin_free_remote(container_of(t, struct admin_remote, timeout));
+}
+
+static void admin_accept_cb(ev_io *w, int revents)
+{
+	struct admin_remote *rm;
+	struct sockaddr_storage from;
+	socklen_t fromlen = sizeof(from);
+	int cnx;
+
+	cnx = accept(w->fd, (struct sockaddr *) &from, &fromlen);
+	if (cnx < 0)
+		return;
+	fcntl(cnx, F_SETFD, FD_CLOEXEC);
+
+	rm = calloc(1, sizeof(struct admin_remote));
+
+	ev_io_init(&rm->io, admin_receive_cb, cnx, EV_READ);
+	ev_io_start(&rm->io);
+	ev_timer_init(&rm->timeout, admin_timeout_cb, 10.0, 0.);
+	ev_timer_start(&rm->timeout);
+}
+
+int admin_init(const char *opennhrp_socket)
+{
+	struct sockaddr_un sun;
+	int fd;
+
+	memset(&sun, 0, sizeof(sun));
+	sun.sun_family = AF_UNIX;
+	strncpy(sun.sun_path, opennhrp_socket, sizeof(sun.sun_path));
+
+	fd = socket(AF_UNIX, SOCK_STREAM, 0);
+	if (fd < 0)
+		return 0;
+
+	fcntl(fd, F_SETFD, FD_CLOEXEC);
+	unlink(opennhrp_socket);
+	if (bind(fd, (struct sockaddr *) &sun, sizeof(sun)) != 0)
+		goto err_close;
+
+	if (listen(fd, 5) != 0)
+		goto err_close;
+
+	ev_io_init(&accept_io, admin_accept_cb, fd, EV_READ);
+	ev_io_start(&accept_io);
+
+	return 1;
+
+err_close:
+	nhrp_error("Failed initialize admin socket [%s]: %s",
+		   opennhrp_socket, strerror(errno));
+	close(fd);
+	return 0;
+}
diff --git a/nhrp/afnum.h b/nhrp/afnum.h
new file mode 100644
index 0000000..2dc3d68
--- /dev/null
+++ b/nhrp/afnum.h
@@ -0,0 +1,29 @@
+/* afnum.h - RFC 1700 Address Family Number and
+ *           ethernet protocol number definitions
+ *
+ * Copyright (C) 2007 Timo Teräs <timo.teras@iki.fi>
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 or later as
+ * published by the Free Software Foundation.
+ *
+ * See http://www.gnu.org/ for details.
+ */
+
+#ifndef AFNUM_H
+#define AFNUM_H
+
+#include <linux/if_ether.h>
+#include "nhrp_defines.h"
+
+#define AFNUM_RESERVED		constant_htons(0)
+#define AFNUM_INET		constant_htons(1)
+#define AFNUM_INET6		constant_htons(2)
+
+#define ETH_P_NHRP		0x2001
+
+#define ETHPROTO_IP		constant_htons(ETH_P_IP)
+#define ETHPROTO_NHRP		constant_htons(ETH_P_NHRP)
+
+#endif
diff --git a/nhrp/libev.c b/nhrp/libev.c
new file mode 100644
index 0000000..c4af3b9
--- /dev/null
+++ b/nhrp/libev.c
@@ -0,0 +1,3 @@
+#include <string.h>
+#include "libev.h"
+#include "../libev/ev.c"
diff --git a/nhrp/libev.h b/nhrp/libev.h
new file mode 100644
index 0000000..f9f5f23
--- /dev/null
+++ b/nhrp/libev.h
@@ -0,0 +1,22 @@
+#define EV_STANDALONE 1
+#define EV_MULTIPLICITY 0
+#define EV_VERIFY 0
+
+#define EV_USE_CLOCK_SYSCALL 1
+#define EV_USE_SELECT 0
+#define EV_USE_POLL 1
+
+#define EV_IDLE_ENABLE 1
+
+/* Unused stuff, disabled for size optimization */
+#define EV_USE_INOTIFY 0
+#define EV_PERIODIC_ENABLE 0
+#define EV_EMBED_ENABLE 0
+#define EV_STAT_ENABLE 0
+#define EV_FORK_ENABLE 0
+#define EV_ASYNC_ENABLE 0
+
+/* Disable the "void *data;" member of watchers to save memory */
+#define EV_COMMON /* empty */
+
+#include "../libev/ev.h"
diff --git a/nhrp/list.h b/nhrp/list.h
new file mode 100644
index 0000000..4387970
--- /dev/null
+++ b/nhrp/list.h
@@ -0,0 +1,184 @@
+/* list.h - Single and double linked list macros
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 or later as
+ * published by the Free Software Foundation.
+ *
+ * See http://www.gnu.org/ for details.
+ *
+ * This is more or less based on the code in the linux kernel. There are
+ * minor differences and this is only a subset of the kernel version.
+ */
+
+#ifndef LIST_H
+#define LIST_H
+
+#ifndef NULL
+#define NULL 0L
+#endif
+
+#ifndef container_of
+#define container_of(ptr, type, member) ({                      \
+        const typeof( ((type *)0)->member ) *__mptr = (ptr);    \
+        (type *)( (char *)__mptr - offsetof(type,member) );})
+#endif
+
+struct hlist_head {
+	struct hlist_node *first;
+};
+
+struct hlist_node {
+	struct hlist_node *next;
+	struct hlist_node **pprev;
+};
+
+static inline int hlist_empty(const struct hlist_head *h)
+{
+	return !h->first;
+}
+
+static inline int hlist_hashed(const struct hlist_node *n)
+{
+	return n->pprev != NULL;
+}
+
+static inline void hlist_del(struct hlist_node *n)
+{
+	struct hlist_node *next = n->next;
+	struct hlist_node **pprev = n->pprev;
+
+	*pprev = next;
+	if (next)
+		next->pprev = pprev;
+
+	n->next = NULL;
+	n->pprev = NULL;
+}
+
+static inline void hlist_add_head(struct hlist_node *n, struct hlist_head *h)
+{
+	struct hlist_node *first = h->first;
+
+	n->next = first;
+	if (first)
+		first->pprev = &n->next;
+	n->pprev = &h->first;
+	h->first = n;
+}
+
+static inline void hlist_add_after(struct hlist_node *n, struct hlist_node *prev)
+{
+	n->next = prev->next;
+	n->pprev = &prev->next;
+	prev->next = n;
+}
+
+static inline struct hlist_node **hlist_tail_ptr(struct hlist_head *h)
+{
+	struct hlist_node *n = h->first;
+	if (n == NULL)
+		return &h->first;
+	while (n->next != NULL)
+		n = n->next;
+	return &n->next;
+}
+
+#define hlist_entry(ptr, type, member) container_of(ptr,type,member)
+
+#define hlist_for_each(pos, head) \
+	for (pos = (head)->first; pos; pos = pos->next)
+
+#define hlist_for_each_safe(pos, n, head) \
+	for (pos = (head)->first; pos && ({ n = pos->next; 1; }); pos = n)
+
+#define hlist_for_each_entry(tpos, pos, head, member)			 \
+	for (pos = (head)->first; pos && 				 \
+		({ tpos = hlist_entry(pos, typeof(*tpos), member); 1;}); \
+	     pos = pos->next)
+
+#define hlist_for_each_entry_safe(tpos, pos, n, head, member) 		 \
+	for (pos = (head)->first;					 \
+	     pos && ({ n = pos->next; 1; }) &&				 \
+		({ tpos = hlist_entry(pos, typeof(*tpos), member); 1;}); \
+	     pos = n)
+
+
+struct list_head {
+	struct list_head *next, *prev;
+};
+
+#define LIST_INITIALIZER(l) { .next = &l, .prev = &l }
+
+static inline void list_init(struct list_head *list)
+{
+	list->next = list;
+	list->prev = list;
+}
+
+static inline void __list_add(struct list_head *new,
+			      struct list_head *prev,
+			      struct list_head *next)
+{
+	next->prev = new;
+	new->next = next;
+	new->prev = prev;
+	prev->next = new;
+}
+
+static inline void list_add(struct list_head *new, struct list_head *head)
+{
+	__list_add(new, head, head->next);
+}
+
+static inline void list_add_tail(struct list_head *new, struct list_head *head)
+{
+	__list_add(new, head->prev, head);
+}
+
+static inline void __list_del(struct list_head * prev, struct list_head * next)
+{
+	next->prev = prev;
+	prev->next = next;
+}
+
+static inline void list_del(struct list_head *entry)
+{
+	__list_del(entry->prev, entry->next);
+	entry->next = NULL;
+	entry->prev = NULL;
+}
+
+static inline int list_hashed(const struct list_head *n)
+{
+	return n->next != n && n->next != NULL;
+}
+
+static inline int list_empty(const struct list_head *n)
+{
+	return !list_hashed(n);
+}
+
+#define list_next(ptr, type, member) \
+	(list_hashed(ptr) ? container_of((ptr)->next,type,member) : NULL)
+
+#define list_entry(ptr, type, member) container_of(ptr,type,member)
+
+#define list_for_each(pos, head) \
+	for (pos = (head)->next; pos != (head); pos = pos->next)
+
+#define list_for_each_safe(pos, n, head) \
+	for (pos = (head)->next, n = pos->next; pos != (head); \
+		pos = n, n = pos->next)
+
+#define list_for_each_entry(pos, head, member)				\
+	for (pos = list_entry((head)->next, typeof(*pos), member);	\
+	     &pos->member != (head); 					\
+	     pos = list_entry(pos->member.next, typeof(*pos), member))
+
+#define list_for_each_entry_safe(pos, n, head, member)			\
+	for (pos = list_entry((head)->next, typeof(*pos), member),	\
+		n = list_entry(pos->member.next, typeof(*pos), member);	\
+	     &pos->member != (head); 					\
+	     pos = n, n = list_entry(n->member.next, typeof(*n), member))
+
+#endif
diff --git a/nhrp/nhrp_address.c b/nhrp/nhrp_address.c
new file mode 100644
index 0000000..13164e1
--- /dev/null
+++ b/nhrp/nhrp_address.c
@@ -0,0 +1,454 @@
+/* nhrp_address.c - NHRP address conversion functions
+ *
+ * Copyright (C) 2007-2009 Timo Teräs <timo.teras@iki.fi>
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 or later as
+ * published by the Free Software Foundation.
+ *
+ * See http://www.gnu.org/ for details.
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <netdb.h>
+#include <arpa/inet.h>
+#include <arpa/nameser.h>
+#include <linux/ip.h>
+
+#include <ares.h>
+#include <ares_version.h>
+
+#include "afnum.h"
+#include "nhrp_address.h"
+#include "nhrp_packet.h"
+#include "nhrp_common.h"
+
+struct nhrp_resolver {
+	ares_channel channel;
+	struct ev_prepare prepare;
+	struct ev_timer timeout;
+	struct ev_io fds[4];
+};
+
+static struct nhrp_resolver resolver;
+
+static void ares_timeout_cb(struct ev_timer *w, int revents)
+{
+	struct nhrp_resolver *r =
+		container_of(w, struct nhrp_resolver, timeout);
+
+	ares_process(r->channel, NULL, NULL);
+}
+
+static void ares_prepare_cb(struct ev_prepare *w, int revents)
+{
+	struct nhrp_resolver *r =
+		container_of(w, struct nhrp_resolver, prepare);
+	struct timeval *tv, tvbuf;
+
+	tv = ares_timeout(r->channel, NULL, &tvbuf);
+	if (tv != NULL) {
+		r->timeout.repeat = tv->tv_sec + tv->tv_usec * 1e-6;
+		ev_timer_again(&r->timeout);
+	} else {
+		ev_timer_stop(&r->timeout);
+	}
+}
+
+static void ares_io_cb(struct ev_io *w, int revents)
+{
+	ares_socket_t rfd = ARES_SOCKET_BAD, wfd = ARES_SOCKET_BAD;
+
+	if (revents & EV_READ)
+		rfd = w->fd;
+	if (revents & EV_WRITE)
+		wfd = w->fd;
+
+	ares_process_fd(resolver.channel, rfd, wfd);
+}
+
+static void ares_socket_cb(void *data, ares_socket_t fd,
+			   int readable, int writable)
+{
+	struct nhrp_resolver *r = (struct nhrp_resolver *) data;
+	int i, fi = -1, events = 0;
+
+	if (readable)
+		events |= EV_READ;
+	if (writable)
+		events |= EV_WRITE;
+
+	for (i = 0; i < ARRAY_SIZE(r->fds); i++) {
+		if (r->fds[i].fd == fd)
+			break;
+		if (fi < 0 && r->fds[i].fd == 0)
+			fi = i;
+	}
+
+	if (events) {
+		if (i >= ARRAY_SIZE(r->fds)) {
+			NHRP_BUG_ON(fi == -1);
+			i = fi;
+		} else {
+			ev_io_stop(&r->fds[fi]);
+		}
+		ev_io_set(&r->fds[i], fd, events);
+		ev_io_start(&r->fds[i]);
+	} else if (i < ARRAY_SIZE(r->fds)) {
+		ev_io_stop(&r->fds[i]);
+		ev_io_set(&r->fds[i], 0, 0);
+	}
+}
+
+static int bitcmp(const uint8_t *a, const uint8_t *b, int len)
+{
+	int bytes, bits, mask, r;
+
+	bytes = len / 8;
+	bits  = len % 8;
+
+	if (bytes != 0) {
+		r = memcmp(a, b, bytes);
+		if (r != 0)
+			return r;
+	}
+	if (bits != 0) {
+		mask = (0xff << (8 - bits)) & 0xff;
+		return ((int) (a[bytes] & mask)) - ((int) (b[bytes] & mask));
+	}
+	return 0;
+}
+
+uint16_t nhrp_protocol_from_pf(uint16_t pf)
+{
+	switch (pf) {
+	case PF_INET:
+		return ETHPROTO_IP;
+	}
+	return 0;
+}
+
+uint16_t nhrp_pf_from_protocol(uint16_t protocol)
+{
+	switch (protocol) {
+	case ETHPROTO_IP:
+		return PF_INET;
+	}
+	return PF_UNSPEC;
+}
+
+uint16_t nhrp_afnum_from_pf(uint16_t pf)
+{
+	switch (pf) {
+	case PF_INET:
+		return AFNUM_INET;
+	}
+	return AFNUM_RESERVED;
+}
+
+uint16_t nhrp_pf_from_afnum(uint16_t afnum)
+{
+	switch (afnum) {
+	case AFNUM_INET:
+		return PF_INET;
+	}
+	return PF_UNSPEC;
+}
+
+int nhrp_address_parse(const char *string,
+		       struct nhrp_address *addr,
+		       uint8_t *prefix_len)
+{
+	uint8_t tmp;
+	int r;
+
+	/* Try IP address format */
+	r = sscanf(string, "%hhd.%hhd.%hhd.%hhd/%hhd",
+		   &addr->addr[0], &addr->addr[1],
+		   &addr->addr[2], &addr->addr[3],
+		   prefix_len ? prefix_len : &tmp);
+	if ((r == 4) || (r == 5 && prefix_len != NULL)) {
+		addr->type = PF_INET;
+		addr->addr_len = 4;
+		addr->subaddr_len = 0;
+		if (r == 4 && prefix_len != NULL)
+			*prefix_len = 32;
+		return TRUE;
+	}
+
+	return FALSE;
+}
+
+int nhrp_address_parse_packet(uint16_t protocol, size_t len, uint8_t *packet,
+			      struct nhrp_address *src, struct nhrp_address *dst)
+{
+	int pf;
+	struct iphdr *iph;
+
+	pf = nhrp_pf_from_protocol(protocol);
+	switch (protocol) {
+	case ETHPROTO_IP:
+		if (len < sizeof(struct iphdr))
+			return FALSE;
+
+		iph = (struct iphdr *) packet;
+		if (src != NULL)
+			nhrp_address_set(src, pf, 4, (uint8_t *) &iph->saddr);
+		if (dst != NULL)
+			nhrp_address_set(dst, pf, 4, (uint8_t *) &iph->daddr);
+		break;
+	default:
+		return FALSE;
+	}
+
+	return TRUE;
+}
+
+#if ARES_VERSION_MAJOR > 1 || ARES_VERSION_MINOR > 4
+static void ares_address_cb(void *arg, int status, int timeouts,
+			    struct hostent *he)
+#else
+static void ares_address_cb(void *arg, int status, struct hostent *he)
+#endif
+{
+	struct nhrp_address_query *query =
+		(struct nhrp_address_query *) arg;
+	struct nhrp_address addr[16];
+	int i;
+
+	if (status == ARES_SUCCESS) {
+		for (i = 0; he->h_addr_list[i] != NULL &&
+			    i < ARRAY_SIZE(addr); i++)
+			nhrp_address_set(&addr[i], AF_INET, he->h_length,
+					 (uint8_t *) he->h_addr_list[i]);
+	} else
+		i = -1;
+
+	NHRP_BUG_ON(query->callback == NULL);
+
+	query->callback(query, i, &addr[0]);
+	query->callback = NULL;
+}
+
+void nhrp_address_resolve(struct nhrp_address_query *query,
+			  const char *hostname,
+			  nhrp_address_query_callback callback)
+{
+	if (query->callback != NULL) {
+		nhrp_error("Trying to resolve '%s', but previous query "
+			   "was not finished yet", hostname);
+		return;
+	}
+
+	query->callback = callback;
+	ares_gethostbyname(resolver.channel, hostname, AF_INET,
+			   ares_address_cb, query);
+}
+
+void nhrp_address_resolve_cancel(struct nhrp_address_query *query)
+{
+	/* The kills all active queries; not just the one
+	 * given as parameter. But as those will be retried later
+	 * anyway, it is not a problem for now. */
+
+	if (query->callback != NULL)
+		ares_cancel(resolver.channel);
+}
+
+void nhrp_address_set_type(struct nhrp_address *addr, uint16_t type)
+{
+	addr->type = type;
+	addr->addr_len = addr->subaddr_len = 0;
+}
+
+int nhrp_address_set(struct nhrp_address *addr, uint16_t type, uint8_t len, uint8_t *bytes)
+{
+	if (len > NHRP_MAX_ADDRESS_LEN)
+		return FALSE;
+
+	addr->type = type;
+	addr->addr_len = len;
+	addr->subaddr_len = 0;
+	if (len != 0)
+		memcpy(addr->addr, bytes, len);
+	return TRUE;
+}
+
+int nhrp_address_set_full(struct nhrp_address *addr, uint16_t type,
+			  uint8_t len, uint8_t *bytes,
+			  uint8_t sublen, uint8_t *subbytes)
+{
+	if (len + sublen > NHRP_MAX_ADDRESS_LEN)
+		return FALSE;
+
+	addr->type = type;
+	addr->addr_len = len;
+	addr->subaddr_len = 0;
+	if (len != 0)
+		memcpy(addr->addr, bytes, len);
+	if (sublen != 0)
+		memcpy(&addr->addr[len], subbytes, sublen);
+	return TRUE;
+}
+
+int nhrp_address_cmp(const struct nhrp_address *a, const struct nhrp_address *b)
+{
+	if (a->type > b->type)
+		return 1;
+	if (a->type < b->type)
+		return -1;
+	if (a->addr_len > b->addr_len || a->subaddr_len > b->subaddr_len)
+		return 1;
+	if (a->addr_len < b->addr_len || a->subaddr_len < b->subaddr_len)
+		return -1;
+	return memcmp(a->addr, b->addr, a->addr_len + a->subaddr_len);
+}
+
+int nhrp_address_prefix_cmp(const struct nhrp_address *a,
+			    const struct nhrp_address *b, int prefix)
+{
+	if (a->type > b->type)
+		return 1;
+	if (a->type < b->type)
+		return -1;
+	if (a->addr_len * 8 < prefix)
+		return 1;
+	if (b->addr_len * 8 < prefix)
+		return 1;
+	return bitcmp(a->addr, b->addr, prefix);
+}
+
+int nhrp_address_is_multicast(const struct nhrp_address *addr)
+{
+	switch (addr->type) {
+	case PF_INET:
+		if ((addr->addr[0] & 0xf0) == 0xe0)
+			return TRUE;
+		break;
+	}
+	return FALSE;
+}
+
+int nhrp_address_is_any_addr(const struct nhrp_address *addr)
+{
+	switch (addr->type) {
+	case PF_UNSPEC:
+		return TRUE;
+	case PF_INET:
+		if (memcmp(addr->addr, "\x00\x00\x00\x00", 4) == 0)
+			return TRUE;
+		break;
+	}
+	return FALSE;
+}
+
+unsigned int nhrp_address_hash(const struct nhrp_address *addr)
+{
+	unsigned int hash = 5381;
+	int i;
+
+	for (i = 0; i < addr->addr_len; i++)
+		hash = hash * 33 + addr->addr[i];
+
+	return hash;
+}
+
+void nhrp_address_set_network(struct nhrp_address *addr, int prefix)
+{
+	int i, bits = 8 * addr->addr_len;
+
+	for (i = prefix; i < bits; i++)
+		addr->addr[i / 8] &= ~(0x80 >> (i % 8));
+}
+
+void nhrp_address_set_broadcast(struct nhrp_address *addr, int prefix)
+{
+	int i, bits = 8 * addr->addr_len;
+
+	for (i = prefix; i < bits; i++)
+		addr->addr[i / 8] |= 0x80 >> (i % 8);
+}
+
+int nhrp_address_is_network(const struct nhrp_address *addr, int prefix)
+{
+	int i, bits = 8 * addr->addr_len;
+
+	for (i = prefix; i < bits; i++)
+		if (addr->addr[i / 8] & (0x80 >> (i % 8)))
+			return FALSE;
+	return TRUE;
+}
+
+const char *nhrp_address_format(const struct nhrp_address *addr,
+				size_t buflen, char *buffer)
+{
+	switch (addr->type) {
+	case PF_UNSPEC:
+		snprintf(buffer, buflen, "(unspecified)");
+		break;
+	case PF_INET:
+		snprintf(buffer, buflen, "%d.%d.%d.%d",
+			 addr->addr[0], addr->addr[1],
+			 addr->addr[2], addr->addr[3]);
+		break;
+	default:
+		snprintf(buffer, buflen, "(proto 0x%04x)",
+			 addr->type);
+		break;
+	}
+
+	return buffer;
+}
+
+int nhrp_address_match_cie_list(struct nhrp_address *nbma_address,
+				struct nhrp_address *protocol_address,
+				struct list_head *cie_list)
+{
+	struct nhrp_cie *cie;
+
+	list_for_each_entry(cie, cie_list, cie_list_entry) {
+		if (nhrp_address_cmp(&cie->nbma_address, nbma_address) == 0 &&
+		    nhrp_address_cmp(&cie->protocol_address, protocol_address) == 0)
+			return TRUE;
+	}
+
+	return FALSE;
+}
+
+int nhrp_address_init(void)
+{
+	struct ares_options ares_opts;
+	int i;
+
+	memset(&ares_opts, 0, sizeof(ares_opts));
+	ares_opts.sock_state_cb = &ares_socket_cb;
+	ares_opts.sock_state_cb_data = &resolver;
+	ares_opts.timeout = 2;
+	ares_opts.tries = 3;
+	if (ares_init_options(&resolver.channel, &ares_opts,
+			      ARES_OPT_SOCK_STATE_CB | ARES_OPT_TIMEOUT |
+			      ARES_OPT_TRIES) != ARES_SUCCESS)
+		return FALSE;
+
+	ev_timer_init(&resolver.timeout, ares_timeout_cb, 0.0, 0.0);
+	ev_prepare_init(&resolver.prepare, ares_prepare_cb);
+	ev_prepare_start(&resolver.prepare);
+	for (i = 0; i < ARRAY_SIZE(resolver.fds); i++)
+		ev_init(&resolver.fds[i], ares_io_cb);
+
+	return TRUE;
+}
+
+void nhrp_address_cleanup(void)
+{
+	int i;
+
+	ev_timer_stop(&resolver.timeout);
+	ev_prepare_stop(&resolver.prepare);
+	for (i = 0; i < ARRAY_SIZE(resolver.fds); i++)
+		ev_io_stop(&resolver.fds[i]);
+	ares_destroy(resolver.channel);
+}
diff --git a/nhrp/nhrp_address.h b/nhrp/nhrp_address.h
new file mode 100644
index 0000000..e479631
--- /dev/null
+++ b/nhrp/nhrp_address.h
@@ -0,0 +1,80 @@
+/* nhrp_address.h - NHRP address structures and helpers
+ *
+ * Copyright (C) 2007 Timo Teräs <timo.teras@iki.fi>
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 or later as
+ * published by the Free Software Foundation.
+ *
+ * See http://www.gnu.org/ for details.
+ */
+
+#ifndef NHRP_ADDRESS_H
+#define NHRP_ADDRESS_H
+
+#include <stdint.h>
+#include <sys/socket.h>
+#include "list.h"
+
+#define NHRP_MAX_ADDRESS_LEN            6
+
+struct nhrp_cie_list_head;
+struct nhrp_address;
+struct nhrp_address_query;
+
+typedef void (*nhrp_address_query_callback)(struct nhrp_address_query *query,
+					    int num_addr,
+					    struct nhrp_address *addrs);
+
+struct nhrp_address {
+	uint16_t type;
+	uint8_t addr_len;
+	uint8_t subaddr_len;
+	uint8_t addr[NHRP_MAX_ADDRESS_LEN];
+};
+
+struct nhrp_address_query {
+	nhrp_address_query_callback callback;
+};
+
+uint16_t nhrp_protocol_from_pf(uint16_t pf);
+uint16_t nhrp_pf_from_protocol(uint16_t protocol);
+uint16_t nhrp_afnum_from_pf(uint16_t pf);
+uint16_t nhrp_pf_from_afnum(uint16_t afnum);
+
+int nhrp_address_init(void);
+void nhrp_address_cleanup(void);
+int nhrp_address_parse_packet(uint16_t protocol, size_t len, uint8_t *packet,
+			      struct nhrp_address *src,
+			      struct nhrp_address *dst);
+int nhrp_address_parse(const char *string, struct nhrp_address *addr,
+		       uint8_t *prefix_len);
+void nhrp_address_resolve(struct nhrp_address_query *query,
+			  const char *hostname,
+			  nhrp_address_query_callback callback);
+void nhrp_address_resolve_cancel(struct nhrp_address_query *query);
+void nhrp_address_set_type(struct nhrp_address *addr, uint16_t type);
+int nhrp_address_set(struct nhrp_address *addr, uint16_t type,
+		     uint8_t len, uint8_t *bytes);
+int nhrp_address_set_full(struct nhrp_address *addr, uint16_t type,
+			  uint8_t len, uint8_t *bytes,
+			  uint8_t sublen, uint8_t *subbytes);
+int nhrp_address_cmp(const struct nhrp_address *a, const struct nhrp_address *b);
+int nhrp_address_prefix_cmp(const struct nhrp_address *a, const struct nhrp_address *b,
+			    int prefix);
+unsigned int nhrp_address_hash(const struct nhrp_address *addr);
+void nhrp_address_set_network(struct nhrp_address *addr, int prefix);
+void nhrp_address_set_broadcast(struct nhrp_address *addr, int prefix);
+int nhrp_address_is_network(const struct nhrp_address *addr, int prefix);
+int nhrp_address_is_broadcast(const struct nhrp_address *addr, int prefix);
+int nhrp_address_is_multicast(const struct nhrp_address *addr);
+int nhrp_address_is_any_addr(const struct nhrp_address *addr);
+const char *nhrp_address_format(const struct nhrp_address *addr,
+				size_t buflen, char *buffer);
+
+int nhrp_address_match_cie_list(struct nhrp_address *nbma_address,
+				struct nhrp_address *protocol_address,
+				struct list_head *cie_list);
+
+#endif
diff --git a/nhrp/nhrp_common.h b/nhrp/nhrp_common.h
new file mode 100644
index 0000000..6730e74
--- /dev/null
+++ b/nhrp/nhrp_common.h
@@ -0,0 +1,78 @@
+/* nhrp_common.h - Generic helper functions
+ *
+ * Copyright (C) 2007-2009 Timo Teräs <timo.teras@iki.fi>
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 or later as
+ * published by the Free Software Foundation.
+ *
+ * See http://www.gnu.org/ for details.
+ */
+
+#ifndef NHRP_COMMON_H
+#define NHRP_COMMON_H
+
+#include <stdint.h>
+#include <stdlib.h>
+#include <sys/time.h>
+#include <linux/if_ether.h>
+
+struct nhrp_interface;
+struct nhrp_address;
+
+extern const char *nhrp_config_file, *nhrp_script_file;
+extern int nhrp_running, nhrp_verbose;
+
+/* Logging */
+void nhrp_log(int level, const char *format, ...);
+
+#define NHRP_LOG_DEBUG		0
+#define NHRP_LOG_INFO		1
+#define NHRP_LOG_ERROR		2
+
+#define nhrp_debug(...)						\
+	do {							\
+		if (nhrp_verbose)				\
+			nhrp_log(NHRP_LOG_DEBUG, __VA_ARGS__);	\
+	} while(0)
+
+#define nhrp_info(...) \
+		nhrp_log(NHRP_LOG_INFO, __VA_ARGS__)
+
+#define nhrp_error(...) \
+		nhrp_log(NHRP_LOG_ERROR, __VA_ARGS__)
+
+void nhrp_perror(const char *message);
+void nhrp_hex_dump(const char *name, const uint8_t *buf, int bytes);
+
+#define NHRP_BUG_ON(cond) if (cond) { \
+	nhrp_error("BUG: failure at %s:%d/%s(): %s!", \
+		__FILE__, __LINE__, __func__, #cond); \
+	abort(); \
+}
+
+/* Initializers for system dependant stuff */
+int forward_init(void);
+void forward_cleanup(void);
+int forward_local_addresses_changed(void);
+
+int kernel_init(void);
+void kernel_stop_listening(void);
+void kernel_cleanup(void);
+int kernel_route(struct nhrp_interface *out_iface,
+		 struct nhrp_address *dest,
+		 struct nhrp_address *default_source,
+		 struct nhrp_address *next_hop,
+		 u_int16_t *mtu);
+int kernel_send(uint8_t *packet, size_t bytes, struct nhrp_interface *out,
+		struct nhrp_address *to);
+int kernel_inject_neighbor(struct nhrp_address *neighbor,
+			   struct nhrp_address *hwaddr,
+			   struct nhrp_interface *dev);
+
+int log_init(void);
+int admin_init(const char *socket);
+void server_init(void);
+
+#endif
diff --git a/nhrp/nhrp_defines.h b/nhrp/nhrp_defines.h
new file mode 100644
index 0000000..2812a13
--- /dev/null
+++ b/nhrp/nhrp_defines.h
@@ -0,0 +1,87 @@
+/* nhrp_defines.h - NHRP definitions
+ *
+ * Copyright (C) 2007 Timo Teräs <timo.teras@iki.fi>
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 or later as
+ * published by the Free Software Foundation.
+ *
+ * See http://www.gnu.org/ for details.
+ */
+
+#ifndef NHRP_DEFINES_H
+#define NHRP_DEFINES_H
+
+#include <stdint.h>
+#include <byteswap.h>
+#include <sys/param.h>
+#include <linux/version.h>
+
+#ifndef NULL
+#define NULL 0L
+#endif
+
+#ifndef TRUE
+#define TRUE 1
+#endif
+
+#ifndef FALSE
+#define FALSE 0
+#endif
+
+#ifndef __bswap_constant_16
+#define __bswap_constant_16(x) \
+	((((x) >> 8) & 0xff) | (((x) & 0xff) << 8))
+#endif
+#ifndef __bswap_constant_32
+#define __bswap_constant_32(x) \
+	((((x) & 0xff000000) >> 24) | (((x) & 0x00ff0000) >>  8) | \
+	 (((x) & 0x0000ff00) <<  8) | (((x) & 0x000000ff) << 24))
+#endif
+
+#if __BYTE_ORDER == __BIG_ENDIAN
+#define constant_ntohl(x) (x)
+#define constant_ntohs(x) (x)
+#define constant_htonl(x) (x)
+#define constant_htons(x) (x)
+#else
+#define constant_ntohl(x) __bswap_constant_32(x)
+#define constant_ntohs(x) __bswap_constant_16(x)
+#define constant_htonl(x) __bswap_constant_32(x)
+#define constant_htons(x) __bswap_constant_16(x)
+#endif
+
+#ifndef ARRAY_SIZE
+#define ARRAY_SIZE(array) (sizeof(array) / sizeof((array)[0]))
+#endif
+
+#ifndef offsetof
+#ifdef __compiler_offsetof
+#define offsetof(TYPE,MEMBER) __compiler_offsetof(TYPE,MEMBER)
+#else
+#define offsetof(TYPE, MEMBER) ((size_t) &((TYPE *)0)->MEMBER)
+#endif
+#endif
+
+#define BIT(x) (1 << (x))
+
+#ifndef container_of
+#define container_of(ptr, type, member) ({                      \
+        const typeof( ((type *)0)->member ) *__mptr = (ptr);    \
+        (type *)( (char *)__mptr - offsetof(type,member) );})
+#endif
+
+#if __GNUC__ >= 3
+#define NHRP_EMPTY_ARRAY
+#else
+#define NHRP_EMPTY_ARRAY		0
+#endif
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0)
+#define NHRP_NO_NBMA_GRE
+#endif
+
+#define NHRP_DEFAULT_HOLDING_TIME	(2 * 60 * 60)
+
+#endif
diff --git a/nhrp/nhrp_interface.c b/nhrp/nhrp_interface.c
new file mode 100644
index 0000000..32c2383
--- /dev/null
+++ b/nhrp/nhrp_interface.c
@@ -0,0 +1,188 @@
+/* nhrp_interface.c - NHRP configuration per interface
+ *
+ * Copyright (C) 2007 Timo Teräs <timo.teras@iki.fi>
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 or later as
+ * published by the Free Software Foundation.
+ *
+ * See http://www.gnu.org/ for details.
+ */
+
+#include <stdio.h>
+#include <stddef.h>
+#include <string.h>
+#include <malloc.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <sys/socket.h>
+#include <linux/rtnetlink.h>
+#include "nhrp_common.h"
+#include "nhrp_interface.h"
+#include "nhrp_address.h"
+
+#define NHRP_INDEX_HASH_SIZE		(1 << 6)
+
+static struct list_head name_list = LIST_INITIALIZER(name_list);
+static struct hlist_head index_hash[NHRP_INDEX_HASH_SIZE];
+
+static char *env(const char *key, const char *value)
+{
+	char *buf;
+	buf = malloc(strlen(key)+strlen(value)+2);
+	if (buf == NULL)
+		return NULL;
+	sprintf(buf, "%s=%s", key, value);
+	return buf;
+}
+
+static char *envu32(const char *key, uint32_t value)
+{
+	char *buf;
+	buf = malloc(strlen(key)+16);
+	if (buf == NULL)
+		return NULL;
+	sprintf(buf, "%s=%u", key, value);
+	return buf;
+}
+
+void nhrp_interface_cleanup(void)
+{
+	struct nhrp_interface *iface, *n;
+
+	list_for_each_entry_safe(iface, n, &name_list, name_list_entry) {
+		list_del(&iface->name_list_entry);
+		hlist_del(&iface->index_list_entry);
+		free(iface);
+	}
+}
+
+void nhrp_interface_hash(struct nhrp_interface *iface)
+{
+	int iidx = iface->index & (NHRP_INDEX_HASH_SIZE - 1);
+
+	list_del(&iface->name_list_entry);
+	list_add(&iface->name_list_entry, &name_list);
+
+	hlist_del(&iface->index_list_entry);
+	hlist_add_head(&iface->index_list_entry, &index_hash[iidx]);
+}
+
+int nhrp_interface_foreach(nhrp_interface_enumerator enumerator, void *ctx)
+{
+	struct nhrp_interface *iface;
+	int rc;
+
+	list_for_each_entry(iface, &name_list, name_list_entry) {
+		rc = enumerator(ctx, iface);
+		if (rc != 0)
+			return rc;
+	}
+	return 0;
+}
+
+struct nhrp_interface *nhrp_interface_get_by_name(const char *name, int create)
+{
+	struct nhrp_interface *iface;
+
+	list_for_each_entry(iface, &name_list, name_list_entry) {
+		if (strcmp(iface->name, name) == 0)
+			return iface;
+	}
+
+	if (!create)
+		return NULL;
+
+	iface = calloc(1, sizeof(struct nhrp_interface));
+	iface->holding_time = NHRP_DEFAULT_HOLDING_TIME;
+	iface->route_table = RT_TABLE_MAIN;
+	strncpy(iface->name, name, sizeof(iface->name));
+
+	list_init(&iface->peer_list);
+	list_init(&iface->mcast_list);
+	list_add(&iface->name_list_entry, &name_list);
+	hlist_add_head(&iface->index_list_entry, &index_hash[0]);
+
+	return iface;
+}
+
+struct nhrp_interface *nhrp_interface_get_by_index(unsigned int index, int create)
+{
+	struct nhrp_interface *iface;
+	struct hlist_node *n;
+	int iidx = index & (NHRP_INDEX_HASH_SIZE - 1);
+
+	hlist_for_each_entry(iface, n, &index_hash[iidx], index_list_entry) {
+		if (iface->index == index)
+			return iface;
+	}
+
+	return NULL;
+}
+
+struct nhrp_interface *nhrp_interface_get_by_nbma(struct nhrp_address *addr)
+{
+	struct nhrp_interface *iface;
+
+	list_for_each_entry(iface, &name_list, name_list_entry) {
+		if (!(iface->flags & NHRP_INTERFACE_FLAG_CONFIGURED))
+			continue;
+
+		if (nhrp_address_cmp(addr, &iface->nbma_address) == 0)
+			return iface;
+
+		if (iface->nbma_address.type == PF_UNSPEC && !iface->link_index)
+			return iface;
+	}
+
+	return NULL;
+}
+
+struct nhrp_interface *nhrp_interface_get_by_protocol(struct nhrp_address *addr)
+{
+	struct nhrp_interface *iface;
+
+	list_for_each_entry(iface, &name_list, name_list_entry) {
+		if (nhrp_address_cmp(addr, &iface->protocol_address) == 0)
+			return iface;
+	}
+
+	return NULL;
+}
+
+int nhrp_interface_run_script(struct nhrp_interface *iface, char *action)
+{
+	const char *argv[] = { nhrp_script_file, action, NULL };
+	char *envp[6];
+	pid_t pid;
+	int i = 0;
+
+	pid = fork();
+	if (pid == -1)
+		return FALSE;
+	if (pid > 0)
+		return TRUE;
+
+	envp[i++] = "NHRP_TYPE=INTERFACE";
+	envp[i++] = env("NHRP_INTERFACE", iface->name);
+	envp[i++] = envu32("NHRP_GRE_KEY", iface->gre_key);
+	envp[i++] = NULL;
+
+	execve(nhrp_script_file, (char **) argv, envp);
+	exit(1);
+}
+
+struct nhrp_peer *nhrp_interface_find_peer(struct nhrp_interface *iface,
+					   const struct nhrp_address *nbma)
+{
+	unsigned int key = nhrp_address_hash(nbma) % NHRP_INTERFACE_NBMA_HASH_SIZE;
+	struct nhrp_peer *peer;
+	struct hlist_node *n;
+
+	hlist_for_each_entry(peer, n, &iface->nbma_hash[key], nbma_hash_entry) {
+		if (nhrp_address_cmp(nbma, &peer->next_hop_address) == 0)
+			return peer;
+	}
+	return NULL;
+}
diff --git a/nhrp/nhrp_interface.h b/nhrp/nhrp_interface.h
new file mode 100644
index 0000000..8e3e8df
--- /dev/null
+++ b/nhrp/nhrp_interface.h
@@ -0,0 +1,78 @@
+/* nhrp_interface.h - NHRP configuration per interface definitions
+ *
+ * Copyright (C) 2007 Timo Teräs <timo.teras@iki.fi>
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 or later as
+ * published by the Free Software Foundation.
+ *
+ * See http://www.gnu.org/ for details.
+ */
+
+#ifndef NHRP_INTERFACE_H
+#define NHRP_INTERFACE_H
+
+#include "nhrp_packet.h"
+#include "nhrp_peer.h"
+
+#define NHRP_INTERFACE_FLAG_NON_CACHING		0x0001	/* Do not cache entries */
+#define NHRP_INTERFACE_FLAG_SHORTCUT		0x0002	/* Create shortcut routes */
+#define NHRP_INTERFACE_FLAG_REDIRECT		0x0004	/* Send redirects */
+#define NHRP_INTERFACE_FLAG_SHORTCUT_DEST	0x0008	/* Advertise routes */
+#define NHRP_INTERFACE_FLAG_CONFIGURED		0x0010	/* Found in config file */
+
+#define NHRP_INTERFACE_NBMA_HASH_SIZE		256
+
+struct nhrp_interface {
+	struct list_head name_list_entry;
+	struct hlist_node index_list_entry;
+
+	/* Configured information */
+	char name[16];
+	unsigned int flags;
+	unsigned int holding_time;
+	struct nhrp_buffer *auth_token;
+	unsigned int route_table;
+
+	/* Cached from kernel interface */
+	unsigned int index, link_index;
+	uint32_t gre_key;
+	uint16_t afnum;
+	uint16_t mtu, nbma_mtu;
+	struct nhrp_address nbma_address;
+	struct nhrp_cie nat_cie;
+
+	/* Actually, we should have list of protocol addresses;
+	 * we might have multiple address and multiple protocol types */
+	struct nhrp_address protocol_address;
+	int protocol_address_prefix;
+
+        /* Peer cache is interface specific */
+	struct list_head peer_list;
+	struct hlist_head nbma_hash[NHRP_INTERFACE_NBMA_HASH_SIZE];
+
+	/* Multicast related stuff */
+	struct list_head mcast_list;
+	int mcast_mask;
+	int mcast_numaddr;
+	struct nhrp_address *mcast_addr;
+};
+
+typedef int (*nhrp_interface_enumerator)(void *ctx, struct nhrp_interface *iface);
+
+void nhrp_interface_cleanup(void);
+void nhrp_interface_hash(struct nhrp_interface *iface);
+int nhrp_interface_foreach(nhrp_interface_enumerator enumerator, void *ctx);
+struct nhrp_interface *nhrp_interface_get_by_name(const char *name, int create);
+struct nhrp_interface *nhrp_interface_get_by_index(unsigned int index, int create);
+struct nhrp_interface *nhrp_interface_get_by_nbma(struct nhrp_address *addr);
+struct nhrp_interface *nhrp_interface_get_by_protocol(struct nhrp_address *addr);
+int nhrp_interface_run_script(struct nhrp_interface *iface, char *action);
+struct nhrp_peer *nhrp_interface_find_peer(struct nhrp_interface *iface, const struct nhrp_address *nbma);
+
+void nhrp_interface_resolve_nbma(struct nhrp_interface *iface,
+				 struct nhrp_address *nbmadest,
+				 struct nhrp_address *nbma);
+
+#endif
diff --git a/nhrp/nhrp_packet.c b/nhrp/nhrp_packet.c
new file mode 100644
index 0000000..f46b481
--- /dev/null
+++ b/nhrp/nhrp_packet.c
@@ -0,0 +1,1331 @@
+/* nhrp_packet.c - NHRP packet marshalling and tranceiving
+ *
+ * Copyright (C) 2007-2009 Timo Teräs <timo.teras@iki.fi>
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 or later as
+ * published by the Free Software Foundation.
+ *
+ * See http://www.gnu.org/ for details.
+ */
+
+#include <malloc.h>
+#include <string.h>
+#include <stdio.h>
+#include <ctype.h>
+#include <netinet/in.h>
+
+#include "libev.h"
+#include "nhrp_common.h"
+#include "nhrp_packet.h"
+#include "nhrp_peer.h"
+#include "nhrp_interface.h"
+
+#define PACKET_RETRIES			6
+#define PACKET_RETRY_INTERVAL		5.0
+
+#define RATE_LIMIT_HASH_SIZE		256
+#define RATE_LIMIT_MAX_TOKENS		4
+#define RATE_LIMIT_SEND_INTERVAL	5.0
+#define RATE_LIMIT_SILENCE		360.0
+#define RATE_LIMIT_PURGE_INTERVAL	600.0
+
+#define MAX_PDU_SIZE			1500
+
+struct nhrp_rate_limit {
+	struct hlist_node hash_entry;
+	struct nhrp_address src;
+	struct nhrp_address dst;
+	ev_tstamp rate_last;
+	int rate_tokens;
+};
+
+static uint32_t request_id = 0;
+static struct list_head pending_requests = LIST_INITIALIZER(pending_requests);
+static struct hlist_head rate_limit_hash[RATE_LIMIT_HASH_SIZE];
+static ev_timer rate_limit_timer;
+static int num_rate_limit_entries = 0;
+
+static void nhrp_packet_xmit_timeout_cb(struct ev_timer *w, int revents);
+static int unmarshall_packet_header(uint8_t **pdu, size_t *pdusize,
+				    struct nhrp_packet *packet);
+
+static void nhrp_rate_limit_delete(struct nhrp_rate_limit *rl)
+{
+	hlist_del(&rl->hash_entry);
+	free(rl);
+	num_rate_limit_entries--;
+}
+
+int nhrp_rate_limit_clear(struct nhrp_address *a, int pref)
+{
+	struct nhrp_rate_limit *rl;
+	struct hlist_node *n, *c;
+	int i, ret = 0;
+
+	for (i = 0; i < RATE_LIMIT_HASH_SIZE; i++) {
+		hlist_for_each_entry_safe(rl, c, n, &rate_limit_hash[i],
+					  hash_entry) {
+			if (a->type == AF_UNSPEC ||
+			    nhrp_address_prefix_cmp(a, &rl->src, pref) == 0 ||
+			    nhrp_address_prefix_cmp(a, &rl->dst, pref) == 0) {
+				nhrp_rate_limit_delete(rl);
+				ret++;
+			}
+		}
+	}
+
+	if (num_rate_limit_entries == 0)
+		ev_timer_stop(&rate_limit_timer);
+
+	return ret;
+}
+
+static void prune_rate_limit_entries_cb(struct ev_timer *w, int revents)
+{
+	struct nhrp_rate_limit *rl;
+	struct hlist_node *c, *n;
+	int i;
+
+	for (i = 0; i < RATE_LIMIT_HASH_SIZE; i++) {
+		hlist_for_each_entry_safe(rl, c, n, &rate_limit_hash[i],
+					  hash_entry) {
+
+			if (ev_now() > rl->rate_last + 2 * RATE_LIMIT_SILENCE)
+				nhrp_rate_limit_delete(rl);
+		}
+	}
+
+	if (num_rate_limit_entries == 0)
+		ev_timer_stop(&rate_limit_timer);
+}
+
+static struct nhrp_rate_limit *get_rate_limit(struct nhrp_address *src,
+					      struct nhrp_address *dst)
+{
+	unsigned int key;
+	struct nhrp_rate_limit *e;
+	struct hlist_node *n;
+
+	key = nhrp_address_hash(src) ^ nhrp_address_hash(dst);
+	key %= RATE_LIMIT_HASH_SIZE;
+
+	hlist_for_each_entry(e, n, &rate_limit_hash[key], hash_entry) {
+		if (nhrp_address_cmp(&e->src, src) == 0 &&
+		    nhrp_address_cmp(&e->dst, dst) == 0)
+			return e;
+	}
+
+	e = calloc(1, sizeof(struct nhrp_rate_limit));
+	e->src = *src;
+	e->dst = *dst;
+	hlist_add_head(&e->hash_entry, &rate_limit_hash[key]);
+
+	if (num_rate_limit_entries == 0) {
+		ev_timer_init(&rate_limit_timer, prune_rate_limit_entries_cb,
+			      RATE_LIMIT_PURGE_INTERVAL,
+			      RATE_LIMIT_PURGE_INTERVAL);
+		ev_timer_start(&rate_limit_timer);
+	}
+
+	num_rate_limit_entries++;
+
+	return e;
+}
+
+static uint16_t nhrp_calculate_checksum(uint8_t *pdu, uint16_t len)
+{
+	uint16_t *pdu16 = (uint16_t *) pdu;
+	uint32_t csum = 0;
+	int i;
+
+	for (i = 0; i < len / 2; i++)
+		csum += pdu16[i];
+	if (len & 1)
+		csum += htons(pdu[len - 1]);
+
+	while (csum & 0xffff0000)
+		csum = (csum & 0xffff) + (csum >> 16);
+
+	return (~csum) & 0xffff;
+}
+
+struct nhrp_buffer *nhrp_buffer_alloc(uint32_t size)
+{
+	struct nhrp_buffer *buf;
+
+	buf = malloc(sizeof(struct nhrp_buffer) + size);
+	buf->length = size;
+
+	return buf;
+}
+
+struct nhrp_buffer *nhrp_buffer_copy(struct nhrp_buffer *buffer)
+{
+	struct nhrp_buffer *copy;
+
+	copy = nhrp_buffer_alloc(buffer->length);
+	memcpy(copy->data, buffer->data, buffer->length);
+	return copy;
+}
+
+int nhrp_buffer_cmp(struct nhrp_buffer *a, struct nhrp_buffer *b)
+{
+	if (a->length > b->length)
+		return 1;
+	if (a->length < b->length)
+		return -1;
+	return memcmp(a->data, b->data, a->length);
+}
+
+void nhrp_buffer_free(struct nhrp_buffer *buffer)
+{
+	free(buffer);
+}
+
+struct nhrp_cie *nhrp_cie_alloc(void)
+{
+	return calloc(1, sizeof(struct nhrp_cie));
+}
+
+void nhrp_cie_free(struct nhrp_cie *cie)
+{
+	free(cie);
+}
+
+void nhrp_cie_reset(struct nhrp_cie *cie)
+{
+	memset(&cie->cie_list_entry, 0, sizeof(cie->cie_list_entry));
+}
+
+void nhrp_payload_free(struct nhrp_payload *payload)
+{
+	struct nhrp_cie *cie, *n;
+
+	switch (payload->payload_type) {
+	case NHRP_PAYLOAD_TYPE_RAW:
+		nhrp_buffer_free(payload->u.raw);
+		break;
+	case NHRP_PAYLOAD_TYPE_CIE_LIST:
+		list_for_each_entry_safe(cie, n, &payload->u.cie_list, cie_list_entry) {
+			list_del(&cie->cie_list_entry);
+			nhrp_cie_free(cie);
+		}
+		break;
+	}
+	payload->payload_type = NHRP_PAYLOAD_TYPE_NONE;
+}
+
+void nhrp_payload_set_type(struct nhrp_payload *payload, int type)
+{
+	if (payload->payload_type == type)
+		return;
+
+	nhrp_payload_free(payload);
+	payload->payload_type = type;
+	switch (type) {
+	case NHRP_PAYLOAD_TYPE_CIE_LIST:
+		list_init(&payload->u.cie_list);
+		break;
+	default:
+		payload->u.raw = NULL;
+		break;
+	}
+}
+
+void nhrp_payload_set_raw(struct nhrp_payload *payload, struct nhrp_buffer *raw)
+{
+	nhrp_payload_set_type(payload, NHRP_PAYLOAD_TYPE_RAW);
+	payload->u.raw = raw;
+}
+
+void nhrp_payload_add_cie(struct nhrp_payload *payload, struct nhrp_cie *cie)
+{
+	if (payload->payload_type != NHRP_PAYLOAD_TYPE_CIE_LIST) {
+		nhrp_cie_free(cie);
+		nhrp_info("Trying to add CIE payload to non-CIE payload %d\n",
+			  payload->payload_type);
+		return;
+	}
+
+	list_add_tail(&cie->cie_list_entry, &payload->u.cie_list);
+}
+
+struct nhrp_cie *nhrp_payload_get_cie(struct nhrp_payload *payload, int index)
+{
+	struct nhrp_cie *cie;
+
+	if (payload->payload_type != NHRP_PAYLOAD_TYPE_CIE_LIST)
+		return NULL;
+
+	list_for_each_entry(cie, &payload->u.cie_list, cie_list_entry) {
+		index--;
+		if (index == 0)
+			return cie;
+	}
+
+	return NULL;
+}
+
+struct nhrp_packet *nhrp_packet_alloc(void)
+{
+	struct nhrp_packet *packet;
+	packet = calloc(1, sizeof(struct nhrp_packet));
+	packet->ref = 1;
+	packet->hdr.hop_count = NHRP_PACKET_DEFAULT_HOP_COUNT;
+	list_init(&packet->request_list_entry);
+	ev_timer_init(&packet->timeout, nhrp_packet_xmit_timeout_cb,
+		      PACKET_RETRY_INTERVAL, PACKET_RETRY_INTERVAL);
+	return packet;
+}
+
+struct nhrp_packet *nhrp_packet_get(struct nhrp_packet *packet)
+{
+	packet->ref++;
+	return packet;
+}
+
+struct nhrp_payload *nhrp_packet_payload(struct nhrp_packet *packet, int payload_type)
+{
+	return nhrp_packet_extension(packet, NHRP_EXTENSION_PAYLOAD, payload_type);
+}
+
+struct nhrp_payload *nhrp_packet_extension(struct nhrp_packet *packet,
+					   uint32_t extension, int payload_type)
+{
+	struct nhrp_payload *p;
+
+	p = packet->extension_by_type[extension & 0x7fff];
+	if (p != NULL) {
+		if (payload_type == NHRP_PAYLOAD_TYPE_ANY ||
+		    payload_type == p->payload_type)
+			return p;
+		if (extension & NHRP_EXTENSION_FLAG_NOCREATE)
+			return NULL;
+		nhrp_payload_set_type(p, payload_type);
+		return p;
+	}
+
+	if (extension & NHRP_EXTENSION_FLAG_NOCREATE)
+		return NULL;
+
+	p = &packet->extension_by_order[packet->num_extensions++];
+	p->extension_type = extension & 0xffff;
+	packet->extension_by_type[extension & 0x7fff] = p;
+	if (payload_type != NHRP_PAYLOAD_TYPE_ANY)
+		nhrp_payload_set_type(p, payload_type);
+
+	return p;
+}
+
+static void nhrp_packet_release(struct nhrp_packet *packet)
+{
+	int i;
+
+	if (packet->dst_peer != NULL)
+		nhrp_peer_put(packet->dst_peer);
+	for (i = 0; i < packet->num_extensions; i++)
+		nhrp_payload_free(&packet->extension_by_order[i]);
+	free(packet);
+}
+
+void nhrp_packet_put(struct nhrp_packet *packet)
+{
+	NHRP_BUG_ON(packet->ref == 0);
+
+	packet->ref--;
+	if (packet->ref == 0)
+		nhrp_packet_release(packet);
+}
+
+int nhrp_packet_reroute(struct nhrp_packet *packet, struct nhrp_peer *dst_peer)
+{
+	packet->dst_iface = packet->src_iface;
+	if (packet->dst_peer != NULL)
+		nhrp_peer_put(packet->dst_peer);
+	packet->dst_peer = nhrp_peer_get(dst_peer);
+	return nhrp_packet_route(packet);
+}
+
+static void nhrp_packet_dequeue(struct nhrp_packet *packet)
+{
+	ev_timer_stop(&packet->timeout);
+	if (list_hashed(&packet->request_list_entry))
+		list_del(&packet->request_list_entry);
+	nhrp_packet_put(packet);
+}
+
+static int nhrp_do_handle_error_indication(struct nhrp_packet *error_pkt,
+					   struct nhrp_packet *orig_pkt)
+{
+	struct nhrp_packet *req;
+
+	list_for_each_entry(req, &pending_requests, request_list_entry) {
+		if (orig_pkt->hdr.u.request_id != req->hdr.u.request_id)
+			continue;
+
+		if (nhrp_address_cmp(&orig_pkt->src_nbma_address,
+				     &req->src_nbma_address))
+			continue;
+		if (nhrp_address_cmp(&orig_pkt->src_protocol_address,
+				     &req->src_protocol_address))
+			continue;
+
+		if (req->handler != NULL)
+			req->handler(req->handler_ctx, error_pkt);
+		nhrp_packet_dequeue(req);
+
+		return TRUE;
+	}
+
+	return FALSE;
+}
+
+static int nhrp_handle_error_indication(struct nhrp_packet *error_packet)
+{
+	struct nhrp_packet *packet;
+	struct nhrp_payload *payload;
+	uint8_t *pdu;
+	size_t pduleft;
+	int r;
+
+	packet = nhrp_packet_alloc();
+	if (packet == NULL)
+		return FALSE;
+
+	payload = nhrp_packet_payload(error_packet, NHRP_PAYLOAD_TYPE_RAW);
+	pdu = payload->u.raw->data;
+	pduleft = payload->u.raw->length;
+
+	if (!unmarshall_packet_header(&pdu, &pduleft, packet)) {
+		nhrp_packet_put(packet);
+		return FALSE;
+	}
+
+	r = nhrp_do_handle_error_indication(error_packet, packet);
+	nhrp_packet_put(packet);
+
+	return r;
+}
+
+#define NHRP_TYPE_REQUEST	0
+#define NHRP_TYPE_REPLY		1
+#define NHRP_TYPE_INDICATION	2
+
+static struct {
+	int type;
+	uint16_t payload_type;
+	int (*handler)(struct nhrp_packet *packet);
+} packet_types[] = {
+	[NHRP_PACKET_RESOLUTION_REQUEST] = {
+		.type = NHRP_TYPE_REQUEST,
+		.payload_type = NHRP_PAYLOAD_TYPE_CIE_LIST,
+	},
+	[NHRP_PACKET_RESOLUTION_REPLY] = {
+		.type = NHRP_TYPE_REPLY,
+		.payload_type = NHRP_PAYLOAD_TYPE_CIE_LIST,
+	},
+	[NHRP_PACKET_REGISTRATION_REQUEST] = {
+		.type = NHRP_TYPE_REQUEST,
+		.payload_type = NHRP_PAYLOAD_TYPE_CIE_LIST,
+	},
+	[NHRP_PACKET_REGISTRATION_REPLY] = {
+		.type = NHRP_TYPE_REPLY,
+		.payload_type = NHRP_PAYLOAD_TYPE_CIE_LIST,
+	},
+	[NHRP_PACKET_PURGE_REQUEST] = {
+		.type = NHRP_TYPE_REQUEST,
+		.payload_type = NHRP_PAYLOAD_TYPE_CIE_LIST,
+	},
+	[NHRP_PACKET_PURGE_REPLY] = {
+		.type = NHRP_TYPE_REPLY,
+		.payload_type = NHRP_PAYLOAD_TYPE_CIE_LIST,
+	},
+	[NHRP_PACKET_ERROR_INDICATION] = {
+		.type = NHRP_TYPE_INDICATION,
+		.payload_type = NHRP_PAYLOAD_TYPE_RAW,
+		.handler = nhrp_handle_error_indication,
+	},
+	[NHRP_PACKET_TRAFFIC_INDICATION] = {
+		.type = NHRP_TYPE_INDICATION,
+		.payload_type = NHRP_PAYLOAD_TYPE_RAW,
+	}
+};
+static int extension_types[] = {
+	[NHRP_EXTENSION_RESPONDER_ADDRESS] = NHRP_PAYLOAD_TYPE_CIE_LIST,
+	[NHRP_EXTENSION_FORWARD_TRANSIT_NHS] = NHRP_PAYLOAD_TYPE_CIE_LIST,
+	[NHRP_EXTENSION_REVERSE_TRANSIT_NHS] = NHRP_PAYLOAD_TYPE_CIE_LIST,
+	[NHRP_EXTENSION_NAT_ADDRESS] = NHRP_PAYLOAD_TYPE_CIE_LIST
+};
+
+static int unmarshall_binary(uint8_t **pdu, size_t *pduleft, size_t size, void *raw)
+{
+	if (*pduleft < size)
+		return FALSE;
+
+	memcpy(raw, *pdu, size);
+	*pdu += size;
+	*pduleft -= size;
+	return TRUE;
+}
+
+static inline int unmarshall_protocol_address(uint8_t **pdu, size_t *pduleft, struct nhrp_address *pa)
+{
+	if (*pduleft < pa->addr_len)
+		return FALSE;
+
+	if (pa->addr_len) {
+		if (!nhrp_address_set(pa, pa->type, pa->addr_len, *pdu))
+			return FALSE;
+	} else {
+		nhrp_address_set_type(pa, PF_UNSPEC);
+	}
+
+	*pdu += pa->addr_len;
+	*pduleft -= pa->addr_len;
+	return TRUE;
+}
+
+static inline int unmarshall_nbma_address(uint8_t **pdu, size_t *pduleft, struct nhrp_address *na)
+{
+	if (*pduleft < na->addr_len + na->subaddr_len)
+		return FALSE;
+
+	if (na->addr_len || na->subaddr_len) {
+		if (!nhrp_address_set_full(na, na->type,
+					   na->addr_len, *pdu,
+					   na->subaddr_len, *pdu + na->addr_len))
+			return FALSE;
+	} else {
+		nhrp_address_set_type(na, PF_UNSPEC);
+	}
+
+	*pdu += na->addr_len + na->subaddr_len;
+	*pduleft -= na->addr_len + na->subaddr_len;
+	return TRUE;
+}
+
+static int unmarshall_cie(uint8_t **pdu, size_t *pduleft, struct nhrp_packet *p, struct nhrp_cie *cie)
+{
+	if (!unmarshall_binary(pdu, pduleft, sizeof(struct nhrp_cie_header), &cie->hdr))
+		return FALSE;
+
+	cie->nbma_address.type = nhrp_pf_from_afnum(p->hdr.afnum);
+	cie->nbma_address.addr_len = cie->hdr.nbma_address_len;
+	cie->nbma_address.subaddr_len = cie->hdr.nbma_subaddress_len;
+	cie->protocol_address.type = nhrp_pf_from_protocol(p->hdr.protocol_type);
+	cie->protocol_address.addr_len = cie->hdr.protocol_address_len;
+
+	if (!unmarshall_nbma_address(pdu, pduleft, &cie->nbma_address))
+		return FALSE;
+	return unmarshall_protocol_address(pdu, pduleft, &cie->protocol_address);
+}
+
+static int unmarshall_payload(uint8_t **pdu, size_t *pduleft,
+			      struct nhrp_packet *packet,
+			      int type, size_t size,
+			      struct nhrp_payload *p)
+{
+	struct nhrp_cie *cie;
+	size_t cieleft;
+
+	if (*pduleft < size)
+		return FALSE;
+
+	nhrp_payload_set_type(p, type);
+	switch (p->payload_type) {
+	case NHRP_PAYLOAD_TYPE_NONE:
+		*pdu += size;
+		*pduleft -= size;
+		return TRUE;
+	case NHRP_PAYLOAD_TYPE_RAW:
+		p->u.raw = nhrp_buffer_alloc(size);
+		return unmarshall_binary(pdu, pduleft, size, p->u.raw->data);
+	case NHRP_PAYLOAD_TYPE_CIE_LIST:
+		cieleft = size;
+		while (cieleft) {
+			cie = nhrp_cie_alloc();
+			list_add_tail(&cie->cie_list_entry, &p->u.cie_list);
+			if (!unmarshall_cie(pdu, &cieleft, packet, cie))
+				return FALSE;
+		}
+		*pduleft -= size;
+		return TRUE;
+	default:
+		return FALSE;
+	}
+}
+
+static int unmarshall_packet_header(uint8_t **pdu, size_t *pduleft, struct nhrp_packet *packet)
+{
+	struct nhrp_packet_header *phdr = (struct nhrp_packet_header *) *pdu;
+
+	if (!unmarshall_binary(pdu, pduleft, sizeof(packet->hdr), &packet->hdr))
+		return FALSE;
+
+	if (packet->hdr.type >= ARRAY_SIZE(packet_types))
+		return FALSE;
+
+	packet->src_nbma_address.type = nhrp_pf_from_afnum(packet->hdr.afnum);
+	packet->src_nbma_address.addr_len = phdr->src_nbma_address_len;
+	packet->src_nbma_address.subaddr_len = phdr->src_nbma_subaddress_len;
+	packet->src_protocol_address.type = nhrp_pf_from_protocol(packet->hdr.protocol_type);
+	packet->src_protocol_address.addr_len = phdr->src_protocol_address_len;
+	packet->dst_protocol_address.type = nhrp_pf_from_protocol(packet->hdr.protocol_type);
+	packet->dst_protocol_address.addr_len = phdr->dst_protocol_address_len;
+
+	if (!unmarshall_nbma_address(pdu, pduleft, &packet->src_nbma_address))
+		return FALSE;
+	if (!unmarshall_protocol_address(pdu, pduleft, &packet->src_protocol_address))
+		return FALSE;
+	return unmarshall_protocol_address(pdu, pduleft, &packet->dst_protocol_address);
+}
+
+static int unmarshall_packet(uint8_t *pdu, size_t pdusize, struct nhrp_packet *packet)
+{
+	size_t pduleft = pdusize;
+	uint8_t *pos = pdu;
+	int size, extension_offset;
+
+	if (!unmarshall_packet_header(&pos, &pduleft, packet))
+		return FALSE;
+
+	extension_offset = ntohs(packet->hdr.extension_offset);
+	if (extension_offset == 0) {
+		/* No extensions; rest of data is payload */
+		size = pduleft;
+	} else {
+		/* Extensions present; exclude those from payload */
+		size = extension_offset - (pos - pdu);
+		if (size < 0 || size > pduleft) {
+			nhrp_packet_send_error(packet, NHRP_ERROR_PROTOCOL_ERROR, pos - pdu);
+			return FALSE;
+		}
+	}
+
+	if (!unmarshall_payload(&pos, &pduleft, packet,
+				packet_types[packet->hdr.type].payload_type,
+				size, nhrp_packet_payload(packet, NHRP_PAYLOAD_TYPE_ANY))) {
+		nhrp_packet_send_error(packet, NHRP_ERROR_PROTOCOL_ERROR, pos - pdu);
+		return FALSE;
+	}
+
+	if (extension_offset == 0)
+		return TRUE;
+
+	pos = &pdu[extension_offset];
+	pduleft = pdusize - extension_offset;
+	do {
+		struct nhrp_extension_header eh;
+		int extension_type, payload_type;
+
+		if (!unmarshall_binary(&pos, &pduleft, sizeof(eh), &eh)) {
+			nhrp_packet_send_error(packet, NHRP_ERROR_PROTOCOL_ERROR, pos - pdu);
+			return FALSE;
+		}
+
+		extension_type = ntohs(eh.type) & ~NHRP_EXTENSION_FLAG_COMPULSORY;
+		if (extension_type == NHRP_EXTENSION_END)
+			break;
+
+		payload_type = NHRP_PAYLOAD_TYPE_NONE;
+		if (extension_type < ARRAY_SIZE(extension_types))
+			payload_type = extension_types[extension_type];
+		if (payload_type == NHRP_PAYLOAD_TYPE_NONE)
+			payload_type = NHRP_PAYLOAD_TYPE_RAW;
+		if (payload_type == NHRP_PAYLOAD_TYPE_RAW &&
+		    ntohs(eh.length) == 0)
+			payload_type = NHRP_PAYLOAD_TYPE_NONE;
+
+		if (!unmarshall_payload(&pos, &pduleft, packet,
+					payload_type, ntohs(eh.length),
+					nhrp_packet_extension(packet, ntohs(eh.type), NHRP_PAYLOAD_TYPE_ANY))) {
+			nhrp_packet_send_error(packet, NHRP_ERROR_PROTOCOL_ERROR, pos - pdu);
+			return FALSE;
+		}
+	} while (1);
+
+	return TRUE;
+}
+
+static int nhrp_packet_forward(struct nhrp_packet *packet)
+{
+	char tmp[64], tmp2[64], tmp3[64];
+	struct nhrp_payload *p = NULL;
+
+	nhrp_info("Forwarding packet from nbma src %s, proto src %s to proto dst %s, hop count %d",
+		nhrp_address_format(&packet->src_nbma_address,
+				    sizeof(tmp), tmp),
+		nhrp_address_format(&packet->src_protocol_address,
+				    sizeof(tmp2), tmp2),
+		nhrp_address_format(&packet->dst_protocol_address,
+				    sizeof(tmp3), tmp3),
+		packet->hdr.hop_count);
+
+	if (packet->hdr.hop_count == 0) {
+		nhrp_packet_send_error(packet, NHRP_ERROR_HOP_COUNT_EXCEEDED, 0);
+		return TRUE;
+	}
+	packet->hdr.hop_count--;
+
+	if (!nhrp_packet_reroute(packet, NULL)) {
+		nhrp_packet_send_error(packet, NHRP_ERROR_PROTOCOL_ADDRESS_UNREACHABLE, 0);
+		return FALSE;
+	}
+
+	switch (packet_types[packet->hdr.type].type) {
+	case NHRP_TYPE_REQUEST:
+	case NHRP_TYPE_INDICATION:
+		p = nhrp_packet_extension(packet,
+					  NHRP_EXTENSION_FORWARD_TRANSIT_NHS |
+					  NHRP_EXTENSION_FLAG_NOCREATE,
+					  NHRP_PAYLOAD_TYPE_CIE_LIST);
+		break;
+	case NHRP_TYPE_REPLY:
+		p = nhrp_packet_extension(packet,
+					  NHRP_EXTENSION_REVERSE_TRANSIT_NHS |
+					  NHRP_EXTENSION_FLAG_NOCREATE,
+					  NHRP_PAYLOAD_TYPE_CIE_LIST);
+		break;
+	}
+	if (p != NULL) {
+		struct nhrp_cie *cie;
+
+		if (nhrp_address_match_cie_list(&packet->dst_peer->my_nbma_address,
+						&packet->dst_iface->protocol_address,
+						&p->u.cie_list)) {
+			nhrp_packet_send_error(packet, NHRP_ERROR_LOOP_DETECTED, 0);
+			return FALSE;
+		}
+
+		cie = nhrp_cie_alloc();
+		if (cie != NULL) {
+			cie->hdr = (struct nhrp_cie_header) {
+				.code = NHRP_CODE_SUCCESS,
+				.holding_time = htons(packet->dst_iface->holding_time),
+			};
+			cie->nbma_address = packet->dst_peer->my_nbma_address;
+			cie->protocol_address = packet->dst_iface->protocol_address;
+			nhrp_payload_add_cie(p, cie);
+		}
+	}
+
+	return nhrp_packet_route_and_send(packet);
+}
+
+static int nhrp_packet_receive_local(struct nhrp_packet *packet)
+{
+	struct nhrp_packet *req;
+	char tmp[64], tmp2[64], tmp3[64];
+
+	if (packet_types[packet->hdr.type].type == NHRP_TYPE_REPLY) {
+		list_for_each_entry(req, &pending_requests, request_list_entry) {
+			if (packet->hdr.u.request_id != req->hdr.u.request_id)
+				continue;
+			if (nhrp_address_cmp(&packet->src_nbma_address,
+					     &req->src_nbma_address))
+				continue;
+			if (nhrp_address_cmp(&packet->src_protocol_address,
+					     &req->src_protocol_address))
+				continue;
+
+			if (req->handler != NULL)
+				req->handler(req->handler_ctx, packet);
+			nhrp_packet_dequeue(req);
+
+			return TRUE;
+		}
+
+		/* Reply to unsent request? */
+		nhrp_info("Packet type %d from nbma src %s, proto src %s, "
+			  "proto dst %s dropped: no matching request",
+			  packet->hdr.type,
+			  nhrp_address_format(&packet->src_nbma_address,
+					      sizeof(tmp), tmp),
+			  nhrp_address_format(&packet->src_protocol_address,
+					      sizeof(tmp2), tmp2),
+			  nhrp_address_format(&packet->dst_protocol_address,
+					      sizeof(tmp3), tmp3));
+
+		nhrp_packet_send_error(
+			packet, NHRP_ERROR_INVALID_RESOLUTION_REPLY, 0);
+		return TRUE;
+	}
+
+	if (packet_types[packet->hdr.type].handler == NULL) {
+		nhrp_info("Packet type %d from nbma src %s, proto src %s, "
+			  "proto dst %s not supported",
+			  packet->hdr.type,
+			  nhrp_address_format(&packet->src_nbma_address,
+					      sizeof(tmp), tmp),
+			  nhrp_address_format(&packet->src_protocol_address,
+					      sizeof(tmp2), tmp2),
+			  nhrp_address_format(&packet->dst_protocol_address,
+					      sizeof(tmp3), tmp3));
+		return FALSE;
+	}
+
+	if (packet->dst_peer->next_hop_address.type != PF_UNSPEC) {
+		/* Broadcast destinations gets rewritten as if destinied to
+		 * our local address */
+		packet->dst_protocol_address =
+			packet->dst_peer->next_hop_address;
+	}
+
+	return packet_types[packet->hdr.type].handler(packet);
+}
+
+int nhrp_packet_receive(uint8_t *pdu, size_t pdulen,
+			struct nhrp_interface *iface,
+			struct nhrp_address *from)
+{
+	char tmp[64];
+	struct nhrp_packet *packet;
+	struct nhrp_address *dest;
+	struct nhrp_peer *peer;
+	int ret = FALSE;
+
+	if (nhrp_calculate_checksum(pdu, pdulen) != 0) {
+		nhrp_error("Bad checksum in packet from %s",
+			   nhrp_address_format(from, sizeof(tmp), tmp));
+		return FALSE;
+	}
+
+	packet = nhrp_packet_alloc();
+	if (packet == NULL)
+		return FALSE;
+
+	if (!unmarshall_packet(pdu, pdulen, packet)) {
+		nhrp_error("Failed to unmarshall packet from %s",
+			   nhrp_address_format(from, sizeof(tmp), tmp));
+		goto error;
+	}
+
+	packet->req_pdu = pdu;
+	packet->req_pdulen = pdulen;
+
+	if (packet_types[packet->hdr.type].type == NHRP_TYPE_REPLY)
+		dest = &packet->src_protocol_address;
+	else
+		dest = &packet->dst_protocol_address;
+
+	peer = nhrp_peer_route(iface, dest, 0, BIT(NHRP_PEER_TYPE_LOCAL_ADDR));
+	packet->src_linklayer_address = *from;
+	packet->src_iface = iface;
+	packet->dst_peer = nhrp_peer_get(peer);
+
+	/* RFC2332 5.3.4 - Authentication is always done pairwise on an NHRP
+	 * hop-by-hop basis; i.e. regenerated at each hop. */
+	if (packet->src_iface->auth_token &&
+	    (packet->hdr.type != NHRP_PACKET_ERROR_INDICATION ||
+	     packet->hdr.u.error.code != NHRP_ERROR_AUTHENTICATION_FAILURE)) {
+		struct nhrp_payload *p;
+		p = nhrp_packet_extension(packet,
+					  NHRP_EXTENSION_AUTHENTICATION |
+					  NHRP_EXTENSION_FLAG_NOCREATE,
+					  NHRP_PAYLOAD_TYPE_RAW);
+		if (p == NULL ||
+		    nhrp_buffer_cmp(packet->src_iface->auth_token, p->u.raw) != 0) {
+			nhrp_error("Dropping packet from %s with bad authentication",
+				nhrp_address_format(from, sizeof(tmp), tmp));
+			nhrp_packet_send_error(packet, NHRP_ERROR_AUTHENTICATION_FAILURE, 0);
+			goto error;
+		}
+	}
+
+	if (peer != NULL &&
+	    peer->type == NHRP_PEER_TYPE_LOCAL_ADDR)
+		ret = nhrp_packet_receive_local(packet);
+	else
+		ret = nhrp_packet_forward(packet);
+
+	packet->req_pdu = NULL;
+	packet->req_pdulen = 0;
+
+error:
+	nhrp_packet_put(packet);
+	return ret;
+}
+
+static int marshall_binary(uint8_t **pdu, size_t *pduleft, size_t size, void *raw)
+{
+	if (*pduleft < size)
+		return FALSE;
+
+	memcpy(*pdu, raw, size);
+	*pdu += size;
+	*pduleft -= size;
+
+	return TRUE;
+}
+
+static inline int marshall_protocol_address(uint8_t **pdu, size_t *pduleft, struct nhrp_address *pa)
+{
+	if (pa->subaddr_len != 0)
+		return FALSE;
+	return marshall_binary(pdu, pduleft, pa->addr_len, pa->addr);
+}
+
+static inline int marshall_nbma_address(uint8_t **pdu, size_t *pduleft, struct nhrp_address *na)
+{
+	return marshall_binary(pdu, pduleft, na->addr_len + na->subaddr_len, na->addr);
+}
+
+static int marshall_cie(uint8_t **pdu, size_t *pduleft, struct nhrp_cie *cie)
+{
+	cie->hdr.nbma_address_len = cie->nbma_address.addr_len;
+	cie->hdr.nbma_subaddress_len = cie->nbma_address.subaddr_len;
+	cie->hdr.protocol_address_len = cie->protocol_address.addr_len;
+
+	if (!marshall_binary(pdu, pduleft, sizeof(struct nhrp_cie_header), &cie->hdr))
+		return FALSE;
+	if (!marshall_nbma_address(pdu, pduleft, &cie->nbma_address))
+		return FALSE;
+	return marshall_protocol_address(pdu, pduleft, &cie->protocol_address);
+}
+
+static int marshall_payload(uint8_t **pdu, size_t *pduleft, struct nhrp_payload *p)
+{
+	struct nhrp_cie *cie;
+
+	switch (p->payload_type) {
+	case NHRP_PAYLOAD_TYPE_NONE:
+		return TRUE;
+	case NHRP_PAYLOAD_TYPE_RAW:
+		if (p->u.raw->length == 0)
+			return TRUE;
+		return marshall_binary(pdu, pduleft, p->u.raw->length, p->u.raw->data);
+	case NHRP_PAYLOAD_TYPE_CIE_LIST:
+		list_for_each_entry(cie, &p->u.cie_list, cie_list_entry) {
+			if (!marshall_cie(pdu, pduleft, cie))
+				return FALSE;
+		}
+		return TRUE;
+	default:
+		return FALSE;
+	}
+}
+
+static int marshall_packet_header(uint8_t **pdu, size_t *pduleft, struct nhrp_packet *packet)
+{
+	if (!marshall_binary(pdu, pduleft, sizeof(packet->hdr), &packet->hdr))
+		return FALSE;
+	if (!marshall_nbma_address(pdu, pduleft, &packet->src_nbma_address))
+		return FALSE;
+	if (!marshall_protocol_address(pdu, pduleft, &packet->src_protocol_address))
+		return FALSE;
+	return marshall_protocol_address(pdu, pduleft, &packet->dst_protocol_address);
+}
+
+static int marshall_packet(uint8_t *pdu, size_t pduleft, struct nhrp_packet *packet)
+{
+	uint8_t *pos = pdu;
+	struct nhrp_packet_header *phdr = (struct nhrp_packet_header *) pdu;
+	struct nhrp_extension_header neh;
+	int i, size;
+
+	if (!marshall_packet_header(&pos, &pduleft, packet))
+		return -1;
+	if (!marshall_payload(&pos, &pduleft, nhrp_packet_payload(packet, NHRP_PAYLOAD_TYPE_ANY)))
+		return -2;
+
+	phdr->extension_offset = htons((int)(pos - pdu));
+	for (i = 1; i < packet->num_extensions; i++) {
+		struct nhrp_extension_header *eh = (struct nhrp_extension_header *) pos;
+
+		if (packet->extension_by_order[i].payload_type == NHRP_PAYLOAD_TYPE_NONE)
+			continue;
+
+		neh.type = htons(packet->extension_by_order[i].extension_type);
+		neh.length = 0;
+
+		if (!marshall_binary(&pos, &pduleft, sizeof(neh), &neh))
+			return -3;
+		if (!marshall_payload(&pos, &pduleft, &packet->extension_by_order[i]))
+			return -4;
+		eh->length = htons((pos - (uint8_t *) eh) - sizeof(neh));
+	}
+	neh.type = htons(NHRP_EXTENSION_END | NHRP_EXTENSION_FLAG_COMPULSORY);
+	neh.length = 0;
+	if (!marshall_binary(&pos, &pduleft, sizeof(neh), &neh))
+		return -5;
+
+	/* Cisco is seriously brain damaged. It needs some extra garbage
+         * at the end of error indication or it'll barf out spurious errors. */
+	if (packet->hdr.type == NHRP_PACKET_ERROR_INDICATION &&
+	    pduleft >= 0x10) {
+		memset(pos, 0, 0x10);
+		pos += 0x10;
+		pduleft -= 0x10;
+	}
+
+	size = (int)(pos - pdu);
+	phdr->packet_size = htons(size);
+	phdr->checksum = 0;
+	phdr->src_nbma_address_len = packet->src_nbma_address.addr_len;
+	phdr->src_nbma_subaddress_len = packet->src_nbma_address.subaddr_len;
+	phdr->src_protocol_address_len = packet->src_protocol_address.addr_len;
+	phdr->dst_protocol_address_len = packet->dst_protocol_address.addr_len;
+	phdr->checksum = nhrp_calculate_checksum(pdu, size);
+
+	return size;
+}
+
+int nhrp_packet_route(struct nhrp_packet *packet)
+{
+	struct nhrp_address proto_nexthop, *src, *dst;
+	struct list_head *cielist = NULL;
+	struct nhrp_payload *payload;
+	struct nhrp_peer *peer;
+	char tmp[64];
+	int r;
+
+	if (packet->dst_iface == NULL) {
+		nhrp_error("nhrp_packet_route called without destination interface");
+		return FALSE;
+	}
+
+	if (packet_types[packet->hdr.type].type == NHRP_TYPE_REPLY) {
+		dst = &packet->src_protocol_address;
+		src = &packet->dst_protocol_address;
+		r = NHRP_EXTENSION_REVERSE_TRANSIT_NHS;
+	} else {
+		dst = &packet->dst_protocol_address;
+		src = &packet->src_protocol_address;
+		r = NHRP_EXTENSION_FORWARD_TRANSIT_NHS;
+	}
+	payload = nhrp_packet_extension(packet,
+					r | NHRP_EXTENSION_FLAG_NOCREATE,
+					NHRP_PAYLOAD_TYPE_CIE_LIST);
+	if (payload != NULL)
+		cielist = &payload->u.cie_list;
+
+	if (packet->dst_peer != NULL) {
+		proto_nexthop = packet->dst_peer->next_hop_address;
+	} else {
+		proto_nexthop = *dst;
+		do {
+			peer = nhrp_peer_route_full(
+				packet->dst_iface, &proto_nexthop, 0,
+				NHRP_PEER_TYPEMASK_ROUTE_VIA_NHS, src, cielist);
+			if (peer == NULL || peer->type == NHRP_PEER_TYPE_NEGATIVE) {
+				nhrp_error("No peer entry for protocol address %s",
+					   nhrp_address_format(&proto_nexthop,
+							       sizeof(tmp), tmp));
+				return FALSE;
+			}
+			if (peer->type != NHRP_PEER_TYPE_LOCAL_ROUTE)
+				break;
+			if (peer->next_hop_address.type == AF_UNSPEC)
+				break;
+			proto_nexthop = peer->next_hop_address;
+		} while (1);
+
+		packet->dst_peer = nhrp_peer_get(peer);
+	}
+
+	return TRUE;
+}
+
+int nhrp_packet_marshall_and_send(struct nhrp_packet *packet)
+{
+	uint8_t pdu[MAX_PDU_SIZE];
+	char tmp[4][64];
+	int size;
+
+	nhrp_debug("Sending packet %d, from: %s (nbma %s), to: %s (nbma %s)",
+		   packet->hdr.type,
+		   nhrp_address_format(&packet->src_protocol_address,
+				       sizeof(tmp[0]), tmp[0]),
+		   nhrp_address_format(&packet->src_nbma_address,
+				       sizeof(tmp[1]), tmp[1]),
+		   nhrp_address_format(&packet->dst_protocol_address,
+				       sizeof(tmp[2]), tmp[2]),
+		   nhrp_address_format(&packet->dst_peer->next_hop_address,
+				       sizeof(tmp[3]), tmp[3]));
+
+	size = marshall_packet(pdu, sizeof(pdu), packet);
+	if (size < 0) {
+		nhrp_error("Packet marshalling failed (r=%d)", size);
+		return FALSE;
+	}
+
+	if (!kernel_send(pdu, size, packet->dst_iface,
+			 &packet->dst_peer->next_hop_address))
+		return FALSE;
+
+	return TRUE;
+}
+
+int nhrp_packet_route_and_send(struct nhrp_packet *packet)
+{
+	struct nhrp_payload *payload;
+
+	if (packet->dst_peer == NULL || packet->dst_iface == NULL) {
+		if (!nhrp_packet_route(packet)) {
+			nhrp_packet_send_error(packet, NHRP_ERROR_PROTOCOL_ADDRESS_UNREACHABLE, 0);
+			return TRUE;
+		}
+	}
+
+	if (packet->src_nbma_address.addr_len == 0)
+		packet->src_nbma_address = packet->dst_peer->my_nbma_address;
+	if (packet->src_protocol_address.addr_len == 0)
+		packet->src_protocol_address = packet->dst_iface->protocol_address;
+	if (packet->hdr.afnum == AFNUM_RESERVED)
+		packet->hdr.afnum = packet->dst_peer->afnum;
+	if (packet->hdr.protocol_type == 0)
+		packet->hdr.protocol_type = packet->dst_peer->protocol_type;
+
+	/* RFC2332 5.3.1 */
+	payload = nhrp_packet_extension(
+		packet, NHRP_EXTENSION_RESPONDER_ADDRESS |
+		NHRP_EXTENSION_FLAG_COMPULSORY | NHRP_EXTENSION_FLAG_NOCREATE,
+		NHRP_PAYLOAD_TYPE_CIE_LIST);
+	if (packet_types[packet->hdr.type].type == NHRP_TYPE_REPLY &&
+	    (payload != NULL && list_empty(&payload->u.cie_list))) {
+		struct nhrp_cie *cie;
+
+		cie = nhrp_cie_alloc();
+		if (cie == NULL)
+			return FALSE;
+
+		cie->hdr.holding_time = htons(packet->dst_iface->holding_time);
+		cie->nbma_address = packet->dst_peer->my_nbma_address;
+		cie->protocol_address = packet->dst_iface->protocol_address;
+		nhrp_payload_set_type(payload, NHRP_PAYLOAD_TYPE_CIE_LIST);
+		nhrp_payload_add_cie(payload, cie);
+	}
+
+	/* RFC2332 5.3.4 - Authentication is always done pairwise on an NHRP
+	 * hop-by-hop basis; i.e. regenerated at each hop. */
+	payload = nhrp_packet_extension(packet,
+					NHRP_EXTENSION_AUTHENTICATION |
+					NHRP_EXTENSION_FLAG_COMPULSORY,
+					NHRP_PAYLOAD_TYPE_RAW);
+	nhrp_payload_free(payload);
+	if (packet->dst_iface->auth_token != NULL)
+		nhrp_payload_set_raw(payload,
+			nhrp_buffer_copy(packet->dst_iface->auth_token));
+
+	if (packet->dst_peer->type == NHRP_PEER_TYPE_LOCAL_ADDR) {
+		packet->src_iface = packet->dst_peer->interface;
+		return nhrp_packet_receive_local(packet);
+	}
+
+	if (packet->dst_peer->flags & (NHRP_PEER_FLAG_UP |
+				       NHRP_PEER_FLAG_LOWER_UP))
+		return nhrp_packet_marshall_and_send(packet);
+
+	if (packet->dst_peer->queued_packet != NULL)
+		nhrp_packet_put(packet->dst_peer->queued_packet);
+	packet->dst_peer->queued_packet = nhrp_packet_get(packet);
+
+	return TRUE;
+}
+
+int nhrp_packet_send(struct nhrp_packet *packet)
+{
+	struct nhrp_payload *payload;
+	struct nhrp_cie *cie;
+
+	if (packet->dst_iface == NULL) {
+		if (!nhrp_packet_route(packet)) {
+			nhrp_packet_send_error(packet, NHRP_ERROR_PROTOCOL_ADDRESS_UNREACHABLE, 0);
+			return TRUE;
+		}
+	}
+
+	/* Cisco NAT extension CIE */
+	if (packet_types[packet->hdr.type].type != NHRP_TYPE_INDICATION &&
+	    (packet->hdr.flags & NHRP_FLAG_REGISTRATION_NAT)) {
+		payload = nhrp_packet_extension(packet, NHRP_EXTENSION_NAT_ADDRESS,
+						NHRP_PAYLOAD_TYPE_CIE_LIST);
+
+		if (packet->dst_iface->nat_cie.nbma_address.addr_len &&
+		    payload != NULL && list_empty(&payload->u.cie_list)) {
+			cie = nhrp_cie_alloc();
+			if (cie != NULL) {
+				*cie = packet->dst_iface->nat_cie;
+				nhrp_cie_reset(cie);
+				nhrp_payload_add_cie(payload, cie);
+			}
+		}
+	}
+
+	return nhrp_packet_route_and_send(packet);
+}
+
+static void nhrp_packet_xmit_timeout_cb(struct ev_timer *w, int revents)
+{
+	struct nhrp_packet *packet =
+		container_of(w, struct nhrp_packet, timeout);
+
+	list_del(&packet->request_list_entry);
+
+	if (packet->dst_peer != NULL &&
+	    ++packet->retry < PACKET_RETRIES) {
+		nhrp_packet_marshall_and_send(packet);
+
+		list_add(&packet->request_list_entry, &pending_requests);
+	} else {
+		ev_timer_stop(&packet->timeout);
+		if (packet->dst_peer == NULL)
+			nhrp_error("nhrp_packet_xmit_timeout: no destination peer!");
+		if (packet->handler != NULL)
+			packet->handler(packet->handler_ctx, NULL);
+		nhrp_packet_dequeue(packet);
+	}
+}
+
+int nhrp_packet_send_request(struct nhrp_packet *pkt,
+			     void (*handler)(void *ctx, struct nhrp_packet *packet),
+			     void *ctx)
+{
+	struct nhrp_packet *packet;
+
+	packet = nhrp_packet_get(pkt);
+
+	packet->retry = 0;
+	if (packet->hdr.u.request_id == constant_htonl(0)) {
+		request_id++;
+		packet->hdr.u.request_id = htonl(request_id);
+	}
+
+	packet->handler = handler;
+	packet->handler_ctx = ctx;
+	list_add(&packet->request_list_entry, &pending_requests);
+	ev_timer_again(&packet->timeout);
+
+	return nhrp_packet_send(packet);
+}
+
+int nhrp_packet_send_error(struct nhrp_packet *error_packet,
+			   uint16_t indication_code, uint16_t offset)
+{
+	struct nhrp_packet *p;
+	struct nhrp_payload *pl;
+	int r;
+
+	/* RFC2332 5.2.7 Never generate errors about errors */
+	if (error_packet->hdr.type == NHRP_PACKET_ERROR_INDICATION)
+		return TRUE;
+
+	p = nhrp_packet_alloc();
+	p->hdr = error_packet->hdr;
+	p->hdr.type = NHRP_PACKET_ERROR_INDICATION;
+	p->hdr.hop_count = 0;
+	p->hdr.u.error.code = indication_code;
+	p->hdr.u.error.offset = htons(offset);
+	p->dst_iface = error_packet->src_iface;
+
+	if (packet_types[error_packet->hdr.type].type == NHRP_TYPE_REPLY)
+		p->dst_protocol_address = error_packet->dst_protocol_address;
+	else
+		p->dst_protocol_address = error_packet->src_protocol_address;
+
+	pl = nhrp_packet_payload(p, NHRP_PAYLOAD_TYPE_RAW);
+	pl->u.raw = nhrp_buffer_alloc(error_packet->req_pdulen);
+	memcpy(pl->u.raw->data, error_packet->req_pdu, error_packet->req_pdulen);
+
+	/* Standard extensions */
+	nhrp_packet_extension(p,
+			      NHRP_EXTENSION_FORWARD_TRANSIT_NHS |
+			      NHRP_EXTENSION_FLAG_COMPULSORY,
+			      NHRP_PAYLOAD_TYPE_CIE_LIST);
+
+	if (p->dst_protocol_address.type == PF_UNSPEC)
+		r = nhrp_do_handle_error_indication(p, error_packet);
+	else
+		r = nhrp_packet_send(p);
+
+	nhrp_packet_put(p);
+
+	return r;
+}
+
+int nhrp_packet_send_traffic(struct nhrp_interface *iface,
+			     struct nhrp_address *nbma_src,
+			     struct nhrp_address *protocol_src,
+			     struct nhrp_address *protocol_dst,
+			     int protocol_type, uint8_t *pdu, size_t pdulen)
+{
+	struct nhrp_rate_limit *rl;
+	struct nhrp_packet *p;
+	struct nhrp_payload *pl;
+	struct nhrp_peer *peer;
+	char tmp1[64], tmp2[64], tmp3[64], tmp4[64];
+	int r;
+
+	if (!(iface->flags & NHRP_INTERFACE_FLAG_REDIRECT))
+		return FALSE;
+
+	/* Are we serving the NBMA source */
+	peer = nhrp_interface_find_peer(iface, nbma_src);
+	if (peer == NULL || peer->type != NHRP_PEER_TYPE_DYNAMIC)
+		return FALSE;
+
+	rl = get_rate_limit(protocol_src, protocol_dst);
+	if (rl == NULL)
+		return FALSE;
+
+	/* If silence period has elapsed, reset algorithm */
+	if (ev_now() > rl->rate_last + RATE_LIMIT_SILENCE)
+		rl->rate_tokens = 0;
+
+	/* Too many ignored redirects; just update time of last packet */
+	if (rl->rate_tokens >= RATE_LIMIT_MAX_TOKENS) {
+		rl->rate_last = ev_now();
+		return FALSE;
+	}
+
+	/* Check for load limit; set rate_last to last sent redirect */
+	if (rl->rate_tokens != 0 &&
+	    ev_now() < rl->rate_last + RATE_LIMIT_SEND_INTERVAL)
+		return FALSE;
+
+	rl->rate_tokens++;
+	rl->rate_last = ev_now();
+
+	p = nhrp_packet_alloc();
+	p->hdr = (struct nhrp_packet_header) {
+		.protocol_type = protocol_type,
+		.version = NHRP_VERSION_RFC2332,
+		.type = NHRP_PACKET_TRAFFIC_INDICATION,
+		.hop_count = 0,
+	};
+	p->dst_protocol_address = *protocol_src;
+
+	pl = nhrp_packet_payload(p, NHRP_PAYLOAD_TYPE_RAW);
+	pl->u.raw = nhrp_buffer_alloc(pdulen);
+	memcpy(pl->u.raw->data, pdu, pdulen);
+
+	/* Standard extensions */
+	nhrp_packet_extension(p,
+			      NHRP_EXTENSION_FORWARD_TRANSIT_NHS |
+			      NHRP_EXTENSION_FLAG_COMPULSORY,
+			      NHRP_PAYLOAD_TYPE_CIE_LIST);
+
+	nhrp_info("Sending Traffic Indication about packet from %s to %s (to %s/%s)",
+		nhrp_address_format(protocol_src, sizeof(tmp1), tmp1),
+		nhrp_address_format(protocol_dst, sizeof(tmp2), tmp2),
+		nhrp_address_format(&peer->protocol_address, sizeof(tmp3), tmp3),
+		nhrp_address_format(&peer->next_hop_address, sizeof(tmp4), tmp4));
+
+	p->dst_iface = iface;
+	p->dst_peer = nhrp_peer_get(peer);
+	r = nhrp_packet_send(p);
+	nhrp_packet_put(p);
+
+	return r;
+}
+
+void nhrp_packet_hook_request(int request,
+			      int (*handler)(struct nhrp_packet *packet))
+{
+	NHRP_BUG_ON(request < 0 || request >= ARRAY_SIZE(packet_types));
+	NHRP_BUG_ON(packet_types[request].handler != NULL);
+
+	packet_types[request].handler = handler;
+}
diff --git a/nhrp/nhrp_packet.h b/nhrp/nhrp_packet.h
new file mode 100644
index 0000000..3f435c8
--- /dev/null
+++ b/nhrp/nhrp_packet.h
@@ -0,0 +1,128 @@
+/* nhrp_packet.h - In-memory NHRP packet definitions
+ *
+ * Copyright (C) 2007-2009 Timo Teräs <timo.teras@iki.fi>
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 or later as
+ * published by the Free Software Foundation.
+ *
+ * See http://www.gnu.org/ for details.
+ */
+
+#ifndef NHRP_PACKET_H
+#define NHRP_PACKET_H
+
+#include "libev.h"
+#include "list.h"
+#include "nhrp_protocol.h"
+#include "nhrp_address.h"
+
+#define NHRP_MAX_EXTENSIONS		10
+
+#define NHRP_PACKET_DEFAULT_HOP_COUNT	16
+
+struct nhrp_interface;
+
+struct nhrp_buffer {
+	uint32_t length;
+	uint8_t data[NHRP_EMPTY_ARRAY];
+};
+
+struct nhrp_cie {
+	struct list_head	cie_list_entry;
+	struct nhrp_cie_header	hdr;
+	struct nhrp_address	nbma_address;
+	struct nhrp_address	protocol_address;
+};
+
+#define NHRP_PAYLOAD_TYPE_ANY		-1
+#define NHRP_PAYLOAD_TYPE_NONE		0
+#define NHRP_PAYLOAD_TYPE_RAW		1
+#define NHRP_PAYLOAD_TYPE_CIE_LIST	2
+
+struct nhrp_payload {
+	uint16_t extension_type;
+	uint16_t payload_type;
+	union {
+		struct nhrp_buffer *raw;
+		struct list_head cie_list;
+	} u;
+};
+
+struct nhrp_packet {
+	int ref;
+
+	struct nhrp_packet_header	hdr;
+	struct nhrp_address		src_nbma_address;
+	struct nhrp_address		src_protocol_address;
+	struct nhrp_address		dst_protocol_address;
+
+	int				num_extensions;
+	struct nhrp_payload		extension_by_order[NHRP_MAX_EXTENSIONS];
+	struct nhrp_payload *		extension_by_type[NHRP_MAX_EXTENSIONS];
+
+	struct list_head		request_list_entry;
+	struct ev_timer			timeout;
+	void				(*handler)(void *ctx, struct nhrp_packet *packet);
+	void *				handler_ctx;
+	int				retry;
+
+	uint8_t *			req_pdu;
+	size_t				req_pdulen;
+
+	struct nhrp_interface *		src_iface;
+	struct nhrp_address		src_linklayer_address;
+	struct nhrp_interface *		dst_iface;
+	struct nhrp_peer *		dst_peer;
+};
+
+#define NHRP_EXTENSION_FLAG_NOCREATE	0x00010000
+
+int nhrp_rate_limit_clear(struct nhrp_address *addr, int prefix_len);
+
+struct nhrp_buffer *nhrp_buffer_alloc(uint32_t size);
+struct nhrp_buffer *nhrp_buffer_copy(struct nhrp_buffer *buffer);
+int nhrp_buffer_cmp(struct nhrp_buffer *a, struct nhrp_buffer *b);
+void nhrp_buffer_free(struct nhrp_buffer *buffer);
+
+struct nhrp_cie *nhrp_cie_alloc(void);
+void nhrp_cie_free(struct nhrp_cie *cie);
+void nhrp_cie_reset(struct nhrp_cie *cie);
+
+void nhrp_payload_set_type(struct nhrp_payload *payload, int type);
+void nhrp_payload_set_raw(struct nhrp_payload *payload, struct nhrp_buffer *buf);
+void nhrp_payload_add_cie(struct nhrp_payload *payload, struct nhrp_cie *cie);
+struct nhrp_cie *nhrp_payload_get_cie(struct nhrp_payload *payload, int index);
+void nhrp_payload_free(struct nhrp_payload *payload);
+
+struct nhrp_packet *nhrp_packet_alloc(void);
+struct nhrp_packet *nhrp_packet_get(struct nhrp_packet *packet);
+void nhrp_packet_put(struct nhrp_packet *packet);
+
+struct nhrp_payload *nhrp_packet_payload(struct nhrp_packet *packet, int payload_type);
+struct nhrp_payload *nhrp_packet_extension(struct nhrp_packet *packet,
+					   uint32_t extension, int payload_type);
+int nhrp_packet_receive(uint8_t *pdu, size_t pdulen,
+			struct nhrp_interface *iface,
+			struct nhrp_address *from);
+int nhrp_packet_route(struct nhrp_packet *packet);
+int nhrp_packet_reroute(struct nhrp_packet *packet, struct nhrp_peer *dst_peer);
+int nhrp_packet_marshall_and_send(struct nhrp_packet *packet);
+int nhrp_packet_route_and_send(struct nhrp_packet *packet);
+int nhrp_packet_send(struct nhrp_packet *packet);
+int nhrp_packet_send_request(struct nhrp_packet *packet,
+			     void (*handler)(void *ctx, struct nhrp_packet *packet),
+			     void *ctx);
+int nhrp_packet_send_error(struct nhrp_packet *error_packet,
+			   uint16_t indication_code, uint16_t offset);
+int nhrp_packet_send_traffic(struct nhrp_interface *iface,
+			     struct nhrp_address *nbma_src,
+			     struct nhrp_address *protocol_src,
+			     struct nhrp_address *protocol_dst,
+			     int protocol_type, uint8_t *pdu, size_t pdulen);
+
+void nhrp_packet_hook_request(int request,
+			      int (*handler)(struct nhrp_packet *packet));
+
+#endif
diff --git a/nhrp/nhrp_peer.c b/nhrp/nhrp_peer.c
new file mode 100644
index 0000000..c53d4c4
--- /dev/null
+++ b/nhrp/nhrp_peer.c
@@ -0,0 +1,2106 @@
+/* nhrp_peer.c - NHRP peer cache implementation
+ *
+ * Copyright (C) 2007-2009 Timo Teräs <timo.teras@iki.fi>
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 or later as
+ * published by the Free Software Foundation.
+ *
+ * See http://www.gnu.org/ for details.
+ */
+
+#include <stdio.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/wait.h>
+#include <sys/time.h>
+#include <netinet/in.h>
+#include "nhrp_common.h"
+#include "nhrp_peer.h"
+#include "nhrp_interface.h"
+
+#define NHRP_PEER_FORMAT_LEN		128
+
+#define NHRP_SCRIPT_TIMEOUT		(2*60)
+#define NHRP_NEGATIVE_CACHE_TIME	(3*60)
+#define NHRP_EXPIRY_TIME		(5*60)
+
+#define NHRP_HOLDING_TIME_DIVISOR	3	/* See RFC-2332 5.2.3 */
+
+#define NHRP_RETRY_REGISTER_TIME	(30 + random()/(RAND_MAX/60))
+#define NHRP_RETRY_ERROR_TIME		(60 + random()/(RAND_MAX/120))
+
+#define NHRP_PEER_FLAG_PRUNE_PENDING	0x00010000
+
+const char * const nhrp_peer_type[] = {
+	[NHRP_PEER_TYPE_INCOMPLETE]	= "incomplete",
+	[NHRP_PEER_TYPE_NEGATIVE]	= "negative",
+	[NHRP_PEER_TYPE_CACHED]		= "cached",
+	[NHRP_PEER_TYPE_SHORTCUT_ROUTE]	= "shortcut-route",
+	[NHRP_PEER_TYPE_DYNAMIC]	= "dynamic",
+	[NHRP_PEER_TYPE_DYNAMIC_NHS]	= "dynamic-nhs",
+	[NHRP_PEER_TYPE_STATIC]		= "static",
+	[NHRP_PEER_TYPE_STATIC_DNS]	= "dynamic-map",
+	[NHRP_PEER_TYPE_LOCAL_ROUTE]	= "local-route",
+	[NHRP_PEER_TYPE_LOCAL_ADDR]	= "local",
+};
+
+static int nhrp_peer_num_total = 0;
+static struct list_head local_peer_list = LIST_INITIALIZER(local_peer_list);
+
+/* Peer entrys life, pending callbacks and their call order are listed
+ * here.
+ *
+ * Generally everything starts from nhrp_peer_insert() call which schedules
+ * (during startup) or directly invokes nhrp_peer_insert_cb().
+ *
+ * INCOMPLETE:
+ * 1. nhrp_peer_insert_cb: send resolution request
+ * 2. nhrp_peer_handle_resolution_reply: entry deleted or reinserted NEGATIVE
+ *
+ * NEGATIVE:
+ * 1. nhrp_peer_insert_cb: schedule task remove
+ *
+ * CACHED, STATIC, DYNAMIC, DYNAMIC_NHS:
+ * 1. nhrp_peer_insert_cb: calls nhrp_peer_restart_cb
+ * 2. nhrp_peer_restart_cb: resolves dns name, or calls nhrp_run_up_script()
+ * 3. nhrp_peer_address_query_cb: calls nhrp_peer_run_up_script()
+ * 4. nhrp_peer_run_up_script: spawns script, or goes to nhrp_peer_lower_is_up()
+ * 5. nhrp_peer_script_peer_up_done: calls nhrp_peer_lower_is_up()
+ * 6. nhrp_peer_lower_is_up: sends registration, or goes to nhrp_peer_is_up()
+ * 7. nhrp_peer_handle_registration_reply:
+ *	a. on success: calls nhrp_peer_is_up()
+ *	b. on error reply: calls nhrp_peer_send_purge_protocol()
+ *	   nhrp_peer_handle_purge_protocol_reply: sends new registration
+ * 8. nhrp_peer_is_up: schedules re-register, expire or deletion
+ *
+ * ON EXPIRE:
+ *	schedule remove
+ *	nhrp_peer_renew is called if peer has USED flag set or becomes set,
+ *	while the peer is expired
+ * ON RENEW: calls sends resolution request, schedule EXPIRE
+ *
+ * ON ERROR for CACHED: reinsert as NEGATIVE
+ * ON ERROR for STATIC: fork peer-down script (if was lower up)
+ *			schedule task request link
+ * ON ERROR for DYNAMIC: fork peer-down script (if was lower up)
+ *			 delete peer
+ *
+ * SHORTCUT_ROUTE:
+ * 1. nhrp_peer_insert_cb: spawns route-up script, or schedules EXPIRE
+ *
+ * STATIC_DNS:
+ * 1. nhrp_peer_insert_cb: calls nhrp_peer_dnsmap_restart_cb
+ * 2. nhrp_peer_dnsmap_restart_cb: resolves dns name
+ * 3. nhrp_peer_dnsmap_query_cb: create new peer entries,
+ *      renew existing and delete expired, schedule restart
+ *
+ * LOCAL:
+ * nothing, only netlink code modifies these
+ */
+
+static void nhrp_peer_reinsert(struct nhrp_peer *peer, int type);
+static void nhrp_peer_restart_cb(struct ev_timer *w, int revents);
+static void nhrp_peer_dnsmap_restart_cb(struct ev_timer *w, int revents);
+static void nhrp_peer_remove_cb(struct ev_timer *w, int revents);
+static void nhrp_peer_send_resolve(struct nhrp_peer *peer);
+static void nhrp_peer_send_register_cb(struct ev_timer *w, int revents);
+static void nhrp_peer_expire_cb(struct ev_timer *w, int revents);
+
+static const char *nhrp_error_indication_text(int ei)
+{
+	switch (ei) {
+	case -1:
+		return "timeout";
+	case NHRP_ERROR_UNRECOGNIZED_EXTENSION:
+		return "unrecognized extension";
+	case NHRP_ERROR_LOOP_DETECTED:
+		return "loop detected";
+	case NHRP_ERROR_PROTOCOL_ADDRESS_UNREACHABLE:
+		return "protocol address unreachable";
+	case NHRP_ERROR_PROTOCOL_ERROR:
+		return "protocol error";
+	case NHRP_ERROR_SDU_SIZE_EXCEEDED:
+		return "SDU size exceeded";
+	case NHRP_ERROR_INVALID_EXTENSION:
+		return "invalid extension";
+	case NHRP_ERROR_INVALID_RESOLUTION_REPLY:
+		return "unexpected resolution reply";
+	case NHRP_ERROR_AUTHENTICATION_FAILURE:
+		return "authentication failure";
+	case NHRP_ERROR_HOP_COUNT_EXCEEDED:
+		return "hop count exceeded";
+	}
+	return "unknown";
+}
+
+static const char *nhrp_cie_code_text(int ct)
+{
+	switch (ct) {
+	case NHRP_CODE_SUCCESS:
+		return "success";
+	case NHRP_CODE_ADMINISTRATIVELY_PROHIBITED:
+		return "administratively prohibited";
+	case NHRP_CODE_INSUFFICIENT_RESOURCES:
+		return "insufficient resources";
+	case NHRP_CODE_NO_BINDING_EXISTS:
+		return "no binding exists";
+	case NHRP_CODE_BINDING_NON_UNIQUE:
+		return "binding non-unique";
+	case NHRP_CODE_UNIQUE_ADDRESS_REGISTERED:
+		return "unique address already registered";
+	}
+	return "unknown";
+}
+
+static char *nhrp_peer_format_full(struct nhrp_peer *peer, size_t len,
+				   char *buf, int full)
+{
+	char tmp[NHRP_PEER_FORMAT_LEN], *str;
+	int i = 0;
+
+	if (peer == NULL) {
+		snprintf(buf, len, "(null)");
+		return buf;
+	}
+
+	i += snprintf(&buf[i], len - i, "%s/%d",
+		nhrp_address_format(&peer->protocol_address, sizeof(tmp), tmp),
+		peer->prefix_length);
+
+	if (peer->next_hop_address.type != PF_UNSPEC) {
+		switch (peer->type) {
+		case NHRP_PEER_TYPE_SHORTCUT_ROUTE:
+		case NHRP_PEER_TYPE_LOCAL_ROUTE:
+			str = "nexthop";
+			break;
+		case NHRP_PEER_TYPE_LOCAL_ADDR:
+			str = "alias";
+			break;
+		default:
+			str = "nbma";
+			break;
+		}
+		i += snprintf(&buf[i], len - i, " %s %s",
+			str,
+			nhrp_address_format(&peer->next_hop_address,
+			sizeof(tmp), tmp));
+	}
+	if (peer->nbma_hostname != NULL) {
+		i += snprintf(&buf[i], len - i, " hostname %s",
+			peer->nbma_hostname);
+	}
+	if (peer->next_hop_nat_oa.type != PF_UNSPEC) {
+		i += snprintf(&buf[i], len - i, " nbma-nat-oa %s",
+			      nhrp_address_format(&peer->next_hop_nat_oa,
+						  sizeof(tmp), tmp));
+	}
+	if (peer->interface != NULL)
+		i += snprintf(&buf[i], len - i, " dev %s",
+			      peer->interface->name);
+	if (peer->mtu)
+		i += snprintf(&buf[i], len - i, " mtu %d", peer->mtu);
+
+	if (!full)
+		return buf;
+
+	if (peer->flags & NHRP_PEER_FLAG_USED)
+		i += snprintf(&buf[i], len - i, " used");
+	if (peer->flags & NHRP_PEER_FLAG_UNIQUE)
+		i += snprintf(&buf[i], len - i, " unique");
+	if (peer->flags & NHRP_PEER_FLAG_UP)
+		i += snprintf(&buf[i], len - i, " up");
+	else if (peer->flags & NHRP_PEER_FLAG_LOWER_UP)
+		i += snprintf(&buf[i], len - i, " lower-up");
+	if (peer->expire_time != 0.0) {
+		int rel;
+
+		rel = peer->expire_time - ev_now();
+		if (rel >= 0) {
+			i += snprintf(&buf[i], len - i, " expires_in %d:%02d",
+				      rel / 60, rel % 60);
+		} else {
+			i += snprintf(&buf[i], len - i, " expired");
+		}
+	}
+	if (peer->flags & NHRP_PEER_FLAG_PRUNE_PENDING)
+		i += snprintf(&buf[i], len - i, " dying");
+
+	return buf;
+}
+
+static inline char *nhrp_peer_format(struct nhrp_peer *peer,
+				     size_t len, char *buf)
+{
+	return nhrp_peer_format_full(peer, len, buf, TRUE);
+}
+
+static inline void nhrp_peer_debug_refcount(const char *func,
+					    struct nhrp_peer *peer)
+{
+#if 0
+	char tmp[NHRP_PEER_FORMAT_LEN];
+	nhrp_debug("%s(%s %s) ref=%d",
+		   func, nhrp_peer_type[peer->type],
+		   nhrp_peer_format(peer, sizeof(tmp), tmp),
+		   peer->ref);
+#endif
+}
+
+static void nhrp_peer_resolve_nbma(struct nhrp_peer *peer)
+{
+	char tmp[64];
+	int r;
+
+	if (peer->interface->nbma_address.type == PF_UNSPEC) {
+		r = kernel_route(NULL, &peer->next_hop_address,
+				 &peer->my_nbma_address, NULL,
+				 &peer->my_nbma_mtu);
+		if (!r) {
+			nhrp_error("No route to next hop address %s",
+				   nhrp_address_format(&peer->next_hop_address,
+						       sizeof(tmp), tmp));
+		}
+	} else {
+		peer->my_nbma_address = peer->interface->nbma_address;
+		peer->my_nbma_mtu = peer->interface->nbma_mtu;
+	}
+}
+
+static char *env(const char *key, const char *value)
+{
+	char *buf;
+	buf = malloc(strlen(key)+strlen(value)+2);
+	if (buf == NULL)
+		return NULL;
+	sprintf(buf, "%s=%s", key, value);
+	return buf;
+}
+
+static char *envu32(const char *key, uint32_t value)
+{
+	char *buf;
+	buf = malloc(strlen(key)+16);
+	if (buf == NULL)
+		return NULL;
+	sprintf(buf, "%s=%u", key, value);
+	return buf;
+}
+
+int nhrp_peer_event_ok(union nhrp_peer_event e, int revents)
+{
+	int status;
+
+	if (revents == 0)
+		return TRUE;
+	if (!(revents & EV_CHILD))
+		return FALSE;
+	status = e.child->rstatus;
+	if (WIFEXITED(status) && WEXITSTATUS(status) == 0)
+		return TRUE;
+	return FALSE;
+}
+
+char *nhrp_peer_event_reason(union nhrp_peer_event e, int revents,
+			     size_t buflen, char *buf)
+{
+	int status;
+
+	if (revents & EV_CHILD) {
+		status = e.child->rstatus;
+		if (WIFEXITED(status))
+			snprintf(buf, buflen, "exitstatus %d",
+				 WEXITSTATUS(status));
+		else if (WIFSIGNALED(status))
+			snprintf(buf, buflen, "signal %d",
+				 WTERMSIG(status));
+		else
+			snprintf(buf, buflen, "rstatus %d", status);
+	} else if (revents & EV_TIMEOUT) {
+		snprintf(buf, buflen, "timeout");
+	} else if (revents == 0) {
+		snprintf(buf, buflen, "success");
+	} else {
+		snprintf(buf, buflen, "unknown, revents=%x", revents);
+	}
+	return buf;
+}
+
+struct nhrp_peer *nhrp_peer_from_event(union nhrp_peer_event e, int revents)
+{
+	struct nhrp_peer *peer;
+
+	if (revents & EV_CHILD) {
+		peer = container_of(e.child, struct nhrp_peer, child);
+	} else if (revents & EV_TIMEOUT) {
+		peer = container_of(e.timer, struct nhrp_peer, timer);
+	} else {
+		NHRP_BUG_ON(revents != 0);
+		peer = container_of(e.child, struct nhrp_peer, child);
+	}
+
+	ev_child_stop(&peer->child);
+	ev_timer_stop(&peer->timer);
+
+	return peer;
+}
+
+void nhrp_peer_run_script(struct nhrp_peer *peer, char *action,
+			  void (*cb)(union nhrp_peer_event, int))
+{
+	struct nhrp_interface *iface = peer->interface;
+	const char *argv[] = { nhrp_script_file, action, NULL };
+	char *envp[32];
+	char tmp[64];
+	pid_t pid;
+	int i = 0;
+
+	/* Resolve own NBMA address before forking if required
+	 * since it requires traversing peer cache and can trigger
+	 * logging and other stuff. */
+	if (peer->my_nbma_address.type == PF_UNSPEC)
+		nhrp_peer_resolve_nbma(peer);
+
+	/* Fork and execute script */
+	pid = fork();
+	if (pid == -1) {
+		if (cb != NULL)
+			cb(&peer->child, EV_CHILD | EV_ERROR);
+		return;
+	} else if (pid > 0) {
+		if (cb != NULL) {
+			ev_child_stop(&peer->child);
+			ev_child_init(&peer->child, cb, pid, 0);
+			ev_child_start(&peer->child);
+
+			ev_set_cb(&peer->timer, cb);
+			peer->timer.repeat = NHRP_SCRIPT_TIMEOUT;
+			ev_timer_again(&peer->timer);
+		}
+		return;
+	}
+
+	envp[i++] = env("NHRP_TYPE", nhrp_peer_type[peer->type]);
+	if (iface->protocol_address.type != PF_UNSPEC)
+		envp[i++] = env("NHRP_SRCADDR",
+				nhrp_address_format(&iface->protocol_address,
+						    sizeof(tmp), tmp));
+	if (peer->my_nbma_address.type != PF_UNSPEC)
+		envp[i++] = env("NHRP_SRCNBMA",
+				nhrp_address_format(&peer->my_nbma_address,
+						    sizeof(tmp), tmp));
+	envp[i++] = env("NHRP_DESTADDR",
+			nhrp_address_format(&peer->protocol_address,
+					    sizeof(tmp), tmp));
+	envp[i++] = envu32("NHRP_DESTPREFIX", peer->prefix_length);
+
+	if (peer->purge_reason)
+		envp[i++] = env("NHRP_PEER_DOWN_REASON", peer->purge_reason);
+
+	switch (peer->type) {
+	case NHRP_PEER_TYPE_CACHED:
+	case NHRP_PEER_TYPE_LOCAL_ADDR:
+	case NHRP_PEER_TYPE_STATIC:
+	case NHRP_PEER_TYPE_DYNAMIC:
+	case NHRP_PEER_TYPE_DYNAMIC_NHS:
+		envp[i++] = env("NHRP_DESTNBMA",
+			nhrp_address_format(&peer->next_hop_address,
+					    sizeof(tmp), tmp));
+		if (peer->mtu)
+			envp[i++] = envu32("NHRP_DESTMTU", peer->mtu);
+		if (peer->next_hop_nat_oa.type != PF_UNSPEC)
+			envp[i++] = env("NHRP_DESTNBMA_NAT_OA",
+				nhrp_address_format(&peer->next_hop_nat_oa,
+						    sizeof(tmp), tmp));
+		break;
+	case NHRP_PEER_TYPE_SHORTCUT_ROUTE:
+	case NHRP_PEER_TYPE_LOCAL_ROUTE:
+		envp[i++] = env("NHRP_NEXTHOP",
+			nhrp_address_format(&peer->next_hop_address,
+					    sizeof(tmp), tmp));
+		break;
+	default:
+		NHRP_BUG_ON("invalid peer type");
+	}
+	envp[i++] = env("NHRP_INTERFACE", peer->interface->name);
+	envp[i++] = envu32("NHRP_GRE_KEY", peer->interface->gre_key);
+	envp[i++] = NULL;
+
+	execve(nhrp_script_file, (char **) argv, envp);
+	exit(1);
+}
+
+void nhrp_peer_cancel_async(struct nhrp_peer *peer)
+{
+	if (peer->queued_packet) {
+		nhrp_packet_put(peer->queued_packet);
+		peer->queued_packet = NULL;
+	}
+	if (peer->request) {
+		nhrp_server_finish_request(peer->request);
+		peer->request = NULL;
+	}
+
+	nhrp_address_resolve_cancel(&peer->address_query);
+	ev_timer_stop(&peer->timer);
+	if (ev_is_active(&peer->child)) {
+		kill(SIGINT, peer->child.pid);
+		ev_child_stop(&peer->child);
+	}
+}
+
+void nhrp_peer_send_packet_queue(struct nhrp_peer *peer)
+{
+	if (peer->queued_packet == NULL)
+		return;
+
+	nhrp_packet_marshall_and_send(peer->queued_packet);
+	nhrp_packet_put(peer->queued_packet);
+	peer->queued_packet = NULL;
+}
+
+static void nhrp_peer_schedule(struct nhrp_peer *peer, ev_tstamp timeout,
+			       void (*cb)(struct ev_timer *w, int revents))
+{
+	ev_timer_stop(&peer->timer);
+	ev_timer_init(&peer->timer, cb, timeout, 0.);
+	ev_timer_start(&peer->timer);
+}
+
+static void nhrp_peer_restart_error(struct nhrp_peer *peer)
+{
+	switch (peer->type) {
+	case NHRP_PEER_TYPE_STATIC:
+	case NHRP_PEER_TYPE_DYNAMIC_NHS:
+		nhrp_peer_schedule(peer, NHRP_RETRY_ERROR_TIME,
+				   nhrp_peer_restart_cb);
+		break;
+	default:
+		nhrp_peer_reinsert(peer, NHRP_PEER_TYPE_NEGATIVE);
+		break;
+	}
+}
+
+static void nhrp_peer_script_route_up_done(union nhrp_peer_event e, int revents)
+{
+	struct nhrp_peer *peer = nhrp_peer_from_event(e, revents);
+	char tmp[64], reason[32];
+
+	if (nhrp_peer_event_ok(e, revents)) {
+		if (revents)
+			nhrp_debug("[%s] Route up script: success",
+				   nhrp_address_format(&peer->protocol_address,
+						       sizeof(tmp), tmp));
+
+		peer->flags |= NHRP_PEER_FLAG_UP;
+		nhrp_peer_schedule(peer, peer->expire_time - NHRP_EXPIRY_TIME
+				   - 10 - ev_now(), nhrp_peer_expire_cb);
+	} else {
+		nhrp_info("[%s] Route up script: %s; "
+			  "adding negative cached entry",
+			  nhrp_address_format(&peer->protocol_address,
+					      sizeof(tmp), tmp),
+			  nhrp_peer_event_reason(e, revents,
+						 sizeof(reason), reason));
+
+		nhrp_peer_reinsert(peer, NHRP_PEER_TYPE_NEGATIVE);
+	}
+}
+
+static int nhrp_peer_routes_up(void *ctx, struct nhrp_peer *peer)
+{
+	if (!(peer->flags & NHRP_PEER_FLAG_UP))
+		nhrp_peer_run_script(peer, "route-up",
+				     nhrp_peer_script_route_up_done);
+
+	return 0;
+}
+
+static int nhrp_peer_routes_renew(void *ctx, struct nhrp_peer *peer)
+{
+	int *num_routes = (int *) ctx;
+
+	if (peer->flags & NHRP_PEER_FLAG_PRUNE_PENDING) {
+		peer->flags &= ~NHRP_PEER_FLAG_PRUNE_PENDING;
+		nhrp_peer_cancel_async(peer);
+		nhrp_peer_send_resolve(peer);
+		(*num_routes)++;
+	}
+
+	return 0;
+}
+
+static void nhrp_peer_renew(struct nhrp_peer *peer)
+{
+	struct nhrp_interface *iface = peer->interface;
+	struct nhrp_peer_selector sel;
+	int num_routes = 0;
+
+	/* Renew the cached information: all related routes
+	 * or the peer itself */
+	if (peer->type != NHRP_PEER_TYPE_SHORTCUT_ROUTE) {
+		memset(&sel, 0, sizeof(sel));
+		sel.flags = NHRP_PEER_FIND_UP;
+		sel.type_mask = BIT(NHRP_PEER_TYPE_SHORTCUT_ROUTE);
+		sel.interface = iface;
+		sel.next_hop_address = peer->protocol_address;
+		nhrp_peer_foreach(nhrp_peer_routes_renew, &num_routes, &sel);
+	}
+
+	if (peer->flags & NHRP_PEER_FLAG_PRUNE_PENDING) {
+		peer->flags &= ~NHRP_PEER_FLAG_PRUNE_PENDING;
+		nhrp_peer_cancel_async(peer);
+		nhrp_peer_send_resolve(peer);
+	}
+}
+
+static int is_used(void *ctx, struct nhrp_peer *peer)
+{
+	if (peer->flags & NHRP_PEER_FLAG_USED)
+		return 1;
+
+	return 0;
+}
+
+static void nhrp_peer_expire_cb(struct ev_timer *w, int revents)
+{
+	struct nhrp_peer *peer = container_of(w, struct nhrp_peer, timer);
+	struct nhrp_peer_selector sel;
+	int used;
+
+	peer->flags |= NHRP_PEER_FLAG_PRUNE_PENDING;
+	nhrp_peer_schedule(peer, peer->expire_time - ev_now(),
+			   nhrp_peer_remove_cb);
+
+	if (peer->type == NHRP_PEER_TYPE_SHORTCUT_ROUTE) {
+		memset(&sel, 0, sizeof(sel));
+		sel.interface = peer->interface;
+		sel.protocol_address = peer->next_hop_address;
+		used = nhrp_peer_foreach(is_used, NULL, &sel);
+	} else
+		used = peer->flags & NHRP_PEER_FLAG_USED;
+
+	if (used)
+		nhrp_peer_renew(peer);
+}
+
+static void nhrp_peer_is_down(struct nhrp_peer *peer)
+{
+	struct nhrp_peer_selector sel;
+
+	/* Remove UP flags if not being removed permanently, so futher
+	 * lookups are valid */
+	if (!(peer->flags & NHRP_PEER_FLAG_REMOVED))
+		peer->flags &= ~(NHRP_PEER_FLAG_LOWER_UP | NHRP_PEER_FLAG_UP);
+
+	/* Check if there are routes using this peer as next-hop */
+	if (peer->type != NHRP_PEER_TYPE_SHORTCUT_ROUTE) {
+		memset(&sel, 0, sizeof(sel));
+		sel.type_mask = BIT(NHRP_PEER_TYPE_SHORTCUT_ROUTE);
+		sel.interface = peer->interface;
+		sel.next_hop_address = peer->protocol_address;
+		nhrp_peer_foreach(nhrp_peer_remove_matching, NULL, &sel);
+	}
+
+	/* Remove from lists */
+	if (list_hashed(&peer->mcast_list_entry))
+		list_del(&peer->mcast_list_entry);
+	if (hlist_hashed(&peer->nbma_hash_entry))
+		hlist_del(&peer->nbma_hash_entry);
+}
+
+static void nhrp_peer_is_up(struct nhrp_peer *peer)
+{
+	struct nhrp_interface *iface = peer->interface;
+	struct nhrp_peer_selector sel;
+	int mcast = 0, i;
+	char tmp[64];
+
+	if ((peer->flags & (NHRP_PEER_FLAG_UP | NHRP_PEER_FLAG_REGISTER))
+	    == NHRP_PEER_FLAG_REGISTER) {
+		/* First time registration reply received */
+		nhrp_peer_run_script(peer, "nhs-up", NULL);
+	}
+
+	/* Remove from mcast list if previously there */
+	if (list_hashed(&peer->mcast_list_entry))
+		list_del(&peer->mcast_list_entry);
+
+	/* Check if this one needs multicast traffic */
+	if (BIT(peer->type) & iface->mcast_mask) {
+		mcast = 1;
+	} else {
+		for (i = 0; i < iface->mcast_numaddr; i++) {
+			if (!nhrp_address_cmp(&peer->protocol_address,
+					      &iface->mcast_addr[i])) {
+				mcast = 1;
+				break;
+			}
+		}
+	}
+
+	if (mcast) {
+		list_add(&peer->mcast_list_entry, &iface->mcast_list);
+		nhrp_info("[%s] Peer inserted to multicast list",
+			   nhrp_address_format(&peer->protocol_address,
+					       sizeof(tmp), tmp));
+	}
+
+	/* Searchable by NBMA */
+	if (hlist_hashed(&peer->nbma_hash_entry))
+		hlist_del(&peer->nbma_hash_entry);
+	if (BIT(peer->type) & (BIT(NHRP_PEER_TYPE_CACHED) |
+			       BIT(NHRP_PEER_TYPE_DYNAMIC) |
+			       BIT(NHRP_PEER_TYPE_DYNAMIC_NHS) |
+			       BIT(NHRP_PEER_TYPE_STATIC))) {
+		i = nhrp_address_hash(&peer->next_hop_address) % NHRP_INTERFACE_NBMA_HASH_SIZE;
+		hlist_add_head(&peer->nbma_hash_entry, &iface->nbma_hash[i]);
+	}
+
+	peer->flags |= NHRP_PEER_FLAG_UP | NHRP_PEER_FLAG_LOWER_UP;
+
+	/* Check if there are routes using this peer as next-hop*/
+	if (peer->type != NHRP_PEER_TYPE_SHORTCUT_ROUTE) {
+		memset(&sel, 0, sizeof(sel));
+		sel.type_mask = BIT(NHRP_PEER_TYPE_SHORTCUT_ROUTE);
+		sel.interface = iface;
+		sel.next_hop_address = peer->protocol_address;
+		nhrp_peer_foreach(nhrp_peer_routes_up, NULL, &sel);
+	}
+
+	nhrp_peer_send_packet_queue(peer);
+
+	/* Schedule expiry or renewal */
+	switch (peer->type) {
+	case NHRP_PEER_TYPE_DYNAMIC:
+		nhrp_peer_schedule(peer, peer->expire_time - ev_now(),
+				   nhrp_peer_remove_cb);
+		break;
+	case NHRP_PEER_TYPE_CACHED:
+		nhrp_peer_schedule(
+			peer,
+			peer->expire_time - NHRP_EXPIRY_TIME - ev_now(),
+			nhrp_peer_expire_cb);
+		break;
+	case NHRP_PEER_TYPE_STATIC:
+	case NHRP_PEER_TYPE_DYNAMIC_NHS:
+		if (peer->flags & NHRP_PEER_FLAG_REGISTER) {
+			nhrp_peer_schedule(
+				peer, iface->holding_time /
+				NHRP_HOLDING_TIME_DIVISOR + 1,
+				nhrp_peer_send_register_cb);
+		}
+		break;
+	default:
+		NHRP_BUG_ON("invalid peer type");
+		break;
+	}
+}
+
+static void nhrp_peer_lower_is_up(struct nhrp_peer *peer)
+{
+	peer->flags |= NHRP_PEER_FLAG_LOWER_UP;
+
+	if (peer->flags & NHRP_PEER_FLAG_REGISTER)
+		nhrp_peer_send_register_cb(&peer->timer, 0);
+	else
+		nhrp_peer_is_up(peer);
+}
+
+static void nhrp_peer_script_peer_up_done(union nhrp_peer_event e, int revents)
+{
+	struct nhrp_peer *peer = nhrp_peer_from_event(e, revents);
+	char tmp[64], reason[32];
+
+	if (nhrp_peer_event_ok(e, revents)) {
+		nhrp_debug("[%s] Peer up script: success",
+			   nhrp_address_format(&peer->protocol_address,
+					       sizeof(tmp), tmp));
+
+		kernel_inject_neighbor(&peer->protocol_address,
+				       &peer->next_hop_address,
+				       peer->interface);
+		nhrp_peer_lower_is_up(peer);
+	} else {
+		nhrp_error("[%s] Peer up script failed: %s",
+			   nhrp_address_format(&peer->protocol_address,
+					       sizeof(tmp), tmp),
+			   nhrp_peer_event_reason(e, revents,
+						  sizeof(reason), reason));
+		nhrp_peer_restart_error(peer);
+	}
+}
+
+static void nhrp_peer_run_up_script(struct nhrp_peer *peer)
+{
+	nhrp_peer_run_script(peer, "peer-up",
+			     nhrp_peer_script_peer_up_done);
+}
+
+static void nhrp_peer_address_query_cb(struct nhrp_address_query *query,
+				       int num_addr, struct nhrp_address *addrs)
+{
+	struct nhrp_peer *peer = container_of(query, struct nhrp_peer,
+					      address_query);
+	char host[64];
+
+	if (num_addr > 0) {
+		nhrp_info("Resolved '%s' as %s",
+			  peer->nbma_hostname,
+			  nhrp_address_format(&addrs[0], sizeof(host), host));
+		peer->next_hop_address = addrs[0];
+		peer->afnum = nhrp_afnum_from_pf(peer->next_hop_address.type);
+		nhrp_peer_run_up_script(peer);
+	} else {
+		nhrp_error("Failed to resolve '%s'", peer->nbma_hostname);
+		nhrp_peer_restart_error(peer);
+	}
+}
+
+static void nhrp_peer_restart_cb(struct ev_timer *w, int revents)
+{
+	struct nhrp_peer *peer = container_of(w, struct nhrp_peer, timer);
+
+	if (peer->nbma_hostname != NULL) {
+		nhrp_address_resolve(&peer->address_query,
+				     peer->nbma_hostname,
+				     nhrp_peer_address_query_cb);
+	} else {
+		nhrp_peer_resolve_nbma(peer);
+
+		if (!(peer->flags & NHRP_PEER_FLAG_LOWER_UP))
+			nhrp_peer_run_up_script(peer);
+		else
+			nhrp_peer_script_peer_up_done(&peer->child, 0);
+	}
+}
+
+static void nhrp_peer_send_protocol_purge(struct nhrp_peer *peer)
+{
+	char tmp[64];
+	struct nhrp_packet *packet;
+	struct nhrp_cie *cie;
+	struct nhrp_payload *payload;
+	int sent = FALSE;
+
+	packet = nhrp_packet_alloc();
+	if (packet == NULL)
+		goto error;
+
+	packet->hdr = (struct nhrp_packet_header) {
+		.afnum = peer->afnum,
+		.protocol_type = peer->protocol_type,
+		.version = NHRP_VERSION_RFC2332,
+		.type = NHRP_PACKET_PURGE_REQUEST,
+		.hop_count = NHRP_PACKET_DEFAULT_HOP_COUNT,
+		.flags = NHRP_FLAG_PURGE_NO_REPLY,
+	};
+	if (peer->flags & NHRP_PEER_FLAG_CISCO) {
+		/* Cisco IOS seems to require reqistration and purge
+		 * request id to match, so we need to used a fixed
+		 * value. This is in violation of RFC, though. */
+		packet->hdr.u.request_id =
+			nhrp_address_hash(&peer->interface->protocol_address);
+	}
+	packet->dst_protocol_address = peer->protocol_address;
+
+	/* Payload CIE */
+	cie = nhrp_cie_alloc();
+	if (cie == NULL)
+		goto error_free_packet;
+
+	*cie = (struct nhrp_cie) {
+		.hdr.code = NHRP_CODE_SUCCESS,
+		.hdr.mtu = 0,
+		.hdr.preference = 0,
+		.hdr.prefix_length = 0xff,
+	};
+	cie->protocol_address = peer->interface->protocol_address;
+
+	payload = nhrp_packet_payload(packet, NHRP_PAYLOAD_TYPE_CIE_LIST);
+	nhrp_payload_add_cie(payload, cie);
+
+	nhrp_info("Sending Purge Request (of protocol address) to %s",
+		  nhrp_address_format(&peer->protocol_address,
+				      sizeof(tmp), tmp));
+
+	packet->dst_peer = nhrp_peer_get(peer);
+	packet->dst_iface = peer->interface;
+	sent = nhrp_packet_send(packet);
+error_free_packet:
+	nhrp_packet_put(packet);
+error:
+	if (sent)
+		nhrp_peer_schedule(peer, 2, nhrp_peer_send_register_cb);
+	else
+		nhrp_peer_restart_error(peer);
+}
+
+static int nhrp_add_local_route_cie(void *ctx, struct nhrp_peer *route)
+{
+	struct nhrp_packet *packet = (struct nhrp_packet *) ctx;
+	struct nhrp_payload *payload;
+	struct nhrp_cie *cie;
+
+	if (route->interface != NULL &&
+	    !(route->interface->flags & NHRP_INTERFACE_FLAG_SHORTCUT_DEST))
+		return 0;
+
+	cie = nhrp_cie_alloc();
+	if (cie == NULL)
+		return 0;
+
+	*cie = (struct nhrp_cie) {
+		.hdr.code = 0,
+		.hdr.prefix_length = route->prefix_length,
+		.protocol_address = route->protocol_address,
+	};
+
+	payload = nhrp_packet_payload(packet, NHRP_PAYLOAD_TYPE_CIE_LIST);
+	nhrp_payload_add_cie(payload, cie);
+
+	return 0;
+}
+
+int nhrp_peer_discover_nhs(struct nhrp_peer *peer,
+			   struct nhrp_address *newaddr)
+{
+	struct nhrp_peer_selector sel;
+	char tmp[32], tmp2[32];
+
+	if (nhrp_address_cmp(&peer->protocol_address, newaddr) == 0)
+		return TRUE;
+
+	if (peer->type != NHRP_PEER_TYPE_DYNAMIC_NHS ||
+	    !nhrp_address_is_network(&peer->protocol_address,
+				     peer->prefix_length)) {
+		nhrp_error("Unexpected NHS protocol address change %s -> %s",
+			   nhrp_address_format(&peer->protocol_address,
+					       sizeof(tmp2), tmp2),
+			   nhrp_address_format(newaddr, sizeof(tmp), tmp));
+		return FALSE;
+	}
+
+	if (nhrp_address_prefix_cmp(&peer->protocol_address, newaddr,
+				    peer->prefix_length) != 0) {
+		nhrp_error("Protocol address change to %s is not within %s/%d",
+			   nhrp_address_format(newaddr, sizeof(tmp), tmp),
+			   nhrp_address_format(&peer->protocol_address,
+					       sizeof(tmp2), tmp2),
+			   peer->prefix_length);
+		return FALSE;
+	}
+
+	/* Remove incomplete/cached entries */
+	memset(&sel, 0, sizeof(sel));
+	sel.flags = NHRP_PEER_FIND_EXACT;
+	sel.type_mask = NHRP_PEER_TYPEMASK_REMOVABLE;
+	sel.interface = peer->interface;
+	sel.protocol_address = *newaddr;
+	nhrp_peer_foreach(nhrp_peer_remove_matching, NULL, &sel);
+
+	/* Update protocol address */
+	peer->protocol_address = *newaddr;
+
+	return TRUE;
+}
+
+static void nhrp_peer_handle_registration_reply(void *ctx,
+						struct nhrp_packet *reply)
+{
+	struct nhrp_peer *peer = (struct nhrp_peer *) ctx;
+	struct nhrp_payload *payload;
+	struct nhrp_cie *cie;
+	struct nhrp_packet *packet;
+	char tmp[NHRP_PEER_FORMAT_LEN];
+	int ec = -1;
+
+	if (peer->flags & NHRP_PEER_FLAG_REMOVED)
+		goto ret;
+
+	if (reply == NULL ||
+	    reply->hdr.type != NHRP_PACKET_REGISTRATION_REPLY) {
+		ec = reply ? reply->hdr.u.error.code : -1;
+		nhrp_info("Failed to register to %s: %s (%d)",
+			  nhrp_address_format(&peer->protocol_address,
+					      sizeof(tmp), tmp),
+			  nhrp_error_indication_text(ec), ntohs(ec));
+
+		if (ec == NHRP_ERROR_HOP_COUNT_EXCEEDED)
+			nhrp_peer_discover_nhs(peer,
+					       &reply->src_protocol_address);
+
+		if (reply != NULL) {
+			nhrp_peer_schedule(peer, NHRP_RETRY_REGISTER_TIME,
+					   nhrp_peer_send_register_cb);
+		} else {
+			nhrp_peer_restart_error(peer);
+		}
+		goto ret;
+	}
+
+	/* Check servers protocol address */
+	if (!nhrp_peer_discover_nhs(peer, &reply->dst_protocol_address)) {
+		nhrp_peer_restart_error(peer);
+		goto ret;
+	}
+
+	/* Check result */
+	payload = nhrp_packet_payload(reply, NHRP_PAYLOAD_TYPE_CIE_LIST);
+	if (payload != NULL) {
+		cie = nhrp_payload_get_cie(payload, 1);
+		if (cie != NULL)
+			ec = cie->hdr.code;
+	}
+
+	nhrp_info("Received Registration Reply from %s: %s",
+		  nhrp_address_format(&peer->protocol_address,
+				      sizeof(tmp), tmp),
+		  nhrp_cie_code_text(ec));
+
+	switch (ec) {
+	case NHRP_CODE_SUCCESS:
+		break;
+	case NHRP_CODE_UNIQUE_ADDRESS_REGISTERED:
+		nhrp_peer_send_protocol_purge(peer);
+		goto ret;
+	default:
+		nhrp_peer_schedule(peer, NHRP_RETRY_REGISTER_TIME,
+				   nhrp_peer_send_register_cb);
+		goto ret;
+	}
+
+	/* Check for NAT */
+	payload = nhrp_packet_extension(reply,
+					NHRP_EXTENSION_NAT_ADDRESS |
+					NHRP_EXTENSION_FLAG_NOCREATE,
+					NHRP_PAYLOAD_TYPE_CIE_LIST);
+	if (payload != NULL) {
+		cie = nhrp_payload_get_cie(payload, 2);
+		if (cie != NULL) {
+			nhrp_info("NAT detected: our real NBMA address is %s",
+				  nhrp_address_format(&cie->nbma_address,
+						      sizeof(tmp), tmp));
+			peer->interface->nat_cie = *cie;
+		}
+	}
+
+	/* If not re-registration, send a purge request for each subnet
+	 * we accept shortcuts to, to clear server redirection cache. */
+	if (!(peer->flags & NHRP_PEER_FLAG_UP) &&
+	    (packet = nhrp_packet_alloc()) != NULL) {
+		struct nhrp_peer_selector sel;
+
+		packet->hdr = (struct nhrp_packet_header) {
+			.afnum = peer->afnum,
+			.protocol_type = peer->protocol_type,
+			.version = NHRP_VERSION_RFC2332,
+			.type = NHRP_PACKET_PURGE_REQUEST,
+			.hop_count = NHRP_PACKET_DEFAULT_HOP_COUNT,
+		};
+		packet->dst_protocol_address = peer->protocol_address;
+
+		memset(&sel, 0, sizeof(sel));
+		sel.type_mask = BIT(NHRP_PEER_TYPE_LOCAL_ADDR);
+		nhrp_peer_foreach(nhrp_add_local_route_cie, packet, &sel);
+
+		nhrp_packet_extension(packet,
+				      NHRP_EXTENSION_FORWARD_TRANSIT_NHS |
+				      NHRP_EXTENSION_FLAG_COMPULSORY,
+				      NHRP_PAYLOAD_TYPE_CIE_LIST);
+		nhrp_packet_extension(packet,
+				      NHRP_EXTENSION_REVERSE_TRANSIT_NHS |
+				      NHRP_EXTENSION_FLAG_COMPULSORY,
+				      NHRP_PAYLOAD_TYPE_CIE_LIST);
+		nhrp_packet_extension(packet,
+				      NHRP_EXTENSION_RESPONDER_ADDRESS |
+				      NHRP_EXTENSION_FLAG_COMPULSORY,
+				      NHRP_PAYLOAD_TYPE_CIE_LIST);
+
+		nhrp_info("Sending Purge Request (of local routes) to %s",
+			  nhrp_address_format(&peer->protocol_address,
+					      sizeof(tmp), tmp));
+
+		packet->dst_peer = nhrp_peer_get(peer);
+		packet->dst_iface = peer->interface;
+		nhrp_packet_send_request(packet, NULL, NULL);
+		nhrp_packet_put(packet);
+	}
+
+	/* Re-register after holding time expires */
+	nhrp_peer_is_up(peer);
+ret:
+	nhrp_peer_put(peer);
+}
+
+static void nhrp_peer_send_register_cb(struct ev_timer *w, int revents)
+{
+	struct nhrp_peer *peer = container_of(w, struct nhrp_peer, timer);
+	char dst[64];
+	struct nhrp_packet *packet;
+	struct nhrp_cie *cie;
+	struct nhrp_payload *payload;
+	int sent = FALSE;
+
+	packet = nhrp_packet_alloc();
+	if (packet == NULL)
+		goto error;
+
+	packet->hdr = (struct nhrp_packet_header) {
+		.afnum = peer->afnum,
+		.protocol_type = peer->protocol_type,
+		.version = NHRP_VERSION_RFC2332,
+		.type = NHRP_PACKET_REGISTRATION_REQUEST,
+		.hop_count = NHRP_PACKET_DEFAULT_HOP_COUNT,
+		.flags = NHRP_FLAG_REGISTRATION_UNIQUE |
+			 NHRP_FLAG_REGISTRATION_NAT
+	};
+	if (peer->flags & NHRP_PEER_FLAG_CISCO) {
+		/* Cisco IOS seems to require reqistration and purge
+		 * request id to match, so we need to used a fixed
+		 * value. This is in violation of RFC, though. */
+		packet->hdr.u.request_id =
+			nhrp_address_hash(&peer->interface->protocol_address);
+	}
+	packet->dst_protocol_address = peer->protocol_address;
+
+	if (peer->type == NHRP_PEER_TYPE_DYNAMIC_NHS &&
+	    nhrp_address_is_network(&peer->protocol_address,
+				    peer->prefix_length)) {
+		/* We are not yet sure of the protocol address of the NHS -
+		 * send registration to the broadcast address with one hop
+		 * limit. Except the NHS to reply with it's real protocol
+		 * address. */
+		nhrp_address_set_broadcast(&packet->dst_protocol_address,
+					   peer->prefix_length);
+		packet->hdr.hop_count = 0;
+	}
+
+
+	/* Payload CIE */
+	cie = nhrp_cie_alloc();
+	if (cie == NULL)
+		goto error;
+
+        *cie = (struct nhrp_cie) {
+		.hdr.code = NHRP_CODE_SUCCESS,
+		.hdr.prefix_length = 0xff,
+		.hdr.mtu = htons(peer->my_nbma_mtu),
+		.hdr.holding_time = htons(peer->interface->holding_time),
+		.hdr.preference = 0,
+	};
+
+	payload = nhrp_packet_payload(packet, NHRP_PAYLOAD_TYPE_CIE_LIST);
+	nhrp_payload_add_cie(payload, cie);
+
+	/* Standard extensions */
+	nhrp_packet_extension(packet,
+			      NHRP_EXTENSION_FORWARD_TRANSIT_NHS |
+			      NHRP_EXTENSION_FLAG_COMPULSORY,
+			      NHRP_PAYLOAD_TYPE_CIE_LIST);
+	nhrp_packet_extension(packet,
+			      NHRP_EXTENSION_REVERSE_TRANSIT_NHS |
+			      NHRP_EXTENSION_FLAG_COMPULSORY,
+			      NHRP_PAYLOAD_TYPE_CIE_LIST);
+	nhrp_packet_extension(packet,
+			      NHRP_EXTENSION_RESPONDER_ADDRESS |
+			      NHRP_EXTENSION_FLAG_COMPULSORY,
+			      NHRP_PAYLOAD_TYPE_CIE_LIST);
+
+	/* Cisco NAT extension CIE */
+	cie = nhrp_cie_alloc();
+	if (cie == NULL)
+		goto error_free_packet;
+
+        *cie = (struct nhrp_cie) {
+		.hdr.code = NHRP_CODE_SUCCESS,
+		.hdr.prefix_length = peer->protocol_address.addr_len * 8,
+		.hdr.preference = 0,
+		.nbma_address = peer->next_hop_address,
+		.protocol_address = peer->protocol_address,
+	};
+
+	payload = nhrp_packet_extension(packet, NHRP_EXTENSION_NAT_ADDRESS,
+					NHRP_PAYLOAD_TYPE_CIE_LIST);
+	nhrp_payload_add_cie(payload, cie);
+
+	nhrp_info("Sending Registration Request to %s (my mtu=%d)",
+		  nhrp_address_format(&peer->protocol_address,
+				      sizeof(dst), dst),
+		  peer->my_nbma_mtu);
+
+	packet->dst_peer = nhrp_peer_get(peer);
+	packet->dst_iface = peer->interface;
+	sent = nhrp_packet_send_request(packet,
+					nhrp_peer_handle_registration_reply,
+					nhrp_peer_get(peer));
+
+error_free_packet:
+	nhrp_packet_put(packet);
+error:
+	if (!sent)
+		nhrp_peer_restart_error(peer);
+}
+
+static int error_on_matching(void *ctx, struct nhrp_peer *peer)
+{
+	return 1;
+}
+
+static void nhrp_peer_handle_resolution_reply(void *ctx,
+					      struct nhrp_packet *reply)
+{
+	struct nhrp_peer *peer = (struct nhrp_peer *) ctx, *np;
+	struct nhrp_payload *payload;
+	struct nhrp_cie *cie, *natcie = NULL, *natoacie = NULL;
+	struct nhrp_interface *iface;
+	struct nhrp_peer_selector sel;
+	char dst[64], tmp[64], nbma[64];
+	int ec;
+
+	if (peer->flags & NHRP_PEER_FLAG_REMOVED)
+		goto ret;
+
+	if (reply == NULL ||
+	    reply->hdr.type != NHRP_PACKET_RESOLUTION_REPLY) {
+		ec = reply ? reply->hdr.u.error.code : -1;
+
+		nhrp_info("Failed to resolve %s: %s (%d)",
+			  nhrp_address_format(&peer->protocol_address,
+					      sizeof(tmp), tmp),
+			  nhrp_error_indication_text(ec), ntohs(ec));
+
+		if (reply != NULL) {
+			/* We got reply that this address is not available -
+			 * negative cache it. */
+			peer->flags |= NHRP_PEER_FLAG_UP;
+			nhrp_peer_reinsert(peer, NHRP_PEER_TYPE_NEGATIVE);
+		} else {
+			/* Time out - NHS reachable, or packet lost multiple
+			 * times. Keep trying if still needed. */
+			nhrp_peer_remove(peer);
+		}
+		goto ret;
+	}
+
+	payload = nhrp_packet_payload(reply, NHRP_PAYLOAD_TYPE_CIE_LIST);
+	cie = list_next(&payload->u.cie_list, struct nhrp_cie, cie_list_entry);
+	if (cie == NULL)
+		goto ret;
+
+	nhrp_info("Received Resolution Reply %s/%d is at proto %s nbma %s",
+		  nhrp_address_format(&peer->protocol_address,
+				      sizeof(dst), dst),
+		  cie->hdr.prefix_length,
+		  nhrp_address_format(&cie->protocol_address,
+				      sizeof(tmp), tmp),
+		  nhrp_address_format(&cie->nbma_address,
+				      sizeof(nbma), nbma));
+
+	payload = nhrp_packet_extension(reply,
+					NHRP_EXTENSION_NAT_ADDRESS |
+					NHRP_EXTENSION_FLAG_NOCREATE,
+					NHRP_PAYLOAD_TYPE_CIE_LIST);
+	if ((reply->hdr.flags & NHRP_FLAG_RESOLUTION_NAT) &&
+	    (payload != NULL)) {
+		natcie = list_next(&payload->u.cie_list, struct nhrp_cie, cie_list_entry);
+		if (natcie != NULL) {
+			natoacie = cie;
+			nhrp_info("NAT detected: really at proto %s nbma %s",
+				nhrp_address_format(&natcie->protocol_address,
+					sizeof(tmp), tmp),
+				nhrp_address_format(&natcie->nbma_address,
+					sizeof(nbma), nbma));
+		}
+	}
+	if (natcie == NULL)
+		natcie = cie;
+
+	if (nhrp_address_cmp(&peer->protocol_address, &cie->protocol_address)
+	    == 0) {
+		/* Destination is within NBMA network; update cache */
+		peer->mtu = ntohs(cie->hdr.mtu);
+		peer->prefix_length = cie->hdr.prefix_length;
+		peer->next_hop_address = natcie->nbma_address;
+		if (natoacie != NULL)
+			peer->next_hop_nat_oa = natoacie->nbma_address;
+		peer->expire_time = ev_now() + ntohs(cie->hdr.holding_time);
+		nhrp_address_set_network(&peer->protocol_address,
+					 peer->prefix_length);
+		nhrp_peer_reinsert(peer, NHRP_PEER_TYPE_CACHED);
+		goto ret;
+	}
+
+	/* Check that we won't replace a local address */
+	sel = (struct nhrp_peer_selector) {
+		.flags = NHRP_PEER_FIND_EXACT,
+		.type_mask = BIT(NHRP_PEER_TYPE_LOCAL_ADDR),
+		.protocol_address = peer->protocol_address,
+		.prefix_length = cie->hdr.prefix_length,
+	};
+	if (nhrp_peer_foreach(error_on_matching, NULL, &sel)) {
+		nhrp_error("Local route %s/%d exists: not replacing "
+			   "with shortcut",
+			   nhrp_address_format(&peer->protocol_address,
+					       sizeof(tmp), tmp),
+			   cie->hdr.prefix_length);
+		peer->flags |= NHRP_PEER_FLAG_UP;
+		nhrp_peer_reinsert(peer, NHRP_PEER_TYPE_NEGATIVE);
+		goto ret;
+	}
+
+	/* Update the received NBMA address to nexthop */
+	iface = peer->interface;
+	np = nhrp_peer_route(iface, &cie->protocol_address,
+			     NHRP_PEER_FIND_EXACT, 0);
+	if (np == NULL) {
+		np = nhrp_peer_alloc(iface);
+		np->type = NHRP_PEER_TYPE_CACHED;
+		np->afnum = reply->hdr.afnum;
+		np->protocol_type = reply->hdr.protocol_type;
+		np->protocol_address = cie->protocol_address;
+		np->next_hop_address = natcie->nbma_address;
+		if (natoacie != NULL)
+			np->next_hop_nat_oa = natoacie->nbma_address;
+		np->mtu = ntohs(cie->hdr.mtu);
+		np->prefix_length = cie->protocol_address.addr_len * 8;
+		np->expire_time = ev_now() + ntohs(cie->hdr.holding_time);
+		nhrp_peer_insert(np);
+		nhrp_peer_put(np);
+	}
+
+	/* Off NBMA destination; a shortcut route */
+	np = nhrp_peer_alloc(iface);
+	np->type = NHRP_PEER_TYPE_SHORTCUT_ROUTE;
+	np->afnum = reply->hdr.afnum;
+	np->protocol_type = reply->hdr.protocol_type;
+	np->protocol_address = peer->protocol_address;
+	np->prefix_length = cie->hdr.prefix_length;
+	np->next_hop_address = cie->protocol_address;
+	np->expire_time = ev_now() + ntohs(cie->hdr.holding_time);
+	nhrp_address_set_network(&np->protocol_address, np->prefix_length);
+	nhrp_peer_insert(np);
+	nhrp_peer_put(np);
+
+	/* Delete the incomplete entry */
+	nhrp_peer_remove(peer);
+ret:
+	nhrp_peer_put(peer);
+}
+
+static void nhrp_peer_send_resolve(struct nhrp_peer *peer)
+{
+	char dst[64];
+	struct nhrp_packet *packet;
+	struct nhrp_cie *cie;
+	struct nhrp_payload *payload;
+
+	packet = nhrp_packet_alloc();
+	if (packet == NULL)
+		goto error;
+
+	packet->hdr = (struct nhrp_packet_header) {
+		.afnum = peer->afnum,
+		.protocol_type = peer->protocol_type,
+		.version = NHRP_VERSION_RFC2332,
+		.type = NHRP_PACKET_RESOLUTION_REQUEST,
+		.hop_count = NHRP_PACKET_DEFAULT_HOP_COUNT,
+		.flags = NHRP_FLAG_RESOLUTION_SOURCE_IS_ROUTER |
+			 NHRP_FLAG_RESOLUTION_AUTHORATIVE |
+			 NHRP_FLAG_RESOLUTION_NAT
+	};
+	packet->dst_protocol_address = peer->protocol_address;
+
+	/* Payload CIE */
+	cie = nhrp_cie_alloc();
+	if (cie == NULL)
+		goto error;
+
+        *cie = (struct nhrp_cie) {
+		.hdr.code = NHRP_CODE_SUCCESS,
+		.hdr.prefix_length = 0,
+		.hdr.mtu = 0,
+		.hdr.holding_time = htons(peer->interface->holding_time),
+	};
+
+	payload = nhrp_packet_payload(packet, NHRP_PAYLOAD_TYPE_CIE_LIST);
+	nhrp_payload_add_cie(payload, cie);
+
+	nhrp_info("Sending Resolution Request to %s",
+		  nhrp_address_format(&peer->protocol_address,
+				      sizeof(dst), dst));
+
+	/* Standard extensions */
+	nhrp_packet_extension(packet,
+			      NHRP_EXTENSION_FORWARD_TRANSIT_NHS |
+			      NHRP_EXTENSION_FLAG_COMPULSORY,
+			      NHRP_PAYLOAD_TYPE_CIE_LIST);
+	nhrp_packet_extension(packet,
+			      NHRP_EXTENSION_REVERSE_TRANSIT_NHS |
+			      NHRP_EXTENSION_FLAG_COMPULSORY,
+			      NHRP_PAYLOAD_TYPE_CIE_LIST);
+	nhrp_packet_extension(packet,
+			      NHRP_EXTENSION_RESPONDER_ADDRESS |
+			      NHRP_EXTENSION_FLAG_COMPULSORY,
+			      NHRP_PAYLOAD_TYPE_CIE_LIST);
+	nhrp_packet_extension(packet,
+			      NHRP_EXTENSION_NAT_ADDRESS,
+			      NHRP_PAYLOAD_TYPE_CIE_LIST);
+
+	packet->dst_iface = peer->interface;
+	nhrp_packet_send_request(packet,
+				 nhrp_peer_handle_resolution_reply,
+				 nhrp_peer_get(peer));
+
+error:
+	nhrp_packet_put(packet);
+}
+
+struct nhrp_peer *nhrp_peer_alloc(struct nhrp_interface *iface)
+{
+	struct nhrp_peer *p;
+
+	nhrp_peer_num_total++;
+	p = calloc(1, sizeof(struct nhrp_peer));
+	p->ref = 1;
+	p->interface = iface;
+	list_init(&p->peer_list_entry);
+	list_init(&p->mcast_list_entry);
+	ev_timer_init(&p->timer, NULL, 0., 0.);
+	ev_child_init(&p->child, NULL, 0, 0);
+
+	return p;
+}
+
+struct nhrp_peer *nhrp_peer_get(struct nhrp_peer *peer)
+{
+	if (peer == NULL)
+		return NULL;
+
+	peer->ref++;
+	nhrp_peer_debug_refcount(__FUNCTION__, peer);
+
+	return peer;
+}
+
+static void nhrp_peer_run_nhs_down(struct nhrp_peer *peer)
+{
+	if ((peer->flags & (NHRP_PEER_FLAG_REGISTER |
+			    NHRP_PEER_FLAG_UP |
+			    NHRP_PEER_FLAG_REPLACED))
+	    == (NHRP_PEER_FLAG_REGISTER | NHRP_PEER_FLAG_UP))
+		nhrp_peer_run_script(peer, "nhs-down", NULL);
+}
+
+static void nhrp_peer_release(struct nhrp_peer *peer)
+{
+	struct nhrp_interface *iface = peer->interface;
+	struct nhrp_peer_selector sel;
+
+	nhrp_peer_cancel_async(peer);
+
+	/* Remove from lists */
+	if (list_hashed(&peer->mcast_list_entry))
+		list_del(&peer->mcast_list_entry);
+	if (hlist_hashed(&peer->nbma_hash_entry))
+		hlist_del(&peer->nbma_hash_entry);
+
+	if (peer->parent != NULL) {
+		nhrp_peer_put(peer->parent);
+		peer->parent = NULL;
+	}
+
+	switch (peer->type) {
+	case NHRP_PEER_TYPE_SHORTCUT_ROUTE:
+		if ((peer->flags & NHRP_PEER_FLAG_UP) &&
+		    !(peer->flags & NHRP_PEER_FLAG_REPLACED))
+			nhrp_peer_run_script(peer, "route-down", NULL);
+		break;
+	case NHRP_PEER_TYPE_CACHED:
+	case NHRP_PEER_TYPE_DYNAMIC:
+	case NHRP_PEER_TYPE_STATIC:
+	case NHRP_PEER_TYPE_DYNAMIC_NHS:
+		if (peer->flags & NHRP_PEER_FLAG_REPLACED)
+			break;
+
+		/* Remove cached routes using this entry as next-hop */
+		memset(&sel, 0, sizeof(sel));
+		sel.type_mask = BIT(NHRP_PEER_TYPE_SHORTCUT_ROUTE);
+		sel.interface = iface;
+		sel.next_hop_address = peer->protocol_address;
+		nhrp_peer_foreach(nhrp_peer_remove_matching, NULL,
+				  &sel);
+
+		/* Execute peer-down */
+		nhrp_peer_run_nhs_down(peer);
+		if (peer->flags & NHRP_PEER_FLAG_UP) {
+			peer->purge_reason = "timeout";
+			nhrp_peer_run_script(peer, "peer-down", NULL);
+		}
+
+		/* Remove from arp cache */
+		if (peer->protocol_address.type != PF_UNSPEC)
+			kernel_inject_neighbor(&peer->protocol_address,
+					       NULL, peer->interface);
+		break;
+	case NHRP_PEER_TYPE_INCOMPLETE:
+	case NHRP_PEER_TYPE_NEGATIVE:
+	case NHRP_PEER_TYPE_LOCAL_ADDR:
+	case NHRP_PEER_TYPE_LOCAL_ROUTE:
+	case NHRP_PEER_TYPE_STATIC_DNS:
+		break;
+	default:
+		NHRP_BUG_ON("invalid peer type");
+		break;
+	}
+
+	if (peer->nbma_hostname) {
+		free(peer->nbma_hostname);
+		peer->nbma_hostname = NULL;
+	}
+
+	free(peer);
+	nhrp_peer_num_total--;
+}
+
+int nhrp_peer_put(struct nhrp_peer *peer)
+{
+	NHRP_BUG_ON(peer->ref == 0);
+
+	peer->ref--;
+	nhrp_peer_debug_refcount(__FUNCTION__, peer);
+
+	if (peer->ref > 0)
+		return FALSE;
+
+	nhrp_peer_release(peer);
+
+	return TRUE;
+}
+
+static int nhrp_peer_mark_matching(void *ctx, struct nhrp_peer *peer)
+{
+	peer->flags |= NHRP_PEER_FLAG_MARK;
+	return 0;
+}
+
+static int nhrp_peer_renew_nhs_matching(void *ctx, struct nhrp_peer *peer)
+{
+	peer->flags &= ~NHRP_PEER_FLAG_MARK;
+	return 1;
+}
+
+static void nhrp_peer_dnsmap_query_cb(struct nhrp_address_query *query,
+				      int num_addr, struct nhrp_address *addrs)
+{
+	struct nhrp_peer *np, *peer =
+		container_of(query, struct nhrp_peer, address_query);
+	struct nhrp_peer_selector sel;
+	int i;
+
+	if (num_addr < 0) {
+		nhrp_error("Failed to resolve '%s'", peer->nbma_hostname);
+		nhrp_peer_schedule(peer, 10, nhrp_peer_dnsmap_restart_cb);
+		return;
+	}
+
+	if (num_addr > 0) {
+		/* Refresh protocol */
+		peer->afnum = nhrp_afnum_from_pf(addrs[0].type);
+	}
+
+	/* Mark existing dynamic nhs entries as expired */
+	memset(&sel, 0, sizeof(sel));
+	sel.type_mask = BIT(NHRP_PEER_TYPE_DYNAMIC_NHS);
+	sel.interface = peer->interface;
+	sel.parent = peer;
+	nhrp_peer_foreach(nhrp_peer_mark_matching, NULL, &sel);
+
+	for (i = 0; i < num_addr; i++) {
+		/* If this NBMA exists as dynamic NHS, mark it ok. */
+		sel.next_hop_address = addrs[i];
+		if (nhrp_peer_foreach(nhrp_peer_renew_nhs_matching,
+				      NULL, &sel) != 0)
+			continue;
+
+		/* New NHS, create a peer entry */
+		np = nhrp_peer_alloc(peer->interface);
+		np->type = NHRP_PEER_TYPE_DYNAMIC_NHS;
+		np->flags |= NHRP_PEER_FLAG_REGISTER;
+		np->afnum = peer->afnum;
+		np->protocol_type = peer->protocol_type;
+		np->protocol_address = peer->protocol_address;
+		np->prefix_length = peer->prefix_length;
+		np->next_hop_address = addrs[i];
+		np->parent = nhrp_peer_get(peer);
+		nhrp_address_set_network(&np->protocol_address,
+					 np->prefix_length);
+		nhrp_peer_insert(np);
+		nhrp_peer_put(np);
+	}
+
+	/* Delete all dynamic nhs:s that were not in the DNS reply */
+	nhrp_address_set_type(&sel.next_hop_address, AF_UNSPEC);
+	sel.flags = NHRP_PEER_FIND_MARK;
+	nhrp_peer_foreach(nhrp_peer_remove_matching, NULL, &sel);
+
+	/* Refresh DNS info */
+	nhrp_peer_schedule(peer, peer->interface->holding_time,
+			   nhrp_peer_dnsmap_restart_cb);
+}
+
+static void nhrp_peer_dnsmap_restart_cb(struct ev_timer *w, int revents)
+{
+	struct nhrp_peer *peer = container_of(w, struct nhrp_peer, timer);
+
+	NHRP_BUG_ON(peer->nbma_hostname == NULL);
+	nhrp_address_resolve(&peer->address_query, peer->nbma_hostname,
+			     nhrp_peer_dnsmap_query_cb);
+}
+
+static void nhrp_peer_insert_cb(struct ev_timer *w, int revents)
+{
+	struct nhrp_peer *peer = container_of(w, struct nhrp_peer, timer);
+
+	nhrp_peer_cancel_async(peer);
+	switch (peer->type) {
+	case NHRP_PEER_TYPE_LOCAL_ADDR:
+		peer->flags |= NHRP_PEER_FLAG_UP;
+		forward_local_addresses_changed();
+		break;
+	case NHRP_PEER_TYPE_LOCAL_ROUTE:
+		peer->flags |= NHRP_PEER_FLAG_UP;
+		break;
+	case NHRP_PEER_TYPE_INCOMPLETE:
+		nhrp_peer_send_resolve(peer);
+		break;
+	case NHRP_PEER_TYPE_CACHED:
+	case NHRP_PEER_TYPE_DYNAMIC:
+	case NHRP_PEER_TYPE_STATIC:
+	case NHRP_PEER_TYPE_DYNAMIC_NHS:
+		nhrp_peer_restart_cb(w, 0);
+		break;
+	case NHRP_PEER_TYPE_STATIC_DNS:
+		nhrp_peer_dnsmap_restart_cb(w, 0);
+		break;
+	case NHRP_PEER_TYPE_SHORTCUT_ROUTE:
+		if (peer->flags & NHRP_PEER_FLAG_UP)
+			nhrp_peer_script_route_up_done(&peer->child, 0);
+		else if (nhrp_peer_route(peer->interface,
+					 &peer->next_hop_address,
+					 NHRP_PEER_FIND_UP | NHRP_PEER_FIND_EXACT,
+					 NHRP_PEER_TYPEMASK_ADJACENT) != NULL)
+			nhrp_peer_run_script(peer, "route-up",
+					     nhrp_peer_script_route_up_done);
+		else
+			nhrp_peer_schedule(peer, peer->expire_time - NHRP_EXPIRY_TIME
+					   - 10 - ev_now(), nhrp_peer_expire_cb);
+		break;
+	case NHRP_PEER_TYPE_NEGATIVE:
+		peer->expire_time = ev_now() + NHRP_NEGATIVE_CACHE_TIME;
+
+		if (peer->flags & NHRP_PEER_FLAG_UP)
+			kernel_inject_neighbor(&peer->protocol_address,
+					       NULL, peer->interface);
+		nhrp_peer_schedule(peer, NHRP_NEGATIVE_CACHE_TIME,
+				   nhrp_peer_remove_cb);
+		break;
+	default:
+		NHRP_BUG_ON("invalid peer type");
+		break;
+	}
+}
+
+static void nhrp_peer_reinsert(struct nhrp_peer *peer, int type)
+{
+	NHRP_BUG_ON((peer->type == NHRP_PEER_TYPE_LOCAL_ADDR) !=
+		    (type == NHRP_PEER_TYPE_LOCAL_ADDR));
+	NHRP_BUG_ON((peer->type == NHRP_PEER_TYPE_LOCAL_ROUTE) !=
+		    (type == NHRP_PEER_TYPE_LOCAL_ROUTE));
+
+	peer->flags &= ~NHRP_PEER_FLAG_REMOVED;
+	peer->type = type;
+	nhrp_peer_insert_cb(&peer->timer, 0);
+}
+
+static int nhrp_peer_replace_shortcut(void *ctx, struct nhrp_peer *peer)
+{
+	struct nhrp_peer *shortcut = (struct nhrp_peer *) ctx;
+
+	/* Shortcut of identical prefix is replacement, either
+	 * due to renewal, or new shortcut next-hop. */
+	if (nhrp_address_cmp(&peer->protocol_address,
+			     &shortcut->protocol_address) == 0 &&
+	    peer->prefix_length == shortcut->prefix_length) {
+		peer->flags |= NHRP_PEER_FLAG_REPLACED;
+
+		/* If identical shortcut is being refreshed,
+		 * mark the refresher peer entry up. */
+		if ((peer->flags & NHRP_PEER_FLAG_UP) &&
+		    nhrp_address_cmp(&peer->next_hop_address,
+				     &shortcut->next_hop_address) == 0)
+			shortcut->flags |= NHRP_PEER_FLAG_UP;
+	}
+
+	/* Delete the old peer unconditionally */
+	nhrp_peer_remove(peer);
+
+	return 0;
+}
+
+void nhrp_peer_insert(struct nhrp_peer *peer)
+{
+	struct nhrp_peer_selector sel;
+	char tmp[NHRP_PEER_FORMAT_LEN];
+
+	/* First, prune all duplicates */
+	memset(&sel, 0, sizeof(sel));
+	sel.interface = peer->interface;
+	sel.protocol_address = peer->protocol_address;
+	sel.prefix_length = peer->prefix_length;
+	switch (peer->type) {
+	case NHRP_PEER_TYPE_SHORTCUT_ROUTE:
+		/* remove all existing shortcuts with same nexthop */
+		sel.flags = NHRP_PEER_FIND_SUBNET;
+		sel.type_mask |= BIT(NHRP_PEER_TYPE_SHORTCUT_ROUTE);
+		nhrp_peer_foreach(nhrp_peer_replace_shortcut, peer, &sel);
+		break;
+	case NHRP_PEER_TYPE_LOCAL_ROUTE:
+		sel.type_mask |= BIT(NHRP_PEER_TYPE_LOCAL_ROUTE);
+	default:
+		/* remove exact protocol address matches */
+		sel.flags = NHRP_PEER_FIND_EXACT;
+		sel.type_mask |= NHRP_PEER_TYPEMASK_REMOVABLE;
+		nhrp_peer_foreach(nhrp_peer_remove_matching, NULL, &sel);
+		break;
+	}
+
+	/* Keep a reference as long as we are on the list */
+	peer = nhrp_peer_get(peer);
+	nhrp_debug("Adding %s %s",
+		   nhrp_peer_type[peer->type],
+		   nhrp_peer_format(peer, sizeof(tmp), tmp));
+
+	if (peer->type == NHRP_PEER_TYPE_LOCAL_ADDR)
+		list_add(&peer->peer_list_entry, &local_peer_list);
+	else
+		list_add(&peer->peer_list_entry, &peer->interface->peer_list);
+
+	/* Start peers life */
+	if (nhrp_running || peer->type == NHRP_PEER_TYPE_LOCAL_ADDR)
+		nhrp_peer_insert_cb(&peer->timer, 0);
+	else
+		nhrp_peer_schedule(peer, 0, &nhrp_peer_insert_cb);
+}
+
+static void nhrp_peer_script_peer_down_done(union nhrp_peer_event e,
+					    int revents)
+{
+	struct nhrp_peer *peer = nhrp_peer_from_event(e, revents);
+
+	nhrp_peer_schedule(peer, 5, nhrp_peer_restart_cb);
+}
+
+void nhrp_peer_purge(struct nhrp_peer *peer, const char *purge_reason)
+{
+	switch (peer->type) {
+	case NHRP_PEER_TYPE_STATIC:
+	case NHRP_PEER_TYPE_DYNAMIC_NHS:
+		peer->purge_reason = purge_reason;
+		nhrp_peer_run_nhs_down(peer);
+		nhrp_peer_is_down(peer);
+		nhrp_peer_cancel_async(peer);
+		if (peer->flags & NHRP_PEER_FLAG_LOWER_UP) {
+			nhrp_peer_run_script(peer, "peer-down",
+					     nhrp_peer_script_peer_down_done);
+		} else {
+			nhrp_peer_script_peer_down_done(&peer->child, 0);
+		}
+		nhrp_address_set_type(&peer->my_nbma_address, PF_UNSPEC);
+		break;
+	case NHRP_PEER_TYPE_STATIC_DNS:
+		nhrp_peer_schedule(peer, 0, nhrp_peer_dnsmap_restart_cb);
+		break;
+	default:
+		peer->purge_reason = purge_reason;
+		nhrp_peer_remove(peer);
+		break;
+	}
+}
+
+int nhrp_peer_purge_matching(void *ctx, struct nhrp_peer *peer)
+{
+	int *count = (int *) ctx;
+	nhrp_peer_purge(peer, "user-request");
+	if (count != NULL)
+		(*count)++;
+	return 0;
+}
+
+int nhrp_peer_lowerdown_matching(void *ctx, struct nhrp_peer *peer)
+{
+	int *count = (int *) ctx;
+	nhrp_peer_purge(peer, "lower-down");
+	if (count != NULL)
+		(*count)++;
+	return 0;
+}
+
+static void nhrp_peer_remove_cb(struct ev_timer *w, int revents)
+{
+	struct nhrp_peer *peer = container_of(w, struct nhrp_peer, timer);
+	int type;
+
+	peer->flags |= NHRP_PEER_FLAG_REMOVED;
+	peer->purge_reason = "expired";
+	nhrp_peer_is_down(peer);
+	list_del(&peer->peer_list_entry);
+
+	type = peer->type;
+	nhrp_peer_put(peer);
+
+	if (type == NHRP_PEER_TYPE_LOCAL_ADDR)
+		forward_local_addresses_changed();
+}
+
+void nhrp_peer_remove(struct nhrp_peer *peer)
+{
+	char tmp[NHRP_PEER_FORMAT_LEN];
+
+	if (peer->flags & NHRP_PEER_FLAG_REMOVED)
+		return;
+
+	nhrp_debug("Removing %s %s",
+		   nhrp_peer_type[peer->type],
+		   nhrp_peer_format(peer, sizeof(tmp), tmp));
+
+	peer->flags |= NHRP_PEER_FLAG_REMOVED;
+	nhrp_peer_is_down(peer);
+	nhrp_peer_cancel_async(peer);
+	nhrp_peer_schedule(peer, 0, nhrp_peer_remove_cb);
+}
+
+int nhrp_peer_remove_matching(void *ctx, struct nhrp_peer *peer)
+{
+	int *count = (int *) ctx;
+
+	nhrp_peer_remove(peer);
+	if (count != NULL)
+		(*count)++;
+
+	return 0;
+}
+
+int nhrp_peer_set_used_matching(void *ctx, struct nhrp_peer *peer)
+{
+	int used = (int) (intptr_t) ctx;
+
+	if (used) {
+		peer->flags |= NHRP_PEER_FLAG_USED;
+		nhrp_peer_renew(peer);
+	} else {
+		peer->flags &= ~NHRP_PEER_FLAG_USED;
+	}
+	return 0;
+}
+
+int nhrp_peer_match(struct nhrp_peer *p, struct nhrp_peer_selector *sel)
+{
+	if (sel->type_mask && !(sel->type_mask & BIT(p->type)))
+		return FALSE;
+
+	if ((sel->flags & NHRP_PEER_FIND_UP) &&
+	    !(p->flags & NHRP_PEER_FLAG_UP))
+		return FALSE;
+
+	if ((sel->flags & NHRP_PEER_FIND_MARK) &&
+	   !(p->flags & NHRP_PEER_FLAG_MARK))
+		return FALSE;
+
+	if (sel->interface != NULL &&
+	    p->interface != sel->interface &&
+	    !(p->interface->flags & NHRP_INTERFACE_FLAG_SHORTCUT_DEST))
+		return FALSE;
+
+	if (sel->hostname != NULL &&
+	    (p->nbma_hostname == NULL ||
+	     strcmp(sel->hostname, p->nbma_hostname) != 0))
+		return FALSE;
+
+	if (sel->parent != NULL &&
+	    p->parent != sel->parent)
+		return FALSE;
+
+	if (sel->protocol_address.type != PF_UNSPEC) {
+		if (sel->prefix_length == 0)
+			sel->prefix_length = sel->protocol_address.addr_len * 8;
+
+		if (sel->flags & NHRP_PEER_FIND_EXACT) {
+			if (nhrp_address_cmp(&p->protocol_address,
+					     &sel->protocol_address) != 0)
+				return FALSE;
+
+			if (p->prefix_length != sel->prefix_length &&
+			    p->type != NHRP_PEER_TYPE_STATIC &&
+			    p->type != NHRP_PEER_TYPE_DYNAMIC_NHS)
+				return FALSE;
+		} else if (sel->flags & NHRP_PEER_FIND_ROUTE) {
+			if (nhrp_address_prefix_cmp(&p->protocol_address,
+						    &sel->protocol_address,
+						    p->prefix_length) != 0)
+				return FALSE;
+		} else {
+			if (p->prefix_length < sel->prefix_length) {
+				if (sel->prefix_length
+				    == sel->protocol_address.addr_len * 8 &&
+				    nhrp_address_cmp(&p->protocol_address,
+						     &sel->protocol_address)
+				    == 0)
+					return TRUE;
+
+				return FALSE;
+			}
+
+			if (nhrp_address_prefix_cmp(&p->protocol_address,
+						    &sel->protocol_address,
+						    sel->prefix_length) != 0)
+				return FALSE;
+		}
+	}
+
+	if (sel->next_hop_address.type != PF_UNSPEC) {
+		if (nhrp_address_cmp(&p->next_hop_address,
+				     &sel->next_hop_address) != 0)
+			return FALSE;
+	}
+
+	return TRUE;
+}
+
+struct enum_interface_peers_ctx {
+	nhrp_peer_enumerator enumerator;
+	void *ctx;
+	struct nhrp_peer_selector *sel;
+};
+
+static int enumerate_peer_cache(struct list_head *peer_cache,
+				nhrp_peer_enumerator e, void *ctx,
+				struct nhrp_peer_selector *sel)
+{
+	struct nhrp_peer *p;
+	int rc = 0;
+
+	list_for_each_entry(p, peer_cache, peer_list_entry) {
+		if (p->flags & NHRP_PEER_FLAG_REMOVED)
+			continue;
+
+		if (sel == NULL || nhrp_peer_match(p, sel)) {
+			rc = e(ctx, p);
+			if (rc != 0)
+				break;
+		}
+	}
+
+	return rc;
+}
+
+static int enum_interface_peers(void *ctx, struct nhrp_interface *iface)
+{
+	struct enum_interface_peers_ctx *ectx =
+		(struct enum_interface_peers_ctx *) ctx;
+
+	return enumerate_peer_cache(&iface->peer_list,
+				    ectx->enumerator, ectx->ctx,
+				    ectx->sel);
+}
+
+int nhrp_peer_foreach(nhrp_peer_enumerator e, void *ctx,
+		      struct nhrp_peer_selector *sel)
+{
+	struct nhrp_interface *iface = NULL;
+	struct enum_interface_peers_ctx ectx = { e, ctx, sel };
+	int rc;
+
+	if (sel != NULL)
+		iface = sel->interface;
+
+	rc = enumerate_peer_cache(&local_peer_list, e, ctx, sel);
+	if (rc != 0)
+		return rc;
+
+	/* Speed optimization: TYPE_LOCAL peers cannot be found from
+	 * other places */
+	if (sel != NULL &&
+	    sel->type_mask == BIT(NHRP_PEER_TYPE_LOCAL_ADDR))
+		return 0;
+
+	if (iface == NULL)
+		rc = nhrp_interface_foreach(enum_interface_peers, &ectx);
+	else
+		rc = enumerate_peer_cache(&iface->peer_list, e, ctx, sel);
+
+	return rc;
+}
+
+struct route_decision {
+	struct nhrp_peer_selector sel;
+	struct list_head *exclude;
+	struct nhrp_peer *best_found;
+	struct nhrp_address *src;
+	int found_exact, found_up;
+};
+
+static int decide_route(void *ctx, struct nhrp_peer *peer)
+{
+	struct route_decision *rd = (struct route_decision *) ctx;
+	int exact;
+
+	if (peer->type != NHRP_PEER_TYPE_SHORTCUT_ROUTE) {
+		/* Exclude addresses from CIE from routing decision
+		 * to avoid routing loops within NHS clusters. */
+		if (rd->exclude != NULL &&
+		    nhrp_address_match_cie_list(&peer->next_hop_address,
+						&peer->protocol_address,
+						rd->exclude))
+			return 0;
+
+		/* Exclude also source address, we don't want to
+		 * forward questions back to who's asking. */
+		if (rd->src != NULL &&
+		    nhrp_address_cmp(rd->src, &peer->protocol_address) == 0)
+			return 0;
+	} else {
+		/* Exclude routes that point back to the sender
+		 * of the packet */
+		if (rd->src != NULL &&
+		    nhrp_address_cmp(rd->src, &peer->next_hop_address) == 0)
+			return 0;
+	}
+
+	exact = (peer->type >= NHRP_PEER_TYPE_DYNAMIC_NHS) &&
+		(nhrp_address_cmp(&peer->protocol_address,
+				  &rd->sel.protocol_address) == 0);
+	if (rd->found_exact > exact)
+		return 0;
+
+	if (rd->found_up && !(peer->flags & NHRP_PEER_FLAG_UP))
+		return 0;
+
+	if (rd->best_found != NULL &&
+	    rd->found_exact == exact &&
+	    rd->found_up == (peer->flags & NHRP_PEER_FLAG_UP)) {
+		if (rd->best_found->prefix_length > peer->prefix_length)
+			return 0;
+
+		if (rd->best_found->prefix_length == peer->prefix_length &&
+		    rd->best_found->last_used < peer->last_used)
+			return 0;
+	}
+
+	rd->best_found = peer;
+	rd->found_exact = exact;
+	rd->found_up = peer->flags & NHRP_PEER_FLAG_UP;
+	return 0;
+}
+
+struct nhrp_peer *nhrp_peer_route_full(struct nhrp_interface *interface,
+				       struct nhrp_address *dst,
+				       int flags, int type_mask,
+				       struct nhrp_address *src,
+				       struct list_head *exclude)
+{
+	struct route_decision rd;
+
+	memset(&rd, 0, sizeof(rd));
+	rd.sel.flags = flags & ~NHRP_PEER_FIND_UP;
+	if ((flags & (NHRP_PEER_FIND_ROUTE | NHRP_PEER_FIND_EXACT |
+		      NHRP_PEER_FIND_SUBNET)) == 0)
+		rd.sel.flags |= NHRP_PEER_FIND_ROUTE;
+	rd.sel.type_mask = type_mask;
+	rd.sel.interface = interface;
+	rd.sel.protocol_address = *dst;
+	rd.exclude = exclude;
+	rd.src = src;
+	nhrp_peer_foreach(decide_route, &rd, &rd.sel);
+
+	if (rd.best_found == NULL)
+		return NULL;
+
+	if ((flags & NHRP_PEER_FIND_UP) &&
+	    !(rd.best_found->flags & NHRP_PEER_FLAG_UP))
+		return NULL;
+
+	rd.best_found->last_used = ev_now();
+	return rd.best_found;
+}
+
+void nhrp_peer_traffic_indication(struct nhrp_interface *iface,
+				  uint16_t afnum, struct nhrp_address *dst)
+{
+	struct nhrp_peer *peer;
+	int type;
+
+	/* For off-NBMA destinations, we consider all shortcut routes,
+	 * but NBMA destinations should be exact because we want to drop
+	 * NHS from the path. */
+	if (nhrp_address_prefix_cmp(dst, &iface->protocol_address,
+				    iface->protocol_address_prefix) != 0)
+		type = NHRP_PEER_FIND_ROUTE;
+	else
+		type = NHRP_PEER_FIND_EXACT;
+
+	/* Have we done something for this destination already? */
+	peer = nhrp_peer_route(iface, dst, type,
+			       ~BIT(NHRP_PEER_TYPE_LOCAL_ROUTE));
+	if (peer != NULL)
+		return;
+
+	/* Initiate resolution */
+	peer = nhrp_peer_alloc(iface);
+	peer->type = NHRP_PEER_TYPE_INCOMPLETE;
+	peer->afnum = afnum;
+	peer->protocol_type = nhrp_protocol_from_pf(dst->type);
+	peer->protocol_address = *dst;
+	peer->prefix_length = dst->addr_len * 8;
+	nhrp_peer_insert(peer);
+	nhrp_peer_put(peer);
+}
+
+static int dump_peer(void *ctx, struct nhrp_peer *peer)
+{
+	int *num_total = (int *) ctx;
+	char tmp[NHRP_PEER_FORMAT_LEN];
+
+	nhrp_info("%s %s",
+		  nhrp_peer_type[peer->type],
+		  nhrp_peer_format(peer, sizeof(tmp), tmp));
+	(*num_total)++;
+	return 0;
+}
+
+void nhrp_peer_dump_cache(void)
+{
+	int num_total = 0;
+
+	nhrp_info("Peer cache dump:");
+	nhrp_peer_foreach(dump_peer, &num_total, NULL);
+	nhrp_info("Total %d peer cache entries, %d allocated entries",
+		  num_total, nhrp_peer_num_total);
+}
+
+void nhrp_peer_cleanup(void)
+{
+	ev_tstamp prev = ev_now();
+
+	nhrp_peer_foreach(nhrp_peer_remove_matching, NULL, NULL);
+
+	while (nhrp_peer_num_total > 0) {
+		if (ev_now() > prev + 5.0) {
+			nhrp_info("Waiting for peers to die, %d left", nhrp_peer_num_total);
+			prev = ev_now();
+		}
+		ev_loop(EVLOOP_ONESHOT);
+	}
+}
diff --git a/nhrp/nhrp_peer.h b/nhrp/nhrp_peer.h
new file mode 100644
index 0000000..dea8d66
--- /dev/null
+++ b/nhrp/nhrp_peer.h
@@ -0,0 +1,194 @@
+/* nhrp_peer.h - NHRP peer cache definitions
+ *
+ * Copyright (C) 2007-2009 Timo Teräs <timo.teras@iki.fi>
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 or later as
+ * published by the Free Software Foundation.
+ *
+ * See http://www.gnu.org/ for details.
+ */
+
+#ifndef NHRP_PEER_H
+#define NHRP_PEER_H
+
+#include <time.h>
+#include <stdint.h>
+#include <sys/types.h>
+#include "nhrp_address.h"
+#include "libev.h"
+#include "list.h"
+
+#define NHRP_PEER_TYPE_INCOMPLETE	0x00	/* Resolution request sent */
+#define NHRP_PEER_TYPE_NEGATIVE		0x01	/* Negative cached */
+#define NHRP_PEER_TYPE_CACHED		0x02	/* Received/relayed resolution reply */
+#define NHRP_PEER_TYPE_SHORTCUT_ROUTE	0x03	/* Received/relayed resolution for route */
+#define NHRP_PEER_TYPE_DYNAMIC		0x04	/* NHC registration */
+#define NHRP_PEER_TYPE_DYNAMIC_NHS	0x05	/* Dynamic NHS from dns-map */
+#define NHRP_PEER_TYPE_STATIC		0x06	/* Static mapping from config file */
+#define NHRP_PEER_TYPE_STATIC_DNS	0x07	/* Static dns-map from config file */
+#define NHRP_PEER_TYPE_LOCAL_ROUTE	0x08	/* Non-local destination, with local route */
+#define NHRP_PEER_TYPE_LOCAL_ADDR	0x09	/* Local destination (IP or off-NBMA subnet) */
+#define NHRP_PEER_TYPE_MAX		(NHRP_PEER_TYPE_LOCAL_ADDR+1)
+
+#define NHRP_PEER_TYPEMASK_ADJACENT \
+	(BIT(NHRP_PEER_TYPE_CACHED) | \
+	 BIT(NHRP_PEER_TYPE_DYNAMIC) | \
+	 BIT(NHRP_PEER_TYPE_DYNAMIC_NHS) | \
+	 BIT(NHRP_PEER_TYPE_STATIC) | \
+	 BIT(NHRP_PEER_TYPE_LOCAL_ADDR))
+
+#define NHRP_PEER_TYPEMASK_REMOVABLE \
+	(BIT(NHRP_PEER_TYPE_INCOMPLETE) | \
+	 BIT(NHRP_PEER_TYPE_NEGATIVE) | \
+	 BIT(NHRP_PEER_TYPE_CACHED) | \
+	 BIT(NHRP_PEER_TYPE_SHORTCUT_ROUTE) | \
+	 BIT(NHRP_PEER_TYPE_DYNAMIC))
+
+#define NHRP_PEER_TYPEMASK_PURGEABLE \
+	(NHRP_PEER_TYPEMASK_REMOVABLE | \
+	 BIT(NHRP_PEER_TYPE_DYNAMIC_NHS) | \
+	 BIT(NHRP_PEER_TYPE_STATIC) | \
+	 BIT(NHRP_PEER_TYPE_STATIC_DNS))
+
+#define NHRP_PEER_TYPEMASK_ALL \
+	(NHRP_PEER_TYPEMASK_PURGEABLE | \
+	 BIT(NHRP_PEER_TYPE_LOCAL_ROUTE) | \
+	 BIT(NHRP_PEER_TYPE_LOCAL_ADDR))
+
+/* For routing via NHS */
+#define NHRP_PEER_TYPEMASK_ROUTE_VIA_NHS \
+	(BIT(NHRP_PEER_TYPE_DYNAMIC) | \
+	 BIT(NHRP_PEER_TYPE_DYNAMIC_NHS) | \
+	 BIT(NHRP_PEER_TYPE_STATIC) | \
+	 BIT(NHRP_PEER_TYPE_LOCAL_ROUTE) | \
+	 BIT(NHRP_PEER_TYPE_LOCAL_ADDR))
+
+#define NHRP_PEER_FLAG_UNIQUE		0x01	/* Peer is unique; see RFC2332 */
+#define NHRP_PEER_FLAG_REGISTER		0x02	/* For TYPE_STATIC: send registration */
+#define NHRP_PEER_FLAG_CISCO		0x04	/* For TYPE_STATIC: peer is Cisco */
+#define NHRP_PEER_FLAG_USED		0x10	/* Peer is in kernel ARP table */
+#define NHRP_PEER_FLAG_LOWER_UP		0x20	/* Script executed succesfully */
+#define NHRP_PEER_FLAG_UP		0x40	/* Can send all packets (registration ok) */
+#define NHRP_PEER_FLAG_REPLACED		0x80	/* Peer has been replaced */
+#define NHRP_PEER_FLAG_REMOVED		0x100	/* Deleted, but not removed from cache yet */
+#define NHRP_PEER_FLAG_MARK		0x200	/* Can be used to temporarily mark peers */
+
+#define NHRP_PEER_FIND_ROUTE		0x01
+#define NHRP_PEER_FIND_EXACT		0x02
+#define NHRP_PEER_FIND_SUBNET		0x04
+#define NHRP_PEER_FIND_UP		0x10
+#define NHRP_PEER_FIND_MARK		0x20
+
+struct nhrp_interface;
+struct nhrp_packet;
+struct nhrp_pending_request;
+
+union __attribute__ ((__transparent_union__)) nhrp_peer_event {
+	struct ev_timer *timer;
+	struct ev_child *child;
+};
+
+struct nhrp_peer {
+	unsigned int ref;
+	unsigned int flags;
+
+	struct list_head peer_list_entry;
+	struct list_head mcast_list_entry;
+	struct hlist_node nbma_hash_entry;
+
+	const char *purge_reason;
+	struct nhrp_interface *interface;
+	struct nhrp_peer *parent;
+	struct nhrp_packet *queued_packet;
+	struct nhrp_pending_request *request;
+
+	struct ev_timer timer;
+	struct ev_child child;
+	struct nhrp_address_query address_query;
+
+	uint8_t type;
+	uint8_t prefix_length;
+	uint16_t afnum;
+	uint16_t protocol_type;
+	uint16_t mtu, my_nbma_mtu;
+	ev_tstamp expire_time;
+	ev_tstamp last_used;
+	struct nhrp_address my_nbma_address;
+	struct nhrp_address protocol_address;
+	unsigned int holding_time;
+
+	char *nbma_hostname;
+	/* NHRP_PEER_TYPE_ROUTE: protocol addr., others: NBMA addr. */
+	struct nhrp_address next_hop_address;
+	struct nhrp_address next_hop_nat_oa;
+};
+
+struct nhrp_peer_selector {
+	int flags; /* NHRP_PEER_FIND_xxx */
+	int type_mask;
+
+	struct nhrp_interface *interface;
+	struct nhrp_peer *parent;
+	const char *hostname;
+
+	int prefix_length;
+	struct nhrp_address protocol_address;
+	struct nhrp_address next_hop_address;
+};
+
+const char * const nhrp_peer_type[NHRP_PEER_TYPE_MAX];
+typedef int (*nhrp_peer_enumerator)(void *ctx, struct nhrp_peer *peer);
+
+void nhrp_peer_cleanup(void);
+
+struct nhrp_peer *nhrp_peer_alloc(struct nhrp_interface *iface);
+struct nhrp_peer *nhrp_peer_get(struct nhrp_peer *peer);
+int nhrp_peer_put(struct nhrp_peer *peer);
+void nhrp_peer_cancel_async(struct nhrp_peer *peer);
+
+void nhrp_peer_insert(struct nhrp_peer *peer);
+void nhrp_peer_remove(struct nhrp_peer *peer);
+void nhrp_peer_purge(struct nhrp_peer *peer, const char *purge_reason);
+
+int nhrp_peer_match(struct nhrp_peer *peer, struct nhrp_peer_selector *sel);
+
+int nhrp_peer_foreach(nhrp_peer_enumerator e, void *ctx,
+		      struct nhrp_peer_selector *sel);
+int nhrp_peer_remove_matching(void *count, struct nhrp_peer *peer);
+int nhrp_peer_purge_matching(void *count, struct nhrp_peer *peer);
+int nhrp_peer_lowerdown_matching(void *count, struct nhrp_peer *peer);
+int nhrp_peer_set_used_matching(void *ctx, struct nhrp_peer *peer);
+struct nhrp_peer *nhrp_peer_find_by_nbma(struct nhrp_interface *iface, struct nhrp_address *nbma);
+
+int nhrp_peer_event_ok(union nhrp_peer_event e, int revents);
+char *nhrp_peer_event_reason(union nhrp_peer_event e, int revents,
+			     size_t buflen, char *buf);
+struct nhrp_peer *nhrp_peer_from_event(union nhrp_peer_event e, int revents);
+void nhrp_peer_run_script(struct nhrp_peer *peer, char *action,
+			  void (*cb)(union nhrp_peer_event, int));
+void nhrp_peer_send_packet_queue(struct nhrp_peer *peer);
+int nhrp_peer_discover_nhs(struct nhrp_peer *peer,
+			   struct nhrp_address *newaddr);
+
+struct nhrp_peer *nhrp_peer_route_full(struct nhrp_interface *iface,
+				       struct nhrp_address *dest,
+				       int flags, int type_mask,
+				       struct nhrp_address *source,
+				       struct list_head *exclude_cie_list);
+
+static inline struct nhrp_peer *nhrp_peer_route(struct nhrp_interface *iface,
+						struct nhrp_address *dest,
+						int flags, int type_mask)
+{
+	return nhrp_peer_route_full(iface, dest, flags, type_mask, NULL, NULL);
+}
+
+void nhrp_peer_traffic_indication(struct nhrp_interface *iface,
+				  uint16_t afnum, struct nhrp_address *dst);
+void nhrp_peer_dump_cache(void);
+
+void nhrp_server_finish_request(struct nhrp_pending_request *pr);
+
+#endif
diff --git a/nhrp/nhrp_protocol.h b/nhrp/nhrp_protocol.h
new file mode 100644
index 0000000..8cf213b
--- /dev/null
+++ b/nhrp/nhrp_protocol.h
@@ -0,0 +1,130 @@
+/* nhrp_protocol.h - NHRP protocol definitions
+ *
+ * Copyright (C) 2007 Timo Teräs <timo.teras@iki.fi>
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 or later as
+ * published by the Free Software Foundation.
+ *
+ * See http://www.gnu.org/ for details.
+ */
+
+#ifndef NHRP_PROTOCOL_H
+#define NHRP_PROTOCOL_H
+
+#include <stdint.h>
+#include "afnum.h"
+
+/* NHRP Version */
+#define NHRP_VERSION_RFC2332			1
+
+/* NHRP Packet Types */
+#define NHRP_PACKET_RESOLUTION_REQUEST		1
+#define NHRP_PACKET_RESOLUTION_REPLY		2
+#define NHRP_PACKET_REGISTRATION_REQUEST	3
+#define NHRP_PACKET_REGISTRATION_REPLY		4
+#define NHRP_PACKET_PURGE_REQUEST		5
+#define NHRP_PACKET_PURGE_REPLY			6
+#define NHRP_PACKET_ERROR_INDICATION		7
+#define NHRP_PACKET_TRAFFIC_INDICATION		8
+
+/* NHRP Extension Types */
+#define NHRP_EXTENSION_FLAG_COMPULSORY		0x8000
+#define NHRP_EXTENSION_END			0
+#define NHRP_EXTENSION_PAYLOAD			0
+#define NHRP_EXTENSION_RESPONDER_ADDRESS	3
+#define NHRP_EXTENSION_FORWARD_TRANSIT_NHS	4
+#define NHRP_EXTENSION_REVERSE_TRANSIT_NHS	5
+#define NHRP_EXTENSION_AUTHENTICATION		7
+#define NHRP_EXTENSION_VENDOR			8
+#define NHRP_EXTENSION_NAT_ADDRESS		9
+
+/* NHRP Error Indication Codes */
+#define NHRP_ERROR_UNRECOGNIZED_EXTENSION	constant_htons(1)
+#define NHRP_ERROR_LOOP_DETECTED		constant_htons(2)
+#define NHRP_ERROR_PROTOCOL_ADDRESS_UNREACHABLE	constant_htons(6)
+#define NHRP_ERROR_PROTOCOL_ERROR		constant_htons(7)
+#define NHRP_ERROR_SDU_SIZE_EXCEEDED		constant_htons(8)
+#define NHRP_ERROR_INVALID_EXTENSION		constant_htons(9)
+#define NHRP_ERROR_INVALID_RESOLUTION_REPLY	constant_htons(10)
+#define NHRP_ERROR_AUTHENTICATION_FAILURE	constant_htons(11)
+#define NHRP_ERROR_HOP_COUNT_EXCEEDED		constant_htons(15)
+
+/* NHRP CIE Codes */
+#define NHRP_CODE_SUCCESS			0
+#define NHRP_CODE_ADMINISTRATIVELY_PROHIBITED	4
+#define NHRP_CODE_INSUFFICIENT_RESOURCES	5
+#define NHRP_CODE_NO_BINDING_EXISTS		11
+#define NHRP_CODE_BINDING_NON_UNIQUE		13
+#define NHRP_CODE_UNIQUE_ADDRESS_REGISTERED     14
+
+/* NHRP Flags for Resolution request/reply */
+#define NHRP_FLAG_RESOLUTION_SOURCE_IS_ROUTER	constant_htons(0x8000)
+#define NHRP_FLAG_RESOLUTION_AUTHORATIVE	constant_htons(0x4000)
+#define NHRP_FLAG_RESOLUTION_DESTINATION_STABLE	constant_htons(0x2000)
+#define NHRP_FLAG_RESOLUTION_UNIQUE		constant_htons(0x1000)
+#define NHRP_FLAG_RESOLUTION_SOURCE_STABLE	constant_htons(0x0800)
+#define NHRP_FLAG_RESOLUTION_NAT		constant_htons(0x0002)
+
+/* NHRP Flags for Registration request/reply */
+#define NHRP_FLAG_REGISTRATION_UNIQUE		constant_htons(0x8000)
+#define NHRP_FLAG_REGISTRATION_NAT		constant_htons(0x0002)
+
+/* NHRP Flags for Purge request/reply */
+#define NHRP_FLAG_PURGE_NO_REPLY		constant_htons(0x8000)
+
+/* NHRP Authentication extension types (ala Cisco) */
+#define NHRP_AUTHENTICATION_PLAINTEXT		constant_htonl(0x00000001)
+
+/* NHRP Packet Structures */
+struct nhrp_packet_header {
+	/* Fixed header */
+	uint16_t	afnum;
+	uint16_t	protocol_type;
+	uint8_t		snap[5];
+	uint8_t		hop_count;
+	uint16_t	packet_size;
+	uint16_t	checksum;
+	uint16_t	extension_offset;
+	uint8_t		version;
+	uint8_t		type;
+	uint8_t		src_nbma_address_len;
+	uint8_t		src_nbma_subaddress_len;
+
+	/* Mandatory header */
+	uint8_t		src_protocol_address_len;
+	uint8_t		dst_protocol_address_len;
+	uint16_t	flags;
+	union {
+		uint32_t		request_id;
+		struct {
+			uint16_t	code;
+			uint16_t	offset;
+		} error;
+	} u;
+};
+
+struct nhrp_cie_header {
+	uint8_t		code;
+	uint8_t		prefix_length;
+	uint16_t	unused;
+	uint16_t	mtu;
+	uint16_t	holding_time;
+	uint8_t		nbma_address_len;
+	uint8_t		nbma_subaddress_len;
+	uint8_t		protocol_address_len;
+	uint8_t		preference;
+};
+
+struct nhrp_extension_header {
+	uint16_t	type;
+	uint16_t	length;
+};
+
+struct nhrp_cisco_authentication_extension {
+	uint32_t	type;
+	uint8_t		secret[8];
+};
+
+#endif
diff --git a/nhrp/nhrp_server.c b/nhrp/nhrp_server.c
new file mode 100644
index 0000000..b41e4b8
--- /dev/null
+++ b/nhrp/nhrp_server.c
@@ -0,0 +1,566 @@
+/* nhrp_server.c - NHRP request handling
+ *
+ * Copyright (C) 2007-2009 Timo Teräs <timo.teras@iki.fi>
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 or later as
+ * published by the Free Software Foundation.
+ *
+ * See http://www.gnu.org/ for details.
+ */
+
+#include <string.h>
+#include <netinet/in.h>
+
+#include "nhrp_common.h"
+#include "nhrp_packet.h"
+#include "nhrp_interface.h"
+#include "nhrp_peer.h"
+
+#define NHRP_MAX_PENDING_REQUESTS 16
+
+struct nhrp_pending_request {
+	struct list_head request_list_entry;
+	int natted;
+	int num_ok, num_error;
+	struct nhrp_packet *packet;
+	struct nhrp_cie *cie;
+	struct nhrp_payload *payload;
+	struct nhrp_peer *peer, *rpeer;
+	ev_tstamp now;
+};
+
+static struct list_head request_list = LIST_INITIALIZER(request_list);
+static int num_pending_requests = 0;
+
+static void nhrp_server_start_cie_reg(struct nhrp_pending_request *pr);
+
+static struct nhrp_pending_request *
+nhrp_server_record_request(struct nhrp_packet *packet)
+{
+	struct nhrp_pending_request *pr;
+
+	pr = calloc(1, sizeof(struct nhrp_pending_request));
+	list_init(&pr->request_list_entry);
+	if (pr != NULL) {
+		num_pending_requests++;
+		list_add(&pr->request_list_entry, &request_list);
+		pr->packet = nhrp_packet_get(packet);
+		pr->now = ev_now();
+	}
+	return pr;
+}
+
+void nhrp_server_finish_request(struct nhrp_pending_request *pr)
+{
+	list_del(&pr->request_list_entry);
+	if (pr->rpeer != NULL) {
+		struct nhrp_peer *peer = pr->rpeer;
+		if (peer->flags & NHRP_PEER_FLAG_REPLACED) {
+			/* The route peer entry was not accepted. We still
+			 * send the replies here, and cancel anything pending
+			 * so it'll get deleted cleanly on next put(). */
+			nhrp_peer_send_packet_queue(peer);
+			nhrp_peer_cancel_async(peer);
+		}
+		nhrp_peer_put(pr->rpeer);
+	}
+	if (pr->peer != NULL)
+		nhrp_peer_put(pr->peer);
+	if (pr->packet != NULL)
+		nhrp_packet_put(pr->packet);
+	free(pr);
+	num_pending_requests--;
+}
+
+static int nhrp_server_request_pending(struct nhrp_packet *packet)
+{
+	struct nhrp_pending_request *r;
+
+	list_for_each_entry(r, &request_list, request_list_entry) {
+		if (nhrp_address_cmp(&packet->src_nbma_address,
+				     &r->packet->src_nbma_address) != 0)
+			continue;
+		if (nhrp_address_cmp(&packet->src_protocol_address,
+				     &r->packet->src_protocol_address) != 0)
+			continue;
+		if (nhrp_address_cmp(&packet->dst_protocol_address,
+				     &r->packet->dst_protocol_address) != 0)
+			continue;
+
+		/* Request from the same address being already processed */
+		return TRUE;
+	}
+
+	return FALSE;
+}
+
+static int nhrp_handle_resolution_request(struct nhrp_packet *packet)
+{
+	char tmp[64], tmp2[64];
+	struct nhrp_payload *payload;
+	struct nhrp_peer *peer = packet->dst_peer;
+	struct nhrp_peer_selector sel;
+	struct nhrp_cie *cie;
+
+	nhrp_info("Received Resolution Request from proto src %s to %s",
+		  nhrp_address_format(&packet->src_protocol_address,
+				      sizeof(tmp), tmp),
+		  nhrp_address_format(&packet->dst_protocol_address,
+				      sizeof(tmp2), tmp2));
+
+	/* As first thing, flush all negative entries for the
+	 * requestor */
+	memset(&sel, 0, sizeof(sel));
+	sel.flags = NHRP_PEER_FIND_EXACT;
+	sel.type_mask = BIT(NHRP_PEER_TYPE_NEGATIVE);
+	sel.interface = packet->src_iface;
+	sel.protocol_address = packet->src_protocol_address;
+	nhrp_peer_foreach(nhrp_peer_remove_matching, NULL, &sel);
+
+	/* Send reply */
+	packet->hdr.type = NHRP_PACKET_RESOLUTION_REPLY;
+	packet->hdr.hop_count = NHRP_PACKET_DEFAULT_HOP_COUNT;
+	packet->hdr.flags &= NHRP_FLAG_RESOLUTION_SOURCE_IS_ROUTER |
+			     NHRP_FLAG_RESOLUTION_SOURCE_STABLE |
+			     NHRP_FLAG_RESOLUTION_UNIQUE |
+			     NHRP_FLAG_RESOLUTION_NAT;
+	packet->hdr.flags |= NHRP_FLAG_RESOLUTION_DESTINATION_STABLE |
+			     NHRP_FLAG_RESOLUTION_AUTHORATIVE;
+
+	cie = nhrp_cie_alloc();
+	if (cie == NULL)
+		return FALSE;
+
+	cie->hdr = (struct nhrp_cie_header) {
+		.code = NHRP_CODE_SUCCESS,
+		.prefix_length = peer->prefix_length,
+	};
+	if (peer->holding_time)
+		cie->hdr.holding_time = htons(peer->holding_time);
+	else if (peer->interface != NULL)
+		cie->hdr.holding_time = htons(peer->interface->holding_time);
+	else
+		cie->hdr.holding_time = NHRP_DEFAULT_HOLDING_TIME;
+
+	payload = nhrp_packet_payload(packet, NHRP_PAYLOAD_TYPE_ANY);
+	nhrp_payload_free(payload);
+	nhrp_payload_set_type(payload, NHRP_PAYLOAD_TYPE_CIE_LIST);
+	nhrp_payload_add_cie(payload, cie);
+
+	if (!nhrp_packet_reroute(packet, NULL))
+		return FALSE;
+
+	peer = packet->dst_peer;
+	cie->hdr.mtu = htons(peer->my_nbma_mtu);
+	cie->nbma_address = peer->my_nbma_address;
+	cie->protocol_address = packet->dst_iface->protocol_address;
+
+	nhrp_info("Sending Resolution Reply %s/%d is-at %s (holdtime %d)",
+		  nhrp_address_format(&packet->dst_protocol_address,
+				      sizeof(tmp), tmp),
+		  cie->hdr.prefix_length,
+		  nhrp_address_format(&cie->nbma_address,
+				      sizeof(tmp2), tmp2),
+		  ntohs(cie->hdr.holding_time));
+
+	/* Reset NAT header to regenerate it for reply */
+	payload = nhrp_packet_extension(packet,
+					NHRP_EXTENSION_NAT_ADDRESS |
+					NHRP_EXTENSION_FLAG_NOCREATE,
+					NHRP_PAYLOAD_TYPE_ANY);
+	if (payload != NULL) {
+		nhrp_payload_free(payload);
+		nhrp_payload_set_type(payload, NHRP_PAYLOAD_TYPE_CIE_LIST);
+	}
+
+	return nhrp_packet_send(packet);
+}
+
+static int find_one(void *ctx, struct nhrp_peer *p)
+{
+	return 1;
+}
+
+static int remove_old_registrations(void *ctx, struct nhrp_peer *p)
+{
+	struct nhrp_peer *peer = (struct nhrp_peer *) ctx;
+
+	/* If re-registration, mark the new connection up */
+	if (nhrp_address_cmp(&peer->protocol_address,
+			     &p->protocol_address) == 0 &&
+	    nhrp_address_cmp(&peer->next_hop_address,
+			     &p->next_hop_address) == 0 &&
+	    peer->prefix_length == p->prefix_length)
+		peer->flags |= p->flags & (NHRP_PEER_FLAG_UP |
+					   NHRP_PEER_FLAG_LOWER_UP);
+
+	p->flags |= NHRP_PEER_FLAG_REPLACED;
+	nhrp_peer_remove(p);
+	return 0;
+}
+
+static void nhrp_server_finish_reg(struct nhrp_pending_request *pr)
+{
+	char tmp[64], tmp2[64];
+	struct nhrp_packet *packet = pr->packet;
+
+	if (pr->rpeer != NULL &&
+	    nhrp_packet_reroute(packet, pr->rpeer)) {
+		nhrp_info("Sending Registration Reply from proto src %s to %s (%d bindings accepted, %d rejected)",
+			  nhrp_address_format(&packet->dst_protocol_address,
+					      sizeof(tmp), tmp),
+			  nhrp_address_format(&packet->src_protocol_address,
+					      sizeof(tmp2), tmp2),
+			  pr->num_ok, pr->num_error);
+
+		nhrp_packet_send(packet);
+	} else {
+		/* We could not create route peer entry (likely out of memory),
+		 * so we can't do much more here. */
+		nhrp_info("Dropping Registration Reply from proto src %s to %s",
+			  nhrp_address_format(&packet->dst_protocol_address,
+					      sizeof(tmp), tmp),
+			  nhrp_address_format(&packet->src_protocol_address,
+					      sizeof(tmp2), tmp2));
+	}
+
+	nhrp_server_finish_request(pr);
+}
+
+static void nhrp_server_finish_cie_reg_cb(union nhrp_peer_event e, int revents)
+{
+	struct nhrp_peer *peer;
+	struct nhrp_pending_request *pr;
+	struct nhrp_packet *packet;
+	struct nhrp_cie *cie;
+	struct nhrp_peer_selector sel;
+	char tmp[64], reason[32];
+
+	peer = nhrp_peer_from_event(e, revents);
+	pr = peer->request;
+	packet = pr->packet;
+	cie = pr->cie;
+
+	peer->request = NULL;
+	nhrp_address_format(&peer->protocol_address, sizeof(tmp), tmp);
+	if (revents != 0 && nhrp_peer_event_ok(e, revents)) {
+		nhrp_debug("[%s] Peer registration authorized", tmp);
+
+		/* Remove all old stuff and accept registration */
+		memset(&sel, 0, sizeof(sel));
+		sel.flags = NHRP_PEER_FIND_EXACT;
+		sel.type_mask = NHRP_PEER_TYPEMASK_REMOVABLE;
+		sel.interface = packet->src_iface;
+		sel.protocol_address = peer->protocol_address;
+		sel.prefix_length = peer->prefix_length;
+		nhrp_peer_foreach(remove_old_registrations, peer, &sel);
+
+		pr->num_ok++;
+		cie->hdr.code = NHRP_CODE_SUCCESS;
+		nhrp_peer_insert(peer);
+	} else {
+		if (revents == 0)
+			nhrp_error("[%s] Peer registration failed: "
+				   "static entry exists", tmp);
+		else
+			nhrp_error("[%s] Peer registration failed: %s",
+				   tmp,
+				   nhrp_peer_event_reason(e, revents,
+							  sizeof(reason),
+							  reason));
+		pr->num_error++;
+		cie->hdr.code = NHRP_CODE_ADMINISTRATIVELY_PROHIBITED;
+		peer->flags |= NHRP_PEER_FLAG_REPLACED;
+	}
+	if (pr->rpeer == NULL)
+		pr->rpeer = nhrp_peer_get(peer);
+
+	nhrp_peer_put(peer);
+	pr->peer = NULL;
+
+	/* Process next CIE or finish registration handling */
+	if (cie->cie_list_entry.next != &pr->payload->u.cie_list) {
+		pr->cie = list_next(&cie->cie_list_entry, struct nhrp_cie, cie_list_entry);
+		nhrp_server_start_cie_reg(pr);
+	} else {
+		nhrp_server_finish_reg(pr);
+	}
+
+}
+
+static void nhrp_server_start_cie_reg(struct nhrp_pending_request *pr)
+{
+	struct nhrp_cie *cie = pr->cie;
+	struct nhrp_packet *packet = pr->packet;
+	struct nhrp_peer *peer;
+	struct nhrp_peer_selector sel;
+
+	peer = nhrp_peer_alloc(packet->src_iface);
+	if (peer == NULL) {
+		/* Mark all remaining registration requests as failed
+		 * due to lack of memory, and send reply */
+		for (; cie->cie_list_entry.next != &pr->payload->u.cie_list;
+		     cie = list_next(&cie->cie_list_entry, struct nhrp_cie, cie_list_entry)) {
+			pr->num_error++;
+			cie->hdr.code = NHRP_CODE_INSUFFICIENT_RESOURCES;
+		}
+		pr->num_error++;
+		cie->hdr.code = NHRP_CODE_INSUFFICIENT_RESOURCES;
+		nhrp_server_finish_reg(pr);
+		return;
+	}
+
+	peer->type = NHRP_PEER_TYPE_DYNAMIC;
+	peer->afnum = packet->hdr.afnum;
+	peer->protocol_type = packet->hdr.protocol_type;
+	peer->expire_time = pr->now + ntohs(cie->hdr.holding_time);
+	peer->mtu = ntohs(cie->hdr.mtu);
+	if (cie->nbma_address.addr_len != 0)
+		peer->next_hop_address = cie->nbma_address;
+	else
+		peer->next_hop_address = packet->src_nbma_address;
+
+	if (pr->natted) {
+		peer->next_hop_nat_oa  = peer->next_hop_address;
+		peer->next_hop_address = packet->src_linklayer_address;
+	}
+
+	if (cie->protocol_address.addr_len != 0)
+		peer->protocol_address = cie->protocol_address;
+	else
+		peer->protocol_address = packet->src_protocol_address;
+
+	peer->prefix_length = cie->hdr.prefix_length;
+	if (peer->prefix_length == 0xff)
+		peer->prefix_length = peer->protocol_address.addr_len * 8;
+
+	memset(&sel, 0, sizeof(sel));
+	sel.flags = NHRP_PEER_FIND_EXACT;
+	sel.type_mask = ~NHRP_PEER_TYPEMASK_REMOVABLE;
+	sel.interface = packet->src_iface;
+	sel.protocol_address = peer->protocol_address;
+	sel.prefix_length = peer->prefix_length;
+
+	/* Link the created peer and pending request structures */
+	pr->peer = peer;
+	peer->request = pr;
+
+	/* Check that there is no conflicting peers */
+	if (nhrp_peer_foreach(find_one, peer, &sel) != 0) {
+		cie->hdr.code = NHRP_CODE_ADMINISTRATIVELY_PROHIBITED;
+		peer->flags |= NHRP_PEER_FLAG_REPLACED;
+		nhrp_server_finish_cie_reg_cb(&peer->child, 0);
+	} else {
+		nhrp_peer_run_script(peer, "peer-register",
+				     nhrp_server_finish_cie_reg_cb);
+	}
+}
+
+static int nhrp_handle_registration_request(struct nhrp_packet *packet)
+{
+	char tmp[64], tmp2[64];
+	struct nhrp_payload *payload;
+	struct nhrp_cie *cie;
+	struct nhrp_pending_request *pr;
+	int natted = 0;
+
+	nhrp_info("Received Registration Request from proto src %s to %s",
+		  nhrp_address_format(&packet->src_protocol_address,
+				      sizeof(tmp), tmp),
+		  nhrp_address_format(&packet->dst_protocol_address,
+				      sizeof(tmp2), tmp2));
+
+	if (nhrp_server_request_pending(packet)) {
+		nhrp_info("Already processing: resent packet ignored.");
+		return TRUE;
+	}
+
+	if (num_pending_requests >= NHRP_MAX_PENDING_REQUESTS) {
+		/* We should probably send Registration Reply with CIE
+		 * error NHRP_CODE_INSUFFICIENT_RESOURCES, or an Error
+		 * Indication. However, we do not have a direct peer entry
+		 * nor can we make sure that the lower layer is up, so
+		 * we just lamely drop the packet for now. */
+		nhrp_info("Too many pending requests: dropping this one");
+		return TRUE;
+	}
+
+	/* Cisco NAT extension, CIE added IF all of the following is true:
+	 * 1. We are the first hop registration server
+	 *    (=no entries in forward transit CIE list)
+	 * 2. NAT is detected (link layer address != announced address)
+	 * 3. NAT extension is requested */
+	payload = nhrp_packet_extension(packet,
+					NHRP_EXTENSION_FORWARD_TRANSIT_NHS |
+					NHRP_EXTENSION_FLAG_NOCREATE,
+					NHRP_PAYLOAD_TYPE_CIE_LIST);
+	if (payload != NULL && list_empty(&payload->u.cie_list) &&
+	    packet->src_linklayer_address.type != PF_UNSPEC &&
+	    nhrp_address_cmp(&packet->src_nbma_address,
+			     &packet->src_linklayer_address) != 0) {
+		natted = 1;
+		payload = nhrp_packet_extension(packet,
+						NHRP_EXTENSION_NAT_ADDRESS |
+						NHRP_EXTENSION_FLAG_NOCREATE,
+						NHRP_PAYLOAD_TYPE_CIE_LIST);
+		if (payload != NULL) {
+			cie = nhrp_cie_alloc();
+			if (cie != NULL) {
+				cie->nbma_address = packet->src_linklayer_address;
+				cie->protocol_address = packet->src_protocol_address;
+				nhrp_payload_add_cie(payload, cie);
+			}
+		}
+	}
+
+	packet->hdr.type = NHRP_PACKET_REGISTRATION_REPLY;
+	packet->hdr.hop_count = NHRP_PACKET_DEFAULT_HOP_COUNT;
+	packet->hdr.flags &= NHRP_FLAG_REGISTRATION_UNIQUE |
+			     NHRP_FLAG_REGISTRATION_NAT;
+
+	payload = nhrp_packet_payload(packet, NHRP_PAYLOAD_TYPE_CIE_LIST);
+	if (list_empty(&payload->u.cie_list)) {
+		nhrp_error("Received registration request has no CIEs");
+		return TRUE;
+	}
+
+	/* Start processing the CIEs */
+	pr = nhrp_server_record_request(packet);
+	pr->natted = natted;
+	pr->payload = payload;
+
+	pr->cie = nhrp_payload_get_cie(payload, 1);
+	nhrp_server_start_cie_reg(pr);
+
+	return TRUE;
+}
+
+static int remove_peer_by_nbma(void *ctx, struct nhrp_peer *peer)
+{
+	struct nhrp_address *nbma = ctx;
+	struct nhrp_address *peer_nbma = NULL;
+
+	if (!nhrp_address_is_any_addr(nbma)) {
+		if (peer->type == NHRP_PEER_TYPE_SHORTCUT_ROUTE) {
+			struct nhrp_peer *nexthop;
+
+			nexthop = nhrp_peer_route(peer->interface,
+				&peer->next_hop_address,
+				NHRP_PEER_FIND_EXACT,
+				NHRP_PEER_TYPEMASK_ADJACENT);
+			if (nexthop != NULL)
+				peer_nbma = &nexthop->next_hop_address;
+		} else {
+			peer_nbma = &peer->next_hop_address;
+		}
+	} else {
+		peer_nbma = nbma;
+	}
+
+	if (peer_nbma != NULL &&
+	    nhrp_address_cmp(peer_nbma, nbma) == 0)
+		nhrp_peer_remove(peer);
+
+	return 0;
+}
+
+static int nhrp_handle_purge_request(struct nhrp_packet *packet)
+{
+	char tmp[64], tmp2[64];
+	struct nhrp_peer_selector sel;
+	struct nhrp_payload *payload;
+	struct nhrp_cie *cie;
+	int flags, ret = TRUE;
+
+	nhrp_info("Received Purge Request from proto src %s to %s",
+		  nhrp_address_format(&packet->src_protocol_address,
+				      sizeof(tmp), tmp),
+		  nhrp_address_format(&packet->dst_protocol_address,
+				      sizeof(tmp2), tmp2));
+
+	flags = packet->hdr.flags;
+	packet->hdr.type = NHRP_PACKET_PURGE_REPLY;
+	packet->hdr.hop_count = NHRP_PACKET_DEFAULT_HOP_COUNT;
+	packet->hdr.flags = 0;
+
+	if (!(flags & NHRP_FLAG_PURGE_NO_REPLY)) {
+		if (nhrp_packet_reroute(packet, NULL))
+			ret = nhrp_packet_send(packet);
+		else
+			ret = FALSE;
+	}
+
+	payload = nhrp_packet_payload(packet, NHRP_PAYLOAD_TYPE_CIE_LIST);
+	list_for_each_entry(cie, &payload->u.cie_list, cie_list_entry) {
+		nhrp_info("Purge proto %s/%d nbma %s",
+			nhrp_address_format(&cie->protocol_address,
+					    sizeof(tmp), tmp),
+			cie->hdr.prefix_length,
+			nhrp_address_format(&cie->nbma_address,
+					    sizeof(tmp2), tmp2));
+
+		memset(&sel, 0, sizeof(sel));
+		sel.flags = NHRP_PEER_FIND_EXACT;
+		sel.type_mask = NHRP_PEER_TYPEMASK_REMOVABLE;
+		sel.interface = packet->src_iface;
+		sel.protocol_address = cie->protocol_address;
+		sel.prefix_length = cie->hdr.prefix_length;
+		nhrp_peer_foreach(remove_peer_by_nbma,
+				  &cie->nbma_address, &sel);
+		nhrp_rate_limit_clear(&cie->protocol_address,
+				      cie->hdr.prefix_length);
+	}
+
+	return ret;
+}
+
+static int nhrp_handle_traffic_indication(struct nhrp_packet *packet)
+{
+	char tmp[64], tmp2[64];
+	struct nhrp_address dst;
+	struct nhrp_payload *pl;
+
+	pl = nhrp_packet_payload(packet, NHRP_PAYLOAD_TYPE_RAW);
+	if (pl == NULL)
+		return FALSE;
+
+	if (!nhrp_address_parse_packet(packet->hdr.protocol_type,
+				       pl->u.raw->length, pl->u.raw->data,
+				       NULL, &dst))
+		return FALSE;
+
+	/* Shortcuts enabled? */
+	if (packet->src_iface->flags & NHRP_INTERFACE_FLAG_SHORTCUT) {
+		nhrp_info("Traffic Indication from proto src %s; "
+			  "about packet to %s",
+			  nhrp_address_format(&packet->src_protocol_address,
+					      sizeof(tmp), tmp),
+			  nhrp_address_format(&dst, sizeof(tmp2), tmp2));
+
+		nhrp_peer_traffic_indication(packet->src_iface,
+					     packet->hdr.afnum,
+					     &dst);
+	} else {
+		nhrp_info("Traffic Indication ignored from proto src %s; "
+			  "about packet to %s",
+			  nhrp_address_format(&packet->src_protocol_address,
+					      sizeof(tmp), tmp),
+			  nhrp_address_format(&dst, sizeof(tmp2), tmp2));
+	}
+
+	return TRUE;
+}
+
+void server_init(void)
+{
+	nhrp_packet_hook_request(NHRP_PACKET_RESOLUTION_REQUEST,
+				 nhrp_handle_resolution_request);
+	nhrp_packet_hook_request(NHRP_PACKET_REGISTRATION_REQUEST,
+				 nhrp_handle_registration_request);
+	nhrp_packet_hook_request(NHRP_PACKET_PURGE_REQUEST,
+				 nhrp_handle_purge_request);
+	nhrp_packet_hook_request(NHRP_PACKET_TRAFFIC_INDICATION,
+				 nhrp_handle_traffic_indication);
+}
diff --git a/nhrp/opennhrp.c b/nhrp/opennhrp.c
new file mode 100644
index 0000000..8ba870d
--- /dev/null
+++ b/nhrp/opennhrp.c
@@ -0,0 +1,524 @@
+/* opennhrp.c - OpenNHRP main routines
+ *
+ * Copyright (C) 2007-2009 Timo Teräs <timo.teras@iki.fi>
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 or later as
+ * published by the Free Software Foundation.
+ *
+ * See http://www.gnu.org/ for details.
+ */
+
+#include <ctype.h>
+#include <stdio.h>
+#include <errno.h>
+#include <malloc.h>
+#include <stddef.h>
+#include <string.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <sys/file.h>
+#include <sys/stat.h>
+
+#include "nhrp_common.h"
+#include "nhrp_peer.h"
+#include "nhrp_interface.h"
+
+const char *nhrp_version_string =
+	"OpenNHRP " OPENNHRP_VERSION
+#ifdef NHRP_NO_NBMA_GRE
+	" (no NBMA GRE support)"
+#endif
+	;
+
+const char *nhrp_admin_socket = OPENNHRP_ADMIN_SOCKET;
+const char *nhrp_pid_file     = "/var/run/opennhrp.pid";
+const char *nhrp_config_file  = "/etc/opennhrp/opennhrp.conf";
+const char *nhrp_script_file  = "/etc/opennhrp/opennhrp-script";
+int nhrp_verbose = 0;
+int nhrp_running = FALSE;
+
+static int pid_file_fd;
+
+void nhrp_hex_dump(const char *name, const uint8_t *buf, int bytes)
+{
+	int i, j;
+	int left;
+
+	fprintf(stderr, "%s:\n", name);
+	for (i = 0; i < bytes; i++) {
+		fprintf(stderr, "%02X ", buf[i]);
+		if (i % 0x10 == 0x0f) {
+			fprintf(stderr, "    ");
+			for (j = 0; j < 0x10; j++)
+				fprintf(stderr, "%c", isgraph(buf[i+j-0xf]) ?
+					buf[i+j-0xf]: '.');
+			fprintf(stderr, "\n");
+		}
+	}
+
+	left = i % 0x10;
+	if (left != 0) {
+		fprintf(stderr, "%*s    ", 3 * (0x10 - left), "");
+
+		for (j = 0; j < left; j++)
+			fprintf(stderr, "%c", isgraph(buf[i+j-left]) ?
+				buf[i+j-left]: '.');
+		fprintf(stderr, "\n");
+	}
+	fprintf(stderr, "\n");
+}
+
+static void handle_signal_cb(struct ev_signal *w, int revents)
+{
+	struct nhrp_peer_selector sel;
+
+	switch (w->signum) {
+	case SIGUSR1:
+		nhrp_peer_dump_cache();
+		break;
+	case SIGINT:
+	case SIGTERM:
+		ev_unloop(EVUNLOOP_ALL);
+		break;
+	case SIGHUP:
+		memset(&sel, 0, sizeof(sel));
+		sel.type_mask = NHRP_PEER_TYPEMASK_REMOVABLE;
+		nhrp_peer_foreach(nhrp_peer_remove_matching, NULL, &sel);
+		break;
+	}
+}
+
+static int hook_signal[] = { SIGUSR1, SIGHUP, SIGINT, SIGTERM };
+static ev_signal signal_event[ARRAY_SIZE(hook_signal)];
+
+static void signal_init(void)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(hook_signal); i++) {
+		ev_signal_init(&signal_event[i], handle_signal_cb,
+			       hook_signal[i]);
+		ev_signal_start(&signal_event[i]);
+	}
+}
+
+static int read_word(FILE *in, int *lineno, size_t len, char *word)
+{
+	int ch, i, comment = 0;
+
+	ch = fgetc(in);
+	while (1) {
+		if (ch == EOF)
+			return FALSE;
+		if (ch == '#')
+			comment = 1;
+		if (!comment && !isspace(ch))
+			break;
+		if (ch == '\n') {
+			(*lineno)++;
+			comment = 0;
+		}
+		ch = fgetc(in);
+	}
+
+	for (i = 0; i < len-1 && !isspace(ch); i++) {
+		word[i] = ch;
+		ch = fgetc(in);
+		if (ch == EOF)
+			break;
+		if (ch == '\n')
+			(*lineno)++;
+	}
+	word[i] = 0;
+
+	return TRUE;
+}
+
+static int load_config(const char *config_file)
+{
+#define NEED_INTERFACE() if (iface == NULL) { rc = 2; break; } peer = NULL;
+#define NEED_PEER() if (peer == NULL || peer->type == NHRP_PEER_TYPE_LOCAL_ADDR) { rc = 3; break; }
+
+	static const char *errors[] = {
+		"syntax error",
+		"missing keyword",
+		"keyword valid only for 'interface' definition",
+		"keyword valid only for 'map' definition",
+		"invalid address",
+		"dynamic-map requires a network address",
+		"bad multicast destination",
+		"keyword valid only for 'interace' and 'shortcut-target' definition",
+	};
+	struct nhrp_interface *iface = NULL;
+	struct nhrp_peer *peer = NULL;
+	struct nhrp_address paddr;
+	char word[32], nbma[32], addr[32];
+	FILE *in;
+	int lineno = 1, rc = -1;
+
+	in = fopen(config_file, "r");
+	if (in == NULL) {
+		nhrp_error("Unable to open configuration file '%s'.",
+			   config_file);
+		return FALSE;
+	}
+
+	while (read_word(in, &lineno, sizeof(word), word)) {
+		if (strcmp(word, "interface") == 0) {
+			if (!read_word(in, &lineno, sizeof(word), word)) {
+				rc = 1;
+				break;
+			}
+			iface = nhrp_interface_get_by_name(word, TRUE);
+			if (iface != NULL)
+				iface->flags |= NHRP_INTERFACE_FLAG_CONFIGURED;
+			peer = NULL;
+		} else if (strcmp(word, "shortcut-target") == 0) {
+			NEED_INTERFACE();
+			if (!read_word(in, &lineno, sizeof(addr), addr)) {
+				rc = 1;
+				break;
+			}
+			peer = nhrp_peer_alloc(iface);
+			peer->type = NHRP_PEER_TYPE_LOCAL_ADDR;
+			peer->afnum = AFNUM_RESERVED;
+			if (!nhrp_address_parse(addr, &peer->protocol_address,
+						&peer->prefix_length)) {
+				rc = 4;
+				break;
+			}
+			peer->protocol_type = nhrp_protocol_from_pf(peer->protocol_address.type);
+			nhrp_peer_insert(peer);
+			nhrp_peer_put(peer);
+		} else if (strcmp(word, "dynamic-map") == 0) {
+			NEED_INTERFACE();
+			read_word(in, &lineno, sizeof(addr), addr);
+			read_word(in, &lineno, sizeof(nbma), nbma);
+
+			peer = nhrp_peer_alloc(iface);
+			peer->type = NHRP_PEER_TYPE_STATIC_DNS;
+			if (!nhrp_address_parse(addr, &peer->protocol_address,
+						&peer->prefix_length)) {
+				rc = 4;
+				break;
+			}
+			if (!nhrp_address_is_network(&peer->protocol_address,
+						     peer->prefix_length)) {
+				rc = 5;
+				break;
+			}
+			peer->protocol_type = nhrp_protocol_from_pf(
+				peer->protocol_address.type);
+			peer->nbma_hostname = strdup(nbma);
+			peer->afnum = nhrp_afnum_from_pf(
+				peer->next_hop_address.type);
+			nhrp_peer_insert(peer);
+			nhrp_peer_put(peer);
+		} else if (strcmp(word, "map") == 0) {
+			NEED_INTERFACE();
+			read_word(in, &lineno, sizeof(addr), addr);
+			read_word(in, &lineno, sizeof(nbma), nbma);
+
+			peer = nhrp_peer_alloc(iface);
+			peer->type = NHRP_PEER_TYPE_STATIC;
+			if (!nhrp_address_parse(addr, &peer->protocol_address,
+						&peer->prefix_length)) {
+				rc = 4;
+				break;
+			}
+			peer->protocol_type = nhrp_protocol_from_pf(
+				peer->protocol_address.type);
+			if (!nhrp_address_parse(nbma, &peer->next_hop_address,
+						NULL))
+				peer->nbma_hostname = strdup(nbma);
+			peer->afnum = nhrp_afnum_from_pf(peer->next_hop_address.type);
+			nhrp_peer_insert(peer);
+			nhrp_peer_put(peer);
+		} else if (strcmp(word, "register") == 0) {
+			NEED_PEER();
+			peer->flags |= NHRP_PEER_FLAG_REGISTER;
+		} else if (strcmp(word, "cisco") == 0) {
+			NEED_PEER();
+			peer->flags |= NHRP_PEER_FLAG_CISCO;
+		} else if (strcmp(word, "holding-time") == 0) {
+			read_word(in, &lineno, sizeof(word), word);
+			if (peer != NULL &&
+			    peer->type == NHRP_PEER_TYPE_LOCAL_ADDR) {
+				peer->holding_time = atoi(word);
+			} else if (iface != NULL) {
+				iface->holding_time = atoi(word);
+				peer = NULL;
+			} else {
+				rc = 7;
+			}
+		} else if (strcmp(word, "cisco-authentication") == 0) {
+			struct nhrp_buffer *buf;
+			struct nhrp_cisco_authentication_extension *auth;
+
+			NEED_INTERFACE();
+			read_word(in, &lineno, sizeof(word), word);
+
+			buf = nhrp_buffer_alloc(strlen(word) + sizeof(uint32_t));
+			auth = (struct nhrp_cisco_authentication_extension *) buf->data;
+			auth->type = NHRP_AUTHENTICATION_PLAINTEXT;
+			memcpy(auth->secret, word, strlen(word));
+
+			iface->auth_token = buf;
+		} else if (strcmp(word, "route-table") == 0) {
+			NEED_INTERFACE();
+			read_word(in, &lineno, sizeof(word), word);
+			iface->route_table = atoi(word);
+		} else if (strcmp(word, "shortcut") == 0) {
+			NEED_INTERFACE();
+			iface->flags |= NHRP_INTERFACE_FLAG_SHORTCUT;
+		} else if (strcmp(word, "redirect") == 0) {
+			NEED_INTERFACE();
+			iface->flags |= NHRP_INTERFACE_FLAG_REDIRECT;
+		} else if (strcmp(word, "non-caching") == 0) {
+			NEED_INTERFACE();
+			iface->flags |= NHRP_INTERFACE_FLAG_NON_CACHING;
+		} else if (strcmp(word, "shortcut-destination") == 0) {
+			NEED_INTERFACE();
+			iface->flags |= NHRP_INTERFACE_FLAG_SHORTCUT_DEST;
+		} else if (strcmp(word, "multicast") == 0) {
+			NEED_INTERFACE();
+			read_word(in, &lineno, sizeof(word), word);
+			if (strcmp(word, "dynamic") == 0) {
+				iface->mcast_mask = \
+					BIT(NHRP_PEER_TYPE_STATIC) |
+					BIT(NHRP_PEER_TYPE_DYNAMIC_NHS) |
+					BIT(NHRP_PEER_TYPE_DYNAMIC);
+			} else if (strcmp(word, "nhs") == 0) {
+				iface->mcast_mask = \
+					BIT(NHRP_PEER_TYPE_STATIC) |
+					BIT(NHRP_PEER_TYPE_DYNAMIC_NHS);
+			} else if (nhrp_address_parse(word, &paddr, NULL)) {
+				iface->mcast_numaddr++;
+				iface->mcast_addr = realloc(iface->mcast_addr,
+					iface->mcast_numaddr *
+					sizeof(struct nhrp_address));
+				iface->mcast_addr[iface->mcast_numaddr-1] =
+					paddr;
+			} else {
+				rc = 6;
+				break;
+			}
+		} else {
+			rc = 0;
+			break;
+		}
+	}
+	fclose(in);
+
+	if (rc >= 0) {
+		nhrp_error("Configuration file %s in %s:%d, near word '%s'",
+			   errors[rc], config_file, lineno, word);
+		return FALSE;
+	}
+	return TRUE;
+}
+
+static void remove_pid_file(void)
+{
+	if (pid_file_fd != 0) {
+		close(pid_file_fd);
+		pid_file_fd = 0;
+		remove(nhrp_pid_file);
+	}
+}
+
+static int open_pid_file(void)
+{
+	if (strlen(nhrp_pid_file) == 0)
+		return TRUE;
+
+	pid_file_fd = open(nhrp_pid_file, O_CREAT | O_WRONLY,
+			   S_IRUSR | S_IWUSR);
+	if (pid_file_fd < 0)
+		goto err;
+
+	fcntl(pid_file_fd, F_SETFD, FD_CLOEXEC);
+	if (flock(pid_file_fd, LOCK_EX | LOCK_NB) < 0)
+		goto err_close;
+
+	return TRUE;
+
+err_close:
+	close(pid_file_fd);
+err:
+	nhrp_error("Unable to open/lock pid file: %s.", strerror(errno));
+	return FALSE;
+}
+
+static int write_pid(void)
+{
+	char tmp[16];
+	int n;
+
+	if (pid_file_fd >= 0) {
+		if (ftruncate(pid_file_fd, 0) < 0)
+			return FALSE;
+
+		n = sprintf(tmp, "%d\n", getpid());
+		if (write(pid_file_fd, tmp, n) != n)
+			return FALSE;
+
+		atexit(remove_pid_file);
+	}
+
+	return TRUE;
+}
+
+static int daemonize(void)
+{
+	pid_t pid;
+
+	pid = fork();
+	if (pid < 0)
+		return FALSE;
+	if (pid > 0)
+		exit(0);
+
+	if (setsid() < 0)
+		return FALSE;
+
+	pid = fork();
+	if (pid < 0)
+		return FALSE;
+	if (pid > 0)
+		exit(0);
+
+	if (chdir("/") < 0)
+		return FALSE;
+
+	umask(0);
+
+	if (freopen("/dev/null", "r", stdin) == NULL ||
+	    freopen("/dev/null", "w", stdout) == NULL ||
+	    freopen("/dev/null", "w", stderr) == NULL) {
+		nhrp_error("Unable reopen standard file descriptors");
+		goto err;
+	}
+
+	ev_default_fork();
+
+	return TRUE;
+
+err:
+	close(pid_file_fd);
+	pid_file_fd = 0;
+	return FALSE;
+}
+
+int usage(const char *prog)
+{
+	fprintf(stderr,
+		"usage: opennhrp [-a admin-socket] [-c config-file] [-s script-file]\n"
+		"                [-p pid-file] [-d] [-v]\n"
+		"       opennhrp -V\n"
+		"\n"
+		"\t-a admin-socket\tspecify management interface socket\n"
+		"\t-c config-file\tread configuration from config-file\n"
+		"\t-s script-file\tuse specified script-file for event handling\n"
+		"\t-p pid-file\tspecify pid-file\n"
+		"\t-d\t\tfork to background after startup\n"
+		"\t-v\t\tverbose logging\n"
+		"\t-V\t\tshow version number and exit\n"
+		"\n");
+	return 1;
+}
+
+int main(int argc, char **argv)
+{
+	struct nhrp_address any;
+	int i, daemonmode = 0;
+
+	nhrp_address_set_type(&any, AF_UNSPEC);
+
+	for (i = 1; i < argc; i++) {
+		if (strlen(argv[i]) != 2 || argv[i][0] != '-')
+			return usage(argv[0]);
+
+		switch (argv[i][1]) {
+		case 'c':
+			if (++i >= argc)
+				return usage(argv[0]);
+			nhrp_config_file = argv[i];
+			break;
+		case 's':
+			if (++i >= argc)
+				return usage(argv[0]);
+			nhrp_script_file = argv[i];
+			break;
+		case 'a':
+			if (++i >= argc)
+				return usage(argv[0]);
+			nhrp_admin_socket = argv[i];
+			break;
+		case 'p':
+			if (++i >= argc)
+				return usage(argv[0]);
+			nhrp_pid_file = argv[i];
+			break;
+		case 'd':
+			daemonmode = 1;
+			break;
+		case 'v':
+			nhrp_verbose = 1;
+			break;
+		case 'V':
+			puts(nhrp_version_string);
+			return 0;
+		default:
+			return usage(argv[0]);
+		}
+	}
+
+	srandom(time(NULL));
+	if (!log_init())
+		return 1;
+	if (!open_pid_file())
+		return 1;
+
+	nhrp_info("%s starting", nhrp_version_string);
+
+	ev_default_loop(0);
+	signal_init();
+	server_init();
+	if (!nhrp_address_init())
+		return 3;
+	if (!load_config(nhrp_config_file))
+		return 4;
+	if (!kernel_init())
+		return 5;
+	if (!admin_init(nhrp_admin_socket))
+		return 6;
+	if (!forward_init())
+		return 7;
+
+	if (daemonmode && !daemonize()) {
+		nhrp_error("Failed to daemonize. Exit.");
+		return 8;
+	}
+
+	write_pid();
+
+	nhrp_running = TRUE;
+	ev_loop(0);
+	nhrp_running = FALSE;
+
+	forward_cleanup();
+	kernel_stop_listening();
+	nhrp_peer_cleanup();
+	kernel_cleanup();
+	nhrp_interface_cleanup();
+	nhrp_rate_limit_clear(&any, 0);
+	nhrp_address_cleanup();
+
+	ev_default_destroy();
+
+	return 0;
+}
+
diff --git a/nhrp/opennhrpctl.c b/nhrp/opennhrpctl.c
new file mode 100644
index 0000000..92fb5b5
--- /dev/null
+++ b/nhrp/opennhrpctl.c
@@ -0,0 +1,121 @@
+/* opennhrpctl.c - OpenNHRP command line control utility
+ *
+ * Copyright (C) 2007 Timo Teräs <timo.teras@iki.fi>
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 or later as
+ * published by the Free Software Foundation.
+ *
+ * See http://www.gnu.org/ for details.
+ */
+
+#include <errno.h>
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+#include <sys/un.h>
+#include <sys/socket.h>
+
+static int admin_init(const char *opennhrp_socket)
+{
+	struct sockaddr_un sun;
+	int fd;
+
+	memset(&sun, 0, sizeof(sun));
+	sun.sun_family = AF_UNIX;
+	strncpy(sun.sun_path, opennhrp_socket, sizeof(sun.sun_path));
+
+	fd = socket(AF_UNIX, SOCK_STREAM, 0);
+	if (fd < 0)
+		return -1;
+
+	if (connect(fd, (struct sockaddr *) &sun, sizeof(sun)) < 0) {
+		close(fd);
+		return -1;
+	}
+
+	return fd;
+}
+
+static void admin_close(int fd)
+{
+	close(fd);
+}
+
+static int admin_send(int fd, const char *str)
+{
+	int len = strlen(str);
+
+	if (write(fd, str, len) != len)
+		return -1;
+	shutdown(fd, SHUT_WR);
+	return 0;
+}
+
+static int admin_receive(int fd)
+{
+	char msg[512];
+	size_t len;
+
+	while ((len = recv(fd, msg, sizeof(msg), 0)) > 0) {
+		if (write(fileno(stdout), msg, len) != len)
+			return -1;
+	}
+
+	if (len < 0)
+		return -1;
+
+	return 0;
+}
+
+static int usage(const char *prog)
+{
+	fprintf(stderr, "usage: %s [-a admin-socket] <command>\n", prog);
+	return 1;
+}
+
+int main(int argc, char **argv)
+{
+	const char *socket = OPENNHRP_ADMIN_SOCKET;
+	char cmd[1024] = "", *pos = cmd;
+	int i, fd;
+
+	for (i = 1; i < argc; i++) {
+		if (strlen(argv[i]) != 2 || argv[i][0] != '-') {
+			pos += snprintf(pos, &cmd[sizeof(cmd)-1]-pos,
+					" %s\n", argv[i]) - 1;
+			continue;
+		}
+
+		switch (argv[i][1]) {
+		case 'a':
+			if (++i >= argc)
+				return usage(argv[0]);
+			socket = argv[i];
+			break;
+		default:
+			return usage(argv[0]);
+		}
+	}
+	if (cmd == pos)
+		return usage(argv[0]);
+
+	fd = admin_init(socket);
+	if (fd < 0) {
+		fprintf(stderr,
+			"Failed to connect to opennhrp daemon [%s]: %s.\n\n",
+			socket, strerror(errno));
+		return 1;
+	}
+
+	if (admin_send(fd, &cmd[1]) < 0 ||
+	    admin_receive(fd) < 0) {
+		fprintf(stderr, "Failed to send request: %s.\n",
+			strerror(errno));
+		return 2;
+	}
+
+	admin_close(fd);
+	return 0;
+}
diff --git a/nhrp/sysdep_netlink.c b/nhrp/sysdep_netlink.c
new file mode 100644
index 0000000..d058a98
--- /dev/null
+++ b/nhrp/sysdep_netlink.c
@@ -0,0 +1,1159 @@
+/* sysdep_netlink.c - Linux netlink glue
+ *
+ * Copyright (C) 2007-2009 Timo Teräs <timo.teras@iki.fi>
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 or later as
+ * published by the Free Software Foundation.
+ *
+ * See http://www.gnu.org/ for details.
+ */
+
+#include <time.h>
+#include <stdio.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <malloc.h>
+#include <string.h>
+#include <sys/uio.h>
+#include <sys/wait.h>
+#include <sys/types.h>
+#include <sys/ioctl.h>
+#include <sys/socket.h>
+#include <asm/types.h>
+#include <arpa/inet.h>
+#include <linux/netlink.h>
+#include <linux/rtnetlink.h>
+#include <linux/ip.h>
+#include <linux/if_arp.h>
+#include <linux/if_tunnel.h>
+
+#include "libev.h"
+#include "nhrp_common.h"
+#include "nhrp_interface.h"
+#include "nhrp_peer.h"
+
+#define NETLINK_KERNEL_BUFFER	(256 * 1024)
+#define NETLINK_RECV_BUFFER	(8 * 1024)
+
+#define NLMSG_TAIL(nmsg) \
+	((struct rtattr *) (((void *) (nmsg)) + NLMSG_ALIGN((nmsg)->nlmsg_len)))
+
+#define NDA_RTA(r)  ((struct rtattr*)(((char*)(r)) + NLMSG_ALIGN(sizeof(struct ndmsg))))
+#define NDA_PAYLOAD(n) NLMSG_PAYLOAD(n,sizeof(struct ndmsg))
+
+typedef void (*netlink_dispatch_f)(struct nlmsghdr *msg);
+
+struct netlink_fd {
+	int fd;
+	__u32 seq;
+	struct ev_io io;
+
+	int dispatch_size;
+	const netlink_dispatch_f *dispatch;
+};
+
+static const int netlink_groups[] = {
+	0,
+	RTMGRP_NEIGH,
+	RTMGRP_LINK,
+	RTMGRP_IPV4_IFADDR,
+	RTMGRP_IPV4_ROUTE,
+};
+static struct netlink_fd netlink_fds[ARRAY_SIZE(netlink_groups)];
+#define talk_fd netlink_fds[0]
+
+static struct ev_io packet_io;
+
+static u_int16_t translate_mtu(u_int16_t mtu)
+{
+	/* if mtu is ethernet standard, do not advertise it
+	 * pmtu should be working */
+	if (mtu == 1500)
+		return 0;
+	return mtu;
+}
+
+static void netlink_parse_rtattr(struct rtattr *tb[], int max, struct rtattr *rta, int len)
+{
+	memset(tb, 0, sizeof(struct rtattr *) * (max + 1));
+	while (RTA_OK(rta, len)) {
+		if (rta->rta_type <= max)
+			tb[rta->rta_type] = rta;
+		rta = RTA_NEXT(rta,len);
+	}
+}
+
+static int netlink_add_rtattr_l(struct nlmsghdr *n, int maxlen, int type,
+				const void *data, int alen)
+{
+	int len = RTA_LENGTH(alen);
+	struct rtattr *rta;
+
+	if (NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len) > maxlen)
+		return FALSE;
+
+	rta = NLMSG_TAIL(n);
+	rta->rta_type = type;
+	rta->rta_len = len;
+	memcpy(RTA_DATA(rta), data, alen);
+#ifdef VALGRIND
+	/* Clear the padding area to avoid spurious warnings */
+	memset(RTA_DATA(rta) + alen, 0, RTA_ALIGN(len) - alen);
+#endif
+	n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + RTA_ALIGN(len);
+	return TRUE;
+}
+
+static int netlink_receive(struct netlink_fd *fd, struct nlmsghdr *reply)
+{
+	struct sockaddr_nl nladdr;
+	struct iovec iov;
+	struct msghdr msg = {
+		.msg_name = &nladdr,
+		.msg_namelen = sizeof(nladdr),
+		.msg_iov = &iov,
+		.msg_iovlen = 1,
+	};
+	int got_reply = FALSE, len;
+	char buf[NETLINK_RECV_BUFFER];
+
+	iov.iov_base = buf;
+	while (!got_reply) {
+		int status;
+		struct nlmsghdr *h;
+
+		iov.iov_len = sizeof(buf);
+		status = recvmsg(fd->fd, &msg, MSG_DONTWAIT);
+		if (status < 0) {
+			if (errno == EINTR)
+				continue;
+			if (errno == EAGAIN)
+				return reply == NULL;
+			nhrp_perror("Netlink overrun");
+			continue;
+		}
+
+		if (status == 0) {
+			nhrp_error("Netlink returned EOF");
+			return FALSE;
+		}
+
+		h = (struct nlmsghdr *) buf;
+		while (NLMSG_OK(h, status)) {
+			if (reply != NULL &&
+			    h->nlmsg_seq == reply->nlmsg_seq) {
+				len = h->nlmsg_len;
+				if (len > reply->nlmsg_len) {
+					nhrp_error("Netlink message truncated");
+					len = reply->nlmsg_len;
+				}
+				memcpy(reply, h, len);
+				got_reply = TRUE;
+			} else if (h->nlmsg_type <= fd->dispatch_size &&
+				fd->dispatch[h->nlmsg_type] != NULL) {
+				fd->dispatch[h->nlmsg_type](h);
+			} else if (h->nlmsg_type != NLMSG_DONE) {
+				nhrp_info("Unknown NLmsg: 0x%08x, len %d",
+					  h->nlmsg_type, h->nlmsg_len);
+			}
+			h = NLMSG_NEXT(h, status);
+		}
+	}
+
+	return TRUE;
+}
+
+static int netlink_send(struct netlink_fd *fd, struct nlmsghdr *req)
+{
+	struct sockaddr_nl nladdr;
+	struct iovec iov = {
+		.iov_base = (void*) req,
+		.iov_len = req->nlmsg_len
+	};
+	struct msghdr msg = {
+		.msg_name = &nladdr,
+		.msg_namelen = sizeof(nladdr),
+		.msg_iov = &iov,
+		.msg_iovlen = 1,
+	};
+	int status;
+
+	memset(&nladdr, 0, sizeof(nladdr));
+	nladdr.nl_family = AF_NETLINK;
+
+	req->nlmsg_seq = ++fd->seq;
+
+	status = sendmsg(fd->fd, &msg, 0);
+	if (status < 0) {
+		nhrp_perror("Cannot talk to rtnetlink");
+		return FALSE;
+	}
+	return TRUE;
+}
+
+static int netlink_talk(struct netlink_fd *fd, struct nlmsghdr *req,
+		 size_t replysize, struct nlmsghdr *reply)
+{
+	if (reply == NULL)
+		req->nlmsg_flags |= NLM_F_ACK;
+
+	if (!netlink_send(fd, req))
+		return FALSE;
+
+	if (reply == NULL)
+		return TRUE;
+
+	reply->nlmsg_len = replysize;
+	return netlink_receive(fd, reply);
+}
+
+static int netlink_enumerate(struct netlink_fd *fd, int family, int type)
+{
+	struct {
+		struct nlmsghdr nlh;
+		struct rtgenmsg g;
+	} req;
+	struct sockaddr_nl addr;
+
+	memset(&addr, 0, sizeof(addr));
+	addr.nl_family = AF_NETLINK;
+
+	memset(&req, 0, sizeof(req));
+	req.nlh.nlmsg_len = sizeof(req);
+	req.nlh.nlmsg_type = type;
+	req.nlh.nlmsg_flags = NLM_F_ROOT | NLM_F_MATCH | NLM_F_REQUEST;
+	req.nlh.nlmsg_pid = 0;
+	req.nlh.nlmsg_seq = ++fd->seq;
+	req.g.rtgen_family = family;
+
+	return sendto(fd->fd, (void *) &req, sizeof(req), 0,
+		      (struct sockaddr *) &addr, sizeof(addr)) >= 0;
+}
+
+static void netlink_read_cb(struct ev_io *w, int revents)
+{
+	struct netlink_fd *nfd = container_of(w, struct netlink_fd, io);
+
+	if (revents & EV_READ)
+		netlink_receive(nfd, NULL);
+}
+
+static int do_get_ioctl(const char *basedev, struct ip_tunnel_parm *p)
+{
+	struct ifreq ifr;
+
+#ifdef VALGRIND
+	/* Valgrind does not have SIOCGETTUNNEL description, so clear
+	 * the memory structs to avoid spurious warnings */
+	memset(&ifr, 0, sizeof(ifr));
+	memset(p, 0, sizeof(*p));
+#endif
+
+	strncpy(ifr.ifr_name, basedev, IFNAMSIZ);
+	ifr.ifr_ifru.ifru_data = (void *) p;
+	if (ioctl(packet_io.fd, SIOCGETTUNNEL, &ifr)) {
+		nhrp_perror("ioctl(SIOCGETTUNNEL)");
+		return FALSE;
+	}
+	return TRUE;
+}
+
+#ifndef NHRP_NO_NBMA_GRE
+
+static int netlink_add_nested_rtattr_u32(struct rtattr *rta, int maxlen,
+					 int type, uint32_t value)
+{
+	int len = RTA_LENGTH(4);
+	struct rtattr *subrta;
+
+	if (RTA_ALIGN(rta->rta_len) + len > maxlen)
+		return FALSE;
+
+	subrta = (struct rtattr*)(((char*)rta) + RTA_ALIGN(rta->rta_len));
+	subrta->rta_type = type;
+	subrta->rta_len = len;
+	memcpy(RTA_DATA(subrta), &value, 4);
+	rta->rta_len = NLMSG_ALIGN(rta->rta_len) + len;
+	return TRUE;
+}
+
+static int netlink_configure_arp(struct nhrp_interface *iface, int pf)
+{
+	struct {
+		struct nlmsghdr n;
+		struct ndtmsg ndtm;
+		char buf[256];
+	} req;
+	struct {
+		struct rtattr rta;
+		char buf[256];
+	} parms;
+
+	memset(&req.n, 0, sizeof(req.n));
+	memset(&req.ndtm, 0, sizeof(req.ndtm));
+
+	req.n.nlmsg_len = NLMSG_LENGTH(sizeof(struct ndtmsg));
+	req.n.nlmsg_flags = NLM_F_REQUEST | NLM_F_REPLACE;
+	req.n.nlmsg_type = RTM_SETNEIGHTBL;
+
+	req.ndtm.ndtm_family = pf;
+
+	netlink_add_rtattr_l(&req.n, sizeof(req), NDTA_NAME,
+			     "arp_cache", 10);
+
+	parms.rta.rta_type = NDTA_PARMS;
+	parms.rta.rta_len = RTA_LENGTH(0);
+	netlink_add_nested_rtattr_u32(&parms.rta, sizeof(parms),
+				      NDTPA_IFINDEX, iface->index);
+	netlink_add_nested_rtattr_u32(&parms.rta, sizeof(parms),
+				      NDTPA_APP_PROBES, 1);
+	netlink_add_nested_rtattr_u32(&parms.rta, sizeof(parms),
+				      NDTPA_MCAST_PROBES, 0);
+	netlink_add_nested_rtattr_u32(&parms.rta, sizeof(parms),
+				      NDTPA_UCAST_PROBES, 0);
+
+	netlink_add_rtattr_l(&req.n, sizeof(req), NDTA_PARMS,
+			     parms.buf, parms.rta.rta_len - RTA_LENGTH(0));
+
+	return netlink_send(&talk_fd, &req.n);
+}
+
+static int netlink_link_arp_on(struct nhrp_interface *iface)
+{
+	struct ifreq ifr;
+
+	strncpy(ifr.ifr_name, iface->name, IFNAMSIZ);
+	if (ioctl(packet_io.fd, SIOCGIFFLAGS, &ifr)) {
+		nhrp_perror("ioctl(SIOCGIFFLAGS)");
+		return FALSE;
+	}
+	if (ifr.ifr_flags & IFF_NOARP) {
+		ifr.ifr_flags &= ~IFF_NOARP;
+		if (ioctl(packet_io.fd, SIOCSIFFLAGS, &ifr)) {
+			nhrp_perror("ioctl(SIOCSIFFLAGS)");
+			return FALSE;
+		}
+	}
+	return TRUE;
+}
+
+#else
+
+static int netlink_configure_arp(struct nhrp_interface *iface, int pf)
+{
+	return TRUE;
+}
+
+static int netlink_link_arp_on(struct nhrp_interface *iface)
+{
+	return TRUE;
+}
+
+#endif
+
+static int proc_icmp_redirect_off(const char *interface)
+{
+	char fname[256];
+	int fd, ret = FALSE;
+
+	sprintf(fname, "/proc/sys/net/ipv4/conf/%s/send_redirects", interface);
+	fd = open(fname, O_WRONLY);
+	if (fd < 0)
+		return FALSE;
+	if (write(fd, "0\n", 2) == 2)
+		ret = TRUE;
+	close(fd);
+
+	return ret;
+}
+
+static void netlink_neigh_request(struct nlmsghdr *msg)
+{
+	struct ndmsg *ndm = NLMSG_DATA(msg);
+	struct rtattr *rta[NDA_MAX+1];
+	struct nhrp_peer *peer;
+	struct nhrp_address addr;
+	struct nhrp_interface *iface;
+	char tmp[64];
+
+	netlink_parse_rtattr(rta, NDA_MAX, NDA_RTA(ndm), NDA_PAYLOAD(msg));
+	if (rta[NDA_DST] == NULL)
+		return;
+
+	iface = nhrp_interface_get_by_index(ndm->ndm_ifindex, 0);
+	if (iface == NULL)
+		return;
+
+	nhrp_address_set(&addr, ndm->ndm_family,
+			 RTA_PAYLOAD(rta[NDA_DST]),
+			 RTA_DATA(rta[NDA_DST]));
+
+	nhrp_debug("NL-ARP(%s) who-has %s",
+		   iface->name, nhrp_address_format(&addr, sizeof(tmp), tmp));
+
+	peer = nhrp_peer_route(iface, &addr, 0, ~BIT(NHRP_PEER_TYPE_LOCAL_ROUTE));
+	if (peer == NULL)
+		return;
+
+	if (peer->flags & NHRP_PEER_FLAG_UP)
+		kernel_inject_neighbor(&addr, &peer->next_hop_address, iface);
+
+	if (peer->next_hop_address.type != PF_UNSPEC &&
+	    nhrp_address_cmp(&addr, &peer->protocol_address) != 0)
+		nhrp_peer_traffic_indication(iface, peer->afnum, &addr);
+}
+
+static void netlink_neigh_update(struct nlmsghdr *msg)
+{
+	struct ndmsg *ndm = NLMSG_DATA(msg);
+	struct rtattr *rta[NDA_MAX+1];
+	struct nhrp_interface *iface;
+	struct nhrp_peer_selector sel;
+	int used = FALSE;
+
+	netlink_parse_rtattr(rta, NDA_MAX, NDA_RTA(ndm), NDA_PAYLOAD(msg));
+	if (rta[NDA_DST] == NULL)
+		return;
+
+	if (!(ndm->ndm_state & (NUD_STALE | NUD_FAILED | NUD_REACHABLE)))
+		return;
+
+	iface = nhrp_interface_get_by_index(ndm->ndm_ifindex, 0);
+	if (iface == NULL)
+		return;
+
+	memset(&sel, 0, sizeof(sel));
+	sel.flags = NHRP_PEER_FIND_EXACT;
+	sel.interface = iface;
+	nhrp_address_set(&sel.protocol_address, ndm->ndm_family,
+			 RTA_PAYLOAD(rta[NDA_DST]),
+			 RTA_DATA(rta[NDA_DST]));
+
+	if (msg->nlmsg_type == RTM_NEWNEIGH && (ndm->ndm_state & NUD_REACHABLE))
+		used = TRUE;
+
+	nhrp_peer_foreach(nhrp_peer_set_used_matching,
+			  (void*) (intptr_t) used, &sel);
+}
+
+static void netlink_link_new(struct nlmsghdr *msg)
+{
+	struct nhrp_interface *iface;
+	struct ifinfomsg *ifi = NLMSG_DATA(msg);
+	struct rtattr *rta[IFLA_MAX+1];
+	const char *ifname;
+	struct ip_tunnel_parm cfg;
+	int configuration_changed = FALSE;
+
+	netlink_parse_rtattr(rta, IFLA_MAX, IFLA_RTA(ifi), IFLA_PAYLOAD(msg));
+	if (rta[IFLA_IFNAME] == NULL)
+		return;
+
+	ifname = RTA_DATA(rta[IFLA_IFNAME]);
+	iface = nhrp_interface_get_by_name(ifname, TRUE);
+	if (iface == NULL)
+		return;
+
+	if (rta[IFLA_MTU])
+		iface->mtu = *((unsigned*)RTA_DATA(rta[IFLA_MTU]));
+
+	if (iface->index == 0 || (ifi->ifi_flags & ifi->ifi_change & IFF_UP)) {
+		nhrp_info("Interface %s: new or configured up, mtu=%d",
+			  ifname, iface->mtu);
+		nhrp_interface_run_script(iface, "interface-up");
+	} else {
+		nhrp_info("Interface %s: config change, mtu=%d",
+			  ifname, iface->mtu);
+	}
+
+	iface->index = ifi->ifi_index;
+	nhrp_interface_hash(iface);
+
+	if (!(iface->flags & NHRP_INTERFACE_FLAG_CONFIGURED))
+		return;
+
+	switch (ifi->ifi_type) {
+	case ARPHRD_IPGRE:
+		iface->afnum = AFNUM_INET;
+		/* try hard to get the interface nbma address */
+		do_get_ioctl(ifname, &cfg);
+		if (iface->gre_key != ntohl(cfg.i_key)) {
+			configuration_changed = TRUE;
+			iface->gre_key = ntohl(cfg.i_key);
+		}
+		if (cfg.iph.saddr) {
+			struct nhrp_address saddr;
+			nhrp_address_set(&saddr, PF_INET, 4, (uint8_t *) &cfg.iph.saddr);
+			if (nhrp_address_cmp(&iface->nbma_address, &saddr) || iface->link_index) {
+				configuration_changed = TRUE;
+				iface->nbma_address = saddr;
+				iface->link_index = 0;
+			}
+		} else if (cfg.link) {
+			if (cfg.link != iface->link_index) {
+				configuration_changed = TRUE;
+				nhrp_address_set_type(&iface->nbma_address, PF_UNSPEC);
+				iface->link_index = cfg.link;
+			}
+		} else {
+			if (iface->link_index || iface->nbma_address.type != PF_UNSPEC) {
+				configuration_changed = TRUE;
+				/* Mark the interface as owning all NBMA addresses
+				 * this works when there's only one GRE interface */
+				iface->link_index = 0;
+				nhrp_address_set_type(&iface->nbma_address, PF_UNSPEC);
+				nhrp_info("WARNING: Cannot figure out NBMA address for "
+					  "interface '%s'. Using route hints.", ifname);
+			}
+		}
+		break;
+	}
+
+	if (!(iface->flags & NHRP_INTERFACE_FLAG_SHORTCUT_DEST)) {
+		netlink_configure_arp(iface, PF_INET);
+		netlink_link_arp_on(iface);
+		proc_icmp_redirect_off(iface->name);
+	}
+
+	if (configuration_changed) {
+		struct nhrp_peer_selector sel;
+		int count = 0;
+
+		/* Reset the interface values we detect later */
+		memset(&iface->nat_cie, 0, sizeof(iface->nat_cie));
+		iface->nbma_mtu = 0;
+		if (iface->link_index) {
+			/* Reenumerate addresses if needed */
+			netlink_enumerate(&talk_fd, PF_UNSPEC, RTM_GETADDR);
+			netlink_read_cb(&talk_fd.io, EV_READ);
+		}
+
+		/* Purge all NHRP entries for this interface */
+		memset(&sel, 0, sizeof(sel));
+		sel.type_mask = NHRP_PEER_TYPEMASK_PURGEABLE;
+		sel.interface = iface;
+		nhrp_peer_foreach(nhrp_peer_purge_matching, &count, &sel);
+		nhrp_info("Interface %s: GRE configuration changed. Purged %d peers.",
+			  ifname, count);
+	}
+}
+
+static void netlink_link_del(struct nlmsghdr *msg)
+{
+	struct nhrp_interface *iface;
+	struct ifinfomsg *ifi = NLMSG_DATA(msg);
+	struct rtattr *rta[IFLA_MAX+1];
+	const char *ifname;
+
+	netlink_parse_rtattr(rta, IFLA_MAX, IFLA_RTA(ifi), IFLA_PAYLOAD(msg));
+	if (rta[IFLA_IFNAME] == NULL)
+		return;
+
+	ifname = RTA_DATA(rta[IFLA_IFNAME]);
+	iface = nhrp_interface_get_by_name(ifname, FALSE);
+	if (iface == NULL)
+		return;
+
+	nhrp_info("Interface '%s' deleted", ifname);
+	iface->index = 0;
+	iface->link_index = 0;
+	nhrp_interface_hash(iface);
+
+	nhrp_address_set_type(&iface->nbma_address, PF_UNSPEC);
+	nhrp_address_set_type(&iface->protocol_address, PF_UNSPEC);
+}
+
+static int netlink_addr_new_nbma(void *ctx, struct nhrp_interface *iface)
+{
+	struct nlmsghdr *msg = (struct nlmsghdr *) ctx;
+	struct ifaddrmsg *ifa = NLMSG_DATA(msg);
+	struct rtattr *rta[IFA_MAX+1];
+	struct nhrp_interface *nbma_iface;
+
+	if (iface->link_index == ifa->ifa_index) {
+		netlink_parse_rtattr(rta, IFA_MAX, IFA_RTA(ifa),
+				     IFA_PAYLOAD(msg));
+
+		if (rta[IFA_LOCAL] == NULL)
+			return 0;
+
+		nhrp_address_set(&iface->nbma_address, ifa->ifa_family,
+				 RTA_PAYLOAD(rta[IFA_LOCAL]),
+				 RTA_DATA(rta[IFA_LOCAL]));
+
+		nbma_iface = nhrp_interface_get_by_index(ifa->ifa_index, FALSE);
+		if (nbma_iface != NULL) {
+			iface->nbma_mtu = translate_mtu(nbma_iface->mtu);
+		}
+	}
+
+	return 0;
+}
+
+static void netlink_addr_new(struct nlmsghdr *msg)
+{
+	struct nhrp_interface *iface;
+	struct nhrp_peer *peer, *bcast;
+	struct ifaddrmsg *ifa = NLMSG_DATA(msg);
+	struct rtattr *rta[IFA_MAX+1];
+
+	if (!(ifa->ifa_flags & IFA_F_SECONDARY))
+		nhrp_interface_foreach(netlink_addr_new_nbma, msg);
+
+	netlink_parse_rtattr(rta, IFA_MAX, IFA_RTA(ifa), IFA_PAYLOAD(msg));
+	iface = nhrp_interface_get_by_index(ifa->ifa_index, FALSE);
+	if (iface == NULL || rta[IFA_LOCAL] == NULL)
+		return;
+
+	/* Shortcut destination stuff is extracted from routes;
+	 * not from local address information. */
+	if (iface->flags & NHRP_INTERFACE_FLAG_SHORTCUT_DEST)
+		return;
+	if (!(iface->flags & NHRP_INTERFACE_FLAG_CONFIGURED))
+		return;
+
+	nhrp_address_set(&iface->protocol_address, ifa->ifa_family,
+			 RTA_PAYLOAD(rta[IFA_LOCAL]),
+			 RTA_DATA(rta[IFA_LOCAL]));
+	iface->protocol_address_prefix = ifa->ifa_prefixlen;
+
+	peer = nhrp_peer_alloc(iface);
+	peer->type = NHRP_PEER_TYPE_LOCAL_ADDR;
+	peer->afnum = AFNUM_RESERVED;
+	nhrp_address_set(&peer->protocol_address, ifa->ifa_family,
+			 RTA_PAYLOAD(rta[IFA_LOCAL]),
+			 RTA_DATA(rta[IFA_LOCAL]));
+	switch (ifa->ifa_family) {
+	case PF_INET:
+		peer->protocol_type = ETHPROTO_IP;
+		peer->prefix_length = peer->protocol_address.addr_len * 8;
+		nhrp_peer_insert(peer);
+		break;
+	default:
+		nhrp_peer_put(peer);
+		return;
+	}
+
+	bcast = nhrp_peer_alloc(iface);
+	bcast->type = peer->type;
+	bcast->afnum = peer->afnum;
+	bcast->protocol_type = peer->protocol_type;
+	bcast->prefix_length = peer->prefix_length;
+	bcast->protocol_address = peer->protocol_address;
+	nhrp_address_set_broadcast(&bcast->protocol_address,
+				   ifa->ifa_prefixlen);
+	bcast->next_hop_address = peer->protocol_address;
+	nhrp_peer_insert(bcast);
+	nhrp_peer_put(bcast);
+
+	nhrp_peer_put(peer);
+}
+
+struct netlink_del_addr_msg {
+	int interface_index;
+	struct nhrp_address address;
+};
+
+static int netlink_addr_del_nbma(void *ctx, struct nhrp_interface *iface)
+{
+	struct netlink_del_addr_msg *msg = (struct netlink_del_addr_msg *) ctx;
+
+	if (iface->link_index == msg->interface_index &&
+	    nhrp_address_cmp(&msg->address, &iface->nbma_address) == 0)
+		nhrp_address_set_type(&iface->nbma_address, PF_UNSPEC);
+
+	return 0;
+}
+
+static int netlink_addr_purge_nbma(void *ctx, struct nhrp_peer *peer)
+{
+	struct netlink_del_addr_msg *msg = (struct netlink_del_addr_msg *) ctx;
+
+	if (nhrp_address_cmp(&peer->my_nbma_address, &msg->address) == 0)
+		nhrp_peer_purge(peer, "address-removed");
+
+	return 0;
+}
+
+static void netlink_addr_del(struct nlmsghdr *nlmsg)
+{
+	struct netlink_del_addr_msg msg;
+	struct nhrp_interface *iface;
+	struct ifaddrmsg *ifa = NLMSG_DATA(nlmsg);
+	struct rtattr *rta[IFA_MAX+1];
+	struct nhrp_peer_selector sel;
+
+	netlink_parse_rtattr(rta, IFA_MAX, IFA_RTA(ifa), IFA_PAYLOAD(nlmsg));
+	if (rta[IFA_LOCAL] == NULL)
+		return;
+
+	msg.interface_index = ifa->ifa_index;
+	nhrp_address_set(&msg.address, ifa->ifa_family,
+			 RTA_PAYLOAD(rta[IFA_LOCAL]),
+			 RTA_DATA(rta[IFA_LOCAL]));
+
+	if (!(ifa->ifa_flags & IFA_F_SECONDARY))
+		nhrp_interface_foreach(netlink_addr_del_nbma, &msg);
+	nhrp_peer_foreach(netlink_addr_purge_nbma, &msg, NULL);
+
+	iface = nhrp_interface_get_by_index(ifa->ifa_index, FALSE);
+	if (iface == NULL)
+		return;
+
+	memset(&sel, 0, sizeof(sel));
+	sel.flags = NHRP_PEER_FIND_EXACT;
+	sel.type_mask = BIT(NHRP_PEER_TYPE_LOCAL_ADDR);
+	sel.interface = iface;
+	sel.protocol_address = msg.address;
+	sel.prefix_length = sel.protocol_address.addr_len * 8;
+
+	if (nhrp_address_cmp(&sel.protocol_address, &iface->protocol_address) == 0)
+		nhrp_address_set_type(&iface->protocol_address, PF_UNSPEC);
+	nhrp_peer_foreach(nhrp_peer_remove_matching, NULL, &sel);
+
+	nhrp_address_set_broadcast(&sel.protocol_address, ifa->ifa_prefixlen);
+	sel.next_hop_address = msg.address;
+	nhrp_peer_foreach(nhrp_peer_remove_matching, NULL, &sel);
+}
+
+static void netlink_route_new(struct nlmsghdr *msg)
+{
+	struct nhrp_interface *iface;
+	struct nhrp_peer *peer;
+	struct rtmsg *rtm = NLMSG_DATA(msg);
+	struct rtattr *rta[RTA_MAX+1];
+	int type = 0;
+
+	netlink_parse_rtattr(rta, RTA_MAX, RTM_RTA(rtm), RTM_PAYLOAD(msg));
+	if (rta[RTA_OIF] == NULL || rta[RTA_DST] == NULL)
+		return;
+
+	if (rtm->rtm_family != PF_INET)
+		return;
+
+	iface = nhrp_interface_get_by_index(*(int*)RTA_DATA(rta[RTA_OIF]),
+					    FALSE);
+	if (iface == NULL)
+		return;
+
+	if (iface->flags & NHRP_INTERFACE_FLAG_SHORTCUT_DEST) {
+		/* Local shortcut target routes */
+		if (rtm->rtm_table != RT_TABLE_MAIN)
+			return;
+		type = NHRP_PEER_TYPE_LOCAL_ADDR;
+	} else if (iface->flags & NHRP_INTERFACE_FLAG_CONFIGURED) {
+		/* Routes which might get additional outbound
+		 * shortcuts */
+		if (rtm->rtm_table != iface->route_table ||
+		    rtm->rtm_protocol == RTPROT_KERNEL)
+			return;
+		type = NHRP_PEER_TYPE_LOCAL_ROUTE;
+	}
+	if (type == 0)
+		return;
+
+	peer = nhrp_peer_alloc(iface);
+	peer->type = type;
+	peer->afnum = AFNUM_RESERVED;
+	nhrp_address_set(&peer->protocol_address, rtm->rtm_family,
+			 RTA_PAYLOAD(rta[RTA_DST]),
+			 RTA_DATA(rta[RTA_DST]));
+	if (rta[RTA_GATEWAY] != NULL) {
+		nhrp_address_set(&peer->next_hop_address,
+				 rtm->rtm_family,
+				 RTA_PAYLOAD(rta[RTA_GATEWAY]),
+				 RTA_DATA(rta[RTA_GATEWAY]));
+	}
+	peer->protocol_type = nhrp_protocol_from_pf(rtm->rtm_family);
+	peer->prefix_length = rtm->rtm_dst_len;
+	nhrp_peer_insert(peer);
+	nhrp_peer_put(peer);
+}
+
+static void netlink_route_del(struct nlmsghdr *msg)
+{
+	struct nhrp_interface *iface;
+	struct rtmsg *rtm = NLMSG_DATA(msg);
+	struct rtattr *rta[RTA_MAX+1];
+	struct nhrp_peer_selector sel;
+	int type = 0;
+
+	netlink_parse_rtattr(rta, RTA_MAX, RTM_RTA(rtm), RTM_PAYLOAD(msg));
+	if (rta[RTA_OIF] == NULL || rta[RTA_DST] == NULL)
+		return;
+
+	if (rtm->rtm_family != PF_INET)
+		return;
+
+	iface = nhrp_interface_get_by_index(*(int*)RTA_DATA(rta[RTA_OIF]),
+					    FALSE);
+	if (iface == NULL)
+		return;
+
+	if (iface->flags & NHRP_INTERFACE_FLAG_SHORTCUT_DEST) {
+		/* Local shortcut target routes */
+		if (rtm->rtm_table != RT_TABLE_MAIN)
+			return;
+		type = NHRP_PEER_TYPE_LOCAL_ADDR;
+	} else if (iface->flags & NHRP_INTERFACE_FLAG_CONFIGURED) {
+		/* Routes which might get additional outbound
+		 * shortcuts */
+		if (rtm->rtm_table != iface->route_table ||
+		    rtm->rtm_protocol == RTPROT_KERNEL)
+			return;
+		type = NHRP_PEER_TYPE_LOCAL_ROUTE;
+	}
+	if (type == 0)
+		return;
+
+	memset(&sel, 0, sizeof(sel));
+	sel.flags = NHRP_PEER_FIND_EXACT;
+	sel.type_mask = BIT(type);
+	sel.interface = iface;
+	nhrp_address_set(&sel.protocol_address, rtm->rtm_family,
+			 RTA_PAYLOAD(rta[RTA_DST]),
+			 RTA_DATA(rta[RTA_DST]));
+	if (rta[RTA_GATEWAY] != NULL) {
+		nhrp_address_set(&sel.next_hop_address,
+				 rtm->rtm_family,
+				 RTA_PAYLOAD(rta[RTA_GATEWAY]),
+				 RTA_DATA(rta[RTA_GATEWAY]));
+	}
+	sel.prefix_length = rtm->rtm_dst_len;
+	nhrp_peer_foreach(nhrp_peer_remove_matching, NULL, &sel);
+}
+
+static const netlink_dispatch_f route_dispatch[RTM_MAX] = {
+	[RTM_GETNEIGH] = netlink_neigh_request,
+	[RTM_NEWNEIGH] = netlink_neigh_update,
+	[RTM_DELNEIGH] = netlink_neigh_update,
+	[RTM_NEWLINK] = netlink_link_new,
+	[RTM_DELLINK] = netlink_link_del,
+	[RTM_NEWADDR] = netlink_addr_new,
+	[RTM_DELADDR] = netlink_addr_del,
+	[RTM_NEWROUTE] = netlink_route_new,
+	[RTM_DELROUTE] = netlink_route_del,
+};
+
+static void netlink_stop_listening(struct netlink_fd *fd)
+{
+	ev_io_stop(&fd->io);
+}
+
+static void netlink_close(struct netlink_fd *fd)
+{
+	if (fd->fd >= 0) {
+		netlink_stop_listening(fd);
+		close(fd->fd);
+		fd->fd = 0;
+	}
+}
+
+static int netlink_open(struct netlink_fd *fd, int protocol, int groups)
+{
+	struct sockaddr_nl addr;
+	int buf = NETLINK_KERNEL_BUFFER;
+
+	fd->fd = socket(AF_NETLINK, SOCK_RAW, protocol);
+	fd->seq = time(NULL);
+	if (fd->fd < 0) {
+		nhrp_perror("Cannot open netlink socket");
+		return FALSE;
+	}
+
+	fcntl(fd->fd, F_SETFD, FD_CLOEXEC);
+	if (setsockopt(fd->fd, SOL_SOCKET, SO_SNDBUF, &buf, sizeof(buf)) < 0) {
+		nhrp_perror("SO_SNDBUF");
+		goto error;
+	}
+
+	if (setsockopt(fd->fd, SOL_SOCKET, SO_RCVBUF, &buf, sizeof(buf)) < 0) {
+		nhrp_perror("SO_RCVBUF");
+		goto error;
+	}
+
+	memset(&addr, 0, sizeof(addr));
+	addr.nl_family = AF_NETLINK;
+	addr.nl_groups = groups;
+	if (bind(fd->fd, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
+		nhrp_perror("Cannot bind netlink socket");
+		goto error;
+	}
+
+	ev_io_init(&fd->io, netlink_read_cb, fd->fd, EV_READ);
+	ev_io_start(&fd->io);
+
+	return TRUE;
+
+error:
+	netlink_close(fd);
+	return FALSE;
+}
+
+static void pfpacket_read_cb(struct ev_io *w, int revents)
+{
+	struct sockaddr_ll lladdr;
+	struct nhrp_interface *iface;
+	struct iovec iov;
+	struct msghdr msg = {
+		.msg_name = &lladdr,
+		.msg_namelen = sizeof(lladdr),
+		.msg_iov = &iov,
+		.msg_iovlen = 1,
+	};
+	uint8_t buf[1500];
+	struct nhrp_address from;
+	int fd = w->fd;
+	int i;
+
+	iov.iov_base = buf;
+	for (i = 0; i < 2; i++) {
+		int status;
+
+		iov.iov_len = sizeof(buf);
+		status = recvmsg(fd, &msg, MSG_DONTWAIT);
+		if (status < 0) {
+			if (errno == EINTR)
+				continue;
+			if (errno == EAGAIN)
+				return;
+			nhrp_perror("PF_PACKET overrun");
+			continue;
+		}
+
+		if (status == 0) {
+			nhrp_error("PF_PACKET returned EOF");
+			return;
+		}
+
+		iface = nhrp_interface_get_by_index(lladdr.sll_ifindex, FALSE);
+		if (iface == NULL)
+			continue;
+
+		nhrp_address_set(&from, PF_INET, lladdr.sll_halen, lladdr.sll_addr);
+		if (memcmp(lladdr.sll_addr, "\x00\x00\x00\x00", 4) == 0)
+			nhrp_address_set_type(&from, PF_UNSPEC);
+		nhrp_packet_receive(buf, status, iface, &from);
+	}
+}
+
+int kernel_init(void)
+{
+	int fd, i;
+
+	proc_icmp_redirect_off("all");
+
+	fd = socket(PF_PACKET, SOCK_DGRAM, ETHPROTO_NHRP);
+	if (fd < 0) {
+		nhrp_error("Unable to create PF_PACKET socket");
+		return FALSE;
+	}
+
+	fcntl(fd, F_SETFD, FD_CLOEXEC);
+	ev_io_init(&packet_io, pfpacket_read_cb, fd, EV_READ);
+	ev_io_start(&packet_io);
+
+	for (i = 0; i < ARRAY_SIZE(netlink_groups); i++) {
+		netlink_fds[i].dispatch_size = sizeof(route_dispatch) / sizeof(route_dispatch[0]);
+		netlink_fds[i].dispatch = route_dispatch;
+		if (!netlink_open(&netlink_fds[i], NETLINK_ROUTE,
+				  netlink_groups[i]))
+			goto err_close_all;
+	}
+
+	netlink_enumerate(&talk_fd, PF_UNSPEC, RTM_GETLINK);
+	netlink_read_cb(&talk_fd.io, EV_READ);
+
+	netlink_enumerate(&talk_fd, PF_UNSPEC, RTM_GETADDR);
+	netlink_read_cb(&talk_fd.io, EV_READ);
+
+	netlink_enumerate(&talk_fd, PF_UNSPEC, RTM_GETROUTE);
+	netlink_read_cb(&talk_fd.io, EV_READ);
+
+	return TRUE;
+
+err_close_all:
+	kernel_cleanup();
+	return FALSE;
+}
+
+void kernel_stop_listening(void)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(netlink_groups); i++)
+		netlink_stop_listening(&netlink_fds[i]);
+	ev_io_stop(&packet_io);
+}
+
+void kernel_cleanup(void)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(netlink_groups); i++)
+		netlink_close(&netlink_fds[i]);
+	ev_io_stop(&packet_io);
+	close(packet_io.fd);
+}
+
+int kernel_route(struct nhrp_interface *out_iface,
+		 struct nhrp_address *dest,
+		 struct nhrp_address *default_source,
+		 struct nhrp_address *next_hop,
+		 u_int16_t *mtu)
+{
+	struct {
+		struct nlmsghdr 	n;
+		struct rtmsg 		r;
+		char   			buf[1024];
+	} req;
+	struct rtmsg *r = NLMSG_DATA(&req.n);
+	struct rtattr *rta[RTA_MAX+1];
+
+	memset(&req, 0, sizeof(req));
+	req.n.nlmsg_len = NLMSG_LENGTH(sizeof(struct rtmsg));
+	req.n.nlmsg_flags = NLM_F_REQUEST;
+	req.n.nlmsg_type = RTM_GETROUTE;
+	req.r.rtm_family = dest->type;
+
+	netlink_add_rtattr_l(&req.n, sizeof(req), RTA_DST,
+			     dest->addr, dest->addr_len);
+	req.r.rtm_dst_len = dest->addr_len * 8;
+
+	if (default_source != NULL && default_source->type != PF_UNSPEC)
+		netlink_add_rtattr_l(&req.n, sizeof(req), RTA_SRC,
+				     default_source->addr,
+				     default_source->addr_len);
+	if (out_iface != NULL)
+		netlink_add_rtattr_l(&req.n, sizeof(req), RTA_OIF,
+				     &out_iface->index, sizeof(int));
+
+	if (!netlink_talk(&talk_fd, &req.n, sizeof(req), &req.n))
+		return FALSE;
+
+	netlink_parse_rtattr(rta, RTA_MAX, RTM_RTA(r), RTM_PAYLOAD(&req.n));
+
+	if (default_source != NULL && default_source->type == PF_UNSPEC &&
+	    rta[RTA_PREFSRC] != NULL) {
+		nhrp_address_set(default_source, dest->type,
+				 RTA_PAYLOAD(rta[RTA_PREFSRC]),
+				 RTA_DATA(rta[RTA_PREFSRC]));
+	}
+
+	if (next_hop != NULL) {
+		if (rta[RTA_GATEWAY] != NULL) {
+			nhrp_address_set(next_hop, dest->type,
+					 RTA_PAYLOAD(rta[RTA_GATEWAY]),
+					 RTA_DATA(rta[RTA_GATEWAY]));
+		} else {
+			*next_hop = *dest;
+		}
+	}
+
+	if (mtu != NULL) {
+		*mtu = 0;
+
+		if (rta[RTA_OIF] != NULL) {
+			struct nhrp_interface *nbma_iface;
+
+			/* We use interface MTU here instead of the route
+			 * cache MTU from RTA_METRICS/RTAX_MTU since we
+			 * don't want to announce mtu if PMTU works */
+			nbma_iface = nhrp_interface_get_by_index(
+				*(int*)RTA_DATA(rta[RTA_OIF]),
+				FALSE);
+			if (nbma_iface != NULL)
+				*mtu = translate_mtu(nbma_iface->mtu);
+		}
+	}
+
+	return TRUE;
+}
+
+int kernel_send(uint8_t *packet, size_t bytes, struct nhrp_interface *out,
+		struct nhrp_address *to)
+{
+	struct sockaddr_ll lladdr;
+	struct iovec iov = {
+		.iov_base = (void*) packet,
+		.iov_len = bytes
+	};
+	struct msghdr msg = {
+		.msg_name = &lladdr,
+		.msg_namelen = sizeof(lladdr),
+		.msg_iov = &iov,
+		.msg_iovlen = 1,
+	};
+	int status;
+
+	if (to->addr_len > sizeof(lladdr.sll_addr)) {
+		nhrp_error("Destination NBMA address too long");
+		return FALSE;
+	}
+
+	memset(&lladdr, 0, sizeof(lladdr));
+	lladdr.sll_family = AF_PACKET;
+	lladdr.sll_protocol = ETHPROTO_NHRP;
+	lladdr.sll_ifindex = out->index;
+	lladdr.sll_halen = to->addr_len;
+	memcpy(lladdr.sll_addr, to->addr, to->addr_len);
+
+	status = sendmsg(packet_io.fd, &msg, 0);
+	if (status < 0) {
+		nhrp_error("Cannot send packet to %s(%d): %s",
+			   out->name, out->index, strerror(errno));
+		return FALSE;
+	}
+
+	return TRUE;
+}
+
+int kernel_inject_neighbor(struct nhrp_address *neighbor,
+			   struct nhrp_address *hwaddr,
+			   struct nhrp_interface *dev)
+{
+	struct {
+		struct nlmsghdr 	n;
+		struct ndmsg 		ndm;
+		char   			buf[256];
+	} req;
+	char neigh[64], nbma[64];
+
+	memset(&req.n, 0, sizeof(req.n));
+	memset(&req.ndm, 0, sizeof(req.ndm));
+
+	req.n.nlmsg_len = NLMSG_LENGTH(sizeof(struct ndmsg));
+	req.n.nlmsg_flags = NLM_F_REQUEST | NLM_F_REPLACE | NLM_F_CREATE;
+	req.n.nlmsg_type = RTM_NEWNEIGH;
+	req.ndm.ndm_family = neighbor->type;
+	req.ndm.ndm_ifindex = dev->index;
+	req.ndm.ndm_type = RTN_UNICAST;
+
+	netlink_add_rtattr_l(&req.n, sizeof(req), NDA_DST,
+			     neighbor->addr, neighbor->addr_len);
+
+	if (hwaddr != NULL && hwaddr->type != PF_UNSPEC) {
+		req.ndm.ndm_state = NUD_REACHABLE;
+
+		netlink_add_rtattr_l(&req.n, sizeof(req), NDA_LLADDR,
+				     hwaddr->addr, hwaddr->addr_len);
+
+		nhrp_debug("NL-ARP(%s) %s is-at %s",
+			   dev->name,
+			   nhrp_address_format(neighbor, sizeof(neigh), neigh),
+			   nhrp_address_format(hwaddr, sizeof(nbma), nbma));
+	} else {
+		req.ndm.ndm_state = NUD_FAILED;
+
+		nhrp_debug("NL-ARP(%s) %s not-reachable",
+			   dev->name,
+			   nhrp_address_format(neighbor, sizeof(neigh), neigh));
+	}
+
+	return netlink_send(&talk_fd, &req.n);
+}
+
diff --git a/nhrp/sysdep_pfpacket.c b/nhrp/sysdep_pfpacket.c
new file mode 100644
index 0000000..514b848
--- /dev/null
+++ b/nhrp/sysdep_pfpacket.c
@@ -0,0 +1,388 @@
+/* sysdep_pfpacket.c - Tracing of forwarded packets using PF_PACKET
+ *
+ * Copyright (C) 2007-2009 Timo Teräs <timo.teras@iki.fi>
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 or later as
+ * published by the Free Software Foundation.
+ *
+ * See http://www.gnu.org/ for details.
+ */
+
+#include <errno.h>
+#include <fcntl.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <string.h>
+#include <sys/socket.h>
+#include <sys/ioctl.h>
+#include <netinet/in.h>
+#include <linux/types.h>
+#include <linux/filter.h>
+#include <linux/if_ether.h>
+#include <linux/if_packet.h>
+#include <linux/ip.h>
+
+#include "libev.h"
+#include "nhrp_defines.h"
+#include "nhrp_common.h"
+#include "nhrp_interface.h"
+#include "nhrp_peer.h"
+
+#define MAX_OPCODES 100
+
+struct multicast_packet {
+	struct nhrp_interface *iface;
+	struct sockaddr_ll lladdr;
+	unsigned int pdulen;
+	unsigned char pdu[1500];
+};
+
+static struct ev_io packet_io;
+static struct ev_timer install_filter_timer;
+static struct ev_idle mcast_route;
+
+static struct multicast_packet mcast_queue[16];
+static int mcast_head = 0, mcast_tail = 0;
+
+
+enum {
+	LABEL_NEXT = 0,
+	LABEL_SKIP1,
+	LABEL_SKIPN,
+	LABEL_DROP,
+	LABEL_CHECK_MULTICAST,
+	LABEL_CHECK_MULTICAST_DESTINATION,
+	LABEL_CHECK_TRAFFIC_INDICATION,
+	LABEL_CHECK_NON_LOCAL_ADDRESS,
+	NUM_LABELS
+};
+
+struct filter {
+	int pos[NUM_LABELS];
+	int numops;
+	struct sock_filter code[MAX_OPCODES];
+};
+
+static void emit_stmt(struct filter *f, __u16 code, __u32 k)
+{
+	if (f->numops < MAX_OPCODES) {
+		f->code[f->numops].code = code;
+		f->code[f->numops].jt = 0;
+		f->code[f->numops].jf = 0;
+		f->code[f->numops].k = k;
+	}
+	f->numops++;
+}
+
+static void emit_jump(struct filter *f, __u16 code, __u32 k, __u8 jt, __u8 jf)
+{
+	if (f->numops < MAX_OPCODES) {
+		f->code[f->numops].code = code;
+		f->code[f->numops].jt = jt;
+		f->code[f->numops].jf = jf;
+		f->code[f->numops].k = k;
+	}
+	f->numops++;
+}
+
+static void mark(struct filter *f, int label)
+{
+	f->pos[label] = f->numops;
+}
+
+static int check_interface_multicast(void *ctx, struct nhrp_interface *iface)
+{
+	struct filter *f = (struct filter *) ctx;
+
+	if (!(iface->flags & NHRP_INTERFACE_FLAG_CONFIGURED))
+		return 0;
+	if (iface->flags & NHRP_INTERFACE_FLAG_SHORTCUT_DEST)
+		return 0;
+
+	if (iface->mcast_mask || iface->mcast_numaddr)
+		emit_jump(f, BPF_JMP|BPF_JEQ|BPF_K, iface->index,
+			  LABEL_CHECK_MULTICAST_DESTINATION, LABEL_NEXT);
+
+	return 0;
+}
+
+static int drop_matching_address(void *ctx, struct nhrp_peer *peer)
+{
+	struct filter *f = (struct filter *) ctx;
+	unsigned long addr, mask;
+
+	if (peer->protocol_type != ETHPROTO_IP)
+		return 0;
+
+	addr = htonl(*((unsigned long *) peer->protocol_address.addr));
+	if (peer->prefix_length != 32) {
+		mask = 0xffffffff >> peer->prefix_length;
+		emit_jump(f, BPF_JMP|BPF_JGE|BPF_K, addr & ~mask, LABEL_NEXT, LABEL_SKIP1);
+		emit_jump(f, BPF_JMP|BPF_JGT|BPF_K, addr |  mask, LABEL_NEXT, LABEL_DROP);
+	} else {
+		emit_jump(f, BPF_JMP|BPF_JEQ|BPF_K, addr, LABEL_DROP, LABEL_NEXT);
+	}
+
+	return 0;
+}
+
+static int check_interface_traffic_indication(void *ctx, struct nhrp_interface *iface)
+{
+	struct filter *f = (struct filter *) ctx;
+
+	if (!(iface->flags & NHRP_INTERFACE_FLAG_CONFIGURED))
+		return 0;
+	if (iface->flags & NHRP_INTERFACE_FLAG_SHORTCUT_DEST)
+		return 0;
+	if (!(iface->flags & NHRP_INTERFACE_FLAG_REDIRECT))
+		return 0;
+
+	emit_jump(f, BPF_JMP|BPF_JEQ|BPF_K, iface->index,
+		  LABEL_CHECK_NON_LOCAL_ADDRESS, LABEL_NEXT);
+
+	return 0;
+}
+
+static void install_filter_cb(struct ev_timer *w, int revents)
+{
+	struct nhrp_peer_selector sel;
+	struct sock_fprog prog;
+	struct filter f;
+	int i;
+
+	memset(&prog, 0, sizeof(prog));
+	memset(&f, 0, sizeof(f));
+
+	/* Check for IPv4 */
+	emit_stmt(&f, BPF_LD |BPF_W  |BPF_ABS, SKF_AD_OFF+SKF_AD_PROTOCOL);
+	emit_jump(&f, BPF_JMP|BPF_JEQ|BPF_K,   ETH_P_IP, LABEL_NEXT, LABEL_DROP);
+
+	/* Traffic indication checking is for incoming packets
+	 * Multicast checking is for outgoing packets */
+	emit_stmt(&f, BPF_LD |BPF_W  |BPF_ABS, SKF_AD_OFF+SKF_AD_PKTTYPE);
+	emit_jump(&f, BPF_JMP|BPF_JEQ|BPF_K, PACKET_OUTGOING,  LABEL_CHECK_MULTICAST, LABEL_NEXT);
+	emit_jump(&f, BPF_JMP|BPF_JEQ|BPF_K, PACKET_HOST, LABEL_CHECK_TRAFFIC_INDICATION, LABEL_DROP);
+
+	/* MULTICAST check - for interfaces that have MC forwarding enabled */
+	mark(&f, LABEL_CHECK_MULTICAST);
+	emit_stmt(&f, BPF_LD |BPF_W  |BPF_ABS, SKF_AD_OFF+SKF_AD_IFINDEX);
+	nhrp_interface_foreach(check_interface_multicast, &f);
+	emit_stmt(&f, BPF_RET|BPF_K, 0);
+
+	/* Check for multicast IPv4 destination - accept on match (all packet) */
+	mark(&f, LABEL_CHECK_MULTICAST_DESTINATION);
+	emit_stmt(&f, BPF_LD |BPF_W  |BPF_ABS, offsetof(struct iphdr, daddr));
+	emit_jump(&f, BPF_JMP|BPF_JGE|BPF_K, 0xe0000000, LABEL_NEXT, LABEL_DROP);
+	emit_jump(&f, BPF_JMP|BPF_JGE|BPF_K, 0xf0000000, LABEL_DROP, LABEL_NEXT);
+	emit_stmt(&f, BPF_RET|BPF_K, 65535);
+
+	/* TRAFFIC INDICATION check - is destination non-local
+	 * if yes, capture headers for NHRP traffic indication */
+	mark(&f, LABEL_CHECK_TRAFFIC_INDICATION);
+	emit_stmt(&f, BPF_LD |BPF_W  |BPF_ABS, SKF_AD_OFF+SKF_AD_IFINDEX);
+	nhrp_interface_foreach(check_interface_traffic_indication, &f);
+	emit_stmt(&f, BPF_RET|BPF_K, 0);
+
+	mark(&f, LABEL_CHECK_NON_LOCAL_ADDRESS);
+	memset(&sel, 0, sizeof(sel));
+	sel.type_mask = BIT(NHRP_PEER_TYPE_LOCAL_ADDR);
+	emit_stmt(&f, BPF_LD |BPF_W  |BPF_ABS, offsetof(struct iphdr, daddr));
+	nhrp_peer_foreach(drop_matching_address, &f, &sel);
+	emit_stmt(&f, BPF_RET|BPF_K, 68);
+
+	mark(&f, LABEL_DROP);
+	emit_stmt(&f, BPF_RET|BPF_K, 0);
+
+	/* All ok so far? */
+	if (f.numops >= MAX_OPCODES) {
+		nhrp_error("Filter code buffer too small (code actual length %d)",
+			   f.numops);
+		return;
+	}
+
+	/* Fixup jumps to be relative */
+	for (i = 0; i < f.numops; i++) {
+		if (BPF_CLASS(f.code[i].code) == BPF_JMP) {
+			if (f.code[i].jt > LABEL_SKIPN)
+				f.code[i].jt = f.pos[f.code[i].jt] - i - 1;
+			if (f.code[i].jf > LABEL_SKIPN)
+				f.code[i].jf = f.pos[f.code[i].jf] - i - 1;
+		}
+	}
+
+	/* Attach filter */
+	prog.len = f.numops;
+	prog.filter = f.code;
+	if (setsockopt(packet_io.fd, SOL_SOCKET, SO_ATTACH_FILTER,
+		       &prog, sizeof(prog)))
+		return;
+
+	nhrp_info("Filter code installed (%d opcodes)", f.numops);
+}
+
+int forward_local_addresses_changed(void)
+{
+	if (install_filter_timer.cb != NULL)
+		ev_timer_start(&install_filter_timer);
+	return TRUE;
+}
+
+static void send_multicast(struct ev_idle *w, int revents)
+{
+	struct multicast_packet *pkt;
+	struct nhrp_peer *peer;
+	struct iovec iov;
+	struct msghdr msg;
+
+	if (mcast_head == mcast_tail) {
+		ev_idle_stop(&mcast_route);
+		return;
+	}
+
+	/* Pop a packet */
+	pkt = &mcast_queue[mcast_tail];
+	mcast_tail = (mcast_tail + 1) % ARRAY_SIZE(mcast_queue);
+
+	/* And softroute it forward */
+	iov.iov_base = pkt->pdu;
+	iov.iov_len = pkt->pdulen;
+	msg = (struct msghdr) {
+		.msg_name = &pkt->lladdr,
+		.msg_namelen = sizeof(pkt->lladdr),
+		.msg_iov = &iov,
+		.msg_iovlen = 1,
+	};
+
+	list_for_each_entry(peer, &pkt->iface->mcast_list, mcast_list_entry) {
+		/* Update NBMA destination */
+		pkt->lladdr.sll_halen = peer->next_hop_address.addr_len;
+		memcpy(pkt->lladdr.sll_addr, peer->next_hop_address.addr,
+		       pkt->lladdr.sll_halen);
+
+		/* Best effort attempt to emulate multicast */
+		(void) sendmsg(packet_io.fd, &msg, 0);
+	}
+}
+
+static void pfp_read_cb(struct ev_io *w, int revents)
+{
+	struct nhrp_address nbma_src, src, dst;
+	struct nhrp_interface *iface;
+	struct sockaddr_ll *lladdr;
+	struct iovec iov;
+	struct msghdr msg = {
+		.msg_iov = &iov,
+		.msg_iovlen = 1,
+	};
+	char fr[32], to[32];
+	int r, fd = w->fd;
+
+	if (!(revents & EV_READ))
+		return;
+
+	while (TRUE) {
+		/* Get a scracth buffer directly from mcast queue, so we do
+		 * not need copy the data later. */
+		msg.msg_name = &mcast_queue[mcast_head].lladdr;
+		msg.msg_namelen = sizeof(mcast_queue[mcast_head].lladdr);
+		iov.iov_base = mcast_queue[mcast_head].pdu;
+		iov.iov_len = sizeof(mcast_queue[mcast_head].pdu);
+
+		/* Receive */
+		r = recvmsg(fd, &msg, MSG_DONTWAIT);
+		mcast_queue[mcast_head].pdulen = r;
+
+		/* Process */
+		if (r < 0) {
+			if (errno == EINTR)
+				continue;
+			if (errno == EAGAIN)
+				return;
+			nhrp_perror("PF_PACKET overrun");
+			continue;
+		}
+
+		if (r == 0) {
+			nhrp_error("PF_PACKET returned EOF");
+			return;
+		}
+
+		lladdr = &mcast_queue[mcast_head].lladdr;
+		if (lladdr->sll_pkttype != PACKET_OUTGOING &&
+		    lladdr->sll_pkttype != PACKET_HOST)
+			continue;
+
+		iface = nhrp_interface_get_by_index(lladdr->sll_ifindex, FALSE);
+		if (iface == NULL)
+			continue;
+		if (!(iface->flags & NHRP_INTERFACE_FLAG_CONFIGURED))
+			continue;
+
+		if (!nhrp_address_parse_packet(lladdr->sll_protocol,
+					       r, iov.iov_base,
+					       &src, &dst))
+			return;
+
+		if (nhrp_address_is_multicast(&dst) &&
+		    lladdr->sll_pkttype == PACKET_OUTGOING) {
+			nhrp_debug("Multicast from %s to %s",
+				   nhrp_address_format(&src, sizeof(fr), fr),
+				   nhrp_address_format(&dst, sizeof(to), to));
+
+			/* Queue packet for processing later (handle important
+			 * stuff first) */
+			mcast_queue[mcast_head].iface = iface;
+			mcast_head = (mcast_head + 1) % ARRAY_SIZE(mcast_queue);
+
+			/* Drop packets from queue tail, if we haven't processed
+			 * them yet. */
+			if (mcast_head == mcast_tail)
+				mcast_tail = (mcast_tail + 1) %
+					ARRAY_SIZE(mcast_queue);
+
+			ev_idle_start(&mcast_route);
+		} else if (lladdr->sll_pkttype == PACKET_HOST) {
+			nhrp_address_set(&nbma_src, PF_INET,
+					 lladdr->sll_halen,
+					 lladdr->sll_addr);
+			nhrp_packet_send_traffic(iface,
+						 &nbma_src, &src, &dst,
+						 lladdr->sll_protocol,
+						 iov.iov_base, r);
+		}
+	}
+}
+
+int forward_init(void)
+{
+	int fd;
+
+	fd = socket(PF_PACKET, SOCK_DGRAM, ntohs(ETH_P_ALL));
+	if (fd < 0) {
+		nhrp_error("Unable to create PF_PACKET socket");
+		return FALSE;
+	}
+
+	fcntl(fd, F_SETFD, FD_CLOEXEC);
+
+	ev_io_init(&packet_io, pfp_read_cb, fd, EV_READ);
+	ev_io_start(&packet_io);
+
+	ev_timer_init(&install_filter_timer, install_filter_cb, .01, .0);
+	install_filter_cb(&install_filter_timer, 0);
+
+	ev_idle_init(&mcast_route, send_multicast);
+	ev_set_priority(&mcast_route, -1);
+
+	return TRUE;
+}
+
+void forward_cleanup(void)
+{
+	ev_io_stop(&packet_io);
+	close(packet_io.fd);
+	ev_timer_stop(&install_filter_timer);
+	ev_idle_stop(&mcast_route);
+}
diff --git a/nhrp/sysdep_syslog.c b/nhrp/sysdep_syslog.c
new file mode 100644
index 0000000..c8f9f7e
--- /dev/null
+++ b/nhrp/sysdep_syslog.c
@@ -0,0 +1,55 @@
+/* sysdep_syslog.c - Logging via syslog
+ *
+ * Copyright (C) 2007 Timo Teräs <timo.teras@iki.fi>
+ * All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 or later as
+ * published by the Free Software Foundation.
+ *
+ * See http://www.gnu.org/ for details.
+ */
+
+#include <errno.h>
+#include <stdio.h>
+#include <string.h>
+#include <syslog.h>
+#include <stdarg.h>
+
+#include "nhrp_defines.h"
+#include "nhrp_common.h"
+
+int log_init(void)
+{
+	openlog("opennhrp", LOG_PERROR | LOG_PID, LOG_DAEMON);
+
+	return TRUE;
+}
+
+void nhrp_log(int level, const char *format, ...)
+{
+	va_list va;
+	int l;
+
+	switch (level) {
+	case NHRP_LOG_ERROR:
+		l = LOG_ERR;
+		break;
+	case NHRP_LOG_INFO:
+		l = LOG_INFO;
+		break;
+	case NHRP_LOG_DEBUG:
+	default:
+		l = LOG_DEBUG;
+		break;
+	}
+
+	va_start(va, format);
+	vsyslog(l, format, va);
+	va_end(va);
+}
+
+void nhrp_perror(const char *message)
+{
+	nhrp_error("%s: %s", message, strerror(errno));
+}