mac80211: fix forwarded mesh frame queue mapping

We can't rely on ieee80211_select_queue() to do its job at this point
since the skb->protocol is not yet known. Instead, factor out and reuse
the queue mapping logic for injected frames.

Also, to mitigate congestion, forwarded frames should be dropped if the
outgoing queue was stopped.  This was not correctly implemented as we
were not checking the right queue.  Furthermore, we were dropping frames
that had arrived to their destination if that queue was stopped.

Signed-off-by: Thomas Pedersen <thomas@cozybit.com>
Signed-off-by: Javier Cardona <javier@cozybit.com>
Signed-off-by: John W. Linville <linville@tuxdriver.com>
diff --git a/net/mac80211/wme.c b/net/mac80211/wme.c
index a98f24a..89511be 100644
--- a/net/mac80211/wme.c
+++ b/net/mac80211/wme.c
@@ -52,6 +52,30 @@
 	}
 }
 
+/* Indicate which queue to use for this fully formed 802.11 frame */
+u16 ieee80211_select_queue_80211(struct ieee80211_local *local,
+				 struct sk_buff *skb,
+				 struct ieee80211_hdr *hdr)
+{
+	u8 *p;
+
+	if (local->hw.queues < 4)
+		return 0;
+
+	if (!ieee80211_is_data(hdr->frame_control)) {
+		skb->priority = 7;
+		return ieee802_1d_to_ac[skb->priority];
+	}
+	if (!ieee80211_is_data_qos(hdr->frame_control)) {
+		skb->priority = 0;
+		return ieee802_1d_to_ac[skb->priority];
+	}
+
+	p = ieee80211_get_qos_ctl(hdr);
+	skb->priority = *p & IEEE80211_QOS_CTL_TAG1D_MASK;
+
+	return ieee80211_downgrade_queue(local, skb);
+}
 
 /* Indicate which queue to use. */
 u16 ieee80211_select_queue(struct ieee80211_sub_if_data *sdata,