1 /* $NetBSD: if_mpls.c,v 1.16 2014/07/17 10:46:57 bouyer Exp $ */ 2 3 /* 4 * Copyright (c) 2010 The NetBSD Foundation, Inc. 5 * All rights reserved. 6 * 7 * This code is derived from software contributed to The NetBSD Foundation 8 * by Mihai Chelaru <kefren@NetBSD.org> 9 * 10 * Redistribution and use in source and binary forms, with or without 11 * modification, are permitted provided that the following conditions 12 * are met: 13 * 1. Redistributions of source code must retain the above copyright 14 * notice, this list of conditions and the following disclaimer. 15 * 2. Redistributions in binary form must reproduce the above copyright 16 * notice, this list of conditions and the following disclaimer in the 17 * documentation and/or other materials provided with the distribution. 18 * 19 * THIS SOFTWARE IS PROVIDED BY THE NETBSD FOUNDATION, INC. AND CONTRIBUTORS 20 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 21 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 22 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE FOUNDATION OR CONTRIBUTORS 23 * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 26 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 * POSSIBILITY OF SUCH DAMAGE. 30 */ 31 32 #include <sys/cdefs.h> 33 __KERNEL_RCSID(0, "$NetBSD: if_mpls.c,v 1.16 2014/07/17 10:46:57 bouyer Exp $"); 34 35 #include "opt_inet.h" 36 #include "opt_mpls.h" 37 38 #include <sys/param.h> 39 40 #include <sys/errno.h> 41 #include <sys/malloc.h> 42 #include <sys/mbuf.h> 43 #include <sys/sysctl.h> 44 45 #include <net/bpf.h> 46 #include <net/if.h> 47 #include <net/if_types.h> 48 #include <net/netisr.h> 49 #include <net/route.h> 50 51 #ifdef INET 52 #include <netinet/in.h> 53 #include <netinet/in_systm.h> 54 #include <netinet/in_var.h> 55 #include <netinet/ip.h> 56 #endif 57 58 #ifdef INET6 59 #include <netinet/ip6.h> 60 #include <netinet6/in6_var.h> 61 #include <netinet6/ip6_var.h> 62 #endif 63 64 #include <netmpls/mpls.h> 65 #include <netmpls/mpls_var.h> 66 67 #include "if_mpls.h" 68 69 #define TRIM_LABEL do { \ 70 m_adj(m, sizeof(union mpls_shim)); \ 71 if (m->m_len < sizeof(union mpls_shim) && \ 72 (m = m_pullup(m, sizeof(union mpls_shim))) == NULL) \ 73 goto done; \ 74 dst.smpls_addr.s_addr = ntohl(mtod(m, union mpls_shim *)->s_addr); \ 75 } while (/* CONSTCOND */ 0) 76 77 78 void ifmplsattach(int); 79 80 static int mpls_clone_create(struct if_clone *, int); 81 static int mpls_clone_destroy(struct ifnet *); 82 83 static struct if_clone mpls_if_cloner = 84 IF_CLONE_INITIALIZER("mpls", mpls_clone_create, mpls_clone_destroy); 85 86 87 static void mpls_input(struct ifnet *, struct mbuf *); 88 static int mpls_output(struct ifnet *, struct mbuf *, const struct sockaddr *, 89 struct rtentry *); 90 static int mpls_ioctl(struct ifnet *, u_long, void *); 91 static int mpls_send_frame(struct mbuf *, struct ifnet *, struct rtentry *); 92 static int mpls_lse(struct mbuf *); 93 94 #ifdef INET 95 static int mpls_unlabel_inet(struct mbuf *); 96 static struct mbuf *mpls_label_inet(struct mbuf *, union mpls_shim *, uint); 97 #endif 98 99 #ifdef INET6 100 static int mpls_unlabel_inet6(struct mbuf *); 101 static struct mbuf *mpls_label_inet6(struct mbuf *, union mpls_shim *, uint); 102 #endif 103 104 static struct mbuf *mpls_prepend_shim(struct mbuf *, union mpls_shim *); 105 106 extern int mpls_defttl, mpls_mapttl_inet, mpls_mapttl_inet6, mpls_icmp_respond, 107 mpls_forwarding, mpls_frame_accept, mpls_mapprec_inet, mpls_mapclass_inet6, 108 mpls_rfc4182; 109 110 /* ARGSUSED */ 111 void 112 ifmplsattach(int count) 113 { 114 if_clone_attach(&mpls_if_cloner); 115 } 116 117 static int 118 mpls_clone_create(struct if_clone *ifc, int unit) 119 { 120 struct mpls_softc *sc; 121 122 sc = malloc(sizeof(*sc), M_DEVBUF, M_WAITOK | M_ZERO); 123 124 if_initname(&sc->sc_if, ifc->ifc_name, unit); 125 sc->sc_if.if_softc = sc; 126 sc->sc_if.if_type = IFT_MPLS; 127 sc->sc_if.if_addrlen = 0; 128 sc->sc_if.if_hdrlen = sizeof(union mpls_shim); 129 sc->sc_if.if_dlt = DLT_NULL; 130 sc->sc_if.if_mtu = 1500; 131 sc->sc_if.if_flags = 0; 132 sc->sc_if.if_input = mpls_input; 133 sc->sc_if.if_output = mpls_output; 134 sc->sc_if.if_ioctl = mpls_ioctl; 135 136 if_attach(&sc->sc_if); 137 if_alloc_sadl(&sc->sc_if); 138 bpf_attach(&sc->sc_if, DLT_NULL, sizeof(uint32_t)); 139 return 0; 140 } 141 142 static int 143 mpls_clone_destroy(struct ifnet *ifp) 144 { 145 int s; 146 147 bpf_detach(ifp); 148 149 s = splnet(); 150 if_detach(ifp); 151 splx(s); 152 153 free(ifp->if_softc, M_DEVBUF); 154 return 0; 155 } 156 157 static void 158 mpls_input(struct ifnet *ifp, struct mbuf *m) 159 { 160 #if 0 161 /* 162 * TODO - kefren 163 * I'd love to unshim the packet, guess family 164 * and pass it to bpf 165 */ 166 bpf_mtap_af(ifp, AF_MPLS, m); 167 #endif 168 169 mpls_lse(m); 170 } 171 172 void 173 mplsintr(void) 174 { 175 struct mbuf *m; 176 int s; 177 178 while (!IF_IS_EMPTY(&mplsintrq)) { 179 s = splnet(); 180 IF_DEQUEUE(&mplsintrq, m); 181 splx(s); 182 183 if (!m) 184 return; 185 186 if (((m->m_flags & M_PKTHDR) == 0) || 187 (m->m_pkthdr.rcvif == 0)) 188 panic("mplsintr(): no pkthdr or rcvif"); 189 190 #ifdef MBUFTRACE 191 m_claimm(m, &mpls_owner); 192 #endif 193 mpls_input(m->m_pkthdr.rcvif, m); 194 } 195 } 196 197 /* 198 * prepend shim and deliver 199 */ 200 static int 201 mpls_output(struct ifnet *ifp, struct mbuf *m, const struct sockaddr *dst, struct rtentry *rt) 202 { 203 union mpls_shim mh, *pms; 204 struct rtentry *rt1; 205 int err; 206 uint psize = sizeof(struct sockaddr_mpls); 207 208 KASSERT(KERNEL_LOCKED_P()); 209 210 if ((ifp->if_flags & (IFF_UP|IFF_RUNNING)) != (IFF_UP|IFF_RUNNING)) { 211 m_freem(m); 212 return ENETDOWN; 213 } 214 215 if (rt_gettag(rt) == NULL || rt_gettag(rt)->sa_family != AF_MPLS) { 216 m_freem(m); 217 return EINVAL; 218 } 219 220 bpf_mtap_af(ifp, dst->sa_family, m); 221 222 memset(&mh, 0, sizeof(mh)); 223 mh.s_addr = MPLS_GETSADDR(rt); 224 mh.shim.bos = 1; 225 mh.shim.exp = 0; 226 mh.shim.ttl = mpls_defttl; 227 228 pms = &((struct sockaddr_mpls*)rt_gettag(rt))->smpls_addr; 229 230 while (psize <= rt_gettag(rt)->sa_len - sizeof(mh)) { 231 pms++; 232 if (mh.shim.label != MPLS_LABEL_IMPLNULL && 233 ((m = mpls_prepend_shim(m, &mh)) == NULL)) 234 return ENOBUFS; 235 memset(&mh, 0, sizeof(mh)); 236 mh.s_addr = ntohl(pms->s_addr); 237 mh.shim.bos = mh.shim.exp = 0; 238 mh.shim.ttl = mpls_defttl; 239 psize += sizeof(mh); 240 } 241 242 switch(dst->sa_family) { 243 #ifdef INET 244 case AF_INET: 245 m = mpls_label_inet(m, &mh, psize - sizeof(struct sockaddr_mpls)); 246 break; 247 #endif 248 #ifdef INET6 249 case AF_INET6: 250 m = mpls_label_inet6(m, &mh, psize - sizeof(struct sockaddr_mpls)); 251 break; 252 #endif 253 default: 254 m = mpls_prepend_shim(m, &mh); 255 break; 256 } 257 258 if (m == NULL) { 259 IF_DROP(&ifp->if_snd); 260 ifp->if_oerrors++; 261 return ENOBUFS; 262 } 263 264 ifp->if_opackets++; 265 ifp->if_obytes += m->m_pkthdr.len; 266 267 if ((rt1=rtalloc1(rt->rt_gateway, 1)) == NULL) { 268 m_freem(m); 269 return EHOSTUNREACH; 270 } 271 272 err = mpls_send_frame(m, rt1->rt_ifp, rt); 273 rtfree(rt1); 274 return err; 275 } 276 277 static int 278 mpls_ioctl(struct ifnet *ifp, u_long cmd, void *data) 279 { 280 int error = 0, s = splnet(); 281 struct ifreq *ifr = data; 282 283 switch(cmd) { 284 case SIOCINITIFADDR: 285 ifp->if_flags |= IFF_UP | IFF_RUNNING; 286 break; 287 case SIOCSIFMTU: 288 if (ifr != NULL && ifr->ifr_mtu < 576) { 289 error = EINVAL; 290 break; 291 } 292 /* FALLTHROUGH */ 293 case SIOCGIFMTU: 294 if ((error = ifioctl_common(ifp, cmd, data)) == ENETRESET) 295 error = 0; 296 break; 297 case SIOCSIFFLAGS: 298 if ((error = ifioctl_common(ifp, cmd, data)) != 0) 299 break; 300 if (ifp->if_flags & IFF_UP) 301 ifp->if_flags |= IFF_RUNNING; 302 break; 303 default: 304 error = ifioctl_common(ifp, cmd, data); 305 break; 306 } 307 splx(s); 308 return error; 309 } 310 311 /* 312 * MPLS Label Switch Engine 313 */ 314 static int 315 mpls_lse(struct mbuf *m) 316 { 317 struct sockaddr_mpls dst; 318 union mpls_shim tshim, *htag; 319 struct rtentry *rt = NULL; 320 int error = ENOBUFS; 321 uint psize = sizeof(struct sockaddr_mpls); 322 bool push_back_alert = false; 323 324 if (m->m_len < sizeof(union mpls_shim) && 325 (m = m_pullup(m, sizeof(union mpls_shim))) == NULL) 326 goto done; 327 328 dst.smpls_len = sizeof(struct sockaddr_mpls); 329 dst.smpls_family = AF_MPLS; 330 dst.smpls_addr.s_addr = ntohl(mtod(m, union mpls_shim *)->s_addr); 331 332 /* Check if we're accepting MPLS Frames */ 333 error = EINVAL; 334 if (!mpls_frame_accept) 335 goto done; 336 337 /* TTL decrement */ 338 if ((m = mpls_ttl_dec(m)) == NULL) 339 goto done; 340 341 /* RFC 4182 */ 342 if (mpls_rfc4182 != 0) 343 while((dst.smpls_addr.shim.label == MPLS_LABEL_IPV4NULL || 344 dst.smpls_addr.shim.label == MPLS_LABEL_IPV6NULL) && 345 __predict_false(dst.smpls_addr.shim.bos == 0)) 346 TRIM_LABEL; 347 348 /* RFC 3032 Section 2.1 Page 4 */ 349 if (__predict_false(dst.smpls_addr.shim.label == MPLS_LABEL_RTALERT) && 350 dst.smpls_addr.shim.bos == 0) { 351 TRIM_LABEL; 352 push_back_alert = true; 353 } 354 355 if (dst.smpls_addr.shim.label <= MPLS_LABEL_RESMAX) { 356 /* Don't swap reserved labels */ 357 switch (dst.smpls_addr.shim.label) { 358 #ifdef INET 359 case MPLS_LABEL_IPV4NULL: 360 /* Pop shim and push mbuf to IP stack */ 361 if (dst.smpls_addr.shim.bos) 362 error = mpls_unlabel_inet(m); 363 break; 364 #endif 365 #ifdef INET6 366 case MPLS_LABEL_IPV6NULL: 367 /* Pop shim and push mbuf to IPv6 stack */ 368 if (dst.smpls_addr.shim.bos) 369 error = mpls_unlabel_inet6(m); 370 break; 371 #endif 372 case MPLS_LABEL_RTALERT: /* Yeah, I'm all alerted */ 373 case MPLS_LABEL_IMPLNULL: /* This is logical only */ 374 default: /* Rest are not allowed */ 375 break; 376 } 377 goto done; 378 } 379 380 /* Check if we should do MPLS forwarding */ 381 error = EHOSTUNREACH; 382 if (!mpls_forwarding) 383 goto done; 384 385 /* Get a route to dst */ 386 dst.smpls_addr.shim.ttl = 387 dst.smpls_addr.shim.bos = 388 dst.smpls_addr.shim.exp = 0; 389 dst.smpls_addr.s_addr = htonl(dst.smpls_addr.s_addr); 390 if ((rt = rtalloc1((const struct sockaddr*)&dst, 1)) == NULL) 391 goto done; 392 393 /* MPLS packet with no MPLS tagged route ? */ 394 if ((rt->rt_flags & RTF_GATEWAY) == 0 || 395 rt_gettag(rt) == NULL || 396 rt_gettag(rt)->sa_family != AF_MPLS) 397 goto done; 398 399 tshim.s_addr = MPLS_GETSADDR(rt); 400 401 /* Swap labels */ 402 if ((m->m_len < sizeof(union mpls_shim)) && 403 (m = m_pullup(m, sizeof(union mpls_shim))) == 0) { 404 error = ENOBUFS; 405 goto done; 406 } 407 408 /* Replace only the label */ 409 htag = mtod(m, union mpls_shim *); 410 htag->s_addr = ntohl(htag->s_addr); 411 htag->shim.label = tshim.shim.label; 412 htag->s_addr = htonl(htag->s_addr); 413 414 /* check if there is anything more to prepend */ 415 htag = &((struct sockaddr_mpls*)rt_gettag(rt))->smpls_addr; 416 while (psize <= rt_gettag(rt)->sa_len - sizeof(tshim)) { 417 htag++; 418 memset(&tshim, 0, sizeof(tshim)); 419 tshim.s_addr = ntohl(htag->s_addr); 420 tshim.shim.bos = tshim.shim.exp = 0; 421 tshim.shim.ttl = mpls_defttl; 422 if (tshim.shim.label != MPLS_LABEL_IMPLNULL && 423 ((m = mpls_prepend_shim(m, &tshim)) == NULL)) 424 return ENOBUFS; 425 psize += sizeof(tshim); 426 } 427 428 if (__predict_false(push_back_alert == true)) { 429 /* re-add the router alert label */ 430 memset(&tshim, 0, sizeof(tshim)); 431 tshim.s_addr = MPLS_LABEL_RTALERT; 432 tshim.shim.bos = tshim.shim.exp = 0; 433 tshim.shim.ttl = mpls_defttl; 434 if ((m = mpls_prepend_shim(m, &tshim)) == NULL) 435 return ENOBUFS; 436 } 437 438 error = mpls_send_frame(m, rt->rt_ifp, rt); 439 440 done: 441 if (error != 0 && m != NULL) 442 m_freem(m); 443 if (rt != NULL) 444 rtfree(rt); 445 446 return error; 447 } 448 449 static int 450 mpls_send_frame(struct mbuf *m, struct ifnet *ifp, struct rtentry *rt) 451 { 452 union mpls_shim msh; 453 int ret; 454 455 if ((rt->rt_flags & RTF_GATEWAY) == 0) 456 return EHOSTUNREACH; 457 458 rt->rt_use++; 459 460 msh.s_addr = MPLS_GETSADDR(rt); 461 if (msh.shim.label == MPLS_LABEL_IMPLNULL || 462 (m->m_flags & (M_MCAST | M_BCAST))) { 463 m_adj(m, sizeof(union mpls_shim)); 464 m->m_pkthdr.csum_flags = 0; 465 } 466 467 switch(ifp->if_type) { 468 /* only these are supported for now */ 469 case IFT_ETHER: 470 case IFT_TUNNEL: 471 case IFT_LOOP: 472 KERNEL_LOCK(1, NULL); 473 ret = (*ifp->if_output)(ifp, m, rt->rt_gateway, rt); 474 KERNEL_UNLOCK_ONE(NULL); 475 return ret; 476 break; 477 default: 478 return ENETUNREACH; 479 } 480 return 0; 481 } 482 483 484 485 #ifdef INET 486 static int 487 mpls_unlabel_inet(struct mbuf *m) 488 { 489 struct ip *iph; 490 union mpls_shim *ms; 491 int iphlen; 492 493 if (mpls_mapttl_inet || mpls_mapprec_inet) { 494 495 /* get shim info */ 496 ms = mtod(m, union mpls_shim *); 497 ms->s_addr = ntohl(ms->s_addr); 498 499 /* and get rid of it */ 500 m_adj(m, sizeof(union mpls_shim)); 501 502 /* get ip header */ 503 if (m->m_len < sizeof (struct ip) && 504 (m = m_pullup(m, sizeof(struct ip))) == NULL) 505 return ENOBUFS; 506 iph = mtod(m, struct ip *); 507 iphlen = iph->ip_hl << 2; 508 509 /* get it all */ 510 if (m->m_len < iphlen) { 511 if ((m = m_pullup(m, iphlen)) == NULL) 512 return ENOBUFS; 513 iph = mtod(m, struct ip *); 514 } 515 516 /* check ipsum */ 517 if (in_cksum(m, iphlen) != 0) { 518 m_freem(m); 519 return EINVAL; 520 } 521 522 /* set IP ttl from MPLS ttl */ 523 if (mpls_mapttl_inet) 524 iph->ip_ttl = ms->shim.ttl; 525 526 /* set IP Precedence from MPLS Exp */ 527 if (mpls_mapprec_inet) { 528 iph->ip_tos = (iph->ip_tos << 3) >> 3; 529 iph->ip_tos |= ms->shim.exp << 5; 530 } 531 532 /* reset ipsum because we modified TTL and TOS */ 533 iph->ip_sum = 0; 534 iph->ip_sum = in_cksum(m, iphlen); 535 } else 536 m_adj(m, sizeof(union mpls_shim)); 537 538 /* Put it on IP queue */ 539 if (__predict_false(!pktq_enqueue(ip_pktq, m, 0))) { 540 m_freem(m); 541 return ENOBUFS; 542 } 543 return 0; 544 } 545 546 /* 547 * Prepend MPLS label 548 */ 549 static struct mbuf * 550 mpls_label_inet(struct mbuf *m, union mpls_shim *ms, uint offset) 551 { 552 struct ip iphdr; 553 554 if (mpls_mapttl_inet || mpls_mapprec_inet) { 555 if ((m->m_len < sizeof(struct ip)) && 556 (m = m_pullup(m, offset + sizeof(struct ip))) == 0) 557 return NULL; /* XXX */ 558 m_copydata(m, offset, sizeof(struct ip), &iphdr); 559 560 /* Map TTL */ 561 if (mpls_mapttl_inet) 562 ms->shim.ttl = iphdr.ip_ttl; 563 564 /* Copy IP precedence to EXP */ 565 if (mpls_mapprec_inet) 566 ms->shim.exp = ((u_int8_t)iphdr.ip_tos) >> 5; 567 } 568 569 if ((m = mpls_prepend_shim(m, ms)) == NULL) 570 return NULL; 571 572 return m; 573 } 574 575 #endif /* INET */ 576 577 #ifdef INET6 578 579 static int 580 mpls_unlabel_inet6(struct mbuf *m) 581 { 582 struct ip6_hdr *ip6hdr; 583 union mpls_shim ms; 584 585 /* TODO: mapclass */ 586 if (mpls_mapttl_inet6) { 587 ms.s_addr = ntohl(mtod(m, union mpls_shim *)->s_addr); 588 m_adj(m, sizeof(union mpls_shim)); 589 590 if (m->m_len < sizeof (struct ip6_hdr) && 591 (m = m_pullup(m, sizeof(struct ip6_hdr))) == 0) 592 return ENOBUFS; 593 ip6hdr = mtod(m, struct ip6_hdr *); 594 595 /* Because we just decremented this in mpls_lse */ 596 ip6hdr->ip6_hlim = ms.shim.ttl + 1; 597 } else 598 m_adj(m, sizeof(union mpls_shim)); 599 600 /* Put it back on IPv6 queue. */ 601 if (__predict_false(!pktq_enqueue(ip6_pktq, m, 0))) { 602 m_freem(m); 603 return ENOBUFS; 604 } 605 return 0; 606 } 607 608 static struct mbuf * 609 mpls_label_inet6(struct mbuf *m, union mpls_shim *ms, uint offset) 610 { 611 struct ip6_hdr ip6h; 612 613 if (mpls_mapttl_inet6 || mpls_mapclass_inet6) { 614 if (m->m_len < sizeof(struct ip6_hdr) && 615 (m = m_pullup(m, offset + sizeof(struct ip6_hdr))) == 0) 616 return NULL; 617 m_copydata(m, offset, sizeof(struct ip6_hdr), &ip6h); 618 619 if (mpls_mapttl_inet6) 620 ms->shim.ttl = ip6h.ip6_hlim; 621 622 if (mpls_mapclass_inet6) 623 ms->shim.exp = ip6h.ip6_vfc << 1 >> 5; 624 } 625 626 if ((m = mpls_prepend_shim(m, ms)) == NULL) 627 return NULL; 628 629 return m; 630 } 631 632 #endif /* INET6 */ 633 634 static struct mbuf * 635 mpls_prepend_shim(struct mbuf *m, union mpls_shim *ms) 636 { 637 union mpls_shim *shim; 638 639 M_PREPEND(m, sizeof(*ms), M_DONTWAIT); 640 if (m == NULL) 641 return NULL; 642 643 if (m->m_len < sizeof(union mpls_shim) && 644 (m = m_pullup(m, sizeof(union mpls_shim))) == 0) 645 return NULL; 646 647 shim = mtod(m, union mpls_shim *); 648 649 memcpy(shim, ms, sizeof(*shim)); 650 shim->s_addr = htonl(shim->s_addr); 651 652 return m; 653 } 654