1 /* $OpenBSD: if_mpe.c,v 1.34 2014/07/12 18:44:22 tedu Exp $ */ 2 3 /* 4 * Copyright (c) 2008 Pierre-Yves Ritschard <pyr@spootnik.org> 5 * 6 * Permission to use, copy, modify, and distribute this software for any 7 * purpose with or without fee is hereby granted, provided that the above 8 * copyright notice and this permission notice appear in all copies. 9 * 10 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES 11 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF 12 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR 13 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES 14 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN 15 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF 16 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. 17 */ 18 #include "mpe.h" 19 20 #include <sys/param.h> 21 #include <sys/systm.h> 22 #include <sys/mbuf.h> 23 #include <sys/socket.h> 24 #include <sys/sockio.h> 25 #include <sys/ioctl.h> 26 27 #include <net/if.h> 28 #include <net/if_types.h> 29 #include <net/netisr.h> 30 #include <net/route.h> 31 32 #ifdef INET 33 #include <netinet/in.h> 34 #include <netinet/in_systm.h> 35 #include <netinet/ip.h> 36 #endif 37 38 #ifdef INET6 39 #include <netinet/ip6.h> 40 #ifndef INET 41 #include <netinet/in.h> 42 #endif 43 #endif /* INET6 */ 44 45 #include "bpfilter.h" 46 #if NBPFILTER > 0 47 #include <net/bpf.h> 48 #endif 49 50 #include <netmpls/mpls.h> 51 52 #ifdef MPLS_DEBUG 53 #define DPRINTF(x) do { if (mpedebug) printf x ; } while (0) 54 #else 55 #define DPRINTF(x) 56 #endif 57 58 void mpeattach(int); 59 int mpeoutput(struct ifnet *, struct mbuf *, struct sockaddr *, 60 struct rtentry *); 61 int mpeioctl(struct ifnet *, u_long, caddr_t); 62 void mpestart(struct ifnet *); 63 int mpe_clone_create(struct if_clone *, int); 64 int mpe_clone_destroy(struct ifnet *); 65 int mpe_newlabel(struct ifnet *, int, struct shim_hdr *); 66 67 LIST_HEAD(, mpe_softc) mpeif_list; 68 struct if_clone mpe_cloner = 69 IF_CLONE_INITIALIZER("mpe", mpe_clone_create, mpe_clone_destroy); 70 71 extern int mpls_mapttl_ip; 72 #ifdef INET6 73 extern int mpls_mapttl_ip6; 74 #endif 75 76 void 77 mpeattach(int nmpe) 78 { 79 LIST_INIT(&mpeif_list); 80 if_clone_attach(&mpe_cloner); 81 } 82 83 int 84 mpe_clone_create(struct if_clone *ifc, int unit) 85 { 86 struct ifnet *ifp; 87 struct mpe_softc *mpeif; 88 int s; 89 90 if ((mpeif = malloc(sizeof(*mpeif), 91 M_DEVBUF, M_NOWAIT|M_ZERO)) == NULL) 92 return (ENOMEM); 93 94 mpeif->sc_shim.shim_label = 0; 95 mpeif->sc_unit = unit; 96 ifp = &mpeif->sc_if; 97 snprintf(ifp->if_xname, sizeof ifp->if_xname, "mpe%d", unit); 98 ifp->if_flags = IFF_POINTOPOINT; 99 ifp->if_softc = mpeif; 100 ifp->if_mtu = MPE_MTU; 101 ifp->if_ioctl = mpeioctl; 102 ifp->if_output = mpeoutput; 103 ifp->if_start = mpestart; 104 ifp->if_type = IFT_MPLS; 105 ifp->if_hdrlen = MPE_HDRLEN; 106 IFQ_SET_MAXLEN(&ifp->if_snd, IFQ_MAXLEN); 107 IFQ_SET_READY(&ifp->if_snd); 108 if_attach(ifp); 109 if_alloc_sadl(ifp); 110 #if NBPFILTER > 0 111 bpfattach(&ifp->if_bpf, ifp, DLT_LOOP, sizeof(u_int32_t)); 112 #endif 113 114 s = splnet(); 115 LIST_INSERT_HEAD(&mpeif_list, mpeif, sc_list); 116 splx(s); 117 118 return (0); 119 } 120 121 int 122 mpe_clone_destroy(struct ifnet *ifp) 123 { 124 struct mpe_softc *mpeif = ifp->if_softc; 125 int s; 126 127 s = splnet(); 128 LIST_REMOVE(mpeif, sc_list); 129 splx(s); 130 131 if_detach(ifp); 132 free(mpeif, M_DEVBUF, 0); 133 return (0); 134 } 135 136 struct sockaddr_storage mpedst; 137 /* 138 * Start output on the mpe interface. 139 */ 140 void 141 mpestart(struct ifnet *ifp) 142 { 143 struct mbuf *m; 144 struct sockaddr *sa = (struct sockaddr *)&mpedst; 145 int s; 146 sa_family_t af; 147 struct rtentry *rt; 148 149 for (;;) { 150 s = splnet(); 151 IFQ_DEQUEUE(&ifp->if_snd, m); 152 splx(s); 153 154 if (m == NULL) 155 return; 156 157 af = *mtod(m, sa_family_t *); 158 m_adj(m, sizeof(af)); 159 switch (af) { 160 case AF_INET: 161 bzero(sa, sizeof(struct sockaddr_in)); 162 satosin(sa)->sin_family = af; 163 satosin(sa)->sin_len = sizeof(struct sockaddr_in); 164 bcopy(mtod(m, caddr_t), &satosin(sa)->sin_addr, 165 sizeof(in_addr_t)); 166 m_adj(m, sizeof(in_addr_t)); 167 break; 168 default: 169 m_freem(m); 170 continue; 171 } 172 173 rt = rtalloc1(sa, RT_REPORT, 0); 174 if (rt == NULL) { 175 /* no route give up */ 176 m_freem(m); 177 continue; 178 } 179 180 #if NBPFILTER > 0 181 if (ifp->if_bpf) { 182 /* remove MPLS label before passing packet to bpf */ 183 m->m_data += sizeof(struct shim_hdr); 184 m->m_len -= sizeof(struct shim_hdr); 185 m->m_pkthdr.len -= sizeof(struct shim_hdr); 186 bpf_mtap_af(ifp->if_bpf, af, m, BPF_DIRECTION_OUT); 187 m->m_data -= sizeof(struct shim_hdr); 188 m->m_len += sizeof(struct shim_hdr); 189 m->m_pkthdr.len += sizeof(struct shim_hdr); 190 } 191 #endif 192 /* XXX lie, but mpls_output will only look at sa_family */ 193 sa->sa_family = AF_MPLS; 194 195 mpls_output(rt->rt_ifp, m, sa, rt); 196 RTFREE(rt); 197 } 198 } 199 200 int 201 mpeoutput(struct ifnet *ifp, struct mbuf *m, struct sockaddr *dst, 202 struct rtentry *rt) 203 { 204 struct shim_hdr shim; 205 int s; 206 int error; 207 int off; 208 u_int8_t op = 0; 209 210 #ifdef DIAGNOSTIC 211 if (ifp->if_rdomain != rtable_l2(m->m_pkthdr.ph_rtableid)) { 212 printf("%s: trying to send packet on wrong domain. " 213 "if %d vs. mbuf %d\n", ifp->if_xname, 214 ifp->if_rdomain, rtable_l2(m->m_pkthdr.ph_rtableid)); 215 } 216 #endif 217 m->m_pkthdr.rcvif = ifp; 218 /* XXX assumes MPLS is always in rdomain 0 */ 219 m->m_pkthdr.ph_rtableid = 0; 220 221 error = 0; 222 switch (dst->sa_family) { 223 #ifdef INET 224 case AF_INET: 225 if (rt && rt->rt_flags & RTF_MPLS) { 226 shim.shim_label = 227 ((struct rt_mpls *)rt->rt_llinfo)->mpls_label; 228 shim.shim_label |= MPLS_BOS_MASK; 229 op = ((struct rt_mpls *)rt->rt_llinfo)->mpls_operation; 230 } 231 if (op != MPLS_OP_PUSH) { 232 m_freem(m); 233 error = ENETUNREACH; 234 goto out; 235 } 236 if (mpls_mapttl_ip) { 237 struct ip *ip; 238 ip = mtod(m, struct ip *); 239 shim.shim_label |= htonl(ip->ip_ttl) & MPLS_TTL_MASK; 240 } else 241 shim.shim_label |= htonl(mpls_defttl) & MPLS_TTL_MASK; 242 off = sizeof(sa_family_t) + sizeof(in_addr_t); 243 M_PREPEND(m, sizeof(shim) + off, M_DONTWAIT); 244 if (m == NULL) { 245 error = ENOBUFS; 246 goto out; 247 } 248 *mtod(m, sa_family_t *) = AF_INET; 249 m_copyback(m, sizeof(sa_family_t), sizeof(in_addr_t), 250 (caddr_t)&((satosin(dst)->sin_addr)), M_NOWAIT); 251 break; 252 #endif 253 default: 254 m_freem(m); 255 error = EPFNOSUPPORT; 256 goto out; 257 } 258 259 m_copyback(m, off, sizeof(shim), (caddr_t)&shim, M_NOWAIT); 260 261 s = splnet(); 262 IFQ_ENQUEUE(&ifp->if_snd, m, NULL, error); 263 if (error) { 264 /* mbuf is already freed */ 265 splx(s); 266 goto out; 267 } 268 if_start(ifp); 269 splx(s); 270 271 out: 272 if (error) 273 ifp->if_oerrors++; 274 return (error); 275 } 276 277 /* ARGSUSED */ 278 int 279 mpeioctl(struct ifnet *ifp, u_long cmd, caddr_t data) 280 { 281 int error; 282 struct mpe_softc *ifm; 283 struct ifreq *ifr; 284 struct shim_hdr shim; 285 286 ifr = (struct ifreq *)data; 287 error = 0; 288 switch (cmd) { 289 case SIOCSIFADDR: 290 if (!ISSET(ifp->if_flags, IFF_UP)) 291 if_up(ifp); 292 break; 293 case SIOCSIFFLAGS: 294 if (ifp->if_flags & IFF_UP) 295 ifp->if_flags |= IFF_RUNNING; 296 else 297 ifp->if_flags &= ~IFF_RUNNING; 298 break; 299 case SIOCSIFMTU: 300 if (ifr->ifr_mtu < MPE_MTU_MIN || 301 ifr->ifr_mtu > MPE_MTU_MAX) 302 error = EINVAL; 303 else 304 ifp->if_mtu = ifr->ifr_mtu; 305 break; 306 case SIOCGETLABEL: 307 ifm = ifp->if_softc; 308 shim.shim_label = 309 ((ntohl(ifm->sc_shim.shim_label & MPLS_LABEL_MASK)) >> 310 MPLS_LABEL_OFFSET); 311 error = copyout(&shim, ifr->ifr_data, sizeof(shim)); 312 break; 313 case SIOCSETLABEL: 314 ifm = ifp->if_softc; 315 if ((error = copyin(ifr->ifr_data, &shim, sizeof(shim)))) 316 break; 317 if (shim.shim_label > MPLS_LABEL_MAX || 318 shim.shim_label <= MPLS_LABEL_RESERVED_MAX) { 319 error = EINVAL; 320 break; 321 } 322 shim.shim_label = htonl(shim.shim_label << MPLS_LABEL_OFFSET); 323 if (ifm->sc_shim.shim_label == shim.shim_label) 324 break; 325 LIST_FOREACH(ifm, &mpeif_list, sc_list) { 326 if (ifm != ifp->if_softc && 327 ifm->sc_shim.shim_label == shim.shim_label) { 328 error = EEXIST; 329 break; 330 } 331 } 332 if (error) 333 break; 334 ifm = ifp->if_softc; 335 if (ifm->sc_shim.shim_label) { 336 /* remove old MPLS route */ 337 mpe_newlabel(ifp, RTM_DELETE, &ifm->sc_shim); 338 } 339 /* add new MPLS route */ 340 error = mpe_newlabel(ifp, RTM_ADD, &shim); 341 if (error) 342 break; 343 ifm->sc_shim.shim_label = shim.shim_label; 344 break; 345 case SIOCSIFRDOMAIN: 346 /* must readd the MPLS "route" for our label */ 347 ifm = ifp->if_softc; 348 if (ifr->ifr_rdomainid != ifp->if_rdomain) { 349 if (ifm->sc_shim.shim_label) { 350 shim.shim_label = ifm->sc_shim.shim_label; 351 error = mpe_newlabel(ifp, RTM_ADD, &shim); 352 } 353 } 354 /* return with ENOTTY so that the parent handler finishes */ 355 return (ENOTTY); 356 default: 357 return (ENOTTY); 358 } 359 360 return (error); 361 } 362 363 void 364 mpe_input(struct mbuf *m, struct ifnet *ifp, struct sockaddr_mpls *smpls, 365 u_int8_t ttl) 366 { 367 struct ip *ip; 368 int s, hlen; 369 370 /* label -> AF lookup */ 371 372 if (mpls_mapttl_ip) { 373 if (m->m_len < sizeof (struct ip) && 374 (m = m_pullup(m, sizeof(struct ip))) == NULL) 375 return; 376 ip = mtod(m, struct ip *); 377 hlen = ip->ip_hl << 2; 378 if (m->m_len < hlen) { 379 if ((m = m_pullup(m, hlen)) == NULL) 380 return; 381 ip = mtod(m, struct ip *); 382 } 383 384 if (in_cksum(m, hlen) != 0) { 385 m_freem(m); 386 return; 387 } 388 389 /* set IP ttl from MPLS ttl */ 390 ip->ip_ttl = ttl; 391 392 /* recalculate checksum */ 393 ip->ip_sum = 0; 394 ip->ip_sum = in_cksum(m, hlen); 395 } 396 397 /* new receive if and move into correct rtable */ 398 m->m_pkthdr.rcvif = ifp; 399 m->m_pkthdr.ph_rtableid = ifp->if_rdomain; 400 401 #if NBPFILTER > 0 402 if (ifp && ifp->if_bpf) 403 bpf_mtap_af(ifp->if_bpf, AF_INET, m, BPF_DIRECTION_IN); 404 #endif 405 s = splnet(); 406 IF_INPUT_ENQUEUE(&ipintrq, m); 407 schednetisr(NETISR_IP); 408 splx(s); 409 } 410 411 #ifdef INET6 412 void 413 mpe_input6(struct mbuf *m, struct ifnet *ifp, struct sockaddr_mpls *smpls, 414 u_int8_t ttl) 415 { 416 struct ip6_hdr *ip6hdr; 417 int s; 418 419 /* label -> AF lookup */ 420 421 if (mpls_mapttl_ip6) { 422 if (m->m_len < sizeof (struct ip6_hdr) && 423 (m = m_pullup(m, sizeof(struct ip6_hdr))) == NULL) 424 return; 425 426 ip6hdr = mtod(m, struct ip6_hdr *); 427 428 /* set IPv6 ttl from MPLS ttl */ 429 ip6hdr->ip6_hlim = ttl; 430 } 431 432 /* new receive if and move into correct rtable */ 433 m->m_pkthdr.rcvif = ifp; 434 m->m_pkthdr.ph_rtableid = ifp->if_rdomain; 435 436 #if NBPFILTER > 0 437 if (ifp && ifp->if_bpf) 438 bpf_mtap_af(ifp->if_bpf, AF_INET6, m, BPF_DIRECTION_IN); 439 #endif 440 s = splnet(); 441 IF_INPUT_ENQUEUE(&ip6intrq, m); 442 schednetisr(NETISR_IPV6); 443 splx(s); 444 } 445 #endif /* INET6 */ 446 447 int 448 mpe_newlabel(struct ifnet *ifp, int cmd, struct shim_hdr *shim) 449 { 450 struct rtentry *nrt; 451 struct sockaddr_mpls dst; 452 struct rt_addrinfo info; 453 int error; 454 455 bzero(&dst, sizeof(dst)); 456 dst.smpls_len = sizeof(dst); 457 dst.smpls_family = AF_MPLS; 458 dst.smpls_label = shim->shim_label; 459 460 bzero(&info, sizeof(info)); 461 info.rti_flags = RTF_UP | RTF_MPLS; 462 info.rti_mpls = MPLS_OP_POP; 463 info.rti_info[RTAX_DST] = smplstosa(&dst); 464 info.rti_info[RTAX_GATEWAY] = (struct sockaddr *)ifp->if_sadl; 465 466 error = rtrequest1(cmd, &info, RTP_CONNECTED, &nrt, 0); 467 rt_missmsg(cmd, &info, error ? 0 : nrt->rt_flags, ifp, error, 0); 468 if (cmd == RTM_DELETE) { 469 if (error == 0 && nrt != NULL) { 470 if (nrt->rt_refcnt <= 0) { 471 nrt->rt_refcnt++; 472 rtfree(nrt); 473 } 474 } 475 } 476 if (cmd == RTM_ADD && error == 0 && nrt != NULL) { 477 nrt->rt_refcnt--; 478 } 479 return (error); 480 } 481