2/* $KAME: ip_encap.c,v 1.41 2001/03/15 08:35:08 itojun Exp $ */ 3 4/* 5 * Copyright (C) 1995, 1996, 1997, and 1998 WIDE Project. 6 * All rights reserved. 7 * 8 * Redistribution and use in source and binary forms, with or without 9 * modification, are permitted provided that the following conditions 10 * are met: 11 * 1. Redistributions of source code must retain the above copyright 12 * notice, this list of conditions and the following disclaimer. 13 * 2. Redistributions in binary form must reproduce the above copyright 14 * notice, this list of conditions and the following disclaimer in the 15 * documentation and/or other materials provided with the distribution. 16 * 3. Neither the name of the project nor the names of its contributors 17 * may be used to endorse or promote products derived from this software 18 * without specific prior written permission. 19 * 20 * THIS SOFTWARE IS PROVIDED BY THE PROJECT AND CONTRIBUTORS ``AS IS'' AND 21 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23 * ARE DISCLAIMED. IN NO EVENT SHALL THE PROJECT OR CONTRIBUTORS BE LIABLE 24 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 26 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 27 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 29 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 30 * SUCH DAMAGE. 31 */ 32/* 33 * My grandfather said that there's a devil inside tunnelling technology... 34 * 35 * We have surprisingly many protocols that want packets with IP protocol 36 * #4 or #41. Here's a list of protocols that want protocol #41: 37 * RFC1933 configured tunnel 38 * RFC1933 automatic tunnel 39 * RFC2401 IPsec tunnel 40 * RFC2473 IPv6 generic packet tunnelling 41 * RFC2529 6over4 tunnel 42 * mobile-ip6 (uses RFC2473) 43 * RFC3056 6to4 tunnel 44 * isatap tunnel 45 * Here's a list of protocol that want protocol #4: 46 * RFC1853 IPv4-in-IPv4 tunnelling 47 * RFC2003 IPv4 encapsulation within IPv4 48 * RFC2344 reverse tunnelling for mobile-ip4 49 * RFC2401 IPsec tunnel 50 * Well, what can I say. They impose different en/decapsulation mechanism 51 * from each other, so they need separate protocol handler. The only one 52 * we can easily determine by protocol # is IPsec, which always has 53 * AH/ESP/IPComp header right after outer IP header. 54 * 55 * So, clearly good old protosw does not work for protocol #4 and #41. 56 * The code will let you match protocol via src/dst address pair. 57 */ 58/* XXX is M_NETADDR correct? */ 59 60#include "opt_mrouting.h" 61#include "opt_inet.h" 62#include "opt_inet6.h" 63 64#include <sys/param.h> 65#include <sys/systm.h> 66#include <sys/socket.h> 67#include <sys/sockio.h> 68#include <sys/mbuf.h> 69#include <sys/errno.h> 70#include <sys/protosw.h> 71#include <sys/queue.h> 72 73#include <net/if.h> 74#include <net/route.h> 75 76#include <netinet/in.h> 77#include <netinet/in_systm.h> 78#include <netinet/ip.h> 79#include <netinet/ip_var.h> 80#include <netinet/ip_encap.h> 81 82#ifdef INET6 83#include <netinet/ip6.h> 84#include <netinet6/ip6_var.h> 85#include <netinet6/ip6protosw.h> 86#endif 87 88#include <machine/stdarg.h> 89 90#include <net/net_osdep.h> 91 92#include <sys/kernel.h> 93#include <sys/malloc.h> 94static MALLOC_DEFINE(M_NETADDR, "Export Host", "Export host address structure"); 95 96static void encap_add(struct encaptab *); 97static int mask_match(const struct encaptab *, const struct sockaddr *, 98 const struct sockaddr *); 99static void encap_fillarg(struct mbuf *, const struct encaptab *); 100 101#ifndef LIST_HEAD_INITIALIZER 102/* rely upon BSS initialization */ 103LIST_HEAD(, encaptab) encaptab; 104#else 105LIST_HEAD(, encaptab) encaptab = LIST_HEAD_INITIALIZER(&encaptab); 106#endif 107 108void 109encap_init() 110{ 111 static int initialized = 0; 112 113 if (initialized) 114 return; 115 initialized++; 116#if 0 117 /* 118 * we cannot use LIST_INIT() here, since drivers may want to call 119 * encap_attach(), on driver attach. encap_init() will be called 120 * on AF_INET{,6} initialization, which happens after driver 121 * initialization - using LIST_INIT() here can nuke encap_attach() 122 * from drivers. 123 */ 124 LIST_INIT(&encaptab); 125#endif 126} 127 128#ifdef INET 129void 130encap4_input(m, off) 131 struct mbuf *m; 132 int off; 133{ 134 struct ip *ip; 135 int proto; 136 struct sockaddr_in s, d; 137 const struct protosw *psw; 138 struct encaptab *ep, *match; 139 int prio, matchprio; 140 141 ip = mtod(m, struct ip *); 142 proto = ip->ip_p; 143 144 bzero(&s, sizeof(s)); 145 s.sin_family = AF_INET; 146 s.sin_len = sizeof(struct sockaddr_in); 147 s.sin_addr = ip->ip_src; 148 bzero(&d, sizeof(d)); 149 d.sin_family = AF_INET; 150 d.sin_len = sizeof(struct sockaddr_in); 151 d.sin_addr = ip->ip_dst; 152 153 match = NULL; 154 matchprio = 0; 155 LIST_FOREACH(ep, &encaptab, chain) { 156 if (ep->af != AF_INET) 157 continue; 158 if (ep->proto >= 0 && ep->proto != proto) 159 continue; 160 if (ep->func) 161 prio = (*ep->func)(m, off, proto, ep->arg); 162 else { 163 /* 164 * it's inbound traffic, we need to match in reverse 165 * order 166 */ 167 prio = mask_match(ep, (struct sockaddr *)&d, 168 (struct sockaddr *)&s); 169 } 170 171 /* 172 * We prioritize the matches by using bit length of the 173 * matches. mask_match() and user-supplied matching function 174 * should return the bit length of the matches (for example, 175 * if both src/dst are matched for IPv4, 64 should be returned). 176 * 0 or negative return value means "it did not match". 177 * 178 * The question is, since we have two "mask" portion, we 179 * cannot really define total order between entries. 180 * For example, which of these should be preferred? 181 * mask_match() returns 48 (32 + 16) for both of them. 182 * src=3ffe::/16, dst=3ffe:501::/32 183 * src=3ffe:501::/32, dst=3ffe::/16 184 * 185 * We need to loop through all the possible candidates 186 * to get the best match - the search takes O(n) for 187 * n attachments (i.e. interfaces). 188 */ 189 if (prio <= 0) 190 continue; 191 if (prio > matchprio) { 192 matchprio = prio; 193 match = ep; 194 } 195 } 196 197 if (match) { 198 /* found a match, "match" has the best one */ 199 psw = match->psw; 200 if (psw && psw->pr_input) { 201 encap_fillarg(m, match); 202 (*psw->pr_input)(m, off); 203 } else 204 m_freem(m); 205 return; 206 } 207 208 /* last resort: inject to raw socket */ 209 rip_input(m, off); 210} 211#endif 212 213#ifdef INET6 214int 215encap6_input(mp, offp, proto) 216 struct mbuf **mp; 217 int *offp; 218 int proto; 219{ 220 struct mbuf *m = *mp; 221 struct ip6_hdr *ip6; 222 struct sockaddr_in6 s, d; 223 const struct ip6protosw *psw; 224 struct encaptab *ep, *match; 225 int prio, matchprio; 226 227 ip6 = mtod(m, struct ip6_hdr *); 228 229 bzero(&s, sizeof(s)); 230 s.sin6_family = AF_INET6; 231 s.sin6_len = sizeof(struct sockaddr_in6); 232 s.sin6_addr = ip6->ip6_src; 233 bzero(&d, sizeof(d)); 234 d.sin6_family = AF_INET6; 235 d.sin6_len = sizeof(struct sockaddr_in6); 236 d.sin6_addr = ip6->ip6_dst; 237 238 match = NULL; 239 matchprio = 0; 240 LIST_FOREACH(ep, &encaptab, chain) { 241 if (ep->af != AF_INET6) 242 continue; 243 if (ep->proto >= 0 && ep->proto != proto) 244 continue; 245 if (ep->func) 246 prio = (*ep->func)(m, *offp, proto, ep->arg); 247 else { 248 /* 249 * it's inbound traffic, we need to match in reverse 250 * order 251 */ 252 prio = mask_match(ep, (struct sockaddr *)&d, 253 (struct sockaddr *)&s); 254 } 255 256 /* see encap4_input() for issues here */ 257 if (prio <= 0) 258 continue; 259 if (prio > matchprio) { 260 matchprio = prio; 261 match = ep; 262 } 263 } 264 265 if (match) { 266 /* found a match */ 267 psw = (const struct ip6protosw *)match->psw; 268 if (psw && psw->pr_input) { 269 encap_fillarg(m, match); 270 return (*psw->pr_input)(mp, offp, proto); 271 } else { 272 m_freem(m); 273 return IPPROTO_DONE; 274 } 275 } 276 277 /* last resort: inject to raw socket */ 278 return rip6_input(mp, offp, proto); 279} 280#endif 281 282static void 283encap_add(ep) 284 struct encaptab *ep; 285{ 286 287 LIST_INSERT_HEAD(&encaptab, ep, chain); 288} 289 290/* 291 * sp (src ptr) is always my side, and dp (dst ptr) is always remote side. 292 * length of mask (sm and dm) is assumed to be same as sp/dp. 293 * Return value will be necessary as input (cookie) for encap_detach(). 294 */ 295const struct encaptab * 296encap_attach(af, proto, sp, sm, dp, dm, psw, arg) 297 int af; 298 int proto; 299 const struct sockaddr *sp, *sm; 300 const struct sockaddr *dp, *dm; 301 const struct protosw *psw; 302 void *arg; 303{ 304 struct encaptab *ep; 305 int error; 306 int s; 307 308 s = splnet(); 309 /* sanity check on args */ 310 if (sp->sa_len > sizeof(ep->src) || dp->sa_len > sizeof(ep->dst)) { 311 error = EINVAL; 312 goto fail; 313 } 314 if (sp->sa_len != dp->sa_len) { 315 error = EINVAL; 316 goto fail; 317 } 318 if (af != sp->sa_family || af != dp->sa_family) { 319 error = EINVAL; 320 goto fail; 321 } 322 323 /* check if anyone have already attached with exactly same config */ 324 LIST_FOREACH(ep, &encaptab, chain) { 325 if (ep->af != af) 326 continue; 327 if (ep->proto != proto) 328 continue; 329 if (ep->src.ss_len != sp->sa_len || 330 bcmp(&ep->src, sp, sp->sa_len) != 0 || 331 bcmp(&ep->srcmask, sm, sp->sa_len) != 0) 332 continue; 333 if (ep->dst.ss_len != dp->sa_len || 334 bcmp(&ep->dst, dp, dp->sa_len) != 0 || 335 bcmp(&ep->dstmask, dm, dp->sa_len) != 0) 336 continue; 337 338 error = EEXIST; 339 goto fail; 340 } 341 342 ep = malloc(sizeof(*ep), M_NETADDR, M_NOWAIT); /*XXX*/ 343 if (ep == NULL) { 344 error = ENOBUFS; 345 goto fail; 346 } 347 bzero(ep, sizeof(*ep)); 348 349 ep->af = af; 350 ep->proto = proto; 351 bcopy(sp, &ep->src, sp->sa_len); 352 bcopy(sm, &ep->srcmask, sp->sa_len); 353 bcopy(dp, &ep->dst, dp->sa_len); 354 bcopy(dm, &ep->dstmask, dp->sa_len); 355 ep->psw = psw; 356 ep->arg = arg; 357 358 encap_add(ep); 359 360 error = 0; 361 splx(s); 362 return ep; 363 364fail: 365 splx(s); 366 return NULL; 367} 368 369const struct encaptab * 370encap_attach_func(af, proto, func, psw, arg) 371 int af; 372 int proto; 373 int (*func)(const struct mbuf *, int, int, void *); 374 const struct protosw *psw; 375 void *arg; 376{ 377 struct encaptab *ep; 378 int error; 379 int s; 380 381 s = splnet(); 382 /* sanity check on args */ 383 if (!func) { 384 error = EINVAL; 385 goto fail; 386 } 387 388 ep = malloc(sizeof(*ep), M_NETADDR, M_NOWAIT); /*XXX*/ 389 if (ep == NULL) { 390 error = ENOBUFS; 391 goto fail; 392 } 393 bzero(ep, sizeof(*ep)); 394 395 ep->af = af; 396 ep->proto = proto; 397 ep->func = func; 398 ep->psw = psw; 399 ep->arg = arg; 400 401 encap_add(ep); 402 403 error = 0; 404 splx(s); 405 return ep; 406 407fail: 408 splx(s); 409 return NULL; 410} 411 412int 413encap_detach(cookie) 414 const struct encaptab *cookie; 415{ 416 const struct encaptab *ep = cookie; 417 struct encaptab *p; 418 419 LIST_FOREACH(p, &encaptab, chain) { 420 if (p == ep) { 421 LIST_REMOVE(p, chain); 422 free(p, M_NETADDR); /*XXX*/ 423 return 0; 424 } 425 } 426 427 return EINVAL; 428} 429 430static int 431mask_match(ep, sp, dp) 432 const struct encaptab *ep; 433 const struct sockaddr *sp; 434 const struct sockaddr *dp; 435{ 436 struct sockaddr_storage s; 437 struct sockaddr_storage d; 438 int i; 439 const u_int8_t *p, *q; 440 u_int8_t *r; 441 int matchlen; 442 443 if (sp->sa_len > sizeof(s) || dp->sa_len > sizeof(d)) 444 return 0; 445 if (sp->sa_family != ep->af || dp->sa_family != ep->af) 446 return 0; 447 if (sp->sa_len != ep->src.ss_len || dp->sa_len != ep->dst.ss_len) 448 return 0; 449 450 matchlen = 0; 451 452 p = (const u_int8_t *)sp; 453 q = (const u_int8_t *)&ep->srcmask; 454 r = (u_int8_t *)&s; 455 for (i = 0 ; i < sp->sa_len; i++) { 456 r[i] = p[i] & q[i]; 457 /* XXX estimate */ 458 matchlen += (q[i] ? 8 : 0); 459 } 460 461 p = (const u_int8_t *)dp; 462 q = (const u_int8_t *)&ep->dstmask; 463 r = (u_int8_t *)&d; 464 for (i = 0 ; i < dp->sa_len; i++) { 465 r[i] = p[i] & q[i]; 466 /* XXX rough estimate */ 467 matchlen += (q[i] ? 8 : 0); 468 } 469 470 /* need to overwrite len/family portion as we don't compare them */ 471 s.ss_len = sp->sa_len; 472 s.ss_family = sp->sa_family; 473 d.ss_len = dp->sa_len; 474 d.ss_family = dp->sa_family; 475 476 if (bcmp(&s, &ep->src, ep->src.ss_len) == 0 && 477 bcmp(&d, &ep->dst, ep->dst.ss_len) == 0) { 478 return matchlen; 479 } else 480 return 0; 481} 482 483static void 484encap_fillarg(m, ep) 485 struct mbuf *m; 486 const struct encaptab *ep; 487{
| 2/* $KAME: ip_encap.c,v 1.41 2001/03/15 08:35:08 itojun Exp $ */ 3 4/* 5 * Copyright (C) 1995, 1996, 1997, and 1998 WIDE Project. 6 * All rights reserved. 7 * 8 * Redistribution and use in source and binary forms, with or without 9 * modification, are permitted provided that the following conditions 10 * are met: 11 * 1. Redistributions of source code must retain the above copyright 12 * notice, this list of conditions and the following disclaimer. 13 * 2. Redistributions in binary form must reproduce the above copyright 14 * notice, this list of conditions and the following disclaimer in the 15 * documentation and/or other materials provided with the distribution. 16 * 3. Neither the name of the project nor the names of its contributors 17 * may be used to endorse or promote products derived from this software 18 * without specific prior written permission. 19 * 20 * THIS SOFTWARE IS PROVIDED BY THE PROJECT AND CONTRIBUTORS ``AS IS'' AND 21 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 23 * ARE DISCLAIMED. IN NO EVENT SHALL THE PROJECT OR CONTRIBUTORS BE LIABLE 24 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS 26 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 27 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY 29 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 30 * SUCH DAMAGE. 31 */ 32/* 33 * My grandfather said that there's a devil inside tunnelling technology... 34 * 35 * We have surprisingly many protocols that want packets with IP protocol 36 * #4 or #41. Here's a list of protocols that want protocol #41: 37 * RFC1933 configured tunnel 38 * RFC1933 automatic tunnel 39 * RFC2401 IPsec tunnel 40 * RFC2473 IPv6 generic packet tunnelling 41 * RFC2529 6over4 tunnel 42 * mobile-ip6 (uses RFC2473) 43 * RFC3056 6to4 tunnel 44 * isatap tunnel 45 * Here's a list of protocol that want protocol #4: 46 * RFC1853 IPv4-in-IPv4 tunnelling 47 * RFC2003 IPv4 encapsulation within IPv4 48 * RFC2344 reverse tunnelling for mobile-ip4 49 * RFC2401 IPsec tunnel 50 * Well, what can I say. They impose different en/decapsulation mechanism 51 * from each other, so they need separate protocol handler. The only one 52 * we can easily determine by protocol # is IPsec, which always has 53 * AH/ESP/IPComp header right after outer IP header. 54 * 55 * So, clearly good old protosw does not work for protocol #4 and #41. 56 * The code will let you match protocol via src/dst address pair. 57 */ 58/* XXX is M_NETADDR correct? */ 59 60#include "opt_mrouting.h" 61#include "opt_inet.h" 62#include "opt_inet6.h" 63 64#include <sys/param.h> 65#include <sys/systm.h> 66#include <sys/socket.h> 67#include <sys/sockio.h> 68#include <sys/mbuf.h> 69#include <sys/errno.h> 70#include <sys/protosw.h> 71#include <sys/queue.h> 72 73#include <net/if.h> 74#include <net/route.h> 75 76#include <netinet/in.h> 77#include <netinet/in_systm.h> 78#include <netinet/ip.h> 79#include <netinet/ip_var.h> 80#include <netinet/ip_encap.h> 81 82#ifdef INET6 83#include <netinet/ip6.h> 84#include <netinet6/ip6_var.h> 85#include <netinet6/ip6protosw.h> 86#endif 87 88#include <machine/stdarg.h> 89 90#include <net/net_osdep.h> 91 92#include <sys/kernel.h> 93#include <sys/malloc.h> 94static MALLOC_DEFINE(M_NETADDR, "Export Host", "Export host address structure"); 95 96static void encap_add(struct encaptab *); 97static int mask_match(const struct encaptab *, const struct sockaddr *, 98 const struct sockaddr *); 99static void encap_fillarg(struct mbuf *, const struct encaptab *); 100 101#ifndef LIST_HEAD_INITIALIZER 102/* rely upon BSS initialization */ 103LIST_HEAD(, encaptab) encaptab; 104#else 105LIST_HEAD(, encaptab) encaptab = LIST_HEAD_INITIALIZER(&encaptab); 106#endif 107 108void 109encap_init() 110{ 111 static int initialized = 0; 112 113 if (initialized) 114 return; 115 initialized++; 116#if 0 117 /* 118 * we cannot use LIST_INIT() here, since drivers may want to call 119 * encap_attach(), on driver attach. encap_init() will be called 120 * on AF_INET{,6} initialization, which happens after driver 121 * initialization - using LIST_INIT() here can nuke encap_attach() 122 * from drivers. 123 */ 124 LIST_INIT(&encaptab); 125#endif 126} 127 128#ifdef INET 129void 130encap4_input(m, off) 131 struct mbuf *m; 132 int off; 133{ 134 struct ip *ip; 135 int proto; 136 struct sockaddr_in s, d; 137 const struct protosw *psw; 138 struct encaptab *ep, *match; 139 int prio, matchprio; 140 141 ip = mtod(m, struct ip *); 142 proto = ip->ip_p; 143 144 bzero(&s, sizeof(s)); 145 s.sin_family = AF_INET; 146 s.sin_len = sizeof(struct sockaddr_in); 147 s.sin_addr = ip->ip_src; 148 bzero(&d, sizeof(d)); 149 d.sin_family = AF_INET; 150 d.sin_len = sizeof(struct sockaddr_in); 151 d.sin_addr = ip->ip_dst; 152 153 match = NULL; 154 matchprio = 0; 155 LIST_FOREACH(ep, &encaptab, chain) { 156 if (ep->af != AF_INET) 157 continue; 158 if (ep->proto >= 0 && ep->proto != proto) 159 continue; 160 if (ep->func) 161 prio = (*ep->func)(m, off, proto, ep->arg); 162 else { 163 /* 164 * it's inbound traffic, we need to match in reverse 165 * order 166 */ 167 prio = mask_match(ep, (struct sockaddr *)&d, 168 (struct sockaddr *)&s); 169 } 170 171 /* 172 * We prioritize the matches by using bit length of the 173 * matches. mask_match() and user-supplied matching function 174 * should return the bit length of the matches (for example, 175 * if both src/dst are matched for IPv4, 64 should be returned). 176 * 0 or negative return value means "it did not match". 177 * 178 * The question is, since we have two "mask" portion, we 179 * cannot really define total order between entries. 180 * For example, which of these should be preferred? 181 * mask_match() returns 48 (32 + 16) for both of them. 182 * src=3ffe::/16, dst=3ffe:501::/32 183 * src=3ffe:501::/32, dst=3ffe::/16 184 * 185 * We need to loop through all the possible candidates 186 * to get the best match - the search takes O(n) for 187 * n attachments (i.e. interfaces). 188 */ 189 if (prio <= 0) 190 continue; 191 if (prio > matchprio) { 192 matchprio = prio; 193 match = ep; 194 } 195 } 196 197 if (match) { 198 /* found a match, "match" has the best one */ 199 psw = match->psw; 200 if (psw && psw->pr_input) { 201 encap_fillarg(m, match); 202 (*psw->pr_input)(m, off); 203 } else 204 m_freem(m); 205 return; 206 } 207 208 /* last resort: inject to raw socket */ 209 rip_input(m, off); 210} 211#endif 212 213#ifdef INET6 214int 215encap6_input(mp, offp, proto) 216 struct mbuf **mp; 217 int *offp; 218 int proto; 219{ 220 struct mbuf *m = *mp; 221 struct ip6_hdr *ip6; 222 struct sockaddr_in6 s, d; 223 const struct ip6protosw *psw; 224 struct encaptab *ep, *match; 225 int prio, matchprio; 226 227 ip6 = mtod(m, struct ip6_hdr *); 228 229 bzero(&s, sizeof(s)); 230 s.sin6_family = AF_INET6; 231 s.sin6_len = sizeof(struct sockaddr_in6); 232 s.sin6_addr = ip6->ip6_src; 233 bzero(&d, sizeof(d)); 234 d.sin6_family = AF_INET6; 235 d.sin6_len = sizeof(struct sockaddr_in6); 236 d.sin6_addr = ip6->ip6_dst; 237 238 match = NULL; 239 matchprio = 0; 240 LIST_FOREACH(ep, &encaptab, chain) { 241 if (ep->af != AF_INET6) 242 continue; 243 if (ep->proto >= 0 && ep->proto != proto) 244 continue; 245 if (ep->func) 246 prio = (*ep->func)(m, *offp, proto, ep->arg); 247 else { 248 /* 249 * it's inbound traffic, we need to match in reverse 250 * order 251 */ 252 prio = mask_match(ep, (struct sockaddr *)&d, 253 (struct sockaddr *)&s); 254 } 255 256 /* see encap4_input() for issues here */ 257 if (prio <= 0) 258 continue; 259 if (prio > matchprio) { 260 matchprio = prio; 261 match = ep; 262 } 263 } 264 265 if (match) { 266 /* found a match */ 267 psw = (const struct ip6protosw *)match->psw; 268 if (psw && psw->pr_input) { 269 encap_fillarg(m, match); 270 return (*psw->pr_input)(mp, offp, proto); 271 } else { 272 m_freem(m); 273 return IPPROTO_DONE; 274 } 275 } 276 277 /* last resort: inject to raw socket */ 278 return rip6_input(mp, offp, proto); 279} 280#endif 281 282static void 283encap_add(ep) 284 struct encaptab *ep; 285{ 286 287 LIST_INSERT_HEAD(&encaptab, ep, chain); 288} 289 290/* 291 * sp (src ptr) is always my side, and dp (dst ptr) is always remote side. 292 * length of mask (sm and dm) is assumed to be same as sp/dp. 293 * Return value will be necessary as input (cookie) for encap_detach(). 294 */ 295const struct encaptab * 296encap_attach(af, proto, sp, sm, dp, dm, psw, arg) 297 int af; 298 int proto; 299 const struct sockaddr *sp, *sm; 300 const struct sockaddr *dp, *dm; 301 const struct protosw *psw; 302 void *arg; 303{ 304 struct encaptab *ep; 305 int error; 306 int s; 307 308 s = splnet(); 309 /* sanity check on args */ 310 if (sp->sa_len > sizeof(ep->src) || dp->sa_len > sizeof(ep->dst)) { 311 error = EINVAL; 312 goto fail; 313 } 314 if (sp->sa_len != dp->sa_len) { 315 error = EINVAL; 316 goto fail; 317 } 318 if (af != sp->sa_family || af != dp->sa_family) { 319 error = EINVAL; 320 goto fail; 321 } 322 323 /* check if anyone have already attached with exactly same config */ 324 LIST_FOREACH(ep, &encaptab, chain) { 325 if (ep->af != af) 326 continue; 327 if (ep->proto != proto) 328 continue; 329 if (ep->src.ss_len != sp->sa_len || 330 bcmp(&ep->src, sp, sp->sa_len) != 0 || 331 bcmp(&ep->srcmask, sm, sp->sa_len) != 0) 332 continue; 333 if (ep->dst.ss_len != dp->sa_len || 334 bcmp(&ep->dst, dp, dp->sa_len) != 0 || 335 bcmp(&ep->dstmask, dm, dp->sa_len) != 0) 336 continue; 337 338 error = EEXIST; 339 goto fail; 340 } 341 342 ep = malloc(sizeof(*ep), M_NETADDR, M_NOWAIT); /*XXX*/ 343 if (ep == NULL) { 344 error = ENOBUFS; 345 goto fail; 346 } 347 bzero(ep, sizeof(*ep)); 348 349 ep->af = af; 350 ep->proto = proto; 351 bcopy(sp, &ep->src, sp->sa_len); 352 bcopy(sm, &ep->srcmask, sp->sa_len); 353 bcopy(dp, &ep->dst, dp->sa_len); 354 bcopy(dm, &ep->dstmask, dp->sa_len); 355 ep->psw = psw; 356 ep->arg = arg; 357 358 encap_add(ep); 359 360 error = 0; 361 splx(s); 362 return ep; 363 364fail: 365 splx(s); 366 return NULL; 367} 368 369const struct encaptab * 370encap_attach_func(af, proto, func, psw, arg) 371 int af; 372 int proto; 373 int (*func)(const struct mbuf *, int, int, void *); 374 const struct protosw *psw; 375 void *arg; 376{ 377 struct encaptab *ep; 378 int error; 379 int s; 380 381 s = splnet(); 382 /* sanity check on args */ 383 if (!func) { 384 error = EINVAL; 385 goto fail; 386 } 387 388 ep = malloc(sizeof(*ep), M_NETADDR, M_NOWAIT); /*XXX*/ 389 if (ep == NULL) { 390 error = ENOBUFS; 391 goto fail; 392 } 393 bzero(ep, sizeof(*ep)); 394 395 ep->af = af; 396 ep->proto = proto; 397 ep->func = func; 398 ep->psw = psw; 399 ep->arg = arg; 400 401 encap_add(ep); 402 403 error = 0; 404 splx(s); 405 return ep; 406 407fail: 408 splx(s); 409 return NULL; 410} 411 412int 413encap_detach(cookie) 414 const struct encaptab *cookie; 415{ 416 const struct encaptab *ep = cookie; 417 struct encaptab *p; 418 419 LIST_FOREACH(p, &encaptab, chain) { 420 if (p == ep) { 421 LIST_REMOVE(p, chain); 422 free(p, M_NETADDR); /*XXX*/ 423 return 0; 424 } 425 } 426 427 return EINVAL; 428} 429 430static int 431mask_match(ep, sp, dp) 432 const struct encaptab *ep; 433 const struct sockaddr *sp; 434 const struct sockaddr *dp; 435{ 436 struct sockaddr_storage s; 437 struct sockaddr_storage d; 438 int i; 439 const u_int8_t *p, *q; 440 u_int8_t *r; 441 int matchlen; 442 443 if (sp->sa_len > sizeof(s) || dp->sa_len > sizeof(d)) 444 return 0; 445 if (sp->sa_family != ep->af || dp->sa_family != ep->af) 446 return 0; 447 if (sp->sa_len != ep->src.ss_len || dp->sa_len != ep->dst.ss_len) 448 return 0; 449 450 matchlen = 0; 451 452 p = (const u_int8_t *)sp; 453 q = (const u_int8_t *)&ep->srcmask; 454 r = (u_int8_t *)&s; 455 for (i = 0 ; i < sp->sa_len; i++) { 456 r[i] = p[i] & q[i]; 457 /* XXX estimate */ 458 matchlen += (q[i] ? 8 : 0); 459 } 460 461 p = (const u_int8_t *)dp; 462 q = (const u_int8_t *)&ep->dstmask; 463 r = (u_int8_t *)&d; 464 for (i = 0 ; i < dp->sa_len; i++) { 465 r[i] = p[i] & q[i]; 466 /* XXX rough estimate */ 467 matchlen += (q[i] ? 8 : 0); 468 } 469 470 /* need to overwrite len/family portion as we don't compare them */ 471 s.ss_len = sp->sa_len; 472 s.ss_family = sp->sa_family; 473 d.ss_len = dp->sa_len; 474 d.ss_family = dp->sa_family; 475 476 if (bcmp(&s, &ep->src, ep->src.ss_len) == 0 && 477 bcmp(&d, &ep->dst, ep->dst.ss_len) == 0) { 478 return matchlen; 479 } else 480 return 0; 481} 482 483static void 484encap_fillarg(m, ep) 485 struct mbuf *m; 486 const struct encaptab *ep; 487{
|