use txprio to control the use of exp as a priority field

by default txprio is set to 0, so the exp field will be 0. howerver,
txprio on mpe/mpw/mpip can be configured with other values or
settings like our other tunnel or encapsulation interfaces.
intermediate LSPs can use the exp field to manage their prioritisation
of encapsulated traffic.

3 files changed, 139 insertions, 13 deletions

diff --git a/sys/net/if_mpe.c b/sys/net/if_mpe.c
index f6fe6f01087..5fbf1fbf7a4 100644
--- a/sys/net/if_mpe.c
+++ b/sys/net/if_mpe.c
@@ -1,4 +1,4 @@
-/* $OpenBSD: if_mpe.c,v 1.90 2019/04/02 10:52:33 dlg Exp $ */
+/* $OpenBSD: if_mpe.c,v 1.91 2019/04/17 00:45:03 dlg Exp $ */
 
 /*
  * Copyright (c) 2008 Pierre-Yves Ritschard <pyr@spootnik.org>
@@ -55,6 +55,7 @@
 
 struct mpe_softc {
 	struct ifnet		sc_if;		/* the interface */
+	int			sc_txhprio;
 	unsigned int		sc_rdomain;
 	struct ifaddr		sc_ifa;
 	struct sockaddr_mpls	sc_smpls;
@@ -121,6 +122,7 @@ mpe_clone_create(struct if_clone *ifc, int unit)
 	bpfattach(&ifp->if_bpf, ifp, DLT_LOOP, sizeof(u_int32_t));
 #endif
 
+	sc->sc_txhprio = 0;
 	sc->sc_rdomain = 0;
 	sc->sc_ifa.ifa_ifp = ifp;
 	sc->sc_ifa.ifa_addr = sdltosa(ifp->if_sadl);
@@ -210,10 +212,13 @@ int
 mpe_output(struct ifnet *ifp, struct mbuf *m, struct sockaddr *dst,
 	struct rtentry *rt)
 {
+	struct mpe_softc *sc;
 	struct rt_mpls	*rtmpls;
 	struct shim_hdr	shim;
 	int		error;
+	int		txprio;
 	uint8_t		ttl = mpls_defttl;
+	uint8_t		tos, prio;
 	size_t		ttloff;
 	socklen_t	slen;
 
@@ -243,15 +248,22 @@ mpe_output(struct ifnet *ifp, struct mbuf *m, struct sockaddr *dst,
 
 	error = 0;
 	switch (dst->sa_family) {
-	case AF_INET:
+	case AF_INET: {
+		struct ip *ip = mtod(m, struct ip *);
+		tos = ip->ip_tos;
 		ttloff = offsetof(struct ip, ip_ttl);
 		slen = sizeof(struct sockaddr_in);
 		break;
+	}
 #ifdef INET6
-	case AF_INET6:
+	case AF_INET6: {
+		struct ip6_hdr *ip6 = mtod(m, struct ip6_hdr *);
+		uint32_t flow = bemtoh32(&ip6->ip6_flow);
+		tos = flow >> 20;
 		ttloff = offsetof(struct ip6_hdr, ip6_hlim);
 		slen = sizeof(struct sockaddr_in6);
 		break;
+	}
 #endif
 	default:
 		m_freem(m);
@@ -263,7 +275,23 @@ mpe_output(struct ifnet *ifp, struct mbuf *m, struct sockaddr *dst,
 		ttl = *(mtod(m, uint8_t *) + ttloff);
 	}
 
-	shim.shim_label = rtmpls->mpls_label | MPLS_BOS_MASK | htonl(ttl);
+	sc = ifp->if_softc;
+	txprio = sc->sc_txhprio;
+
+	switch (txprio) {
+	case IF_HDRPRIO_PACKET:
+		prio = m->m_pkthdr.pf.prio;
+		break;
+	case IF_HDRPRIO_PAYLOAD:
+		prio = IFQ_TOS2PRIO(tos);
+		break;
+	default:
+		prio = txprio;
+		break;
+	}
+
+	shim.shim_label = rtmpls->mpls_label | htonl(prio << MPLS_EXP_OFFSET) |
+	    MPLS_BOS_MASK | htonl(ttl);
 
 	m = m_prepend(m, sizeof(shim), M_NOWAIT);
 	if (m == NULL) {
@@ -389,6 +417,22 @@ mpe_ioctl(struct ifnet *ifp, u_long cmd, caddr_t data)
 		ifr->ifr_rdomainid = sc->sc_rdomain;
 		break;
 
+	case SIOCSTXHPRIO:
+		if (ifr->ifr_hdrprio == IF_HDRPRIO_PACKET ||
+		    ifr->ifr_hdrprio == IF_HDRPRIO_PAYLOAD)
+			;
+		else if (ifr->ifr_hdrprio > IF_HDRPRIO_MAX ||
+		    ifr->ifr_hdrprio < IF_HDRPRIO_MIN) {
+			error = EINVAL;
+			break;
+		}
+
+		sc->sc_txhprio = ifr->ifr_hdrprio;
+		break;
+	case SIOCGTXHPRIO:
+		ifr->ifr_hdrprio = sc->sc_txhprio;
+		break;
+
 	default:
 		return (ENOTTY);
 	}
diff --git a/sys/net/if_mpip.c b/sys/net/if_mpip.c
index 5b794c05d0f..02943d3545b 100644
--- a/sys/net/if_mpip.c
+++ b/sys/net/if_mpip.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: if_mpip.c,v 1.4 2019/04/02 10:50:16 dlg Exp $ */
+/*	$OpenBSD: if_mpip.c,v 1.5 2019/04/17 00:45:03 dlg Exp $ */
 
 /*
  * Copyright (c) 2015 Rafael Zalamena <rzalamena@openbsd.org>
@@ -55,6 +55,7 @@ struct mpip_softc {
 	unsigned int		sc_dead;
 	uint32_t		sc_flow; /* xor for mbuf flowid */
 
+	int			sc_txhprio;
 	struct ifaddr		sc_ifa;
 	struct sockaddr_mpls	sc_smpls; /* Local label */
 	unsigned int		sc_rdomain;
@@ -92,6 +93,7 @@ mpip_clone_create(struct if_clone *ifc, int unit)
 	if (sc == NULL)
 		return (ENOMEM);
 
+	sc->sc_txhprio = 0;
 	sc->sc_neighbor = 0;
 	sc->sc_cword = 0; /* default to no control word */
 	sc->sc_fword = 0; /* both sides have to agree on FAT first */
@@ -415,6 +417,22 @@ mpip_ioctl(struct ifnet *ifp, u_long cmd, caddr_t data)
 		ifr->ifr_ttl = sc->sc_ttl;
 		break;
 
+	case SIOCSTXHPRIO:
+		if (ifr->ifr_hdrprio == IF_HDRPRIO_PACKET ||
+		    ifr->ifr_hdrprio == IF_HDRPRIO_PAYLOAD)
+			;
+		else if (ifr->ifr_hdrprio > IF_HDRPRIO_MAX ||
+		    ifr->ifr_hdrprio < IF_HDRPRIO_MIN) {
+			error = EINVAL;
+			break;
+		}
+
+		sc->sc_txhprio = ifr->ifr_hdrprio;
+		break;
+	case SIOCGTXHPRIO:
+		ifr->ifr_hdrprio = sc->sc_txhprio;
+		break;
+
 	case SIOCADDMULTI:
 	case SIOCDELMULTI:
 		break;
@@ -611,8 +629,9 @@ mpip_start(struct ifnet *ifp)
 		.smpls_len = sizeof(smpls),
 		.smpls_family = AF_MPLS,
 	};
-	uint32_t bos;
-	uint8_t ttl;
+	int txprio = sc->sc_txhprio;
+	uint32_t exp, bos;
+	uint8_t tos, prio, ttl;
 
 	if (!ISSET(ifp->if_flags, IFF_RUNNING) || n == NULL) {
 		IFQ_PURGE(&ifp->if_snd);
@@ -662,6 +681,40 @@ mpip_start(struct ifnet *ifp)
 		} else
 			ttl = mpls_defttl;
 
+		switch (txprio) {
+		case IF_HDRPRIO_PACKET:
+			prio = m->m_pkthdr.pf.prio;
+			break;
+		case IF_HDRPRIO_PAYLOAD:
+			switch (m->m_pkthdr.ph_family) {
+			case AF_INET: {
+				struct ip *ip;
+				ip = mtod(m, struct ip *);
+				tos = ip->ip_tos;
+				break;
+			}
+#ifdef INET6
+			case AF_INET6: {
+				struct ip6_hdr *ip6;
+				uint32_t flow;
+				ip6 = mtod(m, struct ip6_hdr *);
+				flow = bemtoh32(&ip6->ip6_flow);
+				tos = flow >> 20;
+				break;
+			}
+#endif
+			default:
+				unhandled_af(m->m_pkthdr.ph_family);
+			}
+
+			prio = IFQ_TOS2PRIO(tos);
+			break;
+		default:
+			prio = txprio;
+			break;
+		}
+		exp = htonl(prio << MPLS_EXP_OFFSET);
+
 		if (sc->sc_cword) {
 			m = m_prepend(m, sizeof(shim), M_NOWAIT);
 			if (m == NULL)
@@ -686,7 +739,7 @@ mpip_start(struct ifnet *ifp)
 			shim = htonl(1) & MPLS_TTL_MASK;
 			shim |= htonl(flow << MPLS_LABEL_OFFSET) &
 			    MPLS_LABEL_MASK;
-			shim |= bos;
+			shim |= exp | bos;
 			*mtod(m, uint32_t *) = shim;
 
 			bos = 0;
@@ -698,7 +751,7 @@ mpip_start(struct ifnet *ifp)
 
 		shim = htonl(ttl) & MPLS_TTL_MASK;
 		shim |= n->n_rshim.shim_label;
-		shim |= bos;
+		shim |= exp | bos;
 		*mtod(m, uint32_t *) = shim;
 
 		m->m_pkthdr.ph_rtableid = sc->sc_rdomain;
diff --git a/sys/net/if_mpw.c b/sys/net/if_mpw.c
index f512239343b..f75b22d9640 100644
--- a/sys/net/if_mpw.c
+++ b/sys/net/if_mpw.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: if_mpw.c,v 1.49 2019/04/02 10:50:16 dlg Exp $ */
+/*	$OpenBSD: if_mpw.c,v 1.50 2019/04/17 00:45:03 dlg Exp $ */
 
 /*
  * Copyright (c) 2015 Rafael Zalamena <rzalamena@openbsd.org>
@@ -53,6 +53,7 @@ struct mpw_softc {
 	struct arpcom		sc_ac;
 #define sc_if			sc_ac.ac_if
 
+	int			sc_txhprio;
 	unsigned int		sc_rdomain;
 	struct ifaddr		sc_ifa;
 	struct sockaddr_mpls	sc_smpls; /* Local label */
@@ -117,6 +118,7 @@ mpw_clone_create(struct if_clone *ifc, int unit)
 	if_attach(ifp);
 	ether_ifattach(ifp);
 
+	sc->sc_txhprio = 0;
 	sc->sc_rdomain = 0;
 	sc->sc_ifa.ifa_ifp = ifp;
 	sc->sc_ifa.ifa_addr = sdltosa(ifp->if_sadl);
@@ -465,6 +467,21 @@ mpw_ioctl(struct ifnet *ifp, u_long cmd, caddr_t data)
 		ifr->ifr_rdomainid = sc->sc_rdomain;
 		break;
 
+	case SIOCSTXHPRIO:
+		if (ifr->ifr_hdrprio == IF_HDRPRIO_PACKET)
+			;
+		else if (ifr->ifr_hdrprio > IF_HDRPRIO_MAX ||
+		    ifr->ifr_hdrprio < IF_HDRPRIO_MIN) {
+			error = EINVAL;
+			break;
+		}
+
+		sc->sc_txhprio = ifr->ifr_hdrprio;
+		break;
+	case SIOCGTXHPRIO:
+		ifr->ifr_hdrprio = sc->sc_txhprio;
+		break;
+
 	case SIOCADDMULTI:
 	case SIOCDELMULTI:
 		break;
@@ -605,7 +622,9 @@ mpw_start(struct ifnet *ifp)
 		.smpls_len = sizeof(smpls),
 		.smpls_family = AF_MPLS,
 	};
-	uint32_t bos;
+	int txprio = sc->sc_txhprio;
+	uint8_t prio;
+	uint32_t exp, bos;
 
 	n = sc->sc_neighbor;
 	if (!ISSET(ifp->if_flags, IFF_RUNNING) ||
@@ -652,6 +671,16 @@ mpw_start(struct ifnet *ifp)
 			memset(shim, 0, sizeof(*shim));
 		}
 
+		switch (txprio) {
+		case IF_HDRPRIO_PACKET:
+			prio = m->m_pkthdr.pf.prio;
+			break;
+		default:
+			prio = txprio;
+			break;
+		}
+		exp = htonl(prio << MPLS_EXP_OFFSET);
+
 		bos = MPLS_BOS_MASK;
 		if (sc->sc_fword) {
 			uint32_t flow = sc->sc_flow;
@@ -665,7 +694,7 @@ mpw_start(struct ifnet *ifp)
 
 			shim = mtod(m0, struct shim_hdr *);
 			shim->shim_label = htonl(1) & MPLS_TTL_MASK;
-			shim->shim_label = MPLS_LABEL2SHIM(flow) | bos;
+			shim->shim_label = MPLS_LABEL2SHIM(flow) | exp | bos;
 
 			bos = 0;
 		}
@@ -676,7 +705,7 @@ mpw_start(struct ifnet *ifp)
 
 		shim = mtod(m0, struct shim_hdr *);
 		shim->shim_label = htonl(mpls_defttl) & MPLS_TTL_MASK;
-		shim->shim_label |= n->n_rshim.shim_label | bos;
+		shim->shim_label |= n->n_rshim.shim_label | exp | bos;
 
 		m0->m_pkthdr.ph_rtableid = sc->sc_rdomain;
 		CLR(m0->m_flags, M_BCAST|M_MCAST);