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