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