Index: if_gre.c
===================================================================
RCS file: /cvs/src/sys/net/if_gre.c,v
retrieving revision 1.119
diff -u -p -r1.119 if_gre.c
--- if_gre.c	27 Feb 2018 22:36:38 -0000	1.119
+++ if_gre.c	28 Feb 2018 05:02:44 -0000
@@ -217,6 +217,8 @@ static int
 static int	gre_tunnel_ioctl(struct ifnet *, struct gre_tunnel *,
 		    u_long, void *);
 
+static int	gre_set_parent(unsigned int *, const char *);
+
 /*
  * layer 3 GRE tunnels
  */
@@ -283,18 +285,38 @@ static struct mbuf *
 		gre_l3_encap_dst((_t), &(_t)->t_dst, (_m), (_af))
 
 struct mgre_softc {
-	struct gre_tunnel	sc_tunnel; /* must be first */
-	RBT_ENTRY(mgre_softc)	sc_entry;
+	struct gre_tunnel	 sc_tunnel; /* must be first */
+	unsigned int		 sc_ifp0;
+	RBT_ENTRY(mgre_softc)	 sc_uentry;
+	RBT_ENTRY(mgre_softc)	 sc_mentry;
 
-	struct ifnet		sc_if;
+	struct ifnet		 sc_if;
+
+	struct mbuf_queue	 sc_send_list;
+	struct task		 sc_send_task;
+
+	void			*sc_inm;
+	void			*sc_dhcookie;
+
+	void			(*sc_patch_dst)(struct mbuf *m, const void *);
 };
 
-RBT_HEAD(mgre_tree, mgre_softc);
+RBT_HEAD(mgre_ucast_tree, mgre_softc);
+RBT_HEAD(mgre_mcast_tree, mgre_softc);
 
 static inline int
-		mgre_cmp(const struct mgre_softc *, const struct mgre_softc *);
+		mgre_cmp_ucast(const struct mgre_softc *,
+		    const struct mgre_softc *);
+static int	mgre_cmp_mcast(const struct gre_tunnel *,
+		    const union gre_addr *, unsigned int,
+		    const struct gre_tunnel *, const union gre_addr *,
+		    unsigned int);
+static inline int
+		mgre_cmp_mcast_sc(const struct mgre_softc *,
+		    const struct mgre_softc *);
 
-RBT_PROTOTYPE(mgre_tree, mgre_softc, sc_entry, mgre_cmp);
+RBT_PROTOTYPE(mgre_ucast_tree, mgre_softc, sc_uentry, mgre_ucast_cmp);
+RBT_PROTOTYPE(mgre_mcast_tree, mgre_softc, sc_mentry, mgre_mcast_cmp_sc);
 
 static int	mgre_clone_create(struct if_clone *, int);
 static int	mgre_clone_destroy(struct ifnet *);
@@ -310,10 +332,21 @@ static int	mgre_ioctl(struct ifnet *, u_
 static int	mgre_set_tunnel(struct mgre_softc *, struct if_laddrreq *);
 static int	mgre_get_tunnel(struct mgre_softc *, struct if_laddrreq *);
 static int	mgre_up(struct mgre_softc *);
+static int	mgre_up_ucast(struct mgre_softc *);
+static int	mgre_up_mcast(struct mgre_softc *);
 static int	mgre_down(struct mgre_softc *);
+static void	mgre_detach(void *);
+
+static void	mgre_send(void *);
+
+static void	mgre_patch4(struct mbuf *, const void *);
+#ifdef INET6
+static void	mgre_patch6(struct mbuf *, const void *);
+#endif
 
 /* protected by NET_LOCK */
-struct mgre_tree mgre_tree = RBT_INITIALIZER();
+struct mgre_ucast_tree mgre_ucast_tree = RBT_INITIALIZER();
+struct mgre_mcast_tree mgre_mcast_tree = RBT_INITIALIZER();
 
 /*
  * Ethernet GRE tunnels
@@ -433,7 +466,6 @@ static int	nvgre_ioctl(struct ifnet *, u
 
 static int	nvgre_up(struct nvgre_softc *);
 static int	nvgre_down(struct nvgre_softc *);
-static int	nvgre_set_parent(struct nvgre_softc *, const char *);
 static void	nvgre_link_change(void *);
 static void	nvgre_detach(void *);
 
@@ -561,7 +593,7 @@ mgre_clone_create(struct if_clone *ifc, 
 	ifp->if_type = IFT_L3IPVLAN;
 	ifp->if_hdrlen = GRE_HDRLEN;
 	ifp->if_mtu = GREMTU;
-	ifp->if_flags = 0; /* it's not p2p, and can't mcast or bcast */
+	ifp->if_flags = IFF_MULTICAST|IFF_BROADCAST|IFF_SIMPLEX;
 	ifp->if_xflags = IFXF_CLONED;
 	ifp->if_rtrequest = p2p_rtrequest; /* maybe? */;
 	ifp->if_output = mgre_output;
@@ -571,6 +603,9 @@ mgre_clone_create(struct if_clone *ifc, 
 	sc->sc_tunnel.t_ttl = ip_defttl;
 	sc->sc_tunnel.t_df = htons(0);
 
+	mq_init(&sc->sc_send_list, IFQ_MAXLEN * 2, IPL_SOFTNET);
+	task_set(&sc->sc_send_task, mgre_send, sc);
+
 	if_attach(ifp);
 	if_alloc_sadl(ifp);
 
@@ -789,18 +824,47 @@ gre_find(const struct gre_tunnel *key)
 }
 
 static inline struct ifnet *
-mgre_find(const struct gre_tunnel *key)
+mgre_ucast_find(const struct gre_tunnel *key)
 {
 	struct mgre_softc *sc;
 
 	NET_ASSERT_LOCKED();
-	sc = RBT_FIND(mgre_tree, &mgre_tree, (const struct mgre_softc *)key);
+	sc = RBT_FIND(mgre_ucast_tree, &mgre_ucast_tree,
+	    (const struct mgre_softc *)key);
 	if (sc != NULL)
 		return (&sc->sc_if);
 
 	return (NULL);
 }
 
+static inline struct ifnet *
+mgre_mcast_find(const struct gre_tunnel *key, unsigned int if0idx)
+{
+	struct mgre_softc *sc;
+	int rv;
+
+	/*
+	 * building an mgre_softc to use with RBT_FIND is expensive, and
+	 * would need to swap the src and dst addresses in the key. so do the
+	 * find by hand.
+	 */
+
+	NET_ASSERT_LOCKED();
+	sc = RBT_ROOT(mgre_mcast_tree, &mgre_mcast_tree);
+	while (sc != NULL) {
+		rv = mgre_cmp_mcast(key, &key->t_src, if0idx,
+		    &sc->sc_tunnel, &sc->sc_tunnel.t_dst, sc->sc_ifp0);
+		if (rv == 0)
+			return (&sc->sc_if);
+		if (rv < 0)
+			sc = RBT_LEFT(mgre_mcast_tree, sc);
+		else
+			sc = RBT_RIGHT(mgre_mcast_tree, sc);
+	}
+
+	return (NULL);
+}
+
 static int
 gre_input_key(struct mbuf **mp, int *offp, int type, int af,
     struct gre_tunnel *key)
@@ -886,9 +950,14 @@ gre_input_key(struct mbuf **mp, int *off
 
 	ifp = gre_find(key);
 	if (ifp == NULL) {
-		ifp = mgre_find(key);
-		if (ifp == NULL)
-			goto decline;
+		ifp = mgre_ucast_find(key);
+		if (ifp == NULL) {
+			ifp = mgre_mcast_find(key, m->m_pkthdr.ph_ifidx);
+			if (ifp == NULL)
+				goto decline;
+
+			mcast = M_MCAST|M_BCAST;
+		}
 	}
 
 	switch (gh->gre_proto) {
@@ -1522,34 +1591,6 @@ mgre_output(struct ifnet *ifp, struct mb
 		error = EAFNOSUPPORT;
 		goto drop;
 	}
-
-	if (ISSET(m->m_flags, M_MCAST|M_BCAST)) {
-		error = ENETUNREACH;
-		goto drop;
-	}
-
-	rt = rt_getll(rt0);
-
-	/* chech rt_expire? */
-	if (ISSET(rt->rt_flags, RTF_REJECT)) {
-		error = (rt == rt0) ? EHOSTDOWN : EHOSTUNREACH;
-		goto drop;
-	}
-	if (!ISSET(rt->rt_flags, RTF_HOST)) {
-		error = EHOSTUNREACH;
-		goto drop;
-	}
-	if (ISSET(rt->rt_flags, RTF_GATEWAY)) {
-		error = EINVAL;
-		goto drop;
-	}
-
-	gate = rt->rt_gateway;
-	af = gate->sa_family;
-	if (af != sc->sc_tunnel.t_af) {
-		error = EAGAIN;
-		goto drop;
-	}
  
 	/* Try to limit infinite recursion through misconfiguration. */
 	for (mtag = m_tag_find(m, PACKET_TAG_GRE, NULL); mtag;
@@ -1561,6 +1602,51 @@ mgre_output(struct ifnet *ifp, struct mb
 		}
 	}
 
+	if (ISSET(m->m_flags, M_MCAST|M_BCAST))
+		addr = &sc->sc_tunnel.t_dst;
+	else {
+		rt = rt_getll(rt0);
+
+		/* chech rt_expire? */
+		if (ISSET(rt->rt_flags, RTF_REJECT)) {
+			error = (rt == rt0) ? EHOSTDOWN : EHOSTUNREACH;
+			goto drop;
+		}
+		if (ISSET(rt->rt_flags, RTF_GATEWAY)) {
+			error = EINVAL;
+			goto drop;
+		}
+		if (!ISSET(rt->rt_flags, RTF_HOST)) {
+			error = EHOSTUNREACH;
+			goto drop;
+		}
+
+		gate = rt->rt_gateway;
+		af = gate->sa_family;
+		if (af != sc->sc_tunnel.t_af) {
+			error = EAGAIN;
+			goto drop;
+		}
+
+		switch (af) {
+		case AF_INET:
+			addr = &satosin(gate)->sin_addr;
+			break;
+	#ifdef INET6
+		case AF_INET6:
+			addr = &satosin6(gate)->sin6_addr;
+			break;
+	 #endif
+		default:
+			unhandled_af(af);
+			/* NOTREACHED */
+		}
+	}
+
+	m = gre_l3_encap_dst(&sc->sc_tunnel, addr, m, dest->sa_family);
+	if (m == NULL)
+		return (ENOBUFS);
+
 	mtag = m_tag_get(PACKET_TAG_GRE, sizeof(ifp->if_index), M_NOWAIT);
 	if (mtag == NULL) {
 		error = ENOBUFS;
@@ -1569,28 +1655,6 @@ mgre_output(struct ifnet *ifp, struct mb
 	memcpy((caddr_t)(mtag + 1), &ifp->if_index, sizeof(ifp->if_index));
 	m_tag_prepend(m, mtag);
 
-	switch (af) {
-	case AF_INET: {
-		struct sockaddr_in *sin = (struct sockaddr_in *)gate;
-		addr = &sin->sin_addr;
-		break;
-	}
-#ifdef INET6
-	case AF_INET6: {
-		struct sockaddr_in6 *sin6 = (struct sockaddr_in6 *)gate;
-		addr = &sin6->sin6_addr;
-		break;
-	}
- #endif
-	default:
-		unhandled_af(af);
-		/* NOTREACHED */
-	}
-
-	m = gre_l3_encap_dst(&sc->sc_tunnel, addr, m, dest->sa_family);
-	if (m == NULL)
-		return (ENOBUFS);
-
 	m->m_pkthdr.ph_family = dest->sa_family;
 
 	error = if_enqueue(ifp, m);
@@ -1604,9 +1668,42 @@ drop:
 }
 
 static void
+mgre_patch4(struct mbuf *m, const void *addrp)
+{
+	const struct in_addr *addr = addrp;
+	struct ip *ip;
+
+	ip = mtod(m, struct ip *);
+	ip->ip_dst = *addr;
+}
+
+static void
+mgre_patch6(struct mbuf *m, const void *addrp)
+{
+	const struct in6_addr *addr = addrp;
+	struct ip6_hdr *ip6;
+
+	ip6 = mtod(m, struct ip6_hdr *);
+	ip6->ip6_dst = *addr;
+}
+
+static inline void
+mgre_start_mcast(struct mgre_softc *sc, struct mbuf_list *ml, struct mbuf *m)
+{
+	/* if map multicast dynamic, copy packet to llinfo */
+
+	if (sc->sc_ifp0 == 0) {
+		m_freem(m);
+		return;
+	}
+	ml_enqueue(ml, m);
+}
+
+static void
 mgre_start(struct ifnet *ifp)
 {
 	struct mgre_softc *sc = ifp->if_softc;
+	struct mbuf_list ml = MBUF_LIST_INITIALIZER();
 	struct mbuf *m;
 #if NBPFILTER > 0
 	caddr_t if_bpf;
@@ -1632,11 +1729,23 @@ mgre_start(struct ifnet *ifp)
 			    (struct mbuf *)&mh, BPF_DIRECTION_OUT);
 		}
 #endif
+		m->m_pkthdr.ph_rtableid = sc->sc_tunnel.t_rtableid;
+#if NPF > 0
+		pf_pkt_addr_changed(m);
+#endif
 
-		if (m == NULL || gre_ip_output(&sc->sc_tunnel, m) != 0) {
- 			ifp->if_oerrors++;
- 			continue;
- 		}
+		if (ISSET(m->m_flags, M_MCAST|M_BCAST)) {
+			mgre_start_mcast(sc, &ml, m);
+			continue;
+		}
+
+		ml_enqueue(&ml, m);
+	}
+
+	if (!ml_empty(&ml)) {
+		if (mq_enlist(&sc->sc_send_list, &ml) == 0)
+			task_add(net_tq(ifp->if_index), &sc->sc_send_task);
+		/* else set OACTIVE? */
 	}
 }
 
@@ -2006,6 +2115,8 @@ mgre_ioctl(struct ifnet *ifp, u_long cmd
 {
 	struct mgre_softc *sc = ifp->if_softc;
 	struct ifreq *ifr = (struct ifreq *)data;
+	struct if_parent *parent = (struct if_parent *)data;
+	struct ifnet *ifp0;
 	int error = 0;
 
 	switch(cmd) {
@@ -2038,6 +2149,32 @@ mgre_ioctl(struct ifnet *ifp, u_long cmd
 		ifr->ifr_ttl = sc->sc_tunnel.t_ttl;
 		break;
 
+	case SIOCSIFPARENT:
+		if (ISSET(ifp->if_flags, IFF_RUNNING)) {
+			error = EBUSY;
+			break;
+		}
+		error = gre_set_parent(&sc->sc_ifp0, parent->ifp_parent);
+		break;
+	case SIOCGIFPARENT:
+		ifp0 = if_get(sc->sc_ifp0);
+		if (ifp0 == NULL)
+			error = EADDRNOTAVAIL;
+		else {
+			memcpy(parent->ifp_parent, ifp0->if_xname,
+			    sizeof(parent->ifp_parent));
+		}
+		if_put(ifp0);
+		break;
+	case SIOCDIFPARENT:
+		if (ISSET(ifp->if_flags, IFF_RUNNING)) {
+			error = EBUSY;
+			break;
+		}
+		/* commit */
+		sc->sc_ifp0 = 0;
+		break;
+
 	case SIOCSLIFPHYADDR:
 		if (ISSET(ifp->if_flags, IFF_RUNNING)) {
 			error = EBUSY;
@@ -2080,7 +2217,7 @@ mgre_set_tunnel(struct mgre_softc *sc, s
 #endif
 
 	if (dstaddr->sa_family != AF_UNSPEC)
-		return (EINVAL);
+		return (gre_set_tunnel(tunnel, req, 0));
 
 	/* validate */
 	switch (addr->sa_family) {
@@ -2139,6 +2276,9 @@ mgre_get_tunnel(struct mgre_softc *sc, s
 	case AF_UNSPEC:
 		return (EADDRNOTAVAIL);
 	case AF_INET:
+		if (!in_nullhost(tunnel->t_dst4))
+			return (gre_get_tunnel(tunnel, req));
+
 		sin = (struct sockaddr_in *)&req->addr;
 		memset(sin, 0, sizeof(*sin));
 		sin->sin_family = AF_INET;
@@ -2148,6 +2288,9 @@ mgre_get_tunnel(struct mgre_softc *sc, s
 
 #ifdef INET6
 	case AF_INET6:
+		if (!IN6_IS_ADDR_UNSPECIFIED(&tunnel->t_dst6))
+			return (gre_get_tunnel(tunnel, req));
+
 		sin6 = (struct sockaddr_in6 *)&req->addr;
 		memset(sin6, 0, sizeof(*sin6));
 		sin6->sin6_family = AF_INET6;
@@ -2278,7 +2421,7 @@ nvgre_ioctl(struct ifnet *ifp, u_long cm
 			error = EBUSY;
 			break;
 		}
-		error = nvgre_set_parent(sc, parent->ifp_parent);
+		error = gre_set_parent(&sc->sc_ifp0, parent->ifp_parent);
 		if (error == 0)
 			nvgre_flush_map(sc);
 		break;
@@ -2816,16 +2959,35 @@ static int
 mgre_up(struct mgre_softc *sc)
 {
 	unsigned int hlen;
+	void (*patch)(struct mbuf *, const void *);
+	int error;
 
 	switch (sc->sc_tunnel.t_af) {
 	case AF_UNSPEC:
 		return (EDESTADDRREQ);
 	case AF_INET:
+		if (sc->sc_ifp0 != 0) {
+			if (!IN_MULTICAST(sc->sc_tunnel.t_dst4.s_addr))
+				return (EDESTADDRREQ);
+		} else {
+			if (IN_MULTICAST(sc->sc_tunnel.t_dst4.s_addr))
+				return (ENXIO);
+		}
+
 		hlen = sizeof(struct ip);
+		patch = mgre_patch4;
 		break;
 #ifdef INET6
 	case AF_INET6:
+		if (sc->sc_ifp0 != 0) {
+			if (!IN6_IS_ADDR_MULTICAST(&sc->sc_tunnel.t_dst6))
+				return (EDESTADDRREQ);
+		} else {
+			if (IN6_IS_ADDR_MULTICAST(&sc->sc_tunnel.t_dst6))
+				return (ENXIO);
+		}
 		hlen = sizeof(struct ip6_hdr);
+		patch = mgre_patch6;
 		break;
 #endif /* INET6 */
 	}
@@ -2836,30 +2998,171 @@ mgre_up(struct mgre_softc *sc)
 
 	NET_ASSERT_LOCKED();
 
-	if (RBT_INSERT(mgre_tree, &mgre_tree, sc) != NULL)
-		return (EADDRINUSE);
+	error = sc->sc_ifp0 == 0 ? mgre_up_ucast(sc) : mgre_up_mcast(sc);
+	if (error != 0)
+		return (error);
 
 	sc->sc_if.if_hdrlen = hlen;
+	sc->sc_patch_dst = patch;
 	SET(sc->sc_if.if_flags, IFF_RUNNING);
 
 	return (0);
 }
 
 static int
+mgre_up_ucast(struct mgre_softc *sc)
+{
+	if (RBT_INSERT(mgre_ucast_tree, &mgre_ucast_tree, sc) != NULL)
+		return (EADDRINUSE);
+
+	return (0);
+}
+
+static int
+mgre_up_mcast(struct mgre_softc *sc)
+{
+	struct gre_tunnel *tunnel = &sc->sc_tunnel;
+	struct ifnet *ifp0;
+	void *inm;
+	int error;
+
+	ifp0 = if_get(sc->sc_ifp0);
+	if (ifp0 == NULL)
+		return (ENXIO);
+	if (!ISSET(ifp0->if_flags, IFF_MULTICAST)) {
+		error = ENODEV;
+		goto put;
+	}
+
+	NET_ASSERT_LOCKED();
+
+	if (RBT_INSERT(mgre_mcast_tree, &mgre_mcast_tree, sc) != NULL) {
+		error = EADDRINUSE;
+		goto put;
+	}
+	if (RBT_INSERT(mgre_ucast_tree, &mgre_ucast_tree, sc) != NULL) {
+		error = EADDRINUSE;
+		goto remove_mcast;
+	}
+
+	switch (tunnel->t_af) {
+	case AF_INET:
+		inm = in_addmulti(&tunnel->t_dst4, ifp0);
+		if (inm == NULL) {
+			error = ECONNABORTED;
+			goto remove_ucast;
+		}
+		break;
+#ifdef INET6
+	case AF_INET6:
+		inm = in6_addmulti(&tunnel->t_dst6, ifp0, &error);
+		if (inm == NULL) {
+			/* error is already set */
+			goto remove_ucast;
+		}
+		break;
+#endif /* INET6 */
+	default:
+		unhandled_af(tunnel->t_af);
+	}
+
+	sc->sc_dhcookie = hook_establish(ifp0->if_detachhooks, 0,
+	    mgre_detach, sc);
+	if (sc->sc_dhcookie == NULL) {
+		error = ENOMEM;
+		goto delmulti;
+	}
+
+	if_put(ifp0);
+
+	sc->sc_inm = inm;
+
+	return (0);
+
+delmulti:
+	switch (tunnel->t_af) {
+	case AF_INET:
+		in_delmulti(inm);
+		break;
+#ifdef INET6
+	case AF_INET6:
+		in6_delmulti(inm);
+		break;
+#endif
+	}
+remove_ucast:
+	RBT_REMOVE(mgre_ucast_tree, &mgre_ucast_tree, sc);
+remove_mcast:
+	RBT_REMOVE(mgre_mcast_tree, &mgre_mcast_tree, sc);
+put:
+	if_put(ifp0);
+
+	return (error);
+}
+
+static int
 mgre_down(struct mgre_softc *sc)
 {
+	struct gre_tunnel *tunnel = &sc->sc_tunnel;
+	struct ifnet *ifp = &sc->sc_if;
+	struct taskq *softnet = net_tq(ifp->if_index);
+	struct ifnet *ifp0;
+
 	NET_ASSERT_LOCKED();
 
-	CLR(sc->sc_if.if_flags, IFF_RUNNING);
-	sc->sc_if.if_hdrlen = GRE_HDRLEN; /* symmetry */
+	CLR(ifp->if_flags, IFF_RUNNING);
 
-	RBT_REMOVE(mgre_tree, &mgre_tree, sc);
+	NET_UNLOCK();
+	ifq_barrier(&ifp->if_snd);
+	if (!task_del(softnet, &sc->sc_send_task))
+		taskq_barrier(softnet);
+	NET_LOCK();
 
-	/* barrier? */
+	mq_purge(&sc->sc_send_list);
+
+	if (sc->sc_ifp0 != 0) {
+		ifp0 = if_get(sc->sc_ifp0);
+		if (ifp0 != NULL) {
+			hook_disestablish(ifp0->if_detachhooks,
+			    sc->sc_dhcookie);
+		}
+		if_put(ifp0);
+
+		switch (tunnel->t_af) {
+		case AF_INET:
+			in_delmulti(sc->sc_inm);
+			break;
+
+	#ifdef INET6
+		case AF_INET6:
+			in6_delmulti(sc->sc_inm);
+			break;
+	#endif
+		}
+
+		RBT_REMOVE(mgre_mcast_tree, &mgre_mcast_tree, sc);
+	}
+
+	RBT_REMOVE(mgre_ucast_tree, &mgre_ucast_tree, sc);
+	sc->sc_if.if_hdrlen = GRE_HDRLEN; /* symmetry */
 
 	return (0);
 }
 
+static void
+mgre_detach(void *arg)
+{
+	struct mgre_softc *sc = arg;
+	struct ifnet *ifp = &sc->sc_if;
+
+	if (ISSET(ifp->if_flags, IFF_RUNNING)) {
+		mgre_down(sc);
+		if_down(ifp);
+	}
+
+	sc->sc_ifp0 = 0;
+}
+
 static int
 egre_up(struct egre_softc *sc)
 {
@@ -3067,7 +3370,7 @@ nvgre_detach(void *arg)
 }
 
 static int
-nvgre_set_parent(struct nvgre_softc *sc, const char *parent)
+gre_set_parent(unsigned int *ifidxp, const char *parent)
 {
 	struct ifnet *ifp0;
 
@@ -3079,7 +3382,7 @@ nvgre_set_parent(struct nvgre_softc *sc,
 		return (EPROTONOSUPPORT);
 
 	/* commit */
-	sc->sc_ifp0 = ifp0->if_index;
+	*ifidxp = ifp0->if_index;
 
 	return (0);
 }
@@ -3211,18 +3514,21 @@ nvgre_start(struct ifnet *ifp)
 }
 
 static uint64_t
-nvgre_send4(struct nvgre_softc *sc, struct mbuf_list *ml)
+gre_send4(struct mbuf_list *ml, unsigned int ifidx)
 {
 	struct ip_moptions imo;
 	struct mbuf *m;
 	uint64_t oerrors = 0;
+	struct ip *ip;
 
-	imo.imo_ifidx = sc->sc_ifp0;
-	imo.imo_ttl = sc->sc_tunnel.t_ttl;
+	imo.imo_ifidx = ifidx;
 	imo.imo_loop = 0;
 
 	NET_RLOCK();
 	while ((m = ml_dequeue(ml)) != NULL) {
+		ip = mtod(m, struct ip *);
+		imo.imo_ttl = ip->ip_ttl;
+
 		if (ip_output(m, NULL, NULL, IP_RAWOUTPUT, &imo, NULL, 0) != 0)
 			oerrors++;
 	}
@@ -3233,18 +3539,21 @@ nvgre_send4(struct nvgre_softc *sc, stru
 
 #ifdef INET6
 static uint64_t
-nvgre_send6(struct nvgre_softc *sc, struct mbuf_list *ml)
+gre_send6(struct mbuf_list *ml, unsigned int ifidx)
 {
 	struct ip6_moptions im6o;
 	struct mbuf *m;
 	uint64_t oerrors = 0;
+	struct ip6_hdr *ip6;
 
-	im6o.im6o_ifidx = sc->sc_ifp0;
-	im6o.im6o_hlim = sc->sc_tunnel.t_ttl;
+	im6o.im6o_ifidx = ifidx;
 	im6o.im6o_loop = 0;
 
 	NET_RLOCK();
 	while ((m = ml_dequeue(ml)) != NULL) {
+		ip6 = mtod(m, struct ip6_hdr *);
+		im6o.im6o_hlim = ip6->ip6_hlim;
+		
 		if (ip6_output(m, NULL, NULL, 0, &im6o, NULL) != 0)
 			oerrors++;
 	}
@@ -3254,29 +3563,23 @@ nvgre_send6(struct nvgre_softc *sc, stru
 }
 #endif /* INET6 */
 
-static void
-nvgre_send(void *arg)
+static uint64_t
+gre_send(unsigned int if0idx, sa_family_t af, struct mbuf_queue *mq)
 {
-	struct nvgre_softc *sc = arg;
-	struct ifnet *ifp = &sc->sc_ac.ac_if;
-	sa_family_t af = sc->sc_tunnel.t_af;
 	struct mbuf_list ml;
 	uint64_t oerrors;
 
-	if (!ISSET(ifp->if_flags, IFF_RUNNING))
-		return;
-
-	mq_delist(&sc->sc_send_list, &ml);
+	mq_delist(mq, &ml);
 	if (ml_empty(&ml))
-		return;
+		return (0);
 
 	switch (af) {
 	case AF_INET:
-		oerrors = nvgre_send4(sc, &ml);
+		oerrors = gre_send4(&ml, if0idx);
 		break;
 #ifdef INET6
 	case AF_INET6:
-		oerrors = nvgre_send6(sc, &ml);
+		oerrors = gre_send6(&ml, if0idx);
 		break;
 #endif
 	default:
@@ -3284,7 +3587,33 @@ nvgre_send(void *arg)
 		/* NOTREACHED */
 	}
 
-	ifp->if_oerrors += oerrors; /* XXX should be ifq_oerrors */
+	return (oerrors);
+}
+
+static void
+nvgre_send(void *arg)
+{
+	struct nvgre_softc *sc = arg;
+	struct ifnet *ifp = &sc->sc_ac.ac_if;
+
+	if (!ISSET(ifp->if_flags, IFF_RUNNING))
+		return;
+
+	ifp->if_oerrors +=
+	    gre_send(sc->sc_ifp0, sc->sc_tunnel.t_af, &sc->sc_send_list);
+}
+
+static void
+mgre_send(void *arg)
+{
+	struct mgre_softc *sc = arg;
+	struct ifnet *ifp = &sc->sc_if;
+
+	if (!ISSET(ifp->if_flags, IFF_RUNNING))
+		return;
+
+	ifp->if_oerrors +=
+	    gre_send(sc->sc_ifp0, sc->sc_tunnel.t_af, &sc->sc_send_list);
 }
 
 int
@@ -3332,11 +3661,10 @@ gre_ip_cmp(int af, const union gre_addr 
 }
 
 static int
-gre_cmp_src(const struct gre_tunnel *a, const struct gre_tunnel *b)
+gre_cmp_tunnel(const struct gre_tunnel *a, const struct gre_tunnel *b)
 {
 	uint32_t ka, kb;
 	uint32_t mask;
-	int rv;
 
 	/* is K set at all? */
 	ka = a->t_key_mask & GRE_KEY_ENTROPY;
@@ -3375,6 +3703,18 @@ gre_cmp_src(const struct gre_tunnel *a, 
 	if (a->t_af < b->t_af)
 		return (-1);
 
+	return (0);
+}
+
+static int
+gre_cmp_src(const struct gre_tunnel *a, const struct gre_tunnel *b)
+{
+	int rv;
+
+	rv = gre_cmp_tunnel(a, b);
+	if (rv != 0)
+		return (rv);
+
 	rv = gre_ip_cmp(a->t_af, &a->t_src, &b->t_src);
 	if (rv != 0)
 		return (rv);
@@ -3394,13 +3734,47 @@ gre_cmp(const struct gre_tunnel *a, cons
 	return (gre_ip_cmp(a->t_af, &a->t_dst, &b->t_dst));
 }
 
+static int
+mgre_cmp_mcast(const struct gre_tunnel *a, const union gre_addr *aa,
+    unsigned int if0idxa, const struct gre_tunnel *b,
+    const union gre_addr *ab, unsigned int if0idxb)
+{
+	int rv;
+
+	rv = gre_cmp_tunnel(a, b);
+	if (rv != 0)
+		return (rv);
+
+	rv = gre_ip_cmp(a->t_af, aa, ab);
+	if (rv != 0)
+		return (rv);
+
+	if (if0idxa > if0idxb)
+		return (1);
+	if (if0idxa < if0idxb)
+		return (-1);
+
+	return (0);
+}
+
+static inline int
+mgre_cmp_mcast_sc(const struct mgre_softc *na, const struct mgre_softc *nb)
+{
+	const struct gre_tunnel *a = &na->sc_tunnel;
+	const struct gre_tunnel *b = &nb->sc_tunnel;
+
+	return (mgre_cmp_mcast(a, &a->t_dst, na->sc_ifp0,
+	    b, &b->t_dst, nb->sc_ifp0));
+}
+
 static inline int
-mgre_cmp(const struct mgre_softc *a, const struct mgre_softc *b)
+mgre_cmp_ucast(const struct mgre_softc *a, const struct mgre_softc *b)
 {
 	return (gre_cmp_src(&a->sc_tunnel, &b->sc_tunnel));
 }
 
-RBT_GENERATE(mgre_tree, mgre_softc, sc_entry, mgre_cmp);
+RBT_GENERATE(mgre_ucast_tree, mgre_softc, sc_uentry, mgre_cmp_ucast);
+RBT_GENERATE(mgre_mcast_tree, mgre_softc, sc_mentry, mgre_cmp_mcast_sc);
 
 static inline int
 egre_cmp(const struct egre_softc *a, const struct egre_softc *b)