rework etherip to make it more consistent and reduce code duplication.

ok claudio@

1 files changed, 360 insertions, 302 deletions

diff --git a/sys/net/if_etherip.c b/sys/net/if_etherip.c
index aebcb2d5bf4..0c59f9f649b 100644
--- a/sys/net/if_etherip.c
+++ b/sys/net/if_etherip.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: if_etherip.c,v 1.31 2018/02/01 21:18:12 bluhm Exp $	*/
+/*	$OpenBSD: if_etherip.c,v 1.32 2018/02/09 04:05:58 dlg Exp $	*/
 /*
  * Copyright (c) 2015 Kazuya GODA <goda@openbsd.org>
  *
@@ -25,6 +25,7 @@
 #include <sys/ioctl.h>
 #include <sys/device.h>
 #include <sys/sysctl.h>
+#include <sys/tree.h>
 
 #include <net/if.h>
 #include <net/if_var.h>
@@ -53,16 +54,40 @@
 
 #include <net/if_etherip.h>
 
-struct etherip_softc {
-	struct arpcom sc_ac;
-	struct ifmedia sc_media;
-	unsigned int sc_rdomain;
-	struct sockaddr_storage sc_src;
-	struct sockaddr_storage sc_dst;
-	LIST_ENTRY(etherip_softc) sc_entry;
+union etherip_addr {
+	struct in_addr	in4;
+	struct in6_addr	in6;
+};
+
+struct etherip_tunnel {
+	union etherip_addr
+			_t_src;
+#define t_src4	_t_src.in4
+#define t_src6	_t_src.in6
+	union etherip_addr
+			_t_dst;
+#define t_dst4	_t_dst.in4
+#define t_dst6	_t_dst.in6
+
+	unsigned int	t_rtableid;
+	sa_family_t	t_af;
+
+	RBT_ENTRY(etherip_tunnel)
+			t_entry;;
 };
 
-LIST_HEAD(, etherip_softc) etherip_softc_list;
+RBT_HEAD(etherip_tree, etherip_tunnel);
+
+static inline int etherip_cmp(const struct etherip_tunnel *,
+    const struct etherip_tunnel *);
+
+RBT_PROTOTYPE(etherip_tree, etherip_tunnel, t_entry, etherip_cmp);
+
+struct etherip_softc {
+	struct etherip_tunnel	sc_tunnel; /* must be first */
+	struct arpcom		sc_ac;
+	struct ifmedia		sc_media;
+};
 
 /*
  * We can control the acceptance of EtherIP packets by altering the sysctl
@@ -79,12 +104,17 @@ int etherip_ioctl(struct ifnet *, u_long, caddr_t);
 void etherip_start(struct ifnet *);
 int etherip_media_change(struct ifnet *);
 void etherip_media_status(struct ifnet *, struct ifmediareq *);
-int etherip_set_tunnel_addr(struct ifnet *, struct sockaddr_storage *,
-   struct sockaddr_storage *);
+int etherip_set_tunnel(struct etherip_softc *, struct if_laddrreq *);
+int etherip_get_tunnel(struct etherip_softc *, struct if_laddrreq *);
+int etherip_del_tunnel(struct etherip_softc *);
+int etherip_up(struct etherip_softc *);
+int etherip_down(struct etherip_softc *);
+int etherip_input(struct etherip_tunnel *, struct mbuf *, int);
 
 struct if_clone	etherip_cloner = IF_CLONE_INITIALIZER("etherip",
     etherip_clone_create, etherip_clone_destroy);
 
+struct etherip_tree etherip_tree = RBT_INITIALIZER();
 
 void
 etheripattach(int count)
@@ -101,17 +131,18 @@ etherip_clone_create(struct if_clone *ifc, int unit)
 
 	sc = malloc(sizeof(*sc), M_DEVBUF, M_WAITOK|M_ZERO);
 	ifp = &sc->sc_ac.ac_if;
-	snprintf(ifp->if_xname, sizeof ifp->if_xname, "etherip%d", unit);
-	ifp->if_flags = IFF_BROADCAST | IFF_SIMPLEX | IFF_MULTICAST;
-	ether_fakeaddr(ifp);
+
+	snprintf(ifp->if_xname, sizeof(ifp->if_xname), "%s%d",
+	    ifc->ifc_name, unit);
 
 	ifp->if_softc = sc;
 	ifp->if_ioctl = etherip_ioctl;
 	ifp->if_start = etherip_start;
+	ifp->if_flags = IFF_BROADCAST | IFF_SIMPLEX | IFF_MULTICAST;
 	ifp->if_xflags = IFXF_CLONED;
 	IFQ_SET_MAXLEN(&ifp->if_snd, IFQ_MAXLEN);
-
 	ifp->if_capabilities = IFCAP_VLAN_MTU;
+	ether_fakeaddr(ifp);
 
 	ifmedia_init(&sc->sc_media, 0, etherip_media_change,
 	    etherip_media_status);
@@ -121,9 +152,7 @@ etherip_clone_create(struct if_clone *ifc, int unit)
 	if_attach(ifp);
 	ether_ifattach(ifp);
 
-	LIST_INSERT_HEAD(&etherip_softc_list, sc, sc_entry);
-
-	return 0;
+	return (0);
 }
 
 int
@@ -131,14 +160,18 @@ etherip_clone_destroy(struct ifnet *ifp)
 {
 	struct etherip_softc *sc = ifp->if_softc;
 
-	LIST_REMOVE(sc, sc_entry);
+	NET_LOCK();
+	if (ISSET(ifp->if_flags, IFF_RUNNING))
+		etherip_down(sc);
+	NET_UNLOCK();
 
 	ifmedia_delete_instance(&sc->sc_media, IFM_INST_ANY);
 	ether_ifdetach(ifp);
 	if_detach(ifp);
+
 	free(sc, M_DEVBUF, sizeof(*sc));
 
-	return 0;
+	return (0);
 }
 
 int
@@ -160,23 +193,18 @@ etherip_start(struct ifnet *ifp)
 	struct etherip_softc *sc = ifp->if_softc;
 	struct mbuf *m;
 	int error;
+#if NBPFILTER > 0
+	caddr_t if_bpf;
+#endif
 
-	for (;;) {
-		IFQ_DEQUEUE(&ifp->if_snd, m);
-		if (m == NULL)
-			break;
-
+	while ((m = ifq_dequeue(&ifp->if_snd)) != NULL) {
 #if NBPFILTER > 0
-		if (ifp->if_bpf)
-			bpf_mtap(ifp->if_bpf, m, BPF_DIRECTION_OUT);
+		if_bpf = ifp->if_bpf;
+		if (if_bpf)
+			bpf_mtap_ether(if_bpf, m, BPF_DIRECTION_OUT);
 #endif
-		if (sc->sc_src.ss_family == AF_UNSPEC ||
-		    sc->sc_dst.ss_family == AF_UNSPEC) {
-			m_freem(m);
-			continue;
-		}
 
-		switch (sc->sc_src.ss_family) {
+		switch (sc->sc_tunnel.t_af) {
 		case AF_INET:
 			error = ip_etherip_output(ifp, m);
 			break;
@@ -186,24 +214,19 @@ etherip_start(struct ifnet *ifp)
 			break;
 #endif
 		default:
-			unhandled_af(sc->sc_src.ss_family);
+			unhandled_af(sc->sc_tunnel.t_af);
 		}
 
 		if (error)
 			ifp->if_oerrors++;
 	}
-
 }
 
-
 int
 etherip_ioctl(struct ifnet *ifp, u_long cmd, caddr_t data)
 {
 	struct etherip_softc *sc = ifp->if_softc;
-	struct if_laddrreq *lifr = (struct if_laddrreq *)data;
 	struct ifreq *ifr = (struct ifreq *)data;
-	struct sockaddr_storage *src, *dst;
-	struct proc *p = curproc;
 	int error = 0;
 
 	switch (cmd) {
@@ -212,77 +235,52 @@ etherip_ioctl(struct ifnet *ifp, u_long cmd, caddr_t data)
 		/* FALLTHROUGH */
 
 	case SIOCSIFFLAGS:
-		if (ifp->if_flags & IFF_UP)
-			ifp->if_flags |= IFF_RUNNING;
-		else
-			ifp->if_flags &= ~IFF_RUNNING;
-
+		if (ISSET(ifp->if_flags, IFF_UP)) {
+			if (!ISSET(ifp->if_flags, IFF_RUNNING))
+				error = etherip_up(sc);
+			else
+				error = 0;
+		} else {
+			if (ISSET(ifp->if_flags, IFF_RUNNING))
+				error = etherip_down(sc);
+		}
 		break;
 
 	case SIOCSLIFPHYRTABLE:
-		if ((error = suser(p, 0)) != 0)
+		if (ISSET(ifp->if_flags, IFF_RUNNING)) {
+			error = EBUSY;
 			break;
-
+		}
 		if (ifr->ifr_rdomainid < 0 ||
 		    ifr->ifr_rdomainid > RT_TABLEID_MAX ||
 		    !rtable_exists(ifr->ifr_rdomainid)) {
 			error = EINVAL;
 			break;
 		}
-		sc->sc_rdomain = ifr->ifr_rdomainid;
+		sc->sc_tunnel.t_rtableid = ifr->ifr_rdomainid;
 		break;
 
 	case SIOCGLIFPHYRTABLE:
-		ifr->ifr_rdomainid = sc->sc_rdomain;
+		ifr->ifr_rdomainid = sc->sc_tunnel.t_rtableid;
 		break;
 
 	case SIOCSLIFPHYADDR:
-		if ((error = suser(p, 0)) != 0)
-			break;
-
-		src = &lifr->addr;
-		dst = &lifr->dstaddr;
-		if (src->ss_family == AF_UNSPEC || dst->ss_family == AF_UNSPEC)
-			return EADDRNOTAVAIL;
-
-		switch (src->ss_family) {
-		case AF_INET:
-			if (src->ss_len != sizeof(struct sockaddr_in) ||
-			    dst->ss_len != sizeof(struct sockaddr_in))
-				return EINVAL;
+		if (ISSET(ifp->if_flags, IFF_RUNNING)) {
+			error = EBUSY;
 			break;
-#ifdef INET6
-		case AF_INET6:
-			if (src->ss_len != sizeof(struct sockaddr_in6) ||
-			    dst->ss_len != sizeof(struct sockaddr_in6))
-				return EINVAL;
-			break;
-#endif
-		default:
-			return EAFNOSUPPORT;
 		}
 
-		error = etherip_set_tunnel_addr(ifp, src, dst);
+		error = etherip_set_tunnel(sc, (struct if_laddrreq *)data);
+		break;
+	case SIOCGLIFPHYADDR:
+		error = etherip_get_tunnel(sc, (struct if_laddrreq *)data);
 		break;
-
 	case SIOCDIFPHYADDR:
-		if ((error = suser(p, 0)) != 0)
+		if (ISSET(ifp->if_flags, IFF_RUNNING)) {
+			error = EBUSY;
 			break;
-
-		ifp->if_flags &= ~IFF_RUNNING;
-		memset(&sc->sc_src, 0, sizeof(sc->sc_src));
-		memset(&sc->sc_dst, 0, sizeof(sc->sc_dst));
-		break;
-
-	case SIOCGLIFPHYADDR:
-		if (sc->sc_dst.ss_family == AF_UNSPEC)
-			return EADDRNOTAVAIL;
-
-		memset(&lifr->addr, 0, sizeof(lifr->addr));
-		memset(&lifr->dstaddr, 0, sizeof(lifr->dstaddr));
-		memcpy(&lifr->addr, &sc->sc_src, sc->sc_src.ss_len);
-		memcpy(&lifr->dstaddr, &sc->sc_dst, sc->sc_dst.ss_len);
-
+		}
+		error = etherip_del_tunnel(sc);
 		break;
 
 	case SIOCSIFMEDIA:
@@ -295,98 +293,200 @@ etherip_ioctl(struct ifnet *ifp, u_long cmd, caddr_t data)
 		break;
 	}
 
-	return error;
+	return (error);
 }
 
 int
-etherip_set_tunnel_addr(struct ifnet *ifp, struct sockaddr_storage *src,
-    struct sockaddr_storage *dst)
+etherip_set_tunnel(struct etherip_softc *sc, struct if_laddrreq *req)
 {
-	struct etherip_softc *sc, *tsc;
-	int error = 0;
+	struct sockaddr *src = (struct sockaddr *)&req->addr;
+	struct sockaddr *dst = (struct sockaddr *)&req->dstaddr;
+	struct sockaddr_in *src4, *dst4;
+#ifdef INET6
+	struct sockaddr_in6 *src6, *dst6;
+	int error;
+#endif
 
-	sc  = ifp->if_softc;
+	/* sa_family and sa_len must be equal */
+	if (src->sa_family != dst->sa_family || src->sa_len != dst->sa_len)
+		return (EINVAL);
 
-	LIST_FOREACH(tsc, &etherip_softc_list, sc_entry) {
-		if (tsc == sc)
-			continue;
+	/* validate */
+	switch (dst->sa_family) {
+	case AF_INET:
+		if (dst->sa_len != sizeof(*dst4))
+			return (EINVAL);
 
-		if (tsc->sc_src.ss_family != src->ss_family ||
-		    tsc->sc_dst.ss_family != dst->ss_family ||
-		    tsc->sc_src.ss_len != src->ss_len ||
-		    tsc->sc_dst.ss_len != dst->ss_len)
-			continue;
+		src4 = (struct sockaddr_in *)src;
+		if (in_nullhost(src4->sin_addr) ||
+		    IN_MULTICAST(src4->sin_addr.s_addr))
+			return (EINVAL);
 
-		if (tsc->sc_rdomain == sc->sc_rdomain &&
-		    memcmp(&tsc->sc_dst, dst, dst->ss_len) == 0 &&
-		    memcmp(&tsc->sc_src, src, src->ss_len) == 0) {
-			error = EADDRNOTAVAIL;
-			goto out;
-		}
+		dst4 = (struct sockaddr_in *)dst;
+		if (in_nullhost(dst4->sin_addr) ||
+		    IN_MULTICAST(dst4->sin_addr.s_addr))
+			return (EINVAL);
+
+		sc->sc_tunnel.t_src4 = src4->sin_addr;
+		sc->sc_tunnel.t_dst4 = dst4->sin_addr;
+		break;
+#ifdef INET6
+	case AF_INET6:
+		if (dst->sa_len != sizeof(*dst6))
+			return (EINVAL);
+
+		src6 = (struct sockaddr_in6 *)src;
+		if (IN6_IS_ADDR_UNSPECIFIED(&src6->sin6_addr) ||
+		    IN6_IS_ADDR_MULTICAST(&src6->sin6_addr))
+			return (EINVAL);
+
+		dst6 = (struct sockaddr_in6 *)dst;
+		if (IN6_IS_ADDR_UNSPECIFIED(&dst6->sin6_addr) ||
+		    IN6_IS_ADDR_MULTICAST(&dst6->sin6_addr))
+			return (EINVAL);
+
+		error = in6_embedscope(&sc->sc_tunnel.t_src6, src6, NULL);
+		if (error != 0)
+			return (error);
+
+		error = in6_embedscope(&sc->sc_tunnel.t_dst6, dst6, NULL);
+		if (error != 0)
+			return (error);
+
+		break;
+#endif
+	default:
+		return (EAFNOSUPPORT);
 	}
 
-	memcpy(&sc->sc_src, src, src->ss_len);
-	memcpy(&sc->sc_dst, dst, dst->ss_len);
-out:
-	return error;
+	/* commit */
+	sc->sc_tunnel.t_af = dst->sa_family;
+
+	return (0);
 }
 
 int
-ip_etherip_output(struct ifnet *ifp, struct mbuf *m)
+etherip_get_tunnel(struct etherip_softc *sc, struct if_laddrreq *req)
 {
-	struct etherip_softc *sc = (struct etherip_softc *)ifp->if_softc;
-	struct sockaddr_in *src, *dst;
-	struct etherip_header *eip;
-	struct ip *ip;
+	struct sockaddr *src = (struct sockaddr *)&req->addr;
+	struct sockaddr *dst = (struct sockaddr *)&req->dstaddr;
+	struct sockaddr_in *sin;
+#ifdef INET6 /* ifconfig already embeds the scopeid */
+	struct sockaddr_in6 *sin6;
+#endif
 
-	src = (struct sockaddr_in *)&sc->sc_src;
-	dst = (struct sockaddr_in *)&sc->sc_dst;
+	switch (sc->sc_tunnel.t_af) {
+	case AF_UNSPEC:
+		return (EADDRNOTAVAIL);
+	case AF_INET:
+		sin = (struct sockaddr_in *)src;
+		memset(sin, 0, sizeof(*sin));
+		sin->sin_family = AF_INET;
+		sin->sin_len = sizeof(*sin);
+		sin->sin_addr = sc->sc_tunnel.t_src4;
+
+		sin = (struct sockaddr_in *)dst;
+		memset(sin, 0, sizeof(*sin));
+		sin->sin_family = AF_INET;
+		sin->sin_len = sizeof(*sin);
+		sin->sin_addr = sc->sc_tunnel.t_dst4;
 
-	if (src == NULL || dst == NULL ||
-	    src->sin_family != AF_INET || dst->sin_family != AF_INET) {
-		m_freem(m);
-		return EAFNOSUPPORT;
-	}
-	if (dst->sin_addr.s_addr == INADDR_ANY) {
-		m_freem(m);
-		return ENETUNREACH;
+		break;
+#ifdef INET6
+	case AF_INET6:
+		sin6 = (struct sockaddr_in6 *)src;
+		memset(sin6, 0, sizeof(*sin6));
+		sin6->sin6_family = AF_INET6;
+		sin6->sin6_len = sizeof(*sin6);
+		in6_recoverscope(sin6, &sc->sc_tunnel.t_src6);
+
+		sin6 = (struct sockaddr_in6 *)dst;
+		memset(sin6, 0, sizeof(*sin6));
+		sin6->sin6_family = AF_INET6;
+		sin6->sin6_len = sizeof(*sin6);
+		in6_recoverscope(sin6, &sc->sc_tunnel.t_dst6);
+
+		break;
+#endif
+	default:
+		return (EAFNOSUPPORT);
 	}
 
-	/*
-	 * Remove multicast and broadcast flags or encapsulated packet
-	 * ends up as multicast or broadcast packet.
-	 */
-	m->m_flags &= ~(M_BCAST|M_MCAST);
+	return (0);
+}
 
-	M_PREPEND(m, sizeof(struct etherip_header), M_DONTWAIT);
-	if (m == NULL) {
-		etheripstat_inc(etherips_adrops);
-		return ENOBUFS;
-	}
-	eip = mtod(m, struct etherip_header *);
-	eip->eip_ver = ETHERIP_VERSION;
-	eip->eip_res = 0;
-	eip->eip_pad = 0;
+int
+etherip_del_tunnel(struct etherip_softc *sc)
+{
+	/* commit */
+	sc->sc_tunnel.t_af = AF_UNSPEC;
+
+	return (0);
+}
+
+int
+etherip_up(struct etherip_softc *sc)
+{
+	if (sc->sc_tunnel.t_af == AF_UNSPEC)
+		return (ENXIO);
 
-	M_PREPEND(m, sizeof(struct ip), M_DONTWAIT);
+	NET_ASSERT_LOCKED();
+	if (RBT_INSERT(etherip_tree, &etherip_tree, &sc->sc_tunnel) != NULL)
+		return (EADDRINUSE);
+
+	SET(sc->sc_ac.ac_if.if_flags, IFF_RUNNING);
+
+	return (0);
+}
+
+int
+etherip_down(struct etherip_softc *sc)
+{
+	struct ifnet *ifp = &sc->sc_ac.ac_if;
+	NET_ASSERT_LOCKED();
+
+	RBT_REMOVE(etherip_tree, &etherip_tree, &sc->sc_tunnel);
+
+	CLR(ifp->if_flags, IFF_RUNNING);
+
+	ifq_barrier(&ifp->if_snd);
+
+	return (0);
+}
+
+int
+ip_etherip_output(struct ifnet *ifp, struct mbuf *m)
+{
+	struct etherip_softc *sc = (struct etherip_softc *)ifp->if_softc;
+	struct etherip_header *eip;
+	struct ip *ip;
+
+	M_PREPEND(m, sizeof(*ip) + sizeof(*eip), M_DONTWAIT);
 	if (m == NULL) {
 		etheripstat_inc(etherips_adrops);
 		return ENOBUFS;
 	}
+
 	ip = mtod(m, struct ip *);
 	memset(ip, 0, sizeof(struct ip));
 
 	ip->ip_v = IPVERSION;
-	ip->ip_hl = sizeof(struct ip) >> 2;
+	ip->ip_hl = sizeof(*ip) >> 2;
 	ip->ip_id = htons(ip_randomid());
 	ip->ip_tos = IPTOS_LOWDELAY;
 	ip->ip_p = IPPROTO_ETHERIP;
 	ip->ip_len = htons(m->m_pkthdr.len);
 	ip->ip_ttl = IPDEFTTL;
-	ip->ip_src = src->sin_addr;
-	ip->ip_dst = dst->sin_addr;
+	ip->ip_src = sc->sc_tunnel.t_src4;
+	ip->ip_dst = sc->sc_tunnel.t_dst4;
 
-	m->m_pkthdr.ph_rtableid = sc->sc_rdomain;
+	eip = (struct etherip_header *)(ip + 1);
+	eip->eip_ver = ETHERIP_VERSION;
+	eip->eip_res = 0;
+	eip->eip_pad = 0;
+
+	m->m_flags &= ~(M_BCAST|M_MCAST);
+	m->m_pkthdr.ph_rtableid = sc->sc_tunnel.t_rtableid;
 
 #if NPF > 0
 	pf_pkt_addr_changed(m);
@@ -395,60 +495,51 @@ ip_etherip_output(struct ifnet *ifp, struct mbuf *m)
 	    (sizeof(struct ip) + sizeof(struct etherip_header)));
 
 	ip_send(m);
+
 	return (0);
 }
 
 int
-ip_etherip_input(struct mbuf **mp, int *offp, int proto, int af)
+ip_etherip_input(struct mbuf **mp, int *offp, int type, int af)
 {
 	struct mbuf *m = *mp;
-	struct mbuf_list ml = MBUF_LIST_INITIALIZER();
-	struct etherip_softc *sc;
-	const struct ip *ip;
-	struct etherip_header *eip;
-	struct sockaddr_in *src, *dst;
-	struct ifnet *ifp = NULL;
+	struct etherip_tunnel key;
+	struct ip *ip;
 
 	ip = mtod(m, struct ip *);
 
-	if (ip->ip_p != IPPROTO_ETHERIP) {
-		m_freem(m);
-		ipstat_inc(ips_noproto);
-		return IPPROTO_DONE;
-	}
+	key.t_af = AF_INET;
+	key.t_src4 = ip->ip_dst;
+	key.t_dst4 = ip->ip_src;
+
+	return (etherip_input(&key, m, *offp));
+}
+
+int
+etherip_input(struct etherip_tunnel *key, struct mbuf *m, int hlen)
+{
+	struct mbuf_list ml = MBUF_LIST_INITIALIZER();
+	struct etherip_softc *sc;
+	struct ifnet *ifp;
+	struct etherip_header *eip;
 
 	if (!etherip_allow && (m->m_flags & (M_AUTH|M_CONF)) == 0) {
-		m_freem(m);
 		etheripstat_inc(etherips_pdrops);
-		return IPPROTO_DONE;
+		goto drop;
 	}
 
-	NET_ASSERT_LOCKED();
-	LIST_FOREACH(sc, &etherip_softc_list, sc_entry) {
-		if (sc->sc_src.ss_family != AF_INET ||
-		    sc->sc_dst.ss_family != AF_INET)
-			continue;
-
-		src = (struct sockaddr_in *)&sc->sc_src;
-		dst = (struct sockaddr_in *)&sc->sc_dst;
+	key->t_rtableid = m->m_pkthdr.ph_rtableid;
 
-		if (sc->sc_rdomain != rtable_l2(m->m_pkthdr.ph_rtableid) ||
-		    src->sin_addr.s_addr != ip->ip_dst.s_addr ||
-		    dst->sin_addr.s_addr != ip->ip_src.s_addr)
-			continue;
-
-		ifp = &sc->sc_ac.ac_if;
-		break;
-	}
-
-	if (ifp == NULL) {
+	NET_ASSERT_LOCKED();
+	sc = (struct etherip_softc *)RBT_FIND(etherip_tree,
+	    &etherip_tree, key);
+	if (sc == NULL) {
 		etheripstat_inc(etherips_noifdrops);
-		m_freem(m);
-		return IPPROTO_DONE;
+		goto drop;
 	}
 
-	m_adj(m, *offp);
-	m = *mp = m_pullup(m, sizeof(struct etherip_header));
+	m_adj(m, hlen);
+	m = m_pullup(m, sizeof(*eip));
 	if (m == NULL) {
 		etheripstat_inc(etherips_adrops);
 		return IPPROTO_DONE;
@@ -457,20 +548,24 @@ ip_etherip_input(struct mbuf **mp, int *offp, int proto, int af)
 	eip = mtod(m, struct etherip_header *);
 	if (eip->eip_ver != ETHERIP_VERSION || eip->eip_pad) {
 		etheripstat_inc(etherips_adrops);
-		m_freem(m);
-		return IPPROTO_DONE;
+		goto drop;
 	}
 
-	etheripstat_pkt(etherips_ipackets, etherips_ibytes, m->m_pkthdr.len -
-	    sizeof(struct etherip_header));
-
 	m_adj(m, sizeof(struct etherip_header));
-	m = *mp = m_pullup(m, sizeof(struct ether_header));
+
+	etheripstat_pkt(etherips_ipackets, etherips_ibytes, m->m_pkthdr.len);
+
+	m = m_pullup(m, sizeof(struct ether_header));
 	if (m == NULL) {
 		etheripstat_inc(etherips_adrops);
 		return IPPROTO_DONE;
 	}
+
+	ifp = &sc->sc_ac.ac_if;
+
 	m->m_flags &= ~(M_BCAST|M_MCAST);
+	m->m_pkthdr.ph_ifidx = ifp->if_index;
+	m->m_pkthdr.ph_rtableid = ifp->if_rdomain;
 
 #if NPF > 0
 	pf_pkt_addr_changed(m);
@@ -479,161 +574,76 @@ ip_etherip_input(struct mbuf **mp, int *offp, int proto, int af)
 	ml_enqueue(&ml, m);
 	if_input(ifp, &ml);
 	return IPPROTO_DONE;
+
+drop:
+	m_freem(m);
+	return (IPPROTO_DONE);
 }
 
 #ifdef INET6
 int
 ip6_etherip_output(struct ifnet *ifp, struct mbuf *m)
 {
-	struct etherip_softc *sc = (struct etherip_softc *)ifp->if_softc;
-	struct sockaddr_in6 *src, *dst;
-	struct etherip_header *eip;
+	struct etherip_softc *sc = ifp->if_softc;
 	struct ip6_hdr *ip6;
-	int error;
-
-	src = (struct sockaddr_in6 *)&sc->sc_src;
-	dst = (struct sockaddr_in6 *)&sc->sc_dst;
+	struct etherip_header *eip;
+	uint16_t len;
 
-	if (src == NULL || dst == NULL ||
-	    src->sin6_family != AF_INET6 || dst->sin6_family != AF_INET6) {
-		error = EAFNOSUPPORT;
-		goto drop;
-	}
-	if (IN6_IS_ADDR_UNSPECIFIED(&dst->sin6_addr)) {
-		error = ENETUNREACH;
-		goto drop;
+	if (IN6_IS_ADDR_UNSPECIFIED(&sc->sc_tunnel.t_dst6)) {
+		m_freem(m);
+		return (ENETUNREACH);
 	}
 
-	/*
-	 * Remove multicast and broadcast flags or encapsulated packet
-	 * ends up as multicast or broadcast packet.
-	 */
-	m->m_flags &= ~(M_BCAST|M_MCAST);
+	len = m->m_pkthdr.len;
 
-	M_PREPEND(m, sizeof(struct etherip_header), M_DONTWAIT);
+	M_PREPEND(m, sizeof(*ip6) + sizeof(*eip), M_DONTWAIT);
 	if (m == NULL) {
 		etheripstat_inc(etherips_adrops);
 		return ENOBUFS;
 	}
-	eip = mtod(m, struct etherip_header *);
-	eip->eip_ver = ETHERIP_VERSION;
-	eip->eip_res = 0;
-	eip->eip_pad = 0;
 
-	M_PREPEND(m, sizeof(struct ip6_hdr), M_DONTWAIT);
-	if (m == NULL) {
-		etheripstat_inc(etherips_adrops);
-		return ENOBUFS;
-	}
 	ip6 = mtod(m, struct ip6_hdr *);
 	ip6->ip6_flow = 0;
 	ip6->ip6_vfc &= ~IPV6_VERSION_MASK;
 	ip6->ip6_vfc |= IPV6_VERSION;
 	ip6->ip6_nxt  = IPPROTO_ETHERIP;
 	ip6->ip6_hlim = ip6_defhlim;
-	ip6->ip6_plen = htons(m->m_pkthdr.len - sizeof(struct ip6_hdr));
-	error = in6_embedscope(&ip6->ip6_src, src, NULL);
-	if (error != 0)
-		goto drop;
-	error = in6_embedscope(&ip6->ip6_dst, dst, NULL);
-	if (error != 0)
-		goto drop;
+	ip6->ip6_plen = htons(len);
+	memcpy(&ip6->ip6_src, &sc->sc_tunnel.t_src6, sizeof(ip6->ip6_src));
+	memcpy(&ip6->ip6_dst, &sc->sc_tunnel.t_dst6, sizeof(ip6->ip6_dst));
 
-	m->m_pkthdr.ph_rtableid = sc->sc_rdomain;
+	eip = (struct etherip_header *)(ip6 + 1);
+	eip->eip_ver = ETHERIP_VERSION;
+	eip->eip_res = 0;
+	eip->eip_pad = 0;
+
+	m->m_flags &= ~(M_BCAST|M_MCAST);
+	m->m_pkthdr.ph_rtableid = sc->sc_tunnel.t_rtableid;
 
 #if NPF > 0
 	pf_pkt_addr_changed(m);
 #endif
-	etheripstat_pkt(etherips_opackets, etherips_obytes, m->m_pkthdr.len -
-	    (sizeof(struct ip6_hdr) + sizeof(struct etherip_header)));
+
+	etheripstat_pkt(etherips_opackets, etherips_obytes, len);
 
 	ip6_send(m);
 	return (0);
-
-drop:
-	m_freem(m);
-	return (error);
 }
 
 int
 ip6_etherip_input(struct mbuf **mp, int *offp, int proto, int af)
 {
 	struct mbuf *m = *mp;
-	struct mbuf_list ml = MBUF_LIST_INITIALIZER();
-	struct etherip_softc *sc;
+	struct etherip_tunnel key;
 	const struct ip6_hdr *ip6;
-	struct etherip_header *eip;
-	struct sockaddr_in6 ipsrc, ipdst;
-	struct sockaddr_in6 *src6, *dst6;
-	struct ifnet *ifp = NULL;
-
-
-	if (!etherip_allow && (m->m_flags & (M_AUTH|M_CONF)) == 0) {
-		m_freem(m);
-		etheripstat_inc(etherips_pdrops);
-		return IPPROTO_DONE;
-	}
 
 	ip6 = mtod(m, const struct ip6_hdr *);
-	in6_recoverscope(&ipsrc, &ip6->ip6_src);
-	in6_recoverscope(&ipdst, &ip6->ip6_dst);
-
-	NET_ASSERT_LOCKED();
-	LIST_FOREACH(sc, &etherip_softc_list, sc_entry) {
-		if (sc->sc_src.ss_family != AF_INET6 ||
-		    sc->sc_dst.ss_family != AF_INET6)
-			continue;
-
-		src6 = (struct sockaddr_in6 *)&sc->sc_src;
-		dst6 = (struct sockaddr_in6 *)&sc->sc_dst;
-
-		if (IN6_ARE_ADDR_EQUAL(&src6->sin6_addr, &ipdst.sin6_addr) &&
-		    src6->sin6_scope_id == ipdst.sin6_scope_id &&
-		    IN6_ARE_ADDR_EQUAL(&dst6->sin6_addr, &ipsrc.sin6_addr) &&
-		    dst6->sin6_scope_id == ipsrc.sin6_scope_id) {
-			ifp = &sc->sc_ac.ac_if;
-			break;
-		}
-	}
-
-	if (ifp == NULL) {
-		etheripstat_inc(etherips_noifdrops);
-		m_freem(m);
-		return IPPROTO_DONE;
-	}
-
-	m_adj(m, *offp);
-	m = *mp = m_pullup(m, sizeof(struct etherip_header));
-	if (m == NULL) {
-		etheripstat_inc(etherips_adrops);
-		return IPPROTO_DONE;
-	}
-
-	eip = mtod(m, struct etherip_header *);
-	if ((eip->eip_ver != ETHERIP_VERSION) || eip->eip_pad) {
-		etheripstat_inc(etherips_adrops);
-		m_freem(m);
-		return IPPROTO_DONE;
-	}
-	etheripstat_pkt(etherips_ipackets, etherips_ibytes, m->m_pkthdr.len -
-	    sizeof(struct etherip_header));
-
-	m_adj(m, sizeof(struct etherip_header));
-	m = *mp = m_pullup(m, sizeof(struct ether_header));
-	if (m == NULL) {
-		etheripstat_inc(etherips_adrops);
-		return IPPROTO_DONE;
-	}
 
-	m->m_flags &= ~(M_BCAST|M_MCAST);
+	key.t_af = AF_INET6;
+	key.t_src6 = ip6->ip6_dst;
+	key.t_dst6 = ip6->ip6_src;
 
-#if NPF > 0
-	pf_pkt_addr_changed(m);
-#endif
-
-	ml_enqueue(&ml, m);
-	if_input(ifp, &ml);
-	return IPPROTO_DONE;
+	return (etherip_input(&key, m, *offp));
 }
 #endif /* INET6 */
 
@@ -675,3 +685,51 @@ etherip_sysctl(int *name, u_int namelen, void *oldp, size_t *oldlenp,
 
 	return ENOPROTOOPT;
 }
+
+static inline int
+etherip_ip_cmp(int af, const union etherip_addr *a, const union etherip_addr *b)
+{
+	switch (af) {
+#ifdef INET6
+	case AF_INET6:
+		return (memcmp(&a->in6, &b->in6, sizeof(a->in6)));
+		/* FALLTHROUGH */
+#endif /* INET6 */
+	case AF_INET:
+		return (memcmp(&a->in4, &b->in4, sizeof(a->in4)));
+		break;
+	default:
+		panic("%s: unsupported af %d\n", __func__, af);
+	}
+
+	return (0);
+}
+
+static inline int
+etherip_cmp(const struct etherip_tunnel *a, const struct etherip_tunnel *b)
+{
+	int rv;
+
+	if (a->t_rtableid > b->t_rtableid)
+		return (1);
+	if (a->t_rtableid < b->t_rtableid)
+		return (-1);
+
+	/* sort by address */
+	if (a->t_af > b->t_af)
+		return (1);
+	if (a->t_af < b->t_af)
+		return (-1);
+
+	rv = etherip_ip_cmp(a->t_af, &a->_t_dst, &b->_t_dst);
+	if (rv != 0)
+		return (rv);
+
+	rv = etherip_ip_cmp(a->t_af, &a->_t_src, &b->_t_src);
+	if (rv != 0)
+		return (rv);
+
+	return (0);
+}
+
+RBT_GENERATE(etherip_tree, etherip_tunnel, t_entry, etherip_cmp);