UnivNautes

diff --git a/sbin/ifconfig/Makefile b/sbin/ifconfig/Makefile

index a10d1fb..bbd7b89 100644

--- a/sbin/ifconfig/Makefile

+++ b/sbin/ifconfig/Makefile

@@ -33,6 +33,7 @@ SRCS+=	iffib.c			# non-default FIB support

 SRCS+=	ifvlan.c		# SIOC[GS]ETVLAN support

 SRCS+=	ifgre.c			# GRE keys etc

 SRCS+=	ifgif.c			# GIF reversed header workaround

+SRCS+=	ifstf.c			# STF configuration options

 SRCS+=	ifieee80211.c regdomain.c # SIOC[GS]IEEE80211 support

 DPADD+=	${LIBBSDXML} ${LIBSBUF}

diff --git a/sbin/ifconfig/ifstf.c b/sbin/ifconfig/ifstf.c

new file mode 100644

index 0000000..52efd70

--- /dev/null

+++ b/sbin/ifconfig/ifstf.c

@@ -0,0 +1,156 @@

+/*-

+ * Copyright 2013 Ermal Luci

+ * All rights reserved.

+ *

+ * Redistribution and use in source and binary forms, with or without

+ * modification, are permitted provided that the following conditions

+ * are met:

+ * 1. Redistributions of source code must retain the above copyright

+ *    notice, this list of conditions and the following disclaimer.

+ * 2. Redistributions in binary form must reproduce the above copyright

+ *    notice, this list of conditions and the following disclaimer in the

+ *    documentation and/or other materials provided with the distribution.

+ *

+ * THIS SOFTWARE IS PROVIDED BY WASABI SYSTEMS, INC. ``AS IS'' AND

+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED

+ * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR

+ * PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL WASABI SYSTEMS, INC

+ * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR

+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF

+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS

+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN

+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)

+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

+ * POSSIBILITY OF SUCH DAMAGE.

+ */

+

+#include <sys/param.h>

+#include <sys/ioctl.h>

+#include <sys/socket.h>

+#include <sys/sockio.h>

+

+#include <stdlib.h>

+#include <unistd.h>

+

+#include <net/ethernet.h>

+#include <net/if.h>

+#include <net/route.h>

+

+#include <netinet/in.h>

+#include <net/if_stf.h>

+#include <arpa/inet.h>

+

+#include <ctype.h>

+#include <stdio.h>

+#include <string.h>

+#include <stdlib.h>

+#include <unistd.h>

+#include <err.h>

+#include <errno.h>

+

+#include "ifconfig.h"

+

+static int

+do_cmd(int sock, u_long op, void *arg, size_t argsize, int set)

+{

+	struct ifdrv ifd;

+

+	memset(&ifd, 0, sizeof(ifd));

+

+	strlcpy(ifd.ifd_name, ifr.ifr_name, sizeof(ifd.ifd_name));

+	ifd.ifd_cmd = op;

+	ifd.ifd_len = argsize;

+	ifd.ifd_data = arg;

+

+	return (ioctl(sock, set ? SIOCSDRVSPEC : SIOCGDRVSPEC, &ifd));

+}

+

+static void

+stf_status(int s)

+{

+	struct stfv4args param;

+

+	if (do_cmd(s, STF_GV4NET, &param, sizeof(param), 0) < 0)

+		return;

+

+	printf("\tv4net %s/%d -> ", inet_ntoa(param.inaddr), param.prefix ? param.prefix : 32);

+	printf("tv4br %s\n", inet_ntoa(param.dstv4_addr));

+	    

+	return;

+}

+

+static void

+setstf_br(const char *val, int d, int s, const struct afswtch *afp)

+{

+	struct stfv4args req;

+        struct sockaddr_in sin;

+

+	memset(&req, 0, sizeof(req));

+

+        sin.sin_len = sizeof(sin);

+	sin.sin_family = AF_INET;

+

+        if (!inet_aton(val, &sin.sin_addr))

+                errx(1, "%s: bad value", val);

+

+	req.dstv4_addr = sin.sin_addr;

+	if (do_cmd(s, STF_SDSTV4, &req, sizeof(req), 1) < 0)

+		err(1, "STF_SV4DST %s",  val);

+}

+

+static void

+setstf_set(const char *val, int d, int s, const struct afswtch *afp)

+{

+	struct stfv4args req;

+        struct sockaddr_in sin;

+	const char *errstr;

+	char *p = NULL;

+

+	memset(&req, 0, sizeof(req));

+

+        sin.sin_len = sizeof(sin);

+	sin.sin_family = AF_INET;

+

+	p = strrchr(val, '/');

+	if (p == NULL)

+		errx(2, "Wrong argument given");

+

+	*p = '\0';

+	if (!isdigit(*(p + 1)))

+		errstr = "invalid";

+	else

+		req.prefix = (int)strtonum(p + 1, 0, 32, &errstr);

+	if (errstr != NULL) {

+		*p = '/';

+		errx(1, "%s: bad value (width %s)", val, errstr);

+	}

+

+        if (!inet_aton(val, &sin.sin_addr))

+                errx(1, "%s: bad value", val);

+

+	req.inaddr = sin.sin_addr;

+	if (do_cmd(s, STF_SV4NET, &req, sizeof(req), 1) < 0)

+		err(1, "STF_SV4NET %s",  val);

+}

+

+static struct cmd stf_cmds[] = {

+	DEF_CMD_ARG("stfv4net",		setstf_set),

+	DEF_CMD_ARG("stfv4br",		setstf_br),

+};

+static struct afswtch af_stf = {

+	.af_name	= "af_stf",

+	.af_af		= AF_UNSPEC,

+	.af_other_status = stf_status,

+};

+

+static __constructor void

+stf_ctor(void)

+{

+#define	N(a)	(sizeof(a) / sizeof(a[0]))

+	int i;

+

+	for (i = 0; i < N(stf_cmds);  i++)

+		cmd_register(&stf_cmds[i]);

+	af_register(&af_stf);

+#undef N

+}

diff --git a/sys/net/if_stf.c b/sys/net/if_stf.c

index 20251dc..b13c8a5 100644

--- a/sys/net/if_stf.c

+++ b/sys/net/if_stf.c

@@ -3,6 +3,8 @@

 /*-

  * Copyright (C) 2000 WIDE Project.

+ * Copyright (c) 2010 Hiroki Sato <hrs@FreeBSD.org>

+ * Copyright (c) 2013 Ermal Lu?i <eri@FreeBSD.org>

  * All rights reserved.

  *

  * Redistribution and use in source and binary forms, with or without

@@ -31,7 +33,7 @@

  */

 /*

- * 6to4 interface, based on RFC3056.

+ * 6to4 interface, based on RFC3056 + 6rd (RFC5569) support.

  *

  * 6to4 interface is NOT capable of link-layer (I mean, IPv4) multicasting.

  * There is no address mapping defined from IPv6 multicast address to IPv4

@@ -60,7 +62,7 @@

  * ICMPv6:

  * - Redirects cannot be used due to the lack of link-local address.

  *

- * stf interface does not have, and will not need, a link-local address.  

+ * stf interface does not have, and will not need, a link-local address.

  * It seems to have no real benefit and does not help the above symptoms much.

  * Even if we assign link-locals to interface, we cannot really

  * use link-local unicast/multicast on top of 6to4 cloud (since there's no

@@ -72,6 +74,12 @@

  * http://playground.iijlab.net/i-d/draft-itojun-ipv6-transition-abuse-00.txt

  * for details.  The code tries to filter out some of malicious packets.

  * Note that there is no way to be 100% secure.

+ *

+ * 6rd (RFC5569 & RFC5969) extension is enabled when an IPv6 GUA other than

+ * 2002::/16 is assigned.  The stf(4) recognizes a 32-bit just after

+ * prefixlen as the IPv4 address of the 6rd customer site.  The

+ * prefixlen must be shorter than 32.

+ *

  */

 #include "opt_inet.h"

@@ -92,13 +100,14 @@

 #include <machine/cpu.h>

 #include <sys/malloc.h>

+#include <sys/priv.h>

 #include <net/if.h>

+#include <net/if_var.h>

 #include <net/if_clone.h>

 #include <net/route.h>

 #include <net/netisr.h>

 #include <net/if_types.h>

-#include <net/if_stf.h>

 #include <net/vnet.h>

 #include <netinet/in.h>

@@ -106,6 +115,7 @@

 #include <netinet/ip.h>

 #include <netinet/ip_var.h>

 #include <netinet/in_var.h>

+#include <net/if_stf.h>

 #include <netinet/ip6.h>

 #include <netinet6/ip6_var.h>

@@ -120,20 +130,48 @@

 #include <security/mac/mac_framework.h>

+#define	STF_DEBUG 1

+#if	STF_DEBUG > 3

+#define	ip_sprintf(buf, a)						\

+	sprintf(buf, "%u.%u.%u.%u",					\

+		(ntohl((a)->s_addr)>>24)&0xFF,				\

+		(ntohl((a)->s_addr)>>16)&0xFF,				\

+		(ntohl((a)->s_addr)>>8)&0xFF,				\

+		(ntohl((a)->s_addr))&0xFF);

+#endif

+

+#if STF_DEBUG

+#define	DEBUG_PRINTF(a, ...)						\

+	do {								\

+		if (V_stf_debug >= a)                                   \

+		printf(__VA_ARGS__);					\

+	} while (0)

+#else

+#define DEBUG_PRINTF(a, ...)

+#endif

+

 SYSCTL_DECL(_net_link);

 static SYSCTL_NODE(_net_link, IFT_STF, stf, CTLFLAG_RW, 0, "6to4 Interface");

-static int stf_route_cache = 1;

-SYSCTL_INT(_net_link_stf, OID_AUTO, route_cache, CTLFLAG_RW,

-    &stf_route_cache, 0, "Caching of IPv4 routes for 6to4 Output");

+static	VNET_DEFINE(int, stf_route_cache) = 0;

+#define	V_stf_route_cache     VNET(stf_route_cache)

+SYSCTL_VNET_INT(_net_link_stf, OID_AUTO, route_cache, CTLFLAG_RW,

+	&VNET_NAME(stf_route_cache), 0,

+	"Enable caching of IPv4 routes for 6to4 output.");

+

+#if STF_DEBUG

+static VNET_DEFINE(int, stf_debug) = 0;

+#define	V_stf_debug   VNET(stf_debug)

+SYSCTL_VNET_INT(_net_link_stf, OID_AUTO, stf_debug, CTLFLAG_RW,

+	&VNET_NAME(stf_debug), 0,

+	"Enable displaying verbose debug message of stf interfaces");

+#endif

 static int stf_permit_rfc1918 = 0;

 TUNABLE_INT("net.link.stf.permit_rfc1918", &stf_permit_rfc1918);

 SYSCTL_INT(_net_link_stf, OID_AUTO, permit_rfc1918, CTLFLAG_RW | CTLFLAG_TUN,

     &stf_permit_rfc1918, 0, "Permit the use of private IPv4 addresses");

-#define STFUNIT		0

-

 #define IN6_IS_ADDR_6TO4(x)	(ntohs((x)->s6_addr16[0]) == 0x2002)

 /*

@@ -144,24 +182,37 @@ SYSCTL_INT(_net_link_stf, OID_AUTO, permit_rfc1918, CTLFLAG_RW | CTLFLAG_TUN,

 struct stf_softc {

 	struct ifnet	*sc_ifp;

+	in_addr_t dstv4_addr;

+	in_addr_t srcv4_addr;

+	in_addr_t inaddr;

+	u_int   v4prefixlen;

 	union {

 		struct route  __sc_ro4;

 		struct route_in6 __sc_ro6; /* just for safety */

 	} __sc_ro46;

 #define sc_ro	__sc_ro46.__sc_ro4

-	struct mtx	sc_ro_mtx;

+	struct mtx	sc_mtx;

 	u_int	sc_fibnum;

 	const struct encaptab *encap_cookie;

+	u_int   sc_flags;

+	LIST_ENTRY(stf_softc) stf_list;

 };

 #define STF2IFP(sc)	((sc)->sc_ifp)

 static const char stfname[] = "stf";

-/*

- * Note that mutable fields in the softc are not currently locked.

- * We do lock sc_ro in stf_output though.

- */

+static struct mtx stf_mtx;

 static MALLOC_DEFINE(M_STF, stfname, "6to4 Tunnel Interface");

+static VNET_DEFINE(LIST_HEAD(, stf_softc), stf_softc_list);

+#define	V_stf_softc_list      VNET(stf_softc_list)

+

+#define	STF_LOCK_INIT(sc)     mtx_init(&(sc)->sc_mtx, "stf softc",    \

+	NULL, MTX_DEF);

+#define	STF_LOCK_DESTROY(sc)  mtx_destroy(&(sc)->sc_mtx)

+#define	STF_LOCK(sc)          mtx_lock(&(sc)->sc_mtx)

+#define	STF_UNLOCK(sc)                mtx_unlock(&(sc)->sc_mtx)

+#define	STF_LOCK_ASSERT(sc)   mtx_assert(&(sc)->sc_mtx, MA_OWNED)

+

 static const int ip_stf_ttl = 40;

 extern  struct domain inetdomain;

@@ -176,8 +227,6 @@ struct protosw in_stf_protosw = {

 	.pr_usrreqs =		&rip_usrreqs

 };

-static char *stfnames[] = {"stf0", "stf", "6to4", NULL};

-

 static int stfmodevent(module_t, int, void *);

 static int stf_encapcheck(const struct mbuf *, int, int, void *);

 static struct in6_ifaddr *stf_getsrcifa6(struct ifnet *);

@@ -191,66 +240,42 @@ static int stf_checkaddr6(struct stf_softc *, struct in6_addr *,

 static void stf_rtrequest(int, struct rtentry *, struct rt_addrinfo *);

 static int stf_ioctl(struct ifnet *, u_long, caddr_t);

-static int stf_clone_match(struct if_clone *, const char *);

-static int stf_clone_create(struct if_clone *, char *, size_t, caddr_t);

-static int stf_clone_destroy(struct if_clone *, struct ifnet *);

-static struct if_clone *stf_cloner;

+#define	STF_GETIN4_USE_CACHE  1

+static struct sockaddr_in *stf_getin4addr(struct stf_softc *, struct sockaddr_in *,

+	struct ifaddr *, int);

+static struct sockaddr_in *stf_getin4addr_in6(struct stf_softc *, struct sockaddr_in *,

+	struct ifaddr *, const struct in6_addr *);

+static struct sockaddr_in *stf_getin4addr_sin6(struct stf_softc *, struct sockaddr_in *,

+	struct ifaddr *, struct sockaddr_in6 *);

+static int stf_clone_create(struct if_clone *, int, caddr_t);

+static void stf_clone_destroy(struct ifnet *);

-static int

-stf_clone_match(struct if_clone *ifc, const char *name)

-{

-	int i;

-

-	for(i = 0; stfnames[i] != NULL; i++) {

-		if (strcmp(stfnames[i], name) == 0)

-			return (1);

-	}

-

-	return (0);

-}

+static struct if_clone *stf_cloner;

 static int

-stf_clone_create(struct if_clone *ifc, char *name, size_t len, caddr_t params)

+stf_clone_create(struct if_clone *ifc, int unit, caddr_t params)

 {

-	int err, unit;

 	struct stf_softc *sc;

 	struct ifnet *ifp;

-	/*

-	 * We can only have one unit, but since unit allocation is

-	 * already locked, we use it to keep from allocating extra

-	 * interfaces.

-	 */

-	unit = STFUNIT;

-	err = ifc_alloc_unit(ifc, &unit);

-	if (err != 0)

-		return (err);

-

 	sc = malloc(sizeof(struct stf_softc), M_STF, M_WAITOK | M_ZERO);

+	sc->sc_fibnum = curthread->td_proc->p_fibnum;

 	ifp = STF2IFP(sc) = if_alloc(IFT_STF);

-	if (ifp == NULL) {

+	if (sc->sc_ifp == NULL) {

 		free(sc, M_STF);

-		ifc_free_unit(ifc, unit);

-		return (ENOSPC);

+		return (ENOMEM);

 	}

+	STF_LOCK_INIT(sc);

 	ifp->if_softc = sc;

-	sc->sc_fibnum = curthread->td_proc->p_fibnum;

-	/*

-	 * Set the name manually rather then using if_initname because

-	 * we don't conform to the default naming convention for interfaces.

-	 */

-	strlcpy(ifp->if_xname, name, IFNAMSIZ);

-	ifp->if_dname = stfname;

-	ifp->if_dunit = IF_DUNIT_NONE;

+	if_initname(ifp, stfname, unit);

-	mtx_init(&(sc)->sc_ro_mtx, "stf ro", NULL, MTX_DEF);

 	sc->encap_cookie = encap_attach_func(AF_INET, IPPROTO_IPV6,

 	    stf_encapcheck, &in_stf_protosw, sc);

 	if (sc->encap_cookie == NULL) {

 		if_printf(ifp, "attach failed\n");

+		if_free(ifp);

 		free(sc, M_STF);

-		ifc_free_unit(ifc, unit);

 		return (ENOMEM);

 	}

@@ -260,42 +285,56 @@ stf_clone_create(struct if_clone *ifc, char *name, size_t len, caddr_t params)

 	ifp->if_snd.ifq_maxlen = ifqmaxlen;

 	if_attach(ifp);

 	bpfattach(ifp, DLT_NULL, sizeof(u_int32_t));

+

+	mtx_lock(&stf_mtx);

+	LIST_INSERT_HEAD(&V_stf_softc_list, sc, stf_list);

+	mtx_unlock(&stf_mtx);

+

 	return (0);

 }

-static int

-stf_clone_destroy(struct if_clone *ifc, struct ifnet *ifp)

+static void

+stf_clone_destroy(struct ifnet *ifp)

 {

 	struct stf_softc *sc = ifp->if_softc;

 	int err;

+	mtx_lock(&stf_mtx);

+	LIST_REMOVE(sc, stf_list);

+	mtx_unlock(&stf_mtx);

+

 	err = encap_detach(sc->encap_cookie);

 	KASSERT(err == 0, ("Unexpected error detaching encap_cookie"));

-	mtx_destroy(&(sc)->sc_ro_mtx);

 	bpfdetach(ifp);

 	if_detach(ifp);

 	if_free(ifp);

+	STF_LOCK_DESTROY(sc);

 	free(sc, M_STF);

-	ifc_free_unit(ifc, STFUNIT);

+}

-	return (0);

+static void

+vnet_stf_init(const void *unused __unused)

+{

+

+	LIST_INIT(&V_stf_softc_list);

 }

+VNET_SYSINIT(vnet_stf_init, SI_SUB_PSEUDO, SI_ORDER_MIDDLE, vnet_stf_init,

+	NULL);

 static int

-stfmodevent(mod, type, data)

-	module_t mod;

-	int type;

-	void *data;

+stfmodevent(module_t mod, int type, void *data)

 {

 	switch (type) {

 	case MOD_LOAD:

-		stf_cloner = if_clone_advanced(stfname, 0, stf_clone_match,

-		    stf_clone_create, stf_clone_destroy);

+		mtx_init(&stf_mtx, "stf_mtx", NULL, MTX_DEF);

+		stf_cloner = if_clone_simple(stfname,

+		    stf_clone_create, stf_clone_destroy, 0);

 		break;

 	case MOD_UNLOAD:

 		if_clone_detach(stf_cloner);

+		mtx_destroy(&stf_mtx);

 		break;

 	default:

 		return (EOPNOTSUPP);

@@ -311,28 +350,31 @@ static moduledata_t stf_mod = {

 };

 DECLARE_MODULE(if_stf, stf_mod, SI_SUB_PSEUDO, SI_ORDER_ANY);

+MODULE_VERSION(if_stf, 1);

 static int

-stf_encapcheck(m, off, proto, arg)

-	const struct mbuf *m;

-	int off;

-	int proto;

-	void *arg;

+stf_encapcheck(const struct mbuf *m, int off, int proto, void *arg)

 {

 	struct ip ip;

 	struct in6_ifaddr *ia6;

+	struct sockaddr_in ia6_in4addr;

+	struct sockaddr_in ia6_in4mask;

+	struct sockaddr_in *sin;

 	struct stf_softc *sc;

-	struct in_addr a, b, mask;

+	struct ifnet *ifp;

+	int ret = 0;

+	DEBUG_PRINTF(1, "%s: enter\n", __func__);

 	sc = (struct stf_softc *)arg;

 	if (sc == NULL)

 		return 0;

+	ifp = STF2IFP(sc);

-	if ((STF2IFP(sc)->if_flags & IFF_UP) == 0)

+	if ((ifp->if_flags & IFF_UP) == 0)

 		return 0;

 	/* IFF_LINK0 means "no decapsulation" */

-	if ((STF2IFP(sc)->if_flags & IFF_LINK0) != 0)

+	if ((ifp->if_flags & IFF_LINK0) != 0)

 		return 0;

 	if (proto != IPPROTO_IPV6)

@@ -344,72 +386,163 @@ stf_encapcheck(m, off, proto, arg)

 	if (ip.ip_v != 4)

 		return 0;

-	ia6 = stf_getsrcifa6(STF2IFP(sc));

+	/* Lookup an ia6 whose IPv4 addr encoded in the IPv6 addr is valid. */

+	ia6 = stf_getsrcifa6(ifp);

 	if (ia6 == NULL)

 		return 0;

+	if (sc->srcv4_addr != INADDR_ANY) {

+		sin = &ia6_in4addr;

+		sin->sin_addr.s_addr = sc->srcv4_addr;

+		sin->sin_family = AF_INET;

+	} else {

+		sin = stf_getin4addr(sc, &ia6_in4addr, &ia6->ia_ifa, STF_GETIN4_USE_CACHE);

+		if (sin == NULL)

+			return (0);

+	}

+

+#if STF_DEBUG > 3

+	{

+		char buf[INET6_ADDRSTRLEN + 1];

+		memset(&buf, 0, sizeof(buf));

+

+		ip6_sprintf(buf, &satosin6(ia6->ia_ifa.ifa_addr)->sin6_addr);

+		DEBUG_PRINTF(1, "%s: ia6->ia_ifa.ifa_addr = %s\n", __func__, buf);

+		ip6_sprintf(buf, &ia6->ia_addr.sin6_addr);

+		DEBUG_PRINTF(1, "%s: ia6->ia_addr = %s\n", __func__, buf);

+		ip6_sprintf(buf, &satosin6(ia6->ia_ifa.ifa_netmask)->sin6_addr);

+		DEBUG_PRINTF(1, "%s: ia6->ia_ifa.ifa_netmask = %s\n", __func__, buf);

+		ip6_sprintf(buf, &ia6->ia_prefixmask.sin6_addr);

+		DEBUG_PRINTF(1, "%s: ia6->ia_prefixmask = %s\n", __func__, buf);

+

+		ip_sprintf(buf, &ia6_in4addr.sin_addr);

+		DEBUG_PRINTF(1, "%s: ia6_in4addr.sin_addr = %s\n", __func__, buf);

+		ip_sprintf(buf, &ip.ip_src);

+		DEBUG_PRINTF(1, "%s: ip.ip_src = %s\n", __func__, buf);

+		ip_sprintf(buf, &ip.ip_dst);

+		DEBUG_PRINTF(1, "%s: ip.ip_dst = %s\n", __func__, buf);

+	}

+#endif

 	/*

 	 * check if IPv4 dst matches the IPv4 address derived from the

 	 * local 6to4 address.

 	 * success on: dst = 10.1.1.1, ia6->ia_addr = 2002:0a01:0101:...

 	 */

-	if (bcmp(GET_V4(&ia6->ia_addr.sin6_addr), &ip.ip_dst,

-	    sizeof(ip.ip_dst)) != 0) {

-		ifa_free(&ia6->ia_ifa);

-		return 0;

+	DEBUG_PRINTF(1, "%s: check1: ia6_in4addr.sin_addr == ip.ip_dst?\n", __func__);

+	if (ia6_in4addr.sin_addr.s_addr != ip.ip_dst.s_addr) {

+		DEBUG_PRINTF(1, "%s: check1: false.  Ignore this packet.\n", __func__);

+		goto freeit;

 	}

-	/*

-	 * check if IPv4 src matches the IPv4 address derived from the

-	 * local 6to4 address masked by prefixmask.

-	 * success on: src = 10.1.1.1, ia6->ia_addr = 2002:0a00:.../24

-	 * fail on: src = 10.1.1.1, ia6->ia_addr = 2002:0b00:.../24

-	 */

-	bzero(&a, sizeof(a));

-	bcopy(GET_V4(&ia6->ia_addr.sin6_addr), &a, sizeof(a));

-	bcopy(GET_V4(&ia6->ia_prefixmask.sin6_addr), &mask, sizeof(mask));

-	ifa_free(&ia6->ia_ifa);

-	a.s_addr &= mask.s_addr;

-	b = ip.ip_src;

-	b.s_addr &= mask.s_addr;

-	if (a.s_addr != b.s_addr)

-		return 0;

+	DEBUG_PRINTF(1, "%s: check2: ia6->ia_addr is 2002::/16?\n", __func__);

+

+	if (IN6_IS_ADDR_6TO4(&ia6->ia_addr.sin6_addr)) {

+		/* 6to4 (RFC 3056) */

+		/*

+		 * check if IPv4 src matches the IPv4 address derived

+		 * from the local 6to4 address masked by prefixmask.

+		 * success on: src = 10.1.1.1, ia6->ia_addr = 2002:0a00:.../24

+		 * fail on: src = 10.1.1.1, ia6->ia_addr = 2002:0b00:.../24

+		 */

+		DEBUG_PRINTF(1, "%s: check2: true.\n", __func__);

+

+		memcpy(&ia6_in4mask.sin_addr,

+		GET_V4(&ia6->ia_prefixmask.sin6_addr),

+		sizeof(ia6_in4mask));

+#if STF_DEBUG > 3

+		{

+			char buf[INET6_ADDRSTRLEN + 1];

+			memset(&buf, 0, sizeof(buf));

+

+			ip_sprintf(buf, &ia6_in4addr.sin_addr);

+			DEBUG_PRINTF(1, "%s: ia6->ia_addr = %s\n",

+				__func__, buf);

+			ip_sprintf(buf, &ip.ip_src);

+			DEBUG_PRINTF(1, "%s: ip.ip_src = %s\n",

+				__func__, buf);

+			ip_sprintf(buf, &ia6_in4mask.sin_addr);

+			DEBUG_PRINTF(1, "%s: ia6->ia_prefixmask = %s\n",

+				__func__, buf);

+

+			DEBUG_PRINTF(1, "%s: check3: ia6_in4addr.sin_addr & mask == ip.ip_src & mask\n",

+				__func__);

+		}

+#endif

+		  

+		if ((ia6_in4addr.sin_addr.s_addr & ia6_in4mask.sin_addr.s_addr) !=

+		    (ip.ip_src.s_addr & ia6_in4mask.sin_addr.s_addr)) {

+			DEBUG_PRINTF(1, "%s: check3: false.  Ignore this packet.\n",

+				__func__);

+			goto freeit;

+		}

+	} else {

+		/* 6rd (RFC 5569) */

+		DEBUG_PRINTF(1, "%s: check2: false.  6rd.\n", __func__);

+		/*

+		 * No restriction on the src address in the case of

+		 * 6rd because the stf(4) interface always has a

+		 * prefix which covers whole of IPv4 src address

+		 * range.  So, stf_output() will catch all of

+		 * 6rd-capsuled IPv4 traffic with suspicious inner dst

+		 * IPv4 address (i.e. the IPv6 destination address is

+		 * one the admin does not like to route to outside),

+		 * and then it discard them silently.

+		 */

+	}

+	DEBUG_PRINTF(1, "%s: all clear!\n", __func__);

 	/* stf interface makes single side match only */

-	return 32;

+	ret = 32;

+freeit:

+	ifa_free(&ia6->ia_ifa);

+

+	return (ret);

 }

 static struct in6_ifaddr *

-stf_getsrcifa6(ifp)

-	struct ifnet *ifp;

+stf_getsrcifa6(struct ifnet *ifp)

 {

-	struct ifaddr *ia;

+	struct ifaddr *ifa;

 	struct in_ifaddr *ia4;

-	struct sockaddr_in6 *sin6;

-	struct in_addr in;

+	struct sockaddr_in *sin;

+	struct sockaddr_in in4;

 	if_addr_rlock(ifp);

-	TAILQ_FOREACH(ia, &ifp->if_addrhead, ifa_link) {

-		if (ia->ifa_addr->sa_family != AF_INET6)

+	TAILQ_FOREACH(ifa, &ifp->if_addrhead, ifa_link) {

+		if (ifa->ifa_addr->sa_family != AF_INET6)

 			continue;

-		sin6 = (struct sockaddr_in6 *)ia->ifa_addr;

-		if (!IN6_IS_ADDR_6TO4(&sin6->sin6_addr))

+

+		if ((sin = stf_getin4addr(ifp->if_softc, &in4, ifa,

+		    STF_GETIN4_USE_CACHE)) == NULL)

 			continue;

-		bcopy(GET_V4(&sin6->sin6_addr), &in, sizeof(in));

-		LIST_FOREACH(ia4, INADDR_HASH(in.s_addr), ia_hash)

-			if (ia4->ia_addr.sin_addr.s_addr == in.s_addr)

+		LIST_FOREACH(ia4, INADDR_HASH(sin->sin_addr.s_addr), ia_hash)

+			if (ia4->ia_addr.sin_addr.s_addr == sin->sin_addr.s_addr)

 				break;

 		if (ia4 == NULL)

 			continue;

-		ifa_ref(ia);

+#if STF_DEBUG > 3

+	{

+		char buf[INET6_ADDRSTRLEN + 1];

+		memset(&buf, 0, sizeof(buf));

+

+		ip6_sprintf(buf, &((struct sockaddr_in6 *)ifa->ifa_addr)->sin6_addr);

+		DEBUG_PRINTF(1, "%s: ifa->ifa_addr->sin6_addr = %s\n",

+			__func__, buf);

+		ip_sprintf(buf, &ia4->ia_addr.sin_addr);

+		DEBUG_PRINTF(1, "%s: ia4->ia_addr.sin_addr = %s\n",

+			__func__, buf);

+	}

+#endif

+

+		ifa_ref(ifa);

 		if_addr_runlock(ifp);

-		return (struct in6_ifaddr *)ia;

+		return (ifatoia6(ifa));

 	}

 	if_addr_runlock(ifp);

-	return NULL;

+	return (NULL);

 }

 static int

@@ -419,8 +552,8 @@ stf_output(struct ifnet *ifp, struct mbuf *m, const struct sockaddr *dst,

 	struct stf_softc *sc;

 	const struct sockaddr_in6 *dst6;

 	struct route *cached_route;

-	struct in_addr in4;

-	const void *ptr;

+	struct sockaddr_in *sin;

+	struct sockaddr_in in4;

 	struct sockaddr_in *dst4;

 	u_int8_t tos;

 	struct ip *ip;

@@ -472,20 +605,33 @@ stf_output(struct ifnet *ifp, struct mbuf *m, const struct sockaddr *dst,

 	/*

 	 * Pickup the right outer dst addr from the list of candidates.

 	 * ip6_dst has priority as it may be able to give us shorter IPv4 hops.

+	 *   ip6_dst: destination addr in the packet header.

+	 *   dst6: destination addr specified in function argument.

 	 */

-	ptr = NULL;

-	if (IN6_IS_ADDR_6TO4(&ip6->ip6_dst))

-		ptr = GET_V4(&ip6->ip6_dst);

-	else if (IN6_IS_ADDR_6TO4(&dst6->sin6_addr))

-		ptr = GET_V4(&dst6->sin6_addr);

-	else {

-		ifa_free(&ia6->ia_ifa);

-		m_freem(m);

-		ifp->if_oerrors++;

-		return ENETUNREACH;

+	DEBUG_PRINTF(1, "%s: dst addr selection\n", __func__);

+	sin = stf_getin4addr_in6(sc, &in4, &ia6->ia_ifa, &ip6->ip6_dst);

+	if (sin == NULL) {

+		if (sc->dstv4_addr != INADDR_ANY)

+			in4.sin_addr.s_addr = sc->dstv4_addr;

+		else {

+			sin = stf_getin4addr_in6(sc, &in4, &ia6->ia_ifa, &dst6->sin6_addr);

+			if (sin == NULL) {

+				ifa_free(&ia6->ia_ifa);

+				m_freem(m);

+				ifp->if_oerrors++;

+				return ENETUNREACH;

+			}

+		}

 	}

-	bcopy(ptr, &in4, sizeof(in4));

+#if STF_DEBUG > 3

+	{

+		char buf[INET6_ADDRSTRLEN + 1];

+		memset(&buf, 0, sizeof(buf));

+		ip_sprintf(buf, &in4.sin_addr);

+		DEBUG_PRINTF(1, "%s: ip_dst = %s\n", __func__, buf);

+	}

+#endif

 	if (bpf_peers_present(ifp->if_bpf)) {

 		/*

 		 * We need to prepend the address family as

@@ -509,11 +655,30 @@ stf_output(struct ifnet *ifp, struct mbuf *m, const struct sockaddr *dst,

 	ip = mtod(m, struct ip *);

 	bzero(ip, sizeof(*ip));

+	bcopy(&in4.sin_addr, &ip->ip_dst, sizeof(ip->ip_dst));

-	bcopy(GET_V4(&((struct sockaddr_in6 *)&ia6->ia_addr)->sin6_addr),

-	    &ip->ip_src, sizeof(ip->ip_src));

+	if (sc->srcv4_addr != INADDR_ANY)

+		in4.sin_addr.s_addr = sc->srcv4_addr;

+	else {

+		sin = stf_getin4addr_sin6(sc, &in4, &ia6->ia_ifa, &ia6->ia_addr);

+		if (sin == NULL) {

+			ifa_free(&ia6->ia_ifa);

+			m_freem(m);

+			ifp->if_oerrors++;

+			return ENETUNREACH;

+		}

+	}

+	bcopy(&in4.sin_addr, &ip->ip_src, sizeof(ip->ip_src));

+#if STF_DEBUG > 3

+	{

+		char buf[INET6_ADDRSTRLEN + 1];

+		memset(&buf, 0, sizeof(buf));

+

+		ip_sprintf(buf, &ip->ip_src);

+		DEBUG_PRINTF(1, "%s: ip_src = %s\n", __func__, buf);

+	}

+#endif

 	ifa_free(&ia6->ia_ifa);

-	bcopy(&in4, &ip->ip_dst, sizeof(ip->ip_dst));

 	ip->ip_p = IPPROTO_IPV6;

 	ip->ip_ttl = ip_stf_ttl;

 	ip->ip_len = htons(m->m_pkthdr.len);

@@ -522,7 +687,7 @@ stf_output(struct ifnet *ifp, struct mbuf *m, const struct sockaddr *dst,

 	else

 		ip_ecn_ingress(ECN_NOCARE, &ip->ip_tos, &tos);

-	if (!stf_route_cache) {

+	if (!V_stf_route_cache) {

 		cached_route = NULL;

 		goto sendit;

 	}

@@ -530,7 +695,7 @@ stf_output(struct ifnet *ifp, struct mbuf *m, const struct sockaddr *dst,

 	/*

 	 * Do we have a cached route?

 	 */

-	mtx_lock(&(sc)->sc_ro_mtx);

+	STF_LOCK(sc);

 	dst4 = (struct sockaddr_in *)&sc->sc_ro.ro_dst;

 	if (dst4->sin_family != AF_INET ||

 	    bcmp(&dst4->sin_addr, &ip->ip_dst, sizeof(ip->ip_dst)) != 0) {

@@ -548,8 +713,15 @@ stf_output(struct ifnet *ifp, struct mbuf *m, const struct sockaddr *dst,

 		rtalloc_fib(&sc->sc_ro, sc->sc_fibnum);

 		if (sc->sc_ro.ro_rt == NULL) {

 			m_freem(m);

-			mtx_unlock(&(sc)->sc_ro_mtx);

 			ifp->if_oerrors++;

+			STF_UNLOCK(sc);

+			return ENETUNREACH;

+		}

+		if (sc->sc_ro.ro_rt->rt_ifp == ifp) {

+			/* infinite loop detection */

+			m_free(m);

+			ifp->if_oerrors++;

+			STF_UNLOCK(sc);

 			return ENETUNREACH;

 		}

 	}

@@ -558,35 +730,33 @@ stf_output(struct ifnet *ifp, struct mbuf *m, const struct sockaddr *dst,

 sendit:

 	M_SETFIB(m, sc->sc_fibnum);

 	ifp->if_opackets++;

+	DEBUG_PRINTF(1, "%s: ip_output dispatch.\n", __func__);

 	error = ip_output(m, NULL, cached_route, 0, NULL, NULL);

 	if (cached_route != NULL)

-		mtx_unlock(&(sc)->sc_ro_mtx);

-	return error;

+		STF_UNLOCK(sc);

+

+	return (error);

 }

 static int

-isrfc1918addr(in)

-	struct in_addr *in;

+isrfc1918addr(struct in_addr *in)

 {

 	/*

 	 * returns 1 if private address range:

 	 * 10.0.0.0/8 172.16.0.0/12 192.168.0.0/16

 	 */

 	if (stf_permit_rfc1918 == 0 && (

-	    (ntohl(in->s_addr) & 0xff000000) >> 24 == 10 ||

-	    (ntohl(in->s_addr) & 0xfff00000) >> 16 == 172 * 256 + 16 ||

-	    (ntohl(in->s_addr) & 0xffff0000) >> 16 == 192 * 256 + 168))

+	    (ntohl(in->s_addr) & 0xff000000) == 10 << 24 ||

+	    (ntohl(in->s_addr) & 0xfff00000) == (172 * 256 + 16) << 16 ||

+	    (ntohl(in->s_addr) & 0xffff0000) == (192 * 256 + 168) << 16 ))

 		return 1;

 	return 0;

 }

 static int

-stf_checkaddr4(sc, in, inifp)

-	struct stf_softc *sc;

-	struct in_addr *in;

-	struct ifnet *inifp;	/* incoming interface */

+stf_checkaddr4(struct stf_softc *sc, struct in_addr *in, struct ifnet *inifp)

 {

 	struct in_ifaddr *ia4;

@@ -602,13 +772,6 @@ stf_checkaddr4(sc, in, inifp)

 	}

 	/*

-	 * reject packets with private address range.

-	 * (requirement from RFC3056 section 2 1st paragraph)

-	 */

-	if (isrfc1918addr(in))

-		return -1;

-

-	/*

 	 * reject packets with broadcast

 	 */

 	IN_IFADDR_RLOCK();

@@ -631,7 +794,7 @@ stf_checkaddr4(sc, in, inifp)

 		bzero(&sin, sizeof(sin));

 		sin.sin_family = AF_INET;

-		sin.sin_len = sizeof(struct sockaddr_in);

+		sin.sin_len = sizeof(sin);

 		sin.sin_addr = *in;

 		rt = rtalloc1_fib((struct sockaddr *)&sin, 0,

 		    0UL, sc->sc_fibnum);

@@ -652,10 +815,7 @@ stf_checkaddr4(sc, in, inifp)

 }

 static int

-stf_checkaddr6(sc, in6, inifp)

-	struct stf_softc *sc;

-	struct in6_addr *in6;

-	struct ifnet *inifp;	/* incoming interface */

+stf_checkaddr6(struct stf_softc *sc, struct in6_addr *in6, struct ifnet *inifp)

 {

 	/*

 	 * check 6to4 addresses

@@ -679,9 +839,7 @@ stf_checkaddr6(sc, in6, inifp)

 }

 void

-in_stf_input(m, off)

-	struct mbuf *m;

-	int off;

+in_stf_input(struct mbuf *m, int off)

 {

 	int proto;

 	struct stf_softc *sc;

@@ -689,6 +847,7 @@ in_stf_input(m, off)

 	struct ip6_hdr *ip6;

 	u_int8_t otos, itos;

 	struct ifnet *ifp;

+	struct route_in6 rin6;

 	proto = mtod(m, struct ip *)->ip_p;

@@ -712,6 +871,17 @@ in_stf_input(m, off)

 	mac_ifnet_create_mbuf(ifp, m);

 #endif

+#if STF_DEBUG > 3

+	{

+		char buf[INET6_ADDRSTRLEN + 1];

+		memset(&buf, 0, sizeof(buf));

+

+		ip_sprintf(buf, &ip->ip_dst);

+		DEBUG_PRINTF(1, "%s: ip->ip_dst = %s\n", __func__, buf);

+		ip_sprintf(buf, &ip->ip_src);

+		DEBUG_PRINTF(1, "%s: ip->ip_src = %s\n", __func__, buf);

+	}

+#endif

 	/*

 	 * perform sanity check against outer src/dst.

 	 * for source, perform ingress filter as well.

@@ -732,6 +902,17 @@ in_stf_input(m, off)

 	}

 	ip6 = mtod(m, struct ip6_hdr *);

+#if STF_DEBUG > 3

+	{

+		char buf[INET6_ADDRSTRLEN + 1];

+		memset(&buf, 0, sizeof(buf));

+

+		ip6_sprintf(buf, &ip6->ip6_dst);

+		DEBUG_PRINTF(1, "%s: ip6->ip6_dst = %s\n", __func__, buf);

+		ip6_sprintf(buf, &ip6->ip6_src);

+		DEBUG_PRINTF(1, "%s: ip6->ip6_src = %s\n", __func__, buf);

+	}

+#endif

 	/*

 	 * perform sanity check against inner src/dst.

 	 * for source, perform ingress filter as well.

@@ -742,6 +923,41 @@ in_stf_input(m, off)

 		return;

 	}

+	/*

+	 * reject packets with private address range.

+	 * (requirement from RFC3056 section 2 1st paragraph)

+	 */

+	if ((IN6_IS_ADDR_6TO4(&ip6->ip6_src) && isrfc1918addr(&ip->ip_src)) ||

+	    (IN6_IS_ADDR_6TO4(&ip6->ip6_dst) && isrfc1918addr(&ip->ip_dst))) {

+		m_freem(m);

+		return;

+	}

+

+	/*

+	 * Ignore if the destination is the same stf interface because

+	 * all of valid IPv6 outgoing traffic should go interfaces

+	 * except for it.

+	 */

+	memset(&rin6, 0, sizeof(rin6));

+	rin6.ro_dst.sin6_len = sizeof(rin6.ro_dst);

+	rin6.ro_dst.sin6_family = AF_INET6;

+	memcpy(&rin6.ro_dst.sin6_addr, &ip6->ip6_dst,

+	       sizeof(rin6.ro_dst.sin6_addr));

+	rtalloc((struct route *)&rin6);

+	if (rin6.ro_rt == NULL) {

+		DEBUG_PRINTF(1, "%s: no IPv6 dst.  Ignored.\n", __func__);

+		m_free(m);

+		return;

+	}

+	if ((rin6.ro_rt->rt_ifp == ifp) &&

+	    (!IN6_ARE_ADDR_EQUAL(&ip6->ip6_src, &rin6.ro_dst.sin6_addr))) {

+		DEBUG_PRINTF(1, "%s: IPv6 dst is the same stf.  Ignored.\n", __func__);

+		RTFREE(rin6.ro_rt);

+		m_free(m);

+		return;

+	}

+	RTFREE(rin6.ro_rt);

+

 	itos = (ntohl(ip6->ip6_flow) >> 20) & 0xff;

 	if ((ifp->if_flags & IFF_LINK1) != 0)

 		ip_ecn_egress(ECN_ALLOWED, &otos, &itos);

@@ -751,7 +967,7 @@ in_stf_input(m, off)

 	ip6->ip6_flow |= htonl((u_int32_t)itos << 20);

 	m->m_pkthdr.rcvif = ifp;

-	

+

 	if (bpf_peers_present(ifp->if_bpf)) {

 		/*

 		 * We need to prepend the address family as

@@ -764,6 +980,7 @@ in_stf_input(m, off)

 		bpf_mtap2(ifp->if_bpf, &af, sizeof(af), m);

 	}

+	DEBUG_PRINTF(1, "%s: netisr_dispatch(NETISR_IPV6)\n", __func__);

 	/*

 	 * Put the packet to the network layer input queue according to the

 	 * specified address family.

@@ -778,46 +995,359 @@ in_stf_input(m, off)

 /* ARGSUSED */

 static void

-stf_rtrequest(cmd, rt, info)

-	int cmd;

-	struct rtentry *rt;

-	struct rt_addrinfo *info;

+stf_rtrequest(int cmd, struct rtentry *rt, struct rt_addrinfo *info)

 {

+

 	RT_LOCK_ASSERT(rt);

 	rt->rt_mtu = rt->rt_ifp->if_mtu;

 }

+static struct sockaddr_in *

+stf_getin4addr_in6(struct stf_softc *sc, struct sockaddr_in *sin,

+      struct ifaddr *ifa,

+      const struct in6_addr *in6)

+{

+      struct sockaddr_in6 sin6;

+

+      DEBUG_PRINTF(1, "%s: enter.\n", __func__);

+      if (ifa == NULL || in6 == NULL)

+              return NULL;

+

+      memset(&sin6, 0, sizeof(sin6));

+      memcpy(&sin6.sin6_addr, in6, sizeof(sin6.sin6_addr));

+      sin6.sin6_len = sizeof(sin6);

+      sin6.sin6_family = AF_INET6;

+

+      return(stf_getin4addr_sin6(sc, sin, ifa, &sin6));

+}

+

+static struct sockaddr_in *

+stf_getin4addr_sin6(struct stf_softc *sc, struct sockaddr_in *sin,

+      struct ifaddr *ifa,

+      struct sockaddr_in6 *sin6)

+{

+      struct in6_ifaddr ia6;

+      int i;

+

+      DEBUG_PRINTF(1, "%s: enter.\n", __func__);

+      if (ifa == NULL || sin6 == NULL)

+      return NULL;

+

+      memset(&ia6, 0, sizeof(ia6));

+      memcpy(&ia6, ifatoia6(ifa), sizeof(ia6));

+

+      /*

+      * Use prefixmask information from ifa, and

+      * address information from sin6.

+      */

+      ia6.ia_addr.sin6_family = AF_INET6;

+      ia6.ia_ifa.ifa_addr = (struct sockaddr *)&ia6.ia_addr;

+      ia6.ia_ifa.ifa_dstaddr = NULL;

+      ia6.ia_ifa.ifa_netmask = (struct sockaddr *)&ia6.ia_prefixmask;

+

+#if STF_DEBUG > 3

+      {

+              char buf[INET6_ADDRSTRLEN + 1];

+              memset(&buf, 0, sizeof(buf));

+

+              ip6_sprintf(buf, &sin6->sin6_addr);

+              DEBUG_PRINTF(1, "%s: sin6->sin6_addr = %s\n", __func__, buf);

+              ip6_sprintf(buf, &ia6.ia_addr.sin6_addr);

+              DEBUG_PRINTF(1, "%s: ia6.ia_addr.sin6_addr = %s\n", __func__, buf);

+              ip6_sprintf(buf, &ia6.ia_prefixmask.sin6_addr);

+              DEBUG_PRINTF(1, "%s: ia6.ia_prefixmask.sin6_addr = %s\n", __func__, buf);

+      }

+#endif

+

+      /*

+      * When (src addr & src mask) != (dst (sin6) addr & src mask),

+      * the dst is not in the 6rd domain.  The IPv4 address must

+      * not be used.

+      */

+      for (i = 0; i < sizeof(ia6.ia_addr.sin6_addr); i++) {

+              if ((((u_char *)&ia6.ia_addr.sin6_addr)[i] &

+                  ((u_char *)&ia6.ia_prefixmask.sin6_addr)[i])

+                  !=

+                  (((u_char *)&sin6->sin6_addr)[i] &

+                  ((u_char *)&ia6.ia_prefixmask.sin6_addr)[i]))

+              return NULL;

+      }

+

+      /* After the mask check, overwrite ia6.ia_addr with sin6. */

+      memcpy(&ia6.ia_addr, sin6, sizeof(ia6.ia_addr));

+      return(stf_getin4addr(sc, sin, (struct ifaddr *)&ia6, 0));

+}

+

+static struct sockaddr_in *

+stf_getin4addr(struct stf_softc *sc, struct sockaddr_in *sin,

+      struct ifaddr *ifa,

+      int flags)

+{

+      struct in_addr *in;

+      struct sockaddr_in6 *sin6;

+      struct in6_ifaddr *ia6;

+

+      DEBUG_PRINTF(1, "%s: enter.\n", __func__);

+      if (ifa == NULL ||

+          ifa->ifa_addr == NULL ||

+          ifa->ifa_addr->sa_family != AF_INET6)

+      return NULL;

+

+      sin6 = satosin6(ifa->ifa_addr);

+      ia6 = ifatoia6(ifa);

+

+      if ((flags & STF_GETIN4_USE_CACHE) &&

+          (ifa->ifa_dstaddr != NULL) &&

+          (ifa->ifa_dstaddr->sa_family == AF_INET)) {

+              /*

+               * XXX: ifa_dstaddr is used as a cache of the

+               * extracted IPv4 address.

+               */

+              memcpy(sin, satosin(ifa->ifa_dstaddr), sizeof(*sin));

+

+#if STF_DEBUG > 3

+              {

+                      char tmpbuf[INET6_ADDRSTRLEN + 1];

+                      memset(&tmpbuf, 0, INET6_ADDRSTRLEN);

+

+                      ip_sprintf(tmpbuf, &sin->sin_addr);

+                      DEBUG_PRINTF(1, "%s: cached address was used = %s\n", __func__, tmpbuf);

+              }

+#endif

+              return (sin);

+      }

+      memset(sin, 0, sizeof(*sin));

+      in = &sin->sin_addr;

+

+#if STF_DEBUG > 3

+      {

+              char tmpbuf[INET6_ADDRSTRLEN + 1];

+              memset(&tmpbuf, 0, INET6_ADDRSTRLEN);

+

+              ip6_sprintf(tmpbuf, &sin6->sin6_addr);

+              DEBUG_PRINTF(1, "%s: sin6->sin6_addr = %s\n", __func__, tmpbuf);

+      }

+#endif

+

+      if (IN6_IS_ADDR_6TO4(&sin6->sin6_addr)) {

+              /* 6to4 (RFC 3056) */

+              bcopy(GET_V4(&sin6->sin6_addr), in, sizeof(*in));

+              if (isrfc1918addr(in))

+                      return NULL;

+      } else {

+              /* 6rd (RFC 5569) */

+              struct in6_addr buf;

+              u_char *p = (u_char *)&buf;

+              u_char *q = (u_char *)&in->s_addr;

+              u_int residue = 0, v4residue = 0;

+              u_char mask, v4mask = 0;

+              int i;

+              u_int plen, loop;

+

+              /*

+               * 6rd-relays IPv6 prefix is located at a 32-bit just

+               * after the prefix edge.

+               */

+              plen = in6_mask2len(&satosin6(ifa->ifa_netmask)->sin6_addr, NULL);

+              if (64 < plen) {

+                      DEBUG_PRINTF(1, "prefixlen is %d\n", plen);

+                      return NULL;

+              }

+

+	      loop = 4; /* Normal 6rd operation */

+              memcpy(&buf, &sin6->sin6_addr, sizeof(buf));

+              if (sc->v4prefixlen != 0 && sc->v4prefixlen != 32) {

+                      v4residue = sc->v4prefixlen % 8;

+              }

+	      plen -= 32;

+

+              p += plen / 8;

+              residue = plen % 8;

+              mask = ((u_char)(-1) >> (8 - residue));

+              if (v4residue) {

+                      loop++;

+                      v4mask = ((u_char)(-1) << v4residue);

+              }

+              /*

+               * The p points head of the IPv4 address part in

+               * bytes.  The residue is a bit-shift factor when

+               * prefixlen is not a multiple of 8.

+               */

+              DEBUG_PRINTF(2, "residue = %d 0x%x\n", residue, mask);

+              for (i = (loop - (sc->v4prefixlen / 8)); i < loop; i++) {

+                      if (residue) {

+                              q[i] = ((p[i] & mask) << (8 - residue));

+				q[i] |=  ((p[i + 1] >> residue) & mask); 

+			      DEBUG_PRINTF(2, "FINAL  i = %d  q[%d] - p[%d/%d] %x\n",

+				      i, q[i], p[i], p[i + 1] >> residue, q[i]);

+                      } else {

+			      q[i] = p[i];

+			      DEBUG_PRINTF(2, "FINAL q[%d] - p[%d] %x\n",

+				      q[i], p[i], q[i]);

+			}

+              }

+              if (v4residue) {

+		      q[loop - (sc->v4prefixlen / 8)] &= v4mask;

+

+		      if (sc->v4prefixlen > 0 && sc->v4prefixlen < 32)

+			      in->s_addr |= sc->inaddr;

+		}

+

+		if (in->s_addr != sc->srcv4_addr)

+			printf("Wrong decoded address %x/%x!!!!\n", in->s_addr, sc->srcv4_addr);

+      }

+

+#if STF_DEBUG > 3

+      {

+              char tmpbuf[INET6_ADDRSTRLEN + 1];

+              memset(&tmpbuf, 0, INET_ADDRSTRLEN);

+

+              ip_sprintf(tmpbuf, in);

+              DEBUG_PRINTF(1, "%s: in->in_addr = %s\n", __func__, tmpbuf);

+              DEBUG_PRINTF(1, "%s: leave\n", __func__);

+      }

+#endif

+

+      if (flags & STF_GETIN4_USE_CACHE) {

+              DEBUG_PRINTF(1, "%s: try to access ifa->ifa_dstaddr.\n", __func__);

+              ifa->ifa_dstaddr = (struct sockaddr *)&ia6->ia_dstaddr;

+              DEBUG_PRINTF(1, "%s: try to memset 0 to ia_dstaddr.\n", __func__);

+                      memset(&ia6->ia_dstaddr, 0, sizeof(ia6->ia_dstaddr));

+              DEBUG_PRINTF(1, "%s: try to memcpy ifa->ifa_dstaddr.\n", __func__);

+              memcpy((struct sockaddr_in *)ifa->ifa_dstaddr,

+                      sin, sizeof(struct sockaddr_in));

+              DEBUG_PRINTF(1, "%s: try to set sa_family.\n", __func__);

+              ifa->ifa_dstaddr->sa_family = AF_INET;

+              DEBUG_PRINTF(1, "%s: in->in_addr is stored in ifa_dstaddr.\n",

+                      __func__);

+      }

+

+      return (sin);

+}

+

+

 static int

-stf_ioctl(ifp, cmd, data)

-	struct ifnet *ifp;

-	u_long cmd;

-	caddr_t data;

+stf_ioctl(struct ifnet *ifp, u_long cmd, caddr_t data)

 {

+	struct stf_softc *sc, *sc_cur;

 	struct ifaddr *ifa;

 	struct ifreq *ifr;

-	struct sockaddr_in6 *sin6;

-	struct in_addr addr;

+	struct sockaddr_in in4;

+	struct stfv4args args;

+	struct in6_ifaddr *ia6;

+	struct ifdrv *ifd;

 	int error, mtu;

 	error = 0;

+	sc_cur = ifp->if_softc;

+

 	switch (cmd) {

+	case SIOCSDRVSPEC:

+		ifd = (struct ifdrv *) data;

+		error = priv_check(curthread, PRIV_NET_ADDIFADDR);

+		if (error)

+			break;

+		if (ifd->ifd_cmd == STF_SV4NET) {

+			if (ifd->ifd_len != sizeof(args)) {

+				error = EINVAL;

+				break;

+			}

+			mtx_lock(&stf_mtx);

+			LIST_FOREACH(sc, &V_stf_softc_list, stf_list) {

+				if (sc == sc_cur)

+					continue;

+				if (sc->inaddr == 0 || sc->v4prefixlen == 0)

+					continue;

+

+				if ((ntohl(sc->inaddr) & ((uint32_t)(-1) << sc_cur->v4prefixlen)) == ntohl(sc_cur->inaddr)) {

+					error = EEXIST;

+					mtx_unlock(&stf_mtx);

+					return (error);

+				}

+				if ((ntohl(sc_cur->inaddr) & ((uint32_t)(-1) << sc->v4prefixlen)) == ntohl(sc->inaddr)) {

+					error = EEXIST;

+					mtx_unlock(&stf_mtx);

+					return (error);

+				}

+			}

+			mtx_unlock(&stf_mtx);

+			bzero(&args, sizeof args);

+			error = copyin(ifd->ifd_data, &args, ifd->ifd_len); 

+			if (error)

+				break;

+

+			sc_cur->srcv4_addr = args.inaddr.s_addr;

+			sc_cur->inaddr = ntohl(args.inaddr.s_addr);

+			sc_cur->inaddr &= ((uint32_t)(-1) << args.prefix);

+			sc_cur->inaddr = htonl(sc_cur->inaddr);

+			sc_cur->v4prefixlen = args.prefix;

+			if (sc_cur->v4prefixlen == 0)

+				sc_cur->v4prefixlen = 32;

+		} else if (ifd->ifd_cmd == STF_SDSTV4) {

+			if (ifd->ifd_len != sizeof(args)) {

+				error = EINVAL;

+				break;

+			}

+			bzero(&args, sizeof args);

+			error = copyin(ifd->ifd_data, &args, ifd->ifd_len); 

+			if (error)

+				break;

+			sc_cur->dstv4_addr = args.dstv4_addr.s_addr;

+		} else

+			error = EINVAL;