p54: enable proper frame injection

This patch enables frame injection in monitor mode for all p54 devices.
As a result, any user can finally use the aircrack-ng suite out of the box.

e.g:
aireplay-ng --test wlan0
Trying broadcast probe requests...
Injection is working!
Found 1 AP

Trying directed probe requests...
XX:XX:XX:XX:XX:XX - channel: i - 'SSID'
Ping (min/avg/max): 1.536ms/3.193ms/4.377ms Power: 193.00
30/30: 100%
Signed-off-by: Christian Lamparter <chunkeey@web.de>
Signed-off-by: John W. Linville <linville@tuxdriver.com>

drivers/net/wireless/p54/p54common.c

@@ -775,9 +775,16 @@ static void p54_rx_frame_sent(struct ieee80211_hw *dev, struct sk_buff *skb)
 		priv->tx_stats[entry_data->hw_queue].len--;
 		priv->stats.dot11ACKFailureCount += payload->tries - 1;
 
-		if (unlikely(entry == priv->cached_beacon)) {
+		/*
+		 * Frames in P54_QUEUE_FWSCAN and P54_QUEUE_BEACON are
+		 * generated by the driver. Therefore tx_status is bogus
+		 * and we don't want to confuse the mac80211 stack.
+		 */
+		if (unlikely(entry_data->hw_queue < P54_QUEUE_FWSCAN)) {
+			if (entry_data->hw_queue == P54_QUEUE_BEACON)
+				priv->cached_beacon = NULL;
+
 			kfree_skb(entry);
-			priv->cached_beacon = NULL;
 			goto out;
 		}
 
@@ -1240,33 +1247,26 @@ static int p54_tx_fill(struct ieee80211_hw *dev, struct sk_buff *skb,
 {
 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
 	struct p54_common *priv = dev->priv;
-	int ret = 0;
-
-	if (unlikely(ieee80211_is_mgmt(hdr->frame_control))) {
-		if (ieee80211_is_beacon(hdr->frame_control)) {
-			*aid = 0;
-			*queue = P54_QUEUE_BEACON;
-			*extra_len = IEEE80211_MAX_TIM_LEN;
-			*flags = P54_HDR_FLAG_DATA_OUT_TIMESTAMP;
-			return 0;
-		} else if (ieee80211_is_probe_resp(hdr->frame_control)) {
-			*aid = 0;
-			*queue = P54_QUEUE_MGMT;
-			*flags = P54_HDR_FLAG_DATA_OUT_TIMESTAMP |
-				 P54_HDR_FLAG_DATA_OUT_NOCANCEL;
-			return 0;
-		} else {
-			*queue = P54_QUEUE_MGMT;
-			ret = 0;
-		}
-	} else {
-		*queue += P54_QUEUE_DATA;
-		ret = 1;
-	}
+	int ret = 1;
 
 	switch (priv->mode) {
+	case NL80211_IFTYPE_MONITOR:
+		/*
+		 * We have to set P54_HDR_FLAG_DATA_OUT_PROMISC for
+		 * every frame in promiscuous/monitor mode.
+		 * see STSW45x0C LMAC API - page 12.
+		 */
+		*aid = 0;
+		*flags = P54_HDR_FLAG_DATA_OUT_PROMISC;
+		*queue += P54_QUEUE_DATA;
+		break;
 	case NL80211_IFTYPE_STATION:
 		*aid = 1;
+		if (unlikely(ieee80211_is_mgmt(hdr->frame_control))) {
+			*queue = P54_QUEUE_MGMT;
+			ret = 0;
+		} else
+			*queue += P54_QUEUE_DATA;
 		break;
 	case NL80211_IFTYPE_AP:
 	case NL80211_IFTYPE_ADHOC:
@@ -1276,10 +1276,44 @@ static int p54_tx_fill(struct ieee80211_hw *dev, struct sk_buff *skb,
 			*queue = P54_QUEUE_CAB;
 			return 0;
 		}
+
+		if (unlikely(ieee80211_is_mgmt(hdr->frame_control))) {
+			if (ieee80211_is_probe_resp(hdr->frame_control)) {
+				*aid = 0;
+				*queue = P54_QUEUE_MGMT;
+				*flags = P54_HDR_FLAG_DATA_OUT_TIMESTAMP |
+					 P54_HDR_FLAG_DATA_OUT_NOCANCEL;
+				return 0;
+			} else if (ieee80211_is_beacon(hdr->frame_control)) {
+				*aid = 0;
+
+				if (info->flags & IEEE80211_TX_CTL_INJECTED) {
+					/*
+					 * Injecting beacons on top of a AP is
+					 * not a good idea... nevertheless,
+					 * it should be doable.
+					 */
+
+					*queue += P54_QUEUE_DATA;
+					return 1;
+				}
+
+				*flags = P54_HDR_FLAG_DATA_OUT_TIMESTAMP;
+				*queue = P54_QUEUE_BEACON;
+				*extra_len = IEEE80211_MAX_TIM_LEN;
+				return 0;
+			} else {
+				*queue = P54_QUEUE_MGMT;
+				ret = 0;
+			}
+		} else
+			*queue += P54_QUEUE_DATA;
+
 		if (info->control.sta)
 			*aid = info->control.sta->aid;
 		else
 			*flags |= P54_HDR_FLAG_DATA_OUT_NOCANCEL;
+		break;
 	}
 	return ret;
 }
@@ -1497,11 +1531,20 @@ static int p54_setup_mac(struct ieee80211_hw *dev)
 		case NL80211_IFTYPE_MESH_POINT:
 			mode = P54_FILTER_TYPE_IBSS;
 			break;
+		case NL80211_IFTYPE_MONITOR:
+			mode = P54_FILTER_TYPE_PROMISCUOUS;
+			break;
 		default:
 			mode = P54_FILTER_TYPE_NONE;
 			break;
 		}
-		if (priv->filter_flags & FIF_PROMISC_IN_BSS)
+
+		/*
+		 * "TRANSPARENT and PROMISCUOUS are mutually exclusive"
+		 * STSW45X0C LMAC API - page 12
+		 */
+		if ((priv->filter_flags & FIF_PROMISC_IN_BSS) &&
+		    (mode != P54_FILTER_TYPE_PROMISCUOUS))
 			mode |= P54_FILTER_TYPE_TRANSPARENT;
 	} else
 		mode = P54_FILTER_TYPE_RX_DISABLED;