1 /* $NetBSD: if_mpls.c,v 1.3 2010/06/27 13:39:11 kefren 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.3 2010/06/27 13:39:11 kefren 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 void ifmplsattach(int); 70 71 static int mpls_clone_create(struct if_clone *, int); 72 static int mpls_clone_destroy(struct ifnet *); 73 74 static struct if_clone mpls_if_cloner = 75 IF_CLONE_INITIALIZER("mpls", mpls_clone_create, mpls_clone_destroy); 76 77 78 static void mpls_input(struct ifnet *, struct mbuf *); 79 static int mpls_output(struct ifnet *, struct mbuf *, const struct sockaddr *, 80 struct rtentry *); 81 static int mpls_ioctl(struct ifnet *, u_long, void *); 82 static int mpls_send_frame(struct mbuf *, struct ifnet *, struct rtentry *); 83 static int mpls_lse(struct mbuf *); 84 85 #ifdef INET 86 static int mpls_unlabel_inet(struct mbuf *); 87 static struct mbuf *mpls_label_inet(struct mbuf *, union mpls_shim *); 88 #endif 89 90 #ifdef INET6 91 static int mpls_unlabel_inet6(struct mbuf *); 92 static struct mbuf *mpls_label_inet6(struct mbuf *, union mpls_shim *); 93 #endif 94 95 static struct mbuf *mpls_prepend_shim(struct mbuf *, union mpls_shim *); 96 97 extern int mpls_defttl, mpls_mapttl_inet, mpls_mapttl_inet6, mpls_icmp_respond, 98 mpls_forwarding, mpls_accept, mpls_mapprec_inet, mpls_mapclass_inet6; 99 100 /* ARGSUSED */ 101 void 102 ifmplsattach(int count) 103 { 104 if_clone_attach(&mpls_if_cloner); 105 } 106 107 static int 108 mpls_clone_create(struct if_clone *ifc, int unit) 109 { 110 struct mpls_softc *sc; 111 112 sc = malloc(sizeof(*sc), M_DEVBUF, M_WAITOK | M_ZERO); 113 114 if_initname(&sc->sc_if, ifc->ifc_name, unit); 115 sc->sc_if.if_softc = sc; 116 sc->sc_if.if_type = IFT_MPLS; 117 sc->sc_if.if_addrlen = 0; 118 sc->sc_if.if_hdrlen = sizeof(union mpls_shim); 119 sc->sc_if.if_dlt = DLT_NULL; 120 sc->sc_if.if_mtu = 1500; 121 sc->sc_if.if_flags = 0; 122 sc->sc_if.if_input = mpls_input; 123 sc->sc_if.if_output = mpls_output; 124 sc->sc_if.if_ioctl = mpls_ioctl; 125 126 if_attach(&sc->sc_if); 127 if_alloc_sadl(&sc->sc_if); 128 bpf_attach(&sc->sc_if, DLT_NULL, sizeof(uint32_t)); 129 return 0; 130 } 131 132 static int 133 mpls_clone_destroy(struct ifnet *ifp) 134 { 135 int s; 136 137 bpf_detach(ifp); 138 139 s = splnet(); 140 if_detach(ifp); 141 splx(s); 142 143 free(ifp->if_softc, M_DEVBUF); 144 return 0; 145 } 146 147 static void 148 mpls_input(struct ifnet *ifp, struct mbuf *m) 149 { 150 #if 0 151 /* 152 * TODO - kefren 153 * I'd love to unshim the packet, guess family 154 * and pass it to bpf 155 */ 156 bpf_mtap_af(ifp, AF_MPLS, m); 157 #endif 158 159 mpls_lse(m); 160 } 161 162 void 163 mplsintr(void) 164 { 165 struct mbuf *m; 166 int s; 167 168 while (!IF_IS_EMPTY(&mplsintrq)) { 169 s = splnet(); 170 IF_DEQUEUE(&mplsintrq, m); 171 splx(s); 172 173 if (!m) 174 return; 175 176 if (((m->m_flags & M_PKTHDR) == 0) || 177 (m->m_pkthdr.rcvif == 0)) 178 panic("mplsintr(): no pkthdr or rcvif"); 179 180 #ifdef MBUFTRACE 181 m_claimm(m, &mpls_owner); 182 #endif 183 mpls_input(m->m_pkthdr.rcvif, m); 184 } 185 } 186 187 /* 188 * prepend shim and deliver 189 */ 190 static int 191 mpls_output(struct ifnet *ifp, struct mbuf *m, const struct sockaddr *dst, struct rtentry *rt) 192 { 193 union mpls_shim mh; 194 struct rtentry *rt1; 195 int err; 196 197 if ((ifp->if_flags & (IFF_UP|IFF_RUNNING)) != (IFF_UP|IFF_RUNNING)) { 198 m_freem(m); 199 return ENETDOWN; 200 } 201 202 if (rt_gettag(rt) == NULL || rt_gettag(rt)->sa_family != AF_MPLS) { 203 m_freem(m); 204 return EINVAL; 205 } 206 207 bpf_mtap_af(ifp, dst->sa_family, m); 208 209 mh.s_addr=MPLS_GETSADDR(rt); 210 mh.shim.bos=1; 211 mh.shim.exp=0; 212 mh.shim.ttl=mpls_defttl; 213 214 switch(dst->sa_family) { 215 #ifdef INET 216 case AF_INET: 217 m = mpls_label_inet(m, &mh); 218 break; 219 #endif 220 #ifdef INET6 221 case AF_INET6: 222 m = mpls_label_inet6(m, &mh); 223 break; 224 #endif 225 default: 226 m = mpls_prepend_shim(m, &mh); 227 break; 228 } 229 230 if (m == NULL) { 231 IF_DROP(&ifp->if_snd); 232 ifp->if_oerrors++; 233 return ENOBUFS; 234 } 235 236 ifp->if_opackets++; 237 ifp->if_obytes += m->m_pkthdr.len; 238 239 if ((rt1=rtalloc1(rt->rt_gateway, 1)) == NULL) { 240 m_freem(m); 241 return EHOSTUNREACH; 242 } 243 244 err = mpls_send_frame(m, rt1->rt_ifp, rt); 245 RTFREE(rt1); 246 return err; 247 } 248 249 static int 250 mpls_ioctl(struct ifnet *ifp, u_long cmd, void *data) 251 { 252 int error = 0, s = splnet(); 253 struct ifreq *ifr = data; 254 255 switch(cmd) { 256 case SIOCINITIFADDR: 257 ifp->if_flags |= IFF_UP | IFF_RUNNING; 258 break; 259 case SIOCSIFMTU: 260 if (ifr != NULL && ifr->ifr_mtu < 576) { 261 error = EINVAL; 262 break; 263 } 264 /* FALLTHROUGH */ 265 case SIOCGIFMTU: 266 if ((error = ifioctl_common(ifp, cmd, data)) == ENETRESET) 267 error = 0; 268 break; 269 case SIOCSIFFLAGS: 270 if ((error = ifioctl_common(ifp, cmd, data)) != 0) 271 break; 272 if (ifp->if_flags & IFF_UP) 273 ifp->if_flags |= IFF_RUNNING; 274 break; 275 default: 276 error = ifioctl_common(ifp, cmd, data); 277 break; 278 } 279 splx(s); 280 return error; 281 } 282 283 /* 284 * MPLS Label Switch Engine 285 */ 286 static int 287 mpls_lse(struct mbuf *m) 288 { 289 struct sockaddr_mpls dst; 290 union mpls_shim tshim, *htag; 291 struct rtentry *rt = NULL; 292 int error = ENOBUFS; 293 294 if (m->m_len < sizeof(union mpls_shim) && 295 (m = m_pullup(m, sizeof(union mpls_shim))) == NULL) 296 goto done; 297 298 dst.smpls_len = sizeof(struct sockaddr_mpls); 299 dst.smpls_family = AF_MPLS; 300 dst.smpls_addr.s_addr = ntohl(mtod(m, union mpls_shim *)->s_addr); 301 302 /* Check if we're accepting MPLS Frames */ 303 error = EINVAL; 304 if (!mpls_accept) 305 goto done; 306 307 /* TTL decrement */ 308 if ((m = mpls_ttl_dec(m)) == NULL) 309 goto done; 310 311 if (dst.smpls_addr.shim.label <= MPLS_LABEL_RESMAX) { 312 /* Don't swap reserved labels */ 313 switch (dst.smpls_addr.shim.label) { 314 #ifdef INET 315 case MPLS_LABEL_IPV4NULL: 316 /* Pop shim and push mbuf to IP stack */ 317 if (dst.smpls_addr.shim.bos) 318 error = mpls_unlabel_inet(m); 319 break; 320 #endif 321 #ifdef INET6 322 case MPLS_LABEL_IPV6NULL: 323 /* Pop shim and push mbuf to IPv6 stack */ 324 if (dst.smpls_addr.shim.bos) 325 error = mpls_unlabel_inet6(m); 326 break; 327 #endif 328 case MPLS_LABEL_RTALERT: /* Yeah, I'm all alerted */ 329 case MPLS_LABEL_IMPLNULL: /* This is logical only */ 330 default: /* Rest are not allowed */ 331 break; 332 } 333 goto done; 334 } 335 336 /* Check if we should do MPLS forwarding */ 337 error = EHOSTUNREACH; 338 if (!mpls_forwarding) 339 goto done; 340 341 /* Get a route to dst */ 342 dst.smpls_addr.shim.ttl = 343 dst.smpls_addr.shim.bos = 344 dst.smpls_addr.shim.exp = 0; 345 dst.smpls_addr.s_addr = htonl(dst.smpls_addr.s_addr); 346 if ((rt = rtalloc1((const struct sockaddr*)&dst, 1)) == NULL) 347 goto done; 348 349 /* MPLS packet with no MPLS tagged route ? */ 350 if ((rt->rt_flags & RTF_GATEWAY) == 0 || 351 rt_gettag(rt) == NULL || 352 rt_gettag(rt)->sa_family != AF_MPLS) 353 goto done; 354 355 tshim.s_addr = MPLS_GETSADDR(rt); 356 357 /* Swap labels */ 358 if ((m->m_len < sizeof(union mpls_shim)) && 359 (m = m_pullup(m, sizeof(union mpls_shim))) == 0) { 360 error = ENOBUFS; 361 goto done; 362 } 363 364 /* Replace only the label */ 365 htag = mtod(m, union mpls_shim *); 366 htag->s_addr = ntohl(htag->s_addr); 367 htag->shim.label = tshim.shim.label; 368 htag->s_addr = htonl(htag->s_addr); 369 370 error = mpls_send_frame(m, rt->rt_ifp, rt); 371 372 done: 373 if (error != 0 && m != NULL) 374 m_freem(m); 375 if (rt != NULL) 376 RTFREE(rt); 377 378 return error; 379 } 380 381 static int 382 mpls_send_frame(struct mbuf *m, struct ifnet *ifp, struct rtentry *rt) 383 { 384 union mpls_shim msh; 385 386 if ((rt->rt_flags & RTF_GATEWAY) == 0) 387 return EHOSTUNREACH; 388 389 rt->rt_use++; 390 391 msh.s_addr = MPLS_GETSADDR(rt); 392 if (msh.shim.label == MPLS_LABEL_IMPLNULL) { 393 m_adj(m, sizeof(union mpls_shim)); 394 m->m_pkthdr.csum_flags = 0; 395 } 396 397 switch(ifp->if_type) { 398 /* only these two are supported for now */ 399 case IFT_ETHER: 400 case IFT_TUNNEL: 401 return (*ifp->if_output)(ifp, m, rt->rt_gateway, rt); 402 case IFT_LOOP: 403 break; 404 default: 405 return ENETUNREACH; 406 } 407 return 0; 408 } 409 410 411 412 #ifdef INET 413 static int 414 mpls_unlabel_inet(struct mbuf *m) 415 { 416 int s, iphlen; 417 struct ip *iph; 418 union mpls_shim *ms; 419 struct ifqueue *inq; 420 421 if (mpls_mapttl_inet || mpls_mapprec_inet) { 422 423 /* get shim info */ 424 ms = mtod(m, union mpls_shim *); 425 ms->s_addr = ntohl(ms->s_addr); 426 427 /* and get rid of it */ 428 m_adj(m, sizeof(union mpls_shim)); 429 430 /* get ip header */ 431 if (m->m_len < sizeof (struct ip) && 432 (m = m_pullup(m, sizeof(struct ip))) == NULL) 433 return ENOBUFS; 434 iph = mtod(m, struct ip *); 435 iphlen = iph->ip_hl << 2; 436 437 /* get it all */ 438 if (m->m_len < iphlen) { 439 if ((m = m_pullup(m, iphlen)) == NULL) 440 return ENOBUFS; 441 iph = mtod(m, struct ip *); 442 } 443 444 /* check ipsum */ 445 if (in_cksum(m, iphlen) != 0) { 446 m_freem(m); 447 return EINVAL; 448 } 449 450 /* set IP ttl from MPLS ttl */ 451 if (mpls_mapttl_inet) 452 iph->ip_ttl = ms->shim.ttl; 453 454 /* set IP Precedence from MPLS Exp */ 455 if (mpls_mapprec_inet) { 456 iph->ip_tos = (iph->ip_tos << 3) >> 3; 457 iph->ip_tos |= ms->shim.exp << 5; 458 } 459 460 /* reset ipsum because we modified TTL and TOS */ 461 iph->ip_sum = 0; 462 iph->ip_sum = in_cksum(m, iphlen); 463 } else 464 m_adj(m, sizeof(union mpls_shim)); 465 466 /* Put it on IP queue */ 467 inq = &ipintrq; 468 s = splnet(); 469 if (IF_QFULL(inq)) { 470 IF_DROP(inq); 471 splx(s); 472 m_freem(m); 473 return ENOBUFS; 474 } 475 IF_ENQUEUE(inq, m); 476 splx(s); 477 schednetisr(NETISR_IP); 478 479 return 0; 480 } 481 482 /* 483 * Prepend MPLS label 484 */ 485 static struct mbuf * 486 mpls_label_inet(struct mbuf *m, union mpls_shim *ms) 487 { 488 struct ip *iphdr; 489 490 if (mpls_mapttl_inet || mpls_mapprec_inet) { 491 if ((m->m_len < sizeof(struct ip)) && 492 (m = m_pullup(m, sizeof(struct ip))) == 0) 493 return NULL; 494 iphdr = mtod(m, struct ip *); 495 496 /* Map TTL */ 497 if (mpls_mapttl_inet) 498 ms->shim.ttl = iphdr->ip_ttl; 499 500 /* Copy IP precedence to EXP */ 501 if (mpls_mapprec_inet) 502 ms->shim.exp = ((u_int8_t)iphdr->ip_tos) >> 5; 503 } 504 505 if ((m = mpls_prepend_shim(m, ms)) == NULL) 506 return NULL; 507 508 return m; 509 } 510 511 #endif /* INET */ 512 513 #ifdef INET6 514 515 static int 516 mpls_unlabel_inet6(struct mbuf *m) 517 { 518 struct ip6_hdr *ip6hdr; 519 union mpls_shim ms; 520 struct ifqueue *inq; 521 int s; 522 523 /* TODO: mapclass */ 524 if (mpls_mapttl_inet6) { 525 ms.s_addr = ntohl(mtod(m, union mpls_shim *)->s_addr); 526 m_adj(m, sizeof(union mpls_shim)); 527 528 if (m->m_len < sizeof (struct ip6_hdr) && 529 (m = m_pullup(m, sizeof(struct ip6_hdr))) == 0) 530 return ENOBUFS; 531 ip6hdr = mtod(m, struct ip6_hdr *); 532 533 /* Because we just decremented this in mpls_lse */ 534 ip6hdr->ip6_hlim = ms.shim.ttl + 1; 535 } else 536 m_adj(m, sizeof(union mpls_shim)); 537 538 /* Put it back on IPv6 stack */ 539 schednetisr(NETISR_IPV6); 540 inq = &ip6intrq; 541 s = splnet(); 542 if (IF_QFULL(inq)) { 543 IF_DROP(inq); 544 splx(s); 545 m_freem(m); 546 return ENOBUFS; 547 } 548 549 IF_ENQUEUE(inq, m); 550 splx(s); 551 552 return 0; 553 } 554 555 static struct mbuf * 556 mpls_label_inet6(struct mbuf *m, union mpls_shim *ms) 557 { 558 struct ip6_hdr *ip6h; 559 560 if (mpls_mapttl_inet6 || mpls_mapclass_inet6) { 561 if (m->m_len < sizeof(struct ip6_hdr) && 562 (m = m_pullup(m, sizeof(struct ip6_hdr))) == 0) 563 return NULL; 564 ip6h = mtod(m, struct ip6_hdr *); 565 566 if (mpls_mapttl_inet6) 567 ms->shim.ttl = ip6h->ip6_hlim; 568 569 if (mpls_mapclass_inet6) 570 ms->shim.exp = ip6h->ip6_vfc << 1 >> 5; 571 } 572 573 if ((m = mpls_prepend_shim(m, ms)) == NULL) 574 return NULL; 575 576 return m; 577 } 578 579 #endif /* INET6 */ 580 581 static struct mbuf * 582 mpls_prepend_shim(struct mbuf *m, union mpls_shim *ms) 583 { 584 union mpls_shim *shim; 585 586 M_PREPEND(m, sizeof(*ms), M_DONTWAIT); 587 if (m == NULL) 588 return NULL; 589 590 if (m->m_len < sizeof(union mpls_shim) && 591 (m = m_pullup(m, sizeof(union mpls_shim))) == 0) 592 return NULL; 593 594 shim = mtod(m, union mpls_shim *); 595 596 memcpy(shim, ms, sizeof(*shim)); 597 shim->s_addr = htonl(shim->s_addr); 598 599 return m; 600 } 601