o fix wpa w/ wme: don't strip the QoS header on recv as tkip requires

  it; instead pass the space occupied by the header down into the
  crypto modules (except in the demic case which needs it only when
  doing int in s/w)
o while here fix defrag to strip the header from 2nd and later frames
o teach decap code how to handle 4-address frames

7 files changed, 41 insertions, 67 deletions

diff --git a/sys/net80211/ieee80211_crypto.c b/sys/net80211/ieee80211_crypto.c
index d6678c6..d68ca47 100644
--- a/sys/net80211/ieee80211_crypto.c
+++ b/sys/net80211/ieee80211_crypto.c
@@ -522,7 +522,7 @@ ieee80211_crypto_encap(struct ieee80211com *ic,
  */
 struct ieee80211_key *
 ieee80211_crypto_decap(struct ieee80211com *ic,
-	struct ieee80211_node *ni, struct mbuf *m)
+	struct ieee80211_node *ni, struct mbuf *m, int hdrlen)
 {
 #define	IEEE80211_WEP_HDRLEN	(IEEE80211_WEP_IVLEN + IEEE80211_WEP_KIDLEN)
 #define	IEEE80211_WEP_MINLEN \
@@ -533,7 +533,6 @@ ieee80211_crypto_decap(struct ieee80211com *ic,
 	const struct ieee80211_cipher *cip;
 	const u_int8_t *ivp;
 	u_int8_t keyid;
-	int hdrlen;
 
 	/* NB: this minimum size data frame could be bigger */
 	if (m->m_pkthdr.len < IEEE80211_WEP_MINLEN) {
@@ -551,7 +550,6 @@ ieee80211_crypto_decap(struct ieee80211com *ic,
 	 * the key id in the header is meaningless (typically 0).
 	 */
 	wh = mtod(m, struct ieee80211_frame *);
-	hdrlen = ieee80211_hdrsize(wh);
 	ivp = mtod(m, const u_int8_t *) + hdrlen;	/* XXX contig */
 	keyid = ivp[IEEE80211_WEP_IVLEN];
 	if (IEEE80211_IS_MULTICAST(wh->i_addr1) ||
@@ -573,7 +571,7 @@ ieee80211_crypto_decap(struct ieee80211com *ic,
 		return 0;
 	}
 
-	return (cip->ic_decap(k, m) ? k : NULL);
+	return (cip->ic_decap(k, m, hdrlen) ? k : NULL);
 #undef IEEE80211_WEP_MINLEN
 #undef IEEE80211_WEP_HDRLEN
 }
diff --git a/sys/net80211/ieee80211_crypto.h b/sys/net80211/ieee80211_crypto.h
index 2d09c1b..19b6dcd 100644
--- a/sys/net80211/ieee80211_crypto.h
+++ b/sys/net80211/ieee80211_crypto.h
@@ -160,7 +160,7 @@ struct ieee80211_cipher {
 	int	(*ic_setkey)(struct ieee80211_key *);
 	int	(*ic_encap)(struct ieee80211_key *, struct mbuf *,
 			u_int8_t keyid);
-	int	(*ic_decap)(struct ieee80211_key *, struct mbuf *);
+	int	(*ic_decap)(struct ieee80211_key *, struct mbuf *, int);
 	int	(*ic_enmic)(struct ieee80211_key *, struct mbuf *, int);
 	int	(*ic_demic)(struct ieee80211_key *, struct mbuf *, int);
 };
@@ -173,7 +173,7 @@ int	ieee80211_crypto_available(u_int cipher);
 struct ieee80211_key *ieee80211_crypto_encap(struct ieee80211com *,
 		struct ieee80211_node *, struct mbuf *);
 struct ieee80211_key *ieee80211_crypto_decap(struct ieee80211com *,
-		struct ieee80211_node *, struct mbuf *);
+		struct ieee80211_node *, struct mbuf *, int);
 
 /*
  * Check and remove any MIC.
diff --git a/sys/net80211/ieee80211_crypto_ccmp.c b/sys/net80211/ieee80211_crypto_ccmp.c
index 7f95966..6c28c67 100644
--- a/sys/net80211/ieee80211_crypto_ccmp.c
+++ b/sys/net80211/ieee80211_crypto_ccmp.c
@@ -67,7 +67,7 @@ static	void *ccmp_attach(struct ieee80211com *, struct ieee80211_key *);
 static	void ccmp_detach(struct ieee80211_key *);
 static	int ccmp_setkey(struct ieee80211_key *);
 static	int ccmp_encap(struct ieee80211_key *k, struct mbuf *, u_int8_t keyid);
-static	int ccmp_decap(struct ieee80211_key *, struct mbuf *);
+static	int ccmp_decap(struct ieee80211_key *, struct mbuf *, int);
 static	int ccmp_enmic(struct ieee80211_key *, struct mbuf *, int);
 static	int ccmp_demic(struct ieee80211_key *, struct mbuf *, int);
 
@@ -197,20 +197,18 @@ READ_6(uint8_t b0, uint8_t b1, uint8_t b2, uint8_t b3, uint8_t b4, uint8_t b5)
  * is also verified.
  */
 static int
-ccmp_decap(struct ieee80211_key *k, struct mbuf *m)
+ccmp_decap(struct ieee80211_key *k, struct mbuf *m, int hdrlen)
 {
 	struct ccmp_ctx *ctx = k->wk_private;
 	struct ieee80211_frame *wh;
 	uint8_t *ivp;
 	uint64_t pn;
-	int hdrlen;
 
 	/*
 	 * Header should have extended IV and sequence number;
 	 * verify the former and validate the latter.
 	 */
 	wh = mtod(m, struct ieee80211_frame *);
-	hdrlen = ieee80211_hdrsize(wh);
 	ivp = mtod(m, uint8_t *) + hdrlen;
 	if ((ivp[IEEE80211_WEP_IVLEN] & IEEE80211_WEP_EXTIV) == 0) {
 		/*
diff --git a/sys/net80211/ieee80211_crypto_none.c b/sys/net80211/ieee80211_crypto_none.c
index c8ce724..ce43134 100644
--- a/sys/net80211/ieee80211_crypto_none.c
+++ b/sys/net80211/ieee80211_crypto_none.c
@@ -52,7 +52,7 @@ static	void *none_attach(struct ieee80211com *, struct ieee80211_key *);
 static	void none_detach(struct ieee80211_key *);
 static	int none_setkey(struct ieee80211_key *);
 static	int none_encap(struct ieee80211_key *, struct mbuf *, u_int8_t);
-static	int none_decap(struct ieee80211_key *, struct mbuf *);
+static	int none_decap(struct ieee80211_key *, struct mbuf *, int);
 static	int none_enmic(struct ieee80211_key *, struct mbuf *, int);
 static	int none_demic(struct ieee80211_key *, struct mbuf *, int);
 
@@ -110,7 +110,7 @@ none_encap(struct ieee80211_key *k, struct mbuf *m, u_int8_t keyid)
 }
 
 static int
-none_decap(struct ieee80211_key *k, struct mbuf *m)
+none_decap(struct ieee80211_key *k, struct mbuf *m, int hdrlen)
 {
 	struct ieee80211com *ic = k->wk_private;
 #ifdef IEEE80211_DEBUG
diff --git a/sys/net80211/ieee80211_crypto_tkip.c b/sys/net80211/ieee80211_crypto_tkip.c
index ebafda8..c4ab09f 100644
--- a/sys/net80211/ieee80211_crypto_tkip.c
+++ b/sys/net80211/ieee80211_crypto_tkip.c
@@ -60,7 +60,7 @@ static	void tkip_detach(struct ieee80211_key *);
 static	int tkip_setkey(struct ieee80211_key *);
 static	int tkip_encap(struct ieee80211_key *, struct mbuf *m, u_int8_t keyid);
 static	int tkip_enmic(struct ieee80211_key *, struct mbuf *, int);
-static	int tkip_decap(struct ieee80211_key *, struct mbuf *);
+static	int tkip_decap(struct ieee80211_key *, struct mbuf *, int);
 static	int tkip_demic(struct ieee80211_key *, struct mbuf *, int);
 
 static const struct ieee80211_cipher tkip  = {
@@ -244,20 +244,18 @@ READ_6(uint8_t b0, uint8_t b1, uint8_t b2, uint8_t b3, uint8_t b4, uint8_t b5)
  * the specified key.
  */
 static int
-tkip_decap(struct ieee80211_key *k, struct mbuf *m)
+tkip_decap(struct ieee80211_key *k, struct mbuf *m, int hdrlen)
 {
 	struct tkip_ctx *ctx = k->wk_private;
 	struct ieee80211com *ic = ctx->tc_ic;
 	struct ieee80211_frame *wh;
 	uint8_t *ivp;
-	int hdrlen;
 
 	/*
 	 * Header should have extended IV and sequence number;
 	 * verify the former and validate the latter.
 	 */
 	wh = mtod(m, struct ieee80211_frame *);
-	hdrlen = ieee80211_hdrsize(wh);
 	ivp = mtod(m, uint8_t *) + hdrlen;
 	if ((ivp[IEEE80211_WEP_IVLEN] & IEEE80211_WEP_EXTIV) == 0) {
 		/*
@@ -327,11 +325,12 @@ tkip_demic(struct ieee80211_key *k, struct mbuf *m, int force)
 
 	if (force || (k->wk_flags & IEEE80211_KEY_SWMIC)) {
 		struct ieee80211_frame *wh = mtod(m, struct ieee80211_frame *);
-		int hdrlen = ieee80211_hdrsize(wh);
+		struct ieee80211com *ic = ctx->tc_ic;
+		int hdrlen = ieee80211_hdrspace(ic, wh);
 		u8 mic[IEEE80211_WEP_MICLEN];
 		u8 mic0[IEEE80211_WEP_MICLEN];
 
-		ctx->tc_ic->ic_stats.is_crypto_tkipdemic++;
+		ic->ic_stats.is_crypto_tkipdemic++;
 
 		michael_mic(ctx, k->wk_rxmic, 
 			m, hdrlen, m->m_pkthdr.len - (hdrlen + tkip.ic_miclen),
@@ -340,8 +339,7 @@ tkip_demic(struct ieee80211_key *k, struct mbuf *m, int force)
 			tkip.ic_miclen, mic0);
 		if (memcmp(mic, mic0, tkip.ic_miclen)) {
 			/* NB: 802.11 layer handles statistic and debug msg */
-			ieee80211_notify_michael_failure(ctx->tc_ic, wh,
-				k->wk_keyix);
+			ieee80211_notify_michael_failure(ic, wh, k->wk_keyix);
 			return 0;
 		}
 	}
diff --git a/sys/net80211/ieee80211_crypto_wep.c b/sys/net80211/ieee80211_crypto_wep.c
index f392886..45132419 100644
--- a/sys/net80211/ieee80211_crypto_wep.c
+++ b/sys/net80211/ieee80211_crypto_wep.c
@@ -55,7 +55,7 @@ static	void *wep_attach(struct ieee80211com *, struct ieee80211_key *);
 static	void wep_detach(struct ieee80211_key *);
 static	int wep_setkey(struct ieee80211_key *);
 static	int wep_encap(struct ieee80211_key *, struct mbuf *, u_int8_t keyid);
-static	int wep_decap(struct ieee80211_key *, struct mbuf *);
+static	int wep_decap(struct ieee80211_key *, struct mbuf *, int hdrlen);
 static	int wep_enmic(struct ieee80211_key *, struct mbuf *, int);
 static	int wep_demic(struct ieee80211_key *, struct mbuf *, int);
 
@@ -205,14 +205,12 @@ wep_enmic(struct ieee80211_key *k, struct mbuf *m, int force)
  * the specified key.
  */
 static int
-wep_decap(struct ieee80211_key *k, struct mbuf *m)
+wep_decap(struct ieee80211_key *k, struct mbuf *m, int hdrlen)
 {
 	struct wep_ctx *ctx = k->wk_private;
 	struct ieee80211_frame *wh;
-	int hdrlen;
 
 	wh = mtod(m, struct ieee80211_frame *);
-	hdrlen = ieee80211_hdrsize(wh);
 
 	/*
 	 * Check if the device handled the decrypt in hardware.
diff --git a/sys/net80211/ieee80211_input.c b/sys/net80211/ieee80211_input.c
index eda22a0..0ed2e94 100644
--- a/sys/net80211/ieee80211_input.c
+++ b/sys/net80211/ieee80211_input.c
@@ -108,8 +108,8 @@ static void ieee80211_discard_mac(struct ieee80211com *,
 #endif /* IEEE80211_DEBUG */
 
 static struct mbuf *ieee80211_defrag(struct ieee80211com *,
-	struct ieee80211_node *, struct mbuf *);
-static struct mbuf *ieee80211_decap(struct ieee80211com *, struct mbuf *);
+	struct ieee80211_node *, struct mbuf *, int);
+static struct mbuf *ieee80211_decap(struct ieee80211com *, struct mbuf *, int);
 static void ieee80211_send_error(struct ieee80211com *, struct ieee80211_node *,
 		const u_int8_t *mac, int subtype, int arg);
 static void ieee80211_node_pwrsave(struct ieee80211_node *, int enable);
@@ -136,7 +136,7 @@ ieee80211_input(struct ieee80211com *ic, struct mbuf *m,
 	struct ieee80211_frame *wh;
 	struct ieee80211_key *key;
 	struct ether_header *eh;
-	int len, hdrsize, off;
+	int len, hdrspace;
 	u_int8_t dir, type, subtype;
 	u_int8_t *bssid;
 	u_int16_t rxseq;
@@ -286,33 +286,15 @@ ieee80211_input(struct ieee80211com *ic, struct mbuf *m,
 
 	switch (type) {
 	case IEEE80211_FC0_TYPE_DATA:
-		hdrsize = ieee80211_hdrsize(wh);
-		if (ic->ic_flags & IEEE80211_F_DATAPAD)
-			hdrsize = roundup(hdrsize, sizeof(u_int32_t));
-		if (m->m_len < hdrsize &&
-		    (m = m_pullup(m, hdrsize)) == NULL) {
+		hdrspace = ieee80211_hdrspace(ic, wh);
+		if (m->m_len < hdrspace &&
+		    (m = m_pullup(m, hdrspace)) == NULL) {
 			IEEE80211_DISCARD_MAC(ic, IEEE80211_MSG_ANY,
 			    ni->ni_macaddr, NULL,
-			    "data too short: expecting %u", hdrsize);
+			    "data too short: expecting %u", hdrspace);
 			ic->ic_stats.is_rx_tooshort++;
 			goto out;		/* XXX */
 		}
-		if (subtype & IEEE80211_FC0_SUBTYPE_QOS) {
-			/* XXX discard if node w/o IEEE80211_NODE_QOS? */
-			/*
-			 * Strip QoS control and any padding so only a
-			 * stock 802.11 header is at the front.
-			 */
-			/* XXX 4-address QoS frame */
-			off = hdrsize - sizeof(struct ieee80211_frame);
-			ovbcopy(mtod(m, u_int8_t *), mtod(m, u_int8_t *) + off,
-				hdrsize - off);
-			m_adj(m, off);
-			wh = mtod(m, struct ieee80211_frame *);
-			wh->i_fc[0] &= ~IEEE80211_FC0_SUBTYPE_QOS;
-		} else {
-			/* XXX copy up for 4-address frames w/ padding */
-		}
 		switch (ic->ic_opmode) {
 		case IEEE80211_M_STA:
 			if (dir != IEEE80211_FC1_DIR_FROMDS) {
@@ -399,7 +381,7 @@ ieee80211_input(struct ieee80211com *ic, struct mbuf *m,
 				IEEE80211_NODE_STAT(ni, rx_noprivacy);
 				goto out;
 			}
-			key = ieee80211_crypto_decap(ic, ni, m);
+			key = ieee80211_crypto_decap(ic, ni, m, hdrspace);
 			if (key == NULL) {
 				/* NB: stats+msgs handled in crypto_decap */
 				IEEE80211_NODE_STAT(ni, rx_wepfail);
@@ -415,7 +397,7 @@ ieee80211_input(struct ieee80211com *ic, struct mbuf *m,
 		 * Next up, any fragmentation.
 		 */
 		if (!IEEE80211_IS_MULTICAST(wh->i_addr1)) {
-			m = ieee80211_defrag(ic, ni, m);
+			m = ieee80211_defrag(ic, ni, m, hdrspace);
 			if (m == NULL) {
 				/* Fragment dropped or frame not complete yet */
 				goto out;
@@ -440,7 +422,7 @@ ieee80211_input(struct ieee80211com *ic, struct mbuf *m,
 		/*
 		 * Finally, strip the 802.11 header.
 		 */
-		m = ieee80211_decap(ic, m);
+		m = ieee80211_decap(ic, m, hdrspace);
 		if (m == NULL) {
 			/* don't count Null data frames as errors */
 			if (subtype == IEEE80211_FC0_SUBTYPE_NODATA)
@@ -579,7 +561,8 @@ ieee80211_input(struct ieee80211com *ic, struct mbuf *m,
 				ic->ic_stats.is_rx_noprivacy++;
 				goto out;
 			}
-			key = ieee80211_crypto_decap(ic, ni, m);
+			hdrspace = ieee80211_hdrspace(ic, wh);
+			key = ieee80211_crypto_decap(ic, ni, m, hdrspace);
 			if (key == NULL) {
 				/* NB: stats+msgs handled in crypto_decap */
 				goto out;
@@ -627,7 +610,7 @@ out:
  */
 static struct mbuf *
 ieee80211_defrag(struct ieee80211com *ic, struct ieee80211_node *ni,
-	struct mbuf *m)
+	struct mbuf *m, int hdrspace)
 {
 	struct ieee80211_frame *wh = mtod(m, struct ieee80211_frame *);
 	struct ieee80211_frame *lwh;
@@ -696,6 +679,7 @@ ieee80211_defrag(struct ieee80211com *ic, struct ieee80211_node *ni,
 		}
 		mfrag = m;
 	} else {				/* concatenate */
+		m_adj(m, hdrspace);		/* strip header */
 		m_cat(mfrag, m);
 		/* NB: m_cat doesn't update the packet header */
 		mfrag->m_pkthdr.len += m->m_pkthdr.len;
@@ -712,26 +696,26 @@ ieee80211_defrag(struct ieee80211com *ic, struct ieee80211_node *ni,
 }
 
 static struct mbuf *
-ieee80211_decap(struct ieee80211com *ic, struct mbuf *m)
+ieee80211_decap(struct ieee80211com *ic, struct mbuf *m, int hdrlen)
 {
-	struct ieee80211_frame wh;	/* NB: QoS stripped above */
+	struct ieee80211_qosframe_addr4 wh;	/* Max size address frames */
 	struct ether_header *eh;
 	struct llc *llc;
 
-	if (m->m_len < sizeof(wh) + sizeof(*llc) &&
-	    (m = m_pullup(m, sizeof(wh) + sizeof(*llc))) == NULL) {
+	if (m->m_len < hdrlen + sizeof(*llc) &&
+	    (m = m_pullup(m, hdrlen + sizeof(*llc))) == NULL) {
 		/* XXX stat, msg */
 		return NULL;
 	}
-	memcpy(&wh, mtod(m, caddr_t), sizeof(wh));
-	llc = (struct llc *)(mtod(m, caddr_t) + sizeof(wh));
+	memcpy(&wh, mtod(m, caddr_t), hdrlen);
+	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) {
@@ -748,11 +732,9 @@ ieee80211_decap(struct ieee80211com *ic, struct mbuf *m)
 		IEEE80211_ADDR_COPY(eh->ether_shost, wh.i_addr3);
 		break;
 	case IEEE80211_FC1_DIR_DSTODS:
-		/* not yet supported */
-		IEEE80211_DISCARD(ic, IEEE80211_MSG_ANY,
-		    &wh, "data", "%s", "DS to DS not supported");
-		m_freem(m);
-		return NULL;
+		IEEE80211_ADDR_COPY(eh->ether_dhost, wh.i_addr3);
+		IEEE80211_ADDR_COPY(eh->ether_shost, wh.i_addr4);
+		break;
 	}
 #ifdef ALIGNED_POINTER
 	if (!ALIGNED_POINTER(mtod(m, caddr_t) + sizeof(*eh), u_int32_t)) {