Home | History | Annotate | Line # | Download | only in net
if_mpls.c revision 1.2 
      1 /*	$NetBSD: if_mpls.c,v 1.2 2010/06/26 15:17:56 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 (at) 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.2 2010/06/26 15:17:56 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) {
    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 tagged route ? */
    350 	if ((rt->rt_flags & RTF_GATEWAY) == 0 ||
    351 	     rt_gettag(rt) == NULL)
    352 		goto done;
    353 
    354 	tshim.s_addr = MPLS_GETSADDR(rt);
    355 
    356 	/* Swap labels */
    357 	if ((m->m_len < sizeof(union mpls_shim)) &&
    358 	    (m = m_pullup(m, sizeof(union mpls_shim))) == 0) {
    359 		error = ENOBUFS;
    360 		goto done;
    361 	}
    362 
    363 	/* Replace only the label */
    364 	htag = mtod(m, union mpls_shim *);
    365 	htag->s_addr = ntohl(htag->s_addr);
    366 	htag->shim.label = tshim.shim.label;
    367 	htag->s_addr = htonl(htag->s_addr);
    368 
    369 	error = mpls_send_frame(m, rt->rt_ifp, rt);
    370 
    371 done:
    372 	if (error != 0 && m != NULL)
    373 		m_freem(m);
    374 	if (rt != NULL)
    375 		RTFREE(rt);
    376 
    377 	return error;
    378 }
    379 
    380 static int
    381 mpls_send_frame(struct mbuf *m, struct ifnet *ifp, struct rtentry *rt)
    382 {
    383 	union mpls_shim msh;
    384 
    385 	if ((rt->rt_flags & RTF_GATEWAY) == 0)
    386 		return EHOSTUNREACH;
    387 
    388 	rt->rt_use++;
    389 
    390 	msh.s_addr = MPLS_GETSADDR(rt);
    391 	if (msh.shim.label == MPLS_LABEL_IMPLNULL) {
    392 		m_adj(m, sizeof(union mpls_shim));
    393 		m->m_pkthdr.csum_flags = 0;
    394 	}
    395 
    396 	switch(ifp->if_type) {
    397 	/* only these two are supported for now */
    398 	case IFT_ETHER:
    399 	case IFT_TUNNEL:
    400 		return (*ifp->if_output)(ifp, m, rt->rt_gateway, rt);
    401 	case IFT_LOOP:
    402 		break;
    403 	default:
    404 		return ENETUNREACH;
    405 	}
    406 	return 0;
    407 }
    408 
    409 
    410 
    411 #ifdef INET
    412 static int
    413 mpls_unlabel_inet(struct mbuf *m)
    414 {
    415 	int s, iphlen;
    416 	struct ip *iph;
    417 	union mpls_shim *ms;
    418 	struct ifqueue *inq;
    419 
    420 	if (mpls_mapttl_inet || mpls_mapprec_inet) {
    421 
    422 		/* get shim info */
    423 		ms = mtod(m, union mpls_shim *);
    424 		ms->s_addr = ntohl(ms->s_addr);
    425 
    426 		/* and get rid of it */
    427 		m_adj(m, sizeof(union mpls_shim));
    428 
    429 		/* get ip header */
    430 		if (m->m_len < sizeof (struct ip) &&
    431 		    (m = m_pullup(m, sizeof(struct ip))) == NULL)
    432 			return ENOBUFS;
    433 		iph = mtod(m, struct ip *);
    434 		iphlen = iph->ip_hl << 2;
    435 
    436 		/* get it all */
    437 		if (m->m_len < iphlen) {
    438 			if ((m = m_pullup(m, iphlen)) == NULL)
    439 				return ENOBUFS;
    440 			iph = mtod(m, struct ip *);
    441 		}
    442 
    443 		/* check ipsum */
    444 		if (in_cksum(m, iphlen) != 0) {
    445 			m_freem(m);
    446 			return EINVAL;
    447 		}
    448 
    449 		/* set IP ttl from MPLS ttl */
    450 		if (mpls_mapttl_inet)
    451 			iph->ip_ttl = ms->shim.ttl;
    452 
    453 		/* set IP Precedence from MPLS Exp */
    454 		if (mpls_mapprec_inet) {
    455 			iph->ip_tos = (iph->ip_tos << 3) >> 3;
    456 			iph->ip_tos |= ms->shim.exp << 5;
    457 		}
    458 
    459 		/* reset ipsum because we modified TTL and TOS */
    460 		iph->ip_sum = 0;
    461 		iph->ip_sum = in_cksum(m, iphlen);
    462 	} else
    463 		m_adj(m, sizeof(union mpls_shim));
    464 
    465 	/* Put it on IP queue */
    466 	inq = &ipintrq;
    467 	s = splnet();
    468 	if (IF_QFULL(inq)) {
    469 		IF_DROP(inq);
    470 		splx(s);
    471 		m_freem(m);
    472 		return ENOBUFS;
    473 	}
    474 	IF_ENQUEUE(inq, m);
    475 	splx(s);
    476 	schednetisr(NETISR_IP);
    477 
    478 	return 0;
    479 }
    480 
    481 /*
    482  * Prepend MPLS label
    483  */
    484 static struct mbuf *
    485 mpls_label_inet(struct mbuf *m, union mpls_shim *ms)
    486 {
    487 	struct ip *iphdr;
    488 
    489 	if (mpls_mapttl_inet || mpls_mapprec_inet) {
    490 		if ((m->m_len < sizeof(struct ip)) &&
    491 		    (m = m_pullup(m, sizeof(struct ip))) == 0)
    492 			return NULL;
    493 		iphdr = mtod(m, struct ip *);
    494 
    495 		/* Map TTL */
    496 		if (mpls_mapttl_inet)
    497 			ms->shim.ttl = iphdr->ip_ttl;
    498 
    499 		/* Copy IP precedence to EXP */
    500 		if (mpls_mapprec_inet)
    501 			ms->shim.exp = ((u_int8_t)iphdr->ip_tos) >> 5;
    502 	}
    503 
    504 	if ((m = mpls_prepend_shim(m, ms)) == NULL)
    505 		return NULL;
    506 
    507 	return m;
    508 }
    509 
    510 #endif	/* INET */
    511 
    512 #ifdef INET6
    513 
    514 static int
    515 mpls_unlabel_inet6(struct mbuf *m)
    516 {
    517 	struct ip6_hdr *ip6hdr;
    518 	union mpls_shim ms;
    519 	struct ifqueue *inq;
    520 	int s;
    521 
    522 	/* TODO: mapclass */
    523 	if (mpls_mapttl_inet6) {
    524 		ms.s_addr = ntohl(mtod(m, union mpls_shim *)->s_addr);
    525 		m_adj(m, sizeof(union mpls_shim));
    526 
    527 		if (m->m_len < sizeof (struct ip6_hdr) &&
    528 		    (m = m_pullup(m, sizeof(struct ip6_hdr))) == 0)
    529 			return ENOBUFS;
    530 		ip6hdr = mtod(m, struct ip6_hdr *);
    531 
    532 		/* Because we just decremented this in mpls_lse */
    533 		ip6hdr->ip6_hlim = ms.shim.ttl + 1;
    534 	} else
    535 		m_adj(m, sizeof(union mpls_shim));
    536 
    537 	/* Put it back on IPv6 stack */
    538 	schednetisr(NETISR_IPV6);
    539 	inq = &ip6intrq;
    540 	s = splnet();
    541 	if (IF_QFULL(inq)) {
    542 		IF_DROP(inq);
    543 		splx(s);
    544 		m_freem(m);
    545 		return ENOBUFS;
    546 	}
    547 
    548 	IF_ENQUEUE(inq, m);
    549 	splx(s);
    550 
    551 	return 0;
    552 }
    553 
    554 static struct mbuf *
    555 mpls_label_inet6(struct mbuf *m, union mpls_shim *ms)
    556 {
    557 	struct ip6_hdr *ip6h;
    558 
    559 	if (mpls_mapttl_inet6 || mpls_mapclass_inet6) {
    560 		if (m->m_len < sizeof(struct ip6_hdr) &&
    561 		    (m = m_pullup(m, sizeof(struct ip6_hdr))) == 0)
    562 			return NULL;
    563 		ip6h = mtod(m, struct ip6_hdr *);
    564 
    565 		if (mpls_mapttl_inet6)
    566 			ms->shim.ttl = ip6h->ip6_hlim;
    567 
    568 		if (mpls_mapclass_inet6)
    569 			ms->shim.exp = ip6h->ip6_vfc << 1 >> 5;
    570 	}
    571 
    572 	if ((m = mpls_prepend_shim(m, ms)) == NULL)
    573 		return NULL;
    574 
    575 	return m;
    576 }
    577 
    578 #endif	/* INET6 */
    579 
    580 static struct mbuf *
    581 mpls_prepend_shim(struct mbuf *m, union mpls_shim *ms)
    582 {
    583 	union mpls_shim *shim;
    584 
    585 	M_PREPEND(m, sizeof(*ms), M_DONTWAIT);
    586 	if (m == NULL)
    587 		return NULL;
    588 
    589 	if (m->m_len < sizeof(union mpls_shim) &&
    590 	    (m = m_pullup(m, sizeof(union mpls_shim))) == 0)
    591 		return NULL;
    592 
    593 	shim = mtod(m, union mpls_shim *);
    594 
    595 	memcpy(shim, ms, sizeof(*shim));
    596 	shim->s_addr = htonl(shim->s_addr);
    597 
    598 	return m;
    599 }
    600