1 /* $OpenBSD: if_mpw.c,v 1.14 2016/04/13 11:41:15 mpi Exp $ */ 2 3 /* 4 * Copyright (c) 2015 Rafael Zalamena <rzalamena@openbsd.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 19 #include "bpfilter.h" 20 #include "vlan.h" 21 22 #include <sys/param.h> 23 #include <sys/systm.h> 24 #include <sys/mbuf.h> 25 #include <sys/socket.h> 26 #include <sys/ioctl.h> 27 #include <sys/errno.h> 28 29 #include <net/if.h> 30 #include <net/if_dl.h> 31 #include <net/if_types.h> 32 #include <net/route.h> 33 34 #include <netinet/in.h> 35 36 #include <netinet/if_ether.h> 37 #include <netmpls/mpls.h> 38 39 #if NBPFILTER > 0 40 #include <net/bpf.h> 41 #endif /* NBPFILTER */ 42 43 #if NVLAN > 0 44 #include <net/if_vlan_var.h> 45 #endif 46 47 struct mpw_softc { 48 struct ifnet sc_if; 49 50 struct ifaddr sc_ifa; 51 struct sockaddr_mpls sc_smpls; /* Local label */ 52 53 uint32_t sc_flags; 54 uint32_t sc_type; 55 struct shim_hdr sc_rshim; 56 struct sockaddr_storage sc_nexthop; 57 }; 58 59 void mpwattach(int); 60 int mpw_clone_create(struct if_clone *, int); 61 int mpw_clone_destroy(struct ifnet *); 62 int mpw_ioctl(struct ifnet *, u_long, caddr_t); 63 int mpw_output(struct ifnet *, struct mbuf *, struct sockaddr *, 64 struct rtentry *); 65 void mpw_start(struct ifnet *); 66 int mpw_input(struct ifnet *, struct mbuf *, void *); 67 #if NVLAN > 0 68 struct mbuf *mpw_vlan_handle(struct mbuf *, struct mpw_softc *); 69 #endif /* NVLAN */ 70 71 struct if_clone mpw_cloner = 72 IF_CLONE_INITIALIZER("mpw", mpw_clone_create, mpw_clone_destroy); 73 74 void 75 mpwattach(int n) 76 { 77 if_clone_attach(&mpw_cloner); 78 } 79 80 int 81 mpw_clone_create(struct if_clone *ifc, int unit) 82 { 83 struct mpw_softc *sc; 84 struct ifnet *ifp; 85 86 sc = malloc(sizeof(*sc), M_DEVBUF, M_NOWAIT | M_ZERO); 87 if (sc == NULL) 88 return (ENOMEM); 89 90 ifp = &sc->sc_if; 91 snprintf(ifp->if_xname, sizeof(ifp->if_xname), "mpw%d", unit); 92 ifp->if_softc = sc; 93 ifp->if_mtu = ETHERMTU; 94 ifp->if_flags = IFF_POINTOPOINT; 95 ifp->if_ioctl = mpw_ioctl; 96 ifp->if_output = mpw_output; 97 ifp->if_start = mpw_start; 98 ifp->if_type = IFT_MPLSTUNNEL; 99 ifp->if_hdrlen = ETHER_HDR_LEN; 100 IFQ_SET_MAXLEN(&ifp->if_snd, IFQ_MAXLEN); 101 102 if_attach(ifp); 103 if_alloc_sadl(ifp); 104 105 sc->sc_ifa.ifa_ifp = ifp; 106 sc->sc_ifa.ifa_addr = sdltosa(ifp->if_sadl); 107 sc->sc_smpls.smpls_len = sizeof(sc->sc_smpls); 108 sc->sc_smpls.smpls_family = AF_MPLS; 109 110 if_ih_insert(ifp, mpw_input, NULL); 111 112 #if NBPFILTER > 0 113 bpfattach(&ifp->if_bpf, ifp, DLT_EN10MB, ETHER_HDR_LEN); 114 #endif /* NBFILTER */ 115 116 return (0); 117 } 118 119 int 120 mpw_clone_destroy(struct ifnet *ifp) 121 { 122 struct mpw_softc *sc = ifp->if_softc; 123 int s; 124 125 ifp->if_flags &= ~IFF_RUNNING; 126 127 if (sc->sc_smpls.smpls_label) { 128 s = splsoftnet(); 129 rt_ifa_del(&sc->sc_ifa, RTF_MPLS, 130 smplstosa(&sc->sc_smpls)); 131 splx(s); 132 } 133 134 if_ih_remove(ifp, mpw_input, NULL); 135 136 if_detach(ifp); 137 free(sc, M_DEVBUF, sizeof(*sc)); 138 139 return (0); 140 } 141 142 int 143 mpw_input(struct ifnet *ifp, struct mbuf *m, void *cookie) 144 { 145 /* Don't have local broadcast. */ 146 m_freem(m); 147 return (1); 148 } 149 150 int 151 mpw_ioctl(struct ifnet *ifp, u_long cmd, caddr_t data) 152 { 153 struct ifreq *ifr = (struct ifreq *) data; 154 struct mpw_softc *sc = ifp->if_softc; 155 struct sockaddr_in *sin; 156 struct sockaddr_in *sin_nexthop; 157 int error = 0; 158 int s; 159 struct ifmpwreq imr; 160 161 switch (cmd) { 162 case SIOCSIFMTU: 163 if (ifr->ifr_mtu < MPE_MTU_MIN || 164 ifr->ifr_mtu > MPE_MTU_MAX) 165 error = EINVAL; 166 else 167 ifp->if_mtu = ifr->ifr_mtu; 168 break; 169 170 case SIOCSIFFLAGS: 171 if ((ifp->if_flags & IFF_UP)) 172 ifp->if_flags |= IFF_RUNNING; 173 else 174 ifp->if_flags &= ~IFF_RUNNING; 175 break; 176 177 case SIOCSETMPWCFG: 178 error = suser(curproc, 0); 179 if (error != 0) 180 break; 181 182 error = copyin(ifr->ifr_data, &imr, sizeof(imr)); 183 if (error != 0) 184 break; 185 186 /* Teardown all configuration if got no nexthop */ 187 sin = (struct sockaddr_in *) &imr.imr_nexthop; 188 if (sin->sin_addr.s_addr == 0) { 189 s = splsoftnet(); 190 if (rt_ifa_del(&sc->sc_ifa, RTF_MPLS, 191 smplstosa(&sc->sc_smpls)) == 0) 192 sc->sc_smpls.smpls_label = 0; 193 splx(s); 194 195 memset(&sc->sc_rshim, 0, sizeof(sc->sc_rshim)); 196 memset(&sc->sc_nexthop, 0, sizeof(sc->sc_nexthop)); 197 sc->sc_flags = 0; 198 sc->sc_type = 0; 199 break; 200 } 201 202 /* Validate input */ 203 if (sin->sin_family != AF_INET || 204 imr.imr_lshim.shim_label > MPLS_LABEL_MAX || 205 imr.imr_lshim.shim_label <= MPLS_LABEL_RESERVED_MAX || 206 imr.imr_rshim.shim_label > MPLS_LABEL_MAX || 207 imr.imr_rshim.shim_label <= MPLS_LABEL_RESERVED_MAX) { 208 error = EINVAL; 209 break; 210 } 211 212 /* Setup labels and create inbound route */ 213 imr.imr_lshim.shim_label = 214 htonl(imr.imr_lshim.shim_label << MPLS_LABEL_OFFSET); 215 imr.imr_rshim.shim_label = 216 htonl(imr.imr_rshim.shim_label << MPLS_LABEL_OFFSET); 217 218 if (sc->sc_smpls.smpls_label != imr.imr_lshim.shim_label) { 219 s = splsoftnet(); 220 if (sc->sc_smpls.smpls_label) 221 rt_ifa_del(&sc->sc_ifa, RTF_MPLS, 222 smplstosa(&sc->sc_smpls)); 223 224 sc->sc_smpls.smpls_label = imr.imr_lshim.shim_label; 225 error = rt_ifa_add(&sc->sc_ifa, RTF_MPLS, 226 smplstosa(&sc->sc_smpls)); 227 splx(s); 228 if (error != 0) { 229 sc->sc_smpls.smpls_label = 0; 230 break; 231 } 232 } 233 234 /* Apply configuration */ 235 sc->sc_flags = imr.imr_flags; 236 sc->sc_type = imr.imr_type; 237 sc->sc_rshim.shim_label = imr.imr_rshim.shim_label; 238 sc->sc_rshim.shim_label |= MPLS_BOS_MASK; 239 240 memset(&sc->sc_nexthop, 0, sizeof(sc->sc_nexthop)); 241 sin_nexthop = (struct sockaddr_in *) &sc->sc_nexthop; 242 sin_nexthop->sin_family = sin->sin_family; 243 sin_nexthop->sin_len = sizeof(struct sockaddr_in); 244 sin_nexthop->sin_addr.s_addr = sin->sin_addr.s_addr; 245 break; 246 247 case SIOCGETMPWCFG: 248 imr.imr_flags = sc->sc_flags; 249 imr.imr_type = sc->sc_type; 250 imr.imr_lshim.shim_label = 251 ((ntohl(sc->sc_smpls.smpls_label & MPLS_LABEL_MASK)) >> 252 MPLS_LABEL_OFFSET); 253 imr.imr_rshim.shim_label = 254 ((ntohl(sc->sc_rshim.shim_label & MPLS_LABEL_MASK)) >> 255 MPLS_LABEL_OFFSET); 256 memcpy(&imr.imr_nexthop, &sc->sc_nexthop, 257 sizeof(imr.imr_nexthop)); 258 259 error = copyout(&imr, ifr->ifr_data, sizeof(imr)); 260 break; 261 262 default: 263 error = ENOTTY; 264 break; 265 } 266 267 return (error); 268 } 269 270 int 271 mpw_output(struct ifnet *ifp, struct mbuf *m, struct sockaddr *sa, 272 struct rtentry *rt) 273 { 274 struct mpw_softc *sc = ifp->if_softc; 275 struct mbuf_list ml = MBUF_LIST_INITIALIZER(); 276 struct ether_header *eh, ehc; 277 struct shim_hdr *shim; 278 int s; 279 280 if (sc->sc_type == IMR_TYPE_NONE) { 281 m_freem(m); 282 return (EHOSTUNREACH); 283 } 284 285 if (sc->sc_flags & IMR_FLAG_CONTROLWORD) { 286 shim = mtod(m, struct shim_hdr *); 287 m_adj(m, MPLS_HDRLEN); 288 289 /* 290 * The first 4 bits identifies that this packet is a 291 * control word. If the control word is configured and 292 * we received an IP datagram we shall drop it. 293 */ 294 if (shim->shim_label & CW_ZERO_MASK) { 295 ifp->if_ierrors++; 296 m_freem(m); 297 return (EINVAL); 298 } 299 300 /* We don't support fragmentation just yet. */ 301 if (shim->shim_label & CW_FRAG_MASK) { 302 ifp->if_ierrors++; 303 m_freem(m); 304 return (EINVAL); 305 } 306 } 307 308 if (sc->sc_type == IMR_TYPE_ETHERNET_TAGGED) { 309 m_copydata(m, 0, sizeof(ehc), (caddr_t) &ehc); 310 m_adj(m, ETHER_HDR_LEN); 311 312 /* Ethernet tagged expects at least 2 VLANs */ 313 if (ntohs(ehc.ether_type) != ETHERTYPE_QINQ) { 314 ifp->if_ierrors++; 315 m_freem(m); 316 return (EINVAL); 317 } 318 319 /* Remove dummy VLAN and update ethertype */ 320 if (EVL_VLANOFTAG(*mtod(m, uint16_t *)) == 0) { 321 m_adj(m, EVL_ENCAPLEN); 322 ehc.ether_type = htons(ETHERTYPE_VLAN); 323 } 324 325 M_PREPEND(m, sizeof(*eh), M_NOWAIT); 326 if (m == NULL) 327 return (ENOMEM); 328 329 eh = mtod(m, struct ether_header *); 330 memcpy(eh, &ehc, sizeof(*eh)); 331 } 332 333 ml_enqueue(&ml, m); 334 335 s = splnet(); 336 if_input(ifp, &ml); 337 splx(s); 338 339 return (0); 340 } 341 342 #if NVLAN > 0 343 extern void vlan_start(struct ifnet *ifp); 344 345 /* 346 * This routine handles VLAN tag reinsertion in packets flowing through 347 * the pseudowire. Also it does the necessary modifications to the VLANs 348 * to respect the RFC. 349 */ 350 struct mbuf * 351 mpw_vlan_handle(struct mbuf *m, struct mpw_softc *sc) 352 { 353 struct ifnet *ifp; 354 struct ifvlan *ifv; 355 356 uint16_t type = ETHERTYPE_QINQ; 357 uint16_t tag = 0; 358 359 ifp = if_get(m->m_pkthdr.ph_ifidx); 360 if (ifp != NULL && ifp->if_start == vlan_start && 361 ISSET(ifp->if_flags, IFF_RUNNING)) { 362 ifv = ifp->if_softc; 363 type = ifv->ifv_type; 364 tag = ifv->ifv_tag; 365 } 366 if_put(ifp); 367 368 return (vlan_inject(m, type, tag)); 369 } 370 #endif /* NVLAN */ 371 372 void 373 mpw_start(struct ifnet *ifp) 374 { 375 struct mpw_softc *sc = ifp->if_softc; 376 struct rtentry *rt; 377 struct ifnet *p; 378 struct mbuf *m; 379 struct shim_hdr *shim; 380 struct sockaddr_storage ss; 381 382 if (!ISSET(ifp->if_flags, IFF_RUNNING) || 383 sc->sc_rshim.shim_label == 0 || 384 sc->sc_type == IMR_TYPE_NONE) { 385 IFQ_PURGE(&ifp->if_snd); 386 return; 387 } 388 389 rt = rtalloc((struct sockaddr *)&sc->sc_nexthop, RT_RESOLVE, 0); 390 if (!rtisvalid(rt)) { 391 IFQ_PURGE(&ifp->if_snd); 392 goto rtfree; 393 } 394 395 p = if_get(rt->rt_ifidx); 396 if (p == NULL) { 397 IFQ_PURGE(&ifp->if_snd); 398 goto rtfree; 399 } 400 401 /* 402 * XXX: lie about being MPLS, so mpls_output() get the TTL from 403 * the right place. 404 */ 405 memcpy(&ss, &sc->sc_nexthop, sizeof(sc->sc_nexthop)); 406 ((struct sockaddr *)&ss)->sa_family = AF_MPLS; 407 408 while ((m = ifq_dequeue(&ifp->if_snd)) != NULL) { 409 #if NBPFILTER > 0 410 if (sc->sc_if.if_bpf) 411 bpf_mtap(sc->sc_if.if_bpf, m, BPF_DIRECTION_OUT); 412 #endif /* NBPFILTER */ 413 414 if (sc->sc_type == IMR_TYPE_ETHERNET_TAGGED) { 415 #if NVLAN > 0 416 m = mpw_vlan_handle(m, sc); 417 if (m == NULL) { 418 ifp->if_oerrors++; 419 continue; 420 } 421 #else 422 /* Ethernet tagged doesn't work without VLANs'*/ 423 m_freem(m); 424 continue; 425 #endif /* NVLAN */ 426 } 427 428 if (sc->sc_flags & IMR_FLAG_CONTROLWORD) { 429 M_PREPEND(m, sizeof(*shim), M_NOWAIT); 430 if (m == NULL) 431 continue; 432 433 shim = mtod(m, struct shim_hdr *); 434 memset(shim, 0, sizeof(*shim)); 435 } 436 437 M_PREPEND(m, sizeof(*shim), M_NOWAIT); 438 if (m == NULL) 439 continue; 440 441 shim = mtod(m, struct shim_hdr *); 442 shim->shim_label = htonl(mpls_defttl) & MPLS_TTL_MASK; 443 shim->shim_label |= sc->sc_rshim.shim_label; 444 445 /* XXX: MPLS only uses domain 0 */ 446 m->m_pkthdr.ph_rtableid = 0; 447 448 mpls_output(p, m, (struct sockaddr *)&ss, rt); 449 } 450 451 if_put(p); 452 rtfree: 453 rtfree(rt); 454 } 455