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>

1 files changed, 69 insertions, 26 deletions

diff --git a/drivers/net/wireless/p54/p54common.c b/drivers/net/wireless/p54/p54common.c
index 3298cb464f7..28d98338957 100644
--- a/drivers/net/wireless/p54/p54common.c
+++ b/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;