nhrpd: implement next hop resolution protocol

This provides DMVPN support and integrates to strongSwan. Please read
README.nhrpd and README.kernel for more details.

[DL: cherry-picked from dafa05e65fe4b3b3ed5525443f554215ba14f42c]
[DL: merge partially resolved, this commit will not build.]
Signed-off-by: Timo Teräs <timo.teras@iki.fi>
Signed-off-by: David Lamparter <equinox@opensourcerouting.org>

.gitignore

@@ -30,6 +30,7 @@ Makefile.in
 *.tar.gz.asc
 .nfs*
 libtool
+.libs
 .arch-inventory
 .arch-ids
 {arch}
@@ -37,6 +38,7 @@ build
 .msg
 .rebase-*
 *~
+*.o
 *.loT
 m4/*.m4
 !m4/ax_sys_weak_alias.m4

Makefile.am

@@ -2,12 +2,13 @@
 
 SUBDIRS = lib qpb fpm @ZEBRA@ @LIBRFP@ @RFPTEST@ \
 	 @BGPD@ @RIPD@ @RIPNGD@ @OSPFD@ @OSPF6D@ @LDPD@ \
-         @ISISD@ @PIMD@ @WATCHFRR@ @VTYSH@ @OSPFCLIENT@ @DOC@ m4 @pkgsrcdir@ \
+         @ISISD@ @PIMD@ @NHRPD@ \
+	 @WATCHFRR@ @VTYSH@ @OSPFCLIENT@ @DOC@ m4 @pkgsrcdir@ \
          redhat @SOLARIS@ tests tools cumulus snapcraft
 
 DIST_SUBDIRS = lib qpb fpm zebra bgpd ripd ripngd ospfd ospf6d ldpd \
 	  isisd watchfrr vtysh ospfclient doc m4 pkgsrc redhat tests \
-	  solaris pimd @LIBRFP@ @RFPTEST@ tools cumulus snapcraft
+	  solaris pimd nhrpd @LIBRFP@ @RFPTEST@ tools cumulus snapcraft
 
 EXTRA_DIST = aclocal.m4 SERVICES REPORTING-BUGS \
 	update-autotools \

SERVICES

@@ -17,5 +17,7 @@ bgpd		2605/tcp
 ospf6d		2606/tcp
 ospfapi		2607/tcp
 isisd		2608/tcp
+# babeld	2609/tcp
+nhrpd		2610/tcp
 pimd		2611/tcp
 ldpd		2612/tcp

configure.ac

@@ -80,6 +80,7 @@ CFLAGS="$orig_cflags"
 AC_PROG_CC_C99
 
 AC_PROG_EGREP
+PKG_PROG_PKG_CONFIG
 
 dnl autoconf 2.59 appears not to support AC_PROG_SED
 dnl AC_PROG_SED
@@ -214,6 +215,8 @@ AC_ARG_ENABLE(ospf6d,
   AS_HELP_STRING([--disable-ospf6d], [do not build ospf6d]))
 AC_ARG_ENABLE(ldpd,
   AS_HELP_STRING([--enable-ldpd], [build ldpd]))
+AC_ARG_ENABLE(nhrpd,
+  AS_HELP_STRING([--disable-nhrpd], [do not build nhrpd]))
 AC_ARG_ENABLE(watchfrr,
   AS_HELP_STRING([--disable-watchfrr], [do not build watchfrr]))
 AC_ARG_ENABLE(isisd,
@@ -1185,6 +1188,13 @@ else
 fi
 AM_CONDITIONAL(LDPD, test "x$LDPD" = "xldpd")
 
+if test "${enable_nhrpd}" = "no";then
+  NHRPD=""
+else
+  NHRPD="nhrpd"
+fi
+AM_CONDITIONAL(NHRPD, test "x$NHRPD" = "xnhrpd")
+
 if test "${enable_watchfrr}" = "no";then
   WATCHFRR=""
 else
@@ -1265,6 +1275,7 @@ AC_SUBST(RIPNGD)
 AC_SUBST(OSPFD)
 AC_SUBST(OSPF6D)
 AC_SUBST(LDPD)
+AC_SUBST(NHRPD)
 AC_SUBST(WATCHFRR)
 AC_SUBST(ISISD)
 AC_SUBST(PIMD)
@@ -1285,6 +1296,14 @@ if test "x$enable_pcreposix" = "xyes"; then
 fi
 AC_SUBST(HAVE_LIBPCREPOSIX)
 
+dnl ------------------
+dnl check C-Ares library
+dnl ------------------
+if test "${enable_nhrpd}" != "no";then
+   PKG_CHECK_MODULES([CARES], [libcares])
+fi
+
+
 dnl ------------------
 dnl check Net-SNMP library
 dnl ------------------
@@ -1614,6 +1633,7 @@ AC_DEFINE_UNQUOTED(PATH_LDPD_PID, "$frr_statedir/ldpd.pid",ldpd PID)
 AC_DEFINE_UNQUOTED(LDPD_SOCKET, "$frr_statedir/ldpd.sock",ldpd control socket)
 AC_DEFINE_UNQUOTED(PATH_ISISD_PID, "$frr_statedir/isisd.pid",isisd PID)
 AC_DEFINE_UNQUOTED(PATH_PIMD_PID, "$frr_statedir/pimd.pid",pimd PID)
+AC_DEFINE_UNQUOTED(PATH_NHRPD_PID, "$frr_statedir/nhrpd.pid",nhrpd PID)
 AC_DEFINE_UNQUOTED(PATH_WATCHFRR_PID, "$frr_statedir/watchfrr.pid",watchfrr PID)
 AC_DEFINE_UNQUOTED(ZEBRA_SERV_PATH, "$frr_statedir/zserv.api",zebra api socket)
 AC_DEFINE_UNQUOTED(ZEBRA_VTYSH_PATH, "$frr_statedir/zebra.vty",zebra vty socket)
@@ -1625,6 +1645,7 @@ AC_DEFINE_UNQUOTED(OSPF6_VTYSH_PATH, "$frr_statedir/ospf6d.vty",ospf6d vty socke
 AC_DEFINE_UNQUOTED(LDP_VTYSH_PATH, "$frr_statedir/ldpd.vty",ldpd vty socket)
 AC_DEFINE_UNQUOTED(ISIS_VTYSH_PATH, "$frr_statedir/isisd.vty",isisd vty socket)
 AC_DEFINE_UNQUOTED(PIM_VTYSH_PATH, "$frr_statedir/pimd.vty",pimd vty socket)
+AC_DEFINE_UNQUOTED(NHRP_VTYSH_PATH, "$frr_statedir/nhrpd.vty",nhrpd vty socket)
 AC_DEFINE_UNQUOTED(WATCHFRR_VTYSH_PATH, "$frr_statedir/watchfrr.vty",watchfrr vty socket)
 AC_DEFINE_UNQUOTED(DAEMON_VTY_DIR, "$frr_statedir",daemon vty directory)
 
@@ -1667,6 +1688,7 @@ AC_CONFIG_FILES([Makefile lib/Makefile qpb/Makefile zebra/Makefile ripd/Makefile
 	  ospf6d/Makefile ldpd/Makefile isisd/Makefile vtysh/Makefile
 	  doc/Makefile ospfclient/Makefile tests/Makefile m4/Makefile
 	  pimd/Makefile
+	  nhrpd/Makefile
 	  redhat/Makefile
 	  tools/Makefile
 	  cumulus/Makefile

lib/log.c

@@ -59,6 +59,7 @@ const char *zlog_proto_names[] =
   "LDP",
   "ISIS",
   "PIM",
+  "NHRP",
   "RFP",
   "WATCHFRR",
   NULL,
@@ -1083,6 +1084,8 @@ proto_redistnum(int afi, const char *s)
 	return ZEBRA_ROUTE_VNC;
       else if (strmatch (s, "vnc-direct"))
 	return ZEBRA_ROUTE_VNC_DIRECT;
+      else if (strncmp (s, "n", 1) == 0)
+	return ZEBRA_ROUTE_NHRP;
     }
   if (afi == AFI_IP6)
     {
@@ -1106,6 +1109,8 @@ proto_redistnum(int afi, const char *s)
 	return ZEBRA_ROUTE_VNC;
       else if (strmatch (s, "vnc-direct"))
 	return ZEBRA_ROUTE_VNC_DIRECT;
+      else if (strncmp (s, "n", 1) == 0)
+	return ZEBRA_ROUTE_NHRP;
     }
   return -1;
 }

lib/log.h

@@ -24,6 +24,7 @@
 #define _ZEBRA_LOG_H
 
 #include <syslog.h>
+#include <stdio.h>
 
 /* Here is some guidance on logging levels to use:
  *
@@ -58,6 +59,7 @@ typedef enum
   ZLOG_LDP,
   ZLOG_ISIS,
   ZLOG_PIM,
+  ZLOG_NHRP,
   ZLOG_RFP,
   ZLOG_WATCHFRR,
 } zlog_proto_t;

lib/route_types.txt

@@ -52,6 +52,7 @@ ZEBRA_ROUTE_OSPF6,      ospf6,     ospf6d, 'O', 0, 1, "OSPFv3"
 ZEBRA_ROUTE_ISIS,       isis,      isisd,  'I', 1, 1, "IS-IS"
 ZEBRA_ROUTE_BGP,        bgp,       bgpd,   'B', 1, 1, "BGP"
 ZEBRA_ROUTE_PIM,	pim,	   pimd,   'P', 1, 0, "PIM"
+ZEBRA_ROUTE_NHRP,       nhrp,      nhrpd,  'N', 1, 1, "NHRP"
 # HSLS and OLSR both are AFI independent (so: 1, 1), however
 # we want to disable for them for general Quagga distribution.
 # This at least makes it trivial for users of these protocols
@@ -85,6 +86,7 @@ ZEBRA_ROUTE_OSPF6,  "Open Shortest Path First (IPv6) (OSPFv3)"
 ZEBRA_ROUTE_ISIS,   "Intermediate System to Intermediate System (IS-IS)"
 ZEBRA_ROUTE_BGP,    "Border Gateway Protocol (BGP)"
 ZEBRA_ROUTE_PIM,    "Protocol Independent Multicast (PIM)"
+ZEBRA_ROUTE_NHRP,   "Next Hop Resolution Protocol (NHRP)"
 ZEBRA_ROUTE_HSLS,   "Hazy-Sighted Link State Protocol (HSLS)"
 ZEBRA_ROUTE_VNC,    "Virtual Network Control (VNC)"
 ZEBRA_ROUTE_OLSR,   "Optimised Link State Routing (OLSR)"

nhrpd/Makefile.am

@@ -0,0 +1,35 @@
+## Process this file with automake to produce Makefile.in.
+
+AM_CPPFLAGS = -I.. -I$(top_srcdir) -I$(top_srcdir)/lib -I$(top_builddir)/lib -DQUAGGA_NO_DEPRECATED_INTERFACES
+DEFS = @DEFS@ @CARES_CFLAGS@ -DSYSCONFDIR=\"$(sysconfdir)/\"
+INSTALL_SDATA=@INSTALL@ -m 600
+
+AM_CFLAGS = $(PICFLAGS) #$(WERROR)
+AM_LDFLAGS = $(PICLDFLAGS)
+
+sbin_PROGRAMS = nhrpd
+
+nhrpd_SOURCES = \
+	zbuf.c \
+	znl.c \
+	resolver.c \
+	linux.c \
+	netlink_arp.c \
+	netlink_gre.c \
+	vici.c \
+	reqid.c \
+	nhrp_event.c \
+	nhrp_packet.c \
+	nhrp_interface.c \
+	nhrp_vc.c \
+	nhrp_peer.c \
+	nhrp_cache.c \
+	nhrp_nhs.c \
+	nhrp_route.c \
+	nhrp_shortcut.c \
+	nhrp_vty.c \
+	nhrp_main.c
+
+nhrpd_LDADD = ../lib/libzebra.la @LIBCAP@ @CARES_LIBS@
+
+#dist_examples_DATA = nhrpd.conf.sample

nhrpd/README.kernel

@@ -0,0 +1,145 @@
+KERNEL REQUIREMENTS
+===================
+
+The linux kernel has had various major regressions, performance
+issues and subtle bugs (especially in pmtu). Here is a short list
+of some -stable kernels and the first point release that is supposedly
+working well with opennhrp/dmvpn:
+  3.12.8 or later
+  3.14.54 or later
+  3.18.22 or later[1]
+
+[1] But you need to apply the following two backported commits:
+    3cdaa5be9e ipv4: Don't increase PMTU with Datagram Too Big message
+    cb6ccf09d6 route: Use ipv4_mtu instead of raw rt_pmtu
+
+See below for list of known issues in various kernel versions.
+
+Kernels earlier than 3.12 need CONFIG_ARPD enabled in the configuration.
+Many distributions do not enable it by default, and you may need to
+compile your own kernel.
+
+KERNEL BUGS
+===========
+
+DMVPN and mGRE support in the kernel has been brittle. There are various
+regressions in multiple kernel versions.
+
+This list tries to collect them to one source of information:
+
+- forward pmtu is disabled intentionally (but tunnel devices rely on it)
+  Broken since 3.14-rc1:
+    commit "ipv4: introduce ip_dst_mtu_maybe_forward and protect forwarding path against pmtu spoofing"
+  Workaround:
+    Set sysctl net.ipv4.ip_forward_use_pmtu=1
+    (Should fix kernel to have this by default on for tunnel devices)
+
+- subtle path mtu mishandling issues
+  Broken since (uncertain)
+  Fixed in 4.1-rc2:
+    commit "ipv4: Don't increase PMTU with Datagram Too Big message."
+    commit "route: Use ipv4_mtu instead of raw rt_pmtu"
+
+- fragmentation of large packets inside tunnel not working
+  Broken since 3.11-rc1
+    commit "ip_tunnels: Use skb-len to PMTU check."
+  Fixed in 3.14.54, 3.18.22, 4.1.9, 4.2-rc3
+    commit "ip_tunnel: fix ipv4 pmtu check to honor inner ip header df"
+
+- ipsec will crash during xfrm gc
+  Broke since 3.15-rc1
+    commit "flowcache: Make flow cache name space aware"
+  Fixed in 3.18.10, 4.0
+    commit "flowcache: Fix kernel panic in flow_cache_flush_task"
+
+- TSO on GRE tunnels failed, and resulted in very slow performance
+  Broke since 3.14.24, 3.18-rc3
+    commit "gre: Use inner mac length when computing tunnel length"
+  Fixed in 3.14.30, 3.18.4
+    commit "gre: fix the inner mac header in nbma tunnel xmit path"
+    commit "gre: Set inner mac header in gro complete"
+
+- NAPI GRO handling was broken; causing immediate crash (32-bit only?)
+  Broken since 3.13-rc1
+    commit "net: gro: allow to build full sized skb"
+  Fixed 3.14.5, 3.15-rc7
+    commit "net: gro: make sure skb->cb[] initial content has not to be zero"
+
+- ip_gre dst caching broke NBMA GRE tunnels
+  Broken since 3.14-rc1
+  Fixed in 3.14.5, 3.15-rc6
+    commit "ipv4: ip_tunnels: disable cache for nbma gre tunnels"
+
+- Few packets can be lost when neighbor entry is in NUD_PROBE state,
+  and there is continuous traffic to it.
+  Broken since dawn of time
+  Fixed in 3.15-rc1
+    commit "neigh: probe application via netlink in NUD_PROBE"
+
+- GRO was implemented for GRE, but the hw capabilities were not updated
+  correctly. In practice forwarding from non-GRE (physical) interface
+  to GRE interface with gro/gso/tx offloads enabled (also on the target
+  interface) does not work properly.
+  Broken around 3.9 to 3.11, need to check details.
+
+- recvfrom() returned incorrect NBMA address, breaking NAT detection
+  Broken since 3.10-rc1
+    commit "GRE: Refactor GRE tunneling code."
+  Fixed in 3.10.27, 3.12.8, 3.13-rc7
+    commit "ip_gre: fix msg_name parsing for recvfrom/recvmsg"
+
+- sendto() was broken causing opennhrp not work at all
+  Broken since 3.10-rc1
+    commit "GRE: Refactor GRE tunneling code."
+  Fixed in 3.10.12, 3.11-rc6
+    commit "ip_gre: fix ipgre_header to return correct offset"
+
+- PMTU was broken due to GRE driver rewrite
+  Broken since 3.10-rc1
+    commit "GRE: Refactor GRE tunneling code."
+  Fixed in 3.11-rc1
+    commit "ip_tunnels: Use skb-len to PMTU check."
+
+- PMTU was broken due to routing cache removal
+  Broken since 3.6-rc1
+    commit "ipv4: Cache input routes in fib_info nexthops"
+  Fixed in 3.11-rc1
+    commit "ipv4: use next hop exceptions also for input routes"
+    + 3 other commits
+    Patches exist for 3.10, but they were not approved to 3.10-stable.
+
+- Race condition during bootup: changing ARP flag did not flush
+  existing neighbor entries, causing problems if traffic was routed
+  to gre interface before opennhrp was running.
+  Broken since dawn of time
+  Fixed in 3.11-rc1
+    commit "arp: flush arp cache on IFF_NOARP change"
+
+- Crash in IPsec
+  Broken since 3.9-rc1
+    commit "xfrm: removes a superfluous check and add a statistic"
+  Fixed in 3.10-rc3
+    commit "xfrm: properly handle invalid states as an error"
+
+- An incorrect ip_gre change broke NHRP traffic over GRE
+  Broken since 3.8-rc2
+    commit "ip_gre: make ipgre_tunnel_xmit() not parse network header as IP unconditionally"
+  Fixed in 3.8.5, 3.9-rc4
+    commit "Revert "ip_gre: make ipgre_tunnel_xmit() not parse network header as IP unconditionally""
+
+- Multicast traffic over mGRE was broken.
+  Broken since 2.6.34-rc2
+    commit "gre: fix hard header destination address checking"
+  Fixed in 2.6.39-rc2
+    commit "net: gre: provide multicast mappings for ipv4 and ipv6"
+
+- Serious performance issues causing small throughput on medium to large DMVPN networks
+  Broken since dawn of time
+  Fixed in 2.6.35
+    multiple commits rewriting ipsec caching
+
+- Even though around 2.6.24 is the first version where opennhrp started
+  to work, there has been various PMTU, performance, and functionality
+  bugs before 2.6.34. That's one of the first version I consider stable
+  wrt. to opennhrp functionality.
+

nhrpd/README.nhrpd

@@ -0,0 +1,137 @@
+Quagga / NHRP Design and Configuration Notes
+============================================
+
+Quagga/NHRP is an NHRP (RFC2332) implementation for Linux. The primary
+use case is to implement DMVPN. The aim is thus to be compatible with
+Cisco DMVPN (and potentially with FlexVPN in the future).
+
+
+Current Status
+--------------
+
+- IPsec integration with strongSwan (requires patched strongSwan)
+- IPv4 over IPv4 NBMA GRE
+- IPv6 over IPv4 NBMA GRE -- majority of code exist; but is not tested
+- Spoke (NHC) functionality complete
+- Hub (NHS) functionality complete
+- Multicast support is not done yet
+  (so OSPF will not work, use BGP for now)
+
+The code is not (yet) compatible with Cisco FlexVPN style DMVPN. It
+would require relaying IKEv2 routing messages from strongSwan to nhrpd
+and parsing that. It is doable, but not implemented for the time being.
+
+
+Routing Design
+--------------
+
+In contrast to opennhrp routing design, Quagga/NHRP routes each NHRP
+domain address individually (similar to Cisco FlexVPN).
+
+To create NBMA GRE tunnel you might use following:
+	ip tunnel add gre1 mode gre key 42 ttl 64 dev eth0
+	ip addr add 10.255.255.2/32 dev gre1
+	ip link set gre1 up
+
+This has two important differences compared to opennhrp setup:
+ 1. The 'tunnel add' now specifies physical device binding. Quagga/NHRP
+    wants to know stable protocol address to NBMA address mapping. Thus,
+    add 'dev <physdev>' binding, or specify 'local <nbma-address>'. If
+    neither of this is specified, NHRP will not be enabled on the interface.
+    Alternatively you can skip 'dev' binding on tunnel if you allow
+    nhrpd to manage it using 'tunnel source' command (see below).
+
+ 2. The 'addr add' now has host prefix. In opennhrp you would have used
+    the GRE subnet prefix length here instead, e.g. /24.
+
+Quagga/NHRP will automatically create additional host routes pointing to
+gre1 when a connection with these hosts is established. The gre1 subnet
+should be announced by routing protocol. This allows routing protocol
+to decide which is the closest hub and get the gre addresses' traffic.
+
+The second benefit is that hubs can then easily exchange host prefixes
+of directly connected gre addresses. And thus routing of gre addresses
+inside hubs is based on routing protocol's shortest path choice -- not
+on random choice from next hop server list.
+
+
+Configuring nhrpd
+-----------------
+
+The configuration is done using vtysh, and most commands do what they
+do in Cisco. As minimal configuration example one can do:
+ configure terminal
+ interface gre1
+   tunnel protection vici profile dmvpn
+   tunnel source eth0
+   ip nhrp network-id 1
+   ip nhrp shortcut
+   ip nhrp registration no-unique
+   ip nhrp nhs dynamic nbma hubs.example.com
+
+There's important notes about the "ip nhrp nhs" command:
+
+ 1. The 'dynamic' works only against Cisco (or nhrpd), but is not
+    compatible with opennhrp. To use dynamic detection of opennhrp hub's
+    protocol address use the GRE broadcast address there. For the above
+    example of 10.255.255.0/24 the configuration should read instead:
+      ip nhrp nhs 10.255.255.255 nbma hubs.example.com
+
+ 2. nbma <FQDN> works like opennhrp dynamic-map. That is, all of the
+    A-records are configured as NBMA addresses of different hubs, and
+    each hub protocol address will be dynamically detected.
+
+
+Hub functionality
+-----------------
+
+Sending Traffic Indication (redirect) notifications is now accomplished
+using NFLOG.
+
+Use:
+iptables -A FORWARD -i gre1 -o gre1 \
+	-m hashlimit --hashlimit-upto 4/minute --hashlimit-burst 1 \
+	--hashlimit-mode srcip,dstip --hashlimit-srcmask 16 --hashlimit-dstmask 16 \
+	--hashlimit-name loglimit-0 -j NFLOG --nflog-group 1 --nflog-range 128
+
+or similar to get rate-limited samples of the packets that match traffic
+flow needing redirection. This kernel NFLOG target's nflog-group is configured
+in global nhrp config with:
+	nhrp nflog-group 1
+
+To start sending these traffic notices out from hubs, use the nhrp per-interface
+directive:
+	ip nhrp redirect
+
+opennhrp used PF_PACKET and tried to create packet filter to get only
+the packets of interest. Though, this was bad if shortcut fails to
+establish (remote policy, or both are behind NAT or restrictive
+firewalls), all of the relayaed traffic would match always.
+
+
+Getting information via vtysh
+-----------------------------
+
+Some commands of interest:
+ - show dmvpn
+ - show ip nhrp cache
+ - show ip nhrp shortcut
+ - show ip route nhrp
+ - clear ip nhrp cache
+ - clear ip nhrp shortcut
+
+
+Integration with strongSwan
+---------------------------
+
+Contrary to opennhrp, Quagga/NHRP has tight integration with IKE daemon.
+Currently strongSwan is supported using the VICI protocol. strongSwan
+is connected using UNIX socket (hardcoded now as /var/run/charon.vici).
+Thus nhrpd needs to be run as user that can open that file.
+
+Currently, you will need patched strongSwan. The working tree is at:
+	http://git.alpinelinux.org/cgit/user/tteras/strongswan/log/?h=tteras
+
+And the branch with patches against latest release are:
+	http://git.alpinelinux.org/cgit/user/tteras/strongswan/log/?h=tteras-release
+

nhrpd/debug.h

@@ -0,0 +1,42 @@
+#include "log.h"
+
+#if defined(__GNUC__) && (__GNUC__ >= 3)
+#define likely(_x) __builtin_expect(!!(_x), 1)
+#define unlikely(_x) __builtin_expect(!!(_x), 0)
+#else
+#define likely(_x) !!(_x)
+#define unlikely(_x) !!(_x)
+#endif
+
+#define NHRP_DEBUG_COMMON	(1 << 0)
+#define NHRP_DEBUG_KERNEL	(1 << 1)
+#define NHRP_DEBUG_IF		(1 << 2)
+#define NHRP_DEBUG_ROUTE	(1 << 3)
+#define NHRP_DEBUG_VICI		(1 << 4)
+#define NHRP_DEBUG_EVENT	(1 << 5)
+#define NHRP_DEBUG_ALL		(0xFFFF)
+
+extern unsigned int debug_flags;
+
+#if defined __STDC_VERSION__ && __STDC_VERSION__ >= 199901L
+
+#define debugf(level, ...) \
+	do {						\
+		if (unlikely(debug_flags & level))	\
+			zlog_debug(__VA_ARGS__);	\
+	} while(0)
+
+#elif defined __GNUC__
+
+#define debugf(level, _args...)				\
+	do {						\
+		if (unlikely(debug_flags & level))	\
+			zlog_debug(_args);		\
+	} while(0)
+
+#else
+
+static inline void debugf(int level, const char *format, ...) { }
+
+#endif
+

nhrpd/linux.c

@@ -0,0 +1,153 @@
+/* NHRP daemon Linux specific glue
+ * Copyright (c) 2014-2015 Timo Teräs
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <fcntl.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <string.h>
+#include <sys/ioctl.h>
+#include <sys/socket.h>
+#include <sys/types.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 "nhrp_protocol.h"
+#include "os.h"
+#include "netlink.h"
+
+static int nhrp_socket_fd = -1;
+
+int os_socket(void)
+{
+	if (nhrp_socket_fd < 0)
+		nhrp_socket_fd = socket(PF_PACKET, SOCK_DGRAM, htons(ETH_P_NHRP));
+	return nhrp_socket_fd;
+}
+
+int os_sendmsg(const uint8_t *buf, size_t len, int ifindex, const uint8_t *addr, size_t addrlen)
+{
+	struct sockaddr_ll lladdr;
+	struct iovec iov = {
+		.iov_base = (void*) buf,
+		.iov_len = len,
+	};
+	struct msghdr msg = {
+		.msg_name = &lladdr,
+		.msg_namelen = sizeof(lladdr),
+		.msg_iov = &iov,
+		.msg_iovlen = 1,
+	};
+	int status;
+
+	if (addrlen > sizeof(lladdr.sll_addr))
+		return -1;
+
+	memset(&lladdr, 0, sizeof(lladdr));
+	lladdr.sll_family = AF_PACKET;
+	lladdr.sll_protocol = htons(ETH_P_NHRP);
+	lladdr.sll_ifindex = ifindex;
+	lladdr.sll_halen = addrlen;
+	memcpy(lladdr.sll_addr, addr, addrlen);
+
+	status = sendmsg(nhrp_socket_fd, &msg, 0);
+	if (status < 0)
+		return -1;
+
+	return 0;
+}
+
+int os_recvmsg(uint8_t *buf, size_t *len, int *ifindex, uint8_t *addr, size_t *addrlen)
+{
+	struct sockaddr_ll lladdr;
+	struct iovec iov = {
+		.iov_base = buf,
+		.iov_len = *len,
+	};
+	struct msghdr msg = {
+		.msg_name = &lladdr,
+		.msg_namelen = sizeof(lladdr),
+		.msg_iov = &iov,
+		.msg_iovlen = 1,
+	};
+	int r;
+
+	r = recvmsg(nhrp_socket_fd, &msg, MSG_DONTWAIT);
+	if (r < 0)
+		return r;
+
+	*len = r;
+	*ifindex = lladdr.sll_ifindex;
+
+	if (*addrlen <= (size_t) lladdr.sll_addr) {
+		if (memcmp(lladdr.sll_addr, "\x00\x00\x00\x00", 4) != 0) {
+			memcpy(addr, lladdr.sll_addr, lladdr.sll_halen);
+			*addrlen = lladdr.sll_halen;
+		} else {
+			*addrlen = 0;
+		}
+	}
+
+	return 0;
+}
+
+static int linux_configure_arp(const char *iface, int on)
+{
+	struct ifreq ifr;
+
+	strncpy(ifr.ifr_name, iface, IFNAMSIZ);
+	if (ioctl(nhrp_socket_fd, SIOCGIFFLAGS, &ifr))
+		return -1;
+
+	if (on)
+		ifr.ifr_flags &= ~IFF_NOARP;
+	else
+		ifr.ifr_flags |= IFF_NOARP;
+
+	if (ioctl(nhrp_socket_fd, SIOCSIFFLAGS, &ifr))
+		return -1;
+
+	return 0;
+}
+
+static int linux_icmp_redirect_off(const char *iface)
+{
+	char fname[256];
+	int fd, ret = -1;
+
+	sprintf(fname, "/proc/sys/net/ipv4/conf/%s/send_redirects", iface);
+	fd = open(fname, O_WRONLY);
+	if (fd < 0)
+		return -1;
+	if (write(fd, "0\n", 2) == 2)
+		ret = 0;
+	close(fd);
+
+	return ret;
+}
+
+int os_configure_dmvpn(unsigned int ifindex, const char *ifname, int af)
+{
+	int ret = -1;
+
+	switch (af) {
+	case AF_INET:
+		ret  = linux_icmp_redirect_off("all");
+		ret |= linux_icmp_redirect_off(ifname);
+		ret |= netlink_configure_arp(ifindex, AF_INET);
+		ret |= linux_configure_arp(ifname, 1);
+		break;
+	}
+
+	return ret;
+}

nhrpd/list.h

@@ -0,0 +1,191 @@
+/* Linux kernel style list handling function
+ *
+ * Written from scratch by Timo Teräs <timo.teras@iki.fi>, but modeled
+ * after the linux kernel code.
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#ifndef LIST_H
+#define LIST_H
+
+#ifndef NULL
+#define NULL 0L
+#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
+
+#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

nhrpd/netlink.h

@@ -0,0 +1,24 @@
+/* NHRP netlink/neighbor table API
+ * Copyright (c) 2014-2015 Timo Teräs
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <stdint.h>
+
+union sockunion;
+struct interface;
+
+extern int netlink_nflog_group;
+extern int netlink_req_fd;
+
+int netlink_init(void);
+int netlink_configure_arp(unsigned int ifindex, int pf);
+void netlink_update_binding(struct interface *ifp, union sockunion *proto, union sockunion *nbma);
+void netlink_set_nflog_group(int nlgroup);
+
+void netlink_gre_get_info(unsigned int ifindex, uint32_t *gre_key, unsigned int *link_index, struct in_addr *saddr);
+void netlink_gre_set_link(unsigned int ifindex, unsigned int link_index);

nhrpd/netlink_arp.c

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

nhrpd/netlink_gre.c

@@ -0,0 +1,141 @@
+/* NHRP netlink/GRE tunnel configuration code
+ * Copyright (c) 2014-2016 Timo Teräs
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <netinet/in.h>
+#include <linux/netlink.h>
+#include <linux/rtnetlink.h>
+#include <linux/if.h>
+#include <linux/ip.h>
+#include <linux/ipv6.h>
+#include <linux/if_tunnel.h>
+
+#include "debug.h"
+#include "netlink.h"
+#include "znl.h"
+
+static int __netlink_gre_get_data(struct zbuf *zb, struct zbuf *data, int ifindex)
+{
+	struct nlmsghdr *n;
+	struct ifinfomsg *ifi;
+	struct zbuf payload, rtapayload;
+	struct rtattr *rta;
+
+	debugf(NHRP_DEBUG_KERNEL, "netlink-link-gre: get-info %u", ifindex);
+
+	n = znl_nlmsg_push(zb, RTM_GETLINK, NLM_F_REQUEST);
+	ifi = znl_push(zb, sizeof(*ifi));
+	*ifi = (struct ifinfomsg) {
+		.ifi_index = ifindex,
+	};
+	znl_nlmsg_complete(zb, n);
+
+	if (zbuf_send(zb, netlink_req_fd) < 0 ||
+	    zbuf_recv(zb, netlink_req_fd) < 0)
+		return -1;
+
+	n = znl_nlmsg_pull(zb, &payload);
+	if (!n) return -1;
+
+	if (n->nlmsg_type != RTM_NEWLINK)
+		return -1;
+
+	ifi = znl_pull(&payload, sizeof(struct ifinfomsg));
+	if (!ifi)
+		return -1;
+
+	debugf(NHRP_DEBUG_KERNEL, "netlink-link-gre: ifindex %u, receive msg_type %u, msg_flags %u",
+		ifi->ifi_index, n->nlmsg_type, n->nlmsg_flags);
+
+	if (ifi->ifi_index != ifindex)
+		return -1;
+
+	while ((rta = znl_rta_pull(&payload, &rtapayload)) != NULL)
+		if (rta->rta_type == IFLA_LINKINFO)
+			break;
+	if (!rta) return -1;
+
+	payload = rtapayload;
+	while ((rta = znl_rta_pull(&payload, &rtapayload)) != NULL)
+		if (rta->rta_type == IFLA_INFO_DATA)
+			break;
+	if (!rta) return -1;
+
+	*data = rtapayload;
+	return 0;
+}
+
+void netlink_gre_get_info(unsigned int ifindex, uint32_t *gre_key, unsigned int *link_index, struct in_addr *saddr)
+{
+	struct zbuf *zb = zbuf_alloc(8192), data, rtapl;
+	struct rtattr *rta;
+
+	*link_index = 0;
+	*gre_key = 0;
+	saddr->s_addr = 0;
+
+	if (__netlink_gre_get_data(zb, &data, ifindex) < 0)
+		goto err;
+
+	while ((rta = znl_rta_pull(&data, &rtapl)) != NULL) {
+		switch (rta->rta_type) {
+		case IFLA_GRE_LINK:
+			*link_index = zbuf_get32(&rtapl);
+			break;
+		case IFLA_GRE_IKEY:
+		case IFLA_GRE_OKEY:
+			*gre_key = zbuf_get32(&rtapl);
+			break;
+		case IFLA_GRE_LOCAL:
+			saddr->s_addr = zbuf_get32(&rtapl);
+			break;
+		}
+	}
+err:
+	zbuf_free(zb);
+}
+
+void netlink_gre_set_link(unsigned int ifindex, unsigned int link_index)
+{
+	struct nlmsghdr *n;
+	struct ifinfomsg *ifi;
+	struct rtattr *rta_info, *rta_data, *rta;
+	struct zbuf *zr = zbuf_alloc(8192), data, rtapl;
+	struct zbuf *zb = zbuf_alloc(8192);
+	size_t len;
+
+	if (__netlink_gre_get_data(zr, &data, ifindex) < 0)
+		goto err;
+
+	n = znl_nlmsg_push(zb, RTM_NEWLINK, NLM_F_REQUEST);
+	ifi = znl_push(zb, sizeof(*ifi));
+	*ifi = (struct ifinfomsg) {
+		.ifi_index = ifindex,
+	};
+	rta_info = znl_rta_nested_push(zb, IFLA_LINKINFO);
+	znl_rta_push(zb, IFLA_INFO_KIND, "gre", 3);
+	rta_data = znl_rta_nested_push(zb, IFLA_INFO_DATA);
+
+	znl_rta_push_u32(zb, IFLA_GRE_LINK, link_index);
+	while ((rta = znl_rta_pull(&data, &rtapl)) != NULL) {
+		if (rta->rta_type == IFLA_GRE_LINK)
+			continue;
+		len = zbuf_used(&rtapl);
+		znl_rta_push(zb, rta->rta_type, zbuf_pulln(&rtapl, len), len);
+	}
+
+	znl_rta_nested_complete(zb, rta_data);
+	znl_rta_nested_complete(zb, rta_info);
+
+	znl_nlmsg_complete(zb, n);
+	zbuf_send(zb, netlink_req_fd);
+	zbuf_recv(zb, netlink_req_fd);
+err:
+	zbuf_free(zb);
+	zbuf_free(zr);
+}

nhrpd/nhrp_cache.c

@@ -0,0 +1,341 @@
+/* NHRP cache
+ * Copyright (c) 2014-2015 Timo Teräs
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include "zebra.h"
+#include "memory.h"
+#include "thread.h"
+#include "hash.h"
+#include "nhrpd.h"
+
+#include "netlink.h"
+
+unsigned long nhrp_cache_counts[NHRP_CACHE_NUM_TYPES];
+
+const char * const nhrp_cache_type_str[] = {
+	[NHRP_CACHE_INVALID]	= "invalid",
+	[NHRP_CACHE_INCOMPLETE]	= "incomplete",
+	[NHRP_CACHE_NEGATIVE]	= "negative",
+	[NHRP_CACHE_CACHED]	= "cached",
+	[NHRP_CACHE_DYNAMIC]	= "dynamic",
+	[NHRP_CACHE_NHS]	= "nhs",
+	[NHRP_CACHE_STATIC]	= "static",
+	[NHRP_CACHE_LOCAL]	= "local",
+};
+
+static unsigned int nhrp_cache_protocol_key(void *peer_data)
+{
+	struct nhrp_cache *p = peer_data;
+	return sockunion_hash(&p->remote_addr);
+}
+
+static int nhrp_cache_protocol_cmp(const void *cache_data, const void *key_data)
+{
+	const struct nhrp_cache *a = cache_data;
+	const struct nhrp_cache *b = key_data;
+	return sockunion_same(&a->remote_addr, &b->remote_addr);
+}
+
+static void *nhrp_cache_alloc(void *data)
+{
+	struct nhrp_cache *p, *key = data;
+
+	p = XMALLOC(MTYPE_NHRP_CACHE, sizeof(struct nhrp_cache));
+	if (p) {
+		*p = (struct nhrp_cache) {
+			.cur.type = NHRP_CACHE_INVALID,
+			.new.type = NHRP_CACHE_INVALID,
+			.remote_addr = key->remote_addr,
+			.ifp = key->ifp,
+			.notifier_list = NOTIFIER_LIST_INITIALIZER(&p->notifier_list),
+		};
+		nhrp_cache_counts[p->cur.type]++;
+	}
+
+	return p;
+}
+
+static void nhrp_cache_free(struct nhrp_cache *c)
+{
+	struct nhrp_interface *nifp = c->ifp->info;
+
+	zassert(c->cur.type == NHRP_CACHE_INVALID && c->cur.peer == NULL);
+	zassert(c->new.type == NHRP_CACHE_INVALID && c->new.peer == NULL);
+	nhrp_cache_counts[c->cur.type]--;
+	notifier_call(&c->notifier_list, NOTIFY_CACHE_DELETE);
+	zassert(!notifier_active(&c->notifier_list));
+	hash_release(nifp->cache_hash, c);
+	XFREE(MTYPE_NHRP_CACHE, c);
+}
+
+struct nhrp_cache *nhrp_cache_get(struct interface *ifp, union sockunion *remote_addr, int create)
+{
+	struct nhrp_interface *nifp = ifp->info;
+	struct nhrp_cache key;
+
+	if (!nifp->cache_hash) {
+		nifp->cache_hash = hash_create(nhrp_cache_protocol_key, nhrp_cache_protocol_cmp);
+		if (!nifp->cache_hash)
+			return NULL;
+	}
+
+	key.remote_addr = *remote_addr;
+	key.ifp = ifp;
+
+	return hash_get(nifp->cache_hash, &key, create ? nhrp_cache_alloc : NULL);
+}
+
+static int nhrp_cache_do_free(struct thread *t)
+{
+	struct nhrp_cache *c = THREAD_ARG(t);
+	c->t_timeout = NULL;
+	nhrp_cache_free(c);
+	return 0;
+}
+
+static int nhrp_cache_do_timeout(struct thread *t)
+{
+	struct nhrp_cache *c = THREAD_ARG(t);
+	c->t_timeout = NULL;
+	if (c->cur.type != NHRP_CACHE_INVALID)
+		nhrp_cache_update_binding(c, c->cur.type, -1, NULL, 0, NULL);
+	return 0;
+}
+
+static void nhrp_cache_update_route(struct nhrp_cache *c)
+{
+	struct prefix pfx;
+	struct nhrp_peer *p = c->cur.peer;
+
+	sockunion2hostprefix(&c->remote_addr, &pfx);
+
+	if (p && nhrp_peer_check(p, 1)) {
+		netlink_update_binding(p->ifp, &c->remote_addr, &p->vc->remote.nbma);
+		nhrp_route_announce(1, c->cur.type, &pfx, c->ifp, NULL, c->cur.mtu);
+		if (c->cur.type >= NHRP_CACHE_DYNAMIC) {
+			nhrp_route_update_nhrp(&pfx, c->ifp);
+			c->nhrp_route_installed = 1;
+		} else if (c->nhrp_route_installed) {
+			nhrp_route_update_nhrp(&pfx, NULL);
+			c->nhrp_route_installed = 0;
+		}
+		if (!c->route_installed) {
+			notifier_call(&c->notifier_list, NOTIFY_CACHE_UP);
+			c->route_installed = 1;
+		}
+	} else {
+		if (c->nhrp_route_installed) {
+			nhrp_route_update_nhrp(&pfx, NULL);
+			c->nhrp_route_installed = 0;
+		}
+		if (c->route_installed) {
+			sockunion2hostprefix(&c->remote_addr, &pfx);
+			notifier_call(&c->notifier_list, NOTIFY_CACHE_DOWN);
+			nhrp_route_announce(0, c->cur.type, &pfx, NULL, NULL, 0);
+			c->route_installed = 0;
+		}
+	}
+}
+
+static void nhrp_cache_peer_notifier(struct notifier_block *n, unsigned long cmd)
+{
+	struct nhrp_cache *c = container_of(n, struct nhrp_cache, peer_notifier);
+
+	switch (cmd) {
+	case NOTIFY_PEER_UP:
+		nhrp_cache_update_route(c);
+		break;
+	case NOTIFY_PEER_DOWN:
+	case NOTIFY_PEER_IFCONFIG_CHANGED:
+		notifier_call(&c->notifier_list, NOTIFY_CACHE_DOWN);
+		nhrp_cache_update_binding(c, c->cur.type, -1, NULL, 0, NULL);
+		break;
+	case NOTIFY_PEER_NBMA_CHANGING:
+		if (c->cur.type == NHRP_CACHE_DYNAMIC)
+			c->cur.peer->vc->abort_migration = 1;
+		break;
+	}
+}
+
+static void nhrp_cache_reset_new(struct nhrp_cache *c)
+{
+	THREAD_OFF(c->t_auth);
+	if (list_hashed(&c->newpeer_notifier.notifier_entry))
+		nhrp_peer_notify_del(c->new.peer, &c->newpeer_notifier);
+	nhrp_peer_unref(c->new.peer);
+	memset(&c->new, 0, sizeof(c->new));
+	c->new.type = NHRP_CACHE_INVALID;
+}
+
+static void nhrp_cache_update_timers(struct nhrp_cache *c)
+{
+	THREAD_OFF(c->t_timeout);
+
+	switch (c->cur.type) {
+	case NHRP_CACHE_INVALID:
+		if (!c->t_auth)
+			THREAD_TIMER_MSEC_ON(master, c->t_timeout, nhrp_cache_do_free, c, 10);
+		break;
+	default:
+		if (c->cur.expires)
+			THREAD_TIMER_ON(master, c->t_timeout, nhrp_cache_do_timeout, c, c->cur.expires - recent_relative_time().tv_sec);
+		break;
+	}
+}
+
+static void nhrp_cache_authorize_binding(struct nhrp_reqid *r, void *arg)
+{
+	struct nhrp_cache *c = container_of(r, struct nhrp_cache, eventid);
+	char buf[SU_ADDRSTRLEN];
+
+	debugf(NHRP_DEBUG_COMMON, "cache: %s %s: %s",
+		c->ifp->name, sockunion2str(&c->remote_addr, buf, sizeof buf),
+		(const char *) arg);
+
+	nhrp_reqid_free(&nhrp_event_reqid, r);
+
+	if (arg && strcmp(arg, "accept") == 0) {
+		if (c->cur.peer) {
+			netlink_update_binding(c->cur.peer->ifp, &c->remote_addr, NULL);
+			nhrp_peer_notify_del(c->cur.peer, &c->peer_notifier);
+			nhrp_peer_unref(c->cur.peer);
+		}
+		nhrp_cache_counts[c->cur.type]--;
+		nhrp_cache_counts[c->new.type]++;
+		c->cur = c->new;
+		c->cur.peer = nhrp_peer_ref(c->cur.peer);
+		nhrp_cache_reset_new(c);
+		if (c->cur.peer)
+			nhrp_peer_notify_add(c->cur.peer, &c->peer_notifier, nhrp_cache_peer_notifier);
+		nhrp_cache_update_route(c);
+		notifier_call(&c->notifier_list, NOTIFY_CACHE_BINDING_CHANGE);
+	} else {
+		nhrp_cache_reset_new(c);
+	}
+
+	nhrp_cache_update_timers(c);
+}
+
+static int nhrp_cache_do_auth_timeout(struct thread *t)
+{
+	struct nhrp_cache *c = THREAD_ARG(t);
+	c->t_auth = NULL;
+	nhrp_cache_authorize_binding(&c->eventid, (void *) "timeout");
+	return 0;
+}
+
+static void nhrp_cache_newpeer_notifier(struct notifier_block *n, unsigned long cmd)
+{
+	struct nhrp_cache *c = container_of(n, struct nhrp_cache, newpeer_notifier);
+
+	switch (cmd) {
+	case NOTIFY_PEER_UP:
+		if (nhrp_peer_check(c->new.peer, 1)) {
+			evmgr_notify("authorize-binding", c, nhrp_cache_authorize_binding);
+			THREAD_TIMER_ON(master, c->t_auth, nhrp_cache_do_auth_timeout, c, 10);
+		}
+		break;
+	case NOTIFY_PEER_DOWN:
+	case NOTIFY_PEER_IFCONFIG_CHANGED:
+		nhrp_cache_reset_new(c);
+		break;
+	}
+}
+
+int nhrp_cache_update_binding(struct nhrp_cache *c, enum nhrp_cache_type type, int holding_time, struct nhrp_peer *p, uint32_t mtu, union sockunion *nbma_oa)
+{
+	if (c->cur.type > type || c->new.type > type) {
+		nhrp_peer_unref(p);
+		return 0;
+	}
+
+	/* Sanitize MTU */
+	switch (sockunion_family(&c->remote_addr)) {
+	case AF_INET:
+		if (mtu < 576 || mtu >= 1500)
+			mtu = 0;
+		/* Opennhrp announces nbma mtu, but we use protocol mtu.
+		 * This heuristic tries to fix up it. */
+		if (mtu > 1420) mtu = (mtu & -16) - 80;
+		break;
+	default:
+		mtu = 0;
+		break;
+	}
+
+	nhrp_cache_reset_new(c);
+	if (c->cur.type == type && c->cur.peer == p && c->cur.mtu == mtu) {
+		if (holding_time > 0) c->cur.expires = recent_relative_time().tv_sec + holding_time;
+		if (nbma_oa) c->cur.remote_nbma_natoa = *nbma_oa;
+		else memset(&c->cur.remote_nbma_natoa, 0, sizeof c->cur.remote_nbma_natoa);
+		nhrp_peer_unref(p);
+	} else {
+		c->new.type = type;
+		c->new.peer = p;
+		c->new.mtu = mtu;
+		if (nbma_oa) c->new.remote_nbma_natoa = *nbma_oa;
+
+		if (holding_time > 0)
+			c->new.expires = recent_relative_time().tv_sec + holding_time;
+		else if (holding_time < 0)
+			c->new.type = NHRP_CACHE_INVALID;
+
+		if (c->new.type == NHRP_CACHE_INVALID ||
+		    c->new.type >= NHRP_CACHE_STATIC ||
+		    c->map) {
+			nhrp_cache_authorize_binding(&c->eventid, (void *) "accept");
+		} else {
+			nhrp_peer_notify_add(c->new.peer, &c->newpeer_notifier, nhrp_cache_newpeer_notifier);
+			nhrp_cache_newpeer_notifier(&c->newpeer_notifier, NOTIFY_PEER_UP);
+			THREAD_TIMER_ON(master, c->t_auth, nhrp_cache_do_auth_timeout, c, 60);
+		}
+	}
+	nhrp_cache_update_timers(c);
+
+	return 1;
+}
+
+void nhrp_cache_set_used(struct nhrp_cache *c, int used)
+{
+	c->used = used;
+	if (c->used)
+		notifier_call(&c->notifier_list, NOTIFY_CACHE_USED);
+}
+
+struct nhrp_cache_iterator_ctx {
+	void (*cb)(struct nhrp_cache *, void *);
+	void *ctx;
+};
+
+static void nhrp_cache_iterator(struct hash_backet *b, void *ctx)
+{
+	struct nhrp_cache_iterator_ctx *ic = ctx;
+	ic->cb(b->data, ic->ctx);
+}
+
+void nhrp_cache_foreach(struct interface *ifp, void (*cb)(struct nhrp_cache *, void *), void *ctx)
+{
+	struct nhrp_interface *nifp = ifp->info;
+	struct nhrp_cache_iterator_ctx ic = {
+		.cb = cb,
+		.ctx = ctx,
+	};
+
+	if (nifp->cache_hash)
+		hash_iterate(nifp->cache_hash, nhrp_cache_iterator, &ic);
+}
+
+void nhrp_cache_notify_add(struct nhrp_cache *c, struct notifier_block *n, notifier_fn_t fn)
+{
+	notifier_add(n, &c->notifier_list, fn);
+}
+
+void nhrp_cache_notify_del(struct nhrp_cache *c, struct notifier_block *n)
+{
+	notifier_del(n);
+}

nhrpd/nhrp_event.c

@@ -0,0 +1,280 @@
+/* NHRP event manager
+ * Copyright (c) 2014-2015 Timo Teräs
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <string.h>
+#include <sys/socket.h>
+#include <sys/un.h>
+
+#include "thread.h"
+#include "zbuf.h"
+#include "log.h"
+#include "nhrpd.h"
+
+const char *nhrp_event_socket_path;
+struct nhrp_reqid_pool nhrp_event_reqid;
+
+struct event_manager {
+	struct thread *t_reconnect, *t_read, *t_write;
+	struct zbuf ibuf;
+	struct zbuf_queue obuf;
+	int fd;
+	uint8_t ibuf_data[4*1024];
+};
+
+static int evmgr_reconnect(struct thread *t);
+
+static void evmgr_connection_error(struct event_manager *evmgr)
+{
+	THREAD_OFF(evmgr->t_read);
+	THREAD_OFF(evmgr->t_write);
+	zbuf_reset(&evmgr->ibuf);
+	zbufq_reset(&evmgr->obuf);
+
+	if (evmgr->fd >= 0)
+		close(evmgr->fd);
+	evmgr->fd = -1;
+	if (nhrp_event_socket_path)
+		THREAD_TIMER_MSEC_ON(master, evmgr->t_reconnect, evmgr_reconnect,
+				     evmgr, 10);
+}
+
+static void evmgr_recv_message(struct event_manager *evmgr, struct zbuf *zb)
+{
+	struct zbuf zl;
+	uint32_t eventid = 0;
+	size_t len;
+	char buf[256], result[64] = "";
+
+	while (zbuf_may_pull_until(zb, "\n", &zl)) {
+		len = zbuf_used(&zl) - 1;
+		if (len >= sizeof(buf)-1)
+			continue;
+		memcpy(buf, zbuf_pulln(&zl, len), len);
+		buf[len] = 0;
+
+		debugf(NHRP_DEBUG_EVENT, "evmgr: msg: %s", buf);
+		sscanf(buf, "eventid=%d", &eventid);
+		sscanf(buf, "result=%63s", result);
+	}
+	debugf(NHRP_DEBUG_EVENT, "evmgr: received: eventid=%d result=%s", eventid, result);
+	if (eventid && result[0]) {
+		struct nhrp_reqid *r = nhrp_reqid_lookup(&nhrp_event_reqid, eventid);
+		if (r) r->cb(r, result);
+	}
+}
+
+static int evmgr_read(struct thread *t)
+{
+	struct event_manager *evmgr = THREAD_ARG(t);
+	struct zbuf *ibuf = &evmgr->ibuf;
+	struct zbuf msg;
+
+	evmgr->t_read = NULL;
+	if (zbuf_read(ibuf, evmgr->fd, (size_t) -1) < 0) {
+		evmgr_connection_error(evmgr);
+		return 0;
+	}
+
+	/* Process all messages in buffer */
+	while (zbuf_may_pull_until(ibuf, "\n\n", &msg))
+		evmgr_recv_message(evmgr, &msg);
+
+	THREAD_READ_ON(master, evmgr->t_read, evmgr_read, evmgr, evmgr->fd);
+	return 0;
+}
+
+static int evmgr_write(struct thread *t)
+{
+	struct event_manager *evmgr = THREAD_ARG(t);
+	int r;
+
+	evmgr->t_write = NULL;
+	r = zbufq_write(&evmgr->obuf, evmgr->fd);
+	if (r > 0) {
+		THREAD_WRITE_ON(master, evmgr->t_write, evmgr_write, evmgr, evmgr->fd);
+	} else if (r < 0) {
+		evmgr_connection_error(evmgr);
+	}
+
+	return 0;
+}
+
+static void evmgr_hexdump(struct zbuf *zb, const uint8_t *val, size_t vallen)
+{
+	static const char xd[] = "0123456789abcdef";
+	size_t i;
+	char *ptr;
+
+	ptr  = zbuf_pushn(zb, 2*vallen);
+	if (!ptr) return;
+
+	for (i = 0; i < vallen; i++) {
+		uint8_t b = val[i];
+		*(ptr++) = xd[b >> 4];
+		*(ptr++) = xd[b & 0xf];
+	}
+}
+
+static void evmgr_put(struct zbuf *zb, const char *fmt, ...)
+{
+	const char *pos, *nxt, *str;
+	const uint8_t *bin;
+	const union sockunion *su;
+	int len;
+	va_list va;
+
+	va_start(va, fmt);
+	for (pos = fmt; (nxt = strchr(pos, '%')) != NULL; pos = nxt + 2) {
+		zbuf_put(zb, pos, nxt-pos);
+		switch (nxt[1]) {
+		case '%':
+			zbuf_put8(zb, '%');
+			break;
+		case 'u':
+			zb->tail += snprintf((char *) zb->tail, zbuf_tailroom(zb), "%u", va_arg(va, uint32_t));
+			break;
+		case 's':
+			str = va_arg(va, const char *);
+			zbuf_put(zb, str, strlen(str));
+			break;
+		case 'U':
+			su = va_arg(va, const union sockunion *);
+			if (sockunion2str(su, (char *) zb->tail, zbuf_tailroom(zb)))
+				zb->tail += strlen((char *) zb->tail);
+			else
+				zbuf_set_werror(zb);
+			break;
+		case 'H':
+			bin = va_arg(va, const uint8_t *);
+			len = va_arg(va, int);
+			evmgr_hexdump(zb, bin, len);
+			break;
+		}
+	}
+	va_end(va);
+	zbuf_put(zb, pos, strlen(pos));
+}
+
+static void evmgr_submit(struct event_manager *evmgr, struct zbuf *obuf)
+{
+	if (obuf->error) {
+		zbuf_free(obuf);
+		return;
+	}
+	zbuf_put(obuf, "\n", 1);
+	zbufq_queue(&evmgr->obuf, obuf);
+	if (evmgr->fd >= 0)
+		THREAD_WRITE_ON(master, evmgr->t_write, evmgr_write, evmgr, evmgr->fd);
+}
+
+static int evmgr_reconnect(struct thread *t)
+{
+	struct event_manager *evmgr = THREAD_ARG(t);
+	int fd;
+
+	evmgr->t_reconnect = NULL;
+	if (evmgr->fd >= 0 || !nhrp_event_socket_path) return 0;
+
+	fd = sock_open_unix(nhrp_event_socket_path);
+	if (fd < 0) {
+		zlog_warn("%s: failure connecting nhrp-event socket: %s",
+			__PRETTY_FUNCTION__, strerror(errno));
+		zbufq_reset(&evmgr->obuf);
+		THREAD_TIMER_ON(master, evmgr->t_reconnect, evmgr_reconnect, evmgr, 10);
+		return 0;
+	}
+
+	zlog_info("Connected to Event Manager");
+	evmgr->fd = fd;
+	THREAD_READ_ON(master, evmgr->t_read, evmgr_read, evmgr, evmgr->fd);
+
+	return 0;
+}
+
+static struct event_manager evmgr_connection;
+
+void evmgr_init(void)
+{
+	struct event_manager *evmgr = &evmgr_connection;
+
+	evmgr->fd = -1;
+	zbuf_init(&evmgr->ibuf, evmgr->ibuf_data, sizeof(evmgr->ibuf_data), 0);
+	zbufq_init(&evmgr->obuf);
+	THREAD_TIMER_MSEC_ON(master, evmgr->t_reconnect, evmgr_reconnect, evmgr, 10);
+}
+
+void evmgr_set_socket(const char *socket)
+{
+	if (nhrp_event_socket_path)
+		free((char *) nhrp_event_socket_path);
+	nhrp_event_socket_path = strdup(socket);
+	evmgr_connection_error(&evmgr_connection);
+}
+
+void evmgr_terminate(void)
+{
+}
+
+void evmgr_notify(const char *name, struct nhrp_cache *c, void (*cb)(struct nhrp_reqid *, void *))
+{
+	struct event_manager *evmgr = &evmgr_connection;
+	struct nhrp_vc *vc;
+	struct nhrp_interface *nifp = c->ifp->info;
+	struct zbuf *zb;
+	afi_t afi = family2afi(sockunion_family(&c->remote_addr));
+
+	if (!nhrp_event_socket_path) {
+		cb(&c->eventid, (void*) "accept");
+		return;
+	}
+
+	debugf(NHRP_DEBUG_EVENT, "evmgr: sending event %s", name);
+
+	vc = c->new.peer ? c->new.peer->vc : NULL;
+	zb = zbuf_alloc(1024 + (vc ? (vc->local.certlen + vc->remote.certlen) * 2 : 0));
+
+	if (cb) {
+		nhrp_reqid_free(&nhrp_event_reqid, &c->eventid);
+		evmgr_put(zb,
+			"eventid=%u\n",
+			nhrp_reqid_alloc(&nhrp_event_reqid, &c->eventid, cb));
+	}
+
+	evmgr_put(zb,
+		"event=%s\n"
+		"type=%s\n"
+		"old_type=%s\n"
+		"num_nhs=%u\n"
+		"interface=%s\n"
+		"local_addr=%U\n",
+		name,
+		nhrp_cache_type_str[c->new.type],
+		nhrp_cache_type_str[c->cur.type],
+		(unsigned int) nhrp_cache_counts[NHRP_CACHE_NHS],
+		c->ifp->name,
+		&nifp->afi[afi].addr);
+
+	if (vc) {
+		evmgr_put(zb,
+			"vc_initiated=%s\n"
+			"local_nbma=%U\n"
+			"local_cert=%H\n"
+			"remote_addr=%U\n"
+			"remote_nbma=%U\n"
+			"remote_cert=%H\n",
+			c->new.peer->requested ? "yes" : "no",
+			&vc->local.nbma,
+			vc->local.cert, vc->local.certlen,
+			&c->remote_addr, &vc->remote.nbma,
+			vc->remote.cert, vc->remote.certlen);
+	}
+
+	evmgr_submit(evmgr, zb);
+}
+

nhrpd/nhrp_interface.c

@@ -0,0 +1,404 @@
+/* NHRP interface
+ * Copyright (c) 2014-2015 Timo Teräs
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <net/if_arp.h>
+#include "zebra.h"
+#include "linklist.h"
+#include "memory.h"
+#include "thread.h"
+
+#include "nhrpd.h"
+#include "os.h"
+#include "netlink.h"
+
+static int nhrp_if_new_hook(struct interface *ifp)
+{
+	struct nhrp_interface *nifp;
+	afi_t afi;
+
+	nifp = XCALLOC(MTYPE_NHRP_IF, sizeof(struct nhrp_interface));
+	if (!nifp) return 0;
+
+	ifp->info = nifp;
+	nifp->ifp = ifp;
+
+	notifier_init(&nifp->notifier_list);
+	for (afi = 0; afi < AFI_MAX; afi++) {
+		struct nhrp_afi_data *ad = &nifp->afi[afi];
+		ad->holdtime = NHRPD_DEFAULT_HOLDTIME;
+		list_init(&ad->nhslist_head);
+	}
+
+	return 0;
+}
+
+static int nhrp_if_delete_hook(struct interface *ifp)
+{
+	XFREE(MTYPE_NHRP_IF, ifp->info);
+	return 0;
+}
+
+void nhrp_interface_init(void)
+{
+	if_add_hook(IF_NEW_HOOK,    nhrp_if_new_hook);
+	if_add_hook(IF_DELETE_HOOK, nhrp_if_delete_hook);
+}
+
+void nhrp_interface_update_mtu(struct interface *ifp, afi_t afi)
+{
+	struct nhrp_interface *nifp = ifp->info;
+	struct nhrp_afi_data *if_ad = &nifp->afi[afi];
+	unsigned short new_mtu;
+
+	if (if_ad->configured_mtu < 0)
+		new_mtu = nifp->nbmaifp ? nifp->nbmaifp->mtu : 0;
+	else
+		new_mtu = if_ad->configured_mtu;
+	if (new_mtu >= 1500)
+		new_mtu = 0;
+
+	if (new_mtu != if_ad->mtu) {
+		debugf(NHRP_DEBUG_IF, "%s: MTU changed to %d", ifp->name, new_mtu);
+		if_ad->mtu = new_mtu;
+		notifier_call(&nifp->notifier_list, NOTIFY_INTERFACE_MTU_CHANGED);
+	}
+}
+
+static void nhrp_interface_update_source(struct interface *ifp)
+{
+	struct nhrp_interface *nifp = ifp->info;
+
+	if (!nifp->source || !nifp->nbmaifp ||
+	    nifp->linkidx == nifp->nbmaifp->ifindex)
+		return;
+
+	nifp->linkidx = nifp->nbmaifp->ifindex;
+	debugf(NHRP_DEBUG_IF, "%s: bound device index changed to %d", ifp->name, nifp->linkidx);
+	netlink_gre_set_link(ifp->ifindex, nifp->linkidx);
+}
+
+static void nhrp_interface_interface_notifier(struct notifier_block *n, unsigned long cmd)
+{
+	struct nhrp_interface *nifp = container_of(n, struct nhrp_interface, nbmanifp_notifier);
+	struct interface *nbmaifp = nifp->nbmaifp;
+	struct nhrp_interface *nbmanifp = nbmaifp->info;
+	char buf[SU_ADDRSTRLEN];
+
+	switch (cmd) {
+	case NOTIFY_INTERFACE_CHANGED:
+		nhrp_interface_update_mtu(nifp->ifp, AFI_IP);
+		nhrp_interface_update_source(nifp->ifp);
+		break;
+	case NOTIFY_INTERFACE_ADDRESS_CHANGED:
+		nifp->nbma = nbmanifp->afi[AFI_IP].addr;
+		nhrp_interface_update(nifp->ifp);
+		notifier_call(&nifp->notifier_list, NOTIFY_INTERFACE_NBMA_CHANGED);
+		debugf(NHRP_DEBUG_IF, "%s: NBMA change: address %s",
+			nifp->ifp->name,
+			sockunion2str(&nifp->nbma, buf, sizeof buf));
+		break;
+	}
+}
+
+static void nhrp_interface_update_nbma(struct interface *ifp)
+{
+	struct nhrp_interface *nifp = ifp->info, *nbmanifp = NULL;
+	struct interface *nbmaifp = NULL;
+	union sockunion nbma;
+
+	sockunion_family(&nbma) = AF_UNSPEC;
+
+	if (nifp->source)
+		nbmaifp = if_lookup_by_name(nifp->source);
+
+	switch (ifp->ll_type) {
+	case ZEBRA_LLT_IPGRE: {
+			struct in_addr saddr = {0};
+			netlink_gre_get_info(ifp->ifindex, &nifp->grekey, &nifp->linkidx, &saddr);
+			debugf(NHRP_DEBUG_IF, "%s: GRE: %x %x %x", ifp->name, nifp->grekey, nifp->linkidx, saddr.s_addr);
+			if (saddr.s_addr)
+				sockunion_set(&nbma, AF_INET, (u_char *) &saddr.s_addr, sizeof(saddr.s_addr));
+			else if (!nbmaifp && nifp->linkidx != IFINDEX_INTERNAL)
+				nbmaifp = if_lookup_by_index(nifp->linkidx);
+		}
+		break;
+	default:
+		break;
+	}
+
+	if (nbmaifp)
+		nbmanifp = nbmaifp->info;
+
+	if (nbmaifp != nifp->nbmaifp) {
+		if (nifp->nbmaifp)
+			notifier_del(&nifp->nbmanifp_notifier);
+		nifp->nbmaifp = nbmaifp;
+		if (nbmaifp) {
+			notifier_add(&nifp->nbmanifp_notifier, &nbmanifp->notifier_list, nhrp_interface_interface_notifier);
+			debugf(NHRP_DEBUG_IF, "%s: bound to %s", ifp->name, nbmaifp->name);
+		}
+	}
+
+	if (nbmaifp) {
+		if (sockunion_family(&nbma) == AF_UNSPEC)
+			nbma = nbmanifp->afi[AFI_IP].addr;
+		nhrp_interface_update_mtu(ifp, AFI_IP);
+		nhrp_interface_update_source(ifp);
+	}
+
+	if (!sockunion_same(&nbma, &nifp->nbma)) {
+		nifp->nbma = nbma;
+		nhrp_interface_update(nifp->ifp);
+		debugf(NHRP_DEBUG_IF, "%s: NBMA address changed", ifp->name);
+		notifier_call(&nifp->notifier_list, NOTIFY_INTERFACE_NBMA_CHANGED);
+	}
+
+	nhrp_interface_update(ifp);
+}
+
+static void nhrp_interface_update_address(struct interface *ifp, afi_t afi, int force)
+{
+	const int family = afi2family(afi);
+	struct nhrp_interface *nifp = ifp->info;
+	struct nhrp_afi_data *if_ad = &nifp->afi[afi];
+	struct nhrp_cache *nc;
+	struct connected *c, *best;
+	struct listnode *cnode;
+	union sockunion addr;
+	char buf[PREFIX_STRLEN];
+
+	/* Select new best match preferring primary address */
+	best = NULL;
+	for (ALL_LIST_ELEMENTS_RO(ifp->connected, cnode, c)) {
+		if (PREFIX_FAMILY(c->address) != family)
+			continue;
+		if (best == NULL) {
+			best = c;
+			continue;
+		}
+		if ((best->flags & ZEBRA_IFA_SECONDARY) && !(c->flags & ZEBRA_IFA_SECONDARY)) {
+			best = c;
+			continue;
+		}
+		if (!(best->flags & ZEBRA_IFA_SECONDARY) && (c->flags & ZEBRA_IFA_SECONDARY))
+			continue;
+		if (best->address->prefixlen > c->address->prefixlen) {
+			best = c;
+			continue;
+		}
+		if (best->address->prefixlen < c->address->prefixlen)
+			continue;
+	}
+
+	/* On NHRP interfaces a host prefix is required */
+	if (best && if_ad->configured && best->address->prefixlen != 8 * prefix_blen(best->address)) {
+		zlog_notice("%s: %s is not a host prefix", ifp->name,
+			prefix2str(best->address, buf, sizeof buf));
+		best = NULL;
+	}
+
+	/* Update address if it changed */
+	if (best)
+		prefix2sockunion(best->address, &addr);
+	else
+		memset(&addr, 0, sizeof(addr));
+
+	if (!force && sockunion_same(&if_ad->addr, &addr))
+		return;
+
+	if (sockunion_family(&if_ad->addr) != AF_UNSPEC) {
+		nc = nhrp_cache_get(ifp, &if_ad->addr, 0);
+		if (nc) nhrp_cache_update_binding(nc, NHRP_CACHE_LOCAL, -1, NULL, 0, NULL);
+	}
+
+	debugf(NHRP_DEBUG_KERNEL, "%s: IPv%d address changed to %s",
+		ifp->name, afi == AFI_IP ? 4 : 6,
+		best ? prefix2str(best->address, buf, sizeof buf) : "(none)");
+	if_ad->addr = addr;
+
+	if (if_ad->configured && sockunion_family(&if_ad->addr) != AF_UNSPEC) {
+		nc = nhrp_cache_get(ifp, &addr, 1);
+		if (nc) nhrp_cache_update_binding(nc, NHRP_CACHE_LOCAL, 0, NULL, 0, NULL);
+	}
+
+	notifier_call(&nifp->notifier_list, NOTIFY_INTERFACE_ADDRESS_CHANGED);
+}
+
+void nhrp_interface_update(struct interface *ifp)
+{
+	struct nhrp_interface *nifp = ifp->info;
+	struct nhrp_afi_data *if_ad;
+	afi_t afi;
+	int enabled = 0;
+
+	notifier_call(&nifp->notifier_list, NOTIFY_INTERFACE_CHANGED);
+
+	for (afi = 0; afi < AFI_MAX; afi++) {
+		if_ad = &nifp->afi[afi];
+
+		if (sockunion_family(&nifp->nbma) == AF_UNSPEC ||
+		    ifp->ifindex == IFINDEX_INTERNAL || !if_is_up(ifp) ||
+		    !if_ad->network_id) {
+			if (if_ad->configured) {
+				if_ad->configured = 0;
+				nhrp_interface_update_address(ifp, afi, 1);
+			}
+			continue;
+		}
+
+		if (!if_ad->configured) {
+			os_configure_dmvpn(ifp->ifindex, ifp->name, afi2family(afi));
+			if_ad->configured = 1;
+			nhrp_interface_update_address(ifp, afi, 1);
+		}
+
+		enabled = 1;
+	}
+
+	if (enabled != nifp->enabled) {
+		nifp->enabled = enabled;
+		notifier_call(&nifp->notifier_list, enabled ? NOTIFY_INTERFACE_UP : NOTIFY_INTERFACE_DOWN);
+	}
+}
+
+int nhrp_interface_add(int cmd, struct zclient *client, zebra_size_t length, vrf_id_t vrf_id)
+{
+	struct interface *ifp;
+
+	/* read and add the interface in the iflist. */
+	ifp = zebra_interface_add_read(client->ibuf, vrf_id);
+	if (ifp == NULL)
+		return 0;
+
+	debugf(NHRP_DEBUG_IF, "if-add: %s, ifindex: %u, hw_type: %d %s",
+		ifp->name, ifp->ifindex,
+		ifp->ll_type, if_link_type_str(ifp->ll_type));
+
+	nhrp_interface_update_nbma(ifp);
+
+	return 0;
+}
+
+int nhrp_interface_delete(int cmd, struct zclient *client,
+			  zebra_size_t length, vrf_id_t vrf_id)
+{
+	struct interface *ifp;
+	struct stream *s;
+
+	s = client->ibuf;
+	ifp = zebra_interface_state_read(s, vrf_id);
+	if (ifp == NULL)
+		return 0;
+
+	debugf(NHRP_DEBUG_IF, "if-delete: %s", ifp->name);
+	ifp->ifindex = IFINDEX_INTERNAL;
+	nhrp_interface_update(ifp);
+	/* if_delete(ifp); */
+	return 0;
+}
+
+int nhrp_interface_up(int cmd, struct zclient *client,
+		      zebra_size_t length, vrf_id_t vrf_id)
+{
+	struct interface *ifp;
+
+	ifp = zebra_interface_state_read(client->ibuf, vrf_id);
+	if (ifp == NULL)
+		return 0;
+
+	debugf(NHRP_DEBUG_IF, "if-up: %s", ifp->name);
+	nhrp_interface_update_nbma(ifp);
+
+	return 0;
+}
+
+int nhrp_interface_down(int cmd, struct zclient *client,
+			zebra_size_t length, vrf_id_t vrf_id)
+{
+	struct interface *ifp;
+
+	ifp = zebra_interface_state_read(client->ibuf, vrf_id);
+	if (ifp == NULL)
+		return 0;
+
+	debugf(NHRP_DEBUG_IF, "if-down: %s", ifp->name);
+	nhrp_interface_update(ifp);
+	return 0;
+}
+
+int nhrp_interface_address_add(int cmd, struct zclient *client,
+			       zebra_size_t length, vrf_id_t vrf_id)
+{
+	struct connected *ifc;
+	char buf[PREFIX_STRLEN];
+
+	ifc = zebra_interface_address_read(cmd, client->ibuf, vrf_id);
+	if (ifc == NULL)
+		return 0;
+
+	debugf(NHRP_DEBUG_IF, "if-addr-add: %s: %s",
+		ifc->ifp->name,
+		prefix2str(ifc->address, buf, sizeof buf));
+
+	nhrp_interface_update_address(ifc->ifp, family2afi(PREFIX_FAMILY(ifc->address)), 0);
+
+	return 0;
+}
+
+int nhrp_interface_address_delete(int cmd, struct zclient *client,
+				  zebra_size_t length, vrf_id_t vrf_id)
+{
+	struct connected *ifc;
+	char buf[PREFIX_STRLEN];
+
+	ifc = zebra_interface_address_read(cmd, client->ibuf, vrf_id);
+	if (ifc == NULL)
+		return 0;
+
+	debugf(NHRP_DEBUG_IF, "if-addr-del: %s: %s",
+		ifc->ifp->name,
+		prefix2str(ifc->address, buf, sizeof buf));
+
+	nhrp_interface_update_address(ifc->ifp, family2afi(PREFIX_FAMILY(ifc->address)), 0);
+	connected_free(ifc);
+
+	return 0;
+}
+
+void nhrp_interface_notify_add(struct interface *ifp, struct notifier_block *n, notifier_fn_t fn)
+{
+	struct nhrp_interface *nifp = ifp->info;
+	notifier_add(n, &nifp->notifier_list, fn);
+}
+
+void nhrp_interface_notify_del(struct interface *ifp, struct notifier_block *n)
+{
+	notifier_del(n);
+}
+
+void nhrp_interface_set_protection(struct interface *ifp, const char *profile, const char *fallback_profile)
+{
+	struct nhrp_interface *nifp = ifp->info;
+
+	if (nifp->ipsec_profile) free(nifp->ipsec_profile);
+	nifp->ipsec_profile = profile ? strdup(profile) : NULL;
+
+	if (nifp->ipsec_fallback_profile) free(nifp->ipsec_fallback_profile);
+	nifp->ipsec_fallback_profile = fallback_profile ? strdup(fallback_profile) : NULL;
+}
+
+void nhrp_interface_set_source(struct interface *ifp, const char *ifname)
+{
+	struct nhrp_interface *nifp = ifp->info;
+
+	if (nifp->source) free(nifp->source);
+	nifp->source = ifname ? strdup(ifname) : NULL;
+
+	nhrp_interface_update_nbma(ifp);
+}

nhrpd/nhrp_main.c

@@ -0,0 +1,246 @@
+/* NHRP daemon main functions
+ * Copyright (c) 2014-2015 Timo Teräs
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <unistd.h>
+
+#include "zebra.h"
+#include "privs.h"
+#include "getopt.h"
+#include "thread.h"
+#include "sigevent.h"
+#include "version.h"
+#include "log.h"
+#include "memory.h"
+#include "command.h"
+
+#include "nhrpd.h"
+#include "netlink.h"
+
+unsigned int debug_flags = 0;
+
+struct thread_master *master;
+struct timeval current_time;
+static const char *pid_file = PATH_NHRPD_PID;
+static char config_default[] = SYSCONFDIR NHRP_DEFAULT_CONFIG;
+static char *config_file = NULL;
+static char *vty_addr = NULL;
+static int vty_port = NHRP_VTY_PORT;
+static int do_daemonise = 0;
+
+/* nhrpd options. */
+struct option longopts[] = {
+	{ "daemon",      no_argument,       NULL, 'd'},
+	{ "config_file", required_argument, NULL, 'f'},
+	{ "pid_file",    required_argument, NULL, 'i'},
+	{ "socket",      required_argument, NULL, 'z'},
+	{ "help",        no_argument,       NULL, 'h'},
+	{ "vty_addr",    required_argument, NULL, 'A'},
+	{ "vty_port",    required_argument, NULL, 'P'},
+	{ "user",        required_argument, NULL, 'u'},
+	{ "group",       required_argument, NULL, 'g'},
+	{ "version",     no_argument,       NULL, 'v'},
+	{ 0 }
+};
+
+/* nhrpd privileges */
+static zebra_capabilities_t _caps_p [] = {
+	ZCAP_NET_RAW,
+	ZCAP_NET_ADMIN,
+	ZCAP_DAC_OVERRIDE,	/* for now needed to write to /proc/sys/net/ipv4/<if>/send_redirect */
+};
+
+static struct zebra_privs_t nhrpd_privs = {
+#ifdef QUAGGA_USER
+	.user = QUAGGA_USER,
+#endif
+#ifdef QUAGGA_GROUP
+	.group = QUAGGA_GROUP,
+#endif
+#ifdef VTY_GROUP
+	.vty_group = VTY_GROUP,
+#endif
+	.caps_p = _caps_p,
+	.cap_num_p = ZEBRA_NUM_OF(_caps_p),
+};
+
+static void usage(const char *progname, int status)
+{
+	if (status != 0)
+		fprintf(stderr, "Try `%s --help' for more information.\n", progname);
+	else
+		printf(
+"Usage : %s [OPTION...]\n\
+Daemon which manages NHRP protocol.\n\n\
+-d, --daemon       Runs in daemon mode\n\
+-f, --config_file  Set configuration file name\n\
+-i, --pid_file     Set process identifier file name\n\
+-z, --socket       Set path of zebra socket\n\
+-A, --vty_addr     Set vty's bind address\n\
+-P, --vty_port     Set vty's port number\n\
+-u, --user         User to run as\n\
+-g, --group        Group to run as\n\
+-v, --version      Print program version\n\
+-h, --help         Display this help and exit\n\
+\n\
+Report bugs to %s\n", progname, ZEBRA_BUG_ADDRESS);
+
+	exit(status);
+}
+
+static void parse_arguments(const char *progname, int argc, char **argv)
+{
+	int opt;
+
+	while (1) {
+		opt = getopt_long(argc, argv, "df:i:z:hA:P:u:g:v", longopts, 0);
+		if(opt < 0) break;
+
+		switch (opt) {
+		case 0:
+			break;
+		case 'd':
+			do_daemonise = -1;
+			break;
+		case 'f':
+			config_file = optarg;
+			break;
+		case 'i':
+			pid_file = optarg;
+			break;
+		case 'z':
+			zclient_serv_path_set(optarg);
+			break;
+		case 'A':
+			vty_addr = optarg;
+			break;
+		case 'P':
+			vty_port = atoi (optarg);
+			if (vty_port <= 0 || vty_port > 0xffff)
+				vty_port = NHRP_VTY_PORT;
+			break;
+		case 'u':
+			nhrpd_privs.user = optarg;
+			break;
+		case 'g':
+			nhrpd_privs.group = optarg;
+			break;
+		case 'v':
+			print_version(progname);
+			exit(0);
+			break;
+		case 'h':
+			usage(progname, 0);
+			break;
+		default:
+			usage(progname, 1);
+			break;
+		}
+	}
+}
+
+static void nhrp_sigusr1(void)
+{
+	zlog_rotate(NULL);
+}
+
+static void nhrp_request_stop(void)
+{
+	debugf(NHRP_DEBUG_COMMON, "Exiting...");
+
+	nhrp_shortcut_terminate();
+	nhrp_nhs_terminate();
+	nhrp_zebra_terminate();
+	vici_terminate();
+	evmgr_terminate();
+	nhrp_vc_terminate();
+	vrf_terminate();
+	/* memory_terminate(); */
+	/* vty_terminate(); */
+	cmd_terminate();
+	/* signal_terminate(); */
+	zprivs_terminate(&nhrpd_privs);
+
+	debugf(NHRP_DEBUG_COMMON, "Remove pid file.");
+	if (pid_file) unlink(pid_file);
+	debugf(NHRP_DEBUG_COMMON, "Done.");
+
+	closezlog(zlog_default);
+
+	exit(0);
+}
+
+static struct quagga_signal_t sighandlers[] = {
+	{ .signal = SIGUSR1, .handler = &nhrp_sigusr1, },
+	{ .signal = SIGINT,  .handler = &nhrp_request_stop, },
+	{ .signal = SIGTERM, .handler = &nhrp_request_stop, },
+};
+
+int main(int argc, char **argv)
+{
+	struct thread thread;
+	const char *progname;
+
+	/* Set umask before anything for security */
+	umask(0027);
+	progname = basename(argv[0]);
+	zlog_default = openzlog(progname, ZLOG_NHRP, LOG_CONS|LOG_NDELAY|LOG_PID, LOG_DAEMON);
+	zlog_set_level(NULL, ZLOG_DEST_STDOUT, LOG_WARNING);
+
+	parse_arguments(progname, argc, argv);
+
+	/* Library inits. */
+	master = thread_master_create();
+	zprivs_init(&nhrpd_privs);
+	signal_init(master, array_size(sighandlers), sighandlers);
+	cmd_init(1);
+	vty_init(master);
+	memory_init();
+	nhrp_interface_init();
+	vrf_init();
+	resolver_init();
+
+	/* Run with elevated capabilities, as for all netlink activity
+	 * we need privileges anyway. */
+	nhrpd_privs.change(ZPRIVS_RAISE);
+
+	netlink_init();
+	evmgr_init();
+	nhrp_vc_init();
+	nhrp_packet_init();
+	vici_init();
+	nhrp_zebra_init();
+	nhrp_shortcut_init();
+
+	nhrp_config_init();
+
+	/* Get zebra configuration file. */
+	zlog_set_level(NULL, ZLOG_DEST_STDOUT, do_daemonise ? ZLOG_DISABLED : LOG_DEBUG);
+	vty_read_config(config_file, config_default);
+
+	if (do_daemonise && daemon(0, 0) < 0) {
+		zlog_err("daemonise: %s", safe_strerror(errno));
+		exit (1);
+	}
+
+	/* write pid file */
+	if (pid_output(pid_file) < 0) {
+		zlog_err("error while writing pidfile");
+		exit (1);
+	}
+
+	/* Create VTY socket */
+	vty_serv_sock(vty_addr, vty_port, NHRP_VTYSH_PATH);
+	zlog_notice("nhrpd starting: vty@%d", vty_port);
+
+	/* Main loop */
+	while (thread_fetch(master, &thread))
+		thread_call(&thread);
+
+	return 0;
+}

nhrpd/nhrp_nhs.c

@@ -0,0 +1,369 @@
+/* NHRP NHC nexthop server functions (registration)
+ * Copyright (c) 2014-2015 Timo Teräs
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include "zebra.h"
+#include "zbuf.h"
+#include "memory.h"
+#include "thread.h"
+#include "nhrpd.h"
+#include "nhrp_protocol.h"
+
+static int nhrp_nhs_resolve(struct thread *t);
+
+struct nhrp_registration {
+	struct list_head reglist_entry;
+	struct thread *t_register;
+	struct nhrp_nhs *nhs;
+	struct nhrp_reqid reqid;
+	unsigned int timeout;
+	unsigned mark : 1;
+	union sockunion proto_addr;
+	struct nhrp_peer *peer;
+	struct notifier_block peer_notifier;
+};
+
+static int nhrp_reg_send_req(struct thread *t);
+
+static void nhrp_reg_reply(struct nhrp_reqid *reqid, void *arg)
+{
+	struct nhrp_packet_parser *p = arg;
+	struct nhrp_registration *r = container_of(reqid, struct nhrp_registration, reqid);
+	struct nhrp_nhs *nhs = r->nhs;
+	struct interface *ifp = nhs->ifp;
+	struct nhrp_interface *nifp = ifp->info;
+	struct nhrp_extension_header *ext;
+	struct nhrp_cie_header *cie;
+	struct nhrp_cache *c;
+	struct zbuf extpl;
+	union sockunion cie_nbma, cie_proto, *proto;
+	char buf[64];
+	int ok = 0, holdtime;
+
+	nhrp_reqid_free(&nhrp_packet_reqid, &r->reqid);
+
+	if (p->hdr->type != NHRP_PACKET_REGISTRATION_REPLY) {
+		debugf(NHRP_DEBUG_COMMON, "NHS: Registration failed");
+		return;
+	}
+
+	debugf(NHRP_DEBUG_COMMON, "NHS: Reg.reply received");
+
+	ok = 1;
+	while ((cie = nhrp_cie_pull(&p->payload, p->hdr, &cie_nbma, &cie_proto)) != NULL) {
+		proto = sockunion_family(&cie_proto) != AF_UNSPEC ? &cie_proto : &p->src_proto;
+		debugf(NHRP_DEBUG_COMMON, "NHS: CIE registration: %s: %d",
+			sockunion2str(proto, buf, sizeof(buf)),
+			cie->code);
+		if (!((cie->code == NHRP_CODE_SUCCESS) ||
+                      (cie->code == NHRP_CODE_ADMINISTRATIVELY_PROHIBITED && nhs->hub)))
+			ok = 0;
+	}
+
+	if (!ok)
+		return;
+
+	/* Parse extensions */
+	sockunion_family(&nifp->nat_nbma) = AF_UNSPEC;
+	while ((ext = nhrp_ext_pull(&p->extensions, &extpl)) != NULL) {
+		switch (htons(ext->type) & ~NHRP_EXTENSION_FLAG_COMPULSORY) {
+		case NHRP_EXTENSION_NAT_ADDRESS:
+			/* NHS adds second CIE if NAT is detected */
+			if (nhrp_cie_pull(&extpl, p->hdr, &cie_nbma, &cie_proto) &&
+			    nhrp_cie_pull(&extpl, p->hdr, &cie_nbma, &cie_proto)) {
+				nifp->nat_nbma = cie_nbma;
+				debugf(NHRP_DEBUG_IF, "%s: NAT detected, real NBMA address: %s",
+					ifp->name, sockunion2str(&nifp->nbma, buf, sizeof(buf)));
+			}
+			break;
+		}
+	}
+
+	/* Success - schedule next registration, and route NHS */
+	r->timeout = 2;
+	holdtime = nifp->afi[nhs->afi].holdtime;
+	THREAD_OFF(r->t_register);
+
+	/* RFC 2332 5.2.3 - Registration is recommend to be renewed
+	 * every one third of holdtime */
+	THREAD_TIMER_ON(master, r->t_register, nhrp_reg_send_req, r, holdtime / 3);
+
+	r->proto_addr = p->dst_proto;
+	c = nhrp_cache_get(ifp, &p->dst_proto, 1);
+	if (c) nhrp_cache_update_binding(c, NHRP_CACHE_NHS, holdtime, nhrp_peer_ref(r->peer), 0, NULL);
+}
+
+static int nhrp_reg_timeout(struct thread *t)
+{
+	struct nhrp_registration *r = THREAD_ARG(t);
+	struct nhrp_cache *c;
+
+	r->t_register = NULL;
+
+	if (r->timeout >= 16 && sockunion_family(&r->proto_addr) != AF_UNSPEC) {
+		nhrp_reqid_free(&nhrp_packet_reqid, &r->reqid);
+		c = nhrp_cache_get(r->nhs->ifp, &r->proto_addr, 0);
+		if (c) nhrp_cache_update_binding(c, NHRP_CACHE_NHS, -1, NULL, 0, NULL);
+		sockunion_family(&r->proto_addr) = AF_UNSPEC;
+	}
+
+	r->timeout <<= 1;
+	if (r->timeout > 64) r->timeout = 2;
+	THREAD_TIMER_MSEC_ON(master, r->t_register, nhrp_reg_send_req, r, 10);
+
+	return 0;
+}
+
+static void nhrp_reg_peer_notify(struct notifier_block *n, unsigned long cmd)
+{
+	struct nhrp_registration *r = container_of(n, struct nhrp_registration, peer_notifier);
+	char buf[SU_ADDRSTRLEN];
+
+	switch (cmd) {
+	case NOTIFY_PEER_UP:
+	case NOTIFY_PEER_DOWN:
+	case NOTIFY_PEER_IFCONFIG_CHANGED:
+	case NOTIFY_PEER_MTU_CHANGED:
+		debugf(NHRP_DEBUG_COMMON, "NHS: Flush timer for %s",
+			sockunion2str(&r->peer->vc->remote.nbma, buf, sizeof buf));
+		THREAD_TIMER_OFF(r->t_register);
+		THREAD_TIMER_MSEC_ON(master, r->t_register, nhrp_reg_send_req, r, 10);
+		break;
+	}
+}
+
+static int nhrp_reg_send_req(struct thread *t)
+{
+	struct nhrp_registration *r = THREAD_ARG(t);
+	struct nhrp_nhs *nhs = r->nhs;
+	char buf1[SU_ADDRSTRLEN], buf2[SU_ADDRSTRLEN];
+	struct interface *ifp = nhs->ifp;
+	struct nhrp_interface *nifp = ifp->info;
+	struct nhrp_afi_data *if_ad = &nifp->afi[nhs->afi];
+	union sockunion *dst_proto;
+	struct zbuf *zb;
+	struct nhrp_packet_header *hdr;
+	struct nhrp_extension_header *ext;
+	struct nhrp_cie_header *cie;
+
+	r->t_register = NULL;
+	if (!nhrp_peer_check(r->peer, 2)) {
+		debugf(NHRP_DEBUG_COMMON, "NHS: Waiting link for %s",
+			sockunion2str(&r->peer->vc->remote.nbma, buf1, sizeof buf1));
+		THREAD_TIMER_ON(master, r->t_register, nhrp_reg_send_req, r, 120);
+		return 0;
+	}
+
+	THREAD_TIMER_ON(master, r->t_register, nhrp_reg_timeout, r, r->timeout);
+
+	/* RFC2332 5.2.3 NHC uses it's own address as dst if NHS is unknown */
+	dst_proto = &nhs->proto_addr;
+	if (sockunion_family(dst_proto) == AF_UNSPEC)
+		dst_proto = &if_ad->addr;
+
+	sockunion2str(&if_ad->addr, buf1, sizeof(buf1));
+	sockunion2str(dst_proto, buf2, sizeof(buf2));
+	debugf(NHRP_DEBUG_COMMON, "NHS: Register %s -> %s (timeout %d)", buf1, buf2, r->timeout);
+
+	/* No protocol address configured for tunnel interface */
+	if (sockunion_family(&if_ad->addr) == AF_UNSPEC)
+		return 0;
+
+	zb = zbuf_alloc(1400);
+	hdr = nhrp_packet_push(zb, NHRP_PACKET_REGISTRATION_REQUEST, &nifp->nbma, &if_ad->addr, dst_proto);
+	hdr->hop_count = 0;
+	if (!(if_ad->flags & NHRP_IFF_REG_NO_UNIQUE))
+		hdr->flags |= htons(NHRP_FLAG_REGISTRATION_UNIQUE);
+
+	hdr->u.request_id = htonl(nhrp_reqid_alloc(&nhrp_packet_reqid, &r->reqid, nhrp_reg_reply));
+
+	/* FIXME: push CIE for each local protocol address */
+	cie = nhrp_cie_push(zb, NHRP_CODE_SUCCESS, NULL, NULL);
+	cie->prefix_length = 0xff;
+	cie->holding_time = htons(if_ad->holdtime);
+	cie->mtu = htons(if_ad->mtu);
+
+	nhrp_ext_request(zb, hdr, ifp);
+
+	/* Cisco NAT detection extension */
+	hdr->flags |= htons(NHRP_FLAG_REGISTRATION_NAT);
+	ext = nhrp_ext_push(zb, hdr, NHRP_EXTENSION_NAT_ADDRESS);
+	cie = nhrp_cie_push(zb, NHRP_CODE_SUCCESS, &nifp->nbma, &if_ad->addr);
+	cie->prefix_length = 8 * sockunion_get_addrlen(&nifp->nbma);
+	nhrp_ext_complete(zb, ext);
+
+	nhrp_packet_complete(zb, hdr);
+	nhrp_peer_send(r->peer, zb);
+	zbuf_free(zb);
+
+	return 0;
+}
+
+static void nhrp_reg_delete(struct nhrp_registration *r)
+{
+	nhrp_peer_notify_del(r->peer, &r->peer_notifier);
+	nhrp_peer_unref(r->peer);
+	list_del(&r->reglist_entry);
+	THREAD_OFF(r->t_register);
+	XFREE(MTYPE_NHRP_REGISTRATION, r);
+}
+
+static struct nhrp_registration *nhrp_reg_by_nbma(struct nhrp_nhs *nhs, const union sockunion *nbma_addr)
+{
+	struct nhrp_registration *r;
+
+	list_for_each_entry(r, &nhs->reglist_head, reglist_entry)
+		if (sockunion_same(&r->peer->vc->remote.nbma, nbma_addr))
+			return r;
+	return NULL;
+}
+
+static void nhrp_nhs_resolve_cb(struct resolver_query *q, int n, union sockunion *addrs)
+{
+	struct nhrp_nhs *nhs = container_of(q, struct nhrp_nhs, dns_resolve);
+	struct nhrp_interface *nifp = nhs->ifp->info;
+	struct nhrp_registration *reg, *regn;
+	int i;
+
+	nhs->t_resolve = NULL;
+	if (n < 0) {
+		/* Failed, retry in a moment */
+		THREAD_TIMER_ON(master, nhs->t_resolve, nhrp_nhs_resolve, nhs, 5);
+		return;
+	}
+
+	THREAD_TIMER_ON(master, nhs->t_resolve, nhrp_nhs_resolve, nhs, 2*60*60);
+
+	list_for_each_entry(reg, &nhs->reglist_head, reglist_entry)
+		reg->mark = 1;
+
+	nhs->hub = 0;
+	for (i = 0; i < n; i++) {
+		if (sockunion_same(&addrs[i], &nifp->nbma)) {
+			nhs->hub = 1;
+			continue;
+		}
+
+		reg = nhrp_reg_by_nbma(nhs, &addrs[i]);
+		if (reg) {
+			reg->mark = 0;
+			continue;
+		}
+
+		reg = XCALLOC(MTYPE_NHRP_REGISTRATION, sizeof(*reg));
+		reg->peer = nhrp_peer_get(nhs->ifp, &addrs[i]);
+		reg->nhs = nhs;
+		reg->timeout = 1;
+		list_init(&reg->reglist_entry);
+		list_add_tail(&reg->reglist_entry, &nhs->reglist_head);
+		nhrp_peer_notify_add(reg->peer, &reg->peer_notifier, nhrp_reg_peer_notify);
+		THREAD_TIMER_MSEC_ON(master, reg->t_register, nhrp_reg_send_req, reg, 50);
+	}
+
+	list_for_each_entry_safe(reg, regn, &nhs->reglist_head, reglist_entry) {
+		if (reg->mark)
+			nhrp_reg_delete(reg);
+	}
+}
+
+static int nhrp_nhs_resolve(struct thread *t)
+{
+	struct nhrp_nhs *nhs = THREAD_ARG(t);
+
+	resolver_resolve(&nhs->dns_resolve, AF_INET, nhs->nbma_fqdn, nhrp_nhs_resolve_cb);
+
+	return 0;
+}
+
+int nhrp_nhs_add(struct interface *ifp, afi_t afi, union sockunion *proto_addr, const char *nbma_fqdn)
+{
+	struct nhrp_interface *nifp = ifp->info;
+	struct nhrp_nhs *nhs;
+
+	if (sockunion_family(proto_addr) != AF_UNSPEC &&
+	    sockunion_family(proto_addr) != afi2family(afi))
+		return NHRP_ERR_PROTOCOL_ADDRESS_MISMATCH;
+
+	list_for_each_entry(nhs, &nifp->afi[afi].nhslist_head, nhslist_entry) {
+		if (sockunion_family(&nhs->proto_addr) != AF_UNSPEC &&
+		    sockunion_family(proto_addr) != AF_UNSPEC &&
+		    sockunion_same(&nhs->proto_addr, proto_addr))
+			return NHRP_ERR_ENTRY_EXISTS;
+
+		if (strcmp(nhs->nbma_fqdn, nbma_fqdn) == 0)
+			return NHRP_ERR_ENTRY_EXISTS;
+	}
+
+	nhs = XMALLOC(MTYPE_NHRP_NHS, sizeof(struct nhrp_nhs));
+	if (!nhs) return NHRP_ERR_NO_MEMORY;
+
+	*nhs = (struct nhrp_nhs) {
+		.afi = afi,
+		.ifp = ifp,
+		.proto_addr = *proto_addr,
+		.nbma_fqdn = strdup(nbma_fqdn),
+		.reglist_head = LIST_INITIALIZER(nhs->reglist_head),
+	};
+	list_add_tail(&nhs->nhslist_entry, &nifp->afi[afi].nhslist_head);
+	THREAD_TIMER_MSEC_ON(master, nhs->t_resolve, nhrp_nhs_resolve, nhs, 1000);
+
+	return NHRP_OK;
+}
+
+int nhrp_nhs_del(struct interface *ifp, afi_t afi, union sockunion *proto_addr, const char *nbma_fqdn)
+{
+	struct nhrp_interface *nifp = ifp->info;
+	struct nhrp_nhs *nhs, *nnhs;
+	int ret = NHRP_ERR_ENTRY_NOT_FOUND;
+
+	if (sockunion_family(proto_addr) != AF_UNSPEC &&
+	    sockunion_family(proto_addr) != afi2family(afi))
+		return NHRP_ERR_PROTOCOL_ADDRESS_MISMATCH;
+
+	list_for_each_entry_safe(nhs, nnhs, &nifp->afi[afi].nhslist_head, nhslist_entry) {
+		if (!sockunion_same(&nhs->proto_addr, proto_addr))
+			continue;
+		if (strcmp(nhs->nbma_fqdn, nbma_fqdn) != 0)
+			continue;
+
+		nhrp_nhs_free(nhs);
+		ret = NHRP_OK;
+	}
+
+	return ret;
+}
+
+int nhrp_nhs_free(struct nhrp_nhs *nhs)
+{
+	struct nhrp_registration *r, *rn;
+
+	list_for_each_entry_safe(r, rn, &nhs->reglist_head, reglist_entry)
+		nhrp_reg_delete(r);
+	THREAD_OFF(nhs->t_resolve);
+	list_del(&nhs->nhslist_entry);
+	free((void*) nhs->nbma_fqdn);
+	XFREE(MTYPE_NHRP_NHS, nhs);
+	return 0;
+}
+
+void nhrp_nhs_terminate(void)
+{
+	struct interface *ifp;
+	struct nhrp_interface *nifp;
+	struct nhrp_nhs *nhs, *tmp;
+	struct listnode *node;
+	afi_t afi;
+
+	for (ALL_LIST_ELEMENTS_RO(iflist, node, ifp)) {
+		nifp = ifp->info;
+		for (afi = 0; afi < AFI_MAX; afi++) {
+			list_for_each_entry_safe(nhs, tmp, &nifp->afi[afi].nhslist_head, nhslist_entry)
+				nhrp_nhs_free(nhs);
+		}
+	}
+}

nhrpd/nhrp_packet.c

@@ -0,0 +1,312 @@
+/* NHRP packet handling functions
+ * Copyright (c) 2014-2015 Timo Teräs
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <netinet/if_ether.h>
+#include "nhrpd.h"
+#include "zbuf.h"
+#include "thread.h"
+#include "hash.h"
+
+#include "nhrp_protocol.h"
+#include "os.h"
+
+struct nhrp_reqid_pool nhrp_packet_reqid;
+
+static uint16_t family2proto(int family)
+{
+	switch (family) {
+	case AF_INET: return ETH_P_IP;
+	case AF_INET6: return ETH_P_IPV6;
+	}
+	return 0;
+}
+
+static int proto2family(uint16_t proto)
+{
+	switch (proto) {
+	case ETH_P_IP: return AF_INET;
+	case ETH_P_IPV6: return AF_INET6;
+	}
+	return AF_UNSPEC;
+}
+
+struct nhrp_packet_header *nhrp_packet_push(
+	struct zbuf *zb, uint8_t type,
+	const union sockunion *src_nbma,
+	const union sockunion *src_proto,
+	const union sockunion *dst_proto)
+{
+	struct nhrp_packet_header *hdr;
+
+	hdr = zbuf_push(zb, struct nhrp_packet_header);
+	if (!hdr) return NULL;
+
+	*hdr = (struct nhrp_packet_header) {
+		.afnum = htons(family2afi(sockunion_family(src_nbma))),
+		.protocol_type = htons(family2proto(sockunion_family(src_proto))),
+		.version = NHRP_VERSION_RFC2332,
+		.type = type,
+		.hop_count = 64,
+		.src_nbma_address_len = sockunion_get_addrlen(src_nbma),
+		.src_protocol_address_len = sockunion_get_addrlen(src_proto),
+		.dst_protocol_address_len = sockunion_get_addrlen(dst_proto),
+	};
+
+	zbuf_put(zb, sockunion_get_addr(src_nbma), hdr->src_nbma_address_len);
+	zbuf_put(zb, sockunion_get_addr(src_proto), hdr->src_protocol_address_len);
+	zbuf_put(zb, sockunion_get_addr(dst_proto), hdr->dst_protocol_address_len);
+
+	return hdr;
+}
+
+struct nhrp_packet_header *nhrp_packet_pull(
+	struct zbuf *zb,
+	union sockunion *src_nbma,
+	union sockunion *src_proto,
+	union sockunion *dst_proto)
+{
+	struct nhrp_packet_header *hdr;
+
+	hdr = zbuf_pull(zb, struct nhrp_packet_header);
+	if (!hdr) return NULL;
+
+	sockunion_set(
+		src_nbma, afi2family(htons(hdr->afnum)),
+		zbuf_pulln(zb, hdr->src_nbma_address_len + hdr->src_nbma_subaddress_len),
+		hdr->src_nbma_address_len + hdr->src_nbma_subaddress_len);
+	sockunion_set(
+		src_proto, proto2family(htons(hdr->protocol_type)),
+		zbuf_pulln(zb, hdr->src_protocol_address_len),
+		hdr->src_protocol_address_len);
+	sockunion_set(
+		dst_proto, proto2family(htons(hdr->protocol_type)),
+		zbuf_pulln(zb, hdr->dst_protocol_address_len),
+		hdr->dst_protocol_address_len);
+
+	return hdr;
+}
+
+uint16_t nhrp_packet_calculate_checksum(const uint8_t *pdu, uint16_t len)
+{
+	const uint16_t *pdu16 = (const 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;
+}
+
+void nhrp_packet_complete(struct zbuf *zb, struct nhrp_packet_header *hdr)
+{
+	unsigned short size;
+
+	if (hdr->extension_offset)
+		nhrp_ext_push(zb, hdr, NHRP_EXTENSION_END | NHRP_EXTENSION_FLAG_COMPULSORY);
+
+	size = zb->tail - (uint8_t *)hdr;
+	hdr->packet_size = htons(size);
+	hdr->checksum = 0;
+	hdr->checksum = nhrp_packet_calculate_checksum((uint8_t *) hdr, size);
+}
+
+struct nhrp_cie_header *nhrp_cie_push(
+	struct zbuf *zb,
+	uint8_t code,
+	const union sockunion *nbma,
+	const union sockunion *proto)
+{
+	struct nhrp_cie_header *cie;
+
+	cie = zbuf_push(zb, struct nhrp_cie_header);
+	*cie = (struct nhrp_cie_header) {
+		.code = code,
+	};
+	if (nbma) {
+		cie->nbma_address_len = sockunion_get_addrlen(nbma);
+		zbuf_put(zb, sockunion_get_addr(nbma), cie->nbma_address_len);
+	}
+	if (proto) {
+		cie->protocol_address_len = sockunion_get_addrlen(proto);
+		zbuf_put(zb, sockunion_get_addr(proto), cie->protocol_address_len);
+	}
+
+	return cie;
+}
+
+struct nhrp_cie_header *nhrp_cie_pull(
+	struct zbuf *zb,
+	struct nhrp_packet_header *hdr,
+	union sockunion *nbma,
+	union sockunion *proto)
+{
+	struct nhrp_cie_header *cie;
+
+	cie = zbuf_pull(zb, struct nhrp_cie_header);
+	if (!cie) return NULL;
+
+	if (cie->nbma_address_len + cie->nbma_subaddress_len) {
+		sockunion_set(
+			nbma, afi2family(htons(hdr->afnum)),
+			zbuf_pulln(zb, cie->nbma_address_len + cie->nbma_subaddress_len),
+			cie->nbma_address_len + cie->nbma_subaddress_len);
+	} else {
+		sockunion_family(nbma) = AF_UNSPEC;
+	}
+
+	if (cie->protocol_address_len) {
+		sockunion_set(
+			proto, proto2family(htons(hdr->protocol_type)),
+			zbuf_pulln(zb, cie->protocol_address_len),
+			cie->protocol_address_len);
+	} else {
+		sockunion_family(proto) = AF_UNSPEC;
+	}
+
+	return cie;
+}
+
+struct nhrp_extension_header *nhrp_ext_push(struct zbuf *zb, struct nhrp_packet_header *hdr, uint16_t type)
+{
+	struct nhrp_extension_header *ext;
+	ext = zbuf_push(zb, struct nhrp_extension_header);
+	if (!ext) return NULL;
+
+	if (!hdr->extension_offset)
+		hdr->extension_offset = htons(zb->tail - (uint8_t*) hdr - sizeof(struct nhrp_extension_header));
+
+	*ext = (struct nhrp_extension_header) {
+		.type = htons(type),
+		.length = 0,
+	};
+	return ext;
+}
+
+void nhrp_ext_complete(struct zbuf *zb, struct nhrp_extension_header *ext)
+{
+	ext->length = htons(zb->tail - (uint8_t*)ext - sizeof(struct nhrp_extension_header));
+}
+
+struct nhrp_extension_header *nhrp_ext_pull(struct zbuf *zb, struct zbuf *payload)
+{
+	struct nhrp_extension_header *ext;
+	uint16_t plen;
+
+	ext = zbuf_pull(zb, struct nhrp_extension_header);
+	if (!ext) return NULL;
+
+	plen = htons(ext->length);
+	zbuf_init(payload, zbuf_pulln(zb, plen), plen, plen);
+	return ext;
+}
+
+void nhrp_ext_request(struct zbuf *zb, struct nhrp_packet_header *hdr, struct interface *ifp)
+{
+	/* Place holders for standard extensions */
+	nhrp_ext_push(zb, hdr, NHRP_EXTENSION_FORWARD_TRANSIT_NHS | NHRP_EXTENSION_FLAG_COMPULSORY);
+	nhrp_ext_push(zb, hdr, NHRP_EXTENSION_REVERSE_TRANSIT_NHS | NHRP_EXTENSION_FLAG_COMPULSORY);
+	nhrp_ext_push(zb, hdr, NHRP_EXTENSION_RESPONDER_ADDRESS | NHRP_EXTENSION_FLAG_COMPULSORY);
+}
+
+int nhrp_ext_reply(struct zbuf *zb, struct nhrp_packet_header *hdr, struct interface *ifp, struct nhrp_extension_header *ext, struct zbuf *extpayload)
+{
+	struct nhrp_interface *nifp = ifp->info;
+	struct nhrp_afi_data *ad = &nifp->afi[htons(hdr->afnum)];
+	struct nhrp_extension_header *dst;
+	struct nhrp_cie_header *cie;
+	uint16_t type;
+
+	type = htons(ext->type) & ~NHRP_EXTENSION_FLAG_COMPULSORY;
+	if (type == NHRP_EXTENSION_END)
+		return 0;
+
+	dst = nhrp_ext_push(zb, hdr, htons(ext->type));
+	if (!dst) goto err;
+
+	switch (type) {
+	case NHRP_EXTENSION_RESPONDER_ADDRESS:
+		cie = nhrp_cie_push(zb, NHRP_CODE_SUCCESS, &nifp->nbma, &ad->addr);
+		if (!cie) goto err;
+		cie->holding_time = htons(ad->holdtime);
+		break;
+	default:
+		if (type & NHRP_EXTENSION_FLAG_COMPULSORY)
+			goto err;
+	case NHRP_EXTENSION_FORWARD_TRANSIT_NHS:
+	case NHRP_EXTENSION_REVERSE_TRANSIT_NHS:
+		/* Supported compulsory extensions, and any
+		 * non-compulsory that is not explicitly handled,
+		 * should be just copied. */
+		zbuf_copy(zb, extpayload, zbuf_used(extpayload));
+		break;
+	}
+	nhrp_ext_complete(zb, dst);
+	return 0;
+err:
+	zbuf_set_werror(zb);
+	return -1;
+}
+
+static int nhrp_packet_recvraw(struct thread *t)
+{
+	int fd = THREAD_FD(t), ifindex;
+	struct zbuf *zb;
+	struct interface *ifp;
+	struct nhrp_peer *p;
+	union sockunion remote_nbma;
+	uint8_t addr[64];
+	size_t len, addrlen;
+
+	thread_add_read(master, nhrp_packet_recvraw, 0, fd);
+
+	zb = zbuf_alloc(1500);
+	if (!zb) return 0;
+
+	len = zbuf_size(zb);
+	addrlen = sizeof(addr);
+	if (os_recvmsg(zb->buf, &len, &ifindex, addr, &addrlen) < 0)
+		goto err;
+
+	zb->head = zb->buf;
+	zb->tail = zb->buf + len;
+
+	switch (addrlen) {
+	case 4:
+		sockunion_set(&remote_nbma, AF_INET, addr, addrlen);
+		break;
+	default:
+		goto err;
+	}
+
+	ifp = if_lookup_by_index(ifindex);
+	if (!ifp) goto err;
+
+	p = nhrp_peer_get(ifp, &remote_nbma);
+	if (!p) goto err;
+
+	nhrp_peer_recv(p, zb);
+	nhrp_peer_unref(p);
+	return 0;
+
+err:
+	zbuf_free(zb);
+	return 0;
+}
+
+int nhrp_packet_init(void)
+{
+	thread_add_read(master, nhrp_packet_recvraw, 0, os_socket());
+	return 0;
+}

nhrpd/nhrp_peer.c

@@ -0,0 +1,860 @@
+/* NHRP peer functions
+ * Copyright (c) 2014-2015 Timo Teräs
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <netinet/if_ether.h>
+
+#include "zebra.h"
+#include "memory.h"
+#include "thread.h"
+#include "hash.h"
+
+#include "nhrpd.h"
+#include "nhrp_protocol.h"
+#include "os.h"
+
+struct ipv6hdr {
+	uint8_t priority_version;
+	uint8_t flow_lbl[3];
+	uint16_t payload_len;
+	uint8_t nexthdr;
+	uint8_t hop_limit;
+	struct in6_addr saddr;
+	struct in6_addr daddr;
+};
+
+static void nhrp_packet_debug(struct zbuf *zb, const char *dir);
+
+static void nhrp_peer_check_delete(struct nhrp_peer *p)
+{
+	struct nhrp_interface *nifp = p->ifp->info;
+
+	if (p->ref || notifier_active(&p->notifier_list))
+		return;
+
+	THREAD_OFF(p->t_fallback);
+	hash_release(nifp->peer_hash, p);
+	nhrp_interface_notify_del(p->ifp, &p->ifp_notifier);
+	nhrp_vc_notify_del(p->vc, &p->vc_notifier);
+	XFREE(MTYPE_NHRP_PEER, p);
+}
+
+static int nhrp_peer_notify_up(struct thread *t)
+{
+	struct nhrp_peer *p = THREAD_ARG(t);
+	struct nhrp_vc *vc = p->vc;
+	struct interface *ifp = p->ifp;
+	struct nhrp_interface *nifp = ifp->info;
+
+	p->t_fallback = NULL;
+	if (nifp->enabled && (!nifp->ipsec_profile || vc->ipsec)) {
+		p->online = 1;
+		nhrp_peer_ref(p);
+		notifier_call(&p->notifier_list, NOTIFY_PEER_UP);
+		nhrp_peer_unref(p);
+	}
+
+	return 0;
+}
+
+static void __nhrp_peer_check(struct nhrp_peer *p)
+{
+	struct nhrp_vc *vc = p->vc;
+	struct interface *ifp = p->ifp;
+	struct nhrp_interface *nifp = ifp->info;
+	unsigned online;
+
+	online = nifp->enabled && (!nifp->ipsec_profile || vc->ipsec);
+	if (p->online != online) {
+		THREAD_OFF(p->t_fallback);
+		if (online && notifier_active(&p->notifier_list)) {
+			/* If we requested the IPsec connection, delay
+			 * the up notification a bit to allow things
+			 * settle down. This allows IKE to install
+			 * SPDs and SAs. */
+			THREAD_TIMER_MSEC_ON(
+				master, p->t_fallback,
+				nhrp_peer_notify_up, p, 50);
+		} else {
+			nhrp_peer_ref(p);
+			p->online = online;
+			if (online) {
+				notifier_call(&p->notifier_list, NOTIFY_PEER_UP);
+			} else {
+				p->requested = p->fallback_requested = 0;
+				notifier_call(&p->notifier_list, NOTIFY_PEER_DOWN);
+			}
+			nhrp_peer_unref(p);
+		}
+	}
+}
+
+static void nhrp_peer_vc_notify(struct notifier_block *n, unsigned long cmd)
+{
+	struct nhrp_peer *p = container_of(n, struct nhrp_peer, vc_notifier);
+
+	switch (cmd) {
+	case NOTIFY_VC_IPSEC_CHANGED:
+		__nhrp_peer_check(p);
+		break;
+	case NOTIFY_VC_IPSEC_UPDATE_NBMA:
+		nhrp_peer_ref(p);
+		notifier_call(&p->notifier_list, NOTIFY_PEER_NBMA_CHANGING);
+		nhrp_peer_unref(p);
+		break;
+	}
+}
+
+static void nhrp_peer_ifp_notify(struct notifier_block *n, unsigned long cmd)
+{
+	struct nhrp_peer *p = container_of(n, struct nhrp_peer, ifp_notifier);
+	struct nhrp_interface *nifp;
+	struct nhrp_vc *vc;
+
+	nhrp_peer_ref(p);
+	switch (cmd) {
+	case NOTIFY_INTERFACE_UP:
+	case NOTIFY_INTERFACE_DOWN:
+		__nhrp_peer_check(p);
+		break;
+	case NOTIFY_INTERFACE_NBMA_CHANGED:
+		/* Source NBMA changed, rebind to new VC */
+		nifp = p->ifp->info;
+		vc = nhrp_vc_get(&nifp->nbma, &p->vc->remote.nbma, 1);
+		if (vc && p->vc != vc) {
+			nhrp_vc_notify_del(p->vc, &p->vc_notifier);
+			p->vc = vc;
+			nhrp_vc_notify_add(p->vc, &p->vc_notifier, nhrp_peer_vc_notify);
+			__nhrp_peer_check(p);
+		}
+		/* Fall-through to post config update */
+	case NOTIFY_INTERFACE_ADDRESS_CHANGED:
+		notifier_call(&p->notifier_list, NOTIFY_PEER_IFCONFIG_CHANGED);
+		break;
+	case NOTIFY_INTERFACE_MTU_CHANGED:
+		notifier_call(&p->notifier_list, NOTIFY_PEER_MTU_CHANGED);
+		break;
+	}
+	nhrp_peer_unref(p);
+}
+
+static unsigned int nhrp_peer_key(void *peer_data)
+{
+	struct nhrp_peer *p = peer_data;
+	return sockunion_hash(&p->vc->remote.nbma);
+}
+
+static int nhrp_peer_cmp(const void *cache_data, const void *key_data)
+{
+	const struct nhrp_peer *a = cache_data;
+	const struct nhrp_peer *b = key_data;
+	return a->ifp == b->ifp && a->vc == b->vc;
+}
+
+static void *nhrp_peer_create(void *data)
+{
+	struct nhrp_peer *p, *key = data;
+
+	p = XMALLOC(MTYPE_NHRP_PEER, sizeof(*p));
+	if (p) {
+		*p = (struct nhrp_peer) {
+			.ref = 0,
+			.ifp = key->ifp,
+			.vc = key->vc,
+			.notifier_list = NOTIFIER_LIST_INITIALIZER(&p->notifier_list),
+		};
+		nhrp_vc_notify_add(p->vc, &p->vc_notifier, nhrp_peer_vc_notify);
+		nhrp_interface_notify_add(p->ifp, &p->ifp_notifier, nhrp_peer_ifp_notify);
+	}
+	return p;
+}
+
+struct nhrp_peer *nhrp_peer_get(struct interface *ifp, const union sockunion *remote_nbma)
+{
+	struct nhrp_interface *nifp = ifp->info;
+	struct nhrp_peer key, *p;
+	struct nhrp_vc *vc;
+
+	if (!nifp->peer_hash) {
+		nifp->peer_hash = hash_create(nhrp_peer_key, nhrp_peer_cmp);
+		if (!nifp->peer_hash) return NULL;
+	}
+
+	vc = nhrp_vc_get(&nifp->nbma, remote_nbma, 1);
+	if (!vc) return NULL;
+
+	key.ifp = ifp;
+	key.vc = vc;
+
+	p = hash_get(nifp->peer_hash, &key, nhrp_peer_create);
+	nhrp_peer_ref(p);
+	if (p->ref == 1) __nhrp_peer_check(p);
+
+	return p;
+}
+
+struct nhrp_peer *nhrp_peer_ref(struct nhrp_peer *p)
+{
+	if (p) p->ref++;
+	return p;
+}
+
+void nhrp_peer_unref(struct nhrp_peer *p)
+{
+	if (p) {
+		p->ref--;
+		nhrp_peer_check_delete(p);
+	}
+}
+
+static int nhrp_peer_request_timeout(struct thread *t)
+{
+	struct nhrp_peer *p = THREAD_ARG(t);
+	struct nhrp_vc *vc = p->vc;
+	struct interface *ifp = p->ifp;
+	struct nhrp_interface *nifp = ifp->info;
+
+	p->t_fallback = NULL;
+
+	if (p->online)
+		return 0;
+
+	if (nifp->ipsec_fallback_profile && !p->prio && !p->fallback_requested) {
+		p->fallback_requested = 1;
+		vici_request_vc(nifp->ipsec_fallback_profile,
+				&vc->local.nbma, &vc->remote.nbma, p->prio);
+		THREAD_TIMER_ON(master, p->t_fallback, nhrp_peer_request_timeout, p, 30);
+	} else {
+		p->requested = p->fallback_requested = 0;
+	}
+
+	return 0;
+}
+
+int nhrp_peer_check(struct nhrp_peer *p, int establish)
+{
+	struct nhrp_vc *vc = p->vc;
+	struct interface *ifp = p->ifp;
+	struct nhrp_interface *nifp = ifp->info;
+
+	if (p->online)
+		return 1;
+	if (!establish)
+		return 0;
+	if (p->requested)
+		return 0;
+	if (sockunion_family(&vc->local.nbma) == AF_UNSPEC)
+		return 0;
+
+	p->prio = establish > 1;
+	p->requested = 1;
+	vici_request_vc(nifp->ipsec_profile, &vc->local.nbma, &vc->remote.nbma, p->prio);
+	THREAD_TIMER_ON(master, p->t_fallback, nhrp_peer_request_timeout, p,
+			(nifp->ipsec_fallback_profile && !p->prio) ? 15 : 30);
+
+	return 0;
+}
+
+void nhrp_peer_notify_add(struct nhrp_peer *p, struct notifier_block *n, notifier_fn_t fn)
+{
+	notifier_add(n, &p->notifier_list, fn);
+}
+
+void nhrp_peer_notify_del(struct nhrp_peer *p, struct notifier_block *n)
+{
+	notifier_del(n);
+	nhrp_peer_check_delete(p);
+}
+
+void nhrp_peer_send(struct nhrp_peer *p, struct zbuf *zb)
+{
+	char buf[2][256];
+
+	nhrp_packet_debug(zb, "Send");
+
+	if (!p->online)
+		return;
+
+	debugf(NHRP_DEBUG_KERNEL, "PACKET: Send %s -> %s",
+		sockunion2str(&p->vc->local.nbma, buf[0], sizeof buf[0]),
+		sockunion2str(&p->vc->remote.nbma, buf[1], sizeof buf[1]));
+
+	os_sendmsg(zb->head, zbuf_used(zb),
+		p->ifp->ifindex,
+		sockunion_get_addr(&p->vc->remote.nbma),
+		sockunion_get_addrlen(&p->vc->remote.nbma));
+	zbuf_reset(zb);
+}
+
+static void nhrp_handle_resolution_req(struct nhrp_packet_parser *p)
+{
+	struct zbuf *zb, payload;
+	struct nhrp_packet_header *hdr;
+	struct nhrp_cie_header *cie;
+	struct nhrp_extension_header *ext;
+	struct nhrp_interface *nifp;
+	struct nhrp_peer *peer;
+
+	if (!(p->if_ad->flags & NHRP_IFF_SHORTCUT)) {
+		debugf(NHRP_DEBUG_COMMON, "Shortcuts disabled");
+		/* FIXME: Send error indication? */
+		return;
+	}
+
+	if (p->if_ad->network_id &&
+	    p->route_type == NHRP_ROUTE_OFF_NBMA &&
+	    p->route_prefix.prefixlen < 8) {
+		debugf(NHRP_DEBUG_COMMON, "Shortcut to more generic than /8 dropped");
+		return;
+	}
+
+	debugf(NHRP_DEBUG_COMMON, "Parsing and replying to Resolution Req");
+
+	if (nhrp_route_address(p->ifp, &p->src_proto, NULL, &peer) != NHRP_ROUTE_NBMA_NEXTHOP)
+		return;
+
+#if 0
+	/* FIXME: Update requestors binding if CIE specifies holding time */
+	nhrp_cache_update_binding(
+			NHRP_CACHE_CACHED, &p->src_proto,
+			nhrp_peer_get(p->ifp, &p->src_nbma),
+			htons(cie->holding_time));
+#endif
+
+	nifp = peer->ifp->info;
+
+	/* Create reply */
+	zb = zbuf_alloc(1500);
+	hdr = nhrp_packet_push(zb, NHRP_PACKET_RESOLUTION_REPLY, &p->src_nbma, &p->src_proto, &p->dst_proto);
+
+	/* Copied information from request */
+	hdr->flags = p->hdr->flags & htons(NHRP_FLAG_RESOLUTION_SOURCE_IS_ROUTER|NHRP_FLAG_RESOLUTION_SOURCE_STABLE);
+	hdr->flags |= htons(NHRP_FLAG_RESOLUTION_DESTINATION_STABLE | NHRP_FLAG_RESOLUTION_AUTHORATIVE);
+	hdr->u.request_id = p->hdr->u.request_id;
+
+	/* CIE payload */
+	cie = nhrp_cie_push(zb, NHRP_CODE_SUCCESS, &nifp->nbma, &p->if_ad->addr);
+	cie->holding_time = htons(p->if_ad->holdtime);
+	cie->mtu = htons(p->if_ad->mtu);
+	if (p->if_ad->network_id && p->route_type == NHRP_ROUTE_OFF_NBMA)
+		cie->prefix_length = p->route_prefix.prefixlen;
+	else
+		cie->prefix_length = 8 * sockunion_get_addrlen(&p->if_ad->addr);
+
+	/* Handle extensions */
+	while ((ext = nhrp_ext_pull(&p->extensions, &payload)) != NULL) {
+		switch (htons(ext->type) & ~NHRP_EXTENSION_FLAG_COMPULSORY) {
+		case NHRP_EXTENSION_NAT_ADDRESS:
+			if (sockunion_family(&nifp->nat_nbma) == AF_UNSPEC)
+				break;
+			ext = nhrp_ext_push(zb, hdr, NHRP_EXTENSION_NAT_ADDRESS);
+			if (!ext) goto err;
+			cie = nhrp_cie_push(zb, NHRP_CODE_SUCCESS, &nifp->nat_nbma, &p->if_ad->addr);
+			if (!cie) goto err;
+			nhrp_ext_complete(zb, ext);
+			break;
+		default:
+			if (nhrp_ext_reply(zb, hdr, p->ifp, ext, &payload) < 0)
+				goto err;
+			break;
+		}
+	}
+
+	nhrp_packet_complete(zb, hdr);
+	nhrp_peer_send(peer, zb);
+err:
+	nhrp_peer_unref(peer);
+	zbuf_free(zb);
+}
+
+static void nhrp_handle_registration_request(struct nhrp_packet_parser *p)
+{
+	struct interface *ifp = p->ifp;
+	struct zbuf *zb, payload;
+	struct nhrp_packet_header *hdr;
+	struct nhrp_cie_header *cie;
+	struct nhrp_extension_header *ext;
+	struct nhrp_cache *c;
+	union sockunion cie_nbma, cie_proto, *proto_addr, *nbma_addr, *nbma_natoa;
+	int holdtime, natted = 0;
+	size_t paylen;
+	void *pay;
+
+	debugf(NHRP_DEBUG_COMMON, "Parsing and replying to Registration Req");
+
+	if (!sockunion_same(&p->src_nbma, &p->peer->vc->remote.nbma))
+		natted = 1;
+
+	/* Create reply */
+	zb = zbuf_alloc(1500);
+	hdr = nhrp_packet_push(zb, NHRP_PACKET_REGISTRATION_REPLY,
+		&p->src_nbma, &p->src_proto, &p->if_ad->addr);
+
+	/* Copied information from request */
+	hdr->flags = p->hdr->flags & htons(NHRP_FLAG_REGISTRATION_UNIQUE | NHRP_FLAG_REGISTRATION_NAT);
+	hdr->u.request_id = p->hdr->u.request_id;
+
+	/* Copy payload CIEs */
+	paylen = zbuf_used(&p->payload);
+	pay = zbuf_pushn(zb, paylen);
+	if (!pay) goto err;
+	memcpy(pay, zbuf_pulln(&p->payload, paylen), paylen);
+	zbuf_init(&payload, pay, paylen, paylen);
+
+	while ((cie = nhrp_cie_pull(&payload, hdr, &cie_nbma, &cie_proto)) != NULL) {
+		if (cie->prefix_length != 0xff && !(p->hdr->flags & htons(NHRP_FLAG_REGISTRATION_UNIQUE))) {
+			cie->code = NHRP_CODE_BINDING_NON_UNIQUE;
+			continue;
+		}
+
+		/* We currently support only unique prefix registrations */
+		if (cie->prefix_length != 0xff) {
+			cie->code = NHRP_CODE_ADMINISTRATIVELY_PROHIBITED;
+			continue;
+		}
+
+		proto_addr = (sockunion_family(&cie_proto) == AF_UNSPEC) ? &p->src_proto : &cie_proto;
+		nbma_addr = (sockunion_family(&cie_nbma) == AF_UNSPEC) ? &p->src_nbma : &cie_nbma;
+		nbma_natoa = NULL;
+		if (natted) {
+			nbma_natoa = nbma_addr;
+			nbma_addr = &p->peer->vc->remote.nbma;
+		}
+
+		holdtime = htons(cie->holding_time);
+		if (!holdtime) holdtime = p->if_ad->holdtime;
+
+		c = nhrp_cache_get(ifp, proto_addr, 1);
+		if (!c) {
+			cie->code = NHRP_CODE_INSUFFICIENT_RESOURCES;
+			continue;
+		}
+
+		if (!nhrp_cache_update_binding(c, NHRP_CACHE_DYNAMIC, holdtime, nhrp_peer_ref(p->peer), htons(cie->mtu), nbma_natoa)) {
+			cie->code = NHRP_CODE_ADMINISTRATIVELY_PROHIBITED;
+			continue;
+		}
+
+		cie->code = NHRP_CODE_SUCCESS;
+	}
+
+	/* Handle extensions */
+	while ((ext = nhrp_ext_pull(&p->extensions, &payload)) != NULL) {
+		switch (htons(ext->type) & ~NHRP_EXTENSION_FLAG_COMPULSORY) {
+		case NHRP_EXTENSION_NAT_ADDRESS:
+			ext = nhrp_ext_push(zb, hdr, NHRP_EXTENSION_NAT_ADDRESS);
+			if (!ext) goto err;
+			zbuf_copy(zb, &payload, zbuf_used(&payload));
+			if (natted) {
+				nhrp_cie_push(zb, NHRP_CODE_SUCCESS,
+					&p->peer->vc->remote.nbma,
+					&p->src_proto);
+			}
+			nhrp_ext_complete(zb, ext);
+			break;
+		default:
+			if (nhrp_ext_reply(zb, hdr, ifp, ext, &payload) < 0)
+				goto err;
+			break;
+		}
+	}
+
+	nhrp_packet_complete(zb, hdr);
+	nhrp_peer_send(p->peer, zb);
+err:
+	zbuf_free(zb);
+}
+
+static int parse_ether_packet(struct zbuf *zb, uint16_t protocol_type, union sockunion *src, union sockunion *dst)
+{
+	switch (protocol_type) {
+	case ETH_P_IP: {
+			struct iphdr *iph = zbuf_pull(zb, struct iphdr);
+			if (iph) {
+				if (src) sockunion_set(src, AF_INET, (uint8_t*) &iph->saddr, sizeof(iph->saddr));
+				if (dst) sockunion_set(dst, AF_INET, (uint8_t*) &iph->daddr, sizeof(iph->daddr));
+			}
+		}
+		break;
+	case ETH_P_IPV6: {
+			struct ipv6hdr *iph = zbuf_pull(zb, struct ipv6hdr);
+			if (iph) {
+				if (src) sockunion_set(src, AF_INET6, (uint8_t*) &iph->saddr, sizeof(iph->saddr));
+				if (dst) sockunion_set(dst, AF_INET6, (uint8_t*) &iph->daddr, sizeof(iph->daddr));
+			}
+		}
+		break;
+	default:
+		return 0;
+	}
+	return 1;
+}
+
+void nhrp_peer_send_indication(struct interface *ifp, uint16_t protocol_type, struct zbuf *pkt)
+{
+	union sockunion dst;
+	struct zbuf *zb, payload;
+	struct nhrp_interface *nifp = ifp->info;
+	struct nhrp_afi_data *if_ad;
+	struct nhrp_packet_header *hdr;
+	struct nhrp_peer *p;
+	char buf[2][SU_ADDRSTRLEN];
+
+	if (!nifp->enabled) return;
+
+	payload = *pkt;
+	if (!parse_ether_packet(&payload, protocol_type, &dst, NULL))
+		return;
+
+	if (nhrp_route_address(ifp, &dst, NULL, &p) != NHRP_ROUTE_NBMA_NEXTHOP)
+		return;
+
+	if_ad = &nifp->afi[family2afi(sockunion_family(&dst))];
+	if (!(if_ad->flags & NHRP_IFF_REDIRECT)) {
+		debugf(NHRP_DEBUG_COMMON, "Send Traffic Indication to %s about packet to %s ignored",
+			sockunion2str(&p->vc->remote.nbma, buf[0], sizeof buf[0]),
+			sockunion2str(&dst, buf[1], sizeof buf[1]));
+		return;
+	}
+
+	debugf(NHRP_DEBUG_COMMON, "Send Traffic Indication to %s (online=%d) about packet to %s",
+		sockunion2str(&p->vc->remote.nbma, buf[0], sizeof buf[0]),
+		p->online,
+		sockunion2str(&dst, buf[1], sizeof buf[1]));
+
+	/* Create reply */
+	zb = zbuf_alloc(1500);
+	hdr = nhrp_packet_push(zb, NHRP_PACKET_TRAFFIC_INDICATION, &nifp->nbma, &if_ad->addr, &dst);
+	hdr->hop_count = 0;
+
+	/* Payload is the packet causing indication */
+	zbuf_copy(zb, pkt, zbuf_used(pkt));
+	nhrp_packet_complete(zb, hdr);
+	nhrp_peer_send(p, zb);
+	nhrp_peer_unref(p);
+	zbuf_free(zb);
+}
+
+static void nhrp_handle_error_ind(struct nhrp_packet_parser *pp)
+{
+	struct zbuf origmsg = pp->payload;
+	struct nhrp_packet_header *hdr;
+	struct nhrp_reqid *reqid;
+	union sockunion src_nbma, src_proto, dst_proto;
+	char buf[2][SU_ADDRSTRLEN];
+
+	hdr = nhrp_packet_pull(&origmsg, &src_nbma, &src_proto, &dst_proto);
+	if (!hdr) return;
+
+	debugf(NHRP_DEBUG_COMMON, "Error Indication from %s about packet to %s ignored",
+		sockunion2str(&pp->src_proto, buf[0], sizeof buf[0]),
+		sockunion2str(&dst_proto, buf[1], sizeof buf[1]));
+
+	reqid = nhrp_reqid_lookup(&nhrp_packet_reqid, htonl(hdr->u.request_id));
+	if (reqid)
+		reqid->cb(reqid, pp);
+}
+
+static void nhrp_handle_traffic_ind(struct nhrp_packet_parser *p)
+{
+	union sockunion dst;
+	char buf[2][SU_ADDRSTRLEN];
+
+	if (!parse_ether_packet(&p->payload, htons(p->hdr->protocol_type), NULL, &dst))
+		return;
+
+	debugf(NHRP_DEBUG_COMMON, "Traffic Indication from %s about packet to %s: %s",
+		sockunion2str(&p->src_proto, buf[0], sizeof buf[0]),
+		sockunion2str(&dst, buf[1], sizeof buf[1]),
+		(p->if_ad->flags & NHRP_IFF_SHORTCUT) ? "trying shortcut" : "ignored");
+
+	if (p->if_ad->flags & NHRP_IFF_SHORTCUT)
+		nhrp_shortcut_initiate(&dst);
+}
+
+enum packet_type_t {
+	PACKET_UNKNOWN = 0,
+	PACKET_REQUEST,
+	PACKET_REPLY,
+	PACKET_INDICATION,
+};
+
+static struct {
+	enum packet_type_t type;
+	const char *name;
+	void (*handler)(struct nhrp_packet_parser *);
+} packet_types[] = {
+	[NHRP_PACKET_RESOLUTION_REQUEST] = {
+		.type = PACKET_REQUEST,
+		.name = "Resolution-Request",
+		.handler = nhrp_handle_resolution_req,
+	},
+	[NHRP_PACKET_RESOLUTION_REPLY] = {
+		.type = PACKET_REPLY,
+		.name = "Resolution-Reply",
+	},
+	[NHRP_PACKET_REGISTRATION_REQUEST] = {
+		.type = PACKET_REQUEST,
+		.name = "Registration-Request",
+		.handler = nhrp_handle_registration_request,
+	},
+	[NHRP_PACKET_REGISTRATION_REPLY] = {
+		.type = PACKET_REPLY,
+		.name = "Registration-Reply",
+	},
+	[NHRP_PACKET_PURGE_REQUEST] = {
+		.type = PACKET_REQUEST,
+		.name = "Purge-Request",
+	},
+	[NHRP_PACKET_PURGE_REPLY] = {
+		.type = PACKET_REPLY,
+		.name = "Purge-Reply",
+	},
+	[NHRP_PACKET_ERROR_INDICATION] = {
+		.type = PACKET_INDICATION,
+		.name = "Error-Indication",
+		.handler = nhrp_handle_error_ind,
+	},
+	[NHRP_PACKET_TRAFFIC_INDICATION] = {
+		.type = PACKET_INDICATION,
+		.name = "Traffic-Indication",
+		.handler = nhrp_handle_traffic_ind,
+	}
+};
+
+static void nhrp_peer_forward(struct nhrp_peer *p, struct nhrp_packet_parser *pp)
+{
+	struct zbuf *zb, extpl;
+	struct nhrp_packet_header *hdr;
+	struct nhrp_extension_header *ext, *dst;
+	struct nhrp_cie_header *cie;
+	struct nhrp_interface *nifp = pp->ifp->info;
+	struct nhrp_afi_data *if_ad = pp->if_ad;
+	union sockunion cie_nbma, cie_protocol;
+	uint16_t type, len;
+
+	if (pp->hdr->hop_count == 0)
+		return;
+
+	/* Create forward packet - copy header */
+	zb = zbuf_alloc(1500);
+	hdr = nhrp_packet_push(zb, pp->hdr->type, &pp->src_nbma, &pp->src_proto, &pp->dst_proto);
+	hdr->flags = pp->hdr->flags;
+	hdr->hop_count = pp->hdr->hop_count - 1;
+	hdr->u.request_id = pp->hdr->u.request_id;
+
+	/* Copy payload */
+	zbuf_copy(zb, &pp->payload, zbuf_used(&pp->payload));
+
+	/* Copy extensions */
+	while ((ext = nhrp_ext_pull(&pp->extensions, &extpl)) != NULL) {
+		type = htons(ext->type) & ~NHRP_EXTENSION_FLAG_COMPULSORY;
+		len = htons(ext->length);
+
+		if (type == NHRP_EXTENSION_END)
+			break;
+
+		dst = nhrp_ext_push(zb, hdr, htons(ext->type));
+		if (!dst) goto err;
+
+		switch (type) {
+		case NHRP_EXTENSION_FORWARD_TRANSIT_NHS:
+		case NHRP_EXTENSION_REVERSE_TRANSIT_NHS:
+			zbuf_put(zb, extpl.head, len);
+			if ((type == NHRP_EXTENSION_REVERSE_TRANSIT_NHS) ==
+			    (packet_types[hdr->type].type == PACKET_REPLY)) {
+				/* Check NHS list for forwarding loop */
+				while ((cie = nhrp_cie_pull(&extpl, pp->hdr, &cie_nbma, &cie_protocol)) != NULL) {
+					if (sockunion_same(&p->vc->remote.nbma, &cie_nbma))
+						goto err;
+				}
+				/* Append our selves to the list */
+				cie = nhrp_cie_push(zb, NHRP_CODE_SUCCESS, &nifp->nbma, &if_ad->addr);
+				if (!cie) goto err;
+				cie->holding_time = htons(if_ad->holdtime);
+			}
+			break;
+		default:
+			if (htons(ext->type) & NHRP_EXTENSION_FLAG_COMPULSORY)
+				/* FIXME: RFC says to just copy, but not
+				 * append our selves to the transit NHS list */
+				goto err;
+		case NHRP_EXTENSION_RESPONDER_ADDRESS:
+			/* Supported compulsory extensions, and any
+			 * non-compulsory that is not explicitly handled,
+			 * should be just copied. */
+			zbuf_copy(zb, &extpl, len);
+			break;
+		}
+		nhrp_ext_complete(zb, dst);
+	}
+
+	nhrp_packet_complete(zb, hdr);
+	nhrp_peer_send(p, zb);
+	zbuf_free(zb);
+	return;
+err:
+	nhrp_packet_debug(pp->pkt, "FWD-FAIL");
+	zbuf_free(zb);
+}
+
+static void nhrp_packet_debug(struct zbuf *zb, const char *dir)
+{
+	char buf[2][SU_ADDRSTRLEN];
+	union sockunion src_nbma, src_proto, dst_proto;
+	struct nhrp_packet_header *hdr;
+	struct zbuf zhdr;
+	int reply;
+
+	if (likely(!(debug_flags & NHRP_DEBUG_COMMON)))
+		return;
+
+	zbuf_init(&zhdr, zb->buf, zb->tail-zb->buf, zb->tail-zb->buf);
+	hdr = nhrp_packet_pull(&zhdr, &src_nbma, &src_proto, &dst_proto);
+
+	sockunion2str(&src_proto, buf[0], sizeof buf[0]);
+	sockunion2str(&dst_proto, buf[1], sizeof buf[1]);
+
+	reply = packet_types[hdr->type].type == PACKET_REPLY;
+	debugf(NHRP_DEBUG_COMMON, "%s %s(%d) %s -> %s",
+		dir,
+		packet_types[hdr->type].name ? : "Unknown",
+		hdr->type,
+		reply ? buf[1] : buf[0],
+		reply ? buf[0] : buf[1]);
+}
+
+struct nhrp_route_info {
+	int local;
+	struct interface *ifp;
+	struct nhrp_vc *vc;
+};
+
+void nhrp_peer_recv(struct nhrp_peer *p, struct zbuf *zb)
+{
+	char buf[2][SU_ADDRSTRLEN];
+	struct nhrp_packet_header *hdr;
+	struct nhrp_vc *vc = p->vc;
+	struct interface *ifp = p->ifp;
+	struct nhrp_interface *nifp = ifp->info;
+	struct nhrp_packet_parser pp;
+	struct nhrp_peer *peer = NULL;
+	struct nhrp_reqid *reqid;
+	const char *info = NULL;
+	union sockunion *target_addr;
+	unsigned paylen, extoff, extlen, realsize;
+	afi_t afi;
+
+	debugf(NHRP_DEBUG_KERNEL, "PACKET: Recv %s -> %s",
+		sockunion2str(&vc->remote.nbma, buf[0], sizeof buf[0]),
+		sockunion2str(&vc->local.nbma, buf[1], sizeof buf[1]));
+
+	if (!p->online) {
+		info = "peer not online";
+		goto drop;
+	}
+
+	if (nhrp_packet_calculate_checksum(zb->head, zbuf_used(zb)) != 0) {
+		info = "bad checksum";
+		goto drop;
+	}
+
+	realsize = zbuf_used(zb);
+	hdr = nhrp_packet_pull(zb, &pp.src_nbma, &pp.src_proto, &pp.dst_proto);
+	if (!hdr) {
+		info = "corrupt header";
+		goto drop;
+	}
+
+	pp.ifp = ifp;
+	pp.pkt = zb;
+	pp.hdr = hdr;
+	pp.peer = p;
+
+	afi = htons(hdr->afnum);
+	if (hdr->type > ZEBRA_NUM_OF(packet_types) ||
+	    hdr->version != NHRP_VERSION_RFC2332 ||
+	    afi >= AFI_MAX ||
+	    packet_types[hdr->type].type == PACKET_UNKNOWN ||
+	    htons(hdr->packet_size) > realsize) {
+		zlog_info("From %s: error: packet type %d, version %d, AFI %d, size %d (real size %d)",
+			   sockunion2str(&vc->remote.nbma, buf[0], sizeof buf[0]),
+			   (int) hdr->type, (int) hdr->version, (int) afi,
+			   (int) htons(hdr->packet_size),
+			   (int) realsize);
+		goto drop;
+	}
+	pp.if_ad = &((struct nhrp_interface *)ifp->info)->afi[afi];
+
+	extoff = htons(hdr->extension_offset);
+	if (extoff) {
+		if (extoff >= realsize) {
+			info = "extoff larger than packet";
+			goto drop;
+		}
+		paylen = extoff - (zb->head - zb->buf);
+	} else {
+		paylen = zbuf_used(zb);
+	}
+	zbuf_init(&pp.payload, zbuf_pulln(zb, paylen), paylen, paylen);
+	extlen = zbuf_used(zb);
+	zbuf_init(&pp.extensions, zbuf_pulln(zb, extlen), extlen, extlen);
+
+	if (!nifp->afi[afi].network_id) {
+		info = "nhrp not enabled";
+		goto drop;
+	}
+
+	nhrp_packet_debug(zb, "Recv");
+
+	/* FIXME: Check authentication here. This extension needs to be
+	 * pre-handled. */
+
+	/* Figure out if this is local */
+	target_addr = (packet_types[hdr->type].type == PACKET_REPLY) ? &pp.src_proto : &pp.dst_proto;
+
+	if (sockunion_same(&pp.src_proto, &pp.dst_proto))
+		pp.route_type = NHRP_ROUTE_LOCAL;
+	else
+		pp.route_type = nhrp_route_address(pp.ifp, target_addr, &pp.route_prefix, &peer);
+
+	switch (pp.route_type) {
+	case NHRP_ROUTE_LOCAL:
+		nhrp_packet_debug(zb, "!LOCAL");
+		if (packet_types[hdr->type].type == PACKET_REPLY) {
+			reqid = nhrp_reqid_lookup(&nhrp_packet_reqid, htonl(hdr->u.request_id));
+			if (reqid) {
+				reqid->cb(reqid, &pp);
+				break;
+			} else {
+				nhrp_packet_debug(zb, "!UNKNOWN-REQID");
+				/* FIXME: send error-indication */
+			}
+		}
+	case NHRP_ROUTE_OFF_NBMA:
+		if (packet_types[hdr->type].handler) {
+			packet_types[hdr->type].handler(&pp);
+			break;
+		}
+		break;
+	case NHRP_ROUTE_NBMA_NEXTHOP:
+		nhrp_peer_forward(peer, &pp);
+		break;
+	case NHRP_ROUTE_BLACKHOLE:
+		break;
+	}
+
+drop:
+	if (info) {
+		zlog_info("From %s: error: %s",
+			  sockunion2str(&vc->remote.nbma, buf[0], sizeof buf[0]),
+			  info);
+	}
+	if (peer) nhrp_peer_unref(peer);
+	zbuf_free(zb);
+}

nhrpd/nhrp_protocol.h

@@ -0,0 +1,128 @@
+/* nhrp_protocol.h - NHRP protocol definitions
+ *
+ * Copyright (c) 2007-2012 Timo Teräs <timo.teras@iki.fi>
+ *
+ * This software is licensed under the MIT License.
+ * See MIT-LICENSE.txt for additional details.
+ */
+
+#ifndef NHRP_PROTOCOL_H
+#define NHRP_PROTOCOL_H
+
+#include <stdint.h>
+
+/* NHRP Ethernet protocol number */
+#define ETH_P_NHRP				0x2001
+
+/* 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	1
+#define NHRP_ERROR_LOOP_DETECTED		2
+#define NHRP_ERROR_PROTOCOL_ADDRESS_UNREACHABLE	6
+#define NHRP_ERROR_PROTOCOL_ERROR		7
+#define NHRP_ERROR_SDU_SIZE_EXCEEDED		8
+#define NHRP_ERROR_INVALID_EXTENSION		9
+#define NHRP_ERROR_INVALID_RESOLUTION_REPLY	10
+#define NHRP_ERROR_AUTHENTICATION_FAILURE	11
+#define NHRP_ERROR_HOP_COUNT_EXCEEDED		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	0x8000
+#define NHRP_FLAG_RESOLUTION_AUTHORATIVE	0x4000
+#define NHRP_FLAG_RESOLUTION_DESTINATION_STABLE	0x2000
+#define NHRP_FLAG_RESOLUTION_UNIQUE		0x1000
+#define NHRP_FLAG_RESOLUTION_SOURCE_STABLE	0x0800
+#define NHRP_FLAG_RESOLUTION_NAT		0x0002
+
+/* NHRP Flags for Registration request/reply */
+#define NHRP_FLAG_REGISTRATION_UNIQUE		0x8000
+#define NHRP_FLAG_REGISTRATION_NAT		0x0002
+
+/* NHRP Flags for Purge request/reply */
+#define NHRP_FLAG_PURGE_NO_REPLY		0x8000
+
+/* NHRP Authentication extension types (ala Cisco) */
+#define NHRP_AUTHENTICATION_PLAINTEXT		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;
+} __attribute__((packed));
+
+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;
+} __attribute__((packed));
+
+struct nhrp_extension_header {
+	uint16_t	type;
+	uint16_t	length;
+} __attribute__((packed));
+
+struct nhrp_cisco_authentication_extension {
+	uint32_t	type;
+	uint8_t		secret[8];
+} __attribute__((packed));
+
+#endif

nhrpd/nhrp_route.c

@@ -0,0 +1,345 @@
+/* NHRP routing functions
+ * Copyright (c) 2014-2015 Timo Teräs
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include "nhrpd.h"
+#include "table.h"
+#include "memory.h"
+#include "stream.h"
+#include "log.h"
+#include "zclient.h"
+
+static struct zclient *zclient;
+static struct route_table *zebra_rib[AFI_MAX];
+
+struct route_info {
+	union sockunion via;
+	struct interface *ifp;
+	struct interface *nhrp_ifp;
+};
+
+static void nhrp_zebra_connected(struct zclient *zclient)
+{
+	/* No real VRF support yet -- bind only to the default vrf */
+	zclient_send_requests (zclient, VRF_DEFAULT);
+}
+
+static struct route_node *nhrp_route_update_get(const struct prefix *p, int create)
+{
+	struct route_node *rn;
+	afi_t afi = family2afi(PREFIX_FAMILY(p));
+
+	if (!zebra_rib[afi])
+		return NULL;
+
+	if (create) {
+		rn = route_node_get(zebra_rib[afi], p);
+		if (!rn->info) {
+			rn->info = XCALLOC(MTYPE_NHRP_ROUTE, sizeof(struct route_info));
+			route_lock_node(rn);
+		}
+		return rn;
+	} else {
+		return route_node_lookup(zebra_rib[afi], p);
+	}
+}
+
+static void nhrp_route_update_put(struct route_node *rn)
+{
+	struct route_info *ri = rn->info;
+
+	if (!ri->ifp && !ri->nhrp_ifp && sockunion_family(&ri->via) == AF_UNSPEC) {
+		XFREE(MTYPE_NHRP_ROUTE, rn->info);
+		rn->info = NULL;
+		route_unlock_node(rn);
+	}
+	route_unlock_node(rn);
+}
+
+static void nhrp_route_update_zebra(const struct prefix *p, union sockunion *nexthop, struct interface *ifp)
+{
+	struct route_node *rn;
+	struct route_info *ri;
+
+	rn = nhrp_route_update_get(p, (sockunion_family(nexthop) != AF_UNSPEC) || ifp);
+	if (rn) {
+		ri = rn->info;
+		ri->via = *nexthop;
+		ri->ifp = ifp;
+		nhrp_route_update_put(rn);
+	}
+}
+
+void nhrp_route_update_nhrp(const struct prefix *p, struct interface *ifp)
+{
+	struct route_node *rn;
+	struct route_info *ri;
+
+	rn = nhrp_route_update_get(p, ifp != NULL);
+	if (rn) {
+		ri = rn->info;
+		ri->nhrp_ifp = ifp;
+		nhrp_route_update_put(rn);
+	}
+}
+
+void nhrp_route_announce(int add, enum nhrp_cache_type type, const struct prefix *p, struct interface *ifp, const union sockunion *nexthop, uint32_t mtu)
+{
+	struct in_addr *nexthop_ipv4;
+	int flags = 0;
+
+	if (zclient->sock < 0)
+		return;
+
+	switch (type) {
+	case NHRP_CACHE_NEGATIVE:
+		SET_FLAG(flags, ZEBRA_FLAG_REJECT);
+		break;
+	case NHRP_CACHE_DYNAMIC:
+	case NHRP_CACHE_NHS:
+	case NHRP_CACHE_STATIC:
+		/* Regular route, so these are announced
+		 * to other routing daemons */
+		break;
+	default:
+		SET_FLAG(flags, ZEBRA_FLAG_FIB_OVERRIDE);
+		break;
+	}
+	SET_FLAG(flags, ZEBRA_FLAG_INTERNAL);
+
+	if (p->family == AF_INET) {
+		struct zapi_ipv4 api;
+
+		memset(&api, 0, sizeof(api));
+		api.flags = flags;
+		api.type = ZEBRA_ROUTE_NHRP;
+		api.safi = SAFI_UNICAST;
+
+		SET_FLAG(api.message, ZAPI_MESSAGE_NEXTHOP);
+		if (nexthop) {
+			SET_FLAG(api.message, ZAPI_MESSAGE_NEXTHOP);
+			nexthop_ipv4 = (struct in_addr *) sockunion_get_addr(nexthop);
+			api.nexthop_num = 1;
+			api.nexthop = &nexthop_ipv4;
+		}
+		if (ifp) {
+			SET_FLAG(api.message, ZAPI_MESSAGE_IFINDEX);
+			api.ifindex_num = 1;
+			api.ifindex = &ifp->ifindex;
+		}
+		if (mtu) {
+			SET_FLAG(api.message, ZAPI_MESSAGE_MTU);
+			api.mtu = mtu;
+		}
+
+		if (unlikely(debug_flags & NHRP_DEBUG_ROUTE)) {
+			char buf[2][INET_ADDRSTRLEN];
+			zlog_debug("Zebra send: IPv4 route %s %s/%d nexthop %s metric %u"
+				" count %d dev %s",
+				add ? "add" : "del",
+				inet_ntop(AF_INET, &p->u.prefix4, buf[0], sizeof(buf[0])),
+				p->prefixlen,
+				nexthop ? inet_ntop(AF_INET, api.nexthop[0], buf[1], sizeof(buf[1])) : "<onlink>",
+				api.metric, api.nexthop_num, ifp->name);
+		}
+
+		zapi_ipv4_route(
+			add ? ZEBRA_IPV4_ROUTE_ADD : ZEBRA_IPV4_ROUTE_DELETE,
+			zclient, (struct prefix_ipv4 *) p, &api);
+	}
+}
+
+int nhrp_route_read(int cmd, struct zclient *zclient, zebra_size_t length, vrf_id_t vrf_id)
+{
+	struct stream *s;
+	struct interface *ifp = NULL;
+	struct prefix prefix;
+	union sockunion nexthop_addr;
+	unsigned char message, nexthop_num, ifindex_num;
+	unsigned ifindex;
+	char buf[2][PREFIX_STRLEN];
+	int i, afaddrlen, added;
+
+	s = zclient->ibuf;
+	memset(&prefix, 0, sizeof(prefix));
+	sockunion_family(&nexthop_addr) = AF_UNSPEC;
+
+	/* Type, flags, message. */
+	/*type =*/ stream_getc(s);
+	/*flags =*/ stream_getc(s);
+	message = stream_getc(s);
+
+	/* Prefix */
+	switch (cmd) {
+	case ZEBRA_IPV4_ROUTE_ADD:
+	case ZEBRA_IPV4_ROUTE_DELETE:
+		prefix.family = AF_INET;
+		break;
+	case ZEBRA_IPV6_ROUTE_ADD:
+	case ZEBRA_IPV6_ROUTE_DELETE:
+		prefix.family = AF_INET6;
+		break;
+	default:
+		return -1;
+	}
+	afaddrlen = family2addrsize(prefix.family);
+	prefix.prefixlen = stream_getc(s);
+	stream_get(&prefix.u.val, s, PSIZE(prefix.prefixlen));
+
+	/* Nexthop, ifindex, distance, metric. */
+	if (CHECK_FLAG(message, ZAPI_MESSAGE_NEXTHOP|ZAPI_MESSAGE_IFINDEX)) {
+		nexthop_num = stream_getc(s);
+		for (i = 0; i < nexthop_num; i++) {
+			stream_get(buf[0], s, afaddrlen);
+			if (i == 0) sockunion_set(&nexthop_addr, prefix.family, (u_char*) buf[0], afaddrlen);
+		}
+		ifindex_num = stream_getc(s);
+		for (i = 0; i < ifindex_num; i++) {
+			ifindex = stream_getl(s);
+			if (i == 0 && ifindex != IFINDEX_INTERNAL)
+				ifp = if_lookup_by_index(ifindex);
+		}
+	}
+	if (CHECK_FLAG(message, ZAPI_MESSAGE_DISTANCE))
+		/*distance =*/ stream_getc(s);
+	if (CHECK_FLAG(message, ZAPI_MESSAGE_METRIC))
+		/*metric =*/ stream_getl(s);
+
+	added = (cmd == ZEBRA_IPV4_ROUTE_ADD || cmd == ZEBRA_IPV6_ROUTE_ADD);
+	debugf(NHRP_DEBUG_ROUTE, "if-route-%s: %s via %s dev %s",
+		added ? "add" : "del",
+		prefix2str(&prefix, buf[0], sizeof buf[0]),
+		sockunion2str(&nexthop_addr, buf[1], sizeof buf[1]),
+		ifp ? ifp->name : "(none)");
+
+	nhrp_route_update_zebra(&prefix, &nexthop_addr, ifp);
+	nhrp_shortcut_prefix_change(&prefix, !added);
+
+	return 0;
+}
+
+int nhrp_route_get_nexthop(const union sockunion *addr, struct prefix *p, union sockunion *via, struct interface **ifp)
+{
+	struct route_node *rn;
+	struct route_info *ri;
+	struct prefix lookup;
+	afi_t afi = family2afi(sockunion_family(addr));
+	char buf[PREFIX_STRLEN];
+
+	sockunion2hostprefix(addr, &lookup);
+
+	rn = route_node_match(zebra_rib[afi], &lookup);
+	if (!rn) return 0;
+
+	ri = rn->info;
+	if (ri->nhrp_ifp) {
+		debugf(NHRP_DEBUG_ROUTE, "lookup %s: nhrp_if=%s",
+			prefix2str(&lookup, buf, sizeof buf),
+			ri->nhrp_ifp->name);
+
+		if (via) sockunion_family(via) = AF_UNSPEC;
+		if (ifp) *ifp = ri->nhrp_ifp;
+	} else {
+		debugf(NHRP_DEBUG_ROUTE, "lookup %s: zebra route dev %s",
+			prefix2str(&lookup, buf, sizeof buf),
+			ri->ifp ? ri->ifp->name : "(none)");
+
+		if (via) *via = ri->via;
+		if (ifp) *ifp = ri->ifp;
+	}
+	if (p) *p = rn->p;
+	route_unlock_node(rn);
+	return 1;
+}
+
+enum nhrp_route_type nhrp_route_address(struct interface *in_ifp, union sockunion *addr, struct prefix *p, struct nhrp_peer **peer)
+{
+	struct interface *ifp = in_ifp;
+	struct nhrp_interface *nifp;
+	struct nhrp_cache *c;
+	union sockunion via[4];
+	uint32_t network_id = 0;
+	afi_t afi = family2afi(sockunion_family(addr));
+	int i;
+
+	if (ifp) {
+		nifp = ifp->info;
+		network_id = nifp->afi[afi].network_id;
+
+		c = nhrp_cache_get(ifp, addr, 0);
+		if (c && c->cur.type == NHRP_CACHE_LOCAL) {
+			if (p) memset(p, 0, sizeof(*p));
+			return NHRP_ROUTE_LOCAL;
+		}
+	}
+
+	for (i = 0; i < 4; i++) {
+		if (!nhrp_route_get_nexthop(addr, p, &via[i], &ifp))
+			return NHRP_ROUTE_BLACKHOLE;
+		if (ifp) {
+			/* Departing from nbma network? */
+			nifp = ifp->info;
+			if (network_id && network_id != nifp->afi[afi].network_id)
+				return NHRP_ROUTE_OFF_NBMA;
+		}
+		if (sockunion_family(&via[i]) == AF_UNSPEC)
+			break;
+		/* Resolve via node, but return the prefix of first match */
+		addr = &via[i];
+		p = NULL;
+	}
+
+	if (ifp) {
+		c = nhrp_cache_get(ifp, addr, 0);
+		if (c && c->cur.type >= NHRP_CACHE_DYNAMIC) {
+			if (p) memset(p, 0, sizeof(*p));
+			if (c->cur.type == NHRP_CACHE_LOCAL)
+				return NHRP_ROUTE_LOCAL;
+			if (peer) *peer = nhrp_peer_ref(c->cur.peer);
+			return NHRP_ROUTE_NBMA_NEXTHOP;
+		}
+	}
+
+	return NHRP_ROUTE_BLACKHOLE;
+}
+
+void nhrp_zebra_init(void)
+{
+	zebra_rib[AFI_IP] = route_table_init();
+	zebra_rib[AFI_IP6] = route_table_init();
+
+	zclient = zclient_new(master);
+	zclient->zebra_connected = nhrp_zebra_connected;
+	zclient->interface_add = nhrp_interface_add;
+	zclient->interface_delete = nhrp_interface_delete;
+	zclient->interface_up = nhrp_interface_up;
+	zclient->interface_down = nhrp_interface_down;
+	zclient->interface_address_add = nhrp_interface_address_add;
+	zclient->interface_address_delete = nhrp_interface_address_delete;
+	zclient->ipv4_route_add = nhrp_route_read;
+	zclient->ipv4_route_delete = nhrp_route_read;
+	zclient->ipv6_route_add = nhrp_route_read;
+	zclient->ipv6_route_delete = nhrp_route_read;
+
+	zclient_init(zclient, ZEBRA_ROUTE_NHRP);
+	zclient_redistribute(ZEBRA_REDISTRIBUTE_ADD, zclient, ZEBRA_ROUTE_KERNEL, VRF_DEFAULT);
+	zclient_redistribute(ZEBRA_REDISTRIBUTE_ADD, zclient, ZEBRA_ROUTE_CONNECT, VRF_DEFAULT);
+	zclient_redistribute(ZEBRA_REDISTRIBUTE_ADD, zclient, ZEBRA_ROUTE_STATIC, VRF_DEFAULT);
+	zclient_redistribute(ZEBRA_REDISTRIBUTE_ADD, zclient, ZEBRA_ROUTE_RIP, VRF_DEFAULT);
+	zclient_redistribute(ZEBRA_REDISTRIBUTE_ADD, zclient, ZEBRA_ROUTE_OSPF, VRF_DEFAULT);
+	zclient_redistribute(ZEBRA_REDISTRIBUTE_ADD, zclient, ZEBRA_ROUTE_ISIS, VRF_DEFAULT);
+	zclient_redistribute(ZEBRA_REDISTRIBUTE_ADD, zclient, ZEBRA_ROUTE_BGP, VRF_DEFAULT);
+}
+
+void nhrp_zebra_terminate(void)
+{
+	zclient_stop(zclient);
+	route_table_finish(zebra_rib[AFI_IP]);
+	route_table_finish(zebra_rib[AFI_IP6]);
+}
+

nhrpd/nhrp_shortcut.c

@@ -0,0 +1,402 @@
+/* NHRP shortcut related functions
+ * Copyright (c) 2014-2015 Timo Teräs
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include "nhrpd.h"
+#include "table.h"
+#include "memory.h"
+#include "thread.h"
+#include "log.h"
+#include "nhrp_protocol.h"
+
+static struct route_table *shortcut_rib[AFI_MAX];
+
+static int nhrp_shortcut_do_purge(struct thread *t);
+static void nhrp_shortcut_delete(struct nhrp_shortcut *s);
+static void nhrp_shortcut_send_resolution_req(struct nhrp_shortcut *s);
+
+static void nhrp_shortcut_check_use(struct nhrp_shortcut *s)
+{
+	char buf[PREFIX_STRLEN];
+
+	if (s->expiring && s->cache && s->cache->used) {
+		debugf(NHRP_DEBUG_ROUTE, "Shortcut %s used and expiring",
+			prefix2str(s->p, buf, sizeof buf));
+		nhrp_shortcut_send_resolution_req(s);
+	}
+}
+
+static int nhrp_shortcut_do_expire(struct thread *t)
+{
+	struct nhrp_shortcut *s = THREAD_ARG(t);
+
+	s->t_timer = NULL;
+	THREAD_TIMER_ON(master, s->t_timer, nhrp_shortcut_do_purge, s, s->holding_time/3);
+	s->expiring = 1;
+	nhrp_shortcut_check_use(s);
+
+	return 0;
+}
+
+static void nhrp_shortcut_cache_notify(struct notifier_block *n, unsigned long cmd)
+{
+	struct nhrp_shortcut *s = container_of(n, struct nhrp_shortcut, cache_notifier);
+
+	switch (cmd) {
+	case NOTIFY_CACHE_UP:
+		if (!s->route_installed) {
+			nhrp_route_announce(1, s->type, s->p, NULL, &s->cache->remote_addr, 0);
+			s->route_installed = 1;
+		}
+		break;
+	case NOTIFY_CACHE_USED:
+		nhrp_shortcut_check_use(s);
+		break;
+	case NOTIFY_CACHE_DOWN:
+	case NOTIFY_CACHE_DELETE:
+		if (s->route_installed) {
+			nhrp_route_announce(0, NHRP_CACHE_INVALID, s->p, NULL, NULL, 0);
+			s->route_installed = 0;
+		}
+		if (cmd == NOTIFY_CACHE_DELETE)
+			nhrp_shortcut_delete(s);
+		break;
+	}
+}
+
+static void nhrp_shortcut_update_binding(struct nhrp_shortcut *s, enum nhrp_cache_type type, struct nhrp_cache *c, int holding_time)
+{
+	s->type = type;
+	if (c != s->cache) {
+		if (s->cache) {
+			nhrp_cache_notify_del(s->cache, &s->cache_notifier);
+			s->cache = NULL;
+		}
+		s->cache = c;
+		if (s->cache) {
+			nhrp_cache_notify_add(s->cache, &s->cache_notifier, nhrp_shortcut_cache_notify);
+			if (s->cache->route_installed) {
+				/* Force renewal of Zebra announce on prefix change */
+				s->route_installed = 0;
+				nhrp_shortcut_cache_notify(&s->cache_notifier, NOTIFY_CACHE_UP);
+			}
+		}
+		if (!s->cache || !s->cache->route_installed)
+			nhrp_shortcut_cache_notify(&s->cache_notifier, NOTIFY_CACHE_DOWN);
+	}
+	if (s->type == NHRP_CACHE_NEGATIVE && !s->route_installed) {
+		nhrp_route_announce(1, s->type, s->p, NULL, NULL, 0);
+		s->route_installed = 1;
+	} else if (s->type == NHRP_CACHE_INVALID && s->route_installed) {
+		nhrp_route_announce(0, NHRP_CACHE_INVALID, s->p, NULL, NULL, 0);
+		s->route_installed = 0;
+	}
+
+	THREAD_OFF(s->t_timer);
+	if (holding_time) {
+		s->expiring = 0;
+		s->holding_time = holding_time;
+		THREAD_TIMER_ON(master, s->t_timer, nhrp_shortcut_do_expire, s, 2*holding_time/3);
+	}
+}
+
+static void nhrp_shortcut_delete(struct nhrp_shortcut *s)
+{
+	struct route_node *rn;
+	afi_t afi = family2afi(PREFIX_FAMILY(s->p));
+	char buf[PREFIX_STRLEN];
+
+	THREAD_OFF(s->t_timer);
+	nhrp_reqid_free(&nhrp_packet_reqid, &s->reqid);
+
+	debugf(NHRP_DEBUG_ROUTE, "Shortcut %s purged",
+		prefix2str(s->p, buf, sizeof buf));
+
+	nhrp_shortcut_update_binding(s, NHRP_CACHE_INVALID, NULL, 0);
+
+	/* Delete node */
+	rn = route_node_lookup(shortcut_rib[afi], s->p);
+	if (rn) {
+		XFREE(MTYPE_NHRP_SHORTCUT, rn->info);
+		rn->info = NULL;
+		route_unlock_node(rn);
+		route_unlock_node(rn);
+	}
+}
+
+static int nhrp_shortcut_do_purge(struct thread *t)
+{
+	struct nhrp_shortcut *s = THREAD_ARG(t);
+	s->t_timer = NULL;
+	nhrp_shortcut_delete(s);
+	return 0;
+}
+
+static struct nhrp_shortcut *nhrp_shortcut_get(struct prefix *p)
+{
+	struct nhrp_shortcut *s;
+	struct route_node *rn;
+	char buf[PREFIX_STRLEN];
+	afi_t afi = family2afi(PREFIX_FAMILY(p));
+
+	if (!shortcut_rib[afi])
+		return 0;
+
+	rn = route_node_get(shortcut_rib[afi], p);
+	if (!rn->info) {
+		s = rn->info = XCALLOC(MTYPE_NHRP_SHORTCUT, sizeof(struct nhrp_shortcut));
+		s->type = NHRP_CACHE_INVALID;
+		s->p = &rn->p;
+
+		debugf(NHRP_DEBUG_ROUTE, "Shortcut %s created",
+			prefix2str(s->p, buf, sizeof buf));
+	} else {
+		s = rn->info;
+		route_unlock_node(rn);
+	}
+	return s;
+}
+
+static void nhrp_shortcut_recv_resolution_rep(struct nhrp_reqid *reqid, void *arg)
+{
+	struct nhrp_packet_parser *pp = arg;
+	struct nhrp_shortcut *s = container_of(reqid, struct nhrp_shortcut, reqid);
+	struct nhrp_shortcut *ps;
+	struct nhrp_extension_header *ext;
+	struct nhrp_cie_header *cie;
+	struct nhrp_cache *c = NULL;
+	union sockunion *proto, cie_proto, *nbma, *nbma_natoa, cie_nbma, nat_nbma;
+	struct prefix prefix, route_prefix;
+	struct zbuf extpl;
+	char bufp[PREFIX_STRLEN], buf[3][SU_ADDRSTRLEN];
+	int holding_time = pp->if_ad->holdtime;
+
+	nhrp_reqid_free(&nhrp_packet_reqid, &s->reqid);
+	THREAD_OFF(s->t_timer);
+	THREAD_TIMER_ON(master, s->t_timer, nhrp_shortcut_do_purge, s, 1);
+
+	if (pp->hdr->type != NHRP_PACKET_RESOLUTION_REPLY) {
+		if (pp->hdr->type == NHRP_PACKET_ERROR_INDICATION &&
+		    pp->hdr->u.error.code == NHRP_ERROR_PROTOCOL_ADDRESS_UNREACHABLE) {
+			debugf(NHRP_DEBUG_COMMON, "Shortcut: Resolution: Protocol address unreachable");
+			nhrp_shortcut_update_binding(s, NHRP_CACHE_NEGATIVE, NULL, holding_time);
+		} else {
+			debugf(NHRP_DEBUG_COMMON, "Shortcut: Resolution failed");
+		}
+		return;
+	}
+
+	/* Parse extensions */
+	memset(&nat_nbma, 0, sizeof nat_nbma);
+	while ((ext = nhrp_ext_pull(&pp->extensions, &extpl)) != NULL) {
+		switch (htons(ext->type) & ~NHRP_EXTENSION_FLAG_COMPULSORY) {
+		case NHRP_EXTENSION_NAT_ADDRESS:
+			nhrp_cie_pull(&extpl, pp->hdr, &nat_nbma, &cie_proto);
+			break;
+		}
+	}
+
+	/* Minor sanity check */
+	prefix2sockunion(s->p, &cie_proto);
+	if (!sockunion_same(&cie_proto, &pp->dst_proto)) {
+		debugf(NHRP_DEBUG_COMMON, "Shortcut: Warning dst_proto altered from %s to %s",
+			sockunion2str(&cie_proto, buf[0], sizeof buf[0]),
+			sockunion2str(&pp->dst_proto, buf[1], sizeof buf[1]));
+	}
+
+	/* One or more CIEs should be given as reply, we support only one */
+	cie = nhrp_cie_pull(&pp->payload, pp->hdr, &cie_nbma, &cie_proto);
+	if (!cie || cie->code != NHRP_CODE_SUCCESS) {
+		debugf(NHRP_DEBUG_COMMON, "Shortcut: CIE code %d", cie ? cie->code : -1);
+		return;
+	}
+
+	proto = sockunion_family(&cie_proto) != AF_UNSPEC ? &cie_proto : &pp->dst_proto;
+	if (cie->holding_time)
+		holding_time = htons(cie->holding_time);
+
+	prefix = *s->p;
+	prefix.prefixlen = cie->prefix_length;
+
+	/* Sanity check prefix length */
+	if (prefix.prefixlen >= 8*prefix_blen(&prefix)) {
+		prefix.prefixlen = 8*prefix_blen(&prefix);
+	} else if (nhrp_route_address(NULL, &pp->dst_proto, &route_prefix, NULL) == NHRP_ROUTE_NBMA_NEXTHOP) {
+		if (prefix.prefixlen < route_prefix.prefixlen)
+			prefix.prefixlen = route_prefix.prefixlen;
+	}
+
+	debugf(NHRP_DEBUG_COMMON, "Shortcut: %s is at proto %s cie-nbma %s nat-nbma %s cie-holdtime %d",
+		prefix2str(&prefix, bufp, sizeof bufp),
+		sockunion2str(proto, buf[0], sizeof buf[0]),
+		sockunion2str(&cie_nbma, buf[1], sizeof buf[1]),
+		sockunion2str(&nat_nbma, buf[2], sizeof buf[2]),
+		htons(cie->holding_time));
+
+	/* Update cache entry for the protocol to nbma binding */
+	if (sockunion_family(&nat_nbma) != AF_UNSPEC) {
+		nbma = &nat_nbma;
+		nbma_natoa = &cie_nbma;
+	} else {
+		nbma = &cie_nbma;
+		nbma_natoa = NULL;
+	}
+	if (sockunion_family(nbma)) {
+		c = nhrp_cache_get(pp->ifp, proto, 1);
+		if (c) {
+			nhrp_cache_update_binding(
+					c, NHRP_CACHE_CACHED, holding_time,
+					nhrp_peer_get(pp->ifp, nbma),
+					htons(cie->mtu), nbma_natoa);
+		}
+	}
+
+	/* Update shortcut entry for subnet to protocol gw binding */
+	if (c && !sockunion_same(proto, &pp->dst_proto)) {
+		ps = nhrp_shortcut_get(&prefix);
+		if (ps) {
+			ps->addr = s->addr;
+			nhrp_shortcut_update_binding(ps, NHRP_CACHE_CACHED, c, holding_time);
+		}
+	}
+
+	debugf(NHRP_DEBUG_COMMON, "Shortcut: Resolution reply handled");
+}
+
+static void nhrp_shortcut_send_resolution_req(struct nhrp_shortcut *s)
+{
+	struct zbuf *zb;
+	struct nhrp_packet_header *hdr;
+	struct interface *ifp;
+	struct nhrp_interface *nifp;
+	struct nhrp_peer *peer;
+
+	if (nhrp_route_address(NULL, &s->addr, NULL, &peer) != NHRP_ROUTE_NBMA_NEXTHOP)
+		return;
+
+	if (s->type == NHRP_CACHE_INVALID || s->type == NHRP_CACHE_NEGATIVE)
+		s->type = NHRP_CACHE_INCOMPLETE;
+
+	ifp = peer->ifp;
+	nifp = ifp->info;
+
+	/* Create request */
+	zb = zbuf_alloc(1500);
+	hdr = nhrp_packet_push(zb, NHRP_PACKET_RESOLUTION_REQUEST,
+		&nifp->nbma, &nifp->afi[family2afi(sockunion_family(&s->addr))].addr, &s->addr);
+	hdr->u.request_id = htonl(nhrp_reqid_alloc(&nhrp_packet_reqid, &s->reqid, nhrp_shortcut_recv_resolution_rep));
+	hdr->flags = htons(NHRP_FLAG_RESOLUTION_SOURCE_IS_ROUTER |
+			   NHRP_FLAG_RESOLUTION_AUTHORATIVE |
+			   NHRP_FLAG_RESOLUTION_SOURCE_STABLE);
+
+	/* RFC2332 - One or zero CIEs, if CIE is present contains:
+	 *  - Prefix length: widest acceptable prefix we accept (if U set, 0xff)
+	 *  - MTU: MTU of the source station
+	 *  - Holding Time: Max time to cache the source information
+	 * */
+	/* FIXME: Send holding time, and MTU */
+
+	nhrp_ext_request(zb, hdr, ifp);
+
+	/* Cisco NAT detection extension */
+	hdr->flags |= htons(NHRP_FLAG_RESOLUTION_NAT);
+	nhrp_ext_push(zb, hdr, NHRP_EXTENSION_NAT_ADDRESS);
+
+	nhrp_packet_complete(zb, hdr);
+
+	nhrp_peer_send(peer, zb);
+	nhrp_peer_unref(peer);
+	zbuf_free(zb);
+}
+
+void nhrp_shortcut_initiate(union sockunion *addr)
+{
+	struct prefix p;
+	struct nhrp_shortcut *s;
+
+	sockunion2hostprefix(addr, &p);
+	s = nhrp_shortcut_get(&p);
+	if (s && s->type != NHRP_CACHE_INCOMPLETE) {
+		s->addr = *addr;
+		THREAD_OFF(s->t_timer);
+		THREAD_TIMER_ON(master, s->t_timer, nhrp_shortcut_do_purge, s, 30);
+		nhrp_shortcut_send_resolution_req(s);
+	}
+}
+
+void nhrp_shortcut_init(void)
+{
+	shortcut_rib[AFI_IP] = route_table_init();
+	shortcut_rib[AFI_IP6] = route_table_init();
+}
+
+void nhrp_shortcut_terminate(void)
+{
+	route_table_finish(shortcut_rib[AFI_IP]);
+	route_table_finish(shortcut_rib[AFI_IP6]);
+}
+
+void nhrp_shortcut_foreach(afi_t afi, void (*cb)(struct nhrp_shortcut *, void *), void *ctx)
+{
+	struct route_table *rt = shortcut_rib[afi];
+	struct route_node *rn;
+	route_table_iter_t iter;
+
+	if (!rt) return;
+
+	route_table_iter_init(&iter, rt);
+	while ((rn = route_table_iter_next(&iter)) != NULL) {
+		if (rn->info) cb(rn->info, ctx);
+	}
+	route_table_iter_cleanup(&iter);
+}
+
+struct purge_ctx {
+	const struct prefix *p;
+	int deleted;
+};
+
+void nhrp_shortcut_purge(struct nhrp_shortcut *s, int force)
+{
+	THREAD_OFF(s->t_timer);
+	nhrp_reqid_free(&nhrp_packet_reqid, &s->reqid);
+
+	if (force) {
+		/* Immediate purge on route with draw or pending shortcut */
+		THREAD_TIMER_MSEC_ON(master, s->t_timer, nhrp_shortcut_do_purge, s, 5);
+	} else {
+		/* Soft expire - force immediate renewal, but purge
+		 * in few seconds to make sure stale route is not
+		 * used too long. In practice most purges are caused
+		 * by hub bgp change, but target usually stays same.
+		 * This allows to keep nhrp route up, and to not
+		 * cause temporary rerouting via hubs causing latency
+		 * jitter. */
+		THREAD_TIMER_MSEC_ON(master, s->t_timer, nhrp_shortcut_do_purge, s, 3000);
+		s->expiring = 1;
+		nhrp_shortcut_check_use(s);
+	}
+}
+
+static void nhrp_shortcut_purge_prefix(struct nhrp_shortcut *s, void *ctx)
+{
+	struct purge_ctx *pctx = ctx;
+
+	if (prefix_match(pctx->p, s->p))
+		nhrp_shortcut_purge(s, pctx->deleted || !s->cache);
+}
+
+void nhrp_shortcut_prefix_change(const struct prefix *p, int deleted)
+{
+	struct purge_ctx pctx = {
+		.p = p,
+		.deleted = deleted,
+	};
+	nhrp_shortcut_foreach(family2afi(PREFIX_FAMILY(p)), nhrp_shortcut_purge_prefix, &pctx);
+}
+

nhrpd/nhrp_vc.c

@@ -0,0 +1,217 @@
+/* NHRP virtual connection
+ * Copyright (c) 2014-2015 Timo Teräs
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include "zebra.h"
+#include "memory.h"
+#include "stream.h"
+#include "hash.h"
+#include "thread.h"
+#include "jhash.h"
+
+#include "nhrpd.h"
+#include "os.h"
+
+struct child_sa {
+	uint32_t id;
+	struct nhrp_vc *vc;
+	struct list_head childlist_entry;
+};
+
+static struct hash *nhrp_vc_hash;
+static struct list_head childlist_head[512];
+
+static unsigned int nhrp_vc_key(void *peer_data)
+{
+	struct nhrp_vc *vc = peer_data;
+	return jhash_2words(
+		sockunion_hash(&vc->local.nbma),
+		sockunion_hash(&vc->remote.nbma),
+		0);
+}
+
+static int nhrp_vc_cmp(const void *cache_data, const void *key_data)
+{
+	const struct nhrp_vc *a = cache_data;
+	const struct nhrp_vc *b = key_data;
+	return	sockunion_same(&a->local.nbma, &b->local.nbma) &&
+		sockunion_same(&a->remote.nbma, &b->remote.nbma);
+}
+
+static void *nhrp_vc_alloc(void *data)
+{
+	struct nhrp_vc *vc, *key = data;
+
+	vc = XMALLOC(MTYPE_NHRP_VC, sizeof(struct nhrp_vc));
+	if (vc) {
+		*vc = (struct nhrp_vc) {
+			.local.nbma = key->local.nbma,
+			.remote.nbma = key->remote.nbma,
+			.notifier_list = NOTIFIER_LIST_INITIALIZER(&vc->notifier_list),
+		};
+	}
+
+	return vc;
+}
+
+static void nhrp_vc_free(void *data)
+{
+	XFREE(MTYPE_NHRP_VC, data);
+}
+
+struct nhrp_vc *nhrp_vc_get(const union sockunion *src, const union sockunion *dst, int create)
+{
+	struct nhrp_vc key;
+	key.local.nbma = *src;
+	key.remote.nbma = *dst;
+	return hash_get(nhrp_vc_hash, &key, create ? nhrp_vc_alloc : 0);
+}
+
+static void nhrp_vc_check_delete(struct nhrp_vc *vc)
+{
+	if (vc->updating || vc->ipsec || notifier_active(&vc->notifier_list))
+		return;
+	hash_release(nhrp_vc_hash, vc);
+	nhrp_vc_free(vc);
+}
+
+static void nhrp_vc_update(struct nhrp_vc *vc, long cmd)
+{
+	vc->updating = 1;
+	notifier_call(&vc->notifier_list, cmd);
+	vc->updating = 0;
+	nhrp_vc_check_delete(vc);
+}
+
+static void nhrp_vc_ipsec_reset(struct nhrp_vc *vc)
+{
+	vc->local.id[0] = 0;
+	vc->local.certlen = 0;
+	vc->remote.id[0] = 0;
+	vc->remote.certlen = 0;
+}
+
+int nhrp_vc_ipsec_updown(uint32_t child_id, struct nhrp_vc *vc)
+{
+	char buf[2][SU_ADDRSTRLEN];
+	struct child_sa *sa = NULL, *lsa;
+	uint32_t child_hash = child_id % ZEBRA_NUM_OF(childlist_head);
+	int abort_migration = 0;
+
+	list_for_each_entry(lsa, &childlist_head[child_hash], childlist_entry) {
+		if (lsa->id == child_id) {
+			sa = lsa;
+			break;
+		}
+	}
+
+	if (!sa) {
+		if (!vc) return 0;
+
+		sa = XMALLOC(MTYPE_NHRP_VC, sizeof(struct child_sa));
+		if (!sa) return 0;
+
+		*sa = (struct child_sa) {
+			.id = child_id,
+			.childlist_entry = LIST_INITIALIZER(sa->childlist_entry),
+			.vc = NULL,
+		};
+		list_add_tail(&sa->childlist_entry, &childlist_head[child_hash]);
+	}
+
+	if (sa->vc == vc)
+		return 0;
+
+	if (vc) {
+		/* Attach first to new VC */
+		vc->ipsec++;
+		nhrp_vc_update(vc, NOTIFY_VC_IPSEC_CHANGED);
+	}
+	if (sa->vc && vc) {
+		/* Notify old VC of migration */
+		sa->vc->abort_migration = 0;
+		debugf(NHRP_DEBUG_COMMON, "IPsec NBMA change of %s to %s",
+			sockunion2str(&sa->vc->remote.nbma, buf[0], sizeof buf[0]),
+			sockunion2str(&vc->remote.nbma, buf[1], sizeof buf[1]));
+		nhrp_vc_update(sa->vc, NOTIFY_VC_IPSEC_UPDATE_NBMA);
+		abort_migration = sa->vc->abort_migration;
+	}
+	if (sa->vc) {
+		/* Deattach old VC */
+		sa->vc->ipsec--;
+		if (!sa->vc->ipsec) nhrp_vc_ipsec_reset(sa->vc);
+		nhrp_vc_update(sa->vc, NOTIFY_VC_IPSEC_CHANGED);
+	}
+
+	/* Update */
+	sa->vc = vc;
+	if (!vc) {
+		list_del(&sa->childlist_entry);
+		XFREE(MTYPE_NHRP_VC, sa);
+	}
+
+	return abort_migration;
+}
+
+void nhrp_vc_notify_add(struct nhrp_vc *vc, struct notifier_block *n, notifier_fn_t action)
+{
+	notifier_add(n, &vc->notifier_list, action);
+}
+
+void nhrp_vc_notify_del(struct nhrp_vc *vc, struct notifier_block *n)
+{
+	notifier_del(n);
+	nhrp_vc_check_delete(vc);
+}
+
+
+struct nhrp_vc_iterator_ctx {
+	void (*cb)(struct nhrp_vc *, void *);
+	void *ctx;
+};
+
+static void nhrp_vc_iterator(struct hash_backet *b, void *ctx)
+{
+	struct nhrp_vc_iterator_ctx *ic = ctx;
+	ic->cb(b->data, ic->ctx);
+}
+
+void nhrp_vc_foreach(void (*cb)(struct nhrp_vc *, void *), void *ctx)
+{
+	struct nhrp_vc_iterator_ctx ic = {
+		.cb = cb,
+		.ctx = ctx,
+	};
+	hash_iterate(nhrp_vc_hash, nhrp_vc_iterator, &ic);
+}
+
+void nhrp_vc_init(void)
+{
+	size_t i;
+
+	nhrp_vc_hash = hash_create(nhrp_vc_key, nhrp_vc_cmp);
+	for (i = 0; i < ZEBRA_NUM_OF(childlist_head); i++)
+		list_init(&childlist_head[i]);
+}
+
+void nhrp_vc_reset(void)
+{
+	struct child_sa *sa, *n;
+	size_t i;
+
+	for (i = 0; i < ZEBRA_NUM_OF(childlist_head); i++) {
+		list_for_each_entry_safe(sa, n, &childlist_head[i], childlist_entry)
+			nhrp_vc_ipsec_updown(sa->id, 0);
+	}
+}
+
+void nhrp_vc_terminate(void)
+{
+	nhrp_vc_reset();
+	hash_clean(nhrp_vc_hash, nhrp_vc_free);
+}

nhrpd/nhrp_vty.c

@@ -0,0 +1,928 @@
+/* NHRP vty handling
+ * Copyright (c) 2014-2015 Timo Teräs
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include "zebra.h"
+#include "command.h"
+#include "zclient.h"
+#include "stream.h"
+
+#include "nhrpd.h"
+#include "netlink.h"
+
+static struct cmd_node zebra_node = {
+	.node   = ZEBRA_NODE,
+	.prompt = "%s(config-router)# ",
+	.vtysh  = 1,
+};
+
+static struct cmd_node nhrp_interface_node = {
+	.node   = INTERFACE_NODE,
+	.prompt = "%s(config-if)# ",
+	.vtysh  = 1,
+};
+
+#define NHRP_DEBUG_FLAGS_CMD "(all|common|event|interface|kernel|route|vici)"
+
+#define NHRP_DEBUG_FLAGS_STR		\
+	"All messages\n"		\
+	"Common messages (default)\n"	\
+	"Event manager messages\n"	\
+	"Interface messages\n"		\
+	"Kernel messages\n"		\
+	"Route messages\n"		\
+	"VICI messages\n"
+
+static const struct message debug_flags_desc[] = {
+	{ NHRP_DEBUG_ALL, "all" },
+	{ NHRP_DEBUG_COMMON, "common" },
+	{ NHRP_DEBUG_IF, "interface" },
+	{ NHRP_DEBUG_KERNEL, "kernel" },
+	{ NHRP_DEBUG_ROUTE, "route" },
+	{ NHRP_DEBUG_VICI, "vici" },
+	{ NHRP_DEBUG_EVENT, "event" },
+	{ 0, NULL },
+};
+
+static const struct message interface_flags_desc[] = {
+	{ NHRP_IFF_SHORTCUT, "shortcut" },
+	{ NHRP_IFF_REDIRECT, "redirect" },
+	{ NHRP_IFF_REG_NO_UNIQUE, "registration no-unique" },
+	{ 0, NULL },
+};
+
+static int nhrp_vty_return(struct vty *vty, int ret)
+{
+	static const char * const errmsgs[] = {
+		[NHRP_ERR_FAIL]				= "Command failed",
+		[NHRP_ERR_NO_MEMORY]			= "Out of memory",
+		[NHRP_ERR_UNSUPPORTED_INTERFACE]	= "NHRP not supported on this interface",
+		[NHRP_ERR_NHRP_NOT_ENABLED]		= "NHRP not enabled (set 'nhrp network-id' first)",
+		[NHRP_ERR_ENTRY_EXISTS]			= "Entry exists already",
+		[NHRP_ERR_ENTRY_NOT_FOUND]		= "Entry not found",
+		[NHRP_ERR_PROTOCOL_ADDRESS_MISMATCH]	= "Protocol address family does not match command (ip/ipv6 mismatch)",
+	};
+	const char *str = NULL;
+	char buf[256];
+
+	if (ret == NHRP_OK)
+		return CMD_SUCCESS;
+
+	if (ret > 0 && ret <= (int)ZEBRA_NUM_OF(errmsgs))
+		if (errmsgs[ret])
+			str = errmsgs[ret];
+
+	if (!str) {
+		str = buf;
+		snprintf(buf, sizeof(buf), "Unknown error %d", ret);
+	}
+
+	vty_out (vty, "%% %s%s", str, VTY_NEWLINE);
+
+	return CMD_WARNING;
+}
+
+static int toggle_flag(
+	struct vty *vty, const struct message *flag_desc,
+	const char *name, int on_off, unsigned *flags)
+{
+	int i;
+
+	for (i = 0; flag_desc[i].str != NULL; i++) {
+		if (strcmp(flag_desc[i].str, name) != 0)
+			continue;
+		if (on_off)
+			*flags |= flag_desc[i].key;
+		else
+			*flags &= ~flag_desc[i].key;
+		return CMD_SUCCESS;
+	}
+
+	vty_out(vty, "%% Invalid value %s%s", name, VTY_NEWLINE);
+	return CMD_WARNING;
+}
+
+#ifndef NO_DEBUG
+
+DEFUN(show_debugging_nhrp, show_debugging_nhrp_cmd,
+	"show debugging nhrp",
+	SHOW_STR
+	"Debugging information\n"
+	"NHRP configuration\n")
+{
+	int i;
+
+	vty_out(vty, "NHRP debugging status:%s", VTY_NEWLINE);
+
+	for (i = 0; debug_flags_desc[i].str != NULL; i++) {
+		if (debug_flags_desc[i].key == NHRP_DEBUG_ALL)
+			continue;
+		if (!(debug_flags_desc[i].key & debug_flags))
+			continue;
+
+		vty_out(vty, "  NHRP %s debugging is on%s",
+			debug_flags_desc[i].str, VTY_NEWLINE);
+	}
+
+	return CMD_SUCCESS;
+}
+
+DEFUN(debug_nhrp, debug_nhrp_cmd,
+	"debug nhrp " NHRP_DEBUG_FLAGS_CMD,
+	"Enable debug messages for specific or all parts.\n"
+	"NHRP information\n"
+	NHRP_DEBUG_FLAGS_STR)
+{
+	return toggle_flag(vty, debug_flags_desc, argv[0], 1, &debug_flags);
+}
+
+DEFUN(no_debug_nhrp, no_debug_nhrp_cmd,
+	"no debug nhrp " NHRP_DEBUG_FLAGS_CMD,
+	NO_STR
+	"Disable debug messages for specific or all parts.\n"
+	"NHRP information\n"
+	NHRP_DEBUG_FLAGS_STR)
+{
+	return toggle_flag(vty, debug_flags_desc, argv[0], 0, &debug_flags);
+}
+
+#endif /* NO_DEBUG */
+
+static int nhrp_config_write(struct vty *vty)
+{
+#ifndef NO_DEBUG
+	if (debug_flags == NHRP_DEBUG_ALL) {
+		vty_out(vty, "debug nhrp all%s", VTY_NEWLINE);
+	} else {
+		int i;
+
+		for (i = 0; debug_flags_desc[i].str != NULL; i++) {
+			if (debug_flags_desc[i].key == NHRP_DEBUG_ALL)
+				continue;
+			if (!(debug_flags & debug_flags_desc[i].key))
+				continue;
+			vty_out(vty, "debug nhrp %s%s", debug_flags_desc[i].str, VTY_NEWLINE);
+		}
+	}
+	vty_out(vty, "!%s", VTY_NEWLINE);
+#endif /* NO_DEBUG */
+
+	if (nhrp_event_socket_path) {
+		vty_out(vty, "nhrp event socket %s%s",
+			nhrp_event_socket_path, VTY_NEWLINE);
+	}
+	if (netlink_nflog_group) {
+		vty_out(vty, "nhrp nflog-group %d%s",
+			netlink_nflog_group, VTY_NEWLINE);
+	}
+
+	return 0;
+}
+
+#define IP_STR		"IP information\n"
+#define IPV6_STR	"IPv6 information\n"
+#define AFI_CMD		"(ip|ipv6)"
+#define AFI_STR		IP_STR IPV6_STR
+#define NHRP_STR	"Next Hop Resolution Protocol functions\n"
+
+static afi_t cmd_to_afi(const char *cmd)
+{
+	return strncmp(cmd, "ipv6", 4) == 0 ? AFI_IP6 : AFI_IP;
+}
+
+static const char *afi_to_cmd(afi_t afi)
+{
+	if (afi == AFI_IP6) return "ipv6";
+	return "ip";
+}
+
+DEFUN(nhrp_event_socket, nhrp_event_socket_cmd,
+	"nhrp event socket SOCKET",
+	NHRP_STR
+	"Event Manager commands\n"
+	"Event Manager unix socket path\n"
+	"Unix path for the socket")
+{
+	evmgr_set_socket(argv[0]);
+	return CMD_SUCCESS;
+}
+
+DEFUN(no_nhrp_event_socket, no_nhrp_event_socket_cmd,
+	"no nhrp event socket [SOCKET]",
+	NO_STR
+	NHRP_STR
+	"Event Manager commands\n"
+	"Event Manager unix socket path\n"
+	"Unix path for the socket")
+{
+	evmgr_set_socket(NULL);
+	return CMD_SUCCESS;
+}
+
+DEFUN(nhrp_nflog_group, nhrp_nflog_group_cmd,
+	"nhrp nflog-group <1-65535>",
+	NHRP_STR
+	"Specify NFLOG group number\n"
+	"NFLOG group number\n")
+{
+	uint32_t nfgroup;
+
+	VTY_GET_INTEGER_RANGE("nflog-group", nfgroup, argv[0], 1, 65535);
+	netlink_set_nflog_group(nfgroup);
+
+	return CMD_SUCCESS;
+}
+
+DEFUN(no_nhrp_nflog_group, no_nhrp_nflog_group_cmd,
+	"no nhrp nflog-group [<1-65535>]",
+	NO_STR
+	NHRP_STR
+	"Specify NFLOG group number\n"
+	"NFLOG group number\n")
+{
+	netlink_set_nflog_group(0);
+	return CMD_SUCCESS;
+}
+
+DEFUN(tunnel_protection, tunnel_protection_cmd,
+	"tunnel protection vici profile PROFILE {fallback-profile FALLBACK}",
+	"NHRP/GRE integration\n"
+	"IPsec protection\n"
+	"VICI (StrongSwan)\n"
+	"IPsec profile\n"
+	"IPsec profile name\n"
+	"Fallback IPsec profile\n"
+	"Fallback IPsec profile name\n")
+{
+	struct interface *ifp = vty->index;
+
+	nhrp_interface_set_protection(ifp, argv[0], argv[1]);
+	return CMD_SUCCESS;
+}
+
+DEFUN(no_tunnel_protection, no_tunnel_protection_cmd,
+	"no tunnel protection",
+	NO_STR
+	"NHRP/GRE integration\n"
+	"IPsec protection\n")
+{
+	struct interface *ifp = vty->index;
+
+	nhrp_interface_set_protection(ifp, NULL, NULL);
+	return CMD_SUCCESS;
+}
+
+DEFUN(tunnel_source, tunnel_source_cmd,
+	"tunnel source INTERFACE",
+	"NHRP/GRE integration\n"
+	"Tunnel device binding tracking\n"
+	"Interface name\n")
+{
+	struct interface *ifp = vty->index;
+	nhrp_interface_set_source(ifp, argv[0]);
+	return CMD_SUCCESS;
+}
+
+DEFUN(no_tunnel_source, no_tunnel_source_cmd,
+	"no tunnel source",
+	"NHRP/GRE integration\n"
+	"Tunnel device binding tracking\n"
+	"Interface name\n")
+{
+	struct interface *ifp = vty->index;
+	nhrp_interface_set_source(ifp, NULL);
+	return CMD_SUCCESS;
+}
+
+DEFUN(if_nhrp_network_id, if_nhrp_network_id_cmd,
+	AFI_CMD " nhrp network-id <1-4294967295>",
+	AFI_STR
+	NHRP_STR
+	"Enable NHRP and specify network-id\n"
+	"System local ID to specify interface group\n")
+{
+	struct interface *ifp = vty->index;
+	struct nhrp_interface *nifp = ifp->info;
+	afi_t afi = cmd_to_afi(argv[0]);
+
+	VTY_GET_INTEGER_RANGE("network-id", nifp->afi[afi].network_id, argv[1], 1, 4294967295);
+	nhrp_interface_update(ifp);
+
+	return CMD_SUCCESS;
+}
+
+DEFUN(if_no_nhrp_network_id, if_no_nhrp_network_id_cmd,
+	"no " AFI_CMD " nhrp network-id [<1-4294967295>]",
+	NO_STR
+	AFI_STR
+	NHRP_STR
+	"Enable NHRP and specify network-id\n"
+	"System local ID to specify interface group\n")
+{
+	struct interface *ifp = vty->index;
+	struct nhrp_interface *nifp = ifp->info;
+	afi_t afi = cmd_to_afi(argv[0]);
+
+	nifp->afi[afi].network_id = 0;
+	nhrp_interface_update(ifp);
+
+	return CMD_SUCCESS;
+}
+
+DEFUN(if_nhrp_flags, if_nhrp_flags_cmd,
+	AFI_CMD " nhrp (shortcut|redirect)",
+	AFI_STR
+	NHRP_STR
+	"Allow shortcut establishment\n"
+	"Send redirect notifications\n")
+{
+	struct interface *ifp = vty->index;
+	struct nhrp_interface *nifp = ifp->info;
+	afi_t afi = cmd_to_afi(argv[0]);
+
+	return toggle_flag(vty, interface_flags_desc, argv[1], 1, &nifp->afi[afi].flags);
+}
+
+DEFUN(if_no_nhrp_flags, if_no_nhrp_flags_cmd,
+	"no " AFI_CMD " nhrp (shortcut|redirect)",
+	NO_STR
+	AFI_STR
+	NHRP_STR
+	"Allow shortcut establishment\n"
+	"Send redirect notifications\n")
+{
+	struct interface *ifp = vty->index;
+	struct nhrp_interface *nifp = ifp->info;
+	afi_t afi = cmd_to_afi(argv[0]);
+
+	return toggle_flag(vty, interface_flags_desc, argv[1], 0, &nifp->afi[afi].flags);
+}
+
+DEFUN(if_nhrp_reg_flags, if_nhrp_reg_flags_cmd,
+	AFI_CMD " nhrp registration (no-unique)",
+	AFI_STR
+	NHRP_STR
+	"Registration configuration\n"
+	"Don't set unique flag\n")
+{
+	struct interface *ifp = vty->index;
+	struct nhrp_interface *nifp = ifp->info;
+	afi_t afi = cmd_to_afi(argv[0]);
+	char name[256];
+	snprintf(name, sizeof(name), "registration %s", argv[1]);
+	return toggle_flag(vty, interface_flags_desc, name, 1, &nifp->afi[afi].flags);
+}
+
+DEFUN(if_no_nhrp_reg_flags, if_no_nhrp_reg_flags_cmd,
+	"no " AFI_CMD " nhrp registration (no-unique)",
+	NO_STR
+	AFI_STR
+	NHRP_STR
+	"Registration configuration\n"
+	"Don't set unique flag\n")
+{
+	struct interface *ifp = vty->index;
+	struct nhrp_interface *nifp = ifp->info;
+	afi_t afi = cmd_to_afi(argv[0]);
+	char name[256];
+	snprintf(name, sizeof(name), "registration %s", argv[1]);
+	return toggle_flag(vty, interface_flags_desc, name, 0, &nifp->afi[afi].flags);
+}
+
+DEFUN(if_nhrp_holdtime, if_nhrp_holdtime_cmd,
+	AFI_CMD " nhrp holdtime <1-65000>",
+	AFI_STR
+	NHRP_STR
+	"Specify NBMA address validity time\n"
+	"Time in seconds that NBMA addresses are advertised valid\n")
+{
+	struct interface *ifp = vty->index;
+	struct nhrp_interface *nifp = ifp->info;
+	afi_t afi = cmd_to_afi(argv[0]);
+
+	VTY_GET_INTEGER_RANGE("holdtime", nifp->afi[afi].holdtime, argv[1], 1, 65000);
+	nhrp_interface_update(ifp);
+
+	return CMD_SUCCESS;
+}
+
+DEFUN(if_no_nhrp_holdtime, if_no_nhrp_holdtime_cmd,
+	"no " AFI_CMD " nhrp holdtime [1-65000]",
+	NO_STR
+	AFI_STR
+	NHRP_STR
+	"Specify NBMA address validity time\n"
+	"Time in seconds that NBMA addresses are advertised valid\n")
+{
+	struct interface *ifp = vty->index;
+	struct nhrp_interface *nifp = ifp->info;
+	afi_t afi = cmd_to_afi(argv[0]);
+
+	nifp->afi[afi].holdtime = NHRPD_DEFAULT_HOLDTIME;
+	nhrp_interface_update(ifp);
+
+	return CMD_SUCCESS;
+}
+
+DEFUN(if_nhrp_mtu, if_nhrp_mtu_cmd,
+	"ip nhrp mtu (<576-1500>|opennhrp)",
+	IP_STR
+	NHRP_STR
+	"Configure NHRP advertised MTU\n"
+	"MTU value\n"
+	"Advertise bound interface MTU similar to OpenNHRP")
+{
+	struct interface *ifp = vty->index;
+	struct nhrp_interface *nifp = ifp->info;
+
+	if (argv[0][0] == 'o') {
+		nifp->afi[AFI_IP].configured_mtu = -1;
+	} else {
+		VTY_GET_INTEGER_RANGE("mtu", nifp->afi[AFI_IP].configured_mtu, argv[0], 576, 1500);
+	}
+	nhrp_interface_update_mtu(ifp, AFI_IP);
+
+	return CMD_SUCCESS;
+}
+
+DEFUN(if_no_nhrp_mtu, if_no_nhrp_mtu_cmd,
+	"no ip nhrp mtu [(<576-1500>|opennhrp)]",
+	NO_STR
+	IP_STR
+	NHRP_STR
+	"Configure NHRP advertised MTU\n"
+	"MTU value\n"
+	"Advertise bound interface MTU similar to OpenNHRP")
+{
+	struct interface *ifp = vty->index;
+	struct nhrp_interface *nifp = ifp->info;
+
+	nifp->afi[AFI_IP].configured_mtu = 0;
+	nhrp_interface_update_mtu(ifp, AFI_IP);
+	return CMD_SUCCESS;
+}
+
+DEFUN(if_nhrp_map, if_nhrp_map_cmd,
+	AFI_CMD " nhrp map (A.B.C.D|X:X::X:X) (A.B.C.D|local)",
+	AFI_STR
+	NHRP_STR
+	"Nexthop Server configuration\n"
+	"IPv4 protocol address\n"
+	"IPv6 protocol address\n"
+	"IPv4 NBMA address\n"
+	"Handle protocol address locally\n")
+{
+	struct interface *ifp = vty->index;
+	afi_t afi = cmd_to_afi(argv[0]);
+	union sockunion proto_addr, nbma_addr;
+	struct nhrp_cache *c;
+
+	if (str2sockunion(argv[1], &proto_addr) < 0 ||
+	    afi2family(afi) != sockunion_family(&proto_addr))
+		return nhrp_vty_return(vty, NHRP_ERR_PROTOCOL_ADDRESS_MISMATCH);
+
+	c = nhrp_cache_get(ifp, &proto_addr, 1);
+	if (!c)
+		return nhrp_vty_return(vty, NHRP_ERR_FAIL);
+
+	c->map = 1;
+	if (strcmp(argv[2], "local") == 0) {
+		nhrp_cache_update_binding(c, NHRP_CACHE_LOCAL, 0, NULL, 0, NULL);
+	} else{
+		if (str2sockunion(argv[2], &nbma_addr) < 0)
+			return nhrp_vty_return(vty, NHRP_ERR_FAIL);
+		nhrp_cache_update_binding(c, NHRP_CACHE_STATIC, 0,
+			nhrp_peer_get(ifp, &nbma_addr), 0, NULL);
+	}
+
+	return CMD_SUCCESS;
+}
+
+DEFUN(if_nhrp_nhs, if_nhrp_nhs_cmd,
+	AFI_CMD " nhrp nhs (A.B.C.D|X:X::X:X|dynamic) nbma (A.B.C.D|FQDN)",
+	AFI_STR
+	NHRP_STR
+	"Nexthop Server configuration\n"
+	"IPv4 protocol address\n"
+	"IPv6 protocol address\n"
+	"Automatic detection of protocol address\n"
+	"IPv4 NBMA address\n"
+	"Fully qualified domain name for NBMA address(es)\n")
+{
+	struct interface *ifp = vty->index;
+	afi_t afi = cmd_to_afi(argv[0]);
+	union sockunion proto_addr;
+	int ret;
+
+	if (str2sockunion(argv[1], &proto_addr) < 0)
+		sockunion_family(&proto_addr) = AF_UNSPEC;
+
+	ret = nhrp_nhs_add(ifp, afi, &proto_addr, argv[2]);
+	return nhrp_vty_return(vty, ret);
+}
+
+DEFUN(if_no_nhrp_nhs, if_no_nhrp_nhs_cmd,
+	"no " AFI_CMD " nhrp nhs (A.B.C.D|X:X::X:X|dynamic) nbma (A.B.C.D|FQDN)",
+	NO_STR
+	AFI_STR
+	NHRP_STR
+	"Nexthop Server configuration\n"
+	"IPv4 protocol address\n"
+	"IPv6 protocol address\n"
+	"Automatic detection of protocol address\n"
+	"IPv4 NBMA address\n"
+	"Fully qualified domain name for NBMA address(es)\n")
+{
+	struct interface *ifp = vty->index;
+	afi_t afi = cmd_to_afi(argv[0]);
+	union sockunion proto_addr;
+	int ret;
+
+	if (str2sockunion(argv[1], &proto_addr) < 0)
+		sockunion_family(&proto_addr) = AF_UNSPEC;
+
+	ret = nhrp_nhs_del(ifp, afi, &proto_addr, argv[2]);
+	return nhrp_vty_return(vty, ret);
+}
+
+struct info_ctx {
+	struct vty *vty;
+	afi_t afi;
+	int count;
+};
+
+static void show_ip_nhrp_cache(struct nhrp_cache *c, void *pctx)
+{
+	struct info_ctx *ctx = pctx;
+	struct vty *vty = ctx->vty;
+	char buf[2][SU_ADDRSTRLEN];
+
+	if (ctx->afi != family2afi(sockunion_family(&c->remote_addr)))
+		return;
+
+	if (!ctx->count) {
+		vty_out(vty, "%-8s %-8s %-24s %-24s %-6s %s%s",
+			"Iface",
+			"Type",
+			"Protocol",
+			"NBMA",
+			"Flags",
+			"Identity",
+			VTY_NEWLINE);
+	}
+	ctx->count++;
+
+	vty_out(ctx->vty, "%-8s %-8s %-24s %-24s %c%c%c    %s%s",
+		c->ifp->name,
+		nhrp_cache_type_str[c->cur.type],
+		sockunion2str(&c->remote_addr, buf[0], sizeof buf[0]),
+		c->cur.peer ? sockunion2str(&c->cur.peer->vc->remote.nbma, buf[1], sizeof buf[1]) : "-",
+		c->used ? 'U' : ' ',
+		c->t_timeout ? 'T' : ' ',
+		c->t_auth ? 'A' : ' ',
+		c->cur.peer ? c->cur.peer->vc->remote.id : "-",
+		VTY_NEWLINE);
+}
+
+static void show_ip_opennhrp_cache(struct nhrp_cache *c, void *pctx)
+{
+	struct info_ctx *ctx = pctx;
+	struct vty *vty = ctx->vty;
+	char buf[SU_ADDRSTRLEN];
+
+	if (ctx->afi != family2afi(sockunion_family(&c->remote_addr)))
+		return;
+
+	vty_out(ctx->vty,
+		"Type: %s%s"
+		"Flags:%s%s%s"
+		"Protocol-Address: %s/%zu%s",
+		nhrp_cache_type_str[c->cur.type],
+		VTY_NEWLINE,
+		(c->cur.peer && c->cur.peer->online) ? " up": "",
+		c->used ? " used": "",
+		VTY_NEWLINE,
+		sockunion2str(&c->remote_addr, buf, sizeof buf),
+		8 * family2addrsize(sockunion_family(&c->remote_addr)),
+		VTY_NEWLINE);
+
+	if (c->cur.peer) {
+		vty_out(ctx->vty,
+			"NBMA-Address: %s%s",
+			sockunion2str(&c->cur.peer->vc->remote.nbma, buf, sizeof buf),
+			VTY_NEWLINE);
+	}
+
+	if (sockunion_family(&c->cur.remote_nbma_natoa) != AF_UNSPEC) {
+		vty_out(ctx->vty,
+			"NBMA-NAT-OA-Address: %s%s",
+			sockunion2str(&c->cur.remote_nbma_natoa, buf, sizeof buf),
+			VTY_NEWLINE);
+	}
+
+	vty_out(ctx->vty, "%s", VTY_NEWLINE);
+}
+
+static void show_ip_nhrp_shortcut(struct nhrp_shortcut *s, void *pctx)
+{
+	struct info_ctx *ctx = pctx;
+	struct nhrp_cache *c;
+	struct vty *vty = ctx->vty;
+	char buf1[PREFIX_STRLEN], buf2[SU_ADDRSTRLEN];
+
+	if (!ctx->count) {
+		vty_out(vty, "%-8s %-24s %-24s %s%s",
+			"Type",
+			"Prefix",
+			"Via",
+			"Identity",
+			VTY_NEWLINE);
+	}
+	ctx->count++;
+
+	c = s->cache;
+	vty_out(ctx->vty, "%-8s %-24s %-24s %s%s",
+		nhrp_cache_type_str[s->type],
+		prefix2str(s->p, buf1, sizeof buf1),
+		c ? sockunion2str(&c->remote_addr, buf2, sizeof buf2) : "",
+		(c && c->cur.peer) ? c->cur.peer->vc->remote.id : "",
+		VTY_NEWLINE);
+}
+
+DEFUN(show_ip_nhrp, show_ip_nhrp_cmd,
+	"show " AFI_CMD " nhrp (cache|shortcut|opennhrp|)",
+	SHOW_STR
+	AFI_STR
+	"NHRP information\n"
+	"Forwarding cache information\n"
+	"Shortcut information\n"
+	"opennhrpctl style cache dump\n")
+{
+	struct listnode *node;
+	struct interface *ifp;
+	struct info_ctx ctx = {
+		.vty = vty,
+		.afi = cmd_to_afi(argv[0]),
+	};
+
+	if (!argv[1] || argv[1][0] == 'c') {
+		for (ALL_LIST_ELEMENTS_RO(iflist, node, ifp))
+			nhrp_cache_foreach(ifp, show_ip_nhrp_cache, &ctx);
+	} else if (argv[1][0] == 'o') {
+		vty_out(vty, "Status: ok%s%s", VTY_NEWLINE, VTY_NEWLINE);
+		ctx.count++;
+		for (ALL_LIST_ELEMENTS_RO(iflist, node, ifp))
+			nhrp_cache_foreach(ifp, show_ip_opennhrp_cache, &ctx);
+	} else {
+		nhrp_shortcut_foreach(ctx.afi, show_ip_nhrp_shortcut, &ctx);
+	}
+
+	if (!ctx.count) {
+		vty_out(vty, "%% No entries%s", VTY_NEWLINE);
+		return CMD_WARNING;
+	}
+
+	return CMD_SUCCESS;
+}
+
+static void show_dmvpn_entry(struct nhrp_vc *vc, void *ctx)
+{
+	struct vty *vty = ctx;
+	char buf[2][SU_ADDRSTRLEN];
+
+	vty_out(vty, "%-24s %-24s %c      %-4d %-24s%s",
+		sockunion2str(&vc->local.nbma, buf[0], sizeof buf[0]),
+		sockunion2str(&vc->remote.nbma, buf[1], sizeof buf[1]),
+		notifier_active(&vc->notifier_list) ? 'n' : ' ',
+		vc->ipsec,
+		vc->remote.id,
+		VTY_NEWLINE);
+}
+
+DEFUN(show_dmvpn, show_dmvpn_cmd,
+	"show dmvpn",
+	SHOW_STR
+	"DMVPN information\n")
+{
+	vty_out(vty, "%-24s %-24s %-6s %-4s %-24s%s",
+		"Src",
+		"Dst",
+		"Flags",
+		"SAs",
+		"Identity",
+		VTY_NEWLINE);
+
+	nhrp_vc_foreach(show_dmvpn_entry, vty);
+
+	return CMD_SUCCESS;
+}
+
+static void clear_nhrp_cache(struct nhrp_cache *c, void *data)
+{
+	struct info_ctx *ctx = data;
+	if (c->cur.type <= NHRP_CACHE_CACHED) {
+		nhrp_cache_update_binding(c, c->cur.type, -1, NULL, 0, NULL);
+		ctx->count++;
+	}
+}
+
+static void clear_nhrp_shortcut(struct nhrp_shortcut *s, void *data)
+{
+	struct info_ctx *ctx = data;
+	nhrp_shortcut_purge(s, 1);
+	ctx->count++;
+}
+
+DEFUN(clear_nhrp, clear_nhrp_cmd,
+	"clear " AFI_CMD " nhrp (cache|shortcut)",
+	CLEAR_STR
+	AFI_STR
+	NHRP_STR
+	"Dynamic cache entries\n"
+	"Shortcut entries\n")
+{
+	struct listnode *node;
+	struct interface *ifp;
+	struct info_ctx ctx = {
+		.vty = vty,
+		.afi = cmd_to_afi(argv[0]),
+		.count = 0,
+	};
+
+	if (!argv[1] || argv[1][0] == 'c') {
+		for (ALL_LIST_ELEMENTS_RO(iflist, node, ifp))
+			nhrp_cache_foreach(ifp, clear_nhrp_cache, &ctx);
+	} else {
+		nhrp_shortcut_foreach(ctx.afi, clear_nhrp_shortcut, &ctx);
+	}
+
+	if (!ctx.count) {
+		vty_out(vty, "%% No entries%s", VTY_NEWLINE);
+		return CMD_WARNING;
+	}
+
+	vty_out(vty, "%% %d entries cleared%s", ctx.count, VTY_NEWLINE);
+	return CMD_SUCCESS;
+}
+
+struct write_map_ctx {
+	struct vty *vty;
+	int family;
+	const char *aficmd;
+};
+
+static void interface_config_write_nhrp_map(struct nhrp_cache *c, void *data)
+{
+	struct write_map_ctx *ctx = data;
+	struct vty *vty = ctx->vty;
+	char buf[2][SU_ADDRSTRLEN];
+
+	if (!c->map) return;
+	if (sockunion_family(&c->remote_addr) != ctx->family) return;
+
+	vty_out(vty, " %s nhrp map %s %s%s",
+		ctx->aficmd,
+		sockunion2str(&c->remote_addr, buf[0], sizeof buf[0]),
+		c->cur.type == NHRP_CACHE_LOCAL ? "local" :
+		sockunion2str(&c->cur.peer->vc->remote.nbma, buf[1], sizeof buf[1]),
+		VTY_NEWLINE);
+}
+
+static int interface_config_write(struct vty *vty)
+{
+	struct write_map_ctx mapctx;
+	struct listnode *node;
+	struct interface *ifp;
+	struct nhrp_interface *nifp;
+	struct nhrp_nhs *nhs;
+	const char *aficmd;
+	afi_t afi;
+	char buf[SU_ADDRSTRLEN];
+	int i;
+
+	for (ALL_LIST_ELEMENTS_RO(iflist, node, ifp)) {
+		vty_out(vty, "interface %s%s", ifp->name, VTY_NEWLINE);
+		if (ifp->desc)
+			vty_out(vty, " description %s%s", ifp->desc, VTY_NEWLINE);
+
+		nifp = ifp->info;
+		if (nifp->ipsec_profile) {
+			vty_out(vty, " tunnel protection vici profile %s",
+				nifp->ipsec_profile);
+			if (nifp->ipsec_fallback_profile)
+				vty_out(vty, " fallback-profile %s",
+					nifp->ipsec_fallback_profile);
+			vty_out(vty, "%s", VTY_NEWLINE);
+		}
+		if (nifp->source)
+			vty_out(vty, " tunnel source %s%s",
+				nifp->source, VTY_NEWLINE);
+
+		for (afi = 0; afi < AFI_MAX; afi++) {
+			struct nhrp_afi_data *ad = &nifp->afi[afi];
+
+			aficmd = afi_to_cmd(afi);
+
+			if (ad->network_id)
+				vty_out(vty, " %s nhrp network-id %u%s",
+					aficmd, ad->network_id,
+					VTY_NEWLINE);
+
+			if (ad->holdtime != NHRPD_DEFAULT_HOLDTIME)
+				vty_out(vty, " %s nhrp holdtime %u%s",
+					aficmd, ad->holdtime,
+					VTY_NEWLINE);
+
+			if (ad->configured_mtu < 0)
+				vty_out(vty, " %s nhrp mtu opennhrp%s",
+					aficmd, VTY_NEWLINE);
+			else if (ad->configured_mtu)
+				vty_out(vty, " %s nhrp mtu %u%s",
+					aficmd, ad->configured_mtu,
+					VTY_NEWLINE);
+
+			for (i = 0; interface_flags_desc[i].str != NULL; i++) {
+				if (!(ad->flags & interface_flags_desc[i].key))
+					continue;
+				vty_out(vty, " %s nhrp %s%s",
+					aficmd, interface_flags_desc[i].str, VTY_NEWLINE);
+			}
+
+			mapctx = (struct write_map_ctx) {
+				.vty = vty,
+				.family = afi2family(afi),
+				.aficmd = aficmd,
+			};
+			nhrp_cache_foreach(ifp, interface_config_write_nhrp_map, &mapctx);
+
+			list_for_each_entry(nhs, &ad->nhslist_head, nhslist_entry) {
+				vty_out(vty, " %s nhrp nhs %s nbma %s%s",
+					aficmd,
+					sockunion_family(&nhs->proto_addr) == AF_UNSPEC ? "dynamic" : sockunion2str(&nhs->proto_addr, buf, sizeof buf),
+					nhs->nbma_fqdn,
+					VTY_NEWLINE);
+			}
+		}
+
+		vty_out (vty, "!%s", VTY_NEWLINE);
+	}
+
+	return 0;
+}
+
+void nhrp_config_init(void)
+{
+	install_node(&zebra_node, nhrp_config_write);
+	install_default(ZEBRA_NODE);
+
+	/* global commands */
+	install_element(VIEW_NODE, &show_debugging_nhrp_cmd);
+	install_element(VIEW_NODE, &show_ip_nhrp_cmd);
+	install_element(VIEW_NODE, &show_dmvpn_cmd);
+	install_element(ENABLE_NODE, &show_debugging_nhrp_cmd);
+	install_element(ENABLE_NODE, &show_ip_nhrp_cmd);
+	install_element(ENABLE_NODE, &show_dmvpn_cmd);
+	install_element(ENABLE_NODE, &clear_nhrp_cmd);
+
+	install_element(ENABLE_NODE, &debug_nhrp_cmd);
+	install_element(ENABLE_NODE, &no_debug_nhrp_cmd);
+
+	install_element(CONFIG_NODE, &debug_nhrp_cmd);
+	install_element(CONFIG_NODE, &no_debug_nhrp_cmd);
+
+	install_element(CONFIG_NODE, &nhrp_event_socket_cmd);
+	install_element(CONFIG_NODE, &no_nhrp_event_socket_cmd);
+	install_element(CONFIG_NODE, &nhrp_nflog_group_cmd);
+	install_element(CONFIG_NODE, &no_nhrp_nflog_group_cmd);
+
+	/* interface specific commands */
+	install_node(&nhrp_interface_node, interface_config_write);
+	install_default(INTERFACE_NODE);
+
+	install_element(CONFIG_NODE, &interface_cmd);
+	install_element(CONFIG_NODE, &no_interface_cmd);
+	install_element(INTERFACE_NODE, &interface_cmd);
+	install_element(INTERFACE_NODE, &no_interface_cmd);
+	install_element(INTERFACE_NODE, &tunnel_protection_cmd);
+	install_element(INTERFACE_NODE, &no_tunnel_protection_cmd);
+	install_element(INTERFACE_NODE, &tunnel_source_cmd);
+	install_element(INTERFACE_NODE, &no_tunnel_source_cmd);
+	install_element(INTERFACE_NODE, &if_nhrp_network_id_cmd);
+	install_element(INTERFACE_NODE, &if_no_nhrp_network_id_cmd);
+	install_element(INTERFACE_NODE, &if_nhrp_holdtime_cmd);
+	install_element(INTERFACE_NODE, &if_no_nhrp_holdtime_cmd);
+	install_element(INTERFACE_NODE, &if_nhrp_mtu_cmd);
+	install_element(INTERFACE_NODE, &if_no_nhrp_mtu_cmd);
+	install_element(INTERFACE_NODE, &if_nhrp_flags_cmd);
+	install_element(INTERFACE_NODE, &if_no_nhrp_flags_cmd);
+	install_element(INTERFACE_NODE, &if_nhrp_reg_flags_cmd);
+	install_element(INTERFACE_NODE, &if_no_nhrp_reg_flags_cmd);
+	install_element(INTERFACE_NODE, &if_nhrp_map_cmd);
+	install_element(INTERFACE_NODE, &if_nhrp_nhs_cmd);
+	install_element(INTERFACE_NODE, &if_no_nhrp_nhs_cmd);
+}

nhrpd/nhrpd.h

@@ -0,0 +1,400 @@
+/* NHRP daemon internal structures and function prototypes
+ * Copyright (c) 2014-2015 Timo Teräs
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#ifndef NHRPD_H
+#define NHRPD_H
+
+#include "list.h"
+
+#include "zbuf.h"
+#include "zclient.h"
+#include "debug.h"
+
+#define NHRPD_DEFAULT_HOLDTIME	7200
+
+#define NHRP_VTY_PORT		2612
+#define NHRP_DEFAULT_CONFIG	"nhrpd.conf"
+
+extern struct thread_master *master;
+
+enum {
+	NHRP_OK = 0,
+	NHRP_ERR_FAIL,
+	NHRP_ERR_NO_MEMORY,
+	NHRP_ERR_UNSUPPORTED_INTERFACE,
+	NHRP_ERR_NHRP_NOT_ENABLED,
+	NHRP_ERR_ENTRY_EXISTS,
+	NHRP_ERR_ENTRY_NOT_FOUND,
+	NHRP_ERR_PROTOCOL_ADDRESS_MISMATCH,
+};
+
+struct notifier_block;
+
+typedef void (*notifier_fn_t)(struct notifier_block *, unsigned long);
+
+struct notifier_block {
+	struct list_head notifier_entry;
+	notifier_fn_t action;
+};
+
+struct notifier_list {
+	struct list_head notifier_head;
+};
+
+#define NOTIFIER_LIST_INITIALIZER(l) \
+	{ .notifier_head = LIST_INITIALIZER((l)->notifier_head) }
+
+static inline void notifier_init(struct notifier_list *l)
+{
+	list_init(&l->notifier_head);
+}
+
+static inline void notifier_add(struct notifier_block *n, struct notifier_list *l, notifier_fn_t action)
+{
+	n->action = action;
+	list_add_tail(&n->notifier_entry, &l->notifier_head);
+}
+
+static inline void notifier_del(struct notifier_block *n)
+{
+	list_del(&n->notifier_entry);
+}
+
+static inline void notifier_call(struct notifier_list *l, int cmd)
+{
+	struct notifier_block *n, *nn;
+	list_for_each_entry_safe(n, nn, &l->notifier_head, notifier_entry)
+		n->action(n, cmd);
+}
+
+static inline int notifier_active(struct notifier_list *l)
+{
+	return !list_empty(&l->notifier_head);
+}
+
+struct resolver_query {
+	void (*callback)(struct resolver_query *, int n, union sockunion *);
+};
+
+void resolver_init(void);
+void resolver_resolve(struct resolver_query *query, int af, const char *hostname, void (*cb)(struct resolver_query *, int, union sockunion *));
+
+void nhrp_zebra_init(void);
+void nhrp_zebra_terminate(void);
+
+struct zbuf;
+struct nhrp_vc;
+struct nhrp_cache;
+struct nhrp_nhs;
+struct nhrp_interface;
+
+#define MAX_ID_LENGTH			64
+#define MAX_CERT_LENGTH			2048
+
+enum nhrp_notify_type {
+	NOTIFY_INTERFACE_UP,
+	NOTIFY_INTERFACE_DOWN,
+	NOTIFY_INTERFACE_CHANGED,
+	NOTIFY_INTERFACE_ADDRESS_CHANGED,
+	NOTIFY_INTERFACE_NBMA_CHANGED,
+	NOTIFY_INTERFACE_MTU_CHANGED,
+
+	NOTIFY_VC_IPSEC_CHANGED,
+	NOTIFY_VC_IPSEC_UPDATE_NBMA,
+
+	NOTIFY_PEER_UP,
+	NOTIFY_PEER_DOWN,
+	NOTIFY_PEER_IFCONFIG_CHANGED,
+	NOTIFY_PEER_MTU_CHANGED,
+	NOTIFY_PEER_NBMA_CHANGING,
+
+	NOTIFY_CACHE_UP,
+	NOTIFY_CACHE_DOWN,
+	NOTIFY_CACHE_DELETE,
+	NOTIFY_CACHE_USED,
+	NOTIFY_CACHE_BINDING_CHANGE,
+};
+
+struct nhrp_vc {
+	struct notifier_list notifier_list;
+	uint8_t ipsec;
+	uint8_t updating;
+	uint8_t abort_migration;
+
+	struct nhrp_vc_peer {
+		union sockunion nbma;
+		char id[MAX_ID_LENGTH];
+		uint16_t certlen;
+		uint8_t cert[MAX_CERT_LENGTH];
+	} local, remote;
+};
+
+enum nhrp_route_type {
+	NHRP_ROUTE_BLACKHOLE,
+	NHRP_ROUTE_LOCAL,
+	NHRP_ROUTE_NBMA_NEXTHOP,
+	NHRP_ROUTE_OFF_NBMA,
+};
+
+struct nhrp_peer {
+	unsigned int ref;
+	unsigned online : 1;
+	unsigned requested : 1;
+	unsigned fallback_requested : 1;
+	unsigned prio : 1;
+	struct notifier_list notifier_list;
+	struct interface *ifp;
+	struct nhrp_vc *vc;
+	struct thread *t_fallback;
+	struct notifier_block vc_notifier, ifp_notifier;
+};
+
+struct nhrp_packet_parser {
+	struct interface *ifp;
+	struct nhrp_afi_data *if_ad;
+	struct nhrp_peer *peer;
+	struct zbuf *pkt;
+	struct zbuf payload;
+	struct zbuf extensions;
+	struct nhrp_packet_header *hdr;
+	enum nhrp_route_type route_type;
+	struct prefix route_prefix;
+	union sockunion src_nbma, src_proto, dst_proto;
+};
+
+struct nhrp_reqid_pool {
+	struct hash *reqid_hash;
+	uint32_t next_request_id;
+};
+
+struct nhrp_reqid {
+	uint32_t request_id;
+	void (*cb)(struct nhrp_reqid *, void *);
+};
+
+extern struct nhrp_reqid_pool nhrp_packet_reqid;
+extern struct nhrp_reqid_pool nhrp_event_reqid;
+
+enum nhrp_cache_type {
+	NHRP_CACHE_INVALID = 0,
+	NHRP_CACHE_INCOMPLETE,
+	NHRP_CACHE_NEGATIVE,
+	NHRP_CACHE_CACHED,
+	NHRP_CACHE_DYNAMIC,
+	NHRP_CACHE_NHS,
+	NHRP_CACHE_STATIC,
+	NHRP_CACHE_LOCAL,
+	NHRP_CACHE_NUM_TYPES
+};
+
+extern const char * const nhrp_cache_type_str[];
+extern unsigned long nhrp_cache_counts[NHRP_CACHE_NUM_TYPES];
+
+struct nhrp_cache {
+	struct interface *ifp;
+	union sockunion remote_addr;
+
+	unsigned map : 1;
+	unsigned used : 1;
+	unsigned route_installed : 1;
+	unsigned nhrp_route_installed : 1;
+
+	struct notifier_block peer_notifier;
+	struct notifier_block newpeer_notifier;
+	struct notifier_list notifier_list;
+	struct nhrp_reqid eventid;
+	struct thread *t_timeout;
+	struct thread *t_auth;
+
+	struct {
+		enum nhrp_cache_type type;
+		union sockunion remote_nbma_natoa;
+		struct nhrp_peer *peer;
+		time_t expires;
+		uint32_t mtu;
+	} cur, new;
+};
+
+struct nhrp_shortcut {
+	struct prefix *p;
+	union sockunion addr;
+
+	struct nhrp_reqid reqid;
+	struct thread *t_timer;
+
+	enum nhrp_cache_type type;
+	unsigned int holding_time;
+	unsigned route_installed : 1;
+	unsigned expiring : 1;
+
+	struct nhrp_cache *cache;
+	struct notifier_block cache_notifier;
+};
+
+struct nhrp_nhs {
+	struct interface *ifp;
+	struct list_head nhslist_entry;
+
+	unsigned hub : 1;
+	afi_t afi;
+	union sockunion proto_addr;
+	const char *nbma_fqdn;			/* IP-address or FQDN */
+
+	struct thread *t_resolve;
+	struct resolver_query dns_resolve;
+	struct list_head reglist_head;
+};
+
+#define NHRP_IFF_SHORTCUT		0x0001
+#define NHRP_IFF_REDIRECT		0x0002
+#define NHRP_IFF_REG_NO_UNIQUE		0x0100
+
+struct nhrp_interface {
+	struct interface *ifp;
+
+	unsigned enabled : 1;
+
+	char *ipsec_profile, *ipsec_fallback_profile, *source;
+	union sockunion nbma;
+	union sockunion nat_nbma;
+	unsigned int linkidx;
+	uint32_t grekey;
+
+	struct hash *peer_hash;
+	struct hash *cache_hash;
+
+	struct notifier_list notifier_list;
+
+	struct interface *nbmaifp;
+	struct notifier_block nbmanifp_notifier;
+
+	struct nhrp_afi_data {
+		unsigned flags;
+		unsigned short configured : 1;
+		union sockunion addr;
+		uint32_t network_id;
+		short configured_mtu;
+		unsigned short mtu;
+		unsigned int holdtime;
+		struct list_head nhslist_head;
+	} afi[AFI_MAX];
+};
+
+int sock_open_unix(const char *path);
+
+void nhrp_interface_init(void);
+void nhrp_interface_update(struct interface *ifp);
+void nhrp_interface_update_mtu(struct interface *ifp, afi_t afi);
+
+int nhrp_interface_add(int cmd, struct zclient *client, zebra_size_t length, vrf_id_t vrf_id);
+int nhrp_interface_delete(int cmd, struct zclient *client, zebra_size_t length, vrf_id_t vrf_id);
+int nhrp_interface_up(int cmd, struct zclient *client, zebra_size_t length, vrf_id_t vrf_id);
+int nhrp_interface_down(int cmd, struct zclient *client, zebra_size_t length, vrf_id_t vrf_id);
+int nhrp_interface_address_add(int cmd, struct zclient *client, zebra_size_t length, vrf_id_t vrf_id);
+int nhrp_interface_address_delete(int cmd, struct zclient *client, zebra_size_t length, vrf_id_t vrf_id);
+
+void nhrp_interface_notify_add(struct interface *ifp, struct notifier_block *n, notifier_fn_t fn);
+void nhrp_interface_notify_del(struct interface *ifp, struct notifier_block *n);
+void nhrp_interface_set_protection(struct interface *ifp, const char *profile, const char *fallback_profile);
+void nhrp_interface_set_source(struct interface *ifp, const char *ifname);
+
+int nhrp_nhs_add(struct interface *ifp, afi_t afi, union sockunion *proto_addr, const char *nbma_fqdn);
+int nhrp_nhs_del(struct interface *ifp, afi_t afi, union sockunion *proto_addr, const char *nbma_fqdn);
+int nhrp_nhs_free(struct nhrp_nhs *nhs);
+void nhrp_nhs_terminate(void);
+
+void nhrp_route_update_nhrp(const struct prefix *p, struct interface *ifp);
+void nhrp_route_announce(int add, enum nhrp_cache_type type, const struct prefix *p, struct interface *ifp, const union sockunion *nexthop, uint32_t mtu);
+int nhrp_route_read(int command, struct zclient *zclient, zebra_size_t length, vrf_id_t vrf_id);
+int nhrp_route_get_nexthop(const union sockunion *addr, struct prefix *p, union sockunion *via, struct interface **ifp);
+enum nhrp_route_type nhrp_route_address(struct interface *in_ifp, union sockunion *addr, struct prefix *p, struct nhrp_peer **peer);
+
+void nhrp_config_init(void);
+
+void nhrp_shortcut_init(void);
+void nhrp_shortcut_terminate(void);
+void nhrp_shortcut_initiate(union sockunion *addr);
+void nhrp_shortcut_foreach(afi_t afi, void (*cb)(struct nhrp_shortcut *, void *), void *ctx);
+void nhrp_shortcut_purge(struct nhrp_shortcut *s, int force);
+void nhrp_shortcut_prefix_change(const struct prefix *p, int deleted);
+
+struct nhrp_cache *nhrp_cache_get(struct interface *ifp, union sockunion *remote_addr, int create);
+void nhrp_cache_foreach(struct interface *ifp, void (*cb)(struct nhrp_cache *, void *), void *ctx);
+void nhrp_cache_set_used(struct nhrp_cache *, int);
+int nhrp_cache_update_binding(struct nhrp_cache *, enum nhrp_cache_type type, int holding_time, struct nhrp_peer *p, uint32_t mtu, union sockunion *nbma_natoa);
+void nhrp_cache_notify_add(struct nhrp_cache *c, struct notifier_block *, notifier_fn_t);
+void nhrp_cache_notify_del(struct nhrp_cache *c, struct notifier_block *);
+
+void nhrp_vc_init(void);
+void nhrp_vc_terminate(void);
+struct nhrp_vc *nhrp_vc_get(const union sockunion *src, const union sockunion *dst, int create);
+int nhrp_vc_ipsec_updown(uint32_t child_id, struct nhrp_vc *vc);
+void nhrp_vc_notify_add(struct nhrp_vc *, struct notifier_block *, notifier_fn_t);
+void nhrp_vc_notify_del(struct nhrp_vc *, struct notifier_block *);
+void nhrp_vc_foreach(void (*cb)(struct nhrp_vc *, void *), void *ctx);
+void nhrp_vc_reset(void);
+
+void vici_init(void);
+void vici_terminate(void);
+void vici_request_vc(const char *profile, union sockunion *src, union sockunion *dst, int prio);
+
+extern const char *nhrp_event_socket_path;
+
+void evmgr_init(void);
+void evmgr_terminate(void);
+void evmgr_set_socket(const char *socket);
+void evmgr_notify(const char *name, struct nhrp_cache *c, void (*cb)(struct nhrp_reqid *, void *));
+
+struct nhrp_packet_header *nhrp_packet_push(
+	struct zbuf *zb, uint8_t type,
+	const union sockunion *src_nbma,
+	const union sockunion *src_proto,
+	const union sockunion *dst_proto);
+void nhrp_packet_complete(struct zbuf *zb, struct nhrp_packet_header *hdr);
+uint16_t nhrp_packet_calculate_checksum(const uint8_t *pdu, uint16_t len);
+
+struct nhrp_packet_header *nhrp_packet_pull(
+	struct zbuf *zb,
+	union sockunion *src_nbma,
+	union sockunion *src_proto,
+	union sockunion *dst_proto);
+
+struct nhrp_cie_header *nhrp_cie_push(
+	struct zbuf *zb, uint8_t code,
+	const union sockunion *nbma,
+	const union sockunion *proto);
+struct nhrp_cie_header *nhrp_cie_pull(
+	struct zbuf *zb,
+	struct nhrp_packet_header *hdr,
+	union sockunion *nbma,
+	union sockunion *proto);
+
+struct nhrp_extension_header *nhrp_ext_push(struct zbuf *zb, struct nhrp_packet_header *hdr, uint16_t type);
+void nhrp_ext_complete(struct zbuf *zb, struct nhrp_extension_header *ext);
+struct nhrp_extension_header *nhrp_ext_pull(struct zbuf *zb, struct zbuf *payload);
+void nhrp_ext_request(struct zbuf *zb, struct nhrp_packet_header *hdr, struct interface *);
+int nhrp_ext_reply(struct zbuf *zb, struct nhrp_packet_header *hdr, struct interface *ifp, struct nhrp_extension_header *ext, struct zbuf *extpayload);
+
+uint32_t nhrp_reqid_alloc(struct nhrp_reqid_pool *, struct nhrp_reqid *r, void (*cb)(struct nhrp_reqid *, void *));
+void nhrp_reqid_free(struct nhrp_reqid_pool *, struct nhrp_reqid *r);
+struct nhrp_reqid *nhrp_reqid_lookup(struct nhrp_reqid_pool *, uint32_t reqid);
+
+int nhrp_packet_init(void);
+
+struct nhrp_peer *nhrp_peer_get(struct interface *ifp, const union sockunion *remote_nbma);
+struct nhrp_peer *nhrp_peer_ref(struct nhrp_peer *p);
+void nhrp_peer_unref(struct nhrp_peer *p);
+int nhrp_peer_check(struct nhrp_peer *p, int establish);
+void nhrp_peer_notify_add(struct nhrp_peer *p, struct notifier_block *, notifier_fn_t);
+void nhrp_peer_notify_del(struct nhrp_peer *p, struct notifier_block *);
+void nhrp_peer_recv(struct nhrp_peer *p, struct zbuf *zb);
+void nhrp_peer_send(struct nhrp_peer *p, struct zbuf *zb);
+void nhrp_peer_send_indication(struct interface *ifp, uint16_t, struct zbuf *);
+
+#endif

nhrpd/os.h

@@ -0,0 +1,5 @@
+
+int os_socket(void);
+int os_sendmsg(const uint8_t *buf, size_t len, int ifindex, const uint8_t *addr, size_t addrlen);
+int os_recvmsg(uint8_t *buf, size_t *len, int *ifindex, uint8_t *addr, size_t *addrlen);
+int os_configure_dmvpn(unsigned int ifindex, const char *ifname, int af);

nhrpd/reqid.c

@@ -0,0 +1,49 @@
+#include "zebra.h"
+#include "hash.h"
+#include "nhrpd.h"
+
+static unsigned int nhrp_reqid_key(void *data)
+{
+	struct nhrp_reqid *r = data;
+	return r->request_id;
+}
+
+static int nhrp_reqid_cmp(const void *data, const void *key)
+{
+	const struct nhrp_reqid *a = data, *b = key;
+	return a->request_id == b->request_id;
+}
+
+uint32_t nhrp_reqid_alloc(struct nhrp_reqid_pool *p, struct nhrp_reqid *r, void (*cb)(struct nhrp_reqid *, void *))
+{
+	if (!p->reqid_hash) {
+		p->reqid_hash = hash_create(nhrp_reqid_key, nhrp_reqid_cmp);
+		p->next_request_id = 1;
+	}
+
+	if (r->cb != cb) {
+		r->request_id = p->next_request_id;
+		if (++p->next_request_id == 0) p->next_request_id = 1;
+		r->cb = cb;
+		hash_get(p->reqid_hash, r, hash_alloc_intern);
+	}
+	return r->request_id;
+}
+
+void nhrp_reqid_free(struct nhrp_reqid_pool *p, struct nhrp_reqid *r)
+{
+	if (r->cb) {
+		hash_release(p->reqid_hash, r);
+		r->cb = NULL;
+	}
+}
+
+struct nhrp_reqid *nhrp_reqid_lookup(struct nhrp_reqid_pool *p, uint32_t reqid)
+{
+	struct nhrp_reqid key;
+	if (!p->reqid_hash) return 0;
+	key.request_id = reqid;
+	return hash_lookup(p->reqid_hash, &key);
+}
+
+

nhrpd/resolver.c

@@ -0,0 +1,190 @@
+/* C-Ares integration to Quagga mainloop
+ * Copyright (c) 2014-2015 Timo Teräs
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <ares.h>
+#include <ares_version.h>
+
+#include "vector.h"
+#include "thread.h"
+#include "nhrpd.h"
+
+struct resolver_state {
+	ares_channel channel;
+	struct thread *timeout;
+	vector read_threads, write_threads;
+};
+
+static struct resolver_state state;
+
+#define THREAD_RUNNING ((struct thread *)-1)
+
+static void resolver_update_timeouts(struct resolver_state *r);
+
+static int resolver_cb_timeout(struct thread *t)
+{
+	struct resolver_state *r = THREAD_ARG(t);
+
+	r->timeout = THREAD_RUNNING;
+	ares_process(r->channel, NULL, NULL);
+	r->timeout = NULL;
+	resolver_update_timeouts(r);
+
+	return 0;
+}
+
+static int resolver_cb_socket_readable(struct thread *t)
+{
+	struct resolver_state *r = THREAD_ARG(t);
+	int fd = THREAD_FD(t);
+
+	vector_set_index(r->read_threads, fd, THREAD_RUNNING);
+	ares_process_fd(r->channel, fd, ARES_SOCKET_BAD);
+	if (vector_lookup(r->read_threads, fd) == THREAD_RUNNING) {
+		t = NULL;
+		THREAD_READ_ON(master, t, resolver_cb_socket_readable, r, fd);
+		vector_set_index(r->read_threads, fd, t);
+	}
+	resolver_update_timeouts(r);
+
+	return 0;
+}
+
+static int resolver_cb_socket_writable(struct thread *t)
+{
+	struct resolver_state *r = THREAD_ARG(t);
+	int fd = THREAD_FD(t);
+
+	vector_set_index(r->write_threads, fd, THREAD_RUNNING);
+	ares_process_fd(r->channel, ARES_SOCKET_BAD, fd);
+	if (vector_lookup(r->write_threads, fd) == THREAD_RUNNING) {
+		t = NULL;
+		THREAD_WRITE_ON(master, t, resolver_cb_socket_writable, r, fd);
+		vector_set_index(r->write_threads, fd, t);
+	}
+	resolver_update_timeouts(r);
+
+	return 0;
+}
+
+static void resolver_update_timeouts(struct resolver_state *r)
+{
+	struct timeval *tv, tvbuf;
+
+	if (r->timeout == THREAD_RUNNING) return;
+
+	THREAD_OFF(r->timeout);
+	tv = ares_timeout(r->channel, NULL, &tvbuf);
+	if (tv) {
+		unsigned int timeoutms = tv->tv_sec * 1000 + tv->tv_usec / 1000;
+		THREAD_TIMER_MSEC_ON(master, r->timeout, resolver_cb_timeout, r, timeoutms);
+	}
+}
+
+static void ares_socket_cb(void *data, ares_socket_t fd, int readable, int writable)
+{
+	struct resolver_state *r = (struct resolver_state *) data;
+	struct thread *t;
+
+	if (readable) {
+		t = vector_lookup_ensure(r->read_threads, fd);
+		if (!t) {
+			THREAD_READ_ON(master, t, resolver_cb_socket_readable, r, fd);
+			vector_set_index(r->read_threads, fd, t);
+		}
+	} else {
+		t = vector_lookup(r->read_threads, fd);
+		if (t) {
+			if (t != THREAD_RUNNING) {
+				THREAD_OFF(t);
+			}
+			vector_unset(r->read_threads, fd);
+		}
+	}
+
+	if (writable) {
+		t = vector_lookup_ensure(r->write_threads, fd);
+		if (!t) {
+			THREAD_READ_ON(master, t, resolver_cb_socket_writable, r, fd);
+			vector_set_index(r->write_threads, fd, t);
+		}
+	} else {
+		t = vector_lookup(r->write_threads, fd);
+		if (t) {
+			if (t != THREAD_RUNNING) {
+				THREAD_OFF(t);
+			}
+			vector_unset(r->write_threads, fd);
+		}
+	}
+}
+
+void resolver_init(void)
+{
+	struct ares_options ares_opts;
+
+	state.read_threads = vector_init(1);
+	state.write_threads = vector_init(1);
+
+	ares_opts = (struct ares_options) {
+		.sock_state_cb = &ares_socket_cb,
+		.sock_state_cb_data = &state,
+		.timeout = 2,
+		.tries = 3,
+	};
+
+	ares_init_options(&state.channel, &ares_opts,
+		ARES_OPT_SOCK_STATE_CB | ARES_OPT_TIMEOUT |
+		ARES_OPT_TRIES);
+}
+
+
+static void ares_address_cb(void *arg, int status, int timeouts, struct hostent *he)
+{
+	struct resolver_query *query = (struct resolver_query *) arg;
+	union sockunion addr[16];
+	size_t i;
+
+	if (status != ARES_SUCCESS) {
+		debugf(NHRP_DEBUG_COMMON, "[%p] Resolving failed", query);
+		query->callback(query, -1, NULL);
+		query->callback = NULL;
+		return;
+	}
+
+	for (i = 0; he->h_addr_list[i] != NULL && i < ZEBRA_NUM_OF(addr); i++) {
+		memset(&addr[i], 0, sizeof(addr[i]));
+		addr[i].sa.sa_family = he->h_addrtype;
+		switch (he->h_addrtype) {
+		case AF_INET:
+			memcpy(&addr[i].sin.sin_addr, (uint8_t *) he->h_addr_list[i], he->h_length);
+			break;
+		case AF_INET6:
+			memcpy(&addr[i].sin6.sin6_addr, (uint8_t *) he->h_addr_list[i], he->h_length);
+			break;
+		}
+	}
+
+	debugf(NHRP_DEBUG_COMMON, "[%p] Resolved with %d results", query, (int) i);
+	query->callback(query, i, &addr[0]);
+	query->callback = NULL;
+}
+
+void resolver_resolve(struct resolver_query *query, int af, const char *hostname, void (*callback)(struct resolver_query *, int, union sockunion *))
+{
+	if (query->callback != NULL) {
+		zlog_err("Trying to resolve '%s', but previous query was not finished yet", hostname);
+		return;
+	}
+
+	debugf(NHRP_DEBUG_COMMON, "[%p] Resolving '%s'", query, hostname);
+
+	query->callback = callback;
+	ares_gethostbyname(state.channel, hostname, af, ares_address_cb, query);
+	resolver_update_timeouts(&state);
+}

nhrpd/vici.c

@@ -0,0 +1,482 @@
+/* strongSwan VICI protocol implementation for NHRP
+ * Copyright (c) 2014-2015 Timo Teräs
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <string.h>
+#include <sys/socket.h>
+#include <sys/un.h>
+
+#include "thread.h"
+#include "zbuf.h"
+#include "log.h"
+#include "nhrpd.h"
+
+#include "vici.h"
+
+#define ERRNO_IO_RETRY(EN) (((EN) == EAGAIN) || ((EN) == EWOULDBLOCK) || ((EN) == EINTR))
+
+struct blob {
+	char *ptr;
+	int len;
+};
+
+static int blob_equal(const struct blob *b, const char *str)
+{
+	if (b->len != (int) strlen(str)) return 0;
+	return memcmp(b->ptr, str, b->len) == 0;
+}
+
+static int blob2buf(const struct blob *b, char *buf, size_t n)
+{
+	if (b->len >= (int) n) return 0;
+	memcpy(buf, b->ptr, b->len);
+	buf[b->len] = 0;
+	return 1;
+}
+
+struct vici_conn {
+	struct thread *t_reconnect, *t_read, *t_write;
+	struct zbuf ibuf;
+	struct zbuf_queue obuf;
+	int fd;
+	uint8_t ibuf_data[VICI_MAX_MSGLEN];
+};
+
+struct vici_message_ctx {
+	const char *sections[8];
+	int nsections;
+};
+
+static int vici_reconnect(struct thread *t);
+static void vici_submit_request(struct vici_conn *vici, const char *name, ...);
+
+static void vici_zbuf_puts(struct zbuf *obuf, const char *str)
+{
+	size_t len = strlen(str);
+	zbuf_put8(obuf, len);
+	zbuf_put(obuf, str, len);
+}
+
+static void vici_connection_error(struct vici_conn *vici)
+{
+	nhrp_vc_reset();
+
+	THREAD_OFF(vici->t_read);
+	THREAD_OFF(vici->t_write);
+	zbuf_reset(&vici->ibuf);
+	zbufq_reset(&vici->obuf);
+
+	close(vici->fd);
+	vici->fd = -1;
+	THREAD_TIMER_ON(master, vici->t_reconnect, vici_reconnect, vici, 2);
+}
+
+static void vici_parse_message(
+	struct vici_conn *vici, struct zbuf *msg,
+	void (*parser)(struct vici_message_ctx *ctx, enum vici_type_t msgtype, const struct blob *key, const struct blob *val),
+	struct vici_message_ctx *ctx)
+{
+	uint8_t *type;
+	struct blob key;
+	struct blob val;
+
+	while ((type = zbuf_may_pull(msg, uint8_t)) != NULL) {
+		switch (*type) {
+		case VICI_SECTION_START:
+			key.len = zbuf_get8(msg);
+			key.ptr = zbuf_pulln(msg, key.len);
+			debugf(NHRP_DEBUG_VICI, "VICI: Section start '%.*s'", key.len, key.ptr);
+			parser(ctx, *type, &key, NULL);
+			ctx->nsections++;
+			break;
+		case VICI_SECTION_END:
+			debugf(NHRP_DEBUG_VICI, "VICI: Section end");
+			parser(ctx, *type, NULL, NULL);
+			ctx->nsections--;
+			break;
+		case VICI_KEY_VALUE:
+			key.len = zbuf_get8(msg);
+			key.ptr = zbuf_pulln(msg, key.len);
+			val.len = zbuf_get_be16(msg);
+			val.ptr = zbuf_pulln(msg, val.len);
+			debugf(NHRP_DEBUG_VICI, "VICI: Key '%.*s'='%.*s'", key.len, key.ptr, val.len, val.ptr);
+			parser(ctx, *type, &key, &val);
+			break;
+		case VICI_LIST_START:
+			key.len = zbuf_get8(msg);
+			key.ptr = zbuf_pulln(msg, key.len);
+			debugf(NHRP_DEBUG_VICI, "VICI: List start '%.*s'", key.len, key.ptr);
+			break;
+		case VICI_LIST_ITEM:
+			val.len = zbuf_get_be16(msg);
+			val.ptr = zbuf_pulln(msg, val.len);
+			debugf(NHRP_DEBUG_VICI, "VICI: List item: '%.*s'", val.len, val.ptr);
+			parser(ctx, *type, &key, &val);
+			break;
+		case VICI_LIST_END:
+			debugf(NHRP_DEBUG_VICI, "VICI: List end");
+			break;
+		default:
+			debugf(NHRP_DEBUG_VICI, "VICI: Unsupported message component type %d", *type);
+			return;
+		}
+	}
+}
+
+struct handle_sa_ctx {
+	struct vici_message_ctx msgctx;
+	int event;
+	int child_ok;
+	int kill_ikesa;
+	uint32_t child_uniqueid, ike_uniqueid;
+	struct {
+		union sockunion host;
+		struct blob id, cert;
+	} local, remote;
+};
+
+static void parse_sa_message(
+	struct vici_message_ctx *ctx,
+	enum vici_type_t msgtype,
+	const struct blob *key, const struct blob *val)
+{
+	struct handle_sa_ctx *sactx = container_of(ctx, struct handle_sa_ctx, msgctx);
+	struct nhrp_vc *vc;
+	char buf[512];
+
+	switch (msgtype) {
+	case VICI_SECTION_START:
+		if (ctx->nsections == 3) {
+			/* Begin of child-sa section, reset child vars */
+			sactx->child_uniqueid = 0;
+			sactx->child_ok = 0;
+		}
+		break;
+	case VICI_SECTION_END:
+		if (ctx->nsections == 3) {
+			/* End of child-sa section, update nhrp_vc */
+			int up = sactx->child_ok || sactx->event == 1;
+			if (up) {
+				vc = nhrp_vc_get(&sactx->local.host, &sactx->remote.host, up);
+				if (vc) {
+					blob2buf(&sactx->local.id, vc->local.id, sizeof(vc->local.id));
+					if (blob2buf(&sactx->local.cert, (char*)vc->local.cert, sizeof(vc->local.cert)))
+						vc->local.certlen = sactx->local.cert.len;
+					blob2buf(&sactx->remote.id, vc->remote.id, sizeof(vc->remote.id));
+					if (blob2buf(&sactx->remote.cert, (char*)vc->remote.cert, sizeof(vc->remote.cert)))
+						vc->remote.certlen = sactx->remote.cert.len;
+					sactx->kill_ikesa |= nhrp_vc_ipsec_updown(sactx->child_uniqueid, vc);
+				}
+			} else {
+				nhrp_vc_ipsec_updown(sactx->child_uniqueid, 0);
+			}
+		}
+		break;
+	default:
+		switch (key->ptr[0]) {
+		case 'l':
+			if (blob_equal(key, "local-host") && ctx->nsections == 1) {
+				if (blob2buf(val, buf, sizeof(buf)))
+					str2sockunion(buf, &sactx->local.host);
+			} else if (blob_equal(key, "local-id") && ctx->nsections == 1) {
+				sactx->local.id = *val;
+			} else if (blob_equal(key, "local-cert-data") && ctx->nsections == 1) {
+				sactx->local.cert = *val;
+			}
+			break;
+		case 'r':
+			if (blob_equal(key, "remote-host") && ctx->nsections == 1) {
+				if (blob2buf(val, buf, sizeof(buf)))
+					str2sockunion(buf, &sactx->remote.host);
+			} else if (blob_equal(key, "remote-id") && ctx->nsections == 1) {
+				sactx->remote.id = *val;
+			} else if (blob_equal(key, "remote-cert-data") && ctx->nsections == 1) {
+				sactx->remote.cert = *val;
+			}
+			break;
+		case 'u':
+			if (blob_equal(key, "uniqueid") && blob2buf(val, buf, sizeof(buf))) {
+				if (ctx->nsections == 3)
+					sactx->child_uniqueid = strtoul(buf, NULL, 0);
+				else if (ctx->nsections == 1)
+					sactx->ike_uniqueid = strtoul(buf, NULL, 0);
+			}
+			break;
+		case 's':
+			if (blob_equal(key, "state") && ctx->nsections == 3) {
+				sactx->child_ok =
+					(sactx->event == 0 &&
+					 (blob_equal(val, "INSTALLED") ||
+					  blob_equal(val, "REKEYED")));
+			}
+			break;
+		}
+		break;
+	}
+}
+
+static void vici_recv_sa(struct vici_conn *vici, struct zbuf *msg, int event)
+{
+	char buf[32];
+	struct handle_sa_ctx ctx = {
+		.event = event,
+	};
+
+	vici_parse_message(vici, msg, parse_sa_message, &ctx.msgctx);
+
+	if (ctx.kill_ikesa && ctx.ike_uniqueid) {
+		debugf(NHRP_DEBUG_COMMON, "VICI: Deleting IKE_SA %u", ctx.ike_uniqueid);
+		snprintf(buf, sizeof buf, "%u", ctx.ike_uniqueid);
+		vici_submit_request(
+			vici, "terminate",
+			VICI_KEY_VALUE, "ike-id", strlen(buf), buf,
+			VICI_END);
+	}
+}
+
+static void vici_recv_message(struct vici_conn *vici, struct zbuf *msg)
+{
+	uint32_t msglen;
+	uint8_t msgtype;
+	struct blob name;
+
+	msglen = zbuf_get_be32(msg);
+	msgtype = zbuf_get8(msg);
+	debugf(NHRP_DEBUG_VICI, "VICI: Message %d, %d bytes", msgtype, msglen);
+
+	switch (msgtype) {
+	case VICI_EVENT:
+		name.len = zbuf_get8(msg);
+		name.ptr = zbuf_pulln(msg, name.len);
+
+		debugf(NHRP_DEBUG_VICI, "VICI: Event '%.*s'", name.len, name.ptr);
+		if (blob_equal(&name, "list-sa") ||
+		    blob_equal(&name, "child-updown") ||
+		    blob_equal(&name, "child-rekey"))
+			vici_recv_sa(vici, msg, 0);
+		else if (blob_equal(&name, "child-state-installed") ||
+			 blob_equal(&name, "child-state-rekeyed"))
+			vici_recv_sa(vici, msg, 1);
+		else if (blob_equal(&name, "child-state-destroying"))
+			vici_recv_sa(vici, msg, 2);
+		break;
+	case VICI_EVENT_UNKNOWN:
+		zlog_err("VICI: StrongSwan does not support mandatory events (unpatched?)");
+		break;
+	case VICI_EVENT_CONFIRM:
+	case VICI_CMD_RESPONSE:
+		break;
+	default:
+		zlog_notice("VICI: Unrecognized message type %d", msgtype);
+		break;
+	}
+}
+
+static int vici_read(struct thread *t)
+{
+	struct vici_conn *vici = THREAD_ARG(t);
+	struct zbuf *ibuf = &vici->ibuf;
+	struct zbuf pktbuf;
+
+	vici->t_read = NULL;
+	if (zbuf_read(ibuf, vici->fd, (size_t) -1) < 0) {
+		vici_connection_error(vici);
+		return 0;
+	}
+
+	/* Process all messages in buffer */
+	do {
+		uint32_t *hdrlen = zbuf_may_pull(ibuf, uint32_t);
+		if (!hdrlen)
+			break;
+		if (!zbuf_may_pulln(ibuf, ntohl(*hdrlen))) {
+			zbuf_reset_head(ibuf, hdrlen);
+			break;
+		}
+
+		/* Handle packet */
+		zbuf_init(&pktbuf, hdrlen, htonl(*hdrlen)+4, htonl(*hdrlen)+4);
+		vici_recv_message(vici, &pktbuf);
+	} while (1);
+
+	THREAD_READ_ON(master, vici->t_read, vici_read, vici, vici->fd);
+	return 0;
+}
+
+static int vici_write(struct thread *t)
+{
+	struct vici_conn *vici = THREAD_ARG(t);
+	int r;
+
+	vici->t_write = NULL;
+	r = zbufq_write(&vici->obuf, vici->fd);
+	if (r > 0) {
+		THREAD_WRITE_ON(master, vici->t_write, vici_write, vici, vici->fd);
+	} else if (r < 0) {
+		vici_connection_error(vici);
+	}
+
+	return 0;
+}
+
+static void vici_submit(struct vici_conn *vici, struct zbuf *obuf)
+{
+	if (vici->fd < 0) {
+		zbuf_free(obuf);
+		return;
+	}
+
+	zbufq_queue(&vici->obuf, obuf);
+	THREAD_WRITE_ON(master, vici->t_write, vici_write, vici, vici->fd);
+}
+
+static void vici_submit_request(struct vici_conn *vici, const char *name, ...)
+{
+	struct zbuf *obuf;
+	uint32_t *hdrlen;
+	va_list va;
+	size_t len;
+	int type;
+
+	obuf = zbuf_alloc(256);
+	if (!obuf) return;
+
+	hdrlen = zbuf_push(obuf, uint32_t);
+	zbuf_put8(obuf, VICI_CMD_REQUEST);
+	vici_zbuf_puts(obuf, name);
+
+	va_start(va, name);
+	for (type = va_arg(va, int); type != VICI_END; type = va_arg(va, int)) {
+		zbuf_put8(obuf, type);
+		switch (type) {
+		case VICI_KEY_VALUE:
+			vici_zbuf_puts(obuf, va_arg(va, const char *));
+			len = va_arg(va, size_t);
+			zbuf_put_be16(obuf, len);
+			zbuf_put(obuf, va_arg(va, void *), len);
+			break;
+		case VICI_END:
+			break;
+		default:
+			break;
+		}
+	}
+	va_end(va);
+	*hdrlen = htonl(zbuf_used(obuf) - 4);
+	vici_submit(vici, obuf);
+}
+
+static void vici_register_event(struct vici_conn *vici, const char *name)
+{
+	struct zbuf *obuf;
+	uint32_t *hdrlen;
+	uint8_t namelen;
+
+	namelen = strlen(name);
+	obuf = zbuf_alloc(4 + 1 + 1 + namelen);
+	if (!obuf) return;
+
+	hdrlen = zbuf_push(obuf, uint32_t);
+	zbuf_put8(obuf, VICI_EVENT_REGISTER);
+	zbuf_put8(obuf, namelen);
+	zbuf_put(obuf, name, namelen);
+	*hdrlen = htonl(zbuf_used(obuf) - 4);
+
+	vici_submit(vici, obuf);
+}
+
+static int vici_reconnect(struct thread *t)
+{
+	struct vici_conn *vici = THREAD_ARG(t);
+	int fd;
+
+	vici->t_reconnect = NULL;
+	if (vici->fd >= 0) return 0;
+
+	fd = sock_open_unix("/var/run/charon.vici");
+	if (fd < 0) {
+		zlog_warn("%s: failure connecting VICI socket: %s",
+			__PRETTY_FUNCTION__, strerror(errno));
+		THREAD_TIMER_ON(master, vici->t_reconnect, vici_reconnect, vici, 2);
+		return 0;
+	}
+
+	debugf(NHRP_DEBUG_COMMON, "VICI: Connected");
+	vici->fd = fd;
+	THREAD_READ_ON(master, vici->t_read, vici_read, vici, vici->fd);
+
+	/* Send event subscribtions */
+	//vici_register_event(vici, "child-updown");
+	//vici_register_event(vici, "child-rekey");
+	vici_register_event(vici, "child-state-installed");
+	vici_register_event(vici, "child-state-rekeyed");
+	vici_register_event(vici, "child-state-destroying");
+	vici_register_event(vici, "list-sa");
+	vici_submit_request(vici, "list-sas", VICI_END);
+
+	return 0;
+}
+
+static struct vici_conn vici_connection;
+
+void vici_init(void)
+{
+	struct vici_conn *vici = &vici_connection;
+
+	vici->fd = -1;
+	zbuf_init(&vici->ibuf, vici->ibuf_data, sizeof(vici->ibuf_data), 0);
+	zbufq_init(&vici->obuf);
+	THREAD_TIMER_MSEC_ON(master, vici->t_reconnect, vici_reconnect, vici, 10);
+}
+
+void vici_terminate(void)
+{
+}
+
+void vici_request_vc(const char *profile, union sockunion *src, union sockunion *dst, int prio)
+{
+	struct vici_conn *vici = &vici_connection;
+	char buf[2][SU_ADDRSTRLEN];
+
+	sockunion2str(src, buf[0], sizeof buf[0]);
+	sockunion2str(dst, buf[1], sizeof buf[1]);
+
+	vici_submit_request(
+		vici, "initiate",
+		VICI_KEY_VALUE, "child", strlen(profile), profile,
+		VICI_KEY_VALUE, "timeout", 2, "-1",
+		VICI_KEY_VALUE, "async", 1, "1",
+		VICI_KEY_VALUE, "init-limits", 1, prio ? "0" : "1",
+		VICI_KEY_VALUE, "my-host", strlen(buf[0]), buf[0],
+		VICI_KEY_VALUE, "other-host", strlen(buf[1]), buf[1],
+		VICI_END);
+}
+
+int sock_open_unix(const char *path)
+{
+	int ret, fd;
+	struct sockaddr_un addr;
+
+	fd = socket(AF_UNIX, SOCK_STREAM, 0);
+	if (fd < 0)
+		return -1;
+
+	memset(&addr, 0, sizeof (struct sockaddr_un));
+	addr.sun_family = AF_UNIX;
+	strncpy(addr.sun_path, path, strlen (path));
+
+	ret = connect(fd, (struct sockaddr *) &addr, sizeof(addr.sun_family) + strlen(addr.sun_path));
+	if (ret < 0) {
+		close(fd);
+		return -1;
+	}
+
+	fcntl(fd, F_SETFL, fcntl(fd, F_GETFL, 0) | O_NONBLOCK);
+
+	return fd;
+}

nhrpd/vici.h

@@ -0,0 +1,24 @@
+
+enum vici_type_t {
+	VICI_START =         0,
+	VICI_SECTION_START = 1,
+	VICI_SECTION_END =   2,
+	VICI_KEY_VALUE =     3,
+	VICI_LIST_START =    4,
+	VICI_LIST_ITEM =     5,
+	VICI_LIST_END =      6,
+	VICI_END =           7
+};
+
+enum vici_operation_t {
+	VICI_CMD_REQUEST = 0,
+	VICI_CMD_RESPONSE,
+	VICI_CMD_UNKNOWN,
+	VICI_EVENT_REGISTER,
+	VICI_EVENT_UNREGISTER,
+	VICI_EVENT_CONFIRM,
+	VICI_EVENT_UNKNOWN,
+	VICI_EVENT,
+};
+
+#define VICI_MAX_MSGLEN		(512*1024)

nhrpd/zbuf.c

@@ -0,0 +1,219 @@
+/* Stream/packet buffer API implementation
+ * Copyright (c) 2014-2015 Timo Teräs
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#define _GNU_SOURCE
+#include <string.h>
+#include <unistd.h>
+#include <errno.h>
+#include "zassert.h"
+#include "zbuf.h"
+#include "memory.h"
+#include "memtypes.h"
+#include "nhrpd.h"
+
+#define ERRNO_IO_RETRY(EN) (((EN) == EAGAIN) || ((EN) == EWOULDBLOCK) || ((EN) == EINTR))
+
+struct zbuf *zbuf_alloc(size_t size)
+{
+	struct zbuf *zb;
+
+	zb = XMALLOC(MTYPE_STREAM_DATA, sizeof(*zb) + size);
+	if (!zb)
+		return NULL;
+
+	zbuf_init(zb, zb+1, size, 0);
+	zb->allocated = 1;
+
+	return zb;
+}
+
+void zbuf_init(struct zbuf *zb, void *buf, size_t len, size_t datalen)
+{
+	*zb = (struct zbuf) {
+		.buf = buf,
+		.end = (uint8_t *)buf + len,
+		.head = buf,
+		.tail = (uint8_t *)buf + datalen,
+	};
+}
+
+void zbuf_free(struct zbuf *zb)
+{
+	if (zb->allocated)
+		XFREE(MTYPE_STREAM_DATA, zb);
+}
+
+void zbuf_reset(struct zbuf *zb)
+{
+	zb->head = zb->tail = zb->buf;
+	zb->error = 0;
+}
+
+void zbuf_reset_head(struct zbuf *zb, void *ptr)
+{
+	zassert((void*)zb->buf <= ptr && ptr <= (void*)zb->tail);
+	zb->head = ptr;
+}
+
+static void zbuf_remove_headroom(struct zbuf *zb)
+{
+	ssize_t headroom = zbuf_headroom(zb);
+	if (!headroom)
+		return;
+	memmove(zb->buf, zb->head, zbuf_used(zb));
+	zb->head -= headroom;
+	zb->tail -= headroom;
+}
+
+ssize_t zbuf_read(struct zbuf *zb, int fd, size_t maxlen)
+{
+	ssize_t r;
+
+	if (zb->error)
+		return -3;
+
+	zbuf_remove_headroom(zb);
+	if (maxlen > zbuf_tailroom(zb))
+		maxlen = zbuf_tailroom(zb);
+
+	r = read(fd, zb->tail, maxlen);
+	if (r > 0) zb->tail += r;
+	else if (r == 0) r = -2;
+	else if (r < 0 && ERRNO_IO_RETRY(errno)) r = 0;
+
+	return r;
+}
+
+ssize_t zbuf_write(struct zbuf *zb, int fd)
+{
+	ssize_t r;
+
+	if (zb->error)
+		return -3;
+
+	r = write(fd, zb->head, zbuf_used(zb));
+	if (r > 0) {
+		zb->head += r;
+		if (zb->head == zb->tail)
+			zbuf_reset(zb);
+	}
+	else if (r == 0) r = -2;
+	else if (r < 0 && ERRNO_IO_RETRY(errno)) r = 0;
+
+	return r;
+}
+
+ssize_t zbuf_recv(struct zbuf *zb, int fd)
+{
+	ssize_t r;
+
+	if (zb->error)
+		return -3;
+
+	zbuf_remove_headroom(zb);
+	r = recv(fd, zb->tail, zbuf_tailroom(zb), 0);
+	if (r > 0) zb->tail += r;
+	else if (r == 0) r = -2;
+	else if (r < 0 && ERRNO_IO_RETRY(errno)) r = 0;
+	return r;
+}
+
+ssize_t zbuf_send(struct zbuf *zb, int fd)
+{
+	ssize_t r;
+
+	if (zb->error)
+		return -3;
+
+	r = send(fd, zb->head, zbuf_used(zb), 0);
+	if (r >= 0)
+		zbuf_reset(zb);
+
+	return r;
+}
+
+void *zbuf_may_pull_until(struct zbuf *zb, const char *sep, struct zbuf *msg)
+{
+	size_t seplen = strlen(sep), len;
+	uint8_t *ptr;
+
+	ptr = memmem(zb->head, zbuf_used(zb), sep, seplen);
+	if (!ptr) return NULL;
+
+	len = ptr - zb->head + seplen;
+	zbuf_init(msg, zbuf_pulln(zb, len), len, len);
+	return msg->head;
+}
+
+void zbufq_init(struct zbuf_queue *zbq)
+{
+	*zbq = (struct zbuf_queue) {
+		.queue_head = LIST_INITIALIZER(zbq->queue_head),
+	};
+}
+
+void zbufq_reset(struct zbuf_queue *zbq)
+{
+	struct zbuf *buf, *bufn;
+
+	list_for_each_entry_safe(buf, bufn, &zbq->queue_head, queue_list) {
+		list_del(&buf->queue_list);
+		zbuf_free(buf);
+	}
+}
+
+void zbufq_queue(struct zbuf_queue *zbq, struct zbuf *zb)
+{
+	list_add_tail(&zb->queue_list, &zbq->queue_head);
+}
+
+int zbufq_write(struct zbuf_queue *zbq, int fd)
+{
+	struct iovec iov[16];
+	struct zbuf *zb, *zbn;
+	ssize_t r;
+	size_t iovcnt = 0;
+
+	list_for_each_entry_safe(zb, zbn, &zbq->queue_head, queue_list) {
+		iov[iovcnt++] = (struct iovec) {
+			.iov_base = zb->head,
+			.iov_len = zbuf_used(zb),
+		};
+		if (iovcnt >= ZEBRA_NUM_OF(iov))
+			break;
+	}
+
+	r = writev(fd, iov, iovcnt);
+	if (r < 0)
+		return r;
+
+	list_for_each_entry_safe(zb, zbn, &zbq->queue_head, queue_list) {
+		if (r < (ssize_t)zbuf_used(zb)) {
+			zb->head += r;
+			return 1;
+		}
+
+		r -= zbuf_used(zb);
+		list_del(&zb->queue_list);
+		zbuf_free(zb);
+	}
+
+	return 0;
+}
+
+void zbuf_copy(struct zbuf *zdst, struct zbuf *zsrc, size_t len)
+{
+	const void *src;
+	void *dst;
+
+	dst = zbuf_pushn(zdst, len);
+	src = zbuf_pulln(zsrc, len);
+	if (!dst || !src) return;
+	memcpy(dst, src, len);
+}

nhrpd/zbuf.h

@@ -0,0 +1,189 @@
+/* Stream/packet buffer API
+ * Copyright (c) 2014-2015 Timo Teräs
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#ifndef ZBUF_H
+#define ZBUF_H
+
+#include <stdint.h>
+#include <string.h>
+#include <endian.h>
+#include <sys/types.h>
+
+#include "zassert.h"
+#include "list.h"
+
+struct zbuf {
+	struct list_head queue_list;
+	unsigned allocated : 1;
+	unsigned error : 1;
+	uint8_t *buf, *end;
+	uint8_t *head, *tail;
+};
+
+struct zbuf_queue {
+	struct list_head queue_head;
+};
+
+struct zbuf *zbuf_alloc(size_t size);
+void zbuf_init(struct zbuf *zb, void *buf, size_t len, size_t datalen);
+void zbuf_free(struct zbuf *zb);
+
+static inline size_t zbuf_size(struct zbuf *zb)
+{
+	return zb->end - zb->buf;
+}
+
+static inline size_t zbuf_used(struct zbuf *zb)
+{
+	return zb->tail - zb->head;
+}
+
+static inline size_t zbuf_tailroom(struct zbuf *zb)
+{
+	return zb->end - zb->tail;
+}
+
+static inline size_t zbuf_headroom(struct zbuf *zb)
+{
+	return zb->head - zb->buf;
+}
+
+void zbuf_reset(struct zbuf *zb);
+void zbuf_reset_head(struct zbuf *zb, void *ptr);
+ssize_t zbuf_read(struct zbuf *zb, int fd, size_t maxlen);
+ssize_t zbuf_write(struct zbuf *zb, int fd);
+ssize_t zbuf_recv(struct zbuf *zb, int fd);
+ssize_t zbuf_send(struct zbuf *zb, int fd);
+
+static inline void zbuf_set_rerror(struct zbuf *zb)
+{
+	zb->error = 1;
+	zb->head = zb->tail;
+}
+
+static inline void zbuf_set_werror(struct zbuf *zb)
+{
+	zb->error = 1;
+	zb->tail = zb->end;
+}
+
+static inline void *__zbuf_pull(struct zbuf *zb, size_t size, int error)
+{
+	void *head = zb->head;
+	if (size > zbuf_used(zb)) {
+		if (error) zbuf_set_rerror(zb);
+		return NULL;
+	}
+	zb->head += size;
+	return head;
+}
+
+#define zbuf_pull(zb, type) ((type *)__zbuf_pull(zb, sizeof(type), 1))
+#define zbuf_pulln(zb, sz) ((void *)__zbuf_pull(zb, sz, 1))
+#define zbuf_may_pull(zb, type) ((type *)__zbuf_pull(zb, sizeof(type), 0))
+#define zbuf_may_pulln(zb, sz) ((void *)__zbuf_pull(zb, sz, 0))
+
+void *zbuf_may_pull_until(struct zbuf *zb, const char *sep, struct zbuf *msg);
+
+static inline void zbuf_get(struct zbuf *zb, void *dst, size_t len)
+{
+	void *src = zbuf_pulln(zb, len);
+	if (src) memcpy(dst, src, len);
+}
+
+static inline uint8_t zbuf_get8(struct zbuf *zb)
+{
+	uint8_t *src = zbuf_pull(zb, uint8_t);
+	if (src) return *src;
+	return 0;
+}
+
+static inline uint32_t zbuf_get32(struct zbuf *zb)
+{
+	struct unaligned32 {
+		uint32_t value;
+	} __attribute__((packed));
+
+	struct unaligned32 *v = zbuf_pull(zb, struct unaligned32);
+	if (v) return v->value;
+	return 0;
+}
+
+static inline uint16_t zbuf_get_be16(struct zbuf *zb)
+{
+	struct unaligned16 {
+		uint16_t value;
+	} __attribute__((packed));
+
+	struct unaligned16 *v = zbuf_pull(zb, struct unaligned16);
+	if (v) return be16toh(v->value);
+	return 0;
+}
+
+static inline uint32_t zbuf_get_be32(struct zbuf *zb)
+{
+	return be32toh(zbuf_get32(zb));
+}
+
+static inline void *__zbuf_push(struct zbuf *zb, size_t size, int error)
+{
+	void *tail = zb->tail;
+	if (size > zbuf_tailroom(zb)) {
+		if (error) zbuf_set_werror(zb);
+		return NULL;
+	}
+	zb->tail += size;
+	return tail;
+}
+
+#define zbuf_push(zb, type) ((type *)__zbuf_push(zb, sizeof(type), 1))
+#define zbuf_pushn(zb, sz) ((void *)__zbuf_push(zb, sz, 1))
+#define zbuf_may_push(zb, type) ((type *)__zbuf_may_push(zb, sizeof(type), 0))
+#define zbuf_may_pushn(zb, sz) ((void *)__zbuf_push(zb, sz, 0))
+
+static inline void zbuf_put(struct zbuf *zb, const void *src, size_t len)
+{
+	void *dst = zbuf_pushn(zb, len);
+	if (dst) memcpy(dst, src, len);
+}
+
+static inline void zbuf_put8(struct zbuf *zb, uint8_t val)
+{
+	uint8_t *dst = zbuf_push(zb, uint8_t);
+	if (dst) *dst = val;
+}
+
+static inline void zbuf_put_be16(struct zbuf *zb, uint16_t val)
+{
+	struct unaligned16 {
+		uint16_t value;
+	} __attribute__((packed));
+
+	struct unaligned16 *v = zbuf_push(zb, struct unaligned16);
+	if (v) v->value = htobe16(val);
+}
+
+static inline void zbuf_put_be32(struct zbuf *zb, uint32_t val)
+{
+	struct unaligned32 {
+		uint32_t value;
+	} __attribute__((packed));
+
+	struct unaligned32 *v = zbuf_push(zb, struct unaligned32);
+	if (v) v->value = htobe32(val);
+}
+
+void zbuf_copy(struct zbuf *zb, struct zbuf *src, size_t len);
+
+void zbufq_init(struct zbuf_queue *);
+void zbufq_reset(struct zbuf_queue *);
+void zbufq_queue(struct zbuf_queue *, struct zbuf *);
+int zbufq_write(struct zbuf_queue *, int);
+
+#endif

nhrpd/znl.c

@@ -0,0 +1,160 @@
+/* Netlink helpers for zbuf
+ * Copyright (c) 2014-2015 Timo Teräs
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include <fcntl.h>
+#include <errno.h>
+#include <string.h>
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <linux/netlink.h>
+#include <linux/rtnetlink.h>
+
+#include "znl.h"
+
+#define ZNL_ALIGN(len)		(((len)+3) & ~3)
+
+void *znl_push(struct zbuf *zb, size_t n)
+{
+	return zbuf_pushn(zb, ZNL_ALIGN(n));
+}
+
+void *znl_pull(struct zbuf *zb, size_t n)
+{
+	return zbuf_pulln(zb, ZNL_ALIGN(n));
+}
+
+struct nlmsghdr *znl_nlmsg_push(struct zbuf *zb, uint16_t type, uint16_t flags)
+{
+	struct nlmsghdr *n;
+
+	n = znl_push(zb, sizeof(*n));
+	if (!n) return NULL;
+
+	*n = (struct nlmsghdr) {
+		.nlmsg_type = type,
+		.nlmsg_flags = flags,
+	};
+	return n;
+}
+
+void znl_nlmsg_complete(struct zbuf *zb, struct nlmsghdr *n)
+{
+	n->nlmsg_len = zb->tail - (uint8_t*)n;
+}
+
+struct nlmsghdr *znl_nlmsg_pull(struct zbuf *zb, struct zbuf *payload)
+{
+	struct nlmsghdr *n;
+	size_t plen;
+
+	n = znl_pull(zb, sizeof(*n));
+	if (!n) return NULL;
+
+	plen = n->nlmsg_len - sizeof(*n);
+	zbuf_init(payload, znl_pull(zb, plen), plen, plen);
+	zbuf_may_pulln(zb, ZNL_ALIGN(plen) - plen);
+
+	return n;
+}
+
+struct rtattr *znl_rta_push(struct zbuf *zb, uint16_t type, const void *val, size_t len)
+{
+	struct rtattr *rta;
+	uint8_t *dst;
+
+	rta = znl_push(zb, ZNL_ALIGN(sizeof(*rta)) + ZNL_ALIGN(len));
+	if (!rta) return NULL;
+
+	*rta = (struct rtattr) {
+		.rta_type = type,
+		.rta_len  = ZNL_ALIGN(sizeof(*rta)) + len,
+	};
+
+	dst = (uint8_t *)(rta+1);
+	memcpy(dst, val, len);
+	memset(dst+len, 0, ZNL_ALIGN(len) - len);
+
+	return rta;
+}
+
+struct rtattr *znl_rta_push_u32(struct zbuf *zb, uint16_t type, uint32_t val)
+{
+	return znl_rta_push(zb, type, &val, sizeof(val));
+}
+
+struct rtattr *znl_rta_nested_push(struct zbuf *zb, uint16_t type)
+{
+	struct rtattr *rta;
+
+	rta = znl_push(zb, sizeof(*rta));
+	if (!rta) return NULL;
+
+	*rta = (struct rtattr) {
+		.rta_type = type,
+	};
+	return rta;
+}
+
+void znl_rta_nested_complete(struct zbuf *zb, struct rtattr *rta)
+{
+	size_t len = zb->tail - (uint8_t*) rta;
+	size_t align = ZNL_ALIGN(len) - len;
+
+	if (align) {
+		void *dst = zbuf_pushn(zb, align);
+		if (dst) memset(dst, 0, align);
+	}
+	rta->rta_len = len;
+}
+
+struct rtattr *znl_rta_pull(struct zbuf *zb, struct zbuf *payload)
+{
+	struct rtattr *rta;
+	size_t plen;
+
+	rta = znl_pull(zb, sizeof(*rta));
+	if (!rta) return NULL;
+
+	if (rta->rta_len > sizeof(*rta)) {
+		plen = rta->rta_len - sizeof(*rta);
+		zbuf_init(payload, znl_pull(zb, plen), plen, plen);
+	} else {
+		zbuf_init(payload, NULL, 0, 0);
+	}
+
+	return rta;
+}
+
+int znl_open(int protocol, int groups)
+{
+	struct sockaddr_nl addr;
+	int fd, buf = 128 * 1024;
+
+	fd = socket(AF_NETLINK, SOCK_RAW, protocol);
+	if (fd < 0)
+		return -1;
+
+	fcntl(fd, F_SETFL, fcntl(fd, F_GETFL, 0) | O_NONBLOCK);
+	fcntl(fd, F_SETFD, FD_CLOEXEC);
+	if (setsockopt(fd, SOL_SOCKET, SO_RCVBUF, &buf, sizeof(buf)) < 0)
+		goto error;
+
+	memset(&addr, 0, sizeof(addr));
+	addr.nl_family = AF_NETLINK;
+	addr.nl_groups = groups;
+	if (bind(fd, (struct sockaddr *) &addr, sizeof(addr)) < 0)
+		goto error;
+
+	return fd;
+error:
+	close(fd);
+	return -1;
+}
+

nhrpd/znl.h

@@ -0,0 +1,29 @@
+/* Netlink helpers for zbuf
+ * Copyright (c) 2014-2015 Timo Teräs
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+#include "zbuf.h"
+
+#define ZNL_BUFFER_SIZE		8192
+
+void *znl_push(struct zbuf *zb, size_t n);
+void *znl_pull(struct zbuf *zb, size_t n);
+
+struct nlmsghdr *znl_nlmsg_push(struct zbuf *zb, uint16_t type, uint16_t flags);
+void znl_nlmsg_complete(struct zbuf *zb, struct nlmsghdr *n);
+struct nlmsghdr *znl_nlmsg_pull(struct zbuf *zb, struct zbuf *payload);
+
+struct rtattr *znl_rta_push(struct zbuf *zb, uint16_t type, const void *val, size_t len);
+struct rtattr *znl_rta_push_u32(struct zbuf *zb, uint16_t type, uint32_t val);
+struct rtattr *znl_rta_nested_push(struct zbuf *zb, uint16_t type);
+void znl_rta_nested_complete(struct zbuf *zb, struct rtattr *rta);
+
+struct rtattr *znl_rta_pull(struct zbuf *zb, struct zbuf *payload);
+
+int znl_open(int protocol, int groups);
+

vtysh/Makefile.am

@@ -67,6 +67,10 @@ if RIPNGD
 vtysh_scan += $(top_srcdir)/ripngd/*.c
 endif
 
+if NHRPD
+vtysh_scan += $(top_srcdir)/nhrpd/nhrp_vty.c
+endif
+
 vtysh_cmd_FILES = $(vtysh_scan) \
 		  $(top_srcdir)/lib/keychain.c $(top_srcdir)/lib/routemap.c \
 		  $(top_srcdir)/lib/filter.c $(top_srcdir)/lib/plist.c \

vtysh/vtysh.c

@@ -73,6 +73,7 @@ struct vtysh_client vtysh_client[] =
   { .fd = -1, .name = "bgpd", .flag = VTYSH_BGPD, .path = BGP_VTYSH_PATH, .next = NULL},
   { .fd = -1, .name = "isisd", .flag = VTYSH_ISISD, .path = ISIS_VTYSH_PATH, .next = NULL},
   { .fd = -1, .name = "pimd", .flag = VTYSH_PIMD, .path = PIM_VTYSH_PATH, .next = NULL},
+  { .fd = -1, .name = "nhrpd", .flag = VTYSH_NHRPD, .path = NHRP_VTYSH_PATH, .next = NULL},
   { .fd = -1, .name = "watchfrr", .flag = VTYSH_WATCHFRR, .path = WATCHFRR_VTYSH_PATH, .next = NULL},
 };
 

vtysh/vtysh.h

@@ -35,15 +35,16 @@ DECLARE_MGROUP(MVTYSH)
 #define VTYSH_PIMD   0x100
 #define VTYSH_LDPD   0x200
 #define VTYSH_WATCHFRR 0x400
+#define VTYSH_NHRPD  0x800
 
 /* commands in REALLYALL are crucial to correct vtysh operation */
 #define VTYSH_REALLYALL	  ~0U
 /* watchfrr is not in ALL since library CLI functions should not be
  * run on it (logging & co. should stay in a fixed/frozen config, and
  * things like prefix lists are not even initialised) */
-#define VTYSH_ALL	  VTYSH_ZEBRA|VTYSH_RIPD|VTYSH_RIPNGD|VTYSH_OSPFD|VTYSH_OSPF6D|VTYSH_LDPD|VTYSH_BGPD|VTYSH_ISISD|VTYSH_PIMD
+#define VTYSH_ALL	  VTYSH_ZEBRA|VTYSH_RIPD|VTYSH_RIPNGD|VTYSH_OSPFD|VTYSH_OSPF6D|VTYSH_LDPD|VTYSH_BGPD|VTYSH_ISISD|VTYSH_PIMD|VTYSH_NHRPD
 #define VTYSH_RMAP	  VTYSH_ZEBRA|VTYSH_RIPD|VTYSH_RIPNGD|VTYSH_OSPFD|VTYSH_OSPF6D|VTYSH_BGPD|VTYSH_PIMD
-#define VTYSH_INTERFACE	  VTYSH_ZEBRA|VTYSH_RIPD|VTYSH_RIPNGD|VTYSH_OSPFD|VTYSH_OSPF6D|VTYSH_LDPD|VTYSH_ISISD|VTYSH_PIMD
+#define VTYSH_INTERFACE	  VTYSH_ZEBRA|VTYSH_RIPD|VTYSH_RIPNGD|VTYSH_OSPFD|VTYSH_OSPF6D|VTYSH_LDPD|VTYSH_ISISD|VTYSH_PIMD|VTYSH_NHRPD
 #define VTYSH_NS          VTYSH_ZEBRA
 #define VTYSH_VRF	  VTYSH_ZEBRA
 

zebra/client_main.c

@@ -115,6 +115,7 @@ struct zebra_info
   { "ospf",   ZEBRA_ROUTE_OSPF },
   { "ospf6",  ZEBRA_ROUTE_OSPF6 },
   { "bgp",    ZEBRA_ROUTE_BGP },
+  { "nhrp",   ZEBRA_ROUTE_NHRP },
   { NULL,     0 }
 };
 

zebra/zebra_rib.c

@@ -78,6 +78,7 @@ static const struct
   [ZEBRA_ROUTE_OSPF6]   = {ZEBRA_ROUTE_OSPF6,   110},
   [ZEBRA_ROUTE_ISIS]    = {ZEBRA_ROUTE_ISIS,    115},
   [ZEBRA_ROUTE_BGP]     = {ZEBRA_ROUTE_BGP,      20  /* IBGP is 200. */},
+  [ZEBRA_ROUTE_NHRP]    = {ZEBRA_ROUTE_NHRP,     10},
   /* no entry/default: 150 */
 };
 
@@ -1822,6 +1823,7 @@ static const u_char meta_queue_map[ZEBRA_ROUTE_MAX] = {
   [ZEBRA_ROUTE_OSPF]    = 2,
   [ZEBRA_ROUTE_OSPF6]   = 2,
   [ZEBRA_ROUTE_ISIS]    = 2,
+  [ZEBRA_ROUTE_NHRP]    = 2,
   [ZEBRA_ROUTE_BGP]     = 3,
   [ZEBRA_ROUTE_HSLS]    = 4,
   [ZEBRA_ROUTE_TABLE]   = 1,

zebra/zebra_rnh.c

@@ -376,19 +376,28 @@ zebra_rnh_resolve_entry (vrf_id_t vrfid, int family, rnh_type_t type,
         {
           if (CHECK_FLAG (rib->status, RIB_ENTRY_REMOVED))
             continue;
-          if (CHECK_FLAG (rib->flags, ZEBRA_FLAG_SELECTED))
+          if (! CHECK_FLAG (rib->status, RIB_ENTRY_SELECTED_FIB))
+            continue;
+
+          if (CHECK_FLAG(rnh->flags, ZEBRA_NHT_CONNECTED))
             {
-              if (CHECK_FLAG(rnh->flags, ZEBRA_NHT_CONNECTED))
+              if (rib->type == ZEBRA_ROUTE_CONNECT)
+                break;
+              if (rib->type == ZEBRA_ROUTE_NHRP)
                 {
-                  if (rib->type == ZEBRA_ROUTE_CONNECT)
+                  struct nexthop *nexthop;
+                  for (nexthop = rib->nexthop; nexthop; nexthop = nexthop->next)
+                    if (nexthop->type == NEXTHOP_TYPE_IFINDEX)
+                      break;
+                  if (nexthop)
                     break;
                 }
-              else if ((type == RNH_IMPORT_CHECK_TYPE) &&
-                       (rib->type == ZEBRA_ROUTE_BGP))
-                continue;
-              else
-                break;
             }
+          else if ((type == RNH_IMPORT_CHECK_TYPE) &&
+                   (rib->type == ZEBRA_ROUTE_BGP))
+            continue;
+          else
+            break;
         }
     }
 

zebra/zebra_vty.c

@@ -684,6 +684,7 @@ vty_show_ip_route_detail (struct vty *vty, struct route_node *rn, int mcast)
       if (rib->type == ZEBRA_ROUTE_RIP
 	  || rib->type == ZEBRA_ROUTE_OSPF
 	  || rib->type == ZEBRA_ROUTE_ISIS
+	  || rib->type == ZEBRA_ROUTE_NHRP
 	  || rib->type == ZEBRA_ROUTE_TABLE
 	  || rib->type == ZEBRA_ROUTE_BGP)
 	{
@@ -835,6 +836,7 @@ vty_show_ip_route (struct vty *vty, struct route_node *rn, struct rib *rib,
       if (rib->type == ZEBRA_ROUTE_RIP
           || rib->type == ZEBRA_ROUTE_OSPF
           || rib->type == ZEBRA_ROUTE_ISIS
+	  || rib->type == ZEBRA_ROUTE_NHRP
           || rib->type == ZEBRA_ROUTE_TABLE
           || rib->type == ZEBRA_ROUTE_BGP)
         {
@@ -1041,6 +1043,7 @@ vty_show_ip_route (struct vty *vty, struct route_node *rn, struct rib *rib,
       if (rib->type == ZEBRA_ROUTE_RIP
 	  || rib->type == ZEBRA_ROUTE_OSPF
 	  || rib->type == ZEBRA_ROUTE_ISIS
+	  || rib->type == ZEBRA_ROUTE_NHRP
 	  || rib->type == ZEBRA_ROUTE_TABLE
 	  || rib->type == ZEBRA_ROUTE_BGP)
 	{