1/*	$OpenBSD: inet_nat64.c,v 1.2 2015/03/14 03:38:51 jsg Exp $	*/
2/*	$vantronix: inet_nat64.c,v 1.2 2011/02/28 14:57:58 mike Exp $	*/
3
4/*
5 * Copyright (c) 2011 Reyk Floeter <reyk@vantronix.net>
6 *
7 * Permission to use, copy, modify, and distribute this software for any
8 * purpose with or without fee is hereby granted, provided that the above
9 * copyright notice and this permission notice appear in all copies.
10 *
11 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
12 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
13 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
14 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
15 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
16 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
17 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
18 */
19
20#include <sys/param.h>
21#include <sys/socket.h>
22#ifdef _KERNEL
23#include <sys/mbuf.h>
24#else
25#include <errno.h>
26#endif
27
28union inet_nat64_addr {
29	u_int32_t	 u32[4];
30	u_int8_t	 u8[16];
31};
32
33u_int32_t inet_nat64_mask(u_int32_t, u_int32_t, u_int8_t);
34
35int	  inet_nat64(int, const void *, void *, const void *, u_int8_t);
36int	  inet_nat64_inet(const void *, void *, const void *, u_int8_t);
37int	  inet_nat64_inet6(const void *, void *, const void *, u_int8_t);
38
39int	  inet_nat46(int, const void *, void *, const void *, u_int8_t);
40int	  inet_nat46_inet(const void *, void *, const void *, u_int8_t);
41int	  inet_nat46_inet6(const void *, void *, const void *, u_int8_t);
42
43u_int32_t
44inet_nat64_mask(u_int32_t src, u_int32_t pfx, u_int8_t pfxlen)
45{
46	u_int32_t	u32;
47	if (pfxlen == 0)
48		return (src);
49	else if (pfxlen > 32)
50		pfxlen = 32;
51	u32 =
52	    (src & ~htonl(0xffffffff << (32 - pfxlen))) |
53	    (pfx & htonl(0xffffffff << (32 - pfxlen)));
54	return (u32);
55
56}
57
58int
59inet_nat64(int af, const void *src, void *dst,
60    const void *pfx, u_int8_t pfxlen)
61{
62	switch (af) {
63	case AF_INET:
64		return (inet_nat64_inet(src, dst, pfx, pfxlen));
65	case AF_INET6:
66		return (inet_nat64_inet6(src, dst, pfx, pfxlen));
67	default:
68#ifndef _KERNEL
69		errno = EAFNOSUPPORT;
70#endif
71		return (-1);
72	}
73	/* NOTREACHED */
74}
75
76int
77inet_nat64_inet(const void *src, void *dst, const void *pfx, u_int8_t pfxlen)
78{
79	const union inet_nat64_addr	*s = src;
80	const union inet_nat64_addr	*p = pfx;
81	union inet_nat64_addr		*d = dst;
82	int				 i, j;
83
84	switch (pfxlen) {
85	case 32:
86	case 40:
87	case 48:
88	case 56:
89	case 64:
90	case 96:
91		i = pfxlen / 8;
92		break;
93	default:
94		if (pfxlen < 96 || pfxlen > 128) {
95#ifndef _KERNEL
96			errno = EINVAL;
97#endif
98			return (-1);
99		}
100
101		/* as an extension, mask out any other bits */
102		d->u32[0] = inet_nat64_mask(s->u32[3], p->u32[3],
103		    (u_int8_t)(32 - (128 - pfxlen)));
104		return (0);
105	}
106
107	/* fill the octets with the source and skip reserved octet 8 */
108	for (j = 0; j < 4; j++) {
109		if (i == 8)
110			i++;
111		d->u8[j] = s->u8[i++];
112	}
113
114	return (0);
115}
116
117int
118inet_nat64_inet6(const void *src, void *dst, const void *pfx, u_int8_t pfxlen)
119{
120	const union inet_nat64_addr	*s = src;
121	const union inet_nat64_addr	*p = pfx;
122	union inet_nat64_addr		*d = dst;
123	int				 i, j;
124
125	/* first copy the prefix octets to the destination */
126	*d = *p;
127
128	switch (pfxlen) {
129	case 32:
130	case 40:
131	case 48:
132	case 56:
133	case 64:
134	case 96:
135		i = pfxlen / 8;
136		break;
137	default:
138		if (pfxlen < 96 || pfxlen > 128) {
139#ifndef _KERNEL
140			errno = EINVAL;
141#endif
142			return (-1);
143		}
144
145		/* as an extension, mask out any other bits */
146		d->u32[3] = inet_nat64_mask(s->u32[0], p->u32[3],
147		    (u_int8_t)(32 - (128 - pfxlen)));
148		return (0);
149	}
150
151	/* octet 8 is reserved and must be set to zero */
152	d->u8[8] = 0;
153
154	/* fill the other octets with the source and skip octet 8 */
155	for (j = 0; j < 4; j++) {
156		if (i == 8)
157			i++;
158		d->u8[i++] = s->u8[j];
159	}
160
161	return (0);
162}
163
164int
165inet_nat46(int af, const void *src, void *dst,
166    const void *pfx, u_int8_t pfxlen)
167{
168	if (pfxlen > 32) {
169#ifndef _KERNEL
170		errno = EINVAL;
171#endif
172		return (-1);
173	}
174
175	switch (af) {
176	case AF_INET:
177		return (inet_nat46_inet(src, dst, pfx, pfxlen));
178	case AF_INET6:
179		return (inet_nat46_inet6(src, dst, pfx, pfxlen));
180	default:
181#ifndef _KERNEL
182		errno = EAFNOSUPPORT;
183#endif
184		return (-1);
185	}
186	/* NOTREACHED */
187}
188
189int
190inet_nat46_inet(const void *src, void *dst, const void *pfx, u_int8_t pfxlen)
191{
192	const union inet_nat64_addr	*s = src;
193	const union inet_nat64_addr	*p = pfx;
194	union inet_nat64_addr		*d = dst;
195
196	/* set the remaining bits to the source */
197	d->u32[0] = inet_nat64_mask(s->u32[3], p->u32[0], pfxlen);
198
199	return (0);
200}
201
202int
203inet_nat46_inet6(const void *src, void *dst, const void *pfx, u_int8_t pfxlen)
204{
205	const union inet_nat64_addr	*s = src;
206	const union inet_nat64_addr	*p = pfx;
207	union inet_nat64_addr		*d = dst;
208
209	/* set the initial octets to zero */
210	d->u32[0] = d->u32[1] = d->u32[2] = 0;
211
212	/* now set the remaining bits to the source */
213	d->u32[3] = inet_nat64_mask(s->u32[0], p->u32[0], pfxlen);
214
215	return (0);
216}
217