xref: /freebsd/sys/netpfil/ipfw/nat64/nat64_translate.c (revision 642870485c089b57000fe538d3485e272b038d59)
1 /*-
2  * Copyright (c) 2015-2016 Yandex LLC
3  * Copyright (c) 2015-2016 Andrey V. Elsukov <ae@FreeBSD.org>
4  * All rights reserved.
5  *
6  * Redistribution and use in source and binary forms, with or without
7  * modification, are permitted provided that the following conditions
8  * are met:
9  *
10  * 1. Redistributions of source code must retain the above copyright
11  *    notice, this list of conditions and the following disclaimer.
12  * 2. Redistributions in binary form must reproduce the above copyright
13  *    notice, this list of conditions and the following disclaimer in the
14  *    documentation and/or other materials provided with the distribution.
15  *
16  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
17  * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
18  * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
19  * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
20  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
21  * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
22  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
23  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
25  * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26  */
27 
28 #include "opt_ipfw.h"
29 
30 #include <sys/cdefs.h>
31 __FBSDID("$FreeBSD$");
32 
33 #include <sys/param.h>
34 #include <sys/systm.h>
35 #include <sys/counter.h>
36 #include <sys/errno.h>
37 #include <sys/kernel.h>
38 #include <sys/lock.h>
39 #include <sys/mbuf.h>
40 #include <sys/module.h>
41 #include <sys/rmlock.h>
42 #include <sys/rwlock.h>
43 #include <sys/socket.h>
44 #include <sys/queue.h>
45 
46 #include <net/if.h>
47 #include <net/if_var.h>
48 #include <net/if_pflog.h>
49 #include <net/pfil.h>
50 #include <net/netisr.h>
51 #include <net/route.h>
52 
53 #include <netinet/in.h>
54 #include <netinet/ip.h>
55 #include <netinet/ip_var.h>
56 #include <netinet/ip_fw.h>
57 #include <netinet/ip6.h>
58 #include <netinet/icmp6.h>
59 #include <netinet/ip_icmp.h>
60 #include <netinet/tcp.h>
61 #include <netinet/udp.h>
62 #include <netinet6/in6_var.h>
63 #include <netinet6/ip6_var.h>
64 
65 #include <netpfil/pf/pf.h>
66 #include <netpfil/ipfw/ip_fw_private.h>
67 #include <netpfil/ipfw/nat64/ip_fw_nat64.h>
68 #include <netpfil/ipfw/nat64/nat64_translate.h>
69 #include <machine/in_cksum.h>
70 
71 static void
72 nat64_log(struct pfloghdr *logdata, struct mbuf *m, sa_family_t family)
73 {
74 
75 	logdata->dir = PF_OUT;
76 	logdata->af = family;
77 	ipfw_bpf_mtap2(logdata, PFLOG_HDRLEN, m);
78 }
79 #ifdef IPFIREWALL_NAT64_DIRECT_OUTPUT
80 static NAT64NOINLINE struct sockaddr* nat64_find_route4(struct route *ro,
81     in_addr_t dest, struct mbuf *m);
82 static NAT64NOINLINE struct sockaddr* nat64_find_route6(struct route_in6 *ro,
83     struct in6_addr *dest, struct mbuf *m);
84 
85 static NAT64NOINLINE int
86 nat64_output(struct ifnet *ifp, struct mbuf *m,
87     struct sockaddr *dst, struct route *ro, nat64_stats_block *stats,
88     void *logdata)
89 {
90 	int error;
91 
92 	if (logdata != NULL)
93 		nat64_log(logdata, m, dst->sa_family);
94 	error = (*ifp->if_output)(ifp, m, dst, ro);
95 	if (error != 0)
96 		NAT64STAT_INC(stats, oerrors);
97 	return (error);
98 }
99 
100 static NAT64NOINLINE int
101 nat64_output_one(struct mbuf *m, nat64_stats_block *stats, void *logdata)
102 {
103 	struct route_in6 ro6;
104 	struct route ro4, *ro;
105 	struct sockaddr *dst;
106 	struct ifnet *ifp;
107 	struct ip6_hdr *ip6;
108 	struct ip *ip4;
109 	int error;
110 
111 	ip4 = mtod(m, struct ip *);
112 	switch (ip4->ip_v) {
113 	case IPVERSION:
114 		ro = &ro4;
115 		dst = nat64_find_route4(&ro4, ip4->ip_dst.s_addr, m);
116 		if (dst == NULL)
117 			NAT64STAT_INC(stats, noroute4);
118 		break;
119 	case (IPV6_VERSION >> 4):
120 		ip6 = (struct ip6_hdr *)ip4;
121 		ro = (struct route *)&ro6;
122 		dst = nat64_find_route6(&ro6, &ip6->ip6_dst, m);
123 		if (dst == NULL)
124 			NAT64STAT_INC(stats, noroute6);
125 		break;
126 	default:
127 		m_freem(m);
128 		NAT64STAT_INC(stats, dropped);
129 		DPRINTF(DP_DROPS, "dropped due to unknown IP version");
130 		return (EAFNOSUPPORT);
131 	}
132 	if (dst == NULL) {
133 		FREE_ROUTE(ro);
134 		m_freem(m);
135 		return (EHOSTUNREACH);
136 	}
137 	if (logdata != NULL)
138 		nat64_log(logdata, m, dst->sa_family);
139 	ifp = ro->ro_rt->rt_ifp;
140 	error = (*ifp->if_output)(ifp, m, dst, ro);
141 	if (error != 0)
142 		NAT64STAT_INC(stats, oerrors);
143 	FREE_ROUTE(ro);
144 	return (error);
145 }
146 #else /* !IPFIREWALL_NAT64_DIRECT_OUTPUT */
147 static NAT64NOINLINE int
148 nat64_output(struct ifnet *ifp, struct mbuf *m,
149     struct sockaddr *dst, struct route *ro, nat64_stats_block *stats,
150     void *logdata)
151 {
152 	struct ip *ip4;
153 	int ret, af;
154 
155 	ip4 = mtod(m, struct ip *);
156 	switch (ip4->ip_v) {
157 	case IPVERSION:
158 		af = AF_INET;
159 		ret = NETISR_IP;
160 		break;
161 	case (IPV6_VERSION >> 4):
162 		af = AF_INET6;
163 		ret = NETISR_IPV6;
164 		break;
165 	default:
166 		m_freem(m);
167 		NAT64STAT_INC(stats, dropped);
168 		DPRINTF(DP_DROPS, "unknown IP version");
169 		return (EAFNOSUPPORT);
170 	}
171 	if (logdata != NULL)
172 		nat64_log(logdata, m, af);
173 	ret = netisr_queue(ret, m);
174 	if (ret != 0)
175 		NAT64STAT_INC(stats, oerrors);
176 	return (ret);
177 }
178 
179 static NAT64NOINLINE int
180 nat64_output_one(struct mbuf *m, nat64_stats_block *stats, void *logdata)
181 {
182 
183 	return (nat64_output(NULL, m, NULL, NULL, stats, logdata));
184 }
185 #endif /* !IPFIREWALL_NAT64_DIRECT_OUTPUT */
186 
187 
188 #if 0
189 void print_ipv6_header(struct ip6_hdr *ip6, char *buf, size_t bufsize);
190 
191 void
192 print_ipv6_header(struct ip6_hdr *ip6, char *buf, size_t bufsize)
193 {
194 	char sbuf[INET6_ADDRSTRLEN], dbuf[INET6_ADDRSTRLEN];
195 
196 	inet_ntop(AF_INET6, &ip6->ip6_src, sbuf, sizeof(sbuf));
197 	inet_ntop(AF_INET6, &ip6->ip6_dst, dbuf, sizeof(dbuf));
198 	snprintf(buf, bufsize, "%s -> %s %d", sbuf, dbuf, ip6->ip6_nxt);
199 }
200 
201 
202 static NAT64NOINLINE int
203 nat64_embed_ip4(struct nat64_cfg *cfg, in_addr_t ia, struct in6_addr *ip6)
204 {
205 
206 	/* assume the prefix is properly filled with zeros */
207 	bcopy(&cfg->prefix, ip6, sizeof(*ip6));
208 	switch (cfg->plen) {
209 	case 32:
210 	case 96:
211 		ip6->s6_addr32[cfg->plen / 32] = ia;
212 		break;
213 	case 40:
214 	case 48:
215 	case 56:
216 #if BYTE_ORDER == BIG_ENDIAN
217 		ip6->s6_addr32[1] = cfg->prefix.s6_addr32[1] |
218 		    (ia >> (cfg->plen % 32));
219 		ip6->s6_addr32[2] = ia << (24 - cfg->plen % 32);
220 #elif BYTE_ORDER == LITTLE_ENDIAN
221 		ip6->s6_addr32[1] = cfg->prefix.s6_addr32[1] |
222 		    (ia << (cfg->plen % 32));
223 		ip6->s6_addr32[2] = ia >> (24 - cfg->plen % 32);
224 #endif
225 		break;
226 	case 64:
227 #if BYTE_ORDER == BIG_ENDIAN
228 		ip6->s6_addr32[2] = ia >> 8;
229 		ip6->s6_addr32[3] = ia << 24;
230 #elif BYTE_ORDER == LITTLE_ENDIAN
231 		ip6->s6_addr32[2] = ia << 8;
232 		ip6->s6_addr32[3] = ia >> 24;
233 #endif
234 		break;
235 	default:
236 		return (0);
237 	};
238 	ip6->s6_addr8[8] = 0;
239 	return (1);
240 }
241 
242 static NAT64NOINLINE in_addr_t
243 nat64_extract_ip4(struct in6_addr *ip6, int plen)
244 {
245 	in_addr_t ia;
246 
247 	/*
248 	 * According to RFC 6052 p2.2:
249 	 * IPv4-embedded IPv6 addresses are composed of a variable-length
250 	 * prefix, the embedded IPv4 address, and a variable length suffix.
251 	 * The suffix bits are reserved for future extensions and SHOULD
252 	 * be set to zero.
253 	 */
254 	switch (plen) {
255 	case 32:
256 		if (ip6->s6_addr32[3] != 0 || ip6->s6_addr32[2] != 0)
257 			goto badip6;
258 		break;
259 	case 40:
260 		if (ip6->s6_addr32[3] != 0 ||
261 		    (ip6->s6_addr32[2] & htonl(0xff00ffff)) != 0)
262 			goto badip6;
263 		break;
264 	case 48:
265 		if (ip6->s6_addr32[3] != 0 ||
266 		    (ip6->s6_addr32[2] & htonl(0xff0000ff)) != 0)
267 			goto badip6;
268 		break;
269 	case 56:
270 		if (ip6->s6_addr32[3] != 0 || ip6->s6_addr8[8] != 0)
271 			goto badip6;
272 		break;
273 	case 64:
274 		if (ip6->s6_addr8[8] != 0 ||
275 		    (ip6->s6_addr32[3] & htonl(0x00ffffff)) != 0)
276 			goto badip6;
277 	};
278 	switch (plen) {
279 	case 32:
280 	case 96:
281 		ia = ip6->s6_addr32[plen / 32];
282 		break;
283 	case 40:
284 	case 48:
285 	case 56:
286 #if BYTE_ORDER == BIG_ENDIAN
287 		ia = (ip6->s6_addr32[1] << (plen % 32)) |
288 		    (ip6->s6_addr32[2] >> (24 - plen % 32));
289 #elif BYTE_ORDER == LITTLE_ENDIAN
290 		ia = (ip6->s6_addr32[1] >> (plen % 32)) |
291 		    (ip6->s6_addr32[2] << (24 - plen % 32));
292 #endif
293 		break;
294 	case 64:
295 #if BYTE_ORDER == BIG_ENDIAN
296 		ia = (ip6->s6_addr32[2] << 8) | (ip6->s6_addr32[3] >> 24);
297 #elif BYTE_ORDER == LITTLE_ENDIAN
298 		ia = (ip6->s6_addr32[2] >> 8) | (ip6->s6_addr32[3] << 24);
299 #endif
300 		break;
301 	default:
302 		return (0);
303 	};
304 	if (nat64_check_ip4(ia) != 0 ||
305 	    nat64_check_private_ip4(ia) != 0)
306 		goto badip4;
307 
308 	return (ia);
309 badip4:
310 	DPRINTF(DP_GENERIC, "invalid destination address: %08x", ia);
311 	return (0);
312 badip6:
313 	DPRINTF(DP_GENERIC, "invalid IPv4-embedded IPv6 address");
314 	return (0);
315 }
316 #endif
317 
318 /*
319  * According to RFC 1624 the equation for incremental checksum update is:
320  *	HC' = ~(~HC + ~m + m')	--	[Eqn. 3]
321  *	HC' = HC - ~m - m'	--	[Eqn. 4]
322  * So, when we are replacing IPv4 addresses to IPv6, we
323  * can assume, that new bytes previously were zeros, and vise versa -
324  * when we replacing IPv6 addresses to IPv4, now unused bytes become
325  * zeros. The payload length in pseudo header has bigger size, but one
326  * half of it should be zero. Using the equation 4 we get:
327  *	HC' = HC - (~m0 + m0')	-- m0 is first changed word
328  *	HC' = (HC - (~m0 + m0')) - (~m1 + m1')	-- m1 is second changed word
329  *	HC' = HC - ~m0 - m0' - ~m1 - m1' - ... =
330  *	  = HC - sum(~m[i] + m'[i])
331  *
332  * The function result should be used as follows:
333  *	IPv6 to IPv4:	HC' = cksum_add(HC, result)
334  *	IPv4 to IPv6:	HC' = cksum_add(HC, ~result)
335  */
336 static NAT64NOINLINE uint16_t
337 nat64_cksum_convert(struct ip6_hdr *ip6, struct ip *ip)
338 {
339 	uint32_t sum;
340 	uint16_t *p;
341 
342 	sum = ~ip->ip_src.s_addr >> 16;
343 	sum += ~ip->ip_src.s_addr & 0xffff;
344 	sum += ~ip->ip_dst.s_addr >> 16;
345 	sum += ~ip->ip_dst.s_addr & 0xffff;
346 
347 	for (p = (uint16_t *)&ip6->ip6_src;
348 	    p < (uint16_t *)(&ip6->ip6_src + 2); p++)
349 		sum += *p;
350 
351 	while (sum >> 16)
352 		sum = (sum & 0xffff) + (sum >> 16);
353 	return (sum);
354 }
355 
356 #if __FreeBSD_version < 1100000
357 #define	ip_fillid(ip)		(ip)->ip_id = ip_newid()
358 #endif
359 static NAT64NOINLINE void
360 nat64_init_ip4hdr(const struct ip6_hdr *ip6, const struct ip6_frag *frag,
361     uint16_t plen, uint8_t proto, struct ip *ip)
362 {
363 
364 	/* assume addresses are already initialized */
365 	ip->ip_v = IPVERSION;
366 	ip->ip_hl = sizeof(*ip) >> 2;
367 	ip->ip_tos = (ntohl(ip6->ip6_flow) >> 20) & 0xff;
368 	ip->ip_len = htons(sizeof(*ip) + plen);
369 #ifdef IPFIREWALL_NAT64_DIRECT_OUTPUT
370 	ip->ip_ttl = ip6->ip6_hlim - IPV6_HLIMDEC;
371 #else
372 	/* Forwarding code will decrement TTL. */
373 	ip->ip_ttl = ip6->ip6_hlim;
374 #endif
375 	ip->ip_sum = 0;
376 	ip->ip_p = (proto == IPPROTO_ICMPV6) ? IPPROTO_ICMP: proto;
377 	ip_fillid(ip);
378 	if (frag != NULL) {
379 		ip->ip_off = htons(ntohs(frag->ip6f_offlg) >> 3);
380 		if (frag->ip6f_offlg & IP6F_MORE_FRAG)
381 			ip->ip_off |= htons(IP_MF);
382 	} else {
383 		ip->ip_off = htons(IP_DF);
384 	}
385 	ip->ip_sum = in_cksum_hdr(ip);
386 }
387 
388 #define	FRAGSZ(mtu) ((mtu) - sizeof(struct ip6_hdr) - sizeof(struct ip6_frag))
389 static NAT64NOINLINE int
390 nat64_fragment6(nat64_stats_block *stats, struct ip6_hdr *ip6, struct mbufq *mq,
391     struct mbuf *m, uint32_t mtu, uint16_t ip_id, uint16_t ip_off)
392 {
393 	struct ip6_frag ip6f;
394 	struct mbuf *n;
395 	uint16_t hlen, len, offset;
396 	int plen;
397 
398 	plen = ntohs(ip6->ip6_plen);
399 	hlen = sizeof(struct ip6_hdr);
400 
401 	/* Fragmentation isn't needed */
402 	if (ip_off == 0 && plen <= mtu - hlen) {
403 		M_PREPEND(m, hlen, M_NOWAIT);
404 		if (m == NULL) {
405 			NAT64STAT_INC(stats, nomem);
406 			return (ENOMEM);
407 		}
408 		bcopy(ip6, mtod(m, void *), hlen);
409 		if (mbufq_enqueue(mq, m) != 0) {
410 			m_freem(m);
411 			NAT64STAT_INC(stats, dropped);
412 			DPRINTF(DP_DROPS, "dropped due to mbufq overflow");
413 			return (ENOBUFS);
414 		}
415 		return (0);
416 	}
417 
418 	hlen += sizeof(struct ip6_frag);
419 	ip6f.ip6f_reserved = 0;
420 	ip6f.ip6f_nxt = ip6->ip6_nxt;
421 	ip6->ip6_nxt = IPPROTO_FRAGMENT;
422 	if (ip_off != 0) {
423 		/*
424 		 * We have got an IPv4 fragment.
425 		 * Use offset value and ip_id from original fragment.
426 		 */
427 		ip6f.ip6f_ident = htonl(ntohs(ip_id));
428 		offset = (ntohs(ip_off) & IP_OFFMASK) << 3;
429 		NAT64STAT_INC(stats, ifrags);
430 	} else {
431 		/* The packet size exceeds interface MTU */
432 		ip6f.ip6f_ident = htonl(ip6_randomid());
433 		offset = 0; /* First fragment*/
434 	}
435 	while (plen > 0 && m != NULL) {
436 		n = NULL;
437 		len = FRAGSZ(mtu) & ~7;
438 		if (len > plen)
439 			len = plen;
440 		ip6->ip6_plen = htons(len + sizeof(ip6f));
441 		ip6f.ip6f_offlg = ntohs(offset);
442 		if (len < plen || (ip_off & htons(IP_MF)) != 0)
443 			ip6f.ip6f_offlg |= IP6F_MORE_FRAG;
444 		offset += len;
445 		plen -= len;
446 		if (plen > 0) {
447 			n = m_split(m, len, M_NOWAIT);
448 			if (n == NULL)
449 				goto fail;
450 		}
451 		M_PREPEND(m, hlen, M_NOWAIT);
452 		if (m == NULL)
453 			goto fail;
454 		bcopy(ip6, mtod(m, void *), sizeof(struct ip6_hdr));
455 		bcopy(&ip6f, mtodo(m, sizeof(struct ip6_hdr)),
456 		    sizeof(struct ip6_frag));
457 		if (mbufq_enqueue(mq, m) != 0)
458 			goto fail;
459 		m = n;
460 	}
461 	NAT64STAT_ADD(stats, ofrags, mbufq_len(mq));
462 	return (0);
463 fail:
464 	if (m != NULL)
465 		m_freem(m);
466 	if (n != NULL)
467 		m_freem(n);
468 	mbufq_drain(mq);
469 	NAT64STAT_INC(stats, nomem);
470 	return (ENOMEM);
471 }
472 
473 #if __FreeBSD_version < 1100000
474 #define	rt_expire	rt_rmx.rmx_expire
475 #define	rt_mtu		rt_rmx.rmx_mtu
476 #endif
477 static NAT64NOINLINE struct sockaddr*
478 nat64_find_route6(struct route_in6 *ro, struct in6_addr *dest, struct mbuf *m)
479 {
480 	struct sockaddr_in6 *dst;
481 	struct rtentry *rt;
482 
483 	bzero(ro, sizeof(*ro));
484 	dst = (struct sockaddr_in6 *)&ro->ro_dst;
485 	dst->sin6_family = AF_INET6;
486 	dst->sin6_len = sizeof(*dst);
487 	dst->sin6_addr = *dest;
488 	IN6_LOOKUP_ROUTE(ro, M_GETFIB(m));
489 	rt = ro->ro_rt;
490 	if (rt && (rt->rt_flags & RTF_UP) &&
491 	    (rt->rt_ifp->if_flags & IFF_UP) &&
492 	    (rt->rt_ifp->if_drv_flags & IFF_DRV_RUNNING)) {
493 		if (rt->rt_flags & RTF_GATEWAY)
494 			dst = (struct sockaddr_in6 *)rt->rt_gateway;
495 	} else
496 		return (NULL);
497 	if (((rt->rt_flags & RTF_REJECT) &&
498 	    (rt->rt_expire == 0 ||
499 	    time_uptime < rt->rt_expire)) ||
500 	    rt->rt_ifp->if_link_state == LINK_STATE_DOWN)
501 		return (NULL);
502 	return ((struct sockaddr *)dst);
503 }
504 
505 #define	NAT64_ICMP6_PLEN	64
506 static NAT64NOINLINE void
507 nat64_icmp6_reflect(struct mbuf *m, uint8_t type, uint8_t code, uint32_t mtu,
508     nat64_stats_block *stats, void *logdata)
509 {
510 	struct icmp6_hdr *icmp6;
511 	struct ip6_hdr *ip6, *oip6;
512 	struct mbuf *n;
513 	int len, plen;
514 
515 	len = 0;
516 	plen = nat64_getlasthdr(m, &len);
517 	if (plen < 0) {
518 		DPRINTF(DP_DROPS, "mbuf isn't contigious");
519 		goto freeit;
520 	}
521 	/*
522 	 * Do not send ICMPv6 in reply to ICMPv6 errors.
523 	 */
524 	if (plen == IPPROTO_ICMPV6) {
525 		if (m->m_len < len + sizeof(*icmp6)) {
526 			DPRINTF(DP_DROPS, "mbuf isn't contigious");
527 			goto freeit;
528 		}
529 		icmp6 = mtodo(m, len);
530 		if (icmp6->icmp6_type < ICMP6_ECHO_REQUEST ||
531 		    icmp6->icmp6_type == ND_REDIRECT) {
532 			DPRINTF(DP_DROPS, "do not send ICMPv6 in reply to "
533 			    "ICMPv6 errors");
534 			goto freeit;
535 		}
536 	}
537 	/*
538 	if (icmp6_ratelimit(&ip6->ip6_src, type, code))
539 		goto freeit;
540 		*/
541 	ip6 = mtod(m, struct ip6_hdr *);
542 	switch (type) {
543 	case ICMP6_DST_UNREACH:
544 	case ICMP6_PACKET_TOO_BIG:
545 	case ICMP6_TIME_EXCEEDED:
546 	case ICMP6_PARAM_PROB:
547 		break;
548 	default:
549 		goto freeit;
550 	}
551 	/* Calculate length of ICMPv6 payload */
552 	len = (m->m_pkthdr.len > NAT64_ICMP6_PLEN) ? NAT64_ICMP6_PLEN:
553 	    m->m_pkthdr.len;
554 
555 	/* Create new ICMPv6 datagram */
556 	plen = len + sizeof(struct icmp6_hdr);
557 	n = m_get2(sizeof(struct ip6_hdr) + plen + max_hdr, M_NOWAIT,
558 	    MT_HEADER, M_PKTHDR);
559 	if (n == NULL) {
560 		NAT64STAT_INC(stats, nomem);
561 		m_freem(m);
562 		return;
563 	}
564 	/*
565 	 * Move pkthdr from original mbuf. We should have initialized some
566 	 * fields, because we can reinject this mbuf to netisr and it will
567 	 * go trough input path (it requires at least rcvif should be set).
568 	 * Also do M_ALIGN() to reduce chances of need to allocate new mbuf
569 	 * in the chain, when we will do M_PREPEND() or make some type of
570 	 * tunneling.
571 	 */
572 	m_move_pkthdr(n, m);
573 	M_ALIGN(n, sizeof(struct ip6_hdr) + plen + max_hdr);
574 
575 	n->m_len = n->m_pkthdr.len = sizeof(struct ip6_hdr) + plen;
576 	oip6 = mtod(n, struct ip6_hdr *);
577 	oip6->ip6_src = ip6->ip6_dst;
578 	oip6->ip6_dst = ip6->ip6_src;
579 	oip6->ip6_nxt = IPPROTO_ICMPV6;
580 	oip6->ip6_flow = 0;
581 	oip6->ip6_vfc |= IPV6_VERSION;
582 	oip6->ip6_hlim = V_ip6_defhlim;
583 	oip6->ip6_plen = htons(plen);
584 
585 	icmp6 = mtodo(n, sizeof(struct ip6_hdr));
586 	icmp6->icmp6_cksum = 0;
587 	icmp6->icmp6_type = type;
588 	icmp6->icmp6_code = code;
589 	icmp6->icmp6_mtu = htonl(mtu);
590 
591 	m_copydata(m, 0, len, mtodo(n, sizeof(struct ip6_hdr) +
592 	    sizeof(struct icmp6_hdr)));
593 	icmp6->icmp6_cksum = in6_cksum(n, IPPROTO_ICMPV6,
594 	    sizeof(struct ip6_hdr), plen);
595 	m_freem(m);
596 	nat64_output_one(n, stats, logdata);
597 	return;
598 freeit:
599 	NAT64STAT_INC(stats, dropped);
600 	m_freem(m);
601 }
602 
603 static NAT64NOINLINE struct sockaddr*
604 nat64_find_route4(struct route *ro, in_addr_t dest, struct mbuf *m)
605 {
606 	struct sockaddr_in *dst;
607 	struct rtentry *rt;
608 
609 	bzero(ro, sizeof(*ro));
610 	dst = (struct sockaddr_in *)&ro->ro_dst;
611 	dst->sin_family = AF_INET;
612 	dst->sin_len = sizeof(*dst);
613 	dst->sin_addr.s_addr = dest;
614 	IN_LOOKUP_ROUTE(ro, M_GETFIB(m));
615 	rt = ro->ro_rt;
616 	if (rt && (rt->rt_flags & RTF_UP) &&
617 	    (rt->rt_ifp->if_flags & IFF_UP) &&
618 	    (rt->rt_ifp->if_drv_flags & IFF_DRV_RUNNING)) {
619 		if (rt->rt_flags & RTF_GATEWAY)
620 			dst = (struct sockaddr_in *)rt->rt_gateway;
621 	} else
622 		return (NULL);
623 	if (((rt->rt_flags & RTF_REJECT) &&
624 	    (rt->rt_expire == 0 ||
625 	    time_uptime < rt->rt_expire)) ||
626 	    rt->rt_ifp->if_link_state == LINK_STATE_DOWN)
627 		return (NULL);
628 	return ((struct sockaddr *)dst);
629 }
630 
631 #define	NAT64_ICMP_PLEN	64
632 static NAT64NOINLINE void
633 nat64_icmp_reflect(struct mbuf *m, uint8_t type,
634     uint8_t code, uint16_t mtu, nat64_stats_block *stats, void *logdata)
635 {
636 	struct icmp *icmp;
637 	struct ip *ip, *oip;
638 	struct mbuf *n;
639 	int len, plen;
640 
641 	ip = mtod(m, struct ip *);
642 	/* Do not send ICMP error if packet is not the first fragment */
643 	if (ip->ip_off & ~ntohs(IP_MF|IP_DF)) {
644 		DPRINTF(DP_DROPS, "not first fragment");
645 		goto freeit;
646 	}
647 	/* Do not send ICMP in reply to ICMP errors */
648 	if (ip->ip_p == IPPROTO_ICMP) {
649 		if (m->m_len < (ip->ip_hl << 2)) {
650 			DPRINTF(DP_DROPS, "mbuf isn't contigious");
651 			goto freeit;
652 		}
653 		icmp = mtodo(m, ip->ip_hl << 2);
654 		if (!ICMP_INFOTYPE(icmp->icmp_type)) {
655 			DPRINTF(DP_DROPS, "do not send ICMP in reply to "
656 			    "ICMP errors");
657 			goto freeit;
658 		}
659 	}
660 	switch (type) {
661 	case ICMP_UNREACH:
662 	case ICMP_TIMXCEED:
663 	case ICMP_PARAMPROB:
664 		break;
665 	default:
666 		goto freeit;
667 	}
668 	/* Calculate length of ICMP payload */
669 	len = (m->m_pkthdr.len > NAT64_ICMP_PLEN) ? (ip->ip_hl << 2) + 8:
670 	    m->m_pkthdr.len;
671 
672 	/* Create new ICMPv4 datagram */
673 	plen = len + sizeof(struct icmphdr) + sizeof(uint32_t);
674 	n = m_get2(sizeof(struct ip) + plen + max_hdr, M_NOWAIT,
675 	    MT_HEADER, M_PKTHDR);
676 	if (n == NULL) {
677 		NAT64STAT_INC(stats, nomem);
678 		m_freem(m);
679 		return;
680 	}
681 	m_move_pkthdr(n, m);
682 	M_ALIGN(n, sizeof(struct ip) + plen + max_hdr);
683 
684 	n->m_len = n->m_pkthdr.len = sizeof(struct ip) + plen;
685 	oip = mtod(n, struct ip *);
686 	oip->ip_v = IPVERSION;
687 	oip->ip_hl = sizeof(struct ip) >> 2;
688 	oip->ip_tos = 0;
689 	oip->ip_len = htons(n->m_pkthdr.len);
690 	oip->ip_ttl = V_ip_defttl;
691 	oip->ip_p = IPPROTO_ICMP;
692 	ip_fillid(oip);
693 	oip->ip_off = htons(IP_DF);
694 	oip->ip_src = ip->ip_dst;
695 	oip->ip_dst = ip->ip_src;
696 	oip->ip_sum = 0;
697 	oip->ip_sum = in_cksum_hdr(oip);
698 
699 	icmp = mtodo(n, sizeof(struct ip));
700 	icmp->icmp_type = type;
701 	icmp->icmp_code = code;
702 	icmp->icmp_cksum = 0;
703 	icmp->icmp_pmvoid = 0;
704 	icmp->icmp_nextmtu = htons(mtu);
705 	m_copydata(m, 0, len, mtodo(n, sizeof(struct ip) +
706 	    sizeof(struct icmphdr) + sizeof(uint32_t)));
707 	icmp->icmp_cksum = in_cksum_skip(n, sizeof(struct ip) + plen,
708 	    sizeof(struct ip));
709 	m_freem(m);
710 	nat64_output_one(n, stats, logdata);
711 	return;
712 freeit:
713 	NAT64STAT_INC(stats, dropped);
714 	m_freem(m);
715 }
716 
717 /* Translate ICMP echo request/reply into ICMPv6 */
718 static void
719 nat64_icmp_handle_echo(struct ip6_hdr *ip6, struct icmp6_hdr *icmp6,
720     uint16_t id, uint8_t type)
721 {
722 	uint16_t old;
723 
724 	old = *(uint16_t *)icmp6;	/* save type+code in one word */
725 	icmp6->icmp6_type = type;
726 	/* Reflect ICMPv6 -> ICMPv4 type translation in the cksum */
727 	icmp6->icmp6_cksum = cksum_adjust(icmp6->icmp6_cksum,
728 	    old, *(uint16_t *)icmp6);
729 	if (id != 0) {
730 		old = icmp6->icmp6_id;
731 		icmp6->icmp6_id = id;
732 		/* Reflect ICMP id translation in the cksum */
733 		icmp6->icmp6_cksum = cksum_adjust(icmp6->icmp6_cksum,
734 		    old, id);
735 	}
736 	/* Reflect IPv6 pseudo header in the cksum */
737 	icmp6->icmp6_cksum = ~in6_cksum_pseudo(ip6, ntohs(ip6->ip6_plen),
738 	    IPPROTO_ICMPV6, ~icmp6->icmp6_cksum);
739 }
740 
741 static NAT64NOINLINE struct mbuf *
742 nat64_icmp_translate(struct mbuf *m, struct ip6_hdr *ip6, uint16_t icmpid,
743     int offset, nat64_stats_block *stats)
744 {
745 	struct ip ip;
746 	struct icmp *icmp;
747 	struct tcphdr *tcp;
748 	struct udphdr *udp;
749 	struct ip6_hdr *eip6;
750 	struct mbuf *n;
751 	uint32_t mtu;
752 	int len, hlen, plen;
753 	uint8_t type, code;
754 
755 	if (m->m_len < offset + ICMP_MINLEN)
756 		m = m_pullup(m, offset + ICMP_MINLEN);
757 	if (m == NULL) {
758 		NAT64STAT_INC(stats, nomem);
759 		return (m);
760 	}
761 	mtu = 0;
762 	icmp = mtodo(m, offset);
763 	/* RFC 7915 p4.2 */
764 	switch (icmp->icmp_type) {
765 	case ICMP_ECHOREPLY:
766 		type = ICMP6_ECHO_REPLY;
767 		code = 0;
768 		break;
769 	case ICMP_UNREACH:
770 		type = ICMP6_DST_UNREACH;
771 		switch (icmp->icmp_code) {
772 		case ICMP_UNREACH_NET:
773 		case ICMP_UNREACH_HOST:
774 		case ICMP_UNREACH_SRCFAIL:
775 		case ICMP_UNREACH_NET_UNKNOWN:
776 		case ICMP_UNREACH_HOST_UNKNOWN:
777 		case ICMP_UNREACH_TOSNET:
778 		case ICMP_UNREACH_TOSHOST:
779 			code = ICMP6_DST_UNREACH_NOROUTE;
780 			break;
781 		case ICMP_UNREACH_PROTOCOL:
782 			type = ICMP6_PARAM_PROB;
783 			code = ICMP6_PARAMPROB_NEXTHEADER;
784 			break;
785 		case ICMP_UNREACH_PORT:
786 			code = ICMP6_DST_UNREACH_NOPORT;
787 			break;
788 		case ICMP_UNREACH_NEEDFRAG:
789 			type = ICMP6_PACKET_TOO_BIG;
790 			code = 0;
791 			/* XXX: needs an additional look */
792 			mtu = max(IPV6_MMTU, ntohs(icmp->icmp_nextmtu) + 20);
793 			break;
794 		case ICMP_UNREACH_NET_PROHIB:
795 		case ICMP_UNREACH_HOST_PROHIB:
796 		case ICMP_UNREACH_FILTER_PROHIB:
797 		case ICMP_UNREACH_PRECEDENCE_CUTOFF:
798 			code = ICMP6_DST_UNREACH_ADMIN;
799 			break;
800 		default:
801 			DPRINTF(DP_DROPS, "Unsupported ICMP type %d, code %d",
802 			    icmp->icmp_type, icmp->icmp_code);
803 			goto freeit;
804 		}
805 		break;
806 	case ICMP_TIMXCEED:
807 		type = ICMP6_TIME_EXCEEDED;
808 		code = icmp->icmp_code;
809 		break;
810 	case ICMP_ECHO:
811 		type = ICMP6_ECHO_REQUEST;
812 		code = 0;
813 		break;
814 	case ICMP_PARAMPROB:
815 		type = ICMP6_PARAM_PROB;
816 		switch (icmp->icmp_code) {
817 		case ICMP_PARAMPROB_ERRATPTR:
818 		case ICMP_PARAMPROB_LENGTH:
819 			code = ICMP6_PARAMPROB_HEADER;
820 			switch (icmp->icmp_pptr) {
821 			case 0: /* Version/IHL */
822 			case 1: /* Type Of Service */
823 				mtu = icmp->icmp_pptr;
824 				break;
825 			case 2: /* Total Length */
826 			case 3: mtu = 4; /* Payload Length */
827 				break;
828 			case 8: /* Time to Live */
829 				mtu = 7; /* Hop Limit */
830 				break;
831 			case 9: /* Protocol */
832 				mtu = 6; /* Next Header */
833 				break;
834 			case 12: /* Source address */
835 			case 13:
836 			case 14:
837 			case 15:
838 				mtu = 8;
839 				break;
840 			case 16: /* Destination address */
841 			case 17:
842 			case 18:
843 			case 19:
844 				mtu = 24;
845 				break;
846 			default: /* Silently drop */
847 				DPRINTF(DP_DROPS, "Unsupported ICMP type %d,"
848 				    " code %d, pptr %d", icmp->icmp_type,
849 				    icmp->icmp_code, icmp->icmp_pptr);
850 				goto freeit;
851 			}
852 			break;
853 		default:
854 			DPRINTF(DP_DROPS, "Unsupported ICMP type %d,"
855 			    " code %d, pptr %d", icmp->icmp_type,
856 			    icmp->icmp_code, icmp->icmp_pptr);
857 			goto freeit;
858 		}
859 		break;
860 	default:
861 		DPRINTF(DP_DROPS, "Unsupported ICMP type %d, code %d",
862 		    icmp->icmp_type, icmp->icmp_code);
863 		goto freeit;
864 	}
865 	/*
866 	 * For echo request/reply we can use original payload,
867 	 * but we need adjust icmp_cksum, because ICMPv6 cksum covers
868 	 * IPv6 pseudo header and ICMPv6 types differs from ICMPv4.
869 	 */
870 	if (type == ICMP6_ECHO_REQUEST || type == ICMP6_ECHO_REPLY) {
871 		nat64_icmp_handle_echo(ip6, ICMP6(icmp), icmpid, type);
872 		return (m);
873 	}
874 	/*
875 	 * For other types of ICMP messages we need to translate inner
876 	 * IPv4 header to IPv6 header.
877 	 * Assume ICMP src is the same as payload dst
878 	 * E.g. we have ( GWsrc1 , NATIP1 ) in outer header
879 	 * and          ( NATIP1, Hostdst1 ) in ICMP copy header.
880 	 * In that case, we already have map for NATIP1 and GWsrc1.
881 	 * The only thing we need is to copy IPv6 map prefix to
882 	 * Hostdst1.
883 	 */
884 	hlen = offset + ICMP_MINLEN;
885 	if (m->m_pkthdr.len < hlen + sizeof(struct ip) + ICMP_MINLEN) {
886 		DPRINTF(DP_DROPS, "Message is too short %d",
887 		    m->m_pkthdr.len);
888 		goto freeit;
889 	}
890 	m_copydata(m, hlen, sizeof(struct ip), (char *)&ip);
891 	if (ip.ip_v != IPVERSION) {
892 		DPRINTF(DP_DROPS, "Wrong IP version %d", ip.ip_v);
893 		goto freeit;
894 	}
895 	hlen += ip.ip_hl << 2; /* Skip inner IP header */
896 	if (nat64_check_ip4(ip.ip_src.s_addr) != 0 ||
897 	    nat64_check_ip4(ip.ip_dst.s_addr) != 0 ||
898 	    nat64_check_private_ip4(ip.ip_src.s_addr) != 0 ||
899 	    nat64_check_private_ip4(ip.ip_dst.s_addr) != 0) {
900 		DPRINTF(DP_DROPS, "IP addresses checks failed %04x -> %04x",
901 		    ntohl(ip.ip_src.s_addr), ntohl(ip.ip_dst.s_addr));
902 		goto freeit;
903 	}
904 	if (m->m_pkthdr.len < hlen + ICMP_MINLEN) {
905 		DPRINTF(DP_DROPS, "Message is too short %d",
906 		    m->m_pkthdr.len);
907 		goto freeit;
908 	}
909 #if 0
910 	/*
911 	 * Check that inner source matches the outer destination.
912 	 * XXX: We need some method to convert IPv4 into IPv6 address here,
913 	 *	and compare IPv6 addresses.
914 	 */
915 	if (ip.ip_src.s_addr != nat64_get_ip4(&ip6->ip6_dst)) {
916 		DPRINTF(DP_GENERIC, "Inner source doesn't match destination ",
917 		    "%04x vs %04x", ip.ip_src.s_addr,
918 		    nat64_get_ip4(&ip6->ip6_dst));
919 		goto freeit;
920 	}
921 #endif
922 	/*
923 	 * Create new mbuf for ICMPv6 datagram.
924 	 * NOTE: len is data length just after inner IP header.
925 	 */
926 	len = m->m_pkthdr.len - hlen;
927 	if (sizeof(struct ip6_hdr) +
928 	    sizeof(struct icmp6_hdr) + len > NAT64_ICMP6_PLEN)
929 		len = NAT64_ICMP6_PLEN - sizeof(struct icmp6_hdr) -
930 		    sizeof(struct ip6_hdr);
931 	plen = sizeof(struct icmp6_hdr) + sizeof(struct ip6_hdr) + len;
932 	n = m_get2(offset + plen + max_hdr, M_NOWAIT, MT_HEADER, M_PKTHDR);
933 	if (n == NULL) {
934 		NAT64STAT_INC(stats, nomem);
935 		m_freem(m);
936 		return (NULL);
937 	}
938 	m_move_pkthdr(n, m);
939 	M_ALIGN(n, offset + plen + max_hdr);
940 	n->m_len = n->m_pkthdr.len = offset + plen;
941 	/* Adjust ip6_plen in outer header */
942 	ip6->ip6_plen = htons(plen);
943 	/* Construct new inner IPv6 header */
944 	eip6 = mtodo(n, offset + sizeof(struct icmp6_hdr));
945 	eip6->ip6_src = ip6->ip6_dst;
946 	/* Use the fact that we have single /96 prefix for IPv4 map */
947 	eip6->ip6_dst = ip6->ip6_src;
948 	nat64_set_ip4(&eip6->ip6_dst, ip.ip_dst.s_addr);
949 
950 	eip6->ip6_flow = htonl(ip.ip_tos << 20);
951 	eip6->ip6_vfc |= IPV6_VERSION;
952 	eip6->ip6_hlim = ip.ip_ttl;
953 	eip6->ip6_plen = htons(ntohs(ip.ip_len) - (ip.ip_hl << 2));
954 	eip6->ip6_nxt = (ip.ip_p == IPPROTO_ICMP) ? IPPROTO_ICMPV6: ip.ip_p;
955 	m_copydata(m, hlen, len, (char *)(eip6 + 1));
956 	/*
957 	 * We need to translate source port in the inner ULP header,
958 	 * and adjust ULP checksum.
959 	 */
960 	switch (ip.ip_p) {
961 	case IPPROTO_TCP:
962 		if (len < offsetof(struct tcphdr, th_sum))
963 			break;
964 		tcp = TCP(eip6 + 1);
965 		if (icmpid != 0) {
966 			tcp->th_sum = cksum_adjust(tcp->th_sum,
967 			    tcp->th_sport, icmpid);
968 			tcp->th_sport = icmpid;
969 		}
970 		tcp->th_sum = cksum_add(tcp->th_sum,
971 		    ~nat64_cksum_convert(eip6, &ip));
972 		break;
973 	case IPPROTO_UDP:
974 		if (len < offsetof(struct udphdr, uh_sum))
975 			break;
976 		udp = UDP(eip6 + 1);
977 		if (icmpid != 0) {
978 			udp->uh_sum = cksum_adjust(udp->uh_sum,
979 			    udp->uh_sport, icmpid);
980 			udp->uh_sport = icmpid;
981 		}
982 		udp->uh_sum = cksum_add(udp->uh_sum,
983 		    ~nat64_cksum_convert(eip6, &ip));
984 		break;
985 	case IPPROTO_ICMP:
986 		/*
987 		 * Check if this is an ICMP error message for echo request
988 		 * that we sent. I.e. ULP in the data containing invoking
989 		 * packet is IPPROTO_ICMP and its type is ICMP_ECHO.
990 		 */
991 		icmp = (struct icmp *)(eip6 + 1);
992 		if (icmp->icmp_type != ICMP_ECHO) {
993 			m_freem(n);
994 			goto freeit;
995 		}
996 		/*
997 		 * For our client this original datagram should looks
998 		 * like it was ICMPv6 datagram with type ICMP6_ECHO_REQUEST.
999 		 * Thus we need adjust icmp_cksum and convert type from
1000 		 * ICMP_ECHO to ICMP6_ECHO_REQUEST.
1001 		 */
1002 		nat64_icmp_handle_echo(eip6, ICMP6(icmp), icmpid,
1003 		    ICMP6_ECHO_REQUEST);
1004 	}
1005 	m_freem(m);
1006 	/* Convert ICMPv4 into ICMPv6 header */
1007 	icmp = mtodo(n, offset);
1008 	ICMP6(icmp)->icmp6_type = type;
1009 	ICMP6(icmp)->icmp6_code = code;
1010 	ICMP6(icmp)->icmp6_mtu = htonl(mtu);
1011 	ICMP6(icmp)->icmp6_cksum = 0;
1012 	ICMP6(icmp)->icmp6_cksum = cksum_add(
1013 	    ~in6_cksum_pseudo(ip6, plen, IPPROTO_ICMPV6, 0),
1014 	    in_cksum_skip(n, n->m_pkthdr.len, offset));
1015 	return (n);
1016 freeit:
1017 	m_freem(m);
1018 	NAT64STAT_INC(stats, dropped);
1019 	return (NULL);
1020 }
1021 
1022 int
1023 nat64_getlasthdr(struct mbuf *m, int *offset)
1024 {
1025 	struct ip6_hdr *ip6;
1026 	struct ip6_hbh *hbh;
1027 	int proto, hlen;
1028 
1029 	if (offset != NULL)
1030 		hlen = *offset;
1031 	else
1032 		hlen = 0;
1033 
1034 	if (m->m_len < hlen + sizeof(*ip6))
1035 		return (-1);
1036 
1037 	ip6 = mtodo(m, hlen);
1038 	hlen += sizeof(*ip6);
1039 	proto = ip6->ip6_nxt;
1040 	/* Skip extension headers */
1041 	while (proto == IPPROTO_HOPOPTS || proto == IPPROTO_ROUTING ||
1042 	    proto == IPPROTO_DSTOPTS) {
1043 		hbh = mtodo(m, hlen);
1044 		/*
1045 		 * We expect mbuf has contigious data up to
1046 		 * upper level header.
1047 		 */
1048 		if (m->m_len < hlen)
1049 			return (-1);
1050 		/*
1051 		 * We doesn't support Jumbo payload option,
1052 		 * so return error.
1053 		 */
1054 		if (proto == IPPROTO_HOPOPTS && ip6->ip6_plen == 0)
1055 			return (-1);
1056 		proto = hbh->ip6h_nxt;
1057 		hlen += hbh->ip6h_len << 3;
1058 	}
1059 	if (offset != NULL)
1060 		*offset = hlen;
1061 	return (proto);
1062 }
1063 
1064 int
1065 nat64_do_handle_ip4(struct mbuf *m, struct in6_addr *saddr,
1066     struct in6_addr *daddr, uint16_t lport, nat64_stats_block *stats,
1067     void *logdata)
1068 {
1069 	struct route_in6 ro;
1070 	struct ip6_hdr ip6;
1071 	struct ifnet *ifp;
1072 	struct ip *ip;
1073 	struct mbufq mq;
1074 	struct sockaddr *dst;
1075 	uint32_t mtu;
1076 	uint16_t ip_id, ip_off;
1077 	uint16_t *csum;
1078 	int plen, hlen;
1079 	uint8_t proto;
1080 
1081 	ip = mtod(m, struct ip*);
1082 
1083 	if (ip->ip_ttl <= IPTTLDEC) {
1084 		nat64_icmp_reflect(m, ICMP_TIMXCEED,
1085 		    ICMP_TIMXCEED_INTRANS, 0, stats, logdata);
1086 		return (NAT64RETURN);
1087 	}
1088 
1089 	ip6.ip6_dst = *daddr;
1090 	ip6.ip6_src = *saddr;
1091 
1092 	hlen = ip->ip_hl << 2;
1093 	plen = ntohs(ip->ip_len) - hlen;
1094 	proto = ip->ip_p;
1095 
1096 	/* Save ip_id and ip_off, both are in network byte order */
1097 	ip_id = ip->ip_id;
1098 	ip_off = ip->ip_off & htons(IP_OFFMASK | IP_MF);
1099 
1100 	/* Fragment length must be multiple of 8 octets */
1101 	if ((ip->ip_off & htons(IP_MF)) != 0 && (plen & 0x7) != 0) {
1102 		nat64_icmp_reflect(m, ICMP_PARAMPROB,
1103 		    ICMP_PARAMPROB_LENGTH, 0, stats, logdata);
1104 		return (NAT64RETURN);
1105 	}
1106 	/* Fragmented ICMP is unsupported */
1107 	if (proto == IPPROTO_ICMP && ip_off != 0) {
1108 		DPRINTF(DP_DROPS, "dropped due to fragmented ICMP");
1109 		NAT64STAT_INC(stats, dropped);
1110 		return (NAT64MFREE);
1111 	}
1112 
1113 	dst = nat64_find_route6(&ro, &ip6.ip6_dst, m);
1114 	if (dst == NULL) {
1115 		FREE_ROUTE(&ro);
1116 		NAT64STAT_INC(stats, noroute6);
1117 		nat64_icmp_reflect(m, ICMP_UNREACH, ICMP_UNREACH_HOST, 0,
1118 		    stats, logdata);
1119 		return (NAT64RETURN);
1120 	}
1121 	ifp = ro.ro_rt->rt_ifp;
1122 	if (ro.ro_rt->rt_mtu != 0)
1123 		mtu = min(ro.ro_rt->rt_mtu, ifp->if_mtu);
1124 	else
1125 		mtu = ifp->if_mtu;
1126 	if (mtu < plen + sizeof(ip6) && (ip->ip_off & htons(IP_DF)) != 0) {
1127 		FREE_ROUTE(&ro);
1128 		nat64_icmp_reflect(m, ICMP_UNREACH, ICMP_UNREACH_NEEDFRAG,
1129 		    FRAGSZ(mtu) + sizeof(struct ip), stats, logdata);
1130 		return (NAT64RETURN);
1131 	}
1132 
1133 	ip6.ip6_flow = htonl(ip->ip_tos << 20);
1134 	ip6.ip6_vfc |= IPV6_VERSION;
1135 #ifdef IPFIREWALL_NAT64_DIRECT_OUTPUT
1136 	ip6.ip6_hlim = ip->ip_ttl - IPTTLDEC;
1137 #else
1138 	/* Forwarding code will decrement HLIM. */
1139 	ip6.ip6_hlim = ip->ip_ttl;
1140 #endif
1141 	ip6.ip6_plen = htons(plen);
1142 	ip6.ip6_nxt = (proto == IPPROTO_ICMP) ? IPPROTO_ICMPV6: proto;
1143 	/* Convert checksums. */
1144 	switch (proto) {
1145 	case IPPROTO_TCP:
1146 		csum = &TCP(mtodo(m, hlen))->th_sum;
1147 		if (lport != 0) {
1148 			struct tcphdr *tcp = TCP(mtodo(m, hlen));
1149 			*csum = cksum_adjust(*csum, tcp->th_dport, lport);
1150 			tcp->th_dport = lport;
1151 		}
1152 		*csum = cksum_add(*csum, ~nat64_cksum_convert(&ip6, ip));
1153 		break;
1154 	case IPPROTO_UDP:
1155 		csum = &UDP(mtodo(m, hlen))->uh_sum;
1156 		if (lport != 0) {
1157 			struct udphdr *udp = UDP(mtodo(m, hlen));
1158 			*csum = cksum_adjust(*csum, udp->uh_dport, lport);
1159 			udp->uh_dport = lport;
1160 		}
1161 		*csum = cksum_add(*csum, ~nat64_cksum_convert(&ip6, ip));
1162 		break;
1163 	case IPPROTO_ICMP:
1164 		m = nat64_icmp_translate(m, &ip6, lport, hlen, stats);
1165 		if (m == NULL) {
1166 			FREE_ROUTE(&ro);
1167 			/* stats already accounted */
1168 			return (NAT64RETURN);
1169 		}
1170 	}
1171 
1172 	m_adj(m, hlen);
1173 	mbufq_init(&mq, 255);
1174 	nat64_fragment6(stats, &ip6, &mq, m, mtu, ip_id, ip_off);
1175 	while ((m = mbufq_dequeue(&mq)) != NULL) {
1176 		if (nat64_output(ifp, m, dst, (struct route *)&ro, stats,
1177 		    logdata) != 0)
1178 			break;
1179 		NAT64STAT_INC(stats, opcnt46);
1180 	}
1181 	mbufq_drain(&mq);
1182 	FREE_ROUTE(&ro);
1183 	return (NAT64RETURN);
1184 }
1185 
1186 int
1187 nat64_handle_icmp6(struct mbuf *m, int hlen, uint32_t aaddr, uint16_t aport,
1188     nat64_stats_block *stats, void *logdata)
1189 {
1190 	struct ip ip;
1191 	struct icmp6_hdr *icmp6;
1192 	struct ip6_frag *ip6f;
1193 	struct ip6_hdr *ip6, *ip6i;
1194 	uint32_t mtu;
1195 	int plen, proto;
1196 	uint8_t type, code;
1197 
1198 	if (hlen == 0) {
1199 		ip6 = mtod(m, struct ip6_hdr *);
1200 		if (nat64_check_ip6(&ip6->ip6_src) != 0 ||
1201 		    nat64_check_ip6(&ip6->ip6_dst) != 0)
1202 			return (NAT64SKIP);
1203 
1204 		proto = nat64_getlasthdr(m, &hlen);
1205 		if (proto != IPPROTO_ICMPV6) {
1206 			DPRINTF(DP_DROPS,
1207 			    "dropped due to mbuf isn't contigious");
1208 			NAT64STAT_INC(stats, dropped);
1209 			return (NAT64MFREE);
1210 		}
1211 	}
1212 
1213 	/*
1214 	 * Translate ICMPv6 type and code to ICMPv4 (RFC7915).
1215 	 * NOTE: ICMPv6 echo handled by nat64_do_handle_ip6().
1216 	 */
1217 	icmp6 = mtodo(m, hlen);
1218 	mtu = 0;
1219 	switch (icmp6->icmp6_type) {
1220 	case ICMP6_DST_UNREACH:
1221 		type = ICMP_UNREACH;
1222 		switch (icmp6->icmp6_code) {
1223 		case ICMP6_DST_UNREACH_NOROUTE:
1224 		case ICMP6_DST_UNREACH_BEYONDSCOPE:
1225 		case ICMP6_DST_UNREACH_ADDR:
1226 			code = ICMP_UNREACH_HOST;
1227 			break;
1228 		case ICMP6_DST_UNREACH_ADMIN:
1229 			code = ICMP_UNREACH_HOST_PROHIB;
1230 			break;
1231 		case ICMP6_DST_UNREACH_NOPORT:
1232 			code = ICMP_UNREACH_PORT;
1233 			break;
1234 		default:
1235 			DPRINTF(DP_DROPS, "Unsupported ICMPv6 type %d,"
1236 			    " code %d", icmp6->icmp6_type,
1237 			    icmp6->icmp6_code);
1238 			NAT64STAT_INC(stats, dropped);
1239 			return (NAT64MFREE);
1240 		}
1241 		break;
1242 	case ICMP6_PACKET_TOO_BIG:
1243 		type = ICMP_UNREACH;
1244 		code = ICMP_UNREACH_NEEDFRAG;
1245 		mtu = ntohl(icmp6->icmp6_mtu);
1246 		if (mtu < IPV6_MMTU) {
1247 			DPRINTF(DP_DROPS, "Wrong MTU %d in ICMPv6 type %d,"
1248 			    " code %d", mtu, icmp6->icmp6_type,
1249 			    icmp6->icmp6_code);
1250 			NAT64STAT_INC(stats, dropped);
1251 			return (NAT64MFREE);
1252 		}
1253 		/*
1254 		 * Adjust MTU to reflect difference between
1255 		 * IPv6 an IPv4 headers.
1256 		 */
1257 		mtu -= sizeof(struct ip6_hdr) - sizeof(struct ip);
1258 		break;
1259 	case ICMP6_TIME_EXCEEDED:
1260 		type = ICMP_TIMXCEED;
1261 		code = icmp6->icmp6_code;
1262 		break;
1263 	case ICMP6_PARAM_PROB:
1264 		switch (icmp6->icmp6_code) {
1265 		case ICMP6_PARAMPROB_HEADER:
1266 			type = ICMP_PARAMPROB;
1267 			code = ICMP_PARAMPROB_ERRATPTR;
1268 			mtu = ntohl(icmp6->icmp6_pptr);
1269 			switch (mtu) {
1270 			case 0: /* Version/Traffic Class */
1271 			case 1: /* Traffic Class/Flow Label */
1272 				break;
1273 			case 4: /* Payload Length */
1274 			case 5:
1275 				mtu = 2;
1276 				break;
1277 			case 6: /* Next Header */
1278 				mtu = 9;
1279 				break;
1280 			case 7: /* Hop Limit */
1281 				mtu = 8;
1282 				break;
1283 			default:
1284 				if (mtu >= 8 && mtu <= 23) {
1285 					mtu = 12; /* Source address */
1286 					break;
1287 				}
1288 				if (mtu >= 24 && mtu <= 39) {
1289 					mtu = 16; /* Destination address */
1290 					break;
1291 				}
1292 				DPRINTF(DP_DROPS, "Unsupported ICMPv6 type %d,"
1293 				    " code %d, pptr %d", icmp6->icmp6_type,
1294 				    icmp6->icmp6_code, mtu);
1295 				NAT64STAT_INC(stats, dropped);
1296 				return (NAT64MFREE);
1297 			}
1298 		case ICMP6_PARAMPROB_NEXTHEADER:
1299 			type = ICMP_UNREACH;
1300 			code = ICMP_UNREACH_PROTOCOL;
1301 			break;
1302 		default:
1303 			DPRINTF(DP_DROPS, "Unsupported ICMPv6 type %d,"
1304 			    " code %d, pptr %d", icmp6->icmp6_type,
1305 			    icmp6->icmp6_code, ntohl(icmp6->icmp6_pptr));
1306 			NAT64STAT_INC(stats, dropped);
1307 			return (NAT64MFREE);
1308 		}
1309 		break;
1310 	default:
1311 		DPRINTF(DP_DROPS, "Unsupported ICMPv6 type %d, code %d",
1312 		    icmp6->icmp6_type, icmp6->icmp6_code);
1313 		NAT64STAT_INC(stats, dropped);
1314 		return (NAT64MFREE);
1315 	}
1316 
1317 	hlen += sizeof(struct icmp6_hdr);
1318 	if (m->m_pkthdr.len < hlen + sizeof(struct ip6_hdr) + ICMP_MINLEN) {
1319 		NAT64STAT_INC(stats, dropped);
1320 		DPRINTF(DP_DROPS, "Message is too short %d",
1321 		    m->m_pkthdr.len);
1322 		return (NAT64MFREE);
1323 	}
1324 	/*
1325 	 * We need at least ICMP_MINLEN bytes of original datagram payload
1326 	 * to generate ICMP message. It is nice that ICMP_MINLEN is equal
1327 	 * to sizeof(struct ip6_frag). So, if embedded datagram had a fragment
1328 	 * header we will not have to do m_pullup() again.
1329 	 *
1330 	 * What we have here:
1331 	 * Outer header: (IPv6iGW, v4mapPRefix+v4exthost)
1332 	 * Inner header: (v4mapPRefix+v4host, IPv6iHost) [sport, dport]
1333 	 * We need to translate it to:
1334 	 *
1335 	 * Outer header: (alias_host, v4exthost)
1336 	 * Inner header: (v4exthost, alias_host) [sport, alias_port]
1337 	 *
1338 	 * Assume caller function has checked if v4mapPRefix+v4host
1339 	 * matches configured prefix.
1340 	 * The only two things we should be provided with are mapping between
1341 	 * IPv6iHost <> alias_host and between dport and alias_port.
1342 	 */
1343 	if (m->m_len < hlen + sizeof(struct ip6_hdr) + ICMP_MINLEN)
1344 		m = m_pullup(m, hlen + sizeof(struct ip6_hdr) + ICMP_MINLEN);
1345 	if (m == NULL) {
1346 		NAT64STAT_INC(stats, nomem);
1347 		return (NAT64RETURN);
1348 	}
1349 	ip6 = mtod(m, struct ip6_hdr *);
1350 	ip6i = mtodo(m, hlen);
1351 	ip6f = NULL;
1352 	proto = ip6i->ip6_nxt;
1353 	plen = ntohs(ip6i->ip6_plen);
1354 	hlen += sizeof(struct ip6_hdr);
1355 	if (proto == IPPROTO_FRAGMENT) {
1356 		if (m->m_pkthdr.len < hlen + sizeof(struct ip6_frag) +
1357 		    ICMP_MINLEN)
1358 			goto fail;
1359 		ip6f = mtodo(m, hlen);
1360 		proto = ip6f->ip6f_nxt;
1361 		plen -= sizeof(struct ip6_frag);
1362 		hlen += sizeof(struct ip6_frag);
1363 		/* Ajust MTU to reflect frag header size */
1364 		if (type == ICMP_UNREACH && code == ICMP_UNREACH_NEEDFRAG)
1365 			mtu -= sizeof(struct ip6_frag);
1366 	}
1367 	if (proto != IPPROTO_TCP && proto != IPPROTO_UDP) {
1368 		DPRINTF(DP_DROPS, "Unsupported proto %d in the inner header",
1369 		    proto);
1370 		goto fail;
1371 	}
1372 	if (nat64_check_ip6(&ip6i->ip6_src) != 0 ||
1373 	    nat64_check_ip6(&ip6i->ip6_dst) != 0) {
1374 		DPRINTF(DP_DROPS, "Inner addresses do not passes the check");
1375 		goto fail;
1376 	}
1377 	/* Check if outer dst is the same as inner src */
1378 	if (!IN6_ARE_ADDR_EQUAL(&ip6->ip6_dst, &ip6i->ip6_src)) {
1379 		DPRINTF(DP_DROPS, "Inner src doesn't match outer dst");
1380 		goto fail;
1381 	}
1382 
1383 	/* Now we need to make a fake IPv4 packet to generate ICMP message */
1384 	ip.ip_dst.s_addr = aaddr;
1385 	ip.ip_src.s_addr = nat64_get_ip4(&ip6i->ip6_src);
1386 	/* XXX: Make fake ulp header */
1387 #ifdef IPFIREWALL_NAT64_DIRECT_OUTPUT
1388 	ip6i->ip6_hlim += IPV6_HLIMDEC; /* init_ip4hdr will decrement it */
1389 #endif
1390 	nat64_init_ip4hdr(ip6i, ip6f, plen, proto, &ip);
1391 	m_adj(m, hlen - sizeof(struct ip));
1392 	bcopy(&ip, mtod(m, void *), sizeof(ip));
1393 	nat64_icmp_reflect(m, type, code, (uint16_t)mtu, stats, logdata);
1394 	return (NAT64RETURN);
1395 fail:
1396 	/*
1397 	 * We must call m_freem() because mbuf pointer could be
1398 	 * changed with m_pullup().
1399 	 */
1400 	m_freem(m);
1401 	NAT64STAT_INC(stats, dropped);
1402 	return (NAT64RETURN);
1403 }
1404 
1405 int
1406 nat64_do_handle_ip6(struct mbuf *m, uint32_t aaddr, uint16_t aport,
1407     nat64_stats_block *stats, void *logdata)
1408 {
1409 	struct route ro;
1410 	struct ip ip;
1411 	struct ifnet *ifp;
1412 	struct ip6_frag *frag;
1413 	struct ip6_hdr *ip6;
1414 	struct icmp6_hdr *icmp6;
1415 	struct sockaddr *dst;
1416 	uint16_t *csum;
1417 	uint32_t mtu;
1418 	int plen, hlen, proto;
1419 
1420 	/*
1421 	 * XXX: we expect ipfw_chk() did m_pullup() up to upper level
1422 	 * protocol's headers. Also we skip some checks, that ip6_input(),
1423 	 * ip6_forward(), ip6_fastfwd() and ipfw_chk() already did.
1424 	 */
1425 	ip6 = mtod(m, struct ip6_hdr *);
1426 	if (nat64_check_ip6(&ip6->ip6_src) != 0 ||
1427 	    nat64_check_ip6(&ip6->ip6_dst) != 0) {
1428 		return (NAT64SKIP);
1429 	}
1430 
1431 	/* Starting from this point we must not return zero */
1432 	ip.ip_src.s_addr = aaddr;
1433 	if (nat64_check_ip4(ip.ip_src.s_addr) != 0) {
1434 		DPRINTF(DP_GENERIC, "invalid source address: %08x",
1435 		    ip.ip_src.s_addr);
1436 		/* XXX: stats? */
1437 		return (NAT64MFREE);
1438 	}
1439 
1440 	ip.ip_dst.s_addr = nat64_get_ip4(&ip6->ip6_dst);
1441 	if (ip.ip_dst.s_addr == 0) {
1442 		/* XXX: stats? */
1443 		return (NAT64MFREE);
1444 	}
1445 
1446 	if (ip6->ip6_hlim <= IPV6_HLIMDEC) {
1447 		nat64_icmp6_reflect(m, ICMP6_TIME_EXCEEDED,
1448 		    ICMP6_TIME_EXCEED_TRANSIT, 0, stats, logdata);
1449 		return (NAT64RETURN);
1450 	}
1451 
1452 	hlen = 0;
1453 	plen = ntohs(ip6->ip6_plen);
1454 	proto = nat64_getlasthdr(m, &hlen);
1455 	if (proto < 0) {
1456 		DPRINTF(DP_DROPS, "dropped due to mbuf isn't contigious");
1457 		NAT64STAT_INC(stats, dropped);
1458 		return (NAT64MFREE);
1459 	}
1460 	frag = NULL;
1461 	if (proto == IPPROTO_FRAGMENT) {
1462 		/* ipfw_chk should m_pullup up to frag header */
1463 		if (m->m_len < hlen + sizeof(*frag)) {
1464 			DPRINTF(DP_DROPS,
1465 			    "dropped due to mbuf isn't contigious");
1466 			NAT64STAT_INC(stats, dropped);
1467 			return (NAT64MFREE);
1468 		}
1469 		frag = mtodo(m, hlen);
1470 		proto = frag->ip6f_nxt;
1471 		hlen += sizeof(*frag);
1472 		/* Fragmented ICMPv6 is unsupported */
1473 		if (proto == IPPROTO_ICMPV6) {
1474 			DPRINTF(DP_DROPS, "dropped due to fragmented ICMPv6");
1475 			NAT64STAT_INC(stats, dropped);
1476 			return (NAT64MFREE);
1477 		}
1478 		/* Fragment length must be multiple of 8 octets */
1479 		if ((frag->ip6f_offlg & IP6F_MORE_FRAG) != 0 &&
1480 		    ((plen + sizeof(struct ip6_hdr) - hlen) & 0x7) != 0) {
1481 			nat64_icmp6_reflect(m, ICMP6_PARAM_PROB,
1482 			    ICMP6_PARAMPROB_HEADER,
1483 			    offsetof(struct ip6_hdr, ip6_plen), stats,
1484 			    logdata);
1485 			return (NAT64RETURN);
1486 		}
1487 	}
1488 	plen -= hlen - sizeof(struct ip6_hdr);
1489 	if (plen < 0 || m->m_pkthdr.len < plen + hlen) {
1490 		DPRINTF(DP_DROPS, "plen %d, pkthdr.len %d, hlen %d",
1491 		    plen, m->m_pkthdr.len, hlen);
1492 		NAT64STAT_INC(stats, dropped);
1493 		return (NAT64MFREE);
1494 	}
1495 
1496 	icmp6 = NULL;	/* Make gcc happy */
1497 	if (proto == IPPROTO_ICMPV6) {
1498 		icmp6 = mtodo(m, hlen);
1499 		if (icmp6->icmp6_type != ICMP6_ECHO_REQUEST &&
1500 		    icmp6->icmp6_type != ICMP6_ECHO_REPLY)
1501 			return (nat64_handle_icmp6(m, hlen, aaddr, aport,
1502 			    stats, logdata));
1503 	}
1504 	dst = nat64_find_route4(&ro, ip.ip_dst.s_addr, m);
1505 	if (dst == NULL) {
1506 		FREE_ROUTE(&ro);
1507 		NAT64STAT_INC(stats, noroute4);
1508 		nat64_icmp6_reflect(m, ICMP6_DST_UNREACH,
1509 		    ICMP6_DST_UNREACH_NOROUTE, 0, stats, logdata);
1510 		return (NAT64RETURN);
1511 	}
1512 
1513 	ifp = ro.ro_rt->rt_ifp;
1514 	if (ro.ro_rt->rt_mtu != 0)
1515 		mtu = min(ro.ro_rt->rt_mtu, ifp->if_mtu);
1516 	else
1517 		mtu = ifp->if_mtu;
1518 	if (mtu < plen + sizeof(ip)) {
1519 		FREE_ROUTE(&ro);
1520 		nat64_icmp6_reflect(m, ICMP6_PACKET_TOO_BIG, 0, mtu, stats,
1521 		    logdata);
1522 		return (NAT64RETURN);
1523 	}
1524 	nat64_init_ip4hdr(ip6, frag, plen, proto, &ip);
1525 	/* Convert checksums. */
1526 	switch (proto) {
1527 	case IPPROTO_TCP:
1528 		csum = &TCP(mtodo(m, hlen))->th_sum;
1529 		if (aport != 0) {
1530 			struct tcphdr *tcp = TCP(mtodo(m, hlen));
1531 			*csum = cksum_adjust(*csum, tcp->th_sport, aport);
1532 			tcp->th_sport = aport;
1533 		}
1534 		*csum = cksum_add(*csum, nat64_cksum_convert(ip6, &ip));
1535 		break;
1536 	case IPPROTO_UDP:
1537 		csum = &UDP(mtodo(m, hlen))->uh_sum;
1538 		if (aport != 0) {
1539 			struct udphdr *udp = UDP(mtodo(m, hlen));
1540 			*csum = cksum_adjust(*csum, udp->uh_sport, aport);
1541 			udp->uh_sport = aport;
1542 		}
1543 		*csum = cksum_add(*csum, nat64_cksum_convert(ip6, &ip));
1544 		break;
1545 	case IPPROTO_ICMPV6:
1546 		/* Checksum in ICMPv6 covers pseudo header */
1547 		csum = &icmp6->icmp6_cksum;
1548 		*csum = cksum_add(*csum, in6_cksum_pseudo(ip6, plen,
1549 		    IPPROTO_ICMPV6, 0));
1550 		/* Convert ICMPv6 types to ICMP */
1551 		mtu = *(uint16_t *)icmp6; /* save old word for cksum_adjust */
1552 		if (icmp6->icmp6_type == ICMP6_ECHO_REQUEST)
1553 			icmp6->icmp6_type = ICMP_ECHO;
1554 		else /* ICMP6_ECHO_REPLY */
1555 			icmp6->icmp6_type = ICMP_ECHOREPLY;
1556 		*csum = cksum_adjust(*csum, (uint16_t)mtu, *(uint16_t *)icmp6);
1557 		if (aport != 0) {
1558 			uint16_t old_id = icmp6->icmp6_id;
1559 			icmp6->icmp6_id = aport;
1560 			*csum = cksum_adjust(*csum, old_id, aport);
1561 		}
1562 		break;
1563 	};
1564 
1565 	m_adj(m, hlen - sizeof(ip));
1566 	bcopy(&ip, mtod(m, void *), sizeof(ip));
1567 	if (nat64_output(ifp, m, dst, &ro, stats, logdata) == 0)
1568 		NAT64STAT_INC(stats, opcnt64);
1569 	FREE_ROUTE(&ro);
1570 	return (NAT64RETURN);
1571 }
1572 
1573