update QoS Tx/Rx sequence numbers for each TID.

add a parameter to ieee80211_decap() to handle different 802.11
header sizes.
cleanup and clarify ieee80211_classify().

4 files changed, 90 insertions, 67 deletions

diff --git a/sys/net80211/ieee80211_crypto.c b/sys/net80211/ieee80211_crypto.c
index 4f1c163cf11..a765ce910d3 100644
--- a/sys/net80211/ieee80211_crypto.c
+++ b/sys/net80211/ieee80211_crypto.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: ieee80211_crypto.c,v 1.14 2007/07/05 20:18:02 damien Exp $	*/
+/*	$OpenBSD: ieee80211_crypto.c,v 1.15 2007/07/14 19:58:04 damien Exp $	*/
 /*	$NetBSD: ieee80211_crypto.c,v 1.5 2003/12/14 09:56:53 dyoung Exp $	*/
 
 /*-
@@ -171,8 +171,14 @@ ieee80211_wep_crypt(struct ifnet *ifp, struct mbuf *m0, int txflag)
 		if (n->m_flags & M_EXT)
 			n->m_len = n->m_ext.ext_size;
 	}
-	len = sizeof(struct ieee80211_frame);
-	memcpy(mtod(n, caddr_t), mtod(m, caddr_t), len);
+	wh = mtod(m, struct ieee80211_frame *);
+	if ((wh->i_fc[0] &
+	     (IEEE80211_FC0_TYPE_MASK | IEEE80211_FC0_SUBTYPE_QOS)) ==
+	    (IEEE80211_FC0_TYPE_DATA | IEEE80211_FC0_SUBTYPE_QOS))
+		len = sizeof(struct ieee80211_qosframe);
+	else
+		len = sizeof(struct ieee80211_frame);
+	memcpy(mtod(n, caddr_t), wh, len);
 	wh = mtod(n, struct ieee80211_frame *);
 	left -= len;
 	moff = len;
diff --git a/sys/net80211/ieee80211_input.c b/sys/net80211/ieee80211_input.c
index e5e251adece..f5c94ef6b32 100644
--- a/sys/net80211/ieee80211_input.c
+++ b/sys/net80211/ieee80211_input.c
@@ -1,5 +1,5 @@
 /*	$NetBSD: ieee80211_input.c,v 1.24 2004/05/31 11:12:24 dyoung Exp $	*/
-/*	$OpenBSD: ieee80211_input.c,v 1.43 2007/07/13 19:56:03 damien Exp $	*/
+/*	$OpenBSD: ieee80211_input.c,v 1.44 2007/07/14 19:58:05 damien Exp $	*/
 /*-
  * Copyright (c) 2001 Atsushi Onoe
  * Copyright (c) 2002, 2003 Sam Leffler, Errno Consulting
@@ -117,9 +117,9 @@ ieee80211_input(struct ifnet *ifp, struct mbuf *m, struct ieee80211_node *ni,
 	struct ieee80211_frame *wh;
 	struct ether_header *eh;
 	struct mbuf *m1;
-	int error, len;
+	int error, hdrlen, len;
 	u_int8_t dir, type, subtype;
-	u_int16_t rxseq;
+	u_int16_t orxseq, nrxseq;
 
 	if (ni == NULL)
 		panic("null mode");
@@ -156,6 +156,7 @@ ieee80211_input(struct ifnet *ifp, struct mbuf *m, struct ieee80211_node *ni,
 
 	dir = wh->i_fc[1] & IEEE80211_FC1_DIR_MASK;
 	type = wh->i_fc[0] & IEEE80211_FC0_TYPE_MASK;
+
 	/*
 	 * NB: We are not yet prepared to handle control frames,
 	 *     but permitting drivers to send them to us allows
@@ -163,26 +164,36 @@ ieee80211_input(struct ifnet *ifp, struct mbuf *m, struct ieee80211_node *ni,
 	 */
 	if (m->m_pkthdr.len < sizeof(struct ieee80211_frame)) {
 		IEEE80211_DPRINTF2(("%s: frame too short (2), len %u\n",
-			__func__, m->m_pkthdr.len));
+		    __func__, m->m_pkthdr.len));
 		ic->ic_stats.is_rx_tooshort++;
 		goto out;
 	}
 	if (ic->ic_state != IEEE80211_S_SCAN) {
 		ni->ni_rssi = rssi;
 		ni->ni_rstamp = rstamp;
-		rxseq = ni->ni_rxseq;
-		ni->ni_rxseq =
-		    letoh16(*(u_int16_t *)wh->i_seq) >> IEEE80211_SEQ_SEQ_SHIFT;
+		if (type == IEEE80211_FC0_TYPE_DATA &&
+		    (wh->i_fc[0] & IEEE80211_FC0_SUBTYPE_QOS)) {
+			struct ieee80211_qosframe *qwh =
+			    (struct ieee80211_qosframe *)wh;
+			int tid = qwh->i_qos[0] & IEEE80211_QOS_TID;
+			orxseq = ni->ni_qos_rxseqs[tid];
+			nrxseq = ni->ni_qos_rxseqs[tid] =
+			    letoh16(*(u_int16_t *)qwh->i_seq) >>
+				IEEE80211_SEQ_SEQ_SHIFT;
+		} else {
+			orxseq = ni->ni_rxseq;
+			nrxseq = ni->ni_rxseq =
+			    letoh16(*(u_int16_t *)wh->i_seq) >>
+				IEEE80211_SEQ_SEQ_SHIFT;
+		}
 		/* TODO: fragment */
 		if ((wh->i_fc[1] & IEEE80211_FC1_RETRY) &&
-		    rxseq == ni->ni_rxseq) {
+		    orxseq == nrxseq) {
 			/* duplicate, silently discarded */
 			ic->ic_stats.is_rx_dup++; /* XXX per-station stat */
 			goto out;
 		}
 		ni->ni_inact = 0;
-		if (ic->ic_opmode == IEEE80211_M_MONITOR)
-			goto out;
 	}
 
 	if (ic->ic_set_tim != NULL &&
@@ -309,8 +320,13 @@ ieee80211_input(struct ifnet *ifp, struct mbuf *m, struct ieee80211_node *ni,
 			}
 			break;
 		case IEEE80211_M_MONITOR:
-			break;
+			/* can't get there */
+			goto out;
 		}
+		if (wh->i_fc[0] & IEEE80211_FC0_SUBTYPE_QOS)
+			hdrlen = sizeof(struct ieee80211_qosframe);
+		else
+			hdrlen = sizeof(struct ieee80211_frame);
 		if (wh->i_fc[1] & IEEE80211_FC1_WEP) {
 			if (ic->ic_flags & IEEE80211_F_WEPON) {
 				m = ieee80211_wep_crypt(ifp, m, 0);
@@ -329,7 +345,7 @@ ieee80211_input(struct ifnet *ifp, struct mbuf *m, struct ieee80211_node *ni,
 		if (ic->ic_rawbpf)
 			bpf_mtap(ic->ic_rawbpf, m, BPF_DIRECTION_IN);
 #endif
-		m = ieee80211_decap(ifp, m);
+		m = ieee80211_decap(ifp, m, hdrlen);
 		if (m == NULL) {
 			IEEE80211_DPRINTF(("%s: "
 			    "decapsulation error for src %s\n",
@@ -484,29 +500,29 @@ ieee80211_input(struct ifnet *ifp, struct mbuf *m, struct ieee80211_node *ni,
 }
 
 struct mbuf *
-ieee80211_decap(struct ifnet *ifp, struct mbuf *m)
+ieee80211_decap(struct ifnet *ifp, struct mbuf *m, int hdrlen)
 {
-	struct ether_header *eh;
 	struct ieee80211_frame wh;
+	struct ether_header *eh;
 	struct llc *llc;
 
-	if (m->m_len < sizeof(wh) + sizeof(*llc)) {
-		m = m_pullup(m, sizeof(wh) + sizeof(*llc));
+	if (m->m_len < hdrlen + sizeof(*llc)) {
+		m = m_pullup(m, hdrlen + sizeof(*llc));
 		if (m == NULL)
 			return NULL;
 	}
 	memcpy(&wh, mtod(m, caddr_t), sizeof(wh));
-	llc = (struct llc *)(mtod(m, caddr_t) + sizeof(wh));
+	llc = (struct llc *)(mtod(m, caddr_t) + hdrlen);
 	if (llc->llc_dsap == LLC_SNAP_LSAP &&
 	    llc->llc_ssap == LLC_SNAP_LSAP &&
 	    llc->llc_control == LLC_UI &&
 	    llc->llc_snap.org_code[0] == 0 &&
 	    llc->llc_snap.org_code[1] == 0 &&
 	    llc->llc_snap.org_code[2] == 0) {
-		m_adj(m, sizeof(wh) + sizeof(struct llc) - sizeof(*eh));
+		m_adj(m, hdrlen + sizeof(struct llc) - sizeof(*eh));
 		llc = NULL;
 	} else {
-		m_adj(m, sizeof(wh) - sizeof(*eh));
+		m_adj(m, hdrlen - sizeof(*eh));
 	}
 	eh = mtod(m, struct ether_header *);
 	switch (wh.i_fc[1] & IEEE80211_FC1_DIR_MASK) {
diff --git a/sys/net80211/ieee80211_output.c b/sys/net80211/ieee80211_output.c
index eb9812fa6a5..5ef66b93889 100644
--- a/sys/net80211/ieee80211_output.c
+++ b/sys/net80211/ieee80211_output.c
@@ -1,4 +1,4 @@
-/*	$OpenBSD: ieee80211_output.c,v 1.46 2007/07/13 19:59:56 damien Exp $	*/
+/*	$OpenBSD: ieee80211_output.c,v 1.47 2007/07/14 19:58:05 damien Exp $	*/
 /*	$NetBSD: ieee80211_output.c,v 1.13 2004/05/31 11:02:55 dyoung Exp $	*/
 
 /*-
@@ -66,7 +66,7 @@
 #include <net80211/ieee80211_var.h>
 
 enum	ieee80211_edca_ac ieee80211_up_to_ac(struct ieee80211com *, int);
-struct	mbuf *ieee80211_classify(struct ieee80211com *, struct mbuf *, int *);
+int	ieee80211_classify(struct ieee80211com *, struct mbuf *);
 int	ieee80211_mgmt_output(struct ifnet *, struct ieee80211_node *,
 	    struct mbuf *, int);
 u_int8_t *ieee80211_add_rsn_body(u_int8_t *, struct ieee80211com *,
@@ -370,10 +370,10 @@ ieee80211_up_to_ac(struct ieee80211com *ic, int up)
 
 /*
  * Get mbuf's user-priority: if mbuf is not VLAN tagged, select user-priority
- * based on the IP TOS precedence field.
+ * based on the DSCP (Differentiated Services Codepoint) field.
  */
-struct mbuf *
-ieee80211_classify(struct ieee80211com *ic, struct mbuf *m, int *up)
+int
+ieee80211_classify(struct ieee80211com *ic, struct mbuf *m)
 {
 #ifdef INET
 	const struct ether_header *eh;
@@ -383,57 +383,37 @@ ieee80211_classify(struct ieee80211com *ic, struct mbuf *m, int *up)
 		const struct ifvlan *ifv = m->m_pkthdr.rcvif->if_softc;
 
 		/* use VLAN 802.1D user-priority */
-		if (ifv->ifv_prio <= 7) {
-			*up = ifv->ifv_prio;
-			return m;
-		}
+		if (ifv->ifv_prio <= 7)
+			return ifv->ifv_prio;
 	}
 #endif
 #ifdef INET
 	eh = mtod(m, struct ether_header *);
 	if (eh->ether_type == htons(ETHERTYPE_IP)) {
-		const struct ip *ip;
-
-		if (m->m_len < sizeof(*eh) + sizeof(*ip)) {
-			m = m_pullup(m, sizeof(*eh) + sizeof(*ip));
-			if (m == NULL)
-				return NULL;
-		}
-		ip = (struct ip *)(mtod(m, struct ether_header *) + 1);
-
+		const struct ip *ip = (const struct ip *)(eh + 1);
 		/*
-		 * IP packet, map TOS precedence field.
+		 * Map Differentiated Services Codepoint field (see RFC2474).
+		 * Preserves backward compatibility with IP Precedence field.
 		 */
 		switch (ip->ip_tos & 0xfc) {
 		case IPTOS_PREC_PRIORITY:
-			*up = 2;
-			break;
+			return 2;
 		case IPTOS_PREC_IMMEDIATE:
-			*up = 1;
-			break;
+			return 1;
 		case IPTOS_PREC_FLASH:
-			*up = 3;
-			break;
+			return 3;
 		case IPTOS_PREC_FLASHOVERRIDE:
-			*up = 4;
-			break;
+			return 4;
 		case IPTOS_PREC_CRITIC_ECP:
-			*up = 5;
-			break;
+			return 5;
 		case IPTOS_PREC_INTERNETCONTROL:
-			*up = 6;
-			break;
+			return 6;
 		case IPTOS_PREC_NETCONTROL:
-			*up = 7;
-			break;
-		default:
-			*up = 0;
+			return 7;
 		}
-		return m;
 	}
 #endif
-	*up = 0;	/* default to Best-Effort */
-	return m;
+	return 0;	/* default to Best-Effort */
 }
 
 /*
@@ -455,7 +435,8 @@ ieee80211_encap(struct ifnet *ifp, struct mbuf *m, struct ieee80211_node **pni)
 	struct llc *llc;
 	struct m_tag *mtag;
 	u_int8_t *addr;
-	u_int dlt;
+	u_int dlt, hdrlen;
+	int addqos, tid;
 
 	/* Handle raw frames if mbuf is tagged as 802.11 */
 	if ((mtag = m_tag_find(m, PACKET_TAG_DLT, NULL)) != NULL) {
@@ -521,6 +502,15 @@ ieee80211_encap(struct ifnet *ifp, struct mbuf *m, struct ieee80211_node **pni)
 	}
 	ni->ni_inact = 0;
 
+	if ((ic->ic_flags & IEEE80211_F_QOS) &&
+	    (ni->ni_flags & IEEE80211_NODE_QOS)) {
+		tid = ieee80211_classify(ic, m);
+		hdrlen = sizeof(struct ieee80211_qosframe);
+		addqos = 1;
+	} else {
+		hdrlen = sizeof(struct ieee80211_frame);
+		addqos = 0;
+	}
 	m_adj(m, sizeof(struct ether_header) - sizeof(struct llc));
 	llc = mtod(m, struct llc *);
 	llc->llc_dsap = llc->llc_ssap = LLC_SNAP_LSAP;
@@ -529,7 +519,7 @@ ieee80211_encap(struct ifnet *ifp, struct mbuf *m, struct ieee80211_node **pni)
 	llc->llc_snap.org_code[1] = 0;
 	llc->llc_snap.org_code[2] = 0;
 	llc->llc_snap.ether_type = eh.ether_type;
-	M_PREPEND(m, sizeof(struct ieee80211_frame), M_DONTWAIT);
+	M_PREPEND(m, hdrlen, M_DONTWAIT);
 	if (m == NULL) {
 		ic->ic_stats.is_tx_nombuf++;
 		goto bad;
@@ -537,9 +527,20 @@ ieee80211_encap(struct ifnet *ifp, struct mbuf *m, struct ieee80211_node **pni)
 	wh = mtod(m, struct ieee80211_frame *);
 	wh->i_fc[0] = IEEE80211_FC0_VERSION_0 | IEEE80211_FC0_TYPE_DATA;
 	*(u_int16_t *)&wh->i_dur[0] = 0;
-	*(u_int16_t *)&wh->i_seq[0] =
-	    htole16(ni->ni_txseq << IEEE80211_SEQ_SEQ_SHIFT);
-	ni->ni_txseq++;
+	if (addqos) {
+		struct ieee80211_qosframe *qwh =
+		    (struct ieee80211_qosframe *)wh;
+		qwh->i_fc[0] |= IEEE80211_FC0_SUBTYPE_QOS;
+		qwh->i_qos[0] = tid & IEEE80211_QOS_TID;
+		qwh->i_qos[1] = 0;	/* no TXOP requested */
+		*(u_int16_t *)&qwh->i_seq[0] =
+		    htole16(ni->ni_qos_txseqs[tid] << IEEE80211_SEQ_SEQ_SHIFT);
+		ni->ni_qos_txseqs[tid]++;
+	} else {
+		*(u_int16_t *)&wh->i_seq[0] =
+		    htole16(ni->ni_txseq << IEEE80211_SEQ_SEQ_SHIFT);
+		ni->ni_txseq++;
+	}
 	switch (ic->ic_opmode) {
 	case IEEE80211_M_STA:
 		wh->i_fc[1] = IEEE80211_FC1_DIR_TODS;
diff --git a/sys/net80211/ieee80211_proto.h b/sys/net80211/ieee80211_proto.h
index 6a1b7d37234..642a956b9e1 100644
--- a/sys/net80211/ieee80211_proto.h
+++ b/sys/net80211/ieee80211_proto.h
@@ -1,4 +1,4 @@
-/*	$OpenBSD: ieee80211_proto.h,v 1.20 2007/07/06 18:18:43 damien Exp $	*/
+/*	$OpenBSD: ieee80211_proto.h,v 1.21 2007/07/14 19:58:05 damien Exp $	*/
 /*	$NetBSD: ieee80211_proto.h,v 1.3 2003/10/13 04:23:56 dyoung Exp $	*/
 
 /*-
@@ -75,7 +75,7 @@ extern	struct mbuf *ieee80211_beacon_alloc(struct ieee80211com *,
 		struct ieee80211_node *);
 extern	void ieee80211_pwrsave(struct ieee80211com *, struct ieee80211_node *,
 		struct mbuf *);
-extern	struct mbuf *ieee80211_decap(struct ifnet *, struct mbuf *);
+extern	struct mbuf *ieee80211_decap(struct ifnet *, struct mbuf *, int);
 #define	ieee80211_new_state(_ic, _nstate, _arg) \
 	(((_ic)->ic_newstate)((_ic), (_nstate), (_arg)))
 extern	u_int8_t *ieee80211_add_capinfo(u_int8_t *, struct ieee80211com *,