[PATCH] prism54: Add iwspy support

        * islpci_eth.c, islpci_dev.h, isl_ioctl.c : iwspy support.

drivers/net/wireless/prism54/isl_ioctl.c

@@ -2098,6 +2098,31 @@ prism54_debug_set_oid(struct net_device *ndev, struct iw_request_info *info,
 	return ret;
 }
 
+static int
+prism54_set_spy(struct net_device *ndev,
+		struct iw_request_info *info,
+		union iwreq_data *uwrq, char *extra)
+{
+	islpci_private *priv = netdev_priv(ndev);
+	u32 u, oid = OID_INL_CONFIG;
+
+	down_write(&priv->mib_sem);
+	mgt_get(priv, OID_INL_CONFIG, &u);
+
+	if ((uwrq->data.length == 0) && (priv->spy_data.spy_number > 0))
+		/* disable spy */
+		u &= ~INL_CONFIG_RXANNEX;
+	else if ((uwrq->data.length > 0) && (priv->spy_data.spy_number == 0))
+		/* enable spy */
+		u |= INL_CONFIG_RXANNEX;
+
+	mgt_set(priv, OID_INL_CONFIG, &u);
+	mgt_commit_list(priv, &oid, 1);
+	up_write(&priv->mib_sem);
+
+	return iw_handler_set_spy(ndev, info, uwrq, extra);
+}
+
 static const iw_handler prism54_handler[] = {
 	(iw_handler) prism54_commit,	/* SIOCSIWCOMMIT */
 	(iw_handler) prism54_get_name,	/* SIOCGIWNAME */
@@ -2115,7 +2140,7 @@ static const iw_handler prism54_handler[] = {
 	(iw_handler) NULL,	/* SIOCGIWPRIV */
 	(iw_handler) NULL,	/* SIOCSIWSTATS */
 	(iw_handler) NULL,	/* SIOCGIWSTATS */
-	iw_handler_set_spy,	/* SIOCSIWSPY */
+	prism54_set_spy,	/* SIOCSIWSPY */
 	iw_handler_get_spy,	/* SIOCGIWSPY */
 	iw_handler_set_thrspy,	/* SIOCSIWTHRSPY */
 	iw_handler_get_thrspy,	/* SIOCGIWTHRSPY */
@@ -2182,7 +2207,6 @@ static const iw_handler prism54_handler[] = {
 #define IWPRIV_SSID(n,x)	IWPRIV_SET_SSID(n,x), IWPRIV_GET(n,x)
 #define IWPRIV_ADDR(n,x)	IWPRIV_SET_ADDR(n,x), IWPRIV_GET(n,x)
 
-
 /* Note : limited to 128 private ioctls */
 
 static const struct iw_priv_args prism54_private_args[] = {
@@ -2261,7 +2285,7 @@ static const struct iw_priv_args prism54_private_args[] = {
 	IWPRIV_GET(DOT11_OID_FREQUENCYACTIVITY, "freqactivity"),
 	IWPRIV_U32(DOT11_OID_NONERPPROTECTION, "nonerpprotec"),
 	IWPRIV_U32(DOT11_OID_PROFILES, "profile"),
-	IWPRIV_GET(DOT11_OID_EXTENDEDRATES,"extrates"),
+	IWPRIV_GET(DOT11_OID_EXTENDEDRATES, "extrates"),
 	IWPRIV_U32(DOT11_OID_MLMEAUTOLEVEL, "mlmelevel"),
 
 	IWPRIV_GET(DOT11_OID_BSSS, "bsss"),
@@ -2308,20 +2332,13 @@ const struct iw_handler_def prism54_handler_def = {
 	.standard = (iw_handler *) prism54_handler,
 	.private = (iw_handler *) prism54_private_handler,
 	.private_args = (struct iw_priv_args *) prism54_private_args,
+	.spy_offset = offsetof(islpci_private, spy_data),
 };
 
-/* These ioctls won't work with the new API */
+/* For ioctls that don't work with the new API */
 
 int
 prism54_ioctl(struct net_device *ndev, struct ifreq *rq, int cmd)
 {
-	/*struct iwreq *wrq = (struct iwreq *) rq;
-	   islpci_private *priv = netdev_priv(ndev);
-	   int ret = 0;
-
-	   switch (cmd) {
-
-	   }
-	 */
 	return -EOPNOTSUPP;
 }

drivers/net/wireless/prism54/isl_oid.h

@@ -91,6 +91,19 @@ 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.h

@@ -26,6 +26,7 @@
 #include <linux/version.h>
 #include <linux/netdevice.h>
 #include <linux/wireless.h>
+#include <net/iw_handler.h>
 #include <linux/list.h>
 
 #if LINUX_VERSION_CODE > KERNEL_VERSION(2,5,41)
@@ -111,6 +112,8 @@ typedef struct {
 	struct iw_statistics local_iwstatistics;
 	struct iw_statistics iwstatistics;
 
+	struct iw_spy_data spy_data; /* iwspy support */
+
 	struct islpci_acl acl;
 
 	/* PCI bus allocation & configuration members */

drivers/net/wireless/prism54/islpci_eth.c

 /*
  *  
  *  Copyright (C) 2002 Intersil Americas Inc.
- *
+ *  Copyright (C) 2004 Aurelien Alleaume <slts@free.fr>
  *  This program is free software; you can redistribute it and/or modify
  *  it under the terms of the GNU General Public License as published by
  *  the Free Software Foundation; either version 2 of the License
@@ -266,7 +266,8 @@ islpci_eth_receive(islpci_private *priv)
 	index = priv->free_data_rx % ISL38XX_CB_RX_QSIZE;
 	size = le16_to_cpu(control_block->rx_data_low[index].size);
 	skb = priv->data_low_rx[index];
-	offset = ((unsigned long) le32_to_cpu(control_block->rx_data_low[index].address) -
+	offset = ((unsigned long)
+		  le32_to_cpu(control_block->rx_data_low[index].address) -
 		  (unsigned long) skb->data) & 3;
 
 #if VERBOSE > SHOW_ERROR_MESSAGES
@@ -320,15 +321,34 @@ islpci_eth_receive(islpci_private *priv)
 		 * 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 !*/
+			/* 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
+	} 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;
+			/* The noise value can be a bit outdated if nobody's 
+			 * reading wireless stats... */
+			wstats.noise = priv->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);
+		}
 		skb->protocol = eth_type_trans(skb, ndev);
-
+	}
 	skb->ip_summed = CHECKSUM_NONE;
 	priv->statistics.rx_packets++;
 	priv->statistics.rx_bytes += size;
@@ -343,8 +363,7 @@ islpci_eth_receive(islpci_private *priv)
 	if (discard) {
 		dev_kfree_skb(skb);
 		skb = NULL;
-	}
-	else
+	} else
 		netif_rx(skb);
 
 	/* increment the read index for the rx data low queue */
@@ -395,7 +414,7 @@ islpci_eth_receive(islpci_private *priv)
 		wmb();
 
 		/* increment the driver read pointer */
-		add_le32p((u32 *) & control_block->
+		add_le32p((u32 *) &control_block->
 			  driver_curr_frag[ISL38XX_CB_RX_DATA_LQ], 1);
 	}
 
@@ -423,7 +442,7 @@ islpci_eth_tx_timeout(struct net_device *ndev)
 	/* increment the transmit error counter */
 	statistics->tx_errors++;
 
-	if(!priv->reset_task_pending) {
+	if (!priv->reset_task_pending) {
 		priv->reset_task_pending = 1;
 		netif_stop_queue(ndev);
 		schedule_work(&priv->reset_task);

drivers/net/wireless/prism54/oid_mgt.c

@@ -570,6 +570,17 @@ mgt_set(islpci_private *priv, enum oid_num_t n, void *data)
 	mgt_cpu_to_le(isl_oid[n].flags & OID_FLAG_TYPE, priv->mib[n]);
 }
 
+void
+mgt_get(islpci_private *priv, enum oid_num_t n, void *res)
+{
+	BUG_ON(OID_NUM_LAST <= n);
+	BUG_ON(priv->mib[n] == NULL);
+	BUG_ON(res == NULL);
+
+	memcpy(res, priv->mib[n], isl_oid[n].size);
+	mgt_le_to_cpu(isl_oid[n].flags & OID_FLAG_TYPE, res);
+}
+
 /* Commits the cache. Lock outside. */
 
 static enum oid_num_t commit_part1[] = {

drivers/net/wireless/prism54/oid_mgt.h

@@ -43,6 +43,8 @@ int mgt_commit_list(islpci_private *, enum oid_num_t *, int);
 
 void mgt_set(islpci_private *, enum oid_num_t, void *);
 
+void mgt_get(islpci_private *, enum oid_num_t, void *);
+
 void mgt_commit(islpci_private *);
 
 int mgt_mlme_answer(islpci_private *);