[PATCH] libertas: monitor mode support for OLPC firmware

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

@@ -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;

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;

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 */

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;

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;
 

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);