1 /* $OpenBSD: if_mpw.c,v 1.15 2016/09/21 07:41:49 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 124 ifp->if_flags &= ~IFF_RUNNING; 125 126 if (sc->sc_smpls.smpls_label) { 127 rt_ifa_del(&sc->sc_ifa, RTF_MPLS, 128 smplstosa(&sc->sc_smpls)); 129 } 130 131 if_ih_remove(ifp, mpw_input, NULL); 132 133 if_detach(ifp); 134 free(sc, M_DEVBUF, sizeof(*sc)); 135 136 return (0); 137 } 138 139 int 140 mpw_input(struct ifnet *ifp, struct mbuf *m, void *cookie) 141 { 142 /* Don't have local broadcast. */ 143 m_freem(m); 144 return (1); 145 } 146 147 int 148 mpw_ioctl(struct ifnet *ifp, u_long cmd, caddr_t data) 149 { 150 struct ifreq *ifr = (struct ifreq *) data; 151 struct mpw_softc *sc = ifp->if_softc; 152 struct sockaddr_in *sin; 153 struct sockaddr_in *sin_nexthop; 154 int error = 0; 155 int s; 156 struct ifmpwreq imr; 157 158 switch (cmd) { 159 case SIOCSIFMTU: 160 if (ifr->ifr_mtu < MPE_MTU_MIN || 161 ifr->ifr_mtu > MPE_MTU_MAX) 162 error = EINVAL; 163 else 164 ifp->if_mtu = ifr->ifr_mtu; 165 break; 166 167 case SIOCSIFFLAGS: 168 if ((ifp->if_flags & IFF_UP)) 169 ifp->if_flags |= IFF_RUNNING; 170 else 171 ifp->if_flags &= ~IFF_RUNNING; 172 break; 173 174 case SIOCSETMPWCFG: 175 error = suser(curproc, 0); 176 if (error != 0) 177 break; 178 179 error = copyin(ifr->ifr_data, &imr, sizeof(imr)); 180 if (error != 0) 181 break; 182 183 /* Teardown all configuration if got no nexthop */ 184 sin = (struct sockaddr_in *) &imr.imr_nexthop; 185 if (sin->sin_addr.s_addr == 0) { 186 s = splsoftnet(); 187 if (rt_ifa_del(&sc->sc_ifa, RTF_MPLS, 188 smplstosa(&sc->sc_smpls)) == 0) 189 sc->sc_smpls.smpls_label = 0; 190 splx(s); 191 192 memset(&sc->sc_rshim, 0, sizeof(sc->sc_rshim)); 193 memset(&sc->sc_nexthop, 0, sizeof(sc->sc_nexthop)); 194 sc->sc_flags = 0; 195 sc->sc_type = 0; 196 break; 197 } 198 199 /* Validate input */ 200 if (sin->sin_family != AF_INET || 201 imr.imr_lshim.shim_label > MPLS_LABEL_MAX || 202 imr.imr_lshim.shim_label <= MPLS_LABEL_RESERVED_MAX || 203 imr.imr_rshim.shim_label > MPLS_LABEL_MAX || 204 imr.imr_rshim.shim_label <= MPLS_LABEL_RESERVED_MAX) { 205 error = EINVAL; 206 break; 207 } 208 209 /* Setup labels and create inbound route */ 210 imr.imr_lshim.shim_label = 211 htonl(imr.imr_lshim.shim_label << MPLS_LABEL_OFFSET); 212 imr.imr_rshim.shim_label = 213 htonl(imr.imr_rshim.shim_label << MPLS_LABEL_OFFSET); 214 215 if (sc->sc_smpls.smpls_label != imr.imr_lshim.shim_label) { 216 s = splsoftnet(); 217 if (sc->sc_smpls.smpls_label) 218 rt_ifa_del(&sc->sc_ifa, RTF_MPLS, 219 smplstosa(&sc->sc_smpls)); 220 221 sc->sc_smpls.smpls_label = imr.imr_lshim.shim_label; 222 error = rt_ifa_add(&sc->sc_ifa, RTF_MPLS, 223 smplstosa(&sc->sc_smpls)); 224 splx(s); 225 if (error != 0) { 226 sc->sc_smpls.smpls_label = 0; 227 break; 228 } 229 } 230 231 /* Apply configuration */ 232 sc->sc_flags = imr.imr_flags; 233 sc->sc_type = imr.imr_type; 234 sc->sc_rshim.shim_label = imr.imr_rshim.shim_label; 235 sc->sc_rshim.shim_label |= MPLS_BOS_MASK; 236 237 memset(&sc->sc_nexthop, 0, sizeof(sc->sc_nexthop)); 238 sin_nexthop = (struct sockaddr_in *) &sc->sc_nexthop; 239 sin_nexthop->sin_family = sin->sin_family; 240 sin_nexthop->sin_len = sizeof(struct sockaddr_in); 241 sin_nexthop->sin_addr.s_addr = sin->sin_addr.s_addr; 242 break; 243 244 case SIOCGETMPWCFG: 245 imr.imr_flags = sc->sc_flags; 246 imr.imr_type = sc->sc_type; 247 imr.imr_lshim.shim_label = 248 ((ntohl(sc->sc_smpls.smpls_label & MPLS_LABEL_MASK)) >> 249 MPLS_LABEL_OFFSET); 250 imr.imr_rshim.shim_label = 251 ((ntohl(sc->sc_rshim.shim_label & MPLS_LABEL_MASK)) >> 252 MPLS_LABEL_OFFSET); 253 memcpy(&imr.imr_nexthop, &sc->sc_nexthop, 254 sizeof(imr.imr_nexthop)); 255 256 error = copyout(&imr, ifr->ifr_data, sizeof(imr)); 257 break; 258 259 default: 260 error = ENOTTY; 261 break; 262 } 263 264 return (error); 265 } 266 267 int 268 mpw_output(struct ifnet *ifp, struct mbuf *m, struct sockaddr *sa, 269 struct rtentry *rt) 270 { 271 struct mpw_softc *sc = ifp->if_softc; 272 struct mbuf_list ml = MBUF_LIST_INITIALIZER(); 273 struct ether_header *eh, ehc; 274 struct shim_hdr *shim; 275 int s; 276 277 if (sc->sc_type == IMR_TYPE_NONE) { 278 m_freem(m); 279 return (EHOSTUNREACH); 280 } 281 282 if (sc->sc_flags & IMR_FLAG_CONTROLWORD) { 283 shim = mtod(m, struct shim_hdr *); 284 m_adj(m, MPLS_HDRLEN); 285 286 /* 287 * The first 4 bits identifies that this packet is a 288 * control word. If the control word is configured and 289 * we received an IP datagram we shall drop it. 290 */ 291 if (shim->shim_label & CW_ZERO_MASK) { 292 ifp->if_ierrors++; 293 m_freem(m); 294 return (EINVAL); 295 } 296 297 /* We don't support fragmentation just yet. */ 298 if (shim->shim_label & CW_FRAG_MASK) { 299 ifp->if_ierrors++; 300 m_freem(m); 301 return (EINVAL); 302 } 303 } 304 305 if (sc->sc_type == IMR_TYPE_ETHERNET_TAGGED) { 306 m_copydata(m, 0, sizeof(ehc), (caddr_t) &ehc); 307 m_adj(m, ETHER_HDR_LEN); 308 309 /* Ethernet tagged expects at least 2 VLANs */ 310 if (ntohs(ehc.ether_type) != ETHERTYPE_QINQ) { 311 ifp->if_ierrors++; 312 m_freem(m); 313 return (EINVAL); 314 } 315 316 /* Remove dummy VLAN and update ethertype */ 317 if (EVL_VLANOFTAG(*mtod(m, uint16_t *)) == 0) { 318 m_adj(m, EVL_ENCAPLEN); 319 ehc.ether_type = htons(ETHERTYPE_VLAN); 320 } 321 322 M_PREPEND(m, sizeof(*eh), M_NOWAIT); 323 if (m == NULL) 324 return (ENOMEM); 325 326 eh = mtod(m, struct ether_header *); 327 memcpy(eh, &ehc, sizeof(*eh)); 328 } 329 330 ml_enqueue(&ml, m); 331 332 s = splnet(); 333 if_input(ifp, &ml); 334 splx(s); 335 336 return (0); 337 } 338 339 #if NVLAN > 0 340 extern void vlan_start(struct ifnet *ifp); 341 342 /* 343 * This routine handles VLAN tag reinsertion in packets flowing through 344 * the pseudowire. Also it does the necessary modifications to the VLANs 345 * to respect the RFC. 346 */ 347 struct mbuf * 348 mpw_vlan_handle(struct mbuf *m, struct mpw_softc *sc) 349 { 350 struct ifnet *ifp; 351 struct ifvlan *ifv; 352 353 uint16_t type = ETHERTYPE_QINQ; 354 uint16_t tag = 0; 355 356 ifp = if_get(m->m_pkthdr.ph_ifidx); 357 if (ifp != NULL && ifp->if_start == vlan_start && 358 ISSET(ifp->if_flags, IFF_RUNNING)) { 359 ifv = ifp->if_softc; 360 type = ifv->ifv_type; 361 tag = ifv->ifv_tag; 362 } 363 if_put(ifp); 364 365 return (vlan_inject(m, type, tag)); 366 } 367 #endif /* NVLAN */ 368 369 void 370 mpw_start(struct ifnet *ifp) 371 { 372 struct mpw_softc *sc = ifp->if_softc; 373 struct rtentry *rt; 374 struct ifnet *p; 375 struct mbuf *m; 376 struct shim_hdr *shim; 377 struct sockaddr_storage ss; 378 379 if (!ISSET(ifp->if_flags, IFF_RUNNING) || 380 sc->sc_rshim.shim_label == 0 || 381 sc->sc_type == IMR_TYPE_NONE) { 382 IFQ_PURGE(&ifp->if_snd); 383 return; 384 } 385 386 rt = rtalloc((struct sockaddr *)&sc->sc_nexthop, RT_RESOLVE, 0); 387 if (!rtisvalid(rt)) { 388 IFQ_PURGE(&ifp->if_snd); 389 goto rtfree; 390 } 391 392 p = if_get(rt->rt_ifidx); 393 if (p == NULL) { 394 IFQ_PURGE(&ifp->if_snd); 395 goto rtfree; 396 } 397 398 /* 399 * XXX: lie about being MPLS, so mpls_output() get the TTL from 400 * the right place. 401 */ 402 memcpy(&ss, &sc->sc_nexthop, sizeof(sc->sc_nexthop)); 403 ((struct sockaddr *)&ss)->sa_family = AF_MPLS; 404 405 while ((m = ifq_dequeue(&ifp->if_snd)) != NULL) { 406 #if NBPFILTER > 0 407 if (sc->sc_if.if_bpf) 408 bpf_mtap(sc->sc_if.if_bpf, m, BPF_DIRECTION_OUT); 409 #endif /* NBPFILTER */ 410 411 if (sc->sc_type == IMR_TYPE_ETHERNET_TAGGED) { 412 #if NVLAN > 0 413 m = mpw_vlan_handle(m, sc); 414 if (m == NULL) { 415 ifp->if_oerrors++; 416 continue; 417 } 418 #else 419 /* Ethernet tagged doesn't work without VLANs'*/ 420 m_freem(m); 421 continue; 422 #endif /* NVLAN */ 423 } 424 425 if (sc->sc_flags & IMR_FLAG_CONTROLWORD) { 426 M_PREPEND(m, sizeof(*shim), M_NOWAIT); 427 if (m == NULL) 428 continue; 429 430 shim = mtod(m, struct shim_hdr *); 431 memset(shim, 0, sizeof(*shim)); 432 } 433 434 M_PREPEND(m, sizeof(*shim), M_NOWAIT); 435 if (m == NULL) 436 continue; 437 438 shim = mtod(m, struct shim_hdr *); 439 shim->shim_label = htonl(mpls_defttl) & MPLS_TTL_MASK; 440 shim->shim_label |= sc->sc_rshim.shim_label; 441 442 /* XXX: MPLS only uses domain 0 */ 443 m->m_pkthdr.ph_rtableid = 0; 444 445 mpls_output(p, m, (struct sockaddr *)&ss, rt); 446 } 447 448 if_put(p); 449 rtfree: 450 rtfree(rt); 451 } 452