9 files changed, 217 insertions, 40 deletions

diff --git a/drivers/net/wireless/libertas/cmd.c b/drivers/net/wireless/libertas/cmd.c
index 517489a06063..cd3bddb243b1 100644
--- a/drivers/net/wireless/libertas/cmd.c
+++ b/drivers/net/wireless/libertas/cmd.c
@@ -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;
diff --git a/drivers/net/wireless/libertas/dev.h b/drivers/net/wireless/libertas/dev.h
index 397c5fca0ff5..5697fec0cb1d 100644
--- a/drivers/net/wireless/libertas/dev.h
+++ b/drivers/net/wireless/libertas/dev.h
@@ -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;
diff --git a/drivers/net/wireless/libertas/host.h b/drivers/net/wireless/libertas/host.h
index c6b44c831594..4ccdbf9c473a 100644
--- a/drivers/net/wireless/libertas/host.h
+++ b/drivers/net/wireless/libertas/host.h
@@ -110,6 +110,8 @@
 
 #define CMD_FWT_ACCESS                0x0095
 
+#define CMD_802_11_MONITOR_MODE       0x0098
+
 #define CMD_MESH_ACCESS               0x009b
 
 /* For the IEEE Power Save */
diff --git a/drivers/net/wireless/libertas/hostcmd.h b/drivers/net/wireless/libertas/hostcmd.h
index 44cf39c8d1b8..52884eaf2edd 100644
--- a/drivers/net/wireless/libertas/hostcmd.h
+++ b/drivers/net/wireless/libertas/hostcmd.h
@@ -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;
diff --git a/drivers/net/wireless/libertas/if_usb.c b/drivers/net/wireless/libertas/if_usb.c
index 670e1d23c043..d28802fb244b 100644
--- a/drivers/net/wireless/libertas/if_usb.c
+++ b/drivers/net/wireless/libertas/if_usb.c
@@ -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;
 
diff --git a/drivers/net/wireless/libertas/main.c b/drivers/net/wireless/libertas/main.c
index a3a17caae439..9a46339ce47e 100644
--- a/drivers/net/wireless/libertas/main.c
+++ b/drivers/net/wireless/libertas/main.c
@@ -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);
 
diff --git a/drivers/net/wireless/libertas/rx.c b/drivers/net/wireless/libertas/rx.c
index 83a7765fd23c..09def943b910 100644
--- a/drivers/net/wireless/libertas/rx.c
+++ b/drivers/net/wireless/libertas/rx.c
@@ -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
diff --git a/drivers/net/wireless/libertas/tx.c b/drivers/net/wireless/libertas/tx.c
index bb6e17506a41..fbec06c10dd7 100644
--- a/drivers/net/wireless/libertas/tx.c
+++ b/drivers/net/wireless/libertas/tx.c
@@ -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;
 
diff --git a/drivers/net/wireless/libertas/wext.h b/drivers/net/wireless/libertas/wext.h
index 5b0bbc99a21e..6aa444c7de8d 100644
--- a/drivers/net/wireless/libertas/wext.h
+++ b/drivers/net/wireless/libertas/wext.h
@@ -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;