[PATCH] prism54: Add avs header support

* islpci_eth.[c,h], islpci_dev.[c,h], isl_ioctl.[c,h] : added
  support for avs header in monitor mode. Based on the work of
  Antonio Eugenio Burriel <aeb@ryanstudios.com>. Unified packets
  header (rfmon_header and rx_annex) for iwspy.

drivers/net/wireless/prism54/isl_ioctl.c

@@ -327,34 +327,15 @@ prism54_set_freq(struct net_device *ndev, struct iw_request_info *info,
 {
 	islpci_private *priv = netdev_priv(ndev);
 	int rvalue;
-	u32 c = 0;
+	u32 c;
 
-	/* prepare the structure for the set object */
 	if (fwrq->m < 1000)
-		/* structure value contains a channel indication */
+		/* we have a channel number */
 		c = fwrq->m;
-	else {
-		/* structure contains a frequency indication and fwrq->e = 1 */
-		int f = fwrq->m / 100000;
-
-		if (fwrq->e != 1)
-			return -EINVAL;
-		if ((f >= 2412) && (f <= 2484)) {
-			while ((c < 14) && (f != frequency_list_bg[c]))
-				c++;
-			if (c >= 14)
-				return -EINVAL;
-		} else if ((f >= (int) 5170) && (f <= (int) 5320)) {
-			while ((c < 12) && (f != frequency_list_a[c]))
-				c++;
-			if (c >= 12)
-				return -EINVAL;
-		} else
-			return -EINVAL;
-		c++;
-	}
+	else
+		c = (fwrq->e == 1) ? channel_of_freq(fwrq->m / 100000) : 0;
 
-	rvalue = mgt_set_request(priv, DOT11_OID_CHANNEL, 0, &c);
+	rvalue = c ? mgt_set_request(priv, DOT11_OID_CHANNEL, 0, &c) : -EINVAL;
 
 	/* Call commit handler */
 	return (rvalue ? rvalue : -EINPROGRESS);
@@ -410,7 +391,7 @@ prism54_set_mode(struct net_device *ndev, struct iw_request_info *info,
 
 	mgt_commit(priv);
 	priv->ndev->type = (priv->iw_mode == IW_MODE_MONITOR)
-	    ? ARPHRD_IEEE80211 : ARPHRD_ETHER;
+	    ? priv->monitor_type : ARPHRD_ETHER;
 	up_write(&priv->mib_sem);
 
 	return 0;
@@ -1962,6 +1943,28 @@ prism54_get_wpa(struct net_device *ndev, struct iw_request_info *info,
 	return 0;
 }
 
+int
+prism54_set_prismhdr(struct net_device *ndev, struct iw_request_info *info,
+		     __u32 * uwrq, char *extra)
+{
+	islpci_private *priv = netdev_priv(ndev);
+	priv->monitor_type =
+	    (*uwrq ? ARPHRD_IEEE80211_PRISM : ARPHRD_IEEE80211);
+	if (priv->iw_mode == IW_MODE_MONITOR)
+		priv->ndev->type = priv->monitor_type;
+
+	return 0;
+}
+
+int
+prism54_get_prismhdr(struct net_device *ndev, struct iw_request_info *info,
+		     __u32 * uwrq, char *extra)
+{
+	islpci_private *priv = netdev_priv(ndev);
+	*uwrq = (priv->monitor_type == ARPHRD_IEEE80211_PRISM);
+	return 0;
+}
+
 int
 prism54_set_maxframeburst(struct net_device *ndev, struct iw_request_info *info,
 			  __u32 * uwrq, char *extra)
@@ -2198,6 +2201,9 @@ static const iw_handler prism54_handler[] = {
 #define	PRISM54_SET_OID_STR	SIOCIWFIRSTPRIV+20
 #define	PRISM54_SET_OID_ADDR	SIOCIWFIRSTPRIV+22
 
+#define PRISM54_GET_PRISMHDR	SIOCIWFIRSTPRIV+23
+#define PRISM54_SET_PRISMHDR	SIOCIWFIRSTPRIV+24
+
 #define IWPRIV_SET_U32(n,x)	{ n, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0, "set_"x }
 #define IWPRIV_SET_SSID(n,x)	{ n, IW_PRIV_TYPE_CHAR | IW_PRIV_SIZE_FIXED | 1, 0, "set_"x }
 #define IWPRIV_SET_ADDR(n,x)	{ n, IW_PRIV_TYPE_ADDR | IW_PRIV_SIZE_FIXED | 1, 0, "set_"x }
@@ -2212,6 +2218,10 @@ static const iw_handler prism54_handler[] = {
 static const struct iw_priv_args prism54_private_args[] = {
 /*{ cmd, set_args, get_args, name } */
 	{PRISM54_RESET, 0, 0, "reset"},
+	{PRISM54_GET_PRISMHDR, 0, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1,
+	 "get_prismhdr"},
+	{PRISM54_SET_PRISMHDR, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0,
+	 "set_prismhdr"},
 	{PRISM54_GET_POLICY, 0, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1,
 	 "getPolicy"},
 	{PRISM54_SET_POLICY, IW_PRIV_TYPE_INT | IW_PRIV_SIZE_FIXED | 1, 0,
@@ -2321,7 +2331,8 @@ static const iw_handler prism54_private_handler[] = {
 	(iw_handler) prism54_set_raw,
 	(iw_handler) NULL,
 	(iw_handler) prism54_set_raw,
-
+	(iw_handler) prism54_get_prismhdr,
+	(iw_handler) prism54_set_prismhdr,
 };
 
 const struct iw_handler_def prism54_handler_def = {

drivers/net/wireless/prism54/isl_oid.h

@@ -91,19 +91,6 @@ struct obj_frequencies {
 	u16 mhz[0];
 } __attribute__ ((packed));
 
-struct obj_rx_annex {
-	u8 addr1[ETH_ALEN];
-	u8 addr2[ETH_ALEN];
-	u32 something0;
-	u32 time;
-	u16 something1;
-	u16 rate;
-	u16 freq;
-	u16 something2;
-	u8 rssi;
-	u8 pad[3];
-} __attribute__ ((packed));
-
 /* 
  * in case everything's ok, the inlined function below will be
  * optimized away by the compiler...

drivers/net/wireless/prism54/islpci_dev.c

@@ -715,9 +715,9 @@ islpci_setup(struct pci_dev *pdev)
 	priv = netdev_priv(ndev);
 	priv->ndev = ndev;
 	priv->pdev = pdev;
-
+	priv->monitor_type = ARPHRD_IEEE80211;
 	priv->ndev->type = (priv->iw_mode == IW_MODE_MONITOR) ?
-		ARPHRD_IEEE80211: ARPHRD_ETHER;
+		priv->monitor_type : ARPHRD_ETHER;
 
 	/* save the start and end address of the PCI memory area */
 	ndev->mem_start = (unsigned long) priv->device_base;

drivers/net/wireless/prism54/islpci_dev.h

@@ -114,6 +114,8 @@ typedef struct {
 
 	struct iw_spy_data spy_data; /* iwspy support */
 
+	int monitor_type; /* ARPHRD_IEEE80211 or ARPHRD_IEEE80211_PRISM */
+
 	struct islpci_acl acl;
 
 	/* PCI bus allocation & configuration members */

drivers/net/wireless/prism54/islpci_eth.c

@@ -24,10 +24,12 @@
 #include <linux/delay.h>
 #include <linux/netdevice.h>
 #include <linux/etherdevice.h>
+#include <linux/if_arp.h>
 
 #include "isl_38xx.h"
 #include "islpci_eth.h"
 #include "islpci_mgt.h"
+#include "oid_mgt.h"
 
 /******************************************************************************
     Network Interface functions
@@ -246,6 +248,69 @@ islpci_eth_transmit(struct sk_buff *skb, struct net_device *ndev)
 	return err;
 }
 
+static inline int
+islpci_monitor_rx(islpci_private *priv, struct sk_buff **skb)
+{
+	/* The card reports full 802.11 packets but with a 20 bytes
+	 * header and without the FCS. But there a is a bit that
+	 * indicates if the packet is corrupted :-) */
+	struct rfmon_header *hdr = (struct rfmon_header *) (*skb)->data;
+	if (hdr->flags & 0x01)
+		/* This one is bad. Drop it ! */
+		return -1;
+	if (priv->ndev->type == ARPHRD_IEEE80211_PRISM) {
+		struct avs_80211_1_header *avs;
+		/* extract the relevant data from the header */
+		u32 clock = hdr->clock;
+		u8 rate = hdr->rate;
+		u16 freq = be16_to_cpu(hdr->freq);
+		u8 rssi = hdr->rssi;
+
+		skb_pull(*skb, sizeof (struct rfmon_header));
+
+		if (skb_headroom(*skb) < sizeof (struct avs_80211_1_header)) {
+			struct sk_buff *newskb = skb_copy_expand(*skb,
+								 sizeof (struct
+									 avs_80211_1_header),
+								 0, GFP_ATOMIC);
+			if (newskb) {
+				kfree_skb(*skb);
+				*skb = newskb;
+			} else
+				return -1;
+			/* This behavior is not very subtile... */
+		}
+
+		/* make room for the new header and fill it. */
+		avs =
+		    (struct avs_80211_1_header *) skb_push(*skb,
+							   sizeof (struct
+								   avs_80211_1_header));
+
+		avs->version = htonl(P80211CAPTURE_VERSION);
+		avs->length = htonl(sizeof (struct avs_80211_1_header));
+		avs->mactime = __cpu_to_be64(clock);
+		avs->hosttime = __cpu_to_be64(jiffies);
+		avs->phytype = htonl(6);	/*OFDM: 6 for (g), 8 for (a) */
+		avs->channel = htonl(channel_of_freq(freq));
+		avs->datarate = htonl(rate * 5);
+		avs->antenna = htonl(0);	/*unknown */
+		avs->priority = htonl(0);	/*unknown */
+		avs->ssi_type = htonl(2);	/*2: dBm, 3: raw RSSI */
+		avs->ssi_signal = htonl(rssi);
+		avs->ssi_noise = htonl(priv->local_iwstatistics.qual.noise);	/*better than 'undefined', I assume */
+		avs->preamble = htonl(0);	/*unknown */
+		avs->encoding = htonl(0);	/*unknown */
+	} else
+		skb_pull(*skb, sizeof (struct rfmon_header));
+
+	(*skb)->protocol = htons(ETH_P_802_2);
+	(*skb)->mac.raw = (*skb)->data;
+	(*skb)->pkt_type = PACKET_OTHERHOST;
+
+	return 0;
+}
+
 int
 islpci_eth_receive(islpci_private *priv)
 {
@@ -315,37 +380,29 @@ islpci_eth_receive(islpci_private *priv)
 	/* do some additional sk_buff and network layer parameters */
 	skb->dev = ndev;
 
-	/* take care of monitor mode */
-	if (priv->iw_mode == IW_MODE_MONITOR) {
-		/* The card reports full 802.11 packets but with a 20 bytes
-		 * header and without the FCS. But there a is a bit that
-		 * indicates if the packet is corrupted :-) */
-		if (skb->data[8] & 0x01)
-			/* This one is bad. Drop it ! */
-			discard = 1;
-		skb_pull(skb, 20);
-		skb->protocol = htons(ETH_P_802_2);
-		skb->mac.raw = skb->data;
-		skb->pkt_type = PACKET_OTHERHOST;
-	} else {
+	/* take care of monitor mode and spy monitoring. */
+	if (priv->iw_mode == IW_MODE_MONITOR)
+		discard = islpci_monitor_rx(priv, &skb);
+	else {
 		if (skb->data[2 * ETH_ALEN] == 0) {
 			/* The packet has a rx_annex. Read it for spy monitoring, Then
 			 * remove it, while keeping the 2 leading MAC addr.
 			 */
 			struct iw_quality wstats;
-			struct obj_rx_annex *annex =
-			    (struct obj_rx_annex *) skb->data;
-			wstats.level = annex->rssi;
+			struct rx_annex_header *annex =
+			    (struct rx_annex_header *) skb->data;
+			wstats.level = annex->rfmon.rssi;
 			/* The noise value can be a bit outdated if nobody's 
 			 * reading wireless stats... */
-			wstats.noise = priv->iwstatistics.qual.noise;
+			wstats.noise = priv->local_iwstatistics.qual.noise;
 			wstats.qual = wstats.level - wstats.noise;
 			wstats.updated = 0x07;
 			/* Update spy records */
 			wireless_spy_update(ndev, annex->addr2, &wstats);
-			/* 20 = sizeof(struct obj_rx_annex) - 2*ETH_ALEN */
-			memcpy(skb->data + 20, skb->data, 2 * ETH_ALEN);
-			skb_pull(skb, 20);
+
+			memcpy(skb->data + sizeof (struct rfmon_header),
+			       skb->data, 2 * ETH_ALEN);
+			skb_pull(skb, sizeof (struct rfmon_header));
 		}
 		skb->protocol = eth_type_trans(skb, ndev);
 	}

drivers/net/wireless/prism54/islpci_eth.h

@@ -23,6 +23,47 @@
 #include "isl_38xx.h"
 #include "islpci_dev.h"
 
+struct rfmon_header {
+	u16 unk0;		/* = 0x0000 */
+	u16 length;		/* = 0x1400 */
+	u32 clock;		/* 1MHz clock */
+	u8 flags;
+	u8 unk1;
+	u8 rate;
+	u8 unk2;
+	u16 freq;
+	u16 unk3;
+	u8 rssi;
+	u8 padding[3];
+} __attribute__ ((packed));
+
+struct rx_annex_header {
+	u8 addr1[ETH_ALEN];
+	u8 addr2[ETH_ALEN];
+	struct rfmon_header rfmon;
+} __attribute__ ((packed));
+
+/* wlan-ng (and hopefully others) AVS header, version one.  Fields in
+ * network byte order. */
+#define P80211CAPTURE_VERSION 0x80211001
+
+struct avs_80211_1_header {
+	uint32_t version;
+	uint32_t length;
+	uint64_t mactime;
+	uint64_t hosttime;
+	uint32_t phytype;
+	uint32_t channel;
+	uint32_t datarate;
+	uint32_t antenna;
+	uint32_t priority;
+	uint32_t ssi_type;
+	int32_t ssi_signal;
+	int32_t ssi_noise;
+	uint32_t preamble;
+	uint32_t encoding;
+};
+
 void islpci_eth_cleanup_transmit(islpci_private *, isl38xx_control_block *);
 int islpci_eth_transmit(struct sk_buff *, struct net_device *);
 int islpci_eth_receive(islpci_private *);

drivers/net/wireless/prism54/oid_mgt.c

@@ -31,6 +31,27 @@ const int frequency_list_a[] = { 5170, 5180, 5190, 5200, 5210, 5220, 5230,
 	5240, 5260, 5280, 5300, 5320
 };
 
+int
+channel_of_freq(int f)
+{
+	int c = 0;
+
+	if ((f >= 2412) && (f <= 2484)) {
+		while ((c < 14) && (f != frequency_list_bg[c]))
+			c++;
+		if (c >= 14)
+			return 0;
+	} else if ((f >= (int) 5170) && (f <= (int) 5320)) {
+		while ((c < 12) && (f != frequency_list_a[c]))
+			c++;
+		if (c >= 12)
+			return 0;
+	} else
+		return 0;
+
+	return ++c;
+}
+
 #define OID_STRUCT(name,oid,s,t) [name] = {oid, 0, sizeof(s), t}
 #define OID_STRUCT_C(name,oid,s,t) OID_STRUCT(name,oid,s,t | OID_FLAG_CACHED)
 #define OID_U32(name,oid) OID_STRUCT(name,oid,u32,OID_TYPE_U32)

drivers/net/wireless/prism54/oid_mgt.h

@@ -28,9 +28,10 @@ int mgt_init(islpci_private *);
 
 void mgt_clean(islpci_private *);
 
+/* I don't know where to put these 3 */
 extern const int frequency_list_bg[];
-
 extern const int frequency_list_a[];
+int channel_of_freq(int);
 
 void mgt_le_to_cpu(int, void *);