implement rxprio

3 files changed, 162 insertions, 10 deletions

diff --git a/sys/net/if_mpe.c b/sys/net/if_mpe.c
index 5fbf1fbf7a4..1cfe92739e9 100644
--- a/sys/net/if_mpe.c
+++ b/sys/net/if_mpe.c
@@ -1,4 +1,4 @@
-/* $OpenBSD: if_mpe.c,v 1.91 2019/04/17 00:45:03 dlg Exp $ */
+/* $OpenBSD: if_mpe.c,v 1.92 2019/04/19 06:36:54 dlg Exp $ */
 
 /*
  * Copyright (c) 2008 Pierre-Yves Ritschard <pyr@spootnik.org>
@@ -56,6 +56,7 @@
 struct mpe_softc {
 	struct ifnet		sc_if;		/* the interface */
 	int			sc_txhprio;
+	int			sc_rxhprio;
 	unsigned int		sc_rdomain;
 	struct ifaddr		sc_ifa;
 	struct sockaddr_mpls	sc_smpls;
@@ -123,6 +124,7 @@ mpe_clone_create(struct if_clone *ifc, int unit)
 #endif
 
 	sc->sc_txhprio = 0;
+	sc->sc_rxhprio = IF_HDRPRIO_PACKET;
 	sc->sc_rdomain = 0;
 	sc->sc_ifa.ifa_ifp = ifp;
 	sc->sc_ifa.ifa_addr = sdltosa(ifp->if_sadl);
@@ -433,6 +435,23 @@ mpe_ioctl(struct ifnet *ifp, u_long cmd, caddr_t data)
 		ifr->ifr_hdrprio = sc->sc_txhprio;
 		break;
 
+	case SIOCSRXHPRIO:
+		if (ifr->ifr_hdrprio == IF_HDRPRIO_PACKET ||
+		    ifr->ifr_hdrprio == IF_HDRPRIO_PAYLOAD ||
+		    ifr->ifr_hdrprio == IF_HDRPRIO_OUTER)
+			;
+		else if (ifr->ifr_hdrprio > IF_HDRPRIO_MAX ||
+		    ifr->ifr_hdrprio < IF_HDRPRIO_MIN) {
+			error = EINVAL;
+			break;
+		}
+
+		sc->sc_rxhprio = ifr->ifr_hdrprio;
+		break;
+	case SIOCGRXHPRIO:
+		ifr->ifr_hdrprio = sc->sc_rxhprio;
+		break;
+
 	default:
 		return (ENOTTY);
 	}
@@ -443,12 +462,16 @@ mpe_ioctl(struct ifnet *ifp, u_long cmd, caddr_t data)
 void
 mpe_input(struct ifnet *ifp, struct mbuf *m)
 {
+	struct mpe_softc *sc = ifp->if_softc;
 	struct shim_hdr	*shim;
 	struct mbuf 	*n;
-	uint8_t		 ttl;
+	uint8_t		 ttl, tos;
+	uint32_t	 exp;
 	void (*input)(struct ifnet *, struct mbuf *);
+	int rxprio = sc->sc_rxhprio;
 
 	shim = mtod(m, struct shim_hdr *);
+	exp = ntohl(shim->shim_label & MPLS_EXP_MASK) >> MPLS_EXP_OFFSET;
 	if (!MPLS_BOS_ISSET(shim->shim_label))
 		goto drop;
 
@@ -463,7 +486,16 @@ mpe_input(struct ifnet *ifp, struct mbuf *m)
 	}
 
 	switch (*mtod(n, uint8_t *) >> 4) {
-	case 4:
+	case 4: {
+		struct ip *ip;
+		if (m->m_len < sizeof(*ip)) {
+			m = m_pullup(m, sizeof(*ip));
+			if (m == NULL)
+				return;
+		}
+		ip = mtod(m, struct ip *);
+		tos = ip->ip_tos;
+
 		if (mpls_mapttl_ip) {
 			m = mpls_ip_adjttl(m, ttl);
 			if (m == NULL)
@@ -472,8 +504,20 @@ mpe_input(struct ifnet *ifp, struct mbuf *m)
 		input = ipv4_input;
 		m->m_pkthdr.ph_family = AF_INET;
 		break;
+	}
 #ifdef INET6
-	case 6:
+	case 6: {
+		struct ip6_hdr *ip6;
+		uint32_t flow;
+		if (m->m_len < sizeof(*ip6)) {
+			m = m_pullup(m, sizeof(*ip6));
+			if (m == NULL)
+				return;
+		}
+		ip6 = mtod(m, struct ip6_hdr *);
+		flow = bemtoh32(&ip6->ip6_flow);
+		tos = flow >> 20;
+
 		if (mpls_mapttl_ip6) {
 			m = mpls_ip6_adjttl(m, ttl);
 			if (m == NULL)
@@ -482,11 +526,27 @@ mpe_input(struct ifnet *ifp, struct mbuf *m)
 		input = ipv6_input;
 		m->m_pkthdr.ph_family = AF_INET6;
 		break;
+	}
 #endif /* INET6 */
 	default:
 		goto drop;
 	}
 
+	switch (rxprio) {
+	case IF_HDRPRIO_PACKET:
+		/* nop */
+		break;
+	case IF_HDRPRIO_OUTER:
+		m->m_pkthdr.pf.prio = exp;
+		break;
+	case IF_HDRPRIO_PAYLOAD:
+		m->m_pkthdr.pf.prio = IFQ_TOS2PRIO(tos);
+		break;
+	default:
+		m->m_pkthdr.pf.prio = rxprio;
+		break;
+	}
+
 	/* new receive if and move into correct rtable */
 	m->m_pkthdr.ph_ifidx = ifp->if_index;
 	m->m_pkthdr.ph_rtableid = ifp->if_rdomain;
diff --git a/sys/net/if_mpip.c b/sys/net/if_mpip.c
index 02943d3545b..f82072be4d4 100644
--- a/sys/net/if_mpip.c
+++ b/sys/net/if_mpip.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: if_mpip.c,v 1.5 2019/04/17 00:45:03 dlg Exp $ */
+/*	$OpenBSD: if_mpip.c,v 1.6 2019/04/19 06:36:54 dlg Exp $ */
 
 /*
  * Copyright (c) 2015 Rafael Zalamena <rzalamena@openbsd.org>
@@ -56,6 +56,7 @@ struct mpip_softc {
 	uint32_t		sc_flow; /* xor for mbuf flowid */
 
 	int			sc_txhprio;
+	int			sc_rxhprio;
 	struct ifaddr		sc_ifa;
 	struct sockaddr_mpls	sc_smpls; /* Local label */
 	unsigned int		sc_rdomain;
@@ -94,6 +95,7 @@ mpip_clone_create(struct if_clone *ifc, int unit)
 		return (ENOMEM);
 
 	sc->sc_txhprio = 0;
+	sc->sc_rxhprio = IF_HDRPRIO_PACKET;
 	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 */
@@ -433,6 +435,23 @@ mpip_ioctl(struct ifnet *ifp, u_long cmd, caddr_t data)
 		ifr->ifr_hdrprio = sc->sc_txhprio;
 		break;
 
+	case SIOCSRXHPRIO:
+		if (ifr->ifr_hdrprio == IF_HDRPRIO_PACKET ||
+		    ifr->ifr_hdrprio == IF_HDRPRIO_PAYLOAD ||
+		    ifr->ifr_hdrprio == IF_HDRPRIO_OUTER)
+			;
+		else if (ifr->ifr_hdrprio > IF_HDRPRIO_MAX ||
+		    ifr->ifr_hdrprio < IF_HDRPRIO_MIN) {
+			error = EINVAL;
+			break;
+		}
+
+		sc->sc_rxhprio = ifr->ifr_hdrprio;
+		break;
+	case SIOCGRXHPRIO:
+		ifr->ifr_hdrprio = sc->sc_rxhprio;
+		break;
+
 	case SIOCADDMULTI:
 	case SIOCDELMULTI:
 		break;
@@ -449,9 +468,10 @@ static void
 mpip_input(struct mpip_softc *sc, struct mbuf *m)
 {
 	struct ifnet *ifp = &sc->sc_if;
-	uint32_t shim;
+	int rxprio = sc->sc_rxhprio;
+	uint32_t shim, exp;
 	struct mbuf *n;
-	uint8_t ttl;
+	uint8_t ttl, tos;
 	void (*input)(struct ifnet *, struct mbuf *);
 
 	if (!ISSET(ifp->if_flags, IFF_RUNNING))
@@ -461,6 +481,7 @@ mpip_input(struct mpip_softc *sc, struct mbuf *m)
 	m_adj(m, sizeof(shim));
 
 	ttl = ntohl(shim & MPLS_TTL_MASK);
+	exp = ntohl(shim & MPLS_EXP_MASK) >> MPLS_EXP_OFFSET;
 
 	if (sc->sc_fword) {
 		uint32_t label;
@@ -527,7 +548,16 @@ mpip_input(struct mpip_softc *sc, struct mbuf *m)
 	}
 
 	switch (*mtod(n, uint8_t *) >> 4) {
-	case 4:
+	case 4: {
+		struct ip *ip;
+		if (m->m_len < sizeof(*ip)) {
+			m = m_pullup(m, sizeof(*ip));
+			if (m == NULL)
+				return;
+		}
+		ip = mtod(m, struct ip *);
+		tos = ip->ip_tos;
+
 		if (sc->sc_ttl == -1) {
 			m = mpls_ip_adjttl(m, ttl);
 			if (m == NULL)
@@ -536,8 +566,20 @@ mpip_input(struct mpip_softc *sc, struct mbuf *m)
 		input = ipv4_input;
 		m->m_pkthdr.ph_family = AF_INET;
 		break;
+	}
 #ifdef INET6
-	case 6:
+	case 6: {
+		struct ip6_hdr *ip6;
+		uint32_t flow;
+		if (m->m_len < sizeof(*ip6)) {
+			m = m_pullup(m, sizeof(*ip6));
+			if (m == NULL)
+				return;
+		}
+		ip6 = mtod(m, struct ip6_hdr *);
+		flow = bemtoh32(&ip6->ip6_flow);
+		tos = flow >> 20;
+
 		if (sc->sc_ttl == -1) {
 			m = mpls_ip6_adjttl(m, ttl);
 			if (m == NULL)
@@ -546,12 +588,28 @@ mpip_input(struct mpip_softc *sc, struct mbuf *m)
 		input = ipv6_input;
 		m->m_pkthdr.ph_family = AF_INET6;
 		break;
+	}
 #endif /* INET6 */
 	default:
 		counters_inc(ifp->if_counters, ifc_noproto);
 		goto drop;
 	}
 
+	switch (rxprio) {
+	case IF_HDRPRIO_PACKET:
+		/* nop */
+		break;
+	case IF_HDRPRIO_OUTER:
+		m->m_pkthdr.pf.prio = exp;
+		break;
+	case IF_HDRPRIO_PAYLOAD:
+		m->m_pkthdr.pf.prio = IFQ_TOS2PRIO(tos);
+		break;
+	default:
+		m->m_pkthdr.pf.prio = rxprio;
+		break;
+	}
+
 	m->m_pkthdr.ph_ifidx = ifp->if_index;
 	m->m_pkthdr.ph_rtableid = ifp->if_rdomain;
 
diff --git a/sys/net/if_mpw.c b/sys/net/if_mpw.c
index b80f21518d3..67d081491fd 100644
--- a/sys/net/if_mpw.c
+++ b/sys/net/if_mpw.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: if_mpw.c,v 1.51 2019/04/17 01:57:21 dlg Exp $ */
+/*	$OpenBSD: if_mpw.c,v 1.52 2019/04/19 06:36:54 dlg Exp $ */
 
 /*
  * Copyright (c) 2015 Rafael Zalamena <rzalamena@openbsd.org>
@@ -54,6 +54,7 @@ struct mpw_softc {
 #define sc_if			sc_ac.ac_if
 
 	int			sc_txhprio;
+	int			sc_rxhprio;
 	unsigned int		sc_rdomain;
 	struct ifaddr		sc_ifa;
 	struct sockaddr_mpls	sc_smpls; /* Local label */
@@ -119,6 +120,7 @@ mpw_clone_create(struct if_clone *ifc, int unit)
 	ether_ifattach(ifp);
 
 	sc->sc_txhprio = 0;
+	sc->sc_rxhprio = IF_HDRPRIO_PACKET;
 	sc->sc_rdomain = 0;
 	sc->sc_ifa.ifa_ifp = ifp;
 	sc->sc_ifa.ifa_addr = sdltosa(ifp->if_sadl);
@@ -482,6 +484,22 @@ mpw_ioctl(struct ifnet *ifp, u_long cmd, caddr_t data)
 		ifr->ifr_hdrprio = sc->sc_txhprio;
 		break;
 
+	case SIOCSRXHPRIO:
+		if (ifr->ifr_hdrprio == IF_HDRPRIO_PACKET ||
+		    ifr->ifr_hdrprio == IF_HDRPRIO_OUTER)
+			;
+		else if (ifr->ifr_hdrprio > IF_HDRPRIO_MAX ||
+		    ifr->ifr_hdrprio < IF_HDRPRIO_MIN) {
+			error = EINVAL;
+			break;
+		}
+
+		sc->sc_rxhprio = ifr->ifr_hdrprio;
+		break;
+	case SIOCGRXHPRIO:
+		ifr->ifr_hdrprio = sc->sc_rxhprio;
+		break;
+
 	case SIOCADDMULTI:
 	case SIOCDELMULTI:
 		break;
@@ -501,12 +519,15 @@ mpw_input(struct mpw_softc *sc, struct mbuf *m)
 	struct ifnet *ifp = &sc->sc_if;
 	struct shim_hdr *shim;
 	struct mbuf *n;
+	uint32_t exp;
+	int rxprio;
 	int off;
 
 	if (!ISSET(ifp->if_flags, IFF_RUNNING))
 		goto drop;
 
 	shim = mtod(m, struct shim_hdr *);
+	exp = ntohl(shim->shim_label & MPLS_EXP_MASK) >> MPLS_EXP_OFFSET;
 	if (sc->sc_fword) {
 		uint32_t flow;
 
@@ -580,6 +601,19 @@ mpw_input(struct mpw_softc *sc, struct mbuf *m)
 		m = n;
 	}
 
+	rxprio = sc->sc_rxhprio;
+	switch (rxprio) {
+	case IF_HDRPRIO_PACKET:
+		/* nop */
+		break;
+	case IF_HDRPRIO_OUTER:
+		m->m_pkthdr.pf.prio = exp;
+		break;
+	default:
+		m->m_pkthdr.pf.prio = rxprio;
+		break;
+	}
+
 	m->m_pkthdr.ph_ifidx = ifp->if_index;
 	m->m_pkthdr.ph_rtableid = ifp->if_rdomain;