1 /*- 2 * Copyright (c) 2014, 2018 Andrey V. Elsukov <ae@FreeBSD.org> 3 * All rights reserved. 4 * 5 * Redistribution and use in source and binary forms, with or without 6 * modification, are permitted provided that the following conditions 7 * are met: 8 * 9 * 1. Redistributions of source code must retain the above copyright 10 * notice, this list of conditions and the following disclaimer. 11 * 2. Redistributions in binary form must reproduce the above copyright 12 * notice, this list of conditions and the following disclaimer in the 13 * documentation and/or other materials provided with the distribution. 14 * 15 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR 16 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 17 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 18 * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, 19 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 20 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 21 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 22 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 24 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 */ 26 27 #include <sys/param.h> 28 #include <sys/systm.h> 29 #include <sys/jail.h> 30 #include <sys/kernel.h> 31 #include <sys/lock.h> 32 #include <sys/malloc.h> 33 #include <sys/module.h> 34 #include <sys/mbuf.h> 35 #include <sys/priv.h> 36 #include <sys/proc.h> 37 #include <sys/socket.h> 38 #include <sys/sockio.h> 39 #include <sys/sx.h> 40 #include <sys/sysctl.h> 41 #include <sys/syslog.h> 42 43 #include <net/bpf.h> 44 #include <net/ethernet.h> 45 #include <net/if.h> 46 #include <net/if_var.h> 47 #include <net/if_private.h> 48 #include <net/if_clone.h> 49 #include <net/if_types.h> 50 #include <net/netisr.h> 51 #include <net/vnet.h> 52 #include <net/route.h> 53 54 #include <netinet/in.h> 55 #include <netinet/in_systm.h> 56 #include <netinet/in_var.h> 57 #include <netinet/ip.h> 58 #include <netinet/ip_var.h> 59 #include <netinet/ip_encap.h> 60 61 #include <machine/in_cksum.h> 62 #include <security/mac/mac_framework.h> 63 64 #define MEMTU (1500 - sizeof(struct mobhdr)) 65 static const char mename[] = "me"; 66 static MALLOC_DEFINE(M_IFME, mename, "Minimal Encapsulation for IP"); 67 /* Minimal forwarding header RFC 2004 */ 68 struct mobhdr { 69 uint8_t mob_proto; /* protocol */ 70 uint8_t mob_flags; /* flags */ 71 #define MOB_FLAGS_SP 0x80 /* source present */ 72 uint16_t mob_csum; /* header checksum */ 73 struct in_addr mob_dst; /* original destination address */ 74 struct in_addr mob_src; /* original source addr (optional) */ 75 } __packed; 76 77 struct me_softc { 78 struct ifnet *me_ifp; 79 u_int me_fibnum; 80 struct in_addr me_src; 81 struct in_addr me_dst; 82 83 CK_LIST_ENTRY(me_softc) chain; 84 CK_LIST_ENTRY(me_softc) srchash; 85 }; 86 CK_LIST_HEAD(me_list, me_softc); 87 #define ME2IFP(sc) ((sc)->me_ifp) 88 #define ME_READY(sc) ((sc)->me_src.s_addr != 0) 89 #define ME_RLOCK_TRACKER struct epoch_tracker me_et 90 #define ME_RLOCK() epoch_enter_preempt(net_epoch_preempt, &me_et) 91 #define ME_RUNLOCK() epoch_exit_preempt(net_epoch_preempt, &me_et) 92 #define ME_WAIT() epoch_wait_preempt(net_epoch_preempt) 93 94 #ifndef ME_HASH_SIZE 95 #define ME_HASH_SIZE (1 << 4) 96 #endif 97 VNET_DEFINE_STATIC(struct me_list *, me_hashtbl) = NULL; 98 VNET_DEFINE_STATIC(struct me_list *, me_srchashtbl) = NULL; 99 #define V_me_hashtbl VNET(me_hashtbl) 100 #define V_me_srchashtbl VNET(me_srchashtbl) 101 #define ME_HASH(src, dst) (V_me_hashtbl[\ 102 me_hashval((src), (dst)) & (ME_HASH_SIZE - 1)]) 103 #define ME_SRCHASH(src) (V_me_srchashtbl[\ 104 fnv_32_buf(&(src), sizeof(src), FNV1_32_INIT) & (ME_HASH_SIZE - 1)]) 105 106 static struct sx me_ioctl_sx; 107 SX_SYSINIT(me_ioctl_sx, &me_ioctl_sx, "me_ioctl"); 108 109 static int me_clone_create(struct if_clone *, int, caddr_t); 110 static void me_clone_destroy(struct ifnet *); 111 VNET_DEFINE_STATIC(struct if_clone *, me_cloner); 112 #define V_me_cloner VNET(me_cloner) 113 114 #ifdef VIMAGE 115 static void me_reassign(struct ifnet *, struct vnet *, char *); 116 #endif 117 static void me_qflush(struct ifnet *); 118 static int me_transmit(struct ifnet *, struct mbuf *); 119 static int me_ioctl(struct ifnet *, u_long, caddr_t); 120 static int me_output(struct ifnet *, struct mbuf *, 121 const struct sockaddr *, struct route *); 122 static int me_input(struct mbuf *, int, int, void *); 123 124 static int me_set_tunnel(struct me_softc *, in_addr_t, in_addr_t); 125 static void me_delete_tunnel(struct me_softc *); 126 127 SYSCTL_DECL(_net_link); 128 static SYSCTL_NODE(_net_link, IFT_TUNNEL, me, CTLFLAG_RW | CTLFLAG_MPSAFE, 0, 129 "Minimal Encapsulation for IP (RFC 2004)"); 130 #ifndef MAX_ME_NEST 131 #define MAX_ME_NEST 1 132 #endif 133 134 VNET_DEFINE_STATIC(int, max_me_nesting) = MAX_ME_NEST; 135 #define V_max_me_nesting VNET(max_me_nesting) 136 SYSCTL_INT(_net_link_me, OID_AUTO, max_nesting, CTLFLAG_RW | CTLFLAG_VNET, 137 &VNET_NAME(max_me_nesting), 0, "Max nested tunnels"); 138 139 static uint32_t 140 me_hashval(in_addr_t src, in_addr_t dst) 141 { 142 uint32_t ret; 143 144 ret = fnv_32_buf(&src, sizeof(src), FNV1_32_INIT); 145 return (fnv_32_buf(&dst, sizeof(dst), ret)); 146 } 147 148 static struct me_list * 149 me_hashinit(void) 150 { 151 struct me_list *hash; 152 int i; 153 154 hash = malloc(sizeof(struct me_list) * ME_HASH_SIZE, 155 M_IFME, M_WAITOK); 156 for (i = 0; i < ME_HASH_SIZE; i++) 157 CK_LIST_INIT(&hash[i]); 158 159 return (hash); 160 } 161 162 static void 163 vnet_me_init(const void *unused __unused) 164 { 165 166 V_me_cloner = if_clone_simple(mename, me_clone_create, 167 me_clone_destroy, 0); 168 } 169 VNET_SYSINIT(vnet_me_init, SI_SUB_PROTO_IFATTACHDOMAIN, SI_ORDER_ANY, 170 vnet_me_init, NULL); 171 172 static void 173 vnet_me_uninit(const void *unused __unused) 174 { 175 176 if (V_me_hashtbl != NULL) { 177 free(V_me_hashtbl, M_IFME); 178 V_me_hashtbl = NULL; 179 ME_WAIT(); 180 free(V_me_srchashtbl, M_IFME); 181 } 182 if_clone_detach(V_me_cloner); 183 } 184 VNET_SYSUNINIT(vnet_me_uninit, SI_SUB_PROTO_IFATTACHDOMAIN, SI_ORDER_ANY, 185 vnet_me_uninit, NULL); 186 187 static int 188 me_clone_create(struct if_clone *ifc, int unit, caddr_t params) 189 { 190 struct me_softc *sc; 191 192 sc = malloc(sizeof(struct me_softc), M_IFME, M_WAITOK | M_ZERO); 193 sc->me_fibnum = curthread->td_proc->p_fibnum; 194 ME2IFP(sc) = if_alloc(IFT_TUNNEL); 195 ME2IFP(sc)->if_softc = sc; 196 if_initname(ME2IFP(sc), mename, unit); 197 198 ME2IFP(sc)->if_mtu = MEMTU; 199 ME2IFP(sc)->if_flags = IFF_POINTOPOINT|IFF_MULTICAST; 200 ME2IFP(sc)->if_output = me_output; 201 ME2IFP(sc)->if_ioctl = me_ioctl; 202 ME2IFP(sc)->if_transmit = me_transmit; 203 ME2IFP(sc)->if_qflush = me_qflush; 204 #ifdef VIMAGE 205 ME2IFP(sc)->if_reassign = me_reassign; 206 #endif 207 ME2IFP(sc)->if_capabilities |= IFCAP_LINKSTATE; 208 ME2IFP(sc)->if_capenable |= IFCAP_LINKSTATE; 209 if_attach(ME2IFP(sc)); 210 bpfattach(ME2IFP(sc), DLT_NULL, sizeof(u_int32_t)); 211 return (0); 212 } 213 214 #ifdef VIMAGE 215 static void 216 me_reassign(struct ifnet *ifp, struct vnet *new_vnet __unused, 217 char *unused __unused) 218 { 219 struct me_softc *sc; 220 221 sx_xlock(&me_ioctl_sx); 222 sc = ifp->if_softc; 223 if (sc != NULL) 224 me_delete_tunnel(sc); 225 sx_xunlock(&me_ioctl_sx); 226 } 227 #endif /* VIMAGE */ 228 229 static void 230 me_clone_destroy(struct ifnet *ifp) 231 { 232 struct me_softc *sc; 233 234 sx_xlock(&me_ioctl_sx); 235 sc = ifp->if_softc; 236 me_delete_tunnel(sc); 237 bpfdetach(ifp); 238 if_detach(ifp); 239 ifp->if_softc = NULL; 240 sx_xunlock(&me_ioctl_sx); 241 242 ME_WAIT(); 243 if_free(ifp); 244 free(sc, M_IFME); 245 } 246 247 static int 248 me_ioctl(struct ifnet *ifp, u_long cmd, caddr_t data) 249 { 250 struct ifreq *ifr = (struct ifreq *)data; 251 struct sockaddr_in *src, *dst; 252 struct me_softc *sc; 253 int error; 254 255 switch (cmd) { 256 case SIOCSIFMTU: 257 if (ifr->ifr_mtu < 576) 258 return (EINVAL); 259 ifp->if_mtu = ifr->ifr_mtu; 260 return (0); 261 case SIOCSIFADDR: 262 ifp->if_flags |= IFF_UP; 263 case SIOCSIFFLAGS: 264 case SIOCADDMULTI: 265 case SIOCDELMULTI: 266 return (0); 267 } 268 sx_xlock(&me_ioctl_sx); 269 sc = ifp->if_softc; 270 if (sc == NULL) { 271 error = ENXIO; 272 goto end; 273 } 274 error = 0; 275 switch (cmd) { 276 case SIOCSIFPHYADDR: 277 src = &((struct in_aliasreq *)data)->ifra_addr; 278 dst = &((struct in_aliasreq *)data)->ifra_dstaddr; 279 if (src->sin_family != dst->sin_family || 280 src->sin_family != AF_INET || 281 src->sin_len != dst->sin_len || 282 src->sin_len != sizeof(struct sockaddr_in)) { 283 error = EINVAL; 284 break; 285 } 286 if (src->sin_addr.s_addr == INADDR_ANY || 287 dst->sin_addr.s_addr == INADDR_ANY) { 288 error = EADDRNOTAVAIL; 289 break; 290 } 291 error = me_set_tunnel(sc, src->sin_addr.s_addr, 292 dst->sin_addr.s_addr); 293 break; 294 case SIOCDIFPHYADDR: 295 me_delete_tunnel(sc); 296 break; 297 case SIOCGIFPSRCADDR: 298 case SIOCGIFPDSTADDR: 299 if (!ME_READY(sc)) { 300 error = EADDRNOTAVAIL; 301 break; 302 } 303 src = (struct sockaddr_in *)&ifr->ifr_addr; 304 memset(src, 0, sizeof(*src)); 305 src->sin_family = AF_INET; 306 src->sin_len = sizeof(*src); 307 switch (cmd) { 308 case SIOCGIFPSRCADDR: 309 src->sin_addr = sc->me_src; 310 break; 311 case SIOCGIFPDSTADDR: 312 src->sin_addr = sc->me_dst; 313 break; 314 } 315 error = prison_if(curthread->td_ucred, sintosa(src)); 316 if (error != 0) 317 memset(src, 0, sizeof(*src)); 318 break; 319 case SIOCGTUNFIB: 320 ifr->ifr_fib = sc->me_fibnum; 321 break; 322 case SIOCSTUNFIB: 323 if ((error = priv_check(curthread, PRIV_NET_ME)) != 0) 324 break; 325 if (ifr->ifr_fib >= rt_numfibs) 326 error = EINVAL; 327 else 328 sc->me_fibnum = ifr->ifr_fib; 329 break; 330 default: 331 error = EINVAL; 332 break; 333 } 334 end: 335 sx_xunlock(&me_ioctl_sx); 336 return (error); 337 } 338 339 static int 340 me_lookup(const struct mbuf *m, int off, int proto, void **arg) 341 { 342 const struct ip *ip; 343 struct me_softc *sc; 344 345 if (V_me_hashtbl == NULL) 346 return (0); 347 348 NET_EPOCH_ASSERT(); 349 ip = mtod(m, const struct ip *); 350 CK_LIST_FOREACH(sc, &ME_HASH(ip->ip_dst.s_addr, 351 ip->ip_src.s_addr), chain) { 352 if (sc->me_src.s_addr == ip->ip_dst.s_addr && 353 sc->me_dst.s_addr == ip->ip_src.s_addr) { 354 if ((ME2IFP(sc)->if_flags & IFF_UP) == 0) 355 return (0); 356 *arg = sc; 357 return (ENCAP_DRV_LOOKUP); 358 } 359 } 360 return (0); 361 } 362 363 /* 364 * Check that ingress address belongs to local host. 365 */ 366 static void 367 me_set_running(struct me_softc *sc) 368 { 369 370 if (in_localip(sc->me_src)) 371 ME2IFP(sc)->if_drv_flags |= IFF_DRV_RUNNING; 372 else 373 ME2IFP(sc)->if_drv_flags &= ~IFF_DRV_RUNNING; 374 } 375 376 /* 377 * ifaddr_event handler. 378 * Clear IFF_DRV_RUNNING flag when ingress address disappears to prevent 379 * source address spoofing. 380 */ 381 static void 382 me_srcaddr(void *arg __unused, const struct sockaddr *sa, 383 int event __unused) 384 { 385 const struct sockaddr_in *sin; 386 struct me_softc *sc; 387 388 /* Check that VNET is ready */ 389 if (V_me_hashtbl == NULL) 390 return; 391 392 NET_EPOCH_ASSERT(); 393 sin = (const struct sockaddr_in *)sa; 394 CK_LIST_FOREACH(sc, &ME_SRCHASH(sin->sin_addr.s_addr), srchash) { 395 if (sc->me_src.s_addr != sin->sin_addr.s_addr) 396 continue; 397 me_set_running(sc); 398 } 399 } 400 401 static int 402 me_set_tunnel(struct me_softc *sc, in_addr_t src, in_addr_t dst) 403 { 404 struct epoch_tracker et; 405 struct me_softc *tmp; 406 407 sx_assert(&me_ioctl_sx, SA_XLOCKED); 408 409 if (V_me_hashtbl == NULL) { 410 V_me_hashtbl = me_hashinit(); 411 V_me_srchashtbl = me_hashinit(); 412 } 413 414 if (sc->me_src.s_addr == src && sc->me_dst.s_addr == dst) 415 return (0); 416 417 CK_LIST_FOREACH(tmp, &ME_HASH(src, dst), chain) { 418 if (tmp == sc) 419 continue; 420 if (tmp->me_src.s_addr == src && 421 tmp->me_dst.s_addr == dst) 422 return (EADDRNOTAVAIL); 423 } 424 425 me_delete_tunnel(sc); 426 sc->me_dst.s_addr = dst; 427 sc->me_src.s_addr = src; 428 CK_LIST_INSERT_HEAD(&ME_HASH(src, dst), sc, chain); 429 CK_LIST_INSERT_HEAD(&ME_SRCHASH(src), sc, srchash); 430 431 NET_EPOCH_ENTER(et); 432 me_set_running(sc); 433 NET_EPOCH_EXIT(et); 434 if_link_state_change(ME2IFP(sc), LINK_STATE_UP); 435 return (0); 436 } 437 438 static void 439 me_delete_tunnel(struct me_softc *sc) 440 { 441 442 sx_assert(&me_ioctl_sx, SA_XLOCKED); 443 if (ME_READY(sc)) { 444 CK_LIST_REMOVE(sc, chain); 445 CK_LIST_REMOVE(sc, srchash); 446 ME_WAIT(); 447 448 sc->me_src.s_addr = 0; 449 sc->me_dst.s_addr = 0; 450 ME2IFP(sc)->if_drv_flags &= ~IFF_DRV_RUNNING; 451 if_link_state_change(ME2IFP(sc), LINK_STATE_DOWN); 452 } 453 } 454 455 static uint16_t 456 me_in_cksum(uint16_t *p, int nwords) 457 { 458 uint32_t sum = 0; 459 460 while (nwords-- > 0) 461 sum += *p++; 462 sum = (sum >> 16) + (sum & 0xffff); 463 sum += (sum >> 16); 464 return (~sum); 465 } 466 467 static int 468 me_input(struct mbuf *m, int off, int proto, void *arg) 469 { 470 struct me_softc *sc = arg; 471 struct mobhdr *mh; 472 struct ifnet *ifp; 473 struct ip *ip; 474 int hlen; 475 476 NET_EPOCH_ASSERT(); 477 478 ifp = ME2IFP(sc); 479 /* checks for short packets */ 480 hlen = sizeof(struct mobhdr); 481 if (m->m_pkthdr.len < sizeof(struct ip) + hlen) 482 hlen -= sizeof(struct in_addr); 483 if (m->m_len < sizeof(struct ip) + hlen) 484 m = m_pullup(m, sizeof(struct ip) + hlen); 485 if (m == NULL) 486 goto drop; 487 mh = (struct mobhdr *)mtodo(m, sizeof(struct ip)); 488 /* check for wrong flags */ 489 if (mh->mob_flags & (~MOB_FLAGS_SP)) { 490 m_freem(m); 491 goto drop; 492 } 493 if (mh->mob_flags) { 494 if (hlen != sizeof(struct mobhdr)) { 495 m_freem(m); 496 goto drop; 497 } 498 } else 499 hlen = sizeof(struct mobhdr) - sizeof(struct in_addr); 500 /* check mobile header checksum */ 501 if (me_in_cksum((uint16_t *)mh, hlen / sizeof(uint16_t)) != 0) { 502 m_freem(m); 503 goto drop; 504 } 505 #ifdef MAC 506 mac_ifnet_create_mbuf(ifp, m); 507 #endif 508 ip = mtod(m, struct ip *); 509 ip->ip_dst = mh->mob_dst; 510 ip->ip_p = mh->mob_proto; 511 ip->ip_sum = 0; 512 ip->ip_len = htons(m->m_pkthdr.len - hlen); 513 if (mh->mob_flags) 514 ip->ip_src = mh->mob_src; 515 memmove(mtodo(m, hlen), ip, sizeof(struct ip)); 516 m_adj(m, hlen); 517 m_clrprotoflags(m); 518 m->m_pkthdr.rcvif = ifp; 519 m->m_pkthdr.csum_flags |= (CSUM_IP_CHECKED | CSUM_IP_VALID); 520 M_SETFIB(m, ifp->if_fib); 521 hlen = AF_INET; 522 BPF_MTAP2(ifp, &hlen, sizeof(hlen), m); 523 if_inc_counter(ifp, IFCOUNTER_IPACKETS, 1); 524 if_inc_counter(ifp, IFCOUNTER_IBYTES, m->m_pkthdr.len); 525 if ((ifp->if_flags & IFF_MONITOR) != 0) 526 m_freem(m); 527 else 528 netisr_dispatch(NETISR_IP, m); 529 return (IPPROTO_DONE); 530 drop: 531 if_inc_counter(ifp, IFCOUNTER_IERRORS, 1); 532 return (IPPROTO_DONE); 533 } 534 535 static int 536 me_output(struct ifnet *ifp, struct mbuf *m, const struct sockaddr *dst, 537 struct route *ro) 538 { 539 uint32_t af; 540 541 /* BPF writes need to be handled specially. */ 542 if (dst->sa_family == AF_UNSPEC || dst->sa_family == pseudo_AF_HDRCMPLT) 543 bcopy(dst->sa_data, &af, sizeof(af)); 544 else 545 af = RO_GET_FAMILY(ro, dst); 546 m->m_pkthdr.csum_data = af; 547 return (ifp->if_transmit(ifp, m)); 548 } 549 550 #define MTAG_ME 1414491977 551 static int 552 me_transmit(struct ifnet *ifp, struct mbuf *m) 553 { 554 ME_RLOCK_TRACKER; 555 struct mobhdr mh; 556 struct me_softc *sc; 557 struct ip *ip; 558 uint32_t af; 559 int error, hlen, plen; 560 561 ME_RLOCK(); 562 #ifdef MAC 563 error = mac_ifnet_check_transmit(ifp, m); 564 if (error != 0) 565 goto drop; 566 #endif 567 error = ENETDOWN; 568 sc = ifp->if_softc; 569 if (sc == NULL || !ME_READY(sc) || 570 (ifp->if_flags & IFF_MONITOR) != 0 || 571 (ifp->if_flags & IFF_UP) == 0 || 572 (ifp->if_drv_flags & IFF_DRV_RUNNING) == 0 || 573 (error = if_tunnel_check_nesting(ifp, m, MTAG_ME, 574 V_max_me_nesting)) != 0) { 575 m_freem(m); 576 goto drop; 577 } 578 af = m->m_pkthdr.csum_data; 579 if (af != AF_INET) { 580 error = EAFNOSUPPORT; 581 m_freem(m); 582 goto drop; 583 } 584 if (m->m_len < sizeof(struct ip)) 585 m = m_pullup(m, sizeof(struct ip)); 586 if (m == NULL) { 587 error = ENOBUFS; 588 goto drop; 589 } 590 ip = mtod(m, struct ip *); 591 /* Fragmented datagramms shouldn't be encapsulated */ 592 if (ip->ip_off & htons(IP_MF | IP_OFFMASK)) { 593 error = EINVAL; 594 m_freem(m); 595 goto drop; 596 } 597 mh.mob_proto = ip->ip_p; 598 mh.mob_src = ip->ip_src; 599 mh.mob_dst = ip->ip_dst; 600 if (in_hosteq(sc->me_src, ip->ip_src)) { 601 hlen = sizeof(struct mobhdr) - sizeof(struct in_addr); 602 mh.mob_flags = 0; 603 } else { 604 hlen = sizeof(struct mobhdr); 605 mh.mob_flags = MOB_FLAGS_SP; 606 } 607 BPF_MTAP2(ifp, &af, sizeof(af), m); 608 plen = m->m_pkthdr.len; 609 ip->ip_src = sc->me_src; 610 ip->ip_dst = sc->me_dst; 611 m->m_flags &= ~(M_BCAST|M_MCAST); 612 M_SETFIB(m, sc->me_fibnum); 613 M_PREPEND(m, hlen, M_NOWAIT); 614 if (m == NULL) { 615 error = ENOBUFS; 616 goto drop; 617 } 618 if (m->m_len < sizeof(struct ip) + hlen) 619 m = m_pullup(m, sizeof(struct ip) + hlen); 620 if (m == NULL) { 621 error = ENOBUFS; 622 goto drop; 623 } 624 memmove(mtod(m, void *), mtodo(m, hlen), sizeof(struct ip)); 625 ip = mtod(m, struct ip *); 626 ip->ip_len = htons(m->m_pkthdr.len); 627 ip->ip_p = IPPROTO_MOBILE; 628 ip->ip_sum = 0; 629 mh.mob_csum = 0; 630 mh.mob_csum = me_in_cksum((uint16_t *)&mh, hlen / sizeof(uint16_t)); 631 bcopy(&mh, mtodo(m, sizeof(struct ip)), hlen); 632 error = ip_output(m, NULL, NULL, IP_FORWARDING, NULL, NULL); 633 drop: 634 if (error) 635 if_inc_counter(ifp, IFCOUNTER_OERRORS, 1); 636 else { 637 if_inc_counter(ifp, IFCOUNTER_OPACKETS, 1); 638 if_inc_counter(ifp, IFCOUNTER_OBYTES, plen); 639 } 640 ME_RUNLOCK(); 641 return (error); 642 } 643 644 static void 645 me_qflush(struct ifnet *ifp __unused) 646 { 647 648 } 649 650 static const struct srcaddrtab *me_srcaddrtab = NULL; 651 static const struct encaptab *ecookie = NULL; 652 static const struct encap_config me_encap_cfg = { 653 .proto = IPPROTO_MOBILE, 654 .min_length = sizeof(struct ip) + sizeof(struct mobhdr) - 655 sizeof(in_addr_t), 656 .exact_match = ENCAP_DRV_LOOKUP, 657 .lookup = me_lookup, 658 .input = me_input 659 }; 660 661 static int 662 memodevent(module_t mod, int type, void *data) 663 { 664 665 switch (type) { 666 case MOD_LOAD: 667 me_srcaddrtab = ip_encap_register_srcaddr(me_srcaddr, 668 NULL, M_WAITOK); 669 ecookie = ip_encap_attach(&me_encap_cfg, NULL, M_WAITOK); 670 break; 671 case MOD_UNLOAD: 672 ip_encap_detach(ecookie); 673 ip_encap_unregister_srcaddr(me_srcaddrtab); 674 break; 675 default: 676 return (EOPNOTSUPP); 677 } 678 return (0); 679 } 680 681 static moduledata_t me_mod = { 682 "if_me", 683 memodevent, 684 0 685 }; 686 687 DECLARE_MODULE(if_me, me_mod, SI_SUB_PSEUDO, SI_ORDER_ANY); 688 MODULE_VERSION(if_me, 1); 689