Rework if_gre(4) to use encap_lookup_t method to speedup lookup

of needed interface when many gre interfaces are present.

Remove rmlock from gre_softc, use epoch(9) and CK_LIST instead.
Move more AF-related code into AF-related locations. Use hash table to
speedup lookup of needed softc.

4 files changed, 552 insertions, 553 deletions

diff --git a/sys/net/if_gre.c b/sys/net/if_gre.c
index 2160f350bf9b..aa9672d2bee1 100644
--- a/sys/net/if_gre.c
+++ b/sys/net/if_gre.c
@@ -2,7 +2,7 @@
  * SPDX-License-Identifier: BSD-2-Clause-FreeBSD
  *
  * Copyright (c) 1998 The NetBSD Foundation, Inc.
- * Copyright (c) 2014 Andrey V. Elsukov <ae@FreeBSD.org>
+ * Copyright (c) 2014, 2018 Andrey V. Elsukov <ae@FreeBSD.org>
  * All rights reserved.
  *
  * This code is derived from software contributed to The NetBSD Foundation
@@ -41,17 +41,13 @@ __FBSDID("$FreeBSD$");
 #include "opt_inet6.h"
 
 #include <sys/param.h>
-#include <sys/jail.h>
 #include <sys/kernel.h>
 #include <sys/lock.h>
-#include <sys/libkern.h>
 #include <sys/malloc.h>
 #include <sys/module.h>
 #include <sys/mbuf.h>
 #include <sys/priv.h>
 #include <sys/proc.h>
-#include <sys/protosw.h>
-#include <sys/rmlock.h>
 #include <sys/socket.h>
 #include <sys/sockio.h>
 #include <sys/sx.h>
@@ -70,7 +66,6 @@ __FBSDID("$FreeBSD$");
 
 #include <netinet/in.h>
 #ifdef INET
-#include <netinet/in_systm.h>
 #include <netinet/in_var.h>
 #include <netinet/ip.h>
 #include <netinet/ip_var.h>
@@ -80,7 +75,6 @@ __FBSDID("$FreeBSD$");
 #include <netinet/ip6.h>
 #include <netinet6/in6_var.h>
 #include <netinet6/ip6_var.h>
-#include <netinet6/scope6_var.h>
 #endif
 
 #include <netinet/ip_encap.h>
@@ -91,18 +85,10 @@ __FBSDID("$FreeBSD$");
 #include <security/mac/mac_framework.h>
 
 #define	GREMTU			1476
+
 static const char grename[] = "gre";
-static MALLOC_DEFINE(M_GRE, grename, "Generic Routing Encapsulation");
-static VNET_DEFINE(struct mtx, gre_mtx);
-#define	V_gre_mtx	VNET(gre_mtx)
-#define	GRE_LIST_LOCK_INIT(x)		mtx_init(&V_gre_mtx, "gre_mtx", NULL, \
-					    MTX_DEF)
-#define	GRE_LIST_LOCK_DESTROY(x)	mtx_destroy(&V_gre_mtx)
-#define	GRE_LIST_LOCK(x)		mtx_lock(&V_gre_mtx)
-#define	GRE_LIST_UNLOCK(x)		mtx_unlock(&V_gre_mtx)
-
-static VNET_DEFINE(LIST_HEAD(, gre_softc), gre_softc_list);
-#define	V_gre_softc_list	VNET(gre_softc_list)
+MALLOC_DEFINE(M_GRE, grename, "Generic Routing Encapsulation");
+
 static struct sx gre_ioctl_sx;
 SX_SYSINIT(gre_ioctl_sx, &gre_ioctl_sx, "gre_ioctl");
 
@@ -116,11 +102,7 @@ static int	gre_transmit(struct ifnet *, struct mbuf *);
 static int	gre_ioctl(struct ifnet *, u_long, caddr_t);
 static int	gre_output(struct ifnet *, struct mbuf *,
 		    const struct sockaddr *, struct route *);
-
-static void	gre_updatehdr(struct gre_softc *);
-static int	gre_set_tunnel(struct ifnet *, struct sockaddr *,
-    struct sockaddr *);
-static void	gre_delete_tunnel(struct ifnet *);
+static void	gre_delete_tunnel(struct gre_softc *);
 
 SYSCTL_DECL(_net_link);
 static SYSCTL_NODE(_net_link, IFT_TUNNEL, gre, CTLFLAG_RW, 0,
@@ -145,10 +127,15 @@ SYSCTL_INT(_net_link_gre, OID_AUTO, max_nesting, CTLFLAG_RW | CTLFLAG_VNET,
 static void
 vnet_gre_init(const void *unused __unused)
 {
-	LIST_INIT(&V_gre_softc_list);
-	GRE_LIST_LOCK_INIT();
+
 	V_gre_cloner = if_clone_simple(grename, gre_clone_create,
 	    gre_clone_destroy, 0);
+#ifdef INET
+	in_gre_init();
+#endif
+#ifdef INET6
+	in6_gre_init();
+#endif
 }
 VNET_SYSINIT(vnet_gre_init, SI_SUB_PROTO_IFATTACHDOMAIN, SI_ORDER_ANY,
     vnet_gre_init, NULL);
@@ -158,7 +145,12 @@ vnet_gre_uninit(const void *unused __unused)
 {
 
 	if_clone_detach(V_gre_cloner);
-	GRE_LIST_LOCK_DESTROY();
+#ifdef INET
+	in_gre_uninit();
+#endif
+#ifdef INET6
+	in6_gre_uninit();
+#endif
 }
 VNET_SYSUNINIT(vnet_gre_uninit, SI_SUB_PROTO_IFATTACHDOMAIN, SI_ORDER_ANY,
     vnet_gre_uninit, NULL);
@@ -171,7 +163,6 @@ gre_clone_create(struct if_clone *ifc, int unit, caddr_t params)
 	sc = malloc(sizeof(struct gre_softc), M_GRE, M_WAITOK | M_ZERO);
 	sc->gre_fibnum = curthread->td_proc->p_fibnum;
 	GRE2IFP(sc) = if_alloc(IFT_TUNNEL);
-	GRE_LOCK_INIT(sc);
 	GRE2IFP(sc)->if_softc = sc;
 	if_initname(GRE2IFP(sc), grename, unit);
 
@@ -185,9 +176,6 @@ gre_clone_create(struct if_clone *ifc, int unit, caddr_t params)
 	GRE2IFP(sc)->if_capenable |= IFCAP_LINKSTATE;
 	if_attach(GRE2IFP(sc));
 	bpfattach(GRE2IFP(sc), DLT_NULL, sizeof(u_int32_t));
-	GRE_LIST_LOCK();
-	LIST_INSERT_HEAD(&V_gre_softc_list, sc, gre_list);
-	GRE_LIST_UNLOCK();
 	return (0);
 }
 
@@ -198,33 +186,22 @@ gre_clone_destroy(struct ifnet *ifp)
 
 	sx_xlock(&gre_ioctl_sx);
 	sc = ifp->if_softc;
-	gre_delete_tunnel(ifp);
-	GRE_LIST_LOCK();
-	LIST_REMOVE(sc, gre_list);
-	GRE_LIST_UNLOCK();
+	gre_delete_tunnel(sc);
 	bpfdetach(ifp);
 	if_detach(ifp);
 	ifp->if_softc = NULL;
 	sx_xunlock(&gre_ioctl_sx);
 
+	GRE_WAIT();
 	if_free(ifp);
-	GRE_LOCK_DESTROY(sc);
 	free(sc, M_GRE);
 }
 
 static int
 gre_ioctl(struct ifnet *ifp, u_long cmd, caddr_t data)
 {
-	GRE_RLOCK_TRACKER;
 	struct ifreq *ifr = (struct ifreq *)data;
-	struct sockaddr *src, *dst;
 	struct gre_softc *sc;
-#ifdef INET
-	struct sockaddr_in *sin = NULL;
-#endif
-#ifdef INET6
-	struct sockaddr_in6 *sin6 = NULL;
-#endif
 	uint32_t opt;
 	int error;
 
@@ -249,7 +226,6 @@ gre_ioctl(struct ifnet *ifp, u_long cmd, caddr_t data)
 	case GREGPROTO:
 		return (EOPNOTSUPP);
 	}
-	src = dst = NULL;
 	sx_xlock(&gre_ioctl_sx);
 	sc = ifp->if_softc;
 	if (sc == NULL) {
@@ -258,189 +234,25 @@ gre_ioctl(struct ifnet *ifp, u_long cmd, caddr_t data)
 	}
 	error = 0;
 	switch (cmd) {
-	case SIOCSIFPHYADDR:
-#ifdef INET6
-	case SIOCSIFPHYADDR_IN6:
-#endif
-		error = EINVAL;
-		switch (cmd) {
-#ifdef INET
-		case SIOCSIFPHYADDR:
-			src = (struct sockaddr *)
-				&(((struct in_aliasreq *)data)->ifra_addr);
-			dst = (struct sockaddr *)
-				&(((struct in_aliasreq *)data)->ifra_dstaddr);
-			break;
-#endif
-#ifdef INET6
-		case SIOCSIFPHYADDR_IN6:
-			src = (struct sockaddr *)
-				&(((struct in6_aliasreq *)data)->ifra_addr);
-			dst = (struct sockaddr *)
-				&(((struct in6_aliasreq *)data)->ifra_dstaddr);
-			break;
-#endif
-		default:
-			error = EAFNOSUPPORT;
-			goto end;
-		}
-		/* sa_family must be equal */
-		if (src->sa_family != dst->sa_family ||
-		    src->sa_len != dst->sa_len)
-			goto end;
-
-		/* validate sa_len */
-		switch (src->sa_family) {
-#ifdef INET
-		case AF_INET:
-			if (src->sa_len != sizeof(struct sockaddr_in))
-				goto end;
-			break;
-#endif
-#ifdef INET6
-		case AF_INET6:
-			if (src->sa_len != sizeof(struct sockaddr_in6))
-				goto end;
-			break;
-#endif
-		default:
-			error = EAFNOSUPPORT;
-			goto end;
-		}
-		/* check sa_family looks sane for the cmd */
-		error = EAFNOSUPPORT;
-		switch (cmd) {
-#ifdef INET
-		case SIOCSIFPHYADDR:
-			if (src->sa_family == AF_INET)
-				break;
-			goto end;
-#endif
-#ifdef INET6
-		case SIOCSIFPHYADDR_IN6:
-			if (src->sa_family == AF_INET6)
-				break;
-			goto end;
-#endif
-		}
-		error = EADDRNOTAVAIL;
-		switch (src->sa_family) {
-#ifdef INET
-		case AF_INET:
-			if (satosin(src)->sin_addr.s_addr == INADDR_ANY ||
-			    satosin(dst)->sin_addr.s_addr == INADDR_ANY)
-				goto end;
-			break;
-#endif
-#ifdef INET6
-		case AF_INET6:
-			if (IN6_IS_ADDR_UNSPECIFIED(&satosin6(src)->sin6_addr)
-			    ||
-			    IN6_IS_ADDR_UNSPECIFIED(&satosin6(dst)->sin6_addr))
-				goto end;
-			/*
-			 * Check validity of the scope zone ID of the
-			 * addresses, and convert it into the kernel
-			 * internal form if necessary.
-			 */
-			error = sa6_embedscope(satosin6(src), 0);
-			if (error != 0)
-				goto end;
-			error = sa6_embedscope(satosin6(dst), 0);
-			if (error != 0)
-				goto end;
-#endif
-		}
-		error = gre_set_tunnel(ifp, src, dst);
-		break;
 	case SIOCDIFPHYADDR:
-		gre_delete_tunnel(ifp);
+		if (sc->gre_family == 0)
+			break;
+		gre_delete_tunnel(sc);
 		break;
+#ifdef INET
+	case SIOCSIFPHYADDR:
 	case SIOCGIFPSRCADDR:
 	case SIOCGIFPDSTADDR:
+		error = in_gre_ioctl(sc, cmd, data);
+		break;
+#endif
 #ifdef INET6
+	case SIOCSIFPHYADDR_IN6:
 	case SIOCGIFPSRCADDR_IN6:
 	case SIOCGIFPDSTADDR_IN6:
-#endif
-		if (sc->gre_family == 0) {
-			error = EADDRNOTAVAIL;
-			break;
-		}
-		GRE_RLOCK(sc);
-		switch (cmd) {
-#ifdef INET
-		case SIOCGIFPSRCADDR:
-		case SIOCGIFPDSTADDR:
-			if (sc->gre_family != AF_INET) {
-				error = EADDRNOTAVAIL;
-				break;
-			}
-			sin = (struct sockaddr_in *)&ifr->ifr_addr;
-			memset(sin, 0, sizeof(*sin));
-			sin->sin_family = AF_INET;
-			sin->sin_len = sizeof(*sin);
-			break;
-#endif
-#ifdef INET6
-		case SIOCGIFPSRCADDR_IN6:
-		case SIOCGIFPDSTADDR_IN6:
-			if (sc->gre_family != AF_INET6) {
-				error = EADDRNOTAVAIL;
-				break;
-			}
-			sin6 = (struct sockaddr_in6 *)
-				&(((struct in6_ifreq *)data)->ifr_addr);
-			memset(sin6, 0, sizeof(*sin6));
-			sin6->sin6_family = AF_INET6;
-			sin6->sin6_len = sizeof(*sin6);
-			break;
-#endif
-		}
-		if (error == 0) {
-			switch (cmd) {
-#ifdef INET
-			case SIOCGIFPSRCADDR:
-				sin->sin_addr = sc->gre_oip.ip_src;
-				break;
-			case SIOCGIFPDSTADDR:
-				sin->sin_addr = sc->gre_oip.ip_dst;
-				break;
-#endif
-#ifdef INET6
-			case SIOCGIFPSRCADDR_IN6:
-				sin6->sin6_addr = sc->gre_oip6.ip6_src;
-				break;
-			case SIOCGIFPDSTADDR_IN6:
-				sin6->sin6_addr = sc->gre_oip6.ip6_dst;
-				break;
-#endif
-			}
-		}
-		GRE_RUNLOCK(sc);
-		if (error != 0)
-			break;
-		switch (cmd) {
-#ifdef INET
-		case SIOCGIFPSRCADDR:
-		case SIOCGIFPDSTADDR:
-			error = prison_if(curthread->td_ucred,
-			    (struct sockaddr *)sin);
-			if (error != 0)
-				memset(sin, 0, sizeof(*sin));
-			break;
-#endif
-#ifdef INET6
-		case SIOCGIFPSRCADDR_IN6:
-		case SIOCGIFPDSTADDR_IN6:
-			error = prison_if(curthread->td_ucred,
-			    (struct sockaddr *)sin6);
-			if (error == 0)
-				error = sa6_recoverscope(sin6);
-			if (error != 0)
-				memset(sin6, 0, sizeof(*sin6));
-#endif
-		}
+		error = in6_gre_ioctl(sc, cmd, data);
 		break;
+#endif
 	case SIOCGTUNFIB:
 		ifr->ifr_fib = sc->gre_fibnum;
 		break;
@@ -453,40 +265,50 @@ gre_ioctl(struct ifnet *ifp, u_long cmd, caddr_t data)
 			sc->gre_fibnum = ifr->ifr_fib;
 		break;
 	case GRESKEY:
+	case GRESOPTS:
 		if ((error = priv_check(curthread, PRIV_NET_GRE)) != 0)
 			break;
 		if ((error = copyin(ifr_data_get_ptr(ifr), &opt,
 		    sizeof(opt))) != 0)
 			break;
-		if (sc->gre_key != opt) {
-			GRE_WLOCK(sc);
-			sc->gre_key = opt;
-			gre_updatehdr(sc);
-			GRE_WUNLOCK(sc);
+		if (cmd == GRESKEY) {
+			if (sc->gre_key == opt)
+				break;
+		} else if (cmd == GRESOPTS) {
+			if (opt & ~GRE_OPTMASK) {
+				error = EINVAL;
+				break;
+			}
+			if (sc->gre_options == opt)
+				break;
+		}
+		switch (sc->gre_family) {
+#ifdef INET
+		case AF_INET:
+			in_gre_setopts(sc, cmd, opt);
+			break;
+#endif
+#ifdef INET6
+		case AF_INET6:
+			in6_gre_setopts(sc, cmd, opt);
+			break;
+#endif
+		default:
+			if (cmd == GRESKEY)
+				sc->gre_key = opt;
+			else
+				sc->gre_options = opt;
+			break;
 		}
+		/*
+		 * XXX: Do we need to initiate change of interface
+		 * state here?
+		 */
 		break;
 	case GREGKEY:
 		error = copyout(&sc->gre_key, ifr_data_get_ptr(ifr),
 		    sizeof(sc->gre_key));
 		break;
-	case GRESOPTS:
-		if ((error = priv_check(curthread, PRIV_NET_GRE)) != 0)
-			break;
-		if ((error = copyin(ifr_data_get_ptr(ifr), &opt,
-		    sizeof(opt))) != 0)
-			break;
-		if (opt & ~GRE_OPTMASK)
-			error = EINVAL;
-		else {
-			if (sc->gre_options != opt) {
-				GRE_WLOCK(sc);
-				sc->gre_options = opt;
-				gre_updatehdr(sc);
-				GRE_WUNLOCK(sc);
-			}
-		}
-		break;
-
 	case GREGOPTS:
 		error = copyout(&sc->gre_options, ifr_data_get_ptr(ifr),
 		    sizeof(sc->gre_options));
@@ -495,40 +317,68 @@ gre_ioctl(struct ifnet *ifp, u_long cmd, caddr_t data)
 		error = EINVAL;
 		break;
 	}
+	if (error == 0 && sc->gre_family != 0) {
+		if (
+#ifdef INET
+		    cmd == SIOCSIFPHYADDR ||
+#endif
+#ifdef INET6
+		    cmd == SIOCSIFPHYADDR_IN6 ||
+#endif
+		    0) {
+			ifp->if_drv_flags |= IFF_DRV_RUNNING;
+			if_link_state_change(ifp, LINK_STATE_UP);
+		}
+	}
 end:
 	sx_xunlock(&gre_ioctl_sx);
 	return (error);
 }
 
 static void
-gre_updatehdr(struct gre_softc *sc)
+gre_delete_tunnel(struct gre_softc *sc)
+{
+
+	sx_assert(&gre_ioctl_sx, SA_XLOCKED);
+	if (sc->gre_family != 0) {
+		CK_LIST_REMOVE(sc, chain);
+		GRE_WAIT();
+		free(sc->gre_hdr, M_GRE);
+		sc->gre_family = 0;
+	}
+	GRE2IFP(sc)->if_drv_flags &= ~IFF_DRV_RUNNING;
+	if_link_state_change(GRE2IFP(sc), LINK_STATE_DOWN);
+}
+
+struct gre_list *
+gre_hashinit(void)
+{
+	struct gre_list *hash;
+	int i;
+
+	hash = malloc(sizeof(struct gre_list) * GRE_HASH_SIZE,
+	    M_GRE, M_WAITOK);
+	for (i = 0; i < GRE_HASH_SIZE; i++)
+		CK_LIST_INIT(&hash[i]);
+
+	return (hash);
+}
+
+void
+gre_hashdestroy(struct gre_list *hash)
+{
+
+	free(hash, M_GRE);
+}
+
+void
+gre_updatehdr(struct gre_softc *sc, struct grehdr *gh)
 {
-	struct grehdr *gh = NULL;
 	uint32_t *opts;
 	uint16_t flags;
 
-	GRE_WLOCK_ASSERT(sc);
-	switch (sc->gre_family) {
-#ifdef INET
-	case AF_INET:
-		sc->gre_hlen = sizeof(struct greip);
-		sc->gre_oip.ip_v = IPPROTO_IPV4;
-		sc->gre_oip.ip_hl = sizeof(struct ip) >> 2;
-		sc->gre_oip.ip_p = IPPROTO_GRE;
-		gh = &sc->gre_gihdr->gi_gre;
-		break;
-#endif
-#ifdef INET6
-	case AF_INET6:
-		sc->gre_hlen = sizeof(struct greip6);
-		sc->gre_oip6.ip6_vfc = IPV6_VERSION;
-		sc->gre_oip6.ip6_nxt = IPPROTO_GRE;
-		gh = &sc->gre_gi6hdr->gi6_gre;
-		break;
-#endif
-	default:
-		return;
-	}
+	sx_assert(&gre_ioctl_sx, SA_XLOCKED);
+
 	flags = 0;
 	opts = gh->gre_opts;
 	if (sc->gre_options & GRE_ENABLE_CSUM) {
@@ -550,141 +400,6 @@ gre_updatehdr(struct gre_softc *sc)
 	gh->gre_flags = htons(flags);
 }
 
-static void
-gre_detach(struct gre_softc *sc, int family)
-{
-
-	sx_assert(&gre_ioctl_sx, SA_XLOCKED);
-	if (sc->gre_ecookie != NULL) {
-		switch (family) {
-#ifdef INET
-		case AF_INET:
-			ip_encap_detach(sc->gre_ecookie);
-			break;
-#endif
-#ifdef INET6
-		case AF_INET6:
-			ip6_encap_detach(sc->gre_ecookie);
-			break;
-#endif
-		}
-	}
-	sc->gre_ecookie = NULL;
-}
-
-static int
-gre_set_tunnel(struct ifnet *ifp, struct sockaddr *src,
-    struct sockaddr *dst)
-{
-	struct gre_softc *sc, *tsc;
-#ifdef INET6
-	struct ip6_hdr *ip6;
-#endif
-#ifdef INET
-	struct ip *ip;
-#endif
-	void *hdr;
-	int error;
-
-	sx_assert(&gre_ioctl_sx, SA_XLOCKED);
-	GRE_LIST_LOCK();
-	sc = ifp->if_softc;
-	LIST_FOREACH(tsc, &V_gre_softc_list, gre_list) {
-		if (tsc == sc || tsc->gre_family != src->sa_family)
-			continue;
-#ifdef INET
-		if (tsc->gre_family == AF_INET &&
-		    tsc->gre_oip.ip_src.s_addr ==
-		    satosin(src)->sin_addr.s_addr &&
-		    tsc->gre_oip.ip_dst.s_addr ==
-		    satosin(dst)->sin_addr.s_addr) {
-			GRE_LIST_UNLOCK();
-			return (EADDRNOTAVAIL);
-		}
-#endif
-#ifdef INET6
-		if (tsc->gre_family == AF_INET6 &&
-		    IN6_ARE_ADDR_EQUAL(&tsc->gre_oip6.ip6_src,
-		    &satosin6(src)->sin6_addr) &&
-		    IN6_ARE_ADDR_EQUAL(&tsc->gre_oip6.ip6_dst,
-			&satosin6(dst)->sin6_addr)) {
-			GRE_LIST_UNLOCK();
-			return (EADDRNOTAVAIL);
-		}
-#endif
-	}
-	GRE_LIST_UNLOCK();
-
-	switch (src->sa_family) {
-#ifdef INET
-	case AF_INET:
-		hdr = ip = malloc(sizeof(struct greip) +
-		    3 * sizeof(uint32_t), M_GRE, M_WAITOK | M_ZERO);
-		ip->ip_src = satosin(src)->sin_addr;
-		ip->ip_dst = satosin(dst)->sin_addr;
-		break;
-#endif
-#ifdef INET6
-	case AF_INET6:
-		hdr = ip6 = malloc(sizeof(struct greip6) +
-		    3 * sizeof(uint32_t), M_GRE, M_WAITOK | M_ZERO);
-		ip6->ip6_src = satosin6(src)->sin6_addr;
-		ip6->ip6_dst = satosin6(dst)->sin6_addr;
-		break;
-#endif
-	default:
-		return (EAFNOSUPPORT);
-	}
-	if (sc->gre_family != 0)
-		gre_detach(sc, sc->gre_family);
-	GRE_WLOCK(sc);
-	if (sc->gre_family != 0)
-		free(sc->gre_hdr, M_GRE);
-	sc->gre_family = src->sa_family;
-	sc->gre_hdr = hdr;
-	sc->gre_oseq = 0;
-	sc->gre_iseq = UINT32_MAX;
-	gre_updatehdr(sc);
-	GRE_WUNLOCK(sc);
-
-	error = 0;
-	switch (src->sa_family) {
-#ifdef INET
-	case AF_INET:
-		error = in_gre_attach(sc);
-		break;
-#endif
-#ifdef INET6
-	case AF_INET6:
-		error = in6_gre_attach(sc);
-		break;
-#endif
-	}
-	if (error == 0) {
-		ifp->if_drv_flags |= IFF_DRV_RUNNING;
-		if_link_state_change(ifp, LINK_STATE_UP);
-	}
-	return (error);
-}
-
-static void
-gre_delete_tunnel(struct ifnet *ifp)
-{
-	struct gre_softc *sc = ifp->if_softc;
-	int family;
-
-	GRE_WLOCK(sc);
-	family = sc->gre_family;
-	sc->gre_family = 0;
-	GRE_WUNLOCK(sc);
-	if (family != 0) {
-		gre_detach(sc, family);
-		free(sc->gre_hdr, M_GRE);
-	}
-	ifp->if_drv_flags &= ~IFF_DRV_RUNNING;
-	if_link_state_change(ifp, LINK_STATE_DOWN);
-}
-
 int
 gre_input(struct mbuf *m, int off, int proto, void *arg)
 {
@@ -823,35 +538,18 @@ gre_output(struct ifnet *ifp, struct mbuf *m, const struct sockaddr *dst,
    struct route *ro)
 {
 	uint32_t af;
-	int error;
-
-#ifdef MAC
-	error = mac_ifnet_check_transmit(ifp, m);
-	if (error != 0)
-		goto drop;
-#endif
-	if ((ifp->if_flags & IFF_MONITOR) != 0 ||
-	    (ifp->if_flags & IFF_UP) == 0) {
-		error = ENETDOWN;
-		goto drop;
-	}
-
-	error = gre_check_nesting(ifp, m);
-	if (error != 0)
-		goto drop;
 
-	m->m_flags &= ~(M_BCAST|M_MCAST);
 	if (dst->sa_family == AF_UNSPEC)
 		bcopy(dst->sa_data, &af, sizeof(af));
 	else
 		af = dst->sa_family;
-	BPF_MTAP2(ifp, &af, sizeof(af), m);
-	m->m_pkthdr.csum_data = af;	/* save af for if_transmit */
+	/*
+	 * Now save the af in the inbound pkt csum data, this is a cheat since
+	 * we are using the inbound csum_data field to carry the af over to
+	 * the gre_transmit() routine, avoiding using yet another mtag.
+	 */
+	m->m_pkthdr.csum_data = af;
 	return (ifp->if_transmit(ifp, m));
-drop:
-	m_freem(m);
-	if_inc_counter(ifp, IFCOUNTER_OERRORS, 1);
-	return (error);
 }
 
 static void
@@ -874,92 +572,90 @@ gre_setseqn(struct grehdr *gh, uint32_t seq)
 static int
 gre_transmit(struct ifnet *ifp, struct mbuf *m)
 {
-	GRE_RLOCK_TRACKER;
 	struct gre_softc *sc;
 	struct grehdr *gh;
-	uint32_t iaf, oaf, oseq;
-	int error, hlen, olen, plen;
-	int want_seq, want_csum;
+	uint32_t af;
+	int error, len;
+	uint16_t proto;
 
-	plen = 0;
-	sc = ifp->if_softc;
-	if (sc == NULL) {
-		error = ENETDOWN;
+	len = 0;
+#ifdef MAC
+	error = mac_ifnet_check_transmit(ifp, m);
+	if (error) {
 		m_freem(m);
 		goto drop;
 	}
-	GRE_RLOCK(sc);
-	if (sc->gre_family == 0) {
-		GRE_RUNLOCK(sc);
-		error = ENETDOWN;
+#endif
+	error = ENETDOWN;
+	GRE_RLOCK();
+	sc = ifp->if_softc;
+	if ((ifp->if_flags & IFF_MONITOR) != 0 ||
+	    (ifp->if_flags & IFF_UP) == 0 ||
+	    sc->gre_family == 0 ||
+	    (error = gre_check_nesting(ifp, m)) != 0) {
 		m_freem(m);
 		goto drop;
 	}
-	iaf = m->m_pkthdr.csum_data;
-	oaf = sc->gre_family;
-	hlen = sc->gre_hlen;
-	want_seq = (sc->gre_options & GRE_ENABLE_SEQ) != 0;
-	if (want_seq)
-		oseq = sc->gre_oseq++; /* XXX */
-	else
-		oseq = 0;		/* Make compiler happy. */
-	want_csum = (sc->gre_options & GRE_ENABLE_CSUM) != 0;
+	af = m->m_pkthdr.csum_data;
 	M_SETFIB(m, sc->gre_fibnum);
-	M_PREPEND(m, hlen, M_NOWAIT);
+	M_PREPEND(m, sc->gre_hlen, M_NOWAIT);
 	if (m == NULL) {
-		GRE_RUNLOCK(sc);
 		error = ENOBUFS;
 		goto drop;
 	}
-	bcopy(sc->gre_hdr, mtod(m, void *), hlen);
-	GRE_RUNLOCK(sc);
-	switch (oaf) {
+	bcopy(sc->gre_hdr, mtod(m, void *), sc->gre_hlen);
+	/* Determine GRE proto */
+	switch (af) {
 #ifdef INET
 	case AF_INET:
-		olen = sizeof(struct ip);
+		proto = htons(ETHERTYPE_IP);
 		break;
 #endif
 #ifdef INET6
 	case AF_INET6:
-		olen = sizeof(struct ip6_hdr);
+		proto = htons(ETHERTYPE_IPV6);
 		break;
 #endif
 	default:
+		m_freem(m);
 		error = ENETDOWN;
 		goto drop;
 	}
-	gh = (struct grehdr *)mtodo(m, olen);
-	switch (iaf) {
+	/* Determine offset of GRE header */
+	switch (sc->gre_family) {
 #ifdef INET
 	case AF_INET:
-		gh->gre_proto = htons(ETHERTYPE_IP);
+		len = sizeof(struct ip);
 		break;
 #endif
 #ifdef INET6
 	case AF_INET6:
-		gh->gre_proto = htons(ETHERTYPE_IPV6);
+		len = sizeof(struct ip6_hdr);
 		break;
 #endif
 	default:
+		m_freem(m);
 		error = ENETDOWN;
 		goto drop;
 	}
-	if (want_seq)
-		gre_setseqn(gh, oseq);
-	if (want_csum) {
+	gh = (struct grehdr *)mtodo(m, len);
+	gh->gre_proto = proto;
+	if (sc->gre_options & GRE_ENABLE_SEQ)
+		gre_setseqn(gh, sc->gre_oseq++);
+	if (sc->gre_options & GRE_ENABLE_CSUM) {
 		*(uint16_t *)gh->gre_opts = in_cksum_skip(m,
-		    m->m_pkthdr.len, olen);
+		    m->m_pkthdr.len, len);
 	}
-	plen = m->m_pkthdr.len - hlen;
-	switch (oaf) {
+	len = m->m_pkthdr.len - len;
+	switch (sc->gre_family) {
 #ifdef INET
 	case AF_INET:
-		error = in_gre_output(m, iaf, hlen);
+		error = in_gre_output(m, af, sc->gre_hlen);
 		break;
 #endif
 #ifdef INET6
 	case AF_INET6:
-		error = in6_gre_output(m, iaf, hlen);
+		error = in6_gre_output(m, af, sc->gre_hlen);
 		break;
 #endif
 	default:
@@ -971,8 +667,9 @@ drop:
 		if_inc_counter(ifp, IFCOUNTER_OERRORS, 1);
 	else {
 		if_inc_counter(ifp, IFCOUNTER_OPACKETS, 1);
-		if_inc_counter(ifp, IFCOUNTER_OBYTES, plen);
+		if_inc_counter(ifp, IFCOUNTER_OBYTES, len);
 	}
+	GRE_RUNLOCK();
 	return (error);
 }
 
diff --git a/sys/net/if_gre.h b/sys/net/if_gre.h
index 1a068d4b1118..da71fbbf7d50 100644
--- a/sys/net/if_gre.h
+++ b/sys/net/if_gre.h
@@ -64,8 +64,6 @@ struct greip6 {
 
 struct gre_softc {
 	struct ifnet		*gre_ifp;
-	LIST_ENTRY(gre_softc)	gre_list;
-	struct rmlock		gre_lock;
 	int			gre_family;	/* AF of delivery header */
 	uint32_t		gre_iseq;
 	uint32_t		gre_oseq;
@@ -82,18 +80,20 @@ struct gre_softc {
 		struct greip6	*gi6hdr;
 #endif
 	} gre_uhdr;
-	const struct encaptab	*gre_ecookie;
+
+	CK_LIST_ENTRY(gre_softc) chain;
 };
+CK_LIST_HEAD(gre_list, gre_softc);
+MALLOC_DECLARE(M_GRE);
+
+#ifndef GRE_HASH_SIZE
+#define	GRE_HASH_SIZE	(1 << 4)
+#endif
+
 #define	GRE2IFP(sc)		((sc)->gre_ifp)
-#define	GRE_LOCK_INIT(sc)	rm_init(&(sc)->gre_lock, "gre softc")
-#define	GRE_LOCK_DESTROY(sc)	rm_destroy(&(sc)->gre_lock)
-#define	GRE_RLOCK_TRACKER	struct rm_priotracker gre_tracker
-#define	GRE_RLOCK(sc)		rm_rlock(&(sc)->gre_lock, &gre_tracker)
-#define	GRE_RUNLOCK(sc)		rm_runlock(&(sc)->gre_lock, &gre_tracker)
-#define	GRE_RLOCK_ASSERT(sc)	rm_assert(&(sc)->gre_lock, RA_RLOCKED)
-#define	GRE_WLOCK(sc)		rm_wlock(&(sc)->gre_lock)
-#define	GRE_WUNLOCK(sc)		rm_wunlock(&(sc)->gre_lock)
-#define	GRE_WLOCK_ASSERT(sc)	rm_assert(&(sc)->gre_lock, RA_WLOCKED)
+#define	GRE_RLOCK()		epoch_enter_preempt(net_epoch_preempt)
+#define	GRE_RUNLOCK()		epoch_exit_preempt(net_epoch_preempt)
+#define	GRE_WAIT()		epoch_wait_preempt(net_epoch_preempt)
 
 #define	gre_hdr			gre_uhdr.hdr
 #define	gre_gihdr		gre_uhdr.gihdr
@@ -101,15 +101,23 @@ struct gre_softc {
 #define	gre_oip			gre_gihdr->gi_ip
 #define	gre_oip6		gre_gi6hdr->gi6_ip6
 
+struct gre_list *gre_hashinit(void);
+void gre_hashdestroy(struct gre_list *);
+
 int	gre_input(struct mbuf *, int, int, void *);
-#ifdef INET
-int	in_gre_attach(struct gre_softc *);
+void	gre_updatehdr(struct gre_softc *, struct grehdr *);
+
+void	in_gre_init(void);
+void	in_gre_uninit(void);
+void	in_gre_setopts(struct gre_softc *, u_long, uint32_t);
+int	in_gre_ioctl(struct gre_softc *, u_long, caddr_t);
 int	in_gre_output(struct mbuf *, int, int);
-#endif
-#ifdef INET6
-int	in6_gre_attach(struct gre_softc *);
+
+void	in6_gre_init(void);
+void	in6_gre_uninit(void);
+void	in6_gre_setopts(struct gre_softc *, u_long, uint32_t);
+int	in6_gre_ioctl(struct gre_softc *, u_long, caddr_t);
 int	in6_gre_output(struct mbuf *, int, int);
-#endif
 /*
  * CISCO uses special type for GRE tunnel created as part of WCCP
  * connection, while in fact those packets are just IPv4 encapsulated
diff --git a/sys/netinet/ip_gre.c b/sys/netinet/ip_gre.c
index 66c1ce895ab9..ee36a0e18fce 100644
--- a/sys/netinet/ip_gre.c
+++ b/sys/netinet/ip_gre.c
@@ -2,7 +2,7 @@
  * SPDX-License-Identifier: BSD-2-Clause-NetBSD
  *
  * Copyright (c) 1998 The NetBSD Foundation, Inc.
- * Copyright (c) 2014 Andrey V. Elsukov <ae@FreeBSD.org>
+ * Copyright (c) 2014, 2018 Andrey V. Elsukov <ae@FreeBSD.org>
  * All rights reserved.
  *
  * This code is derived from software contributed to The NetBSD Foundation
@@ -41,17 +41,16 @@ __FBSDID("$FreeBSD$");
 #include "opt_inet6.h"
 
 #include <sys/param.h>
+#include <sys/jail.h>
 #include <sys/systm.h>
-#include <sys/mbuf.h>
 #include <sys/socket.h>
-#include <sys/socketvar.h>
+#include <sys/sockio.h>
+#include <sys/mbuf.h>
 #include <sys/errno.h>
-#include <sys/time.h>
 #include <sys/kernel.h>
-#include <sys/lock.h>
-#include <sys/rmlock.h>
 #include <sys/sysctl.h>
-#include <net/ethernet.h>
+#include <sys/malloc.h>
+
 #include <net/if.h>
 #include <net/if_var.h>
 #include <net/vnet.h>
@@ -74,36 +73,168 @@ VNET_DEFINE(int, ip_gre_ttl) = GRE_TTL;
 SYSCTL_INT(_net_inet_ip, OID_AUTO, grettl, CTLFLAG_VNET | CTLFLAG_RW,
     &VNET_NAME(ip_gre_ttl), 0, "Default TTL value for encapsulated packets");
 
+static VNET_DEFINE(struct gre_list *, ipv4_hashtbl) = NULL;
+#define	V_ipv4_hashtbl		VNET(ipv4_hashtbl)
+#define	GRE_HASH(src, dst)	(V_ipv4_hashtbl[\
+    in_gre_hashval((src), (dst)) & (GRE_HASH_SIZE - 1)])
+#define	GRE_HASH_SC(sc)		GRE_HASH((sc)->gre_oip.ip_src.s_addr,\
+    (sc)->gre_oip.ip_dst.s_addr)
+
+static uint32_t
+in_gre_hashval(in_addr_t src, in_addr_t dst)
+{
+	uint32_t ret;
+
+	ret = fnv_32_buf(&src, sizeof(src), FNV1_32_INIT);
+	return (fnv_32_buf(&dst, sizeof(dst), ret));
+}
+
+static int
+in_gre_checkdup(const struct gre_softc *sc, in_addr_t src, in_addr_t dst)
+{
+	struct gre_softc *tmp;
+
+	if (sc->gre_family == AF_INET &&
+	    sc->gre_oip.ip_src.s_addr == src &&
+	    sc->gre_oip.ip_dst.s_addr == dst)
+		return (EEXIST);
+
+	CK_LIST_FOREACH(tmp, &GRE_HASH(src, dst), chain) {
+		if (tmp == sc)
+			continue;
+		if (tmp->gre_oip.ip_src.s_addr == src &&
+		    tmp->gre_oip.ip_dst.s_addr == dst)
+			return (EADDRNOTAVAIL);
+	}
+	return (0);
+}
+
 static int
-in_gre_encapcheck(const struct mbuf *m, int off, int proto, void *arg)
+in_gre_lookup(const struct mbuf *m, int off, int proto, void **arg)
 {
-	GRE_RLOCK_TRACKER;
+	const struct ip *ip;
 	struct gre_softc *sc;
-	struct ip *ip;
 
-	sc = (struct gre_softc *)arg;
-	if ((GRE2IFP(sc)->if_flags & IFF_UP) == 0)
-		return (0);
+	MPASS(in_epoch());
+	ip = mtod(m, const struct ip *);
+	CK_LIST_FOREACH(sc, &GRE_HASH(ip->ip_dst.s_addr,
+	    ip->ip_src.s_addr), chain) {
+		/*
+		 * This is an inbound packet, its ip_dst is source address
+		 * in softc.
+		 */
+		if (sc->gre_oip.ip_src.s_addr == ip->ip_dst.s_addr &&
+		    sc->gre_oip.ip_dst.s_addr == ip->ip_src.s_addr) {
+			if ((GRE2IFP(sc)->if_flags & IFF_UP) == 0)
+				return (0);
+			*arg = sc;
+			return (ENCAP_DRV_LOOKUP);
+		}
+	}
+	return (0);
+}
+
+static void
+in_gre_attach(struct gre_softc *sc)
+{
 
-	M_ASSERTPKTHDR(m);
+	sc->gre_hlen = sizeof(struct greip);
+	sc->gre_oip.ip_v = IPVERSION;
+	sc->gre_oip.ip_hl = sizeof(struct ip) >> 2;
+	sc->gre_oip.ip_p = IPPROTO_GRE;
+	gre_updatehdr(sc, &sc->gre_gihdr->gi_gre);
+	CK_LIST_INSERT_HEAD(&GRE_HASH_SC(sc), sc, chain);
+}
 
-	GRE_RLOCK(sc);
-	if (sc->gre_family == 0)
-		goto bad;
+void
+in_gre_setopts(struct gre_softc *sc, u_long cmd, uint32_t value)
+{
 
-	KASSERT(sc->gre_family == AF_INET,
-	    ("wrong gre_family: %d", sc->gre_family));
+	MPASS(cmd == GRESKEY || cmd == GRESOPTS);
 
-	ip = mtod(m, struct ip *);
-	if (sc->gre_oip.ip_src.s_addr != ip->ip_dst.s_addr ||
-	    sc->gre_oip.ip_dst.s_addr != ip->ip_src.s_addr)
-		goto bad;
+	/* NOTE: we are protected with gre_ioctl_sx lock */
+	MPASS(sc->gre_family == AF_INET);
+	CK_LIST_REMOVE(sc, chain);
+	GRE_WAIT();
+	if (cmd == GRESKEY)
+		sc->gre_key = value;
+	else
+		sc->gre_options = value;
+	in_gre_attach(sc);
+}
 
-	GRE_RUNLOCK(sc);
-	return (32 * 3); /* src + dst + gre_hdr */
-bad:
-	GRE_RUNLOCK(sc);
-	return (0);
+int
+in_gre_ioctl(struct gre_softc *sc, u_long cmd, caddr_t data)
+{
+	struct ifreq *ifr = (struct ifreq *)data;
+	struct sockaddr_in *dst, *src;
+	struct ip *ip;
+	int error;
+
+	/* NOTE: we are protected with gre_ioctl_sx lock */
+	error = EINVAL;
+	switch (cmd) {
+	case SIOCSIFPHYADDR:
+		src = &((struct in_aliasreq *)data)->ifra_addr;
+		dst = &((struct in_aliasreq *)data)->ifra_dstaddr;
+
+		/* sanity checks */
+		if (src->sin_family != dst->sin_family ||
+		    src->sin_family != AF_INET ||
+		    src->sin_len != dst->sin_len ||
+		    src->sin_len != sizeof(*src))
+			break;
+		if (src->sin_addr.s_addr == INADDR_ANY ||
+		    dst->sin_addr.s_addr == INADDR_ANY) {
+			error = EADDRNOTAVAIL;
+			break;
+		}
+		if (V_ipv4_hashtbl == NULL)
+			V_ipv4_hashtbl = gre_hashinit();
+		error = in_gre_checkdup(sc, src->sin_addr.s_addr,
+		    dst->sin_addr.s_addr);
+		if (error == EADDRNOTAVAIL)
+			break;
+		if (error == EEXIST) {
+			/* Addresses are the same. Just return. */
+			error = 0;
+			break;
+		}
+		ip = malloc(sizeof(struct greip) + 3 * sizeof(uint32_t),
+		    M_GRE, M_WAITOK | M_ZERO);
+		ip->ip_src.s_addr = src->sin_addr.s_addr;
+		ip->ip_dst.s_addr = dst->sin_addr.s_addr;
+		if (sc->gre_family != 0) {
+			/* Detach existing tunnel first */
+			CK_LIST_REMOVE(sc, chain);
+			GRE_WAIT();
+			free(sc->gre_hdr, M_GRE);
+			/* XXX: should we notify about link state change? */
+		}
+		sc->gre_family = AF_INET;
+		sc->gre_hdr = ip;
+		sc->gre_oseq = 0;
+		sc->gre_iseq = UINT32_MAX;
+		in_gre_attach(sc);
+		break;
+	case SIOCGIFPSRCADDR:
+	case SIOCGIFPDSTADDR:
+		if (sc->gre_family != AF_INET) {
+			error = EADDRNOTAVAIL;
+			break;
+		}
+		src = (struct sockaddr_in *)&ifr->ifr_addr;
+		memset(src, 0, sizeof(*src));
+		src->sin_family = AF_INET;
+		src->sin_len = sizeof(*src);
+		src->sin_addr = (cmd == SIOCGIFPSRCADDR) ?
+		    sc->gre_oip.ip_src: sc->gre_oip.ip_dst;
+		error = prison_if(curthread->td_ucred, (struct sockaddr *)src);
+		if (error != 0)
+			memset(src, 0, sizeof(*src));
+		break;
+	}
+	return (error);
 }
 
 int
@@ -136,19 +267,30 @@ in_gre_output(struct mbuf *m, int af, int hlen)
 	return (ip_output(m, NULL, NULL, IP_FORWARDING, NULL, NULL));
 }
 
+static const struct encaptab *ecookie = NULL;
 static const struct encap_config ipv4_encap_cfg = {
 	.proto = IPPROTO_GRE,
 	.min_length = sizeof(struct greip) + sizeof(struct ip),
-	.exact_match = (sizeof(in_addr_t) << 4) + 32,
-	.check = in_gre_encapcheck,
+	.exact_match = ENCAP_DRV_LOOKUP,
+	.lookup = in_gre_lookup,
 	.input = gre_input
 };
 
-int
-in_gre_attach(struct gre_softc *sc)
+void
+in_gre_init(void)
 {
 
-	KASSERT(sc->gre_ecookie == NULL, ("gre_ecookie isn't NULL"));
-	sc->gre_ecookie = ip_encap_attach(&ipv4_encap_cfg, sc, M_WAITOK);
-	return (0);
+	if (!IS_DEFAULT_VNET(curvnet))
+		return;
+	ecookie = ip_encap_attach(&ipv4_encap_cfg, NULL, M_WAITOK);
+}
+
+void
+in_gre_uninit(void)
+{
+
+	if (IS_DEFAULT_VNET(curvnet))
+		ip_encap_detach(ecookie);
+	if (V_ipv4_hashtbl != NULL)
+		gre_hashdestroy(V_ipv4_hashtbl);
 }
diff --git a/sys/netinet6/ip6_gre.c b/sys/netinet6/ip6_gre.c
index c00a83db8386..b3c419ce6e6a 100644
--- a/sys/netinet6/ip6_gre.c
+++ b/sys/netinet6/ip6_gre.c
@@ -1,5 +1,5 @@
 /*-
- * Copyright (c) 2014 Andrey V. Elsukov <ae@FreeBSD.org>
+ * Copyright (c) 2014, 2018 Andrey V. Elsukov <ae@FreeBSD.org>
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -31,16 +31,13 @@ __FBSDID("$FreeBSD$");
 #include "opt_inet6.h"
 
 #include <sys/param.h>
-#include <sys/lock.h>
-#include <sys/rmlock.h>
+#include <sys/jail.h>
 #include <sys/systm.h>
 #include <sys/socket.h>
 #include <sys/sockio.h>
 #include <sys/mbuf.h>
 #include <sys/errno.h>
 #include <sys/kernel.h>
-#include <sys/queue.h>
-#include <sys/syslog.h>
 #include <sys/sysctl.h>
 #include <sys/malloc.h>
 
@@ -49,7 +46,6 @@ __FBSDID("$FreeBSD$");
 #include <net/vnet.h>
 
 #include <netinet/in.h>
-#include <netinet/in_systm.h>
 #ifdef INET
 #include <net/ethernet.h>
 #include <netinet/ip.h>
@@ -58,6 +54,7 @@ __FBSDID("$FreeBSD$");
 #include <netinet/ip6.h>
 #include <netinet6/ip6_var.h>
 #include <netinet6/in6_var.h>
+#include <netinet6/scope6_var.h>
 #include <net/if_gre.h>
 
 VNET_DEFINE(int, ip6_gre_hlim) = IPV6_DEFHLIM;
@@ -67,40 +64,184 @@ SYSCTL_DECL(_net_inet6_ip6);
 SYSCTL_INT(_net_inet6_ip6, OID_AUTO, grehlim, CTLFLAG_VNET | CTLFLAG_RW,
     &VNET_NAME(ip6_gre_hlim), 0, "Default hop limit for encapsulated packets");
 
+static VNET_DEFINE(struct gre_list *, ipv6_hashtbl) = NULL;
+#define	V_ipv6_hashtbl		VNET(ipv6_hashtbl)
+#define	GRE_HASH(src, dst)	(V_ipv6_hashtbl[\
+    in6_gre_hashval((src), (dst)) & (GRE_HASH_SIZE - 1)])
+#define	GRE_HASH_SC(sc)		GRE_HASH(&(sc)->gre_oip6.ip6_src,\
+    &(sc)->gre_oip6.ip6_dst)
+
+static uint32_t
+in6_gre_hashval(const struct in6_addr *src, const struct in6_addr *dst)
+{
+	uint32_t ret;
+
+	ret = fnv_32_buf(src, sizeof(*src), FNV1_32_INIT);
+	return (fnv_32_buf(dst, sizeof(*dst), ret));
+}
+
+static int
+in6_gre_checkdup(const struct gre_softc *sc, const struct in6_addr *src,
+    const struct in6_addr *dst)
+{
+	struct gre_softc *tmp;
+
+	if (sc->gre_family == AF_INET6 &&
+	    IN6_ARE_ADDR_EQUAL(&sc->gre_oip6.ip6_src, src) &&
+	    IN6_ARE_ADDR_EQUAL(&sc->gre_oip6.ip6_dst, dst))
+		return (EEXIST);
+
+	CK_LIST_FOREACH(tmp, &GRE_HASH(src, dst), chain) {
+		if (tmp == sc)
+			continue;
+		if (IN6_ARE_ADDR_EQUAL(&tmp->gre_oip6.ip6_src, src) &&
+		    IN6_ARE_ADDR_EQUAL(&tmp->gre_oip6.ip6_dst, dst))
+			return (EADDRNOTAVAIL);
+	}
+	return (0);
+}
+
 static int
-in6_gre_encapcheck(const struct mbuf *m, int off, int proto, void *arg)
+in6_gre_lookup(const struct mbuf *m, int off, int proto, void **arg)
 {
-	GRE_RLOCK_TRACKER;
+	const struct ip6_hdr *ip6;
 	struct gre_softc *sc;
-	struct ip6_hdr *ip6;
 
-	sc = (struct gre_softc *)arg;
-	if ((GRE2IFP(sc)->if_flags & IFF_UP) == 0)
-		return (0);
+	MPASS(in_epoch());
+	ip6 = mtod(m, const struct ip6_hdr *);
+	CK_LIST_FOREACH(sc, &GRE_HASH(&ip6->ip6_dst, &ip6->ip6_src), chain) {
+		/*
+		 * This is an inbound packet, its ip6_dst is source address
+		 * in softc.
+		 */
+		if (IN6_ARE_ADDR_EQUAL(&sc->gre_oip6.ip6_src,
+		    &ip6->ip6_dst) &&
+		    IN6_ARE_ADDR_EQUAL(&sc->gre_oip6.ip6_dst,
+		    &ip6->ip6_src)) {
+			if ((GRE2IFP(sc)->if_flags & IFF_UP) == 0)
+				return (0);
+			*arg = sc;
+			return (ENCAP_DRV_LOOKUP);
+		}
+	}
+	return (0);
+}
 
-	M_ASSERTPKTHDR(m);
+static void
+in6_gre_attach(struct gre_softc *sc)
+{
 
-	GRE_RLOCK(sc);
-	if (sc->gre_family == 0)
-		goto bad;
+	sc->gre_hlen = sizeof(struct greip6);
+	sc->gre_oip6.ip6_vfc = IPV6_VERSION;
+	sc->gre_oip6.ip6_nxt = IPPROTO_GRE;
+	gre_updatehdr(sc, &sc->gre_gi6hdr->gi6_gre);
+	CK_LIST_INSERT_HEAD(&GRE_HASH_SC(sc), sc, chain);
+}
 
-	KASSERT(sc->gre_family == AF_INET6,
-	    ("wrong gre_family: %d", sc->gre_family));
+void
+in6_gre_setopts(struct gre_softc *sc, u_long cmd, uint32_t value)
+{
 
-	ip6 = mtod(m, struct ip6_hdr *);
-	if (!IN6_ARE_ADDR_EQUAL(&sc->gre_oip6.ip6_src, &ip6->ip6_dst) ||
-	    !IN6_ARE_ADDR_EQUAL(&sc->gre_oip6.ip6_dst, &ip6->ip6_src))
-		goto bad;
+	MPASS(cmd == GRESKEY || cmd == GRESOPTS);
 
-	GRE_RUNLOCK(sc);
-	return (128 * 2 + 32);
-bad:
-	GRE_RUNLOCK(sc);
-	return (0);
+	/* NOTE: we are protected with gre_ioctl_sx lock */
+	MPASS(sc->gre_family == AF_INET6);
+	CK_LIST_REMOVE(sc, chain);
+	GRE_WAIT();
+	if (cmd == GRESKEY)
+		sc->gre_key = value;
+	else
+		sc->gre_options = value;
+	in6_gre_attach(sc);
 }
 
 int
-in6_gre_output(struct mbuf *m, int af, int hlen)
+in6_gre_ioctl(struct gre_softc *sc, u_long cmd, caddr_t data)
+{
+	struct in6_ifreq *ifr = (struct in6_ifreq *)data;
+	struct sockaddr_in6 *dst, *src;
+	struct ip6_hdr *ip6;
+	int error;
+
+	/* NOTE: we are protected with gre_ioctl_sx lock */
+	error = EINVAL;
+	switch (cmd) {
+	case SIOCSIFPHYADDR_IN6:
+		src = &((struct in6_aliasreq *)data)->ifra_addr;
+		dst = &((struct in6_aliasreq *)data)->ifra_dstaddr;
+
+		/* sanity checks */
+		if (src->sin6_family != dst->sin6_family ||
+		    src->sin6_family != AF_INET6 ||
+		    src->sin6_len != dst->sin6_len ||
+		    src->sin6_len != sizeof(*src))
+			break;
+		if (IN6_IS_ADDR_UNSPECIFIED(&src->sin6_addr) ||
+		    IN6_IS_ADDR_UNSPECIFIED(&dst->sin6_addr)) {
+			error = EADDRNOTAVAIL;
+			break;
+		}
+		/*
+		 * Check validity of the scope zone ID of the
+		 * addresses, and convert it into the kernel
+		 * internal form if necessary.
+		 */
+		if ((error = sa6_embedscope(src, 0)) != 0 ||
+		    (error = sa6_embedscope(dst, 0)) != 0)
+			break;
+
+		if (V_ipv6_hashtbl == NULL)
+			V_ipv6_hashtbl = gre_hashinit();
+		error = in6_gre_checkdup(sc, &src->sin6_addr,
+		    &dst->sin6_addr);
+		if (error == EADDRNOTAVAIL)
+			break;
+		if (error == EEXIST) {
+			/* Addresses are the same. Just return. */
+			error = 0;
+			break;
+		}
+		ip6 = malloc(sizeof(struct greip6) + 3 * sizeof(uint32_t),
+		    M_GRE, M_WAITOK | M_ZERO);
+		ip6->ip6_src = src->sin6_addr;
+		ip6->ip6_dst = dst->sin6_addr;
+		if (sc->gre_family != 0) {
+			/* Detach existing tunnel first */
+			CK_LIST_REMOVE(sc, chain);
+			GRE_WAIT();
+			free(sc->gre_hdr, M_GRE);
+			/* XXX: should we notify about link state change? */
+		}
+		sc->gre_family = AF_INET6;
+		sc->gre_hdr = ip6;
+		sc->gre_oseq = 0;
+		sc->gre_iseq = UINT32_MAX;
+		in6_gre_attach(sc);
+		break;
+	case SIOCGIFPSRCADDR_IN6:
+	case SIOCGIFPDSTADDR_IN6:
+		if (sc->gre_family != AF_INET6) {
+			error = EADDRNOTAVAIL;
+			break;
+		}
+		src = (struct sockaddr_in6 *)&ifr->ifr_addr;
+		memset(src, 0, sizeof(*src));
+		src->sin6_family = AF_INET6;
+		src->sin6_len = sizeof(*src);
+		src->sin6_addr = (cmd == SIOCGIFPSRCADDR_IN6) ?
+		    sc->gre_oip6.ip6_src: sc->gre_oip6.ip6_dst;
+		error = prison_if(curthread->td_ucred, (struct sockaddr *)src);
+		if (error == 0)
+			error = sa6_recoverscope(src);
+		if (error != 0)
+			memset(src, 0, sizeof(*src));
+		break;
+	}
+	return (error);
+}
+
+int
+in6_gre_output(struct mbuf *m, int af __unused, int hlen __unused)
 {
 	struct greip6 *gi6;
 
@@ -109,6 +250,7 @@ in6_gre_output(struct mbuf *m, int af, int hlen)
 	return (ip6_output(m, NULL, NULL, IPV6_MINMTU, NULL, NULL, NULL));
 }
 
+static const struct encaptab *ecookie = NULL;
 static const struct encap_config ipv6_encap_cfg = {
 	.proto = IPPROTO_GRE,
 	.min_length = sizeof(struct greip6) +
@@ -117,16 +259,26 @@ static const struct encap_config ipv6_encap_cfg = {
 #else
 	    sizeof(struct ip6_hdr),
 #endif
-	.exact_match = (sizeof(struct in6_addr) << 4) + 32,
-	.check = in6_gre_encapcheck,
+	.exact_match = ENCAP_DRV_LOOKUP,
+	.lookup = in6_gre_lookup,
 	.input = gre_input
 };
 
-int
-in6_gre_attach(struct gre_softc *sc)
+void
+in6_gre_init(void)
 {
 
-	KASSERT(sc->gre_ecookie == NULL, ("gre_ecookie isn't NULL"));
-	sc->gre_ecookie = ip6_encap_attach(&ipv6_encap_cfg, sc, M_WAITOK);
-	return (0);
+	if (!IS_DEFAULT_VNET(curvnet))
+		return;
+	ecookie = ip6_encap_attach(&ipv6_encap_cfg, NULL, M_WAITOK);
+}
+
+void
+in6_gre_uninit(void)
+{
+
+	if (IS_DEFAULT_VNET(curvnet))
+		ip6_encap_detach(ecookie);
+	if (V_ipv6_hashtbl != NULL)
+		gre_hashdestroy(V_ipv6_hashtbl);
 }