if_me.c 17 KB

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