[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>

9 files changed, 217 insertions, 40 deletions

diff --git a/drivers/net/wireless/libertas/cmd.c b/drivers/net/wireless/libertas/cmd.c
index 517489a..cd3bddb 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 397c5fc..5697fec 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 c6b44c8..4ccdbf9 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 44cf39c..52884ea 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 670e1d2..d28802f 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 a3a17ca..9a46339 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 83a7765..09def94 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 bb6e175..fbec06c 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 5b0bbc9..6aa444c 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;