[PATCH] libertas: monitor mode support for OLPC firmware
authorLuis Carlos Cobo <luisca@cozybit.com>
Thu, 2 Aug 2007 17:16:55 +0000 (13:16 -0400)
committerDavid S. Miller <davem@sunset.davemloft.net>
Wed, 10 Oct 2007 23:50:13 +0000 (16:50 -0700)
Driver support for the monitor mode support that will be available in the next
OLPC 'bleeding edge' Marvell firmware release (most likely, 5.110.16.p2).

To activate monitor mode,

echo mode > /sys/class/net/{ethX,mshX}/device/libertas_rtap

where mode is the hex mask that specifies which frames to sniff (in short, 0x1
for data, 0x2 for all management but beacons, 0x4 for beacons). Any non zero
mode will activate the monitor mode, inhibiting transmission in ethX and mshX
interfaces and routing all the incoming traffic to a new rtapX interface that
will output the packets in 802.11+radiotap headers format.

Signed-off-by: Luis Carlos Cobo <luisca@cozybit.com>
Signed-off-by: John W. Linville <linville@tuxdriver.com>
drivers/net/wireless/libertas/cmd.c
drivers/net/wireless/libertas/dev.h
drivers/net/wireless/libertas/host.h
drivers/net/wireless/libertas/hostcmd.h
drivers/net/wireless/libertas/if_usb.c
drivers/net/wireless/libertas/main.c
drivers/net/wireless/libertas/rx.c
drivers/net/wireless/libertas/tx.c
drivers/net/wireless/libertas/wext.h

index 517489a06063c0dcb39a0644d3737bae3e7a6765..cd3bddb243b1d3583e07f9ebd3648c5dfff5492f 100644 (file)
@@ -565,6 +565,26 @@ static int wlan_cmd_802_11_rf_tx_power(wlan_private * priv,
        return 0;
 }
 
+static int wlan_cmd_802_11_monitor_mode(wlan_private * priv,
+                                     struct cmd_ds_command *cmd,
+                                     u16 cmd_action, void *pdata_buf)
+{
+       struct cmd_ds_802_11_monitor_mode *monitor = &cmd->params.monitor;
+
+       cmd->command = cpu_to_le16(CMD_802_11_MONITOR_MODE);
+       cmd->size =
+           cpu_to_le16(sizeof(struct cmd_ds_802_11_monitor_mode) +
+                            S_DS_GEN);
+
+       monitor->action = cpu_to_le16(cmd_action);
+       if (cmd_action == CMD_ACT_SET) {
+               monitor->mode =
+                   cpu_to_le16((u16) (*(u32 *) pdata_buf));
+       }
+
+       return 0;
+}
+
 static int wlan_cmd_802_11_rate_adapt_rateset(wlan_private * priv,
                                              struct cmd_ds_command *cmd,
                                              u16 cmd_action)
@@ -1239,6 +1259,11 @@ int libertas_prepare_and_send_command(wlan_private * priv,
                ret = wlan_cmd_mac_multicast_adr(priv, cmdptr, cmd_action);
                break;
 
+       case CMD_802_11_MONITOR_MODE:
+               ret = wlan_cmd_802_11_monitor_mode(priv, cmdptr,
+                                         cmd_action, pdata_buf);
+               break;
+
        case CMD_802_11_AD_HOC_JOIN:
                ret = libertas_cmd_80211_ad_hoc_join(priv, cmdptr, pdata_buf);
                break;
index 397c5fca0ff5788323ef6121aea05a0c91dac3d2..5697fec0cb1ddc1b665cd1eaea7447819699802e 100644 (file)
@@ -112,7 +112,9 @@ struct _wlan_private {
        struct net_device *dev;
 
        struct net_device_stats stats;
-       struct net_device *mesh_dev ; /* Virtual device */
+       struct net_device *mesh_dev; /* Virtual device */
+       struct net_device *rtap_net_dev;
+       struct ieee80211_device *ieee;
 
        struct iw_statistics wstats;
        struct wlan_mesh_stats mstats;
@@ -362,8 +364,7 @@ struct _wlan_adapter {
 
        struct cmd_ds_802_11_get_log logmsg;
 
-       u32 linkmode;
-       u32 radiomode;
+       u32 monitormode;
        u8 fw_ready;
 
        u8 last_scanned_channel;
index c6b44c83159448b688c9d1cbe5fee6bd88381150..4ccdbf9c473a725eebd417dbfd95a937ac872df2 100644 (file)
 
 #define CMD_FWT_ACCESS                0x0095
 
+#define CMD_802_11_MONITOR_MODE       0x0098
+
 #define CMD_MESH_ACCESS               0x009b
 
 /* For the IEEE Power Save */
index 44cf39c8d1b88a414b8fb493ce93e0b972cdd3f7..52884eaf2eddfa432fcae10f00f3c5f2e99e6114 100644 (file)
@@ -405,6 +405,11 @@ struct cmd_ds_802_11_rf_antenna {
 
 };
 
+struct cmd_ds_802_11_monitor_mode {
+       u16 action;
+       u16 mode;
+};
+
 struct cmd_ds_802_11_ps_mode {
        __le16 action;
        __le16 nullpktinterval;
@@ -623,6 +628,7 @@ struct cmd_ds_command {
                struct cmd_ds_802_11_snmp_mib smib;
                struct cmd_ds_802_11_rf_tx_power txp;
                struct cmd_ds_802_11_rf_antenna rant;
+               struct cmd_ds_802_11_monitor_mode monitor;
                struct cmd_ds_802_11_data_rate drate;
                struct cmd_ds_802_11_rate_adapt_rateset rateset;
                struct cmd_ds_mac_multicast_adr madr;
index 670e1d23c0431ea55bcacd29ad77f76eb84c260d..d28802fb244b2dadd010fff4cea2fcac7074652a 100644 (file)
@@ -208,6 +208,8 @@ static int if_usb_probe(struct usb_interface *intf,
        if (!(priv = libertas_add_card(cardp, &udev->dev)))
                goto dealloc;
 
+       udev->dev.driver_data = priv;
+
        if (libertas_add_mesh(priv, &udev->dev))
                goto err_add_mesh;
 
index a3a17caae43997e6c2df4605deac195e46781170..9a46339ce47e628bb2be1696586a0b10fd515072 100644 (file)
@@ -21,6 +21,7 @@
 #include "wext.h"
 #include "debugfs.h"
 #include "assoc.h"
+#include "join.h"
 
 #define DRIVER_RELEASE_VERSION "322.p1"
 const char libertas_driver_version[] = "COMM-USB8388-" DRIVER_RELEASE_VERSION
@@ -246,6 +247,66 @@ static ssize_t libertas_anycast_set(struct device * dev,
        return strlen(buf);
 }
 
+int libertas_add_rtap(wlan_private *priv);
+void libertas_remove_rtap(wlan_private *priv);
+
+/**
+ * Get function for sysfs attribute rtap
+ */
+static ssize_t libertas_rtap_get(struct device * dev,
+               struct device_attribute *attr, char * buf)
+{
+       wlan_private *priv = (wlan_private *) dev->driver_data;
+       wlan_adapter *adapter = priv->adapter;
+       return snprintf(buf, 5, "0x%X\n", adapter->monitormode);
+}
+
+/**
+ *  Set function for sysfs attribute rtap
+ */
+static ssize_t libertas_rtap_set(struct device * dev,
+               struct device_attribute *attr, const char * buf, size_t count)
+{
+       int monitor_mode;
+       wlan_private *priv = (wlan_private *) dev->driver_data;
+       wlan_adapter *adapter = priv->adapter;
+
+       sscanf(buf, "%x", &monitor_mode);
+       if (monitor_mode != WLAN_MONITOR_OFF) {
+               if(adapter->monitormode == monitor_mode)
+                       return strlen(buf);
+               if (adapter->monitormode == WLAN_MONITOR_OFF) {
+                       if (adapter->mode == IW_MODE_INFRA)
+                               libertas_send_deauthentication(priv);
+                       else if (adapter->mode == IW_MODE_ADHOC)
+                               libertas_stop_adhoc_network(priv);
+                       libertas_add_rtap(priv);
+               }
+               adapter->monitormode = monitor_mode;
+       }
+
+       else {
+               if(adapter->monitormode == WLAN_MONITOR_OFF)
+                       return strlen(buf);
+               adapter->monitormode = WLAN_MONITOR_OFF;
+               libertas_remove_rtap(priv);
+               netif_wake_queue(priv->dev);
+               netif_wake_queue(priv->mesh_dev);
+       }
+
+       libertas_prepare_and_send_command(priv,
+                       CMD_802_11_MONITOR_MODE, CMD_ACT_SET,
+                       CMD_OPTION_WAITFORRSP, 0, &adapter->monitormode);
+       return strlen(buf);
+}
+
+/**
+ * libertas_rtap attribute to be exported per mshX interface
+ * through sysfs (/sys/class/net/mshX/libertas-rtap)
+ */
+static DEVICE_ATTR(libertas_rtap, 0644, libertas_rtap_get,
+               libertas_rtap_set );
+
 /**
  * anycast_mask attribute to be exported per mshX interface
  * through sysfs (/sys/class/net/mshX/anycast_mask)
@@ -480,6 +541,10 @@ static int libertas_mesh_pre_start_xmit(struct sk_buff *skb,
        int ret;
 
        lbs_deb_enter(LBS_DEB_MESH);
+       if(priv->adapter->monitormode != WLAN_MONITOR_OFF) {
+               netif_stop_queue(dev);
+               return -EOPNOTSUPP;
+       }
 
        SET_MESH_FRAME(skb);
 
@@ -494,10 +559,16 @@ static int libertas_mesh_pre_start_xmit(struct sk_buff *skb,
  */
 static int libertas_pre_start_xmit(struct sk_buff *skb, struct net_device *dev)
 {
+       wlan_private *priv = dev->priv;
        int ret;
 
        lbs_deb_enter(LBS_DEB_NET);
 
+       if(priv->adapter->monitormode != WLAN_MONITOR_OFF) {
+               netif_stop_queue(dev);
+               return -EOPNOTSUPP;
+       }
+
        UNSET_MESH_FRAME(skb);
 
        ret = libertas_hard_start_xmit(skb, dev);
@@ -517,7 +588,7 @@ static void libertas_tx_timeout(struct net_device *dev)
        dev->trans_start = jiffies;
 
        if (priv->adapter->currenttxskb) {
-               if (priv->adapter->radiomode == WLAN_RADIOMODE_RADIOTAP) {
+               if (priv->adapter->monitormode != WLAN_MONITOR_OFF) {
                        /* If we are here, we have not received feedback from
                           the previous packet.  Assume TX_FAIL and move on. */
                        priv->adapter->eventcause = 0x01000000;
@@ -1169,6 +1240,9 @@ wlan_private *libertas_add_card(void *card, struct device *dmdev)
        spin_lock_init(&priv->adapter->driver_lock);
        init_waitqueue_head(&priv->adapter->cmd_pending);
        priv->adapter->nr_cmd_pending = 0;
+       priv->rtap_net_dev = NULL;
+       if (device_create_file(dmdev, &dev_attr_libertas_rtap))
+               goto err_kzalloc;
        goto done;
 
 err_kzalloc:
@@ -1333,6 +1407,7 @@ int libertas_remove_card(wlan_private *priv)
 
        lbs_deb_enter(LBS_DEB_NET);
 
+       libertas_remove_rtap(priv);
        if (!priv)
                goto out;
 
@@ -1342,6 +1417,7 @@ int libertas_remove_card(wlan_private *priv)
                goto out;
 
        dev = priv->dev;
+       device_remove_file(priv->hotplug_device, &dev_attr_libertas_rtap);
 
        netif_stop_queue(priv->dev);
        netif_carrier_off(priv->dev);
@@ -1537,6 +1613,81 @@ static void libertas_exit_module(void)
        lbs_deb_leave(LBS_DEB_MAIN);
 }
 
+/*
+ * rtap interface support fuctions
+ */
+
+static int libertas_rtap_open(struct net_device *dev)
+{
+        netif_carrier_off(dev);
+        netif_stop_queue(dev);
+        return 0;
+}
+
+static int libertas_rtap_stop(struct net_device *dev)
+{
+        return 0;
+}
+
+static int libertas_rtap_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
+{
+        netif_stop_queue(dev);
+        return -EOPNOTSUPP;
+}
+
+static struct net_device_stats *libertas_rtap_get_stats(struct net_device *dev)
+{
+       wlan_private *priv = dev->priv;
+       return &priv->ieee->stats;
+}
+
+
+void libertas_remove_rtap(wlan_private *priv)
+{
+       if (priv->rtap_net_dev == NULL)
+               return;
+       unregister_netdev(priv->rtap_net_dev);
+       free_ieee80211(priv->rtap_net_dev);
+       priv->rtap_net_dev = NULL;
+}
+
+int libertas_add_rtap(wlan_private *priv)
+{
+       int rc = 0;
+
+       if (priv->rtap_net_dev)
+               return -EPERM;
+
+       priv->rtap_net_dev = alloc_ieee80211(0);
+       if (priv->rtap_net_dev == NULL)
+               return -ENOMEM;
+
+
+       priv->ieee = netdev_priv(priv->rtap_net_dev);
+
+       strcpy(priv->rtap_net_dev->name, "rtap%d");
+
+       priv->rtap_net_dev->type = ARPHRD_IEEE80211_RADIOTAP;
+       priv->rtap_net_dev->open = libertas_rtap_open;
+       priv->rtap_net_dev->stop = libertas_rtap_stop;
+       priv->rtap_net_dev->get_stats = libertas_rtap_get_stats;
+       priv->rtap_net_dev->hard_start_xmit = libertas_rtap_hard_start_xmit;
+       priv->rtap_net_dev->set_multicast_list = libertas_set_multicast_list;
+       priv->rtap_net_dev->priv = priv;
+
+       priv->ieee->iw_mode = IW_MODE_MONITOR;
+
+       rc = register_netdev(priv->rtap_net_dev);
+       if (rc) {
+               free_ieee80211(priv->rtap_net_dev);
+               priv->rtap_net_dev = NULL;
+               return rc;
+       }
+
+       return 0;
+}
+
+
 module_init(libertas_init_module);
 module_exit(libertas_exit_module);
 
index 83a7765fd23ce8eaa5ef3f91f14a3dc11c2c9c1b..09def943b910caa912d852bbc0c23536891362d0 100644 (file)
@@ -138,12 +138,15 @@ void libertas_upload_rx_packet(wlan_private * priv, struct sk_buff *skb)
 {
        lbs_deb_rx("skb->data %p\n", skb->data);
 
-       if (priv->mesh_dev && IS_MESH_FRAME(skb))
-               skb->protocol = eth_type_trans(skb, priv->mesh_dev);
-       else
-               skb->protocol = eth_type_trans(skb, priv->dev);
+       if (priv->adapter->monitormode != WLAN_MONITOR_OFF) {
+               skb->protocol = eth_type_trans(skb, priv->rtap_net_dev);
+       } else {
+               if (priv->mesh_dev && IS_MESH_FRAME(skb))
+                       skb->protocol = eth_type_trans(skb, priv->mesh_dev);
+               else
+                       skb->protocol = eth_type_trans(skb, priv->dev);
+       }
        skb->ip_summed = CHECKSUM_UNNECESSARY;
-
        netif_rx(skb);
 }
 
@@ -170,7 +173,7 @@ int libertas_process_rxed_packet(wlan_private * priv, struct sk_buff *skb)
 
        lbs_deb_enter(LBS_DEB_RX);
 
-       if (priv->adapter->linkmode == WLAN_LINKMODE_802_11)
+       if (priv->adapter->monitormode != WLAN_MONITOR_OFF)
                return process_rxed_802_11_packet(priv, skb);
 
        p_rx_pkt = (struct rxpackethdr *) skb->data;
@@ -290,21 +293,22 @@ static u8 convert_mv_rate_to_radiotap(u8 rate)
                return 11;
        case 3:         /*  11 Mbps */
                return 22;
-       case 4:         /*   6 Mbps */
+       /* case 4: reserved */
+       case 5:         /*   6 Mbps */
                return 12;
-       case 5:         /*   9 Mbps */
+       case 6:         /*   9 Mbps */
                return 18;
-       case 6:         /*  12 Mbps */
+       case 7:         /*  12 Mbps */
                return 24;
-       case 7:         /*  18 Mbps */
+       case 8:         /*  18 Mbps */
                return 36;
-       case 8:         /*  24 Mbps */
+       case 9:         /*  24 Mbps */
                return 48;
-       case 9:         /*  36 Mbps */
+       case 10:                /*  36 Mbps */
                return 72;
-       case 10:                /*  48 Mbps */
+       case 11:                /*  48 Mbps */
                return 96;
-       case 11:                /*  54 Mbps */
+       case 12:                /*  54 Mbps */
                return 108;
        }
        lbs_pr_alert("Invalid Marvell WLAN rate %i\n", rate);
@@ -355,14 +359,13 @@ static int process_rxed_802_11_packet(wlan_private * priv, struct sk_buff *skb)
               skb->len, sizeof(struct rxpd), skb->len - sizeof(struct rxpd));
 
        /* create the exported radio header */
-       switch (priv->adapter->radiomode) {
-       case WLAN_RADIOMODE_NONE:
+       if(priv->adapter->monitormode == WLAN_MONITOR_OFF) {
                /* no radio header */
                /* chop the rxpd */
                skb_pull(skb, sizeof(struct rxpd));
-               break;
+       }
 
-       case WLAN_RADIOMODE_RADIOTAP:
+       else {
                /* radiotap header */
                radiotap_hdr.hdr.it_version = 0;
                /* XXX must check this value for pad */
@@ -400,16 +403,6 @@ static int process_rxed_802_11_packet(wlan_private * priv, struct sk_buff *skb)
                                                            rx_radiotap_hdr));
                memcpy(pradiotap_hdr, &radiotap_hdr,
                       sizeof(struct rx_radiotap_hdr));
-               break;
-
-       default:
-               /* unknown header */
-               lbs_pr_alert("Unknown radiomode %i\n",
-                      priv->adapter->radiomode);
-               /* don't export any header */
-               /* chop the rxpd */
-               skb_pull(skb, sizeof(struct rxpd));
-               break;
        }
 
        /* Take the data rate from the rxpd structure
index bb6e17506a41f3aa15840a7c439d626202f7a84a..fbec06c10dd7c227941811f739c5ee820bb8e1f3 100644 (file)
@@ -86,7 +86,7 @@ static int SendSinglePacket(wlan_private * priv, struct sk_buff *skb)
        plocaltxpd->tx_packet_location = cpu_to_le32(sizeof(struct txpd));
 
        p802x_hdr = skb->data;
-       if (priv->adapter->radiomode == WLAN_RADIOMODE_RADIOTAP) {
+       if (priv->adapter->monitormode != WLAN_MONITOR_OFF) {
 
                /* locate radiotap header */
                pradiotap_hdr = (struct tx_radiotap_hdr *)skb->data;
@@ -106,7 +106,7 @@ static int SendSinglePacket(wlan_private * priv, struct sk_buff *skb)
 
        }
        /* copy destination address from 802.3 or 802.11 header */
-       if (priv->adapter->linkmode == WLAN_LINKMODE_802_11)
+       if (priv->adapter->monitormode != WLAN_MONITOR_OFF)
                memcpy(plocaltxpd->tx_dest_addr_high, p802x_hdr + 4, ETH_ALEN);
        else
                memcpy(plocaltxpd->tx_dest_addr_high, p802x_hdr, ETH_ALEN);
@@ -144,7 +144,7 @@ done:
                priv->stats.tx_errors++;
        }
 
-       if (!ret && priv->adapter->radiomode == WLAN_RADIOMODE_RADIOTAP) {
+       if (!ret && priv->adapter->monitormode != WLAN_MONITOR_OFF) {
                /* Keep the skb to echo it back once Tx feedback is
                   received from FW */
                skb_orphan(skb);
@@ -252,7 +252,7 @@ void libertas_send_tx_feedback(wlan_private * priv)
        int txfail;
        int try_count;
 
-       if (adapter->radiomode != WLAN_RADIOMODE_RADIOTAP ||
+       if (adapter->monitormode == WLAN_MONITOR_OFF ||
            adapter->currenttxskb == NULL)
                return;
 
index 5b0bbc99a21e632d8be970de57ec5807f189f174..6aa444c7de8d6d43fc144ee5beb4397beca880b4 100644 (file)
@@ -15,10 +15,7 @@ struct wlan_ioctl_regrdwr {
        u32 value;
 };
 
-#define WLAN_LINKMODE_802_3                    0
-#define WLAN_LINKMODE_802_11                   2
-#define WLAN_RADIOMODE_NONE                    0
-#define WLAN_RADIOMODE_RADIOTAP                        2
+#define WLAN_MONITOR_OFF                       0
 
 extern struct iw_handler_def libertas_handler_def;
 extern struct iw_handler_def mesh_handler_def;