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