* Introduce new flag for QoS control field;

* Change in mesh_input to validate that QoS is set and Mesh Control field
  is present, also both bytes of the QoS are read;
* Moved defragmentation in mesh_input before we try to forward packet as
  inferred from amendment spec, because Mesh Control field only present in first
  fragment;
* Changed in ieee80211_encap to set QoS subtype and Mesh Control field present,
  only first fragment have Mesh Control field present bit equal to 1;

Submitted by:	monthadar@gmail.com

3 files changed, 88 insertions, 28 deletions

diff --git a/sys/net80211/ieee80211.h b/sys/net80211/ieee80211.h
index d3425b5..c43c71b 100644
--- a/sys/net80211/ieee80211.h
+++ b/sys/net80211/ieee80211.h
@@ -199,6 +199,13 @@ struct ieee80211_qosframe_addr4 {
 #define	IEEE80211_QOS_EOSP			0x10	/* EndOfService Period*/
 #define	IEEE80211_QOS_EOSP_S			4
 #define	IEEE80211_QOS_TID			0x0f
+/* qos[1] byte used for all frames sent by mesh STAs in a mesh BSS */
+#define IEEE80211_QOS_MC			0x10	/* Mesh control */
+/* Mesh power save level*/
+#define IEEE80211_QOS_MESH_PSL			0x20
+/* Mesh Receiver Service Period Initiated */
+#define IEEE80211_QOS_RSPI			0x40
+/* bits 11 to 15 reserved */
 
 /* does frame have QoS sequence control data */
 #define	IEEE80211_QOS_HAS_SEQ(wh) \
diff --git a/sys/net80211/ieee80211_mesh.c b/sys/net80211/ieee80211_mesh.c
index b812ca4..576e20b 100644
--- a/sys/net80211/ieee80211_mesh.c
+++ b/sys/net80211/ieee80211_mesh.c
@@ -1040,9 +1040,9 @@ mesh_input(struct ieee80211_node *ni, struct mbuf *m, int rssi, int nf)
 	struct ieee80211_frame *wh;
 	const struct ieee80211_meshcntl *mc;
 	int hdrspace, meshdrlen, need_tap;
-	uint8_t dir, type, subtype, qos;
+	uint8_t dir, type, subtype;
 	uint32_t seq;
-	uint8_t *addr;
+	uint8_t *addr, qos[2];
 	ieee80211_seq rxseq;
 
 	KASSERT(ni != NULL, ("null node"));
@@ -1139,8 +1139,64 @@ mesh_input(struct ieee80211_node *ni, struct mbuf *m, int rssi, int nf)
 			vap->iv_stats.is_rx_wrongdir++;
 			goto err;
 		}
-		/* pull up enough to get to the mesh control */
+
+		/* All Mesh data frames are QoS subtype */
+		if (!HAS_SEQ(type)) {
+			IEEE80211_DISCARD(vap, IEEE80211_MSG_INPUT,
+			    wh, "data", "incorrect subtype 0x%x", subtype);
+			vap->iv_stats.is_rx_badsubtype++;
+			goto err;
+		}
+
+		/*
+		 * Next up, any fragmentation.
+		 * XXX: we defrag before we even try to forward,
+		 * Mesh Control field is not present in sub-sequent
+		 * fragmented frames. This is in contrast to Draft 4.0.
+		 */
 		hdrspace = ieee80211_hdrspace(ic, wh);
+		if (!IEEE80211_IS_MULTICAST(wh->i_addr1)) {
+			m = ieee80211_defrag(ni, m, hdrspace);
+			if (m == NULL) {
+				/* Fragment dropped or frame not complete yet */
+				goto out;
+			}
+		}
+		wh = mtod(m, struct ieee80211_frame *); /* NB: after defrag */
+
+		/*
+		 * Now we have a complete Mesh Data frame.
+		 */
+
+		/*
+		 * Only fromDStoDS data frames use 4 address qos frames
+		 * as specified in amendment. Otherwise addr4 is located
+		 * in the Mesh Control field and a 3 address qos frame
+		 * is used.
+		 */
+		if (IEEE80211_IS_DSTODS(wh))
+			*(uint16_t *)qos = *(uint16_t *)
+			    ((struct ieee80211_qosframe_addr4 *)wh)->i_qos;
+		else
+			*(uint16_t *)qos = *(uint16_t *)
+			    ((struct ieee80211_qosframe *)wh)->i_qos;
+
+		/*
+		 * NB: The mesh STA sets the Mesh Control Present
+		 * subfield to 1 in the Mesh Data frame containing
+		 * an unfragmented MSDU, an A-MSDU, or the first
+		 * fragment of an MSDU.
+		 * After defrag it should always be present.
+		 */
+		if (!(qos[1] & IEEE80211_QOS_MC)) {
+			IEEE80211_DISCARD_MAC(vap, IEEE80211_MSG_MESH,
+			    ni->ni_macaddr, NULL,
+			    "%s", "Mesh control field not present");
+			vap->iv_stats.is_rx_elem_missing++; /* XXX: kinda */
+			goto err;
+		}
+
+		/* pull up enough to get to the mesh control */
 		if (m->m_len < hdrspace + sizeof(struct ieee80211_meshcntl) &&
 		    (m = m_pullup(m, hdrspace +
 		        sizeof(struct ieee80211_meshcntl))) == NULL) {
@@ -1188,27 +1244,6 @@ mesh_input(struct ieee80211_node *ni, struct mbuf *m, int rssi, int nf)
 			/* NB: fall thru to deliver mcast frames locally */
 		}
 
-		/*
-		 * Save QoS bits for use below--before we strip the header.
-		 */
-		if (subtype == IEEE80211_FC0_SUBTYPE_QOS) {
-			qos = (dir == IEEE80211_FC1_DIR_DSTODS) ?
-			    ((struct ieee80211_qosframe_addr4 *)wh)->i_qos[0] :
-			    ((struct ieee80211_qosframe *)wh)->i_qos[0];
-		} else
-			qos = 0;
-		/*
-		 * Next up, any fragmentation.
-		 */
-		if (!IEEE80211_IS_MULTICAST(wh->i_addr1)) {
-			m = ieee80211_defrag(ni, m, hdrspace);
-			if (m == NULL) {
-				/* Fragment dropped or frame not complete yet */
-				goto out;
-			}
-		}
-		wh = NULL;		/* no longer valid, catch any uses */
-
 		if (ieee80211_radiotap_active_vap(vap))
 			ieee80211_radiotap_rx(vap, m);
 		need_tap = 0;
@@ -1229,7 +1264,7 @@ mesh_input(struct ieee80211_node *ni, struct mbuf *m, int rssi, int nf)
 			IEEE80211_NODE_STAT(ni, rx_decap);
 			goto err;
 		}
-		if (qos & IEEE80211_QOS_AMSDU) {
+		if (qos[0] & IEEE80211_QOS_AMSDU) {
 			m = ieee80211_decap_amsdu(ni, m);
 			if (m == NULL)
 				return IEEE80211_FC0_TYPE_DATA;
diff --git a/sys/net80211/ieee80211_output.c b/sys/net80211/ieee80211_output.c
index ca14c52..3334c58 100644
--- a/sys/net80211/ieee80211_output.c
+++ b/sys/net80211/ieee80211_output.c
@@ -1074,9 +1074,11 @@ ieee80211_encap(struct ieee80211vap *vap, struct ieee80211_node *ni,
 	 * ap's require all data frames to be QoS-encapsulated
 	 * once negotiated in which case we'll need to make this
 	 * configurable.
+	 * NB: mesh data frames are QoS.
 	 */
-	addqos = (ni->ni_flags & (IEEE80211_NODE_QOS|IEEE80211_NODE_HT)) &&
-		 (m->m_flags & M_EAPOL) == 0;
+	addqos = ((ni->ni_flags & (IEEE80211_NODE_QOS|IEEE80211_NODE_HT)) ||
+	    (vap->iv_opmode == IEEE80211_M_MBSS)) &&
+	    (m->m_flags & M_EAPOL) == 0;
 	if (addqos)
 		hdrsize = sizeof(struct ieee80211_qosframe);
 	else
@@ -1282,7 +1284,12 @@ ieee80211_encap(struct ieee80211vap *vap, struct ieee80211_node *ni,
 		qos[0] = tid & IEEE80211_QOS_TID;
 		if (ic->ic_wme.wme_wmeChanParams.cap_wmeParams[ac].wmep_noackPolicy)
 			qos[0] |= IEEE80211_QOS_ACKPOLICY_NOACK;
-		qos[1] = 0;
+#ifdef IEEE80211_SUPPORT_MESH
+		if (vap->iv_opmode == IEEE80211_M_MBSS) {
+			qos[1] |= IEEE80211_QOS_MC;
+		} else
+#endif
+			qos[1] = 0;
 		wh->i_fc[0] |= IEEE80211_FC0_SUBTYPE_QOS;
 
 		if ((m->m_flags & M_AMPDU_MPDU) == 0) {
@@ -1407,9 +1414,20 @@ ieee80211_fragment(struct ieee80211vap *vap, struct mbuf *m0,
 		 * we mark the first fragment with the MORE_FRAG bit
 		 * it automatically is propagated to each fragment; we
 		 * need only clear it on the last fragment (done below).
+		 * NB: frag 1+ dont have Mesh Control field present.
 		 */
 		whf = mtod(m, struct ieee80211_frame *);
 		memcpy(whf, wh, hdrsize);
+#ifdef IEEE80211_SUPPORT_MESH
+		if (vap->iv_opmode == IEEE80211_M_MBSS) {
+			if (IEEE80211_IS_DSTODS(wh))
+				((struct ieee80211_qosframe_addr4 *)
+				    whf)->i_qos[1] &= ~IEEE80211_QOS_MC;
+			else
+				((struct ieee80211_qosframe *)
+				    whf)->i_qos[1] &= ~IEEE80211_QOS_MC;
+		}
+#endif
 		*(uint16_t *)&whf->i_seq[0] |= htole16(
 			(fragno & IEEE80211_SEQ_FRAG_MASK) <<
 				IEEE80211_SEQ_FRAG_SHIFT);