openwrt-xburst/package/madwifi/patches/371-wds_sta_separation.patch

--- a/net80211/ieee80211_input.c
+++ b/net80211/ieee80211_input.c
@@ -202,6 +202,7 @@
 	struct ieee80211com *ic = vap->iv_ic;
 	struct net_device *dev = vap->iv_dev;
 	struct ieee80211_node *ni_wds = NULL;
+	struct net_device_stats *stats;
 	struct ieee80211_frame *wh;
 	struct ieee80211_key *key;
 	struct ether_header *eh;
@@ -435,7 +436,7 @@
 
 	switch (type) {
 	case IEEE80211_FC0_TYPE_DATA:
-		hdrspace = ieee80211_hdrspace(ic, wh);
+		hdrspace = ieee80211_hdrsize(wh);
 		if (skb->len < hdrspace) {
 			IEEE80211_DISCARD(vap, IEEE80211_MSG_ANY,
 				wh, "data", "too short: len %u, expecting %u",
@@ -445,16 +446,24 @@
 		}
 		switch (vap->iv_opmode) {
 		case IEEE80211_M_STA:
-			if ((dir != IEEE80211_FC1_DIR_FROMDS) &&
-			    (!((vap->iv_flags_ext & IEEE80211_FEXT_WDS) &&
-			    (dir == IEEE80211_FC1_DIR_DSTODS)))) {
+			switch(dir) {
+			case IEEE80211_FC1_DIR_FROMDS:
+				break;
+			case IEEE80211_FC1_DIR_DSTODS:
+				if (vap->iv_flags_ext & IEEE80211_FEXT_WDS)
+					break;
+			default:
 				IEEE80211_DISCARD(vap, IEEE80211_MSG_ANY,
 					wh, "data", "invalid dir 0x%x", dir);
 				vap->iv_stats.is_rx_wrongdir++;
 				goto out;
 			}
 
-	        	if (IEEE80211_IS_MULTICAST(wh->i_addr1)) {
+			if (IEEE80211_IS_MULTICAST(wh->i_addr1)) {
+				/* ignore 3-addr mcast if we're WDS STA */
+				if (vap->iv_flags_ext & IEEE80211_FEXT_WDS)
+					goto out;
+
 				/* Discard multicast if IFF_MULTICAST not set */
 				if ((0 != memcmp(wh->i_addr3, dev->broadcast, ETH_ALEN)) && 
 					(0 == (dev->flags & IFF_MULTICAST))) {
@@ -482,24 +491,10 @@
 					vap->iv_stats.is_rx_mcastecho++;
 					goto out;
 				}
-				/* 
-				 * if it is brodcasted by me on behalf of
-				 * a station behind me, drop it.
-				 */
-				if (vap->iv_flags_ext & IEEE80211_FEXT_WDS) {
-					struct ieee80211_node_table *nt;
-					struct ieee80211_node *ni_wds;
-					nt = &ic->ic_sta;
-					ni_wds = ieee80211_find_wds_node(nt, wh->i_addr3);
-					if (ni_wds) {
-						ieee80211_unref_node(&ni_wds);
-						IEEE80211_DISCARD(vap, IEEE80211_MSG_INPUT,
-							wh, NULL, "%s",
-							"multicast echo originated from node behind me");
-						vap->iv_stats.is_rx_mcastecho++;
-						goto out;
-					}
-				}
+			} else {
+				/* Same BSSID, but not meant for us to receive */
+				if (!IEEE80211_ADDR_EQ(wh->i_addr1, vap->iv_myaddr))
+					goto out;
 			}
 			break;
 		case IEEE80211_M_IBSS:
@@ -541,6 +536,11 @@
 				vap->iv_stats.is_rx_notassoc++;
 				goto err;
 			}
+
+			/* subif isn't fully set up yet, drop the frame */
+			if (ni->ni_subif == ni->ni_vap)
+				goto err;
+
 			/*
 			 * If we're a 4 address packet, make sure we have an entry in
 			 * the node table for the packet source address (addr4).
@@ -548,9 +548,16 @@
 			 */
 
 			/* check for wds link first */
-			if (dir == IEEE80211_FC1_DIR_DSTODS) {
+			if ((dir == IEEE80211_FC1_DIR_DSTODS) && !ni->ni_subif) {
 				struct ieee80211vap *avp;
 
+				if (vap->iv_flags_ext & IEEE80211_FEXT_WDSSEP) {
+					ieee80211_wds_addif(ni);
+					/* we must drop frames here until the interface has
+					 * been fully separated, otherwise a bridge might get
+					 * confused */
+					goto err;
+				}
 				TAILQ_FOREACH(avp, &vap->iv_wdslinks, iv_wdsnext) {
 					if (!memcmp(avp->wds_mac, wh->i_addr2, IEEE80211_ADDR_LEN)) {
 						IEEE80211_LOCK_IRQ(ni->ni_ic);
@@ -566,7 +573,7 @@
 			}
 
 			/* XXX: Useless node mgmt API; make better */
-			if ((dir == IEEE80211_FC1_DIR_DSTODS) && !ni_wds) {
+			if ((dir == IEEE80211_FC1_DIR_DSTODS) && !ni_wds && !ni->ni_subif) {
 				struct ieee80211_node_table *nt = &ic->ic_sta;
 				struct ieee80211_frame_addr4 *wh4;
 
@@ -626,6 +633,11 @@
 			goto out;
 		}
 
+		/* check if there is any data left */
+		hdrspace = ieee80211_hdrspace(ic, wh);
+		if (skb->len < hdrspace)
+			goto out;
+
 		/*
 		 * Handle privacy requirements.  Note that we
 		 * must not be preempted from here until after
@@ -698,8 +710,12 @@
 		if (! accept_data_frame(vap, ni, key, skb, eh))
 			goto out;
 
-		vap->iv_devstats.rx_packets++;
-		vap->iv_devstats.rx_bytes += skb->len;
+		if (ni->ni_subif && ((eh)->ether_type != __constant_htons(ETHERTYPE_PAE)))
+			stats = &ni->ni_subif->iv_devstats;
+		else
+			stats = &vap->iv_devstats;
+		stats->rx_packets++;
+		stats->rx_bytes += skb->len;
 		IEEE80211_NODE_STAT(ni, rx_data);
 		IEEE80211_NODE_STAT_ADD(ni, rx_bytes, skb->len);
 		ic->ic_lastdata = jiffies;
@@ -1132,6 +1148,13 @@
 		dev = vap->iv_xrvap->iv_dev;
 #endif
 
+	/* if the node has a wds subif, move data frames there,
+	 * but keep EAP traffic on the master */
+	if (ni->ni_subif && ((eh)->ether_type != __constant_htons(ETHERTYPE_PAE))) {
+		vap = ni->ni_subif;
+		dev = vap->iv_dev;
+	}
+
 	/* perform as a bridge within the vap */
 	/* XXX intra-vap bridging only */
 	if (vap->iv_opmode == IEEE80211_M_HOSTAP &&
@@ -1157,7 +1180,16 @@
 			if (ni1 != NULL) {
 				if (ni1->ni_vap == vap &&
 				    ieee80211_node_is_authorized(ni1) &&
+					!ni1->ni_subif &&
 				    ni1 != vap->iv_bss) {
+
+					/* tried to bridge to a subif, drop the packet */
+					if (ni->ni_subif) {
+						ieee80211_unref_node(&ni1);
+						ieee80211_dev_kfree_skb(&skb);
+						return;
+					}
+
 					skb1 = skb;
 					skb = NULL;
 				}
--- a/net80211/ieee80211_ioctl.h
+++ b/net80211/ieee80211_ioctl.h
@@ -649,6 +649,7 @@
 	IEEE80211_PARAM_BGSCAN_THRESH		= 79,	/* bg scan rssi threshold */
 	IEEE80211_PARAM_RSSI_DIS_THR	= 80,	/* rssi threshold for disconnection */
 	IEEE80211_PARAM_RSSI_DIS_COUNT	= 81,	/* counter for rssi threshold */
+	IEEE80211_PARAM_WDS_SEP			= 82,	/* move wds stations into separate interfaces */
 };
 
 #define	SIOCG80211STATS			(SIOCDEVPRIVATE+2)
--- a/net80211/ieee80211_node.h
+++ b/net80211/ieee80211_node.h
@@ -92,11 +92,13 @@
  * the ieee80211com structure.
  */
 struct ieee80211_node {
-	struct ieee80211vap *ni_vap;
+	struct ieee80211vap *ni_vap, *ni_subif;
 	struct ieee80211com *ni_ic;
 	struct ieee80211_node_table *ni_table;
 	TAILQ_ENTRY(ieee80211_node) ni_list;
 	LIST_ENTRY(ieee80211_node) ni_hash;
+	struct work_struct ni_create;	/* task for creating a subif */
+	struct work_struct ni_destroy;	/* task for destroying a subif */
 	atomic_t ni_refcnt;
 	u_int ni_scangen;			/* gen# for timeout scan */
 	u_int8_t ni_authmode;			/* authentication algorithm */
@@ -430,5 +432,6 @@
 void ieee80211_node_leave(struct ieee80211_node *);
 u_int8_t ieee80211_getrssi(struct ieee80211com *);
 int32_t ieee80211_get_node_count(struct ieee80211com *);
+void ieee80211_wds_addif(struct ieee80211_node *ni);
 #endif /* _NET80211_IEEE80211_NODE_H_ */
 
--- a/net80211/ieee80211_var.h
+++ b/net80211/ieee80211_var.h
@@ -322,6 +322,7 @@
 	u_int8_t ic_myaddr[IEEE80211_ADDR_LEN];
 	struct timer_list ic_inact;		/* mgmt/inactivity timer */
 
+	unsigned int ic_subifs;
 	u_int32_t ic_flags;			/* state flags */
 	u_int32_t ic_flags_ext;			/* extension of state flags */
 	u_int32_t ic_caps;			/* capabilities */
@@ -625,6 +626,7 @@
 #define IEEE80211_FEXT_DROPUNENC_EAPOL	0x00000800	/* CONF: drop unencrypted eapol frames */
 #define IEEE80211_FEXT_APPIE_UPDATE	0x00001000	/* STATE: beacon APP IE updated */
 #define IEEE80211_FEXT_BGSCAN_THR	0x00002000	/* bgscan due to low rssi */
+#define IEEE80211_FEXT_WDSSEP		0x00004000	/* move wds clients into separate interfaces */
 
 #define IEEE80211_COM_UAPSD_ENABLE(_ic)		((_ic)->ic_flags_ext |= IEEE80211_FEXT_UAPSD)
 #define IEEE80211_COM_UAPSD_DISABLE(_ic)	((_ic)->ic_flags_ext &= ~IEEE80211_FEXT_UAPSD)
--- a/net80211/ieee80211_wireless.c
+++ b/net80211/ieee80211_wireless.c
@@ -2867,6 +2867,14 @@
 		else
 			vap->iv_minrateindex = 0;
 		break;
+	case IEEE80211_PARAM_WDS_SEP:
+		if (vap->iv_opmode != IEEE80211_M_HOSTAP)
+			retv = -EINVAL;
+		else if (value)
+			vap->iv_flags_ext |= IEEE80211_FEXT_WDSSEP;
+		else
+			vap->iv_flags_ext &= ~IEEE80211_FEXT_WDSSEP;
+		break;
 #ifdef ATH_REVERSE_ENGINEERING
 	case IEEE80211_PARAM_DUMPREGS:
 		ieee80211_dump_registers(dev, info, w, extra);
@@ -3223,6 +3231,9 @@
 	case IEEE80211_PARAM_MINRATE:
 		param[0] = vap->iv_minrateindex;
 		break;
+	case IEEE80211_PARAM_WDS_SEP:
+		param[0] = !!(vap->iv_flags_ext & IEEE80211_FEXT_WDSSEP);
+		break;
 	default:
 		return -EOPNOTSUPP;
 	}
@@ -4447,6 +4458,8 @@
 	struct ieee80211vap *vap = ni->ni_vap;
 	size_t ielen;
 
+	if (req->vap->iv_wdsnode && ni->ni_subif)
+		vap = ni->ni_subif;
 	if (vap != req->vap && vap != req->vap->iv_xrvap)	/* only entries for this vap */
 		return;
 	if ((vap->iv_opmode == IEEE80211_M_HOSTAP ||
@@ -4466,6 +4479,8 @@
 	size_t ielen, len;
 	u_int8_t *cp;
 
+	if (req->vap->iv_wdsnode && ni->ni_subif)
+		vap = ni->ni_subif;
 	if (vap != req->vap && vap != req->vap->iv_xrvap)	/* only entries for this vap (or) xrvap */
 		return;
 	if ((vap->iv_opmode == IEEE80211_M_HOSTAP ||
@@ -5767,6 +5782,10 @@
 	 0, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, "get_minrate"},
 	{ IEEE80211_IOCTL_SETSCANLIST,
 	 IW_PRIV_TYPE_CHAR | 255, 0, "setscanlist"},
+	{ IEEE80211_PARAM_WDS_SEP,
+	 IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "wdssep"},
+	{ IEEE80211_PARAM_WDS_SEP,
+	 0, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, "get_wdssep"},
 
 #ifdef ATH_REVERSE_ENGINEERING
 	/*
@@ -5890,6 +5909,8 @@
 ieee80211_ioctl(struct net_device *dev, struct ifreq *ifr, int cmd)
 {
 	struct ieee80211vap *vap = dev->priv;
+	struct ieee80211com *ic = vap->iv_ic;
+	struct ieee80211_node *ni;
 
 	switch (cmd) {
 	case SIOCG80211STATS:
@@ -5898,8 +5919,20 @@
 	case SIOC80211IFDESTROY:
 		if (!capable(CAP_NET_ADMIN))
 			return -EPERM;
+		/* drop all node subifs */
+		TAILQ_FOREACH(ni, &ic->ic_sta.nt_node, ni_list) {
+			struct ieee80211vap *avp = ni->ni_subif;
+
+			if (ni->ni_vap != vap)
+				continue;
+			if (!avp)
+				continue;
+			ni->ni_subif = NULL;
+			ieee80211_stop(avp->iv_dev);
+			ic->ic_vap_delete(avp);
+		}
 		ieee80211_stop(vap->iv_dev);	/* force state before cleanup */
-		vap->iv_ic->ic_vap_delete(vap);
+		ic->ic_vap_delete(vap);
 		return 0;
 	case IEEE80211_IOCTL_GETKEY:
 		return ieee80211_ioctl_getkey(dev, (struct iwreq *) ifr);
--- a/net80211/ieee80211_node.c
+++ b/net80211/ieee80211_node.c
@@ -47,6 +47,7 @@
 #include <linux/netdevice.h>
 #include <linux/etherdevice.h>
 #include <linux/random.h>
+#include <linux/rtnetlink.h>
 
 #include "if_media.h"
 
@@ -236,7 +237,11 @@
 ieee80211_node_vdetach(struct ieee80211vap *vap)
 {
 	struct ieee80211com *ic = vap->iv_ic;
+	struct ieee80211_node *ni;
 
+	ni = vap->iv_wdsnode;
+	if (ni)
+		ni->ni_subif = NULL;
 	ieee80211_node_table_reset(&ic->ic_sta, vap);
 	if (vap->iv_bss != NULL) {
 		ieee80211_unref_node(&vap->iv_bss);
@@ -1134,6 +1139,57 @@
 	return ni;
 }
 
+#define WDSIFNAME ".sta%d"
+static void
+ieee80211_wds_do_addif(struct work_struct *work)
+{
+	struct ieee80211_node *ni = container_of(work, struct ieee80211_node, ni_create);
+	struct ieee80211vap *vap = ni->ni_vap;
+	struct ieee80211com *ic = vap->iv_ic;
+	struct ieee80211vap *avp;
+	char *name;
+
+	rtnl_lock();
+	/* did we get cancelled by the destroy call? */
+	if (!ni->ni_subif)
+		goto done;
+
+	ni->ni_subif = NULL;
+	name = kmalloc(strlen(vap->iv_dev->name) + sizeof(WDSIFNAME) + 1, GFP_KERNEL);
+	if (!name)
+		goto done;
+
+	strcpy(name, vap->iv_dev->name);
+	strcat(name, WDSIFNAME);
+	avp = ieee80211_create_vap(ic, name, ic->ic_dev, IEEE80211_M_WDS, 0, vap);
+	kfree(name);
+	if (!avp)
+		goto done;
+
+	memcpy(avp->wds_mac, ni->ni_bssid, IEEE80211_ADDR_LEN);
+	avp->iv_wdsnode = ieee80211_ref_node(ni);
+	ni->ni_subif = avp;
+	ic->ic_subifs++;
+
+done:
+	rtnl_unlock();
+	ieee80211_unref_node(&ni);
+}
+#undef WDSIFNAME
+
+void ieee80211_wds_addif(struct ieee80211_node *ni)
+{
+	/* check if the node is split out already,
+	 * or if we're in progress of setting up a new interface already */
+	if (ni->ni_subif)
+		return;
+
+	ieee80211_ref_node(ni);
+	ni->ni_subif = ni->ni_vap;
+	IEEE80211_INIT_WORK(&ni->ni_create, ieee80211_wds_do_addif);
+	schedule_work(&ni->ni_create);
+}
+
 /* Add wds address to the node table */
 int
 #ifdef IEEE80211_DEBUG_REFCNT
@@ -2254,6 +2310,36 @@
 	}
 }
 
+static void
+ieee80211_subif_destroy(struct work_struct *work)
+{
+	struct ieee80211_node *ni = container_of(work, struct ieee80211_node, ni_destroy);
+	struct ieee80211vap *vap;
+	struct ieee80211com *ic;
+
+	rtnl_lock();
+	vap = ni->ni_subif;
+
+	/* if addif is waiting for the timer to fire, cancel! */
+	if (vap == ni->ni_vap) {
+		ni->ni_subif = NULL;
+		goto done;
+	}
+
+	if (!vap)
+		goto done;
+
+	ic = vap->iv_ic;
+	ni->ni_subif = NULL;
+	ieee80211_stop(vap->iv_dev);
+	ic->ic_vap_delete(vap);
+	ic->ic_subifs--;
+
+done:
+	ieee80211_unref_node(&ni);
+	rtnl_unlock();
+}
+
 /*
  * Handle bookkeeping for a station/neighbor leaving
  * the bss when operating in ap or adhoc modes.
@@ -2270,6 +2356,12 @@
 			ni, "station with aid %d leaves (refcnt %u)",
 			IEEE80211_NODE_AID(ni), atomic_read(&ni->ni_refcnt));
 
+	if (ni->ni_subif) {
+		ieee80211_ref_node(ni);
+		IEEE80211_INIT_WORK(&ni->ni_destroy, ieee80211_subif_destroy);
+		schedule_work(&ni->ni_destroy);
+	}
+
 	/* From this point onwards we can no longer find the node,
 	 * so no more references are generated
 	 */
--- a/net80211/ieee80211_linux.h
+++ b/net80211/ieee80211_linux.h
@@ -81,6 +81,12 @@
 #endif
 }
 
+#ifndef container_of
+#define container_of(ptr, type, member) ({          \
+    const typeof( ((type *)0)->member ) *__mptr = (ptr);    \
+	    (type *)( (char *)__mptr - offsetof(type,member) );})
+#endif
+
 /*
  * Task deferral
  *
@@ -113,6 +119,29 @@
 
 #define	IEEE80211_RESCHEDULE	schedule
 
+#include <linux/sched.h>
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,41)
+#include <linux/tqueue.h>
+#define work_struct			tq_struct
+#define schedule_work(t)		schedule_task((t))
+#define flush_scheduled_work()		flush_scheduled_tasks()
+#define IEEE80211_INIT_WORK(t, f) do { 			\
+	memset((t), 0, sizeof(struct tq_struct)); \
+	(t)->routine = (void (*)(void*)) (f); 	\
+	(t)->data=(void *) (t);			\
+} while (0)
+#else
+#include <linux/workqueue.h>
+
+#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
+#define IEEE80211_INIT_WORK(_t, _f)	INIT_WORK((_t), (void (*)(void *))(_f), (_t));
+#else
+#define IEEE80211_INIT_WORK(_t, _f)	INIT_WORK((_t), (_f));
+#endif
+
+#endif /* KERNEL_VERSION < 2.5.41 */
+
+
 /* Locking */
 /* NB: beware, spin_is_locked() is not usefully defined for !(DEBUG || SMP)
  * because spinlocks do not exist in this configuration. Instead IRQs 
--- a/net80211/ieee80211_proto.c
+++ b/net80211/ieee80211_proto.c
@@ -1081,6 +1081,8 @@
 int
 ieee80211_open(struct net_device *dev)
 {
+	struct ieee80211vap *vap = dev->priv;
+
 	return ieee80211_init(dev, 0);
 }
 
@@ -1116,11 +1118,33 @@
 	struct ieee80211vap *vap = dev->priv;
 	struct ieee80211com *ic = vap->iv_ic;
 	struct net_device *parent = ic->ic_dev;
+	struct ieee80211_node *tni, *ni;
 
 	IEEE80211_DPRINTF(vap,
 		IEEE80211_MSG_STATE | IEEE80211_MSG_DEBUG,
 		"%s\n", "stop running");
 
+	/* get rid of all wds nodes while we're still locked */
+	do {
+		ni = NULL;
+
+		IEEE80211_NODE_TABLE_LOCK_IRQ(&ic->ic_sta);
+		TAILQ_FOREACH(tni, &ic->ic_sta.nt_node, ni_list) {
+			if (tni->ni_vap != vap)
+				continue;
+			if (!tni->ni_subif)
+				continue;
+			ni = tni;
+			break;
+		}
+		IEEE80211_NODE_TABLE_UNLOCK_IRQ(&ic->ic_sta);
+
+		if (!ni)
+			break;
+
+		ieee80211_node_leave(ni);
+	} while (1);
+
 	ieee80211_new_state(vap, IEEE80211_S_INIT, -1);
 	if (dev->flags & IFF_RUNNING) {
 		dev->flags &= ~IFF_RUNNING;		/* mark us stopped */
@@ -1630,6 +1654,7 @@
 		 */
 		if (ni->ni_authmode != IEEE80211_AUTH_8021X)
 			ieee80211_node_authorize(ni);
+
 #ifdef ATH_SUPERG_XR
 		/*
 		 * fire a timer to bring up XR vap if configured.
@@ -1885,8 +1910,15 @@
 		if (ostate == IEEE80211_S_SCAN || 
 		    ostate == IEEE80211_S_AUTH ||
 		    ostate == IEEE80211_S_ASSOC) {
+
 			/* Transition (S_SCAN|S_AUTH|S_ASSOC) -> S_RUN */
 			__ieee80211_newstate(vap, nstate, arg);
+
+			/* if we're in wds, let the ap know that we're doing this */
+			if ((vap->iv_opmode == IEEE80211_M_STA) &&
+				(vap->iv_flags_ext & IEEE80211_FEXT_WDS))
+					ieee80211_send_nulldata(ieee80211_ref_node(vap->iv_bss));
+
 			/* Then bring up all other vaps pending on the scan */
 			dstate = get_dominant_state(ic);
 			if (dstate == IEEE80211_S_RUN) {
--- a/net80211/ieee80211.c
+++ b/net80211/ieee80211.c
@@ -373,10 +373,25 @@
 ieee80211_ifdetach(struct ieee80211com *ic)
 {
 	struct ieee80211vap *vap;
+	int count;
+
+	/* bring down all vaps */
+	TAILQ_FOREACH(vap, &ic->ic_vaps, iv_next) {
+		ieee80211_stop(vap->iv_dev);
+	}
+
+	/* wait for all subifs to disappear */
+	do {
+		schedule();
+		rtnl_lock();
+		count = ic->ic_subifs;
+		rtnl_unlock();
+	} while (count > 0);
 
 	rtnl_lock();
-	while ((vap = TAILQ_FIRST(&ic->ic_vaps)) != NULL)
+	while ((vap = TAILQ_FIRST(&ic->ic_vaps)) != NULL) {
 		ic->ic_vap_delete(vap);
+	}
 	rtnl_unlock();
 
 	del_timer(&ic->ic_dfs_excl_timer);
@@ -599,8 +614,10 @@
 
 	IEEE80211_CANCEL_TQUEUE(&vap->iv_stajoin1tq);
 	IEEE80211_LOCK_IRQ(ic);
-	if (vap->iv_wdsnode)
+	if (vap->iv_wdsnode) {
+		vap->iv_wdsnode->ni_subif = NULL;
 		ieee80211_unref_node(&vap->iv_wdsnode);
+	}
 	if ((vap->iv_opmode == IEEE80211_M_WDS) &&
 		(vap->iv_master != NULL))
 		TAILQ_REMOVE(&vap->iv_master->iv_wdslinks, vap, iv_wdsnext);
--- a/ath/if_athvar.h
+++ b/ath/if_athvar.h
@@ -79,28 +79,6 @@
 #define	tasklet_enable(t)	do { (void) t; local_bh_enable(); } while (0)
 #endif /* !DECLARE_TASKLET */
 
-#include <linux/sched.h>
-#if LINUX_VERSION_CODE < KERNEL_VERSION(2,5,41)
-#include <linux/tqueue.h>
-#define work_struct			tq_struct
-#define schedule_work(t)		schedule_task((t))
-#define flush_scheduled_work()		flush_scheduled_tasks()
-#define ATH_INIT_WORK(t, f) do { 			\
-	memset((t), 0, sizeof(struct tq_struct)); \
-	(t)->routine = (void (*)(void*)) (f); 	\
-	(t)->data=(void *) (t);			\
-} while (0)
-#else
-#include <linux/workqueue.h>
-
-#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
-#define ATH_INIT_WORK(_t, _f)	INIT_WORK((_t), (void (*)(void *))(_f), (_t));
-#else
-#define ATH_INIT_WORK(_t, _f)	INIT_WORK((_t), (_f));
-#endif
-
-#endif /* KERNEL_VERSION < 2.5.41 */
-
 /*
  * Guess how the interrupt handler should work.
  */
--- a/net80211/ieee80211_output.c
+++ b/net80211/ieee80211_output.c
@@ -261,6 +261,10 @@
 		goto bad;
 	}
 
+	if (ni->ni_subif && (vap != ni->ni_subif) &&
+		((eh)->ether_type != __constant_htons(ETHERTYPE_PAE)))
+		goto bad;
+
 	/* calculate priority so drivers can find the TX queue */
 	if (ieee80211_classify(ni, skb)) {
 		IEEE80211_NOTE(vap, IEEE80211_MSG_OUTPUT, ni,
@@ -340,20 +344,33 @@
  * constructing a frame as it sets i_fc[1]; other bits can
  * then be or'd in.
  */
-static void
+static struct ieee80211_frame *
 ieee80211_send_setup(struct ieee80211vap *vap,
 	struct ieee80211_node *ni,
-	struct ieee80211_frame *wh,
+	struct sk_buff *skb,
 	int type,
 	const u_int8_t sa[IEEE80211_ADDR_LEN],
 	const u_int8_t da[IEEE80211_ADDR_LEN],
 	const u_int8_t bssid[IEEE80211_ADDR_LEN])
 {
 #define	WH4(wh)	((struct ieee80211_frame_addr4 *)wh)
+	struct ieee80211_frame *wh;
+	int len = sizeof(struct ieee80211_frame);
+	int opmode = vap->iv_opmode;
+
+	if ((type & IEEE80211_FC0_TYPE_MASK) == IEEE80211_FC0_TYPE_DATA) {
+		if ((opmode == IEEE80211_M_STA) &&
+			(vap->iv_flags_ext & IEEE80211_FEXT_WDS))
+			opmode = IEEE80211_M_WDS;
+
+		if (opmode == IEEE80211_M_WDS)
+			len = sizeof(struct ieee80211_frame_addr4);
+	}
 
+	wh = (struct ieee80211_frame *)skb_push(skb, len);
 	wh->i_fc[0] = IEEE80211_FC0_VERSION_0 | type;
 	if ((type & IEEE80211_FC0_TYPE_MASK) == IEEE80211_FC0_TYPE_DATA) {
-		switch (vap->iv_opmode) {
+		switch (opmode) {
 		case IEEE80211_M_STA:
 			wh->i_fc[1] = IEEE80211_FC1_DIR_TODS;
 			IEEE80211_ADDR_COPY(wh->i_addr1, bssid);
@@ -395,6 +412,8 @@
 	*(__le16 *)&wh->i_seq[0] =
 	    htole16(ni->ni_txseqs[0] << IEEE80211_SEQ_SEQ_SHIFT);
 	ni->ni_txseqs[0]++;
+
+	return wh;
 #undef WH4
 }
 
@@ -416,9 +435,7 @@
 
 	SKB_CB(skb)->ni = ni;
 
-	wh = (struct ieee80211_frame *)
-		skb_push(skb, sizeof(struct ieee80211_frame));
-	ieee80211_send_setup(vap, ni, wh,
+	wh = ieee80211_send_setup(vap, ni, skb,
 		IEEE80211_FC0_TYPE_MGT | type,
 		vap->iv_myaddr, ni->ni_macaddr, vap->iv_bssid);
 	/* XXX power management */
@@ -464,6 +481,9 @@
 	struct ieee80211_frame *wh;
 	u_int8_t *frm;
 
+	if (ni->ni_subif)
+		vap = ni->ni_subif;
+
 	skb = ieee80211_getmgtframe(&frm, 0);
 	if (skb == NULL) {
 		/* XXX debug msg */
@@ -472,9 +492,7 @@
 		return -ENOMEM;
 	}
 
-	wh = (struct ieee80211_frame *)
-		skb_push(skb, sizeof(struct ieee80211_frame));
-	ieee80211_send_setup(vap, ni, wh,
+	wh = ieee80211_send_setup(vap, ni, skb,
 		IEEE80211_FC0_TYPE_DATA | IEEE80211_FC0_SUBTYPE_NODATA,
 		vap->iv_myaddr, ni->ni_macaddr, vap->iv_bssid);
 	/* NB: power management bit is never sent by an AP */
@@ -512,6 +530,7 @@
 	struct sk_buff *skb;
 	struct ieee80211_qosframe *qwh;
 	u_int8_t *frm;
+	u_int8_t *i_qos;
 	int tid;
 
 	skb = ieee80211_getmgtframe(&frm, 2);
@@ -523,11 +542,12 @@
 	SKB_CB(skb)->ni = ieee80211_ref_node(ni);
 
 	skb->priority = ac;
-	qwh = (struct ieee80211_qosframe *)skb_push(skb, sizeof(struct ieee80211_qosframe));
 
-	qwh = (struct ieee80211_qosframe *)skb->data;
+	/* grab a pointer to QoS control and also compensate for the header length
+	 * difference between QoS and non-QoS frame */
+	i_qos = skb_push(skb, sizeof(struct ieee80211_qosframe) - sizeof(struct ieee80211_frame));
 
-	ieee80211_send_setup(vap, ni, (struct ieee80211_frame *)qwh,
+	qwh = (struct ieee80211_qosframe *) ieee80211_send_setup(vap, ni, skb,
 		IEEE80211_FC0_TYPE_DATA,
 		vap->iv_myaddr, /* SA */
 		ni->ni_macaddr, /* DA */
@@ -541,10 +561,10 @@
 
 	/* map from access class/queue to 11e header priority value */
 	tid = WME_AC_TO_TID(ac);
-	qwh->i_qos[0] = tid & IEEE80211_QOS_TID;
+	i_qos[0] = tid & IEEE80211_QOS_TID;
 	if (ic->ic_wme.wme_wmeChanParams.cap_wmeParams[ac].wmep_noackPolicy)
 		qwh->i_qos[0] |= (1 << IEEE80211_QOS_ACKPOLICY_S) & IEEE80211_QOS_ACKPOLICY;
-	qwh->i_qos[1] = 0;
+	i_qos[1] = 0;
 
 	IEEE80211_NODE_STAT(ni, tx_data);
 
@@ -786,6 +806,8 @@
 		hdrsize = sizeof(struct ieee80211_frame);
 
 	SKB_CB(skb)->auth_pkt = (eh.ether_type == __constant_htons(ETHERTYPE_PAE));
+	if (ni->ni_subif)
+		vap = ni->ni_subif;
 
 	switch (vap->iv_opmode) {
 	case IEEE80211_M_IBSS:
@@ -805,20 +827,9 @@
 			ismulticast = IEEE80211_IS_MULTICAST(eh.ether_dhost);
 		break;
 	case IEEE80211_M_STA:
-		if ((vap->iv_flags_ext & IEEE80211_FEXT_WDS) &&
-		    !IEEE80211_ADDR_EQ(eh.ether_shost, vap->iv_myaddr)) {
+		if (vap->iv_flags_ext & IEEE80211_FEXT_WDS) {
 			use4addr = 1;
-			ismulticast = IEEE80211_IS_MULTICAST(ni->ni_macaddr);
-			/* Add a WDS entry to the station VAP */
-			if (IEEE80211_IS_MULTICAST(eh.ether_dhost)) {
-				struct ieee80211_node_table *nt = &ic->ic_sta;
-				struct ieee80211_node *ni_wds 
-					= ieee80211_find_wds_node(nt, eh.ether_shost);
-				if (ni_wds)
-					ieee80211_unref_node(&ni_wds);
-				else
-					ieee80211_add_wds_addr(nt, ni, eh.ether_shost, 0);
-			}
+			ismulticast = 0;
 		} else
 			ismulticast = IEEE80211_IS_MULTICAST(vap->iv_bssid);
 		break;
@@ -1689,9 +1700,7 @@
 
 	SKB_CB(skb)->ni = ieee80211_ref_node(ni);
 
-	wh = (struct ieee80211_frame *)
-		skb_push(skb, sizeof(struct ieee80211_frame));
-	ieee80211_send_setup(vap, ni, wh,
+	wh = ieee80211_send_setup(vap, ni, skb,
 		IEEE80211_FC0_TYPE_MGT | IEEE80211_FC0_SUBTYPE_PROBE_REQ,
 		sa, da, bssid);
 	/* XXX power management? */
--- a/net80211/ieee80211_linux.c
+++ b/net80211/ieee80211_linux.c
@@ -145,7 +145,7 @@
 	struct sk_buff *skb;
 	u_int len;
 
-	len = roundup(sizeof(struct ieee80211_frame) + pktlen, 4);
+	len = roundup(sizeof(struct ieee80211_frame_addr4) + pktlen, 4);
 #ifdef IEEE80211_DEBUG_REFCNT
 	skb = ieee80211_dev_alloc_skb_debug(len + align - 1, func, line);
 #else
@@ -161,7 +161,7 @@
 		SKB_CB(skb)->flags = 0;
 		SKB_CB(skb)->next = NULL;
 
-		skb_reserve(skb, sizeof(struct ieee80211_frame));
+		skb_reserve(skb, sizeof(struct ieee80211_frame_addr4));
 		*frm = skb_put(skb, pktlen);
 	}
 	return skb;