201 files changed, 99370 insertions, 4204 deletions

diff --git a/drivers/net/wireless/Kconfig b/drivers/net/wireless/Kconfig
index ae27af0141c0..5a6fdfd0f140 100644
--- a/drivers/net/wireless/Kconfig
+++ b/drivers/net/wireless/Kconfig
@@ -63,11 +63,6 @@ config WAVELAN
 	  a Radio LAN (wireless Ethernet-like Local Area Network) using the
 	  radio frequencies 900 MHz and 2.4 GHz.
 
-	  This driver support the ISA version of the WaveLAN card.  A separate
-	  driver for the PCMCIA (PC-card) hardware is available in David
-	  Hinds' pcmcia-cs package (see the file <file:Documentation/Changes>
-	  for location).
-
 	  If you want to use an ISA WaveLAN card under Linux, say Y and read
 	  the Ethernet-HOWTO, available from
 	  <http://www.tldp.org/docs.html#howto>. Some more specific
@@ -280,6 +275,13 @@ config LIBERTAS_USB
 	---help---
 	  A driver for Marvell Libertas 8388 USB devices.
 
+config LIBERTAS_CS
+	tristate "Marvell Libertas 8385 CompactFlash 802.11b/g cards"
+	depends on LIBERTAS && PCMCIA && EXPERIMENTAL
+	select FW_LOADER
+	---help---
+	  A driver for Marvell Libertas 8385 CompactFlash devices.
+
 config LIBERTAS_DEBUG
 	bool "Enable full debugging output in the Libertas module."
 	depends on LIBERTAS
@@ -379,30 +381,6 @@ config PCI_HERMES
 	  common.  Some of the built-in wireless adaptors in laptops are of
 	  this variety.
 
-config ATMEL
-      tristate "Atmel at76c50x chipset  802.11b support"
-      depends on (PCI || PCMCIA) && WLAN_80211
-      select WIRELESS_EXT
-      select FW_LOADER
-      select CRC32
-       ---help---
-        A driver 802.11b wireless cards based on the Atmel fast-vnet
-        chips. This driver supports standard Linux wireless extensions. 
- 
-        Many  cards based on this chipset do not have flash memory
-        and need their firmware loaded at start-up. If yours is 
-        one of these, you will need to provide a firmware image
-        to be loaded into the card by the driver. The Atmel
-        firmware package can be downloaded from
-        <http://www.thekelleys.org.uk/atmel>
-
-config PCI_ATMEL
-      tristate "Atmel at76c506 PCI cards"
-      depends on ATMEL && PCI
-       ---help---
-        Enable support for PCI and mini-PCI cards containing the
-        Atmel at76c506 chip.
-
 config PCMCIA_HERMES
 	tristate "Hermes PCMCIA card support"
 	depends on PCMCIA && HERMES
@@ -414,12 +392,7 @@ config PCMCIA_HERMES
 	  such as the Linksys, D-Link and Farallon Skyline.  It should also
 	  work on Symbol cards such as the 3Com AirConnect and Ericsson WLAN.
 
-	  To use your PC-cards, you will need supporting software from David
-	  Hinds' pcmcia-cs package (see the file <file:Documentation/Changes>
-	  for location).  You also want to check out the PCMCIA-HOWTO,
-	  available from <http://www.tldp.org/docs.html#howto>.
-
-	  You will also very likely also need the Wireless Tools in order to
+	  You will very likely need the Wireless Tools in order to
 	  configure your card and that /etc/pcmcia/wireless.opts works:
 	  <http://www.hpl.hp.com/personal/Jean_Tourrilhes/Linux/Tools.html>.
 
@@ -437,6 +410,40 @@ config PCMCIA_SPECTRUM
 	  for downloading Symbol firmware are available at
 	  <http://sourceforge.net/projects/orinoco/>
 
+config ATMEL
+      tristate "Atmel at76c50x chipset  802.11b support"
+      depends on (PCI || PCMCIA) && WLAN_80211
+      select WIRELESS_EXT
+      select FW_LOADER
+      select CRC32
+       ---help---
+        A driver 802.11b wireless cards based on the Atmel fast-vnet
+        chips. This driver supports standard Linux wireless extensions.
+
+        Many  cards based on this chipset do not have flash memory
+        and need their firmware loaded at start-up. If yours is
+        one of these, you will need to provide a firmware image
+        to be loaded into the card by the driver. The Atmel
+        firmware package can be downloaded from
+        <http://www.thekelleys.org.uk/atmel>
+
+config PCI_ATMEL
+      tristate "Atmel at76c506 PCI cards"
+      depends on ATMEL && PCI
+       ---help---
+        Enable support for PCI and mini-PCI cards containing the
+        Atmel at76c506 chip.
+
+config PCMCIA_ATMEL
+	tristate "Atmel at76c502/at76c504 PCMCIA cards"
+	depends on ATMEL && PCMCIA
+	select WIRELESS_EXT
+	select FW_LOADER
+	select CRC32
+	---help---
+	  Enable support for PCMCIA cards containing the
+	  Atmel at76c502 and at76c504 chips.
+
 config AIRO_CS
 	tristate "Cisco/Aironet 34X/35X/4500/4800 PCMCIA cards"
 	depends on PCMCIA && (BROKEN || !M32R) && WLAN_80211
@@ -457,21 +464,6 @@ config AIRO_CS
 	  and Cisco proprietary API, so both the Linux Wireless Tools and the
 	  Cisco Linux utilities can be used to configure the card.
 
-	  To use your PC-cards, you will need supporting software from David
-	  Hinds' pcmcia-cs package (see the file <file:Documentation/Changes>
-	  for location).  You also want to check out the PCMCIA-HOWTO,
-	  available from <http://www.tldp.org/docs.html#howto>.
-
-config PCMCIA_ATMEL
-	tristate "Atmel at76c502/at76c504 PCMCIA cards"
-	depends on ATMEL && PCMCIA
-	select WIRELESS_EXT
-	select FW_LOADER
-	select CRC32
-	---help---
-	  Enable support for PCMCIA cards containing the
-	  Atmel at76c502 and at76c504 chips.
-
 config PCMCIA_WL3501
       tristate "Planet WL3501 PCMCIA cards"
       depends on EXPERIMENTAL && PCMCIA && WLAN_80211
@@ -558,8 +550,52 @@ config RTL8187
 
 	  Thanks to Realtek for their support!
 
+config ADM8211
+	tristate "ADMtek ADM8211 support"
+	depends on MAC80211 && PCI && WLAN_80211 && EXPERIMENTAL
+	select CRC32
+	select EEPROM_93CX6
+	---help---
+	  This driver is for ADM8211A, ADM8211B, and ADM8211C based cards.
+	  These are PCI/mini-PCI/Cardbus 802.11b chips found in cards such as:
+
+	  Xterasys Cardbus XN-2411b
+	  Blitz NetWave Point PC
+	  TrendNet 221pc
+	  Belkin F5D6001
+	  SMC 2635W
+	  Linksys WPC11 v1
+	  Fiberline FL-WL-200X
+	  3com Office Connect (3CRSHPW796)
+	  Corega WLPCIB-11
+	  SMC 2602W V2 EU
+	  D-Link DWL-520 Revision C
+
+	  However, some of these cards have been replaced with other chips
+	  like the RTL8180L (Xterasys Cardbus XN-2411b, Belkin F5D6001) or
+	  the Ralink RT2400 (SMC2635W) without a model number change.
+
+	  Thanks to Infineon-ADMtek for their support of this driver.
+
+config P54_COMMON
+	tristate "Softmac Prism54 support"
+	depends on MAC80211 && WLAN_80211 && FW_LOADER && EXPERIMENTAL
+
+config P54_USB
+	tristate "Prism54 USB support"
+	depends on P54_COMMON && USB
+	select CRC32
+
+config P54_PCI
+	tristate "Prism54 PCI support"
+	depends on P54_COMMON && PCI
+
+source "drivers/net/wireless/iwlwifi/Kconfig"
 source "drivers/net/wireless/hostap/Kconfig"
 source "drivers/net/wireless/bcm43xx/Kconfig"
+source "drivers/net/wireless/b43/Kconfig"
+source "drivers/net/wireless/b43legacy/Kconfig"
 source "drivers/net/wireless/zd1211rw/Kconfig"
+source "drivers/net/wireless/rt2x00/Kconfig"
 
 endmenu
diff --git a/drivers/net/wireless/Makefile b/drivers/net/wireless/Makefile
index ef35bc6c4a22..6f32b53ee128 100644
--- a/drivers/net/wireless/Makefile
+++ b/drivers/net/wireless/Makefile
@@ -36,6 +36,8 @@ obj-$(CONFIG_PRISM54)		+= prism54/
 
 obj-$(CONFIG_HOSTAP)		+= hostap/
 obj-$(CONFIG_BCM43XX)		+= bcm43xx/
+obj-$(CONFIG_B43)		+= b43/
+obj-$(CONFIG_B43LEGACY)		+= b43legacy/
 obj-$(CONFIG_ZD1211RW)		+= zd1211rw/
 
 # 16-bit wireless PCMCIA client drivers
@@ -43,7 +45,16 @@ obj-$(CONFIG_PCMCIA_RAYCS)	+= ray_cs.o
 obj-$(CONFIG_PCMCIA_WL3501)	+= wl3501_cs.o
 
 obj-$(CONFIG_USB_ZD1201)	+= zd1201.o
-obj-$(CONFIG_LIBERTAS_USB)     += libertas/
+obj-$(CONFIG_LIBERTAS)		+= libertas/
 
 rtl8187-objs		:= rtl8187_dev.o rtl8187_rtl8225.o
 obj-$(CONFIG_RTL8187)	+= rtl8187.o
+
+obj-$(CONFIG_ADM8211)	+= adm8211.o
+
+obj-$(CONFIG_IWLWIFI)	+= iwlwifi/
+obj-$(CONFIG_RT2X00)	+= rt2x00/
+
+obj-$(CONFIG_P54_COMMON)	+= p54common.o
+obj-$(CONFIG_P54_USB)		+= p54usb.o
+obj-$(CONFIG_P54_PCI)		+= p54pci.o
diff --git a/drivers/net/wireless/adm8211.c b/drivers/net/wireless/adm8211.c
new file mode 100644
index 000000000000..5bf7913aadda
--- /dev/null
+++ b/drivers/net/wireless/adm8211.c
@@ -0,0 +1,2053 @@
+
+/*
+ * Linux device driver for ADMtek ADM8211 (IEEE 802.11b MAC/BBP)
+ *
+ * Copyright (c) 2003, Jouni Malinen <j@w1.fi>
+ * Copyright (c) 2004-2007, Michael Wu <flamingice@sourmilk.net>
+ * Some parts copyright (c) 2003 by David Young <dyoung@pobox.com>
+ * and used with permission.
+ *
+ * Much thanks to Infineon-ADMtek for their support of this driver.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation. See README and COPYING for
+ * more details.
+ */
+
+#include <linux/init.h>
+#include <linux/if.h>
+#include <linux/skbuff.h>
+#include <linux/etherdevice.h>
+#include <linux/pci.h>
+#include <linux/delay.h>
+#include <linux/crc32.h>
+#include <linux/eeprom_93cx6.h>
+#include <net/mac80211.h>
+
+#include "adm8211.h"
+
+MODULE_AUTHOR("Michael Wu <flamingice@sourmilk.net>");
+MODULE_AUTHOR("Jouni Malinen <j@w1.fi>");
+MODULE_DESCRIPTION("Driver for IEEE 802.11b wireless cards based on ADMtek ADM8211");
+MODULE_SUPPORTED_DEVICE("ADM8211");
+MODULE_LICENSE("GPL");
+
+static unsigned int tx_ring_size __read_mostly = 16;
+static unsigned int rx_ring_size __read_mostly = 16;
+
+module_param(tx_ring_size, uint, 0);
+module_param(rx_ring_size, uint, 0);
+
+static struct pci_device_id adm8211_pci_id_table[] __devinitdata = {
+	/* ADMtek ADM8211 */
+	{ PCI_DEVICE(0x10B7, 0x6000) }, /* 3Com 3CRSHPW796 */
+	{ PCI_DEVICE(0x1200, 0x8201) }, /* ? */
+	{ PCI_DEVICE(0x1317, 0x8201) }, /* ADM8211A */
+	{ PCI_DEVICE(0x1317, 0x8211) }, /* ADM8211B/C */
+	{ 0 }
+};
+
+static void adm8211_eeprom_register_read(struct eeprom_93cx6 *eeprom)
+{
+	struct adm8211_priv *priv = eeprom->data;
+	u32 reg = ADM8211_CSR_READ(SPR);
+
+	eeprom->reg_data_in = reg & ADM8211_SPR_SDI;
+	eeprom->reg_data_out = reg & ADM8211_SPR_SDO;
+	eeprom->reg_data_clock = reg & ADM8211_SPR_SCLK;
+	eeprom->reg_chip_select = reg & ADM8211_SPR_SCS;
+}
+
+static void adm8211_eeprom_register_write(struct eeprom_93cx6 *eeprom)
+{
+	struct adm8211_priv *priv = eeprom->data;
+	u32 reg = 0x4000 | ADM8211_SPR_SRS;
+
+	if (eeprom->reg_data_in)
+		reg |= ADM8211_SPR_SDI;
+	if (eeprom->reg_data_out)
+		reg |= ADM8211_SPR_SDO;
+	if (eeprom->reg_data_clock)
+		reg |= ADM8211_SPR_SCLK;
+	if (eeprom->reg_chip_select)
+		reg |= ADM8211_SPR_SCS;
+
+	ADM8211_CSR_WRITE(SPR, reg);
+	ADM8211_CSR_READ(SPR);		/* eeprom_delay */
+}
+
+static int adm8211_read_eeprom(struct ieee80211_hw *dev)
+{
+	struct adm8211_priv *priv = dev->priv;
+	unsigned int words, i;
+	struct ieee80211_chan_range chan_range;
+	u16 cr49;
+	struct eeprom_93cx6 eeprom = {
+		.data		= priv,
+		.register_read	= adm8211_eeprom_register_read,
+		.register_write	= adm8211_eeprom_register_write
+	};
+
+	if (ADM8211_CSR_READ(CSR_TEST0) & ADM8211_CSR_TEST0_EPTYP) {
+		/* 256 * 16-bit = 512 bytes */
+		eeprom.width = PCI_EEPROM_WIDTH_93C66;
+		words = 256;
+	} else {
+		/* 64 * 16-bit = 128 bytes */
+		eeprom.width = PCI_EEPROM_WIDTH_93C46;
+		words = 64;
+	}
+
+	priv->eeprom_len = words * 2;
+	priv->eeprom = kmalloc(priv->eeprom_len, GFP_KERNEL);
+	if (!priv->eeprom)
+		return -ENOMEM;
+
+	eeprom_93cx6_multiread(&eeprom, 0, (__le16 __force *)priv->eeprom, words);
+
+	cr49 = le16_to_cpu(priv->eeprom->cr49);
+	priv->rf_type = (cr49 >> 3) & 0x7;
+	switch (priv->rf_type) {
+	case ADM8211_TYPE_INTERSIL:
+	case ADM8211_TYPE_RFMD:
+	case ADM8211_TYPE_MARVEL:
+	case ADM8211_TYPE_AIROHA:
+	case ADM8211_TYPE_ADMTEK:
+		break;
+
+	default:
+		if (priv->pdev->revision < ADM8211_REV_CA)
+			priv->rf_type = ADM8211_TYPE_RFMD;
+		else
+			priv->rf_type = ADM8211_TYPE_AIROHA;
+
+		printk(KERN_WARNING "%s (adm8211): Unknown RFtype %d\n",
+		       pci_name(priv->pdev), (cr49 >> 3) & 0x7);
+	}
+
+	priv->bbp_type = cr49 & 0x7;
+	switch (priv->bbp_type) {
+	case ADM8211_TYPE_INTERSIL:
+	case ADM8211_TYPE_RFMD:
+	case ADM8211_TYPE_MARVEL:
+	case ADM8211_TYPE_AIROHA:
+	case ADM8211_TYPE_ADMTEK:
+		break;
+	default:
+		if (priv->pdev->revision < ADM8211_REV_CA)
+			priv->bbp_type = ADM8211_TYPE_RFMD;
+		else
+			priv->bbp_type = ADM8211_TYPE_ADMTEK;
+
+		printk(KERN_WARNING "%s (adm8211): Unknown BBPtype: %d\n",
+		       pci_name(priv->pdev), cr49 >> 3);
+	}
+
+	if (priv->eeprom->country_code >= ARRAY_SIZE(cranges)) {
+		printk(KERN_WARNING "%s (adm8211): Invalid country code (%d)\n",
+		       pci_name(priv->pdev), priv->eeprom->country_code);
+
+		chan_range = cranges[2];
+	} else
+		chan_range = cranges[priv->eeprom->country_code];
+
+	printk(KERN_DEBUG "%s (adm8211): Channel range: %d - %d\n",
+	       pci_name(priv->pdev), (int)chan_range.min, (int)chan_range.max);
+
+	priv->modes[0].num_channels = chan_range.max - chan_range.min + 1;
+	priv->modes[0].channels = priv->channels;
+
+	memcpy(priv->channels, adm8211_channels, sizeof(adm8211_channels));
+
+	for (i = 1; i <= ARRAY_SIZE(adm8211_channels); i++)
+		if (i >= chan_range.min && i <= chan_range.max)
+			priv->channels[i - 1].flag =
+				IEEE80211_CHAN_W_SCAN |
+				IEEE80211_CHAN_W_ACTIVE_SCAN |
+				IEEE80211_CHAN_W_IBSS;
+
+	switch (priv->eeprom->specific_bbptype) {
+	case ADM8211_BBP_RFMD3000:
+	case ADM8211_BBP_RFMD3002:
+	case ADM8211_BBP_ADM8011:
+		priv->specific_bbptype = priv->eeprom->specific_bbptype;
+		break;
+
+	default:
+		if (priv->pdev->revision < ADM8211_REV_CA)
+			priv->specific_bbptype = ADM8211_BBP_RFMD3000;
+		else
+			priv->specific_bbptype = ADM8211_BBP_ADM8011;
+
+		printk(KERN_WARNING "%s (adm8211): Unknown specific BBP: %d\n",
+		       pci_name(priv->pdev), priv->eeprom->specific_bbptype);
+	}
+
+	switch (priv->eeprom->specific_rftype) {
+	case ADM8211_RFMD2948:
+	case ADM8211_RFMD2958:
+	case ADM8211_RFMD2958_RF3000_CONTROL_POWER:
+	case ADM8211_MAX2820:
+	case ADM8211_AL2210L:
+		priv->transceiver_type = priv->eeprom->specific_rftype;
+		break;
+
+	default:
+		if (priv->pdev->revision == ADM8211_REV_BA)
+			priv->transceiver_type = ADM8211_RFMD2958_RF3000_CONTROL_POWER;
+		else if (priv->pdev->revision == ADM8211_REV_CA)
+			priv->transceiver_type = ADM8211_AL2210L;
+		else if (priv->pdev->revision == ADM8211_REV_AB)
+			priv->transceiver_type = ADM8211_RFMD2948;
+
+		printk(KERN_WARNING "%s (adm8211): Unknown transceiver: %d\n",
+		       pci_name(priv->pdev), priv->eeprom->specific_rftype);
+
+		break;
+	}
+
+	printk(KERN_DEBUG "%s (adm8211): RFtype=%d BBPtype=%d Specific BBP=%d "
+               "Transceiver=%d\n", pci_name(priv->pdev), priv->rf_type,
+	       priv->bbp_type, priv->specific_bbptype, priv->transceiver_type);
+
+	return 0;
+}
+
+static inline void adm8211_write_sram(struct ieee80211_hw *dev,
+				      u32 addr, u32 data)
+{
+	struct adm8211_priv *priv = dev->priv;
+
+	ADM8211_CSR_WRITE(WEPCTL, addr | ADM8211_WEPCTL_TABLE_WR |
+			  (priv->pdev->revision < ADM8211_REV_BA ?
+			   0 : ADM8211_WEPCTL_SEL_WEPTABLE ));
+	ADM8211_CSR_READ(WEPCTL);
+	msleep(1);
+
+	ADM8211_CSR_WRITE(WESK, data);
+	ADM8211_CSR_READ(WESK);
+	msleep(1);
+}
+
+static void adm8211_write_sram_bytes(struct ieee80211_hw *dev,
+				     unsigned int addr, u8 *buf,
+				     unsigned int len)
+{
+	struct adm8211_priv *priv = dev->priv;
+	u32 reg = ADM8211_CSR_READ(WEPCTL);
+	unsigned int i;
+
+	if (priv->pdev->revision < ADM8211_REV_BA) {
+		for (i = 0; i < len; i += 2) {
+			u16 val = buf[i] | (buf[i + 1] << 8);
+			adm8211_write_sram(dev, addr + i / 2, val);
+		}
+	} else {
+		for (i = 0; i < len; i += 4) {
+			u32 val = (buf[i + 0] << 0 ) | (buf[i + 1] << 8 ) |
+				  (buf[i + 2] << 16) | (buf[i + 3] << 24);
+			adm8211_write_sram(dev, addr + i / 4, val);
+		}
+	}
+
+	ADM8211_CSR_WRITE(WEPCTL, reg);
+}
+
+static void adm8211_clear_sram(struct ieee80211_hw *dev)
+{
+	struct adm8211_priv *priv = dev->priv;
+	u32 reg = ADM8211_CSR_READ(WEPCTL);
+	unsigned int addr;
+
+	for (addr = 0; addr < ADM8211_SRAM_SIZE; addr++)
+		adm8211_write_sram(dev, addr, 0);
+
+	ADM8211_CSR_WRITE(WEPCTL, reg);
+}
+
+static int adm8211_get_stats(struct ieee80211_hw *dev,
+			     struct ieee80211_low_level_stats *stats)
+{
+	struct adm8211_priv *priv = dev->priv;
+
+	memcpy(stats, &priv->stats, sizeof(*stats));
+
+	return 0;
+}
+
+static int adm8211_get_tx_stats(struct ieee80211_hw *dev,
+				struct ieee80211_tx_queue_stats *stats)
+{
+	struct adm8211_priv *priv = dev->priv;
+	struct ieee80211_tx_queue_stats_data *data = &stats->data[0];
+
+	data->len = priv->cur_tx - priv->dirty_tx;
+	data->limit = priv->tx_ring_size - 2;
+	data->count = priv->dirty_tx;
+
+	return 0;
+}
+
+static void adm8211_interrupt_tci(struct ieee80211_hw *dev)
+{
+	struct adm8211_priv *priv = dev->priv;
+	unsigned int dirty_tx;
+
+	spin_lock(&priv->lock);
+
+	for (dirty_tx = priv->dirty_tx; priv->cur_tx - dirty_tx; dirty_tx++) {
+		unsigned int entry = dirty_tx % priv->tx_ring_size;
+		u32 status = le32_to_cpu(priv->tx_ring[entry].status);
+		struct ieee80211_tx_status tx_status;
+		struct adm8211_tx_ring_info *info;
+		struct sk_buff *skb;
+
+		if (status & TDES0_CONTROL_OWN ||
+		    !(status & TDES0_CONTROL_DONE))
+			break;
+
+		info = &priv->tx_buffers[entry];
+		skb = info->skb;
+
+		/* TODO: check TDES0_STATUS_TUF and TDES0_STATUS_TRO */
+
+		pci_unmap_single(priv->pdev, info->mapping,
+				 info->skb->len, PCI_DMA_TODEVICE);
+
+		memset(&tx_status, 0, sizeof(tx_status));
+		skb_pull(skb, sizeof(struct adm8211_tx_hdr));
+		memcpy(skb_push(skb, info->hdrlen), skb->cb, info->hdrlen);
+		memcpy(&tx_status.control, &info->tx_control,
+		       sizeof(tx_status.control));
+		if (!(tx_status.control.flags & IEEE80211_TXCTL_NO_ACK)) {
+			if (status & TDES0_STATUS_ES)
+				tx_status.excessive_retries = 1;
+			else
+				tx_status.flags |= IEEE80211_TX_STATUS_ACK;
+		}
+		ieee80211_tx_status_irqsafe(dev, skb, &tx_status);
+
+		info->skb = NULL;
+	}
+
+	if (priv->cur_tx - dirty_tx < priv->tx_ring_size - 2)
+		ieee80211_wake_queue(dev, 0);
+
+	priv->dirty_tx = dirty_tx;
+	spin_unlock(&priv->lock);
+}
+
+
+static void adm8211_interrupt_rci(struct ieee80211_hw *dev)
+{
+	struct adm8211_priv *priv = dev->priv;
+	unsigned int entry = priv->cur_rx % priv->rx_ring_size;
+	u32 status;
+	unsigned int pktlen;
+	struct sk_buff *skb, *newskb;
+	unsigned int limit = priv->rx_ring_size;
+	static const u8 rate_tbl[] = {10, 20, 55, 110, 220};
+	u8 rssi, rate;
+
+	while (!(priv->rx_ring[entry].status & cpu_to_le32(RDES0_STATUS_OWN))) {
+		if (!limit--)
+			break;
+
+		status = le32_to_cpu(priv->rx_ring[entry].status);
+		rate = (status & RDES0_STATUS_RXDR) >> 12;
+		rssi = le32_to_cpu(priv->rx_ring[entry].length) &
+			RDES1_STATUS_RSSI;
+
+		pktlen = status & RDES0_STATUS_FL;
+		if (pktlen > RX_PKT_SIZE) {
+			if (net_ratelimit())
+				printk(KERN_DEBUG "%s: frame too long (%d)\n",
+				       wiphy_name(dev->wiphy), pktlen);
+			pktlen = RX_PKT_SIZE;
+		}
+
+		if (!priv->soft_rx_crc && status & RDES0_STATUS_ES) {
+			skb = NULL; /* old buffer will be reused */
+			/* TODO: update RX error stats */
+			/* TODO: check RDES0_STATUS_CRC*E */
+		} else if (pktlen < RX_COPY_BREAK) {
+			skb = dev_alloc_skb(pktlen);
+			if (skb) {
+				pci_dma_sync_single_for_cpu(
+					priv->pdev,
+					priv->rx_buffers[entry].mapping,
+					pktlen, PCI_DMA_FROMDEVICE);
+				memcpy(skb_put(skb, pktlen),
+				       skb_tail_pointer(priv->rx_buffers[entry].skb),
+				       pktlen);
+				pci_dma_sync_single_for_device(
+					priv->pdev,
+					priv->rx_buffers[entry].mapping,
+					RX_PKT_SIZE, PCI_DMA_FROMDEVICE);
+			}
+		} else {
+			newskb = dev_alloc_skb(RX_PKT_SIZE);
+			if (newskb) {
+				skb = priv->rx_buffers[entry].skb;
+				skb_put(skb, pktlen);
+				pci_unmap_single(
+					priv->pdev,
+					priv->rx_buffers[entry].mapping,
+					RX_PKT_SIZE, PCI_DMA_FROMDEVICE);
+				priv->rx_buffers[entry].skb = newskb;
+				priv->rx_buffers[entry].mapping =
+					pci_map_single(priv->pdev,
+						       skb_tail_pointer(newskb),
+						       RX_PKT_SIZE,
+						       PCI_DMA_FROMDEVICE);
+			} else {
+				skb = NULL;
+				/* TODO: update rx dropped stats */
+			}
+
+			priv->rx_ring[entry].buffer1 =
+				cpu_to_le32(priv->rx_buffers[entry].mapping);
+		}
+
+		priv->rx_ring[entry].status = cpu_to_le32(RDES0_STATUS_OWN |
+							  RDES0_STATUS_SQL);
+		priv->rx_ring[entry].length =
+			cpu_to_le32(RX_PKT_SIZE |
+				    (entry == priv->rx_ring_size - 1 ?
+				     RDES1_CONTROL_RER : 0));
+
+		if (skb) {
+			struct ieee80211_rx_status rx_status = {0};
+
+			if (priv->pdev->revision < ADM8211_REV_CA)
+				rx_status.ssi = rssi;
+			else
+				rx_status.ssi = 100 - rssi;
+
+			if (rate <= 4)
+				rx_status.rate = rate_tbl[rate];
+
+			rx_status.channel = priv->channel;
+			rx_status.freq = adm8211_channels[priv->channel - 1].freq;
+			rx_status.phymode = MODE_IEEE80211B;
+
+			ieee80211_rx_irqsafe(dev, skb, &rx_status);
+		}
+
+		entry = (++priv->cur_rx) % priv->rx_ring_size;
+	}
+
+	/* TODO: check LPC and update stats? */
+}
+
+
+static irqreturn_t adm8211_interrupt(int irq, void *dev_id)
+{
+#define ADM8211_INT(x)							   \
+do {									   \
+	if (unlikely(stsr & ADM8211_STSR_ ## x))			   \
+		printk(KERN_DEBUG "%s: " #x "\n", wiphy_name(dev->wiphy)); \
+} while (0)
+
+	struct ieee80211_hw *dev = dev_id;
+	struct adm8211_priv *priv = dev->priv;
+	u32 stsr = ADM8211_CSR_READ(STSR);
+	ADM8211_CSR_WRITE(STSR, stsr);
+	if (stsr == 0xffffffff)
+		return IRQ_HANDLED;
+
+	if (!(stsr & (ADM8211_STSR_NISS | ADM8211_STSR_AISS)))
+		return IRQ_HANDLED;
+
+	if (stsr & ADM8211_STSR_RCI)
+		adm8211_interrupt_rci(dev);
+	if (stsr & ADM8211_STSR_TCI)
+		adm8211_interrupt_tci(dev);
+
+	/*ADM8211_INT(LinkOn);*/
+	/*ADM8211_INT(LinkOff);*/
+
+	ADM8211_INT(PCF);
+	ADM8211_INT(BCNTC);
+	ADM8211_INT(GPINT);
+	ADM8211_INT(ATIMTC);
+	ADM8211_INT(TSFTF);
+	ADM8211_INT(TSCZ);
+	ADM8211_INT(SQL);
+	ADM8211_INT(WEPTD);
+	ADM8211_INT(ATIME);
+	/*ADM8211_INT(TBTT);*/
+	ADM8211_INT(TEIS);
+	ADM8211_INT(FBE);
+	ADM8211_INT(REIS);
+	ADM8211_INT(GPTT);
+	ADM8211_INT(RPS);
+	ADM8211_INT(RDU);
+	ADM8211_INT(TUF);
+	/*ADM8211_INT(TRT);*/
+	/*ADM8211_INT(TLT);*/
+	/*ADM8211_INT(TDU);*/
+	ADM8211_INT(TPS);
+
+	return IRQ_HANDLED;
+
+#undef ADM8211_INT
+}
+
+#define WRITE_SYN(name,v_mask,v_shift,a_mask,a_shift,bits,prewrite,postwrite)\
+static void adm8211_rf_write_syn_ ## name (struct ieee80211_hw *dev,	     \
+					   u16 addr, u32 value) {	     \
+	struct adm8211_priv *priv = dev->priv;				     \
+	unsigned int i;							     \
+	u32 reg, bitbuf;						     \
+									     \
+	value &= v_mask;						     \
+	addr &= a_mask;							     \
+	bitbuf = (value << v_shift) | (addr << a_shift);		     \
+									     \
+	ADM8211_CSR_WRITE(SYNRF, ADM8211_SYNRF_IF_SELECT_1);		     \
+	ADM8211_CSR_READ(SYNRF);					     \
+	ADM8211_CSR_WRITE(SYNRF, ADM8211_SYNRF_IF_SELECT_0);		     \
+	ADM8211_CSR_READ(SYNRF);					     \
+									     \
+	if (prewrite) {							     \
+		ADM8211_CSR_WRITE(SYNRF, ADM8211_SYNRF_WRITE_SYNDATA_0);     \
+		ADM8211_CSR_READ(SYNRF);				     \
+	}								     \
+									     \
+	for (i = 0; i <= bits; i++) {					     \
+		if (bitbuf & (1 << (bits - i)))				     \
+			reg = ADM8211_SYNRF_WRITE_SYNDATA_1;		     \
+		else							     \
+			reg = ADM8211_SYNRF_WRITE_SYNDATA_0;		     \
+									     \
+		ADM8211_CSR_WRITE(SYNRF, reg);				     \
+		ADM8211_CSR_READ(SYNRF);				     \
+									     \
+		ADM8211_CSR_WRITE(SYNRF, reg | ADM8211_SYNRF_WRITE_CLOCK_1); \
+		ADM8211_CSR_READ(SYNRF);				     \
+		ADM8211_CSR_WRITE(SYNRF, reg | ADM8211_SYNRF_WRITE_CLOCK_0); \
+		ADM8211_CSR_READ(SYNRF);				     \
+	}								     \
+									     \
+	if (postwrite == 1) {						     \
+		ADM8211_CSR_WRITE(SYNRF, reg | ADM8211_SYNRF_IF_SELECT_0);   \
+		ADM8211_CSR_READ(SYNRF);				     \
+	}								     \
+	if (postwrite == 2) {						     \
+		ADM8211_CSR_WRITE(SYNRF, reg | ADM8211_SYNRF_IF_SELECT_1);   \
+		ADM8211_CSR_READ(SYNRF);				     \
+	}								     \
+									     \
+	ADM8211_CSR_WRITE(SYNRF, 0);					     \
+	ADM8211_CSR_READ(SYNRF);					     \
+}
+
+WRITE_SYN(max2820,  0x00FFF, 0, 0x0F, 12, 15, 1, 1)
+WRITE_SYN(al2210l,  0xFFFFF, 4, 0x0F,  0, 23, 1, 1)
+WRITE_SYN(rfmd2958, 0x3FFFF, 0, 0x1F, 18, 23, 0, 1)
+WRITE_SYN(rfmd2948, 0x0FFFF, 4, 0x0F,  0, 21, 0, 2)
+
+#undef WRITE_SYN
+
+static int adm8211_write_bbp(struct ieee80211_hw *dev, u8 addr, u8 data)
+{
+	struct adm8211_priv *priv = dev->priv;
+	unsigned int timeout;
+	u32 reg;
+
+	timeout = 10;
+	while (timeout > 0) {
+		reg = ADM8211_CSR_READ(BBPCTL);
+		if (!(reg & (ADM8211_BBPCTL_WR | ADM8211_BBPCTL_RD)))
+			break;
+		timeout--;
+		msleep(2);
+	}
+
+	if (timeout == 0) {
+		printk(KERN_DEBUG "%s: adm8211_write_bbp(%d,%d) failed"
+		       " prewrite (reg=0x%08x)\n",
+		       wiphy_name(dev->wiphy), addr, data, reg);
+		return -ETIMEDOUT;
+	}
+
+	switch (priv->bbp_type) {
+	case ADM8211_TYPE_INTERSIL:
+		reg = ADM8211_BBPCTL_MMISEL;	/* three wire interface */
+		break;
+	case ADM8211_TYPE_RFMD:
+		reg = (0x20 << 24) | ADM8211_BBPCTL_TXCE | ADM8211_BBPCTL_CCAP |
+		      (0x01 << 18);
+		break;
+	case ADM8211_TYPE_ADMTEK:
+		reg = (0x20 << 24) | ADM8211_BBPCTL_TXCE | ADM8211_BBPCTL_CCAP |
+		      (0x05 << 18);
+		break;
+	}
+	reg |= ADM8211_BBPCTL_WR | (addr << 8) | data;
+
+	ADM8211_CSR_WRITE(BBPCTL, reg);
+
+	timeout = 10;
+	while (timeout > 0) {
+		reg = ADM8211_CSR_READ(BBPCTL);
+		if (!(reg & ADM8211_BBPCTL_WR))
+			break;
+		timeout--;
+		msleep(2);
+	}
+
+	if (timeout == 0) {
+		ADM8211_CSR_WRITE(BBPCTL, ADM8211_CSR_READ(BBPCTL) &
+				  ~ADM8211_BBPCTL_WR);
+		printk(KERN_DEBUG "%s: adm8211_write_bbp(%d,%d) failed"
+		       " postwrite (reg=0x%08x)\n",
+		       wiphy_name(dev->wiphy), addr, data, reg);
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+static int adm8211_rf_set_channel(struct ieee80211_hw *dev, unsigned int chan)
+{
+	static const u32 adm8211_rfmd2958_reg5[] =
+		{0x22BD, 0x22D2, 0x22E8, 0x22FE, 0x2314, 0x232A, 0x2340,
+		 0x2355, 0x236B, 0x2381, 0x2397, 0x23AD, 0x23C2, 0x23F7};
+	static const u32 adm8211_rfmd2958_reg6[] =
+		{0x05D17, 0x3A2E8, 0x2E8BA, 0x22E8B, 0x1745D, 0x0BA2E, 0x00000,
+		 0x345D1, 0x28BA2, 0x1D174, 0x11745, 0x05D17, 0x3A2E8, 0x11745};
+
+	struct adm8211_priv *priv = dev->priv;
+	u8 ant_power = priv->ant_power > 0x3F ?
+		priv->eeprom->antenna_power[chan - 1] : priv->ant_power;
+	u8 tx_power = priv->tx_power > 0x3F ?
+		priv->eeprom->tx_power[chan - 1] : priv->tx_power;
+	u8 lpf_cutoff = priv->lpf_cutoff == 0xFF ?
+		priv->eeprom->lpf_cutoff[chan - 1] : priv->lpf_cutoff;
+	u8 lnags_thresh = priv->lnags_threshold == 0xFF ?
+		priv->eeprom->lnags_threshold[chan - 1] : priv->lnags_threshold;
+	u32 reg;
+
+	ADM8211_IDLE();
+
+	/* Program synthesizer to new channel */
+	switch (priv->transceiver_type) {
+	case ADM8211_RFMD2958:
+	case ADM8211_RFMD2958_RF3000_CONTROL_POWER:
+		adm8211_rf_write_syn_rfmd2958(dev, 0x00, 0x04007);
+		adm8211_rf_write_syn_rfmd2958(dev, 0x02, 0x00033);
+
+		adm8211_rf_write_syn_rfmd2958(dev, 0x05,
+			adm8211_rfmd2958_reg5[chan - 1]);
+		adm8211_rf_write_syn_rfmd2958(dev, 0x06,
+			adm8211_rfmd2958_reg6[chan - 1]);
+		break;
+
+	case ADM8211_RFMD2948:
+		adm8211_rf_write_syn_rfmd2948(dev, SI4126_MAIN_CONF,
+					      SI4126_MAIN_XINDIV2);
+		adm8211_rf_write_syn_rfmd2948(dev, SI4126_POWERDOWN,
+					      SI4126_POWERDOWN_PDIB |
+					      SI4126_POWERDOWN_PDRB);
+		adm8211_rf_write_syn_rfmd2948(dev, SI4126_PHASE_DET_GAIN, 0);
+		adm8211_rf_write_syn_rfmd2948(dev, SI4126_RF2_N_DIV,
+					      (chan == 14 ?
+					       2110 : (2033 + (chan * 5))));
+		adm8211_rf_write_syn_rfmd2948(dev, SI4126_IF_N_DIV, 1496);
+		adm8211_rf_write_syn_rfmd2948(dev, SI4126_RF2_R_DIV, 44);
+		adm8211_rf_write_syn_rfmd2948(dev, SI4126_IF_R_DIV, 44);
+		break;
+
+	case ADM8211_MAX2820:
+		adm8211_rf_write_syn_max2820(dev, 0x3,
+			(chan == 14 ? 0x054 : (0x7 + (chan * 5))));
+		break;
+
+	case ADM8211_AL2210L:
+		adm8211_rf_write_syn_al2210l(dev, 0x0,
+			(chan == 14 ? 0x229B4 : (0x22967 + (chan * 5))));
+		break;
+
+	default:
+		printk(KERN_DEBUG "%s: unsupported transceiver type %d\n",
+		       wiphy_name(dev->wiphy), priv->transceiver_type);
+		break;
+	}
+
+	/* write BBP regs */
+	if (priv->bbp_type == ADM8211_TYPE_RFMD) {
+
+	/* SMC 2635W specific? adm8211b doesn't use the 2948 though.. */
+	/* TODO: remove if SMC 2635W doesn't need this */
+	if (priv->transceiver_type == ADM8211_RFMD2948) {
+		reg = ADM8211_CSR_READ(GPIO);
+		reg &= 0xfffc0000;
+		reg |= ADM8211_CSR_GPIO_EN0;
+		if (chan != 14)
+			reg |= ADM8211_CSR_GPIO_O0;
+		ADM8211_CSR_WRITE(GPIO, reg);
+	}
+
+	if (priv->transceiver_type == ADM8211_RFMD2958) {
+		/* set PCNT2 */
+		adm8211_rf_write_syn_rfmd2958(dev, 0x0B, 0x07100);
+		/* set PCNT1 P_DESIRED/MID_BIAS */
+		reg = le16_to_cpu(priv->eeprom->cr49);
+		reg >>= 13;
+		reg <<= 15;
+		reg |= ant_power << 9;
+		adm8211_rf_write_syn_rfmd2958(dev, 0x0A, reg);
+		/* set TXRX TX_GAIN */
+		adm8211_rf_write_syn_rfmd2958(dev, 0x09, 0x00050 |
+			(priv->pdev->revision < ADM8211_REV_CA ? tx_power : 0));
+	} else {
+		reg = ADM8211_CSR_READ(PLCPHD);
+		reg &= 0xff00ffff;
+		reg |= tx_power << 18;
+		ADM8211_CSR_WRITE(PLCPHD, reg);
+	}
+
+	ADM8211_CSR_WRITE(SYNRF, ADM8211_SYNRF_SELRF |
+			  ADM8211_SYNRF_PE1 | ADM8211_SYNRF_PHYRST);
+	ADM8211_CSR_READ(SYNRF);
+	msleep(30);
+
+	/* RF3000 BBP */
+	if (priv->transceiver_type != ADM8211_RFMD2958)
+		adm8211_write_bbp(dev, RF3000_TX_VAR_GAIN__TX_LEN_EXT,
+				  tx_power<<2);
+	adm8211_write_bbp(dev, RF3000_LOW_GAIN_CALIB, lpf_cutoff);
+	adm8211_write_bbp(dev, RF3000_HIGH_GAIN_CALIB, lnags_thresh);
+	adm8211_write_bbp(dev, 0x1c, priv->pdev->revision == ADM8211_REV_BA ?
+				     priv->eeprom->cr28 : 0);
+	adm8211_write_bbp(dev, 0x1d, priv->eeprom->cr29);
+
+	ADM8211_CSR_WRITE(SYNRF, 0);
+
+	/* Nothing to do for ADMtek BBP */
+	} else if (priv->bbp_type != ADM8211_TYPE_ADMTEK)
+		printk(KERN_DEBUG "%s: unsupported BBP type %d\n",
+		       wiphy_name(dev->wiphy), priv->bbp_type);
+
+	ADM8211_RESTORE();
+
+	/* update current channel for adhoc (and maybe AP mode) */
+	reg = ADM8211_CSR_READ(CAP0);
+	reg &= ~0xF;
+	reg |= chan;
+	ADM8211_CSR_WRITE(CAP0, reg);
+
+	return 0;
+}
+
+static void adm8211_update_mode(struct ieee80211_hw *dev)
+{
+	struct adm8211_priv *priv = dev->priv;
+
+	ADM8211_IDLE();
+
+	priv->soft_rx_crc = 0;
+	switch (priv->mode) {
+	case IEEE80211_IF_TYPE_STA:
+		priv->nar &= ~(ADM8211_NAR_PR | ADM8211_NAR_EA);
+		priv->nar |= ADM8211_NAR_ST | ADM8211_NAR_SR;
+		break;
+	case IEEE80211_IF_TYPE_IBSS:
+		priv->nar &= ~ADM8211_NAR_PR;
+		priv->nar |= ADM8211_NAR_EA | ADM8211_NAR_ST | ADM8211_NAR_SR;
+
+		/* don't trust the error bits on rev 0x20 and up in adhoc */
+		if (priv->pdev->revision >= ADM8211_REV_BA)
+			priv->soft_rx_crc = 1;
+		break;
+	case IEEE80211_IF_TYPE_MNTR:
+		priv->nar &= ~(ADM8211_NAR_EA | ADM8211_NAR_ST);
+		priv->nar |= ADM8211_NAR_PR | ADM8211_NAR_SR;
+		break;
+	}
+
+	ADM8211_RESTORE();
+}
+
+static void adm8211_hw_init_syn(struct ieee80211_hw *dev)
+{
+	struct adm8211_priv *priv = dev->priv;
+
+	switch (priv->transceiver_type) {
+	case ADM8211_RFMD2958:
+	case ADM8211_RFMD2958_RF3000_CONTROL_POWER:
+		/* comments taken from ADMtek vendor driver */
+
+		/* Reset RF2958 after power on */
+		adm8211_rf_write_syn_rfmd2958(dev, 0x1F, 0x00000);
+		/* Initialize RF VCO Core Bias to maximum */
+		adm8211_rf_write_syn_rfmd2958(dev, 0x0C, 0x3001F);
+		/* Initialize IF PLL */
+		adm8211_rf_write_syn_rfmd2958(dev, 0x01, 0x29C03);
+		/* Initialize IF PLL Coarse Tuning */
+		adm8211_rf_write_syn_rfmd2958(dev, 0x03, 0x1FF6F);
+		/* Initialize RF PLL */
+		adm8211_rf_write_syn_rfmd2958(dev, 0x04, 0x29403);
+		/* Initialize RF PLL Coarse Tuning */
+		adm8211_rf_write_syn_rfmd2958(dev, 0x07, 0x1456F);
+		/* Initialize TX gain and filter BW (R9) */
+		adm8211_rf_write_syn_rfmd2958(dev, 0x09,
+			(priv->transceiver_type == ADM8211_RFMD2958 ?
+			 0x10050 : 0x00050));
+		/* Initialize CAL register */
+		adm8211_rf_write_syn_rfmd2958(dev, 0x08, 0x3FFF8);
+		break;
+
+	case ADM8211_MAX2820:
+		adm8211_rf_write_syn_max2820(dev, 0x1, 0x01E);
+		adm8211_rf_write_syn_max2820(dev, 0x2, 0x001);
+		adm8211_rf_write_syn_max2820(dev, 0x3, 0x054);
+		adm8211_rf_write_syn_max2820(dev, 0x4, 0x310);
+		adm8211_rf_write_syn_max2820(dev, 0x5, 0x000);
+		break;
+
+	case ADM8211_AL2210L:
+		adm8211_rf_write_syn_al2210l(dev, 0x0, 0x0196C);
+		adm8211_rf_write_syn_al2210l(dev, 0x1, 0x007CB);
+		adm8211_rf_write_syn_al2210l(dev, 0x2, 0x3582F);
+		adm8211_rf_write_syn_al2210l(dev, 0x3, 0x010A9);
+		adm8211_rf_write_syn_al2210l(dev, 0x4, 0x77280);
+		adm8211_rf_write_syn_al2210l(dev, 0x5, 0x45641);
+		adm8211_rf_write_syn_al2210l(dev, 0x6, 0xEA130);
+		adm8211_rf_write_syn_al2210l(dev, 0x7, 0x80000);
+		adm8211_rf_write_syn_al2210l(dev, 0x8, 0x7850F);
+		adm8211_rf_write_syn_al2210l(dev, 0x9, 0xF900C);
+		adm8211_rf_write_syn_al2210l(dev, 0xA, 0x00000);
+		adm8211_rf_write_syn_al2210l(dev, 0xB, 0x00000);
+		break;
+
+	case ADM8211_RFMD2948:
+	default:
+		break;
+	}
+}
+
+static int adm8211_hw_init_bbp(struct ieee80211_hw *dev)
+{
+	struct adm8211_priv *priv = dev->priv;
+	u32 reg;
+
+	/* write addresses */
+	if (priv->bbp_type == ADM8211_TYPE_INTERSIL) {
+		ADM8211_CSR_WRITE(MMIWA,  0x100E0C0A);
+		ADM8211_CSR_WRITE(MMIRD0, 0x00007C7E);
+		ADM8211_CSR_WRITE(MMIRD1, 0x00100000);
+	} else if (priv->bbp_type == ADM8211_TYPE_RFMD ||
+		   priv->bbp_type == ADM8211_TYPE_ADMTEK) {
+		/* check specific BBP type */
+		switch (priv->specific_bbptype) {
+		case ADM8211_BBP_RFMD3000:
+		case ADM8211_BBP_RFMD3002:
+			ADM8211_CSR_WRITE(MMIWA,  0x00009101);
+			ADM8211_CSR_WRITE(MMIRD0, 0x00000301);
+			break;
+
+		case ADM8211_BBP_ADM8011:
+			ADM8211_CSR_WRITE(MMIWA,  0x00008903);
+			ADM8211_CSR_WRITE(MMIRD0, 0x00001716);
+
+			reg = ADM8211_CSR_READ(BBPCTL);
+			reg &= ~ADM8211_BBPCTL_TYPE;
+			reg |= 0x5 << 18;
+			ADM8211_CSR_WRITE(BBPCTL, reg);
+			break;
+		}
+
+		switch (priv->pdev->revision) {
+		case ADM8211_REV_CA:
+			if (priv->transceiver_type == ADM8211_RFMD2958 ||
+			    priv->transceiver_type == ADM8211_RFMD2958_RF3000_CONTROL_POWER ||
+			    priv->transceiver_type == ADM8211_RFMD2948)
+				ADM8211_CSR_WRITE(SYNCTL, 0x1 << 22);
+			else if (priv->transceiver_type == ADM8211_MAX2820 ||
+				 priv->transceiver_type == ADM8211_AL2210L)
+				ADM8211_CSR_WRITE(SYNCTL, 0x3 << 22);
+			break;
+
+		case ADM8211_REV_BA:
+			reg  = ADM8211_CSR_READ(MMIRD1);
+			reg &= 0x0000FFFF;
+			reg |= 0x7e100000;
+			ADM8211_CSR_WRITE(MMIRD1, reg);
+			break;
+
+		case ADM8211_REV_AB:
+		case ADM8211_REV_AF:
+		default:
+			ADM8211_CSR_WRITE(MMIRD1, 0x7e100000);
+			break;
+		}
+
+		/* For RFMD */
+		ADM8211_CSR_WRITE(MACTEST, 0x800);
+	}
+
+	adm8211_hw_init_syn(dev);
+
+	/* Set RF Power control IF pin to PE1+PHYRST# */
+	ADM8211_CSR_WRITE(SYNRF, ADM8211_SYNRF_SELRF |
+			  ADM8211_SYNRF_PE1 | ADM8211_SYNRF_PHYRST);
+	ADM8211_CSR_READ(SYNRF);
+	msleep(20);
+
+	/* write BBP regs */
+	if (priv->bbp_type == ADM8211_TYPE_RFMD) {
+		/* RF3000 BBP */
+		/* another set:
+		 * 11: c8
+		 * 14: 14
+		 * 15: 50 (chan 1..13; chan 14: d0)
+		 * 1c: 00
+		 * 1d: 84
+		 */
+		adm8211_write_bbp(dev, RF3000_CCA_CTRL, 0x80);
+		/* antenna selection: diversity */
+		adm8211_write_bbp(dev, RF3000_DIVERSITY__RSSI, 0x80);
+		adm8211_write_bbp(dev, RF3000_TX_VAR_GAIN__TX_LEN_EXT, 0x74);
+		adm8211_write_bbp(dev, RF3000_LOW_GAIN_CALIB, 0x38);
+		adm8211_write_bbp(dev, RF3000_HIGH_GAIN_CALIB, 0x40);
+
+		if (priv->eeprom->major_version < 2) {
+			adm8211_write_bbp(dev, 0x1c, 0x00);
+			adm8211_write_bbp(dev, 0x1d, 0x80);
+		} else {
+			if (priv->pdev->revision == ADM8211_REV_BA)
+				adm8211_write_bbp(dev, 0x1c, priv->eeprom->cr28);
+			else
+				adm8211_write_bbp(dev, 0x1c, 0x00);
+
+			adm8211_write_bbp(dev, 0x1d, priv->eeprom->cr29);
+		}
+	} else if (priv->bbp_type == ADM8211_TYPE_ADMTEK) {
+		/* reset baseband */
+		adm8211_write_bbp(dev, 0x00, 0xFF);
+		/* antenna selection: diversity */
+		adm8211_write_bbp(dev, 0x07, 0x0A);
+
+		/* TODO: find documentation for this */
+		switch (priv->transceiver_type) {
+		case ADM8211_RFMD2958:
+		case ADM8211_RFMD2958_RF3000_CONTROL_POWER:
+			adm8211_write_bbp(dev, 0x00, 0x00);
+			adm8211_write_bbp(dev, 0x01, 0x00);
+			adm8211_write_bbp(dev, 0x02, 0x00);
+			adm8211_write_bbp(dev, 0x03, 0x00);
+			adm8211_write_bbp(dev, 0x06, 0x0f);
+			adm8211_write_bbp(dev, 0x09, 0x00);
+			adm8211_write_bbp(dev, 0x0a, 0x00);
+			adm8211_write_bbp(dev, 0x0b, 0x00);
+			adm8211_write_bbp(dev, 0x0c, 0x00);
+			adm8211_write_bbp(dev, 0x0f, 0xAA);
+			adm8211_write_bbp(dev, 0x10, 0x8c);
+			adm8211_write_bbp(dev, 0x11, 0x43);
+			adm8211_write_bbp(dev, 0x18, 0x40);
+			adm8211_write_bbp(dev, 0x20, 0x23);
+			adm8211_write_bbp(dev, 0x21, 0x02);
+			adm8211_write_bbp(dev, 0x22, 0x28);
+			adm8211_write_bbp(dev, 0x23, 0x30);
+			adm8211_write_bbp(dev, 0x24, 0x2d);
+			adm8211_write_bbp(dev, 0x28, 0x35);
+			adm8211_write_bbp(dev, 0x2a, 0x8c);
+			adm8211_write_bbp(dev, 0x2b, 0x81);
+			adm8211_write_bbp(dev, 0x2c, 0x44);
+			adm8211_write_bbp(dev, 0x2d, 0x0A);
+			adm8211_write_bbp(dev, 0x29, 0x40);
+			adm8211_write_bbp(dev, 0x60, 0x08);
+			adm8211_write_bbp(dev, 0x64, 0x01);
+			break;
+
+		case ADM8211_MAX2820:
+			adm8211_write_bbp(dev, 0x00, 0x00);
+			adm8211_write_bbp(dev, 0x01, 0x00);
+			adm8211_write_bbp(dev, 0x02, 0x00);
+			adm8211_write_bbp(dev, 0x03, 0x00);
+			adm8211_write_bbp(dev, 0x06, 0x0f);
+			adm8211_write_bbp(dev, 0x09, 0x05);
+			adm8211_write_bbp(dev, 0x0a, 0x02);
+			adm8211_write_bbp(dev, 0x0b, 0x00);
+			adm8211_write_bbp(dev, 0x0c, 0x0f);
+			adm8211_write_bbp(dev, 0x0f, 0x55);
+			adm8211_write_bbp(dev, 0x10, 0x8d);
+			adm8211_write_bbp(dev, 0x11, 0x43);
+			adm8211_write_bbp(dev, 0x18, 0x4a);
+			adm8211_write_bbp(dev, 0x20, 0x20);
+			adm8211_write_bbp(dev, 0x21, 0x02);
+			adm8211_write_bbp(dev, 0x22, 0x23);
+			adm8211_write_bbp(dev, 0x23, 0x30);
+			adm8211_write_bbp(dev, 0x24, 0x2d);
+			adm8211_write_bbp(dev, 0x2a, 0x8c);
+			adm8211_write_bbp(dev, 0x2b, 0x81);
+			adm8211_write_bbp(dev, 0x2c, 0x44);
+			adm8211_write_bbp(dev, 0x29, 0x4a);
+			adm8211_write_bbp(dev, 0x60, 0x2b);
+			adm8211_write_bbp(dev, 0x64, 0x01);
+			break;
+
+		case ADM8211_AL2210L:
+			adm8211_write_bbp(dev, 0x00, 0x00);
+			adm8211_write_bbp(dev, 0x01, 0x00);
+			adm8211_write_bbp(dev, 0x02, 0x00);
+			adm8211_write_bbp(dev, 0x03, 0x00);
+			adm8211_write_bbp(dev, 0x06, 0x0f);
+			adm8211_write_bbp(dev, 0x07, 0x05);
+			adm8211_write_bbp(dev, 0x08, 0x03);
+			adm8211_write_bbp(dev, 0x09, 0x00);
+			adm8211_write_bbp(dev, 0x0a, 0x00);
+			adm8211_write_bbp(dev, 0x0b, 0x00);
+			adm8211_write_bbp(dev, 0x0c, 0x10);
+			adm8211_write_bbp(dev, 0x0f, 0x55);
+			adm8211_write_bbp(dev, 0x10, 0x8d);
+			adm8211_write_bbp(dev, 0x11, 0x43);
+			adm8211_write_bbp(dev, 0x18, 0x4a);
+			adm8211_write_bbp(dev, 0x20, 0x20);
+			adm8211_write_bbp(dev, 0x21, 0x02);
+			adm8211_write_bbp(dev, 0x22, 0x23);
+			adm8211_write_bbp(dev, 0x23, 0x30);
+			adm8211_write_bbp(dev, 0x24, 0x2d);
+			adm8211_write_bbp(dev, 0x2a, 0xaa);
+			adm8211_write_bbp(dev, 0x2b, 0x81);
+			adm8211_write_bbp(dev, 0x2c, 0x44);
+			adm8211_write_bbp(dev, 0x29, 0xfa);
+			adm8211_write_bbp(dev, 0x60, 0x2d);
+			adm8211_write_bbp(dev, 0x64, 0x01);
+			break;
+
+		case ADM8211_RFMD2948:
+			break;
+
+		default:
+			printk(KERN_DEBUG "%s: unsupported transceiver %d\n",
+			       wiphy_name(dev->wiphy), priv->transceiver_type);
+			break;
+		}
+	} else
+		printk(KERN_DEBUG "%s: unsupported BBP %d\n",
+		       wiphy_name(dev->wiphy), priv->bbp_type);
+
+	ADM8211_CSR_WRITE(SYNRF, 0);
+
+	/* Set RF CAL control source to MAC control */
+	reg = ADM8211_CSR_READ(SYNCTL);
+	reg |= ADM8211_SYNCTL_SELCAL;
+	ADM8211_CSR_WRITE(SYNCTL, reg);
+
+	return 0;
+}
+
+/* configures hw beacons/probe responses */
+static int adm8211_set_rate(struct ieee80211_hw *dev)
+{
+	struct adm8211_priv *priv = dev->priv;
+	u32 reg;
+	int i = 0;
+	u8 rate_buf[12] = {0};
+
+	/* write supported rates */
+	if (priv->pdev->revision != ADM8211_REV_BA) {
+		rate_buf[0] = ARRAY_SIZE(adm8211_rates);
+		for (i = 0; i < ARRAY_SIZE(adm8211_rates); i++)
+			rate_buf[i + 1] = (adm8211_rates[i].rate / 5) | 0x80;
+	} else {
+		/* workaround for rev BA specific bug */
+		rate_buf[0] = 0x04;
+		rate_buf[1] = 0x82;
+		rate_buf[2] = 0x04;
+		rate_buf[3] = 0x0b;
+		rate_buf[4] = 0x16;
+	}
+
+	adm8211_write_sram_bytes(dev, ADM8211_SRAM_SUPP_RATE, rate_buf,
+				 ARRAY_SIZE(adm8211_rates) + 1);
+
+	reg = ADM8211_CSR_READ(PLCPHD) & 0x00FFFFFF; /* keep bits 0-23 */
+	reg |= 1 << 15;	/* short preamble */
+	reg |= 110 << 24;
+	ADM8211_CSR_WRITE(PLCPHD, reg);
+
+	/* MTMLT   = 512 TU (max TX MSDU lifetime)
+	 * BCNTSIG = plcp_signal (beacon, probe resp, and atim TX rate)
+	 * SRTYLIM = 224 (short retry limit, TX header value is default) */
+	ADM8211_CSR_WRITE(TXLMT, (512 << 16) | (110 << 8) | (224 << 0));
+
+	return 0;
+}
+
+static void adm8211_hw_init(struct ieee80211_hw *dev)
+{
+	struct adm8211_priv *priv = dev->priv;
+	u32 reg;
+	u8 cline;
+
+	reg = le32_to_cpu(ADM8211_CSR_READ(PAR));
+	reg |= ADM8211_PAR_MRLE | ADM8211_PAR_MRME;
+	reg &= ~(ADM8211_PAR_BAR | ADM8211_PAR_CAL);
+
+	if (!pci_set_mwi(priv->pdev)) {
+		reg |= 0x1 << 24;
+		pci_read_config_byte(priv->pdev, PCI_CACHE_LINE_SIZE, &cline);
+
+		switch (cline) {
+		case  0x8: reg |= (0x1 << 14);
+			   break;
+		case 0x16: reg |= (0x2 << 14);
+			   break;
+		case 0x32: reg |= (0x3 << 14);
+			   break;
+		  default: reg |= (0x0 << 14);
+			   break;
+		}
+	}
+
+	ADM8211_CSR_WRITE(PAR, reg);
+
+	reg = ADM8211_CSR_READ(CSR_TEST1);
+	reg &= ~(0xF << 28);
+	reg |= (1 << 28) | (1 << 31);
+	ADM8211_CSR_WRITE(CSR_TEST1, reg);
+
+	/* lose link after 4 lost beacons */
+	reg = (0x04 << 21) | ADM8211_WCSR_TSFTWE | ADM8211_WCSR_LSOE;
+	ADM8211_CSR_WRITE(WCSR, reg);
+
+	/* Disable APM, enable receive FIFO threshold, and set drain receive
+	 * threshold to store-and-forward */
+	reg = ADM8211_CSR_READ(CMDR);
+	reg &= ~(ADM8211_CMDR_APM | ADM8211_CMDR_DRT);
+	reg |= ADM8211_CMDR_RTE | ADM8211_CMDR_DRT_SF;
+	ADM8211_CSR_WRITE(CMDR, reg);
+
+	adm8211_set_rate(dev);
+
+	/* 4-bit values:
+	 * PWR1UP   = 8 * 2 ms
+	 * PWR0PAPE = 8 us or 5 us
+	 * PWR1PAPE = 1 us or 3 us
+	 * PWR0TRSW = 5 us
+	 * PWR1TRSW = 12 us
+	 * PWR0PE2  = 13 us
+	 * PWR1PE2  = 1 us
+	 * PWR0TXPE = 8 or 6 */
+	if (priv->pdev->revision < ADM8211_REV_CA)
+		ADM8211_CSR_WRITE(TOFS2, 0x8815cd18);
+	else
+		ADM8211_CSR_WRITE(TOFS2, 0x8535cd16);
+
+	/* Enable store and forward for transmit */
+	priv->nar = ADM8211_NAR_SF | ADM8211_NAR_PB;
+	ADM8211_CSR_WRITE(NAR, priv->nar);
+
+	/* Reset RF */
+	ADM8211_CSR_WRITE(SYNRF, ADM8211_SYNRF_RADIO);
+	ADM8211_CSR_READ(SYNRF);
+	msleep(10);
+	ADM8211_CSR_WRITE(SYNRF, 0);
+	ADM8211_CSR_READ(SYNRF);
+	msleep(5);
+
+	/* Set CFP Max Duration to 0x10 TU */
+	reg = ADM8211_CSR_READ(CFPP);
+	reg &= ~(0xffff << 8);
+	reg |= 0x0010 << 8;
+	ADM8211_CSR_WRITE(CFPP, reg);
+
+	/* USCNT = 0x16 (number of system clocks, 22 MHz, in 1us
+	 * TUCNT = 0x3ff - Tu counter 1024 us  */
+	ADM8211_CSR_WRITE(TOFS0, (0x16 << 24) | 0x3ff);
+
+	/* SLOT=20 us, SIFS=110 cycles of 22 MHz (5 us),
+	 * DIFS=50 us, EIFS=100 us */
+	if (priv->pdev->revision < ADM8211_REV_CA)
+		ADM8211_CSR_WRITE(IFST, (20 << 23) | (110 << 15) |
+					(50 << 9)  | 100);
+	else
+		ADM8211_CSR_WRITE(IFST, (20 << 23) | (24 << 15) |
+					(50 << 9)  | 100);
+
+	/* PCNT = 1 (MAC idle time awake/sleep, unit S)
+	 * RMRD = 2346 * 8 + 1 us (max RX duration)  */
+	ADM8211_CSR_WRITE(RMD, (1 << 16) | 18769);
+
+	/* MART=65535 us, MIRT=256 us, TSFTOFST=0 us */
+	ADM8211_CSR_WRITE(RSPT, 0xffffff00);
+
+	/* Initialize BBP (and SYN) */
+	adm8211_hw_init_bbp(dev);
+
+	/* make sure interrupts are off */
+	ADM8211_CSR_WRITE(IER, 0);
+
+	/* ACK interrupts */
+	ADM8211_CSR_WRITE(STSR, ADM8211_CSR_READ(STSR));
+
+	/* Setup WEP (turns it off for now) */
+	reg = ADM8211_CSR_READ(MACTEST);
+	reg &= ~(7 << 20);
+	ADM8211_CSR_WRITE(MACTEST, reg);
+
+	reg = ADM8211_CSR_READ(WEPCTL);
+	reg &= ~ADM8211_WEPCTL_WEPENABLE;
+	reg |= ADM8211_WEPCTL_WEPRXBYP;
+	ADM8211_CSR_WRITE(WEPCTL, reg);
+
+	/* Clear the missed-packet counter. */
+	ADM8211_CSR_READ(LPC);
+}
+
+static int adm8211_hw_reset(struct ieee80211_hw *dev)
+{
+	struct adm8211_priv *priv = dev->priv;
+	u32 reg, tmp;
+	int timeout = 100;
+
+	/* Power-on issue */
+	/* TODO: check if this is necessary */
+	ADM8211_CSR_WRITE(FRCTL, 0);
+
+	/* Reset the chip */
+	tmp = ADM8211_CSR_READ(PAR);
+	ADM8211_CSR_WRITE(PAR, ADM8211_PAR_SWR);
+
+	while ((ADM8211_CSR_READ(PAR) & ADM8211_PAR_SWR) && timeout--)
+		msleep(50);
+
+	if (timeout <= 0)
+		return -ETIMEDOUT;
+
+	ADM8211_CSR_WRITE(PAR, tmp);
+
+	if (priv->pdev->revision == ADM8211_REV_BA &&
+	    (priv->transceiver_type == ADM8211_RFMD2958_RF3000_CONTROL_POWER ||
+	     priv->transceiver_type == ADM8211_RFMD2958)) {
+		reg = ADM8211_CSR_READ(CSR_TEST1);
+		reg |= (1 << 4) | (1 << 5);
+		ADM8211_CSR_WRITE(CSR_TEST1, reg);
+	} else if (priv->pdev->revision == ADM8211_REV_CA) {
+		reg = ADM8211_CSR_READ(CSR_TEST1);
+		reg &= ~((1 << 4) | (1 << 5));
+		ADM8211_CSR_WRITE(CSR_TEST1, reg);
+	}
+
+	ADM8211_CSR_WRITE(FRCTL, 0);
+
+	reg = ADM8211_CSR_READ(CSR_TEST0);
+	reg |= ADM8211_CSR_TEST0_EPRLD;	/* EEPROM Recall */
+	ADM8211_CSR_WRITE(CSR_TEST0, reg);
+
+	adm8211_clear_sram(dev);
+
+	return 0;
+}
+
+static u64 adm8211_get_tsft(struct ieee80211_hw *dev)
+{
+	struct adm8211_priv *priv = dev->priv;
+	u32 tsftl;
+	u64 tsft;
+
+	tsftl = ADM8211_CSR_READ(TSFTL);
+	tsft = ADM8211_CSR_READ(TSFTH);
+	tsft <<= 32;
+	tsft |= tsftl;
+
+	return tsft;
+}
+
+static void adm8211_set_interval(struct ieee80211_hw *dev,
+				 unsigned short bi, unsigned short li)
+{
+	struct adm8211_priv *priv = dev->priv;
+	u32 reg;
+
+	/* BP (beacon interval) = data->beacon_interval
+	 * LI (listen interval) = data->listen_interval (in beacon intervals) */
+	reg = (bi << 16) | li;
+	ADM8211_CSR_WRITE(BPLI, reg);
+}
+
+static void adm8211_set_bssid(struct ieee80211_hw *dev, const u8 *bssid)
+{
+	struct adm8211_priv *priv = dev->priv;
+	u32 reg;
+
+	ADM8211_CSR_WRITE(BSSID0, le32_to_cpu(*(__le32 *)bssid));
+	reg = ADM8211_CSR_READ(ABDA1);
+	reg &= 0x0000ffff;
+	reg |= (bssid[4] << 16) | (bssid[5] << 24);
+	ADM8211_CSR_WRITE(ABDA1, reg);
+}
+
+static int adm8211_set_ssid(struct ieee80211_hw *dev, u8 *ssid, size_t ssid_len)
+{
+	struct adm8211_priv *priv = dev->priv;
+	u8 buf[36];
+
+	if (ssid_len > 32)
+		return -EINVAL;
+
+	memset(buf, 0, sizeof(buf));
+	buf[0] = ssid_len;
+	memcpy(buf + 1, ssid, ssid_len);
+	adm8211_write_sram_bytes(dev, ADM8211_SRAM_SSID, buf, 33);
+	/* TODO: configure beacon for adhoc? */
+	return 0;
+}
+
+static int adm8211_config(struct ieee80211_hw *dev, struct ieee80211_conf *conf)
+{
+	struct adm8211_priv *priv = dev->priv;
+
+	if (conf->channel != priv->channel) {
+		priv->channel = conf->channel;
+		adm8211_rf_set_channel(dev, priv->channel);
+	}
+
+	return 0;
+}
+
+static int adm8211_config_interface(struct ieee80211_hw *dev, int if_id,
+				    struct ieee80211_if_conf *conf)
+{
+	struct adm8211_priv *priv = dev->priv;
+
+	if (memcmp(conf->bssid, priv->bssid, ETH_ALEN)) {
+		adm8211_set_bssid(dev, conf->bssid);
+		memcpy(priv->bssid, conf->bssid, ETH_ALEN);
+	}
+
+	if (conf->ssid_len != priv->ssid_len ||
+	    memcmp(conf->ssid, priv->ssid, conf->ssid_len)) {
+		adm8211_set_ssid(dev, conf->ssid, conf->ssid_len);
+		priv->ssid_len = conf->ssid_len;
+		memcpy(priv->ssid, conf->ssid, conf->ssid_len);
+	}
+
+	return 0;
+}
+
+static void adm8211_configure_filter(struct ieee80211_hw *dev,
+				     unsigned int changed_flags,
+				     unsigned int *total_flags,
+				     int mc_count, struct dev_mc_list *mclist)
+{
+	static const u8 bcast[ETH_ALEN] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };
+	struct adm8211_priv *priv = dev->priv;
+	unsigned int bit_nr, new_flags;
+	u32 mc_filter[2];
+	int i;
+
+	new_flags = 0;
+
+	if (*total_flags & FIF_PROMISC_IN_BSS) {
+		new_flags |= FIF_PROMISC_IN_BSS;
+		priv->nar |= ADM8211_NAR_PR;
+		priv->nar &= ~ADM8211_NAR_MM;
+		mc_filter[1] = mc_filter[0] = ~0;
+	} else if ((*total_flags & FIF_ALLMULTI) || (mc_count > 32)) {
+		new_flags |= FIF_ALLMULTI;
+		priv->nar &= ~ADM8211_NAR_PR;
+		priv->nar |= ADM8211_NAR_MM;
+		mc_filter[1] = mc_filter[0] = ~0;
+	} else {
+		priv->nar &= ~(ADM8211_NAR_MM | ADM8211_NAR_PR);
+		mc_filter[1] = mc_filter[0] = 0;
+		for (i = 0; i < mc_count; i++) {
+			if (!mclist)
+				break;
+			bit_nr = ether_crc(ETH_ALEN, mclist->dmi_addr) >> 26;
+
+			bit_nr &= 0x3F;
+			mc_filter[bit_nr >> 5] |= 1 << (bit_nr & 31);
+			mclist = mclist->next;
+		}
+	}
+
+	ADM8211_IDLE_RX();
+
+	ADM8211_CSR_WRITE(MAR0, mc_filter[0]);
+	ADM8211_CSR_WRITE(MAR1, mc_filter[1]);
+	ADM8211_CSR_READ(NAR);
+
+	if (priv->nar & ADM8211_NAR_PR)
+		dev->flags |= IEEE80211_HW_RX_INCLUDES_FCS;
+	else
+		dev->flags &= ~IEEE80211_HW_RX_INCLUDES_FCS;
+
+	if (*total_flags & FIF_BCN_PRBRESP_PROMISC)
+		adm8211_set_bssid(dev, bcast);
+	else
+		adm8211_set_bssid(dev, priv->bssid);
+
+	ADM8211_RESTORE();
+
+	*total_flags = new_flags;
+}
+
+static int adm8211_add_interface(struct ieee80211_hw *dev,
+				 struct ieee80211_if_init_conf *conf)
+{
+	struct adm8211_priv *priv = dev->priv;
+	if (priv->mode != IEEE80211_IF_TYPE_MNTR)
+		return -EOPNOTSUPP;
+
+	switch (conf->type) {
+	case IEEE80211_IF_TYPE_STA:
+		priv->mode = conf->type;
+		break;
+	default:
+		return -EOPNOTSUPP;
+	}
+
+	ADM8211_IDLE();
+
+	ADM8211_CSR_WRITE(PAR0, le32_to_cpu(*(__le32 *)conf->mac_addr));
+	ADM8211_CSR_WRITE(PAR1, le16_to_cpu(*(__le16 *)(conf->mac_addr + 4)));
+
+	adm8211_update_mode(dev);
+
+	ADM8211_RESTORE();
+
+	return 0;
+}
+
+static void adm8211_remove_interface(struct ieee80211_hw *dev,
+				     struct ieee80211_if_init_conf *conf)
+{
+	struct adm8211_priv *priv = dev->priv;
+	priv->mode = IEEE80211_IF_TYPE_MNTR;
+}
+
+static int adm8211_init_rings(struct ieee80211_hw *dev)
+{
+	struct adm8211_priv *priv = dev->priv;
+	struct adm8211_desc *desc = NULL;
+	struct adm8211_rx_ring_info *rx_info;
+	struct adm8211_tx_ring_info *tx_info;
+	unsigned int i;
+
+	for (i = 0; i < priv->rx_ring_size; i++) {
+		desc = &priv->rx_ring[i];
+		desc->status = 0;
+		desc->length = cpu_to_le32(RX_PKT_SIZE);
+		priv->rx_buffers[i].skb = NULL;
+	}
+	/* Mark the end of RX ring; hw returns to base address after this
+	 * descriptor */
+	desc->length |= cpu_to_le32(RDES1_CONTROL_RER);
+
+	for (i = 0; i < priv->rx_ring_size; i++) {
+		desc = &priv->rx_ring[i];
+		rx_info = &priv->rx_buffers[i];
+
+		rx_info->skb = dev_alloc_skb(RX_PKT_SIZE);
+		if (rx_info->skb == NULL)
+			break;
+		rx_info->mapping = pci_map_single(priv->pdev,
+						  skb_tail_pointer(rx_info->skb),
+						  RX_PKT_SIZE,
+						  PCI_DMA_FROMDEVICE);
+		desc->buffer1 = cpu_to_le32(rx_info->mapping);
+		desc->status = cpu_to_le32(RDES0_STATUS_OWN | RDES0_STATUS_SQL);
+	}
+
+	/* Setup TX ring. TX buffers descriptors will be filled in as needed */
+	for (i = 0; i < priv->tx_ring_size; i++) {
+		desc = &priv->tx_ring[i];
+		tx_info = &priv->tx_buffers[i];
+
+		tx_info->skb = NULL;
+		tx_info->mapping = 0;
+		desc->status = 0;
+	}
+	desc->length = cpu_to_le32(TDES1_CONTROL_TER);
+
+	priv->cur_rx = priv->cur_tx = priv->dirty_tx = 0;
+	ADM8211_CSR_WRITE(RDB, priv->rx_ring_dma);
+	ADM8211_CSR_WRITE(TDBD, priv->tx_ring_dma);
+
+	return 0;
+}
+
+static void adm8211_free_rings(struct ieee80211_hw *dev)
+{
+	struct adm8211_priv *priv = dev->priv;
+	unsigned int i;
+
+	for (i = 0; i < priv->rx_ring_size; i++) {
+		if (!priv->rx_buffers[i].skb)
+			continue;
+
+		pci_unmap_single(
+			priv->pdev,
+			priv->rx_buffers[i].mapping,
+			RX_PKT_SIZE, PCI_DMA_FROMDEVICE);
+
+		dev_kfree_skb(priv->rx_buffers[i].skb);
+	}
+
+	for (i = 0; i < priv->tx_ring_size; i++) {
+		if (!priv->tx_buffers[i].skb)
+			continue;
+
+		pci_unmap_single(priv->pdev,
+				 priv->tx_buffers[i].mapping,
+				 priv->tx_buffers[i].skb->len,
+				 PCI_DMA_TODEVICE);
+
+		dev_kfree_skb(priv->tx_buffers[i].skb);
+	}
+}
+
+static int adm8211_start(struct ieee80211_hw *dev)
+{
+	struct adm8211_priv *priv = dev->priv;
+	int retval;
+
+	/* Power up MAC and RF chips */
+	retval = adm8211_hw_reset(dev);
+	if (retval) {
+		printk(KERN_ERR "%s: hardware reset failed\n",
+		       wiphy_name(dev->wiphy));
+		goto fail;
+	}
+
+	retval = adm8211_init_rings(dev);
+	if (retval) {
+		printk(KERN_ERR "%s: failed to initialize rings\n",
+		       wiphy_name(dev->wiphy));
+		goto fail;
+	}
+
+	/* Init hardware */
+	adm8211_hw_init(dev);
+	adm8211_rf_set_channel(dev, priv->channel);
+
+	retval = request_irq(priv->pdev->irq, &adm8211_interrupt,
+			     IRQF_SHARED, "adm8211", dev);
+	if (retval) {
+		printk(KERN_ERR "%s: failed to register IRQ handler\n",
+		       wiphy_name(dev->wiphy));
+		goto fail;
+	}
+
+	ADM8211_CSR_WRITE(IER, ADM8211_IER_NIE | ADM8211_IER_AIE |
+			       ADM8211_IER_RCIE | ADM8211_IER_TCIE |
+			       ADM8211_IER_TDUIE | ADM8211_IER_GPTIE);
+	priv->mode = IEEE80211_IF_TYPE_MNTR;
+	adm8211_update_mode(dev);
+	ADM8211_CSR_WRITE(RDR, 0);
+
+	adm8211_set_interval(dev, 100, 10);
+	return 0;
+
+fail:
+	return retval;
+}
+
+static void adm8211_stop(struct ieee80211_hw *dev)
+{
+	struct adm8211_priv *priv = dev->priv;
+
+	priv->mode = IEEE80211_IF_TYPE_INVALID;
+	priv->nar = 0;
+	ADM8211_CSR_WRITE(NAR, 0);
+	ADM8211_CSR_WRITE(IER, 0);
+	ADM8211_CSR_READ(NAR);
+
+	free_irq(priv->pdev->irq, dev);
+
+	adm8211_free_rings(dev);
+}
+
+static void adm8211_calc_durations(int *dur, int *plcp, size_t payload_len, int len,
+				   int plcp_signal, int short_preamble)
+{
+	/* Alternative calculation from NetBSD: */
+
+/* IEEE 802.11b durations for DSSS PHY in microseconds */
+#define IEEE80211_DUR_DS_LONG_PREAMBLE	144
+#define IEEE80211_DUR_DS_SHORT_PREAMBLE	72
+#define IEEE80211_DUR_DS_FAST_PLCPHDR	24
+#define IEEE80211_DUR_DS_SLOW_PLCPHDR	48
+#define IEEE80211_DUR_DS_SLOW_ACK	112
+#define IEEE80211_DUR_DS_FAST_ACK	56
+#define IEEE80211_DUR_DS_SLOW_CTS	112
+#define IEEE80211_DUR_DS_FAST_CTS	56
+#define IEEE80211_DUR_DS_SLOT		20
+#define IEEE80211_DUR_DS_SIFS		10
+
+	int remainder;
+
+	*dur = (80 * (24 + payload_len) + plcp_signal - 1)
+		/ plcp_signal;
+
+	if (plcp_signal <= PLCP_SIGNAL_2M)
+		/* 1-2Mbps WLAN: send ACK/CTS at 1Mbps */
+		*dur += 3 * (IEEE80211_DUR_DS_SIFS +
+			     IEEE80211_DUR_DS_SHORT_PREAMBLE +
+			     IEEE80211_DUR_DS_FAST_PLCPHDR) +
+			     IEEE80211_DUR_DS_SLOW_CTS + IEEE80211_DUR_DS_SLOW_ACK;
+	else
+		/* 5-11Mbps WLAN: send ACK/CTS at 2Mbps */
+		*dur += 3 * (IEEE80211_DUR_DS_SIFS +
+			     IEEE80211_DUR_DS_SHORT_PREAMBLE +
+			     IEEE80211_DUR_DS_FAST_PLCPHDR) +
+			     IEEE80211_DUR_DS_FAST_CTS + IEEE80211_DUR_DS_FAST_ACK;
+
+	/* lengthen duration if long preamble */
+	if (!short_preamble)
+		*dur +=	3 * (IEEE80211_DUR_DS_LONG_PREAMBLE -
+			     IEEE80211_DUR_DS_SHORT_PREAMBLE) +
+			3 * (IEEE80211_DUR_DS_SLOW_PLCPHDR -
+			     IEEE80211_DUR_DS_FAST_PLCPHDR);
+
+
+	*plcp = (80 * len) / plcp_signal;
+	remainder = (80 * len) % plcp_signal;
+	if (plcp_signal == PLCP_SIGNAL_11M &&
+	    remainder <= 30 && remainder > 0)
+		*plcp = (*plcp | 0x8000) + 1;
+	else if (remainder)
+		(*plcp)++;
+}
+
+/* Transmit skb w/adm8211_tx_hdr (802.11 header created by hardware) */
+static void adm8211_tx_raw(struct ieee80211_hw *dev, struct sk_buff *skb,
+			   u16 plcp_signal,
+			   struct ieee80211_tx_control *control,
+			   size_t hdrlen)
+{
+	struct adm8211_priv *priv = dev->priv;
+	unsigned long flags;
+	dma_addr_t mapping;
+	unsigned int entry;
+	u32 flag;
+
+	mapping = pci_map_single(priv->pdev, skb->data, skb->len,
+				 PCI_DMA_TODEVICE);
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	if (priv->cur_tx - priv->dirty_tx == priv->tx_ring_size / 2)
+		flag = TDES1_CONTROL_IC | TDES1_CONTROL_LS | TDES1_CONTROL_FS;
+	else
+		flag = TDES1_CONTROL_LS | TDES1_CONTROL_FS;
+
+	if (priv->cur_tx - priv->dirty_tx == priv->tx_ring_size - 2)
+		ieee80211_stop_queue(dev, 0);
+
+	entry = priv->cur_tx % priv->tx_ring_size;
+
+	priv->tx_buffers[entry].skb = skb;
+	priv->tx_buffers[entry].mapping = mapping;
+	memcpy(&priv->tx_buffers[entry].tx_control, control, sizeof(*control));
+	priv->tx_buffers[entry].hdrlen = hdrlen;
+	priv->tx_ring[entry].buffer1 = cpu_to_le32(mapping);
+
+	if (entry == priv->tx_ring_size - 1)
+		flag |= TDES1_CONTROL_TER;
+	priv->tx_ring[entry].length = cpu_to_le32(flag | skb->len);
+
+	/* Set TX rate (SIGNAL field in PLCP PPDU format) */
+	flag = TDES0_CONTROL_OWN | (plcp_signal << 20) | 8 /* ? */;
+	priv->tx_ring[entry].status = cpu_to_le32(flag);
+
+	priv->cur_tx++;
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	/* Trigger transmit poll */
+	ADM8211_CSR_WRITE(TDR, 0);
+}
+
+/* Put adm8211_tx_hdr on skb and transmit */
+static int adm8211_tx(struct ieee80211_hw *dev, struct sk_buff *skb,
+		      struct ieee80211_tx_control *control)
+{
+	struct adm8211_tx_hdr *txhdr;
+	u16 fc;
+	size_t payload_len, hdrlen;
+	int plcp, dur, len, plcp_signal, short_preamble;
+	struct ieee80211_hdr *hdr;
+
+	if (control->tx_rate < 0) {
+		short_preamble = 1;
+		plcp_signal = -control->tx_rate;
+	} else {
+		short_preamble = 0;
+		plcp_signal = control->tx_rate;
+	}
+
+	hdr = (struct ieee80211_hdr *)skb->data;
+	fc = le16_to_cpu(hdr->frame_control) & ~IEEE80211_FCTL_PROTECTED;
+	hdrlen = ieee80211_get_hdrlen(fc);
+	memcpy(skb->cb, skb->data, hdrlen);
+	hdr = (struct ieee80211_hdr *)skb->cb;
+	skb_pull(skb, hdrlen);
+	payload_len = skb->len;
+
+	txhdr = (struct adm8211_tx_hdr *) skb_push(skb, sizeof(*txhdr));
+	memset(txhdr, 0, sizeof(*txhdr));
+	memcpy(txhdr->da, ieee80211_get_DA(hdr), ETH_ALEN);
+	txhdr->signal = plcp_signal;
+	txhdr->frame_body_size = cpu_to_le16(payload_len);
+	txhdr->frame_control = hdr->frame_control;
+
+	len = hdrlen + payload_len + FCS_LEN;
+	if (fc & IEEE80211_FCTL_PROTECTED)
+		len += 8;
+
+	txhdr->frag = cpu_to_le16(0x0FFF);
+	adm8211_calc_durations(&dur, &plcp, payload_len,
+			       len, plcp_signal, short_preamble);
+	txhdr->plcp_frag_head_len = cpu_to_le16(plcp);
+	txhdr->plcp_frag_tail_len = cpu_to_le16(plcp);
+	txhdr->dur_frag_head = cpu_to_le16(dur);
+	txhdr->dur_frag_tail = cpu_to_le16(dur);
+
+	txhdr->header_control = cpu_to_le16(ADM8211_TXHDRCTL_ENABLE_EXTEND_HEADER);
+
+	if (short_preamble)
+		txhdr->header_control |= cpu_to_le16(ADM8211_TXHDRCTL_SHORT_PREAMBLE);
+
+	if (control->flags & IEEE80211_TXCTL_USE_RTS_CTS)
+		txhdr->header_control |= cpu_to_le16(ADM8211_TXHDRCTL_ENABLE_RTS);
+
+	if (fc & IEEE80211_FCTL_PROTECTED)
+		txhdr->header_control |= cpu_to_le16(ADM8211_TXHDRCTL_ENABLE_WEP_ENGINE);
+
+	txhdr->retry_limit = control->retry_limit;
+
+	adm8211_tx_raw(dev, skb, plcp_signal, control, hdrlen);
+
+	return NETDEV_TX_OK;
+}
+
+static int adm8211_alloc_rings(struct ieee80211_hw *dev)
+{
+	struct adm8211_priv *priv = dev->priv;
+	unsigned int ring_size;
+
+	priv->rx_buffers = kmalloc(sizeof(*priv->rx_buffers) * priv->rx_ring_size +
+				   sizeof(*priv->tx_buffers) * priv->tx_ring_size, GFP_KERNEL);
+	if (!priv->rx_buffers)
+		return -ENOMEM;
+
+	priv->tx_buffers = (void *)priv->rx_buffers +
+			   sizeof(*priv->rx_buffers) * priv->rx_ring_size;
+
+	/* Allocate TX/RX descriptors */
+	ring_size = sizeof(struct adm8211_desc) * priv->rx_ring_size +
+		    sizeof(struct adm8211_desc) * priv->tx_ring_size;
+	priv->rx_ring = pci_alloc_consistent(priv->pdev, ring_size,
+					     &priv->rx_ring_dma);
+
+	if (!priv->rx_ring) {
+		kfree(priv->rx_buffers);
+		priv->rx_buffers = NULL;
+		priv->tx_buffers = NULL;
+		return -ENOMEM;
+	}
+
+	priv->tx_ring = (struct adm8211_desc *)(priv->rx_ring +
+						priv->rx_ring_size);
+	priv->tx_ring_dma = priv->rx_ring_dma +
+			    sizeof(struct adm8211_desc) * priv->rx_ring_size;
+
+	return 0;
+}
+
+static const struct ieee80211_ops adm8211_ops = {
+	.tx			= adm8211_tx,
+	.start			= adm8211_start,
+	.stop			= adm8211_stop,
+	.add_interface		= adm8211_add_interface,
+	.remove_interface	= adm8211_remove_interface,
+	.config			= adm8211_config,
+	.config_interface	= adm8211_config_interface,
+	.configure_filter	= adm8211_configure_filter,
+	.get_stats		= adm8211_get_stats,
+	.get_tx_stats		= adm8211_get_tx_stats,
+	.get_tsf		= adm8211_get_tsft
+};
+
+static int __devinit adm8211_probe(struct pci_dev *pdev,
+				   const struct pci_device_id *id)
+{
+	struct ieee80211_hw *dev;
+	struct adm8211_priv *priv;
+	unsigned long mem_addr, mem_len;
+	unsigned int io_addr, io_len;
+	int err;
+	u32 reg;
+	u8 perm_addr[ETH_ALEN];
+	DECLARE_MAC_BUF(mac);
+
+	err = pci_enable_device(pdev);
+	if (err) {
+		printk(KERN_ERR "%s (adm8211): Cannot enable new PCI device\n",
+		       pci_name(pdev));
+		return err;
+	}
+
+	io_addr = pci_resource_start(pdev, 0);
+	io_len = pci_resource_len(pdev, 0);
+	mem_addr = pci_resource_start(pdev, 1);
+	mem_len = pci_resource_len(pdev, 1);
+	if (io_len < 256 || mem_len < 1024) {
+		printk(KERN_ERR "%s (adm8211): Too short PCI resources\n",
+		       pci_name(pdev));
+		goto err_disable_pdev;
+	}
+
+
+	/* check signature */
+	pci_read_config_dword(pdev, 0x80 /* CR32 */, &reg);
+	if (reg != ADM8211_SIG1 && reg != ADM8211_SIG2) {
+		printk(KERN_ERR "%s (adm8211): Invalid signature (0x%x)\n",
+		       pci_name(pdev), reg);
+		goto err_disable_pdev;
+	}
+
+	err = pci_request_regions(pdev, "adm8211");
+	if (err) {
+		printk(KERN_ERR "%s (adm8211): Cannot obtain PCI resources\n",
+		       pci_name(pdev));
+		return err; /* someone else grabbed it? don't disable it */
+	}
+
+	if (pci_set_dma_mask(pdev, DMA_32BIT_MASK) ||
+	    pci_set_consistent_dma_mask(pdev, DMA_32BIT_MASK)) {
+		printk(KERN_ERR "%s (adm8211): No suitable DMA available\n",
+		       pci_name(pdev));
+		goto err_free_reg;
+	}
+
+	pci_set_master(pdev);
+
+	dev = ieee80211_alloc_hw(sizeof(*priv), &adm8211_ops);
+	if (!dev) {
+		printk(KERN_ERR "%s (adm8211): ieee80211 alloc failed\n",
+		       pci_name(pdev));
+		err = -ENOMEM;
+		goto err_free_reg;
+	}
+	priv = dev->priv;
+	priv->pdev = pdev;
+
+	spin_lock_init(&priv->lock);
+
+	SET_IEEE80211_DEV(dev, &pdev->dev);
+
+	pci_set_drvdata(pdev, dev);
+
+	priv->map = pci_iomap(pdev, 1, mem_len);
+	if (!priv->map)
+		priv->map = pci_iomap(pdev, 0, io_len);
+
+	if (!priv->map) {
+		printk(KERN_ERR "%s (adm8211): Cannot map device memory\n",
+		       pci_name(pdev));
+		goto err_free_dev;
+	}
+
+	priv->rx_ring_size = rx_ring_size;
+	priv->tx_ring_size = tx_ring_size;
+
+	if (adm8211_alloc_rings(dev)) {
+		printk(KERN_ERR "%s (adm8211): Cannot allocate TX/RX ring\n",
+		       pci_name(pdev));
+		goto err_iounmap;
+	}
+
+	*(u32 *)perm_addr = le32_to_cpu((__force __le32)ADM8211_CSR_READ(PAR0));
+	*(u16 *)&perm_addr[4] =
+		le16_to_cpu((__force __le16)ADM8211_CSR_READ(PAR1) & 0xFFFF);
+
+	if (!is_valid_ether_addr(perm_addr)) {
+		printk(KERN_WARNING "%s (adm8211): Invalid hwaddr in EEPROM!\n",
+		       pci_name(pdev));
+		random_ether_addr(perm_addr);
+	}
+	SET_IEEE80211_PERM_ADDR(dev, perm_addr);
+
+	dev->extra_tx_headroom = sizeof(struct adm8211_tx_hdr);
+	dev->flags = IEEE80211_HW_DEFAULT_REG_DOMAIN_CONFIGURED;
+	/* IEEE80211_HW_RX_INCLUDES_FCS in promisc mode */
+
+	dev->channel_change_time = 1000;
+	dev->max_rssi = 100;	/* FIXME: find better value */
+
+	priv->modes[0].mode = MODE_IEEE80211B;
+	/* channel info filled in by adm8211_read_eeprom */
+	memcpy(priv->rates, adm8211_rates, sizeof(adm8211_rates));
+	priv->modes[0].num_rates = ARRAY_SIZE(adm8211_rates);
+	priv->modes[0].rates = priv->rates;
+
+	dev->queues = 1; /* ADM8211C supports more, maybe ADM8211B too */
+
+	priv->retry_limit = 3;
+	priv->ant_power = 0x40;
+	priv->tx_power = 0x40;
+	priv->lpf_cutoff = 0xFF;
+	priv->lnags_threshold = 0xFF;
+	priv->mode = IEEE80211_IF_TYPE_INVALID;
+
+	/* Power-on issue. EEPROM won't read correctly without */
+	if (pdev->revision >= ADM8211_REV_BA) {
+		ADM8211_CSR_WRITE(FRCTL, 0);
+		ADM8211_CSR_READ(FRCTL);
+		ADM8211_CSR_WRITE(FRCTL, 1);
+		ADM8211_CSR_READ(FRCTL);
+		msleep(100);
+	}
+
+	err = adm8211_read_eeprom(dev);
+	if (err) {
+		printk(KERN_ERR "%s (adm8211): Can't alloc eeprom buffer\n",
+		       pci_name(pdev));
+		goto err_free_desc;
+	}
+
+	priv->channel = priv->modes[0].channels[0].chan;
+
+	err = ieee80211_register_hwmode(dev, &priv->modes[0]);
+	if (err) {
+		printk(KERN_ERR "%s (adm8211): Can't register hwmode\n",
+		       pci_name(pdev));
+		goto err_free_desc;
+	}
+
+	err = ieee80211_register_hw(dev);
+	if (err) {
+		printk(KERN_ERR "%s (adm8211): Cannot register device\n",
+		       pci_name(pdev));
+		goto err_free_desc;
+	}
+
+	printk(KERN_INFO "%s: hwaddr %s, Rev 0x%02x\n",
+	       wiphy_name(dev->wiphy), print_mac(mac, dev->wiphy->perm_addr),
+	       pdev->revision);
+
+	return 0;
+
+ err_free_desc:
+	pci_free_consistent(pdev,
+			    sizeof(struct adm8211_desc) * priv->rx_ring_size +
+			    sizeof(struct adm8211_desc) * priv->tx_ring_size,
+			    priv->rx_ring, priv->rx_ring_dma);
+	kfree(priv->rx_buffers);
+
+ err_iounmap:
+	pci_iounmap(pdev, priv->map);
+
+ err_free_dev:
+	pci_set_drvdata(pdev, NULL);
+	ieee80211_free_hw(dev);
+
+ err_free_reg:
+	pci_release_regions(pdev);
+
+ err_disable_pdev:
+	pci_disable_device(pdev);
+	return err;
+}
+
+
+static void __devexit adm8211_remove(struct pci_dev *pdev)
+{
+	struct ieee80211_hw *dev = pci_get_drvdata(pdev);
+	struct adm8211_priv *priv;
+
+	if (!dev)
+		return;
+
+	ieee80211_unregister_hw(dev);
+
+	priv = dev->priv;
+
+	pci_free_consistent(pdev,
+			    sizeof(struct adm8211_desc) * priv->rx_ring_size +
+			    sizeof(struct adm8211_desc) * priv->tx_ring_size,
+			    priv->rx_ring, priv->rx_ring_dma);
+
+	kfree(priv->rx_buffers);
+	kfree(priv->eeprom);
+	pci_iounmap(pdev, priv->map);
+	pci_release_regions(pdev);
+	pci_disable_device(pdev);
+	ieee80211_free_hw(dev);
+}
+
+
+#ifdef CONFIG_PM
+static int adm8211_suspend(struct pci_dev *pdev, pm_message_t state)
+{
+	struct ieee80211_hw *dev = pci_get_drvdata(pdev);
+	struct adm8211_priv *priv = dev->priv;
+
+	if (priv->mode != IEEE80211_IF_TYPE_INVALID) {
+		ieee80211_stop_queues(dev);
+		adm8211_stop(dev);
+	}
+
+	pci_save_state(pdev);
+	pci_set_power_state(pdev, pci_choose_state(pdev, state));
+	return 0;
+}
+
+static int adm8211_resume(struct pci_dev *pdev)
+{
+	struct ieee80211_hw *dev = pci_get_drvdata(pdev);
+	struct adm8211_priv *priv = dev->priv;
+
+	pci_set_power_state(pdev, PCI_D0);
+	pci_restore_state(pdev);
+
+	if (priv->mode != IEEE80211_IF_TYPE_INVALID) {
+		adm8211_start(dev);
+		ieee80211_start_queues(dev);
+	}
+
+	return 0;
+}
+#endif /* CONFIG_PM */
+
+
+MODULE_DEVICE_TABLE(pci, adm8211_pci_id_table);
+
+/* TODO: implement enable_wake */
+static struct pci_driver adm8211_driver = {
+	.name		= "adm8211",
+	.id_table	= adm8211_pci_id_table,
+	.probe		= adm8211_probe,
+	.remove		= __devexit_p(adm8211_remove),
+#ifdef CONFIG_PM
+	.suspend	= adm8211_suspend,
+	.resume		= adm8211_resume,
+#endif /* CONFIG_PM */
+};
+
+
+
+static int __init adm8211_init(void)
+{
+	return pci_register_driver(&adm8211_driver);
+}
+
+
+static void __exit adm8211_exit(void)
+{
+	pci_unregister_driver(&adm8211_driver);
+}
+
+
+module_init(adm8211_init);
+module_exit(adm8211_exit);
diff --git a/drivers/net/wireless/adm8211.h b/drivers/net/wireless/adm8211.h
new file mode 100644
index 000000000000..ef326fed42e4
--- /dev/null
+++ b/drivers/net/wireless/adm8211.h
@@ -0,0 +1,656 @@
+#ifndef ADM8211_H
+#define ADM8211_H
+
+/* ADM8211 Registers */
+
+/* CR32 (SIG) signature */
+#define ADM8211_SIG1		0x82011317 /* ADM8211A */
+#define ADM8211_SIG2		0x82111317 /* ADM8211B/ADM8211C */
+
+#define ADM8211_CSR_READ(r) ioread32(&priv->map->r)
+#define ADM8211_CSR_WRITE(r, val) iowrite32((val), &priv->map->r)
+
+/* CSR (Host Control and Status Registers) */
+struct adm8211_csr {
+	__le32 PAR;		/* 0x00 CSR0 */
+	__le32 FRCTL;		/* 0x04 CSR0A */
+	__le32 TDR;		/* 0x08 CSR1 */
+	__le32 WTDP;		/* 0x0C CSR1A */
+	__le32 RDR;		/* 0x10 CSR2 */
+	__le32 WRDP;		/* 0x14 CSR2A */
+	__le32 RDB;		/* 0x18 CSR3 */
+	__le32 TDBH;		/* 0x1C CSR3A */
+	__le32 TDBD;		/* 0x20 CSR4 */
+	__le32 TDBP;		/* 0x24 CSR4A */
+	__le32 STSR;		/* 0x28 CSR5 */
+	__le32 TDBB;		/* 0x2C CSR5A */
+	__le32 NAR;		/* 0x30 CSR6 */
+	__le32 CSR6A;		/* reserved */
+	__le32 IER;		/* 0x38 CSR7 */
+	__le32 TKIPSCEP;	/* 0x3C CSR7A */
+	__le32 LPC;		/* 0x40 CSR8 */
+	__le32 CSR_TEST1;	/* 0x44 CSR8A */
+	__le32 SPR;		/* 0x48 CSR9 */
+	__le32 CSR_TEST0;	/* 0x4C CSR9A */
+	__le32 WCSR;		/* 0x50 CSR10 */
+	__le32 WPDR;		/* 0x54 CSR10A */
+	__le32 GPTMR;		/* 0x58 CSR11 */
+	__le32 GPIO;		/* 0x5C CSR11A */
+	__le32 BBPCTL;		/* 0x60 CSR12 */
+	__le32 SYNCTL;		/* 0x64 CSR12A */
+	__le32 PLCPHD;		/* 0x68 CSR13 */
+	__le32 MMIWA;		/* 0x6C CSR13A */
+	__le32 MMIRD0;		/* 0x70 CSR14 */
+	__le32 MMIRD1;		/* 0x74 CSR14A */
+	__le32 TXBR;		/* 0x78 CSR15 */
+	__le32 SYNDATA;		/* 0x7C CSR15A */
+	__le32 ALCS;		/* 0x80 CSR16 */
+	__le32 TOFS2;		/* 0x84 CSR17 */
+	__le32 CMDR;		/* 0x88 CSR18 */
+	__le32 PCIC;		/* 0x8C CSR19 */
+	__le32 PMCSR;		/* 0x90 CSR20 */
+	__le32 PAR0;		/* 0x94 CSR21 */
+	__le32 PAR1;		/* 0x98 CSR22 */
+	__le32 MAR0;		/* 0x9C CSR23 */
+	__le32 MAR1;		/* 0xA0 CSR24 */
+	__le32 ATIMDA0;		/* 0xA4 CSR25 */
+	__le32 ABDA1;		/* 0xA8 CSR26 */
+	__le32 BSSID0;		/* 0xAC CSR27 */
+	__le32 TXLMT;		/* 0xB0 CSR28 */
+	__le32 MIBCNT;		/* 0xB4 CSR29 */
+	__le32 BCNT;		/* 0xB8 CSR30 */
+	__le32 TSFTH;		/* 0xBC CSR31 */
+	__le32 TSC;		/* 0xC0 CSR32 */
+	__le32 SYNRF;		/* 0xC4 CSR33 */
+	__le32 BPLI;		/* 0xC8 CSR34 */
+	__le32 CAP0;		/* 0xCC CSR35 */
+	__le32 CAP1;		/* 0xD0 CSR36 */
+	__le32 RMD;		/* 0xD4 CSR37 */
+	__le32 CFPP;		/* 0xD8 CSR38 */
+	__le32 TOFS0;		/* 0xDC CSR39 */
+	__le32 TOFS1;		/* 0xE0 CSR40 */
+	__le32 IFST;		/* 0xE4 CSR41 */
+	__le32 RSPT;		/* 0xE8 CSR42 */
+	__le32 TSFTL;		/* 0xEC CSR43 */
+	__le32 WEPCTL;		/* 0xF0 CSR44 */
+	__le32 WESK;		/* 0xF4 CSR45 */
+	__le32 WEPCNT;		/* 0xF8 CSR46 */
+	__le32 MACTEST;		/* 0xFC CSR47 */
+	__le32 FER;		/* 0x100 */
+	__le32 FEMR;		/* 0x104 */
+	__le32 FPSR;		/* 0x108 */
+	__le32 FFER;		/* 0x10C */
+} __attribute__ ((packed));
+
+/* CSR0 - PAR (PCI Address Register) */
+#define ADM8211_PAR_MWIE	(1 << 24)
+#define ADM8211_PAR_MRLE	(1 << 23)
+#define ADM8211_PAR_MRME	(1 << 21)
+#define ADM8211_PAR_RAP		((1 << 18) | (1 << 17))
+#define ADM8211_PAR_CAL		((1 << 15) | (1 << 14))
+#define ADM8211_PAR_PBL		0x00003f00
+#define ADM8211_PAR_BLE		(1 << 7)
+#define ADM8211_PAR_DSL		0x0000007c
+#define ADM8211_PAR_BAR		(1 << 1)
+#define ADM8211_PAR_SWR		(1 << 0)
+
+/* CSR1 - FRCTL (Frame Control Register) */
+#define ADM8211_FRCTL_PWRMGT	(1 << 31)
+#define ADM8211_FRCTL_MAXPSP	(1 << 27)
+#define ADM8211_FRCTL_DRVPRSP	(1 << 26)
+#define ADM8211_FRCTL_DRVBCON	(1 << 25)
+#define ADM8211_FRCTL_AID	0x0000ffff
+#define ADM8211_FRCTL_AID_ON	0x0000c000
+
+/* CSR5 - STSR (Status Register) */
+#define ADM8211_STSR_PCF	(1 << 31)
+#define ADM8211_STSR_BCNTC	(1 << 30)
+#define ADM8211_STSR_GPINT	(1 << 29)
+#define ADM8211_STSR_LinkOff	(1 << 28)
+#define ADM8211_STSR_ATIMTC	(1 << 27)
+#define ADM8211_STSR_TSFTF	(1 << 26)
+#define ADM8211_STSR_TSCZ	(1 << 25)
+#define ADM8211_STSR_LinkOn	(1 << 24)
+#define ADM8211_STSR_SQL	(1 << 23)
+#define ADM8211_STSR_WEPTD	(1 << 22)
+#define ADM8211_STSR_ATIME	(1 << 21)
+#define ADM8211_STSR_TBTT	(1 << 20)
+#define ADM8211_STSR_NISS	(1 << 16)
+#define ADM8211_STSR_AISS	(1 << 15)
+#define ADM8211_STSR_TEIS	(1 << 14)
+#define ADM8211_STSR_FBE	(1 << 13)
+#define ADM8211_STSR_REIS	(1 << 12)
+#define ADM8211_STSR_GPTT	(1 << 11)
+#define ADM8211_STSR_RPS	(1 << 8)
+#define ADM8211_STSR_RDU	(1 << 7)
+#define ADM8211_STSR_RCI	(1 << 6)
+#define ADM8211_STSR_TUF	(1 << 5)
+#define ADM8211_STSR_TRT	(1 << 4)
+#define ADM8211_STSR_TLT	(1 << 3)
+#define ADM8211_STSR_TDU	(1 << 2)
+#define ADM8211_STSR_TPS	(1 << 1)
+#define ADM8211_STSR_TCI	(1 << 0)
+
+/* CSR6 - NAR (Network Access Register) */
+#define ADM8211_NAR_TXCF	(1 << 31)
+#define ADM8211_NAR_HF		(1 << 30)
+#define ADM8211_NAR_UTR		(1 << 29)
+#define ADM8211_NAR_SQ		(1 << 28)
+#define ADM8211_NAR_CFP		(1 << 27)
+#define ADM8211_NAR_SF		(1 << 21)
+#define ADM8211_NAR_TR		((1 << 15) | (1 << 14))
+#define ADM8211_NAR_ST		(1 << 13)
+#define ADM8211_NAR_OM		((1 << 11) | (1 << 10))
+#define ADM8211_NAR_MM		(1 << 7)
+#define ADM8211_NAR_PR		(1 << 6)
+#define ADM8211_NAR_EA		(1 << 5)
+#define ADM8211_NAR_PB		(1 << 3)
+#define ADM8211_NAR_STPDMA	(1 << 2)
+#define ADM8211_NAR_SR		(1 << 1)
+#define ADM8211_NAR_CTX		(1 << 0)
+
+#define ADM8211_IDLE() 							   \
+do { 									   \
+	if (priv->nar & (ADM8211_NAR_SR | ADM8211_NAR_ST)) {		   \
+		ADM8211_CSR_WRITE(NAR, priv->nar &			   \
+				       ~(ADM8211_NAR_SR | ADM8211_NAR_ST));\
+		ADM8211_CSR_READ(NAR);					   \
+		msleep(20);						   \
+	}								   \
+} while (0)
+
+#define ADM8211_IDLE_RX() 						\
+do {									\
+	if (priv->nar & ADM8211_NAR_SR) {				\
+		ADM8211_CSR_WRITE(NAR, priv->nar & ~ADM8211_NAR_SR);	\
+		ADM8211_CSR_READ(NAR);					\
+		mdelay(20);						\
+	}								\
+} while (0)
+
+#define ADM8211_RESTORE()					\
+do {								\
+	if (priv->nar & (ADM8211_NAR_SR | ADM8211_NAR_ST))	\
+		ADM8211_CSR_WRITE(NAR, priv->nar);		\
+} while (0)
+
+/* CSR7 - IER (Interrupt Enable Register) */
+#define ADM8211_IER_PCFIE	(1 << 31)
+#define ADM8211_IER_BCNTCIE	(1 << 30)
+#define ADM8211_IER_GPIE	(1 << 29)
+#define ADM8211_IER_LinkOffIE	(1 << 28)
+#define ADM8211_IER_ATIMTCIE	(1 << 27)
+#define ADM8211_IER_TSFTFIE	(1 << 26)
+#define ADM8211_IER_TSCZE	(1 << 25)
+#define ADM8211_IER_LinkOnIE	(1 << 24)
+#define ADM8211_IER_SQLIE	(1 << 23)
+#define ADM8211_IER_WEPIE	(1 << 22)
+#define ADM8211_IER_ATIMEIE	(1 << 21)
+#define ADM8211_IER_TBTTIE	(1 << 20)
+#define ADM8211_IER_NIE		(1 << 16)
+#define ADM8211_IER_AIE		(1 << 15)
+#define ADM8211_IER_TEIE	(1 << 14)
+#define ADM8211_IER_FBEIE	(1 << 13)
+#define ADM8211_IER_REIE	(1 << 12)
+#define ADM8211_IER_GPTIE	(1 << 11)
+#define ADM8211_IER_RSIE	(1 << 8)
+#define ADM8211_IER_RUIE	(1 << 7)
+#define ADM8211_IER_RCIE	(1 << 6)
+#define ADM8211_IER_TUIE	(1 << 5)
+#define ADM8211_IER_TRTIE	(1 << 4)
+#define ADM8211_IER_TLTTIE	(1 << 3)
+#define ADM8211_IER_TDUIE	(1 << 2)
+#define ADM8211_IER_TPSIE	(1 << 1)
+#define ADM8211_IER_TCIE	(1 << 0)
+
+/* CSR9 - SPR (Serial Port Register) */
+#define ADM8211_SPR_SRS		(1 << 11)
+#define ADM8211_SPR_SDO		(1 << 3)
+#define ADM8211_SPR_SDI		(1 << 2)
+#define ADM8211_SPR_SCLK	(1 << 1)
+#define ADM8211_SPR_SCS		(1 << 0)
+
+/* CSR9A - CSR_TEST0 */
+#define ADM8211_CSR_TEST0_EPNE	(1 << 18)
+#define ADM8211_CSR_TEST0_EPSNM	(1 << 17)
+#define ADM8211_CSR_TEST0_EPTYP	(1 << 16)
+#define ADM8211_CSR_TEST0_EPRLD	(1 << 15)
+
+/* CSR10 - WCSR (Wake-up Control/Status Register) */
+#define ADM8211_WCSR_CRCT	(1 << 30)
+#define ADM8211_WCSR_TSFTWE	(1 << 20)
+#define ADM8211_WCSR_TIMWE	(1 << 19)
+#define ADM8211_WCSR_ATIMWE	(1 << 18)
+#define ADM8211_WCSR_KEYWE	(1 << 17)
+#define ADM8211_WCSR_MPRE	(1 << 9)
+#define ADM8211_WCSR_LSOE	(1 << 8)
+#define ADM8211_WCSR_KEYUP	(1 << 6)
+#define ADM8211_WCSR_TSFTW	(1 << 5)
+#define ADM8211_WCSR_TIMW	(1 << 4)
+#define ADM8211_WCSR_ATIMW	(1 << 3)
+#define ADM8211_WCSR_MPR	(1 << 1)
+#define ADM8211_WCSR_LSO	(1 << 0)
+
+/* CSR11A - GPIO */
+#define ADM8211_CSR_GPIO_EN5	(1 << 17)
+#define ADM8211_CSR_GPIO_EN4	(1 << 16)
+#define ADM8211_CSR_GPIO_EN3	(1 << 15)
+#define ADM8211_CSR_GPIO_EN2	(1 << 14)
+#define ADM8211_CSR_GPIO_EN1	(1 << 13)
+#define ADM8211_CSR_GPIO_EN0	(1 << 12)
+#define ADM8211_CSR_GPIO_O5	(1 << 11)
+#define ADM8211_CSR_GPIO_O4	(1 << 10)
+#define ADM8211_CSR_GPIO_O3	(1 << 9)
+#define ADM8211_CSR_GPIO_O2	(1 << 8)
+#define ADM8211_CSR_GPIO_O1	(1 << 7)
+#define ADM8211_CSR_GPIO_O0	(1 << 6)
+#define ADM8211_CSR_GPIO_IN	0x0000003f
+
+/* CSR12 - BBPCTL (BBP Control port) */
+#define ADM8211_BBPCTL_MMISEL	(1 << 31)
+#define ADM8211_BBPCTL_SPICADD  (0x7F << 24)
+#define ADM8211_BBPCTL_RF3000	(0x20 << 24)
+#define ADM8211_BBPCTL_TXCE	(1 << 23)
+#define ADM8211_BBPCTL_RXCE	(1 << 22)
+#define ADM8211_BBPCTL_CCAP	(1 << 21)
+#define ADM8211_BBPCTL_TYPE	0x001c0000
+#define ADM8211_BBPCTL_WR	(1 << 17)
+#define ADM8211_BBPCTL_RD	(1 << 16)
+#define ADM8211_BBPCTL_ADDR	0x0000ff00
+#define ADM8211_BBPCTL_DATA	0x000000ff
+
+/* CSR12A - SYNCTL (Synthesizer Control port) */
+#define ADM8211_SYNCTL_WR	(1 << 31)
+#define ADM8211_SYNCTL_RD	(1 << 30)
+#define ADM8211_SYNCTL_CS0	(1 << 29)
+#define ADM8211_SYNCTL_CS1	(1 << 28)
+#define ADM8211_SYNCTL_CAL	(1 << 27)
+#define ADM8211_SYNCTL_SELCAL	(1 << 26)
+#define ADM8211_SYNCTL_RFtype	((1 << 24) || (1 << 23) || (1 << 22))
+#define ADM8211_SYNCTL_RFMD	(1 << 22)
+#define ADM8211_SYNCTL_GENERAL	(0x7 << 22)
+/* SYNCTL 21:0 Data (Si4126: 18-bit data, 4-bit address) */
+
+/* CSR18 - CMDR (Command Register) */
+#define ADM8211_CMDR_PM		(1 << 19)
+#define ADM8211_CMDR_APM	(1 << 18)
+#define ADM8211_CMDR_RTE	(1 << 4)
+#define ADM8211_CMDR_DRT	((1 << 3) | (1 << 2))
+#define ADM8211_CMDR_DRT_8DW	(0x0 << 2)
+#define ADM8211_CMDR_DRT_16DW	(0x1 << 2)
+#define ADM8211_CMDR_DRT_SF	(0x2 << 2)
+
+/* CSR33 - SYNRF (SYNRF direct control) */
+#define ADM8211_SYNRF_SELSYN	(1 << 31)
+#define ADM8211_SYNRF_SELRF	(1 << 30)
+#define ADM8211_SYNRF_LERF	(1 << 29)
+#define ADM8211_SYNRF_LEIF	(1 << 28)
+#define ADM8211_SYNRF_SYNCLK	(1 << 27)
+#define ADM8211_SYNRF_SYNDATA	(1 << 26)
+#define ADM8211_SYNRF_PE1	(1 << 25)
+#define ADM8211_SYNRF_PE2	(1 << 24)
+#define ADM8211_SYNRF_PA_PE	(1 << 23)
+#define ADM8211_SYNRF_TR_SW	(1 << 22)
+#define ADM8211_SYNRF_TR_SWN	(1 << 21)
+#define ADM8211_SYNRF_RADIO	(1 << 20)
+#define ADM8211_SYNRF_CAL_EN	(1 << 19)
+#define ADM8211_SYNRF_PHYRST	(1 << 18)
+
+#define ADM8211_SYNRF_IF_SELECT_0 	(1 << 31)
+#define ADM8211_SYNRF_IF_SELECT_1 	((1 << 31) | (1 << 28))
+#define ADM8211_SYNRF_WRITE_SYNDATA_0	(1 << 31)
+#define ADM8211_SYNRF_WRITE_SYNDATA_1	((1 << 31) | (1 << 26))
+#define ADM8211_SYNRF_WRITE_CLOCK_0	(1 << 31)
+#define ADM8211_SYNRF_WRITE_CLOCK_1	((1 << 31) | (1 << 27))
+
+/* CSR44 - WEPCTL (WEP Control) */
+#define ADM8211_WEPCTL_WEPENABLE   (1 << 31)
+#define ADM8211_WEPCTL_WPAENABLE   (1 << 30)
+#define ADM8211_WEPCTL_CURRENT_TABLE (1 << 29)
+#define ADM8211_WEPCTL_TABLE_WR	(1 << 28)
+#define ADM8211_WEPCTL_TABLE_RD	(1 << 27)
+#define ADM8211_WEPCTL_WEPRXBYP	(1 << 25)
+#define ADM8211_WEPCTL_SEL_WEPTABLE (1 << 23)
+#define ADM8211_WEPCTL_ADDR	(0x000001ff)
+
+/* CSR45 - WESK (Data Entry for Share/Individual Key) */
+#define ADM8211_WESK_DATA	(0x0000ffff)
+
+/* FER (Function Event Register) */
+#define ADM8211_FER_INTR_EV_ENT	(1 << 15)
+
+
+/* Si4126 RF Synthesizer - Control Registers */
+#define SI4126_MAIN_CONF	0
+#define SI4126_PHASE_DET_GAIN	1
+#define SI4126_POWERDOWN	2
+#define SI4126_RF1_N_DIV	3 /* only Si4136 */
+#define SI4126_RF2_N_DIV	4
+#define SI4126_IF_N_DIV		5
+#define SI4126_RF1_R_DIV	6 /* only Si4136 */
+#define SI4126_RF2_R_DIV	7
+#define SI4126_IF_R_DIV		8
+
+/* Main Configuration */
+#define SI4126_MAIN_XINDIV2	(1 << 6)
+#define SI4126_MAIN_IFDIV	((1 << 11) | (1 << 10))
+/* Powerdown */
+#define SI4126_POWERDOWN_PDIB	(1 << 1)
+#define SI4126_POWERDOWN_PDRB	(1 << 0)
+
+
+/* RF3000 BBP - Control Port Registers */
+/* 0x00 - reserved */
+#define RF3000_MODEM_CTRL__RX_STATUS 0x01
+#define RF3000_CCA_CTRL 0x02
+#define RF3000_DIVERSITY__RSSI 0x03
+#define RF3000_RX_SIGNAL_FIELD 0x04
+#define RF3000_RX_LEN_MSB 0x05
+#define RF3000_RX_LEN_LSB 0x06
+#define RF3000_RX_SERVICE_FIELD 0x07
+#define RF3000_TX_VAR_GAIN__TX_LEN_EXT 0x11
+#define RF3000_TX_LEN_MSB 0x12
+#define RF3000_TX_LEN_LSB 0x13
+#define RF3000_LOW_GAIN_CALIB 0x14
+#define RF3000_HIGH_GAIN_CALIB 0x15
+
+/* ADM8211 revisions */
+#define ADM8211_REV_AB 0x11
+#define ADM8211_REV_AF 0x15
+#define ADM8211_REV_BA 0x20
+#define ADM8211_REV_CA 0x30
+
+struct adm8211_desc {
+	__le32 status;
+	__le32 length;
+	__le32 buffer1;
+	__le32 buffer2;
+};
+
+#define RDES0_STATUS_OWN	(1 << 31)
+#define RDES0_STATUS_ES		(1 << 30)
+#define RDES0_STATUS_SQL	(1 << 29)
+#define RDES0_STATUS_DE		(1 << 28)
+#define RDES0_STATUS_FS		(1 << 27)
+#define RDES0_STATUS_LS		(1 << 26)
+#define RDES0_STATUS_PCF	(1 << 25)
+#define RDES0_STATUS_SFDE	(1 << 24)
+#define RDES0_STATUS_SIGE	(1 << 23)
+#define RDES0_STATUS_CRC16E	(1 << 22)
+#define RDES0_STATUS_RXTOE	(1 << 21)
+#define RDES0_STATUS_CRC32E	(1 << 20)
+#define RDES0_STATUS_ICVE	(1 << 19)
+#define RDES0_STATUS_DA1	(1 << 17)
+#define RDES0_STATUS_DA0	(1 << 16)
+#define RDES0_STATUS_RXDR	((1 << 15) | (1 << 14) | (1 << 13) | (1 << 12))
+#define RDES0_STATUS_FL		(0x00000fff)
+
+#define RDES1_CONTROL_RER	(1 << 25)
+#define RDES1_CONTROL_RCH	(1 << 24)
+#define RDES1_CONTROL_RBS2	(0x00fff000)
+#define RDES1_CONTROL_RBS1	(0x00000fff)
+
+#define RDES1_STATUS_RSSI	(0x0000007f)
+
+
+#define TDES0_CONTROL_OWN	(1 << 31)
+#define TDES0_CONTROL_DONE	(1 << 30)
+#define TDES0_CONTROL_TXDR	(0x0ff00000)
+
+#define TDES0_STATUS_OWN	(1 << 31)
+#define TDES0_STATUS_DONE	(1 << 30)
+#define TDES0_STATUS_ES		(1 << 29)
+#define TDES0_STATUS_TLT	(1 << 28)
+#define TDES0_STATUS_TRT	(1 << 27)
+#define TDES0_STATUS_TUF	(1 << 26)
+#define TDES0_STATUS_TRO	(1 << 25)
+#define TDES0_STATUS_SOFBR	(1 << 24)
+#define TDES0_STATUS_ACR	(0x00000fff)
+
+#define TDES1_CONTROL_IC	(1 << 31)
+#define TDES1_CONTROL_LS	(1 << 30)
+#define TDES1_CONTROL_FS	(1 << 29)
+#define TDES1_CONTROL_TER	(1 << 25)
+#define TDES1_CONTROL_TCH	(1 << 24)
+#define TDES1_CONTROL_RBS2	(0x00fff000)
+#define TDES1_CONTROL_RBS1	(0x00000fff)
+
+/* SRAM offsets */
+#define ADM8211_SRAM(x) (priv->pdev->revision < ADM8211_REV_BA ? \
+        ADM8211_SRAM_A_ ## x : ADM8211_SRAM_B_ ## x)
+
+#define ADM8211_SRAM_INDIV_KEY   0x0000
+#define ADM8211_SRAM_A_SHARE_KEY 0x0160
+#define ADM8211_SRAM_B_SHARE_KEY 0x00c0
+
+#define ADM8211_SRAM_A_SSID      0x0180
+#define ADM8211_SRAM_B_SSID      0x00d4
+#define ADM8211_SRAM_SSID ADM8211_SRAM(SSID)
+
+#define ADM8211_SRAM_A_SUPP_RATE 0x0191
+#define ADM8211_SRAM_B_SUPP_RATE 0x00dd
+#define ADM8211_SRAM_SUPP_RATE ADM8211_SRAM(SUPP_RATE)
+
+#define ADM8211_SRAM_A_SIZE      0x0200
+#define ADM8211_SRAM_B_SIZE      0x01c0
+#define ADM8211_SRAM_SIZE ADM8211_SRAM(SIZE)
+
+struct adm8211_rx_ring_info {
+	struct sk_buff *skb;
+	dma_addr_t mapping;
+};
+
+struct adm8211_tx_ring_info {
+	struct sk_buff *skb;
+	dma_addr_t mapping;
+	struct ieee80211_tx_control tx_control;
+	size_t hdrlen;
+};
+
+#define PLCP_SIGNAL_1M		0x0a
+#define PLCP_SIGNAL_2M		0x14
+#define PLCP_SIGNAL_5M5		0x37
+#define PLCP_SIGNAL_11M		0x6e
+
+struct adm8211_tx_hdr {
+	u8 da[6];
+	u8 signal; /* PLCP signal / TX rate in 100 Kbps */
+	u8 service;
+	__le16 frame_body_size;
+	__le16 frame_control;
+	__le16 plcp_frag_tail_len;
+	__le16 plcp_frag_head_len;
+	__le16 dur_frag_tail;
+	__le16 dur_frag_head;
+	u8 addr4[6];
+
+#define ADM8211_TXHDRCTL_SHORT_PREAMBLE		(1 <<  0)
+#define ADM8211_TXHDRCTL_MORE_FRAG		(1 <<  1)
+#define ADM8211_TXHDRCTL_MORE_DATA		(1 <<  2)
+#define ADM8211_TXHDRCTL_FRAG_NO		(1 <<  3) /* ? */
+#define ADM8211_TXHDRCTL_ENABLE_RTS		(1 <<  4)
+#define ADM8211_TXHDRCTL_ENABLE_WEP_ENGINE	(1 <<  5)
+#define ADM8211_TXHDRCTL_ENABLE_EXTEND_HEADER	(1 << 15) /* ? */
+	__le16 header_control;
+	__le16 frag;
+	u8 reserved_0;
+	u8 retry_limit;
+
+	u32 wep2key0;
+	u32 wep2key1;
+	u32 wep2key2;
+	u32 wep2key3;
+
+	u8 keyid;
+	u8 entry_control;	// huh??
+	u16 reserved_1;
+	u32 reserved_2;
+} __attribute__ ((packed));
+
+
+#define RX_COPY_BREAK 128
+#define RX_PKT_SIZE 2500
+
+struct adm8211_eeprom {
+	__le16	signature;		/* 0x00 */
+	u8	major_version;		/* 0x02 */
+	u8	minor_version;		/* 0x03 */
+	u8	reserved_1[4];		/* 0x04 */
+	u8	hwaddr[6];		/* 0x08 */
+	u8	reserved_2[8];		/* 0x1E */
+	__le16	cr49;			/* 0x16 */
+	u8	cr03;			/* 0x18 */
+	u8	cr28;			/* 0x19 */
+	u8	cr29;			/* 0x1A */
+	u8	country_code;		/* 0x1B */
+
+/* specific bbp types */
+#define ADM8211_BBP_RFMD3000	0x00
+#define ADM8211_BBP_RFMD3002	0x01
+#define ADM8211_BBP_ADM8011	0x04
+	u8	specific_bbptype;	/* 0x1C */
+	u8	specific_rftype;	/* 0x1D */
+	u8	reserved_3[2];		/* 0x1E */
+	__le16	device_id;		/* 0x20 */
+	__le16	vendor_id;		/* 0x22 */
+	__le16	subsystem_id;		/* 0x24 */
+	__le16	subsystem_vendor_id;	/* 0x26 */
+	u8	maxlat;			/* 0x28 */
+	u8	mingnt;			/* 0x29 */
+	__le16	cis_pointer_low;	/* 0x2A */
+	__le16	cis_pointer_high;	/* 0x2C */
+	__le16	csr18;			/* 0x2E */
+	u8	reserved_4[16];		/* 0x30 */
+	u8	d1_pwrdara;		/* 0x40 */
+	u8	d0_pwrdara;		/* 0x41 */
+	u8	d3_pwrdara;		/* 0x42 */
+	u8	d2_pwrdara;		/* 0x43 */
+	u8	antenna_power[14];	/* 0x44 */
+	__le16	cis_wordcnt;		/* 0x52 */
+	u8	tx_power[14];		/* 0x54 */
+	u8	lpf_cutoff[14];		/* 0x62 */
+	u8	lnags_threshold[14];	/* 0x70 */
+	__le16	checksum;		/* 0x7E */
+	u8	cis_data[0];		/* 0x80, 384 bytes */
+} __attribute__ ((packed));
+
+static const struct ieee80211_rate adm8211_rates[] = {
+	{ .rate = 10,
+	  .val = 10,
+	  .val2 = -10,
+	  .flags = IEEE80211_RATE_CCK_2 },
+	{ .rate = 20,
+	  .val = 20,
+	  .val2 = -20,
+	  .flags = IEEE80211_RATE_CCK_2 },
+	{ .rate = 55,
+	  .val = 55,
+	  .val2 = -55,
+	  .flags = IEEE80211_RATE_CCK_2 },
+	{ .rate = 110,
+	  .val = 110,
+	  .val2 = -110,
+	  .flags = IEEE80211_RATE_CCK_2 }
+};
+
+struct ieee80211_chan_range {
+	u8 min;
+	u8 max;
+};
+
+static const struct ieee80211_channel adm8211_channels[] = {
+	{ .chan = 1,
+	  .freq = 2412},
+	{ .chan = 2,
+	  .freq = 2417},
+	{ .chan = 3,
+	  .freq = 2422},
+	{ .chan = 4,
+	  .freq = 2427},
+	{ .chan = 5,
+	  .freq = 2432},
+	{ .chan = 6,
+	  .freq = 2437},
+	{ .chan = 7,
+	  .freq = 2442},
+	{ .chan = 8,
+	  .freq = 2447},
+	{ .chan = 9,
+	  .freq = 2452},
+	{ .chan = 10,
+	  .freq = 2457},
+	{ .chan = 11,
+	  .freq = 2462},
+	{ .chan = 12,
+	  .freq = 2467},
+	{ .chan = 13,
+	  .freq = 2472},
+	{ .chan = 14,
+	  .freq = 2484},
+};
+
+struct adm8211_priv {
+	struct pci_dev *pdev;
+	spinlock_t lock;
+	struct adm8211_csr __iomem *map;
+	struct adm8211_desc *rx_ring;
+	struct adm8211_desc *tx_ring;
+	dma_addr_t rx_ring_dma;
+	dma_addr_t tx_ring_dma;
+	struct adm8211_rx_ring_info *rx_buffers;
+	struct adm8211_tx_ring_info *tx_buffers;
+	unsigned int rx_ring_size, tx_ring_size;
+	unsigned int cur_tx, dirty_tx, cur_rx;
+
+	struct ieee80211_low_level_stats stats;
+	struct ieee80211_hw_mode modes[1];
+	struct ieee80211_channel channels[ARRAY_SIZE(adm8211_channels)];
+	struct ieee80211_rate rates[ARRAY_SIZE(adm8211_rates)];
+	int mode;
+
+	int channel;
+	u8 bssid[ETH_ALEN];
+	u8 ssid[32];
+	size_t ssid_len;
+
+	u8 soft_rx_crc;
+	u8 retry_limit;
+
+	u8 ant_power;
+	u8 tx_power;
+	u8 lpf_cutoff;
+	u8 lnags_threshold;
+	struct adm8211_eeprom *eeprom;
+	size_t eeprom_len;
+
+	u32 nar;
+
+#define ADM8211_TYPE_INTERSIL	0x00
+#define ADM8211_TYPE_RFMD	0x01
+#define ADM8211_TYPE_MARVEL	0x02
+#define ADM8211_TYPE_AIROHA	0x03
+#define ADM8211_TYPE_ADMTEK     0x05
+	unsigned int rf_type:3;
+	unsigned int bbp_type:3;
+
+	u8 specific_bbptype;
+	enum {
+		ADM8211_RFMD2948 = 0x0,
+		ADM8211_RFMD2958 = 0x1,
+		ADM8211_RFMD2958_RF3000_CONTROL_POWER = 0x2,
+		ADM8211_MAX2820 = 0x8,
+		ADM8211_AL2210L = 0xC,	/* Airoha */
+	} transceiver_type;
+};
+
+static const struct ieee80211_chan_range cranges[] = {
+	{1,  11},	/* FCC */
+	{1,  11},	/* IC */
+	{1,  13},	/* ETSI */
+	{10, 11},	/* SPAIN */
+	{10, 13},	/* FRANCE */
+	{14, 14},	/* MMK */
+	{1,  14},	/* MMK2 */
+};
+
+#endif /* ADM8211_H */
diff --git a/drivers/net/wireless/airo.c b/drivers/net/wireless/airo.c
index ee1cc14db389..074055e18c5c 100644
--- a/drivers/net/wireless/airo.c
+++ b/drivers/net/wireless/airo.c
@@ -241,8 +241,8 @@ static int proc_perm = 0644;
 
 MODULE_AUTHOR("Benjamin Reed");
 MODULE_DESCRIPTION("Support for Cisco/Aironet 802.11 wireless ethernet \
-                   cards.  Direct support for ISA/PCI/MPI cards and support \
-		   for PCMCIA when used with airo_cs.");
+cards.  Direct support for ISA/PCI/MPI cards and support \
+for PCMCIA when used with airo_cs.");
 MODULE_LICENSE("Dual BSD/GPL");
 MODULE_SUPPORTED_DEVICE("Aironet 4500, 4800 and Cisco 340/350");
 module_param_array(io, int, NULL, 0);
@@ -2481,7 +2481,7 @@ void stop_airo_card( struct net_device *dev, int freeres )
 
 EXPORT_SYMBOL(stop_airo_card);
 
-static int wll_header_parse(struct sk_buff *skb, unsigned char *haddr)
+static int wll_header_parse(const struct sk_buff *skb, unsigned char *haddr)
 {
 	memcpy(haddr, skb_mac_header(skb) + 10, ETH_ALEN);
 	return ETH_ALEN;
@@ -2696,14 +2696,13 @@ static int mpi_map_card(struct airo_info *ai, struct pci_dev *pci)
 	return rc;
 }
 
+static const struct header_ops airo_header_ops = {
+	.parse = wll_header_parse,
+};
+
 static void wifi_setup(struct net_device *dev)
 {
-	dev->hard_header        = NULL;
-	dev->rebuild_header     = NULL;
-	dev->hard_header_cache  = NULL;
-	dev->header_cache_update= NULL;
-
-	dev->hard_header_parse  = wll_header_parse;
+	dev->header_ops = &airo_header_ops;
 	dev->hard_start_xmit = &airo_start_xmit11;
 	dev->get_stats = &airo_get_stats;
 	dev->set_mac_address = &airo_set_mac_address;
@@ -2821,6 +2820,7 @@ static struct net_device *_init_airo_card( unsigned short irq, int port,
 	struct net_device *dev;
 	struct airo_info *ai;
 	int i, rc;
+	DECLARE_MAC_BUF(mac);
 
 	/* Create the network device object. */
 	dev = alloc_netdev(sizeof(*ai), "", ether_setup);
@@ -2870,7 +2870,6 @@ static struct net_device *_init_airo_card( unsigned short irq, int port,
 	dev->base_addr = port;
 
 	SET_NETDEV_DEV(dev, dmdev);
-	SET_MODULE_OWNER(dev);
 
 	reset_card (dev, 1);
 	msleep(400);
@@ -2924,9 +2923,8 @@ static struct net_device *_init_airo_card( unsigned short irq, int port,
 		goto err_out_reg;
 
 	set_bit(FLAG_REGISTERED,&ai->flags);
-	airo_print_info(dev->name, "MAC enabled %x:%x:%x:%x:%x:%x",
-		dev->dev_addr[0], dev->dev_addr[1], dev->dev_addr[2],
-		dev->dev_addr[3], dev->dev_addr[4], dev->dev_addr[5] );
+	airo_print_info(dev->name, "MAC enabled %s",
+			print_mac(mac, dev->dev_addr));
 
 	/* Allocate the transmit buffers */
 	if (probe && !test_bit(FLAG_MPI,&ai->flags))
@@ -2983,6 +2981,7 @@ int reset_airo_card( struct net_device *dev )
 {
 	int i;
 	struct airo_info *ai = dev->priv;
+	DECLARE_MAC_BUF(mac);
 
 	if (reset_card (dev, 1))
 		return -1;
@@ -2991,9 +2990,8 @@ int reset_airo_card( struct net_device *dev )
 		airo_print_err(dev->name, "MAC could not be enabled");
 		return -1;
 	}
-	airo_print_info(dev->name, "MAC enabled %x:%x:%x:%x:%x:%x",
-			dev->dev_addr[0], dev->dev_addr[1], dev->dev_addr[2],
-			dev->dev_addr[3], dev->dev_addr[4], dev->dev_addr[5]);
+	airo_print_info(dev->name, "MAC enabled %s",
+			print_mac(mac, dev->dev_addr));
 	/* Allocate the transmit buffers if needed */
 	if (!test_bit(FLAG_MPI,&ai->flags))
 		for( i = 0; i < MAX_FIDS; i++ )
@@ -5427,6 +5425,7 @@ static int proc_APList_open( struct inode *inode, struct file *file ) {
 	int i;
 	char *ptr;
 	APListRid APList_rid;
+	DECLARE_MAC_BUF(mac);
 
 	if ((file->private_data = kzalloc(sizeof(struct proc_data ), GFP_KERNEL)) == NULL)
 		return -ENOMEM;
@@ -5450,13 +5449,8 @@ static int proc_APList_open( struct inode *inode, struct file *file ) {
 // We end when we find a zero MAC
 		if ( !*(int*)APList_rid.ap[i] &&
 		     !*(int*)&APList_rid.ap[i][2]) break;
-		ptr += sprintf(ptr, "%02x:%02x:%02x:%02x:%02x:%02x\n",
-			       (int)APList_rid.ap[i][0],
-			       (int)APList_rid.ap[i][1],
-			       (int)APList_rid.ap[i][2],
-			       (int)APList_rid.ap[i][3],
-			       (int)APList_rid.ap[i][4],
-			       (int)APList_rid.ap[i][5]);
+		ptr += sprintf(ptr, "%s\n",
+			       print_mac(mac, APList_rid.ap[i]));
 	}
 	if (i==0) ptr += sprintf(ptr, "Not using specific APs\n");
 
@@ -5475,6 +5469,7 @@ static int proc_BSSList_open( struct inode *inode, struct file *file ) {
 	int rc;
 	/* If doLoseSync is not 1, we won't do a Lose Sync */
 	int doLoseSync = -1;
+	DECLARE_MAC_BUF(mac);
 
 	if ((file->private_data = kzalloc(sizeof(struct proc_data ), GFP_KERNEL)) == NULL)
 		return -ENOMEM;
@@ -5511,13 +5506,8 @@ static int proc_BSSList_open( struct inode *inode, struct file *file ) {
            we have to add a spin lock... */
 	rc = readBSSListRid(ai, doLoseSync, &BSSList_rid);
 	while(rc == 0 && BSSList_rid.index != 0xffff) {
-		ptr += sprintf(ptr, "%02x:%02x:%02x:%02x:%02x:%02x %*s rssi = %d",
-				(int)BSSList_rid.bssid[0],
-				(int)BSSList_rid.bssid[1],
-				(int)BSSList_rid.bssid[2],
-				(int)BSSList_rid.bssid[3],
-				(int)BSSList_rid.bssid[4],
-				(int)BSSList_rid.bssid[5],
+		ptr += sprintf(ptr, "%s %*s rssi = %d",
+			       print_mac(mac, BSSList_rid.bssid),
 				(int)BSSList_rid.ssidLen,
 				BSSList_rid.ssid,
 				(int)BSSList_rid.dBm);
@@ -7579,9 +7569,9 @@ static const iw_handler		airo_private_handler[] =
 
 static const struct iw_handler_def	airo_handler_def =
 {
-	.num_standard	= sizeof(airo_handler)/sizeof(iw_handler),
-	.num_private	= sizeof(airo_private_handler)/sizeof(iw_handler),
-	.num_private_args = sizeof(airo_private_args)/sizeof(struct iw_priv_args),
+	.num_standard	= ARRAY_SIZE(airo_handler),
+	.num_private	= ARRAY_SIZE(airo_private_handler),
+	.num_private_args = ARRAY_SIZE(airo_private_args),
 	.standard	= airo_handler,
 	.private	= airo_private_handler,
 	.private_args	= airo_private_args,
diff --git a/drivers/net/wireless/airport.c b/drivers/net/wireless/airport.c
index 7d5b8c2cc614..6f7eb9f59223 100644
--- a/drivers/net/wireless/airport.c
+++ b/drivers/net/wireless/airport.c
@@ -197,7 +197,6 @@ airport_attach(struct macio_dev *mdev, const struct of_device_id *match)
 		return -EBUSY;
 	}
 
-	SET_MODULE_OWNER(dev);
 	SET_NETDEV_DEV(dev, &mdev->ofdev.dev);
 
 	macio_set_drvdata(mdev, dev);
diff --git a/drivers/net/wireless/arlan-main.c b/drivers/net/wireless/arlan-main.c
index 498e8486d125..dbdfc9e39d20 100644
--- a/drivers/net/wireless/arlan-main.c
+++ b/drivers/net/wireless/arlan-main.c
@@ -1469,10 +1469,10 @@ static void arlan_rx_interrupt(struct net_device *dev, u_char rxStatus, u_short
 					while (dmi)
 					{							if (dmi->dmi_addrlen == 6)
 						{
+							DECLARE_MAC_BUF(mac);
 							if (arlan_debug & ARLAN_DEBUG_HEADER_DUMP)
-								printk(KERN_ERR "%s mcl %2x:%2x:%2x:%2x:%2x:%2x \n", dev->name,
-										 dmi->dmi_addr[0], dmi->dmi_addr[1], dmi->dmi_addr[2],
-										 dmi->dmi_addr[3], dmi->dmi_addr[4], dmi->dmi_addr[5]);
+								printk(KERN_ERR "%s mcl %s\n",
+								       dev->name, print_mac(mac, dmi->dmi_addr));
 							for (i = 0; i < 6; i++)
 								if (dmi->dmi_addr[i] != hw_dst_addr[i])
 									break;
@@ -1512,17 +1512,18 @@ static void arlan_rx_interrupt(struct net_device *dev, u_char rxStatus, u_short
 			{
 				char immedDestAddress[6];
 				char immedSrcAddress[6];
+				DECLARE_MAC_BUF(mac);
+				DECLARE_MAC_BUF(mac2);
+				DECLARE_MAC_BUF(mac3);
+				DECLARE_MAC_BUF(mac4);
 				memcpy_fromio(immedDestAddress, arlan->immedDestAddress, 6);
 				memcpy_fromio(immedSrcAddress, arlan->immedSrcAddress, 6);
 
-				printk(KERN_WARNING "%s t %2x:%2x:%2x:%2x:%2x:%2x f %2x:%2x:%2x:%2x:%2x:%2x imd %2x:%2x:%2x:%2x:%2x:%2x ims %2x:%2x:%2x:%2x:%2x:%2x\n", dev->name,
-					(unsigned char) skbtmp[0], (unsigned char) skbtmp[1], (unsigned char) skbtmp[2], (unsigned char) skbtmp[3],
-					(unsigned char) skbtmp[4], (unsigned char) skbtmp[5], (unsigned char) skbtmp[6], (unsigned char) skbtmp[7],
-					(unsigned char) skbtmp[8], (unsigned char) skbtmp[9], (unsigned char) skbtmp[10], (unsigned char) skbtmp[11],
-					immedDestAddress[0], immedDestAddress[1], immedDestAddress[2],
-					immedDestAddress[3], immedDestAddress[4], immedDestAddress[5],
-					immedSrcAddress[0], immedSrcAddress[1], immedSrcAddress[2],
-					immedSrcAddress[3], immedSrcAddress[4], immedSrcAddress[5]);
+				printk(KERN_WARNING "%s t %s f %s imd %s ims %s\n",
+				       dev->name, print_mac(mac, skbtmp),
+				       print_mac(mac2, &skbtmp[6]),
+				       print_mac(mac3, immedDestAddress),
+				       print_mac(mac4, immedSrcAddress));
 			}
 			skb->protocol = eth_type_trans(skb, dev);
 			IFDEBUG(ARLAN_DEBUG_HEADER_DUMP)
@@ -1792,8 +1793,6 @@ struct net_device * __init arlan_probe(int unit)
 	if (!dev)
 		return ERR_PTR(-ENOMEM);
 
-	SET_MODULE_OWNER(dev);
-
 	if (unit >= 0) {
 		sprintf(dev->name, "eth%d", unit);
 		netdev_boot_setup_check(dev);
diff --git a/drivers/net/wireless/arlan-proc.c b/drivers/net/wireless/arlan-proc.c
index 015abd928ab0..c6e70dbc5de8 100644
--- a/drivers/net/wireless/arlan-proc.c
+++ b/drivers/net/wireless/arlan-proc.c
@@ -435,7 +435,7 @@ static int arlan_sysctl_info(ctl_table * ctl, int write, struct file *filp,
 		goto final;
 	}
 	else
-		priva = arlan_device[devnum]->priv;
+		priva = netdev_priv(arlan_device[devnum]);
 
 	if (priva == NULL)
 	{
@@ -654,7 +654,7 @@ static int arlan_sysctl_info161719(ctl_table * ctl, int write, struct file *filp
 		goto final;
 	}
 	else
-		priva = arlan_device[devnum]->priv;
+		priva = netdev_priv(arlan_device[devnum]);
 	if (priva == NULL)
 	{
 		printk(KERN_WARNING " Could not find the device private in arlan procsys, bad\n ");
@@ -688,7 +688,7 @@ static int arlan_sysctl_infotxRing(ctl_table * ctl, int write, struct file *filp
 		  goto final;
 	}
 	else
-		priva = arlan_device[devnum]->priv;
+		priva = netdev_priv(arlan_device[devnum]);
 	if (priva == NULL)
 	{
 		printk(KERN_WARNING " Could not find the device private in arlan procsys, bad\n ");
@@ -716,7 +716,7 @@ static int arlan_sysctl_inforxRing(ctl_table * ctl, int write, struct file *filp
 		  pos += sprintf(arlan_drive_info + pos, "No device found here \n");
 		  goto final;
 	} else
-		priva = arlan_device[devnum]->priv;
+		priva = netdev_priv(arlan_device[devnum]);
 	if (priva == NULL)
 	{
 		printk(KERN_WARNING " Could not find the device private in arlan procsys, bad\n ");
@@ -745,7 +745,7 @@ static int arlan_sysctl_info18(ctl_table * ctl, int write, struct file *filp,
 		goto final;
 	}
 	else
-		priva = arlan_device[devnum]->priv;
+		priva = netdev_priv(arlan_device[devnum]);
 	if (priva == NULL)
 	{
 		printk(KERN_WARNING " Could not find the device private in arlan procsys, bad\n ");
@@ -780,7 +780,7 @@ static int arlan_configure(ctl_table * ctl, int write, struct file *filp,
 	}
 	else if (arlan_device[devnum] != NULL)
 	{
-		  priv = arlan_device[devnum]->priv;
+		  priv = netdev_priv(arlan_device[devnum]);
 
 		  arlan_command(arlan_device[devnum], ARLAN_COMMAND_CLEAN_AND_CONF);
 	}
@@ -805,7 +805,7 @@ static int arlan_sysctl_reset(ctl_table * ctl, int write, struct file *filp,
 	}
 	else if (arlan_device[devnum] != NULL)
 	{
-		priv = arlan_device[devnum]->priv;
+		priv = netdev_priv(arlan_device[devnum]);
 		arlan_command(arlan_device[devnum], ARLAN_COMMAND_CLEAN_AND_RESET);
 
 	} else
diff --git a/drivers/net/wireless/atmel.c b/drivers/net/wireless/atmel.c
index 51a7db53afa5..059ce3f07dba 100644
--- a/drivers/net/wireless/atmel.c
+++ b/drivers/net/wireless/atmel.c
@@ -1484,6 +1484,7 @@ struct net_device *init_atmel_card(unsigned short irq, unsigned long port,
 	struct net_device *dev;
 	struct atmel_private *priv;
 	int rc;
+	DECLARE_MAC_BUF(mac);
 
 	/* Create the network device object. */
         dev = alloc_etherdev(sizeof(*priv));
@@ -1598,12 +1599,9 @@ struct net_device *init_atmel_card(unsigned short irq, unsigned long port,
 	if (!ent)
 		printk(KERN_WARNING "atmel: unable to create /proc entry.\n");
 
-	printk(KERN_INFO "%s: Atmel at76c50x. Version %d.%d. MAC %.2x:%.2x:%.2x:%.2x:%.2x:%.2x\n",
-	       dev->name, DRIVER_MAJOR, DRIVER_MINOR,
-	       dev->dev_addr[0], dev->dev_addr[1], dev->dev_addr[2],
-	       dev->dev_addr[3], dev->dev_addr[4], dev->dev_addr[5] );
+	printk(KERN_INFO "%s: Atmel at76c50x. Version %d.%d. MAC %s\n",
+	       dev->name, DRIVER_MAJOR, DRIVER_MINOR, print_mac(mac, dev->dev_addr));
 
-	SET_MODULE_OWNER(dev);
 	return dev;
 
 err_out_res:
diff --git a/drivers/net/wireless/b43/Kconfig b/drivers/net/wireless/b43/Kconfig
new file mode 100644
index 000000000000..e3c573e56b63
--- /dev/null
+++ b/drivers/net/wireless/b43/Kconfig
@@ -0,0 +1,131 @@
+config B43
+	tristate "Broadcom 43xx wireless support (mac80211 stack)"
+	depends on SSB_POSSIBLE && MAC80211 && WLAN_80211
+	select SSB
+	select FW_LOADER
+	select HW_RANDOM
+	---help---
+	  b43 is a driver for the Broadcom 43xx series wireless devices.
+
+	  Check "lspci" for something like
+	  "Broadcom Corporation BCM43XX 802.11 Wireless LAN Controller"
+	  to determine whether you own such a device.
+
+	  This driver supports the new BCM43xx IEEE 802.11G devices, but not
+	  the old IEEE 802.11B devices. Old devices are supported by
+	  the b43legacy driver.
+	  Note that this has nothing to do with the standard that your AccessPoint
+	  supports (A, B, G or a combination).
+	  IEEE 802.11G devices can talk to IEEE 802.11B AccessPoints.
+
+	  It is safe to include both b43 and b43legacy as the underlying glue
+	  layer will automatically load the correct version for your device.
+
+	  This driver uses V4 firmware, which must be installed separately using
+	  b43-fwcutter.
+
+	  This driver can be built as a module (recommended) that will be called "b43".
+	  If unsure, say M.
+
+# Auto-select SSB PCI-HOST support, if possible
+config B43_PCI_AUTOSELECT
+	bool
+	depends on B43 && SSB_PCIHOST_POSSIBLE
+	select SSB_PCIHOST
+	default y
+
+# Auto-select SSB PCICORE driver, if possible
+config B43_PCICORE_AUTOSELECT
+	bool
+	depends on B43 && SSB_DRIVER_PCICORE_POSSIBLE
+	select SSB_DRIVER_PCICORE
+	default y
+
+config B43_PCMCIA
+	bool "Broadcom 43xx PCMCIA device support (EXPERIMENTAL)"
+	depends on B43 && SSB_PCMCIAHOST_POSSIBLE && EXPERIMENTAL
+	select SSB_PCMCIAHOST
+	---help---
+	  Broadcom 43xx PCMCIA device support.
+
+	  Support for 16bit PCMCIA devices.
+	  Please note that most PC-CARD devices are _NOT_ 16bit PCMCIA
+	  devices, but 32bit CardBUS devices. CardBUS devices are supported
+	  out of the box by b43.
+
+	  With this config option you can drive b43 cards in
+	  CompactFlash formfactor in a PCMCIA adaptor.
+	  CF b43 cards can sometimes be found in handheld PCs.
+
+	  It's safe to select Y here, even if you don't have a B43 PCMCIA device.
+
+	  If unsure, say N.
+
+# LED support
+config B43_LEDS
+	bool
+	depends on B43 && MAC80211_LEDS
+	default y
+
+# RFKILL support
+config B43_RFKILL
+	bool
+	depends on B43 && RFKILL && RFKILL_INPUT && INPUT_POLLDEV
+	default y
+
+config B43_DEBUG
+	bool "Broadcom 43xx debugging"
+	depends on B43
+	---help---
+	  Broadcom 43xx debugging messages.
+
+	  Say Y, if you want to find out why the driver does not
+	  work for you.
+
+config B43_DMA
+	bool
+	depends on B43
+config B43_PIO
+	bool
+	depends on B43
+
+choice
+	prompt "Broadcom 43xx data transfer mode"
+	depends on B43
+	default B43_DMA_AND_PIO_MODE
+
+config B43_DMA_AND_PIO_MODE
+	bool "DMA + PIO"
+	select B43_DMA
+	select B43_PIO
+	---help---
+	  Include both, Direct Memory Access (DMA) and Programmed I/O (PIO)
+	  data transfer modes.
+	  The actually used mode is selectable through the module
+	  parameter "pio". If the module parameter is pio=0, DMA is used.
+	  Otherwise PIO is used. DMA is default.
+
+	  If unsure, choose this option.
+
+config B43_DMA_MODE
+	bool "DMA (Direct Memory Access) only"
+	select B43_DMA
+	---help---
+	  Only include Direct Memory Access (DMA).
+	  This reduces the size of the driver module, by omitting the PIO code.
+
+config B43_PIO_MODE
+	bool "PIO (Programmed I/O) only"
+	select B43_PIO
+	---help---
+	  Only include Programmed I/O (PIO).
+	  This reduces the size of the driver module, by omitting the DMA code.
+	  Please note that PIO transfers are slow (compared to DMA).
+
+	  Also note that not all devices of the 43xx series support PIO.
+	  The 4306 (Apple Airport Extreme and others) supports PIO, while
+	  the 4318 is known to _not_ support PIO.
+
+	  Only use PIO, if DMA does not work for you.
+
+endchoice
diff --git a/drivers/net/wireless/b43/Makefile b/drivers/net/wireless/b43/Makefile
new file mode 100644
index 000000000000..485e59e2dfab
--- /dev/null
+++ b/drivers/net/wireless/b43/Makefile
@@ -0,0 +1,20 @@
+# b43 core
+b43-y				+= main.o
+b43-y				+= tables.o
+b43-y				+= phy.o
+b43-y				+= sysfs.o
+b43-y				+= xmit.o
+b43-y				+= lo.o
+# b43 RFKILL button support
+b43-$(CONFIG_B43_RFKILL)	+= rfkill.o
+# b43 LED support
+b43-$(CONFIG_B43_LEDS)		+= leds.o
+# b43 PCMCIA support
+b43-$(CONFIG_B43_PCMCIA)	+= pcmcia.o
+# b43 debugging
+b43-$(CONFIG_B43_DEBUG)		+= debugfs.o
+# b43 DMA and PIO
+b43-$(CONFIG_B43_DMA)		+= dma.o
+b43-$(CONFIG_B43_PIO)		+= pio.o
+
+obj-$(CONFIG_B43)		+= b43.o
diff --git a/drivers/net/wireless/b43/b43.h b/drivers/net/wireless/b43/b43.h
new file mode 100644
index 000000000000..a28ad230d63e
--- /dev/null
+++ b/drivers/net/wireless/b43/b43.h
@@ -0,0 +1,854 @@
+#ifndef B43_H_
+#define B43_H_
+
+#include <linux/kernel.h>
+#include <linux/spinlock.h>
+#include <linux/interrupt.h>
+#include <linux/hw_random.h>
+#include <linux/ssb/ssb.h>
+#include <net/mac80211.h>
+
+#include "debugfs.h"
+#include "leds.h"
+#include "rfkill.h"
+#include "lo.h"
+#include "phy.h"
+
+#ifdef CONFIG_B43_DEBUG
+# define B43_DEBUG	1
+#else
+# define B43_DEBUG	0
+#endif
+
+#define B43_RX_MAX_SSI			60
+
+/* MMIO offsets */
+#define B43_MMIO_DMA0_REASON		0x20
+#define B43_MMIO_DMA0_IRQ_MASK		0x24
+#define B43_MMIO_DMA1_REASON		0x28
+#define B43_MMIO_DMA1_IRQ_MASK		0x2C
+#define B43_MMIO_DMA2_REASON		0x30
+#define B43_MMIO_DMA2_IRQ_MASK		0x34
+#define B43_MMIO_DMA3_REASON		0x38
+#define B43_MMIO_DMA3_IRQ_MASK		0x3C
+#define B43_MMIO_DMA4_REASON		0x40
+#define B43_MMIO_DMA4_IRQ_MASK		0x44
+#define B43_MMIO_DMA5_REASON		0x48
+#define B43_MMIO_DMA5_IRQ_MASK		0x4C
+#define B43_MMIO_MACCTL			0x120
+#define B43_MMIO_STATUS2_BITFIELD	0x124
+#define B43_MMIO_GEN_IRQ_REASON		0x128
+#define B43_MMIO_GEN_IRQ_MASK		0x12C
+#define B43_MMIO_RAM_CONTROL		0x130
+#define B43_MMIO_RAM_DATA		0x134
+#define B43_MMIO_PS_STATUS		0x140
+#define B43_MMIO_RADIO_HWENABLED_HI	0x158
+#define B43_MMIO_SHM_CONTROL		0x160
+#define B43_MMIO_SHM_DATA		0x164
+#define B43_MMIO_SHM_DATA_UNALIGNED	0x166
+#define B43_MMIO_XMITSTAT_0		0x170
+#define B43_MMIO_XMITSTAT_1		0x174
+#define B43_MMIO_REV3PLUS_TSF_LOW	0x180	/* core rev >= 3 only */
+#define B43_MMIO_REV3PLUS_TSF_HIGH	0x184	/* core rev >= 3 only */
+
+/* 32-bit DMA */
+#define B43_MMIO_DMA32_BASE0		0x200
+#define B43_MMIO_DMA32_BASE1		0x220
+#define B43_MMIO_DMA32_BASE2		0x240
+#define B43_MMIO_DMA32_BASE3		0x260
+#define B43_MMIO_DMA32_BASE4		0x280
+#define B43_MMIO_DMA32_BASE5		0x2A0
+/* 64-bit DMA */
+#define B43_MMIO_DMA64_BASE0		0x200
+#define B43_MMIO_DMA64_BASE1		0x240
+#define B43_MMIO_DMA64_BASE2		0x280
+#define B43_MMIO_DMA64_BASE3		0x2C0
+#define B43_MMIO_DMA64_BASE4		0x300
+#define B43_MMIO_DMA64_BASE5		0x340
+/* PIO */
+#define B43_MMIO_PIO1_BASE		0x300
+#define B43_MMIO_PIO2_BASE		0x310
+#define B43_MMIO_PIO3_BASE		0x320
+#define B43_MMIO_PIO4_BASE		0x330
+
+#define B43_MMIO_PHY_VER		0x3E0
+#define B43_MMIO_PHY_RADIO		0x3E2
+#define B43_MMIO_PHY0			0x3E6
+#define B43_MMIO_ANTENNA		0x3E8
+#define B43_MMIO_CHANNEL		0x3F0
+#define B43_MMIO_CHANNEL_EXT		0x3F4
+#define B43_MMIO_RADIO_CONTROL		0x3F6
+#define B43_MMIO_RADIO_DATA_HIGH	0x3F8
+#define B43_MMIO_RADIO_DATA_LOW		0x3FA
+#define B43_MMIO_PHY_CONTROL		0x3FC
+#define B43_MMIO_PHY_DATA		0x3FE
+#define B43_MMIO_MACFILTER_CONTROL	0x420
+#define B43_MMIO_MACFILTER_DATA		0x422
+#define B43_MMIO_RCMTA_COUNT		0x43C
+#define B43_MMIO_RADIO_HWENABLED_LO	0x49A
+#define B43_MMIO_GPIO_CONTROL		0x49C
+#define B43_MMIO_GPIO_MASK		0x49E
+#define B43_MMIO_TSF_0			0x632	/* core rev < 3 only */
+#define B43_MMIO_TSF_1			0x634	/* core rev < 3 only */
+#define B43_MMIO_TSF_2			0x636	/* core rev < 3 only */
+#define B43_MMIO_TSF_3			0x638	/* core rev < 3 only */
+#define B43_MMIO_RNG			0x65A
+#define B43_MMIO_POWERUP_DELAY		0x6A8
+
+/* SPROM boardflags_lo values */
+#define B43_BFL_BTCOEXIST		0x0001	/* implements Bluetooth coexistance */
+#define B43_BFL_PACTRL			0x0002	/* GPIO 9 controlling the PA */
+#define B43_BFL_AIRLINEMODE		0x0004	/* implements GPIO 13 radio disable indication */
+#define B43_BFL_RSSI			0x0008	/* software calculates nrssi slope. */
+#define B43_BFL_ENETSPI			0x0010	/* has ephy roboswitch spi */
+#define B43_BFL_XTAL_NOSLOW		0x0020	/* no slow clock available */
+#define B43_BFL_CCKHIPWR		0x0040	/* can do high power CCK transmission */
+#define B43_BFL_ENETADM			0x0080	/* has ADMtek switch */
+#define B43_BFL_ENETVLAN		0x0100	/* can do vlan */
+#define B43_BFL_AFTERBURNER		0x0200	/* supports Afterburner mode */
+#define B43_BFL_NOPCI			0x0400	/* leaves PCI floating */
+#define B43_BFL_FEM			0x0800	/* supports the Front End Module */
+#define B43_BFL_EXTLNA			0x1000	/* has an external LNA */
+#define B43_BFL_HGPA			0x2000	/* had high gain PA */
+#define B43_BFL_BTCMOD			0x4000	/* BFL_BTCOEXIST is given in alternate GPIOs */
+#define B43_BFL_ALTIQ			0x8000	/* alternate I/Q settings */
+
+/* GPIO register offset, in both ChipCommon and PCI core. */
+#define B43_GPIO_CONTROL		0x6c
+
+/* SHM Routing */
+enum {
+	B43_SHM_UCODE,		/* Microcode memory */
+	B43_SHM_SHARED,		/* Shared memory */
+	B43_SHM_SCRATCH,	/* Scratch memory */
+	B43_SHM_HW,		/* Internal hardware register */
+	B43_SHM_RCMTA,		/* Receive match transmitter address (rev >= 5 only) */
+};
+/* SHM Routing modifiers */
+#define B43_SHM_AUTOINC_R		0x0200	/* Auto-increment address on read */
+#define B43_SHM_AUTOINC_W		0x0100	/* Auto-increment address on write */
+#define B43_SHM_AUTOINC_RW		(B43_SHM_AUTOINC_R | \
+					 B43_SHM_AUTOINC_W)
+
+/* Misc SHM_SHARED offsets */
+#define B43_SHM_SH_WLCOREREV		0x0016	/* 802.11 core revision */
+#define B43_SHM_SH_PCTLWDPOS		0x0008
+#define B43_SHM_SH_RXPADOFF		0x0034	/* RX Padding data offset (PIO only) */
+#define B43_SHM_SH_PHYVER		0x0050	/* PHY version */
+#define B43_SHM_SH_PHYTYPE		0x0052	/* PHY type */
+#define B43_SHM_SH_ANTSWAP		0x005C	/* Antenna swap threshold */
+#define B43_SHM_SH_HOSTFLO		0x005E	/* Hostflags for ucode options (low) */
+#define B43_SHM_SH_HOSTFHI		0x0060	/* Hostflags for ucode options (high) */
+#define B43_SHM_SH_RFATT		0x0064	/* Current radio attenuation value */
+#define B43_SHM_SH_RADAR		0x0066	/* Radar register */
+#define B43_SHM_SH_PHYTXNOI		0x006E	/* PHY noise directly after TX (lower 8bit only) */
+#define B43_SHM_SH_RFRXSP1		0x0072	/* RF RX SP Register 1 */
+#define B43_SHM_SH_CHAN			0x00A0	/* Current channel (low 8bit only) */
+#define  B43_SHM_SH_CHAN_5GHZ		0x0100	/* Bit set, if 5Ghz channel */
+#define B43_SHM_SH_BCMCFIFOID		0x0108	/* Last posted cookie to the bcast/mcast FIFO */
+/* SHM_SHARED TX FIFO variables */
+#define B43_SHM_SH_SIZE01		0x0098	/* TX FIFO size for FIFO 0 (low) and 1 (high) */
+#define B43_SHM_SH_SIZE23		0x009A	/* TX FIFO size for FIFO 2 and 3 */
+#define B43_SHM_SH_SIZE45		0x009C	/* TX FIFO size for FIFO 4 and 5 */
+#define B43_SHM_SH_SIZE67		0x009E	/* TX FIFO size for FIFO 6 and 7 */
+/* SHM_SHARED background noise */
+#define B43_SHM_SH_JSSI0		0x0088	/* Measure JSSI 0 */
+#define B43_SHM_SH_JSSI1		0x008A	/* Measure JSSI 1 */
+#define B43_SHM_SH_JSSIAUX		0x008C	/* Measure JSSI AUX */
+/* SHM_SHARED crypto engine */
+#define B43_SHM_SH_DEFAULTIV		0x003C	/* Default IV location */
+#define B43_SHM_SH_NRRXTRANS		0x003E	/* # of soft RX transmitter addresses (max 8) */
+#define B43_SHM_SH_KTP			0x0056	/* Key table pointer */
+#define B43_SHM_SH_TKIPTSCTTAK		0x0318
+#define B43_SHM_SH_KEYIDXBLOCK		0x05D4	/* Key index/algorithm block (v4 firmware) */
+#define B43_SHM_SH_PSM			0x05F4	/* PSM transmitter address match block (rev < 5) */
+/* SHM_SHARED WME variables */
+#define B43_SHM_SH_EDCFSTAT		0x000E	/* EDCF status */
+#define B43_SHM_SH_TXFCUR		0x0030	/* TXF current index */
+#define B43_SHM_SH_EDCFQ		0x0240	/* EDCF Q info */
+/* SHM_SHARED powersave mode related */
+#define B43_SHM_SH_SLOTT		0x0010	/* Slot time */
+#define B43_SHM_SH_DTIMPER		0x0012	/* DTIM period */
+#define B43_SHM_SH_NOSLPZNATDTIM	0x004C	/* NOSLPZNAT DTIM */
+/* SHM_SHARED beacon variables */
+#define B43_SHM_SH_BTL0			0x0018	/* Beacon template length 0 */
+#define B43_SHM_SH_BTL1			0x001A	/* Beacon template length 1 */
+#define B43_SHM_SH_BTSFOFF		0x001C	/* Beacon TSF offset */
+#define B43_SHM_SH_TIMBPOS		0x001E	/* TIM B position in beacon */
+#define B43_SHM_SH_SFFBLIM		0x0044	/* Short frame fallback retry limit */
+#define B43_SHM_SH_LFFBLIM		0x0046	/* Long frame fallback retry limit */
+#define B43_SHM_SH_BEACPHYCTL		0x0054	/* Beacon PHY TX control word (see PHY TX control) */
+/* SHM_SHARED ACK/CTS control */
+#define B43_SHM_SH_ACKCTSPHYCTL		0x0022	/* ACK/CTS PHY control word (see PHY TX control) */
+/* SHM_SHARED probe response variables */
+#define B43_SHM_SH_PRSSID		0x0160	/* Probe Response SSID */
+#define B43_SHM_SH_PRSSIDLEN		0x0048	/* Probe Response SSID length */
+#define B43_SHM_SH_PRTLEN		0x004A	/* Probe Response template length */
+#define B43_SHM_SH_PRMAXTIME		0x0074	/* Probe Response max time */
+#define B43_SHM_SH_PRPHYCTL		0x0188	/* Probe Response PHY TX control word */
+/* SHM_SHARED rate tables */
+#define B43_SHM_SH_OFDMDIRECT		0x01C0	/* Pointer to OFDM direct map */
+#define B43_SHM_SH_OFDMBASIC		0x01E0	/* Pointer to OFDM basic rate map */
+#define B43_SHM_SH_CCKDIRECT		0x0200	/* Pointer to CCK direct map */
+#define B43_SHM_SH_CCKBASIC		0x0220	/* Pointer to CCK basic rate map */
+/* SHM_SHARED microcode soft registers */
+#define B43_SHM_SH_UCODEREV		0x0000	/* Microcode revision */
+#define B43_SHM_SH_UCODEPATCH		0x0002	/* Microcode patchlevel */
+#define B43_SHM_SH_UCODEDATE		0x0004	/* Microcode date */
+#define B43_SHM_SH_UCODETIME		0x0006	/* Microcode time */
+#define B43_SHM_SH_UCODESTAT		0x0040	/* Microcode debug status code */
+#define  B43_SHM_SH_UCODESTAT_INVALID	0
+#define  B43_SHM_SH_UCODESTAT_INIT	1
+#define  B43_SHM_SH_UCODESTAT_ACTIVE	2
+#define  B43_SHM_SH_UCODESTAT_SUSP	3	/* suspended */
+#define  B43_SHM_SH_UCODESTAT_SLEEP	4	/* asleep (PS) */
+#define B43_SHM_SH_MAXBFRAMES		0x0080	/* Maximum number of frames in a burst */
+#define B43_SHM_SH_SPUWKUP		0x0094	/* pre-wakeup for synth PU in us */
+#define B43_SHM_SH_PRETBTT		0x0096	/* pre-TBTT in us */
+
+/* SHM_SCRATCH offsets */
+#define B43_SHM_SC_MINCONT		0x0003	/* Minimum contention window */
+#define B43_SHM_SC_MAXCONT		0x0004	/* Maximum contention window */
+#define B43_SHM_SC_CURCONT		0x0005	/* Current contention window */
+#define B43_SHM_SC_SRLIMIT		0x0006	/* Short retry count limit */
+#define B43_SHM_SC_LRLIMIT		0x0007	/* Long retry count limit */
+#define B43_SHM_SC_DTIMC		0x0008	/* Current DTIM count */
+#define B43_SHM_SC_BTL0LEN		0x0015	/* Beacon 0 template length */
+#define B43_SHM_SC_BTL1LEN		0x0016	/* Beacon 1 template length */
+#define B43_SHM_SC_SCFB			0x0017	/* Short frame transmit count threshold for rate fallback */
+#define B43_SHM_SC_LCFB			0x0018	/* Long frame transmit count threshold for rate fallback */
+
+/* Hardware Radio Enable masks */
+#define B43_MMIO_RADIO_HWENABLED_HI_MASK (1 << 16)
+#define B43_MMIO_RADIO_HWENABLED_LO_MASK (1 << 4)
+
+/* HostFlags. See b43_hf_read/write() */
+#define B43_HF_ANTDIVHELP		0x00000001	/* ucode antenna div helper */
+#define B43_HF_SYMW			0x00000002	/* G-PHY SYM workaround */
+#define B43_HF_RXPULLW			0x00000004	/* RX pullup workaround */
+#define B43_HF_CCKBOOST			0x00000008	/* 4dB CCK power boost (exclusive with OFDM boost) */
+#define B43_HF_BTCOEX			0x00000010	/* Bluetooth coexistance */
+#define B43_HF_GDCW			0x00000020	/* G-PHY DV canceller filter bw workaround */
+#define B43_HF_OFDMPABOOST		0x00000040	/* Enable PA gain boost for OFDM */
+#define B43_HF_ACPR			0x00000080	/* Disable for Japan, channel 14 */
+#define B43_HF_EDCF			0x00000100	/* on if WME and MAC suspended */
+#define B43_HF_TSSIRPSMW		0x00000200	/* TSSI reset PSM ucode workaround */
+#define B43_HF_DSCRQ			0x00000400	/* Disable slow clock request in ucode */
+#define B43_HF_ACIW			0x00000800	/* ACI workaround: shift bits by 2 on PHY CRS */
+#define B43_HF_2060W			0x00001000	/* 2060 radio workaround */
+#define B43_HF_RADARW			0x00002000	/* Radar workaround */
+#define B43_HF_USEDEFKEYS		0x00004000	/* Enable use of default keys */
+#define B43_HF_BT4PRIOCOEX		0x00010000	/* Bluetooth 2-priority coexistance */
+#define B43_HF_FWKUP			0x00020000	/* Fast wake-up ucode */
+#define B43_HF_VCORECALC		0x00040000	/* Force VCO recalculation when powering up synthpu */
+#define B43_HF_PCISCW			0x00080000	/* PCI slow clock workaround */
+#define B43_HF_4318TSSI			0x00200000	/* 4318 TSSI */
+#define B43_HF_FBCMCFIFO		0x00400000	/* Flush bcast/mcast FIFO immediately */
+#define B43_HF_HWPCTL			0x00800000	/* Enable hardwarre power control */
+#define B43_HF_BTCOEXALT		0x01000000	/* Bluetooth coexistance in alternate pins */
+#define B43_HF_TXBTCHECK		0x02000000	/* Bluetooth check during transmission */
+#define B43_HF_SKCFPUP			0x04000000	/* Skip CFP update */
+
+/* MacFilter offsets. */
+#define B43_MACFILTER_SELF		0x0000
+#define B43_MACFILTER_BSSID		0x0003
+
+/* PowerControl */
+#define B43_PCTL_IN			0xB0
+#define B43_PCTL_OUT			0xB4
+#define B43_PCTL_OUTENABLE		0xB8
+#define B43_PCTL_XTAL_POWERUP		0x40
+#define B43_PCTL_PLL_POWERDOWN		0x80
+
+/* PowerControl Clock Modes */
+#define B43_PCTL_CLK_FAST		0x00
+#define B43_PCTL_CLK_SLOW		0x01
+#define B43_PCTL_CLK_DYNAMIC		0x02
+
+#define B43_PCTL_FORCE_SLOW		0x0800
+#define B43_PCTL_FORCE_PLL		0x1000
+#define B43_PCTL_DYN_XTAL		0x2000
+
+/* PHYVersioning */
+#define B43_PHYTYPE_A			0x00
+#define B43_PHYTYPE_B			0x01
+#define B43_PHYTYPE_G			0x02
+
+/* PHYRegisters */
+#define B43_PHY_ILT_A_CTRL		0x0072
+#define B43_PHY_ILT_A_DATA1		0x0073
+#define B43_PHY_ILT_A_DATA2		0x0074
+#define B43_PHY_G_LO_CONTROL		0x0810
+#define B43_PHY_ILT_G_CTRL		0x0472
+#define B43_PHY_ILT_G_DATA1		0x0473
+#define B43_PHY_ILT_G_DATA2		0x0474
+#define B43_PHY_A_PCTL			0x007B
+#define B43_PHY_G_PCTL			0x0029
+#define B43_PHY_A_CRS			0x0029
+#define B43_PHY_RADIO_BITFIELD		0x0401
+#define B43_PHY_G_CRS			0x0429
+#define B43_PHY_NRSSILT_CTRL		0x0803
+#define B43_PHY_NRSSILT_DATA		0x0804
+
+/* RadioRegisters */
+#define B43_RADIOCTL_ID			0x01
+
+/* MAC Control bitfield */
+#define B43_MACCTL_ENABLED		0x00000001	/* MAC Enabled */
+#define B43_MACCTL_PSM_RUN		0x00000002	/* Run Microcode */
+#define B43_MACCTL_PSM_JMP0		0x00000004	/* Microcode jump to 0 */
+#define B43_MACCTL_SHM_ENABLED		0x00000100	/* SHM Enabled */
+#define B43_MACCTL_SHM_UPPER		0x00000200	/* SHM Upper */
+#define B43_MACCTL_IHR_ENABLED		0x00000400	/* IHR Region Enabled */
+#define B43_MACCTL_PSM_DBG		0x00002000	/* Microcode debugging enabled */
+#define B43_MACCTL_GPOUTSMSK		0x0000C000	/* GPOUT Select Mask */
+#define B43_MACCTL_BE			0x00010000	/* Big Endian mode */
+#define B43_MACCTL_INFRA		0x00020000	/* Infrastructure mode */
+#define B43_MACCTL_AP			0x00040000	/* AccessPoint mode */
+#define B43_MACCTL_RADIOLOCK		0x00080000	/* Radio lock */
+#define B43_MACCTL_BEACPROMISC		0x00100000	/* Beacon Promiscuous */
+#define B43_MACCTL_KEEP_BADPLCP		0x00200000	/* Keep frames with bad PLCP */
+#define B43_MACCTL_KEEP_CTL		0x00400000	/* Keep control frames */
+#define B43_MACCTL_KEEP_BAD		0x00800000	/* Keep bad frames (FCS) */
+#define B43_MACCTL_PROMISC		0x01000000	/* Promiscuous mode */
+#define B43_MACCTL_HWPS			0x02000000	/* Hardware Power Saving */
+#define B43_MACCTL_AWAKE		0x04000000	/* Device is awake */
+#define B43_MACCTL_CLOSEDNET		0x08000000	/* Closed net (no SSID bcast) */
+#define B43_MACCTL_TBTTHOLD		0x10000000	/* TBTT Hold */
+#define B43_MACCTL_DISCTXSTAT		0x20000000	/* Discard TX status */
+#define B43_MACCTL_DISCPMQ		0x40000000	/* Discard Power Management Queue */
+#define B43_MACCTL_GMODE		0x80000000	/* G Mode */
+
+/* 802.11 core specific TM State Low flags */
+#define B43_TMSLOW_GMODE		0x20000000	/* G Mode Enable */
+#define B43_TMSLOW_PLLREFSEL		0x00200000	/* PLL Frequency Reference Select */
+#define B43_TMSLOW_MACPHYCLKEN		0x00100000	/* MAC PHY Clock Control Enable (rev >= 5) */
+#define B43_TMSLOW_PHYRESET		0x00080000	/* PHY Reset */
+#define B43_TMSLOW_PHYCLKEN		0x00040000	/* PHY Clock Enable */
+
+/* 802.11 core specific TM State High flags */
+#define B43_TMSHIGH_FCLOCK		0x00040000	/* Fast Clock Available (rev >= 5) */
+#define B43_TMSHIGH_APHY		0x00020000	/* A-PHY available (rev >= 5) */
+#define B43_TMSHIGH_GPHY		0x00010000	/* G-PHY available (rev >= 5) */
+
+/* Generic-Interrupt reasons. */
+#define B43_IRQ_MAC_SUSPENDED		0x00000001
+#define B43_IRQ_BEACON			0x00000002
+#define B43_IRQ_TBTT_INDI		0x00000004
+#define B43_IRQ_BEACON_TX_OK		0x00000008
+#define B43_IRQ_BEACON_CANCEL		0x00000010
+#define B43_IRQ_ATIM_END		0x00000020
+#define B43_IRQ_PMQ			0x00000040
+#define B43_IRQ_PIO_WORKAROUND		0x00000100
+#define B43_IRQ_MAC_TXERR		0x00000200
+#define B43_IRQ_PHY_TXERR		0x00000800
+#define B43_IRQ_PMEVENT			0x00001000
+#define B43_IRQ_TIMER0			0x00002000
+#define B43_IRQ_TIMER1			0x00004000
+#define B43_IRQ_DMA			0x00008000
+#define B43_IRQ_TXFIFO_FLUSH_OK		0x00010000
+#define B43_IRQ_CCA_MEASURE_OK		0x00020000
+#define B43_IRQ_NOISESAMPLE_OK		0x00040000
+#define B43_IRQ_UCODE_DEBUG		0x08000000
+#define B43_IRQ_RFKILL			0x10000000
+#define B43_IRQ_TX_OK			0x20000000
+#define B43_IRQ_PHY_G_CHANGED		0x40000000
+#define B43_IRQ_TIMEOUT			0x80000000
+
+#define B43_IRQ_ALL			0xFFFFFFFF
+#define B43_IRQ_MASKTEMPLATE		(B43_IRQ_MAC_SUSPENDED | \
+					 B43_IRQ_BEACON | \
+					 B43_IRQ_TBTT_INDI | \
+					 B43_IRQ_ATIM_END | \
+					 B43_IRQ_PMQ | \
+					 B43_IRQ_MAC_TXERR | \
+					 B43_IRQ_PHY_TXERR | \
+					 B43_IRQ_DMA | \
+					 B43_IRQ_TXFIFO_FLUSH_OK | \
+					 B43_IRQ_NOISESAMPLE_OK | \
+					 B43_IRQ_UCODE_DEBUG | \
+					 B43_IRQ_RFKILL | \
+					 B43_IRQ_TX_OK)
+
+/* Device specific rate values.
+ * The actual values defined here are (rate_in_mbps * 2).
+ * Some code depends on this. Don't change it. */
+#define B43_CCK_RATE_1MB		0x02
+#define B43_CCK_RATE_2MB		0x04
+#define B43_CCK_RATE_5MB		0x0B
+#define B43_CCK_RATE_11MB		0x16
+#define B43_OFDM_RATE_6MB		0x0C
+#define B43_OFDM_RATE_9MB		0x12
+#define B43_OFDM_RATE_12MB		0x18
+#define B43_OFDM_RATE_18MB		0x24
+#define B43_OFDM_RATE_24MB		0x30
+#define B43_OFDM_RATE_36MB		0x48
+#define B43_OFDM_RATE_48MB		0x60
+#define B43_OFDM_RATE_54MB		0x6C
+/* Convert a b43 rate value to a rate in 100kbps */
+#define B43_RATE_TO_BASE100KBPS(rate)	(((rate) * 10) / 2)
+
+#define B43_DEFAULT_SHORT_RETRY_LIMIT	7
+#define B43_DEFAULT_LONG_RETRY_LIMIT	4
+
+/* Max size of a security key */
+#define B43_SEC_KEYSIZE			16
+/* Security algorithms. */
+enum {
+	B43_SEC_ALGO_NONE = 0,	/* unencrypted, as of TX header. */
+	B43_SEC_ALGO_WEP40,
+	B43_SEC_ALGO_TKIP,
+	B43_SEC_ALGO_AES,
+	B43_SEC_ALGO_WEP104,
+	B43_SEC_ALGO_AES_LEGACY,
+};
+
+struct b43_dmaring;
+struct b43_pioqueue;
+
+/* The firmware file header */
+#define B43_FW_TYPE_UCODE	'u'
+#define B43_FW_TYPE_PCM		'p'
+#define B43_FW_TYPE_IV		'i'
+struct b43_fw_header {
+	/* File type */
+	u8 type;
+	/* File format version */
+	u8 ver;
+	u8 __padding[2];
+	/* Size of the data. For ucode and PCM this is in bytes.
+	 * For IV this is number-of-ivs. */
+	__be32 size;
+} __attribute__((__packed__));
+
+/* Initial Value file format */
+#define B43_IV_OFFSET_MASK	0x7FFF
+#define B43_IV_32BIT		0x8000
+struct b43_iv {
+	__be16 offset_size;
+	union {
+		__be16 d16;
+		__be32 d32;
+	} data __attribute__((__packed__));
+} __attribute__((__packed__));
+
+
+#define B43_PHYMODE(phytype)		(1 << (phytype))
+#define B43_PHYMODE_A			B43_PHYMODE(B43_PHYTYPE_A)
+#define B43_PHYMODE_B			B43_PHYMODE(B43_PHYTYPE_B)
+#define B43_PHYMODE_G			B43_PHYMODE(B43_PHYTYPE_G)
+
+struct b43_phy {
+	/* Possible PHYMODEs on this PHY */
+	u8 possible_phymodes;
+	/* GMODE bit enabled? */
+	bool gmode;
+	/* Possible ieee80211 subsystem hwmodes for this PHY.
+	 * Which mode is selected, depends on thr GMODE enabled bit */
+#define B43_MAX_PHYHWMODES	2
+	struct ieee80211_hw_mode hwmodes[B43_MAX_PHYHWMODES];
+
+	/* Analog Type */
+	u8 analog;
+	/* B43_PHYTYPE_ */
+	u8 type;
+	/* PHY revision number. */
+	u8 rev;
+
+	/* Radio versioning */
+	u16 radio_manuf;	/* Radio manufacturer */
+	u16 radio_ver;		/* Radio version */
+	u8 radio_rev;		/* Radio revision */
+
+	bool locked;		/* Only used in b43_phy_{un}lock() */
+	bool dyn_tssi_tbl;	/* tssi2dbm is kmalloc()ed. */
+
+	/* ACI (adjacent channel interference) flags. */
+	bool aci_enable;
+	bool aci_wlan_automatic;
+	bool aci_hw_rssi;
+
+	/* Radio switched on/off */
+	bool radio_on;
+	struct {
+		/* Values saved when turning the radio off.
+		 * They are needed when turning it on again. */
+		bool valid;
+		u16 rfover;
+		u16 rfoverval;
+	} radio_off_context;
+
+	u16 minlowsig[2];
+	u16 minlowsigpos[2];
+
+	/* TSSI to dBm table in use */
+	const s8 *tssi2dbm;
+	/* Target idle TSSI */
+	int tgt_idle_tssi;
+	/* Current idle TSSI */
+	int cur_idle_tssi;
+
+	/* LocalOscillator control values. */
+	struct b43_txpower_lo_control *lo_control;
+	/* Values from b43_calc_loopback_gain() */
+	s16 max_lb_gain;	/* Maximum Loopback gain in hdB */
+	s16 trsw_rx_gain;	/* TRSW RX gain in hdB */
+	s16 lna_lod_gain;	/* LNA lod */
+	s16 lna_gain;		/* LNA */
+	s16 pga_gain;		/* PGA */
+
+	/* PHY lock for core.rev < 3
+	 * This lock is only used by b43_phy_{un}lock()
+	 */
+	spinlock_t lock;
+
+	/* Desired TX power level (in dBm).
+	 * This is set by the user and adjusted in b43_phy_xmitpower(). */
+	u8 power_level;
+	/* A-PHY TX Power control value. */
+	u16 txpwr_offset;
+
+	/* Current TX power level attenuation control values */
+	struct b43_bbatt bbatt;
+	struct b43_rfatt rfatt;
+	u8 tx_control;		/* B43_TXCTL_XXX */
+#ifdef CONFIG_B43_DEBUG
+	bool manual_txpower_control;	/* Manual TX-power control enabled? */
+#endif
+	/* Hardware Power Control enabled? */
+	bool hardware_power_control;
+
+	/* Current Interference Mitigation mode */
+	int interfmode;
+	/* Stack of saved values from the Interference Mitigation code.
+	 * Each value in the stack is layed out as follows:
+	 * bit 0-11:  offset
+	 * bit 12-15: register ID
+	 * bit 16-32: value
+	 * register ID is: 0x1 PHY, 0x2 Radio, 0x3 ILT
+	 */
+#define B43_INTERFSTACK_SIZE	26
+	u32 interfstack[B43_INTERFSTACK_SIZE];	//FIXME: use a data structure
+
+	/* Saved values from the NRSSI Slope calculation */
+	s16 nrssi[2];
+	s32 nrssislope;
+	/* In memory nrssi lookup table. */
+	s8 nrssi_lt[64];
+
+	/* current channel */
+	u8 channel;
+
+	u16 lofcal;
+
+	u16 initval;		//FIXME rename?
+};
+
+/* Data structures for DMA transmission, per 80211 core. */
+struct b43_dma {
+	struct b43_dmaring *tx_ring0;
+	struct b43_dmaring *tx_ring1;
+	struct b43_dmaring *tx_ring2;
+	struct b43_dmaring *tx_ring3;
+	struct b43_dmaring *tx_ring4;
+	struct b43_dmaring *tx_ring5;
+
+	struct b43_dmaring *rx_ring0;
+	struct b43_dmaring *rx_ring3;	/* only available on core.rev < 5 */
+};
+
+/* Data structures for PIO transmission, per 80211 core. */
+struct b43_pio {
+	struct b43_pioqueue *queue0;
+	struct b43_pioqueue *queue1;
+	struct b43_pioqueue *queue2;
+	struct b43_pioqueue *queue3;
+};
+
+/* Context information for a noise calculation (Link Quality). */
+struct b43_noise_calculation {
+	u8 channel_at_start;
+	bool calculation_running;
+	u8 nr_samples;
+	s8 samples[8][4];
+};
+
+struct b43_stats {
+	u8 link_noise;
+	/* Store the last TX/RX times here for updating the leds. */
+	unsigned long last_tx;
+	unsigned long last_rx;
+};
+
+struct b43_key {
+	/* If keyconf is NULL, this key is disabled.
+	 * keyconf is a cookie. Don't derefenrence it outside of the set_key
+	 * path, because b43 doesn't own it. */
+	struct ieee80211_key_conf *keyconf;
+	u8 algorithm;
+};
+
+struct b43_wldev;
+
+/* Data structure for the WLAN parts (802.11 cores) of the b43 chip. */
+struct b43_wl {
+	/* Pointer to the active wireless device on this chip */
+	struct b43_wldev *current_dev;
+	/* Pointer to the ieee80211 hardware data structure */
+	struct ieee80211_hw *hw;
+
+	spinlock_t irq_lock;
+	struct mutex mutex;
+	spinlock_t leds_lock;
+
+	/* We can only have one operating interface (802.11 core)
+	 * at a time. General information about this interface follows.
+	 */
+
+	/* Opaque ID of the operating interface from the ieee80211
+	 * subsystem. Do not modify.
+	 */
+	int if_id;
+	/* The MAC address of the operating interface. */
+	u8 mac_addr[ETH_ALEN];
+	/* Current BSSID */
+	u8 bssid[ETH_ALEN];
+	/* Interface type. (IEEE80211_IF_TYPE_XXX) */
+	int if_type;
+	/* Is the card operating in AP, STA or IBSS mode? */
+	bool operating;
+	/* filter flags */
+	unsigned int filter_flags;
+	/* Stats about the wireless interface */
+	struct ieee80211_low_level_stats ieee_stats;
+
+	struct hwrng rng;
+	u8 rng_initialized;
+	char rng_name[30 + 1];
+
+	/* The RF-kill button */
+	struct b43_rfkill rfkill;
+
+	/* List of all wireless devices on this chip */
+	struct list_head devlist;
+	u8 nr_devs;
+};
+
+/* Pointers to the firmware data and meta information about it. */
+struct b43_firmware {
+	/* Microcode */
+	const struct firmware *ucode;
+	/* PCM code */
+	const struct firmware *pcm;
+	/* Initial MMIO values for the firmware */
+	const struct firmware *initvals;
+	/* Initial MMIO values for the firmware, band-specific */
+	const struct firmware *initvals_band;
+	/* Firmware revision */
+	u16 rev;
+	/* Firmware patchlevel */
+	u16 patch;
+};
+
+/* Device (802.11 core) initialization status. */
+enum {
+	B43_STAT_UNINIT = 0,	/* Uninitialized. */
+	B43_STAT_INITIALIZED = 1,	/* Initialized, but not started, yet. */
+	B43_STAT_STARTED = 2,	/* Up and running. */
+};
+#define b43_status(wldev)		atomic_read(&(wldev)->__init_status)
+#define b43_set_status(wldev, stat)	do {			\
+		atomic_set(&(wldev)->__init_status, (stat));	\
+		smp_wmb();					\
+					} while (0)
+
+/* XXX---   HOW LOCKING WORKS IN B43   ---XXX
+ *
+ * You should always acquire both, wl->mutex and wl->irq_lock unless:
+ * - You don't need to acquire wl->irq_lock, if the interface is stopped.
+ * - You don't need to acquire wl->mutex in the IRQ handler, IRQ tasklet
+ *   and packet TX path (and _ONLY_ there.)
+ */
+
+/* Data structure for one wireless device (802.11 core) */
+struct b43_wldev {
+	struct ssb_device *dev;
+	struct b43_wl *wl;
+
+	/* The device initialization status.
+	 * Use b43_status() to query. */
+	atomic_t __init_status;
+	/* Saved init status for handling suspend. */
+	int suspend_init_status;
+
+	bool __using_pio;	/* Internal, use b43_using_pio(). */
+	bool bad_frames_preempt;	/* Use "Bad Frames Preemption" (default off) */
+	bool reg124_set_0x4;	/* Some variable to keep track of IRQ stuff. */
+	bool short_preamble;	/* TRUE, if short preamble is enabled. */
+	bool short_slot;	/* TRUE, if short slot timing is enabled. */
+	bool radio_hw_enable;	/* saved state of radio hardware enabled state */
+
+	/* PHY/Radio device. */
+	struct b43_phy phy;
+	union {
+		/* DMA engines. */
+		struct b43_dma dma;
+		/* PIO engines. */
+		struct b43_pio pio;
+	};
+
+	/* Various statistics about the physical device. */
+	struct b43_stats stats;
+
+	/* The device LEDs. */
+	struct b43_led led_tx;
+	struct b43_led led_rx;
+	struct b43_led led_assoc;
+	struct b43_led led_radio;
+
+	/* Reason code of the last interrupt. */
+	u32 irq_reason;
+	u32 dma_reason[6];
+	/* saved irq enable/disable state bitfield. */
+	u32 irq_savedstate;
+	/* Link Quality calculation context. */
+	struct b43_noise_calculation noisecalc;
+	/* if > 0 MAC is suspended. if == 0 MAC is enabled. */
+	int mac_suspended;
+
+	/* Interrupt Service Routine tasklet (bottom-half) */
+	struct tasklet_struct isr_tasklet;
+
+	/* Periodic tasks */
+	struct delayed_work periodic_work;
+	unsigned int periodic_state;
+
+	struct work_struct restart_work;
+
+	/* encryption/decryption */
+	u16 ktp;		/* Key table pointer */
+	u8 max_nr_keys;
+	struct b43_key key[58];
+
+	/* Cached beacon template while uploading the template. */
+	struct sk_buff *cached_beacon;
+
+	/* Firmware data */
+	struct b43_firmware fw;
+
+	/* Devicelist in struct b43_wl (all 802.11 cores) */
+	struct list_head list;
+
+	/* Debugging stuff follows. */
+#ifdef CONFIG_B43_DEBUG
+	struct b43_dfsentry *dfsentry;
+#endif
+};
+
+static inline struct b43_wl *hw_to_b43_wl(struct ieee80211_hw *hw)
+{
+	return hw->priv;
+}
+
+/* Helper function, which returns a boolean.
+ * TRUE, if PIO is used; FALSE, if DMA is used.
+ */
+#if defined(CONFIG_B43_DMA) && defined(CONFIG_B43_PIO)
+static inline int b43_using_pio(struct b43_wldev *dev)
+{
+	return dev->__using_pio;
+}
+#elif defined(CONFIG_B43_DMA)
+static inline int b43_using_pio(struct b43_wldev *dev)
+{
+	return 0;
+}
+#elif defined(CONFIG_B43_PIO)
+static inline int b43_using_pio(struct b43_wldev *dev)
+{
+	return 1;
+}
+#else
+# error "Using neither DMA nor PIO? Confused..."
+#endif
+
+static inline struct b43_wldev *dev_to_b43_wldev(struct device *dev)
+{
+	struct ssb_device *ssb_dev = dev_to_ssb_dev(dev);
+	return ssb_get_drvdata(ssb_dev);
+}
+
+/* Is the device operating in a specified mode (IEEE80211_IF_TYPE_XXX). */
+static inline int b43_is_mode(struct b43_wl *wl, int type)
+{
+	return (wl->operating && wl->if_type == type);
+}
+
+static inline u16 b43_read16(struct b43_wldev *dev, u16 offset)
+{
+	return ssb_read16(dev->dev, offset);
+}
+
+static inline void b43_write16(struct b43_wldev *dev, u16 offset, u16 value)
+{
+	ssb_write16(dev->dev, offset, value);
+}
+
+static inline u32 b43_read32(struct b43_wldev *dev, u16 offset)
+{
+	return ssb_read32(dev->dev, offset);
+}
+
+static inline void b43_write32(struct b43_wldev *dev, u16 offset, u32 value)
+{
+	ssb_write32(dev->dev, offset, value);
+}
+
+/* Message printing */
+void b43info(struct b43_wl *wl, const char *fmt, ...)
+    __attribute__ ((format(printf, 2, 3)));
+void b43err(struct b43_wl *wl, const char *fmt, ...)
+    __attribute__ ((format(printf, 2, 3)));
+void b43warn(struct b43_wl *wl, const char *fmt, ...)
+    __attribute__ ((format(printf, 2, 3)));
+#if B43_DEBUG
+void b43dbg(struct b43_wl *wl, const char *fmt, ...)
+    __attribute__ ((format(printf, 2, 3)));
+#else /* DEBUG */
+# define b43dbg(wl, fmt...) do { /* nothing */ } while (0)
+#endif /* DEBUG */
+
+/* A WARN_ON variant that vanishes when b43 debugging is disabled.
+ * This _also_ evaluates the arg with debugging disabled. */
+#if B43_DEBUG
+# define B43_WARN_ON(x)	WARN_ON(x)
+#else
+static inline bool __b43_warn_on_dummy(bool x) { return x; }
+# define B43_WARN_ON(x)	__b43_warn_on_dummy(unlikely(!!(x)))
+#endif
+
+/** Limit a value between two limits */
+#ifdef limit_value
+# undef limit_value
+#endif
+#define limit_value(value, min, max)  \
+	({						\
+		typeof(value) __value = (value);	\
+		typeof(value) __min = (min);		\
+		typeof(value) __max = (max);		\
+		if (__value < __min)			\
+			__value = __min;		\
+		else if (__value > __max)		\
+			__value = __max;		\
+		__value;				\
+	})
+
+/* Convert an integer to a Q5.2 value */
+#define INT_TO_Q52(i)	((i) << 2)
+/* Convert a Q5.2 value to an integer (precision loss!) */
+#define Q52_TO_INT(q52)	((q52) >> 2)
+/* Macros for printing a value in Q5.2 format */
+#define Q52_FMT		"%u.%u"
+#define Q52_ARG(q52)	Q52_TO_INT(q52), ((((q52) & 0x3) * 100) / 4)
+
+#endif /* B43_H_ */
diff --git a/drivers/net/wireless/b43/debugfs.c b/drivers/net/wireless/b43/debugfs.c
new file mode 100644
index 000000000000..734e70e1a06d
--- /dev/null
+++ b/drivers/net/wireless/b43/debugfs.c
@@ -0,0 +1,656 @@
+/*
+
+  Broadcom B43 wireless driver
+
+  debugfs driver debugging code
+
+  Copyright (c) 2005-2007 Michael Buesch <mb@bu3sch.de>
+
+  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, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with this program; see the file COPYING.  If not, write to
+  the Free Software Foundation, Inc., 51 Franklin Steet, Fifth Floor,
+  Boston, MA 02110-1301, USA.
+
+*/
+
+#include <linux/fs.h>
+#include <linux/debugfs.h>
+#include <linux/slab.h>
+#include <linux/netdevice.h>
+#include <linux/pci.h>
+#include <linux/mutex.h>
+
+#include "b43.h"
+#include "main.h"
+#include "debugfs.h"
+#include "dma.h"
+#include "pio.h"
+#include "xmit.h"
+
+
+/* The root directory. */
+static struct dentry *rootdir;
+
+struct b43_debugfs_fops {
+	ssize_t (*read)(struct b43_wldev *dev, char *buf, size_t bufsize);
+	int (*write)(struct b43_wldev *dev, const char *buf, size_t count);
+	struct file_operations fops;
+	/* Offset of struct b43_dfs_file in struct b43_dfsentry */
+	size_t file_struct_offset;
+	/* Take wl->irq_lock before calling read/write? */
+	bool take_irqlock;
+};
+
+static inline
+struct b43_dfs_file * fops_to_dfs_file(struct b43_wldev *dev,
+				       const struct b43_debugfs_fops *dfops)
+{
+	void *p;
+
+	p = dev->dfsentry;
+	p += dfops->file_struct_offset;
+
+	return p;
+}
+
+
+#define fappend(fmt, x...)	\
+	do {							\
+		if (bufsize - count)				\
+			count += snprintf(buf + count,		\
+					  bufsize - count,	\
+					  fmt , ##x);		\
+		else						\
+			printk(KERN_ERR "b43: fappend overflow\n"); \
+	} while (0)
+
+
+/* wl->irq_lock is locked */
+static ssize_t tsf_read_file(struct b43_wldev *dev,
+			     char *buf, size_t bufsize)
+{
+	ssize_t count = 0;
+	u64 tsf;
+
+	b43_tsf_read(dev, &tsf);
+	fappend("0x%08x%08x\n",
+		(unsigned int)((tsf & 0xFFFFFFFF00000000ULL) >> 32),
+		(unsigned int)(tsf & 0xFFFFFFFFULL));
+
+	return count;
+}
+
+/* wl->irq_lock is locked */
+static int tsf_write_file(struct b43_wldev *dev,
+			  const char *buf, size_t count)
+{
+	u64 tsf;
+
+	if (sscanf(buf, "%llu", (unsigned long long *)(&tsf)) != 1)
+		return -EINVAL;
+	b43_tsf_write(dev, tsf);
+
+	return 0;
+}
+
+/* wl->irq_lock is locked */
+static ssize_t ucode_regs_read_file(struct b43_wldev *dev,
+				    char *buf, size_t bufsize)
+{
+	ssize_t count = 0;
+	int i;
+
+	for (i = 0; i < 64; i++) {
+		fappend("r%d = 0x%04x\n", i,
+			b43_shm_read16(dev, B43_SHM_SCRATCH, i));
+	}
+
+	return count;
+}
+
+/* wl->irq_lock is locked */
+static ssize_t shm_read_file(struct b43_wldev *dev,
+			     char *buf, size_t bufsize)
+{
+	ssize_t count = 0;
+	int i;
+	u16 tmp;
+	__le16 *le16buf = (__le16 *)buf;
+
+	for (i = 0; i < 0x1000; i++) {
+		if (bufsize <= 0)
+			break;
+		tmp = b43_shm_read16(dev, B43_SHM_SHARED, 2 * i);
+		le16buf[i] = cpu_to_le16(tmp);
+		count += sizeof(tmp);
+		bufsize -= sizeof(tmp);
+	}
+
+	return count;
+}
+
+static ssize_t txstat_read_file(struct b43_wldev *dev,
+				char *buf, size_t bufsize)
+{
+	struct b43_txstatus_log *log = &dev->dfsentry->txstatlog;
+	ssize_t count = 0;
+	unsigned long flags;
+	int i, idx;
+	struct b43_txstatus *stat;
+
+	spin_lock_irqsave(&log->lock, flags);
+	if (log->end < 0) {
+		fappend("Nothing transmitted, yet\n");
+		goto out_unlock;
+	}
+	fappend("b43 TX status reports:\n\n"
+		"index | cookie | seq | phy_stat | frame_count | "
+		"rts_count | supp_reason | pm_indicated | "
+		"intermediate | for_ampdu | acked\n" "---\n");
+	i = log->end + 1;
+	idx = 0;
+	while (1) {
+		if (i == B43_NR_LOGGED_TXSTATUS)
+			i = 0;
+		stat = &(log->log[i]);
+		if (stat->cookie) {
+			fappend("%03d | "
+				"0x%04X | 0x%04X | 0x%02X | "
+				"0x%X | 0x%X | "
+				"%u | %u | "
+				"%u | %u | %u\n",
+				idx,
+				stat->cookie, stat->seq, stat->phy_stat,
+				stat->frame_count, stat->rts_count,
+				stat->supp_reason, stat->pm_indicated,
+				stat->intermediate, stat->for_ampdu,
+				stat->acked);
+			idx++;
+		}
+		if (i == log->end)
+			break;
+		i++;
+	}
+out_unlock:
+	spin_unlock_irqrestore(&log->lock, flags);
+
+	return count;
+}
+
+static ssize_t txpower_g_read_file(struct b43_wldev *dev,
+				   char *buf, size_t bufsize)
+{
+	ssize_t count = 0;
+
+	if (dev->phy.type != B43_PHYTYPE_G) {
+		fappend("Device is not a G-PHY\n");
+		goto out;
+	}
+	fappend("Control:               %s\n", dev->phy.manual_txpower_control ?
+		"MANUAL" : "AUTOMATIC");
+	fappend("Baseband attenuation:  %u\n", dev->phy.bbatt.att);
+	fappend("Radio attenuation:     %u\n", dev->phy.rfatt.att);
+	fappend("TX Mixer Gain:         %s\n",
+		(dev->phy.tx_control & B43_TXCTL_TXMIX) ? "ON" : "OFF");
+	fappend("PA Gain 2dB:           %s\n",
+		(dev->phy.tx_control & B43_TXCTL_PA2DB) ? "ON" : "OFF");
+	fappend("PA Gain 3dB:           %s\n",
+		(dev->phy.tx_control & B43_TXCTL_PA3DB) ? "ON" : "OFF");
+	fappend("\n\n");
+	fappend("You can write to this file:\n");
+	fappend("Writing \"auto\" enables automatic txpower control.\n");
+	fappend
+	    ("Writing the attenuation values as \"bbatt rfatt txmix pa2db pa3db\" "
+	     "enables manual txpower control.\n");
+	fappend("Example: 5 4 0 0 1\n");
+	fappend("Enables manual control with Baseband attenuation 5, "
+		"Radio attenuation 4, No TX Mixer Gain, "
+		"No PA Gain 2dB, With PA Gain 3dB.\n");
+out:
+	return count;
+}
+
+static int txpower_g_write_file(struct b43_wldev *dev,
+				const char *buf, size_t count)
+{
+	unsigned long phy_flags;
+
+	if (dev->phy.type != B43_PHYTYPE_G)
+		return -ENODEV;
+	if ((count >= 4) && (memcmp(buf, "auto", 4) == 0)) {
+		/* Automatic control */
+		dev->phy.manual_txpower_control = 0;
+		b43_phy_xmitpower(dev);
+	} else {
+		int bbatt = 0, rfatt = 0, txmix = 0, pa2db = 0, pa3db = 0;
+		/* Manual control */
+		if (sscanf(buf, "%d %d %d %d %d", &bbatt, &rfatt,
+			   &txmix, &pa2db, &pa3db) != 5)
+			return -EINVAL;
+		b43_put_attenuation_into_ranges(dev, &bbatt, &rfatt);
+		dev->phy.manual_txpower_control = 1;
+		dev->phy.bbatt.att = bbatt;
+		dev->phy.rfatt.att = rfatt;
+		dev->phy.tx_control = 0;
+		if (txmix)
+			dev->phy.tx_control |= B43_TXCTL_TXMIX;
+		if (pa2db)
+			dev->phy.tx_control |= B43_TXCTL_PA2DB;
+		if (pa3db)
+			dev->phy.tx_control |= B43_TXCTL_PA3DB;
+		b43_phy_lock(dev, phy_flags);
+		b43_radio_lock(dev);
+		b43_set_txpower_g(dev, &dev->phy.bbatt,
+				  &dev->phy.rfatt, dev->phy.tx_control);
+		b43_radio_unlock(dev);
+		b43_phy_unlock(dev, phy_flags);
+	}
+
+	return 0;
+}
+
+/* wl->irq_lock is locked */
+static int restart_write_file(struct b43_wldev *dev,
+			      const char *buf, size_t count)
+{
+	int err = 0;
+
+	if (count > 0 && buf[0] == '1') {
+		b43_controller_restart(dev, "manually restarted");
+	} else
+		err = -EINVAL;
+
+	return err;
+}
+
+static ssize_t append_lo_table(ssize_t count, char *buf, const size_t bufsize,
+			       struct b43_loctl table[B43_NR_BB][B43_NR_RF])
+{
+	unsigned int i, j;
+	struct b43_loctl *ctl;
+
+	for (i = 0; i < B43_NR_BB; i++) {
+		for (j = 0; j < B43_NR_RF; j++) {
+			ctl = &(table[i][j]);
+			fappend("(bbatt %2u, rfatt %2u)  ->  "
+				"(I %+3d, Q %+3d, Used: %d, Calibrated: %d)\n",
+				i, j, ctl->i, ctl->q,
+				ctl->used,
+				b43_loctl_is_calibrated(ctl));
+		}
+	}
+
+	return count;
+}
+
+static ssize_t loctls_read_file(struct b43_wldev *dev,
+				char *buf, size_t bufsize)
+{
+	ssize_t count = 0;
+	struct b43_txpower_lo_control *lo;
+	int i, err = 0;
+
+	if (dev->phy.type != B43_PHYTYPE_G) {
+		fappend("Device is not a G-PHY\n");
+		err = -ENODEV;
+		goto out;
+	}
+	lo = dev->phy.lo_control;
+	fappend("-- Local Oscillator calibration data --\n\n");
+	fappend("Measured: %d,  Rebuild: %d,  HW-power-control: %d\n",
+		lo->lo_measured,
+		lo->rebuild,
+		dev->phy.hardware_power_control);
+	fappend("TX Bias: 0x%02X,  TX Magn: 0x%02X\n",
+		lo->tx_bias, lo->tx_magn);
+	fappend("Power Vector: 0x%08X%08X\n",
+		(unsigned int)((lo->power_vector & 0xFFFFFFFF00000000ULL) >> 32),
+		(unsigned int)(lo->power_vector & 0x00000000FFFFFFFFULL));
+	fappend("\nControl table WITH PADMIX:\n");
+	count = append_lo_table(count, buf, bufsize, lo->with_padmix);
+	fappend("\nControl table WITHOUT PADMIX:\n");
+	count = append_lo_table(count, buf, bufsize, lo->no_padmix);
+	fappend("\nUsed RF attenuation values:  Value(WithPadmix flag)\n");
+	for (i = 0; i < lo->rfatt_list.len; i++) {
+		fappend("%u(%d), ",
+			lo->rfatt_list.list[i].att,
+			lo->rfatt_list.list[i].with_padmix);
+	}
+	fappend("\n");
+	fappend("\nUsed Baseband attenuation values:\n");
+	for (i = 0; i < lo->bbatt_list.len; i++) {
+		fappend("%u, ",
+			lo->bbatt_list.list[i].att);
+	}
+	fappend("\n");
+
+out:
+	return err ? err : count;
+}
+
+#undef fappend
+
+static int b43_debugfs_open(struct inode *inode, struct file *file)
+{
+	file->private_data = inode->i_private;
+	return 0;
+}
+
+static ssize_t b43_debugfs_read(struct file *file, char __user *userbuf,
+				size_t count, loff_t *ppos)
+{
+	struct b43_wldev *dev;
+	struct b43_debugfs_fops *dfops;
+	struct b43_dfs_file *dfile;
+	ssize_t ret;
+	char *buf;
+	const size_t bufsize = 1024 * 128;
+	const size_t buforder = get_order(bufsize);
+	int err = 0;
+
+	if (!count)
+		return 0;
+	dev = file->private_data;
+	if (!dev)
+		return -ENODEV;
+
+	mutex_lock(&dev->wl->mutex);
+	if (b43_status(dev) < B43_STAT_INITIALIZED) {
+		err = -ENODEV;
+		goto out_unlock;
+	}
+
+	dfops = container_of(file->f_op, struct b43_debugfs_fops, fops);
+	if (!dfops->read) {
+		err = -ENOSYS;
+		goto out_unlock;
+	}
+	dfile = fops_to_dfs_file(dev, dfops);
+
+	if (!dfile->buffer) {
+		buf = (char *)__get_free_pages(GFP_KERNEL, buforder);
+		if (!buf) {
+			err = -ENOMEM;
+			goto out_unlock;
+		}
+		/* Sparse warns about the following memset, because it has a big
+		 * size value. That warning is bogus, so I will ignore it. --mb */
+		memset(buf, 0, bufsize);
+		if (dfops->take_irqlock) {
+			spin_lock_irq(&dev->wl->irq_lock);
+			ret = dfops->read(dev, buf, bufsize);
+			spin_unlock_irq(&dev->wl->irq_lock);
+		} else
+			ret = dfops->read(dev, buf, bufsize);
+		if (ret <= 0) {
+			free_pages((unsigned long)buf, buforder);
+			err = ret;
+			goto out_unlock;
+		}
+		dfile->data_len = ret;
+		dfile->buffer = buf;
+	}
+
+	ret = simple_read_from_buffer(userbuf, count, ppos,
+				      dfile->buffer,
+				      dfile->data_len);
+	if (*ppos >= dfile->data_len) {
+		free_pages((unsigned long)dfile->buffer, buforder);
+		dfile->buffer = NULL;
+		dfile->data_len = 0;
+	}
+out_unlock:
+	mutex_unlock(&dev->wl->mutex);
+
+	return err ? err : ret;
+}
+
+static ssize_t b43_debugfs_write(struct file *file,
+				 const char __user *userbuf,
+				 size_t count, loff_t *ppos)
+{
+	struct b43_wldev *dev;
+	struct b43_debugfs_fops *dfops;
+	char *buf;
+	int err = 0;
+
+	if (!count)
+		return 0;
+	if (count > PAGE_SIZE)
+		return -E2BIG;
+	dev = file->private_data;
+	if (!dev)
+		return -ENODEV;
+
+	mutex_lock(&dev->wl->mutex);
+	if (b43_status(dev) < B43_STAT_INITIALIZED) {
+		err = -ENODEV;
+		goto out_unlock;
+	}
+
+	dfops = container_of(file->f_op, struct b43_debugfs_fops, fops);
+	if (!dfops->write) {
+		err = -ENOSYS;
+		goto out_unlock;
+	}
+
+	buf = (char *)get_zeroed_page(GFP_KERNEL);
+	if (!buf) {
+		err = -ENOMEM;
+		goto out_unlock;
+	}
+	if (copy_from_user(buf, userbuf, count)) {
+		err = -EFAULT;
+		goto out_freepage;
+	}
+	if (dfops->take_irqlock) {
+		spin_lock_irq(&dev->wl->irq_lock);
+		err = dfops->write(dev, buf, count);
+		spin_unlock_irq(&dev->wl->irq_lock);
+	} else
+		err = dfops->write(dev, buf, count);
+	if (err)
+		goto out_freepage;
+
+out_freepage:
+	free_page((unsigned long)buf);
+out_unlock:
+	mutex_unlock(&dev->wl->mutex);
+
+	return err ? err : count;
+}
+
+
+#define B43_DEBUGFS_FOPS(name, _read, _write, _take_irqlock)	\
+	static struct b43_debugfs_fops fops_##name = {		\
+		.read	= _read,				\
+		.write	= _write,				\
+		.fops	= {					\
+			.open	= b43_debugfs_open,		\
+			.read	= b43_debugfs_read,		\
+			.write	= b43_debugfs_write,		\
+		},						\
+		.file_struct_offset = offsetof(struct b43_dfsentry, \
+					       file_##name),	\
+		.take_irqlock	= _take_irqlock,		\
+	}
+
+B43_DEBUGFS_FOPS(tsf, tsf_read_file, tsf_write_file, 1);
+B43_DEBUGFS_FOPS(ucode_regs, ucode_regs_read_file, NULL, 1);
+B43_DEBUGFS_FOPS(shm, shm_read_file, NULL, 1);
+B43_DEBUGFS_FOPS(txstat, txstat_read_file, NULL, 0);
+B43_DEBUGFS_FOPS(txpower_g, txpower_g_read_file, txpower_g_write_file, 0);
+B43_DEBUGFS_FOPS(restart, NULL, restart_write_file, 1);
+B43_DEBUGFS_FOPS(loctls, loctls_read_file, NULL, 0);
+
+
+int b43_debug(struct b43_wldev *dev, enum b43_dyndbg feature)
+{
+	return !!(dev->dfsentry && dev->dfsentry->dyn_debug[feature]);
+}
+
+static void b43_remove_dynamic_debug(struct b43_wldev *dev)
+{
+	struct b43_dfsentry *e = dev->dfsentry;
+	int i;
+
+	for (i = 0; i < __B43_NR_DYNDBG; i++)
+		debugfs_remove(e->dyn_debug_dentries[i]);
+}
+
+static void b43_add_dynamic_debug(struct b43_wldev *dev)
+{
+	struct b43_dfsentry *e = dev->dfsentry;
+	struct dentry *d;
+
+#define add_dyn_dbg(name, id, initstate) do {		\
+	e->dyn_debug[id] = (initstate);			\
+	d = debugfs_create_bool(name, 0600, e->subdir,	\
+				&(e->dyn_debug[id]));	\
+	if (!IS_ERR(d))					\
+		e->dyn_debug_dentries[id] = d;		\
+				} while (0)
+
+	add_dyn_dbg("debug_xmitpower", B43_DBG_XMITPOWER, 0);
+	add_dyn_dbg("debug_dmaoverflow", B43_DBG_DMAOVERFLOW, 0);
+	add_dyn_dbg("debug_dmaverbose", B43_DBG_DMAVERBOSE, 0);
+	add_dyn_dbg("debug_pwork_fast", B43_DBG_PWORK_FAST, 0);
+	add_dyn_dbg("debug_pwork_stop", B43_DBG_PWORK_STOP, 0);
+
+#undef add_dyn_dbg
+}
+
+void b43_debugfs_add_device(struct b43_wldev *dev)
+{
+	struct b43_dfsentry *e;
+	struct b43_txstatus_log *log;
+	char devdir[16];
+
+	B43_WARN_ON(!dev);
+	e = kzalloc(sizeof(*e), GFP_KERNEL);
+	if (!e) {
+		b43err(dev->wl, "debugfs: add device OOM\n");
+		return;
+	}
+	e->dev = dev;
+	log = &e->txstatlog;
+	log->log = kcalloc(B43_NR_LOGGED_TXSTATUS,
+			   sizeof(struct b43_txstatus), GFP_KERNEL);
+	if (!log->log) {
+		b43err(dev->wl, "debugfs: add device txstatus OOM\n");
+		kfree(e);
+		return;
+	}
+	log->end = -1;
+	spin_lock_init(&log->lock);
+
+	dev->dfsentry = e;
+
+	snprintf(devdir, sizeof(devdir), "%s", wiphy_name(dev->wl->hw->wiphy));
+	e->subdir = debugfs_create_dir(devdir, rootdir);
+	if (!e->subdir || IS_ERR(e->subdir)) {
+		if (e->subdir == ERR_PTR(-ENODEV)) {
+			b43dbg(dev->wl, "DebugFS (CONFIG_DEBUG_FS) not "
+			       "enabled in kernel config\n");
+		} else {
+			b43err(dev->wl, "debugfs: cannot create %s directory\n",
+			       devdir);
+		}
+		dev->dfsentry = NULL;
+		kfree(log->log);
+		kfree(e);
+		return;
+	}
+
+#define ADD_FILE(name, mode)	\
+	do {							\
+		struct dentry *d;				\
+		d = debugfs_create_file(__stringify(name),	\
+					mode, e->subdir, dev,	\
+					&fops_##name.fops);	\
+		e->file_##name.dentry = NULL;			\
+		if (!IS_ERR(d))					\
+			e->file_##name.dentry = d;		\
+	} while (0)
+
+
+	ADD_FILE(tsf, 0600);
+	ADD_FILE(ucode_regs, 0400);
+	ADD_FILE(shm, 0400);
+	ADD_FILE(txstat, 0400);
+	ADD_FILE(txpower_g, 0600);
+	ADD_FILE(restart, 0200);
+	ADD_FILE(loctls, 0400);
+
+#undef ADD_FILE
+
+	b43_add_dynamic_debug(dev);
+}
+
+void b43_debugfs_remove_device(struct b43_wldev *dev)
+{
+	struct b43_dfsentry *e;
+
+	if (!dev)
+		return;
+	e = dev->dfsentry;
+	if (!e)
+		return;
+	b43_remove_dynamic_debug(dev);
+
+	debugfs_remove(e->file_tsf.dentry);
+	debugfs_remove(e->file_ucode_regs.dentry);
+	debugfs_remove(e->file_shm.dentry);
+	debugfs_remove(e->file_txstat.dentry);
+	debugfs_remove(e->file_txpower_g.dentry);
+	debugfs_remove(e->file_restart.dentry);
+	debugfs_remove(e->file_loctls.dentry);
+
+	debugfs_remove(e->subdir);
+	kfree(e->txstatlog.log);
+	kfree(e);
+}
+
+void b43_debugfs_log_txstat(struct b43_wldev *dev,
+			    const struct b43_txstatus *status)
+{
+	struct b43_dfsentry *e = dev->dfsentry;
+	struct b43_txstatus_log *log;
+	struct b43_txstatus *cur;
+	int i;
+
+	if (!e)
+		return;
+	log = &e->txstatlog;
+	B43_WARN_ON(!irqs_disabled());
+	spin_lock(&log->lock);
+	i = log->end + 1;
+	if (i == B43_NR_LOGGED_TXSTATUS)
+		i = 0;
+	log->end = i;
+	cur = &(log->log[i]);
+	memcpy(cur, status, sizeof(*cur));
+	spin_unlock(&log->lock);
+}
+
+void b43_debugfs_init(void)
+{
+	rootdir = debugfs_create_dir(KBUILD_MODNAME, NULL);
+	if (IS_ERR(rootdir))
+		rootdir = NULL;
+}
+
+void b43_debugfs_exit(void)
+{
+	debugfs_remove(rootdir);
+}
diff --git a/drivers/net/wireless/b43/debugfs.h b/drivers/net/wireless/b43/debugfs.h
new file mode 100644
index 000000000000..6eebe858db5a
--- /dev/null
+++ b/drivers/net/wireless/b43/debugfs.h
@@ -0,0 +1,89 @@
+#ifndef B43_DEBUGFS_H_
+#define B43_DEBUGFS_H_
+
+struct b43_wldev;
+struct b43_txstatus;
+
+enum b43_dyndbg {		/* Dynamic debugging features */
+	B43_DBG_XMITPOWER,
+	B43_DBG_DMAOVERFLOW,
+	B43_DBG_DMAVERBOSE,
+	B43_DBG_PWORK_FAST,
+	B43_DBG_PWORK_STOP,
+	__B43_NR_DYNDBG,
+};
+
+#ifdef CONFIG_B43_DEBUG
+
+struct dentry;
+
+#define B43_NR_LOGGED_TXSTATUS	100
+
+struct b43_txstatus_log {
+	struct b43_txstatus *log;
+	int end;
+	spinlock_t lock;
+};
+
+struct b43_dfs_file {
+	struct dentry *dentry;
+	char *buffer;
+	size_t data_len;
+};
+
+struct b43_dfsentry {
+	struct b43_wldev *dev;
+	struct dentry *subdir;
+
+	struct b43_dfs_file file_tsf;
+	struct b43_dfs_file file_ucode_regs;
+	struct b43_dfs_file file_shm;
+	struct b43_dfs_file file_txstat;
+	struct b43_dfs_file file_txpower_g;
+	struct b43_dfs_file file_restart;
+	struct b43_dfs_file file_loctls;
+
+	struct b43_txstatus_log txstatlog;
+
+	/* Enabled/Disabled list for the dynamic debugging features. */
+	u32 dyn_debug[__B43_NR_DYNDBG];
+	/* Dentries for the dynamic debugging entries. */
+	struct dentry *dyn_debug_dentries[__B43_NR_DYNDBG];
+};
+
+int b43_debug(struct b43_wldev *dev, enum b43_dyndbg feature);
+
+void b43_debugfs_init(void);
+void b43_debugfs_exit(void);
+void b43_debugfs_add_device(struct b43_wldev *dev);
+void b43_debugfs_remove_device(struct b43_wldev *dev);
+void b43_debugfs_log_txstat(struct b43_wldev *dev,
+			    const struct b43_txstatus *status);
+
+#else /* CONFIG_B43_DEBUG */
+
+static inline int b43_debug(struct b43_wldev *dev, enum b43_dyndbg feature)
+{
+	return 0;
+}
+
+static inline void b43_debugfs_init(void)
+{
+}
+static inline void b43_debugfs_exit(void)
+{
+}
+static inline void b43_debugfs_add_device(struct b43_wldev *dev)
+{
+}
+static inline void b43_debugfs_remove_device(struct b43_wldev *dev)
+{
+}
+static inline void b43_debugfs_log_txstat(struct b43_wldev *dev,
+					  const struct b43_txstatus *status)
+{
+}
+
+#endif /* CONFIG_B43_DEBUG */
+
+#endif /* B43_DEBUGFS_H_ */
diff --git a/drivers/net/wireless/b43/dma.c b/drivers/net/wireless/b43/dma.c
new file mode 100644
index 000000000000..5e8f8ac0f1dd
--- /dev/null
+++ b/drivers/net/wireless/b43/dma.c
@@ -0,0 +1,1494 @@
+/*
+
+  Broadcom B43 wireless driver
+
+  DMA ringbuffer and descriptor allocation/management
+
+  Copyright (c) 2005, 2006 Michael Buesch <mb@bu3sch.de>
+
+  Some code in this file is derived from the b44.c driver
+  Copyright (C) 2002 David S. Miller
+  Copyright (C) Pekka Pietikainen
+
+  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, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with this program; see the file COPYING.  If not, write to
+  the Free Software Foundation, Inc., 51 Franklin Steet, Fifth Floor,
+  Boston, MA 02110-1301, USA.
+
+*/
+
+#include "b43.h"
+#include "dma.h"
+#include "main.h"
+#include "debugfs.h"
+#include "xmit.h"
+
+#include <linux/dma-mapping.h>
+#include <linux/pci.h>
+#include <linux/delay.h>
+#include <linux/skbuff.h>
+
+/* 32bit DMA ops. */
+static
+struct b43_dmadesc_generic *op32_idx2desc(struct b43_dmaring *ring,
+					  int slot,
+					  struct b43_dmadesc_meta **meta)
+{
+	struct b43_dmadesc32 *desc;
+
+	*meta = &(ring->meta[slot]);
+	desc = ring->descbase;
+	desc = &(desc[slot]);
+
+	return (struct b43_dmadesc_generic *)desc;
+}
+
+static void op32_fill_descriptor(struct b43_dmaring *ring,
+				 struct b43_dmadesc_generic *desc,
+				 dma_addr_t dmaaddr, u16 bufsize,
+				 int start, int end, int irq)
+{
+	struct b43_dmadesc32 *descbase = ring->descbase;
+	int slot;
+	u32 ctl;
+	u32 addr;
+	u32 addrext;
+
+	slot = (int)(&(desc->dma32) - descbase);
+	B43_WARN_ON(!(slot >= 0 && slot < ring->nr_slots));
+
+	addr = (u32) (dmaaddr & ~SSB_DMA_TRANSLATION_MASK);
+	addrext = (u32) (dmaaddr & SSB_DMA_TRANSLATION_MASK)
+	    >> SSB_DMA_TRANSLATION_SHIFT;
+	addr |= ssb_dma_translation(ring->dev->dev);
+	ctl = (bufsize - ring->frameoffset)
+	    & B43_DMA32_DCTL_BYTECNT;
+	if (slot == ring->nr_slots - 1)
+		ctl |= B43_DMA32_DCTL_DTABLEEND;
+	if (start)
+		ctl |= B43_DMA32_DCTL_FRAMESTART;
+	if (end)
+		ctl |= B43_DMA32_DCTL_FRAMEEND;
+	if (irq)
+		ctl |= B43_DMA32_DCTL_IRQ;
+	ctl |= (addrext << B43_DMA32_DCTL_ADDREXT_SHIFT)
+	    & B43_DMA32_DCTL_ADDREXT_MASK;
+
+	desc->dma32.control = cpu_to_le32(ctl);
+	desc->dma32.address = cpu_to_le32(addr);
+}
+
+static void op32_poke_tx(struct b43_dmaring *ring, int slot)
+{
+	b43_dma_write(ring, B43_DMA32_TXINDEX,
+		      (u32) (slot * sizeof(struct b43_dmadesc32)));
+}
+
+static void op32_tx_suspend(struct b43_dmaring *ring)
+{
+	b43_dma_write(ring, B43_DMA32_TXCTL, b43_dma_read(ring, B43_DMA32_TXCTL)
+		      | B43_DMA32_TXSUSPEND);
+}
+
+static void op32_tx_resume(struct b43_dmaring *ring)
+{
+	b43_dma_write(ring, B43_DMA32_TXCTL, b43_dma_read(ring, B43_DMA32_TXCTL)
+		      & ~B43_DMA32_TXSUSPEND);
+}
+
+static int op32_get_current_rxslot(struct b43_dmaring *ring)
+{
+	u32 val;
+
+	val = b43_dma_read(ring, B43_DMA32_RXSTATUS);
+	val &= B43_DMA32_RXDPTR;
+
+	return (val / sizeof(struct b43_dmadesc32));
+}
+
+static void op32_set_current_rxslot(struct b43_dmaring *ring, int slot)
+{
+	b43_dma_write(ring, B43_DMA32_RXINDEX,
+		      (u32) (slot * sizeof(struct b43_dmadesc32)));
+}
+
+static const struct b43_dma_ops dma32_ops = {
+	.idx2desc = op32_idx2desc,
+	.fill_descriptor = op32_fill_descriptor,
+	.poke_tx = op32_poke_tx,
+	.tx_suspend = op32_tx_suspend,
+	.tx_resume = op32_tx_resume,
+	.get_current_rxslot = op32_get_current_rxslot,
+	.set_current_rxslot = op32_set_current_rxslot,
+};
+
+/* 64bit DMA ops. */
+static
+struct b43_dmadesc_generic *op64_idx2desc(struct b43_dmaring *ring,
+					  int slot,
+					  struct b43_dmadesc_meta **meta)
+{
+	struct b43_dmadesc64 *desc;
+
+	*meta = &(ring->meta[slot]);
+	desc = ring->descbase;
+	desc = &(desc[slot]);
+
+	return (struct b43_dmadesc_generic *)desc;
+}
+
+static void op64_fill_descriptor(struct b43_dmaring *ring,
+				 struct b43_dmadesc_generic *desc,
+				 dma_addr_t dmaaddr, u16 bufsize,
+				 int start, int end, int irq)
+{
+	struct b43_dmadesc64 *descbase = ring->descbase;
+	int slot;
+	u32 ctl0 = 0, ctl1 = 0;
+	u32 addrlo, addrhi;
+	u32 addrext;
+
+	slot = (int)(&(desc->dma64) - descbase);
+	B43_WARN_ON(!(slot >= 0 && slot < ring->nr_slots));
+
+	addrlo = (u32) (dmaaddr & 0xFFFFFFFF);
+	addrhi = (((u64) dmaaddr >> 32) & ~SSB_DMA_TRANSLATION_MASK);
+	addrext = (((u64) dmaaddr >> 32) & SSB_DMA_TRANSLATION_MASK)
+	    >> SSB_DMA_TRANSLATION_SHIFT;
+	addrhi |= ssb_dma_translation(ring->dev->dev);
+	if (slot == ring->nr_slots - 1)
+		ctl0 |= B43_DMA64_DCTL0_DTABLEEND;
+	if (start)
+		ctl0 |= B43_DMA64_DCTL0_FRAMESTART;
+	if (end)
+		ctl0 |= B43_DMA64_DCTL0_FRAMEEND;
+	if (irq)
+		ctl0 |= B43_DMA64_DCTL0_IRQ;
+	ctl1 |= (bufsize - ring->frameoffset)
+	    & B43_DMA64_DCTL1_BYTECNT;
+	ctl1 |= (addrext << B43_DMA64_DCTL1_ADDREXT_SHIFT)
+	    & B43_DMA64_DCTL1_ADDREXT_MASK;
+
+	desc->dma64.control0 = cpu_to_le32(ctl0);
+	desc->dma64.control1 = cpu_to_le32(ctl1);
+	desc->dma64.address_low = cpu_to_le32(addrlo);
+	desc->dma64.address_high = cpu_to_le32(addrhi);
+}
+
+static void op64_poke_tx(struct b43_dmaring *ring, int slot)
+{
+	b43_dma_write(ring, B43_DMA64_TXINDEX,
+		      (u32) (slot * sizeof(struct b43_dmadesc64)));
+}
+
+static void op64_tx_suspend(struct b43_dmaring *ring)
+{
+	b43_dma_write(ring, B43_DMA64_TXCTL, b43_dma_read(ring, B43_DMA64_TXCTL)
+		      | B43_DMA64_TXSUSPEND);
+}
+
+static void op64_tx_resume(struct b43_dmaring *ring)
+{
+	b43_dma_write(ring, B43_DMA64_TXCTL, b43_dma_read(ring, B43_DMA64_TXCTL)
+		      & ~B43_DMA64_TXSUSPEND);
+}
+
+static int op64_get_current_rxslot(struct b43_dmaring *ring)
+{
+	u32 val;
+
+	val = b43_dma_read(ring, B43_DMA64_RXSTATUS);
+	val &= B43_DMA64_RXSTATDPTR;
+
+	return (val / sizeof(struct b43_dmadesc64));
+}
+
+static void op64_set_current_rxslot(struct b43_dmaring *ring, int slot)
+{
+	b43_dma_write(ring, B43_DMA64_RXINDEX,
+		      (u32) (slot * sizeof(struct b43_dmadesc64)));
+}
+
+static const struct b43_dma_ops dma64_ops = {
+	.idx2desc = op64_idx2desc,
+	.fill_descriptor = op64_fill_descriptor,
+	.poke_tx = op64_poke_tx,
+	.tx_suspend = op64_tx_suspend,
+	.tx_resume = op64_tx_resume,
+	.get_current_rxslot = op64_get_current_rxslot,
+	.set_current_rxslot = op64_set_current_rxslot,
+};
+
+static inline int free_slots(struct b43_dmaring *ring)
+{
+	return (ring->nr_slots - ring->used_slots);
+}
+
+static inline int next_slot(struct b43_dmaring *ring, int slot)
+{
+	B43_WARN_ON(!(slot >= -1 && slot <= ring->nr_slots - 1));
+	if (slot == ring->nr_slots - 1)
+		return 0;
+	return slot + 1;
+}
+
+static inline int prev_slot(struct b43_dmaring *ring, int slot)
+{
+	B43_WARN_ON(!(slot >= 0 && slot <= ring->nr_slots - 1));
+	if (slot == 0)
+		return ring->nr_slots - 1;
+	return slot - 1;
+}
+
+#ifdef CONFIG_B43_DEBUG
+static void update_max_used_slots(struct b43_dmaring *ring,
+				  int current_used_slots)
+{
+	if (current_used_slots <= ring->max_used_slots)
+		return;
+	ring->max_used_slots = current_used_slots;
+	if (b43_debug(ring->dev, B43_DBG_DMAVERBOSE)) {
+		b43dbg(ring->dev->wl,
+		       "max_used_slots increased to %d on %s ring %d\n",
+		       ring->max_used_slots,
+		       ring->tx ? "TX" : "RX", ring->index);
+	}
+}
+#else
+static inline
+    void update_max_used_slots(struct b43_dmaring *ring, int current_used_slots)
+{
+}
+#endif /* DEBUG */
+
+/* Request a slot for usage. */
+static inline int request_slot(struct b43_dmaring *ring)
+{
+	int slot;
+
+	B43_WARN_ON(!ring->tx);
+	B43_WARN_ON(ring->stopped);
+	B43_WARN_ON(free_slots(ring) == 0);
+
+	slot = next_slot(ring, ring->current_slot);
+	ring->current_slot = slot;
+	ring->used_slots++;
+
+	update_max_used_slots(ring, ring->used_slots);
+
+	return slot;
+}
+
+/* Mac80211-queue to b43-ring mapping */
+static struct b43_dmaring *priority_to_txring(struct b43_wldev *dev,
+					      int queue_priority)
+{
+	struct b43_dmaring *ring;
+
+/*FIXME: For now we always run on TX-ring-1 */
+	return dev->dma.tx_ring1;
+
+	/* 0 = highest priority */
+	switch (queue_priority) {
+	default:
+		B43_WARN_ON(1);
+		/* fallthrough */
+	case 0:
+		ring = dev->dma.tx_ring3;
+		break;
+	case 1:
+		ring = dev->dma.tx_ring2;
+		break;
+	case 2:
+		ring = dev->dma.tx_ring1;
+		break;
+	case 3:
+		ring = dev->dma.tx_ring0;
+		break;
+	case 4:
+		ring = dev->dma.tx_ring4;
+		break;
+	case 5:
+		ring = dev->dma.tx_ring5;
+		break;
+	}
+
+	return ring;
+}
+
+/* Bcm43xx-ring to mac80211-queue mapping */
+static inline int txring_to_priority(struct b43_dmaring *ring)
+{
+	static const u8 idx_to_prio[] = { 3, 2, 1, 0, 4, 5, };
+
+/*FIXME: have only one queue, for now */
+	return 0;
+
+	return idx_to_prio[ring->index];
+}
+
+u16 b43_dmacontroller_base(int dma64bit, int controller_idx)
+{
+	static const u16 map64[] = {
+		B43_MMIO_DMA64_BASE0,
+		B43_MMIO_DMA64_BASE1,
+		B43_MMIO_DMA64_BASE2,
+		B43_MMIO_DMA64_BASE3,
+		B43_MMIO_DMA64_BASE4,
+		B43_MMIO_DMA64_BASE5,
+	};
+	static const u16 map32[] = {
+		B43_MMIO_DMA32_BASE0,
+		B43_MMIO_DMA32_BASE1,
+		B43_MMIO_DMA32_BASE2,
+		B43_MMIO_DMA32_BASE3,
+		B43_MMIO_DMA32_BASE4,
+		B43_MMIO_DMA32_BASE5,
+	};
+
+	if (dma64bit) {
+		B43_WARN_ON(!(controller_idx >= 0 &&
+			      controller_idx < ARRAY_SIZE(map64)));
+		return map64[controller_idx];
+	}
+	B43_WARN_ON(!(controller_idx >= 0 &&
+		      controller_idx < ARRAY_SIZE(map32)));
+	return map32[controller_idx];
+}
+
+static inline
+    dma_addr_t map_descbuffer(struct b43_dmaring *ring,
+			      unsigned char *buf, size_t len, int tx)
+{
+	dma_addr_t dmaaddr;
+
+	if (tx) {
+		dmaaddr = dma_map_single(ring->dev->dev->dev,
+					 buf, len, DMA_TO_DEVICE);
+	} else {
+		dmaaddr = dma_map_single(ring->dev->dev->dev,
+					 buf, len, DMA_FROM_DEVICE);
+	}
+
+	return dmaaddr;
+}
+
+static inline
+    void unmap_descbuffer(struct b43_dmaring *ring,
+			  dma_addr_t addr, size_t len, int tx)
+{
+	if (tx) {
+		dma_unmap_single(ring->dev->dev->dev, addr, len, DMA_TO_DEVICE);
+	} else {
+		dma_unmap_single(ring->dev->dev->dev,
+				 addr, len, DMA_FROM_DEVICE);
+	}
+}
+
+static inline
+    void sync_descbuffer_for_cpu(struct b43_dmaring *ring,
+				 dma_addr_t addr, size_t len)
+{
+	B43_WARN_ON(ring->tx);
+	dma_sync_single_for_cpu(ring->dev->dev->dev,
+				addr, len, DMA_FROM_DEVICE);
+}
+
+static inline
+    void sync_descbuffer_for_device(struct b43_dmaring *ring,
+				    dma_addr_t addr, size_t len)
+{
+	B43_WARN_ON(ring->tx);
+	dma_sync_single_for_device(ring->dev->dev->dev,
+				   addr, len, DMA_FROM_DEVICE);
+}
+
+static inline
+    void free_descriptor_buffer(struct b43_dmaring *ring,
+				struct b43_dmadesc_meta *meta)
+{
+	if (meta->skb) {
+		dev_kfree_skb_any(meta->skb);
+		meta->skb = NULL;
+	}
+}
+
+static int alloc_ringmemory(struct b43_dmaring *ring)
+{
+	struct device *dev = ring->dev->dev->dev;
+
+	ring->descbase = dma_alloc_coherent(dev, B43_DMA_RINGMEMSIZE,
+					    &(ring->dmabase), GFP_KERNEL);
+	if (!ring->descbase) {
+		b43err(ring->dev->wl, "DMA ringmemory allocation failed\n");
+		return -ENOMEM;
+	}
+	memset(ring->descbase, 0, B43_DMA_RINGMEMSIZE);
+
+	return 0;
+}
+
+static void free_ringmemory(struct b43_dmaring *ring)
+{
+	struct device *dev = ring->dev->dev->dev;
+
+	dma_free_coherent(dev, B43_DMA_RINGMEMSIZE,
+			  ring->descbase, ring->dmabase);
+}
+
+/* Reset the RX DMA channel */
+int b43_dmacontroller_rx_reset(struct b43_wldev *dev, u16 mmio_base, int dma64)
+{
+	int i;
+	u32 value;
+	u16 offset;
+
+	might_sleep();
+
+	offset = dma64 ? B43_DMA64_RXCTL : B43_DMA32_RXCTL;
+	b43_write32(dev, mmio_base + offset, 0);
+	for (i = 0; i < 10; i++) {
+		offset = dma64 ? B43_DMA64_RXSTATUS : B43_DMA32_RXSTATUS;
+		value = b43_read32(dev, mmio_base + offset);
+		if (dma64) {
+			value &= B43_DMA64_RXSTAT;
+			if (value == B43_DMA64_RXSTAT_DISABLED) {
+				i = -1;
+				break;
+			}
+		} else {
+			value &= B43_DMA32_RXSTATE;
+			if (value == B43_DMA32_RXSTAT_DISABLED) {
+				i = -1;
+				break;
+			}
+		}
+		msleep(1);
+	}
+	if (i != -1) {
+		b43err(dev->wl, "DMA RX reset timed out\n");
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+/* Reset the RX DMA channel */
+int b43_dmacontroller_tx_reset(struct b43_wldev *dev, u16 mmio_base, int dma64)
+{
+	int i;
+	u32 value;
+	u16 offset;
+
+	might_sleep();
+
+	for (i = 0; i < 10; i++) {
+		offset = dma64 ? B43_DMA64_TXSTATUS : B43_DMA32_TXSTATUS;
+		value = b43_read32(dev, mmio_base + offset);
+		if (dma64) {
+			value &= B43_DMA64_TXSTAT;
+			if (value == B43_DMA64_TXSTAT_DISABLED ||
+			    value == B43_DMA64_TXSTAT_IDLEWAIT ||
+			    value == B43_DMA64_TXSTAT_STOPPED)
+				break;
+		} else {
+			value &= B43_DMA32_TXSTATE;
+			if (value == B43_DMA32_TXSTAT_DISABLED ||
+			    value == B43_DMA32_TXSTAT_IDLEWAIT ||
+			    value == B43_DMA32_TXSTAT_STOPPED)
+				break;
+		}
+		msleep(1);
+	}
+	offset = dma64 ? B43_DMA64_TXCTL : B43_DMA32_TXCTL;
+	b43_write32(dev, mmio_base + offset, 0);
+	for (i = 0; i < 10; i++) {
+		offset = dma64 ? B43_DMA64_TXSTATUS : B43_DMA32_TXSTATUS;
+		value = b43_read32(dev, mmio_base + offset);
+		if (dma64) {
+			value &= B43_DMA64_TXSTAT;
+			if (value == B43_DMA64_TXSTAT_DISABLED) {
+				i = -1;
+				break;
+			}
+		} else {
+			value &= B43_DMA32_TXSTATE;
+			if (value == B43_DMA32_TXSTAT_DISABLED) {
+				i = -1;
+				break;
+			}
+		}
+		msleep(1);
+	}
+	if (i != -1) {
+		b43err(dev->wl, "DMA TX reset timed out\n");
+		return -ENODEV;
+	}
+	/* ensure the reset is completed. */
+	msleep(1);
+
+	return 0;
+}
+
+static int setup_rx_descbuffer(struct b43_dmaring *ring,
+			       struct b43_dmadesc_generic *desc,
+			       struct b43_dmadesc_meta *meta, gfp_t gfp_flags)
+{
+	struct b43_rxhdr_fw4 *rxhdr;
+	struct b43_hwtxstatus *txstat;
+	dma_addr_t dmaaddr;
+	struct sk_buff *skb;
+
+	B43_WARN_ON(ring->tx);
+
+	skb = __dev_alloc_skb(ring->rx_buffersize, gfp_flags);
+	if (unlikely(!skb))
+		return -ENOMEM;
+	dmaaddr = map_descbuffer(ring, skb->data, ring->rx_buffersize, 0);
+	if (dma_mapping_error(dmaaddr)) {
+		/* ugh. try to realloc in zone_dma */
+		gfp_flags |= GFP_DMA;
+
+		dev_kfree_skb_any(skb);
+
+		skb = __dev_alloc_skb(ring->rx_buffersize, gfp_flags);
+		if (unlikely(!skb))
+			return -ENOMEM;
+		dmaaddr = map_descbuffer(ring, skb->data,
+					 ring->rx_buffersize, 0);
+	}
+
+	if (dma_mapping_error(dmaaddr)) {
+		dev_kfree_skb_any(skb);
+		return -EIO;
+	}
+
+	meta->skb = skb;
+	meta->dmaaddr = dmaaddr;
+	ring->ops->fill_descriptor(ring, desc, dmaaddr,
+				   ring->rx_buffersize, 0, 0, 0);
+
+	rxhdr = (struct b43_rxhdr_fw4 *)(skb->data);
+	rxhdr->frame_len = 0;
+	txstat = (struct b43_hwtxstatus *)(skb->data);
+	txstat->cookie = 0;
+
+	return 0;
+}
+
+/* Allocate the initial descbuffers.
+ * This is used for an RX ring only.
+ */
+static int alloc_initial_descbuffers(struct b43_dmaring *ring)
+{
+	int i, err = -ENOMEM;
+	struct b43_dmadesc_generic *desc;
+	struct b43_dmadesc_meta *meta;
+
+	for (i = 0; i < ring->nr_slots; i++) {
+		desc = ring->ops->idx2desc(ring, i, &meta);
+
+		err = setup_rx_descbuffer(ring, desc, meta, GFP_KERNEL);
+		if (err) {
+			b43err(ring->dev->wl,
+			       "Failed to allocate initial descbuffers\n");
+			goto err_unwind;
+		}
+	}
+	mb();
+	ring->used_slots = ring->nr_slots;
+	err = 0;
+      out:
+	return err;
+
+      err_unwind:
+	for (i--; i >= 0; i--) {
+		desc = ring->ops->idx2desc(ring, i, &meta);
+
+		unmap_descbuffer(ring, meta->dmaaddr, ring->rx_buffersize, 0);
+		dev_kfree_skb(meta->skb);
+	}
+	goto out;
+}
+
+/* Do initial setup of the DMA controller.
+ * Reset the controller, write the ring busaddress
+ * and switch the "enable" bit on.
+ */
+static int dmacontroller_setup(struct b43_dmaring *ring)
+{
+	int err = 0;
+	u32 value;
+	u32 addrext;
+	u32 trans = ssb_dma_translation(ring->dev->dev);
+
+	if (ring->tx) {
+		if (ring->dma64) {
+			u64 ringbase = (u64) (ring->dmabase);
+
+			addrext = ((ringbase >> 32) & SSB_DMA_TRANSLATION_MASK)
+			    >> SSB_DMA_TRANSLATION_SHIFT;
+			value = B43_DMA64_TXENABLE;
+			value |= (addrext << B43_DMA64_TXADDREXT_SHIFT)
+			    & B43_DMA64_TXADDREXT_MASK;
+			b43_dma_write(ring, B43_DMA64_TXCTL, value);
+			b43_dma_write(ring, B43_DMA64_TXRINGLO,
+				      (ringbase & 0xFFFFFFFF));
+			b43_dma_write(ring, B43_DMA64_TXRINGHI,
+				      ((ringbase >> 32) &
+				       ~SSB_DMA_TRANSLATION_MASK)
+				      | trans);
+		} else {
+			u32 ringbase = (u32) (ring->dmabase);
+
+			addrext = (ringbase & SSB_DMA_TRANSLATION_MASK)
+			    >> SSB_DMA_TRANSLATION_SHIFT;
+			value = B43_DMA32_TXENABLE;
+			value |= (addrext << B43_DMA32_TXADDREXT_SHIFT)
+			    & B43_DMA32_TXADDREXT_MASK;
+			b43_dma_write(ring, B43_DMA32_TXCTL, value);
+			b43_dma_write(ring, B43_DMA32_TXRING,
+				      (ringbase & ~SSB_DMA_TRANSLATION_MASK)
+				      | trans);
+		}
+	} else {
+		err = alloc_initial_descbuffers(ring);
+		if (err)
+			goto out;
+		if (ring->dma64) {
+			u64 ringbase = (u64) (ring->dmabase);
+
+			addrext = ((ringbase >> 32) & SSB_DMA_TRANSLATION_MASK)
+			    >> SSB_DMA_TRANSLATION_SHIFT;
+			value = (ring->frameoffset << B43_DMA64_RXFROFF_SHIFT);
+			value |= B43_DMA64_RXENABLE;
+			value |= (addrext << B43_DMA64_RXADDREXT_SHIFT)
+			    & B43_DMA64_RXADDREXT_MASK;
+			b43_dma_write(ring, B43_DMA64_RXCTL, value);
+			b43_dma_write(ring, B43_DMA64_RXRINGLO,
+				      (ringbase & 0xFFFFFFFF));
+			b43_dma_write(ring, B43_DMA64_RXRINGHI,
+				      ((ringbase >> 32) &
+				       ~SSB_DMA_TRANSLATION_MASK)
+				      | trans);
+			b43_dma_write(ring, B43_DMA64_RXINDEX, 200);
+		} else {
+			u32 ringbase = (u32) (ring->dmabase);
+
+			addrext = (ringbase & SSB_DMA_TRANSLATION_MASK)
+			    >> SSB_DMA_TRANSLATION_SHIFT;
+			value = (ring->frameoffset << B43_DMA32_RXFROFF_SHIFT);
+			value |= B43_DMA32_RXENABLE;
+			value |= (addrext << B43_DMA32_RXADDREXT_SHIFT)
+			    & B43_DMA32_RXADDREXT_MASK;
+			b43_dma_write(ring, B43_DMA32_RXCTL, value);
+			b43_dma_write(ring, B43_DMA32_RXRING,
+				      (ringbase & ~SSB_DMA_TRANSLATION_MASK)
+				      | trans);
+			b43_dma_write(ring, B43_DMA32_RXINDEX, 200);
+		}
+	}
+
+      out:
+	return err;
+}
+
+/* Shutdown the DMA controller. */
+static void dmacontroller_cleanup(struct b43_dmaring *ring)
+{
+	if (ring->tx) {
+		b43_dmacontroller_tx_reset(ring->dev, ring->mmio_base,
+					   ring->dma64);
+		if (ring->dma64) {
+			b43_dma_write(ring, B43_DMA64_TXRINGLO, 0);
+			b43_dma_write(ring, B43_DMA64_TXRINGHI, 0);
+		} else
+			b43_dma_write(ring, B43_DMA32_TXRING, 0);
+	} else {
+		b43_dmacontroller_rx_reset(ring->dev, ring->mmio_base,
+					   ring->dma64);
+		if (ring->dma64) {
+			b43_dma_write(ring, B43_DMA64_RXRINGLO, 0);
+			b43_dma_write(ring, B43_DMA64_RXRINGHI, 0);
+		} else
+			b43_dma_write(ring, B43_DMA32_RXRING, 0);
+	}
+}
+
+static void free_all_descbuffers(struct b43_dmaring *ring)
+{
+	struct b43_dmadesc_generic *desc;
+	struct b43_dmadesc_meta *meta;
+	int i;
+
+	if (!ring->used_slots)
+		return;
+	for (i = 0; i < ring->nr_slots; i++) {
+		desc = ring->ops->idx2desc(ring, i, &meta);
+
+		if (!meta->skb) {
+			B43_WARN_ON(!ring->tx);
+			continue;
+		}
+		if (ring->tx) {
+			unmap_descbuffer(ring, meta->dmaaddr,
+					 meta->skb->len, 1);
+		} else {
+			unmap_descbuffer(ring, meta->dmaaddr,
+					 ring->rx_buffersize, 0);
+		}
+		free_descriptor_buffer(ring, meta);
+	}
+}
+
+static u64 supported_dma_mask(struct b43_wldev *dev)
+{
+	u32 tmp;
+	u16 mmio_base;
+
+	tmp = b43_read32(dev, SSB_TMSHIGH);
+	if (tmp & SSB_TMSHIGH_DMA64)
+		return DMA_64BIT_MASK;
+	mmio_base = b43_dmacontroller_base(0, 0);
+	b43_write32(dev, mmio_base + B43_DMA32_TXCTL, B43_DMA32_TXADDREXT_MASK);
+	tmp = b43_read32(dev, mmio_base + B43_DMA32_TXCTL);
+	if (tmp & B43_DMA32_TXADDREXT_MASK)
+		return DMA_32BIT_MASK;
+
+	return DMA_30BIT_MASK;
+}
+
+/* Main initialization function. */
+static
+struct b43_dmaring *b43_setup_dmaring(struct b43_wldev *dev,
+				      int controller_index,
+				      int for_tx, int dma64)
+{
+	struct b43_dmaring *ring;
+	int err;
+	int nr_slots;
+	dma_addr_t dma_test;
+
+	ring = kzalloc(sizeof(*ring), GFP_KERNEL);
+	if (!ring)
+		goto out;
+
+	nr_slots = B43_RXRING_SLOTS;
+	if (for_tx)
+		nr_slots = B43_TXRING_SLOTS;
+
+	ring->meta = kcalloc(nr_slots, sizeof(struct b43_dmadesc_meta),
+			     GFP_KERNEL);
+	if (!ring->meta)
+		goto err_kfree_ring;
+	if (for_tx) {
+		ring->txhdr_cache = kcalloc(nr_slots,
+					    sizeof(struct b43_txhdr_fw4),
+					    GFP_KERNEL);
+		if (!ring->txhdr_cache)
+			goto err_kfree_meta;
+
+		/* test for ability to dma to txhdr_cache */
+		dma_test = dma_map_single(dev->dev->dev,
+					  ring->txhdr_cache,
+					  sizeof(struct b43_txhdr_fw4),
+					  DMA_TO_DEVICE);
+
+		if (dma_mapping_error(dma_test)) {
+			/* ugh realloc */
+			kfree(ring->txhdr_cache);
+			ring->txhdr_cache = kcalloc(nr_slots,
+						    sizeof(struct
+							   b43_txhdr_fw4),
+						    GFP_KERNEL | GFP_DMA);
+			if (!ring->txhdr_cache)
+				goto err_kfree_meta;
+
+			dma_test = dma_map_single(dev->dev->dev,
+						  ring->txhdr_cache,
+						  sizeof(struct b43_txhdr_fw4),
+						  DMA_TO_DEVICE);
+
+			if (dma_mapping_error(dma_test))
+				goto err_kfree_txhdr_cache;
+		}
+
+		dma_unmap_single(dev->dev->dev,
+				 dma_test, sizeof(struct b43_txhdr_fw4),
+				 DMA_TO_DEVICE);
+	}
+
+	ring->dev = dev;
+	ring->nr_slots = nr_slots;
+	ring->mmio_base = b43_dmacontroller_base(dma64, controller_index);
+	ring->index = controller_index;
+	ring->dma64 = !!dma64;
+	if (dma64)
+		ring->ops = &dma64_ops;
+	else
+		ring->ops = &dma32_ops;
+	if (for_tx) {
+		ring->tx = 1;
+		ring->current_slot = -1;
+	} else {
+		if (ring->index == 0) {
+			ring->rx_buffersize = B43_DMA0_RX_BUFFERSIZE;
+			ring->frameoffset = B43_DMA0_RX_FRAMEOFFSET;
+		} else if (ring->index == 3) {
+			ring->rx_buffersize = B43_DMA3_RX_BUFFERSIZE;
+			ring->frameoffset = B43_DMA3_RX_FRAMEOFFSET;
+		} else
+			B43_WARN_ON(1);
+	}
+	spin_lock_init(&ring->lock);
+#ifdef CONFIG_B43_DEBUG
+	ring->last_injected_overflow = jiffies;
+#endif
+
+	err = alloc_ringmemory(ring);
+	if (err)
+		goto err_kfree_txhdr_cache;
+	err = dmacontroller_setup(ring);
+	if (err)
+		goto err_free_ringmemory;
+
+      out:
+	return ring;
+
+      err_free_ringmemory:
+	free_ringmemory(ring);
+      err_kfree_txhdr_cache:
+	kfree(ring->txhdr_cache);
+      err_kfree_meta:
+	kfree(ring->meta);
+      err_kfree_ring:
+	kfree(ring);
+	ring = NULL;
+	goto out;
+}
+
+/* Main cleanup function. */
+static void b43_destroy_dmaring(struct b43_dmaring *ring)
+{
+	if (!ring)
+		return;
+
+	b43dbg(ring->dev->wl, "DMA-%s 0x%04X (%s) max used slots: %d/%d\n",
+	       (ring->dma64) ? "64" : "32",
+	       ring->mmio_base,
+	       (ring->tx) ? "TX" : "RX", ring->max_used_slots, ring->nr_slots);
+	/* Device IRQs are disabled prior entering this function,
+	 * so no need to take care of concurrency with rx handler stuff.
+	 */
+	dmacontroller_cleanup(ring);
+	free_all_descbuffers(ring);
+	free_ringmemory(ring);
+
+	kfree(ring->txhdr_cache);
+	kfree(ring->meta);
+	kfree(ring);
+}
+
+void b43_dma_free(struct b43_wldev *dev)
+{
+	struct b43_dma *dma;
+
+	if (b43_using_pio(dev))
+		return;
+	dma = &dev->dma;
+
+	b43_destroy_dmaring(dma->rx_ring3);
+	dma->rx_ring3 = NULL;
+	b43_destroy_dmaring(dma->rx_ring0);
+	dma->rx_ring0 = NULL;
+
+	b43_destroy_dmaring(dma->tx_ring5);
+	dma->tx_ring5 = NULL;
+	b43_destroy_dmaring(dma->tx_ring4);
+	dma->tx_ring4 = NULL;
+	b43_destroy_dmaring(dma->tx_ring3);
+	dma->tx_ring3 = NULL;
+	b43_destroy_dmaring(dma->tx_ring2);
+	dma->tx_ring2 = NULL;
+	b43_destroy_dmaring(dma->tx_ring1);
+	dma->tx_ring1 = NULL;
+	b43_destroy_dmaring(dma->tx_ring0);
+	dma->tx_ring0 = NULL;
+}
+
+int b43_dma_init(struct b43_wldev *dev)
+{
+	struct b43_dma *dma = &dev->dma;
+	struct b43_dmaring *ring;
+	int err;
+	u64 dmamask;
+	int dma64 = 0;
+
+	dmamask = supported_dma_mask(dev);
+	if (dmamask == DMA_64BIT_MASK)
+		dma64 = 1;
+
+	err = ssb_dma_set_mask(dev->dev, dmamask);
+	if (err) {
+#ifdef B43_PIO
+		b43warn(dev->wl, "DMA for this device not supported. "
+			"Falling back to PIO\n");
+		dev->__using_pio = 1;
+		return -EAGAIN;
+#else
+		b43err(dev->wl, "DMA for this device not supported and "
+		       "no PIO support compiled in\n");
+		return -EOPNOTSUPP;
+#endif
+	}
+
+	err = -ENOMEM;
+	/* setup TX DMA channels. */
+	ring = b43_setup_dmaring(dev, 0, 1, dma64);
+	if (!ring)
+		goto out;
+	dma->tx_ring0 = ring;
+
+	ring = b43_setup_dmaring(dev, 1, 1, dma64);
+	if (!ring)
+		goto err_destroy_tx0;
+	dma->tx_ring1 = ring;
+
+	ring = b43_setup_dmaring(dev, 2, 1, dma64);
+	if (!ring)
+		goto err_destroy_tx1;
+	dma->tx_ring2 = ring;
+
+	ring = b43_setup_dmaring(dev, 3, 1, dma64);
+	if (!ring)
+		goto err_destroy_tx2;
+	dma->tx_ring3 = ring;
+
+	ring = b43_setup_dmaring(dev, 4, 1, dma64);
+	if (!ring)
+		goto err_destroy_tx3;
+	dma->tx_ring4 = ring;
+
+	ring = b43_setup_dmaring(dev, 5, 1, dma64);
+	if (!ring)
+		goto err_destroy_tx4;
+	dma->tx_ring5 = ring;
+
+	/* setup RX DMA channels. */
+	ring = b43_setup_dmaring(dev, 0, 0, dma64);
+	if (!ring)
+		goto err_destroy_tx5;
+	dma->rx_ring0 = ring;
+
+	if (dev->dev->id.revision < 5) {
+		ring = b43_setup_dmaring(dev, 3, 0, dma64);
+		if (!ring)
+			goto err_destroy_rx0;
+		dma->rx_ring3 = ring;
+	}
+
+	b43dbg(dev->wl, "%d-bit DMA initialized\n",
+	       (dmamask == DMA_64BIT_MASK) ? 64 :
+	       (dmamask == DMA_32BIT_MASK) ? 32 : 30);
+	err = 0;
+      out:
+	return err;
+
+      err_destroy_rx0:
+	b43_destroy_dmaring(dma->rx_ring0);
+	dma->rx_ring0 = NULL;
+      err_destroy_tx5:
+	b43_destroy_dmaring(dma->tx_ring5);
+	dma->tx_ring5 = NULL;
+      err_destroy_tx4:
+	b43_destroy_dmaring(dma->tx_ring4);
+	dma->tx_ring4 = NULL;
+      err_destroy_tx3:
+	b43_destroy_dmaring(dma->tx_ring3);
+	dma->tx_ring3 = NULL;
+      err_destroy_tx2:
+	b43_destroy_dmaring(dma->tx_ring2);
+	dma->tx_ring2 = NULL;
+      err_destroy_tx1:
+	b43_destroy_dmaring(dma->tx_ring1);
+	dma->tx_ring1 = NULL;
+      err_destroy_tx0:
+	b43_destroy_dmaring(dma->tx_ring0);
+	dma->tx_ring0 = NULL;
+	goto out;
+}
+
+/* Generate a cookie for the TX header. */
+static u16 generate_cookie(struct b43_dmaring *ring, int slot)
+{
+	u16 cookie = 0x1000;
+
+	/* Use the upper 4 bits of the cookie as
+	 * DMA controller ID and store the slot number
+	 * in the lower 12 bits.
+	 * Note that the cookie must never be 0, as this
+	 * is a special value used in RX path.
+	 */
+	switch (ring->index) {
+	case 0:
+		cookie = 0xA000;
+		break;
+	case 1:
+		cookie = 0xB000;
+		break;
+	case 2:
+		cookie = 0xC000;
+		break;
+	case 3:
+		cookie = 0xD000;
+		break;
+	case 4:
+		cookie = 0xE000;
+		break;
+	case 5:
+		cookie = 0xF000;
+		break;
+	}
+	B43_WARN_ON(slot & ~0x0FFF);
+	cookie |= (u16) slot;
+
+	return cookie;
+}
+
+/* Inspect a cookie and find out to which controller/slot it belongs. */
+static
+struct b43_dmaring *parse_cookie(struct b43_wldev *dev, u16 cookie, int *slot)
+{
+	struct b43_dma *dma = &dev->dma;
+	struct b43_dmaring *ring = NULL;
+
+	switch (cookie & 0xF000) {
+	case 0xA000:
+		ring = dma->tx_ring0;
+		break;
+	case 0xB000:
+		ring = dma->tx_ring1;
+		break;
+	case 0xC000:
+		ring = dma->tx_ring2;
+		break;
+	case 0xD000:
+		ring = dma->tx_ring3;
+		break;
+	case 0xE000:
+		ring = dma->tx_ring4;
+		break;
+	case 0xF000:
+		ring = dma->tx_ring5;
+		break;
+	default:
+		B43_WARN_ON(1);
+	}
+	*slot = (cookie & 0x0FFF);
+	B43_WARN_ON(!(ring && *slot >= 0 && *slot < ring->nr_slots));
+
+	return ring;
+}
+
+static int dma_tx_fragment(struct b43_dmaring *ring,
+			   struct sk_buff *skb,
+			   struct ieee80211_tx_control *ctl)
+{
+	const struct b43_dma_ops *ops = ring->ops;
+	u8 *header;
+	int slot;
+	int err;
+	struct b43_dmadesc_generic *desc;
+	struct b43_dmadesc_meta *meta;
+	struct b43_dmadesc_meta *meta_hdr;
+	struct sk_buff *bounce_skb;
+
+#define SLOTS_PER_PACKET  2
+	B43_WARN_ON(skb_shinfo(skb)->nr_frags);
+
+	/* Get a slot for the header. */
+	slot = request_slot(ring);
+	desc = ops->idx2desc(ring, slot, &meta_hdr);
+	memset(meta_hdr, 0, sizeof(*meta_hdr));
+
+	header = &(ring->txhdr_cache[slot * sizeof(struct b43_txhdr_fw4)]);
+	b43_generate_txhdr(ring->dev, header,
+			   skb->data, skb->len, ctl,
+			   generate_cookie(ring, slot));
+
+	meta_hdr->dmaaddr = map_descbuffer(ring, (unsigned char *)header,
+					   sizeof(struct b43_txhdr_fw4), 1);
+	if (dma_mapping_error(meta_hdr->dmaaddr))
+		return -EIO;
+	ops->fill_descriptor(ring, desc, meta_hdr->dmaaddr,
+			     sizeof(struct b43_txhdr_fw4), 1, 0, 0);
+
+	/* Get a slot for the payload. */
+	slot = request_slot(ring);
+	desc = ops->idx2desc(ring, slot, &meta);
+	memset(meta, 0, sizeof(*meta));
+
+	memcpy(&meta->txstat.control, ctl, sizeof(*ctl));
+	meta->skb = skb;
+	meta->is_last_fragment = 1;
+
+	meta->dmaaddr = map_descbuffer(ring, skb->data, skb->len, 1);
+	/* create a bounce buffer in zone_dma on mapping failure. */
+	if (dma_mapping_error(meta->dmaaddr)) {
+		bounce_skb = __dev_alloc_skb(skb->len, GFP_ATOMIC | GFP_DMA);
+		if (!bounce_skb) {
+			err = -ENOMEM;
+			goto out_unmap_hdr;
+		}
+
+		memcpy(skb_put(bounce_skb, skb->len), skb->data, skb->len);
+		dev_kfree_skb_any(skb);
+		skb = bounce_skb;
+		meta->skb = skb;
+		meta->dmaaddr = map_descbuffer(ring, skb->data, skb->len, 1);
+		if (dma_mapping_error(meta->dmaaddr)) {
+			err = -EIO;
+			goto out_free_bounce;
+		}
+	}
+
+	ops->fill_descriptor(ring, desc, meta->dmaaddr, skb->len, 0, 1, 1);
+
+	/* Now transfer the whole frame. */
+	wmb();
+	ops->poke_tx(ring, next_slot(ring, slot));
+	return 0;
+
+      out_free_bounce:
+	dev_kfree_skb_any(skb);
+      out_unmap_hdr:
+	unmap_descbuffer(ring, meta_hdr->dmaaddr,
+			 sizeof(struct b43_txhdr_fw4), 1);
+	return err;
+}
+
+static inline int should_inject_overflow(struct b43_dmaring *ring)
+{
+#ifdef CONFIG_B43_DEBUG
+	if (unlikely(b43_debug(ring->dev, B43_DBG_DMAOVERFLOW))) {
+		/* Check if we should inject another ringbuffer overflow
+		 * to test handling of this situation in the stack. */
+		unsigned long next_overflow;
+
+		next_overflow = ring->last_injected_overflow + HZ;
+		if (time_after(jiffies, next_overflow)) {
+			ring->last_injected_overflow = jiffies;
+			b43dbg(ring->dev->wl,
+			       "Injecting TX ring overflow on "
+			       "DMA controller %d\n", ring->index);
+			return 1;
+		}
+	}
+#endif /* CONFIG_B43_DEBUG */
+	return 0;
+}
+
+int b43_dma_tx(struct b43_wldev *dev,
+	       struct sk_buff *skb, struct ieee80211_tx_control *ctl)
+{
+	struct b43_dmaring *ring;
+	int err = 0;
+	unsigned long flags;
+
+	ring = priority_to_txring(dev, ctl->queue);
+	spin_lock_irqsave(&ring->lock, flags);
+	B43_WARN_ON(!ring->tx);
+	if (unlikely(free_slots(ring) < SLOTS_PER_PACKET)) {
+		b43warn(dev->wl, "DMA queue overflow\n");
+		err = -ENOSPC;
+		goto out_unlock;
+	}
+	/* Check if the queue was stopped in mac80211,
+	 * but we got called nevertheless.
+	 * That would be a mac80211 bug. */
+	B43_WARN_ON(ring->stopped);
+
+	err = dma_tx_fragment(ring, skb, ctl);
+	if (unlikely(err)) {
+		b43err(dev->wl, "DMA tx mapping failure\n");
+		goto out_unlock;
+	}
+	ring->nr_tx_packets++;
+	if ((free_slots(ring) < SLOTS_PER_PACKET) ||
+	    should_inject_overflow(ring)) {
+		/* This TX ring is full. */
+		ieee80211_stop_queue(dev->wl->hw, txring_to_priority(ring));
+		ring->stopped = 1;
+		if (b43_debug(dev, B43_DBG_DMAVERBOSE)) {
+			b43dbg(dev->wl, "Stopped TX ring %d\n", ring->index);
+		}
+	}
+      out_unlock:
+	spin_unlock_irqrestore(&ring->lock, flags);
+
+	return err;
+}
+
+void b43_dma_handle_txstatus(struct b43_wldev *dev,
+			     const struct b43_txstatus *status)
+{
+	const struct b43_dma_ops *ops;
+	struct b43_dmaring *ring;
+	struct b43_dmadesc_generic *desc;
+	struct b43_dmadesc_meta *meta;
+	int slot;
+
+	ring = parse_cookie(dev, status->cookie, &slot);
+	if (unlikely(!ring))
+		return;
+	B43_WARN_ON(!irqs_disabled());
+	spin_lock(&ring->lock);
+
+	B43_WARN_ON(!ring->tx);
+	ops = ring->ops;
+	while (1) {
+		B43_WARN_ON(!(slot >= 0 && slot < ring->nr_slots));
+		desc = ops->idx2desc(ring, slot, &meta);
+
+		if (meta->skb)
+			unmap_descbuffer(ring, meta->dmaaddr, meta->skb->len,
+					 1);
+		else
+			unmap_descbuffer(ring, meta->dmaaddr,
+					 sizeof(struct b43_txhdr_fw4), 1);
+
+		if (meta->is_last_fragment) {
+			B43_WARN_ON(!meta->skb);
+			/* Call back to inform the ieee80211 subsystem about the
+			 * status of the transmission.
+			 * Some fields of txstat are already filled in dma_tx().
+			 */
+			if (status->acked) {
+				meta->txstat.flags |= IEEE80211_TX_STATUS_ACK;
+			} else {
+				if (!(meta->txstat.control.flags
+				      & IEEE80211_TXCTL_NO_ACK))
+					meta->txstat.excessive_retries = 1;
+			}
+			if (status->frame_count == 0) {
+				/* The frame was not transmitted at all. */
+				meta->txstat.retry_count = 0;
+			} else
+				meta->txstat.retry_count = status->frame_count - 1;
+			ieee80211_tx_status_irqsafe(dev->wl->hw, meta->skb,
+						    &(meta->txstat));
+			/* skb is freed by ieee80211_tx_status_irqsafe() */
+			meta->skb = NULL;
+		} else {
+			/* No need to call free_descriptor_buffer here, as
+			 * this is only the txhdr, which is not allocated.
+			 */
+			B43_WARN_ON(meta->skb);
+		}
+
+		/* Everything unmapped and free'd. So it's not used anymore. */
+		ring->used_slots--;
+
+		if (meta->is_last_fragment)
+			break;
+		slot = next_slot(ring, slot);
+	}
+	dev->stats.last_tx = jiffies;
+	if (ring->stopped) {
+		B43_WARN_ON(free_slots(ring) < SLOTS_PER_PACKET);
+		ieee80211_wake_queue(dev->wl->hw, txring_to_priority(ring));
+		ring->stopped = 0;
+		if (b43_debug(dev, B43_DBG_DMAVERBOSE)) {
+			b43dbg(dev->wl, "Woke up TX ring %d\n", ring->index);
+		}
+	}
+
+	spin_unlock(&ring->lock);
+}
+
+void b43_dma_get_tx_stats(struct b43_wldev *dev,
+			  struct ieee80211_tx_queue_stats *stats)
+{
+	const int nr_queues = dev->wl->hw->queues;
+	struct b43_dmaring *ring;
+	struct ieee80211_tx_queue_stats_data *data;
+	unsigned long flags;
+	int i;
+
+	for (i = 0; i < nr_queues; i++) {
+		data = &(stats->data[i]);
+		ring = priority_to_txring(dev, i);
+
+		spin_lock_irqsave(&ring->lock, flags);
+		data->len = ring->used_slots / SLOTS_PER_PACKET;
+		data->limit = ring->nr_slots / SLOTS_PER_PACKET;
+		data->count = ring->nr_tx_packets;
+		spin_unlock_irqrestore(&ring->lock, flags);
+	}
+}
+
+static void dma_rx(struct b43_dmaring *ring, int *slot)
+{
+	const struct b43_dma_ops *ops = ring->ops;
+	struct b43_dmadesc_generic *desc;
+	struct b43_dmadesc_meta *meta;
+	struct b43_rxhdr_fw4 *rxhdr;
+	struct sk_buff *skb;
+	u16 len;
+	int err;
+	dma_addr_t dmaaddr;
+
+	desc = ops->idx2desc(ring, *slot, &meta);
+
+	sync_descbuffer_for_cpu(ring, meta->dmaaddr, ring->rx_buffersize);
+	skb = meta->skb;
+
+	if (ring->index == 3) {
+		/* We received an xmit status. */
+		struct b43_hwtxstatus *hw = (struct b43_hwtxstatus *)skb->data;
+		int i = 0;
+
+		while (hw->cookie == 0) {
+			if (i > 100)
+				break;
+			i++;
+			udelay(2);
+			barrier();
+		}
+		b43_handle_hwtxstatus(ring->dev, hw);
+		/* recycle the descriptor buffer. */
+		sync_descbuffer_for_device(ring, meta->dmaaddr,
+					   ring->rx_buffersize);
+
+		return;
+	}
+	rxhdr = (struct b43_rxhdr_fw4 *)skb->data;
+	len = le16_to_cpu(rxhdr->frame_len);
+	if (len == 0) {
+		int i = 0;
+
+		do {
+			udelay(2);
+			barrier();
+			len = le16_to_cpu(rxhdr->frame_len);
+		} while (len == 0 && i++ < 5);
+		if (unlikely(len == 0)) {
+			/* recycle the descriptor buffer. */
+			sync_descbuffer_for_device(ring, meta->dmaaddr,
+						   ring->rx_buffersize);
+			goto drop;
+		}
+	}
+	if (unlikely(len > ring->rx_buffersize)) {
+		/* The data did not fit into one descriptor buffer
+		 * and is split over multiple buffers.
+		 * This should never happen, as we try to allocate buffers
+		 * big enough. So simply ignore this packet.
+		 */
+		int cnt = 0;
+		s32 tmp = len;
+
+		while (1) {
+			desc = ops->idx2desc(ring, *slot, &meta);
+			/* recycle the descriptor buffer. */
+			sync_descbuffer_for_device(ring, meta->dmaaddr,
+						   ring->rx_buffersize);
+			*slot = next_slot(ring, *slot);
+			cnt++;
+			tmp -= ring->rx_buffersize;
+			if (tmp <= 0)
+				break;
+		}
+		b43err(ring->dev->wl, "DMA RX buffer too small "
+		       "(len: %u, buffer: %u, nr-dropped: %d)\n",
+		       len, ring->rx_buffersize, cnt);
+		goto drop;
+	}
+
+	dmaaddr = meta->dmaaddr;
+	err = setup_rx_descbuffer(ring, desc, meta, GFP_ATOMIC);
+	if (unlikely(err)) {
+		b43dbg(ring->dev->wl, "DMA RX: setup_rx_descbuffer() failed\n");
+		sync_descbuffer_for_device(ring, dmaaddr, ring->rx_buffersize);
+		goto drop;
+	}
+
+	unmap_descbuffer(ring, dmaaddr, ring->rx_buffersize, 0);
+	skb_put(skb, len + ring->frameoffset);
+	skb_pull(skb, ring->frameoffset);
+
+	b43_rx(ring->dev, skb, rxhdr);
+      drop:
+	return;
+}
+
+void b43_dma_rx(struct b43_dmaring *ring)
+{
+	const struct b43_dma_ops *ops = ring->ops;
+	int slot, current_slot;
+	int used_slots = 0;
+
+	B43_WARN_ON(ring->tx);
+	current_slot = ops->get_current_rxslot(ring);
+	B43_WARN_ON(!(current_slot >= 0 && current_slot < ring->nr_slots));
+
+	slot = ring->current_slot;
+	for (; slot != current_slot; slot = next_slot(ring, slot)) {
+		dma_rx(ring, &slot);
+		update_max_used_slots(ring, ++used_slots);
+	}
+	ops->set_current_rxslot(ring, slot);
+	ring->current_slot = slot;
+}
+
+static void b43_dma_tx_suspend_ring(struct b43_dmaring *ring)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&ring->lock, flags);
+	B43_WARN_ON(!ring->tx);
+	ring->ops->tx_suspend(ring);
+	spin_unlock_irqrestore(&ring->lock, flags);
+}
+
+static void b43_dma_tx_resume_ring(struct b43_dmaring *ring)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&ring->lock, flags);
+	B43_WARN_ON(!ring->tx);
+	ring->ops->tx_resume(ring);
+	spin_unlock_irqrestore(&ring->lock, flags);
+}
+
+void b43_dma_tx_suspend(struct b43_wldev *dev)
+{
+	b43_power_saving_ctl_bits(dev, B43_PS_AWAKE);
+	b43_dma_tx_suspend_ring(dev->dma.tx_ring0);
+	b43_dma_tx_suspend_ring(dev->dma.tx_ring1);
+	b43_dma_tx_suspend_ring(dev->dma.tx_ring2);
+	b43_dma_tx_suspend_ring(dev->dma.tx_ring3);
+	b43_dma_tx_suspend_ring(dev->dma.tx_ring4);
+	b43_dma_tx_suspend_ring(dev->dma.tx_ring5);
+}
+
+void b43_dma_tx_resume(struct b43_wldev *dev)
+{
+	b43_dma_tx_resume_ring(dev->dma.tx_ring5);
+	b43_dma_tx_resume_ring(dev->dma.tx_ring4);
+	b43_dma_tx_resume_ring(dev->dma.tx_ring3);
+	b43_dma_tx_resume_ring(dev->dma.tx_ring2);
+	b43_dma_tx_resume_ring(dev->dma.tx_ring1);
+	b43_dma_tx_resume_ring(dev->dma.tx_ring0);
+	b43_power_saving_ctl_bits(dev, 0);
+}
diff --git a/drivers/net/wireless/b43/dma.h b/drivers/net/wireless/b43/dma.h
new file mode 100644
index 000000000000..3eed185be725
--- /dev/null
+++ b/drivers/net/wireless/b43/dma.h
@@ -0,0 +1,337 @@
+#ifndef B43_DMA_H_
+#define B43_DMA_H_
+
+#include <linux/list.h>
+#include <linux/spinlock.h>
+#include <linux/workqueue.h>
+#include <linux/linkage.h>
+#include <asm/atomic.h>
+
+#include "b43.h"
+
+/* DMA-Interrupt reasons. */
+#define B43_DMAIRQ_FATALMASK	((1 << 10) | (1 << 11) | (1 << 12) \
+					 | (1 << 14) | (1 << 15))
+#define B43_DMAIRQ_NONFATALMASK	(1 << 13)
+#define B43_DMAIRQ_RX_DONE		(1 << 16)
+
+/*** 32-bit DMA Engine. ***/
+
+/* 32-bit DMA controller registers. */
+#define B43_DMA32_TXCTL				0x00
+#define		B43_DMA32_TXENABLE			0x00000001
+#define		B43_DMA32_TXSUSPEND			0x00000002
+#define		B43_DMA32_TXLOOPBACK		0x00000004
+#define		B43_DMA32_TXFLUSH			0x00000010
+#define		B43_DMA32_TXADDREXT_MASK		0x00030000
+#define		B43_DMA32_TXADDREXT_SHIFT		16
+#define B43_DMA32_TXRING				0x04
+#define B43_DMA32_TXINDEX				0x08
+#define B43_DMA32_TXSTATUS				0x0C
+#define		B43_DMA32_TXDPTR			0x00000FFF
+#define		B43_DMA32_TXSTATE			0x0000F000
+#define			B43_DMA32_TXSTAT_DISABLED	0x00000000
+#define			B43_DMA32_TXSTAT_ACTIVE	0x00001000
+#define			B43_DMA32_TXSTAT_IDLEWAIT	0x00002000
+#define			B43_DMA32_TXSTAT_STOPPED	0x00003000
+#define			B43_DMA32_TXSTAT_SUSP	0x00004000
+#define		B43_DMA32_TXERROR			0x000F0000
+#define			B43_DMA32_TXERR_NOERR	0x00000000
+#define			B43_DMA32_TXERR_PROT	0x00010000
+#define			B43_DMA32_TXERR_UNDERRUN	0x00020000
+#define			B43_DMA32_TXERR_BUFREAD	0x00030000
+#define			B43_DMA32_TXERR_DESCREAD	0x00040000
+#define		B43_DMA32_TXACTIVE			0xFFF00000
+#define B43_DMA32_RXCTL				0x10
+#define		B43_DMA32_RXENABLE			0x00000001
+#define		B43_DMA32_RXFROFF_MASK		0x000000FE
+#define		B43_DMA32_RXFROFF_SHIFT		1
+#define		B43_DMA32_RXDIRECTFIFO		0x00000100
+#define		B43_DMA32_RXADDREXT_MASK		0x00030000
+#define		B43_DMA32_RXADDREXT_SHIFT		16
+#define B43_DMA32_RXRING				0x14
+#define B43_DMA32_RXINDEX				0x18
+#define B43_DMA32_RXSTATUS				0x1C
+#define		B43_DMA32_RXDPTR			0x00000FFF
+#define		B43_DMA32_RXSTATE			0x0000F000
+#define			B43_DMA32_RXSTAT_DISABLED	0x00000000
+#define			B43_DMA32_RXSTAT_ACTIVE	0x00001000
+#define			B43_DMA32_RXSTAT_IDLEWAIT	0x00002000
+#define			B43_DMA32_RXSTAT_STOPPED	0x00003000
+#define		B43_DMA32_RXERROR			0x000F0000
+#define			B43_DMA32_RXERR_NOERR	0x00000000
+#define			B43_DMA32_RXERR_PROT	0x00010000
+#define			B43_DMA32_RXERR_OVERFLOW	0x00020000
+#define			B43_DMA32_RXERR_BUFWRITE	0x00030000
+#define			B43_DMA32_RXERR_DESCREAD	0x00040000
+#define		B43_DMA32_RXACTIVE			0xFFF00000
+
+/* 32-bit DMA descriptor. */
+struct b43_dmadesc32 {
+	__le32 control;
+	__le32 address;
+} __attribute__ ((__packed__));
+#define B43_DMA32_DCTL_BYTECNT		0x00001FFF
+#define B43_DMA32_DCTL_ADDREXT_MASK		0x00030000
+#define B43_DMA32_DCTL_ADDREXT_SHIFT	16
+#define B43_DMA32_DCTL_DTABLEEND		0x10000000
+#define B43_DMA32_DCTL_IRQ			0x20000000
+#define B43_DMA32_DCTL_FRAMEEND		0x40000000
+#define B43_DMA32_DCTL_FRAMESTART		0x80000000
+
+/*** 64-bit DMA Engine. ***/
+
+/* 64-bit DMA controller registers. */
+#define B43_DMA64_TXCTL				0x00
+#define		B43_DMA64_TXENABLE			0x00000001
+#define		B43_DMA64_TXSUSPEND			0x00000002
+#define		B43_DMA64_TXLOOPBACK		0x00000004
+#define		B43_DMA64_TXFLUSH			0x00000010
+#define		B43_DMA64_TXADDREXT_MASK		0x00030000
+#define		B43_DMA64_TXADDREXT_SHIFT		16
+#define B43_DMA64_TXINDEX				0x04
+#define B43_DMA64_TXRINGLO				0x08
+#define B43_DMA64_TXRINGHI				0x0C
+#define B43_DMA64_TXSTATUS				0x10
+#define		B43_DMA64_TXSTATDPTR		0x00001FFF
+#define		B43_DMA64_TXSTAT			0xF0000000
+#define			B43_DMA64_TXSTAT_DISABLED	0x00000000
+#define			B43_DMA64_TXSTAT_ACTIVE	0x10000000
+#define			B43_DMA64_TXSTAT_IDLEWAIT	0x20000000
+#define			B43_DMA64_TXSTAT_STOPPED	0x30000000
+#define			B43_DMA64_TXSTAT_SUSP	0x40000000
+#define B43_DMA64_TXERROR				0x14
+#define		B43_DMA64_TXERRDPTR			0x0001FFFF
+#define		B43_DMA64_TXERR			0xF0000000
+#define			B43_DMA64_TXERR_NOERR	0x00000000
+#define			B43_DMA64_TXERR_PROT	0x10000000
+#define			B43_DMA64_TXERR_UNDERRUN	0x20000000
+#define			B43_DMA64_TXERR_TRANSFER	0x30000000
+#define			B43_DMA64_TXERR_DESCREAD	0x40000000
+#define			B43_DMA64_TXERR_CORE	0x50000000
+#define B43_DMA64_RXCTL				0x20
+#define		B43_DMA64_RXENABLE			0x00000001
+#define		B43_DMA64_RXFROFF_MASK		0x000000FE
+#define		B43_DMA64_RXFROFF_SHIFT		1
+#define		B43_DMA64_RXDIRECTFIFO		0x00000100
+#define		B43_DMA64_RXADDREXT_MASK		0x00030000
+#define		B43_DMA64_RXADDREXT_SHIFT		16
+#define B43_DMA64_RXINDEX				0x24
+#define B43_DMA64_RXRINGLO				0x28
+#define B43_DMA64_RXRINGHI				0x2C
+#define B43_DMA64_RXSTATUS				0x30
+#define		B43_DMA64_RXSTATDPTR		0x00001FFF
+#define		B43_DMA64_RXSTAT			0xF0000000
+#define			B43_DMA64_RXSTAT_DISABLED	0x00000000
+#define			B43_DMA64_RXSTAT_ACTIVE	0x10000000
+#define			B43_DMA64_RXSTAT_IDLEWAIT	0x20000000
+#define			B43_DMA64_RXSTAT_STOPPED	0x30000000
+#define			B43_DMA64_RXSTAT_SUSP	0x40000000
+#define B43_DMA64_RXERROR				0x34
+#define		B43_DMA64_RXERRDPTR			0x0001FFFF
+#define		B43_DMA64_RXERR			0xF0000000
+#define			B43_DMA64_RXERR_NOERR	0x00000000
+#define			B43_DMA64_RXERR_PROT	0x10000000
+#define			B43_DMA64_RXERR_UNDERRUN	0x20000000
+#define			B43_DMA64_RXERR_TRANSFER	0x30000000
+#define			B43_DMA64_RXERR_DESCREAD	0x40000000
+#define			B43_DMA64_RXERR_CORE	0x50000000
+
+/* 64-bit DMA descriptor. */
+struct b43_dmadesc64 {
+	__le32 control0;
+	__le32 control1;
+	__le32 address_low;
+	__le32 address_high;
+} __attribute__ ((__packed__));
+#define B43_DMA64_DCTL0_DTABLEEND		0x10000000
+#define B43_DMA64_DCTL0_IRQ			0x20000000
+#define B43_DMA64_DCTL0_FRAMEEND		0x40000000
+#define B43_DMA64_DCTL0_FRAMESTART		0x80000000
+#define B43_DMA64_DCTL1_BYTECNT		0x00001FFF
+#define B43_DMA64_DCTL1_ADDREXT_MASK	0x00030000
+#define B43_DMA64_DCTL1_ADDREXT_SHIFT	16
+
+struct b43_dmadesc_generic {
+	union {
+		struct b43_dmadesc32 dma32;
+		struct b43_dmadesc64 dma64;
+	} __attribute__ ((__packed__));
+} __attribute__ ((__packed__));
+
+/* Misc DMA constants */
+#define B43_DMA_RINGMEMSIZE		PAGE_SIZE
+#define B43_DMA0_RX_FRAMEOFFSET	30
+#define B43_DMA3_RX_FRAMEOFFSET	0
+
+/* DMA engine tuning knobs */
+#define B43_TXRING_SLOTS		128
+#define B43_RXRING_SLOTS		64
+#define B43_DMA0_RX_BUFFERSIZE	(2304 + 100)
+#define B43_DMA3_RX_BUFFERSIZE	16
+
+#ifdef CONFIG_B43_DMA
+
+struct sk_buff;
+struct b43_private;
+struct b43_txstatus;
+
+struct b43_dmadesc_meta {
+	/* The kernel DMA-able buffer. */
+	struct sk_buff *skb;
+	/* DMA base bus-address of the descriptor buffer. */
+	dma_addr_t dmaaddr;
+	/* ieee80211 TX status. Only used once per 802.11 frag. */
+	bool is_last_fragment;
+	struct ieee80211_tx_status txstat;
+};
+
+struct b43_dmaring;
+
+/* Lowlevel DMA operations that differ between 32bit and 64bit DMA. */
+struct b43_dma_ops {
+	struct b43_dmadesc_generic *(*idx2desc) (struct b43_dmaring * ring,
+						 int slot,
+						 struct b43_dmadesc_meta **
+						 meta);
+	void (*fill_descriptor) (struct b43_dmaring * ring,
+				 struct b43_dmadesc_generic * desc,
+				 dma_addr_t dmaaddr, u16 bufsize, int start,
+				 int end, int irq);
+	void (*poke_tx) (struct b43_dmaring * ring, int slot);
+	void (*tx_suspend) (struct b43_dmaring * ring);
+	void (*tx_resume) (struct b43_dmaring * ring);
+	int (*get_current_rxslot) (struct b43_dmaring * ring);
+	void (*set_current_rxslot) (struct b43_dmaring * ring, int slot);
+};
+
+struct b43_dmaring {
+	/* Lowlevel DMA ops. */
+	const struct b43_dma_ops *ops;
+	/* Kernel virtual base address of the ring memory. */
+	void *descbase;
+	/* Meta data about all descriptors. */
+	struct b43_dmadesc_meta *meta;
+	/* Cache of TX headers for each slot.
+	 * This is to avoid an allocation on each TX.
+	 * This is NULL for an RX ring.
+	 */
+	u8 *txhdr_cache;
+	/* (Unadjusted) DMA base bus-address of the ring memory. */
+	dma_addr_t dmabase;
+	/* Number of descriptor slots in the ring. */
+	int nr_slots;
+	/* Number of used descriptor slots. */
+	int used_slots;
+	/* Currently used slot in the ring. */
+	int current_slot;
+	/* Total number of packets sent. Statistics only. */
+	unsigned int nr_tx_packets;
+	/* Frameoffset in octets. */
+	u32 frameoffset;
+	/* Descriptor buffer size. */
+	u16 rx_buffersize;
+	/* The MMIO base register of the DMA controller. */
+	u16 mmio_base;
+	/* DMA controller index number (0-5). */
+	int index;
+	/* Boolean. Is this a TX ring? */
+	bool tx;
+	/* Boolean. 64bit DMA if true, 32bit DMA otherwise. */
+	bool dma64;
+	/* Boolean. Is this ring stopped at ieee80211 level? */
+	bool stopped;
+	/* Lock, only used for TX. */
+	spinlock_t lock;
+	struct b43_wldev *dev;
+#ifdef CONFIG_B43_DEBUG
+	/* Maximum number of used slots. */
+	int max_used_slots;
+	/* Last time we injected a ring overflow. */
+	unsigned long last_injected_overflow;
+#endif				/* CONFIG_B43_DEBUG */
+};
+
+static inline u32 b43_dma_read(struct b43_dmaring *ring, u16 offset)
+{
+	return b43_read32(ring->dev, ring->mmio_base + offset);
+}
+
+static inline
+    void b43_dma_write(struct b43_dmaring *ring, u16 offset, u32 value)
+{
+	b43_write32(ring->dev, ring->mmio_base + offset, value);
+}
+
+int b43_dma_init(struct b43_wldev *dev);
+void b43_dma_free(struct b43_wldev *dev);
+
+int b43_dmacontroller_rx_reset(struct b43_wldev *dev,
+			       u16 dmacontroller_mmio_base, int dma64);
+int b43_dmacontroller_tx_reset(struct b43_wldev *dev,
+			       u16 dmacontroller_mmio_base, int dma64);
+
+u16 b43_dmacontroller_base(int dma64bit, int dmacontroller_idx);
+
+void b43_dma_tx_suspend(struct b43_wldev *dev);
+void b43_dma_tx_resume(struct b43_wldev *dev);
+
+void b43_dma_get_tx_stats(struct b43_wldev *dev,
+			  struct ieee80211_tx_queue_stats *stats);
+
+int b43_dma_tx(struct b43_wldev *dev,
+	       struct sk_buff *skb, struct ieee80211_tx_control *ctl);
+void b43_dma_handle_txstatus(struct b43_wldev *dev,
+			     const struct b43_txstatus *status);
+
+void b43_dma_rx(struct b43_dmaring *ring);
+
+#else /* CONFIG_B43_DMA */
+
+static inline int b43_dma_init(struct b43_wldev *dev)
+{
+	return 0;
+}
+static inline void b43_dma_free(struct b43_wldev *dev)
+{
+}
+static inline
+    int b43_dmacontroller_rx_reset(struct b43_wldev *dev,
+				   u16 dmacontroller_mmio_base, int dma64)
+{
+	return 0;
+}
+static inline
+    int b43_dmacontroller_tx_reset(struct b43_wldev *dev,
+				   u16 dmacontroller_mmio_base, int dma64)
+{
+	return 0;
+}
+static inline
+    void b43_dma_get_tx_stats(struct b43_wldev *dev,
+			      struct ieee80211_tx_queue_stats *stats)
+{
+}
+static inline
+    int b43_dma_tx(struct b43_wldev *dev,
+		   struct sk_buff *skb, struct ieee80211_tx_control *ctl)
+{
+	return 0;
+}
+static inline
+    void b43_dma_handle_txstatus(struct b43_wldev *dev,
+				 const struct b43_txstatus *status)
+{
+}
+static inline void b43_dma_rx(struct b43_dmaring *ring)
+{
+}
+static inline void b43_dma_tx_suspend(struct b43_wldev *dev)
+{
+}
+static inline void b43_dma_tx_resume(struct b43_wldev *dev)
+{
+}
+
+#endif /* CONFIG_B43_DMA */
+#endif /* B43_DMA_H_ */
diff --git a/drivers/net/wireless/b43/leds.c b/drivers/net/wireless/b43/leds.c
new file mode 100644
index 000000000000..19e588582c7c
--- /dev/null
+++ b/drivers/net/wireless/b43/leds.c
@@ -0,0 +1,235 @@
+/*
+
+  Broadcom B43 wireless driver
+  LED control
+
+  Copyright (c) 2005 Martin Langer <martin-langer@gmx.de>,
+  Copyright (c) 2005 Stefano Brivio <st3@riseup.net>
+  Copyright (c) 2005-2007 Michael Buesch <mb@bu3sch.de>
+  Copyright (c) 2005 Danny van Dyk <kugelfang@gentoo.org>
+  Copyright (c) 2005 Andreas Jaggi <andreas.jaggi@waterwave.ch>
+
+  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, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with this program; see the file COPYING.  If not, write to
+  the Free Software Foundation, Inc., 51 Franklin Steet, Fifth Floor,
+  Boston, MA 02110-1301, USA.
+
+*/
+
+#include "b43.h"
+#include "leds.h"
+
+
+static void b43_led_turn_on(struct b43_wldev *dev, u8 led_index,
+			    bool activelow)
+{
+	struct b43_wl *wl = dev->wl;
+	unsigned long flags;
+	u16 ctl;
+
+	spin_lock_irqsave(&wl->leds_lock, flags);
+	ctl = b43_read16(dev, B43_MMIO_GPIO_CONTROL);
+	if (activelow)
+		ctl &= ~(1 << led_index);
+	else
+		ctl |= (1 << led_index);
+	b43_write16(dev, B43_MMIO_GPIO_CONTROL, ctl);
+	spin_unlock_irqrestore(&wl->leds_lock, flags);
+}
+
+static void b43_led_turn_off(struct b43_wldev *dev, u8 led_index,
+			     bool activelow)
+{
+	struct b43_wl *wl = dev->wl;
+	unsigned long flags;
+	u16 ctl;
+
+	spin_lock_irqsave(&wl->leds_lock, flags);
+	ctl = b43_read16(dev, B43_MMIO_GPIO_CONTROL);
+	if (activelow)
+		ctl |= (1 << led_index);
+	else
+		ctl &= ~(1 << led_index);
+	b43_write16(dev, B43_MMIO_GPIO_CONTROL, ctl);
+	spin_unlock_irqrestore(&wl->leds_lock, flags);
+}
+
+/* Callback from the LED subsystem. */
+static void b43_led_brightness_set(struct led_classdev *led_dev,
+				   enum led_brightness brightness)
+{
+	struct b43_led *led = container_of(led_dev, struct b43_led, led_dev);
+	struct b43_wldev *dev = led->dev;
+	bool radio_enabled;
+
+	/* Checking the radio-enabled status here is slightly racy,
+	 * but we want to avoid the locking overhead and we don't care
+	 * whether the LED has the wrong state for a second. */
+	radio_enabled = (dev->phy.radio_on && dev->radio_hw_enable);
+
+	if (brightness == LED_OFF || !radio_enabled)
+		b43_led_turn_off(dev, led->index, led->activelow);
+	else
+		b43_led_turn_on(dev, led->index, led->activelow);
+}
+
+static int b43_register_led(struct b43_wldev *dev, struct b43_led *led,
+			    const char *name, char *default_trigger,
+			    u8 led_index, bool activelow)
+{
+	int err;
+
+	b43_led_turn_off(dev, led_index, activelow);
+	if (led->dev)
+		return -EEXIST;
+	if (!default_trigger)
+		return -EINVAL;
+	led->dev = dev;
+	led->index = led_index;
+	led->activelow = activelow;
+	strncpy(led->name, name, sizeof(led->name));
+
+	led->led_dev.name = led->name;
+	led->led_dev.default_trigger = default_trigger;
+	led->led_dev.brightness_set = b43_led_brightness_set;
+
+	err = led_classdev_register(dev->dev->dev, &led->led_dev);
+	if (err) {
+		b43warn(dev->wl, "LEDs: Failed to register %s\n", name);
+		led->dev = NULL;
+		return err;
+	}
+	return 0;
+}
+
+static void b43_unregister_led(struct b43_led *led)
+{
+	if (!led->dev)
+		return;
+	led_classdev_unregister(&led->led_dev);
+	b43_led_turn_off(led->dev, led->index, led->activelow);
+	led->dev = NULL;
+}
+
+static void b43_map_led(struct b43_wldev *dev,
+			u8 led_index,
+			enum b43_led_behaviour behaviour,
+			bool activelow)
+{
+	struct ieee80211_hw *hw = dev->wl->hw;
+	char name[B43_LED_MAX_NAME_LEN + 1];
+
+	/* Map the b43 specific LED behaviour value to the
+	 * generic LED triggers. */
+	switch (behaviour) {
+	case B43_LED_INACTIVE:
+		break;
+	case B43_LED_OFF:
+		b43_led_turn_off(dev, led_index, activelow);
+		break;
+	case B43_LED_ON:
+		b43_led_turn_on(dev, led_index, activelow);
+		break;
+	case B43_LED_ACTIVITY:
+	case B43_LED_TRANSFER:
+	case B43_LED_APTRANSFER:
+		snprintf(name, sizeof(name),
+			 "b43-%s:tx", wiphy_name(hw->wiphy));
+		b43_register_led(dev, &dev->led_tx, name,
+				 ieee80211_get_tx_led_name(hw),
+				 led_index, activelow);
+		snprintf(name, sizeof(name),
+			 "b43-%s:rx", wiphy_name(hw->wiphy));
+		b43_register_led(dev, &dev->led_rx, name,
+				 ieee80211_get_rx_led_name(hw),
+				 led_index, activelow);
+		break;
+	case B43_LED_RADIO_ALL:
+	case B43_LED_RADIO_A:
+	case B43_LED_RADIO_B:
+	case B43_LED_MODE_BG:
+		snprintf(name, sizeof(name),
+			 "b43-%s:radio", wiphy_name(hw->wiphy));
+		b43_register_led(dev, &dev->led_radio, name,
+				 b43_rfkill_led_name(dev),
+				 led_index, activelow);
+		break;
+	case B43_LED_WEIRD:
+	case B43_LED_ASSOC:
+		snprintf(name, sizeof(name),
+			 "b43-%s:assoc", wiphy_name(hw->wiphy));
+		b43_register_led(dev, &dev->led_assoc, name,
+				 ieee80211_get_assoc_led_name(hw),
+				 led_index, activelow);
+		break;
+	default:
+		b43warn(dev->wl, "LEDs: Unknown behaviour 0x%02X\n",
+			behaviour);
+		break;
+	}
+}
+
+void b43_leds_init(struct b43_wldev *dev)
+{
+	struct ssb_bus *bus = dev->dev->bus;
+	u8 sprom[4];
+	int i;
+	enum b43_led_behaviour behaviour;
+	bool activelow;
+
+	sprom[0] = bus->sprom.r1.gpio0;
+	sprom[1] = bus->sprom.r1.gpio1;
+	sprom[2] = bus->sprom.r1.gpio2;
+	sprom[3] = bus->sprom.r1.gpio3;
+
+	for (i = 0; i < 4; i++) {
+		if (sprom[i] == 0xFF) {
+			/* There is no LED information in the SPROM
+			 * for this LED. Hardcode it here. */
+			activelow = 0;
+			switch (i) {
+			case 0:
+				behaviour = B43_LED_ACTIVITY;
+				activelow = 1;
+				if (bus->boardinfo.vendor == PCI_VENDOR_ID_COMPAQ)
+					behaviour = B43_LED_RADIO_ALL;
+				break;
+			case 1:
+				behaviour = B43_LED_RADIO_B;
+				if (bus->boardinfo.vendor == PCI_VENDOR_ID_ASUSTEK)
+					behaviour = B43_LED_ASSOC;
+				break;
+			case 2:
+				behaviour = B43_LED_RADIO_A;
+				break;
+			case 3:
+				behaviour = B43_LED_OFF;
+				break;
+			default:
+				B43_WARN_ON(1);
+				return;
+			}
+		} else {
+			behaviour = sprom[i] & B43_LED_BEHAVIOUR;
+			activelow = !!(sprom[i] & B43_LED_ACTIVELOW);
+		}
+		b43_map_led(dev, i, behaviour, activelow);
+	}
+}
+
+void b43_leds_exit(struct b43_wldev *dev)
+{
+	b43_unregister_led(&dev->led_tx);
+	b43_unregister_led(&dev->led_rx);
+	b43_unregister_led(&dev->led_assoc);
+}
diff --git a/drivers/net/wireless/b43/leds.h b/drivers/net/wireless/b43/leds.h
new file mode 100644
index 000000000000..b8b1dd521243
--- /dev/null
+++ b/drivers/net/wireless/b43/leds.h
@@ -0,0 +1,64 @@
+#ifndef B43_LEDS_H_
+#define B43_LEDS_H_
+
+struct b43_wldev;
+
+#ifdef CONFIG_B43_LEDS
+
+#include <linux/types.h>
+#include <linux/leds.h>
+
+
+#define B43_LED_MAX_NAME_LEN	31
+
+struct b43_led {
+	struct b43_wldev *dev;
+	/* The LED class device */
+	struct led_classdev led_dev;
+	/* The index number of the LED. */
+	u8 index;
+	/* If activelow is true, the LED is ON if the
+	 * bit is switched off. */
+	bool activelow;
+	/* The unique name string for this LED device. */
+	char name[B43_LED_MAX_NAME_LEN + 1];
+};
+
+#define B43_LED_BEHAVIOUR		0x7F
+#define B43_LED_ACTIVELOW		0x80
+/* LED behaviour values */
+enum b43_led_behaviour {
+	B43_LED_OFF,
+	B43_LED_ON,
+	B43_LED_ACTIVITY,
+	B43_LED_RADIO_ALL,
+	B43_LED_RADIO_A,
+	B43_LED_RADIO_B,
+	B43_LED_MODE_BG,
+	B43_LED_TRANSFER,
+	B43_LED_APTRANSFER,
+	B43_LED_WEIRD,		//FIXME
+	B43_LED_ASSOC,
+	B43_LED_INACTIVE,
+};
+
+void b43_leds_init(struct b43_wldev *dev);
+void b43_leds_exit(struct b43_wldev *dev);
+
+
+#else /* CONFIG_B43_LEDS */
+/* LED support disabled */
+
+struct b43_led {
+	/* empty */
+};
+
+static inline void b43_leds_init(struct b43_wldev *dev)
+{
+}
+static inline void b43_leds_exit(struct b43_wldev *dev)
+{
+}
+#endif /* CONFIG_B43_LEDS */
+
+#endif /* B43_LEDS_H_ */
diff --git a/drivers/net/wireless/b43/lo.c b/drivers/net/wireless/b43/lo.c
new file mode 100644
index 000000000000..b14a1753a0d7
--- /dev/null
+++ b/drivers/net/wireless/b43/lo.c
@@ -0,0 +1,1261 @@
+/*
+
+  Broadcom B43 wireless driver
+
+  G PHY LO (LocalOscillator) Measuring and Control routines
+
+  Copyright (c) 2005 Martin Langer <martin-langer@gmx.de>,
+  Copyright (c) 2005, 2006 Stefano Brivio <st3@riseup.net>
+  Copyright (c) 2005-2007 Michael Buesch <mb@bu3sch.de>
+  Copyright (c) 2005, 2006 Danny van Dyk <kugelfang@gentoo.org>
+  Copyright (c) 2005, 2006 Andreas Jaggi <andreas.jaggi@waterwave.ch>
+
+  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, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with this program; see the file COPYING.  If not, write to
+  the Free Software Foundation, Inc., 51 Franklin Steet, Fifth Floor,
+  Boston, MA 02110-1301, USA.
+
+*/
+
+#include "b43.h"
+#include "lo.h"
+#include "phy.h"
+#include "main.h"
+
+#include <linux/delay.h>
+#include <linux/sched.h>
+
+
+/* Define to 1 to always calibrate all possible LO control pairs.
+ * This is a workaround until we fix the partial LO calibration optimization. */
+#define B43_CALIB_ALL_LOCTLS	1
+
+
+/* Write the LocalOscillator Control (adjust) value-pair. */
+static void b43_lo_write(struct b43_wldev *dev, struct b43_loctl *control)
+{
+	struct b43_phy *phy = &dev->phy;
+	u16 value;
+	u16 reg;
+
+	if (B43_DEBUG) {
+		if (unlikely(abs(control->i) > 16 || abs(control->q) > 16)) {
+			b43dbg(dev->wl, "Invalid LO control pair "
+			       "(I: %d, Q: %d)\n", control->i, control->q);
+			dump_stack();
+			return;
+		}
+	}
+
+	value = (u8) (control->q);
+	value |= ((u8) (control->i)) << 8;
+
+	reg = (phy->type == B43_PHYTYPE_B) ? 0x002F : B43_PHY_LO_CTL;
+	b43_phy_write(dev, reg, value);
+}
+
+static int assert_rfatt_and_bbatt(const struct b43_rfatt *rfatt,
+				  const struct b43_bbatt *bbatt,
+				  struct b43_wldev *dev)
+{
+	int err = 0;
+
+	/* Check the attenuation values against the LO control array sizes. */
+	if (unlikely(rfatt->att >= B43_NR_RF)) {
+		b43err(dev->wl, "rfatt(%u) >= size of LO array\n", rfatt->att);
+		err = -EINVAL;
+	}
+	if (unlikely(bbatt->att >= B43_NR_BB)) {
+		b43err(dev->wl, "bbatt(%u) >= size of LO array\n", bbatt->att);
+		err = -EINVAL;
+	}
+
+	return err;
+}
+
+#if !B43_CALIB_ALL_LOCTLS
+static
+struct b43_loctl *b43_get_lo_g_ctl_nopadmix(struct b43_wldev *dev,
+					    const struct b43_rfatt *rfatt,
+					    const struct b43_bbatt *bbatt)
+{
+	struct b43_phy *phy = &dev->phy;
+	struct b43_txpower_lo_control *lo = phy->lo_control;
+
+	if (assert_rfatt_and_bbatt(rfatt, bbatt, dev))
+		return &(lo->no_padmix[0][0]);	/* Just prevent a crash */
+	return &(lo->no_padmix[bbatt->att][rfatt->att]);
+}
+#endif /* !B43_CALIB_ALL_LOCTLS */
+
+struct b43_loctl *b43_get_lo_g_ctl(struct b43_wldev *dev,
+				   const struct b43_rfatt *rfatt,
+				   const struct b43_bbatt *bbatt)
+{
+	struct b43_phy *phy = &dev->phy;
+	struct b43_txpower_lo_control *lo = phy->lo_control;
+
+	if (assert_rfatt_and_bbatt(rfatt, bbatt, dev))
+		return &(lo->no_padmix[0][0]);	/* Just prevent a crash */
+	if (rfatt->with_padmix)
+		return &(lo->with_padmix[bbatt->att][rfatt->att]);
+	return &(lo->no_padmix[bbatt->att][rfatt->att]);
+}
+
+/* Call a function for every possible LO control value-pair. */
+static void b43_call_for_each_loctl(struct b43_wldev *dev,
+				    void (*func) (struct b43_wldev *,
+						  struct b43_loctl *))
+{
+	struct b43_phy *phy = &dev->phy;
+	struct b43_txpower_lo_control *ctl = phy->lo_control;
+	int i, j;
+
+	for (i = 0; i < B43_NR_BB; i++) {
+		for (j = 0; j < B43_NR_RF; j++)
+			func(dev, &(ctl->with_padmix[i][j]));
+	}
+	for (i = 0; i < B43_NR_BB; i++) {
+		for (j = 0; j < B43_NR_RF; j++)
+			func(dev, &(ctl->no_padmix[i][j]));
+	}
+}
+
+static u16 lo_b_r15_loop(struct b43_wldev *dev)
+{
+	int i;
+	u16 ret = 0;
+
+	for (i = 0; i < 10; i++) {
+		b43_phy_write(dev, 0x0015, 0xAFA0);
+		udelay(1);
+		b43_phy_write(dev, 0x0015, 0xEFA0);
+		udelay(10);
+		b43_phy_write(dev, 0x0015, 0xFFA0);
+		udelay(40);
+		ret += b43_phy_read(dev, 0x002C);
+	}
+
+	return ret;
+}
+
+void b43_lo_b_measure(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+	u16 regstack[12] = { 0 };
+	u16 mls;
+	u16 fval;
+	int i, j;
+
+	regstack[0] = b43_phy_read(dev, 0x0015);
+	regstack[1] = b43_radio_read16(dev, 0x0052) & 0xFFF0;
+
+	if (phy->radio_ver == 0x2053) {
+		regstack[2] = b43_phy_read(dev, 0x000A);
+		regstack[3] = b43_phy_read(dev, 0x002A);
+		regstack[4] = b43_phy_read(dev, 0x0035);
+		regstack[5] = b43_phy_read(dev, 0x0003);
+		regstack[6] = b43_phy_read(dev, 0x0001);
+		regstack[7] = b43_phy_read(dev, 0x0030);
+
+		regstack[8] = b43_radio_read16(dev, 0x0043);
+		regstack[9] = b43_radio_read16(dev, 0x007A);
+		regstack[10] = b43_read16(dev, 0x03EC);
+		regstack[11] = b43_radio_read16(dev, 0x0052) & 0x00F0;
+
+		b43_phy_write(dev, 0x0030, 0x00FF);
+		b43_write16(dev, 0x03EC, 0x3F3F);
+		b43_phy_write(dev, 0x0035, regstack[4] & 0xFF7F);
+		b43_radio_write16(dev, 0x007A, regstack[9] & 0xFFF0);
+	}
+	b43_phy_write(dev, 0x0015, 0xB000);
+	b43_phy_write(dev, 0x002B, 0x0004);
+
+	if (phy->radio_ver == 0x2053) {
+		b43_phy_write(dev, 0x002B, 0x0203);
+		b43_phy_write(dev, 0x002A, 0x08A3);
+	}
+
+	phy->minlowsig[0] = 0xFFFF;
+
+	for (i = 0; i < 4; i++) {
+		b43_radio_write16(dev, 0x0052, regstack[1] | i);
+		lo_b_r15_loop(dev);
+	}
+	for (i = 0; i < 10; i++) {
+		b43_radio_write16(dev, 0x0052, regstack[1] | i);
+		mls = lo_b_r15_loop(dev) / 10;
+		if (mls < phy->minlowsig[0]) {
+			phy->minlowsig[0] = mls;
+			phy->minlowsigpos[0] = i;
+		}
+	}
+	b43_radio_write16(dev, 0x0052, regstack[1] | phy->minlowsigpos[0]);
+
+	phy->minlowsig[1] = 0xFFFF;
+
+	for (i = -4; i < 5; i += 2) {
+		for (j = -4; j < 5; j += 2) {
+			if (j < 0)
+				fval = (0x0100 * i) + j + 0x0100;
+			else
+				fval = (0x0100 * i) + j;
+			b43_phy_write(dev, 0x002F, fval);
+			mls = lo_b_r15_loop(dev) / 10;
+			if (mls < phy->minlowsig[1]) {
+				phy->minlowsig[1] = mls;
+				phy->minlowsigpos[1] = fval;
+			}
+		}
+	}
+	phy->minlowsigpos[1] += 0x0101;
+
+	b43_phy_write(dev, 0x002F, phy->minlowsigpos[1]);
+	if (phy->radio_ver == 0x2053) {
+		b43_phy_write(dev, 0x000A, regstack[2]);
+		b43_phy_write(dev, 0x002A, regstack[3]);
+		b43_phy_write(dev, 0x0035, regstack[4]);
+		b43_phy_write(dev, 0x0003, regstack[5]);
+		b43_phy_write(dev, 0x0001, regstack[6]);
+		b43_phy_write(dev, 0x0030, regstack[7]);
+
+		b43_radio_write16(dev, 0x0043, regstack[8]);
+		b43_radio_write16(dev, 0x007A, regstack[9]);
+
+		b43_radio_write16(dev, 0x0052,
+				  (b43_radio_read16(dev, 0x0052) & 0x000F)
+				  | regstack[11]);
+
+		b43_write16(dev, 0x03EC, regstack[10]);
+	}
+	b43_phy_write(dev, 0x0015, regstack[0]);
+}
+
+static u16 lo_measure_feedthrough(struct b43_wldev *dev,
+				  u16 lna, u16 pga, u16 trsw_rx)
+{
+	struct b43_phy *phy = &dev->phy;
+	u16 rfover;
+	u16 feedthrough;
+
+	if (phy->gmode) {
+		lna <<= B43_PHY_RFOVERVAL_LNA_SHIFT;
+		pga <<= B43_PHY_RFOVERVAL_PGA_SHIFT;
+
+		B43_WARN_ON(lna & ~B43_PHY_RFOVERVAL_LNA);
+		B43_WARN_ON(pga & ~B43_PHY_RFOVERVAL_PGA);
+/*FIXME This assertion fails		B43_WARN_ON(trsw_rx & ~(B43_PHY_RFOVERVAL_TRSWRX |
+				    B43_PHY_RFOVERVAL_BW));
+*/
+		trsw_rx &= (B43_PHY_RFOVERVAL_TRSWRX | B43_PHY_RFOVERVAL_BW);
+
+		/* Construct the RF Override Value */
+		rfover = B43_PHY_RFOVERVAL_UNK;
+		rfover |= pga;
+		rfover |= lna;
+		rfover |= trsw_rx;
+		if ((dev->dev->bus->sprom.r1.boardflags_lo & B43_BFL_EXTLNA) &&
+		    phy->rev > 6)
+			rfover |= B43_PHY_RFOVERVAL_EXTLNA;
+
+		b43_phy_write(dev, B43_PHY_PGACTL, 0xE300);
+		b43_phy_write(dev, B43_PHY_RFOVERVAL, rfover);
+		udelay(10);
+		rfover |= B43_PHY_RFOVERVAL_BW_LBW;
+		b43_phy_write(dev, B43_PHY_RFOVERVAL, rfover);
+		udelay(10);
+		rfover |= B43_PHY_RFOVERVAL_BW_LPF;
+		b43_phy_write(dev, B43_PHY_RFOVERVAL, rfover);
+		udelay(10);
+		b43_phy_write(dev, B43_PHY_PGACTL, 0xF300);
+	} else {
+		pga |= B43_PHY_PGACTL_UNKNOWN;
+		b43_phy_write(dev, B43_PHY_PGACTL, pga);
+		udelay(10);
+		pga |= B43_PHY_PGACTL_LOWBANDW;
+		b43_phy_write(dev, B43_PHY_PGACTL, pga);
+		udelay(10);
+		pga |= B43_PHY_PGACTL_LPF;
+		b43_phy_write(dev, B43_PHY_PGACTL, pga);
+	}
+	udelay(21);
+	feedthrough = b43_phy_read(dev, B43_PHY_LO_LEAKAGE);
+
+	/* This is a good place to check if we need to relax a bit,
+	 * as this is the main function called regularly
+	 * in the LO calibration. */
+	cond_resched();
+
+	return feedthrough;
+}
+
+/* TXCTL Register and Value Table.
+ * Returns the "TXCTL Register".
+ * "value" is the "TXCTL Value".
+ * "pad_mix_gain" is the PAD Mixer Gain.
+ */
+static u16 lo_txctl_register_table(struct b43_wldev *dev,
+				   u16 * value, u16 * pad_mix_gain)
+{
+	struct b43_phy *phy = &dev->phy;
+	u16 reg, v, padmix;
+
+	if (phy->type == B43_PHYTYPE_B) {
+		v = 0x30;
+		if (phy->radio_rev <= 5) {
+			reg = 0x43;
+			padmix = 0;
+		} else {
+			reg = 0x52;
+			padmix = 5;
+		}
+	} else {
+		if (phy->rev >= 2 && phy->radio_rev == 8) {
+			reg = 0x43;
+			v = 0x10;
+			padmix = 2;
+		} else {
+			reg = 0x52;
+			v = 0x30;
+			padmix = 5;
+		}
+	}
+	if (value)
+		*value = v;
+	if (pad_mix_gain)
+		*pad_mix_gain = padmix;
+
+	return reg;
+}
+
+static void lo_measure_txctl_values(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+	struct b43_txpower_lo_control *lo = phy->lo_control;
+	u16 reg, mask;
+	u16 trsw_rx, pga;
+	u16 radio_pctl_reg;
+
+	static const u8 tx_bias_values[] = {
+		0x09, 0x08, 0x0A, 0x01, 0x00,
+		0x02, 0x05, 0x04, 0x06,
+	};
+	static const u8 tx_magn_values[] = {
+		0x70, 0x40,
+	};
+
+	if (!has_loopback_gain(phy)) {
+		radio_pctl_reg = 6;
+		trsw_rx = 2;
+		pga = 0;
+	} else {
+		int lb_gain;	/* Loopback gain (in dB) */
+
+		trsw_rx = 0;
+		lb_gain = phy->max_lb_gain / 2;
+		if (lb_gain > 10) {
+			radio_pctl_reg = 0;
+			pga = abs(10 - lb_gain) / 6;
+			pga = limit_value(pga, 0, 15);
+		} else {
+			int cmp_val;
+			int tmp;
+
+			pga = 0;
+			cmp_val = 0x24;
+			if ((phy->rev >= 2) &&
+			    (phy->radio_ver == 0x2050) && (phy->radio_rev == 8))
+				cmp_val = 0x3C;
+			tmp = lb_gain;
+			if ((10 - lb_gain) < cmp_val)
+				tmp = (10 - lb_gain);
+			if (tmp < 0)
+				tmp += 6;
+			else
+				tmp += 3;
+			cmp_val /= 4;
+			tmp /= 4;
+			if (tmp >= cmp_val)
+				radio_pctl_reg = cmp_val;
+			else
+				radio_pctl_reg = tmp;
+		}
+	}
+	b43_radio_write16(dev, 0x43, (b43_radio_read16(dev, 0x43)
+				      & 0xFFF0) | radio_pctl_reg);
+	b43_phy_set_baseband_attenuation(dev, 2);
+
+	reg = lo_txctl_register_table(dev, &mask, NULL);
+	mask = ~mask;
+	b43_radio_write16(dev, reg, b43_radio_read16(dev, reg)
+			  & mask);
+
+	if (has_tx_magnification(phy)) {
+		int i, j;
+		int feedthrough;
+		int min_feedth = 0xFFFF;
+		u8 tx_magn, tx_bias;
+
+		for (i = 0; i < ARRAY_SIZE(tx_magn_values); i++) {
+			tx_magn = tx_magn_values[i];
+			b43_radio_write16(dev, 0x52,
+					  (b43_radio_read16(dev, 0x52)
+					   & 0xFF0F) | tx_magn);
+			for (j = 0; j < ARRAY_SIZE(tx_bias_values); j++) {
+				tx_bias = tx_bias_values[j];
+				b43_radio_write16(dev, 0x52,
+						  (b43_radio_read16(dev, 0x52)
+						   & 0xFFF0) | tx_bias);
+				feedthrough =
+				    lo_measure_feedthrough(dev, 0, pga,
+							   trsw_rx);
+				if (feedthrough < min_feedth) {
+					lo->tx_bias = tx_bias;
+					lo->tx_magn = tx_magn;
+					min_feedth = feedthrough;
+				}
+				if (lo->tx_bias == 0)
+					break;
+			}
+			b43_radio_write16(dev, 0x52,
+					  (b43_radio_read16(dev, 0x52)
+					   & 0xFF00) | lo->tx_bias | lo->
+					  tx_magn);
+		}
+	} else {
+		lo->tx_magn = 0;
+		lo->tx_bias = 0;
+		b43_radio_write16(dev, 0x52, b43_radio_read16(dev, 0x52)
+				  & 0xFFF0);	/* TX bias == 0 */
+	}
+}
+
+static void lo_read_power_vector(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+	struct b43_txpower_lo_control *lo = phy->lo_control;
+	u16 i;
+	u64 tmp;
+	u64 power_vector = 0;
+	int rf_offset, bb_offset;
+	struct b43_loctl *loctl;
+
+	for (i = 0; i < 8; i += 2) {
+		tmp = b43_shm_read16(dev, B43_SHM_SHARED, 0x310 + i);
+		/* Clear the top byte. We get holes in the bitmap... */
+		tmp &= 0xFF;
+		power_vector |= (tmp << (i * 8));
+		/* Clear the vector on the device. */
+		b43_shm_write16(dev, B43_SHM_SHARED, 0x310 + i, 0);
+	}
+
+	if (power_vector)
+		lo->power_vector = power_vector;
+	power_vector = lo->power_vector;
+
+	for (i = 0; i < 64; i++) {
+		if (power_vector & ((u64) 1ULL << i)) {
+			/* Now figure out which b43_loctl corresponds
+			 * to this bit.
+			 */
+			rf_offset = i / lo->rfatt_list.len;
+			bb_offset = i % lo->rfatt_list.len;	//FIXME?
+			loctl =
+			    b43_get_lo_g_ctl(dev,
+					     &lo->rfatt_list.list[rf_offset],
+					     &lo->bbatt_list.list[bb_offset]);
+			/* And mark it as "used", as the device told us
+			 * through the bitmap it is using it.
+			 */
+			loctl->used = 1;
+		}
+	}
+}
+
+/* 802.11/LO/GPHY/MeasuringGains */
+static void lo_measure_gain_values(struct b43_wldev *dev,
+				   s16 max_rx_gain, int use_trsw_rx)
+{
+	struct b43_phy *phy = &dev->phy;
+	u16 tmp;
+
+	if (max_rx_gain < 0)
+		max_rx_gain = 0;
+
+	if (has_loopback_gain(phy)) {
+		int trsw_rx = 0;
+		int trsw_rx_gain;
+
+		if (use_trsw_rx) {
+			trsw_rx_gain = phy->trsw_rx_gain / 2;
+			if (max_rx_gain >= trsw_rx_gain) {
+				trsw_rx_gain = max_rx_gain - trsw_rx_gain;
+				trsw_rx = 0x20;
+			}
+		} else
+			trsw_rx_gain = max_rx_gain;
+		if (trsw_rx_gain < 9) {
+			phy->lna_lod_gain = 0;
+		} else {
+			phy->lna_lod_gain = 1;
+			trsw_rx_gain -= 8;
+		}
+		trsw_rx_gain = limit_value(trsw_rx_gain, 0, 0x2D);
+		phy->pga_gain = trsw_rx_gain / 3;
+		if (phy->pga_gain >= 5) {
+			phy->pga_gain -= 5;
+			phy->lna_gain = 2;
+		} else
+			phy->lna_gain = 0;
+	} else {
+		phy->lna_gain = 0;
+		phy->trsw_rx_gain = 0x20;
+		if (max_rx_gain >= 0x14) {
+			phy->lna_lod_gain = 1;
+			phy->pga_gain = 2;
+		} else if (max_rx_gain >= 0x12) {
+			phy->lna_lod_gain = 1;
+			phy->pga_gain = 1;
+		} else if (max_rx_gain >= 0xF) {
+			phy->lna_lod_gain = 1;
+			phy->pga_gain = 0;
+		} else {
+			phy->lna_lod_gain = 0;
+			phy->pga_gain = 0;
+		}
+	}
+
+	tmp = b43_radio_read16(dev, 0x7A);
+	if (phy->lna_lod_gain == 0)
+		tmp &= ~0x0008;
+	else
+		tmp |= 0x0008;
+	b43_radio_write16(dev, 0x7A, tmp);
+}
+
+struct lo_g_saved_values {
+	u8 old_channel;
+
+	/* Core registers */
+	u16 reg_3F4;
+	u16 reg_3E2;
+
+	/* PHY registers */
+	u16 phy_lo_mask;
+	u16 phy_extg_01;
+	u16 phy_dacctl_hwpctl;
+	u16 phy_dacctl;
+	u16 phy_base_14;
+	u16 phy_hpwr_tssictl;
+	u16 phy_analogover;
+	u16 phy_analogoverval;
+	u16 phy_rfover;
+	u16 phy_rfoverval;
+	u16 phy_classctl;
+	u16 phy_base_3E;
+	u16 phy_crs0;
+	u16 phy_pgactl;
+	u16 phy_base_2A;
+	u16 phy_syncctl;
+	u16 phy_base_30;
+	u16 phy_base_06;
+
+	/* Radio registers */
+	u16 radio_43;
+	u16 radio_7A;
+	u16 radio_52;
+};
+
+static void lo_measure_setup(struct b43_wldev *dev,
+			     struct lo_g_saved_values *sav)
+{
+	struct ssb_sprom *sprom = &dev->dev->bus->sprom;
+	struct b43_phy *phy = &dev->phy;
+	struct b43_txpower_lo_control *lo = phy->lo_control;
+	u16 tmp;
+
+	if (b43_has_hardware_pctl(phy)) {
+		sav->phy_lo_mask = b43_phy_read(dev, B43_PHY_LO_MASK);
+		sav->phy_extg_01 = b43_phy_read(dev, B43_PHY_EXTG(0x01));
+		sav->phy_dacctl_hwpctl = b43_phy_read(dev, B43_PHY_DACCTL);
+		sav->phy_base_14 = b43_phy_read(dev, B43_PHY_BASE(0x14));
+		sav->phy_hpwr_tssictl = b43_phy_read(dev, B43_PHY_HPWR_TSSICTL);
+
+		b43_phy_write(dev, B43_PHY_HPWR_TSSICTL,
+			      b43_phy_read(dev, B43_PHY_HPWR_TSSICTL)
+			      | 0x100);
+		b43_phy_write(dev, B43_PHY_EXTG(0x01),
+			      b43_phy_read(dev, B43_PHY_EXTG(0x01))
+			      | 0x40);
+		b43_phy_write(dev, B43_PHY_DACCTL,
+			      b43_phy_read(dev, B43_PHY_DACCTL)
+			      | 0x40);
+		b43_phy_write(dev, B43_PHY_BASE(0x14),
+			      b43_phy_read(dev, B43_PHY_BASE(0x14))
+			      | 0x200);
+	}
+	if (phy->type == B43_PHYTYPE_B &&
+	    phy->radio_ver == 0x2050 && phy->radio_rev < 6) {
+		b43_phy_write(dev, B43_PHY_BASE(0x16), 0x410);
+		b43_phy_write(dev, B43_PHY_BASE(0x17), 0x820);
+	}
+	if (!lo->rebuild && b43_has_hardware_pctl(phy))
+		lo_read_power_vector(dev);
+	if (phy->rev >= 2) {
+		sav->phy_analogover = b43_phy_read(dev, B43_PHY_ANALOGOVER);
+		sav->phy_analogoverval =
+		    b43_phy_read(dev, B43_PHY_ANALOGOVERVAL);
+		sav->phy_rfover = b43_phy_read(dev, B43_PHY_RFOVER);
+		sav->phy_rfoverval = b43_phy_read(dev, B43_PHY_RFOVERVAL);
+		sav->phy_classctl = b43_phy_read(dev, B43_PHY_CLASSCTL);
+		sav->phy_base_3E = b43_phy_read(dev, B43_PHY_BASE(0x3E));
+		sav->phy_crs0 = b43_phy_read(dev, B43_PHY_CRS0);
+
+		b43_phy_write(dev, B43_PHY_CLASSCTL,
+			      b43_phy_read(dev, B43_PHY_CLASSCTL)
+			      & 0xFFFC);
+		b43_phy_write(dev, B43_PHY_CRS0, b43_phy_read(dev, B43_PHY_CRS0)
+			      & 0x7FFF);
+		b43_phy_write(dev, B43_PHY_ANALOGOVER,
+			      b43_phy_read(dev, B43_PHY_ANALOGOVER)
+			      | 0x0003);
+		b43_phy_write(dev, B43_PHY_ANALOGOVERVAL,
+			      b43_phy_read(dev, B43_PHY_ANALOGOVERVAL)
+			      & 0xFFFC);
+		if (phy->type == B43_PHYTYPE_G) {
+			if ((phy->rev >= 7) &&
+			    (sprom->r1.boardflags_lo & B43_BFL_EXTLNA)) {
+				b43_phy_write(dev, B43_PHY_RFOVER, 0x933);
+			} else {
+				b43_phy_write(dev, B43_PHY_RFOVER, 0x133);
+			}
+		} else {
+			b43_phy_write(dev, B43_PHY_RFOVER, 0);
+		}
+		b43_phy_write(dev, B43_PHY_BASE(0x3E), 0);
+	}
+	sav->reg_3F4 = b43_read16(dev, 0x3F4);
+	sav->reg_3E2 = b43_read16(dev, 0x3E2);
+	sav->radio_43 = b43_radio_read16(dev, 0x43);
+	sav->radio_7A = b43_radio_read16(dev, 0x7A);
+	sav->phy_pgactl = b43_phy_read(dev, B43_PHY_PGACTL);
+	sav->phy_base_2A = b43_phy_read(dev, B43_PHY_BASE(0x2A));
+	sav->phy_syncctl = b43_phy_read(dev, B43_PHY_SYNCCTL);
+	sav->phy_dacctl = b43_phy_read(dev, B43_PHY_DACCTL);
+
+	if (!has_tx_magnification(phy)) {
+		sav->radio_52 = b43_radio_read16(dev, 0x52);
+		sav->radio_52 &= 0x00F0;
+	}
+	if (phy->type == B43_PHYTYPE_B) {
+		sav->phy_base_30 = b43_phy_read(dev, B43_PHY_BASE(0x30));
+		sav->phy_base_06 = b43_phy_read(dev, B43_PHY_BASE(0x06));
+		b43_phy_write(dev, B43_PHY_BASE(0x30), 0x00FF);
+		b43_phy_write(dev, B43_PHY_BASE(0x06), 0x3F3F);
+	} else {
+		b43_write16(dev, 0x3E2, b43_read16(dev, 0x3E2)
+			    | 0x8000);
+	}
+	b43_write16(dev, 0x3F4, b43_read16(dev, 0x3F4)
+		    & 0xF000);
+
+	tmp =
+	    (phy->type == B43_PHYTYPE_G) ? B43_PHY_LO_MASK : B43_PHY_BASE(0x2E);
+	b43_phy_write(dev, tmp, 0x007F);
+
+	tmp = sav->phy_syncctl;
+	b43_phy_write(dev, B43_PHY_SYNCCTL, tmp & 0xFF7F);
+	tmp = sav->radio_7A;
+	b43_radio_write16(dev, 0x007A, tmp & 0xFFF0);
+
+	b43_phy_write(dev, B43_PHY_BASE(0x2A), 0x8A3);
+	if (phy->type == B43_PHYTYPE_G ||
+	    (phy->type == B43_PHYTYPE_B &&
+	     phy->radio_ver == 0x2050 && phy->radio_rev >= 6)) {
+		b43_phy_write(dev, B43_PHY_BASE(0x2B), 0x1003);
+	} else
+		b43_phy_write(dev, B43_PHY_BASE(0x2B), 0x0802);
+	if (phy->rev >= 2)
+		b43_dummy_transmission(dev);
+	b43_radio_selectchannel(dev, 6, 0);
+	b43_radio_read16(dev, 0x51);	/* dummy read */
+	if (phy->type == B43_PHYTYPE_G)
+		b43_phy_write(dev, B43_PHY_BASE(0x2F), 0);
+	if (lo->rebuild)
+		lo_measure_txctl_values(dev);
+	if (phy->type == B43_PHYTYPE_G && phy->rev >= 3) {
+		b43_phy_write(dev, B43_PHY_LO_MASK, 0xC078);
+	} else {
+		if (phy->type == B43_PHYTYPE_B)
+			b43_phy_write(dev, B43_PHY_BASE(0x2E), 0x8078);
+		else
+			b43_phy_write(dev, B43_PHY_LO_MASK, 0x8078);
+	}
+}
+
+static void lo_measure_restore(struct b43_wldev *dev,
+			       struct lo_g_saved_values *sav)
+{
+	struct b43_phy *phy = &dev->phy;
+	struct b43_txpower_lo_control *lo = phy->lo_control;
+	u16 tmp;
+
+	if (phy->rev >= 2) {
+		b43_phy_write(dev, B43_PHY_PGACTL, 0xE300);
+		tmp = (phy->pga_gain << 8);
+		b43_phy_write(dev, B43_PHY_RFOVERVAL, tmp | 0xA0);
+		udelay(5);
+		b43_phy_write(dev, B43_PHY_RFOVERVAL, tmp | 0xA2);
+		udelay(2);
+		b43_phy_write(dev, B43_PHY_RFOVERVAL, tmp | 0xA3);
+	} else {
+		tmp = (phy->pga_gain | 0xEFA0);
+		b43_phy_write(dev, B43_PHY_PGACTL, tmp);
+	}
+	if (b43_has_hardware_pctl(phy)) {
+		b43_gphy_dc_lt_init(dev);
+	} else {
+		if (lo->rebuild)
+			b43_lo_g_adjust_to(dev, 3, 2, 0);
+		else
+			b43_lo_g_adjust(dev);
+	}
+	if (phy->type == B43_PHYTYPE_G) {
+		if (phy->rev >= 3)
+			b43_phy_write(dev, B43_PHY_BASE(0x2E), 0xC078);
+		else
+			b43_phy_write(dev, B43_PHY_BASE(0x2E), 0x8078);
+		if (phy->rev >= 2)
+			b43_phy_write(dev, B43_PHY_BASE(0x2F), 0x0202);
+		else
+			b43_phy_write(dev, B43_PHY_BASE(0x2F), 0x0101);
+	}
+	b43_write16(dev, 0x3F4, sav->reg_3F4);
+	b43_phy_write(dev, B43_PHY_PGACTL, sav->phy_pgactl);
+	b43_phy_write(dev, B43_PHY_BASE(0x2A), sav->phy_base_2A);
+	b43_phy_write(dev, B43_PHY_SYNCCTL, sav->phy_syncctl);
+	b43_phy_write(dev, B43_PHY_DACCTL, sav->phy_dacctl);
+	b43_radio_write16(dev, 0x43, sav->radio_43);
+	b43_radio_write16(dev, 0x7A, sav->radio_7A);
+	if (!has_tx_magnification(phy)) {
+		tmp = sav->radio_52;
+		b43_radio_write16(dev, 0x52, (b43_radio_read16(dev, 0x52)
+					      & 0xFF0F) | tmp);
+	}
+	b43_write16(dev, 0x3E2, sav->reg_3E2);
+	if (phy->type == B43_PHYTYPE_B &&
+	    phy->radio_ver == 0x2050 && phy->radio_rev <= 5) {
+		b43_phy_write(dev, B43_PHY_BASE(0x30), sav->phy_base_30);
+		b43_phy_write(dev, B43_PHY_BASE(0x06), sav->phy_base_06);
+	}
+	if (phy->rev >= 2) {
+		b43_phy_write(dev, B43_PHY_ANALOGOVER, sav->phy_analogover);
+		b43_phy_write(dev, B43_PHY_ANALOGOVERVAL,
+			      sav->phy_analogoverval);
+		b43_phy_write(dev, B43_PHY_CLASSCTL, sav->phy_classctl);
+		b43_phy_write(dev, B43_PHY_RFOVER, sav->phy_rfover);
+		b43_phy_write(dev, B43_PHY_RFOVERVAL, sav->phy_rfoverval);
+		b43_phy_write(dev, B43_PHY_BASE(0x3E), sav->phy_base_3E);
+		b43_phy_write(dev, B43_PHY_CRS0, sav->phy_crs0);
+	}
+	if (b43_has_hardware_pctl(phy)) {
+		tmp = (sav->phy_lo_mask & 0xBFFF);
+		b43_phy_write(dev, B43_PHY_LO_MASK, tmp);
+		b43_phy_write(dev, B43_PHY_EXTG(0x01), sav->phy_extg_01);
+		b43_phy_write(dev, B43_PHY_DACCTL, sav->phy_dacctl_hwpctl);
+		b43_phy_write(dev, B43_PHY_BASE(0x14), sav->phy_base_14);
+		b43_phy_write(dev, B43_PHY_HPWR_TSSICTL, sav->phy_hpwr_tssictl);
+	}
+	b43_radio_selectchannel(dev, sav->old_channel, 1);
+}
+
+struct b43_lo_g_statemachine {
+	int current_state;
+	int nr_measured;
+	int state_val_multiplier;
+	u16 lowest_feedth;
+	struct b43_loctl min_loctl;
+};
+
+/* Loop over each possible value in this state. */
+static int lo_probe_possible_loctls(struct b43_wldev *dev,
+				    struct b43_loctl *probe_loctl,
+				    struct b43_lo_g_statemachine *d)
+{
+	struct b43_phy *phy = &dev->phy;
+	struct b43_txpower_lo_control *lo = phy->lo_control;
+	struct b43_loctl test_loctl;
+	struct b43_loctl orig_loctl;
+	struct b43_loctl prev_loctl = {
+		.i = -100,
+		.q = -100,
+	};
+	int i;
+	int begin, end;
+	int found_lower = 0;
+	u16 feedth;
+
+	static const struct b43_loctl modifiers[] = {
+		{.i = 1,.q = 1,},
+		{.i = 1,.q = 0,},
+		{.i = 1,.q = -1,},
+		{.i = 0,.q = -1,},
+		{.i = -1,.q = -1,},
+		{.i = -1,.q = 0,},
+		{.i = -1,.q = 1,},
+		{.i = 0,.q = 1,},
+	};
+
+	if (d->current_state == 0) {
+		begin = 1;
+		end = 8;
+	} else if (d->current_state % 2 == 0) {
+		begin = d->current_state - 1;
+		end = d->current_state + 1;
+	} else {
+		begin = d->current_state - 2;
+		end = d->current_state + 2;
+	}
+	if (begin < 1)
+		begin += 8;
+	if (end > 8)
+		end -= 8;
+
+	memcpy(&orig_loctl, probe_loctl, sizeof(struct b43_loctl));
+	i = begin;
+	d->current_state = i;
+	while (1) {
+		B43_WARN_ON(!(i >= 1 && i <= 8));
+		memcpy(&test_loctl, &orig_loctl, sizeof(struct b43_loctl));
+		test_loctl.i += modifiers[i - 1].i * d->state_val_multiplier;
+		test_loctl.q += modifiers[i - 1].q * d->state_val_multiplier;
+		if ((test_loctl.i != prev_loctl.i ||
+		     test_loctl.q != prev_loctl.q) &&
+		    (abs(test_loctl.i) <= 16 && abs(test_loctl.q) <= 16)) {
+			b43_lo_write(dev, &test_loctl);
+			feedth = lo_measure_feedthrough(dev, phy->lna_gain,
+							phy->pga_gain,
+							phy->trsw_rx_gain);
+			if (feedth < d->lowest_feedth) {
+				memcpy(probe_loctl, &test_loctl,
+				       sizeof(struct b43_loctl));
+				found_lower = 1;
+				d->lowest_feedth = feedth;
+				if ((d->nr_measured < 2) &&
+				    (!has_loopback_gain(phy) || lo->rebuild))
+					break;
+			}
+		}
+		memcpy(&prev_loctl, &test_loctl, sizeof(prev_loctl));
+		if (i == end)
+			break;
+		if (i == 8)
+			i = 1;
+		else
+			i++;
+		d->current_state = i;
+	}
+
+	return found_lower;
+}
+
+static void lo_probe_loctls_statemachine(struct b43_wldev *dev,
+					 struct b43_loctl *loctl,
+					 int *max_rx_gain)
+{
+	struct b43_phy *phy = &dev->phy;
+	struct b43_txpower_lo_control *lo = phy->lo_control;
+	struct b43_lo_g_statemachine d;
+	u16 feedth;
+	int found_lower;
+	struct b43_loctl probe_loctl;
+	int max_repeat = 1, repeat_cnt = 0;
+
+	d.nr_measured = 0;
+	d.state_val_multiplier = 1;
+	if (has_loopback_gain(phy) && !lo->rebuild)
+		d.state_val_multiplier = 3;
+
+	memcpy(&d.min_loctl, loctl, sizeof(struct b43_loctl));
+	if (has_loopback_gain(phy) && lo->rebuild)
+		max_repeat = 4;
+	do {
+		b43_lo_write(dev, &d.min_loctl);
+		feedth = lo_measure_feedthrough(dev, phy->lna_gain,
+						phy->pga_gain,
+						phy->trsw_rx_gain);
+		if (!lo->rebuild && feedth < 0x258) {
+			if (feedth >= 0x12C)
+				*max_rx_gain += 6;
+			else
+				*max_rx_gain += 3;
+			feedth = lo_measure_feedthrough(dev, phy->lna_gain,
+							phy->pga_gain,
+							phy->trsw_rx_gain);
+		}
+		d.lowest_feedth = feedth;
+
+		d.current_state = 0;
+		do {
+			B43_WARN_ON(!
+				    (d.current_state >= 0
+				     && d.current_state <= 8));
+			memcpy(&probe_loctl, &d.min_loctl,
+			       sizeof(struct b43_loctl));
+			found_lower =
+			    lo_probe_possible_loctls(dev, &probe_loctl, &d);
+			if (!found_lower)
+				break;
+			if ((probe_loctl.i == d.min_loctl.i) &&
+			    (probe_loctl.q == d.min_loctl.q))
+				break;
+			memcpy(&d.min_loctl, &probe_loctl,
+			       sizeof(struct b43_loctl));
+			d.nr_measured++;
+		} while (d.nr_measured < 24);
+		memcpy(loctl, &d.min_loctl, sizeof(struct b43_loctl));
+
+		if (has_loopback_gain(phy)) {
+			if (d.lowest_feedth > 0x1194)
+				*max_rx_gain -= 6;
+			else if (d.lowest_feedth < 0x5DC)
+				*max_rx_gain += 3;
+			if (repeat_cnt == 0) {
+				if (d.lowest_feedth <= 0x5DC) {
+					d.state_val_multiplier = 1;
+					repeat_cnt++;
+				} else
+					d.state_val_multiplier = 2;
+			} else if (repeat_cnt == 2)
+				d.state_val_multiplier = 1;
+		}
+		lo_measure_gain_values(dev, *max_rx_gain,
+				       has_loopback_gain(phy));
+	} while (++repeat_cnt < max_repeat);
+}
+
+#if B43_CALIB_ALL_LOCTLS
+static const struct b43_rfatt b43_full_rfatt_list_items[] = {
+	{ .att = 0, .with_padmix = 0, },
+	{ .att = 1, .with_padmix = 0, },
+	{ .att = 2, .with_padmix = 0, },
+	{ .att = 3, .with_padmix = 0, },
+	{ .att = 4, .with_padmix = 0, },
+	{ .att = 5, .with_padmix = 0, },
+	{ .att = 6, .with_padmix = 0, },
+	{ .att = 7, .with_padmix = 0, },
+	{ .att = 8, .with_padmix = 0, },
+	{ .att = 9, .with_padmix = 0, },
+	{ .att = 10, .with_padmix = 0, },
+	{ .att = 11, .with_padmix = 0, },
+	{ .att = 12, .with_padmix = 0, },
+	{ .att = 13, .with_padmix = 0, },
+	{ .att = 14, .with_padmix = 0, },
+	{ .att = 15, .with_padmix = 0, },
+	{ .att = 0, .with_padmix = 1, },
+	{ .att = 1, .with_padmix = 1, },
+	{ .att = 2, .with_padmix = 1, },
+	{ .att = 3, .with_padmix = 1, },
+	{ .att = 4, .with_padmix = 1, },
+	{ .att = 5, .with_padmix = 1, },
+	{ .att = 6, .with_padmix = 1, },
+	{ .att = 7, .with_padmix = 1, },
+	{ .att = 8, .with_padmix = 1, },
+	{ .att = 9, .with_padmix = 1, },
+	{ .att = 10, .with_padmix = 1, },
+	{ .att = 11, .with_padmix = 1, },
+	{ .att = 12, .with_padmix = 1, },
+	{ .att = 13, .with_padmix = 1, },
+	{ .att = 14, .with_padmix = 1, },
+	{ .att = 15, .with_padmix = 1, },
+};
+static const struct b43_rfatt_list b43_full_rfatt_list = {
+	.list		= b43_full_rfatt_list_items,
+	.len		= ARRAY_SIZE(b43_full_rfatt_list_items),
+};
+
+static const struct b43_bbatt b43_full_bbatt_list_items[] = {
+	{ .att = 0, },
+	{ .att = 1, },
+	{ .att = 2, },
+	{ .att = 3, },
+	{ .att = 4, },
+	{ .att = 5, },
+	{ .att = 6, },
+	{ .att = 7, },
+	{ .att = 8, },
+	{ .att = 9, },
+	{ .att = 10, },
+	{ .att = 11, },
+};
+static const struct b43_bbatt_list b43_full_bbatt_list = {
+	.list		= b43_full_bbatt_list_items,
+	.len		= ARRAY_SIZE(b43_full_bbatt_list_items),
+};
+#endif /* B43_CALIB_ALL_LOCTLS */
+
+static void lo_measure(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+	struct b43_txpower_lo_control *lo = phy->lo_control;
+	struct b43_loctl loctl = {
+		.i = 0,
+		.q = 0,
+	};
+	struct b43_loctl *ploctl;
+	int max_rx_gain;
+	int rfidx, bbidx;
+	const struct b43_bbatt_list *bbatt_list;
+	const struct b43_rfatt_list *rfatt_list;
+
+	/* Values from the "TXCTL Register and Value Table" */
+	u16 txctl_reg;
+	u16 txctl_value;
+	u16 pad_mix_gain;
+
+	bbatt_list = &lo->bbatt_list;
+	rfatt_list = &lo->rfatt_list;
+#if B43_CALIB_ALL_LOCTLS
+	bbatt_list = &b43_full_bbatt_list;
+	rfatt_list = &b43_full_rfatt_list;
+#endif
+
+	txctl_reg = lo_txctl_register_table(dev, &txctl_value, &pad_mix_gain);
+
+	for (rfidx = 0; rfidx < rfatt_list->len; rfidx++) {
+
+		b43_radio_write16(dev, 0x43, (b43_radio_read16(dev, 0x43)
+					      & 0xFFF0) |
+				  rfatt_list->list[rfidx].att);
+		b43_radio_write16(dev, txctl_reg,
+				  (b43_radio_read16(dev, txctl_reg)
+				   & ~txctl_value)
+				  | (rfatt_list->list[rfidx].with_padmix ?
+				     txctl_value : 0));
+
+		for (bbidx = 0; bbidx < bbatt_list->len; bbidx++) {
+			if (lo->rebuild) {
+#if B43_CALIB_ALL_LOCTLS
+				ploctl = b43_get_lo_g_ctl(dev,
+							  &rfatt_list->list[rfidx],
+							  &bbatt_list->list[bbidx]);
+#else
+				ploctl = b43_get_lo_g_ctl_nopadmix(dev,
+								   &rfatt_list->
+								   list[rfidx],
+								   &bbatt_list->
+								   list[bbidx]);
+#endif
+			} else {
+				ploctl = b43_get_lo_g_ctl(dev,
+							  &rfatt_list->list[rfidx],
+							  &bbatt_list->list[bbidx]);
+				if (!ploctl->used)
+					continue;
+			}
+			memcpy(&loctl, ploctl, sizeof(loctl));
+			loctl.i = 0;
+			loctl.q = 0;
+
+			max_rx_gain = rfatt_list->list[rfidx].att * 2;
+			max_rx_gain += bbatt_list->list[bbidx].att / 2;
+			if (rfatt_list->list[rfidx].with_padmix)
+				max_rx_gain -= pad_mix_gain;
+			if (has_loopback_gain(phy))
+				max_rx_gain += phy->max_lb_gain;
+			lo_measure_gain_values(dev, max_rx_gain,
+					       has_loopback_gain(phy));
+
+			b43_phy_set_baseband_attenuation(dev,
+							 bbatt_list->list[bbidx].att);
+			lo_probe_loctls_statemachine(dev, &loctl, &max_rx_gain);
+			if (phy->type == B43_PHYTYPE_B) {
+				loctl.i++;
+				loctl.q++;
+			}
+			b43_loctl_set_calibrated(&loctl, 1);
+			memcpy(ploctl, &loctl, sizeof(loctl));
+		}
+	}
+}
+
+#if B43_DEBUG
+static void do_validate_loctl(struct b43_wldev *dev, struct b43_loctl *control)
+{
+	const int is_initializing = (b43_status(dev) == B43_STAT_UNINIT);
+	int i = control->i;
+	int q = control->q;
+
+	if (b43_loctl_is_calibrated(control)) {
+		if ((abs(i) > 16) || (abs(q) > 16))
+			goto error;
+	} else {
+		if (control->used)
+			goto error;
+		if (dev->phy.lo_control->rebuild) {
+			control->i = 0;
+			control->q = 0;
+			if ((i != B43_LOCTL_POISON) ||
+			    (q != B43_LOCTL_POISON))
+				goto error;
+		}
+	}
+	if (is_initializing && control->used)
+		goto error;
+
+	return;
+error:
+	b43err(dev->wl, "LO control pair validation failed "
+	       "(I: %d, Q: %d, used %u, calib: %u, initing: %d)\n",
+	       i, q, control->used,
+	       b43_loctl_is_calibrated(control),
+	       is_initializing);
+}
+
+static void validate_all_loctls(struct b43_wldev *dev)
+{
+	b43_call_for_each_loctl(dev, do_validate_loctl);
+}
+
+static void do_reset_calib(struct b43_wldev *dev, struct b43_loctl *control)
+{
+	if (dev->phy.lo_control->rebuild ||
+	    control->used) {
+		b43_loctl_set_calibrated(control, 0);
+		control->i = B43_LOCTL_POISON;
+		control->q = B43_LOCTL_POISON;
+	}
+}
+
+static void reset_all_loctl_calibration_states(struct b43_wldev *dev)
+{
+	b43_call_for_each_loctl(dev, do_reset_calib);
+}
+
+#else /* B43_DEBUG */
+static inline void validate_all_loctls(struct b43_wldev *dev) { }
+static inline void reset_all_loctl_calibration_states(struct b43_wldev *dev) { }
+#endif /* B43_DEBUG */
+
+void b43_lo_g_measure(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+	struct lo_g_saved_values uninitialized_var(sav);
+
+	B43_WARN_ON((phy->type != B43_PHYTYPE_B) &&
+		    (phy->type != B43_PHYTYPE_G));
+
+	sav.old_channel = phy->channel;
+	lo_measure_setup(dev, &sav);
+	reset_all_loctl_calibration_states(dev);
+	lo_measure(dev);
+	lo_measure_restore(dev, &sav);
+
+	validate_all_loctls(dev);
+
+	phy->lo_control->lo_measured = 1;
+	phy->lo_control->rebuild = 0;
+}
+
+#if B43_DEBUG
+static void validate_loctl_calibration(struct b43_wldev *dev,
+				       struct b43_loctl *loctl,
+				       struct b43_rfatt *rfatt,
+				       struct b43_bbatt *bbatt)
+{
+	if (b43_loctl_is_calibrated(loctl))
+		return;
+	if (!dev->phy.lo_control->lo_measured) {
+		/* On init we set the attenuation values before we
+		 * calibrated the LO. I guess that's OK. */
+		return;
+	}
+	b43err(dev->wl, "Adjusting Local Oscillator to an uncalibrated "
+	       "control pair: rfatt=%u,%spadmix bbatt=%u\n",
+	       rfatt->att,
+	       (rfatt->with_padmix) ? "" : "no-",
+	       bbatt->att);
+}
+#else
+static inline void validate_loctl_calibration(struct b43_wldev *dev,
+					      struct b43_loctl *loctl,
+					      struct b43_rfatt *rfatt,
+					      struct b43_bbatt *bbatt)
+{
+}
+#endif
+
+static inline void fixup_rfatt_for_txcontrol(struct b43_rfatt *rf,
+					     u8 tx_control)
+{
+	if (tx_control & B43_TXCTL_TXMIX) {
+		if (rf->att < 5)
+			rf->att = 4;
+	}
+}
+
+void b43_lo_g_adjust(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+	struct b43_rfatt rf;
+	struct b43_loctl *loctl;
+
+	memcpy(&rf, &phy->rfatt, sizeof(rf));
+	fixup_rfatt_for_txcontrol(&rf, phy->tx_control);
+
+	loctl = b43_get_lo_g_ctl(dev, &rf, &phy->bbatt);
+	validate_loctl_calibration(dev, loctl, &rf, &phy->bbatt);
+	b43_lo_write(dev, loctl);
+}
+
+void b43_lo_g_adjust_to(struct b43_wldev *dev,
+			u16 rfatt, u16 bbatt, u16 tx_control)
+{
+	struct b43_rfatt rf;
+	struct b43_bbatt bb;
+	struct b43_loctl *loctl;
+
+	memset(&rf, 0, sizeof(rf));
+	memset(&bb, 0, sizeof(bb));
+	rf.att = rfatt;
+	bb.att = bbatt;
+	fixup_rfatt_for_txcontrol(&rf, tx_control);
+	loctl = b43_get_lo_g_ctl(dev, &rf, &bb);
+	validate_loctl_calibration(dev, loctl, &rf, &bb);
+	b43_lo_write(dev, loctl);
+}
+
+static void do_mark_unused(struct b43_wldev *dev, struct b43_loctl *control)
+{
+	control->used = 0;
+}
+
+void b43_lo_g_ctl_mark_all_unused(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+	struct b43_txpower_lo_control *lo = phy->lo_control;
+
+	b43_call_for_each_loctl(dev, do_mark_unused);
+	lo->rebuild = 1;
+}
+
+void b43_lo_g_ctl_mark_cur_used(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+	struct b43_rfatt rf;
+
+	memcpy(&rf, &phy->rfatt, sizeof(rf));
+	fixup_rfatt_for_txcontrol(&rf, phy->tx_control);
+
+	b43_get_lo_g_ctl(dev, &rf, &phy->bbatt)->used = 1;
+}
diff --git a/drivers/net/wireless/b43/lo.h b/drivers/net/wireless/b43/lo.h
new file mode 100644
index 000000000000..455615d1f8c6
--- /dev/null
+++ b/drivers/net/wireless/b43/lo.h
@@ -0,0 +1,112 @@
+#ifndef B43_LO_H_
+#define B43_LO_H_
+
+#include "phy.h"
+
+struct b43_wldev;
+
+/* Local Oscillator control value-pair. */
+struct b43_loctl {
+	/* Control values. */
+	s8 i;
+	s8 q;
+	/* "Used by hardware" flag. */
+	bool used;
+#ifdef CONFIG_B43_DEBUG
+	/* Is this lo-control-array entry calibrated? */
+	bool calibrated;
+#endif
+};
+
+/* Debugging: Poison value for i and q values. */
+#define B43_LOCTL_POISON	111
+
+/* loctl->calibrated debugging mechanism */
+#ifdef CONFIG_B43_DEBUG
+static inline void b43_loctl_set_calibrated(struct b43_loctl *loctl,
+					    bool calibrated)
+{
+	loctl->calibrated = calibrated;
+}
+static inline bool b43_loctl_is_calibrated(struct b43_loctl *loctl)
+{
+	return loctl->calibrated;
+}
+#else
+static inline void b43_loctl_set_calibrated(struct b43_loctl *loctl,
+					    bool calibrated)
+{
+}
+static inline bool b43_loctl_is_calibrated(struct b43_loctl *loctl)
+{
+	return 1;
+}
+#endif
+
+/* TX Power LO Control Array.
+ * Value-pairs to adjust the LocalOscillator are stored
+ * in this structure.
+ * There are two different set of values. One for "Flag is Set"
+ * and one for "Flag is Unset".
+ * By "Flag" the flag in struct b43_rfatt is meant.
+ * The Value arrays are two-dimensional. The first index
+ * is the baseband attenuation and the second index
+ * is the radio attenuation.
+ * Use b43_get_lo_g_ctl() to retrieve a value from the lists.
+ */
+struct b43_txpower_lo_control {
+#define B43_NR_BB	12
+#define B43_NR_RF	16
+	/* LO Control values, with PAD Mixer */
+	struct b43_loctl with_padmix[B43_NR_BB][B43_NR_RF];
+	/* LO Control values, without PAD Mixer */
+	struct b43_loctl no_padmix[B43_NR_BB][B43_NR_RF];
+
+	/* Flag to indicate a complete rebuild of the two tables above
+	 * to the LO measuring code. */
+	bool rebuild;
+
+	/* Lists of valid RF and BB attenuation values for this device. */
+	struct b43_rfatt_list rfatt_list;
+	struct b43_bbatt_list bbatt_list;
+
+	/* Current TX Bias value */
+	u8 tx_bias;
+	/* Current TX Magnification Value (if used by the device) */
+	u8 tx_magn;
+
+	/* GPHY LO is measured. */
+	bool lo_measured;
+
+	/* Saved device PowerVector */
+	u64 power_vector;
+};
+
+/* Measure the BPHY Local Oscillator. */
+void b43_lo_b_measure(struct b43_wldev *dev);
+/* Measure the BPHY/GPHY Local Oscillator. */
+void b43_lo_g_measure(struct b43_wldev *dev);
+
+/* Adjust the Local Oscillator to the saved attenuation
+ * and txctl values.
+ */
+void b43_lo_g_adjust(struct b43_wldev *dev);
+/* Adjust to specific values. */
+void b43_lo_g_adjust_to(struct b43_wldev *dev,
+			u16 rfatt, u16 bbatt, u16 tx_control);
+
+/* Mark all possible b43_lo_g_ctl as "unused" */
+void b43_lo_g_ctl_mark_all_unused(struct b43_wldev *dev);
+/* Mark the b43_lo_g_ctl corresponding to the current
+ * attenuation values as used.
+ */
+void b43_lo_g_ctl_mark_cur_used(struct b43_wldev *dev);
+
+/* Get a reference to a LO Control value pair in the
+ * TX Power LO Control Array.
+ */
+struct b43_loctl *b43_get_lo_g_ctl(struct b43_wldev *dev,
+				   const struct b43_rfatt *rfatt,
+				   const struct b43_bbatt *bbatt);
+
+#endif /* B43_LO_H_ */
diff --git a/drivers/net/wireless/b43/main.c b/drivers/net/wireless/b43/main.c
new file mode 100644
index 000000000000..c141a264ac45
--- /dev/null
+++ b/drivers/net/wireless/b43/main.c
@@ -0,0 +1,4070 @@
+/*
+
+  Broadcom B43 wireless driver
+
+  Copyright (c) 2005 Martin Langer <martin-langer@gmx.de>
+  Copyright (c) 2005 Stefano Brivio <st3@riseup.net>
+  Copyright (c) 2005, 2006 Michael Buesch <mb@bu3sch.de>
+  Copyright (c) 2005 Danny van Dyk <kugelfang@gentoo.org>
+  Copyright (c) 2005 Andreas Jaggi <andreas.jaggi@waterwave.ch>
+
+  Some parts of the code in this file are derived from the ipw2200
+  driver  Copyright(c) 2003 - 2004 Intel Corporation.
+
+  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, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with this program; see the file COPYING.  If not, write to
+  the Free Software Foundation, Inc., 51 Franklin Steet, Fifth Floor,
+  Boston, MA 02110-1301, USA.
+
+*/
+
+#include <linux/delay.h>
+#include <linux/init.h>
+#include <linux/moduleparam.h>
+#include <linux/if_arp.h>
+#include <linux/etherdevice.h>
+#include <linux/version.h>
+#include <linux/firmware.h>
+#include <linux/wireless.h>
+#include <linux/workqueue.h>
+#include <linux/skbuff.h>
+#include <linux/dma-mapping.h>
+#include <asm/unaligned.h>
+
+#include "b43.h"
+#include "main.h"
+#include "debugfs.h"
+#include "phy.h"
+#include "dma.h"
+#include "pio.h"
+#include "sysfs.h"
+#include "xmit.h"
+#include "sysfs.h"
+#include "lo.h"
+#include "pcmcia.h"
+
+MODULE_DESCRIPTION("Broadcom B43 wireless driver");
+MODULE_AUTHOR("Martin Langer");
+MODULE_AUTHOR("Stefano Brivio");
+MODULE_AUTHOR("Michael Buesch");
+MODULE_LICENSE("GPL");
+
+extern char *nvram_get(char *name);
+
+#if defined(CONFIG_B43_DMA) && defined(CONFIG_B43_PIO)
+static int modparam_pio;
+module_param_named(pio, modparam_pio, int, 0444);
+MODULE_PARM_DESC(pio, "enable(1) / disable(0) PIO mode");
+#elif defined(CONFIG_B43_DMA)
+# define modparam_pio	0
+#elif defined(CONFIG_B43_PIO)
+# define modparam_pio	1
+#endif
+
+static int modparam_bad_frames_preempt;
+module_param_named(bad_frames_preempt, modparam_bad_frames_preempt, int, 0444);
+MODULE_PARM_DESC(bad_frames_preempt,
+		 "enable(1) / disable(0) Bad Frames Preemption");
+
+static int modparam_short_retry = B43_DEFAULT_SHORT_RETRY_LIMIT;
+module_param_named(short_retry, modparam_short_retry, int, 0444);
+MODULE_PARM_DESC(short_retry, "Short-Retry-Limit (0 - 15)");
+
+static int modparam_long_retry = B43_DEFAULT_LONG_RETRY_LIMIT;
+module_param_named(long_retry, modparam_long_retry, int, 0444);
+MODULE_PARM_DESC(long_retry, "Long-Retry-Limit (0 - 15)");
+
+static char modparam_fwpostfix[16];
+module_param_string(fwpostfix, modparam_fwpostfix, 16, 0444);
+MODULE_PARM_DESC(fwpostfix, "Postfix for the .fw files to load.");
+
+static int modparam_hwpctl;
+module_param_named(hwpctl, modparam_hwpctl, int, 0444);
+MODULE_PARM_DESC(hwpctl, "Enable hardware-side power control (default off)");
+
+static int modparam_nohwcrypt;
+module_param_named(nohwcrypt, modparam_nohwcrypt, int, 0444);
+MODULE_PARM_DESC(nohwcrypt, "Disable hardware encryption.");
+
+static const struct ssb_device_id b43_ssb_tbl[] = {
+	SSB_DEVICE(SSB_VENDOR_BROADCOM, SSB_DEV_80211, 5),
+	SSB_DEVICE(SSB_VENDOR_BROADCOM, SSB_DEV_80211, 6),
+	SSB_DEVICE(SSB_VENDOR_BROADCOM, SSB_DEV_80211, 7),
+	SSB_DEVICE(SSB_VENDOR_BROADCOM, SSB_DEV_80211, 9),
+	SSB_DEVICE(SSB_VENDOR_BROADCOM, SSB_DEV_80211, 10),
+	SSB_DEVTABLE_END
+};
+
+MODULE_DEVICE_TABLE(ssb, b43_ssb_tbl);
+
+/* Channel and ratetables are shared for all devices.
+ * They can't be const, because ieee80211 puts some precalculated
+ * data in there. This data is the same for all devices, so we don't
+ * get concurrency issues */
+#define RATETAB_ENT(_rateid, _flags) \
+	{							\
+		.rate	= B43_RATE_TO_BASE100KBPS(_rateid),	\
+		.val	= (_rateid),				\
+		.val2	= (_rateid),				\
+		.flags	= (_flags),				\
+	}
+static struct ieee80211_rate __b43_ratetable[] = {
+	RATETAB_ENT(B43_CCK_RATE_1MB, IEEE80211_RATE_CCK),
+	RATETAB_ENT(B43_CCK_RATE_2MB, IEEE80211_RATE_CCK_2),
+	RATETAB_ENT(B43_CCK_RATE_5MB, IEEE80211_RATE_CCK_2),
+	RATETAB_ENT(B43_CCK_RATE_11MB, IEEE80211_RATE_CCK_2),
+	RATETAB_ENT(B43_OFDM_RATE_6MB, IEEE80211_RATE_OFDM),
+	RATETAB_ENT(B43_OFDM_RATE_9MB, IEEE80211_RATE_OFDM),
+	RATETAB_ENT(B43_OFDM_RATE_12MB, IEEE80211_RATE_OFDM),
+	RATETAB_ENT(B43_OFDM_RATE_18MB, IEEE80211_RATE_OFDM),
+	RATETAB_ENT(B43_OFDM_RATE_24MB, IEEE80211_RATE_OFDM),
+	RATETAB_ENT(B43_OFDM_RATE_36MB, IEEE80211_RATE_OFDM),
+	RATETAB_ENT(B43_OFDM_RATE_48MB, IEEE80211_RATE_OFDM),
+	RATETAB_ENT(B43_OFDM_RATE_54MB, IEEE80211_RATE_OFDM),
+};
+
+#define b43_a_ratetable		(__b43_ratetable + 4)
+#define b43_a_ratetable_size	8
+#define b43_b_ratetable		(__b43_ratetable + 0)
+#define b43_b_ratetable_size	4
+#define b43_g_ratetable		(__b43_ratetable + 0)
+#define b43_g_ratetable_size	12
+
+#define CHANTAB_ENT(_chanid, _freq) \
+	{							\
+		.chan	= (_chanid),				\
+		.freq	= (_freq),				\
+		.val	= (_chanid),				\
+		.flag	= IEEE80211_CHAN_W_SCAN |		\
+			  IEEE80211_CHAN_W_ACTIVE_SCAN |	\
+			  IEEE80211_CHAN_W_IBSS,		\
+		.power_level	= 0xFF,				\
+		.antenna_max	= 0xFF,				\
+	}
+static struct ieee80211_channel b43_bg_chantable[] = {
+	CHANTAB_ENT(1, 2412),
+	CHANTAB_ENT(2, 2417),
+	CHANTAB_ENT(3, 2422),
+	CHANTAB_ENT(4, 2427),
+	CHANTAB_ENT(5, 2432),
+	CHANTAB_ENT(6, 2437),
+	CHANTAB_ENT(7, 2442),
+	CHANTAB_ENT(8, 2447),
+	CHANTAB_ENT(9, 2452),
+	CHANTAB_ENT(10, 2457),
+	CHANTAB_ENT(11, 2462),
+	CHANTAB_ENT(12, 2467),
+	CHANTAB_ENT(13, 2472),
+	CHANTAB_ENT(14, 2484),
+};
+
+#define b43_bg_chantable_size	ARRAY_SIZE(b43_bg_chantable)
+static struct ieee80211_channel b43_a_chantable[] = {
+	CHANTAB_ENT(36, 5180),
+	CHANTAB_ENT(40, 5200),
+	CHANTAB_ENT(44, 5220),
+	CHANTAB_ENT(48, 5240),
+	CHANTAB_ENT(52, 5260),
+	CHANTAB_ENT(56, 5280),
+	CHANTAB_ENT(60, 5300),
+	CHANTAB_ENT(64, 5320),
+	CHANTAB_ENT(149, 5745),
+	CHANTAB_ENT(153, 5765),
+	CHANTAB_ENT(157, 5785),
+	CHANTAB_ENT(161, 5805),
+	CHANTAB_ENT(165, 5825),
+};
+
+#define b43_a_chantable_size	ARRAY_SIZE(b43_a_chantable)
+
+static void b43_wireless_core_exit(struct b43_wldev *dev);
+static int b43_wireless_core_init(struct b43_wldev *dev);
+static void b43_wireless_core_stop(struct b43_wldev *dev);
+static int b43_wireless_core_start(struct b43_wldev *dev);
+
+static int b43_ratelimit(struct b43_wl *wl)
+{
+	if (!wl || !wl->current_dev)
+		return 1;
+	if (b43_status(wl->current_dev) < B43_STAT_STARTED)
+		return 1;
+	/* We are up and running.
+	 * Ratelimit the messages to avoid DoS over the net. */
+	return net_ratelimit();
+}
+
+void b43info(struct b43_wl *wl, const char *fmt, ...)
+{
+	va_list args;
+
+	if (!b43_ratelimit(wl))
+		return;
+	va_start(args, fmt);
+	printk(KERN_INFO "b43-%s: ",
+	       (wl && wl->hw) ? wiphy_name(wl->hw->wiphy) : "wlan");
+	vprintk(fmt, args);
+	va_end(args);
+}
+
+void b43err(struct b43_wl *wl, const char *fmt, ...)
+{
+	va_list args;
+
+	if (!b43_ratelimit(wl))
+		return;
+	va_start(args, fmt);
+	printk(KERN_ERR "b43-%s ERROR: ",
+	       (wl && wl->hw) ? wiphy_name(wl->hw->wiphy) : "wlan");
+	vprintk(fmt, args);
+	va_end(args);
+}
+
+void b43warn(struct b43_wl *wl, const char *fmt, ...)
+{
+	va_list args;
+
+	if (!b43_ratelimit(wl))
+		return;
+	va_start(args, fmt);
+	printk(KERN_WARNING "b43-%s warning: ",
+	       (wl && wl->hw) ? wiphy_name(wl->hw->wiphy) : "wlan");
+	vprintk(fmt, args);
+	va_end(args);
+}
+
+#if B43_DEBUG
+void b43dbg(struct b43_wl *wl, const char *fmt, ...)
+{
+	va_list args;
+
+	va_start(args, fmt);
+	printk(KERN_DEBUG "b43-%s debug: ",
+	       (wl && wl->hw) ? wiphy_name(wl->hw->wiphy) : "wlan");
+	vprintk(fmt, args);
+	va_end(args);
+}
+#endif /* DEBUG */
+
+static void b43_ram_write(struct b43_wldev *dev, u16 offset, u32 val)
+{
+	u32 macctl;
+
+	B43_WARN_ON(offset % 4 != 0);
+
+	macctl = b43_read32(dev, B43_MMIO_MACCTL);
+	if (macctl & B43_MACCTL_BE)
+		val = swab32(val);
+
+	b43_write32(dev, B43_MMIO_RAM_CONTROL, offset);
+	mmiowb();
+	b43_write32(dev, B43_MMIO_RAM_DATA, val);
+}
+
+static inline
+    void b43_shm_control_word(struct b43_wldev *dev, u16 routing, u16 offset)
+{
+	u32 control;
+
+	/* "offset" is the WORD offset. */
+
+	control = routing;
+	control <<= 16;
+	control |= offset;
+	b43_write32(dev, B43_MMIO_SHM_CONTROL, control);
+}
+
+u32 b43_shm_read32(struct b43_wldev *dev, u16 routing, u16 offset)
+{
+	u32 ret;
+
+	if (routing == B43_SHM_SHARED) {
+		B43_WARN_ON(offset & 0x0001);
+		if (offset & 0x0003) {
+			/* Unaligned access */
+			b43_shm_control_word(dev, routing, offset >> 2);
+			ret = b43_read16(dev, B43_MMIO_SHM_DATA_UNALIGNED);
+			ret <<= 16;
+			b43_shm_control_word(dev, routing, (offset >> 2) + 1);
+			ret |= b43_read16(dev, B43_MMIO_SHM_DATA);
+
+			return ret;
+		}
+		offset >>= 2;
+	}
+	b43_shm_control_word(dev, routing, offset);
+	ret = b43_read32(dev, B43_MMIO_SHM_DATA);
+
+	return ret;
+}
+
+u16 b43_shm_read16(struct b43_wldev * dev, u16 routing, u16 offset)
+{
+	u16 ret;
+
+	if (routing == B43_SHM_SHARED) {
+		B43_WARN_ON(offset & 0x0001);
+		if (offset & 0x0003) {
+			/* Unaligned access */
+			b43_shm_control_word(dev, routing, offset >> 2);
+			ret = b43_read16(dev, B43_MMIO_SHM_DATA_UNALIGNED);
+
+			return ret;
+		}
+		offset >>= 2;
+	}
+	b43_shm_control_word(dev, routing, offset);
+	ret = b43_read16(dev, B43_MMIO_SHM_DATA);
+
+	return ret;
+}
+
+void b43_shm_write32(struct b43_wldev *dev, u16 routing, u16 offset, u32 value)
+{
+	if (routing == B43_SHM_SHARED) {
+		B43_WARN_ON(offset & 0x0001);
+		if (offset & 0x0003) {
+			/* Unaligned access */
+			b43_shm_control_word(dev, routing, offset >> 2);
+			mmiowb();
+			b43_write16(dev, B43_MMIO_SHM_DATA_UNALIGNED,
+				    (value >> 16) & 0xffff);
+			mmiowb();
+			b43_shm_control_word(dev, routing, (offset >> 2) + 1);
+			mmiowb();
+			b43_write16(dev, B43_MMIO_SHM_DATA, value & 0xffff);
+			return;
+		}
+		offset >>= 2;
+	}
+	b43_shm_control_word(dev, routing, offset);
+	mmiowb();
+	b43_write32(dev, B43_MMIO_SHM_DATA, value);
+}
+
+void b43_shm_write16(struct b43_wldev *dev, u16 routing, u16 offset, u16 value)
+{
+	if (routing == B43_SHM_SHARED) {
+		B43_WARN_ON(offset & 0x0001);
+		if (offset & 0x0003) {
+			/* Unaligned access */
+			b43_shm_control_word(dev, routing, offset >> 2);
+			mmiowb();
+			b43_write16(dev, B43_MMIO_SHM_DATA_UNALIGNED, value);
+			return;
+		}
+		offset >>= 2;
+	}
+	b43_shm_control_word(dev, routing, offset);
+	mmiowb();
+	b43_write16(dev, B43_MMIO_SHM_DATA, value);
+}
+
+/* Read HostFlags */
+u32 b43_hf_read(struct b43_wldev * dev)
+{
+	u32 ret;
+
+	ret = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_HOSTFHI);
+	ret <<= 16;
+	ret |= b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_HOSTFLO);
+
+	return ret;
+}
+
+/* Write HostFlags */
+void b43_hf_write(struct b43_wldev *dev, u32 value)
+{
+	b43_shm_write16(dev, B43_SHM_SHARED,
+			B43_SHM_SH_HOSTFLO, (value & 0x0000FFFF));
+	b43_shm_write16(dev, B43_SHM_SHARED,
+			B43_SHM_SH_HOSTFHI, ((value & 0xFFFF0000) >> 16));
+}
+
+void b43_tsf_read(struct b43_wldev *dev, u64 * tsf)
+{
+	/* We need to be careful. As we read the TSF from multiple
+	 * registers, we should take care of register overflows.
+	 * In theory, the whole tsf read process should be atomic.
+	 * We try to be atomic here, by restaring the read process,
+	 * if any of the high registers changed (overflew).
+	 */
+	if (dev->dev->id.revision >= 3) {
+		u32 low, high, high2;
+
+		do {
+			high = b43_read32(dev, B43_MMIO_REV3PLUS_TSF_HIGH);
+			low = b43_read32(dev, B43_MMIO_REV3PLUS_TSF_LOW);
+			high2 = b43_read32(dev, B43_MMIO_REV3PLUS_TSF_HIGH);
+		} while (unlikely(high != high2));
+
+		*tsf = high;
+		*tsf <<= 32;
+		*tsf |= low;
+	} else {
+		u64 tmp;
+		u16 v0, v1, v2, v3;
+		u16 test1, test2, test3;
+
+		do {
+			v3 = b43_read16(dev, B43_MMIO_TSF_3);
+			v2 = b43_read16(dev, B43_MMIO_TSF_2);
+			v1 = b43_read16(dev, B43_MMIO_TSF_1);
+			v0 = b43_read16(dev, B43_MMIO_TSF_0);
+
+			test3 = b43_read16(dev, B43_MMIO_TSF_3);
+			test2 = b43_read16(dev, B43_MMIO_TSF_2);
+			test1 = b43_read16(dev, B43_MMIO_TSF_1);
+		} while (v3 != test3 || v2 != test2 || v1 != test1);
+
+		*tsf = v3;
+		*tsf <<= 48;
+		tmp = v2;
+		tmp <<= 32;
+		*tsf |= tmp;
+		tmp = v1;
+		tmp <<= 16;
+		*tsf |= tmp;
+		*tsf |= v0;
+	}
+}
+
+static void b43_time_lock(struct b43_wldev *dev)
+{
+	u32 macctl;
+
+	macctl = b43_read32(dev, B43_MMIO_MACCTL);
+	macctl |= B43_MACCTL_TBTTHOLD;
+	b43_write32(dev, B43_MMIO_MACCTL, macctl);
+	/* Commit the write */
+	b43_read32(dev, B43_MMIO_MACCTL);
+}
+
+static void b43_time_unlock(struct b43_wldev *dev)
+{
+	u32 macctl;
+
+	macctl = b43_read32(dev, B43_MMIO_MACCTL);
+	macctl &= ~B43_MACCTL_TBTTHOLD;
+	b43_write32(dev, B43_MMIO_MACCTL, macctl);
+	/* Commit the write */
+	b43_read32(dev, B43_MMIO_MACCTL);
+}
+
+static void b43_tsf_write_locked(struct b43_wldev *dev, u64 tsf)
+{
+	/* Be careful with the in-progress timer.
+	 * First zero out the low register, so we have a full
+	 * register-overflow duration to complete the operation.
+	 */
+	if (dev->dev->id.revision >= 3) {
+		u32 lo = (tsf & 0x00000000FFFFFFFFULL);
+		u32 hi = (tsf & 0xFFFFFFFF00000000ULL) >> 32;
+
+		b43_write32(dev, B43_MMIO_REV3PLUS_TSF_LOW, 0);
+		mmiowb();
+		b43_write32(dev, B43_MMIO_REV3PLUS_TSF_HIGH, hi);
+		mmiowb();
+		b43_write32(dev, B43_MMIO_REV3PLUS_TSF_LOW, lo);
+	} else {
+		u16 v0 = (tsf & 0x000000000000FFFFULL);
+		u16 v1 = (tsf & 0x00000000FFFF0000ULL) >> 16;
+		u16 v2 = (tsf & 0x0000FFFF00000000ULL) >> 32;
+		u16 v3 = (tsf & 0xFFFF000000000000ULL) >> 48;
+
+		b43_write16(dev, B43_MMIO_TSF_0, 0);
+		mmiowb();
+		b43_write16(dev, B43_MMIO_TSF_3, v3);
+		mmiowb();
+		b43_write16(dev, B43_MMIO_TSF_2, v2);
+		mmiowb();
+		b43_write16(dev, B43_MMIO_TSF_1, v1);
+		mmiowb();
+		b43_write16(dev, B43_MMIO_TSF_0, v0);
+	}
+}
+
+void b43_tsf_write(struct b43_wldev *dev, u64 tsf)
+{
+	b43_time_lock(dev);
+	b43_tsf_write_locked(dev, tsf);
+	b43_time_unlock(dev);
+}
+
+static
+void b43_macfilter_set(struct b43_wldev *dev, u16 offset, const u8 * mac)
+{
+	static const u8 zero_addr[ETH_ALEN] = { 0 };
+	u16 data;
+
+	if (!mac)
+		mac = zero_addr;
+
+	offset |= 0x0020;
+	b43_write16(dev, B43_MMIO_MACFILTER_CONTROL, offset);
+
+	data = mac[0];
+	data |= mac[1] << 8;
+	b43_write16(dev, B43_MMIO_MACFILTER_DATA, data);
+	data = mac[2];
+	data |= mac[3] << 8;
+	b43_write16(dev, B43_MMIO_MACFILTER_DATA, data);
+	data = mac[4];
+	data |= mac[5] << 8;
+	b43_write16(dev, B43_MMIO_MACFILTER_DATA, data);
+}
+
+static void b43_write_mac_bssid_templates(struct b43_wldev *dev)
+{
+	const u8 *mac;
+	const u8 *bssid;
+	u8 mac_bssid[ETH_ALEN * 2];
+	int i;
+	u32 tmp;
+
+	bssid = dev->wl->bssid;
+	mac = dev->wl->mac_addr;
+
+	b43_macfilter_set(dev, B43_MACFILTER_BSSID, bssid);
+
+	memcpy(mac_bssid, mac, ETH_ALEN);
+	memcpy(mac_bssid + ETH_ALEN, bssid, ETH_ALEN);
+
+	/* Write our MAC address and BSSID to template ram */
+	for (i = 0; i < ARRAY_SIZE(mac_bssid); i += sizeof(u32)) {
+		tmp = (u32) (mac_bssid[i + 0]);
+		tmp |= (u32) (mac_bssid[i + 1]) << 8;
+		tmp |= (u32) (mac_bssid[i + 2]) << 16;
+		tmp |= (u32) (mac_bssid[i + 3]) << 24;
+		b43_ram_write(dev, 0x20 + i, tmp);
+	}
+}
+
+static void b43_upload_card_macaddress(struct b43_wldev *dev)
+{
+	b43_write_mac_bssid_templates(dev);
+	b43_macfilter_set(dev, B43_MACFILTER_SELF, dev->wl->mac_addr);
+}
+
+static void b43_set_slot_time(struct b43_wldev *dev, u16 slot_time)
+{
+	/* slot_time is in usec. */
+	if (dev->phy.type != B43_PHYTYPE_G)
+		return;
+	b43_write16(dev, 0x684, 510 + slot_time);
+	b43_shm_write16(dev, B43_SHM_SHARED, 0x0010, slot_time);
+}
+
+static void b43_short_slot_timing_enable(struct b43_wldev *dev)
+{
+	b43_set_slot_time(dev, 9);
+	dev->short_slot = 1;
+}
+
+static void b43_short_slot_timing_disable(struct b43_wldev *dev)
+{
+	b43_set_slot_time(dev, 20);
+	dev->short_slot = 0;
+}
+
+/* Enable a Generic IRQ. "mask" is the mask of which IRQs to enable.
+ * Returns the _previously_ enabled IRQ mask.
+ */
+static inline u32 b43_interrupt_enable(struct b43_wldev *dev, u32 mask)
+{
+	u32 old_mask;
+
+	old_mask = b43_read32(dev, B43_MMIO_GEN_IRQ_MASK);
+	b43_write32(dev, B43_MMIO_GEN_IRQ_MASK, old_mask | mask);
+
+	return old_mask;
+}
+
+/* Disable a Generic IRQ. "mask" is the mask of which IRQs to disable.
+ * Returns the _previously_ enabled IRQ mask.
+ */
+static inline u32 b43_interrupt_disable(struct b43_wldev *dev, u32 mask)
+{
+	u32 old_mask;
+
+	old_mask = b43_read32(dev, B43_MMIO_GEN_IRQ_MASK);
+	b43_write32(dev, B43_MMIO_GEN_IRQ_MASK, old_mask & ~mask);
+
+	return old_mask;
+}
+
+/* Synchronize IRQ top- and bottom-half.
+ * IRQs must be masked before calling this.
+ * This must not be called with the irq_lock held.
+ */
+static void b43_synchronize_irq(struct b43_wldev *dev)
+{
+	synchronize_irq(dev->dev->irq);
+	tasklet_kill(&dev->isr_tasklet);
+}
+
+/* DummyTransmission function, as documented on
+ * http://bcm-specs.sipsolutions.net/DummyTransmission
+ */
+void b43_dummy_transmission(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+	unsigned int i, max_loop;
+	u16 value;
+	u32 buffer[5] = {
+		0x00000000,
+		0x00D40000,
+		0x00000000,
+		0x01000000,
+		0x00000000,
+	};
+
+	switch (phy->type) {
+	case B43_PHYTYPE_A:
+		max_loop = 0x1E;
+		buffer[0] = 0x000201CC;
+		break;
+	case B43_PHYTYPE_B:
+	case B43_PHYTYPE_G:
+		max_loop = 0xFA;
+		buffer[0] = 0x000B846E;
+		break;
+	default:
+		B43_WARN_ON(1);
+		return;
+	}
+
+	for (i = 0; i < 5; i++)
+		b43_ram_write(dev, i * 4, buffer[i]);
+
+	/* Commit writes */
+	b43_read32(dev, B43_MMIO_MACCTL);
+
+	b43_write16(dev, 0x0568, 0x0000);
+	b43_write16(dev, 0x07C0, 0x0000);
+	value = ((phy->type == B43_PHYTYPE_A) ? 1 : 0);
+	b43_write16(dev, 0x050C, value);
+	b43_write16(dev, 0x0508, 0x0000);
+	b43_write16(dev, 0x050A, 0x0000);
+	b43_write16(dev, 0x054C, 0x0000);
+	b43_write16(dev, 0x056A, 0x0014);
+	b43_write16(dev, 0x0568, 0x0826);
+	b43_write16(dev, 0x0500, 0x0000);
+	b43_write16(dev, 0x0502, 0x0030);
+
+	if (phy->radio_ver == 0x2050 && phy->radio_rev <= 0x5)
+		b43_radio_write16(dev, 0x0051, 0x0017);
+	for (i = 0x00; i < max_loop; i++) {
+		value = b43_read16(dev, 0x050E);
+		if (value & 0x0080)
+			break;
+		udelay(10);
+	}
+	for (i = 0x00; i < 0x0A; i++) {
+		value = b43_read16(dev, 0x050E);
+		if (value & 0x0400)
+			break;
+		udelay(10);
+	}
+	for (i = 0x00; i < 0x0A; i++) {
+		value = b43_read16(dev, 0x0690);
+		if (!(value & 0x0100))
+			break;
+		udelay(10);
+	}
+	if (phy->radio_ver == 0x2050 && phy->radio_rev <= 0x5)
+		b43_radio_write16(dev, 0x0051, 0x0037);
+}
+
+static void key_write(struct b43_wldev *dev,
+		      u8 index, u8 algorithm, const u8 * key)
+{
+	unsigned int i;
+	u32 offset;
+	u16 value;
+	u16 kidx;
+
+	/* Key index/algo block */
+	kidx = b43_kidx_to_fw(dev, index);
+	value = ((kidx << 4) | algorithm);
+	b43_shm_write16(dev, B43_SHM_SHARED,
+			B43_SHM_SH_KEYIDXBLOCK + (kidx * 2), value);
+
+	/* Write the key to the Key Table Pointer offset */
+	offset = dev->ktp + (index * B43_SEC_KEYSIZE);
+	for (i = 0; i < B43_SEC_KEYSIZE; i += 2) {
+		value = key[i];
+		value |= (u16) (key[i + 1]) << 8;
+		b43_shm_write16(dev, B43_SHM_SHARED, offset + i, value);
+	}
+}
+
+static void keymac_write(struct b43_wldev *dev, u8 index, const u8 * addr)
+{
+	u32 addrtmp[2] = { 0, 0, };
+	u8 per_sta_keys_start = 8;
+
+	if (b43_new_kidx_api(dev))
+		per_sta_keys_start = 4;
+
+	B43_WARN_ON(index < per_sta_keys_start);
+	/* We have two default TX keys and possibly two default RX keys.
+	 * Physical mac 0 is mapped to physical key 4 or 8, depending
+	 * on the firmware version.
+	 * So we must adjust the index here.
+	 */
+	index -= per_sta_keys_start;
+
+	if (addr) {
+		addrtmp[0] = addr[0];
+		addrtmp[0] |= ((u32) (addr[1]) << 8);
+		addrtmp[0] |= ((u32) (addr[2]) << 16);
+		addrtmp[0] |= ((u32) (addr[3]) << 24);
+		addrtmp[1] = addr[4];
+		addrtmp[1] |= ((u32) (addr[5]) << 8);
+	}
+
+	if (dev->dev->id.revision >= 5) {
+		/* Receive match transmitter address mechanism */
+		b43_shm_write32(dev, B43_SHM_RCMTA,
+				(index * 2) + 0, addrtmp[0]);
+		b43_shm_write16(dev, B43_SHM_RCMTA,
+				(index * 2) + 1, addrtmp[1]);
+	} else {
+		/* RXE (Receive Engine) and
+		 * PSM (Programmable State Machine) mechanism
+		 */
+		if (index < 8) {
+			/* TODO write to RCM 16, 19, 22 and 25 */
+		} else {
+			b43_shm_write32(dev, B43_SHM_SHARED,
+					B43_SHM_SH_PSM + (index * 6) + 0,
+					addrtmp[0]);
+			b43_shm_write16(dev, B43_SHM_SHARED,
+					B43_SHM_SH_PSM + (index * 6) + 4,
+					addrtmp[1]);
+		}
+	}
+}
+
+static void do_key_write(struct b43_wldev *dev,
+			 u8 index, u8 algorithm,
+			 const u8 * key, size_t key_len, const u8 * mac_addr)
+{
+	u8 buf[B43_SEC_KEYSIZE] = { 0, };
+	u8 per_sta_keys_start = 8;
+
+	if (b43_new_kidx_api(dev))
+		per_sta_keys_start = 4;
+
+	B43_WARN_ON(index >= dev->max_nr_keys);
+	B43_WARN_ON(key_len > B43_SEC_KEYSIZE);
+
+	if (index >= per_sta_keys_start)
+		keymac_write(dev, index, NULL);	/* First zero out mac. */
+	if (key)
+		memcpy(buf, key, key_len);
+	key_write(dev, index, algorithm, buf);
+	if (index >= per_sta_keys_start)
+		keymac_write(dev, index, mac_addr);
+
+	dev->key[index].algorithm = algorithm;
+}
+
+static int b43_key_write(struct b43_wldev *dev,
+			 int index, u8 algorithm,
+			 const u8 * key, size_t key_len,
+			 const u8 * mac_addr,
+			 struct ieee80211_key_conf *keyconf)
+{
+	int i;
+	int sta_keys_start;
+
+	if (key_len > B43_SEC_KEYSIZE)
+		return -EINVAL;
+	for (i = 0; i < dev->max_nr_keys; i++) {
+		/* Check that we don't already have this key. */
+		B43_WARN_ON(dev->key[i].keyconf == keyconf);
+	}
+	if (index < 0) {
+		/* Either pairwise key or address is 00:00:00:00:00:00
+		 * for transmit-only keys. Search the index. */
+		if (b43_new_kidx_api(dev))
+			sta_keys_start = 4;
+		else
+			sta_keys_start = 8;
+		for (i = sta_keys_start; i < dev->max_nr_keys; i++) {
+			if (!dev->key[i].keyconf) {
+				/* found empty */
+				index = i;
+				break;
+			}
+		}
+		if (index < 0) {
+			b43err(dev->wl, "Out of hardware key memory\n");
+			return -ENOSPC;
+		}
+	} else
+		B43_WARN_ON(index > 3);
+
+	do_key_write(dev, index, algorithm, key, key_len, mac_addr);
+	if ((index <= 3) && !b43_new_kidx_api(dev)) {
+		/* Default RX key */
+		B43_WARN_ON(mac_addr);
+		do_key_write(dev, index + 4, algorithm, key, key_len, NULL);
+	}
+	keyconf->hw_key_idx = index;
+	dev->key[index].keyconf = keyconf;
+
+	return 0;
+}
+
+static int b43_key_clear(struct b43_wldev *dev, int index)
+{
+	if (B43_WARN_ON((index < 0) || (index >= dev->max_nr_keys)))
+		return -EINVAL;
+	do_key_write(dev, index, B43_SEC_ALGO_NONE,
+		     NULL, B43_SEC_KEYSIZE, NULL);
+	if ((index <= 3) && !b43_new_kidx_api(dev)) {
+		do_key_write(dev, index + 4, B43_SEC_ALGO_NONE,
+			     NULL, B43_SEC_KEYSIZE, NULL);
+	}
+	dev->key[index].keyconf = NULL;
+
+	return 0;
+}
+
+static void b43_clear_keys(struct b43_wldev *dev)
+{
+	int i;
+
+	for (i = 0; i < dev->max_nr_keys; i++)
+		b43_key_clear(dev, i);
+}
+
+void b43_power_saving_ctl_bits(struct b43_wldev *dev, unsigned int ps_flags)
+{
+	u32 macctl;
+	u16 ucstat;
+	bool hwps;
+	bool awake;
+	int i;
+
+	B43_WARN_ON((ps_flags & B43_PS_ENABLED) &&
+		    (ps_flags & B43_PS_DISABLED));
+	B43_WARN_ON((ps_flags & B43_PS_AWAKE) && (ps_flags & B43_PS_ASLEEP));
+
+	if (ps_flags & B43_PS_ENABLED) {
+		hwps = 1;
+	} else if (ps_flags & B43_PS_DISABLED) {
+		hwps = 0;
+	} else {
+		//TODO: If powersave is not off and FIXME is not set and we are not in adhoc
+		//      and thus is not an AP and we are associated, set bit 25
+	}
+	if (ps_flags & B43_PS_AWAKE) {
+		awake = 1;
+	} else if (ps_flags & B43_PS_ASLEEP) {
+		awake = 0;
+	} else {
+		//TODO: If the device is awake or this is an AP, or we are scanning, or FIXME,
+		//      or we are associated, or FIXME, or the latest PS-Poll packet sent was
+		//      successful, set bit26
+	}
+
+/* FIXME: For now we force awake-on and hwps-off */
+	hwps = 0;
+	awake = 1;
+
+	macctl = b43_read32(dev, B43_MMIO_MACCTL);
+	if (hwps)
+		macctl |= B43_MACCTL_HWPS;
+	else
+		macctl &= ~B43_MACCTL_HWPS;
+	if (awake)
+		macctl |= B43_MACCTL_AWAKE;
+	else
+		macctl &= ~B43_MACCTL_AWAKE;
+	b43_write32(dev, B43_MMIO_MACCTL, macctl);
+	/* Commit write */
+	b43_read32(dev, B43_MMIO_MACCTL);
+	if (awake && dev->dev->id.revision >= 5) {
+		/* Wait for the microcode to wake up. */
+		for (i = 0; i < 100; i++) {
+			ucstat = b43_shm_read16(dev, B43_SHM_SHARED,
+						B43_SHM_SH_UCODESTAT);
+			if (ucstat != B43_SHM_SH_UCODESTAT_SLEEP)
+				break;
+			udelay(10);
+		}
+	}
+}
+
+/* Turn the Analog ON/OFF */
+static void b43_switch_analog(struct b43_wldev *dev, int on)
+{
+	b43_write16(dev, B43_MMIO_PHY0, on ? 0 : 0xF4);
+}
+
+void b43_wireless_core_reset(struct b43_wldev *dev, u32 flags)
+{
+	u32 tmslow;
+	u32 macctl;
+
+	flags |= B43_TMSLOW_PHYCLKEN;
+	flags |= B43_TMSLOW_PHYRESET;
+	ssb_device_enable(dev->dev, flags);
+	msleep(2);		/* Wait for the PLL to turn on. */
+
+	/* Now take the PHY out of Reset again */
+	tmslow = ssb_read32(dev->dev, SSB_TMSLOW);
+	tmslow |= SSB_TMSLOW_FGC;
+	tmslow &= ~B43_TMSLOW_PHYRESET;
+	ssb_write32(dev->dev, SSB_TMSLOW, tmslow);
+	ssb_read32(dev->dev, SSB_TMSLOW);	/* flush */
+	msleep(1);
+	tmslow &= ~SSB_TMSLOW_FGC;
+	ssb_write32(dev->dev, SSB_TMSLOW, tmslow);
+	ssb_read32(dev->dev, SSB_TMSLOW);	/* flush */
+	msleep(1);
+
+	/* Turn Analog ON */
+	b43_switch_analog(dev, 1);
+
+	macctl = b43_read32(dev, B43_MMIO_MACCTL);
+	macctl &= ~B43_MACCTL_GMODE;
+	if (flags & B43_TMSLOW_GMODE)
+		macctl |= B43_MACCTL_GMODE;
+	macctl |= B43_MACCTL_IHR_ENABLED;
+	b43_write32(dev, B43_MMIO_MACCTL, macctl);
+}
+
+static void handle_irq_transmit_status(struct b43_wldev *dev)
+{
+	u32 v0, v1;
+	u16 tmp;
+	struct b43_txstatus stat;
+
+	while (1) {
+		v0 = b43_read32(dev, B43_MMIO_XMITSTAT_0);
+		if (!(v0 & 0x00000001))
+			break;
+		v1 = b43_read32(dev, B43_MMIO_XMITSTAT_1);
+
+		stat.cookie = (v0 >> 16);
+		stat.seq = (v1 & 0x0000FFFF);
+		stat.phy_stat = ((v1 & 0x00FF0000) >> 16);
+		tmp = (v0 & 0x0000FFFF);
+		stat.frame_count = ((tmp & 0xF000) >> 12);
+		stat.rts_count = ((tmp & 0x0F00) >> 8);
+		stat.supp_reason = ((tmp & 0x001C) >> 2);
+		stat.pm_indicated = !!(tmp & 0x0080);
+		stat.intermediate = !!(tmp & 0x0040);
+		stat.for_ampdu = !!(tmp & 0x0020);
+		stat.acked = !!(tmp & 0x0002);
+
+		b43_handle_txstatus(dev, &stat);
+	}
+}
+
+static void drain_txstatus_queue(struct b43_wldev *dev)
+{
+	u32 dummy;
+
+	if (dev->dev->id.revision < 5)
+		return;
+	/* Read all entries from the microcode TXstatus FIFO
+	 * and throw them away.
+	 */
+	while (1) {
+		dummy = b43_read32(dev, B43_MMIO_XMITSTAT_0);
+		if (!(dummy & 0x00000001))
+			break;
+		dummy = b43_read32(dev, B43_MMIO_XMITSTAT_1);
+	}
+}
+
+static u32 b43_jssi_read(struct b43_wldev *dev)
+{
+	u32 val = 0;
+
+	val = b43_shm_read16(dev, B43_SHM_SHARED, 0x08A);
+	val <<= 16;
+	val |= b43_shm_read16(dev, B43_SHM_SHARED, 0x088);
+
+	return val;
+}
+
+static void b43_jssi_write(struct b43_wldev *dev, u32 jssi)
+{
+	b43_shm_write16(dev, B43_SHM_SHARED, 0x088, (jssi & 0x0000FFFF));
+	b43_shm_write16(dev, B43_SHM_SHARED, 0x08A, (jssi & 0xFFFF0000) >> 16);
+}
+
+static void b43_generate_noise_sample(struct b43_wldev *dev)
+{
+	b43_jssi_write(dev, 0x7F7F7F7F);
+	b43_write32(dev, B43_MMIO_STATUS2_BITFIELD,
+		    b43_read32(dev, B43_MMIO_STATUS2_BITFIELD)
+		    | (1 << 4));
+	B43_WARN_ON(dev->noisecalc.channel_at_start != dev->phy.channel);
+}
+
+static void b43_calculate_link_quality(struct b43_wldev *dev)
+{
+	/* Top half of Link Quality calculation. */
+
+	if (dev->noisecalc.calculation_running)
+		return;
+	dev->noisecalc.channel_at_start = dev->phy.channel;
+	dev->noisecalc.calculation_running = 1;
+	dev->noisecalc.nr_samples = 0;
+
+	b43_generate_noise_sample(dev);
+}
+
+static void handle_irq_noise(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+	u16 tmp;
+	u8 noise[4];
+	u8 i, j;
+	s32 average;
+
+	/* Bottom half of Link Quality calculation. */
+
+	B43_WARN_ON(!dev->noisecalc.calculation_running);
+	if (dev->noisecalc.channel_at_start != phy->channel)
+		goto drop_calculation;
+	*((__le32 *)noise) = cpu_to_le32(b43_jssi_read(dev));
+	if (noise[0] == 0x7F || noise[1] == 0x7F ||
+	    noise[2] == 0x7F || noise[3] == 0x7F)
+		goto generate_new;
+
+	/* Get the noise samples. */
+	B43_WARN_ON(dev->noisecalc.nr_samples >= 8);
+	i = dev->noisecalc.nr_samples;
+	noise[0] = limit_value(noise[0], 0, ARRAY_SIZE(phy->nrssi_lt) - 1);
+	noise[1] = limit_value(noise[1], 0, ARRAY_SIZE(phy->nrssi_lt) - 1);
+	noise[2] = limit_value(noise[2], 0, ARRAY_SIZE(phy->nrssi_lt) - 1);
+	noise[3] = limit_value(noise[3], 0, ARRAY_SIZE(phy->nrssi_lt) - 1);
+	dev->noisecalc.samples[i][0] = phy->nrssi_lt[noise[0]];
+	dev->noisecalc.samples[i][1] = phy->nrssi_lt[noise[1]];
+	dev->noisecalc.samples[i][2] = phy->nrssi_lt[noise[2]];
+	dev->noisecalc.samples[i][3] = phy->nrssi_lt[noise[3]];
+	dev->noisecalc.nr_samples++;
+	if (dev->noisecalc.nr_samples == 8) {
+		/* Calculate the Link Quality by the noise samples. */
+		average = 0;
+		for (i = 0; i < 8; i++) {
+			for (j = 0; j < 4; j++)
+				average += dev->noisecalc.samples[i][j];
+		}
+		average /= (8 * 4);
+		average *= 125;
+		average += 64;
+		average /= 128;
+		tmp = b43_shm_read16(dev, B43_SHM_SHARED, 0x40C);
+		tmp = (tmp / 128) & 0x1F;
+		if (tmp >= 8)
+			average += 2;
+		else
+			average -= 25;
+		if (tmp == 8)
+			average -= 72;
+		else
+			average -= 48;
+
+		dev->stats.link_noise = average;
+	      drop_calculation:
+		dev->noisecalc.calculation_running = 0;
+		return;
+	}
+      generate_new:
+	b43_generate_noise_sample(dev);
+}
+
+static void handle_irq_tbtt_indication(struct b43_wldev *dev)
+{
+	if (b43_is_mode(dev->wl, IEEE80211_IF_TYPE_AP)) {
+		///TODO: PS TBTT
+	} else {
+		if (1 /*FIXME: the last PSpoll frame was sent successfully */ )
+			b43_power_saving_ctl_bits(dev, 0);
+	}
+	dev->reg124_set_0x4 = 0;
+	if (b43_is_mode(dev->wl, IEEE80211_IF_TYPE_IBSS))
+		dev->reg124_set_0x4 = 1;
+}
+
+static void handle_irq_atim_end(struct b43_wldev *dev)
+{
+	if (!dev->reg124_set_0x4 /*FIXME rename this variable */ )
+		return;
+	b43_write32(dev, B43_MMIO_STATUS2_BITFIELD,
+		    b43_read32(dev, B43_MMIO_STATUS2_BITFIELD)
+		    | 0x4);
+}
+
+static void handle_irq_pmq(struct b43_wldev *dev)
+{
+	u32 tmp;
+
+	//TODO: AP mode.
+
+	while (1) {
+		tmp = b43_read32(dev, B43_MMIO_PS_STATUS);
+		if (!(tmp & 0x00000008))
+			break;
+	}
+	/* 16bit write is odd, but correct. */
+	b43_write16(dev, B43_MMIO_PS_STATUS, 0x0002);
+}
+
+static void b43_write_template_common(struct b43_wldev *dev,
+				      const u8 * data, u16 size,
+				      u16 ram_offset,
+				      u16 shm_size_offset, u8 rate)
+{
+	u32 i, tmp;
+	struct b43_plcp_hdr4 plcp;
+
+	plcp.data = 0;
+	b43_generate_plcp_hdr(&plcp, size + FCS_LEN, rate);
+	b43_ram_write(dev, ram_offset, le32_to_cpu(plcp.data));
+	ram_offset += sizeof(u32);
+	/* The PLCP is 6 bytes long, but we only wrote 4 bytes, yet.
+	 * So leave the first two bytes of the next write blank.
+	 */
+	tmp = (u32) (data[0]) << 16;
+	tmp |= (u32) (data[1]) << 24;
+	b43_ram_write(dev, ram_offset, tmp);
+	ram_offset += sizeof(u32);
+	for (i = 2; i < size; i += sizeof(u32)) {
+		tmp = (u32) (data[i + 0]);
+		if (i + 1 < size)
+			tmp |= (u32) (data[i + 1]) << 8;
+		if (i + 2 < size)
+			tmp |= (u32) (data[i + 2]) << 16;
+		if (i + 3 < size)
+			tmp |= (u32) (data[i + 3]) << 24;
+		b43_ram_write(dev, ram_offset + i - 2, tmp);
+	}
+	b43_shm_write16(dev, B43_SHM_SHARED, shm_size_offset,
+			size + sizeof(struct b43_plcp_hdr6));
+}
+
+static void b43_write_beacon_template(struct b43_wldev *dev,
+				      u16 ram_offset,
+				      u16 shm_size_offset, u8 rate)
+{
+	int len;
+	const u8 *data;
+
+	B43_WARN_ON(!dev->cached_beacon);
+	len = min((size_t) dev->cached_beacon->len,
+		  0x200 - sizeof(struct b43_plcp_hdr6));
+	data = (const u8 *)(dev->cached_beacon->data);
+	b43_write_template_common(dev, data,
+				  len, ram_offset, shm_size_offset, rate);
+}
+
+static void b43_write_probe_resp_plcp(struct b43_wldev *dev,
+				      u16 shm_offset, u16 size, u8 rate)
+{
+	struct b43_plcp_hdr4 plcp;
+	u32 tmp;
+	__le16 dur;
+
+	plcp.data = 0;
+	b43_generate_plcp_hdr(&plcp, size + FCS_LEN, rate);
+	dur = ieee80211_generic_frame_duration(dev->wl->hw,
+					       dev->wl->if_id, size,
+					       B43_RATE_TO_BASE100KBPS(rate));
+	/* Write PLCP in two parts and timing for packet transfer */
+	tmp = le32_to_cpu(plcp.data);
+	b43_shm_write16(dev, B43_SHM_SHARED, shm_offset, tmp & 0xFFFF);
+	b43_shm_write16(dev, B43_SHM_SHARED, shm_offset + 2, tmp >> 16);
+	b43_shm_write16(dev, B43_SHM_SHARED, shm_offset + 6, le16_to_cpu(dur));
+}
+
+/* Instead of using custom probe response template, this function
+ * just patches custom beacon template by:
+ * 1) Changing packet type
+ * 2) Patching duration field
+ * 3) Stripping TIM
+ */
+static u8 *b43_generate_probe_resp(struct b43_wldev *dev,
+				   u16 * dest_size, u8 rate)
+{
+	const u8 *src_data;
+	u8 *dest_data;
+	u16 src_size, elem_size, src_pos, dest_pos;
+	__le16 dur;
+	struct ieee80211_hdr *hdr;
+
+	B43_WARN_ON(!dev->cached_beacon);
+	src_size = dev->cached_beacon->len;
+	src_data = (const u8 *)dev->cached_beacon->data;
+
+	if (unlikely(src_size < 0x24)) {
+		b43dbg(dev->wl, "b43_generate_probe_resp: " "invalid beacon\n");
+		return NULL;
+	}
+
+	dest_data = kmalloc(src_size, GFP_ATOMIC);
+	if (unlikely(!dest_data))
+		return NULL;
+
+	/* 0x24 is offset of first variable-len Information-Element
+	 * in beacon frame.
+	 */
+	memcpy(dest_data, src_data, 0x24);
+	src_pos = dest_pos = 0x24;
+	for (; src_pos < src_size - 2; src_pos += elem_size) {
+		elem_size = src_data[src_pos + 1] + 2;
+		if (src_data[src_pos] != 0x05) {	/* TIM */
+			memcpy(dest_data + dest_pos, src_data + src_pos,
+			       elem_size);
+			dest_pos += elem_size;
+		}
+	}
+	*dest_size = dest_pos;
+	hdr = (struct ieee80211_hdr *)dest_data;
+
+	/* Set the frame control. */
+	hdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
+					 IEEE80211_STYPE_PROBE_RESP);
+	dur = ieee80211_generic_frame_duration(dev->wl->hw,
+					       dev->wl->if_id, *dest_size,
+					       B43_RATE_TO_BASE100KBPS(rate));
+	hdr->duration_id = dur;
+
+	return dest_data;
+}
+
+static void b43_write_probe_resp_template(struct b43_wldev *dev,
+					  u16 ram_offset,
+					  u16 shm_size_offset, u8 rate)
+{
+	u8 *probe_resp_data;
+	u16 size;
+
+	B43_WARN_ON(!dev->cached_beacon);
+	size = dev->cached_beacon->len;
+	probe_resp_data = b43_generate_probe_resp(dev, &size, rate);
+	if (unlikely(!probe_resp_data))
+		return;
+
+	/* Looks like PLCP headers plus packet timings are stored for
+	 * all possible basic rates
+	 */
+	b43_write_probe_resp_plcp(dev, 0x31A, size, B43_CCK_RATE_1MB);
+	b43_write_probe_resp_plcp(dev, 0x32C, size, B43_CCK_RATE_2MB);
+	b43_write_probe_resp_plcp(dev, 0x33E, size, B43_CCK_RATE_5MB);
+	b43_write_probe_resp_plcp(dev, 0x350, size, B43_CCK_RATE_11MB);
+
+	size = min((size_t) size, 0x200 - sizeof(struct b43_plcp_hdr6));
+	b43_write_template_common(dev, probe_resp_data,
+				  size, ram_offset, shm_size_offset, rate);
+	kfree(probe_resp_data);
+}
+
+static int b43_refresh_cached_beacon(struct b43_wldev *dev,
+				     struct sk_buff *beacon)
+{
+	if (dev->cached_beacon)
+		kfree_skb(dev->cached_beacon);
+	dev->cached_beacon = beacon;
+
+	return 0;
+}
+
+static void b43_update_templates(struct b43_wldev *dev)
+{
+	u32 status;
+
+	B43_WARN_ON(!dev->cached_beacon);
+
+	b43_write_beacon_template(dev, 0x68, 0x18, B43_CCK_RATE_1MB);
+	b43_write_beacon_template(dev, 0x468, 0x1A, B43_CCK_RATE_1MB);
+	b43_write_probe_resp_template(dev, 0x268, 0x4A, B43_CCK_RATE_11MB);
+
+	status = b43_read32(dev, B43_MMIO_STATUS2_BITFIELD);
+	status |= 0x03;
+	b43_write32(dev, B43_MMIO_STATUS2_BITFIELD, status);
+}
+
+static void b43_refresh_templates(struct b43_wldev *dev, struct sk_buff *beacon)
+{
+	int err;
+
+	err = b43_refresh_cached_beacon(dev, beacon);
+	if (unlikely(err))
+		return;
+	b43_update_templates(dev);
+}
+
+static void b43_set_ssid(struct b43_wldev *dev, const u8 * ssid, u8 ssid_len)
+{
+	u32 tmp;
+	u16 i, len;
+
+	len = min((u16) ssid_len, (u16) 0x100);
+	for (i = 0; i < len; i += sizeof(u32)) {
+		tmp = (u32) (ssid[i + 0]);
+		if (i + 1 < len)
+			tmp |= (u32) (ssid[i + 1]) << 8;
+		if (i + 2 < len)
+			tmp |= (u32) (ssid[i + 2]) << 16;
+		if (i + 3 < len)
+			tmp |= (u32) (ssid[i + 3]) << 24;
+		b43_shm_write32(dev, B43_SHM_SHARED, 0x380 + i, tmp);
+	}
+	b43_shm_write16(dev, B43_SHM_SHARED, 0x48, len);
+}
+
+static void b43_set_beacon_int(struct b43_wldev *dev, u16 beacon_int)
+{
+	b43_time_lock(dev);
+	if (dev->dev->id.revision >= 3) {
+		b43_write32(dev, 0x188, (beacon_int << 16));
+	} else {
+		b43_write16(dev, 0x606, (beacon_int >> 6));
+		b43_write16(dev, 0x610, beacon_int);
+	}
+	b43_time_unlock(dev);
+}
+
+static void handle_irq_beacon(struct b43_wldev *dev)
+{
+	u32 status;
+
+	if (!b43_is_mode(dev->wl, IEEE80211_IF_TYPE_AP))
+		return;
+
+	dev->irq_savedstate &= ~B43_IRQ_BEACON;
+	status = b43_read32(dev, B43_MMIO_STATUS2_BITFIELD);
+
+	if (!dev->cached_beacon || ((status & 0x1) && (status & 0x2))) {
+		/* ACK beacon IRQ. */
+		b43_write32(dev, B43_MMIO_GEN_IRQ_REASON, B43_IRQ_BEACON);
+		dev->irq_savedstate |= B43_IRQ_BEACON;
+		if (dev->cached_beacon)
+			kfree_skb(dev->cached_beacon);
+		dev->cached_beacon = NULL;
+		return;
+	}
+	if (!(status & 0x1)) {
+		b43_write_beacon_template(dev, 0x68, 0x18, B43_CCK_RATE_1MB);
+		status |= 0x1;
+		b43_write32(dev, B43_MMIO_STATUS2_BITFIELD, status);
+	}
+	if (!(status & 0x2)) {
+		b43_write_beacon_template(dev, 0x468, 0x1A, B43_CCK_RATE_1MB);
+		status |= 0x2;
+		b43_write32(dev, B43_MMIO_STATUS2_BITFIELD, status);
+	}
+}
+
+static void handle_irq_ucode_debug(struct b43_wldev *dev)
+{
+	//TODO
+}
+
+/* Interrupt handler bottom-half */
+static void b43_interrupt_tasklet(struct b43_wldev *dev)
+{
+	u32 reason;
+	u32 dma_reason[ARRAY_SIZE(dev->dma_reason)];
+	u32 merged_dma_reason = 0;
+	int i;
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev->wl->irq_lock, flags);
+
+	B43_WARN_ON(b43_status(dev) != B43_STAT_STARTED);
+
+	reason = dev->irq_reason;
+	for (i = 0; i < ARRAY_SIZE(dma_reason); i++) {
+		dma_reason[i] = dev->dma_reason[i];
+		merged_dma_reason |= dma_reason[i];
+	}
+
+	if (unlikely(reason & B43_IRQ_MAC_TXERR))
+		b43err(dev->wl, "MAC transmission error\n");
+
+	if (unlikely(reason & B43_IRQ_PHY_TXERR))
+		b43err(dev->wl, "PHY transmission error\n");
+
+	if (unlikely(merged_dma_reason & (B43_DMAIRQ_FATALMASK |
+					  B43_DMAIRQ_NONFATALMASK))) {
+		if (merged_dma_reason & B43_DMAIRQ_FATALMASK) {
+			b43err(dev->wl, "Fatal DMA error: "
+			       "0x%08X, 0x%08X, 0x%08X, "
+			       "0x%08X, 0x%08X, 0x%08X\n",
+			       dma_reason[0], dma_reason[1],
+			       dma_reason[2], dma_reason[3],
+			       dma_reason[4], dma_reason[5]);
+			b43_controller_restart(dev, "DMA error");
+			mmiowb();
+			spin_unlock_irqrestore(&dev->wl->irq_lock, flags);
+			return;
+		}
+		if (merged_dma_reason & B43_DMAIRQ_NONFATALMASK) {
+			b43err(dev->wl, "DMA error: "
+			       "0x%08X, 0x%08X, 0x%08X, "
+			       "0x%08X, 0x%08X, 0x%08X\n",
+			       dma_reason[0], dma_reason[1],
+			       dma_reason[2], dma_reason[3],
+			       dma_reason[4], dma_reason[5]);
+		}
+	}
+
+	if (unlikely(reason & B43_IRQ_UCODE_DEBUG))
+		handle_irq_ucode_debug(dev);
+	if (reason & B43_IRQ_TBTT_INDI)
+		handle_irq_tbtt_indication(dev);
+	if (reason & B43_IRQ_ATIM_END)
+		handle_irq_atim_end(dev);
+	if (reason & B43_IRQ_BEACON)
+		handle_irq_beacon(dev);
+	if (reason & B43_IRQ_PMQ)
+		handle_irq_pmq(dev);
+	if (reason & B43_IRQ_TXFIFO_FLUSH_OK)
+		;/* TODO */
+	if (reason & B43_IRQ_NOISESAMPLE_OK)
+		handle_irq_noise(dev);
+
+	/* Check the DMA reason registers for received data. */
+	if (dma_reason[0] & B43_DMAIRQ_RX_DONE) {
+		if (b43_using_pio(dev))
+			b43_pio_rx(dev->pio.queue0);
+		else
+			b43_dma_rx(dev->dma.rx_ring0);
+	}
+	B43_WARN_ON(dma_reason[1] & B43_DMAIRQ_RX_DONE);
+	B43_WARN_ON(dma_reason[2] & B43_DMAIRQ_RX_DONE);
+	if (dma_reason[3] & B43_DMAIRQ_RX_DONE) {
+		if (b43_using_pio(dev))
+			b43_pio_rx(dev->pio.queue3);
+		else
+			b43_dma_rx(dev->dma.rx_ring3);
+	}
+	B43_WARN_ON(dma_reason[4] & B43_DMAIRQ_RX_DONE);
+	B43_WARN_ON(dma_reason[5] & B43_DMAIRQ_RX_DONE);
+
+	if (reason & B43_IRQ_TX_OK)
+		handle_irq_transmit_status(dev);
+
+	b43_interrupt_enable(dev, dev->irq_savedstate);
+	mmiowb();
+	spin_unlock_irqrestore(&dev->wl->irq_lock, flags);
+}
+
+static void pio_irq_workaround(struct b43_wldev *dev, u16 base, int queueidx)
+{
+	u16 rxctl;
+
+	rxctl = b43_read16(dev, base + B43_PIO_RXCTL);
+	if (rxctl & B43_PIO_RXCTL_DATAAVAILABLE)
+		dev->dma_reason[queueidx] |= B43_DMAIRQ_RX_DONE;
+	else
+		dev->dma_reason[queueidx] &= ~B43_DMAIRQ_RX_DONE;
+}
+
+static void b43_interrupt_ack(struct b43_wldev *dev, u32 reason)
+{
+	if (b43_using_pio(dev) &&
+	    (dev->dev->id.revision < 3) &&
+	    (!(reason & B43_IRQ_PIO_WORKAROUND))) {
+		/* Apply a PIO specific workaround to the dma_reasons */
+		pio_irq_workaround(dev, B43_MMIO_PIO1_BASE, 0);
+		pio_irq_workaround(dev, B43_MMIO_PIO2_BASE, 1);
+		pio_irq_workaround(dev, B43_MMIO_PIO3_BASE, 2);
+		pio_irq_workaround(dev, B43_MMIO_PIO4_BASE, 3);
+	}
+
+	b43_write32(dev, B43_MMIO_GEN_IRQ_REASON, reason);
+
+	b43_write32(dev, B43_MMIO_DMA0_REASON, dev->dma_reason[0]);
+	b43_write32(dev, B43_MMIO_DMA1_REASON, dev->dma_reason[1]);
+	b43_write32(dev, B43_MMIO_DMA2_REASON, dev->dma_reason[2]);
+	b43_write32(dev, B43_MMIO_DMA3_REASON, dev->dma_reason[3]);
+	b43_write32(dev, B43_MMIO_DMA4_REASON, dev->dma_reason[4]);
+	b43_write32(dev, B43_MMIO_DMA5_REASON, dev->dma_reason[5]);
+}
+
+/* Interrupt handler top-half */
+static irqreturn_t b43_interrupt_handler(int irq, void *dev_id)
+{
+	irqreturn_t ret = IRQ_NONE;
+	struct b43_wldev *dev = dev_id;
+	u32 reason;
+
+	if (!dev)
+		return IRQ_NONE;
+
+	spin_lock(&dev->wl->irq_lock);
+
+	if (b43_status(dev) < B43_STAT_STARTED)
+		goto out;
+	reason = b43_read32(dev, B43_MMIO_GEN_IRQ_REASON);
+	if (reason == 0xffffffff)	/* shared IRQ */
+		goto out;
+	ret = IRQ_HANDLED;
+	reason &= b43_read32(dev, B43_MMIO_GEN_IRQ_MASK);
+	if (!reason)
+		goto out;
+
+	dev->dma_reason[0] = b43_read32(dev, B43_MMIO_DMA0_REASON)
+	    & 0x0001DC00;
+	dev->dma_reason[1] = b43_read32(dev, B43_MMIO_DMA1_REASON)
+	    & 0x0000DC00;
+	dev->dma_reason[2] = b43_read32(dev, B43_MMIO_DMA2_REASON)
+	    & 0x0000DC00;
+	dev->dma_reason[3] = b43_read32(dev, B43_MMIO_DMA3_REASON)
+	    & 0x0001DC00;
+	dev->dma_reason[4] = b43_read32(dev, B43_MMIO_DMA4_REASON)
+	    & 0x0000DC00;
+	dev->dma_reason[5] = b43_read32(dev, B43_MMIO_DMA5_REASON)
+	    & 0x0000DC00;
+
+	b43_interrupt_ack(dev, reason);
+	/* disable all IRQs. They are enabled again in the bottom half. */
+	dev->irq_savedstate = b43_interrupt_disable(dev, B43_IRQ_ALL);
+	/* save the reason code and call our bottom half. */
+	dev->irq_reason = reason;
+	tasklet_schedule(&dev->isr_tasklet);
+      out:
+	mmiowb();
+	spin_unlock(&dev->wl->irq_lock);
+
+	return ret;
+}
+
+static void b43_release_firmware(struct b43_wldev *dev)
+{
+	release_firmware(dev->fw.ucode);
+	dev->fw.ucode = NULL;
+	release_firmware(dev->fw.pcm);
+	dev->fw.pcm = NULL;
+	release_firmware(dev->fw.initvals);
+	dev->fw.initvals = NULL;
+	release_firmware(dev->fw.initvals_band);
+	dev->fw.initvals_band = NULL;
+}
+
+static void b43_print_fw_helptext(struct b43_wl *wl)
+{
+	b43err(wl, "You must go to "
+	       "http://linuxwireless.org/en/users/Drivers/bcm43xx#devicefirmware "
+	       "and download the correct firmware (version 4).\n");
+}
+
+static int do_request_fw(struct b43_wldev *dev,
+			 const char *name,
+			 const struct firmware **fw)
+{
+	char path[sizeof(modparam_fwpostfix) + 32];
+	struct b43_fw_header *hdr;
+	u32 size;
+	int err;
+
+	if (!name)
+		return 0;
+
+	snprintf(path, ARRAY_SIZE(path),
+		 "b43%s/%s.fw",
+		 modparam_fwpostfix, name);
+	err = request_firmware(fw, path, dev->dev->dev);
+	if (err) {
+		b43err(dev->wl, "Firmware file \"%s\" not found "
+		       "or load failed.\n", path);
+		return err;
+	}
+	if ((*fw)->size < sizeof(struct b43_fw_header))
+		goto err_format;
+	hdr = (struct b43_fw_header *)((*fw)->data);
+	switch (hdr->type) {
+	case B43_FW_TYPE_UCODE:
+	case B43_FW_TYPE_PCM:
+		size = be32_to_cpu(hdr->size);
+		if (size != (*fw)->size - sizeof(struct b43_fw_header))
+			goto err_format;
+		/* fallthrough */
+	case B43_FW_TYPE_IV:
+		if (hdr->ver != 1)
+			goto err_format;
+		break;
+	default:
+		goto err_format;
+	}
+
+	return err;
+
+err_format:
+	b43err(dev->wl, "Firmware file \"%s\" format error.\n", path);
+	return -EPROTO;
+}
+
+static int b43_request_firmware(struct b43_wldev *dev)
+{
+	struct b43_firmware *fw = &dev->fw;
+	const u8 rev = dev->dev->id.revision;
+	const char *filename;
+	u32 tmshigh;
+	int err;
+
+	tmshigh = ssb_read32(dev->dev, SSB_TMSHIGH);
+	if (!fw->ucode) {
+		if ((rev >= 5) && (rev <= 10))
+			filename = "ucode5";
+		else if ((rev >= 11) && (rev <= 12))
+			filename = "ucode11";
+		else if (rev >= 13)
+			filename = "ucode13";
+		else
+			goto err_no_ucode;
+		err = do_request_fw(dev, filename, &fw->ucode);
+		if (err)
+			goto err_load;
+	}
+	if (!fw->pcm) {
+		if ((rev >= 5) && (rev <= 10))
+			filename = "pcm5";
+		else if (rev >= 11)
+			filename = NULL;
+		else
+			goto err_no_pcm;
+		err = do_request_fw(dev, filename, &fw->pcm);
+		if (err)
+			goto err_load;
+	}
+	if (!fw->initvals) {
+		switch (dev->phy.type) {
+		case B43_PHYTYPE_A:
+			if ((rev >= 5) && (rev <= 10)) {
+				if (tmshigh & B43_TMSHIGH_GPHY)
+					filename = "a0g1initvals5";
+				else
+					filename = "a0g0initvals5";
+			} else
+				goto err_no_initvals;
+			break;
+		case B43_PHYTYPE_G:
+			if ((rev >= 5) && (rev <= 10))
+				filename = "b0g0initvals5";
+			else if (rev >= 13)
+				filename = "lp0initvals13";
+			else
+				goto err_no_initvals;
+			break;
+		default:
+			goto err_no_initvals;
+		}
+		err = do_request_fw(dev, filename, &fw->initvals);
+		if (err)
+			goto err_load;
+	}
+	if (!fw->initvals_band) {
+		switch (dev->phy.type) {
+		case B43_PHYTYPE_A:
+			if ((rev >= 5) && (rev <= 10)) {
+				if (tmshigh & B43_TMSHIGH_GPHY)
+					filename = "a0g1bsinitvals5";
+				else
+					filename = "a0g0bsinitvals5";
+			} else if (rev >= 11)
+				filename = NULL;
+			else
+				goto err_no_initvals;
+			break;
+		case B43_PHYTYPE_G:
+			if ((rev >= 5) && (rev <= 10))
+				filename = "b0g0bsinitvals5";
+			else if (rev >= 11)
+				filename = NULL;
+			else
+				goto err_no_initvals;
+			break;
+		default:
+			goto err_no_initvals;
+		}
+		err = do_request_fw(dev, filename, &fw->initvals_band);
+		if (err)
+			goto err_load;
+	}
+
+	return 0;
+
+err_load:
+	b43_print_fw_helptext(dev->wl);
+	goto error;
+
+err_no_ucode:
+	err = -ENODEV;
+	b43err(dev->wl, "No microcode available for core rev %u\n", rev);
+	goto error;
+
+err_no_pcm:
+	err = -ENODEV;
+	b43err(dev->wl, "No PCM available for core rev %u\n", rev);
+	goto error;
+
+err_no_initvals:
+	err = -ENODEV;
+	b43err(dev->wl, "No Initial Values firmware file for PHY %u, "
+	       "core rev %u\n", dev->phy.type, rev);
+	goto error;
+
+error:
+	b43_release_firmware(dev);
+	return err;
+}
+
+static int b43_upload_microcode(struct b43_wldev *dev)
+{
+	const size_t hdr_len = sizeof(struct b43_fw_header);
+	const __be32 *data;
+	unsigned int i, len;
+	u16 fwrev, fwpatch, fwdate, fwtime;
+	u32 tmp;
+	int err = 0;
+
+	/* Upload Microcode. */
+	data = (__be32 *) (dev->fw.ucode->data + hdr_len);
+	len = (dev->fw.ucode->size - hdr_len) / sizeof(__be32);
+	b43_shm_control_word(dev, B43_SHM_UCODE | B43_SHM_AUTOINC_W, 0x0000);
+	for (i = 0; i < len; i++) {
+		b43_write32(dev, B43_MMIO_SHM_DATA, be32_to_cpu(data[i]));
+		udelay(10);
+	}
+
+	if (dev->fw.pcm) {
+		/* Upload PCM data. */
+		data = (__be32 *) (dev->fw.pcm->data + hdr_len);
+		len = (dev->fw.pcm->size - hdr_len) / sizeof(__be32);
+		b43_shm_control_word(dev, B43_SHM_HW, 0x01EA);
+		b43_write32(dev, B43_MMIO_SHM_DATA, 0x00004000);
+		/* No need for autoinc bit in SHM_HW */
+		b43_shm_control_word(dev, B43_SHM_HW, 0x01EB);
+		for (i = 0; i < len; i++) {
+			b43_write32(dev, B43_MMIO_SHM_DATA, be32_to_cpu(data[i]));
+			udelay(10);
+		}
+	}
+
+	b43_write32(dev, B43_MMIO_GEN_IRQ_REASON, B43_IRQ_ALL);
+	b43_write32(dev, B43_MMIO_MACCTL,
+		    B43_MACCTL_PSM_RUN |
+		    B43_MACCTL_IHR_ENABLED | B43_MACCTL_INFRA);
+
+	/* Wait for the microcode to load and respond */
+	i = 0;
+	while (1) {
+		tmp = b43_read32(dev, B43_MMIO_GEN_IRQ_REASON);
+		if (tmp == B43_IRQ_MAC_SUSPENDED)
+			break;
+		i++;
+		if (i >= 50) {
+			b43err(dev->wl, "Microcode not responding\n");
+			b43_print_fw_helptext(dev->wl);
+			err = -ENODEV;
+			goto out;
+		}
+		udelay(10);
+	}
+	b43_read32(dev, B43_MMIO_GEN_IRQ_REASON);	/* dummy read */
+
+	/* Get and check the revisions. */
+	fwrev = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_UCODEREV);
+	fwpatch = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_UCODEPATCH);
+	fwdate = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_UCODEDATE);
+	fwtime = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_UCODETIME);
+
+	if (fwrev <= 0x128) {
+		b43err(dev->wl, "YOUR FIRMWARE IS TOO OLD. Firmware from "
+		       "binary drivers older than version 4.x is unsupported. "
+		       "You must upgrade your firmware files.\n");
+		b43_print_fw_helptext(dev->wl);
+		b43_write32(dev, B43_MMIO_MACCTL, 0);
+		err = -EOPNOTSUPP;
+		goto out;
+	}
+	b43dbg(dev->wl, "Loading firmware version %u.%u "
+	       "(20%.2i-%.2i-%.2i %.2i:%.2i:%.2i)\n",
+	       fwrev, fwpatch,
+	       (fwdate >> 12) & 0xF, (fwdate >> 8) & 0xF, fwdate & 0xFF,
+	       (fwtime >> 11) & 0x1F, (fwtime >> 5) & 0x3F, fwtime & 0x1F);
+
+	dev->fw.rev = fwrev;
+	dev->fw.patch = fwpatch;
+
+      out:
+	return err;
+}
+
+static int b43_write_initvals(struct b43_wldev *dev,
+			      const struct b43_iv *ivals,
+			      size_t count,
+			      size_t array_size)
+{
+	const struct b43_iv *iv;
+	u16 offset;
+	size_t i;
+	bool bit32;
+
+	BUILD_BUG_ON(sizeof(struct b43_iv) != 6);
+	iv = ivals;
+	for (i = 0; i < count; i++) {
+		if (array_size < sizeof(iv->offset_size))
+			goto err_format;
+		array_size -= sizeof(iv->offset_size);
+		offset = be16_to_cpu(iv->offset_size);
+		bit32 = !!(offset & B43_IV_32BIT);
+		offset &= B43_IV_OFFSET_MASK;
+		if (offset >= 0x1000)
+			goto err_format;
+		if (bit32) {
+			u32 value;
+
+			if (array_size < sizeof(iv->data.d32))
+				goto err_format;
+			array_size -= sizeof(iv->data.d32);
+
+			value = be32_to_cpu(get_unaligned(&iv->data.d32));
+			b43_write32(dev, offset, value);
+
+			iv = (const struct b43_iv *)((const uint8_t *)iv +
+							sizeof(__be16) +
+							sizeof(__be32));
+		} else {
+			u16 value;
+
+			if (array_size < sizeof(iv->data.d16))
+				goto err_format;
+			array_size -= sizeof(iv->data.d16);
+
+			value = be16_to_cpu(iv->data.d16);
+			b43_write16(dev, offset, value);
+
+			iv = (const struct b43_iv *)((const uint8_t *)iv +
+							sizeof(__be16) +
+							sizeof(__be16));
+		}
+	}
+	if (array_size)
+		goto err_format;
+
+	return 0;
+
+err_format:
+	b43err(dev->wl, "Initial Values Firmware file-format error.\n");
+	b43_print_fw_helptext(dev->wl);
+
+	return -EPROTO;
+}
+
+static int b43_upload_initvals(struct b43_wldev *dev)
+{
+	const size_t hdr_len = sizeof(struct b43_fw_header);
+	const struct b43_fw_header *hdr;
+	struct b43_firmware *fw = &dev->fw;
+	const struct b43_iv *ivals;
+	size_t count;
+	int err;
+
+	hdr = (const struct b43_fw_header *)(fw->initvals->data);
+	ivals = (const struct b43_iv *)(fw->initvals->data + hdr_len);
+	count = be32_to_cpu(hdr->size);
+	err = b43_write_initvals(dev, ivals, count,
+				 fw->initvals->size - hdr_len);
+	if (err)
+		goto out;
+	if (fw->initvals_band) {
+		hdr = (const struct b43_fw_header *)(fw->initvals_band->data);
+		ivals = (const struct b43_iv *)(fw->initvals_band->data + hdr_len);
+		count = be32_to_cpu(hdr->size);
+		err = b43_write_initvals(dev, ivals, count,
+					 fw->initvals_band->size - hdr_len);
+		if (err)
+			goto out;
+	}
+out:
+
+	return err;
+}
+
+/* Initialize the GPIOs
+ * http://bcm-specs.sipsolutions.net/GPIO
+ */
+static int b43_gpio_init(struct b43_wldev *dev)
+{
+	struct ssb_bus *bus = dev->dev->bus;
+	struct ssb_device *gpiodev, *pcidev = NULL;
+	u32 mask, set;
+
+	b43_write32(dev, B43_MMIO_MACCTL, b43_read32(dev, B43_MMIO_MACCTL)
+		    & ~B43_MACCTL_GPOUTSMSK);
+
+	b43_write16(dev, B43_MMIO_GPIO_MASK, b43_read16(dev, B43_MMIO_GPIO_MASK)
+		    | 0x000F);
+
+	mask = 0x0000001F;
+	set = 0x0000000F;
+	if (dev->dev->bus->chip_id == 0x4301) {
+		mask |= 0x0060;
+		set |= 0x0060;
+	}
+	if (0 /* FIXME: conditional unknown */ ) {
+		b43_write16(dev, B43_MMIO_GPIO_MASK,
+			    b43_read16(dev, B43_MMIO_GPIO_MASK)
+			    | 0x0100);
+		mask |= 0x0180;
+		set |= 0x0180;
+	}
+	if (dev->dev->bus->sprom.r1.boardflags_lo & B43_BFL_PACTRL) {
+		b43_write16(dev, B43_MMIO_GPIO_MASK,
+			    b43_read16(dev, B43_MMIO_GPIO_MASK)
+			    | 0x0200);
+		mask |= 0x0200;
+		set |= 0x0200;
+	}
+	if (dev->dev->id.revision >= 2)
+		mask |= 0x0010;	/* FIXME: This is redundant. */
+
+#ifdef CONFIG_SSB_DRIVER_PCICORE
+	pcidev = bus->pcicore.dev;
+#endif
+	gpiodev = bus->chipco.dev ? : pcidev;
+	if (!gpiodev)
+		return 0;
+	ssb_write32(gpiodev, B43_GPIO_CONTROL,
+		    (ssb_read32(gpiodev, B43_GPIO_CONTROL)
+		     & mask) | set);
+
+	return 0;
+}
+
+/* Turn off all GPIO stuff. Call this on module unload, for example. */
+static void b43_gpio_cleanup(struct b43_wldev *dev)
+{
+	struct ssb_bus *bus = dev->dev->bus;
+	struct ssb_device *gpiodev, *pcidev = NULL;
+
+#ifdef CONFIG_SSB_DRIVER_PCICORE
+	pcidev = bus->pcicore.dev;
+#endif
+	gpiodev = bus->chipco.dev ? : pcidev;
+	if (!gpiodev)
+		return;
+	ssb_write32(gpiodev, B43_GPIO_CONTROL, 0);
+}
+
+/* http://bcm-specs.sipsolutions.net/EnableMac */
+void b43_mac_enable(struct b43_wldev *dev)
+{
+	dev->mac_suspended--;
+	B43_WARN_ON(dev->mac_suspended < 0);
+	B43_WARN_ON(irqs_disabled());
+	if (dev->mac_suspended == 0) {
+		b43_write32(dev, B43_MMIO_MACCTL,
+			    b43_read32(dev, B43_MMIO_MACCTL)
+			    | B43_MACCTL_ENABLED);
+		b43_write32(dev, B43_MMIO_GEN_IRQ_REASON,
+			    B43_IRQ_MAC_SUSPENDED);
+		/* Commit writes */
+		b43_read32(dev, B43_MMIO_MACCTL);
+		b43_read32(dev, B43_MMIO_GEN_IRQ_REASON);
+		b43_power_saving_ctl_bits(dev, 0);
+
+		/* Re-enable IRQs. */
+		spin_lock_irq(&dev->wl->irq_lock);
+		b43_interrupt_enable(dev, dev->irq_savedstate);
+		spin_unlock_irq(&dev->wl->irq_lock);
+	}
+}
+
+/* http://bcm-specs.sipsolutions.net/SuspendMAC */
+void b43_mac_suspend(struct b43_wldev *dev)
+{
+	int i;
+	u32 tmp;
+
+	might_sleep();
+	B43_WARN_ON(irqs_disabled());
+	B43_WARN_ON(dev->mac_suspended < 0);
+
+	if (dev->mac_suspended == 0) {
+		/* Mask IRQs before suspending MAC. Otherwise
+		 * the MAC stays busy and won't suspend. */
+		spin_lock_irq(&dev->wl->irq_lock);
+		tmp = b43_interrupt_disable(dev, B43_IRQ_ALL);
+		spin_unlock_irq(&dev->wl->irq_lock);
+		b43_synchronize_irq(dev);
+		dev->irq_savedstate = tmp;
+
+		b43_power_saving_ctl_bits(dev, B43_PS_AWAKE);
+		b43_write32(dev, B43_MMIO_MACCTL,
+			    b43_read32(dev, B43_MMIO_MACCTL)
+			    & ~B43_MACCTL_ENABLED);
+		/* force pci to flush the write */
+		b43_read32(dev, B43_MMIO_MACCTL);
+		for (i = 40; i; i--) {
+			tmp = b43_read32(dev, B43_MMIO_GEN_IRQ_REASON);
+			if (tmp & B43_IRQ_MAC_SUSPENDED)
+				goto out;
+			msleep(1);
+		}
+		b43err(dev->wl, "MAC suspend failed\n");
+	}
+out:
+	dev->mac_suspended++;
+}
+
+static void b43_adjust_opmode(struct b43_wldev *dev)
+{
+	struct b43_wl *wl = dev->wl;
+	u32 ctl;
+	u16 cfp_pretbtt;
+
+	ctl = b43_read32(dev, B43_MMIO_MACCTL);
+	/* Reset status to STA infrastructure mode. */
+	ctl &= ~B43_MACCTL_AP;
+	ctl &= ~B43_MACCTL_KEEP_CTL;
+	ctl &= ~B43_MACCTL_KEEP_BADPLCP;
+	ctl &= ~B43_MACCTL_KEEP_BAD;
+	ctl &= ~B43_MACCTL_PROMISC;
+	ctl &= ~B43_MACCTL_BEACPROMISC;
+	ctl |= B43_MACCTL_INFRA;
+
+	if (b43_is_mode(wl, IEEE80211_IF_TYPE_AP))
+		ctl |= B43_MACCTL_AP;
+	else if (b43_is_mode(wl, IEEE80211_IF_TYPE_IBSS))
+		ctl &= ~B43_MACCTL_INFRA;
+
+	if (wl->filter_flags & FIF_CONTROL)
+		ctl |= B43_MACCTL_KEEP_CTL;
+	if (wl->filter_flags & FIF_FCSFAIL)
+		ctl |= B43_MACCTL_KEEP_BAD;
+	if (wl->filter_flags & FIF_PLCPFAIL)
+		ctl |= B43_MACCTL_KEEP_BADPLCP;
+	if (wl->filter_flags & FIF_PROMISC_IN_BSS)
+		ctl |= B43_MACCTL_PROMISC;
+	if (wl->filter_flags & FIF_BCN_PRBRESP_PROMISC)
+		ctl |= B43_MACCTL_BEACPROMISC;
+
+	/* Workaround: On old hardware the HW-MAC-address-filter
+	 * doesn't work properly, so always run promisc in filter
+	 * it in software. */
+	if (dev->dev->id.revision <= 4)
+		ctl |= B43_MACCTL_PROMISC;
+
+	b43_write32(dev, B43_MMIO_MACCTL, ctl);
+
+	cfp_pretbtt = 2;
+	if ((ctl & B43_MACCTL_INFRA) && !(ctl & B43_MACCTL_AP)) {
+		if (dev->dev->bus->chip_id == 0x4306 &&
+		    dev->dev->bus->chip_rev == 3)
+			cfp_pretbtt = 100;
+		else
+			cfp_pretbtt = 50;
+	}
+	b43_write16(dev, 0x612, cfp_pretbtt);
+}
+
+static void b43_rate_memory_write(struct b43_wldev *dev, u16 rate, int is_ofdm)
+{
+	u16 offset;
+
+	if (is_ofdm) {
+		offset = 0x480;
+		offset += (b43_plcp_get_ratecode_ofdm(rate) & 0x000F) * 2;
+	} else {
+		offset = 0x4C0;
+		offset += (b43_plcp_get_ratecode_cck(rate) & 0x000F) * 2;
+	}
+	b43_shm_write16(dev, B43_SHM_SHARED, offset + 0x20,
+			b43_shm_read16(dev, B43_SHM_SHARED, offset));
+}
+
+static void b43_rate_memory_init(struct b43_wldev *dev)
+{
+	switch (dev->phy.type) {
+	case B43_PHYTYPE_A:
+	case B43_PHYTYPE_G:
+		b43_rate_memory_write(dev, B43_OFDM_RATE_6MB, 1);
+		b43_rate_memory_write(dev, B43_OFDM_RATE_12MB, 1);
+		b43_rate_memory_write(dev, B43_OFDM_RATE_18MB, 1);
+		b43_rate_memory_write(dev, B43_OFDM_RATE_24MB, 1);
+		b43_rate_memory_write(dev, B43_OFDM_RATE_36MB, 1);
+		b43_rate_memory_write(dev, B43_OFDM_RATE_48MB, 1);
+		b43_rate_memory_write(dev, B43_OFDM_RATE_54MB, 1);
+		if (dev->phy.type == B43_PHYTYPE_A)
+			break;
+		/* fallthrough */
+	case B43_PHYTYPE_B:
+		b43_rate_memory_write(dev, B43_CCK_RATE_1MB, 0);
+		b43_rate_memory_write(dev, B43_CCK_RATE_2MB, 0);
+		b43_rate_memory_write(dev, B43_CCK_RATE_5MB, 0);
+		b43_rate_memory_write(dev, B43_CCK_RATE_11MB, 0);
+		break;
+	default:
+		B43_WARN_ON(1);
+	}
+}
+
+/* Set the TX-Antenna for management frames sent by firmware. */
+static void b43_mgmtframe_txantenna(struct b43_wldev *dev, int antenna)
+{
+	u16 ant = 0;
+	u16 tmp;
+
+	switch (antenna) {
+	case B43_ANTENNA0:
+		ant |= B43_TX4_PHY_ANT0;
+		break;
+	case B43_ANTENNA1:
+		ant |= B43_TX4_PHY_ANT1;
+		break;
+	case B43_ANTENNA_AUTO:
+		ant |= B43_TX4_PHY_ANTLAST;
+		break;
+	default:
+		B43_WARN_ON(1);
+	}
+
+	/* FIXME We also need to set the other flags of the PHY control field somewhere. */
+
+	/* For Beacons */
+	tmp = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_BEACPHYCTL);
+	tmp = (tmp & ~B43_TX4_PHY_ANT) | ant;
+	b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_BEACPHYCTL, tmp);
+	/* For ACK/CTS */
+	tmp = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_ACKCTSPHYCTL);
+	tmp = (tmp & ~B43_TX4_PHY_ANT) | ant;
+	b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_ACKCTSPHYCTL, tmp);
+	/* For Probe Resposes */
+	tmp = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_PRPHYCTL);
+	tmp = (tmp & ~B43_TX4_PHY_ANT) | ant;
+	b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_PRPHYCTL, tmp);
+}
+
+/* This is the opposite of b43_chip_init() */
+static void b43_chip_exit(struct b43_wldev *dev)
+{
+	b43_radio_turn_off(dev, 1);
+	b43_leds_exit(dev);
+	b43_gpio_cleanup(dev);
+	/* firmware is released later */
+}
+
+/* Initialize the chip
+ * http://bcm-specs.sipsolutions.net/ChipInit
+ */
+static int b43_chip_init(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+	int err, tmp;
+	u32 value32;
+	u16 value16;
+
+	b43_write32(dev, B43_MMIO_MACCTL,
+		    B43_MACCTL_PSM_JMP0 | B43_MACCTL_IHR_ENABLED);
+
+	err = b43_request_firmware(dev);
+	if (err)
+		goto out;
+	err = b43_upload_microcode(dev);
+	if (err)
+		goto out;	/* firmware is released later */
+
+	err = b43_gpio_init(dev);
+	if (err)
+		goto out;	/* firmware is released later */
+	b43_leds_init(dev);
+
+	err = b43_upload_initvals(dev);
+	if (err)
+		goto err_leds_exit;
+	b43_radio_turn_on(dev);
+
+	b43_write16(dev, 0x03E6, 0x0000);
+	err = b43_phy_init(dev);
+	if (err)
+		goto err_radio_off;
+
+	/* Select initial Interference Mitigation. */
+	tmp = phy->interfmode;
+	phy->interfmode = B43_INTERFMODE_NONE;
+	b43_radio_set_interference_mitigation(dev, tmp);
+
+	b43_set_rx_antenna(dev, B43_ANTENNA_DEFAULT);
+	b43_mgmtframe_txantenna(dev, B43_ANTENNA_DEFAULT);
+
+	if (phy->type == B43_PHYTYPE_B) {
+		value16 = b43_read16(dev, 0x005E);
+		value16 |= 0x0004;
+		b43_write16(dev, 0x005E, value16);
+	}
+	b43_write32(dev, 0x0100, 0x01000000);
+	if (dev->dev->id.revision < 5)
+		b43_write32(dev, 0x010C, 0x01000000);
+
+	b43_write32(dev, B43_MMIO_MACCTL, b43_read32(dev, B43_MMIO_MACCTL)
+		    & ~B43_MACCTL_INFRA);
+	b43_write32(dev, B43_MMIO_MACCTL, b43_read32(dev, B43_MMIO_MACCTL)
+		    | B43_MACCTL_INFRA);
+
+	if (b43_using_pio(dev)) {
+		b43_write32(dev, 0x0210, 0x00000100);
+		b43_write32(dev, 0x0230, 0x00000100);
+		b43_write32(dev, 0x0250, 0x00000100);
+		b43_write32(dev, 0x0270, 0x00000100);
+		b43_shm_write16(dev, B43_SHM_SHARED, 0x0034, 0x0000);
+	}
+
+	/* Probe Response Timeout value */
+	/* FIXME: Default to 0, has to be set by ioctl probably... :-/ */
+	b43_shm_write16(dev, B43_SHM_SHARED, 0x0074, 0x0000);
+
+	/* Initially set the wireless operation mode. */
+	b43_adjust_opmode(dev);
+
+	if (dev->dev->id.revision < 3) {
+		b43_write16(dev, 0x060E, 0x0000);
+		b43_write16(dev, 0x0610, 0x8000);
+		b43_write16(dev, 0x0604, 0x0000);
+		b43_write16(dev, 0x0606, 0x0200);
+	} else {
+		b43_write32(dev, 0x0188, 0x80000000);
+		b43_write32(dev, 0x018C, 0x02000000);
+	}
+	b43_write32(dev, B43_MMIO_GEN_IRQ_REASON, 0x00004000);
+	b43_write32(dev, B43_MMIO_DMA0_IRQ_MASK, 0x0001DC00);
+	b43_write32(dev, B43_MMIO_DMA1_IRQ_MASK, 0x0000DC00);
+	b43_write32(dev, B43_MMIO_DMA2_IRQ_MASK, 0x0000DC00);
+	b43_write32(dev, B43_MMIO_DMA3_IRQ_MASK, 0x0001DC00);
+	b43_write32(dev, B43_MMIO_DMA4_IRQ_MASK, 0x0000DC00);
+	b43_write32(dev, B43_MMIO_DMA5_IRQ_MASK, 0x0000DC00);
+
+	value32 = ssb_read32(dev->dev, SSB_TMSLOW);
+	value32 |= 0x00100000;
+	ssb_write32(dev->dev, SSB_TMSLOW, value32);
+
+	b43_write16(dev, B43_MMIO_POWERUP_DELAY,
+		    dev->dev->bus->chipco.fast_pwrup_delay);
+
+	err = 0;
+	b43dbg(dev->wl, "Chip initialized\n");
+out:
+	return err;
+
+err_radio_off:
+	b43_radio_turn_off(dev, 1);
+err_leds_exit:
+	b43_leds_exit(dev);
+	b43_gpio_cleanup(dev);
+	return err;
+}
+
+static void b43_periodic_every120sec(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+
+	if (phy->type != B43_PHYTYPE_G || phy->rev < 2)
+		return;
+
+	b43_mac_suspend(dev);
+	b43_lo_g_measure(dev);
+	b43_mac_enable(dev);
+	if (b43_has_hardware_pctl(phy))
+		b43_lo_g_ctl_mark_all_unused(dev);
+}
+
+static void b43_periodic_every60sec(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+
+	if (!b43_has_hardware_pctl(phy))
+		b43_lo_g_ctl_mark_all_unused(dev);
+	if (dev->dev->bus->sprom.r1.boardflags_lo & B43_BFL_RSSI) {
+		b43_mac_suspend(dev);
+		b43_calc_nrssi_slope(dev);
+		if ((phy->radio_ver == 0x2050) && (phy->radio_rev == 8)) {
+			u8 old_chan = phy->channel;
+
+			/* VCO Calibration */
+			if (old_chan >= 8)
+				b43_radio_selectchannel(dev, 1, 0);
+			else
+				b43_radio_selectchannel(dev, 13, 0);
+			b43_radio_selectchannel(dev, old_chan, 0);
+		}
+		b43_mac_enable(dev);
+	}
+}
+
+static void b43_periodic_every30sec(struct b43_wldev *dev)
+{
+	/* Update device statistics. */
+	b43_calculate_link_quality(dev);
+}
+
+static void b43_periodic_every15sec(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+
+	if (phy->type == B43_PHYTYPE_G) {
+		//TODO: update_aci_moving_average
+		if (phy->aci_enable && phy->aci_wlan_automatic) {
+			b43_mac_suspend(dev);
+			if (!phy->aci_enable && 1 /*TODO: not scanning? */ ) {
+				if (0 /*TODO: bunch of conditions */ ) {
+					b43_radio_set_interference_mitigation
+					    (dev, B43_INTERFMODE_MANUALWLAN);
+				}
+			} else if (1 /*TODO*/) {
+				/*
+				   if ((aci_average > 1000) && !(b43_radio_aci_scan(dev))) {
+				   b43_radio_set_interference_mitigation(dev,
+				   B43_INTERFMODE_NONE);
+				   }
+				 */
+			}
+			b43_mac_enable(dev);
+		} else if (phy->interfmode == B43_INTERFMODE_NONWLAN &&
+			   phy->rev == 1) {
+			//TODO: implement rev1 workaround
+		}
+	}
+	b43_phy_xmitpower(dev);	//FIXME: unless scanning?
+	//TODO for APHY (temperature?)
+}
+
+static void do_periodic_work(struct b43_wldev *dev)
+{
+	unsigned int state;
+
+	state = dev->periodic_state;
+	if (state % 8 == 0)
+		b43_periodic_every120sec(dev);
+	if (state % 4 == 0)
+		b43_periodic_every60sec(dev);
+	if (state % 2 == 0)
+		b43_periodic_every30sec(dev);
+	b43_periodic_every15sec(dev);
+}
+
+/* Periodic work locking policy:
+ * 	The whole periodic work handler is protected by
+ * 	wl->mutex. If another lock is needed somewhere in the
+ * 	pwork callchain, it's aquired in-place, where it's needed.
+ */
+static void b43_periodic_work_handler(struct work_struct *work)
+{
+	struct b43_wldev *dev = container_of(work, struct b43_wldev,
+					     periodic_work.work);
+	struct b43_wl *wl = dev->wl;
+	unsigned long delay;
+
+	mutex_lock(&wl->mutex);
+
+	if (unlikely(b43_status(dev) != B43_STAT_STARTED))
+		goto out;
+	if (b43_debug(dev, B43_DBG_PWORK_STOP))
+		goto out_requeue;
+
+	do_periodic_work(dev);
+
+	dev->periodic_state++;
+out_requeue:
+	if (b43_debug(dev, B43_DBG_PWORK_FAST))
+		delay = msecs_to_jiffies(50);
+	else
+		delay = round_jiffies(HZ * 15);
+	queue_delayed_work(wl->hw->workqueue, &dev->periodic_work, delay);
+out:
+	mutex_unlock(&wl->mutex);
+}
+
+static void b43_periodic_tasks_setup(struct b43_wldev *dev)
+{
+	struct delayed_work *work = &dev->periodic_work;
+
+	dev->periodic_state = 0;
+	INIT_DELAYED_WORK(work, b43_periodic_work_handler);
+	queue_delayed_work(dev->wl->hw->workqueue, work, 0);
+}
+
+/* Validate access to the chip (SHM) */
+static int b43_validate_chipaccess(struct b43_wldev *dev)
+{
+	u32 value;
+	u32 shm_backup;
+
+	shm_backup = b43_shm_read32(dev, B43_SHM_SHARED, 0);
+	b43_shm_write32(dev, B43_SHM_SHARED, 0, 0xAA5555AA);
+	if (b43_shm_read32(dev, B43_SHM_SHARED, 0) != 0xAA5555AA)
+		goto error;
+	b43_shm_write32(dev, B43_SHM_SHARED, 0, 0x55AAAA55);
+	if (b43_shm_read32(dev, B43_SHM_SHARED, 0) != 0x55AAAA55)
+		goto error;
+	b43_shm_write32(dev, B43_SHM_SHARED, 0, shm_backup);
+
+	value = b43_read32(dev, B43_MMIO_MACCTL);
+	if ((value | B43_MACCTL_GMODE) !=
+	    (B43_MACCTL_GMODE | B43_MACCTL_IHR_ENABLED))
+		goto error;
+
+	value = b43_read32(dev, B43_MMIO_GEN_IRQ_REASON);
+	if (value)
+		goto error;
+
+	return 0;
+      error:
+	b43err(dev->wl, "Failed to validate the chipaccess\n");
+	return -ENODEV;
+}
+
+static void b43_security_init(struct b43_wldev *dev)
+{
+	dev->max_nr_keys = (dev->dev->id.revision >= 5) ? 58 : 20;
+	B43_WARN_ON(dev->max_nr_keys > ARRAY_SIZE(dev->key));
+	dev->ktp = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_KTP);
+	/* KTP is a word address, but we address SHM bytewise.
+	 * So multiply by two.
+	 */
+	dev->ktp *= 2;
+	if (dev->dev->id.revision >= 5) {
+		/* Number of RCMTA address slots */
+		b43_write16(dev, B43_MMIO_RCMTA_COUNT, dev->max_nr_keys - 8);
+	}
+	b43_clear_keys(dev);
+}
+
+static int b43_rng_read(struct hwrng *rng, u32 * data)
+{
+	struct b43_wl *wl = (struct b43_wl *)rng->priv;
+	unsigned long flags;
+
+	/* Don't take wl->mutex here, as it could deadlock with
+	 * hwrng internal locking. It's not needed to take
+	 * wl->mutex here, anyway. */
+
+	spin_lock_irqsave(&wl->irq_lock, flags);
+	*data = b43_read16(wl->current_dev, B43_MMIO_RNG);
+	spin_unlock_irqrestore(&wl->irq_lock, flags);
+
+	return (sizeof(u16));
+}
+
+static void b43_rng_exit(struct b43_wl *wl)
+{
+	if (wl->rng_initialized)
+		hwrng_unregister(&wl->rng);
+}
+
+static int b43_rng_init(struct b43_wl *wl)
+{
+	int err;
+
+	snprintf(wl->rng_name, ARRAY_SIZE(wl->rng_name),
+		 "%s_%s", KBUILD_MODNAME, wiphy_name(wl->hw->wiphy));
+	wl->rng.name = wl->rng_name;
+	wl->rng.data_read = b43_rng_read;
+	wl->rng.priv = (unsigned long)wl;
+	wl->rng_initialized = 1;
+	err = hwrng_register(&wl->rng);
+	if (err) {
+		wl->rng_initialized = 0;
+		b43err(wl, "Failed to register the random "
+		       "number generator (%d)\n", err);
+	}
+
+	return err;
+}
+
+static int b43_tx(struct ieee80211_hw *hw,
+		  struct sk_buff *skb, struct ieee80211_tx_control *ctl)
+{
+	struct b43_wl *wl = hw_to_b43_wl(hw);
+	struct b43_wldev *dev = wl->current_dev;
+	int err = -ENODEV;
+	unsigned long flags;
+
+	if (unlikely(!dev))
+		goto out;
+	if (unlikely(b43_status(dev) < B43_STAT_STARTED))
+		goto out;
+	/* DMA-TX is done without a global lock. */
+	if (b43_using_pio(dev)) {
+		spin_lock_irqsave(&wl->irq_lock, flags);
+		err = b43_pio_tx(dev, skb, ctl);
+		spin_unlock_irqrestore(&wl->irq_lock, flags);
+	} else
+		err = b43_dma_tx(dev, skb, ctl);
+      out:
+	if (unlikely(err))
+		return NETDEV_TX_BUSY;
+	return NETDEV_TX_OK;
+}
+
+static int b43_conf_tx(struct ieee80211_hw *hw,
+		       int queue,
+		       const struct ieee80211_tx_queue_params *params)
+{
+	return 0;
+}
+
+static int b43_get_tx_stats(struct ieee80211_hw *hw,
+			    struct ieee80211_tx_queue_stats *stats)
+{
+	struct b43_wl *wl = hw_to_b43_wl(hw);
+	struct b43_wldev *dev = wl->current_dev;
+	unsigned long flags;
+	int err = -ENODEV;
+
+	if (!dev)
+		goto out;
+	spin_lock_irqsave(&wl->irq_lock, flags);
+	if (likely(b43_status(dev) >= B43_STAT_STARTED)) {
+		if (b43_using_pio(dev))
+			b43_pio_get_tx_stats(dev, stats);
+		else
+			b43_dma_get_tx_stats(dev, stats);
+		err = 0;
+	}
+	spin_unlock_irqrestore(&wl->irq_lock, flags);
+      out:
+	return err;
+}
+
+static int b43_get_stats(struct ieee80211_hw *hw,
+			 struct ieee80211_low_level_stats *stats)
+{
+	struct b43_wl *wl = hw_to_b43_wl(hw);
+	unsigned long flags;
+
+	spin_lock_irqsave(&wl->irq_lock, flags);
+	memcpy(stats, &wl->ieee_stats, sizeof(*stats));
+	spin_unlock_irqrestore(&wl->irq_lock, flags);
+
+	return 0;
+}
+
+static const char *phymode_to_string(unsigned int phymode)
+{
+	switch (phymode) {
+	case B43_PHYMODE_A:
+		return "A";
+	case B43_PHYMODE_B:
+		return "B";
+	case B43_PHYMODE_G:
+		return "G";
+	default:
+		B43_WARN_ON(1);
+	}
+	return "";
+}
+
+static int find_wldev_for_phymode(struct b43_wl *wl,
+				  unsigned int phymode,
+				  struct b43_wldev **dev, bool * gmode)
+{
+	struct b43_wldev *d;
+
+	list_for_each_entry(d, &wl->devlist, list) {
+		if (d->phy.possible_phymodes & phymode) {
+			/* Ok, this device supports the PHY-mode.
+			 * Now figure out how the gmode bit has to be
+			 * set to support it. */
+			if (phymode == B43_PHYMODE_A)
+				*gmode = 0;
+			else
+				*gmode = 1;
+			*dev = d;
+
+			return 0;
+		}
+	}
+
+	return -ESRCH;
+}
+
+static void b43_put_phy_into_reset(struct b43_wldev *dev)
+{
+	struct ssb_device *sdev = dev->dev;
+	u32 tmslow;
+
+	tmslow = ssb_read32(sdev, SSB_TMSLOW);
+	tmslow &= ~B43_TMSLOW_GMODE;
+	tmslow |= B43_TMSLOW_PHYRESET;
+	tmslow |= SSB_TMSLOW_FGC;
+	ssb_write32(sdev, SSB_TMSLOW, tmslow);
+	msleep(1);
+
+	tmslow = ssb_read32(sdev, SSB_TMSLOW);
+	tmslow &= ~SSB_TMSLOW_FGC;
+	tmslow |= B43_TMSLOW_PHYRESET;
+	ssb_write32(sdev, SSB_TMSLOW, tmslow);
+	msleep(1);
+}
+
+/* Expects wl->mutex locked */
+static int b43_switch_phymode(struct b43_wl *wl, unsigned int new_mode)
+{
+	struct b43_wldev *up_dev;
+	struct b43_wldev *down_dev;
+	int err;
+	bool gmode = 0;
+	int prev_status;
+
+	err = find_wldev_for_phymode(wl, new_mode, &up_dev, &gmode);
+	if (err) {
+		b43err(wl, "Could not find a device for %s-PHY mode\n",
+		       phymode_to_string(new_mode));
+		return err;
+	}
+	if ((up_dev == wl->current_dev) &&
+	    (!!wl->current_dev->phy.gmode == !!gmode)) {
+		/* This device is already running. */
+		return 0;
+	}
+	b43dbg(wl, "Reconfiguring PHYmode to %s-PHY\n",
+	       phymode_to_string(new_mode));
+	down_dev = wl->current_dev;
+
+	prev_status = b43_status(down_dev);
+	/* Shutdown the currently running core. */
+	if (prev_status >= B43_STAT_STARTED)
+		b43_wireless_core_stop(down_dev);
+	if (prev_status >= B43_STAT_INITIALIZED)
+		b43_wireless_core_exit(down_dev);
+
+	if (down_dev != up_dev) {
+		/* We switch to a different core, so we put PHY into
+		 * RESET on the old core. */
+		b43_put_phy_into_reset(down_dev);
+	}
+
+	/* Now start the new core. */
+	up_dev->phy.gmode = gmode;
+	if (prev_status >= B43_STAT_INITIALIZED) {
+		err = b43_wireless_core_init(up_dev);
+		if (err) {
+			b43err(wl, "Fatal: Could not initialize device for "
+			       "newly selected %s-PHY mode\n",
+			       phymode_to_string(new_mode));
+			goto init_failure;
+		}
+	}
+	if (prev_status >= B43_STAT_STARTED) {
+		err = b43_wireless_core_start(up_dev);
+		if (err) {
+			b43err(wl, "Fatal: Coult not start device for "
+			       "newly selected %s-PHY mode\n",
+			       phymode_to_string(new_mode));
+			b43_wireless_core_exit(up_dev);
+			goto init_failure;
+		}
+	}
+	B43_WARN_ON(b43_status(up_dev) != prev_status);
+
+	wl->current_dev = up_dev;
+
+	return 0;
+      init_failure:
+	/* Whoops, failed to init the new core. No core is operating now. */
+	wl->current_dev = NULL;
+	return err;
+}
+
+static int b43_antenna_from_ieee80211(u8 antenna)
+{
+	switch (antenna) {
+	case 0:		/* default/diversity */
+		return B43_ANTENNA_DEFAULT;
+	case 1:		/* Antenna 0 */
+		return B43_ANTENNA0;
+	case 2:		/* Antenna 1 */
+		return B43_ANTENNA1;
+	default:
+		return B43_ANTENNA_DEFAULT;
+	}
+}
+
+static int b43_dev_config(struct ieee80211_hw *hw, struct ieee80211_conf *conf)
+{
+	struct b43_wl *wl = hw_to_b43_wl(hw);
+	struct b43_wldev *dev;
+	struct b43_phy *phy;
+	unsigned long flags;
+	unsigned int new_phymode = 0xFFFF;
+	int antenna_tx;
+	int antenna_rx;
+	int err = 0;
+	u32 savedirqs;
+
+	antenna_tx = b43_antenna_from_ieee80211(conf->antenna_sel_tx);
+	antenna_rx = b43_antenna_from_ieee80211(conf->antenna_sel_rx);
+
+	mutex_lock(&wl->mutex);
+
+	/* Switch the PHY mode (if necessary). */
+	switch (conf->phymode) {
+	case MODE_IEEE80211A:
+		new_phymode = B43_PHYMODE_A;
+		break;
+	case MODE_IEEE80211B:
+		new_phymode = B43_PHYMODE_B;
+		break;
+	case MODE_IEEE80211G:
+		new_phymode = B43_PHYMODE_G;
+		break;
+	default:
+		B43_WARN_ON(1);
+	}
+	err = b43_switch_phymode(wl, new_phymode);
+	if (err)
+		goto out_unlock_mutex;
+	dev = wl->current_dev;
+	phy = &dev->phy;
+
+	/* Disable IRQs while reconfiguring the device.
+	 * This makes it possible to drop the spinlock throughout
+	 * the reconfiguration process. */
+	spin_lock_irqsave(&wl->irq_lock, flags);
+	if (b43_status(dev) < B43_STAT_STARTED) {
+		spin_unlock_irqrestore(&wl->irq_lock, flags);
+		goto out_unlock_mutex;
+	}
+	savedirqs = b43_interrupt_disable(dev, B43_IRQ_ALL);
+	spin_unlock_irqrestore(&wl->irq_lock, flags);
+	b43_synchronize_irq(dev);
+
+	/* Switch to the requested channel.
+	 * The firmware takes care of races with the TX handler. */
+	if (conf->channel_val != phy->channel)
+		b43_radio_selectchannel(dev, conf->channel_val, 0);
+
+	/* Enable/Disable ShortSlot timing. */
+	if ((!!(conf->flags & IEEE80211_CONF_SHORT_SLOT_TIME)) !=
+	    dev->short_slot) {
+		B43_WARN_ON(phy->type != B43_PHYTYPE_G);
+		if (conf->flags & IEEE80211_CONF_SHORT_SLOT_TIME)
+			b43_short_slot_timing_enable(dev);
+		else
+			b43_short_slot_timing_disable(dev);
+	}
+
+	/* Adjust the desired TX power level. */
+	if (conf->power_level != 0) {
+		if (conf->power_level != phy->power_level) {
+			phy->power_level = conf->power_level;
+			b43_phy_xmitpower(dev);
+		}
+	}
+
+	/* Antennas for RX and management frame TX. */
+	b43_mgmtframe_txantenna(dev, antenna_tx);
+	b43_set_rx_antenna(dev, antenna_rx);
+
+	/* Update templates for AP mode. */
+	if (b43_is_mode(wl, IEEE80211_IF_TYPE_AP))
+		b43_set_beacon_int(dev, conf->beacon_int);
+
+	if (!!conf->radio_enabled != phy->radio_on) {
+		if (conf->radio_enabled) {
+			b43_radio_turn_on(dev);
+			b43info(dev->wl, "Radio turned on by software\n");
+			if (!dev->radio_hw_enable) {
+				b43info(dev->wl, "The hardware RF-kill button "
+					"still turns the radio physically off. "
+					"Press the button to turn it on.\n");
+			}
+		} else {
+			b43_radio_turn_off(dev, 0);
+			b43info(dev->wl, "Radio turned off by software\n");
+		}
+	}
+
+	spin_lock_irqsave(&wl->irq_lock, flags);
+	b43_interrupt_enable(dev, savedirqs);
+	mmiowb();
+	spin_unlock_irqrestore(&wl->irq_lock, flags);
+      out_unlock_mutex:
+	mutex_unlock(&wl->mutex);
+
+	return err;
+}
+
+static int b43_dev_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
+			   const u8 *local_addr, const u8 *addr,
+			   struct ieee80211_key_conf *key)
+{
+	struct b43_wl *wl = hw_to_b43_wl(hw);
+	struct b43_wldev *dev = wl->current_dev;
+	unsigned long flags;
+	u8 algorithm;
+	u8 index;
+	int err = -EINVAL;
+	DECLARE_MAC_BUF(mac);
+
+	if (modparam_nohwcrypt)
+		return -ENOSPC; /* User disabled HW-crypto */
+
+	if (!dev)
+		return -ENODEV;
+	switch (key->alg) {
+	case ALG_WEP:
+		if (key->keylen == 5)
+			algorithm = B43_SEC_ALGO_WEP40;
+		else
+			algorithm = B43_SEC_ALGO_WEP104;
+		break;
+	case ALG_TKIP:
+		algorithm = B43_SEC_ALGO_TKIP;
+		break;
+	case ALG_CCMP:
+		algorithm = B43_SEC_ALGO_AES;
+		break;
+	default:
+		B43_WARN_ON(1);
+		goto out;
+	}
+
+	index = (u8) (key->keyidx);
+	if (index > 3)
+		goto out;
+
+	mutex_lock(&wl->mutex);
+	spin_lock_irqsave(&wl->irq_lock, flags);
+
+	if (b43_status(dev) < B43_STAT_INITIALIZED) {
+		err = -ENODEV;
+		goto out_unlock;
+	}
+
+	switch (cmd) {
+	case SET_KEY:
+		if (algorithm == B43_SEC_ALGO_TKIP) {
+			/* FIXME: No TKIP hardware encryption for now. */
+			err = -EOPNOTSUPP;
+			goto out_unlock;
+		}
+
+		if (is_broadcast_ether_addr(addr)) {
+			/* addr is FF:FF:FF:FF:FF:FF for default keys */
+			err = b43_key_write(dev, index, algorithm,
+					    key->key, key->keylen, NULL, key);
+		} else {
+			/*
+			 * either pairwise key or address is 00:00:00:00:00:00
+			 * for transmit-only keys
+			 */
+			err = b43_key_write(dev, -1, algorithm,
+					    key->key, key->keylen, addr, key);
+		}
+		if (err)
+			goto out_unlock;
+
+		if (algorithm == B43_SEC_ALGO_WEP40 ||
+		    algorithm == B43_SEC_ALGO_WEP104) {
+			b43_hf_write(dev, b43_hf_read(dev) | B43_HF_USEDEFKEYS);
+		} else {
+			b43_hf_write(dev,
+				     b43_hf_read(dev) & ~B43_HF_USEDEFKEYS);
+		}
+		key->flags |= IEEE80211_KEY_FLAG_GENERATE_IV;
+		break;
+	case DISABLE_KEY: {
+		err = b43_key_clear(dev, key->hw_key_idx);
+		if (err)
+			goto out_unlock;
+		break;
+	}
+	default:
+		B43_WARN_ON(1);
+	}
+out_unlock:
+	spin_unlock_irqrestore(&wl->irq_lock, flags);
+	mutex_unlock(&wl->mutex);
+out:
+	if (!err) {
+		b43dbg(wl, "%s hardware based encryption for keyidx: %d, "
+		       "mac: %s\n",
+		       cmd == SET_KEY ? "Using" : "Disabling", key->keyidx,
+		       print_mac(mac, addr));
+	}
+	return err;
+}
+
+static void b43_configure_filter(struct ieee80211_hw *hw,
+				 unsigned int changed, unsigned int *fflags,
+				 int mc_count, struct dev_addr_list *mc_list)
+{
+	struct b43_wl *wl = hw_to_b43_wl(hw);
+	struct b43_wldev *dev = wl->current_dev;
+	unsigned long flags;
+
+	if (!dev) {
+		*fflags = 0;
+		return;
+	}
+
+	spin_lock_irqsave(&wl->irq_lock, flags);
+	*fflags &= FIF_PROMISC_IN_BSS |
+		  FIF_ALLMULTI |
+		  FIF_FCSFAIL |
+		  FIF_PLCPFAIL |
+		  FIF_CONTROL |
+		  FIF_OTHER_BSS |
+		  FIF_BCN_PRBRESP_PROMISC;
+
+	changed &= FIF_PROMISC_IN_BSS |
+		   FIF_ALLMULTI |
+		   FIF_FCSFAIL |
+		   FIF_PLCPFAIL |
+		   FIF_CONTROL |
+		   FIF_OTHER_BSS |
+		   FIF_BCN_PRBRESP_PROMISC;
+
+	wl->filter_flags = *fflags;
+
+	if (changed && b43_status(dev) >= B43_STAT_INITIALIZED)
+		b43_adjust_opmode(dev);
+	spin_unlock_irqrestore(&wl->irq_lock, flags);
+}
+
+static int b43_config_interface(struct ieee80211_hw *hw,
+				int if_id, struct ieee80211_if_conf *conf)
+{
+	struct b43_wl *wl = hw_to_b43_wl(hw);
+	struct b43_wldev *dev = wl->current_dev;
+	unsigned long flags;
+
+	if (!dev)
+		return -ENODEV;
+	mutex_lock(&wl->mutex);
+	spin_lock_irqsave(&wl->irq_lock, flags);
+	B43_WARN_ON(wl->if_id != if_id);
+	if (conf->bssid)
+		memcpy(wl->bssid, conf->bssid, ETH_ALEN);
+	else
+		memset(wl->bssid, 0, ETH_ALEN);
+	if (b43_status(dev) >= B43_STAT_INITIALIZED) {
+		if (b43_is_mode(wl, IEEE80211_IF_TYPE_AP)) {
+			B43_WARN_ON(conf->type != IEEE80211_IF_TYPE_AP);
+			b43_set_ssid(dev, conf->ssid, conf->ssid_len);
+			if (conf->beacon)
+				b43_refresh_templates(dev, conf->beacon);
+		}
+		b43_write_mac_bssid_templates(dev);
+	}
+	spin_unlock_irqrestore(&wl->irq_lock, flags);
+	mutex_unlock(&wl->mutex);
+
+	return 0;
+}
+
+/* Locking: wl->mutex */
+static void b43_wireless_core_stop(struct b43_wldev *dev)
+{
+	struct b43_wl *wl = dev->wl;
+	unsigned long flags;
+
+	if (b43_status(dev) < B43_STAT_STARTED)
+		return;
+	b43_set_status(dev, B43_STAT_INITIALIZED);
+
+	mutex_unlock(&wl->mutex);
+	/* Must unlock as it would otherwise deadlock. No races here.
+	 * Cancel the possibly running self-rearming periodic work. */
+	cancel_delayed_work_sync(&dev->periodic_work);
+	mutex_lock(&wl->mutex);
+
+	ieee80211_stop_queues(wl->hw);	//FIXME this could cause a deadlock, as mac80211 seems buggy.
+
+	/* Disable and sync interrupts. */
+	spin_lock_irqsave(&wl->irq_lock, flags);
+	dev->irq_savedstate = b43_interrupt_disable(dev, B43_IRQ_ALL);
+	b43_read32(dev, B43_MMIO_GEN_IRQ_MASK);	/* flush */
+	spin_unlock_irqrestore(&wl->irq_lock, flags);
+	b43_synchronize_irq(dev);
+
+	b43_mac_suspend(dev);
+	free_irq(dev->dev->irq, dev);
+	b43dbg(wl, "Wireless interface stopped\n");
+}
+
+/* Locking: wl->mutex */
+static int b43_wireless_core_start(struct b43_wldev *dev)
+{
+	int err;
+
+	B43_WARN_ON(b43_status(dev) != B43_STAT_INITIALIZED);
+
+	drain_txstatus_queue(dev);
+	err = request_irq(dev->dev->irq, b43_interrupt_handler,
+			  IRQF_SHARED, KBUILD_MODNAME, dev);
+	if (err) {
+		b43err(dev->wl, "Cannot request IRQ-%d\n", dev->dev->irq);
+		goto out;
+	}
+
+	/* We are ready to run. */
+	b43_set_status(dev, B43_STAT_STARTED);
+
+	/* Start data flow (TX/RX). */
+	b43_mac_enable(dev);
+	b43_interrupt_enable(dev, dev->irq_savedstate);
+	ieee80211_start_queues(dev->wl->hw);
+
+	/* Start maintainance work */
+	b43_periodic_tasks_setup(dev);
+
+	b43dbg(dev->wl, "Wireless interface started\n");
+      out:
+	return err;
+}
+
+/* Get PHY and RADIO versioning numbers */
+static int b43_phy_versioning(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+	u32 tmp;
+	u8 analog_type;
+	u8 phy_type;
+	u8 phy_rev;
+	u16 radio_manuf;
+	u16 radio_ver;
+	u16 radio_rev;
+	int unsupported = 0;
+
+	/* Get PHY versioning */
+	tmp = b43_read16(dev, B43_MMIO_PHY_VER);
+	analog_type = (tmp & B43_PHYVER_ANALOG) >> B43_PHYVER_ANALOG_SHIFT;
+	phy_type = (tmp & B43_PHYVER_TYPE) >> B43_PHYVER_TYPE_SHIFT;
+	phy_rev = (tmp & B43_PHYVER_VERSION);
+	switch (phy_type) {
+	case B43_PHYTYPE_A:
+		if (phy_rev >= 4)
+			unsupported = 1;
+		break;
+	case B43_PHYTYPE_B:
+		if (phy_rev != 2 && phy_rev != 4 && phy_rev != 6
+		    && phy_rev != 7)
+			unsupported = 1;
+		break;
+	case B43_PHYTYPE_G:
+		if (phy_rev > 8)
+			unsupported = 1;
+		break;
+	default:
+		unsupported = 1;
+	};
+	if (unsupported) {
+		b43err(dev->wl, "FOUND UNSUPPORTED PHY "
+		       "(Analog %u, Type %u, Revision %u)\n",
+		       analog_type, phy_type, phy_rev);
+		return -EOPNOTSUPP;
+	}
+	b43dbg(dev->wl, "Found PHY: Analog %u, Type %u, Revision %u\n",
+	       analog_type, phy_type, phy_rev);
+
+	/* Get RADIO versioning */
+	if (dev->dev->bus->chip_id == 0x4317) {
+		if (dev->dev->bus->chip_rev == 0)
+			tmp = 0x3205017F;
+		else if (dev->dev->bus->chip_rev == 1)
+			tmp = 0x4205017F;
+		else
+			tmp = 0x5205017F;
+	} else {
+		b43_write16(dev, B43_MMIO_RADIO_CONTROL, B43_RADIOCTL_ID);
+		tmp = b43_read16(dev, B43_MMIO_RADIO_DATA_HIGH);
+		tmp <<= 16;
+		b43_write16(dev, B43_MMIO_RADIO_CONTROL, B43_RADIOCTL_ID);
+		tmp |= b43_read16(dev, B43_MMIO_RADIO_DATA_LOW);
+	}
+	radio_manuf = (tmp & 0x00000FFF);
+	radio_ver = (tmp & 0x0FFFF000) >> 12;
+	radio_rev = (tmp & 0xF0000000) >> 28;
+	switch (phy_type) {
+	case B43_PHYTYPE_A:
+		if (radio_ver != 0x2060)
+			unsupported = 1;
+		if (radio_rev != 1)
+			unsupported = 1;
+		if (radio_manuf != 0x17F)
+			unsupported = 1;
+		break;
+	case B43_PHYTYPE_B:
+		if ((radio_ver & 0xFFF0) != 0x2050)
+			unsupported = 1;
+		break;
+	case B43_PHYTYPE_G:
+		if (radio_ver != 0x2050)
+			unsupported = 1;
+		break;
+	default:
+		B43_WARN_ON(1);
+	}
+	if (unsupported) {
+		b43err(dev->wl, "FOUND UNSUPPORTED RADIO "
+		       "(Manuf 0x%X, Version 0x%X, Revision %u)\n",
+		       radio_manuf, radio_ver, radio_rev);
+		return -EOPNOTSUPP;
+	}
+	b43dbg(dev->wl, "Found Radio: Manuf 0x%X, Version 0x%X, Revision %u\n",
+	       radio_manuf, radio_ver, radio_rev);
+
+	phy->radio_manuf = radio_manuf;
+	phy->radio_ver = radio_ver;
+	phy->radio_rev = radio_rev;
+
+	phy->analog = analog_type;
+	phy->type = phy_type;
+	phy->rev = phy_rev;
+
+	return 0;
+}
+
+static void setup_struct_phy_for_init(struct b43_wldev *dev,
+				      struct b43_phy *phy)
+{
+	struct b43_txpower_lo_control *lo;
+	int i;
+
+	memset(phy->minlowsig, 0xFF, sizeof(phy->minlowsig));
+	memset(phy->minlowsigpos, 0, sizeof(phy->minlowsigpos));
+
+	/* Flags */
+	phy->locked = 0;
+
+	phy->aci_enable = 0;
+	phy->aci_wlan_automatic = 0;
+	phy->aci_hw_rssi = 0;
+
+	phy->radio_off_context.valid = 0;
+
+	lo = phy->lo_control;
+	if (lo) {
+		memset(lo, 0, sizeof(*(phy->lo_control)));
+		lo->rebuild = 1;
+		lo->tx_bias = 0xFF;
+	}
+	phy->max_lb_gain = 0;
+	phy->trsw_rx_gain = 0;
+	phy->txpwr_offset = 0;
+
+	/* NRSSI */
+	phy->nrssislope = 0;
+	for (i = 0; i < ARRAY_SIZE(phy->nrssi); i++)
+		phy->nrssi[i] = -1000;
+	for (i = 0; i < ARRAY_SIZE(phy->nrssi_lt); i++)
+		phy->nrssi_lt[i] = i;
+
+	phy->lofcal = 0xFFFF;
+	phy->initval = 0xFFFF;
+
+	spin_lock_init(&phy->lock);
+	phy->interfmode = B43_INTERFMODE_NONE;
+	phy->channel = 0xFF;
+
+	phy->hardware_power_control = !!modparam_hwpctl;
+}
+
+static void setup_struct_wldev_for_init(struct b43_wldev *dev)
+{
+	/* Flags */
+	dev->reg124_set_0x4 = 0;
+	/* Assume the radio is enabled. If it's not enabled, the state will
+	 * immediately get fixed on the first periodic work run. */
+	dev->radio_hw_enable = 1;
+
+	/* Stats */
+	memset(&dev->stats, 0, sizeof(dev->stats));
+
+	setup_struct_phy_for_init(dev, &dev->phy);
+
+	/* IRQ related flags */
+	dev->irq_reason = 0;
+	memset(dev->dma_reason, 0, sizeof(dev->dma_reason));
+	dev->irq_savedstate = B43_IRQ_MASKTEMPLATE;
+
+	dev->mac_suspended = 1;
+
+	/* Noise calculation context */
+	memset(&dev->noisecalc, 0, sizeof(dev->noisecalc));
+}
+
+static void b43_bluetooth_coext_enable(struct b43_wldev *dev)
+{
+	struct ssb_sprom *sprom = &dev->dev->bus->sprom;
+	u32 hf;
+
+	if (!(sprom->r1.boardflags_lo & B43_BFL_BTCOEXIST))
+		return;
+	if (dev->phy.type != B43_PHYTYPE_B && !dev->phy.gmode)
+		return;
+
+	hf = b43_hf_read(dev);
+	if (sprom->r1.boardflags_lo & B43_BFL_BTCMOD)
+		hf |= B43_HF_BTCOEXALT;
+	else
+		hf |= B43_HF_BTCOEX;
+	b43_hf_write(dev, hf);
+	//TODO
+}
+
+static void b43_bluetooth_coext_disable(struct b43_wldev *dev)
+{				//TODO
+}
+
+static void b43_imcfglo_timeouts_workaround(struct b43_wldev *dev)
+{
+#ifdef CONFIG_SSB_DRIVER_PCICORE
+	struct ssb_bus *bus = dev->dev->bus;
+	u32 tmp;
+
+	if (bus->pcicore.dev &&
+	    bus->pcicore.dev->id.coreid == SSB_DEV_PCI &&
+	    bus->pcicore.dev->id.revision <= 5) {
+		/* IMCFGLO timeouts workaround. */
+		tmp = ssb_read32(dev->dev, SSB_IMCFGLO);
+		tmp &= ~SSB_IMCFGLO_REQTO;
+		tmp &= ~SSB_IMCFGLO_SERTO;
+		switch (bus->bustype) {
+		case SSB_BUSTYPE_PCI:
+		case SSB_BUSTYPE_PCMCIA:
+			tmp |= 0x32;
+			break;
+		case SSB_BUSTYPE_SSB:
+			tmp |= 0x53;
+			break;
+		}
+		ssb_write32(dev->dev, SSB_IMCFGLO, tmp);
+	}
+#endif /* CONFIG_SSB_DRIVER_PCICORE */
+}
+
+/* Shutdown a wireless core */
+/* Locking: wl->mutex */
+static void b43_wireless_core_exit(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+
+	B43_WARN_ON(b43_status(dev) > B43_STAT_INITIALIZED);
+	if (b43_status(dev) != B43_STAT_INITIALIZED)
+		return;
+	b43_set_status(dev, B43_STAT_UNINIT);
+
+	mutex_unlock(&dev->wl->mutex);
+	b43_rfkill_exit(dev);
+	mutex_lock(&dev->wl->mutex);
+
+	b43_rng_exit(dev->wl);
+	b43_pio_free(dev);
+	b43_dma_free(dev);
+	b43_chip_exit(dev);
+	b43_radio_turn_off(dev, 1);
+	b43_switch_analog(dev, 0);
+	if (phy->dyn_tssi_tbl)
+		kfree(phy->tssi2dbm);
+	kfree(phy->lo_control);
+	phy->lo_control = NULL;
+	ssb_device_disable(dev->dev, 0);
+	ssb_bus_may_powerdown(dev->dev->bus);
+}
+
+/* Initialize a wireless core */
+static int b43_wireless_core_init(struct b43_wldev *dev)
+{
+	struct b43_wl *wl = dev->wl;
+	struct ssb_bus *bus = dev->dev->bus;
+	struct ssb_sprom *sprom = &bus->sprom;
+	struct b43_phy *phy = &dev->phy;
+	int err;
+	u32 hf, tmp;
+
+	B43_WARN_ON(b43_status(dev) != B43_STAT_UNINIT);
+
+	err = ssb_bus_powerup(bus, 0);
+	if (err)
+		goto out;
+	if (!ssb_device_is_enabled(dev->dev)) {
+		tmp = phy->gmode ? B43_TMSLOW_GMODE : 0;
+		b43_wireless_core_reset(dev, tmp);
+	}
+
+	if ((phy->type == B43_PHYTYPE_B) || (phy->type == B43_PHYTYPE_G)) {
+		phy->lo_control =
+		    kzalloc(sizeof(*(phy->lo_control)), GFP_KERNEL);
+		if (!phy->lo_control) {
+			err = -ENOMEM;
+			goto err_busdown;
+		}
+	}
+	setup_struct_wldev_for_init(dev);
+
+	err = b43_phy_init_tssi2dbm_table(dev);
+	if (err)
+		goto err_kfree_lo_control;
+
+	/* Enable IRQ routing to this device. */
+	ssb_pcicore_dev_irqvecs_enable(&bus->pcicore, dev->dev);
+
+	b43_imcfglo_timeouts_workaround(dev);
+	b43_bluetooth_coext_disable(dev);
+	b43_phy_early_init(dev);
+	err = b43_chip_init(dev);
+	if (err)
+		goto err_kfree_tssitbl;
+	b43_shm_write16(dev, B43_SHM_SHARED,
+			B43_SHM_SH_WLCOREREV, dev->dev->id.revision);
+	hf = b43_hf_read(dev);
+	if (phy->type == B43_PHYTYPE_G) {
+		hf |= B43_HF_SYMW;
+		if (phy->rev == 1)
+			hf |= B43_HF_GDCW;
+		if (sprom->r1.boardflags_lo & B43_BFL_PACTRL)
+			hf |= B43_HF_OFDMPABOOST;
+	} else if (phy->type == B43_PHYTYPE_B) {
+		hf |= B43_HF_SYMW;
+		if (phy->rev >= 2 && phy->radio_ver == 0x2050)
+			hf &= ~B43_HF_GDCW;
+	}
+	b43_hf_write(dev, hf);
+
+	/* Short/Long Retry Limit.
+	 * The retry-limit is a 4-bit counter. Enforce this to avoid overflowing
+	 * the chip-internal counter.
+	 */
+	tmp = limit_value(modparam_short_retry, 0, 0xF);
+	b43_shm_write16(dev, B43_SHM_SCRATCH, B43_SHM_SC_SRLIMIT, tmp);
+	tmp = limit_value(modparam_long_retry, 0, 0xF);
+	b43_shm_write16(dev, B43_SHM_SCRATCH, B43_SHM_SC_LRLIMIT, tmp);
+
+	b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_SFFBLIM, 3);
+	b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_LFFBLIM, 2);
+
+	/* Disable sending probe responses from firmware.
+	 * Setting the MaxTime to one usec will always trigger
+	 * a timeout, so we never send any probe resp.
+	 * A timeout of zero is infinite. */
+	b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_PRMAXTIME, 1);
+
+	b43_rate_memory_init(dev);
+
+	/* Minimum Contention Window */
+	if (phy->type == B43_PHYTYPE_B) {
+		b43_shm_write16(dev, B43_SHM_SCRATCH, B43_SHM_SC_MINCONT, 0x1F);
+	} else {
+		b43_shm_write16(dev, B43_SHM_SCRATCH, B43_SHM_SC_MINCONT, 0xF);
+	}
+	/* Maximum Contention Window */
+	b43_shm_write16(dev, B43_SHM_SCRATCH, B43_SHM_SC_MAXCONT, 0x3FF);
+
+	do {
+		if (b43_using_pio(dev)) {
+			err = b43_pio_init(dev);
+		} else {
+			err = b43_dma_init(dev);
+			if (!err)
+				b43_qos_init(dev);
+		}
+	} while (err == -EAGAIN);
+	if (err)
+		goto err_chip_exit;
+
+//FIXME
+#if 1
+	b43_write16(dev, 0x0612, 0x0050);
+	b43_shm_write16(dev, B43_SHM_SHARED, 0x0416, 0x0050);
+	b43_shm_write16(dev, B43_SHM_SHARED, 0x0414, 0x01F4);
+#endif
+
+	b43_bluetooth_coext_enable(dev);
+
+	ssb_bus_powerup(bus, 1);	/* Enable dynamic PCTL */
+	memset(wl->bssid, 0, ETH_ALEN);
+	memset(wl->mac_addr, 0, ETH_ALEN);
+	b43_upload_card_macaddress(dev);
+	b43_security_init(dev);
+	b43_rfkill_init(dev);
+	b43_rng_init(wl);
+
+	b43_set_status(dev, B43_STAT_INITIALIZED);
+
+      out:
+	return err;
+
+      err_chip_exit:
+	b43_chip_exit(dev);
+      err_kfree_tssitbl:
+	if (phy->dyn_tssi_tbl)
+		kfree(phy->tssi2dbm);
+      err_kfree_lo_control:
+	kfree(phy->lo_control);
+	phy->lo_control = NULL;
+      err_busdown:
+	ssb_bus_may_powerdown(bus);
+	B43_WARN_ON(b43_status(dev) != B43_STAT_UNINIT);
+	return err;
+}
+
+static int b43_add_interface(struct ieee80211_hw *hw,
+			     struct ieee80211_if_init_conf *conf)
+{
+	struct b43_wl *wl = hw_to_b43_wl(hw);
+	struct b43_wldev *dev;
+	unsigned long flags;
+	int err = -EOPNOTSUPP;
+
+	/* TODO: allow WDS/AP devices to coexist */
+
+	if (conf->type != IEEE80211_IF_TYPE_AP &&
+	    conf->type != IEEE80211_IF_TYPE_STA &&
+	    conf->type != IEEE80211_IF_TYPE_WDS &&
+	    conf->type != IEEE80211_IF_TYPE_IBSS)
+		return -EOPNOTSUPP;
+
+	mutex_lock(&wl->mutex);
+	if (wl->operating)
+		goto out_mutex_unlock;
+
+	b43dbg(wl, "Adding Interface type %d\n", conf->type);
+
+	dev = wl->current_dev;
+	wl->operating = 1;
+	wl->if_id = conf->if_id;
+	wl->if_type = conf->type;
+	memcpy(wl->mac_addr, conf->mac_addr, ETH_ALEN);
+
+	spin_lock_irqsave(&wl->irq_lock, flags);
+	b43_adjust_opmode(dev);
+	b43_upload_card_macaddress(dev);
+	spin_unlock_irqrestore(&wl->irq_lock, flags);
+
+	err = 0;
+ out_mutex_unlock:
+	mutex_unlock(&wl->mutex);
+
+	return err;
+}
+
+static void b43_remove_interface(struct ieee80211_hw *hw,
+				 struct ieee80211_if_init_conf *conf)
+{
+	struct b43_wl *wl = hw_to_b43_wl(hw);
+	struct b43_wldev *dev = wl->current_dev;
+	unsigned long flags;
+
+	b43dbg(wl, "Removing Interface type %d\n", conf->type);
+
+	mutex_lock(&wl->mutex);
+
+	B43_WARN_ON(!wl->operating);
+	B43_WARN_ON(wl->if_id != conf->if_id);
+
+	wl->operating = 0;
+
+	spin_lock_irqsave(&wl->irq_lock, flags);
+	b43_adjust_opmode(dev);
+	memset(wl->mac_addr, 0, ETH_ALEN);
+	b43_upload_card_macaddress(dev);
+	spin_unlock_irqrestore(&wl->irq_lock, flags);
+
+	mutex_unlock(&wl->mutex);
+}
+
+static int b43_start(struct ieee80211_hw *hw)
+{
+	struct b43_wl *wl = hw_to_b43_wl(hw);
+	struct b43_wldev *dev = wl->current_dev;
+	int did_init = 0;
+	int err;
+
+	mutex_lock(&wl->mutex);
+
+	if (b43_status(dev) < B43_STAT_INITIALIZED) {
+		err = b43_wireless_core_init(dev);
+		if (err)
+			goto out_mutex_unlock;
+		did_init = 1;
+	}
+
+	if (b43_status(dev) < B43_STAT_STARTED) {
+		err = b43_wireless_core_start(dev);
+		if (err) {
+			if (did_init)
+				b43_wireless_core_exit(dev);
+			goto out_mutex_unlock;
+		}
+	}
+
+ out_mutex_unlock:
+	mutex_unlock(&wl->mutex);
+
+	return err;
+}
+
+void b43_stop(struct ieee80211_hw *hw)
+{
+	struct b43_wl *wl = hw_to_b43_wl(hw);
+	struct b43_wldev *dev = wl->current_dev;
+
+	mutex_lock(&wl->mutex);
+	if (b43_status(dev) >= B43_STAT_STARTED)
+		b43_wireless_core_stop(dev);
+	b43_wireless_core_exit(dev);
+	mutex_unlock(&wl->mutex);
+}
+
+static const struct ieee80211_ops b43_hw_ops = {
+	.tx = b43_tx,
+	.conf_tx = b43_conf_tx,
+	.add_interface = b43_add_interface,
+	.remove_interface = b43_remove_interface,
+	.config = b43_dev_config,
+	.config_interface = b43_config_interface,
+	.configure_filter = b43_configure_filter,
+	.set_key = b43_dev_set_key,
+	.get_stats = b43_get_stats,
+	.get_tx_stats = b43_get_tx_stats,
+	.start = b43_start,
+	.stop = b43_stop,
+};
+
+/* Hard-reset the chip. Do not call this directly.
+ * Use b43_controller_restart()
+ */
+static void b43_chip_reset(struct work_struct *work)
+{
+	struct b43_wldev *dev =
+	    container_of(work, struct b43_wldev, restart_work);
+	struct b43_wl *wl = dev->wl;
+	int err = 0;
+	int prev_status;
+
+	mutex_lock(&wl->mutex);
+
+	prev_status = b43_status(dev);
+	/* Bring the device down... */
+	if (prev_status >= B43_STAT_STARTED)
+		b43_wireless_core_stop(dev);
+	if (prev_status >= B43_STAT_INITIALIZED)
+		b43_wireless_core_exit(dev);
+
+	/* ...and up again. */
+	if (prev_status >= B43_STAT_INITIALIZED) {
+		err = b43_wireless_core_init(dev);
+		if (err)
+			goto out;
+	}
+	if (prev_status >= B43_STAT_STARTED) {
+		err = b43_wireless_core_start(dev);
+		if (err) {
+			b43_wireless_core_exit(dev);
+			goto out;
+		}
+	}
+      out:
+	mutex_unlock(&wl->mutex);
+	if (err)
+		b43err(wl, "Controller restart FAILED\n");
+	else
+		b43info(wl, "Controller restarted\n");
+}
+
+static int b43_setup_modes(struct b43_wldev *dev,
+			   int have_aphy, int have_bphy, int have_gphy)
+{
+	struct ieee80211_hw *hw = dev->wl->hw;
+	struct ieee80211_hw_mode *mode;
+	struct b43_phy *phy = &dev->phy;
+	int cnt = 0;
+	int err;
+
+/*FIXME: Don't tell ieee80211 about an A-PHY, because we currently don't support A-PHY. */
+	have_aphy = 0;
+
+	phy->possible_phymodes = 0;
+	for (; 1; cnt++) {
+		if (have_aphy) {
+			B43_WARN_ON(cnt >= B43_MAX_PHYHWMODES);
+			mode = &phy->hwmodes[cnt];
+
+			mode->mode = MODE_IEEE80211A;
+			mode->num_channels = b43_a_chantable_size;
+			mode->channels = b43_a_chantable;
+			mode->num_rates = b43_a_ratetable_size;
+			mode->rates = b43_a_ratetable;
+			err = ieee80211_register_hwmode(hw, mode);
+			if (err)
+				return err;
+
+			phy->possible_phymodes |= B43_PHYMODE_A;
+			have_aphy = 0;
+			continue;
+		}
+		if (have_bphy) {
+			B43_WARN_ON(cnt >= B43_MAX_PHYHWMODES);
+			mode = &phy->hwmodes[cnt];
+
+			mode->mode = MODE_IEEE80211B;
+			mode->num_channels = b43_bg_chantable_size;
+			mode->channels = b43_bg_chantable;
+			mode->num_rates = b43_b_ratetable_size;
+			mode->rates = b43_b_ratetable;
+			err = ieee80211_register_hwmode(hw, mode);
+			if (err)
+				return err;
+
+			phy->possible_phymodes |= B43_PHYMODE_B;
+			have_bphy = 0;
+			continue;
+		}
+		if (have_gphy) {
+			B43_WARN_ON(cnt >= B43_MAX_PHYHWMODES);
+			mode = &phy->hwmodes[cnt];
+
+			mode->mode = MODE_IEEE80211G;
+			mode->num_channels = b43_bg_chantable_size;
+			mode->channels = b43_bg_chantable;
+			mode->num_rates = b43_g_ratetable_size;
+			mode->rates = b43_g_ratetable;
+			err = ieee80211_register_hwmode(hw, mode);
+			if (err)
+				return err;
+
+			phy->possible_phymodes |= B43_PHYMODE_G;
+			have_gphy = 0;
+			continue;
+		}
+		break;
+	}
+
+	return 0;
+}
+
+static void b43_wireless_core_detach(struct b43_wldev *dev)
+{
+	b43_rfkill_free(dev);
+	/* We release firmware that late to not be required to re-request
+	 * is all the time when we reinit the core. */
+	b43_release_firmware(dev);
+}
+
+static int b43_wireless_core_attach(struct b43_wldev *dev)
+{
+	struct b43_wl *wl = dev->wl;
+	struct ssb_bus *bus = dev->dev->bus;
+	struct pci_dev *pdev = bus->host_pci;
+	int err;
+	int have_aphy = 0, have_bphy = 0, have_gphy = 0;
+	u32 tmp;
+
+	/* Do NOT do any device initialization here.
+	 * Do it in wireless_core_init() instead.
+	 * This function is for gathering basic information about the HW, only.
+	 * Also some structs may be set up here. But most likely you want to have
+	 * that in core_init(), too.
+	 */
+
+	err = ssb_bus_powerup(bus, 0);
+	if (err) {
+		b43err(wl, "Bus powerup failed\n");
+		goto out;
+	}
+	/* Get the PHY type. */
+	if (dev->dev->id.revision >= 5) {
+		u32 tmshigh;
+
+		tmshigh = ssb_read32(dev->dev, SSB_TMSHIGH);
+		have_aphy = !!(tmshigh & B43_TMSHIGH_APHY);
+		have_gphy = !!(tmshigh & B43_TMSHIGH_GPHY);
+		if (!have_aphy && !have_gphy)
+			have_bphy = 1;
+	} else if (dev->dev->id.revision == 4) {
+		have_gphy = 1;
+		have_aphy = 1;
+	} else
+		have_bphy = 1;
+
+	dev->phy.gmode = (have_gphy || have_bphy);
+	tmp = dev->phy.gmode ? B43_TMSLOW_GMODE : 0;
+	b43_wireless_core_reset(dev, tmp);
+
+	err = b43_phy_versioning(dev);
+	if (err)
+		goto err_powerdown;
+	/* Check if this device supports multiband. */
+	if (!pdev ||
+	    (pdev->device != 0x4312 &&
+	     pdev->device != 0x4319 && pdev->device != 0x4324)) {
+		/* No multiband support. */
+		have_aphy = 0;
+		have_bphy = 0;
+		have_gphy = 0;
+		switch (dev->phy.type) {
+		case B43_PHYTYPE_A:
+			have_aphy = 1;
+			break;
+		case B43_PHYTYPE_B:
+			have_bphy = 1;
+			break;
+		case B43_PHYTYPE_G:
+			have_gphy = 1;
+			break;
+		default:
+			B43_WARN_ON(1);
+		}
+	}
+	dev->phy.gmode = (have_gphy || have_bphy);
+	tmp = dev->phy.gmode ? B43_TMSLOW_GMODE : 0;
+	b43_wireless_core_reset(dev, tmp);
+
+	err = b43_validate_chipaccess(dev);
+	if (err)
+		goto err_powerdown;
+	err = b43_setup_modes(dev, have_aphy, have_bphy, have_gphy);
+	if (err)
+		goto err_powerdown;
+
+	/* Now set some default "current_dev" */
+	if (!wl->current_dev)
+		wl->current_dev = dev;
+	INIT_WORK(&dev->restart_work, b43_chip_reset);
+	b43_rfkill_alloc(dev);
+
+	b43_radio_turn_off(dev, 1);
+	b43_switch_analog(dev, 0);
+	ssb_device_disable(dev->dev, 0);
+	ssb_bus_may_powerdown(bus);
+
+out:
+	return err;
+
+err_powerdown:
+	ssb_bus_may_powerdown(bus);
+	return err;
+}
+
+static void b43_one_core_detach(struct ssb_device *dev)
+{
+	struct b43_wldev *wldev;
+	struct b43_wl *wl;
+
+	wldev = ssb_get_drvdata(dev);
+	wl = wldev->wl;
+	cancel_work_sync(&wldev->restart_work);
+	b43_debugfs_remove_device(wldev);
+	b43_wireless_core_detach(wldev);
+	list_del(&wldev->list);
+	wl->nr_devs--;
+	ssb_set_drvdata(dev, NULL);
+	kfree(wldev);
+}
+
+static int b43_one_core_attach(struct ssb_device *dev, struct b43_wl *wl)
+{
+	struct b43_wldev *wldev;
+	struct pci_dev *pdev;
+	int err = -ENOMEM;
+
+	if (!list_empty(&wl->devlist)) {
+		/* We are not the first core on this chip. */
+		pdev = dev->bus->host_pci;
+		/* Only special chips support more than one wireless
+		 * core, although some of the other chips have more than
+		 * one wireless core as well. Check for this and
+		 * bail out early.
+		 */
+		if (!pdev ||
+		    ((pdev->device != 0x4321) &&
+		     (pdev->device != 0x4313) && (pdev->device != 0x431A))) {
+			b43dbg(wl, "Ignoring unconnected 802.11 core\n");
+			return -ENODEV;
+		}
+	}
+
+	wldev = kzalloc(sizeof(*wldev), GFP_KERNEL);
+	if (!wldev)
+		goto out;
+
+	wldev->dev = dev;
+	wldev->wl = wl;
+	b43_set_status(wldev, B43_STAT_UNINIT);
+	wldev->bad_frames_preempt = modparam_bad_frames_preempt;
+	tasklet_init(&wldev->isr_tasklet,
+		     (void (*)(unsigned long))b43_interrupt_tasklet,
+		     (unsigned long)wldev);
+	if (modparam_pio)
+		wldev->__using_pio = 1;
+	INIT_LIST_HEAD(&wldev->list);
+
+	err = b43_wireless_core_attach(wldev);
+	if (err)
+		goto err_kfree_wldev;
+
+	list_add(&wldev->list, &wl->devlist);
+	wl->nr_devs++;
+	ssb_set_drvdata(dev, wldev);
+	b43_debugfs_add_device(wldev);
+
+      out:
+	return err;
+
+      err_kfree_wldev:
+	kfree(wldev);
+	return err;
+}
+
+static void b43_sprom_fixup(struct ssb_bus *bus)
+{
+	/* boardflags workarounds */
+	if (bus->boardinfo.vendor == SSB_BOARDVENDOR_DELL &&
+	    bus->chip_id == 0x4301 && bus->boardinfo.rev == 0x74)
+		bus->sprom.r1.boardflags_lo |= B43_BFL_BTCOEXIST;
+	if (bus->boardinfo.vendor == PCI_VENDOR_ID_APPLE &&
+	    bus->boardinfo.type == 0x4E && bus->boardinfo.rev > 0x40)
+		bus->sprom.r1.boardflags_lo |= B43_BFL_PACTRL;
+
+	/* Handle case when gain is not set in sprom */
+	if (bus->sprom.r1.antenna_gain_a == 0xFF)
+		bus->sprom.r1.antenna_gain_a = 2;
+	if (bus->sprom.r1.antenna_gain_bg == 0xFF)
+		bus->sprom.r1.antenna_gain_bg = 2;
+
+	/* Convert Antennagain values to Q5.2 */
+	bus->sprom.r1.antenna_gain_a <<= 2;
+	bus->sprom.r1.antenna_gain_bg <<= 2;
+}
+
+static void b43_wireless_exit(struct ssb_device *dev, struct b43_wl *wl)
+{
+	struct ieee80211_hw *hw = wl->hw;
+
+	ssb_set_devtypedata(dev, NULL);
+	ieee80211_free_hw(hw);
+}
+
+static int b43_wireless_init(struct ssb_device *dev)
+{
+	struct ssb_sprom *sprom = &dev->bus->sprom;
+	struct ieee80211_hw *hw;
+	struct b43_wl *wl;
+	int err = -ENOMEM;
+
+	b43_sprom_fixup(dev->bus);
+
+	hw = ieee80211_alloc_hw(sizeof(*wl), &b43_hw_ops);
+	if (!hw) {
+		b43err(NULL, "Could not allocate ieee80211 device\n");
+		goto out;
+	}
+
+	/* fill hw info */
+	hw->flags = IEEE80211_HW_HOST_GEN_BEACON_TEMPLATE;
+	hw->max_signal = 100;
+	hw->max_rssi = -110;
+	hw->max_noise = -110;
+	hw->queues = 1;		/* FIXME: hardware has more queues */
+	SET_IEEE80211_DEV(hw, dev->dev);
+	if (is_valid_ether_addr(sprom->r1.et1mac))
+		SET_IEEE80211_PERM_ADDR(hw, sprom->r1.et1mac);
+	else
+		SET_IEEE80211_PERM_ADDR(hw, sprom->r1.il0mac);
+
+	/* Get and initialize struct b43_wl */
+	wl = hw_to_b43_wl(hw);
+	memset(wl, 0, sizeof(*wl));
+	wl->hw = hw;
+	spin_lock_init(&wl->irq_lock);
+	spin_lock_init(&wl->leds_lock);
+	mutex_init(&wl->mutex);
+	INIT_LIST_HEAD(&wl->devlist);
+
+	ssb_set_devtypedata(dev, wl);
+	b43info(wl, "Broadcom %04X WLAN found\n", dev->bus->chip_id);
+	err = 0;
+      out:
+	return err;
+}
+
+static int b43_probe(struct ssb_device *dev, const struct ssb_device_id *id)
+{
+	struct b43_wl *wl;
+	int err;
+	int first = 0;
+
+	wl = ssb_get_devtypedata(dev);
+	if (!wl) {
+		/* Probing the first core. Must setup common struct b43_wl */
+		first = 1;
+		err = b43_wireless_init(dev);
+		if (err)
+			goto out;
+		wl = ssb_get_devtypedata(dev);
+		B43_WARN_ON(!wl);
+	}
+	err = b43_one_core_attach(dev, wl);
+	if (err)
+		goto err_wireless_exit;
+
+	if (first) {
+		err = ieee80211_register_hw(wl->hw);
+		if (err)
+			goto err_one_core_detach;
+	}
+
+      out:
+	return err;
+
+      err_one_core_detach:
+	b43_one_core_detach(dev);
+      err_wireless_exit:
+	if (first)
+		b43_wireless_exit(dev, wl);
+	return err;
+}
+
+static void b43_remove(struct ssb_device *dev)
+{
+	struct b43_wl *wl = ssb_get_devtypedata(dev);
+	struct b43_wldev *wldev = ssb_get_drvdata(dev);
+
+	B43_WARN_ON(!wl);
+	if (wl->current_dev == wldev)
+		ieee80211_unregister_hw(wl->hw);
+
+	b43_one_core_detach(dev);
+
+	if (list_empty(&wl->devlist)) {
+		/* Last core on the chip unregistered.
+		 * We can destroy common struct b43_wl.
+		 */
+		b43_wireless_exit(dev, wl);
+	}
+}
+
+/* Perform a hardware reset. This can be called from any context. */
+void b43_controller_restart(struct b43_wldev *dev, const char *reason)
+{
+	/* Must avoid requeueing, if we are in shutdown. */
+	if (b43_status(dev) < B43_STAT_INITIALIZED)
+		return;
+	b43info(dev->wl, "Controller RESET (%s) ...\n", reason);
+	queue_work(dev->wl->hw->workqueue, &dev->restart_work);
+}
+
+#ifdef CONFIG_PM
+
+static int b43_suspend(struct ssb_device *dev, pm_message_t state)
+{
+	struct b43_wldev *wldev = ssb_get_drvdata(dev);
+	struct b43_wl *wl = wldev->wl;
+
+	b43dbg(wl, "Suspending...\n");
+
+	mutex_lock(&wl->mutex);
+	wldev->suspend_init_status = b43_status(wldev);
+	if (wldev->suspend_init_status >= B43_STAT_STARTED)
+		b43_wireless_core_stop(wldev);
+	if (wldev->suspend_init_status >= B43_STAT_INITIALIZED)
+		b43_wireless_core_exit(wldev);
+	mutex_unlock(&wl->mutex);
+
+	b43dbg(wl, "Device suspended.\n");
+
+	return 0;
+}
+
+static int b43_resume(struct ssb_device *dev)
+{
+	struct b43_wldev *wldev = ssb_get_drvdata(dev);
+	struct b43_wl *wl = wldev->wl;
+	int err = 0;
+
+	b43dbg(wl, "Resuming...\n");
+
+	mutex_lock(&wl->mutex);
+	if (wldev->suspend_init_status >= B43_STAT_INITIALIZED) {
+		err = b43_wireless_core_init(wldev);
+		if (err) {
+			b43err(wl, "Resume failed at core init\n");
+			goto out;
+		}
+	}
+	if (wldev->suspend_init_status >= B43_STAT_STARTED) {
+		err = b43_wireless_core_start(wldev);
+		if (err) {
+			b43_wireless_core_exit(wldev);
+			b43err(wl, "Resume failed at core start\n");
+			goto out;
+		}
+	}
+	mutex_unlock(&wl->mutex);
+
+	b43dbg(wl, "Device resumed.\n");
+      out:
+	return err;
+}
+
+#else /* CONFIG_PM */
+# define b43_suspend	NULL
+# define b43_resume	NULL
+#endif /* CONFIG_PM */
+
+static struct ssb_driver b43_ssb_driver = {
+	.name		= KBUILD_MODNAME,
+	.id_table	= b43_ssb_tbl,
+	.probe		= b43_probe,
+	.remove		= b43_remove,
+	.suspend	= b43_suspend,
+	.resume		= b43_resume,
+};
+
+static int __init b43_init(void)
+{
+	int err;
+
+	b43_debugfs_init();
+	err = b43_pcmcia_init();
+	if (err)
+		goto err_dfs_exit;
+	err = ssb_driver_register(&b43_ssb_driver);
+	if (err)
+		goto err_pcmcia_exit;
+
+	return err;
+
+err_pcmcia_exit:
+	b43_pcmcia_exit();
+err_dfs_exit:
+	b43_debugfs_exit();
+	return err;
+}
+
+static void __exit b43_exit(void)
+{
+	ssb_driver_unregister(&b43_ssb_driver);
+	b43_pcmcia_exit();
+	b43_debugfs_exit();
+}
+
+module_init(b43_init)
+module_exit(b43_exit)
diff --git a/drivers/net/wireless/b43/main.h b/drivers/net/wireless/b43/main.h
new file mode 100644
index 000000000000..284d17da17d1
--- /dev/null
+++ b/drivers/net/wireless/b43/main.h
@@ -0,0 +1,125 @@
+/*
+
+  Broadcom B43 wireless driver
+
+  Copyright (c) 2005 Martin Langer <martin-langer@gmx.de>,
+                     Stefano Brivio <st3@riseup.net>
+                     Michael Buesch <mb@bu3sch.de>
+                     Danny van Dyk <kugelfang@gentoo.org>
+                     Andreas Jaggi <andreas.jaggi@waterwave.ch>
+
+  Some parts of the code in this file are derived from the ipw2200
+  driver  Copyright(c) 2003 - 2004 Intel Corporation.
+
+  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, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with this program; see the file COPYING.  If not, write to
+  the Free Software Foundation, Inc., 51 Franklin Steet, Fifth Floor,
+  Boston, MA 02110-1301, USA.
+
+*/
+
+#ifndef B43_MAIN_H_
+#define B43_MAIN_H_
+
+#include "b43.h"
+
+#define P4D_BYT3S(magic, nr_bytes)	u8 __p4dding##magic[nr_bytes]
+#define P4D_BYTES(line, nr_bytes)	P4D_BYT3S(line, nr_bytes)
+/* Magic helper macro to pad structures. Ignore those above. It's magic. */
+#define PAD_BYTES(nr_bytes)		P4D_BYTES( __LINE__ , (nr_bytes))
+
+/* Lightweight function to convert a frequency (in Mhz) to a channel number. */
+static inline u8 b43_freq_to_channel_a(int freq)
+{
+	return ((freq - 5000) / 5);
+}
+static inline u8 b43_freq_to_channel_bg(int freq)
+{
+	u8 channel;
+
+	if (freq == 2484)
+		channel = 14;
+	else
+		channel = (freq - 2407) / 5;
+
+	return channel;
+}
+static inline u8 b43_freq_to_channel(struct b43_wldev *dev, int freq)
+{
+	if (dev->phy.type == B43_PHYTYPE_A)
+		return b43_freq_to_channel_a(freq);
+	return b43_freq_to_channel_bg(freq);
+}
+
+/* Lightweight function to convert a channel number to a frequency (in Mhz). */
+static inline int b43_channel_to_freq_a(u8 channel)
+{
+	return (5000 + (5 * channel));
+}
+static inline int b43_channel_to_freq_bg(u8 channel)
+{
+	int freq;
+
+	if (channel == 14)
+		freq = 2484;
+	else
+		freq = 2407 + (5 * channel);
+
+	return freq;
+}
+static inline int b43_channel_to_freq(struct b43_wldev *dev, u8 channel)
+{
+	if (dev->phy.type == B43_PHYTYPE_A)
+		return b43_channel_to_freq_a(channel);
+	return b43_channel_to_freq_bg(channel);
+}
+
+static inline int b43_is_cck_rate(int rate)
+{
+	return (rate == B43_CCK_RATE_1MB ||
+		rate == B43_CCK_RATE_2MB ||
+		rate == B43_CCK_RATE_5MB || rate == B43_CCK_RATE_11MB);
+}
+
+static inline int b43_is_ofdm_rate(int rate)
+{
+	return !b43_is_cck_rate(rate);
+}
+
+void b43_tsf_read(struct b43_wldev *dev, u64 * tsf);
+void b43_tsf_write(struct b43_wldev *dev, u64 tsf);
+
+u32 b43_shm_read32(struct b43_wldev *dev, u16 routing, u16 offset);
+u16 b43_shm_read16(struct b43_wldev *dev, u16 routing, u16 offset);
+void b43_shm_write32(struct b43_wldev *dev, u16 routing, u16 offset, u32 value);
+void b43_shm_write16(struct b43_wldev *dev, u16 routing, u16 offset, u16 value);
+
+u32 b43_hf_read(struct b43_wldev *dev);
+void b43_hf_write(struct b43_wldev *dev, u32 value);
+
+void b43_dummy_transmission(struct b43_wldev *dev);
+
+void b43_wireless_core_reset(struct b43_wldev *dev, u32 flags);
+
+void b43_mac_suspend(struct b43_wldev *dev);
+void b43_mac_enable(struct b43_wldev *dev);
+
+void b43_controller_restart(struct b43_wldev *dev, const char *reason);
+
+#define B43_PS_ENABLED	(1 << 0)	/* Force enable hardware power saving */
+#define B43_PS_DISABLED	(1 << 1)	/* Force disable hardware power saving */
+#define B43_PS_AWAKE	(1 << 2)	/* Force device awake */
+#define B43_PS_ASLEEP	(1 << 3)	/* Force device asleep */
+void b43_power_saving_ctl_bits(struct b43_wldev *dev, unsigned int ps_flags);
+
+#endif /* B43_MAIN_H_ */
diff --git a/drivers/net/wireless/b43/pcmcia.c b/drivers/net/wireless/b43/pcmcia.c
new file mode 100644
index 000000000000..b242a9a90dd2
--- /dev/null
+++ b/drivers/net/wireless/b43/pcmcia.c
@@ -0,0 +1,160 @@
+/*
+
+  Broadcom B43 wireless driver
+
+  Copyright (c) 2007 Michael Buesch <mb@bu3sch.de>
+
+  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, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with this program; see the file COPYING.  If not, write to
+  the Free Software Foundation, Inc., 51 Franklin Steet, Fifth Floor,
+  Boston, MA 02110-1301, USA.
+
+*/
+
+#include "pcmcia.h"
+
+#include <linux/ssb/ssb.h>
+
+#include <pcmcia/cs_types.h>
+#include <pcmcia/cs.h>
+#include <pcmcia/cistpl.h>
+#include <pcmcia/ciscode.h>
+#include <pcmcia/ds.h>
+#include <pcmcia/cisreg.h>
+
+
+static /*const */ struct pcmcia_device_id b43_pcmcia_tbl[] = {
+	PCMCIA_DEVICE_MANF_CARD(0x2D0, 0x448),
+	PCMCIA_DEVICE_NULL,
+};
+
+MODULE_DEVICE_TABLE(pcmcia, b43_pcmcia_tbl);
+
+#ifdef CONFIG_PM
+static int b43_pcmcia_suspend(struct pcmcia_device *dev)
+{
+	//TODO
+	return 0;
+}
+
+static int b43_pcmcia_resume(struct pcmcia_device *dev)
+{
+	//TODO
+	return 0;
+}
+#else /* CONFIG_PM */
+# define b43_pcmcia_suspend		NULL
+# define b43_pcmcia_resume		NULL
+#endif /* CONFIG_PM */
+
+static int __devinit b43_pcmcia_probe(struct pcmcia_device *dev)
+{
+	struct ssb_bus *ssb;
+	win_req_t win;
+	memreq_t mem;
+	tuple_t tuple;
+	cisparse_t parse;
+	int err = -ENOMEM;
+	int res;
+	unsigned char buf[64];
+
+	ssb = kzalloc(sizeof(*ssb), GFP_KERNEL);
+	if (!ssb)
+		goto out;
+
+	err = -ENODEV;
+	tuple.DesiredTuple = CISTPL_CONFIG;
+	tuple.Attributes = 0;
+	tuple.TupleData = buf;
+	tuple.TupleDataMax = sizeof(buf);
+	tuple.TupleOffset = 0;
+
+	res = pcmcia_get_first_tuple(dev, &tuple);
+	if (res != CS_SUCCESS)
+		goto err_kfree_ssb;
+	res = pcmcia_get_tuple_data(dev, &tuple);
+	if (res != CS_SUCCESS)
+		goto err_kfree_ssb;
+	res = pcmcia_parse_tuple(dev, &tuple, &parse);
+	if (res != CS_SUCCESS)
+		goto err_kfree_ssb;
+
+	dev->conf.ConfigBase = parse.config.base;
+	dev->conf.Present = parse.config.rmask[0];
+
+	dev->io.BasePort2 = 0;
+	dev->io.NumPorts2 = 0;
+	dev->io.Attributes2 = 0;
+
+	win.Attributes = WIN_MEMORY_TYPE_CM | WIN_ENABLE | WIN_USE_WAIT;
+	win.Base = 0;
+	win.Size = SSB_CORE_SIZE;
+	win.AccessSpeed = 1000;
+	res = pcmcia_request_window(&dev, &win, &dev->win);
+	if (res != CS_SUCCESS)
+		goto err_kfree_ssb;
+
+	mem.CardOffset = 0;
+	mem.Page = 0;
+	res = pcmcia_map_mem_page(dev->win, &mem);
+	if (res != CS_SUCCESS)
+		goto err_kfree_ssb;
+
+	res = pcmcia_request_configuration(dev, &dev->conf);
+	if (res != CS_SUCCESS)
+		goto err_disable;
+
+	err = ssb_bus_pcmciabus_register(ssb, dev, win.Base);
+	dev->priv = ssb;
+
+      out:
+	return err;
+      err_disable:
+	pcmcia_disable_device(dev);
+      err_kfree_ssb:
+	kfree(ssb);
+	return err;
+}
+
+static void __devexit b43_pcmcia_remove(struct pcmcia_device *dev)
+{
+	struct ssb_bus *ssb = dev->priv;
+
+	ssb_bus_unregister(ssb);
+	pcmcia_release_window(dev->win);
+	pcmcia_disable_device(dev);
+	kfree(ssb);
+	dev->priv = NULL;
+}
+
+static struct pcmcia_driver b43_pcmcia_driver = {
+	.owner = THIS_MODULE,
+	.drv = {
+		.name = "b43-pcmcia",
+		},
+	.id_table = b43_pcmcia_tbl,
+	.probe = b43_pcmcia_probe,
+	.remove = b43_pcmcia_remove,
+	.suspend = b43_pcmcia_suspend,
+	.resume = b43_pcmcia_resume,
+};
+
+int b43_pcmcia_init(void)
+{
+	return pcmcia_register_driver(&b43_pcmcia_driver);
+}
+
+void b43_pcmcia_exit(void)
+{
+	pcmcia_unregister_driver(&b43_pcmcia_driver);
+}
diff --git a/drivers/net/wireless/b43/pcmcia.h b/drivers/net/wireless/b43/pcmcia.h
new file mode 100644
index 000000000000..85f120a67cbe
--- /dev/null
+++ b/drivers/net/wireless/b43/pcmcia.h
@@ -0,0 +1,20 @@
+#ifndef B43_PCMCIA_H_
+#define B43_PCMCIA_H_
+
+#ifdef CONFIG_B43_PCMCIA
+
+int b43_pcmcia_init(void);
+void b43_pcmcia_exit(void);
+
+#else /* CONFIG_B43_PCMCIA */
+
+static inline int b43_pcmcia_init(void)
+{
+	return 0;
+}
+static inline void b43_pcmcia_exit(void)
+{
+}
+
+#endif /* CONFIG_B43_PCMCIA */
+#endif /* B43_PCMCIA_H_ */
diff --git a/drivers/net/wireless/b43/phy.c b/drivers/net/wireless/b43/phy.c
new file mode 100644
index 000000000000..3d4ed647c311
--- /dev/null
+++ b/drivers/net/wireless/b43/phy.c
@@ -0,0 +1,4381 @@
+/*
+
+  Broadcom B43 wireless driver
+
+  Copyright (c) 2005 Martin Langer <martin-langer@gmx.de>,
+  Copyright (c) 2005, 2006 Stefano Brivio <st3@riseup.net>
+  Copyright (c) 2005, 2006 Michael Buesch <mb@bu3sch.de>
+  Copyright (c) 2005, 2006 Danny van Dyk <kugelfang@gentoo.org>
+  Copyright (c) 2005, 2006 Andreas Jaggi <andreas.jaggi@waterwave.ch>
+
+  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, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with this program; see the file COPYING.  If not, write to
+  the Free Software Foundation, Inc., 51 Franklin Steet, Fifth Floor,
+  Boston, MA 02110-1301, USA.
+
+*/
+
+#include <linux/delay.h>
+#include <linux/io.h>
+#include <linux/types.h>
+
+#include "b43.h"
+#include "phy.h"
+#include "main.h"
+#include "tables.h"
+#include "lo.h"
+
+static const s8 b43_tssi2dbm_b_table[] = {
+	0x4D, 0x4C, 0x4B, 0x4A,
+	0x4A, 0x49, 0x48, 0x47,
+	0x47, 0x46, 0x45, 0x45,
+	0x44, 0x43, 0x42, 0x42,
+	0x41, 0x40, 0x3F, 0x3E,
+	0x3D, 0x3C, 0x3B, 0x3A,
+	0x39, 0x38, 0x37, 0x36,
+	0x35, 0x34, 0x32, 0x31,
+	0x30, 0x2F, 0x2D, 0x2C,
+	0x2B, 0x29, 0x28, 0x26,
+	0x25, 0x23, 0x21, 0x1F,
+	0x1D, 0x1A, 0x17, 0x14,
+	0x10, 0x0C, 0x06, 0x00,
+	-7, -7, -7, -7,
+	-7, -7, -7, -7,
+	-7, -7, -7, -7,
+};
+
+static const s8 b43_tssi2dbm_g_table[] = {
+	77, 77, 77, 76,
+	76, 76, 75, 75,
+	74, 74, 73, 73,
+	73, 72, 72, 71,
+	71, 70, 70, 69,
+	68, 68, 67, 67,
+	66, 65, 65, 64,
+	63, 63, 62, 61,
+	60, 59, 58, 57,
+	56, 55, 54, 53,
+	52, 50, 49, 47,
+	45, 43, 40, 37,
+	33, 28, 22, 14,
+	5, -7, -20, -20,
+	-20, -20, -20, -20,
+	-20, -20, -20, -20,
+};
+
+const u8 b43_radio_channel_codes_bg[] = {
+	12, 17, 22, 27,
+	32, 37, 42, 47,
+	52, 57, 62, 67,
+	72, 84,
+};
+
+static void b43_phy_initg(struct b43_wldev *dev);
+
+/* Reverse the bits of a 4bit value.
+ * Example:  1101 is flipped 1011
+ */
+static u16 flip_4bit(u16 value)
+{
+	u16 flipped = 0x0000;
+
+	B43_WARN_ON(value & ~0x000F);
+
+	flipped |= (value & 0x0001) << 3;
+	flipped |= (value & 0x0002) << 1;
+	flipped |= (value & 0x0004) >> 1;
+	flipped |= (value & 0x0008) >> 3;
+
+	return flipped;
+}
+
+static void generate_rfatt_list(struct b43_wldev *dev,
+				struct b43_rfatt_list *list)
+{
+	struct b43_phy *phy = &dev->phy;
+
+	/* APHY.rev < 5 || GPHY.rev < 6 */
+	static const struct b43_rfatt rfatt_0[] = {
+		{.att = 3,.with_padmix = 0,},
+		{.att = 1,.with_padmix = 0,},
+		{.att = 5,.with_padmix = 0,},
+		{.att = 7,.with_padmix = 0,},
+		{.att = 9,.with_padmix = 0,},
+		{.att = 2,.with_padmix = 0,},
+		{.att = 0,.with_padmix = 0,},
+		{.att = 4,.with_padmix = 0,},
+		{.att = 6,.with_padmix = 0,},
+		{.att = 8,.with_padmix = 0,},
+		{.att = 1,.with_padmix = 1,},
+		{.att = 2,.with_padmix = 1,},
+		{.att = 3,.with_padmix = 1,},
+		{.att = 4,.with_padmix = 1,},
+	};
+	/* Radio.rev == 8 && Radio.version == 0x2050 */
+	static const struct b43_rfatt rfatt_1[] = {
+		{.att = 2,.with_padmix = 1,},
+		{.att = 4,.with_padmix = 1,},
+		{.att = 6,.with_padmix = 1,},
+		{.att = 8,.with_padmix = 1,},
+		{.att = 10,.with_padmix = 1,},
+		{.att = 12,.with_padmix = 1,},
+		{.att = 14,.with_padmix = 1,},
+	};
+	/* Otherwise */
+	static const struct b43_rfatt rfatt_2[] = {
+		{.att = 0,.with_padmix = 1,},
+		{.att = 2,.with_padmix = 1,},
+		{.att = 4,.with_padmix = 1,},
+		{.att = 6,.with_padmix = 1,},
+		{.att = 8,.with_padmix = 1,},
+		{.att = 9,.with_padmix = 1,},
+		{.att = 9,.with_padmix = 1,},
+	};
+
+	if ((phy->type == B43_PHYTYPE_A && phy->rev < 5) ||
+	    (phy->type == B43_PHYTYPE_G && phy->rev < 6)) {
+		/* Software pctl */
+		list->list = rfatt_0;
+		list->len = ARRAY_SIZE(rfatt_0);
+		list->min_val = 0;
+		list->max_val = 9;
+		return;
+	}
+	if (phy->radio_ver == 0x2050 && phy->radio_rev == 8) {
+		/* Hardware pctl */
+		list->list = rfatt_1;
+		list->len = ARRAY_SIZE(rfatt_1);
+		list->min_val = 2;
+		list->max_val = 14;
+		return;
+	}
+	/* Hardware pctl */
+	list->list = rfatt_2;
+	list->len = ARRAY_SIZE(rfatt_2);
+	list->min_val = 0;
+	list->max_val = 9;
+}
+
+static void generate_bbatt_list(struct b43_wldev *dev,
+				struct b43_bbatt_list *list)
+{
+	static const struct b43_bbatt bbatt_0[] = {
+		{.att = 0,},
+		{.att = 1,},
+		{.att = 2,},
+		{.att = 3,},
+		{.att = 4,},
+		{.att = 5,},
+		{.att = 6,},
+		{.att = 7,},
+		{.att = 8,},
+	};
+
+	list->list = bbatt_0;
+	list->len = ARRAY_SIZE(bbatt_0);
+	list->min_val = 0;
+	list->max_val = 8;
+}
+
+bool b43_has_hardware_pctl(struct b43_phy *phy)
+{
+	if (!phy->hardware_power_control)
+		return 0;
+	switch (phy->type) {
+	case B43_PHYTYPE_A:
+		if (phy->rev >= 5)
+			return 1;
+		break;
+	case B43_PHYTYPE_G:
+		if (phy->rev >= 6)
+			return 1;
+		break;
+	default:
+		B43_WARN_ON(1);
+	}
+	return 0;
+}
+
+static void b43_shm_clear_tssi(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+
+	switch (phy->type) {
+	case B43_PHYTYPE_A:
+		b43_shm_write16(dev, B43_SHM_SHARED, 0x0068, 0x7F7F);
+		b43_shm_write16(dev, B43_SHM_SHARED, 0x006a, 0x7F7F);
+		break;
+	case B43_PHYTYPE_B:
+	case B43_PHYTYPE_G:
+		b43_shm_write16(dev, B43_SHM_SHARED, 0x0058, 0x7F7F);
+		b43_shm_write16(dev, B43_SHM_SHARED, 0x005a, 0x7F7F);
+		b43_shm_write16(dev, B43_SHM_SHARED, 0x0070, 0x7F7F);
+		b43_shm_write16(dev, B43_SHM_SHARED, 0x0072, 0x7F7F);
+		break;
+	}
+}
+
+void b43_raw_phy_lock(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+
+	B43_WARN_ON(!irqs_disabled());
+
+	/* We had a check for MACCTL==0 here, but I think that doesn't
+	 * make sense, as MACCTL is never 0 when this is called.
+	 *      --mb */
+	B43_WARN_ON(b43_read32(dev, B43_MMIO_MACCTL) == 0);
+
+	if (dev->dev->id.revision < 3) {
+		b43_mac_suspend(dev);
+		spin_lock(&phy->lock);
+	} else {
+		if (!b43_is_mode(dev->wl, IEEE80211_IF_TYPE_AP))
+			b43_power_saving_ctl_bits(dev, B43_PS_AWAKE);
+	}
+	phy->locked = 1;
+}
+
+void b43_raw_phy_unlock(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+
+	B43_WARN_ON(!irqs_disabled());
+	if (dev->dev->id.revision < 3) {
+		if (phy->locked) {
+			spin_unlock(&phy->lock);
+			b43_mac_enable(dev);
+		}
+	} else {
+		if (!b43_is_mode(dev->wl, IEEE80211_IF_TYPE_AP))
+			b43_power_saving_ctl_bits(dev, 0);
+	}
+	phy->locked = 0;
+}
+
+/* Different PHYs require different register routing flags.
+ * This adjusts (and does sanity checks on) the routing flags.
+ */
+static inline u16 adjust_phyreg_for_phytype(struct b43_phy *phy,
+					    u16 offset, struct b43_wldev *dev)
+{
+	if (phy->type == B43_PHYTYPE_A) {
+		/* OFDM registers are base-registers for the A-PHY. */
+		offset &= ~B43_PHYROUTE_OFDM_GPHY;
+	}
+	if (offset & B43_PHYROUTE_EXT_GPHY) {
+		/* Ext-G registers are only available on G-PHYs */
+		if (phy->type != B43_PHYTYPE_G) {
+			b43dbg(dev->wl, "EXT-G PHY access at "
+			       "0x%04X on %u type PHY\n", offset, phy->type);
+		}
+	}
+
+	return offset;
+}
+
+u16 b43_phy_read(struct b43_wldev * dev, u16 offset)
+{
+	struct b43_phy *phy = &dev->phy;
+
+	offset = adjust_phyreg_for_phytype(phy, offset, dev);
+	b43_write16(dev, B43_MMIO_PHY_CONTROL, offset);
+	return b43_read16(dev, B43_MMIO_PHY_DATA);
+}
+
+void b43_phy_write(struct b43_wldev *dev, u16 offset, u16 val)
+{
+	struct b43_phy *phy = &dev->phy;
+
+	offset = adjust_phyreg_for_phytype(phy, offset, dev);
+	b43_write16(dev, B43_MMIO_PHY_CONTROL, offset);
+	mmiowb();
+	b43_write16(dev, B43_MMIO_PHY_DATA, val);
+}
+
+static void b43_radio_set_txpower_a(struct b43_wldev *dev, u16 txpower);
+
+/* Adjust the transmission power output (G-PHY) */
+void b43_set_txpower_g(struct b43_wldev *dev,
+		       const struct b43_bbatt *bbatt,
+		       const struct b43_rfatt *rfatt, u8 tx_control)
+{
+	struct b43_phy *phy = &dev->phy;
+	struct b43_txpower_lo_control *lo = phy->lo_control;
+	u16 bb, rf;
+	u16 tx_bias, tx_magn;
+
+	bb = bbatt->att;
+	rf = rfatt->att;
+	tx_bias = lo->tx_bias;
+	tx_magn = lo->tx_magn;
+	if (unlikely(tx_bias == 0xFF))
+		tx_bias = 0;
+
+	/* Save the values for later */
+	phy->tx_control = tx_control;
+	memcpy(&phy->rfatt, rfatt, sizeof(*rfatt));
+	memcpy(&phy->bbatt, bbatt, sizeof(*bbatt));
+
+	if (b43_debug(dev, B43_DBG_XMITPOWER)) {
+		b43dbg(dev->wl, "Tuning TX-power to bbatt(%u), "
+		       "rfatt(%u), tx_control(0x%02X), "
+		       "tx_bias(0x%02X), tx_magn(0x%02X)\n",
+		       bb, rf, tx_control, tx_bias, tx_magn);
+	}
+
+	b43_phy_set_baseband_attenuation(dev, bb);
+	b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_RFATT, rf);
+	if (phy->radio_ver == 0x2050 && phy->radio_rev == 8) {
+		b43_radio_write16(dev, 0x43,
+				  (rf & 0x000F) | (tx_control & 0x0070));
+	} else {
+		b43_radio_write16(dev, 0x43, (b43_radio_read16(dev, 0x43)
+					      & 0xFFF0) | (rf & 0x000F));
+		b43_radio_write16(dev, 0x52, (b43_radio_read16(dev, 0x52)
+					      & ~0x0070) | (tx_control &
+							    0x0070));
+	}
+	if (has_tx_magnification(phy)) {
+		b43_radio_write16(dev, 0x52, tx_magn | tx_bias);
+	} else {
+		b43_radio_write16(dev, 0x52, (b43_radio_read16(dev, 0x52)
+					      & 0xFFF0) | (tx_bias & 0x000F));
+	}
+	if (phy->type == B43_PHYTYPE_G)
+		b43_lo_g_adjust(dev);
+}
+
+static void default_baseband_attenuation(struct b43_wldev *dev,
+					 struct b43_bbatt *bb)
+{
+	struct b43_phy *phy = &dev->phy;
+
+	if (phy->radio_ver == 0x2050 && phy->radio_rev < 6)
+		bb->att = 0;
+	else
+		bb->att = 2;
+}
+
+static void default_radio_attenuation(struct b43_wldev *dev,
+				      struct b43_rfatt *rf)
+{
+	struct ssb_bus *bus = dev->dev->bus;
+	struct b43_phy *phy = &dev->phy;
+
+	rf->with_padmix = 0;
+
+	if (bus->boardinfo.vendor == SSB_BOARDVENDOR_BCM &&
+	    bus->boardinfo.type == SSB_BOARD_BCM4309G) {
+		if (bus->boardinfo.rev < 0x43) {
+			rf->att = 2;
+			return;
+		} else if (bus->boardinfo.rev < 0x51) {
+			rf->att = 3;
+			return;
+		}
+	}
+
+	if (phy->type == B43_PHYTYPE_A) {
+		rf->att = 0x60;
+		return;
+	}
+
+	switch (phy->radio_ver) {
+	case 0x2053:
+		switch (phy->radio_rev) {
+		case 1:
+			rf->att = 6;
+			return;
+		}
+		break;
+	case 0x2050:
+		switch (phy->radio_rev) {
+		case 0:
+			rf->att = 5;
+			return;
+		case 1:
+			if (phy->type == B43_PHYTYPE_G) {
+				if (bus->boardinfo.vendor == SSB_BOARDVENDOR_BCM
+				    && bus->boardinfo.type == SSB_BOARD_BCM4309G
+				    && bus->boardinfo.rev >= 30)
+					rf->att = 3;
+				else if (bus->boardinfo.vendor ==
+					 SSB_BOARDVENDOR_BCM
+					 && bus->boardinfo.type ==
+					 SSB_BOARD_BU4306)
+					rf->att = 3;
+				else
+					rf->att = 1;
+			} else {
+				if (bus->boardinfo.vendor == SSB_BOARDVENDOR_BCM
+				    && bus->boardinfo.type == SSB_BOARD_BCM4309G
+				    && bus->boardinfo.rev >= 30)
+					rf->att = 7;
+				else
+					rf->att = 6;
+			}
+			return;
+		case 2:
+			if (phy->type == B43_PHYTYPE_G) {
+				if (bus->boardinfo.vendor == SSB_BOARDVENDOR_BCM
+				    && bus->boardinfo.type == SSB_BOARD_BCM4309G
+				    && bus->boardinfo.rev >= 30)
+					rf->att = 3;
+				else if (bus->boardinfo.vendor ==
+					 SSB_BOARDVENDOR_BCM
+					 && bus->boardinfo.type ==
+					 SSB_BOARD_BU4306)
+					rf->att = 5;
+				else if (bus->chip_id == 0x4320)
+					rf->att = 4;
+				else
+					rf->att = 3;
+			} else
+				rf->att = 6;
+			return;
+		case 3:
+			rf->att = 5;
+			return;
+		case 4:
+		case 5:
+			rf->att = 1;
+			return;
+		case 6:
+		case 7:
+			rf->att = 5;
+			return;
+		case 8:
+			rf->att = 0xA;
+			rf->with_padmix = 1;
+			return;
+		case 9:
+		default:
+			rf->att = 5;
+			return;
+		}
+	}
+	rf->att = 5;
+}
+
+static u16 default_tx_control(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+
+	if (phy->radio_ver != 0x2050)
+		return 0;
+	if (phy->radio_rev == 1)
+		return B43_TXCTL_PA2DB | B43_TXCTL_TXMIX;
+	if (phy->radio_rev < 6)
+		return B43_TXCTL_PA2DB;
+	if (phy->radio_rev == 8)
+		return B43_TXCTL_TXMIX;
+	return 0;
+}
+
+/* This func is called "PHY calibrate" in the specs... */
+void b43_phy_early_init(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+	struct b43_txpower_lo_control *lo = phy->lo_control;
+
+	default_baseband_attenuation(dev, &phy->bbatt);
+	default_radio_attenuation(dev, &phy->rfatt);
+	phy->tx_control = (default_tx_control(dev) << 4);
+
+	/* Commit previous writes */
+	b43_read32(dev, B43_MMIO_MACCTL);
+
+	if (phy->type == B43_PHYTYPE_B || phy->type == B43_PHYTYPE_G) {
+		generate_rfatt_list(dev, &lo->rfatt_list);
+		generate_bbatt_list(dev, &lo->bbatt_list);
+	}
+	if (phy->type == B43_PHYTYPE_G && phy->rev == 1) {
+		/* Workaround: Temporarly disable gmode through the early init
+		 * phase, as the gmode stuff is not needed for phy rev 1 */
+		phy->gmode = 0;
+		b43_wireless_core_reset(dev, 0);
+		b43_phy_initg(dev);
+		phy->gmode = 1;
+		b43_wireless_core_reset(dev, B43_TMSLOW_GMODE);
+	}
+}
+
+/* GPHY_TSSI_Power_Lookup_Table_Init */
+static void b43_gphy_tssi_power_lt_init(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+	int i;
+	u16 value;
+
+	for (i = 0; i < 32; i++)
+		b43_ofdmtab_write16(dev, 0x3C20, i, phy->tssi2dbm[i]);
+	for (i = 32; i < 64; i++)
+		b43_ofdmtab_write16(dev, 0x3C00, i - 32, phy->tssi2dbm[i]);
+	for (i = 0; i < 64; i += 2) {
+		value = (u16) phy->tssi2dbm[i];
+		value |= ((u16) phy->tssi2dbm[i + 1]) << 8;
+		b43_phy_write(dev, 0x380 + (i / 2), value);
+	}
+}
+
+/* GPHY_Gain_Lookup_Table_Init */
+static void b43_gphy_gain_lt_init(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+	struct b43_txpower_lo_control *lo = phy->lo_control;
+	u16 nr_written = 0;
+	u16 tmp;
+	u8 rf, bb;
+
+	if (!lo->lo_measured) {
+		b43_phy_write(dev, 0x3FF, 0);
+		return;
+	}
+
+	for (rf = 0; rf < lo->rfatt_list.len; rf++) {
+		for (bb = 0; bb < lo->bbatt_list.len; bb++) {
+			if (nr_written >= 0x40)
+				return;
+			tmp = lo->bbatt_list.list[bb].att;
+			tmp <<= 8;
+			if (phy->radio_rev == 8)
+				tmp |= 0x50;
+			else
+				tmp |= 0x40;
+			tmp |= lo->rfatt_list.list[rf].att;
+			b43_phy_write(dev, 0x3C0 + nr_written, tmp);
+			nr_written++;
+		}
+	}
+}
+
+/* GPHY_DC_Lookup_Table */
+void b43_gphy_dc_lt_init(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+	struct b43_txpower_lo_control *lo = phy->lo_control;
+	struct b43_loctl *loctl0;
+	struct b43_loctl *loctl1;
+	int i;
+	int rf_offset, bb_offset;
+	u16 tmp;
+
+	for (i = 0; i < lo->rfatt_list.len + lo->bbatt_list.len; i += 2) {
+		rf_offset = i / lo->rfatt_list.len;
+		bb_offset = i % lo->rfatt_list.len;
+
+		loctl0 = b43_get_lo_g_ctl(dev, &lo->rfatt_list.list[rf_offset],
+					  &lo->bbatt_list.list[bb_offset]);
+		if (i + 1 < lo->rfatt_list.len * lo->bbatt_list.len) {
+			rf_offset = (i + 1) / lo->rfatt_list.len;
+			bb_offset = (i + 1) % lo->rfatt_list.len;
+
+			loctl1 =
+			    b43_get_lo_g_ctl(dev,
+					     &lo->rfatt_list.list[rf_offset],
+					     &lo->bbatt_list.list[bb_offset]);
+		} else
+			loctl1 = loctl0;
+
+		tmp = ((u16) loctl0->q & 0xF);
+		tmp |= ((u16) loctl0->i & 0xF) << 4;
+		tmp |= ((u16) loctl1->q & 0xF) << 8;
+		tmp |= ((u16) loctl1->i & 0xF) << 12;	//FIXME?
+		b43_phy_write(dev, 0x3A0 + (i / 2), tmp);
+	}
+}
+
+static void hardware_pctl_init_aphy(struct b43_wldev *dev)
+{
+	//TODO
+}
+
+static void hardware_pctl_init_gphy(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+
+	b43_phy_write(dev, 0x0036, (b43_phy_read(dev, 0x0036) & 0xFFC0)
+		      | (phy->tgt_idle_tssi - phy->cur_idle_tssi));
+	b43_phy_write(dev, 0x0478, (b43_phy_read(dev, 0x0478) & 0xFF00)
+		      | (phy->tgt_idle_tssi - phy->cur_idle_tssi));
+	b43_gphy_tssi_power_lt_init(dev);
+	b43_gphy_gain_lt_init(dev);
+	b43_phy_write(dev, 0x0060, b43_phy_read(dev, 0x0060) & 0xFFBF);
+	b43_phy_write(dev, 0x0014, 0x0000);
+
+	B43_WARN_ON(phy->rev < 6);
+	b43_phy_write(dev, 0x0478, b43_phy_read(dev, 0x0478)
+		      | 0x0800);
+	b43_phy_write(dev, 0x0478, b43_phy_read(dev, 0x0478)
+		      & 0xFEFF);
+	b43_phy_write(dev, 0x0801, b43_phy_read(dev, 0x0801)
+		      & 0xFFBF);
+
+	b43_gphy_dc_lt_init(dev);
+}
+
+/* HardwarePowerControl init for A and G PHY */
+static void b43_hardware_pctl_init(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+
+	if (!b43_has_hardware_pctl(phy)) {
+		/* No hardware power control */
+		b43_hf_write(dev, b43_hf_read(dev) & ~B43_HF_HWPCTL);
+		return;
+	}
+	/* Init the hwpctl related hardware */
+	switch (phy->type) {
+	case B43_PHYTYPE_A:
+		hardware_pctl_init_aphy(dev);
+		break;
+	case B43_PHYTYPE_G:
+		hardware_pctl_init_gphy(dev);
+		break;
+	default:
+		B43_WARN_ON(1);
+	}
+	/* Enable hardware pctl in firmware. */
+	b43_hf_write(dev, b43_hf_read(dev) | B43_HF_HWPCTL);
+}
+
+static void b43_hardware_pctl_early_init(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+
+	if (!b43_has_hardware_pctl(phy)) {
+		b43_phy_write(dev, 0x047A, 0xC111);
+		return;
+	}
+
+	b43_phy_write(dev, 0x0036, b43_phy_read(dev, 0x0036) & 0xFEFF);
+	b43_phy_write(dev, 0x002F, 0x0202);
+	b43_phy_write(dev, 0x047C, b43_phy_read(dev, 0x047C) | 0x0002);
+	b43_phy_write(dev, 0x047A, b43_phy_read(dev, 0x047A) | 0xF000);
+	if (phy->radio_ver == 0x2050 && phy->radio_rev == 8) {
+		b43_phy_write(dev, 0x047A, (b43_phy_read(dev, 0x047A)
+					    & 0xFF0F) | 0x0010);
+		b43_phy_write(dev, 0x005D, b43_phy_read(dev, 0x005D)
+			      | 0x8000);
+		b43_phy_write(dev, 0x004E, (b43_phy_read(dev, 0x004E)
+					    & 0xFFC0) | 0x0010);
+		b43_phy_write(dev, 0x002E, 0xC07F);
+		b43_phy_write(dev, 0x0036, b43_phy_read(dev, 0x0036)
+			      | 0x0400);
+	} else {
+		b43_phy_write(dev, 0x0036, b43_phy_read(dev, 0x0036)
+			      | 0x0200);
+		b43_phy_write(dev, 0x0036, b43_phy_read(dev, 0x0036)
+			      | 0x0400);
+		b43_phy_write(dev, 0x005D, b43_phy_read(dev, 0x005D)
+			      & 0x7FFF);
+		b43_phy_write(dev, 0x004F, b43_phy_read(dev, 0x004F)
+			      & 0xFFFE);
+		b43_phy_write(dev, 0x004E, (b43_phy_read(dev, 0x004E)
+					    & 0xFFC0) | 0x0010);
+		b43_phy_write(dev, 0x002E, 0xC07F);
+		b43_phy_write(dev, 0x047A, (b43_phy_read(dev, 0x047A)
+					    & 0xFF0F) | 0x0010);
+	}
+}
+
+/* Intialize B/G PHY power control
+ * as described in http://bcm-specs.sipsolutions.net/InitPowerControl
+ */
+static void b43_phy_init_pctl(struct b43_wldev *dev)
+{
+	struct ssb_bus *bus = dev->dev->bus;
+	struct b43_phy *phy = &dev->phy;
+	struct b43_rfatt old_rfatt;
+	struct b43_bbatt old_bbatt;
+	u8 old_tx_control = 0;
+
+	if ((bus->boardinfo.vendor == SSB_BOARDVENDOR_BCM) &&
+	    (bus->boardinfo.type == SSB_BOARD_BU4306))
+		return;
+
+	b43_phy_write(dev, 0x0028, 0x8018);
+
+	/* This does something with the Analog... */
+	b43_write16(dev, B43_MMIO_PHY0, b43_read16(dev, B43_MMIO_PHY0)
+		    & 0xFFDF);
+
+	if (phy->type == B43_PHYTYPE_G && !phy->gmode)
+		return;
+	b43_hardware_pctl_early_init(dev);
+	if (phy->cur_idle_tssi == 0) {
+		if (phy->radio_ver == 0x2050 && phy->analog == 0) {
+			b43_radio_write16(dev, 0x0076,
+					  (b43_radio_read16(dev, 0x0076)
+					   & 0x00F7) | 0x0084);
+		} else {
+			struct b43_rfatt rfatt;
+			struct b43_bbatt bbatt;
+
+			memcpy(&old_rfatt, &phy->rfatt, sizeof(old_rfatt));
+			memcpy(&old_bbatt, &phy->bbatt, sizeof(old_bbatt));
+			old_tx_control = phy->tx_control;
+
+			bbatt.att = 11;
+			if (phy->radio_rev == 8) {
+				rfatt.att = 15;
+				rfatt.with_padmix = 1;
+			} else {
+				rfatt.att = 9;
+				rfatt.with_padmix = 0;
+			}
+			b43_set_txpower_g(dev, &bbatt, &rfatt, 0);
+		}
+		b43_dummy_transmission(dev);
+		phy->cur_idle_tssi = b43_phy_read(dev, B43_PHY_ITSSI);
+		if (B43_DEBUG) {
+			/* Current-Idle-TSSI sanity check. */
+			if (abs(phy->cur_idle_tssi - phy->tgt_idle_tssi) >= 20) {
+				b43dbg(dev->wl,
+				       "!WARNING! Idle-TSSI phy->cur_idle_tssi "
+				       "measuring failed. (cur=%d, tgt=%d). Disabling TX power "
+				       "adjustment.\n", phy->cur_idle_tssi,
+				       phy->tgt_idle_tssi);
+				phy->cur_idle_tssi = 0;
+			}
+		}
+		if (phy->radio_ver == 0x2050 && phy->analog == 0) {
+			b43_radio_write16(dev, 0x0076,
+					  b43_radio_read16(dev, 0x0076)
+					  & 0xFF7B);
+		} else {
+			b43_set_txpower_g(dev, &old_bbatt,
+					  &old_rfatt, old_tx_control);
+		}
+	}
+	b43_hardware_pctl_init(dev);
+	b43_shm_clear_tssi(dev);
+}
+
+static void b43_phy_agcsetup(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+	u16 offset = 0x0000;
+
+	if (phy->rev == 1)
+		offset = 0x4C00;
+
+	b43_ofdmtab_write16(dev, offset, 0, 0x00FE);
+	b43_ofdmtab_write16(dev, offset, 1, 0x000D);
+	b43_ofdmtab_write16(dev, offset, 2, 0x0013);
+	b43_ofdmtab_write16(dev, offset, 3, 0x0019);
+
+	if (phy->rev == 1) {
+		b43_ofdmtab_write16(dev, 0x1800, 0, 0x2710);
+		b43_ofdmtab_write16(dev, 0x1801, 0, 0x9B83);
+		b43_ofdmtab_write16(dev, 0x1802, 0, 0x9B83);
+		b43_ofdmtab_write16(dev, 0x1803, 0, 0x0F8D);
+		b43_phy_write(dev, 0x0455, 0x0004);
+	}
+
+	b43_phy_write(dev, 0x04A5, (b43_phy_read(dev, 0x04A5)
+				    & 0x00FF) | 0x5700);
+	b43_phy_write(dev, 0x041A, (b43_phy_read(dev, 0x041A)
+				    & 0xFF80) | 0x000F);
+	b43_phy_write(dev, 0x041A, (b43_phy_read(dev, 0x041A)
+				    & 0xC07F) | 0x2B80);
+	b43_phy_write(dev, 0x048C, (b43_phy_read(dev, 0x048C)
+				    & 0xF0FF) | 0x0300);
+
+	b43_radio_write16(dev, 0x007A, b43_radio_read16(dev, 0x007A)
+			  | 0x0008);
+
+	b43_phy_write(dev, 0x04A0, (b43_phy_read(dev, 0x04A0)
+				    & 0xFFF0) | 0x0008);
+	b43_phy_write(dev, 0x04A1, (b43_phy_read(dev, 0x04A1)
+				    & 0xF0FF) | 0x0600);
+	b43_phy_write(dev, 0x04A2, (b43_phy_read(dev, 0x04A2)
+				    & 0xF0FF) | 0x0700);
+	b43_phy_write(dev, 0x04A0, (b43_phy_read(dev, 0x04A0)
+				    & 0xF0FF) | 0x0100);
+
+	if (phy->rev == 1) {
+		b43_phy_write(dev, 0x04A2, (b43_phy_read(dev, 0x04A2)
+					    & 0xFFF0) | 0x0007);
+	}
+
+	b43_phy_write(dev, 0x0488, (b43_phy_read(dev, 0x0488)
+				    & 0xFF00) | 0x001C);
+	b43_phy_write(dev, 0x0488, (b43_phy_read(dev, 0x0488)
+				    & 0xC0FF) | 0x0200);
+	b43_phy_write(dev, 0x0496, (b43_phy_read(dev, 0x0496)
+				    & 0xFF00) | 0x001C);
+	b43_phy_write(dev, 0x0489, (b43_phy_read(dev, 0x0489)
+				    & 0xFF00) | 0x0020);
+	b43_phy_write(dev, 0x0489, (b43_phy_read(dev, 0x0489)
+				    & 0xC0FF) | 0x0200);
+	b43_phy_write(dev, 0x0482, (b43_phy_read(dev, 0x0482)
+				    & 0xFF00) | 0x002E);
+	b43_phy_write(dev, 0x0496, (b43_phy_read(dev, 0x0496)
+				    & 0x00FF) | 0x1A00);
+	b43_phy_write(dev, 0x0481, (b43_phy_read(dev, 0x0481)
+				    & 0xFF00) | 0x0028);
+	b43_phy_write(dev, 0x0481, (b43_phy_read(dev, 0x0481)
+				    & 0x00FF) | 0x2C00);
+
+	if (phy->rev == 1) {
+		b43_phy_write(dev, 0x0430, 0x092B);
+		b43_phy_write(dev, 0x041B, (b43_phy_read(dev, 0x041B)
+					    & 0xFFE1) | 0x0002);
+	} else {
+		b43_phy_write(dev, 0x041B, b43_phy_read(dev, 0x041B)
+			      & 0xFFE1);
+		b43_phy_write(dev, 0x041F, 0x287A);
+		b43_phy_write(dev, 0x0420, (b43_phy_read(dev, 0x0420)
+					    & 0xFFF0) | 0x0004);
+	}
+
+	if (phy->rev >= 6) {
+		b43_phy_write(dev, 0x0422, 0x287A);
+		b43_phy_write(dev, 0x0420, (b43_phy_read(dev, 0x0420)
+					    & 0x0FFF) | 0x3000);
+	}
+
+	b43_phy_write(dev, 0x04A8, (b43_phy_read(dev, 0x04A8)
+				    & 0x8080) | 0x7874);
+	b43_phy_write(dev, 0x048E, 0x1C00);
+
+	offset = 0x0800;
+	if (phy->rev == 1) {
+		offset = 0x5400;
+		b43_phy_write(dev, 0x04AB, (b43_phy_read(dev, 0x04AB)
+					    & 0xF0FF) | 0x0600);
+		b43_phy_write(dev, 0x048B, 0x005E);
+		b43_phy_write(dev, 0x048C, (b43_phy_read(dev, 0x048C)
+					    & 0xFF00) | 0x001E);
+		b43_phy_write(dev, 0x048D, 0x0002);
+	}
+	b43_ofdmtab_write16(dev, offset, 0, 0x00);
+	b43_ofdmtab_write16(dev, offset, 1, 0x07);
+	b43_ofdmtab_write16(dev, offset, 2, 0x10);
+	b43_ofdmtab_write16(dev, offset, 3, 0x1C);
+
+	if (phy->rev >= 6) {
+		b43_phy_write(dev, 0x0426, b43_phy_read(dev, 0x0426)
+			      & 0xFFFC);
+		b43_phy_write(dev, 0x0426, b43_phy_read(dev, 0x0426)
+			      & 0xEFFF);
+	}
+}
+
+static void b43_phy_setupg(struct b43_wldev *dev)
+{
+	struct ssb_bus *bus = dev->dev->bus;
+	struct b43_phy *phy = &dev->phy;
+	u16 i;
+
+	B43_WARN_ON(phy->type != B43_PHYTYPE_G);
+	if (phy->rev == 1) {
+		b43_phy_write(dev, 0x0406, 0x4F19);
+		b43_phy_write(dev, B43_PHY_G_CRS,
+			      (b43_phy_read(dev, B43_PHY_G_CRS) & 0xFC3F) |
+			      0x0340);
+		b43_phy_write(dev, 0x042C, 0x005A);
+		b43_phy_write(dev, 0x0427, 0x001A);
+
+		for (i = 0; i < B43_TAB_FINEFREQG_SIZE; i++)
+			b43_ofdmtab_write16(dev, 0x5800, i,
+					    b43_tab_finefreqg[i]);
+		for (i = 0; i < B43_TAB_NOISEG1_SIZE; i++)
+			b43_ofdmtab_write16(dev, 0x1800, i, b43_tab_noiseg1[i]);
+		for (i = 0; i < B43_TAB_ROTOR_SIZE; i++)
+			b43_ofdmtab_write16(dev, 0x2000, i, b43_tab_rotor[i]);
+	} else {
+		/* nrssi values are signed 6-bit values. Not sure why we write 0x7654 here... */
+		b43_nrssi_hw_write(dev, 0xBA98, (s16) 0x7654);
+
+		if (phy->rev == 2) {
+			b43_phy_write(dev, 0x04C0, 0x1861);
+			b43_phy_write(dev, 0x04C1, 0x0271);
+		} else if (phy->rev > 2) {
+			b43_phy_write(dev, 0x04C0, 0x0098);
+			b43_phy_write(dev, 0x04C1, 0x0070);
+			b43_phy_write(dev, 0x04C9, 0x0080);
+		}
+		b43_phy_write(dev, 0x042B, b43_phy_read(dev, 0x042B) | 0x800);
+
+		for (i = 0; i < 64; i++)
+			b43_ofdmtab_write16(dev, 0x4000, i, i);
+		for (i = 0; i < B43_TAB_NOISEG2_SIZE; i++)
+			b43_ofdmtab_write16(dev, 0x1800, i, b43_tab_noiseg2[i]);
+	}
+
+	if (phy->rev <= 2)
+		for (i = 0; i < B43_TAB_NOISESCALEG_SIZE; i++)
+			b43_ofdmtab_write16(dev, 0x1400, i,
+					    b43_tab_noisescaleg1[i]);
+	else if ((phy->rev >= 7) && (b43_phy_read(dev, 0x0449) & 0x0200))
+		for (i = 0; i < B43_TAB_NOISESCALEG_SIZE; i++)
+			b43_ofdmtab_write16(dev, 0x1400, i,
+					    b43_tab_noisescaleg3[i]);
+	else
+		for (i = 0; i < B43_TAB_NOISESCALEG_SIZE; i++)
+			b43_ofdmtab_write16(dev, 0x1400, i,
+					    b43_tab_noisescaleg2[i]);
+
+	if (phy->rev == 2)
+		for (i = 0; i < B43_TAB_SIGMASQR_SIZE; i++)
+			b43_ofdmtab_write16(dev, 0x5000, i,
+					    b43_tab_sigmasqr1[i]);
+	else if ((phy->rev > 2) && (phy->rev <= 8))
+		for (i = 0; i < B43_TAB_SIGMASQR_SIZE; i++)
+			b43_ofdmtab_write16(dev, 0x5000, i,
+					    b43_tab_sigmasqr2[i]);
+
+	if (phy->rev == 1) {
+		for (i = 0; i < B43_TAB_RETARD_SIZE; i++)
+			b43_ofdmtab_write32(dev, 0x2400, i, b43_tab_retard[i]);
+		for (i = 4; i < 20; i++)
+			b43_ofdmtab_write16(dev, 0x5400, i, 0x0020);
+		b43_phy_agcsetup(dev);
+
+		if ((bus->boardinfo.vendor == SSB_BOARDVENDOR_BCM) &&
+		    (bus->boardinfo.type == SSB_BOARD_BU4306) &&
+		    (bus->boardinfo.rev == 0x17))
+			return;
+
+		b43_ofdmtab_write16(dev, 0x5001, 0, 0x0002);
+		b43_ofdmtab_write16(dev, 0x5002, 0, 0x0001);
+	} else {
+		for (i = 0; i < 0x20; i++)
+			b43_ofdmtab_write16(dev, 0x1000, i, 0x0820);
+		b43_phy_agcsetup(dev);
+		b43_phy_read(dev, 0x0400);	/* dummy read */
+		b43_phy_write(dev, 0x0403, 0x1000);
+		b43_ofdmtab_write16(dev, 0x3C02, 0, 0x000F);
+		b43_ofdmtab_write16(dev, 0x3C03, 0, 0x0014);
+
+		if ((bus->boardinfo.vendor == SSB_BOARDVENDOR_BCM) &&
+		    (bus->boardinfo.type == SSB_BOARD_BU4306) &&
+		    (bus->boardinfo.rev == 0x17))
+			return;
+
+		b43_ofdmtab_write16(dev, 0x0401, 0, 0x0002);
+		b43_ofdmtab_write16(dev, 0x0402, 0, 0x0001);
+	}
+}
+
+/* Initialize the noisescaletable for APHY */
+static void b43_phy_init_noisescaletbl(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+	int i;
+
+	for (i = 0; i < 12; i++) {
+		if (phy->rev == 2)
+			b43_ofdmtab_write16(dev, 0x1400, i, 0x6767);
+		else
+			b43_ofdmtab_write16(dev, 0x1400, i, 0x2323);
+	}
+	if (phy->rev == 2)
+		b43_ofdmtab_write16(dev, 0x1400, i, 0x6700);
+	else
+		b43_ofdmtab_write16(dev, 0x1400, i, 0x2300);
+	for (i = 0; i < 11; i++) {
+		if (phy->rev == 2)
+			b43_ofdmtab_write16(dev, 0x1400, i, 0x6767);
+		else
+			b43_ofdmtab_write16(dev, 0x1400, i, 0x2323);
+	}
+	if (phy->rev == 2)
+		b43_ofdmtab_write16(dev, 0x1400, i, 0x0067);
+	else
+		b43_ofdmtab_write16(dev, 0x1400, i, 0x0023);
+}
+
+static void b43_phy_setupa(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+	u16 i;
+
+	B43_WARN_ON(phy->type != B43_PHYTYPE_A);
+	switch (phy->rev) {
+	case 2:
+		b43_phy_write(dev, 0x008E, 0x3800);
+		b43_phy_write(dev, 0x0035, 0x03FF);
+		b43_phy_write(dev, 0x0036, 0x0400);
+
+		b43_ofdmtab_write16(dev, 0x3807, 0, 0x0051);
+
+		b43_phy_write(dev, 0x001C, 0x0FF9);
+		b43_phy_write(dev, 0x0020, b43_phy_read(dev, 0x0020) & 0xFF0F);
+		b43_ofdmtab_write16(dev, 0x3C0C, 0, 0x07BF);
+		b43_radio_write16(dev, 0x0002, 0x07BF);
+
+		b43_phy_write(dev, 0x0024, 0x4680);
+		b43_phy_write(dev, 0x0020, 0x0003);
+		b43_phy_write(dev, 0x001D, 0x0F40);
+		b43_phy_write(dev, 0x001F, 0x1C00);
+
+		b43_phy_write(dev, 0x002A, (b43_phy_read(dev, 0x002A)
+					    & 0x00FF) | 0x0400);
+		b43_phy_write(dev, 0x002B, b43_phy_read(dev, 0x002B)
+			      & 0xFBFF);
+		b43_phy_write(dev, 0x008E, 0x58C1);
+
+		b43_ofdmtab_write16(dev, 0x0803, 0, 0x000F);
+		b43_ofdmtab_write16(dev, 0x0804, 0, 0x001F);
+		b43_ofdmtab_write16(dev, 0x0805, 0, 0x002A);
+		b43_ofdmtab_write16(dev, 0x0805, 0, 0x0030);
+		b43_ofdmtab_write16(dev, 0x0807, 0, 0x003A);
+
+		b43_ofdmtab_write16(dev, 0x0000, 0, 0x0013);
+		b43_ofdmtab_write16(dev, 0x0000, 1, 0x0013);
+		b43_ofdmtab_write16(dev, 0x0000, 2, 0x0013);
+		b43_ofdmtab_write16(dev, 0x0000, 3, 0x0013);
+		b43_ofdmtab_write16(dev, 0x0000, 4, 0x0015);
+		b43_ofdmtab_write16(dev, 0x0000, 5, 0x0015);
+		b43_ofdmtab_write16(dev, 0x0000, 6, 0x0019);
+
+		b43_ofdmtab_write16(dev, 0x0404, 0, 0x0003);
+		b43_ofdmtab_write16(dev, 0x0405, 0, 0x0003);
+		b43_ofdmtab_write16(dev, 0x0406, 0, 0x0007);
+
+		for (i = 0; i < 16; i++)
+			b43_ofdmtab_write16(dev, 0x4000, i, (0x8 + i) & 0x000F);
+
+		b43_ofdmtab_write16(dev, 0x3003, 0, 0x1044);
+		b43_ofdmtab_write16(dev, 0x3004, 0, 0x7201);
+		b43_ofdmtab_write16(dev, 0x3006, 0, 0x0040);
+		b43_ofdmtab_write16(dev, 0x3001, 0,
+				    (b43_ofdmtab_read16(dev, 0x3001, 0) &
+				     0x0010) | 0x0008);
+
+		for (i = 0; i < B43_TAB_FINEFREQA_SIZE; i++)
+			b43_ofdmtab_write16(dev, 0x5800, i,
+					    b43_tab_finefreqa[i]);
+		for (i = 0; i < B43_TAB_NOISEA2_SIZE; i++)
+			b43_ofdmtab_write16(dev, 0x1800, i, b43_tab_noisea2[i]);
+		for (i = 0; i < B43_TAB_ROTOR_SIZE; i++)
+			b43_ofdmtab_write32(dev, 0x2000, i, b43_tab_rotor[i]);
+		b43_phy_init_noisescaletbl(dev);
+		for (i = 0; i < B43_TAB_RETARD_SIZE; i++)
+			b43_ofdmtab_write32(dev, 0x2400, i, b43_tab_retard[i]);
+		break;
+	case 3:
+		for (i = 0; i < 64; i++)
+			b43_ofdmtab_write16(dev, 0x4000, i, i);
+
+		b43_ofdmtab_write16(dev, 0x3807, 0, 0x0051);
+
+		b43_phy_write(dev, 0x001C, 0x0FF9);
+		b43_phy_write(dev, 0x0020, b43_phy_read(dev, 0x0020) & 0xFF0F);
+		b43_radio_write16(dev, 0x0002, 0x07BF);
+
+		b43_phy_write(dev, 0x0024, 0x4680);
+		b43_phy_write(dev, 0x0020, 0x0003);
+		b43_phy_write(dev, 0x001D, 0x0F40);
+		b43_phy_write(dev, 0x001F, 0x1C00);
+		b43_phy_write(dev, 0x002A, (b43_phy_read(dev, 0x002A)
+					    & 0x00FF) | 0x0400);
+
+		b43_ofdmtab_write16(dev, 0x3000, 1,
+				    (b43_ofdmtab_read16(dev, 0x3000, 1)
+				     & 0x0010) | 0x0008);
+		for (i = 0; i < B43_TAB_NOISEA3_SIZE; i++) {
+			b43_ofdmtab_write16(dev, 0x1800, i, b43_tab_noisea3[i]);
+		}
+		b43_phy_init_noisescaletbl(dev);
+		for (i = 0; i < B43_TAB_SIGMASQR_SIZE; i++) {
+			b43_ofdmtab_write16(dev, 0x5000, i,
+					    b43_tab_sigmasqr1[i]);
+		}
+
+		b43_phy_write(dev, 0x0003, 0x1808);
+
+		b43_ofdmtab_write16(dev, 0x0803, 0, 0x000F);
+		b43_ofdmtab_write16(dev, 0x0804, 0, 0x001F);
+		b43_ofdmtab_write16(dev, 0x0805, 0, 0x002A);
+		b43_ofdmtab_write16(dev, 0x0805, 0, 0x0030);
+		b43_ofdmtab_write16(dev, 0x0807, 0, 0x003A);
+
+		b43_ofdmtab_write16(dev, 0x0000, 0, 0x0013);
+		b43_ofdmtab_write16(dev, 0x0001, 0, 0x0013);
+		b43_ofdmtab_write16(dev, 0x0002, 0, 0x0013);
+		b43_ofdmtab_write16(dev, 0x0003, 0, 0x0013);
+		b43_ofdmtab_write16(dev, 0x0004, 0, 0x0015);
+		b43_ofdmtab_write16(dev, 0x0005, 0, 0x0015);
+		b43_ofdmtab_write16(dev, 0x0006, 0, 0x0019);
+
+		b43_ofdmtab_write16(dev, 0x0404, 0, 0x0003);
+		b43_ofdmtab_write16(dev, 0x0405, 0, 0x0003);
+		b43_ofdmtab_write16(dev, 0x0406, 0, 0x0007);
+
+		b43_ofdmtab_write16(dev, 0x3C02, 0, 0x000F);
+		b43_ofdmtab_write16(dev, 0x3C03, 0, 0x0014);
+		break;
+	default:
+		B43_WARN_ON(1);
+	}
+}
+
+/* Initialize APHY. This is also called for the GPHY in some cases. */
+static void b43_phy_inita(struct b43_wldev *dev)
+{
+	struct ssb_bus *bus = dev->dev->bus;
+	struct b43_phy *phy = &dev->phy;
+	u16 tval;
+
+	might_sleep();
+
+	if (phy->type == B43_PHYTYPE_A) {
+		b43_phy_setupa(dev);
+	} else {
+		b43_phy_setupg(dev);
+		if (phy->gmode &&
+		    (dev->dev->bus->sprom.r1.boardflags_lo & B43_BFL_PACTRL))
+			b43_phy_write(dev, 0x046E, 0x03CF);
+		return;
+	}
+
+	b43_phy_write(dev, B43_PHY_A_CRS,
+		      (b43_phy_read(dev, B43_PHY_A_CRS) & 0xF83C) | 0x0340);
+	b43_phy_write(dev, 0x0034, 0x0001);
+
+	//TODO: RSSI AGC
+	b43_phy_write(dev, B43_PHY_A_CRS,
+		      b43_phy_read(dev, B43_PHY_A_CRS) | (1 << 14));
+	b43_radio_init2060(dev);
+
+	if ((bus->boardinfo.vendor == SSB_BOARDVENDOR_BCM) &&
+	    ((bus->boardinfo.type == SSB_BOARD_BU4306) ||
+	     (bus->boardinfo.type == SSB_BOARD_BU4309))) {
+		if (phy->lofcal == 0xFFFF) {
+			//TODO: LOF Cal
+			b43_radio_set_tx_iq(dev);
+		} else
+			b43_radio_write16(dev, 0x001E, phy->lofcal);
+	}
+
+	b43_phy_write(dev, 0x007A, 0xF111);
+
+	if (phy->cur_idle_tssi == 0) {
+		b43_radio_write16(dev, 0x0019, 0x0000);
+		b43_radio_write16(dev, 0x0017, 0x0020);
+
+		tval = b43_ofdmtab_read16(dev, 0x3001, 0);
+		if (phy->rev == 1) {
+			b43_ofdmtab_write16(dev, 0x3001, 0,
+					    (b43_ofdmtab_read16(dev, 0x3001, 0)
+					     & 0xFF87)
+					    | 0x0058);
+		} else {
+			b43_ofdmtab_write16(dev, 0x3001, 0,
+					    (b43_ofdmtab_read16(dev, 0x3001, 0)
+					     & 0xFFC3)
+					    | 0x002C);
+		}
+		b43_dummy_transmission(dev);
+		phy->cur_idle_tssi = b43_phy_read(dev, B43_PHY_A_PCTL);
+		b43_ofdmtab_write16(dev, 0x3001, 0, tval);
+
+		b43_radio_set_txpower_a(dev, 0x0018);
+	}
+	b43_shm_clear_tssi(dev);
+}
+
+static void b43_phy_initb2(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+	u16 offset, val;
+
+	b43_write16(dev, 0x03EC, 0x3F22);
+	b43_phy_write(dev, 0x0020, 0x301C);
+	b43_phy_write(dev, 0x0026, 0x0000);
+	b43_phy_write(dev, 0x0030, 0x00C6);
+	b43_phy_write(dev, 0x0088, 0x3E00);
+	val = 0x3C3D;
+	for (offset = 0x0089; offset < 0x00A7; offset++) {
+		b43_phy_write(dev, offset, val);
+		val -= 0x0202;
+	}
+	b43_phy_write(dev, 0x03E4, 0x3000);
+	b43_radio_selectchannel(dev, phy->channel, 0);
+	if (phy->radio_ver != 0x2050) {
+		b43_radio_write16(dev, 0x0075, 0x0080);
+		b43_radio_write16(dev, 0x0079, 0x0081);
+	}
+	b43_radio_write16(dev, 0x0050, 0x0020);
+	b43_radio_write16(dev, 0x0050, 0x0023);
+	if (phy->radio_ver == 0x2050) {
+		b43_radio_write16(dev, 0x0050, 0x0020);
+		b43_radio_write16(dev, 0x005A, 0x0070);
+		b43_radio_write16(dev, 0x005B, 0x007B);
+		b43_radio_write16(dev, 0x005C, 0x00B0);
+		b43_radio_write16(dev, 0x007A, 0x000F);
+		b43_phy_write(dev, 0x0038, 0x0677);
+		b43_radio_init2050(dev);
+	}
+	b43_phy_write(dev, 0x0014, 0x0080);
+	b43_phy_write(dev, 0x0032, 0x00CA);
+	b43_phy_write(dev, 0x0032, 0x00CC);
+	b43_phy_write(dev, 0x0035, 0x07C2);
+	b43_lo_b_measure(dev);
+	b43_phy_write(dev, 0x0026, 0xCC00);
+	if (phy->radio_ver != 0x2050)
+		b43_phy_write(dev, 0x0026, 0xCE00);
+	b43_write16(dev, B43_MMIO_CHANNEL_EXT, 0x1000);
+	b43_phy_write(dev, 0x002A, 0x88A3);
+	if (phy->radio_ver != 0x2050)
+		b43_phy_write(dev, 0x002A, 0x88C2);
+	b43_set_txpower_g(dev, &phy->bbatt, &phy->rfatt, phy->tx_control);
+	b43_phy_init_pctl(dev);
+}
+
+static void b43_phy_initb4(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+	u16 offset, val;
+
+	b43_write16(dev, 0x03EC, 0x3F22);
+	b43_phy_write(dev, 0x0020, 0x301C);
+	b43_phy_write(dev, 0x0026, 0x0000);
+	b43_phy_write(dev, 0x0030, 0x00C6);
+	b43_phy_write(dev, 0x0088, 0x3E00);
+	val = 0x3C3D;
+	for (offset = 0x0089; offset < 0x00A7; offset++) {
+		b43_phy_write(dev, offset, val);
+		val -= 0x0202;
+	}
+	b43_phy_write(dev, 0x03E4, 0x3000);
+	b43_radio_selectchannel(dev, phy->channel, 0);
+	if (phy->radio_ver != 0x2050) {
+		b43_radio_write16(dev, 0x0075, 0x0080);
+		b43_radio_write16(dev, 0x0079, 0x0081);
+	}
+	b43_radio_write16(dev, 0x0050, 0x0020);
+	b43_radio_write16(dev, 0x0050, 0x0023);
+	if (phy->radio_ver == 0x2050) {
+		b43_radio_write16(dev, 0x0050, 0x0020);
+		b43_radio_write16(dev, 0x005A, 0x0070);
+		b43_radio_write16(dev, 0x005B, 0x007B);
+		b43_radio_write16(dev, 0x005C, 0x00B0);
+		b43_radio_write16(dev, 0x007A, 0x000F);
+		b43_phy_write(dev, 0x0038, 0x0677);
+		b43_radio_init2050(dev);
+	}
+	b43_phy_write(dev, 0x0014, 0x0080);
+	b43_phy_write(dev, 0x0032, 0x00CA);
+	if (phy->radio_ver == 0x2050)
+		b43_phy_write(dev, 0x0032, 0x00E0);
+	b43_phy_write(dev, 0x0035, 0x07C2);
+
+	b43_lo_b_measure(dev);
+
+	b43_phy_write(dev, 0x0026, 0xCC00);
+	if (phy->radio_ver == 0x2050)
+		b43_phy_write(dev, 0x0026, 0xCE00);
+	b43_write16(dev, B43_MMIO_CHANNEL_EXT, 0x1100);
+	b43_phy_write(dev, 0x002A, 0x88A3);
+	if (phy->radio_ver == 0x2050)
+		b43_phy_write(dev, 0x002A, 0x88C2);
+	b43_set_txpower_g(dev, &phy->bbatt, &phy->rfatt, phy->tx_control);
+	if (dev->dev->bus->sprom.r1.boardflags_lo & B43_BFL_RSSI) {
+		b43_calc_nrssi_slope(dev);
+		b43_calc_nrssi_threshold(dev);
+	}
+	b43_phy_init_pctl(dev);
+}
+
+static void b43_phy_initb5(struct b43_wldev *dev)
+{
+	struct ssb_bus *bus = dev->dev->bus;
+	struct b43_phy *phy = &dev->phy;
+	u16 offset, value;
+	u8 old_channel;
+
+	if (phy->analog == 1) {
+		b43_radio_write16(dev, 0x007A, b43_radio_read16(dev, 0x007A)
+				  | 0x0050);
+	}
+	if ((bus->boardinfo.vendor != SSB_BOARDVENDOR_BCM) &&
+	    (bus->boardinfo.type != SSB_BOARD_BU4306)) {
+		value = 0x2120;
+		for (offset = 0x00A8; offset < 0x00C7; offset++) {
+			b43_phy_write(dev, offset, value);
+			value += 0x202;
+		}
+	}
+	b43_phy_write(dev, 0x0035, (b43_phy_read(dev, 0x0035) & 0xF0FF)
+		      | 0x0700);
+	if (phy->radio_ver == 0x2050)
+		b43_phy_write(dev, 0x0038, 0x0667);
+
+	if (phy->gmode || phy->rev >= 2) {
+		if (phy->radio_ver == 0x2050) {
+			b43_radio_write16(dev, 0x007A,
+					  b43_radio_read16(dev, 0x007A)
+					  | 0x0020);
+			b43_radio_write16(dev, 0x0051,
+					  b43_radio_read16(dev, 0x0051)
+					  | 0x0004);
+		}
+		b43_write16(dev, B43_MMIO_PHY_RADIO, 0x0000);
+
+		b43_phy_write(dev, 0x0802, b43_phy_read(dev, 0x0802) | 0x0100);
+		b43_phy_write(dev, 0x042B, b43_phy_read(dev, 0x042B) | 0x2000);
+
+		b43_phy_write(dev, 0x001C, 0x186A);
+
+		b43_phy_write(dev, 0x0013,
+			      (b43_phy_read(dev, 0x0013) & 0x00FF) | 0x1900);
+		b43_phy_write(dev, 0x0035,
+			      (b43_phy_read(dev, 0x0035) & 0xFFC0) | 0x0064);
+		b43_phy_write(dev, 0x005D,
+			      (b43_phy_read(dev, 0x005D) & 0xFF80) | 0x000A);
+	}
+
+	if (dev->bad_frames_preempt) {
+		b43_phy_write(dev, B43_PHY_RADIO_BITFIELD,
+			      b43_phy_read(dev,
+					   B43_PHY_RADIO_BITFIELD) | (1 << 11));
+	}
+
+	if (phy->analog == 1) {
+		b43_phy_write(dev, 0x0026, 0xCE00);
+		b43_phy_write(dev, 0x0021, 0x3763);
+		b43_phy_write(dev, 0x0022, 0x1BC3);
+		b43_phy_write(dev, 0x0023, 0x06F9);
+		b43_phy_write(dev, 0x0024, 0x037E);
+	} else
+		b43_phy_write(dev, 0x0026, 0xCC00);
+	b43_phy_write(dev, 0x0030, 0x00C6);
+	b43_write16(dev, 0x03EC, 0x3F22);
+
+	if (phy->analog == 1)
+		b43_phy_write(dev, 0x0020, 0x3E1C);
+	else
+		b43_phy_write(dev, 0x0020, 0x301C);
+
+	if (phy->analog == 0)
+		b43_write16(dev, 0x03E4, 0x3000);
+
+	old_channel = phy->channel;
+	/* Force to channel 7, even if not supported. */
+	b43_radio_selectchannel(dev, 7, 0);
+
+	if (phy->radio_ver != 0x2050) {
+		b43_radio_write16(dev, 0x0075, 0x0080);
+		b43_radio_write16(dev, 0x0079, 0x0081);
+	}
+
+	b43_radio_write16(dev, 0x0050, 0x0020);
+	b43_radio_write16(dev, 0x0050, 0x0023);
+
+	if (phy->radio_ver == 0x2050) {
+		b43_radio_write16(dev, 0x0050, 0x0020);
+		b43_radio_write16(dev, 0x005A, 0x0070);
+	}
+
+	b43_radio_write16(dev, 0x005B, 0x007B);
+	b43_radio_write16(dev, 0x005C, 0x00B0);
+
+	b43_radio_write16(dev, 0x007A, b43_radio_read16(dev, 0x007A) | 0x0007);
+
+	b43_radio_selectchannel(dev, old_channel, 0);
+
+	b43_phy_write(dev, 0x0014, 0x0080);
+	b43_phy_write(dev, 0x0032, 0x00CA);
+	b43_phy_write(dev, 0x002A, 0x88A3);
+
+	b43_set_txpower_g(dev, &phy->bbatt, &phy->rfatt, phy->tx_control);
+
+	if (phy->radio_ver == 0x2050)
+		b43_radio_write16(dev, 0x005D, 0x000D);
+
+	b43_write16(dev, 0x03E4, (b43_read16(dev, 0x03E4) & 0xFFC0) | 0x0004);
+}
+
+static void b43_phy_initb6(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+	u16 offset, val;
+	u8 old_channel;
+
+	b43_phy_write(dev, 0x003E, 0x817A);
+	b43_radio_write16(dev, 0x007A,
+			  (b43_radio_read16(dev, 0x007A) | 0x0058));
+	if (phy->radio_rev == 4 || phy->radio_rev == 5) {
+		b43_radio_write16(dev, 0x51, 0x37);
+		b43_radio_write16(dev, 0x52, 0x70);
+		b43_radio_write16(dev, 0x53, 0xB3);
+		b43_radio_write16(dev, 0x54, 0x9B);
+		b43_radio_write16(dev, 0x5A, 0x88);
+		b43_radio_write16(dev, 0x5B, 0x88);
+		b43_radio_write16(dev, 0x5D, 0x88);
+		b43_radio_write16(dev, 0x5E, 0x88);
+		b43_radio_write16(dev, 0x7D, 0x88);
+		b43_hf_write(dev, b43_hf_read(dev)
+			     | B43_HF_TSSIRPSMW);
+	}
+	B43_WARN_ON(phy->radio_rev == 6 || phy->radio_rev == 7);	/* We had code for these revs here... */
+	if (phy->radio_rev == 8) {
+		b43_radio_write16(dev, 0x51, 0);
+		b43_radio_write16(dev, 0x52, 0x40);
+		b43_radio_write16(dev, 0x53, 0xB7);
+		b43_radio_write16(dev, 0x54, 0x98);
+		b43_radio_write16(dev, 0x5A, 0x88);
+		b43_radio_write16(dev, 0x5B, 0x6B);
+		b43_radio_write16(dev, 0x5C, 0x0F);
+		if (dev->dev->bus->sprom.r1.boardflags_lo & B43_BFL_ALTIQ) {
+			b43_radio_write16(dev, 0x5D, 0xFA);
+			b43_radio_write16(dev, 0x5E, 0xD8);
+		} else {
+			b43_radio_write16(dev, 0x5D, 0xF5);
+			b43_radio_write16(dev, 0x5E, 0xB8);
+		}
+		b43_radio_write16(dev, 0x0073, 0x0003);
+		b43_radio_write16(dev, 0x007D, 0x00A8);
+		b43_radio_write16(dev, 0x007C, 0x0001);
+		b43_radio_write16(dev, 0x007E, 0x0008);
+	}
+	val = 0x1E1F;
+	for (offset = 0x0088; offset < 0x0098; offset++) {
+		b43_phy_write(dev, offset, val);
+		val -= 0x0202;
+	}
+	val = 0x3E3F;
+	for (offset = 0x0098; offset < 0x00A8; offset++) {
+		b43_phy_write(dev, offset, val);
+		val -= 0x0202;
+	}
+	val = 0x2120;
+	for (offset = 0x00A8; offset < 0x00C8; offset++) {
+		b43_phy_write(dev, offset, (val & 0x3F3F));
+		val += 0x0202;
+	}
+	if (phy->type == B43_PHYTYPE_G) {
+		b43_radio_write16(dev, 0x007A,
+				  b43_radio_read16(dev, 0x007A) | 0x0020);
+		b43_radio_write16(dev, 0x0051,
+				  b43_radio_read16(dev, 0x0051) | 0x0004);
+		b43_phy_write(dev, 0x0802, b43_phy_read(dev, 0x0802) | 0x0100);
+		b43_phy_write(dev, 0x042B, b43_phy_read(dev, 0x042B) | 0x2000);
+		b43_phy_write(dev, 0x5B, 0);
+		b43_phy_write(dev, 0x5C, 0);
+	}
+
+	old_channel = phy->channel;
+	if (old_channel >= 8)
+		b43_radio_selectchannel(dev, 1, 0);
+	else
+		b43_radio_selectchannel(dev, 13, 0);
+
+	b43_radio_write16(dev, 0x0050, 0x0020);
+	b43_radio_write16(dev, 0x0050, 0x0023);
+	udelay(40);
+	if (phy->radio_rev < 6 || phy->radio_rev == 8) {
+		b43_radio_write16(dev, 0x7C, (b43_radio_read16(dev, 0x7C)
+					      | 0x0002));
+		b43_radio_write16(dev, 0x50, 0x20);
+	}
+	if (phy->radio_rev <= 2) {
+		b43_radio_write16(dev, 0x7C, 0x20);
+		b43_radio_write16(dev, 0x5A, 0x70);
+		b43_radio_write16(dev, 0x5B, 0x7B);
+		b43_radio_write16(dev, 0x5C, 0xB0);
+	}
+	b43_radio_write16(dev, 0x007A,
+			  (b43_radio_read16(dev, 0x007A) & 0x00F8) | 0x0007);
+
+	b43_radio_selectchannel(dev, old_channel, 0);
+
+	b43_phy_write(dev, 0x0014, 0x0200);
+	if (phy->radio_rev >= 6)
+		b43_phy_write(dev, 0x2A, 0x88C2);
+	else
+		b43_phy_write(dev, 0x2A, 0x8AC0);
+	b43_phy_write(dev, 0x0038, 0x0668);
+	b43_set_txpower_g(dev, &phy->bbatt, &phy->rfatt, phy->tx_control);
+	if (phy->radio_rev <= 5) {
+		b43_phy_write(dev, 0x5D, (b43_phy_read(dev, 0x5D)
+					  & 0xFF80) | 0x0003);
+	}
+	if (phy->radio_rev <= 2)
+		b43_radio_write16(dev, 0x005D, 0x000D);
+
+	if (phy->analog == 4) {
+		b43_write16(dev, 0x3E4, 9);
+		b43_phy_write(dev, 0x61, b43_phy_read(dev, 0x61)
+			      & 0x0FFF);
+	} else {
+		b43_phy_write(dev, 0x0002, (b43_phy_read(dev, 0x0002) & 0xFFC0)
+			      | 0x0004);
+	}
+	if (phy->type == B43_PHYTYPE_B) {
+		b43_write16(dev, 0x03E6, 0x8140);
+		b43_phy_write(dev, 0x0016, 0x0410);
+		b43_phy_write(dev, 0x0017, 0x0820);
+		b43_phy_write(dev, 0x0062, 0x0007);
+		b43_radio_init2050(dev);
+		b43_lo_g_measure(dev);
+		if (dev->dev->bus->sprom.r1.boardflags_lo & B43_BFL_RSSI) {
+			b43_calc_nrssi_slope(dev);
+			b43_calc_nrssi_threshold(dev);
+		}
+		b43_phy_init_pctl(dev);
+	} else if (phy->type == B43_PHYTYPE_G)
+		b43_write16(dev, 0x03E6, 0x0);
+}
+
+static void b43_calc_loopback_gain(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+	u16 backup_phy[16] = { 0 };
+	u16 backup_radio[3];
+	u16 backup_bband;
+	u16 i, j, loop_i_max;
+	u16 trsw_rx;
+	u16 loop1_outer_done, loop1_inner_done;
+
+	backup_phy[0] = b43_phy_read(dev, B43_PHY_CRS0);
+	backup_phy[1] = b43_phy_read(dev, B43_PHY_CCKBBANDCFG);
+	backup_phy[2] = b43_phy_read(dev, B43_PHY_RFOVER);
+	backup_phy[3] = b43_phy_read(dev, B43_PHY_RFOVERVAL);
+	if (phy->rev != 1) {	/* Not in specs, but needed to prevent PPC machine check */
+		backup_phy[4] = b43_phy_read(dev, B43_PHY_ANALOGOVER);
+		backup_phy[5] = b43_phy_read(dev, B43_PHY_ANALOGOVERVAL);
+	}
+	backup_phy[6] = b43_phy_read(dev, B43_PHY_BASE(0x5A));
+	backup_phy[7] = b43_phy_read(dev, B43_PHY_BASE(0x59));
+	backup_phy[8] = b43_phy_read(dev, B43_PHY_BASE(0x58));
+	backup_phy[9] = b43_phy_read(dev, B43_PHY_BASE(0x0A));
+	backup_phy[10] = b43_phy_read(dev, B43_PHY_BASE(0x03));
+	backup_phy[11] = b43_phy_read(dev, B43_PHY_LO_MASK);
+	backup_phy[12] = b43_phy_read(dev, B43_PHY_LO_CTL);
+	backup_phy[13] = b43_phy_read(dev, B43_PHY_BASE(0x2B));
+	backup_phy[14] = b43_phy_read(dev, B43_PHY_PGACTL);
+	backup_phy[15] = b43_phy_read(dev, B43_PHY_LO_LEAKAGE);
+	backup_bband = phy->bbatt.att;
+	backup_radio[0] = b43_radio_read16(dev, 0x52);
+	backup_radio[1] = b43_radio_read16(dev, 0x43);
+	backup_radio[2] = b43_radio_read16(dev, 0x7A);
+
+	b43_phy_write(dev, B43_PHY_CRS0,
+		      b43_phy_read(dev, B43_PHY_CRS0) & 0x3FFF);
+	b43_phy_write(dev, B43_PHY_CCKBBANDCFG,
+		      b43_phy_read(dev, B43_PHY_CCKBBANDCFG) | 0x8000);
+	b43_phy_write(dev, B43_PHY_RFOVER,
+		      b43_phy_read(dev, B43_PHY_RFOVER) | 0x0002);
+	b43_phy_write(dev, B43_PHY_RFOVERVAL,
+		      b43_phy_read(dev, B43_PHY_RFOVERVAL) & 0xFFFD);
+	b43_phy_write(dev, B43_PHY_RFOVER,
+		      b43_phy_read(dev, B43_PHY_RFOVER) | 0x0001);
+	b43_phy_write(dev, B43_PHY_RFOVERVAL,
+		      b43_phy_read(dev, B43_PHY_RFOVERVAL) & 0xFFFE);
+	if (phy->rev != 1) {	/* Not in specs, but needed to prevent PPC machine check */
+		b43_phy_write(dev, B43_PHY_ANALOGOVER,
+			      b43_phy_read(dev, B43_PHY_ANALOGOVER) | 0x0001);
+		b43_phy_write(dev, B43_PHY_ANALOGOVERVAL,
+			      b43_phy_read(dev,
+					   B43_PHY_ANALOGOVERVAL) & 0xFFFE);
+		b43_phy_write(dev, B43_PHY_ANALOGOVER,
+			      b43_phy_read(dev, B43_PHY_ANALOGOVER) | 0x0002);
+		b43_phy_write(dev, B43_PHY_ANALOGOVERVAL,
+			      b43_phy_read(dev,
+					   B43_PHY_ANALOGOVERVAL) & 0xFFFD);
+	}
+	b43_phy_write(dev, B43_PHY_RFOVER,
+		      b43_phy_read(dev, B43_PHY_RFOVER) | 0x000C);
+	b43_phy_write(dev, B43_PHY_RFOVERVAL,
+		      b43_phy_read(dev, B43_PHY_RFOVERVAL) | 0x000C);
+	b43_phy_write(dev, B43_PHY_RFOVER,
+		      b43_phy_read(dev, B43_PHY_RFOVER) | 0x0030);
+	b43_phy_write(dev, B43_PHY_RFOVERVAL,
+		      (b43_phy_read(dev, B43_PHY_RFOVERVAL)
+		       & 0xFFCF) | 0x10);
+
+	b43_phy_write(dev, B43_PHY_BASE(0x5A), 0x0780);
+	b43_phy_write(dev, B43_PHY_BASE(0x59), 0xC810);
+	b43_phy_write(dev, B43_PHY_BASE(0x58), 0x000D);
+
+	b43_phy_write(dev, B43_PHY_BASE(0x0A),
+		      b43_phy_read(dev, B43_PHY_BASE(0x0A)) | 0x2000);
+	if (phy->rev != 1) {	/* Not in specs, but needed to prevent PPC machine check */
+		b43_phy_write(dev, B43_PHY_ANALOGOVER,
+			      b43_phy_read(dev, B43_PHY_ANALOGOVER) | 0x0004);
+		b43_phy_write(dev, B43_PHY_ANALOGOVERVAL,
+			      b43_phy_read(dev,
+					   B43_PHY_ANALOGOVERVAL) & 0xFFFB);
+	}
+	b43_phy_write(dev, B43_PHY_BASE(0x03),
+		      (b43_phy_read(dev, B43_PHY_BASE(0x03))
+		       & 0xFF9F) | 0x40);
+
+	if (phy->radio_rev == 8) {
+		b43_radio_write16(dev, 0x43, 0x000F);
+	} else {
+		b43_radio_write16(dev, 0x52, 0);
+		b43_radio_write16(dev, 0x43, (b43_radio_read16(dev, 0x43)
+					      & 0xFFF0) | 0x9);
+	}
+	b43_phy_set_baseband_attenuation(dev, 11);
+
+	if (phy->rev >= 3)
+		b43_phy_write(dev, B43_PHY_LO_MASK, 0xC020);
+	else
+		b43_phy_write(dev, B43_PHY_LO_MASK, 0x8020);
+	b43_phy_write(dev, B43_PHY_LO_CTL, 0);
+
+	b43_phy_write(dev, B43_PHY_BASE(0x2B),
+		      (b43_phy_read(dev, B43_PHY_BASE(0x2B))
+		       & 0xFFC0) | 0x01);
+	b43_phy_write(dev, B43_PHY_BASE(0x2B),
+		      (b43_phy_read(dev, B43_PHY_BASE(0x2B))
+		       & 0xC0FF) | 0x800);
+
+	b43_phy_write(dev, B43_PHY_RFOVER,
+		      b43_phy_read(dev, B43_PHY_RFOVER) | 0x0100);
+	b43_phy_write(dev, B43_PHY_RFOVERVAL,
+		      b43_phy_read(dev, B43_PHY_RFOVERVAL) & 0xCFFF);
+
+	if (dev->dev->bus->sprom.r1.boardflags_lo & B43_BFL_EXTLNA) {
+		if (phy->rev >= 7) {
+			b43_phy_write(dev, B43_PHY_RFOVER,
+				      b43_phy_read(dev, B43_PHY_RFOVER)
+				      | 0x0800);
+			b43_phy_write(dev, B43_PHY_RFOVERVAL,
+				      b43_phy_read(dev, B43_PHY_RFOVERVAL)
+				      | 0x8000);
+		}
+	}
+	b43_radio_write16(dev, 0x7A, b43_radio_read16(dev, 0x7A)
+			  & 0x00F7);
+
+	j = 0;
+	loop_i_max = (phy->radio_rev == 8) ? 15 : 9;
+	for (i = 0; i < loop_i_max; i++) {
+		for (j = 0; j < 16; j++) {
+			b43_radio_write16(dev, 0x43, i);
+			b43_phy_write(dev, B43_PHY_RFOVERVAL,
+				      (b43_phy_read(dev, B43_PHY_RFOVERVAL)
+				       & 0xF0FF) | (j << 8));
+			b43_phy_write(dev, B43_PHY_PGACTL,
+				      (b43_phy_read(dev, B43_PHY_PGACTL)
+				       & 0x0FFF) | 0xA000);
+			b43_phy_write(dev, B43_PHY_PGACTL,
+				      b43_phy_read(dev, B43_PHY_PGACTL)
+				      | 0xF000);
+			udelay(20);
+			if (b43_phy_read(dev, B43_PHY_LO_LEAKAGE) >= 0xDFC)
+				goto exit_loop1;
+		}
+	}
+      exit_loop1:
+	loop1_outer_done = i;
+	loop1_inner_done = j;
+	if (j >= 8) {
+		b43_phy_write(dev, B43_PHY_RFOVERVAL,
+			      b43_phy_read(dev, B43_PHY_RFOVERVAL)
+			      | 0x30);
+		trsw_rx = 0x1B;
+		for (j = j - 8; j < 16; j++) {
+			b43_phy_write(dev, B43_PHY_RFOVERVAL,
+				      (b43_phy_read(dev, B43_PHY_RFOVERVAL)
+				       & 0xF0FF) | (j << 8));
+			b43_phy_write(dev, B43_PHY_PGACTL,
+				      (b43_phy_read(dev, B43_PHY_PGACTL)
+				       & 0x0FFF) | 0xA000);
+			b43_phy_write(dev, B43_PHY_PGACTL,
+				      b43_phy_read(dev, B43_PHY_PGACTL)
+				      | 0xF000);
+			udelay(20);
+			trsw_rx -= 3;
+			if (b43_phy_read(dev, B43_PHY_LO_LEAKAGE) >= 0xDFC)
+				goto exit_loop2;
+		}
+	} else
+		trsw_rx = 0x18;
+      exit_loop2:
+
+	if (phy->rev != 1) {	/* Not in specs, but needed to prevent PPC machine check */
+		b43_phy_write(dev, B43_PHY_ANALOGOVER, backup_phy[4]);
+		b43_phy_write(dev, B43_PHY_ANALOGOVERVAL, backup_phy[5]);
+	}
+	b43_phy_write(dev, B43_PHY_BASE(0x5A), backup_phy[6]);
+	b43_phy_write(dev, B43_PHY_BASE(0x59), backup_phy[7]);
+	b43_phy_write(dev, B43_PHY_BASE(0x58), backup_phy[8]);
+	b43_phy_write(dev, B43_PHY_BASE(0x0A), backup_phy[9]);
+	b43_phy_write(dev, B43_PHY_BASE(0x03), backup_phy[10]);
+	b43_phy_write(dev, B43_PHY_LO_MASK, backup_phy[11]);
+	b43_phy_write(dev, B43_PHY_LO_CTL, backup_phy[12]);
+	b43_phy_write(dev, B43_PHY_BASE(0x2B), backup_phy[13]);
+	b43_phy_write(dev, B43_PHY_PGACTL, backup_phy[14]);
+
+	b43_phy_set_baseband_attenuation(dev, backup_bband);
+
+	b43_radio_write16(dev, 0x52, backup_radio[0]);
+	b43_radio_write16(dev, 0x43, backup_radio[1]);
+	b43_radio_write16(dev, 0x7A, backup_radio[2]);
+
+	b43_phy_write(dev, B43_PHY_RFOVER, backup_phy[2] | 0x0003);
+	udelay(10);
+	b43_phy_write(dev, B43_PHY_RFOVER, backup_phy[2]);
+	b43_phy_write(dev, B43_PHY_RFOVERVAL, backup_phy[3]);
+	b43_phy_write(dev, B43_PHY_CRS0, backup_phy[0]);
+	b43_phy_write(dev, B43_PHY_CCKBBANDCFG, backup_phy[1]);
+
+	phy->max_lb_gain =
+	    ((loop1_inner_done * 6) - (loop1_outer_done * 4)) - 11;
+	phy->trsw_rx_gain = trsw_rx * 2;
+}
+
+static void b43_phy_initg(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+	u16 tmp;
+
+	if (phy->rev == 1)
+		b43_phy_initb5(dev);
+	else
+		b43_phy_initb6(dev);
+
+	if (phy->rev >= 2 || phy->gmode)
+		b43_phy_inita(dev);
+
+	if (phy->rev >= 2) {
+		b43_phy_write(dev, B43_PHY_ANALOGOVER, 0);
+		b43_phy_write(dev, B43_PHY_ANALOGOVERVAL, 0);
+	}
+	if (phy->rev == 2) {
+		b43_phy_write(dev, B43_PHY_RFOVER, 0);
+		b43_phy_write(dev, B43_PHY_PGACTL, 0xC0);
+	}
+	if (phy->rev > 5) {
+		b43_phy_write(dev, B43_PHY_RFOVER, 0x400);
+		b43_phy_write(dev, B43_PHY_PGACTL, 0xC0);
+	}
+	if (phy->gmode || phy->rev >= 2) {
+		tmp = b43_phy_read(dev, B43_PHY_VERSION_OFDM);
+		tmp &= B43_PHYVER_VERSION;
+		if (tmp == 3 || tmp == 5) {
+			b43_phy_write(dev, B43_PHY_OFDM(0xC2), 0x1816);
+			b43_phy_write(dev, B43_PHY_OFDM(0xC3), 0x8006);
+		}
+		if (tmp == 5) {
+			b43_phy_write(dev, B43_PHY_OFDM(0xCC),
+				      (b43_phy_read(dev, B43_PHY_OFDM(0xCC))
+				       & 0x00FF) | 0x1F00);
+		}
+	}
+	if ((phy->rev <= 2 && phy->gmode) || phy->rev >= 2)
+		b43_phy_write(dev, B43_PHY_OFDM(0x7E), 0x78);
+	if (phy->radio_rev == 8) {
+		b43_phy_write(dev, B43_PHY_EXTG(0x01),
+			      b43_phy_read(dev, B43_PHY_EXTG(0x01))
+			      | 0x80);
+		b43_phy_write(dev, B43_PHY_OFDM(0x3E),
+			      b43_phy_read(dev, B43_PHY_OFDM(0x3E))
+			      | 0x4);
+	}
+	if (has_loopback_gain(phy))
+		b43_calc_loopback_gain(dev);
+
+	if (phy->radio_rev != 8) {
+		if (phy->initval == 0xFFFF)
+			phy->initval = b43_radio_init2050(dev);
+		else
+			b43_radio_write16(dev, 0x0078, phy->initval);
+	}
+	if (phy->lo_control->tx_bias == 0xFF) {
+		b43_lo_g_measure(dev);
+	} else {
+		if (has_tx_magnification(phy)) {
+			b43_radio_write16(dev, 0x52,
+					  (b43_radio_read16(dev, 0x52) & 0xFF00)
+					  | phy->lo_control->tx_bias | phy->
+					  lo_control->tx_magn);
+		} else {
+			b43_radio_write16(dev, 0x52,
+					  (b43_radio_read16(dev, 0x52) & 0xFFF0)
+					  | phy->lo_control->tx_bias);
+		}
+		if (phy->rev >= 6) {
+			b43_phy_write(dev, B43_PHY_BASE(0x36),
+				      (b43_phy_read(dev, B43_PHY_BASE(0x36))
+				       & 0x0FFF) | (phy->lo_control->
+						    tx_bias << 12));
+		}
+		if (dev->dev->bus->sprom.r1.boardflags_lo & B43_BFL_PACTRL)
+			b43_phy_write(dev, B43_PHY_BASE(0x2E), 0x8075);
+		else
+			b43_phy_write(dev, B43_PHY_BASE(0x2E), 0x807F);
+		if (phy->rev < 2)
+			b43_phy_write(dev, B43_PHY_BASE(0x2F), 0x101);
+		else
+			b43_phy_write(dev, B43_PHY_BASE(0x2F), 0x202);
+	}
+	if (phy->gmode || phy->rev >= 2) {
+		b43_lo_g_adjust(dev);
+		b43_phy_write(dev, B43_PHY_LO_MASK, 0x8078);
+	}
+
+	if (!(dev->dev->bus->sprom.r1.boardflags_lo & B43_BFL_RSSI)) {
+		/* The specs state to update the NRSSI LT with
+		 * the value 0x7FFFFFFF here. I think that is some weird
+		 * compiler optimization in the original driver.
+		 * Essentially, what we do here is resetting all NRSSI LT
+		 * entries to -32 (see the limit_value() in nrssi_hw_update())
+		 */
+		b43_nrssi_hw_update(dev, 0xFFFF);	//FIXME?
+		b43_calc_nrssi_threshold(dev);
+	} else if (phy->gmode || phy->rev >= 2) {
+		if (phy->nrssi[0] == -1000) {
+			B43_WARN_ON(phy->nrssi[1] != -1000);
+			b43_calc_nrssi_slope(dev);
+		} else
+			b43_calc_nrssi_threshold(dev);
+	}
+	if (phy->radio_rev == 8)
+		b43_phy_write(dev, B43_PHY_EXTG(0x05), 0x3230);
+	b43_phy_init_pctl(dev);
+	/* FIXME: The spec says in the following if, the 0 should be replaced
+	   'if OFDM may not be used in the current locale'
+	   but OFDM is legal everywhere */
+	if ((dev->dev->bus->chip_id == 0x4306
+	     && dev->dev->bus->chip_package == 2) || 0) {
+		b43_phy_write(dev, B43_PHY_CRS0, b43_phy_read(dev, B43_PHY_CRS0)
+			      & 0xBFFF);
+		b43_phy_write(dev, B43_PHY_OFDM(0xC3),
+			      b43_phy_read(dev, B43_PHY_OFDM(0xC3))
+			      & 0x7FFF);
+	}
+}
+
+/* Set the baseband attenuation value on chip. */
+void b43_phy_set_baseband_attenuation(struct b43_wldev *dev,
+				      u16 baseband_attenuation)
+{
+	struct b43_phy *phy = &dev->phy;
+
+	if (phy->analog == 0) {
+		b43_write16(dev, B43_MMIO_PHY0, (b43_read16(dev, B43_MMIO_PHY0)
+						 & 0xFFF0) |
+			    baseband_attenuation);
+	} else if (phy->analog > 1) {
+		b43_phy_write(dev, B43_PHY_DACCTL,
+			      (b43_phy_read(dev, B43_PHY_DACCTL)
+			       & 0xFFC3) | (baseband_attenuation << 2));
+	} else {
+		b43_phy_write(dev, B43_PHY_DACCTL,
+			      (b43_phy_read(dev, B43_PHY_DACCTL)
+			       & 0xFF87) | (baseband_attenuation << 3));
+	}
+}
+
+/* http://bcm-specs.sipsolutions.net/EstimatePowerOut
+ * This function converts a TSSI value to dBm in Q5.2
+ */
+static s8 b43_phy_estimate_power_out(struct b43_wldev *dev, s8 tssi)
+{
+	struct b43_phy *phy = &dev->phy;
+	s8 dbm = 0;
+	s32 tmp;
+
+	tmp = (phy->tgt_idle_tssi - phy->cur_idle_tssi + tssi);
+
+	switch (phy->type) {
+	case B43_PHYTYPE_A:
+		tmp += 0x80;
+		tmp = limit_value(tmp, 0x00, 0xFF);
+		dbm = phy->tssi2dbm[tmp];
+		//TODO: There's a FIXME on the specs
+		break;
+	case B43_PHYTYPE_B:
+	case B43_PHYTYPE_G:
+		tmp = limit_value(tmp, 0x00, 0x3F);
+		dbm = phy->tssi2dbm[tmp];
+		break;
+	default:
+		B43_WARN_ON(1);
+	}
+
+	return dbm;
+}
+
+void b43_put_attenuation_into_ranges(struct b43_wldev *dev,
+				     int *_bbatt, int *_rfatt)
+{
+	int rfatt = *_rfatt;
+	int bbatt = *_bbatt;
+	struct b43_txpower_lo_control *lo = dev->phy.lo_control;
+
+	/* Get baseband and radio attenuation values into their permitted ranges.
+	 * Radio attenuation affects power level 4 times as much as baseband. */
+
+	/* Range constants */
+	const int rf_min = lo->rfatt_list.min_val;
+	const int rf_max = lo->rfatt_list.max_val;
+	const int bb_min = lo->bbatt_list.min_val;
+	const int bb_max = lo->bbatt_list.max_val;
+
+	while (1) {
+		if (rfatt > rf_max && bbatt > bb_max - 4)
+			break;	/* Can not get it into ranges */
+		if (rfatt < rf_min && bbatt < bb_min + 4)
+			break;	/* Can not get it into ranges */
+		if (bbatt > bb_max && rfatt > rf_max - 1)
+			break;	/* Can not get it into ranges */
+		if (bbatt < bb_min && rfatt < rf_min + 1)
+			break;	/* Can not get it into ranges */
+
+		if (bbatt > bb_max) {
+			bbatt -= 4;
+			rfatt += 1;
+			continue;
+		}
+		if (bbatt < bb_min) {
+			bbatt += 4;
+			rfatt -= 1;
+			continue;
+		}
+		if (rfatt > rf_max) {
+			rfatt -= 1;
+			bbatt += 4;
+			continue;
+		}
+		if (rfatt < rf_min) {
+			rfatt += 1;
+			bbatt -= 4;
+			continue;
+		}
+		break;
+	}
+
+	*_rfatt = limit_value(rfatt, rf_min, rf_max);
+	*_bbatt = limit_value(bbatt, bb_min, bb_max);
+}
+
+/* http://bcm-specs.sipsolutions.net/RecalculateTransmissionPower */
+void b43_phy_xmitpower(struct b43_wldev *dev)
+{
+	struct ssb_bus *bus = dev->dev->bus;
+	struct b43_phy *phy = &dev->phy;
+
+	if (phy->cur_idle_tssi == 0)
+		return;
+	if ((bus->boardinfo.vendor == SSB_BOARDVENDOR_BCM) &&
+	    (bus->boardinfo.type == SSB_BOARD_BU4306))
+		return;
+#ifdef CONFIG_B43_DEBUG
+	if (phy->manual_txpower_control)
+		return;
+#endif
+
+	switch (phy->type) {
+	case B43_PHYTYPE_A:{
+
+			//TODO: Nothing for A PHYs yet :-/
+
+			break;
+		}
+	case B43_PHYTYPE_B:
+	case B43_PHYTYPE_G:{
+			u16 tmp;
+			s8 v0, v1, v2, v3;
+			s8 average;
+			int max_pwr;
+			int desired_pwr, estimated_pwr, pwr_adjust;
+			int rfatt_delta, bbatt_delta;
+			int rfatt, bbatt;
+			u8 tx_control;
+			unsigned long phylock_flags;
+
+			tmp = b43_shm_read16(dev, B43_SHM_SHARED, 0x0058);
+			v0 = (s8) (tmp & 0x00FF);
+			v1 = (s8) ((tmp & 0xFF00) >> 8);
+			tmp = b43_shm_read16(dev, B43_SHM_SHARED, 0x005A);
+			v2 = (s8) (tmp & 0x00FF);
+			v3 = (s8) ((tmp & 0xFF00) >> 8);
+			tmp = 0;
+
+			if (v0 == 0x7F || v1 == 0x7F || v2 == 0x7F
+			    || v3 == 0x7F) {
+				tmp =
+				    b43_shm_read16(dev, B43_SHM_SHARED, 0x0070);
+				v0 = (s8) (tmp & 0x00FF);
+				v1 = (s8) ((tmp & 0xFF00) >> 8);
+				tmp =
+				    b43_shm_read16(dev, B43_SHM_SHARED, 0x0072);
+				v2 = (s8) (tmp & 0x00FF);
+				v3 = (s8) ((tmp & 0xFF00) >> 8);
+				if (v0 == 0x7F || v1 == 0x7F || v2 == 0x7F
+				    || v3 == 0x7F)
+					return;
+				v0 = (v0 + 0x20) & 0x3F;
+				v1 = (v1 + 0x20) & 0x3F;
+				v2 = (v2 + 0x20) & 0x3F;
+				v3 = (v3 + 0x20) & 0x3F;
+				tmp = 1;
+			}
+			b43_shm_clear_tssi(dev);
+
+			average = (v0 + v1 + v2 + v3 + 2) / 4;
+
+			if (tmp
+			    && (b43_shm_read16(dev, B43_SHM_SHARED, 0x005E) &
+				0x8))
+				average -= 13;
+
+			estimated_pwr =
+			    b43_phy_estimate_power_out(dev, average);
+
+			max_pwr = dev->dev->bus->sprom.r1.maxpwr_bg;
+			if ((dev->dev->bus->sprom.r1.
+			     boardflags_lo & B43_BFL_PACTRL)
+			    && (phy->type == B43_PHYTYPE_G))
+				max_pwr -= 0x3;
+			if (unlikely(max_pwr <= 0)) {
+				b43warn(dev->wl,
+					"Invalid max-TX-power value in SPROM.\n");
+				max_pwr = 60;	/* fake it */
+				dev->dev->bus->sprom.r1.maxpwr_bg = max_pwr;
+			}
+
+			/*TODO:
+			   max_pwr = min(REG - dev->dev->bus->sprom.antennagain_bgphy - 0x6, max_pwr)
+			   where REG is the max power as per the regulatory domain
+			 */
+
+			/* Get desired power (in Q5.2) */
+			desired_pwr = INT_TO_Q52(phy->power_level);
+			/* And limit it. max_pwr already is Q5.2 */
+			desired_pwr = limit_value(desired_pwr, 0, max_pwr);
+			if (b43_debug(dev, B43_DBG_XMITPOWER)) {
+				b43dbg(dev->wl,
+				       "Current TX power output: " Q52_FMT
+				       " dBm, " "Desired TX power output: "
+				       Q52_FMT " dBm\n", Q52_ARG(estimated_pwr),
+				       Q52_ARG(desired_pwr));
+			}
+
+			/* Calculate the adjustment delta. */
+			pwr_adjust = desired_pwr - estimated_pwr;
+
+			/* RF attenuation delta. */
+			rfatt_delta = ((pwr_adjust + 7) / 8);
+			/* Lower attenuation => Bigger power output. Negate it. */
+			rfatt_delta = -rfatt_delta;
+
+			/* Baseband attenuation delta. */
+			bbatt_delta = pwr_adjust / 2;
+			/* Lower attenuation => Bigger power output. Negate it. */
+			bbatt_delta = -bbatt_delta;
+			/* RF att affects power level 4 times as much as
+			 * Baseband attennuation. Subtract it. */
+			bbatt_delta -= 4 * rfatt_delta;
+
+			/* So do we finally need to adjust something? */
+			if ((rfatt_delta == 0) && (bbatt_delta == 0)) {
+				b43_lo_g_ctl_mark_cur_used(dev);
+				return;
+			}
+
+			/* Calculate the new attenuation values. */
+			bbatt = phy->bbatt.att;
+			bbatt += bbatt_delta;
+			rfatt = phy->rfatt.att;
+			rfatt += rfatt_delta;
+
+			b43_put_attenuation_into_ranges(dev, &bbatt, &rfatt);
+			tx_control = phy->tx_control;
+			if ((phy->radio_ver == 0x2050) && (phy->radio_rev == 2)) {
+				if (rfatt <= 1) {
+					if (tx_control == 0) {
+						tx_control =
+						    B43_TXCTL_PA2DB |
+						    B43_TXCTL_TXMIX;
+						rfatt += 2;
+						bbatt += 2;
+					} else if (dev->dev->bus->sprom.r1.
+						   boardflags_lo &
+						   B43_BFL_PACTRL) {
+						bbatt += 4 * (rfatt - 2);
+						rfatt = 2;
+					}
+				} else if (rfatt > 4 && tx_control) {
+					tx_control = 0;
+					if (bbatt < 3) {
+						rfatt -= 3;
+						bbatt += 2;
+					} else {
+						rfatt -= 2;
+						bbatt -= 2;
+					}
+				}
+			}
+			/* Save the control values */
+			phy->tx_control = tx_control;
+			b43_put_attenuation_into_ranges(dev, &bbatt, &rfatt);
+			phy->rfatt.att = rfatt;
+			phy->bbatt.att = bbatt;
+
+			/* Adjust the hardware */
+			b43_phy_lock(dev, phylock_flags);
+			b43_radio_lock(dev);
+			b43_set_txpower_g(dev, &phy->bbatt, &phy->rfatt,
+					  phy->tx_control);
+			b43_lo_g_ctl_mark_cur_used(dev);
+			b43_radio_unlock(dev);
+			b43_phy_unlock(dev, phylock_flags);
+			break;
+		}
+	default:
+		B43_WARN_ON(1);
+	}
+}
+
+static inline s32 b43_tssi2dbm_ad(s32 num, s32 den)
+{
+	if (num < 0)
+		return num / den;
+	else
+		return (num + den / 2) / den;
+}
+
+static inline
+    s8 b43_tssi2dbm_entry(s8 entry[], u8 index, s16 pab0, s16 pab1, s16 pab2)
+{
+	s32 m1, m2, f = 256, q, delta;
+	s8 i = 0;
+
+	m1 = b43_tssi2dbm_ad(16 * pab0 + index * pab1, 32);
+	m2 = max(b43_tssi2dbm_ad(32768 + index * pab2, 256), 1);
+	do {
+		if (i > 15)
+			return -EINVAL;
+		q = b43_tssi2dbm_ad(f * 4096 -
+				    b43_tssi2dbm_ad(m2 * f, 16) * f, 2048);
+		delta = abs(q - f);
+		f = q;
+		i++;
+	} while (delta >= 2);
+	entry[index] = limit_value(b43_tssi2dbm_ad(m1 * f, 8192), -127, 128);
+	return 0;
+}
+
+/* http://bcm-specs.sipsolutions.net/TSSI_to_DBM_Table */
+int b43_phy_init_tssi2dbm_table(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+	s16 pab0, pab1, pab2;
+	u8 idx;
+	s8 *dyn_tssi2dbm;
+
+	if (phy->type == B43_PHYTYPE_A) {
+		pab0 = (s16) (dev->dev->bus->sprom.r1.pa1b0);
+		pab1 = (s16) (dev->dev->bus->sprom.r1.pa1b1);
+		pab2 = (s16) (dev->dev->bus->sprom.r1.pa1b2);
+	} else {
+		pab0 = (s16) (dev->dev->bus->sprom.r1.pa0b0);
+		pab1 = (s16) (dev->dev->bus->sprom.r1.pa0b1);
+		pab2 = (s16) (dev->dev->bus->sprom.r1.pa0b2);
+	}
+
+	if ((dev->dev->bus->chip_id == 0x4301) && (phy->radio_ver != 0x2050)) {
+		phy->tgt_idle_tssi = 0x34;
+		phy->tssi2dbm = b43_tssi2dbm_b_table;
+		return 0;
+	}
+
+	if (pab0 != 0 && pab1 != 0 && pab2 != 0 &&
+	    pab0 != -1 && pab1 != -1 && pab2 != -1) {
+		/* The pabX values are set in SPROM. Use them. */
+		if (phy->type == B43_PHYTYPE_A) {
+			if ((s8) dev->dev->bus->sprom.r1.itssi_a != 0 &&
+			    (s8) dev->dev->bus->sprom.r1.itssi_a != -1)
+				phy->tgt_idle_tssi =
+				    (s8) (dev->dev->bus->sprom.r1.itssi_a);
+			else
+				phy->tgt_idle_tssi = 62;
+		} else {
+			if ((s8) dev->dev->bus->sprom.r1.itssi_bg != 0 &&
+			    (s8) dev->dev->bus->sprom.r1.itssi_bg != -1)
+				phy->tgt_idle_tssi =
+				    (s8) (dev->dev->bus->sprom.r1.itssi_bg);
+			else
+				phy->tgt_idle_tssi = 62;
+		}
+		dyn_tssi2dbm = kmalloc(64, GFP_KERNEL);
+		if (dyn_tssi2dbm == NULL) {
+			b43err(dev->wl, "Could not allocate memory"
+			       "for tssi2dbm table\n");
+			return -ENOMEM;
+		}
+		for (idx = 0; idx < 64; idx++)
+			if (b43_tssi2dbm_entry
+			    (dyn_tssi2dbm, idx, pab0, pab1, pab2)) {
+				phy->tssi2dbm = NULL;
+				b43err(dev->wl, "Could not generate "
+				       "tssi2dBm table\n");
+				kfree(dyn_tssi2dbm);
+				return -ENODEV;
+			}
+		phy->tssi2dbm = dyn_tssi2dbm;
+		phy->dyn_tssi_tbl = 1;
+	} else {
+		/* pabX values not set in SPROM. */
+		switch (phy->type) {
+		case B43_PHYTYPE_A:
+			/* APHY needs a generated table. */
+			phy->tssi2dbm = NULL;
+			b43err(dev->wl, "Could not generate tssi2dBm "
+			       "table (wrong SPROM info)!\n");
+			return -ENODEV;
+		case B43_PHYTYPE_B:
+			phy->tgt_idle_tssi = 0x34;
+			phy->tssi2dbm = b43_tssi2dbm_b_table;
+			break;
+		case B43_PHYTYPE_G:
+			phy->tgt_idle_tssi = 0x34;
+			phy->tssi2dbm = b43_tssi2dbm_g_table;
+			break;
+		}
+	}
+
+	return 0;
+}
+
+int b43_phy_init(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+	int err = -ENODEV;
+
+	switch (phy->type) {
+	case B43_PHYTYPE_A:
+		if (phy->rev == 2 || phy->rev == 3) {
+			b43_phy_inita(dev);
+			err = 0;
+		}
+		break;
+	case B43_PHYTYPE_B:
+		switch (phy->rev) {
+		case 2:
+			b43_phy_initb2(dev);
+			err = 0;
+			break;
+		case 4:
+			b43_phy_initb4(dev);
+			err = 0;
+			break;
+		case 5:
+			b43_phy_initb5(dev);
+			err = 0;
+			break;
+		case 6:
+			b43_phy_initb6(dev);
+			err = 0;
+			break;
+		}
+		break;
+	case B43_PHYTYPE_G:
+		b43_phy_initg(dev);
+		err = 0;
+		break;
+	}
+	if (err)
+		b43err(dev->wl, "Unknown PHYTYPE found\n");
+
+	return err;
+}
+
+void b43_set_rx_antenna(struct b43_wldev *dev, int antenna)
+{
+	struct b43_phy *phy = &dev->phy;
+	u32 hf;
+	u16 tmp;
+	int autodiv = 0;
+
+	if (antenna == B43_ANTENNA_AUTO0 || antenna == B43_ANTENNA_AUTO1)
+		autodiv = 1;
+
+	hf = b43_hf_read(dev);
+	hf &= ~B43_HF_ANTDIVHELP;
+	b43_hf_write(dev, hf);
+
+	switch (phy->type) {
+	case B43_PHYTYPE_A:
+	case B43_PHYTYPE_G:
+		tmp = b43_phy_read(dev, B43_PHY_BBANDCFG);
+		tmp &= ~B43_PHY_BBANDCFG_RXANT;
+		tmp |= (autodiv ? B43_ANTENNA_AUTO0 : antenna)
+		    << B43_PHY_BBANDCFG_RXANT_SHIFT;
+		b43_phy_write(dev, B43_PHY_BBANDCFG, tmp);
+
+		if (autodiv) {
+			tmp = b43_phy_read(dev, B43_PHY_ANTDWELL);
+			if (antenna == B43_ANTENNA_AUTO0)
+				tmp &= ~B43_PHY_ANTDWELL_AUTODIV1;
+			else
+				tmp |= B43_PHY_ANTDWELL_AUTODIV1;
+			b43_phy_write(dev, B43_PHY_ANTDWELL, tmp);
+		}
+		if (phy->type == B43_PHYTYPE_G) {
+			tmp = b43_phy_read(dev, B43_PHY_ANTWRSETT);
+			if (autodiv)
+				tmp |= B43_PHY_ANTWRSETT_ARXDIV;
+			else
+				tmp &= ~B43_PHY_ANTWRSETT_ARXDIV;
+			b43_phy_write(dev, B43_PHY_ANTWRSETT, tmp);
+			if (phy->rev >= 2) {
+				tmp = b43_phy_read(dev, B43_PHY_OFDM61);
+				tmp |= B43_PHY_OFDM61_10;
+				b43_phy_write(dev, B43_PHY_OFDM61, tmp);
+
+				tmp =
+				    b43_phy_read(dev, B43_PHY_DIVSRCHGAINBACK);
+				tmp = (tmp & 0xFF00) | 0x15;
+				b43_phy_write(dev, B43_PHY_DIVSRCHGAINBACK,
+					      tmp);
+
+				if (phy->rev == 2) {
+					b43_phy_write(dev, B43_PHY_ADIVRELATED,
+						      8);
+				} else {
+					tmp =
+					    b43_phy_read(dev,
+							 B43_PHY_ADIVRELATED);
+					tmp = (tmp & 0xFF00) | 8;
+					b43_phy_write(dev, B43_PHY_ADIVRELATED,
+						      tmp);
+				}
+			}
+			if (phy->rev >= 6)
+				b43_phy_write(dev, B43_PHY_OFDM9B, 0xDC);
+		} else {
+			if (phy->rev < 3) {
+				tmp = b43_phy_read(dev, B43_PHY_ANTDWELL);
+				tmp = (tmp & 0xFF00) | 0x24;
+				b43_phy_write(dev, B43_PHY_ANTDWELL, tmp);
+			} else {
+				tmp = b43_phy_read(dev, B43_PHY_OFDM61);
+				tmp |= 0x10;
+				b43_phy_write(dev, B43_PHY_OFDM61, tmp);
+				if (phy->analog == 3) {
+					b43_phy_write(dev, B43_PHY_CLIPPWRDOWNT,
+						      0x1D);
+					b43_phy_write(dev, B43_PHY_ADIVRELATED,
+						      8);
+				} else {
+					b43_phy_write(dev, B43_PHY_CLIPPWRDOWNT,
+						      0x3A);
+					tmp =
+					    b43_phy_read(dev,
+							 B43_PHY_ADIVRELATED);
+					tmp = (tmp & 0xFF00) | 8;
+					b43_phy_write(dev, B43_PHY_ADIVRELATED,
+						      tmp);
+				}
+			}
+		}
+		break;
+	case B43_PHYTYPE_B:
+		tmp = b43_phy_read(dev, B43_PHY_CCKBBANDCFG);
+		tmp &= ~B43_PHY_BBANDCFG_RXANT;
+		tmp |= (autodiv ? B43_ANTENNA_AUTO0 : antenna)
+		    << B43_PHY_BBANDCFG_RXANT_SHIFT;
+		b43_phy_write(dev, B43_PHY_CCKBBANDCFG, tmp);
+		break;
+	default:
+		B43_WARN_ON(1);
+	}
+
+	hf |= B43_HF_ANTDIVHELP;
+	b43_hf_write(dev, hf);
+}
+
+/* Get the freq, as it has to be written to the device. */
+static inline u16 channel2freq_bg(u8 channel)
+{
+	B43_WARN_ON(!(channel >= 1 && channel <= 14));
+
+	return b43_radio_channel_codes_bg[channel - 1];
+}
+
+/* Get the freq, as it has to be written to the device. */
+static inline u16 channel2freq_a(u8 channel)
+{
+	B43_WARN_ON(channel > 200);
+
+	return (5000 + 5 * channel);
+}
+
+void b43_radio_lock(struct b43_wldev *dev)
+{
+	u32 macctl;
+
+	macctl = b43_read32(dev, B43_MMIO_MACCTL);
+	macctl |= B43_MACCTL_RADIOLOCK;
+	b43_write32(dev, B43_MMIO_MACCTL, macctl);
+	/* Commit the write and wait for the device
+	 * to exit any radio register access. */
+	b43_read32(dev, B43_MMIO_MACCTL);
+	udelay(10);
+}
+
+void b43_radio_unlock(struct b43_wldev *dev)
+{
+	u32 macctl;
+
+	/* Commit any write */
+	b43_read16(dev, B43_MMIO_PHY_VER);
+	/* unlock */
+	macctl = b43_read32(dev, B43_MMIO_MACCTL);
+	macctl &= ~B43_MACCTL_RADIOLOCK;
+	b43_write32(dev, B43_MMIO_MACCTL, macctl);
+}
+
+u16 b43_radio_read16(struct b43_wldev *dev, u16 offset)
+{
+	struct b43_phy *phy = &dev->phy;
+
+	switch (phy->type) {
+	case B43_PHYTYPE_A:
+		offset |= 0x0040;
+		break;
+	case B43_PHYTYPE_B:
+		if (phy->radio_ver == 0x2053) {
+			if (offset < 0x70)
+				offset += 0x80;
+			else if (offset < 0x80)
+				offset += 0x70;
+		} else if (phy->radio_ver == 0x2050) {
+			offset |= 0x80;
+		} else
+			B43_WARN_ON(1);
+		break;
+	case B43_PHYTYPE_G:
+		offset |= 0x80;
+		break;
+	}
+
+	b43_write16(dev, B43_MMIO_RADIO_CONTROL, offset);
+	return b43_read16(dev, B43_MMIO_RADIO_DATA_LOW);
+}
+
+void b43_radio_write16(struct b43_wldev *dev, u16 offset, u16 val)
+{
+	b43_write16(dev, B43_MMIO_RADIO_CONTROL, offset);
+	mmiowb();
+	b43_write16(dev, B43_MMIO_RADIO_DATA_LOW, val);
+}
+
+static void b43_set_all_gains(struct b43_wldev *dev,
+			      s16 first, s16 second, s16 third)
+{
+	struct b43_phy *phy = &dev->phy;
+	u16 i;
+	u16 start = 0x08, end = 0x18;
+	u16 tmp;
+	u16 table;
+
+	if (phy->rev <= 1) {
+		start = 0x10;
+		end = 0x20;
+	}
+
+	table = B43_OFDMTAB_GAINX;
+	if (phy->rev <= 1)
+		table = B43_OFDMTAB_GAINX_R1;
+	for (i = 0; i < 4; i++)
+		b43_ofdmtab_write16(dev, table, i, first);
+
+	for (i = start; i < end; i++)
+		b43_ofdmtab_write16(dev, table, i, second);
+
+	if (third != -1) {
+		tmp = ((u16) third << 14) | ((u16) third << 6);
+		b43_phy_write(dev, 0x04A0,
+			      (b43_phy_read(dev, 0x04A0) & 0xBFBF) | tmp);
+		b43_phy_write(dev, 0x04A1,
+			      (b43_phy_read(dev, 0x04A1) & 0xBFBF) | tmp);
+		b43_phy_write(dev, 0x04A2,
+			      (b43_phy_read(dev, 0x04A2) & 0xBFBF) | tmp);
+	}
+	b43_dummy_transmission(dev);
+}
+
+static void b43_set_original_gains(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+	u16 i, tmp;
+	u16 table;
+	u16 start = 0x0008, end = 0x0018;
+
+	if (phy->rev <= 1) {
+		start = 0x0010;
+		end = 0x0020;
+	}
+
+	table = B43_OFDMTAB_GAINX;
+	if (phy->rev <= 1)
+		table = B43_OFDMTAB_GAINX_R1;
+	for (i = 0; i < 4; i++) {
+		tmp = (i & 0xFFFC);
+		tmp |= (i & 0x0001) << 1;
+		tmp |= (i & 0x0002) >> 1;
+
+		b43_ofdmtab_write16(dev, table, i, tmp);
+	}
+
+	for (i = start; i < end; i++)
+		b43_ofdmtab_write16(dev, table, i, i - start);
+
+	b43_phy_write(dev, 0x04A0,
+		      (b43_phy_read(dev, 0x04A0) & 0xBFBF) | 0x4040);
+	b43_phy_write(dev, 0x04A1,
+		      (b43_phy_read(dev, 0x04A1) & 0xBFBF) | 0x4040);
+	b43_phy_write(dev, 0x04A2,
+		      (b43_phy_read(dev, 0x04A2) & 0xBFBF) | 0x4000);
+	b43_dummy_transmission(dev);
+}
+
+/* Synthetic PU workaround */
+static void b43_synth_pu_workaround(struct b43_wldev *dev, u8 channel)
+{
+	struct b43_phy *phy = &dev->phy;
+
+	might_sleep();
+
+	if (phy->radio_ver != 0x2050 || phy->radio_rev >= 6) {
+		/* We do not need the workaround. */
+		return;
+	}
+
+	if (channel <= 10) {
+		b43_write16(dev, B43_MMIO_CHANNEL,
+			    channel2freq_bg(channel + 4));
+	} else {
+		b43_write16(dev, B43_MMIO_CHANNEL, channel2freq_bg(1));
+	}
+	msleep(1);
+	b43_write16(dev, B43_MMIO_CHANNEL, channel2freq_bg(channel));
+}
+
+u8 b43_radio_aci_detect(struct b43_wldev *dev, u8 channel)
+{
+	struct b43_phy *phy = &dev->phy;
+	u8 ret = 0;
+	u16 saved, rssi, temp;
+	int i, j = 0;
+
+	saved = b43_phy_read(dev, 0x0403);
+	b43_radio_selectchannel(dev, channel, 0);
+	b43_phy_write(dev, 0x0403, (saved & 0xFFF8) | 5);
+	if (phy->aci_hw_rssi)
+		rssi = b43_phy_read(dev, 0x048A) & 0x3F;
+	else
+		rssi = saved & 0x3F;
+	/* clamp temp to signed 5bit */
+	if (rssi > 32)
+		rssi -= 64;
+	for (i = 0; i < 100; i++) {
+		temp = (b43_phy_read(dev, 0x047F) >> 8) & 0x3F;
+		if (temp > 32)
+			temp -= 64;
+		if (temp < rssi)
+			j++;
+		if (j >= 20)
+			ret = 1;
+	}
+	b43_phy_write(dev, 0x0403, saved);
+
+	return ret;
+}
+
+u8 b43_radio_aci_scan(struct b43_wldev * dev)
+{
+	struct b43_phy *phy = &dev->phy;
+	u8 ret[13];
+	unsigned int channel = phy->channel;
+	unsigned int i, j, start, end;
+	unsigned long phylock_flags;
+
+	if (!((phy->type == B43_PHYTYPE_G) && (phy->rev > 0)))
+		return 0;
+
+	b43_phy_lock(dev, phylock_flags);
+	b43_radio_lock(dev);
+	b43_phy_write(dev, 0x0802, b43_phy_read(dev, 0x0802) & 0xFFFC);
+	b43_phy_write(dev, B43_PHY_G_CRS,
+		      b43_phy_read(dev, B43_PHY_G_CRS) & 0x7FFF);
+	b43_set_all_gains(dev, 3, 8, 1);
+
+	start = (channel - 5 > 0) ? channel - 5 : 1;
+	end = (channel + 5 < 14) ? channel + 5 : 13;
+
+	for (i = start; i <= end; i++) {
+		if (abs(channel - i) > 2)
+			ret[i - 1] = b43_radio_aci_detect(dev, i);
+	}
+	b43_radio_selectchannel(dev, channel, 0);
+	b43_phy_write(dev, 0x0802,
+		      (b43_phy_read(dev, 0x0802) & 0xFFFC) | 0x0003);
+	b43_phy_write(dev, 0x0403, b43_phy_read(dev, 0x0403) & 0xFFF8);
+	b43_phy_write(dev, B43_PHY_G_CRS,
+		      b43_phy_read(dev, B43_PHY_G_CRS) | 0x8000);
+	b43_set_original_gains(dev);
+	for (i = 0; i < 13; i++) {
+		if (!ret[i])
+			continue;
+		end = (i + 5 < 13) ? i + 5 : 13;
+		for (j = i; j < end; j++)
+			ret[j] = 1;
+	}
+	b43_radio_unlock(dev);
+	b43_phy_unlock(dev, phylock_flags);
+
+	return ret[channel - 1];
+}
+
+/* http://bcm-specs.sipsolutions.net/NRSSILookupTable */
+void b43_nrssi_hw_write(struct b43_wldev *dev, u16 offset, s16 val)
+{
+	b43_phy_write(dev, B43_PHY_NRSSILT_CTRL, offset);
+	mmiowb();
+	b43_phy_write(dev, B43_PHY_NRSSILT_DATA, (u16) val);
+}
+
+/* http://bcm-specs.sipsolutions.net/NRSSILookupTable */
+s16 b43_nrssi_hw_read(struct b43_wldev *dev, u16 offset)
+{
+	u16 val;
+
+	b43_phy_write(dev, B43_PHY_NRSSILT_CTRL, offset);
+	val = b43_phy_read(dev, B43_PHY_NRSSILT_DATA);
+
+	return (s16) val;
+}
+
+/* http://bcm-specs.sipsolutions.net/NRSSILookupTable */
+void b43_nrssi_hw_update(struct b43_wldev *dev, u16 val)
+{
+	u16 i;
+	s16 tmp;
+
+	for (i = 0; i < 64; i++) {
+		tmp = b43_nrssi_hw_read(dev, i);
+		tmp -= val;
+		tmp = limit_value(tmp, -32, 31);
+		b43_nrssi_hw_write(dev, i, tmp);
+	}
+}
+
+/* http://bcm-specs.sipsolutions.net/NRSSILookupTable */
+void b43_nrssi_mem_update(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+	s16 i, delta;
+	s32 tmp;
+
+	delta = 0x1F - phy->nrssi[0];
+	for (i = 0; i < 64; i++) {
+		tmp = (i - delta) * phy->nrssislope;
+		tmp /= 0x10000;
+		tmp += 0x3A;
+		tmp = limit_value(tmp, 0, 0x3F);
+		phy->nrssi_lt[i] = tmp;
+	}
+}
+
+static void b43_calc_nrssi_offset(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+	u16 backup[20] = { 0 };
+	s16 v47F;
+	u16 i;
+	u16 saved = 0xFFFF;
+
+	backup[0] = b43_phy_read(dev, 0x0001);
+	backup[1] = b43_phy_read(dev, 0x0811);
+	backup[2] = b43_phy_read(dev, 0x0812);
+	if (phy->rev != 1) {	/* Not in specs, but needed to prevent PPC machine check */
+		backup[3] = b43_phy_read(dev, 0x0814);
+		backup[4] = b43_phy_read(dev, 0x0815);
+	}
+	backup[5] = b43_phy_read(dev, 0x005A);
+	backup[6] = b43_phy_read(dev, 0x0059);
+	backup[7] = b43_phy_read(dev, 0x0058);
+	backup[8] = b43_phy_read(dev, 0x000A);
+	backup[9] = b43_phy_read(dev, 0x0003);
+	backup[10] = b43_radio_read16(dev, 0x007A);
+	backup[11] = b43_radio_read16(dev, 0x0043);
+
+	b43_phy_write(dev, 0x0429, b43_phy_read(dev, 0x0429) & 0x7FFF);
+	b43_phy_write(dev, 0x0001,
+		      (b43_phy_read(dev, 0x0001) & 0x3FFF) | 0x4000);
+	b43_phy_write(dev, 0x0811, b43_phy_read(dev, 0x0811) | 0x000C);
+	b43_phy_write(dev, 0x0812,
+		      (b43_phy_read(dev, 0x0812) & 0xFFF3) | 0x0004);
+	b43_phy_write(dev, 0x0802, b43_phy_read(dev, 0x0802) & ~(0x1 | 0x2));
+	if (phy->rev >= 6) {
+		backup[12] = b43_phy_read(dev, 0x002E);
+		backup[13] = b43_phy_read(dev, 0x002F);
+		backup[14] = b43_phy_read(dev, 0x080F);
+		backup[15] = b43_phy_read(dev, 0x0810);
+		backup[16] = b43_phy_read(dev, 0x0801);
+		backup[17] = b43_phy_read(dev, 0x0060);
+		backup[18] = b43_phy_read(dev, 0x0014);
+		backup[19] = b43_phy_read(dev, 0x0478);
+
+		b43_phy_write(dev, 0x002E, 0);
+		b43_phy_write(dev, 0x002F, 0);
+		b43_phy_write(dev, 0x080F, 0);
+		b43_phy_write(dev, 0x0810, 0);
+		b43_phy_write(dev, 0x0478, b43_phy_read(dev, 0x0478) | 0x0100);
+		b43_phy_write(dev, 0x0801, b43_phy_read(dev, 0x0801) | 0x0040);
+		b43_phy_write(dev, 0x0060, b43_phy_read(dev, 0x0060) | 0x0040);
+		b43_phy_write(dev, 0x0014, b43_phy_read(dev, 0x0014) | 0x0200);
+	}
+	b43_radio_write16(dev, 0x007A, b43_radio_read16(dev, 0x007A) | 0x0070);
+	b43_radio_write16(dev, 0x007A, b43_radio_read16(dev, 0x007A) | 0x0080);
+	udelay(30);
+
+	v47F = (s16) ((b43_phy_read(dev, 0x047F) >> 8) & 0x003F);
+	if (v47F >= 0x20)
+		v47F -= 0x40;
+	if (v47F == 31) {
+		for (i = 7; i >= 4; i--) {
+			b43_radio_write16(dev, 0x007B, i);
+			udelay(20);
+			v47F =
+			    (s16) ((b43_phy_read(dev, 0x047F) >> 8) & 0x003F);
+			if (v47F >= 0x20)
+				v47F -= 0x40;
+			if (v47F < 31 && saved == 0xFFFF)
+				saved = i;
+		}
+		if (saved == 0xFFFF)
+			saved = 4;
+	} else {
+		b43_radio_write16(dev, 0x007A,
+				  b43_radio_read16(dev, 0x007A) & 0x007F);
+		if (phy->rev != 1) {	/* Not in specs, but needed to prevent PPC machine check */
+			b43_phy_write(dev, 0x0814,
+				      b43_phy_read(dev, 0x0814) | 0x0001);
+			b43_phy_write(dev, 0x0815,
+				      b43_phy_read(dev, 0x0815) & 0xFFFE);
+		}
+		b43_phy_write(dev, 0x0811, b43_phy_read(dev, 0x0811) | 0x000C);
+		b43_phy_write(dev, 0x0812, b43_phy_read(dev, 0x0812) | 0x000C);
+		b43_phy_write(dev, 0x0811, b43_phy_read(dev, 0x0811) | 0x0030);
+		b43_phy_write(dev, 0x0812, b43_phy_read(dev, 0x0812) | 0x0030);
+		b43_phy_write(dev, 0x005A, 0x0480);
+		b43_phy_write(dev, 0x0059, 0x0810);
+		b43_phy_write(dev, 0x0058, 0x000D);
+		if (phy->rev == 0) {
+			b43_phy_write(dev, 0x0003, 0x0122);
+		} else {
+			b43_phy_write(dev, 0x000A, b43_phy_read(dev, 0x000A)
+				      | 0x2000);
+		}
+		if (phy->rev != 1) {	/* Not in specs, but needed to prevent PPC machine check */
+			b43_phy_write(dev, 0x0814,
+				      b43_phy_read(dev, 0x0814) | 0x0004);
+			b43_phy_write(dev, 0x0815,
+				      b43_phy_read(dev, 0x0815) & 0xFFFB);
+		}
+		b43_phy_write(dev, 0x0003, (b43_phy_read(dev, 0x0003) & 0xFF9F)
+			      | 0x0040);
+		b43_radio_write16(dev, 0x007A,
+				  b43_radio_read16(dev, 0x007A) | 0x000F);
+		b43_set_all_gains(dev, 3, 0, 1);
+		b43_radio_write16(dev, 0x0043, (b43_radio_read16(dev, 0x0043)
+						& 0x00F0) | 0x000F);
+		udelay(30);
+		v47F = (s16) ((b43_phy_read(dev, 0x047F) >> 8) & 0x003F);
+		if (v47F >= 0x20)
+			v47F -= 0x40;
+		if (v47F == -32) {
+			for (i = 0; i < 4; i++) {
+				b43_radio_write16(dev, 0x007B, i);
+				udelay(20);
+				v47F =
+				    (s16) ((b43_phy_read(dev, 0x047F) >> 8) &
+					   0x003F);
+				if (v47F >= 0x20)
+					v47F -= 0x40;
+				if (v47F > -31 && saved == 0xFFFF)
+					saved = i;
+			}
+			if (saved == 0xFFFF)
+				saved = 3;
+		} else
+			saved = 0;
+	}
+	b43_radio_write16(dev, 0x007B, saved);
+
+	if (phy->rev >= 6) {
+		b43_phy_write(dev, 0x002E, backup[12]);
+		b43_phy_write(dev, 0x002F, backup[13]);
+		b43_phy_write(dev, 0x080F, backup[14]);
+		b43_phy_write(dev, 0x0810, backup[15]);
+	}
+	if (phy->rev != 1) {	/* Not in specs, but needed to prevent PPC machine check */
+		b43_phy_write(dev, 0x0814, backup[3]);
+		b43_phy_write(dev, 0x0815, backup[4]);
+	}
+	b43_phy_write(dev, 0x005A, backup[5]);
+	b43_phy_write(dev, 0x0059, backup[6]);
+	b43_phy_write(dev, 0x0058, backup[7]);
+	b43_phy_write(dev, 0x000A, backup[8]);
+	b43_phy_write(dev, 0x0003, backup[9]);
+	b43_radio_write16(dev, 0x0043, backup[11]);
+	b43_radio_write16(dev, 0x007A, backup[10]);
+	b43_phy_write(dev, 0x0802, b43_phy_read(dev, 0x0802) | 0x1 | 0x2);
+	b43_phy_write(dev, 0x0429, b43_phy_read(dev, 0x0429) | 0x8000);
+	b43_set_original_gains(dev);
+	if (phy->rev >= 6) {
+		b43_phy_write(dev, 0x0801, backup[16]);
+		b43_phy_write(dev, 0x0060, backup[17]);
+		b43_phy_write(dev, 0x0014, backup[18]);
+		b43_phy_write(dev, 0x0478, backup[19]);
+	}
+	b43_phy_write(dev, 0x0001, backup[0]);
+	b43_phy_write(dev, 0x0812, backup[2]);
+	b43_phy_write(dev, 0x0811, backup[1]);
+}
+
+void b43_calc_nrssi_slope(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+	u16 backup[18] = { 0 };
+	u16 tmp;
+	s16 nrssi0, nrssi1;
+
+	switch (phy->type) {
+	case B43_PHYTYPE_B:
+		backup[0] = b43_radio_read16(dev, 0x007A);
+		backup[1] = b43_radio_read16(dev, 0x0052);
+		backup[2] = b43_radio_read16(dev, 0x0043);
+		backup[3] = b43_phy_read(dev, 0x0030);
+		backup[4] = b43_phy_read(dev, 0x0026);
+		backup[5] = b43_phy_read(dev, 0x0015);
+		backup[6] = b43_phy_read(dev, 0x002A);
+		backup[7] = b43_phy_read(dev, 0x0020);
+		backup[8] = b43_phy_read(dev, 0x005A);
+		backup[9] = b43_phy_read(dev, 0x0059);
+		backup[10] = b43_phy_read(dev, 0x0058);
+		backup[11] = b43_read16(dev, 0x03E2);
+		backup[12] = b43_read16(dev, 0x03E6);
+		backup[13] = b43_read16(dev, B43_MMIO_CHANNEL_EXT);
+
+		tmp = b43_radio_read16(dev, 0x007A);
+		tmp &= (phy->rev >= 5) ? 0x007F : 0x000F;
+		b43_radio_write16(dev, 0x007A, tmp);
+		b43_phy_write(dev, 0x0030, 0x00FF);
+		b43_write16(dev, 0x03EC, 0x7F7F);
+		b43_phy_write(dev, 0x0026, 0x0000);
+		b43_phy_write(dev, 0x0015, b43_phy_read(dev, 0x0015) | 0x0020);
+		b43_phy_write(dev, 0x002A, 0x08A3);
+		b43_radio_write16(dev, 0x007A,
+				  b43_radio_read16(dev, 0x007A) | 0x0080);
+
+		nrssi0 = (s16) b43_phy_read(dev, 0x0027);
+		b43_radio_write16(dev, 0x007A,
+				  b43_radio_read16(dev, 0x007A) & 0x007F);
+		if (phy->rev >= 2) {
+			b43_write16(dev, 0x03E6, 0x0040);
+		} else if (phy->rev == 0) {
+			b43_write16(dev, 0x03E6, 0x0122);
+		} else {
+			b43_write16(dev, B43_MMIO_CHANNEL_EXT,
+				    b43_read16(dev,
+					       B43_MMIO_CHANNEL_EXT) & 0x2000);
+		}
+		b43_phy_write(dev, 0x0020, 0x3F3F);
+		b43_phy_write(dev, 0x0015, 0xF330);
+		b43_radio_write16(dev, 0x005A, 0x0060);
+		b43_radio_write16(dev, 0x0043,
+				  b43_radio_read16(dev, 0x0043) & 0x00F0);
+		b43_phy_write(dev, 0x005A, 0x0480);
+		b43_phy_write(dev, 0x0059, 0x0810);
+		b43_phy_write(dev, 0x0058, 0x000D);
+		udelay(20);
+
+		nrssi1 = (s16) b43_phy_read(dev, 0x0027);
+		b43_phy_write(dev, 0x0030, backup[3]);
+		b43_radio_write16(dev, 0x007A, backup[0]);
+		b43_write16(dev, 0x03E2, backup[11]);
+		b43_phy_write(dev, 0x0026, backup[4]);
+		b43_phy_write(dev, 0x0015, backup[5]);
+		b43_phy_write(dev, 0x002A, backup[6]);
+		b43_synth_pu_workaround(dev, phy->channel);
+		if (phy->rev != 0)
+			b43_write16(dev, 0x03F4, backup[13]);
+
+		b43_phy_write(dev, 0x0020, backup[7]);
+		b43_phy_write(dev, 0x005A, backup[8]);
+		b43_phy_write(dev, 0x0059, backup[9]);
+		b43_phy_write(dev, 0x0058, backup[10]);
+		b43_radio_write16(dev, 0x0052, backup[1]);
+		b43_radio_write16(dev, 0x0043, backup[2]);
+
+		if (nrssi0 == nrssi1)
+			phy->nrssislope = 0x00010000;
+		else
+			phy->nrssislope = 0x00400000 / (nrssi0 - nrssi1);
+
+		if (nrssi0 <= -4) {
+			phy->nrssi[0] = nrssi0;
+			phy->nrssi[1] = nrssi1;
+		}
+		break;
+	case B43_PHYTYPE_G:
+		if (phy->radio_rev >= 9)
+			return;
+		if (phy->radio_rev == 8)
+			b43_calc_nrssi_offset(dev);
+
+		b43_phy_write(dev, B43_PHY_G_CRS,
+			      b43_phy_read(dev, B43_PHY_G_CRS) & 0x7FFF);
+		b43_phy_write(dev, 0x0802, b43_phy_read(dev, 0x0802) & 0xFFFC);
+		backup[7] = b43_read16(dev, 0x03E2);
+		b43_write16(dev, 0x03E2, b43_read16(dev, 0x03E2) | 0x8000);
+		backup[0] = b43_radio_read16(dev, 0x007A);
+		backup[1] = b43_radio_read16(dev, 0x0052);
+		backup[2] = b43_radio_read16(dev, 0x0043);
+		backup[3] = b43_phy_read(dev, 0x0015);
+		backup[4] = b43_phy_read(dev, 0x005A);
+		backup[5] = b43_phy_read(dev, 0x0059);
+		backup[6] = b43_phy_read(dev, 0x0058);
+		backup[8] = b43_read16(dev, 0x03E6);
+		backup[9] = b43_read16(dev, B43_MMIO_CHANNEL_EXT);
+		if (phy->rev >= 3) {
+			backup[10] = b43_phy_read(dev, 0x002E);
+			backup[11] = b43_phy_read(dev, 0x002F);
+			backup[12] = b43_phy_read(dev, 0x080F);
+			backup[13] = b43_phy_read(dev, B43_PHY_G_LO_CONTROL);
+			backup[14] = b43_phy_read(dev, 0x0801);
+			backup[15] = b43_phy_read(dev, 0x0060);
+			backup[16] = b43_phy_read(dev, 0x0014);
+			backup[17] = b43_phy_read(dev, 0x0478);
+			b43_phy_write(dev, 0x002E, 0);
+			b43_phy_write(dev, B43_PHY_G_LO_CONTROL, 0);
+			switch (phy->rev) {
+			case 4:
+			case 6:
+			case 7:
+				b43_phy_write(dev, 0x0478,
+					      b43_phy_read(dev, 0x0478)
+					      | 0x0100);
+				b43_phy_write(dev, 0x0801,
+					      b43_phy_read(dev, 0x0801)
+					      | 0x0040);
+				break;
+			case 3:
+			case 5:
+				b43_phy_write(dev, 0x0801,
+					      b43_phy_read(dev, 0x0801)
+					      & 0xFFBF);
+				break;
+			}
+			b43_phy_write(dev, 0x0060, b43_phy_read(dev, 0x0060)
+				      | 0x0040);
+			b43_phy_write(dev, 0x0014, b43_phy_read(dev, 0x0014)
+				      | 0x0200);
+		}
+		b43_radio_write16(dev, 0x007A,
+				  b43_radio_read16(dev, 0x007A) | 0x0070);
+		b43_set_all_gains(dev, 0, 8, 0);
+		b43_radio_write16(dev, 0x007A,
+				  b43_radio_read16(dev, 0x007A) & 0x00F7);
+		if (phy->rev >= 2) {
+			b43_phy_write(dev, 0x0811,
+				      (b43_phy_read(dev, 0x0811) & 0xFFCF) |
+				      0x0030);
+			b43_phy_write(dev, 0x0812,
+				      (b43_phy_read(dev, 0x0812) & 0xFFCF) |
+				      0x0010);
+		}
+		b43_radio_write16(dev, 0x007A,
+				  b43_radio_read16(dev, 0x007A) | 0x0080);
+		udelay(20);
+
+		nrssi0 = (s16) ((b43_phy_read(dev, 0x047F) >> 8) & 0x003F);
+		if (nrssi0 >= 0x0020)
+			nrssi0 -= 0x0040;
+
+		b43_radio_write16(dev, 0x007A,
+				  b43_radio_read16(dev, 0x007A) & 0x007F);
+		if (phy->rev >= 2) {
+			b43_phy_write(dev, 0x0003, (b43_phy_read(dev, 0x0003)
+						    & 0xFF9F) | 0x0040);
+		}
+
+		b43_write16(dev, B43_MMIO_CHANNEL_EXT,
+			    b43_read16(dev, B43_MMIO_CHANNEL_EXT)
+			    | 0x2000);
+		b43_radio_write16(dev, 0x007A,
+				  b43_radio_read16(dev, 0x007A) | 0x000F);
+		b43_phy_write(dev, 0x0015, 0xF330);
+		if (phy->rev >= 2) {
+			b43_phy_write(dev, 0x0812,
+				      (b43_phy_read(dev, 0x0812) & 0xFFCF) |
+				      0x0020);
+			b43_phy_write(dev, 0x0811,
+				      (b43_phy_read(dev, 0x0811) & 0xFFCF) |
+				      0x0020);
+		}
+
+		b43_set_all_gains(dev, 3, 0, 1);
+		if (phy->radio_rev == 8) {
+			b43_radio_write16(dev, 0x0043, 0x001F);
+		} else {
+			tmp = b43_radio_read16(dev, 0x0052) & 0xFF0F;
+			b43_radio_write16(dev, 0x0052, tmp | 0x0060);
+			tmp = b43_radio_read16(dev, 0x0043) & 0xFFF0;
+			b43_radio_write16(dev, 0x0043, tmp | 0x0009);
+		}
+		b43_phy_write(dev, 0x005A, 0x0480);
+		b43_phy_write(dev, 0x0059, 0x0810);
+		b43_phy_write(dev, 0x0058, 0x000D);
+		udelay(20);
+		nrssi1 = (s16) ((b43_phy_read(dev, 0x047F) >> 8) & 0x003F);
+		if (nrssi1 >= 0x0020)
+			nrssi1 -= 0x0040;
+		if (nrssi0 == nrssi1)
+			phy->nrssislope = 0x00010000;
+		else
+			phy->nrssislope = 0x00400000 / (nrssi0 - nrssi1);
+		if (nrssi0 >= -4) {
+			phy->nrssi[0] = nrssi1;
+			phy->nrssi[1] = nrssi0;
+		}
+		if (phy->rev >= 3) {
+			b43_phy_write(dev, 0x002E, backup[10]);
+			b43_phy_write(dev, 0x002F, backup[11]);
+			b43_phy_write(dev, 0x080F, backup[12]);
+			b43_phy_write(dev, B43_PHY_G_LO_CONTROL, backup[13]);
+		}
+		if (phy->rev >= 2) {
+			b43_phy_write(dev, 0x0812,
+				      b43_phy_read(dev, 0x0812) & 0xFFCF);
+			b43_phy_write(dev, 0x0811,
+				      b43_phy_read(dev, 0x0811) & 0xFFCF);
+		}
+
+		b43_radio_write16(dev, 0x007A, backup[0]);
+		b43_radio_write16(dev, 0x0052, backup[1]);
+		b43_radio_write16(dev, 0x0043, backup[2]);
+		b43_write16(dev, 0x03E2, backup[7]);
+		b43_write16(dev, 0x03E6, backup[8]);
+		b43_write16(dev, B43_MMIO_CHANNEL_EXT, backup[9]);
+		b43_phy_write(dev, 0x0015, backup[3]);
+		b43_phy_write(dev, 0x005A, backup[4]);
+		b43_phy_write(dev, 0x0059, backup[5]);
+		b43_phy_write(dev, 0x0058, backup[6]);
+		b43_synth_pu_workaround(dev, phy->channel);
+		b43_phy_write(dev, 0x0802,
+			      b43_phy_read(dev, 0x0802) | (0x0001 | 0x0002));
+		b43_set_original_gains(dev);
+		b43_phy_write(dev, B43_PHY_G_CRS,
+			      b43_phy_read(dev, B43_PHY_G_CRS) | 0x8000);
+		if (phy->rev >= 3) {
+			b43_phy_write(dev, 0x0801, backup[14]);
+			b43_phy_write(dev, 0x0060, backup[15]);
+			b43_phy_write(dev, 0x0014, backup[16]);
+			b43_phy_write(dev, 0x0478, backup[17]);
+		}
+		b43_nrssi_mem_update(dev);
+		b43_calc_nrssi_threshold(dev);
+		break;
+	default:
+		B43_WARN_ON(1);
+	}
+}
+
+void b43_calc_nrssi_threshold(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+	s32 threshold;
+	s32 a, b;
+	s16 tmp16;
+	u16 tmp_u16;
+
+	switch (phy->type) {
+	case B43_PHYTYPE_B:{
+			if (phy->radio_ver != 0x2050)
+				return;
+			if (!
+			    (dev->dev->bus->sprom.r1.
+			     boardflags_lo & B43_BFL_RSSI))
+				return;
+
+			if (phy->radio_rev >= 6) {
+				threshold =
+				    (phy->nrssi[1] - phy->nrssi[0]) * 32;
+				threshold += 20 * (phy->nrssi[0] + 1);
+				threshold /= 40;
+			} else
+				threshold = phy->nrssi[1] - 5;
+
+			threshold = limit_value(threshold, 0, 0x3E);
+			b43_phy_read(dev, 0x0020);	/* dummy read */
+			b43_phy_write(dev, 0x0020,
+				      (((u16) threshold) << 8) | 0x001C);
+
+			if (phy->radio_rev >= 6) {
+				b43_phy_write(dev, 0x0087, 0x0E0D);
+				b43_phy_write(dev, 0x0086, 0x0C0B);
+				b43_phy_write(dev, 0x0085, 0x0A09);
+				b43_phy_write(dev, 0x0084, 0x0808);
+				b43_phy_write(dev, 0x0083, 0x0808);
+				b43_phy_write(dev, 0x0082, 0x0604);
+				b43_phy_write(dev, 0x0081, 0x0302);
+				b43_phy_write(dev, 0x0080, 0x0100);
+			}
+			break;
+		}
+	case B43_PHYTYPE_G:
+		if (!phy->gmode ||
+		    !(dev->dev->bus->sprom.r1.boardflags_lo & B43_BFL_RSSI)) {
+			tmp16 = b43_nrssi_hw_read(dev, 0x20);
+			if (tmp16 >= 0x20)
+				tmp16 -= 0x40;
+			if (tmp16 < 3) {
+				b43_phy_write(dev, 0x048A,
+					      (b43_phy_read(dev, 0x048A)
+					       & 0xF000) | 0x09EB);
+			} else {
+				b43_phy_write(dev, 0x048A,
+					      (b43_phy_read(dev, 0x048A)
+					       & 0xF000) | 0x0AED);
+			}
+		} else {
+			if (phy->interfmode == B43_INTERFMODE_NONWLAN) {
+				a = 0xE;
+				b = 0xA;
+			} else if (!phy->aci_wlan_automatic && phy->aci_enable) {
+				a = 0x13;
+				b = 0x12;
+			} else {
+				a = 0xE;
+				b = 0x11;
+			}
+
+			a = a * (phy->nrssi[1] - phy->nrssi[0]);
+			a += (phy->nrssi[0] << 6);
+			if (a < 32)
+				a += 31;
+			else
+				a += 32;
+			a = a >> 6;
+			a = limit_value(a, -31, 31);
+
+			b = b * (phy->nrssi[1] - phy->nrssi[0]);
+			b += (phy->nrssi[0] << 6);
+			if (b < 32)
+				b += 31;
+			else
+				b += 32;
+			b = b >> 6;
+			b = limit_value(b, -31, 31);
+
+			tmp_u16 = b43_phy_read(dev, 0x048A) & 0xF000;
+			tmp_u16 |= ((u32) b & 0x0000003F);
+			tmp_u16 |= (((u32) a & 0x0000003F) << 6);
+			b43_phy_write(dev, 0x048A, tmp_u16);
+		}
+		break;
+	default:
+		B43_WARN_ON(1);
+	}
+}
+
+/* Stack implementation to save/restore values from the
+ * interference mitigation code.
+ * It is save to restore values in random order.
+ */
+static void _stack_save(u32 * _stackptr, size_t * stackidx,
+			u8 id, u16 offset, u16 value)
+{
+	u32 *stackptr = &(_stackptr[*stackidx]);
+
+	B43_WARN_ON(offset & 0xF000);
+	B43_WARN_ON(id & 0xF0);
+	*stackptr = offset;
+	*stackptr |= ((u32) id) << 12;
+	*stackptr |= ((u32) value) << 16;
+	(*stackidx)++;
+	B43_WARN_ON(*stackidx >= B43_INTERFSTACK_SIZE);
+}
+
+static u16 _stack_restore(u32 * stackptr, u8 id, u16 offset)
+{
+	size_t i;
+
+	B43_WARN_ON(offset & 0xF000);
+	B43_WARN_ON(id & 0xF0);
+	for (i = 0; i < B43_INTERFSTACK_SIZE; i++, stackptr++) {
+		if ((*stackptr & 0x00000FFF) != offset)
+			continue;
+		if (((*stackptr & 0x0000F000) >> 12) != id)
+			continue;
+		return ((*stackptr & 0xFFFF0000) >> 16);
+	}
+	B43_WARN_ON(1);
+
+	return 0;
+}
+
+#define phy_stacksave(offset)					\
+	do {							\
+		_stack_save(stack, &stackidx, 0x1, (offset),	\
+			    b43_phy_read(dev, (offset)));	\
+	} while (0)
+#define phy_stackrestore(offset)				\
+	do {							\
+		b43_phy_write(dev, (offset),		\
+				  _stack_restore(stack, 0x1,	\
+						 (offset)));	\
+	} while (0)
+#define radio_stacksave(offset)						\
+	do {								\
+		_stack_save(stack, &stackidx, 0x2, (offset),		\
+			    b43_radio_read16(dev, (offset)));	\
+	} while (0)
+#define radio_stackrestore(offset)					\
+	do {								\
+		b43_radio_write16(dev, (offset),			\
+				      _stack_restore(stack, 0x2,	\
+						     (offset)));	\
+	} while (0)
+#define ofdmtab_stacksave(table, offset)			\
+	do {							\
+		_stack_save(stack, &stackidx, 0x3, (offset)|(table),	\
+			    b43_ofdmtab_read16(dev, (table), (offset)));	\
+	} while (0)
+#define ofdmtab_stackrestore(table, offset)			\
+	do {							\
+		b43_ofdmtab_write16(dev, (table),	(offset),	\
+				  _stack_restore(stack, 0x3,	\
+						 (offset)|(table)));	\
+	} while (0)
+
+static void
+b43_radio_interference_mitigation_enable(struct b43_wldev *dev, int mode)
+{
+	struct b43_phy *phy = &dev->phy;
+	u16 tmp, flipped;
+	size_t stackidx = 0;
+	u32 *stack = phy->interfstack;
+
+	switch (mode) {
+	case B43_INTERFMODE_NONWLAN:
+		if (phy->rev != 1) {
+			b43_phy_write(dev, 0x042B,
+				      b43_phy_read(dev, 0x042B) | 0x0800);
+			b43_phy_write(dev, B43_PHY_G_CRS,
+				      b43_phy_read(dev,
+						   B43_PHY_G_CRS) & ~0x4000);
+			break;
+		}
+		radio_stacksave(0x0078);
+		tmp = (b43_radio_read16(dev, 0x0078) & 0x001E);
+		flipped = flip_4bit(tmp);
+		if (flipped < 10 && flipped >= 8)
+			flipped = 7;
+		else if (flipped >= 10)
+			flipped -= 3;
+		flipped = flip_4bit(flipped);
+		flipped = (flipped << 1) | 0x0020;
+		b43_radio_write16(dev, 0x0078, flipped);
+
+		b43_calc_nrssi_threshold(dev);
+
+		phy_stacksave(0x0406);
+		b43_phy_write(dev, 0x0406, 0x7E28);
+
+		b43_phy_write(dev, 0x042B, b43_phy_read(dev, 0x042B) | 0x0800);
+		b43_phy_write(dev, B43_PHY_RADIO_BITFIELD,
+			      b43_phy_read(dev,
+					   B43_PHY_RADIO_BITFIELD) | 0x1000);
+
+		phy_stacksave(0x04A0);
+		b43_phy_write(dev, 0x04A0,
+			      (b43_phy_read(dev, 0x04A0) & 0xC0C0) | 0x0008);
+		phy_stacksave(0x04A1);
+		b43_phy_write(dev, 0x04A1,
+			      (b43_phy_read(dev, 0x04A1) & 0xC0C0) | 0x0605);
+		phy_stacksave(0x04A2);
+		b43_phy_write(dev, 0x04A2,
+			      (b43_phy_read(dev, 0x04A2) & 0xC0C0) | 0x0204);
+		phy_stacksave(0x04A8);
+		b43_phy_write(dev, 0x04A8,
+			      (b43_phy_read(dev, 0x04A8) & 0xC0C0) | 0x0803);
+		phy_stacksave(0x04AB);
+		b43_phy_write(dev, 0x04AB,
+			      (b43_phy_read(dev, 0x04AB) & 0xC0C0) | 0x0605);
+
+		phy_stacksave(0x04A7);
+		b43_phy_write(dev, 0x04A7, 0x0002);
+		phy_stacksave(0x04A3);
+		b43_phy_write(dev, 0x04A3, 0x287A);
+		phy_stacksave(0x04A9);
+		b43_phy_write(dev, 0x04A9, 0x2027);
+		phy_stacksave(0x0493);
+		b43_phy_write(dev, 0x0493, 0x32F5);
+		phy_stacksave(0x04AA);
+		b43_phy_write(dev, 0x04AA, 0x2027);
+		phy_stacksave(0x04AC);
+		b43_phy_write(dev, 0x04AC, 0x32F5);
+		break;
+	case B43_INTERFMODE_MANUALWLAN:
+		if (b43_phy_read(dev, 0x0033) & 0x0800)
+			break;
+
+		phy->aci_enable = 1;
+
+		phy_stacksave(B43_PHY_RADIO_BITFIELD);
+		phy_stacksave(B43_PHY_G_CRS);
+		if (phy->rev < 2) {
+			phy_stacksave(0x0406);
+		} else {
+			phy_stacksave(0x04C0);
+			phy_stacksave(0x04C1);
+		}
+		phy_stacksave(0x0033);
+		phy_stacksave(0x04A7);
+		phy_stacksave(0x04A3);
+		phy_stacksave(0x04A9);
+		phy_stacksave(0x04AA);
+		phy_stacksave(0x04AC);
+		phy_stacksave(0x0493);
+		phy_stacksave(0x04A1);
+		phy_stacksave(0x04A0);
+		phy_stacksave(0x04A2);
+		phy_stacksave(0x048A);
+		phy_stacksave(0x04A8);
+		phy_stacksave(0x04AB);
+		if (phy->rev == 2) {
+			phy_stacksave(0x04AD);
+			phy_stacksave(0x04AE);
+		} else if (phy->rev >= 3) {
+			phy_stacksave(0x04AD);
+			phy_stacksave(0x0415);
+			phy_stacksave(0x0416);
+			phy_stacksave(0x0417);
+			ofdmtab_stacksave(0x1A00, 0x2);
+			ofdmtab_stacksave(0x1A00, 0x3);
+		}
+		phy_stacksave(0x042B);
+		phy_stacksave(0x048C);
+
+		b43_phy_write(dev, B43_PHY_RADIO_BITFIELD,
+			      b43_phy_read(dev, B43_PHY_RADIO_BITFIELD)
+			      & ~0x1000);
+		b43_phy_write(dev, B43_PHY_G_CRS,
+			      (b43_phy_read(dev, B43_PHY_G_CRS)
+			       & 0xFFFC) | 0x0002);
+
+		b43_phy_write(dev, 0x0033, 0x0800);
+		b43_phy_write(dev, 0x04A3, 0x2027);
+		b43_phy_write(dev, 0x04A9, 0x1CA8);
+		b43_phy_write(dev, 0x0493, 0x287A);
+		b43_phy_write(dev, 0x04AA, 0x1CA8);
+		b43_phy_write(dev, 0x04AC, 0x287A);
+
+		b43_phy_write(dev, 0x04A0, (b43_phy_read(dev, 0x04A0)
+					    & 0xFFC0) | 0x001A);
+		b43_phy_write(dev, 0x04A7, 0x000D);
+
+		if (phy->rev < 2) {
+			b43_phy_write(dev, 0x0406, 0xFF0D);
+		} else if (phy->rev == 2) {
+			b43_phy_write(dev, 0x04C0, 0xFFFF);
+			b43_phy_write(dev, 0x04C1, 0x00A9);
+		} else {
+			b43_phy_write(dev, 0x04C0, 0x00C1);
+			b43_phy_write(dev, 0x04C1, 0x0059);
+		}
+
+		b43_phy_write(dev, 0x04A1, (b43_phy_read(dev, 0x04A1)
+					    & 0xC0FF) | 0x1800);
+		b43_phy_write(dev, 0x04A1, (b43_phy_read(dev, 0x04A1)
+					    & 0xFFC0) | 0x0015);
+		b43_phy_write(dev, 0x04A8, (b43_phy_read(dev, 0x04A8)
+					    & 0xCFFF) | 0x1000);
+		b43_phy_write(dev, 0x04A8, (b43_phy_read(dev, 0x04A8)
+					    & 0xF0FF) | 0x0A00);
+		b43_phy_write(dev, 0x04AB, (b43_phy_read(dev, 0x04AB)
+					    & 0xCFFF) | 0x1000);
+		b43_phy_write(dev, 0x04AB, (b43_phy_read(dev, 0x04AB)
+					    & 0xF0FF) | 0x0800);
+		b43_phy_write(dev, 0x04AB, (b43_phy_read(dev, 0x04AB)
+					    & 0xFFCF) | 0x0010);
+		b43_phy_write(dev, 0x04AB, (b43_phy_read(dev, 0x04AB)
+					    & 0xFFF0) | 0x0005);
+		b43_phy_write(dev, 0x04A8, (b43_phy_read(dev, 0x04A8)
+					    & 0xFFCF) | 0x0010);
+		b43_phy_write(dev, 0x04A8, (b43_phy_read(dev, 0x04A8)
+					    & 0xFFF0) | 0x0006);
+		b43_phy_write(dev, 0x04A2, (b43_phy_read(dev, 0x04A2)
+					    & 0xF0FF) | 0x0800);
+		b43_phy_write(dev, 0x04A0, (b43_phy_read(dev, 0x04A0)
+					    & 0xF0FF) | 0x0500);
+		b43_phy_write(dev, 0x04A2, (b43_phy_read(dev, 0x04A2)
+					    & 0xFFF0) | 0x000B);
+
+		if (phy->rev >= 3) {
+			b43_phy_write(dev, 0x048A, b43_phy_read(dev, 0x048A)
+				      & ~0x8000);
+			b43_phy_write(dev, 0x0415, (b43_phy_read(dev, 0x0415)
+						    & 0x8000) | 0x36D8);
+			b43_phy_write(dev, 0x0416, (b43_phy_read(dev, 0x0416)
+						    & 0x8000) | 0x36D8);
+			b43_phy_write(dev, 0x0417, (b43_phy_read(dev, 0x0417)
+						    & 0xFE00) | 0x016D);
+		} else {
+			b43_phy_write(dev, 0x048A, b43_phy_read(dev, 0x048A)
+				      | 0x1000);
+			b43_phy_write(dev, 0x048A, (b43_phy_read(dev, 0x048A)
+						    & 0x9FFF) | 0x2000);
+			b43_hf_write(dev, b43_hf_read(dev) | B43_HF_ACIW);
+		}
+		if (phy->rev >= 2) {
+			b43_phy_write(dev, 0x042B, b43_phy_read(dev, 0x042B)
+				      | 0x0800);
+		}
+		b43_phy_write(dev, 0x048C, (b43_phy_read(dev, 0x048C)
+					    & 0xF0FF) | 0x0200);
+		if (phy->rev == 2) {
+			b43_phy_write(dev, 0x04AE, (b43_phy_read(dev, 0x04AE)
+						    & 0xFF00) | 0x007F);
+			b43_phy_write(dev, 0x04AD, (b43_phy_read(dev, 0x04AD)
+						    & 0x00FF) | 0x1300);
+		} else if (phy->rev >= 6) {
+			b43_ofdmtab_write16(dev, 0x1A00, 0x3, 0x007F);
+			b43_ofdmtab_write16(dev, 0x1A00, 0x2, 0x007F);
+			b43_phy_write(dev, 0x04AD, b43_phy_read(dev, 0x04AD)
+				      & 0x00FF);
+		}
+		b43_calc_nrssi_slope(dev);
+		break;
+	default:
+		B43_WARN_ON(1);
+	}
+}
+
+static void
+b43_radio_interference_mitigation_disable(struct b43_wldev *dev, int mode)
+{
+	struct b43_phy *phy = &dev->phy;
+	u32 *stack = phy->interfstack;
+
+	switch (mode) {
+	case B43_INTERFMODE_NONWLAN:
+		if (phy->rev != 1) {
+			b43_phy_write(dev, 0x042B,
+				      b43_phy_read(dev, 0x042B) & ~0x0800);
+			b43_phy_write(dev, B43_PHY_G_CRS,
+				      b43_phy_read(dev,
+						   B43_PHY_G_CRS) | 0x4000);
+			break;
+		}
+		radio_stackrestore(0x0078);
+		b43_calc_nrssi_threshold(dev);
+		phy_stackrestore(0x0406);
+		b43_phy_write(dev, 0x042B, b43_phy_read(dev, 0x042B) & ~0x0800);
+		if (!dev->bad_frames_preempt) {
+			b43_phy_write(dev, B43_PHY_RADIO_BITFIELD,
+				      b43_phy_read(dev, B43_PHY_RADIO_BITFIELD)
+				      & ~(1 << 11));
+		}
+		b43_phy_write(dev, B43_PHY_G_CRS,
+			      b43_phy_read(dev, B43_PHY_G_CRS) | 0x4000);
+		phy_stackrestore(0x04A0);
+		phy_stackrestore(0x04A1);
+		phy_stackrestore(0x04A2);
+		phy_stackrestore(0x04A8);
+		phy_stackrestore(0x04AB);
+		phy_stackrestore(0x04A7);
+		phy_stackrestore(0x04A3);
+		phy_stackrestore(0x04A9);
+		phy_stackrestore(0x0493);
+		phy_stackrestore(0x04AA);
+		phy_stackrestore(0x04AC);
+		break;
+	case B43_INTERFMODE_MANUALWLAN:
+		if (!(b43_phy_read(dev, 0x0033) & 0x0800))
+			break;
+
+		phy->aci_enable = 0;
+
+		phy_stackrestore(B43_PHY_RADIO_BITFIELD);
+		phy_stackrestore(B43_PHY_G_CRS);
+		phy_stackrestore(0x0033);
+		phy_stackrestore(0x04A3);
+		phy_stackrestore(0x04A9);
+		phy_stackrestore(0x0493);
+		phy_stackrestore(0x04AA);
+		phy_stackrestore(0x04AC);
+		phy_stackrestore(0x04A0);
+		phy_stackrestore(0x04A7);
+		if (phy->rev >= 2) {
+			phy_stackrestore(0x04C0);
+			phy_stackrestore(0x04C1);
+		} else
+			phy_stackrestore(0x0406);
+		phy_stackrestore(0x04A1);
+		phy_stackrestore(0x04AB);
+		phy_stackrestore(0x04A8);
+		if (phy->rev == 2) {
+			phy_stackrestore(0x04AD);
+			phy_stackrestore(0x04AE);
+		} else if (phy->rev >= 3) {
+			phy_stackrestore(0x04AD);
+			phy_stackrestore(0x0415);
+			phy_stackrestore(0x0416);
+			phy_stackrestore(0x0417);
+			ofdmtab_stackrestore(0x1A00, 0x2);
+			ofdmtab_stackrestore(0x1A00, 0x3);
+		}
+		phy_stackrestore(0x04A2);
+		phy_stackrestore(0x048A);
+		phy_stackrestore(0x042B);
+		phy_stackrestore(0x048C);
+		b43_hf_write(dev, b43_hf_read(dev) & ~B43_HF_ACIW);
+		b43_calc_nrssi_slope(dev);
+		break;
+	default:
+		B43_WARN_ON(1);
+	}
+}
+
+#undef phy_stacksave
+#undef phy_stackrestore
+#undef radio_stacksave
+#undef radio_stackrestore
+#undef ofdmtab_stacksave
+#undef ofdmtab_stackrestore
+
+int b43_radio_set_interference_mitigation(struct b43_wldev *dev, int mode)
+{
+	struct b43_phy *phy = &dev->phy;
+	int currentmode;
+
+	if ((phy->type != B43_PHYTYPE_G) || (phy->rev == 0) || (!phy->gmode))
+		return -ENODEV;
+
+	phy->aci_wlan_automatic = 0;
+	switch (mode) {
+	case B43_INTERFMODE_AUTOWLAN:
+		phy->aci_wlan_automatic = 1;
+		if (phy->aci_enable)
+			mode = B43_INTERFMODE_MANUALWLAN;
+		else
+			mode = B43_INTERFMODE_NONE;
+		break;
+	case B43_INTERFMODE_NONE:
+	case B43_INTERFMODE_NONWLAN:
+	case B43_INTERFMODE_MANUALWLAN:
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	currentmode = phy->interfmode;
+	if (currentmode == mode)
+		return 0;
+	if (currentmode != B43_INTERFMODE_NONE)
+		b43_radio_interference_mitigation_disable(dev, currentmode);
+
+	if (mode == B43_INTERFMODE_NONE) {
+		phy->aci_enable = 0;
+		phy->aci_hw_rssi = 0;
+	} else
+		b43_radio_interference_mitigation_enable(dev, mode);
+	phy->interfmode = mode;
+
+	return 0;
+}
+
+static u16 b43_radio_core_calibration_value(struct b43_wldev *dev)
+{
+	u16 reg, index, ret;
+
+	static const u8 rcc_table[] = {
+		0x02, 0x03, 0x01, 0x0F,
+		0x06, 0x07, 0x05, 0x0F,
+		0x0A, 0x0B, 0x09, 0x0F,
+		0x0E, 0x0F, 0x0D, 0x0F,
+	};
+
+	reg = b43_radio_read16(dev, 0x60);
+	index = (reg & 0x001E) >> 1;
+	ret = rcc_table[index] << 1;
+	ret |= (reg & 0x0001);
+	ret |= 0x0020;
+
+	return ret;
+}
+
+#define LPD(L, P, D)	(((L) << 2) | ((P) << 1) | ((D) << 0))
+static u16 radio2050_rfover_val(struct b43_wldev *dev,
+				u16 phy_register, unsigned int lpd)
+{
+	struct b43_phy *phy = &dev->phy;
+	struct ssb_sprom *sprom = &(dev->dev->bus->sprom);
+
+	if (!phy->gmode)
+		return 0;
+
+	if (has_loopback_gain(phy)) {
+		int max_lb_gain = phy->max_lb_gain;
+		u16 extlna;
+		u16 i;
+
+		if (phy->radio_rev == 8)
+			max_lb_gain += 0x3E;
+		else
+			max_lb_gain += 0x26;
+		if (max_lb_gain >= 0x46) {
+			extlna = 0x3000;
+			max_lb_gain -= 0x46;
+		} else if (max_lb_gain >= 0x3A) {
+			extlna = 0x1000;
+			max_lb_gain -= 0x3A;
+		} else if (max_lb_gain >= 0x2E) {
+			extlna = 0x2000;
+			max_lb_gain -= 0x2E;
+		} else {
+			extlna = 0;
+			max_lb_gain -= 0x10;
+		}
+
+		for (i = 0; i < 16; i++) {
+			max_lb_gain -= (i * 6);
+			if (max_lb_gain < 6)
+				break;
+		}
+
+		if ((phy->rev < 7) ||
+		    !(sprom->r1.boardflags_lo & B43_BFL_EXTLNA)) {
+			if (phy_register == B43_PHY_RFOVER) {
+				return 0x1B3;
+			} else if (phy_register == B43_PHY_RFOVERVAL) {
+				extlna |= (i << 8);
+				switch (lpd) {
+				case LPD(0, 1, 1):
+					return 0x0F92;
+				case LPD(0, 0, 1):
+				case LPD(1, 0, 1):
+					return (0x0092 | extlna);
+				case LPD(1, 0, 0):
+					return (0x0093 | extlna);
+				}
+				B43_WARN_ON(1);
+			}
+			B43_WARN_ON(1);
+		} else {
+			if (phy_register == B43_PHY_RFOVER) {
+				return 0x9B3;
+			} else if (phy_register == B43_PHY_RFOVERVAL) {
+				if (extlna)
+					extlna |= 0x8000;
+				extlna |= (i << 8);
+				switch (lpd) {
+				case LPD(0, 1, 1):
+					return 0x8F92;
+				case LPD(0, 0, 1):
+					return (0x8092 | extlna);
+				case LPD(1, 0, 1):
+					return (0x2092 | extlna);
+				case LPD(1, 0, 0):
+					return (0x2093 | extlna);
+				}
+				B43_WARN_ON(1);
+			}
+			B43_WARN_ON(1);
+		}
+	} else {
+		if ((phy->rev < 7) ||
+		    !(sprom->r1.boardflags_lo & B43_BFL_EXTLNA)) {
+			if (phy_register == B43_PHY_RFOVER) {
+				return 0x1B3;
+			} else if (phy_register == B43_PHY_RFOVERVAL) {
+				switch (lpd) {
+				case LPD(0, 1, 1):
+					return 0x0FB2;
+				case LPD(0, 0, 1):
+					return 0x00B2;
+				case LPD(1, 0, 1):
+					return 0x30B2;
+				case LPD(1, 0, 0):
+					return 0x30B3;
+				}
+				B43_WARN_ON(1);
+			}
+			B43_WARN_ON(1);
+		} else {
+			if (phy_register == B43_PHY_RFOVER) {
+				return 0x9B3;
+			} else if (phy_register == B43_PHY_RFOVERVAL) {
+				switch (lpd) {
+				case LPD(0, 1, 1):
+					return 0x8FB2;
+				case LPD(0, 0, 1):
+					return 0x80B2;
+				case LPD(1, 0, 1):
+					return 0x20B2;
+				case LPD(1, 0, 0):
+					return 0x20B3;
+				}
+				B43_WARN_ON(1);
+			}
+			B43_WARN_ON(1);
+		}
+	}
+	return 0;
+}
+
+struct init2050_saved_values {
+	/* Core registers */
+	u16 reg_3EC;
+	u16 reg_3E6;
+	u16 reg_3F4;
+	/* Radio registers */
+	u16 radio_43;
+	u16 radio_51;
+	u16 radio_52;
+	/* PHY registers */
+	u16 phy_pgactl;
+	u16 phy_base_5A;
+	u16 phy_base_59;
+	u16 phy_base_58;
+	u16 phy_base_30;
+	u16 phy_rfover;
+	u16 phy_rfoverval;
+	u16 phy_analogover;
+	u16 phy_analogoverval;
+	u16 phy_crs0;
+	u16 phy_classctl;
+	u16 phy_lo_mask;
+	u16 phy_lo_ctl;
+	u16 phy_syncctl;
+};
+
+u16 b43_radio_init2050(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+	struct init2050_saved_values sav;
+	u16 rcc;
+	u16 radio78;
+	u16 ret;
+	u16 i, j;
+	u32 tmp1 = 0, tmp2 = 0;
+
+	memset(&sav, 0, sizeof(sav));	/* get rid of "may be used uninitialized..." */
+
+	sav.radio_43 = b43_radio_read16(dev, 0x43);
+	sav.radio_51 = b43_radio_read16(dev, 0x51);
+	sav.radio_52 = b43_radio_read16(dev, 0x52);
+	sav.phy_pgactl = b43_phy_read(dev, B43_PHY_PGACTL);
+	sav.phy_base_5A = b43_phy_read(dev, B43_PHY_BASE(0x5A));
+	sav.phy_base_59 = b43_phy_read(dev, B43_PHY_BASE(0x59));
+	sav.phy_base_58 = b43_phy_read(dev, B43_PHY_BASE(0x58));
+
+	if (phy->type == B43_PHYTYPE_B) {
+		sav.phy_base_30 = b43_phy_read(dev, B43_PHY_BASE(0x30));
+		sav.reg_3EC = b43_read16(dev, 0x3EC);
+
+		b43_phy_write(dev, B43_PHY_BASE(0x30), 0xFF);
+		b43_write16(dev, 0x3EC, 0x3F3F);
+	} else if (phy->gmode || phy->rev >= 2) {
+		sav.phy_rfover = b43_phy_read(dev, B43_PHY_RFOVER);
+		sav.phy_rfoverval = b43_phy_read(dev, B43_PHY_RFOVERVAL);
+		sav.phy_analogover = b43_phy_read(dev, B43_PHY_ANALOGOVER);
+		sav.phy_analogoverval =
+		    b43_phy_read(dev, B43_PHY_ANALOGOVERVAL);
+		sav.phy_crs0 = b43_phy_read(dev, B43_PHY_CRS0);
+		sav.phy_classctl = b43_phy_read(dev, B43_PHY_CLASSCTL);
+
+		b43_phy_write(dev, B43_PHY_ANALOGOVER,
+			      b43_phy_read(dev, B43_PHY_ANALOGOVER)
+			      | 0x0003);
+		b43_phy_write(dev, B43_PHY_ANALOGOVERVAL,
+			      b43_phy_read(dev, B43_PHY_ANALOGOVERVAL)
+			      & 0xFFFC);
+		b43_phy_write(dev, B43_PHY_CRS0, b43_phy_read(dev, B43_PHY_CRS0)
+			      & 0x7FFF);
+		b43_phy_write(dev, B43_PHY_CLASSCTL,
+			      b43_phy_read(dev, B43_PHY_CLASSCTL)
+			      & 0xFFFC);
+		if (has_loopback_gain(phy)) {
+			sav.phy_lo_mask = b43_phy_read(dev, B43_PHY_LO_MASK);
+			sav.phy_lo_ctl = b43_phy_read(dev, B43_PHY_LO_CTL);
+
+			if (phy->rev >= 3)
+				b43_phy_write(dev, B43_PHY_LO_MASK, 0xC020);
+			else
+				b43_phy_write(dev, B43_PHY_LO_MASK, 0x8020);
+			b43_phy_write(dev, B43_PHY_LO_CTL, 0);
+		}
+
+		b43_phy_write(dev, B43_PHY_RFOVERVAL,
+			      radio2050_rfover_val(dev, B43_PHY_RFOVERVAL,
+						   LPD(0, 1, 1)));
+		b43_phy_write(dev, B43_PHY_RFOVER,
+			      radio2050_rfover_val(dev, B43_PHY_RFOVER, 0));
+	}
+	b43_write16(dev, 0x3E2, b43_read16(dev, 0x3E2) | 0x8000);
+
+	sav.phy_syncctl = b43_phy_read(dev, B43_PHY_SYNCCTL);
+	b43_phy_write(dev, B43_PHY_SYNCCTL, b43_phy_read(dev, B43_PHY_SYNCCTL)
+		      & 0xFF7F);
+	sav.reg_3E6 = b43_read16(dev, 0x3E6);
+	sav.reg_3F4 = b43_read16(dev, 0x3F4);
+
+	if (phy->analog == 0) {
+		b43_write16(dev, 0x03E6, 0x0122);
+	} else {
+		if (phy->analog >= 2) {
+			b43_phy_write(dev, B43_PHY_BASE(0x03),
+				      (b43_phy_read(dev, B43_PHY_BASE(0x03))
+				       & 0xFFBF) | 0x40);
+		}
+		b43_write16(dev, B43_MMIO_CHANNEL_EXT,
+			    (b43_read16(dev, B43_MMIO_CHANNEL_EXT) | 0x2000));
+	}
+
+	rcc = b43_radio_core_calibration_value(dev);
+
+	if (phy->type == B43_PHYTYPE_B)
+		b43_radio_write16(dev, 0x78, 0x26);
+	if (phy->gmode || phy->rev >= 2) {
+		b43_phy_write(dev, B43_PHY_RFOVERVAL,
+			      radio2050_rfover_val(dev, B43_PHY_RFOVERVAL,
+						   LPD(0, 1, 1)));
+	}
+	b43_phy_write(dev, B43_PHY_PGACTL, 0xBFAF);
+	b43_phy_write(dev, B43_PHY_BASE(0x2B), 0x1403);
+	if (phy->gmode || phy->rev >= 2) {
+		b43_phy_write(dev, B43_PHY_RFOVERVAL,
+			      radio2050_rfover_val(dev, B43_PHY_RFOVERVAL,
+						   LPD(0, 0, 1)));
+	}
+	b43_phy_write(dev, B43_PHY_PGACTL, 0xBFA0);
+	b43_radio_write16(dev, 0x51, b43_radio_read16(dev, 0x51)
+			  | 0x0004);
+	if (phy->radio_rev == 8) {
+		b43_radio_write16(dev, 0x43, 0x1F);
+	} else {
+		b43_radio_write16(dev, 0x52, 0);
+		b43_radio_write16(dev, 0x43, (b43_radio_read16(dev, 0x43)
+					      & 0xFFF0) | 0x0009);
+	}
+	b43_phy_write(dev, B43_PHY_BASE(0x58), 0);
+
+	for (i = 0; i < 16; i++) {
+		b43_phy_write(dev, B43_PHY_BASE(0x5A), 0x0480);
+		b43_phy_write(dev, B43_PHY_BASE(0x59), 0xC810);
+		b43_phy_write(dev, B43_PHY_BASE(0x58), 0x000D);
+		if (phy->gmode || phy->rev >= 2) {
+			b43_phy_write(dev, B43_PHY_RFOVERVAL,
+				      radio2050_rfover_val(dev,
+							   B43_PHY_RFOVERVAL,
+							   LPD(1, 0, 1)));
+		}
+		b43_phy_write(dev, B43_PHY_PGACTL, 0xAFB0);
+		udelay(10);
+		if (phy->gmode || phy->rev >= 2) {
+			b43_phy_write(dev, B43_PHY_RFOVERVAL,
+				      radio2050_rfover_val(dev,
+							   B43_PHY_RFOVERVAL,
+							   LPD(1, 0, 1)));
+		}
+		b43_phy_write(dev, B43_PHY_PGACTL, 0xEFB0);
+		udelay(10);
+		if (phy->gmode || phy->rev >= 2) {
+			b43_phy_write(dev, B43_PHY_RFOVERVAL,
+				      radio2050_rfover_val(dev,
+							   B43_PHY_RFOVERVAL,
+							   LPD(1, 0, 0)));
+		}
+		b43_phy_write(dev, B43_PHY_PGACTL, 0xFFF0);
+		udelay(20);
+		tmp1 += b43_phy_read(dev, B43_PHY_LO_LEAKAGE);
+		b43_phy_write(dev, B43_PHY_BASE(0x58), 0);
+		if (phy->gmode || phy->rev >= 2) {
+			b43_phy_write(dev, B43_PHY_RFOVERVAL,
+				      radio2050_rfover_val(dev,
+							   B43_PHY_RFOVERVAL,
+							   LPD(1, 0, 1)));
+		}
+		b43_phy_write(dev, B43_PHY_PGACTL, 0xAFB0);
+	}
+	udelay(10);
+
+	b43_phy_write(dev, B43_PHY_BASE(0x58), 0);
+	tmp1++;
+	tmp1 >>= 9;
+
+	for (i = 0; i < 16; i++) {
+		radio78 = ((flip_4bit(i) << 1) | 0x20);
+		b43_radio_write16(dev, 0x78, radio78);
+		udelay(10);
+		for (j = 0; j < 16; j++) {
+			b43_phy_write(dev, B43_PHY_BASE(0x5A), 0x0D80);
+			b43_phy_write(dev, B43_PHY_BASE(0x59), 0xC810);
+			b43_phy_write(dev, B43_PHY_BASE(0x58), 0x000D);
+			if (phy->gmode || phy->rev >= 2) {
+				b43_phy_write(dev, B43_PHY_RFOVERVAL,
+					      radio2050_rfover_val(dev,
+								   B43_PHY_RFOVERVAL,
+								   LPD(1, 0,
+								       1)));
+			}
+			b43_phy_write(dev, B43_PHY_PGACTL, 0xAFB0);
+			udelay(10);
+			if (phy->gmode || phy->rev >= 2) {
+				b43_phy_write(dev, B43_PHY_RFOVERVAL,
+					      radio2050_rfover_val(dev,
+								   B43_PHY_RFOVERVAL,
+								   LPD(1, 0,
+								       1)));
+			}
+			b43_phy_write(dev, B43_PHY_PGACTL, 0xEFB0);
+			udelay(10);
+			if (phy->gmode || phy->rev >= 2) {
+				b43_phy_write(dev, B43_PHY_RFOVERVAL,
+					      radio2050_rfover_val(dev,
+								   B43_PHY_RFOVERVAL,
+								   LPD(1, 0,
+								       0)));
+			}
+			b43_phy_write(dev, B43_PHY_PGACTL, 0xFFF0);
+			udelay(10);
+			tmp2 += b43_phy_read(dev, B43_PHY_LO_LEAKAGE);
+			b43_phy_write(dev, B43_PHY_BASE(0x58), 0);
+			if (phy->gmode || phy->rev >= 2) {
+				b43_phy_write(dev, B43_PHY_RFOVERVAL,
+					      radio2050_rfover_val(dev,
+								   B43_PHY_RFOVERVAL,
+								   LPD(1, 0,
+								       1)));
+			}
+			b43_phy_write(dev, B43_PHY_PGACTL, 0xAFB0);
+		}
+		tmp2++;
+		tmp2 >>= 8;
+		if (tmp1 < tmp2)
+			break;
+	}
+
+	/* Restore the registers */
+	b43_phy_write(dev, B43_PHY_PGACTL, sav.phy_pgactl);
+	b43_radio_write16(dev, 0x51, sav.radio_51);
+	b43_radio_write16(dev, 0x52, sav.radio_52);
+	b43_radio_write16(dev, 0x43, sav.radio_43);
+	b43_phy_write(dev, B43_PHY_BASE(0x5A), sav.phy_base_5A);
+	b43_phy_write(dev, B43_PHY_BASE(0x59), sav.phy_base_59);
+	b43_phy_write(dev, B43_PHY_BASE(0x58), sav.phy_base_58);
+	b43_write16(dev, 0x3E6, sav.reg_3E6);
+	if (phy->analog != 0)
+		b43_write16(dev, 0x3F4, sav.reg_3F4);
+	b43_phy_write(dev, B43_PHY_SYNCCTL, sav.phy_syncctl);
+	b43_synth_pu_workaround(dev, phy->channel);
+	if (phy->type == B43_PHYTYPE_B) {
+		b43_phy_write(dev, B43_PHY_BASE(0x30), sav.phy_base_30);
+		b43_write16(dev, 0x3EC, sav.reg_3EC);
+	} else if (phy->gmode) {
+		b43_write16(dev, B43_MMIO_PHY_RADIO,
+			    b43_read16(dev, B43_MMIO_PHY_RADIO)
+			    & 0x7FFF);
+		b43_phy_write(dev, B43_PHY_RFOVER, sav.phy_rfover);
+		b43_phy_write(dev, B43_PHY_RFOVERVAL, sav.phy_rfoverval);
+		b43_phy_write(dev, B43_PHY_ANALOGOVER, sav.phy_analogover);
+		b43_phy_write(dev, B43_PHY_ANALOGOVERVAL,
+			      sav.phy_analogoverval);
+		b43_phy_write(dev, B43_PHY_CRS0, sav.phy_crs0);
+		b43_phy_write(dev, B43_PHY_CLASSCTL, sav.phy_classctl);
+		if (has_loopback_gain(phy)) {
+			b43_phy_write(dev, B43_PHY_LO_MASK, sav.phy_lo_mask);
+			b43_phy_write(dev, B43_PHY_LO_CTL, sav.phy_lo_ctl);
+		}
+	}
+	if (i > 15)
+		ret = radio78;
+	else
+		ret = rcc;
+
+	return ret;
+}
+
+void b43_radio_init2060(struct b43_wldev *dev)
+{
+	int err;
+
+	b43_radio_write16(dev, 0x0004, 0x00C0);
+	b43_radio_write16(dev, 0x0005, 0x0008);
+	b43_radio_write16(dev, 0x0009, 0x0040);
+	b43_radio_write16(dev, 0x0005, 0x00AA);
+	b43_radio_write16(dev, 0x0032, 0x008F);
+	b43_radio_write16(dev, 0x0006, 0x008F);
+	b43_radio_write16(dev, 0x0034, 0x008F);
+	b43_radio_write16(dev, 0x002C, 0x0007);
+	b43_radio_write16(dev, 0x0082, 0x0080);
+	b43_radio_write16(dev, 0x0080, 0x0000);
+	b43_radio_write16(dev, 0x003F, 0x00DA);
+	b43_radio_write16(dev, 0x0005, b43_radio_read16(dev, 0x0005) & ~0x0008);
+	b43_radio_write16(dev, 0x0081, b43_radio_read16(dev, 0x0081) & ~0x0010);
+	b43_radio_write16(dev, 0x0081, b43_radio_read16(dev, 0x0081) & ~0x0020);
+	b43_radio_write16(dev, 0x0081, b43_radio_read16(dev, 0x0081) & ~0x0020);
+	msleep(1);		/* delay 400usec */
+
+	b43_radio_write16(dev, 0x0081,
+			  (b43_radio_read16(dev, 0x0081) & ~0x0020) | 0x0010);
+	msleep(1);		/* delay 400usec */
+
+	b43_radio_write16(dev, 0x0005,
+			  (b43_radio_read16(dev, 0x0005) & ~0x0008) | 0x0008);
+	b43_radio_write16(dev, 0x0085, b43_radio_read16(dev, 0x0085) & ~0x0010);
+	b43_radio_write16(dev, 0x0005, b43_radio_read16(dev, 0x0005) & ~0x0008);
+	b43_radio_write16(dev, 0x0081, b43_radio_read16(dev, 0x0081) & ~0x0040);
+	b43_radio_write16(dev, 0x0081,
+			  (b43_radio_read16(dev, 0x0081) & ~0x0040) | 0x0040);
+	b43_radio_write16(dev, 0x0005,
+			  (b43_radio_read16(dev, 0x0081) & ~0x0008) | 0x0008);
+	b43_phy_write(dev, 0x0063, 0xDDC6);
+	b43_phy_write(dev, 0x0069, 0x07BE);
+	b43_phy_write(dev, 0x006A, 0x0000);
+
+	err = b43_radio_selectchannel(dev, B43_DEFAULT_CHANNEL_A, 0);
+	B43_WARN_ON(err);
+
+	msleep(1);
+}
+
+static inline u16 freq_r3A_value(u16 frequency)
+{
+	u16 value;
+
+	if (frequency < 5091)
+		value = 0x0040;
+	else if (frequency < 5321)
+		value = 0x0000;
+	else if (frequency < 5806)
+		value = 0x0080;
+	else
+		value = 0x0040;
+
+	return value;
+}
+
+void b43_radio_set_tx_iq(struct b43_wldev *dev)
+{
+	static const u8 data_high[5] = { 0x00, 0x40, 0x80, 0x90, 0xD0 };
+	static const u8 data_low[5] = { 0x00, 0x01, 0x05, 0x06, 0x0A };
+	u16 tmp = b43_radio_read16(dev, 0x001E);
+	int i, j;
+
+	for (i = 0; i < 5; i++) {
+		for (j = 0; j < 5; j++) {
+			if (tmp == (data_high[i] << 4 | data_low[j])) {
+				b43_phy_write(dev, 0x0069,
+					      (i - j) << 8 | 0x00C0);
+				return;
+			}
+		}
+	}
+}
+
+int b43_radio_selectchannel(struct b43_wldev *dev,
+			    u8 channel, int synthetic_pu_workaround)
+{
+	struct b43_phy *phy = &dev->phy;
+	u16 r8, tmp;
+	u16 freq;
+	u16 channelcookie;
+
+	if (channel == 0xFF) {
+		switch (phy->type) {
+		case B43_PHYTYPE_A:
+			channel = B43_DEFAULT_CHANNEL_A;
+			break;
+		case B43_PHYTYPE_B:
+		case B43_PHYTYPE_G:
+			channel = B43_DEFAULT_CHANNEL_BG;
+			break;
+		default:
+			B43_WARN_ON(1);
+		}
+	}
+
+	/* First we set the channel radio code to prevent the
+	 * firmware from sending ghost packets.
+	 */
+	channelcookie = channel;
+	if (phy->type == B43_PHYTYPE_A)
+		channelcookie |= 0x100;
+	b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_CHAN, channelcookie);
+
+	if (phy->type == B43_PHYTYPE_A) {
+		if (channel > 200)
+			return -EINVAL;
+		freq = channel2freq_a(channel);
+
+		r8 = b43_radio_read16(dev, 0x0008);
+		b43_write16(dev, 0x03F0, freq);
+		b43_radio_write16(dev, 0x0008, r8);
+
+		//TODO: write max channel TX power? to Radio 0x2D
+		tmp = b43_radio_read16(dev, 0x002E);
+		tmp &= 0x0080;
+		//TODO: OR tmp with the Power out estimation for this channel?
+		b43_radio_write16(dev, 0x002E, tmp);
+
+		if (freq >= 4920 && freq <= 5500) {
+			/*
+			 * r8 = (((freq * 15 * 0xE1FC780F) >> 32) / 29) & 0x0F;
+			 *    = (freq * 0.025862069
+			 */
+			r8 = 3 * freq / 116;	/* is equal to r8 = freq * 0.025862 */
+		}
+		b43_radio_write16(dev, 0x0007, (r8 << 4) | r8);
+		b43_radio_write16(dev, 0x0020, (r8 << 4) | r8);
+		b43_radio_write16(dev, 0x0021, (r8 << 4) | r8);
+		b43_radio_write16(dev, 0x0022, (b43_radio_read16(dev, 0x0022)
+						& 0x000F) | (r8 << 4));
+		b43_radio_write16(dev, 0x002A, (r8 << 4));
+		b43_radio_write16(dev, 0x002B, (r8 << 4));
+		b43_radio_write16(dev, 0x0008, (b43_radio_read16(dev, 0x0008)
+						& 0x00F0) | (r8 << 4));
+		b43_radio_write16(dev, 0x0029, (b43_radio_read16(dev, 0x0029)
+						& 0xFF0F) | 0x00B0);
+		b43_radio_write16(dev, 0x0035, 0x00AA);
+		b43_radio_write16(dev, 0x0036, 0x0085);
+		b43_radio_write16(dev, 0x003A, (b43_radio_read16(dev, 0x003A)
+						& 0xFF20) |
+				  freq_r3A_value(freq));
+		b43_radio_write16(dev, 0x003D,
+				  b43_radio_read16(dev, 0x003D) & 0x00FF);
+		b43_radio_write16(dev, 0x0081, (b43_radio_read16(dev, 0x0081)
+						& 0xFF7F) | 0x0080);
+		b43_radio_write16(dev, 0x0035,
+				  b43_radio_read16(dev, 0x0035) & 0xFFEF);
+		b43_radio_write16(dev, 0x0035, (b43_radio_read16(dev, 0x0035)
+						& 0xFFEF) | 0x0010);
+		b43_radio_set_tx_iq(dev);
+		//TODO: TSSI2dbm workaround
+		b43_phy_xmitpower(dev);	//FIXME correct?
+	} else {
+		if ((channel < 1) || (channel > 14))
+			return -EINVAL;
+
+		if (synthetic_pu_workaround)
+			b43_synth_pu_workaround(dev, channel);
+
+		b43_write16(dev, B43_MMIO_CHANNEL, channel2freq_bg(channel));
+
+		if (channel == 14) {
+			if (dev->dev->bus->sprom.r1.country_code ==
+			    SSB_SPROM1CCODE_JAPAN)
+				b43_hf_write(dev,
+					     b43_hf_read(dev) & ~B43_HF_ACPR);
+			else
+				b43_hf_write(dev,
+					     b43_hf_read(dev) | B43_HF_ACPR);
+			b43_write16(dev, B43_MMIO_CHANNEL_EXT,
+				    b43_read16(dev, B43_MMIO_CHANNEL_EXT)
+				    | (1 << 11));
+		} else {
+			b43_write16(dev, B43_MMIO_CHANNEL_EXT,
+				    b43_read16(dev, B43_MMIO_CHANNEL_EXT)
+				    & 0xF7BF);
+		}
+	}
+
+	phy->channel = channel;
+	/* Wait for the radio to tune to the channel and stabilize. */
+	msleep(8);
+
+	return 0;
+}
+
+/* http://bcm-specs.sipsolutions.net/TX_Gain_Base_Band */
+static u16 b43_get_txgain_base_band(u16 txpower)
+{
+	u16 ret;
+
+	B43_WARN_ON(txpower > 63);
+
+	if (txpower >= 54)
+		ret = 2;
+	else if (txpower >= 49)
+		ret = 4;
+	else if (txpower >= 44)
+		ret = 5;
+	else
+		ret = 6;
+
+	return ret;
+}
+
+/* http://bcm-specs.sipsolutions.net/TX_Gain_Radio_Frequency_Power_Amplifier */
+static u16 b43_get_txgain_freq_power_amp(u16 txpower)
+{
+	u16 ret;
+
+	B43_WARN_ON(txpower > 63);
+
+	if (txpower >= 32)
+		ret = 0;
+	else if (txpower >= 25)
+		ret = 1;
+	else if (txpower >= 20)
+		ret = 2;
+	else if (txpower >= 12)
+		ret = 3;
+	else
+		ret = 4;
+
+	return ret;
+}
+
+/* http://bcm-specs.sipsolutions.net/TX_Gain_Digital_Analog_Converter */
+static u16 b43_get_txgain_dac(u16 txpower)
+{
+	u16 ret;
+
+	B43_WARN_ON(txpower > 63);
+
+	if (txpower >= 54)
+		ret = txpower - 53;
+	else if (txpower >= 49)
+		ret = txpower - 42;
+	else if (txpower >= 44)
+		ret = txpower - 37;
+	else if (txpower >= 32)
+		ret = txpower - 32;
+	else if (txpower >= 25)
+		ret = txpower - 20;
+	else if (txpower >= 20)
+		ret = txpower - 13;
+	else if (txpower >= 12)
+		ret = txpower - 8;
+	else
+		ret = txpower;
+
+	return ret;
+}
+
+static void b43_radio_set_txpower_a(struct b43_wldev *dev, u16 txpower)
+{
+	struct b43_phy *phy = &dev->phy;
+	u16 pamp, base, dac, t;
+
+	txpower = limit_value(txpower, 0, 63);
+
+	pamp = b43_get_txgain_freq_power_amp(txpower);
+	pamp <<= 5;
+	pamp &= 0x00E0;
+	b43_phy_write(dev, 0x0019, pamp);
+
+	base = b43_get_txgain_base_band(txpower);
+	base &= 0x000F;
+	b43_phy_write(dev, 0x0017, base | 0x0020);
+
+	t = b43_ofdmtab_read16(dev, 0x3000, 1);
+	t &= 0x0007;
+
+	dac = b43_get_txgain_dac(txpower);
+	dac <<= 3;
+	dac |= t;
+
+	b43_ofdmtab_write16(dev, 0x3000, 1, dac);
+
+	phy->txpwr_offset = txpower;
+
+	//TODO: FuncPlaceholder (Adjust BB loft cancel)
+}
+
+void b43_radio_turn_on(struct b43_wldev *dev)
+{
+	struct b43_phy *phy = &dev->phy;
+	int err;
+	u8 channel;
+
+	might_sleep();
+
+	if (phy->radio_on)
+		return;
+
+	switch (phy->type) {
+	case B43_PHYTYPE_A:
+		b43_radio_write16(dev, 0x0004, 0x00C0);
+		b43_radio_write16(dev, 0x0005, 0x0008);
+		b43_phy_write(dev, 0x0010, b43_phy_read(dev, 0x0010) & 0xFFF7);
+		b43_phy_write(dev, 0x0011, b43_phy_read(dev, 0x0011) & 0xFFF7);
+		b43_radio_init2060(dev);
+		break;
+	case B43_PHYTYPE_B:
+	case B43_PHYTYPE_G:
+		b43_phy_write(dev, 0x0015, 0x8000);
+		b43_phy_write(dev, 0x0015, 0xCC00);
+		b43_phy_write(dev, 0x0015, (phy->gmode ? 0x00C0 : 0x0000));
+		if (phy->radio_off_context.valid) {
+			/* Restore the RFover values. */
+			b43_phy_write(dev, B43_PHY_RFOVER,
+				      phy->radio_off_context.rfover);
+			b43_phy_write(dev, B43_PHY_RFOVERVAL,
+				      phy->radio_off_context.rfoverval);
+			phy->radio_off_context.valid = 0;
+		}
+		channel = phy->channel;
+		err = b43_radio_selectchannel(dev, B43_DEFAULT_CHANNEL_BG, 1);
+		err |= b43_radio_selectchannel(dev, channel, 0);
+		B43_WARN_ON(err);
+		break;
+	default:
+		B43_WARN_ON(1);
+	}
+	phy->radio_on = 1;
+}
+
+void b43_radio_turn_off(struct b43_wldev *dev, bool force)
+{
+	struct b43_phy *phy = &dev->phy;
+
+	if (!phy->radio_on && !force)
+		return;
+
+	if (phy->type == B43_PHYTYPE_A) {
+		b43_radio_write16(dev, 0x0004, 0x00FF);
+		b43_radio_write16(dev, 0x0005, 0x00FB);
+		b43_phy_write(dev, 0x0010, b43_phy_read(dev, 0x0010) | 0x0008);
+		b43_phy_write(dev, 0x0011, b43_phy_read(dev, 0x0011) | 0x0008);
+	}
+	if (phy->type == B43_PHYTYPE_G && dev->dev->id.revision >= 5) {
+		u16 rfover, rfoverval;
+
+		rfover = b43_phy_read(dev, B43_PHY_RFOVER);
+		rfoverval = b43_phy_read(dev, B43_PHY_RFOVERVAL);
+		if (!force) {
+			phy->radio_off_context.rfover = rfover;
+			phy->radio_off_context.rfoverval = rfoverval;
+			phy->radio_off_context.valid = 1;
+		}
+		b43_phy_write(dev, B43_PHY_RFOVER, rfover | 0x008C);
+		b43_phy_write(dev, B43_PHY_RFOVERVAL, rfoverval & 0xFF73);
+	} else
+		b43_phy_write(dev, 0x0015, 0xAA00);
+	phy->radio_on = 0;
+}
diff --git a/drivers/net/wireless/b43/phy.h b/drivers/net/wireless/b43/phy.h
new file mode 100644
index 000000000000..c64d74504fc0
--- /dev/null
+++ b/drivers/net/wireless/b43/phy.h
@@ -0,0 +1,297 @@
+#ifndef B43_PHY_H_
+#define B43_PHY_H_
+
+#include <linux/types.h>
+
+struct b43_wldev;
+struct b43_phy;
+
+/*** PHY Registers ***/
+
+/* Routing */
+#define B43_PHYROUTE_OFDM_GPHY		0x400
+#define B43_PHYROUTE_EXT_GPHY		0x800
+
+/* Base registers. */
+#define B43_PHY_BASE(reg)		(reg)
+/* OFDM (A) registers of a G-PHY */
+#define B43_PHY_OFDM(reg)		((reg) | B43_PHYROUTE_OFDM_GPHY)
+/* Extended G-PHY registers */
+#define B43_PHY_EXTG(reg)		((reg) | B43_PHYROUTE_EXT_GPHY)
+
+/* OFDM (A) PHY Registers */
+#define B43_PHY_VERSION_OFDM		B43_PHY_OFDM(0x00)	/* Versioning register for A-PHY */
+#define B43_PHY_BBANDCFG		B43_PHY_OFDM(0x01)	/* Baseband config */
+#define  B43_PHY_BBANDCFG_RXANT		0x180	/* RX Antenna selection */
+#define  B43_PHY_BBANDCFG_RXANT_SHIFT	7
+#define B43_PHY_PWRDOWN			B43_PHY_OFDM(0x03)	/* Powerdown */
+#define B43_PHY_CRSTHRES1		B43_PHY_OFDM(0x06)	/* CRS Threshold 1 */
+#define B43_PHY_LNAHPFCTL		B43_PHY_OFDM(0x1C)	/* LNA/HPF control */
+#define B43_PHY_ADIVRELATED		B43_PHY_OFDM(0x27)	/* FIXME rename */
+#define B43_PHY_CRS0			B43_PHY_OFDM(0x29)
+#define B43_PHY_ANTDWELL		B43_PHY_OFDM(0x2B)	/* Antenna dwell */
+#define  B43_PHY_ANTDWELL_AUTODIV1	0x0100	/* Automatic RX diversity start antenna */
+#define B43_PHY_ENCORE			B43_PHY_OFDM(0x49)	/* "Encore" (RangeMax / BroadRange) */
+#define  B43_PHY_ENCORE_EN		0x0200	/* Encore enable */
+#define B43_PHY_LMS			B43_PHY_OFDM(0x55)
+#define B43_PHY_OFDM61			B43_PHY_OFDM(0x61)	/* FIXME rename */
+#define  B43_PHY_OFDM61_10		0x0010	/* FIXME rename */
+#define B43_PHY_IQBAL			B43_PHY_OFDM(0x69)	/* I/Q balance */
+#define B43_PHY_OTABLECTL		B43_PHY_OFDM(0x72)	/* OFDM table control (see below) */
+#define  B43_PHY_OTABLEOFF		0x03FF	/* OFDM table offset (see below) */
+#define  B43_PHY_OTABLENR		0xFC00	/* OFDM table number (see below) */
+#define  B43_PHY_OTABLENR_SHIFT		10
+#define B43_PHY_OTABLEI			B43_PHY_OFDM(0x73)	/* OFDM table data I */
+#define B43_PHY_OTABLEQ			B43_PHY_OFDM(0x74)	/* OFDM table data Q */
+#define B43_PHY_HPWR_TSSICTL		B43_PHY_OFDM(0x78)	/* Hardware power TSSI control */
+#define B43_PHY_NRSSITHRES		B43_PHY_OFDM(0x8A)	/* NRSSI threshold */
+#define B43_PHY_ANTWRSETT		B43_PHY_OFDM(0x8C)	/* Antenna WR settle */
+#define  B43_PHY_ANTWRSETT_ARXDIV	0x2000	/* Automatic RX diversity enabled */
+#define B43_PHY_CLIPPWRDOWNT		B43_PHY_OFDM(0x93)	/* Clip powerdown threshold */
+#define B43_PHY_OFDM9B			B43_PHY_OFDM(0x9B)	/* FIXME rename */
+#define B43_PHY_N1P1GAIN		B43_PHY_OFDM(0xA0)
+#define B43_PHY_P1P2GAIN		B43_PHY_OFDM(0xA1)
+#define B43_PHY_N1N2GAIN		B43_PHY_OFDM(0xA2)
+#define B43_PHY_CLIPTHRES		B43_PHY_OFDM(0xA3)
+#define B43_PHY_CLIPN1P2THRES		B43_PHY_OFDM(0xA4)
+#define B43_PHY_DIVSRCHIDX		B43_PHY_OFDM(0xA8)	/* Divider search gain/index */
+#define B43_PHY_CLIPP2THRES		B43_PHY_OFDM(0xA9)
+#define B43_PHY_CLIPP3THRES		B43_PHY_OFDM(0xAA)
+#define B43_PHY_DIVP1P2GAIN		B43_PHY_OFDM(0xAB)
+#define B43_PHY_DIVSRCHGAINBACK		B43_PHY_OFDM(0xAD)	/* Divider search gain back */
+#define B43_PHY_DIVSRCHGAINCHNG		B43_PHY_OFDM(0xAE)	/* Divider search gain change */
+#define B43_PHY_CRSTHRES1_R1		B43_PHY_OFDM(0xC0)	/* CRS Threshold 1 (rev 1 only) */
+#define B43_PHY_CRSTHRES2_R1		B43_PHY_OFDM(0xC1)	/* CRS Threshold 2 (rev 1 only) */
+#define B43_PHY_TSSIP_LTBASE		B43_PHY_OFDM(0x380)	/* TSSI power lookup table base */
+#define B43_PHY_DC_LTBASE		B43_PHY_OFDM(0x3A0)	/* DC lookup table base */
+#define B43_PHY_GAIN_LTBASE		B43_PHY_OFDM(0x3C0)	/* Gain lookup table base */
+
+/* CCK (B) PHY Registers */
+#define B43_PHY_VERSION_CCK		B43_PHY_BASE(0x00)	/* Versioning register for B-PHY */
+#define B43_PHY_CCKBBANDCFG		B43_PHY_BASE(0x01)	/* Contains antenna 0/1 control bit */
+#define B43_PHY_PGACTL			B43_PHY_BASE(0x15)	/* PGA control */
+#define  B43_PHY_PGACTL_LPF		0x1000	/* Low pass filter (?) */
+#define  B43_PHY_PGACTL_LOWBANDW	0x0040	/* Low bandwidth flag */
+#define  B43_PHY_PGACTL_UNKNOWN		0xEFA0
+#define B43_PHY_FBCTL1			B43_PHY_BASE(0x18)	/* Frequency bandwidth control 1 */
+#define B43_PHY_ITSSI			B43_PHY_BASE(0x29)	/* Idle TSSI */
+#define B43_PHY_LO_LEAKAGE		B43_PHY_BASE(0x2D)	/* Measured LO leakage */
+#define B43_PHY_ENERGY			B43_PHY_BASE(0x33)	/* Energy */
+#define B43_PHY_SYNCCTL			B43_PHY_BASE(0x35)
+#define B43_PHY_FBCTL2			B43_PHY_BASE(0x38)	/* Frequency bandwidth control 2 */
+#define B43_PHY_DACCTL			B43_PHY_BASE(0x60)	/* DAC control */
+#define B43_PHY_RCCALOVER		B43_PHY_BASE(0x78)	/* RC calibration override */
+
+/* Extended G-PHY Registers */
+#define B43_PHY_CLASSCTL		B43_PHY_EXTG(0x02)	/* Classify control */
+#define B43_PHY_GTABCTL			B43_PHY_EXTG(0x03)	/* G-PHY table control (see below) */
+#define  B43_PHY_GTABOFF		0x03FF	/* G-PHY table offset (see below) */
+#define  B43_PHY_GTABNR			0xFC00	/* G-PHY table number (see below) */
+#define  B43_PHY_GTABNR_SHIFT		10
+#define B43_PHY_GTABDATA		B43_PHY_EXTG(0x04)	/* G-PHY table data */
+#define B43_PHY_LO_MASK			B43_PHY_EXTG(0x0F)	/* Local Oscillator control mask */
+#define B43_PHY_LO_CTL			B43_PHY_EXTG(0x10)	/* Local Oscillator control */
+#define B43_PHY_RFOVER			B43_PHY_EXTG(0x11)	/* RF override */
+#define B43_PHY_RFOVERVAL		B43_PHY_EXTG(0x12)	/* RF override value */
+#define  B43_PHY_RFOVERVAL_EXTLNA	0x8000
+#define  B43_PHY_RFOVERVAL_LNA		0x7000
+#define  B43_PHY_RFOVERVAL_LNA_SHIFT	12
+#define  B43_PHY_RFOVERVAL_PGA		0x0F00
+#define  B43_PHY_RFOVERVAL_PGA_SHIFT	8
+#define  B43_PHY_RFOVERVAL_UNK		0x0010	/* Unknown, always set. */
+#define  B43_PHY_RFOVERVAL_TRSWRX	0x00E0
+#define  B43_PHY_RFOVERVAL_BW		0x0003	/* Bandwidth flags */
+#define   B43_PHY_RFOVERVAL_BW_LPF	0x0001	/* Low Pass Filter */
+#define   B43_PHY_RFOVERVAL_BW_LBW	0x0002	/* Low Bandwidth (when set), high when unset */
+#define B43_PHY_ANALOGOVER		B43_PHY_EXTG(0x14)	/* Analog override */
+#define B43_PHY_ANALOGOVERVAL		B43_PHY_EXTG(0x15)	/* Analog override value */
+
+/*** OFDM table numbers ***/
+#define B43_OFDMTAB(number, offset)	(((number) << B43_PHY_OTABLENR_SHIFT) | (offset))
+#define B43_OFDMTAB_AGC1		B43_OFDMTAB(0x00, 0)
+#define B43_OFDMTAB_GAIN0		B43_OFDMTAB(0x00, 0)
+#define B43_OFDMTAB_GAINX		B43_OFDMTAB(0x01, 0)	//TODO rename
+#define B43_OFDMTAB_GAIN1		B43_OFDMTAB(0x01, 4)
+#define B43_OFDMTAB_AGC3		B43_OFDMTAB(0x02, 0)
+#define B43_OFDMTAB_GAIN2		B43_OFDMTAB(0x02, 3)
+#define B43_OFDMTAB_LNAHPFGAIN1		B43_OFDMTAB(0x03, 0)
+#define B43_OFDMTAB_WRSSI		B43_OFDMTAB(0x04, 0)
+#define B43_OFDMTAB_LNAHPFGAIN2		B43_OFDMTAB(0x04, 0)
+#define B43_OFDMTAB_NOISESCALE		B43_OFDMTAB(0x05, 0)
+#define B43_OFDMTAB_AGC2		B43_OFDMTAB(0x06, 0)
+#define B43_OFDMTAB_ROTOR		B43_OFDMTAB(0x08, 0)
+#define B43_OFDMTAB_ADVRETARD		B43_OFDMTAB(0x09, 0)
+#define B43_OFDMTAB_DAC			B43_OFDMTAB(0x0C, 0)
+#define B43_OFDMTAB_DC			B43_OFDMTAB(0x0E, 7)
+#define B43_OFDMTAB_PWRDYN2		B43_OFDMTAB(0x0E, 12)
+#define B43_OFDMTAB_LNAGAIN		B43_OFDMTAB(0x0E, 13)
+//TODO
+#define B43_OFDMTAB_LPFGAIN		B43_OFDMTAB(0x0F, 12)
+#define B43_OFDMTAB_RSSI		B43_OFDMTAB(0x10, 0)
+//TODO
+#define B43_OFDMTAB_AGC1_R1		B43_OFDMTAB(0x13, 0)
+#define B43_OFDMTAB_GAINX_R1		B43_OFDMTAB(0x14, 0)	//TODO rename
+#define B43_OFDMTAB_MINSIGSQ		B43_OFDMTAB(0x14, 1)
+#define B43_OFDMTAB_AGC3_R1		B43_OFDMTAB(0x15, 0)
+#define B43_OFDMTAB_WRSSI_R1		B43_OFDMTAB(0x15, 4)
+#define B43_OFDMTAB_TSSI		B43_OFDMTAB(0x15, 0)
+#define B43_OFDMTAB_DACRFPABB		B43_OFDMTAB(0x16, 0)
+#define B43_OFDMTAB_DACOFF		B43_OFDMTAB(0x17, 0)
+#define B43_OFDMTAB_DCBIAS		B43_OFDMTAB(0x18, 0)
+
+u16 b43_ofdmtab_read16(struct b43_wldev *dev, u16 table, u16 offset);
+void b43_ofdmtab_write16(struct b43_wldev *dev, u16 table,
+			 u16 offset, u16 value);
+u32 b43_ofdmtab_read32(struct b43_wldev *dev, u16 table, u16 offset);
+void b43_ofdmtab_write32(struct b43_wldev *dev, u16 table,
+			 u16 offset, u32 value);
+
+/*** G-PHY table numbers */
+#define B43_GTAB(number, offset)	(((number) << B43_PHY_GTABNR_SHIFT) | (offset))
+#define B43_GTAB_NRSSI			B43_GTAB(0x00, 0)
+#define B43_GTAB_TRFEMW			B43_GTAB(0x0C, 0x120)
+#define B43_GTAB_ORIGTR			B43_GTAB(0x2E, 0x298)
+
+u16 b43_gtab_read(struct b43_wldev *dev, u16 table, u16 offset);	//TODO implement
+void b43_gtab_write(struct b43_wldev *dev, u16 table, u16 offset, u16 value);	//TODO implement
+
+#define B43_DEFAULT_CHANNEL_A	36
+#define B43_DEFAULT_CHANNEL_BG	6
+
+enum {
+	B43_ANTENNA0,		/* Antenna 0 */
+	B43_ANTENNA1,		/* Antenna 0 */
+	B43_ANTENNA_AUTO1,	/* Automatic, starting with antenna 1 */
+	B43_ANTENNA_AUTO0,	/* Automatic, starting with antenna 0 */
+
+	B43_ANTENNA_AUTO = B43_ANTENNA_AUTO0,
+	B43_ANTENNA_DEFAULT = B43_ANTENNA_AUTO,
+};
+
+enum {
+	B43_INTERFMODE_NONE,
+	B43_INTERFMODE_NONWLAN,
+	B43_INTERFMODE_MANUALWLAN,
+	B43_INTERFMODE_AUTOWLAN,
+};
+
+/* Masks for the different PHY versioning registers. */
+#define B43_PHYVER_ANALOG		0xF000
+#define B43_PHYVER_ANALOG_SHIFT		12
+#define B43_PHYVER_TYPE			0x0F00
+#define B43_PHYVER_TYPE_SHIFT		8
+#define B43_PHYVER_VERSION		0x00FF
+
+void b43_raw_phy_lock(struct b43_wldev *dev);
+#define b43_phy_lock(dev, flags) \
+	do {					\
+		local_irq_save(flags);		\
+		b43_raw_phy_lock(dev);	\
+	} while (0)
+void b43_raw_phy_unlock(struct b43_wldev *dev);
+#define b43_phy_unlock(dev, flags) \
+	do {					\
+		b43_raw_phy_unlock(dev);	\
+		local_irq_restore(flags);	\
+	} while (0)
+
+u16 b43_phy_read(struct b43_wldev *dev, u16 offset);
+void b43_phy_write(struct b43_wldev *dev, u16 offset, u16 val);
+
+int b43_phy_init_tssi2dbm_table(struct b43_wldev *dev);
+
+void b43_phy_early_init(struct b43_wldev *dev);
+int b43_phy_init(struct b43_wldev *dev);
+
+void b43_set_rx_antenna(struct b43_wldev *dev, int antenna);
+
+void b43_phy_xmitpower(struct b43_wldev *dev);
+void b43_gphy_dc_lt_init(struct b43_wldev *dev);
+
+/* Returns the boolean whether the board has HardwarePowerControl */
+bool b43_has_hardware_pctl(struct b43_phy *phy);
+/* Returns the boolean whether "TX Magnification" is enabled. */
+#define has_tx_magnification(phy) \
+	(((phy)->rev >= 2) &&			\
+	 ((phy)->radio_ver == 0x2050) &&	\
+	 ((phy)->radio_rev == 8))
+/* Card uses the loopback gain stuff */
+#define has_loopback_gain(phy) \
+	(((phy)->rev > 1) || ((phy)->gmode))
+
+/* Radio Attenuation (RF Attenuation) */
+struct b43_rfatt {
+	u8 att;			/* Attenuation value */
+	bool with_padmix;	/* Flag, PAD Mixer enabled. */
+};
+struct b43_rfatt_list {
+	/* Attenuation values list */
+	const struct b43_rfatt *list;
+	u8 len;
+	/* Minimum/Maximum attenuation values */
+	u8 min_val;
+	u8 max_val;
+};
+
+/* Baseband Attenuation */
+struct b43_bbatt {
+	u8 att;			/* Attenuation value */
+};
+struct b43_bbatt_list {
+	/* Attenuation values list */
+	const struct b43_bbatt *list;
+	u8 len;
+	/* Minimum/Maximum attenuation values */
+	u8 min_val;
+	u8 max_val;
+};
+
+/* tx_control bits. */
+#define B43_TXCTL_PA3DB		0x40	/* PA Gain 3dB */
+#define B43_TXCTL_PA2DB		0x20	/* PA Gain 2dB */
+#define B43_TXCTL_TXMIX		0x10	/* TX Mixer Gain */
+
+/* Write BasebandAttenuation value to the device. */
+void b43_phy_set_baseband_attenuation(struct b43_wldev *dev,
+				      u16 baseband_attenuation);
+
+extern const u8 b43_radio_channel_codes_bg[];
+
+void b43_radio_lock(struct b43_wldev *dev);
+void b43_radio_unlock(struct b43_wldev *dev);
+
+u16 b43_radio_read16(struct b43_wldev *dev, u16 offset);
+void b43_radio_write16(struct b43_wldev *dev, u16 offset, u16 val);
+
+u16 b43_radio_init2050(struct b43_wldev *dev);
+void b43_radio_init2060(struct b43_wldev *dev);
+
+void b43_radio_turn_on(struct b43_wldev *dev);
+void b43_radio_turn_off(struct b43_wldev *dev, bool force);
+
+int b43_radio_selectchannel(struct b43_wldev *dev, u8 channel,
+			    int synthetic_pu_workaround);
+
+u8 b43_radio_aci_detect(struct b43_wldev *dev, u8 channel);
+u8 b43_radio_aci_scan(struct b43_wldev *dev);
+
+int b43_radio_set_interference_mitigation(struct b43_wldev *dev, int mode);
+
+void b43_calc_nrssi_slope(struct b43_wldev *dev);
+void b43_calc_nrssi_threshold(struct b43_wldev *dev);
+s16 b43_nrssi_hw_read(struct b43_wldev *dev, u16 offset);
+void b43_nrssi_hw_write(struct b43_wldev *dev, u16 offset, s16 val);
+void b43_nrssi_hw_update(struct b43_wldev *dev, u16 val);
+void b43_nrssi_mem_update(struct b43_wldev *dev);
+
+void b43_radio_set_tx_iq(struct b43_wldev *dev);
+u16 b43_radio_calibrationvalue(struct b43_wldev *dev);
+
+void b43_put_attenuation_into_ranges(struct b43_wldev *dev,
+				     int *_bbatt, int *_rfatt);
+
+void b43_set_txpower_g(struct b43_wldev *dev,
+		       const struct b43_bbatt *bbatt,
+		       const struct b43_rfatt *rfatt, u8 tx_control);
+
+#endif /* B43_PHY_H_ */
diff --git a/drivers/net/wireless/b43/pio.c b/drivers/net/wireless/b43/pio.c
new file mode 100644
index 000000000000..67752a28eb9c
--- /dev/null
+++ b/drivers/net/wireless/b43/pio.c
@@ -0,0 +1,652 @@
+/*
+
+  Broadcom B43 wireless driver
+
+  PIO Transmission
+
+  Copyright (c) 2005 Michael Buesch <mb@bu3sch.de>
+
+  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, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with this program; see the file COPYING.  If not, write to
+  the Free Software Foundation, Inc., 51 Franklin Steet, Fifth Floor,
+  Boston, MA 02110-1301, USA.
+
+*/
+
+#include "b43.h"
+#include "pio.h"
+#include "main.h"
+#include "xmit.h"
+
+#include <linux/delay.h>
+
+static void tx_start(struct b43_pioqueue *queue)
+{
+	b43_pio_write(queue, B43_PIO_TXCTL, B43_PIO_TXCTL_INIT);
+}
+
+static void tx_octet(struct b43_pioqueue *queue, u8 octet)
+{
+	if (queue->need_workarounds) {
+		b43_pio_write(queue, B43_PIO_TXDATA, octet);
+		b43_pio_write(queue, B43_PIO_TXCTL, B43_PIO_TXCTL_WRITELO);
+	} else {
+		b43_pio_write(queue, B43_PIO_TXCTL, B43_PIO_TXCTL_WRITELO);
+		b43_pio_write(queue, B43_PIO_TXDATA, octet);
+	}
+}
+
+static u16 tx_get_next_word(const u8 * txhdr,
+			    const u8 * packet,
+			    size_t txhdr_size, unsigned int *pos)
+{
+	const u8 *source;
+	unsigned int i = *pos;
+	u16 ret;
+
+	if (i < txhdr_size) {
+		source = txhdr;
+	} else {
+		source = packet;
+		i -= txhdr_size;
+	}
+	ret = le16_to_cpu(*((__le16 *)(source + i)));
+	*pos += 2;
+
+	return ret;
+}
+
+static void tx_data(struct b43_pioqueue *queue,
+		    u8 * txhdr, const u8 * packet, unsigned int octets)
+{
+	u16 data;
+	unsigned int i = 0;
+
+	if (queue->need_workarounds) {
+		data = tx_get_next_word(txhdr, packet,
+					sizeof(struct b43_txhdr_fw4), &i);
+		b43_pio_write(queue, B43_PIO_TXDATA, data);
+	}
+	b43_pio_write(queue, B43_PIO_TXCTL,
+		      B43_PIO_TXCTL_WRITELO | B43_PIO_TXCTL_WRITEHI);
+	while (i < octets - 1) {
+		data = tx_get_next_word(txhdr, packet,
+					sizeof(struct b43_txhdr_fw4), &i);
+		b43_pio_write(queue, B43_PIO_TXDATA, data);
+	}
+	if (octets % 2)
+		tx_octet(queue,
+			 packet[octets - sizeof(struct b43_txhdr_fw4) - 1]);
+}
+
+static void tx_complete(struct b43_pioqueue *queue, struct sk_buff *skb)
+{
+	if (queue->need_workarounds) {
+		b43_pio_write(queue, B43_PIO_TXDATA, skb->data[skb->len - 1]);
+		b43_pio_write(queue, B43_PIO_TXCTL,
+			      B43_PIO_TXCTL_WRITELO | B43_PIO_TXCTL_COMPLETE);
+	} else {
+		b43_pio_write(queue, B43_PIO_TXCTL, B43_PIO_TXCTL_COMPLETE);
+	}
+}
+
+static u16 generate_cookie(struct b43_pioqueue *queue,
+			   struct b43_pio_txpacket *packet)
+{
+	u16 cookie = 0x0000;
+	u16 packetindex;
+
+	/* We use the upper 4 bits for the PIO
+	 * controller ID and the lower 12 bits
+	 * for the packet index (in the cache).
+	 */
+	switch (queue->mmio_base) {
+	case B43_MMIO_PIO1_BASE:
+		break;
+	case B43_MMIO_PIO2_BASE:
+		cookie = 0x1000;
+		break;
+	case B43_MMIO_PIO3_BASE:
+		cookie = 0x2000;
+		break;
+	case B43_MMIO_PIO4_BASE:
+		cookie = 0x3000;
+		break;
+	default:
+		B43_WARN_ON(1);
+	}
+	packetindex = packet->index;
+	B43_WARN_ON(packetindex & ~0x0FFF);
+	cookie |= (u16) packetindex;
+
+	return cookie;
+}
+
+static
+struct b43_pioqueue *parse_cookie(struct b43_wldev *dev,
+				  u16 cookie, struct b43_pio_txpacket **packet)
+{
+	struct b43_pio *pio = &dev->pio;
+	struct b43_pioqueue *queue = NULL;
+	int packetindex;
+
+	switch (cookie & 0xF000) {
+	case 0x0000:
+		queue = pio->queue0;
+		break;
+	case 0x1000:
+		queue = pio->queue1;
+		break;
+	case 0x2000:
+		queue = pio->queue2;
+		break;
+	case 0x3000:
+		queue = pio->queue3;
+		break;
+	default:
+		B43_WARN_ON(1);
+	}
+	packetindex = (cookie & 0x0FFF);
+	B43_WARN_ON(!(packetindex >= 0 && packetindex < B43_PIO_MAXTXPACKETS));
+	*packet = &(queue->tx_packets_cache[packetindex]);
+
+	return queue;
+}
+
+union txhdr_union {
+	struct b43_txhdr_fw4 txhdr_fw4;
+};
+
+static void pio_tx_write_fragment(struct b43_pioqueue *queue,
+				  struct sk_buff *skb,
+				  struct b43_pio_txpacket *packet,
+				  size_t txhdr_size)
+{
+	union txhdr_union txhdr_data;
+	u8 *txhdr = NULL;
+	unsigned int octets;
+
+	txhdr = (u8 *) (&txhdr_data.txhdr_fw4);
+
+	B43_WARN_ON(skb_shinfo(skb)->nr_frags);
+	b43_generate_txhdr(queue->dev,
+			   txhdr, skb->data, skb->len,
+			   &packet->txstat.control,
+			   generate_cookie(queue, packet));
+
+	tx_start(queue);
+	octets = skb->len + txhdr_size;
+	if (queue->need_workarounds)
+		octets--;
+	tx_data(queue, txhdr, (u8 *) skb->data, octets);
+	tx_complete(queue, skb);
+}
+
+static void free_txpacket(struct b43_pio_txpacket *packet)
+{
+	struct b43_pioqueue *queue = packet->queue;
+
+	if (packet->skb)
+		dev_kfree_skb_any(packet->skb);
+	list_move(&packet->list, &queue->txfree);
+	queue->nr_txfree++;
+}
+
+static int pio_tx_packet(struct b43_pio_txpacket *packet)
+{
+	struct b43_pioqueue *queue = packet->queue;
+	struct sk_buff *skb = packet->skb;
+	u16 octets;
+
+	octets = (u16) skb->len + sizeof(struct b43_txhdr_fw4);
+	if (queue->tx_devq_size < octets) {
+		b43warn(queue->dev->wl, "PIO queue too small. "
+			"Dropping packet.\n");
+		/* Drop it silently (return success) */
+		free_txpacket(packet);
+		return 0;
+	}
+	B43_WARN_ON(queue->tx_devq_packets > B43_PIO_MAXTXDEVQPACKETS);
+	B43_WARN_ON(queue->tx_devq_used > queue->tx_devq_size);
+	/* Check if there is sufficient free space on the device
+	 * TX queue. If not, return and let the TX tasklet
+	 * retry later.
+	 */
+	if (queue->tx_devq_packets == B43_PIO_MAXTXDEVQPACKETS)
+		return -EBUSY;
+	if (queue->tx_devq_used + octets > queue->tx_devq_size)
+		return -EBUSY;
+	/* Now poke the device. */
+	pio_tx_write_fragment(queue, skb, packet, sizeof(struct b43_txhdr_fw4));
+
+	/* Account for the packet size.
+	 * (We must not overflow the device TX queue)
+	 */
+	queue->tx_devq_packets++;
+	queue->tx_devq_used += octets;
+
+	/* Transmission started, everything ok, move the
+	 * packet to the txrunning list.
+	 */
+	list_move_tail(&packet->list, &queue->txrunning);
+
+	return 0;
+}
+
+static void tx_tasklet(unsigned long d)
+{
+	struct b43_pioqueue *queue = (struct b43_pioqueue *)d;
+	struct b43_wldev *dev = queue->dev;
+	unsigned long flags;
+	struct b43_pio_txpacket *packet, *tmp_packet;
+	int err;
+	u16 txctl;
+
+	spin_lock_irqsave(&dev->wl->irq_lock, flags);
+	if (queue->tx_frozen)
+		goto out_unlock;
+	txctl = b43_pio_read(queue, B43_PIO_TXCTL);
+	if (txctl & B43_PIO_TXCTL_SUSPEND)
+		goto out_unlock;
+
+	list_for_each_entry_safe(packet, tmp_packet, &queue->txqueue, list) {
+		/* Try to transmit the packet. This can fail, if
+		 * the device queue is full. In case of failure, the
+		 * packet is left in the txqueue.
+		 * If transmission succeed, the packet is moved to txrunning.
+		 * If it is impossible to transmit the packet, it
+		 * is dropped.
+		 */
+		err = pio_tx_packet(packet);
+		if (err)
+			break;
+	}
+      out_unlock:
+	spin_unlock_irqrestore(&dev->wl->irq_lock, flags);
+}
+
+static void setup_txqueues(struct b43_pioqueue *queue)
+{
+	struct b43_pio_txpacket *packet;
+	int i;
+
+	queue->nr_txfree = B43_PIO_MAXTXPACKETS;
+	for (i = 0; i < B43_PIO_MAXTXPACKETS; i++) {
+		packet = &(queue->tx_packets_cache[i]);
+
+		packet->queue = queue;
+		INIT_LIST_HEAD(&packet->list);
+		packet->index = i;
+
+		list_add(&packet->list, &queue->txfree);
+	}
+}
+
+static
+struct b43_pioqueue *b43_setup_pioqueue(struct b43_wldev *dev,
+					u16 pio_mmio_base)
+{
+	struct b43_pioqueue *queue;
+	u16 qsize;
+
+	queue = kzalloc(sizeof(*queue), GFP_KERNEL);
+	if (!queue)
+		goto out;
+
+	queue->dev = dev;
+	queue->mmio_base = pio_mmio_base;
+	queue->need_workarounds = (dev->dev->id.revision < 3);
+
+	INIT_LIST_HEAD(&queue->txfree);
+	INIT_LIST_HEAD(&queue->txqueue);
+	INIT_LIST_HEAD(&queue->txrunning);
+	tasklet_init(&queue->txtask, tx_tasklet, (unsigned long)queue);
+
+	b43_write32(dev, B43_MMIO_MACCTL, b43_read32(dev, B43_MMIO_MACCTL)
+		    & ~B43_MACCTL_BE);
+
+	qsize = b43_read16(dev, queue->mmio_base + B43_PIO_TXQBUFSIZE);
+	if (qsize == 0) {
+		b43err(dev->wl, "This card does not support PIO "
+		       "operation mode. Please use DMA mode "
+		       "(module parameter pio=0).\n");
+		goto err_freequeue;
+	}
+	if (qsize <= B43_PIO_TXQADJUST) {
+		b43err(dev->wl, "PIO tx device-queue too small (%u)\n", qsize);
+		goto err_freequeue;
+	}
+	qsize -= B43_PIO_TXQADJUST;
+	queue->tx_devq_size = qsize;
+
+	setup_txqueues(queue);
+
+      out:
+	return queue;
+
+      err_freequeue:
+	kfree(queue);
+	queue = NULL;
+	goto out;
+}
+
+static void cancel_transfers(struct b43_pioqueue *queue)
+{
+	struct b43_pio_txpacket *packet, *tmp_packet;
+
+	tasklet_disable(&queue->txtask);
+
+	list_for_each_entry_safe(packet, tmp_packet, &queue->txrunning, list)
+	    free_txpacket(packet);
+	list_for_each_entry_safe(packet, tmp_packet, &queue->txqueue, list)
+	    free_txpacket(packet);
+}
+
+static void b43_destroy_pioqueue(struct b43_pioqueue *queue)
+{
+	if (!queue)
+		return;
+
+	cancel_transfers(queue);
+	kfree(queue);
+}
+
+void b43_pio_free(struct b43_wldev *dev)
+{
+	struct b43_pio *pio;
+
+	if (!b43_using_pio(dev))
+		return;
+	pio = &dev->pio;
+
+	b43_destroy_pioqueue(pio->queue3);
+	pio->queue3 = NULL;
+	b43_destroy_pioqueue(pio->queue2);
+	pio->queue2 = NULL;
+	b43_destroy_pioqueue(pio->queue1);
+	pio->queue1 = NULL;
+	b43_destroy_pioqueue(pio->queue0);
+	pio->queue0 = NULL;
+}
+
+int b43_pio_init(struct b43_wldev *dev)
+{
+	struct b43_pio *pio = &dev->pio;
+	struct b43_pioqueue *queue;
+	int err = -ENOMEM;
+
+	queue = b43_setup_pioqueue(dev, B43_MMIO_PIO1_BASE);
+	if (!queue)
+		goto out;
+	pio->queue0 = queue;
+
+	queue = b43_setup_pioqueue(dev, B43_MMIO_PIO2_BASE);
+	if (!queue)
+		goto err_destroy0;
+	pio->queue1 = queue;
+
+	queue = b43_setup_pioqueue(dev, B43_MMIO_PIO3_BASE);
+	if (!queue)
+		goto err_destroy1;
+	pio->queue2 = queue;
+
+	queue = b43_setup_pioqueue(dev, B43_MMIO_PIO4_BASE);
+	if (!queue)
+		goto err_destroy2;
+	pio->queue3 = queue;
+
+	if (dev->dev->id.revision < 3)
+		dev->irq_savedstate |= B43_IRQ_PIO_WORKAROUND;
+
+	b43dbg(dev->wl, "PIO initialized\n");
+	err = 0;
+      out:
+	return err;
+
+      err_destroy2:
+	b43_destroy_pioqueue(pio->queue2);
+	pio->queue2 = NULL;
+      err_destroy1:
+	b43_destroy_pioqueue(pio->queue1);
+	pio->queue1 = NULL;
+      err_destroy0:
+	b43_destroy_pioqueue(pio->queue0);
+	pio->queue0 = NULL;
+	goto out;
+}
+
+int b43_pio_tx(struct b43_wldev *dev,
+	       struct sk_buff *skb, struct ieee80211_tx_control *ctl)
+{
+	struct b43_pioqueue *queue = dev->pio.queue1;
+	struct b43_pio_txpacket *packet;
+
+	B43_WARN_ON(queue->tx_suspended);
+	B43_WARN_ON(list_empty(&queue->txfree));
+
+	packet = list_entry(queue->txfree.next, struct b43_pio_txpacket, list);
+	packet->skb = skb;
+
+	memset(&packet->txstat, 0, sizeof(packet->txstat));
+	memcpy(&packet->txstat.control, ctl, sizeof(*ctl));
+
+	list_move_tail(&packet->list, &queue->txqueue);
+	queue->nr_txfree--;
+	queue->nr_tx_packets++;
+	B43_WARN_ON(queue->nr_txfree >= B43_PIO_MAXTXPACKETS);
+
+	tasklet_schedule(&queue->txtask);
+
+	return 0;
+}
+
+void b43_pio_handle_txstatus(struct b43_wldev *dev,
+			     const struct b43_txstatus *status)
+{
+	struct b43_pioqueue *queue;
+	struct b43_pio_txpacket *packet;
+
+	queue = parse_cookie(dev, status->cookie, &packet);
+	if (B43_WARN_ON(!queue))
+		return;
+
+	queue->tx_devq_packets--;
+	queue->tx_devq_used -=
+	    (packet->skb->len + sizeof(struct b43_txhdr_fw4));
+
+	if (status->acked) {
+		packet->txstat.flags |= IEEE80211_TX_STATUS_ACK;
+	} else {
+		if (!(packet->txstat.control.flags & IEEE80211_TXCTL_NO_ACK))
+			packet->txstat.excessive_retries = 1;
+	}
+	if (status->frame_count == 0) {
+		/* The frame was not transmitted at all. */
+		packet->txstat.retry_count = 0;
+	} else
+		packet->txstat.retry_count = status->frame_count - 1;
+	ieee80211_tx_status_irqsafe(dev->wl->hw, packet->skb,
+				    &(packet->txstat));
+	packet->skb = NULL;
+
+	free_txpacket(packet);
+	/* If there are packets on the txqueue, poke the tasklet
+	 * to transmit them.
+	 */
+	if (!list_empty(&queue->txqueue))
+		tasklet_schedule(&queue->txtask);
+}
+
+void b43_pio_get_tx_stats(struct b43_wldev *dev,
+			  struct ieee80211_tx_queue_stats *stats)
+{
+	struct b43_pio *pio = &dev->pio;
+	struct b43_pioqueue *queue;
+	struct ieee80211_tx_queue_stats_data *data;
+
+	queue = pio->queue1;
+	data = &(stats->data[0]);
+	data->len = B43_PIO_MAXTXPACKETS - queue->nr_txfree;
+	data->limit = B43_PIO_MAXTXPACKETS;
+	data->count = queue->nr_tx_packets;
+}
+
+static void pio_rx_error(struct b43_pioqueue *queue,
+			 int clear_buffers, const char *error)
+{
+	int i;
+
+	b43err(queue->dev->wl, "PIO RX error: %s\n", error);
+	b43_pio_write(queue, B43_PIO_RXCTL, B43_PIO_RXCTL_READY);
+	if (clear_buffers) {
+		B43_WARN_ON(queue->mmio_base != B43_MMIO_PIO1_BASE);
+		for (i = 0; i < 15; i++) {
+			/* Dummy read. */
+			b43_pio_read(queue, B43_PIO_RXDATA);
+		}
+	}
+}
+
+void b43_pio_rx(struct b43_pioqueue *queue)
+{
+	__le16 preamble[21] = { 0 };
+	struct b43_rxhdr_fw4 *rxhdr;
+	u16 tmp, len;
+	u32 macstat;
+	int i, preamble_readwords;
+	struct sk_buff *skb;
+
+	tmp = b43_pio_read(queue, B43_PIO_RXCTL);
+	if (!(tmp & B43_PIO_RXCTL_DATAAVAILABLE))
+		return;
+	b43_pio_write(queue, B43_PIO_RXCTL, B43_PIO_RXCTL_DATAAVAILABLE);
+
+	for (i = 0; i < 10; i++) {
+		tmp = b43_pio_read(queue, B43_PIO_RXCTL);
+		if (tmp & B43_PIO_RXCTL_READY)
+			goto data_ready;
+		udelay(10);
+	}
+	b43dbg(queue->dev->wl, "PIO RX timed out\n");
+	return;
+data_ready:
+
+	len = b43_pio_read(queue, B43_PIO_RXDATA);
+	if (unlikely(len > 0x700)) {
+		pio_rx_error(queue, 0, "len > 0x700");
+		return;
+	}
+	if (unlikely(len == 0 && queue->mmio_base != B43_MMIO_PIO4_BASE)) {
+		pio_rx_error(queue, 0, "len == 0");
+		return;
+	}
+	preamble[0] = cpu_to_le16(len);
+	if (queue->mmio_base == B43_MMIO_PIO4_BASE)
+		preamble_readwords = 14 / sizeof(u16);
+	else
+		preamble_readwords = 18 / sizeof(u16);
+	for (i = 0; i < preamble_readwords; i++) {
+		tmp = b43_pio_read(queue, B43_PIO_RXDATA);
+		preamble[i + 1] = cpu_to_le16(tmp);
+	}
+	rxhdr = (struct b43_rxhdr_fw4 *)preamble;
+	macstat = le32_to_cpu(rxhdr->mac_status);
+	if (macstat & B43_RX_MAC_FCSERR) {
+		pio_rx_error(queue,
+			     (queue->mmio_base == B43_MMIO_PIO1_BASE),
+			     "Frame FCS error");
+		return;
+	}
+	if (queue->mmio_base == B43_MMIO_PIO4_BASE) {
+		/* We received an xmit status. */
+		struct b43_hwtxstatus *hw;
+
+		hw = (struct b43_hwtxstatus *)(preamble + 1);
+		b43_handle_hwtxstatus(queue->dev, hw);
+
+		return;
+	}
+
+	skb = dev_alloc_skb(len);
+	if (unlikely(!skb)) {
+		pio_rx_error(queue, 1, "OOM");
+		return;
+	}
+	skb_put(skb, len);
+	for (i = 0; i < len - 1; i += 2) {
+		tmp = b43_pio_read(queue, B43_PIO_RXDATA);
+		*((__le16 *)(skb->data + i)) = cpu_to_le16(tmp);
+	}
+	if (len % 2) {
+		tmp = b43_pio_read(queue, B43_PIO_RXDATA);
+		skb->data[len - 1] = (tmp & 0x00FF);
+/* The specs say the following is required, but
+ * it is wrong and corrupts the PLCP. If we don't do
+ * this, the PLCP seems to be correct. So ifdef it out for now.
+ */
+#if 0
+		if (rxflags2 & B43_RXHDR_FLAGS2_TYPE2FRAME)
+			skb->data[2] = (tmp & 0xFF00) >> 8;
+		else
+			skb->data[0] = (tmp & 0xFF00) >> 8;
+#endif
+	}
+	b43_rx(queue->dev, skb, rxhdr);
+}
+
+void b43_pio_tx_suspend(struct b43_pioqueue *queue)
+{
+	b43_power_saving_ctl_bits(queue->dev, B43_PS_AWAKE);
+	b43_pio_write(queue, B43_PIO_TXCTL, b43_pio_read(queue, B43_PIO_TXCTL)
+		      | B43_PIO_TXCTL_SUSPEND);
+}
+
+void b43_pio_tx_resume(struct b43_pioqueue *queue)
+{
+	b43_pio_write(queue, B43_PIO_TXCTL, b43_pio_read(queue, B43_PIO_TXCTL)
+		      & ~B43_PIO_TXCTL_SUSPEND);
+	b43_power_saving_ctl_bits(queue->dev, 0);
+	tasklet_schedule(&queue->txtask);
+}
+
+void b43_pio_freeze_txqueues(struct b43_wldev *dev)
+{
+	struct b43_pio *pio;
+
+	B43_WARN_ON(!b43_using_pio(dev));
+	pio = &dev->pio;
+	pio->queue0->tx_frozen = 1;
+	pio->queue1->tx_frozen = 1;
+	pio->queue2->tx_frozen = 1;
+	pio->queue3->tx_frozen = 1;
+}
+
+void b43_pio_thaw_txqueues(struct b43_wldev *dev)
+{
+	struct b43_pio *pio;
+
+	B43_WARN_ON(!b43_using_pio(dev));
+	pio = &dev->pio;
+	pio->queue0->tx_frozen = 0;
+	pio->queue1->tx_frozen = 0;
+	pio->queue2->tx_frozen = 0;
+	pio->queue3->tx_frozen = 0;
+	if (!list_empty(&pio->queue0->txqueue))
+		tasklet_schedule(&pio->queue0->txtask);
+	if (!list_empty(&pio->queue1->txqueue))
+		tasklet_schedule(&pio->queue1->txtask);
+	if (!list_empty(&pio->queue2->txqueue))
+		tasklet_schedule(&pio->queue2->txtask);
+	if (!list_empty(&pio->queue3->txqueue))
+		tasklet_schedule(&pio->queue3->txtask);
+}
diff --git a/drivers/net/wireless/b43/pio.h b/drivers/net/wireless/b43/pio.h
new file mode 100644
index 000000000000..3488f2447bbf
--- /dev/null
+++ b/drivers/net/wireless/b43/pio.h
@@ -0,0 +1,153 @@
+#ifndef B43_PIO_H_
+#define B43_PIO_H_
+
+#include "b43.h"
+
+#include <linux/interrupt.h>
+#include <linux/io.h>
+#include <linux/list.h>
+#include <linux/skbuff.h>
+
+#define B43_PIO_TXCTL		0x00
+#define B43_PIO_TXDATA		0x02
+#define B43_PIO_TXQBUFSIZE		0x04
+#define B43_PIO_RXCTL		0x08
+#define B43_PIO_RXDATA		0x0A
+
+#define B43_PIO_TXCTL_WRITELO	(1 << 0)
+#define B43_PIO_TXCTL_WRITEHI	(1 << 1)
+#define B43_PIO_TXCTL_COMPLETE	(1 << 2)
+#define B43_PIO_TXCTL_INIT		(1 << 3)
+#define B43_PIO_TXCTL_SUSPEND	(1 << 7)
+
+#define B43_PIO_RXCTL_DATAAVAILABLE	(1 << 0)
+#define B43_PIO_RXCTL_READY		(1 << 1)
+
+/* PIO constants */
+#define B43_PIO_MAXTXDEVQPACKETS	31
+#define B43_PIO_TXQADJUST		80
+
+/* PIO tuning knobs */
+#define B43_PIO_MAXTXPACKETS	256
+
+#ifdef CONFIG_B43_PIO
+
+struct b43_pioqueue;
+struct b43_xmitstatus;
+
+struct b43_pio_txpacket {
+	struct b43_pioqueue *queue;
+	struct sk_buff *skb;
+	struct ieee80211_tx_status txstat;
+	struct list_head list;
+	u16 index; /* Index in the tx_packets_cache */
+};
+
+struct b43_pioqueue {
+	struct b43_wldev *dev;
+	u16 mmio_base;
+
+	bool tx_suspended;
+	bool tx_frozen;
+	bool need_workarounds;	/* Workarounds needed for core.rev < 3 */
+
+	/* Adjusted size of the device internal TX buffer. */
+	u16 tx_devq_size;
+	/* Used octets of the device internal TX buffer. */
+	u16 tx_devq_used;
+	/* Used packet slots in the device internal TX buffer. */
+	u8 tx_devq_packets;
+	/* Packets from the txfree list can
+	 * be taken on incoming TX requests.
+	 */
+	struct list_head txfree;
+	unsigned int nr_txfree;
+	/* Packets on the txqueue are queued,
+	 * but not completely written to the chip, yet.
+	 */
+	struct list_head txqueue;
+	/* Packets on the txrunning queue are completely
+	 * posted to the device. We are waiting for the txstatus.
+	 */
+	struct list_head txrunning;
+	/* Total number or packets sent.
+	 * (This counter can obviously wrap).
+	 */
+	unsigned int nr_tx_packets;
+	struct tasklet_struct txtask;
+	struct b43_pio_txpacket tx_packets_cache[B43_PIO_MAXTXPACKETS];
+};
+
+static inline u16 b43_pio_read(struct b43_pioqueue *queue, u16 offset)
+{
+	return b43_read16(queue->dev, queue->mmio_base + offset);
+}
+
+static inline
+    void b43_pio_write(struct b43_pioqueue *queue, u16 offset, u16 value)
+{
+	b43_write16(queue->dev, queue->mmio_base + offset, value);
+	mmiowb();
+}
+
+int b43_pio_init(struct b43_wldev *dev);
+void b43_pio_free(struct b43_wldev *dev);
+
+int b43_pio_tx(struct b43_wldev *dev,
+	       struct sk_buff *skb, struct ieee80211_tx_control *ctl);
+void b43_pio_handle_txstatus(struct b43_wldev *dev,
+			     const struct b43_txstatus *status);
+void b43_pio_get_tx_stats(struct b43_wldev *dev,
+			  struct ieee80211_tx_queue_stats *stats);
+void b43_pio_rx(struct b43_pioqueue *queue);
+
+/* Suspend TX queue in hardware. */
+void b43_pio_tx_suspend(struct b43_pioqueue *queue);
+void b43_pio_tx_resume(struct b43_pioqueue *queue);
+/* Suspend (freeze) the TX tasklet (software level). */
+void b43_pio_freeze_txqueues(struct b43_wldev *dev);
+void b43_pio_thaw_txqueues(struct b43_wldev *dev);
+
+#else /* CONFIG_B43_PIO */
+
+static inline int b43_pio_init(struct b43_wldev *dev)
+{
+	return 0;
+}
+static inline void b43_pio_free(struct b43_wldev *dev)
+{
+}
+static inline
+    int b43_pio_tx(struct b43_wldev *dev,
+		   struct sk_buff *skb, struct ieee80211_tx_control *ctl)
+{
+	return 0;
+}
+static inline
+    void b43_pio_handle_txstatus(struct b43_wldev *dev,
+				 const struct b43_txstatus *status)
+{
+}
+static inline
+    void b43_pio_get_tx_stats(struct b43_wldev *dev,
+			      struct ieee80211_tx_queue_stats *stats)
+{
+}
+static inline void b43_pio_rx(struct b43_pioqueue *queue)
+{
+}
+static inline void b43_pio_tx_suspend(struct b43_pioqueue *queue)
+{
+}
+static inline void b43_pio_tx_resume(struct b43_pioqueue *queue)
+{
+}
+static inline void b43_pio_freeze_txqueues(struct b43_wldev *dev)
+{
+}
+static inline void b43_pio_thaw_txqueues(struct b43_wldev *dev)
+{
+}
+
+#endif /* CONFIG_B43_PIO */
+#endif /* B43_PIO_H_ */
diff --git a/drivers/net/wireless/b43/rfkill.c b/drivers/net/wireless/b43/rfkill.c
new file mode 100644
index 000000000000..800e0a61a7f5
--- /dev/null
+++ b/drivers/net/wireless/b43/rfkill.c
@@ -0,0 +1,184 @@
+/*
+
+  Broadcom B43 wireless driver
+  RFKILL support
+
+  Copyright (c) 2007 Michael Buesch <mb@bu3sch.de>
+
+  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, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with this program; see the file COPYING.  If not, write to
+  the Free Software Foundation, Inc., 51 Franklin Steet, Fifth Floor,
+  Boston, MA 02110-1301, USA.
+
+*/
+
+#include "rfkill.h"
+#include "b43.h"
+
+
+/* Returns TRUE, if the radio is enabled in hardware. */
+static bool b43_is_hw_radio_enabled(struct b43_wldev *dev)
+{
+	if (dev->phy.rev >= 3) {
+		if (!(b43_read32(dev, B43_MMIO_RADIO_HWENABLED_HI)
+		      & B43_MMIO_RADIO_HWENABLED_HI_MASK))
+			return 1;
+	} else {
+		if (b43_read16(dev, B43_MMIO_RADIO_HWENABLED_LO)
+		    & B43_MMIO_RADIO_HWENABLED_LO_MASK)
+			return 1;
+	}
+	return 0;
+}
+
+/* The poll callback for the hardware button. */
+static void b43_rfkill_poll(struct input_polled_dev *poll_dev)
+{
+	struct b43_wldev *dev = poll_dev->private;
+	struct b43_wl *wl = dev->wl;
+	bool enabled;
+
+	mutex_lock(&wl->mutex);
+	B43_WARN_ON(b43_status(dev) < B43_STAT_INITIALIZED);
+	enabled = b43_is_hw_radio_enabled(dev);
+	if (unlikely(enabled != dev->radio_hw_enable)) {
+		dev->radio_hw_enable = enabled;
+		b43info(wl, "Radio hardware status changed to %s\n",
+			enabled ? "ENABLED" : "DISABLED");
+		mutex_unlock(&wl->mutex);
+		input_report_key(poll_dev->input, KEY_WLAN, enabled);
+	} else
+		mutex_unlock(&wl->mutex);
+}
+
+/* Called when the RFKILL toggled in software.
+ * This is called without locking. */
+static int b43_rfkill_soft_toggle(void *data, enum rfkill_state state)
+{
+	struct b43_wldev *dev = data;
+	struct b43_wl *wl = dev->wl;
+	int err = 0;
+
+	mutex_lock(&wl->mutex);
+	if (b43_status(dev) < B43_STAT_INITIALIZED)
+		goto out_unlock;
+
+	switch (state) {
+	case RFKILL_STATE_ON:
+		if (!dev->radio_hw_enable) {
+			/* No luck. We can't toggle the hardware RF-kill
+			 * button from software. */
+			err = -EBUSY;
+			goto out_unlock;
+		}
+		if (!dev->phy.radio_on)
+			b43_radio_turn_on(dev);
+		break;
+	case RFKILL_STATE_OFF:
+		if (dev->phy.radio_on)
+			b43_radio_turn_off(dev, 0);
+		break;
+	}
+
+out_unlock:
+	mutex_unlock(&wl->mutex);
+
+	return err;
+}
+
+char * b43_rfkill_led_name(struct b43_wldev *dev)
+{
+	struct b43_wl *wl = dev->wl;
+
+	if (!wl->rfkill.rfkill)
+		return NULL;
+	return rfkill_get_led_name(wl->rfkill.rfkill);
+}
+
+void b43_rfkill_init(struct b43_wldev *dev)
+{
+	struct b43_wl *wl = dev->wl;
+	struct b43_rfkill *rfk = &(wl->rfkill);
+	int err;
+
+	if (rfk->rfkill) {
+		err = rfkill_register(rfk->rfkill);
+		if (err) {
+			b43warn(wl, "Failed to register RF-kill button\n");
+			goto err_free_rfk;
+		}
+	}
+	if (rfk->poll_dev) {
+		err = input_register_polled_device(rfk->poll_dev);
+		if (err) {
+			b43warn(wl, "Failed to register RF-kill polldev\n");
+			goto err_free_polldev;
+		}
+	}
+
+	return;
+err_free_rfk:
+	rfkill_free(rfk->rfkill);
+	rfk->rfkill = NULL;
+err_free_polldev:
+	input_free_polled_device(rfk->poll_dev);
+	rfk->poll_dev = NULL;
+}
+
+void b43_rfkill_exit(struct b43_wldev *dev)
+{
+	struct b43_rfkill *rfk = &(dev->wl->rfkill);
+
+	if (rfk->poll_dev)
+		input_unregister_polled_device(rfk->poll_dev);
+	if (rfk->rfkill)
+		rfkill_unregister(rfk->rfkill);
+}
+
+void b43_rfkill_alloc(struct b43_wldev *dev)
+{
+	struct b43_wl *wl = dev->wl;
+	struct b43_rfkill *rfk = &(wl->rfkill);
+
+	snprintf(rfk->name, sizeof(rfk->name),
+		 "b43-%s", wiphy_name(wl->hw->wiphy));
+
+	rfk->rfkill = rfkill_allocate(dev->dev->dev, RFKILL_TYPE_WLAN);
+	if (!rfk->rfkill) {
+		b43warn(wl, "Failed to allocate RF-kill button\n");
+		return;
+	}
+	rfk->rfkill->name = rfk->name;
+	rfk->rfkill->state = RFKILL_STATE_ON;
+	rfk->rfkill->data = dev;
+	rfk->rfkill->toggle_radio = b43_rfkill_soft_toggle;
+	rfk->rfkill->user_claim_unsupported = 1;
+
+	rfk->poll_dev = input_allocate_polled_device();
+	if (rfk->poll_dev) {
+		rfk->poll_dev->private = dev;
+		rfk->poll_dev->poll = b43_rfkill_poll;
+		rfk->poll_dev->poll_interval = 1000; /* msecs */
+	} else
+		b43warn(wl, "Failed to allocate RF-kill polldev\n");
+}
+
+void b43_rfkill_free(struct b43_wldev *dev)
+{
+	struct b43_rfkill *rfk = &(dev->wl->rfkill);
+
+	input_free_polled_device(rfk->poll_dev);
+	rfk->poll_dev = NULL;
+	rfkill_free(rfk->rfkill);
+	rfk->rfkill = NULL;
+}
diff --git a/drivers/net/wireless/b43/rfkill.h b/drivers/net/wireless/b43/rfkill.h
new file mode 100644
index 000000000000..29544e8c9e5f
--- /dev/null
+++ b/drivers/net/wireless/b43/rfkill.h
@@ -0,0 +1,58 @@
+#ifndef B43_RFKILL_H_
+#define B43_RFKILL_H_
+
+struct b43_wldev;
+
+
+#ifdef CONFIG_B43_RFKILL
+
+#include <linux/rfkill.h>
+#include <linux/input-polldev.h>
+
+
+struct b43_rfkill {
+	/* The RFKILL subsystem data structure */
+	struct rfkill *rfkill;
+	/* The poll device for the RFKILL input button */
+	struct input_polled_dev *poll_dev;
+	/* The unique name of this rfkill switch */
+	char name[32];
+};
+
+/* All the init functions return void, because we are not interested
+ * in failing the b43 init process when rfkill init failed. */
+void b43_rfkill_alloc(struct b43_wldev *dev);
+void b43_rfkill_free(struct b43_wldev *dev);
+void b43_rfkill_init(struct b43_wldev *dev);
+void b43_rfkill_exit(struct b43_wldev *dev);
+
+char * b43_rfkill_led_name(struct b43_wldev *dev);
+
+
+#else /* CONFIG_B43_RFKILL */
+/* No RFKILL support. */
+
+struct b43_rfkill {
+	/* empty */
+};
+
+static inline void b43_rfkill_alloc(struct b43_wldev *dev)
+{
+}
+static inline void b43_rfkill_free(struct b43_wldev *dev)
+{
+}
+static inline void b43_rfkill_init(struct b43_wldev *dev)
+{
+}
+static inline void b43_rfkill_exit(struct b43_wldev *dev)
+{
+}
+static inline char * b43_rfkill_led_name(struct b43_wldev *dev)
+{
+	return NULL;
+}
+
+#endif /* CONFIG_B43_RFKILL */
+
+#endif /* B43_RFKILL_H_ */
diff --git a/drivers/net/wireless/b43/sysfs.c b/drivers/net/wireless/b43/sysfs.c
new file mode 100644
index 000000000000..f4faff6a7d6c
--- /dev/null
+++ b/drivers/net/wireless/b43/sysfs.c
@@ -0,0 +1,236 @@
+/*
+
+  Broadcom B43 wireless driver
+
+  SYSFS support routines
+
+  Copyright (c) 2006 Michael Buesch <mb@bu3sch.de>
+
+  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, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with this program; see the file COPYING.  If not, write to
+  the Free Software Foundation, Inc., 51 Franklin Steet, Fifth Floor,
+  Boston, MA 02110-1301, USA.
+
+*/
+
+#include <linux/capability.h>
+#include <linux/io.h>
+
+#include "b43.h"
+#include "sysfs.h"
+#include "main.h"
+#include "phy.h"
+
+#define GENERIC_FILESIZE	64
+
+static int get_integer(const char *buf, size_t count)
+{
+	char tmp[10 + 1] = { 0 };
+	int ret = -EINVAL;
+
+	if (count == 0)
+		goto out;
+	count = min(count, (size_t) 10);
+	memcpy(tmp, buf, count);
+	ret = simple_strtol(tmp, NULL, 10);
+      out:
+	return ret;
+}
+
+static int get_boolean(const char *buf, size_t count)
+{
+	if (count != 0) {
+		if (buf[0] == '1')
+			return 1;
+		if (buf[0] == '0')
+			return 0;
+		if (count >= 4 && memcmp(buf, "true", 4) == 0)
+			return 1;
+		if (count >= 5 && memcmp(buf, "false", 5) == 0)
+			return 0;
+		if (count >= 3 && memcmp(buf, "yes", 3) == 0)
+			return 1;
+		if (count >= 2 && memcmp(buf, "no", 2) == 0)
+			return 0;
+		if (count >= 2 && memcmp(buf, "on", 2) == 0)
+			return 1;
+		if (count >= 3 && memcmp(buf, "off", 3) == 0)
+			return 0;
+	}
+	return -EINVAL;
+}
+
+static ssize_t b43_attr_interfmode_show(struct device *dev,
+					struct device_attribute *attr,
+					char *buf)
+{
+	struct b43_wldev *wldev = dev_to_b43_wldev(dev);
+	ssize_t count = 0;
+
+	if (!capable(CAP_NET_ADMIN))
+		return -EPERM;
+
+	mutex_lock(&wldev->wl->mutex);
+
+	switch (wldev->phy.interfmode) {
+	case B43_INTERFMODE_NONE:
+		count =
+		    snprintf(buf, PAGE_SIZE,
+			     "0 (No Interference Mitigation)\n");
+		break;
+	case B43_INTERFMODE_NONWLAN:
+		count =
+		    snprintf(buf, PAGE_SIZE,
+			     "1 (Non-WLAN Interference Mitigation)\n");
+		break;
+	case B43_INTERFMODE_MANUALWLAN:
+		count =
+		    snprintf(buf, PAGE_SIZE,
+			     "2 (WLAN Interference Mitigation)\n");
+		break;
+	default:
+		B43_WARN_ON(1);
+	}
+
+	mutex_unlock(&wldev->wl->mutex);
+
+	return count;
+}
+
+static ssize_t b43_attr_interfmode_store(struct device *dev,
+					 struct device_attribute *attr,
+					 const char *buf, size_t count)
+{
+	struct b43_wldev *wldev = dev_to_b43_wldev(dev);
+	unsigned long flags;
+	int err;
+	int mode;
+
+	if (!capable(CAP_NET_ADMIN))
+		return -EPERM;
+
+	mode = get_integer(buf, count);
+	switch (mode) {
+	case 0:
+		mode = B43_INTERFMODE_NONE;
+		break;
+	case 1:
+		mode = B43_INTERFMODE_NONWLAN;
+		break;
+	case 2:
+		mode = B43_INTERFMODE_MANUALWLAN;
+		break;
+	case 3:
+		mode = B43_INTERFMODE_AUTOWLAN;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	mutex_lock(&wldev->wl->mutex);
+	spin_lock_irqsave(&wldev->wl->irq_lock, flags);
+
+	err = b43_radio_set_interference_mitigation(wldev, mode);
+	if (err) {
+		b43err(wldev->wl, "Interference Mitigation not "
+		       "supported by device\n");
+	}
+	mmiowb();
+	spin_unlock_irqrestore(&wldev->wl->irq_lock, flags);
+	mutex_unlock(&wldev->wl->mutex);
+
+	return err ? err : count;
+}
+
+static DEVICE_ATTR(interference, 0644,
+		   b43_attr_interfmode_show, b43_attr_interfmode_store);
+
+static ssize_t b43_attr_preamble_show(struct device *dev,
+				      struct device_attribute *attr, char *buf)
+{
+	struct b43_wldev *wldev = dev_to_b43_wldev(dev);
+	ssize_t count;
+
+	if (!capable(CAP_NET_ADMIN))
+		return -EPERM;
+
+	mutex_lock(&wldev->wl->mutex);
+
+	if (wldev->short_preamble)
+		count =
+		    snprintf(buf, PAGE_SIZE, "1 (Short Preamble enabled)\n");
+	else
+		count =
+		    snprintf(buf, PAGE_SIZE, "0 (Short Preamble disabled)\n");
+
+	mutex_unlock(&wldev->wl->mutex);
+
+	return count;
+}
+
+static ssize_t b43_attr_preamble_store(struct device *dev,
+				       struct device_attribute *attr,
+				       const char *buf, size_t count)
+{
+	struct b43_wldev *wldev = dev_to_b43_wldev(dev);
+	unsigned long flags;
+	int value;
+
+	if (!capable(CAP_NET_ADMIN))
+		return -EPERM;
+
+	value = get_boolean(buf, count);
+	if (value < 0)
+		return value;
+	mutex_lock(&wldev->wl->mutex);
+	spin_lock_irqsave(&wldev->wl->irq_lock, flags);
+
+	wldev->short_preamble = !!value;
+
+	spin_unlock_irqrestore(&wldev->wl->irq_lock, flags);
+	mutex_unlock(&wldev->wl->mutex);
+
+	return count;
+}
+
+static DEVICE_ATTR(shortpreamble, 0644,
+		   b43_attr_preamble_show, b43_attr_preamble_store);
+
+int b43_sysfs_register(struct b43_wldev *wldev)
+{
+	struct device *dev = wldev->dev->dev;
+	int err;
+
+	B43_WARN_ON(b43_status(wldev) != B43_STAT_INITIALIZED);
+
+	err = device_create_file(dev, &dev_attr_interference);
+	if (err)
+		goto out;
+	err = device_create_file(dev, &dev_attr_shortpreamble);
+	if (err)
+		goto err_remove_interfmode;
+
+      out:
+	return err;
+      err_remove_interfmode:
+	device_remove_file(dev, &dev_attr_interference);
+	goto out;
+}
+
+void b43_sysfs_unregister(struct b43_wldev *wldev)
+{
+	struct device *dev = wldev->dev->dev;
+
+	device_remove_file(dev, &dev_attr_shortpreamble);
+	device_remove_file(dev, &dev_attr_interference);
+}
diff --git a/drivers/net/wireless/b43/sysfs.h b/drivers/net/wireless/b43/sysfs.h
new file mode 100644
index 000000000000..12bda9ef1a85
--- /dev/null
+++ b/drivers/net/wireless/b43/sysfs.h
@@ -0,0 +1,9 @@
+#ifndef B43_SYSFS_H_
+#define B43_SYSFS_H_
+
+struct b43_wldev;
+
+int b43_sysfs_register(struct b43_wldev *dev);
+void b43_sysfs_unregister(struct b43_wldev *dev);
+
+#endif /* B43_SYSFS_H_ */
diff --git a/drivers/net/wireless/b43/tables.c b/drivers/net/wireless/b43/tables.c
new file mode 100644
index 000000000000..15a87183a572
--- /dev/null
+++ b/drivers/net/wireless/b43/tables.c
@@ -0,0 +1,375 @@
+/*
+
+  Broadcom B43 wireless driver
+
+  Copyright (c) 2005 Martin Langer <martin-langer@gmx.de>,
+  Copyright (c) 2005 Stefano Brivio <st3@riseup.net>
+  Copyright (c) 2006, 2006 Michael Buesch <mb@bu3sch.de>
+  Copyright (c) 2005 Danny van Dyk <kugelfang@gentoo.org>
+  Copyright (c) 2005 Andreas Jaggi <andreas.jaggi@waterwave.ch>
+
+  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, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with this program; see the file COPYING.  If not, write to
+  the Free Software Foundation, Inc., 51 Franklin Steet, Fifth Floor,
+  Boston, MA 02110-1301, USA.
+
+*/
+
+#include "b43.h"
+#include "tables.h"
+#include "phy.h"
+
+const u32 b43_tab_rotor[] = {
+	0xFEB93FFD, 0xFEC63FFD,	/* 0 */
+	0xFED23FFD, 0xFEDF3FFD,
+	0xFEEC3FFE, 0xFEF83FFE,
+	0xFF053FFE, 0xFF113FFE,
+	0xFF1E3FFE, 0xFF2A3FFF,	/* 8 */
+	0xFF373FFF, 0xFF443FFF,
+	0xFF503FFF, 0xFF5D3FFF,
+	0xFF693FFF, 0xFF763FFF,
+	0xFF824000, 0xFF8F4000,	/* 16 */
+	0xFF9B4000, 0xFFA84000,
+	0xFFB54000, 0xFFC14000,
+	0xFFCE4000, 0xFFDA4000,
+	0xFFE74000, 0xFFF34000,	/* 24 */
+	0x00004000, 0x000D4000,
+	0x00194000, 0x00264000,
+	0x00324000, 0x003F4000,
+	0x004B4000, 0x00584000,	/* 32 */
+	0x00654000, 0x00714000,
+	0x007E4000, 0x008A3FFF,
+	0x00973FFF, 0x00A33FFF,
+	0x00B03FFF, 0x00BC3FFF,	/* 40 */
+	0x00C93FFF, 0x00D63FFF,
+	0x00E23FFE, 0x00EF3FFE,
+	0x00FB3FFE, 0x01083FFE,
+	0x01143FFE, 0x01213FFD,	/* 48 */
+	0x012E3FFD, 0x013A3FFD,
+	0x01473FFD,
+};
+
+const u32 b43_tab_retard[] = {
+	0xDB93CB87, 0xD666CF64,	/* 0 */
+	0xD1FDD358, 0xCDA6D826,
+	0xCA38DD9F, 0xC729E2B4,
+	0xC469E88E, 0xC26AEE2B,
+	0xC0DEF46C, 0xC073FA62,	/* 8 */
+	0xC01D00D5, 0xC0760743,
+	0xC1560D1E, 0xC2E51369,
+	0xC4ED18FF, 0xC7AC1ED7,
+	0xCB2823B2, 0xCEFA28D9,	/* 16 */
+	0xD2F62D3F, 0xD7BB3197,
+	0xDCE53568, 0xE1FE3875,
+	0xE7D13B35, 0xED663D35,
+	0xF39B3EC4, 0xF98E3FA7,	/* 24 */
+	0x00004000, 0x06723FA7,
+	0x0C653EC4, 0x129A3D35,
+	0x182F3B35, 0x1E023875,
+	0x231B3568, 0x28453197,	/* 32 */
+	0x2D0A2D3F, 0x310628D9,
+	0x34D823B2, 0x38541ED7,
+	0x3B1318FF, 0x3D1B1369,
+	0x3EAA0D1E, 0x3F8A0743,	/* 40 */
+	0x3FE300D5, 0x3F8DFA62,
+	0x3F22F46C, 0x3D96EE2B,
+	0x3B97E88E, 0x38D7E2B4,
+	0x35C8DD9F, 0x325AD826,	/* 48 */
+	0x2E03D358, 0x299ACF64,
+	0x246DCB87,
+};
+
+const u16 b43_tab_finefreqa[] = {
+	0x0082, 0x0082, 0x0102, 0x0182,	/* 0 */
+	0x0202, 0x0282, 0x0302, 0x0382,
+	0x0402, 0x0482, 0x0502, 0x0582,
+	0x05E2, 0x0662, 0x06E2, 0x0762,
+	0x07E2, 0x0842, 0x08C2, 0x0942,	/* 16 */
+	0x09C2, 0x0A22, 0x0AA2, 0x0B02,
+	0x0B82, 0x0BE2, 0x0C62, 0x0CC2,
+	0x0D42, 0x0DA2, 0x0E02, 0x0E62,
+	0x0EE2, 0x0F42, 0x0FA2, 0x1002,	/* 32 */
+	0x1062, 0x10C2, 0x1122, 0x1182,
+	0x11E2, 0x1242, 0x12A2, 0x12E2,
+	0x1342, 0x13A2, 0x1402, 0x1442,
+	0x14A2, 0x14E2, 0x1542, 0x1582,	/* 48 */
+	0x15E2, 0x1622, 0x1662, 0x16C1,
+	0x1701, 0x1741, 0x1781, 0x17E1,
+	0x1821, 0x1861, 0x18A1, 0x18E1,
+	0x1921, 0x1961, 0x19A1, 0x19E1,	/* 64 */
+	0x1A21, 0x1A61, 0x1AA1, 0x1AC1,
+	0x1B01, 0x1B41, 0x1B81, 0x1BA1,
+	0x1BE1, 0x1C21, 0x1C41, 0x1C81,
+	0x1CA1, 0x1CE1, 0x1D01, 0x1D41,	/* 80 */
+	0x1D61, 0x1DA1, 0x1DC1, 0x1E01,
+	0x1E21, 0x1E61, 0x1E81, 0x1EA1,
+	0x1EE1, 0x1F01, 0x1F21, 0x1F41,
+	0x1F81, 0x1FA1, 0x1FC1, 0x1FE1,	/* 96 */
+	0x2001, 0x2041, 0x2061, 0x2081,
+	0x20A1, 0x20C1, 0x20E1, 0x2101,
+	0x2121, 0x2141, 0x2161, 0x2181,
+	0x21A1, 0x21C1, 0x21E1, 0x2201,	/* 112 */
+	0x2221, 0x2241, 0x2261, 0x2281,
+	0x22A1, 0x22C1, 0x22C1, 0x22E1,
+	0x2301, 0x2321, 0x2341, 0x2361,
+	0x2361, 0x2381, 0x23A1, 0x23C1,	/* 128 */
+	0x23E1, 0x23E1, 0x2401, 0x2421,
+	0x2441, 0x2441, 0x2461, 0x2481,
+	0x2481, 0x24A1, 0x24C1, 0x24C1,
+	0x24E1, 0x2501, 0x2501, 0x2521,	/* 144 */
+	0x2541, 0x2541, 0x2561, 0x2561,
+	0x2581, 0x25A1, 0x25A1, 0x25C1,
+	0x25C1, 0x25E1, 0x2601, 0x2601,
+	0x2621, 0x2621, 0x2641, 0x2641,	/* 160 */
+	0x2661, 0x2661, 0x2681, 0x2681,
+	0x26A1, 0x26A1, 0x26C1, 0x26C1,
+	0x26E1, 0x26E1, 0x2701, 0x2701,
+	0x2721, 0x2721, 0x2740, 0x2740,	/* 176 */
+	0x2760, 0x2760, 0x2780, 0x2780,
+	0x2780, 0x27A0, 0x27A0, 0x27C0,
+	0x27C0, 0x27E0, 0x27E0, 0x27E0,
+	0x2800, 0x2800, 0x2820, 0x2820,	/* 192 */
+	0x2820, 0x2840, 0x2840, 0x2840,
+	0x2860, 0x2860, 0x2880, 0x2880,
+	0x2880, 0x28A0, 0x28A0, 0x28A0,
+	0x28C0, 0x28C0, 0x28C0, 0x28E0,	/* 208 */
+	0x28E0, 0x28E0, 0x2900, 0x2900,
+	0x2900, 0x2920, 0x2920, 0x2920,
+	0x2940, 0x2940, 0x2940, 0x2960,
+	0x2960, 0x2960, 0x2960, 0x2980,	/* 224 */
+	0x2980, 0x2980, 0x29A0, 0x29A0,
+	0x29A0, 0x29A0, 0x29C0, 0x29C0,
+	0x29C0, 0x29E0, 0x29E0, 0x29E0,
+	0x29E0, 0x2A00, 0x2A00, 0x2A00,	/* 240 */
+	0x2A00, 0x2A20, 0x2A20, 0x2A20,
+	0x2A20, 0x2A40, 0x2A40, 0x2A40,
+	0x2A40, 0x2A60, 0x2A60, 0x2A60,
+};
+
+const u16 b43_tab_finefreqg[] = {
+	0x0089, 0x02E9, 0x0409, 0x04E9,	/* 0 */
+	0x05A9, 0x0669, 0x0709, 0x0789,
+	0x0829, 0x08A9, 0x0929, 0x0989,
+	0x0A09, 0x0A69, 0x0AC9, 0x0B29,
+	0x0BA9, 0x0BE9, 0x0C49, 0x0CA9,	/* 16 */
+	0x0D09, 0x0D69, 0x0DA9, 0x0E09,
+	0x0E69, 0x0EA9, 0x0F09, 0x0F49,
+	0x0FA9, 0x0FE9, 0x1029, 0x1089,
+	0x10C9, 0x1109, 0x1169, 0x11A9,	/* 32 */
+	0x11E9, 0x1229, 0x1289, 0x12C9,
+	0x1309, 0x1349, 0x1389, 0x13C9,
+	0x1409, 0x1449, 0x14A9, 0x14E9,
+	0x1529, 0x1569, 0x15A9, 0x15E9,	/* 48 */
+	0x1629, 0x1669, 0x16A9, 0x16E8,
+	0x1728, 0x1768, 0x17A8, 0x17E8,
+	0x1828, 0x1868, 0x18A8, 0x18E8,
+	0x1928, 0x1968, 0x19A8, 0x19E8,	/* 64 */
+	0x1A28, 0x1A68, 0x1AA8, 0x1AE8,
+	0x1B28, 0x1B68, 0x1BA8, 0x1BE8,
+	0x1C28, 0x1C68, 0x1CA8, 0x1CE8,
+	0x1D28, 0x1D68, 0x1DC8, 0x1E08,	/* 80 */
+	0x1E48, 0x1E88, 0x1EC8, 0x1F08,
+	0x1F48, 0x1F88, 0x1FE8, 0x2028,
+	0x2068, 0x20A8, 0x2108, 0x2148,
+	0x2188, 0x21C8, 0x2228, 0x2268,	/* 96 */
+	0x22C8, 0x2308, 0x2348, 0x23A8,
+	0x23E8, 0x2448, 0x24A8, 0x24E8,
+	0x2548, 0x25A8, 0x2608, 0x2668,
+	0x26C8, 0x2728, 0x2787, 0x27E7,	/* 112 */
+	0x2847, 0x28C7, 0x2947, 0x29A7,
+	0x2A27, 0x2AC7, 0x2B47, 0x2BE7,
+	0x2CA7, 0x2D67, 0x2E47, 0x2F67,
+	0x3247, 0x3526, 0x3646, 0x3726,	/* 128 */
+	0x3806, 0x38A6, 0x3946, 0x39E6,
+	0x3A66, 0x3AE6, 0x3B66, 0x3BC6,
+	0x3C45, 0x3CA5, 0x3D05, 0x3D85,
+	0x3DE5, 0x3E45, 0x3EA5, 0x3EE5,	/* 144 */
+	0x3F45, 0x3FA5, 0x4005, 0x4045,
+	0x40A5, 0x40E5, 0x4145, 0x4185,
+	0x41E5, 0x4225, 0x4265, 0x42C5,
+	0x4305, 0x4345, 0x43A5, 0x43E5,	/* 160 */
+	0x4424, 0x4464, 0x44C4, 0x4504,
+	0x4544, 0x4584, 0x45C4, 0x4604,
+	0x4644, 0x46A4, 0x46E4, 0x4724,
+	0x4764, 0x47A4, 0x47E4, 0x4824,	/* 176 */
+	0x4864, 0x48A4, 0x48E4, 0x4924,
+	0x4964, 0x49A4, 0x49E4, 0x4A24,
+	0x4A64, 0x4AA4, 0x4AE4, 0x4B23,
+	0x4B63, 0x4BA3, 0x4BE3, 0x4C23,	/* 192 */
+	0x4C63, 0x4CA3, 0x4CE3, 0x4D23,
+	0x4D63, 0x4DA3, 0x4DE3, 0x4E23,
+	0x4E63, 0x4EA3, 0x4EE3, 0x4F23,
+	0x4F63, 0x4FC3, 0x5003, 0x5043,	/* 208 */
+	0x5083, 0x50C3, 0x5103, 0x5143,
+	0x5183, 0x51E2, 0x5222, 0x5262,
+	0x52A2, 0x52E2, 0x5342, 0x5382,
+	0x53C2, 0x5402, 0x5462, 0x54A2,	/* 224 */
+	0x5502, 0x5542, 0x55A2, 0x55E2,
+	0x5642, 0x5682, 0x56E2, 0x5722,
+	0x5782, 0x57E1, 0x5841, 0x58A1,
+	0x5901, 0x5961, 0x59C1, 0x5A21,	/* 240 */
+	0x5AA1, 0x5B01, 0x5B81, 0x5BE1,
+	0x5C61, 0x5D01, 0x5D80, 0x5E20,
+	0x5EE0, 0x5FA0, 0x6080, 0x61C0,
+};
+
+const u16 b43_tab_noisea2[] = {
+	0x0001, 0x0001, 0x0001, 0xFFFE,
+	0xFFFE, 0x3FFF, 0x1000, 0x0393,
+};
+
+const u16 b43_tab_noisea3[] = {
+	0x4C4C, 0x4C4C, 0x4C4C, 0x2D36,
+	0x4C4C, 0x4C4C, 0x4C4C, 0x2D36,
+};
+
+const u16 b43_tab_noiseg1[] = {
+	0x013C, 0x01F5, 0x031A, 0x0631,
+	0x0001, 0x0001, 0x0001, 0x0001,
+};
+
+const u16 b43_tab_noiseg2[] = {
+	0x5484, 0x3C40, 0x0000, 0x0000,
+	0x0000, 0x0000, 0x0000, 0x0000,
+};
+
+const u16 b43_tab_noisescaleg1[] = {
+	0x6C77, 0x5162, 0x3B40, 0x3335,	/* 0 */
+	0x2F2D, 0x2A2A, 0x2527, 0x1F21,
+	0x1A1D, 0x1719, 0x1616, 0x1414,
+	0x1414, 0x1400, 0x1414, 0x1614,
+	0x1716, 0x1A19, 0x1F1D, 0x2521,	/* 16 */
+	0x2A27, 0x2F2A, 0x332D, 0x3B35,
+	0x5140, 0x6C62, 0x0077,
+};
+
+const u16 b43_tab_noisescaleg2[] = {
+	0xD8DD, 0xCBD4, 0xBCC0, 0XB6B7,	/* 0 */
+	0xB2B0, 0xADAD, 0xA7A9, 0x9FA1,
+	0x969B, 0x9195, 0x8F8F, 0x8A8A,
+	0x8A8A, 0x8A00, 0x8A8A, 0x8F8A,
+	0x918F, 0x9695, 0x9F9B, 0xA7A1,	/* 16 */
+	0xADA9, 0xB2AD, 0xB6B0, 0xBCB7,
+	0xCBC0, 0xD8D4, 0x00DD,
+};
+
+const u16 b43_tab_noisescaleg3[] = {
+	0xA4A4, 0xA4A4, 0xA4A4, 0xA4A4,	/* 0 */
+	0xA4A4, 0xA4A4, 0xA4A4, 0xA4A4,
+	0xA4A4, 0xA4A4, 0xA4A4, 0xA4A4,
+	0xA4A4, 0xA400, 0xA4A4, 0xA4A4,
+	0xA4A4, 0xA4A4, 0xA4A4, 0xA4A4,	/* 16 */
+	0xA4A4, 0xA4A4, 0xA4A4, 0xA4A4,
+	0xA4A4, 0xA4A4, 0x00A4,
+};
+
+const u16 b43_tab_sigmasqr1[] = {
+	0x007A, 0x0075, 0x0071, 0x006C,	/* 0 */
+	0x0067, 0x0063, 0x005E, 0x0059,
+	0x0054, 0x0050, 0x004B, 0x0046,
+	0x0042, 0x003D, 0x003D, 0x003D,
+	0x003D, 0x003D, 0x003D, 0x003D,	/* 16 */
+	0x003D, 0x003D, 0x003D, 0x003D,
+	0x003D, 0x003D, 0x0000, 0x003D,
+	0x003D, 0x003D, 0x003D, 0x003D,
+	0x003D, 0x003D, 0x003D, 0x003D,	/* 32 */
+	0x003D, 0x003D, 0x003D, 0x003D,
+	0x0042, 0x0046, 0x004B, 0x0050,
+	0x0054, 0x0059, 0x005E, 0x0063,
+	0x0067, 0x006C, 0x0071, 0x0075,	/* 48 */
+	0x007A,
+};
+
+const u16 b43_tab_sigmasqr2[] = {
+	0x00DE, 0x00DC, 0x00DA, 0x00D8,	/* 0 */
+	0x00D6, 0x00D4, 0x00D2, 0x00CF,
+	0x00CD, 0x00CA, 0x00C7, 0x00C4,
+	0x00C1, 0x00BE, 0x00BE, 0x00BE,
+	0x00BE, 0x00BE, 0x00BE, 0x00BE,	/* 16 */
+	0x00BE, 0x00BE, 0x00BE, 0x00BE,
+	0x00BE, 0x00BE, 0x0000, 0x00BE,
+	0x00BE, 0x00BE, 0x00BE, 0x00BE,
+	0x00BE, 0x00BE, 0x00BE, 0x00BE,	/* 32 */
+	0x00BE, 0x00BE, 0x00BE, 0x00BE,
+	0x00C1, 0x00C4, 0x00C7, 0x00CA,
+	0x00CD, 0x00CF, 0x00D2, 0x00D4,
+	0x00D6, 0x00D8, 0x00DA, 0x00DC,	/* 48 */
+	0x00DE,
+};
+
+static inline void assert_sizes(void)
+{
+	BUILD_BUG_ON(B43_TAB_ROTOR_SIZE != ARRAY_SIZE(b43_tab_rotor));
+	BUILD_BUG_ON(B43_TAB_RETARD_SIZE != ARRAY_SIZE(b43_tab_retard));
+	BUILD_BUG_ON(B43_TAB_FINEFREQA_SIZE != ARRAY_SIZE(b43_tab_finefreqa));
+	BUILD_BUG_ON(B43_TAB_FINEFREQG_SIZE != ARRAY_SIZE(b43_tab_finefreqg));
+	BUILD_BUG_ON(B43_TAB_NOISEA2_SIZE != ARRAY_SIZE(b43_tab_noisea2));
+	BUILD_BUG_ON(B43_TAB_NOISEA3_SIZE != ARRAY_SIZE(b43_tab_noisea3));
+	BUILD_BUG_ON(B43_TAB_NOISEG1_SIZE != ARRAY_SIZE(b43_tab_noiseg1));
+	BUILD_BUG_ON(B43_TAB_NOISEG2_SIZE != ARRAY_SIZE(b43_tab_noiseg2));
+	BUILD_BUG_ON(B43_TAB_NOISESCALEG_SIZE !=
+		     ARRAY_SIZE(b43_tab_noisescaleg1));
+	BUILD_BUG_ON(B43_TAB_NOISESCALEG_SIZE !=
+		     ARRAY_SIZE(b43_tab_noisescaleg2));
+	BUILD_BUG_ON(B43_TAB_NOISESCALEG_SIZE !=
+		     ARRAY_SIZE(b43_tab_noisescaleg3));
+	BUILD_BUG_ON(B43_TAB_SIGMASQR_SIZE != ARRAY_SIZE(b43_tab_sigmasqr1));
+	BUILD_BUG_ON(B43_TAB_SIGMASQR_SIZE != ARRAY_SIZE(b43_tab_sigmasqr2));
+}
+
+u16 b43_ofdmtab_read16(struct b43_wldev *dev, u16 table, u16 offset)
+{
+	assert_sizes();
+
+	b43_phy_write(dev, B43_PHY_OTABLECTL, table + offset);
+	return b43_phy_read(dev, B43_PHY_OTABLEI);
+}
+
+void b43_ofdmtab_write16(struct b43_wldev *dev, u16 table,
+			 u16 offset, u16 value)
+{
+	b43_phy_write(dev, B43_PHY_OTABLECTL, table + offset);
+	b43_phy_write(dev, B43_PHY_OTABLEI, value);
+}
+
+u32 b43_ofdmtab_read32(struct b43_wldev *dev, u16 table, u16 offset)
+{
+	u32 ret;
+
+	b43_phy_write(dev, B43_PHY_OTABLECTL, table + offset);
+	ret = b43_phy_read(dev, B43_PHY_OTABLEQ);
+	ret <<= 16;
+	ret |= b43_phy_read(dev, B43_PHY_OTABLEI);
+
+	return ret;
+}
+
+void b43_ofdmtab_write32(struct b43_wldev *dev, u16 table,
+			 u16 offset, u32 value)
+{
+	b43_phy_write(dev, B43_PHY_OTABLECTL, table + offset);
+	b43_phy_write(dev, B43_PHY_OTABLEI, value);
+	b43_phy_write(dev, B43_PHY_OTABLEQ, (value >> 16));
+}
+
+u16 b43_gtab_read(struct b43_wldev *dev, u16 table, u16 offset)
+{
+	b43_phy_write(dev, B43_PHY_GTABCTL, table + offset);
+	return b43_phy_read(dev, B43_PHY_GTABDATA);
+}
+
+void b43_gtab_write(struct b43_wldev *dev, u16 table, u16 offset, u16 value)
+{
+	b43_phy_write(dev, B43_PHY_GTABCTL, table + offset);
+	b43_phy_write(dev, B43_PHY_GTABDATA, value);
+}
diff --git a/drivers/net/wireless/b43/tables.h b/drivers/net/wireless/b43/tables.h
new file mode 100644
index 000000000000..64635d7b518c
--- /dev/null
+++ b/drivers/net/wireless/b43/tables.h
@@ -0,0 +1,28 @@
+#ifndef B43_TABLES_H_
+#define B43_TABLES_H_
+
+#define B43_TAB_ROTOR_SIZE		53
+extern const u32 b43_tab_rotor[];
+#define B43_TAB_RETARD_SIZE		53
+extern const u32 b43_tab_retard[];
+#define B43_TAB_FINEFREQA_SIZE	256
+extern const u16 b43_tab_finefreqa[];
+#define B43_TAB_FINEFREQG_SIZE	256
+extern const u16 b43_tab_finefreqg[];
+#define B43_TAB_NOISEA2_SIZE	8
+extern const u16 b43_tab_noisea2[];
+#define B43_TAB_NOISEA3_SIZE	8
+extern const u16 b43_tab_noisea3[];
+#define B43_TAB_NOISEG1_SIZE	8
+extern const u16 b43_tab_noiseg1[];
+#define B43_TAB_NOISEG2_SIZE	8
+extern const u16 b43_tab_noiseg2[];
+#define B43_TAB_NOISESCALEG_SIZE	27
+extern const u16 b43_tab_noisescaleg1[];
+extern const u16 b43_tab_noisescaleg2[];
+extern const u16 b43_tab_noisescaleg3[];
+#define B43_TAB_SIGMASQR_SIZE	53
+extern const u16 b43_tab_sigmasqr1[];
+extern const u16 b43_tab_sigmasqr2[];
+
+#endif /* B43_TABLES_H_ */
diff --git a/drivers/net/wireless/b43/xmit.c b/drivers/net/wireless/b43/xmit.c
new file mode 100644
index 000000000000..0bd6f8a348a8
--- /dev/null
+++ b/drivers/net/wireless/b43/xmit.c
@@ -0,0 +1,650 @@
+/*
+
+  Broadcom B43 wireless driver
+
+  Transmission (TX/RX) related functions.
+
+  Copyright (C) 2005 Martin Langer <martin-langer@gmx.de>
+  Copyright (C) 2005 Stefano Brivio <st3@riseup.net>
+  Copyright (C) 2005, 2006 Michael Buesch <mb@bu3sch.de>
+  Copyright (C) 2005 Danny van Dyk <kugelfang@gentoo.org>
+  Copyright (C) 2005 Andreas Jaggi <andreas.jaggi@waterwave.ch>
+
+  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, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with this program; see the file COPYING.  If not, write to
+  the Free Software Foundation, Inc., 51 Franklin Steet, Fifth Floor,
+  Boston, MA 02110-1301, USA.
+
+*/
+
+#include "xmit.h"
+#include "phy.h"
+#include "dma.h"
+#include "pio.h"
+
+/* Extract the bitrate out of a CCK PLCP header. */
+static u8 b43_plcp_get_bitrate_cck(struct b43_plcp_hdr6 *plcp)
+{
+	switch (plcp->raw[0]) {
+	case 0x0A:
+		return B43_CCK_RATE_1MB;
+	case 0x14:
+		return B43_CCK_RATE_2MB;
+	case 0x37:
+		return B43_CCK_RATE_5MB;
+	case 0x6E:
+		return B43_CCK_RATE_11MB;
+	}
+	B43_WARN_ON(1);
+	return 0;
+}
+
+/* Extract the bitrate out of an OFDM PLCP header. */
+static u8 b43_plcp_get_bitrate_ofdm(struct b43_plcp_hdr6 *plcp)
+{
+	switch (plcp->raw[0] & 0xF) {
+	case 0xB:
+		return B43_OFDM_RATE_6MB;
+	case 0xF:
+		return B43_OFDM_RATE_9MB;
+	case 0xA:
+		return B43_OFDM_RATE_12MB;
+	case 0xE:
+		return B43_OFDM_RATE_18MB;
+	case 0x9:
+		return B43_OFDM_RATE_24MB;
+	case 0xD:
+		return B43_OFDM_RATE_36MB;
+	case 0x8:
+		return B43_OFDM_RATE_48MB;
+	case 0xC:
+		return B43_OFDM_RATE_54MB;
+	}
+	B43_WARN_ON(1);
+	return 0;
+}
+
+u8 b43_plcp_get_ratecode_cck(const u8 bitrate)
+{
+	switch (bitrate) {
+	case B43_CCK_RATE_1MB:
+		return 0x0A;
+	case B43_CCK_RATE_2MB:
+		return 0x14;
+	case B43_CCK_RATE_5MB:
+		return 0x37;
+	case B43_CCK_RATE_11MB:
+		return 0x6E;
+	}
+	B43_WARN_ON(1);
+	return 0;
+}
+
+u8 b43_plcp_get_ratecode_ofdm(const u8 bitrate)
+{
+	switch (bitrate) {
+	case B43_OFDM_RATE_6MB:
+		return 0xB;
+	case B43_OFDM_RATE_9MB:
+		return 0xF;
+	case B43_OFDM_RATE_12MB:
+		return 0xA;
+	case B43_OFDM_RATE_18MB:
+		return 0xE;
+	case B43_OFDM_RATE_24MB:
+		return 0x9;
+	case B43_OFDM_RATE_36MB:
+		return 0xD;
+	case B43_OFDM_RATE_48MB:
+		return 0x8;
+	case B43_OFDM_RATE_54MB:
+		return 0xC;
+	}
+	B43_WARN_ON(1);
+	return 0;
+}
+
+void b43_generate_plcp_hdr(struct b43_plcp_hdr4 *plcp,
+			   const u16 octets, const u8 bitrate)
+{
+	__le32 *data = &(plcp->data);
+	__u8 *raw = plcp->raw;
+
+	if (b43_is_ofdm_rate(bitrate)) {
+		u32 d;
+
+		d = b43_plcp_get_ratecode_ofdm(bitrate);
+		B43_WARN_ON(octets & 0xF000);
+		d |= (octets << 5);
+		*data = cpu_to_le32(d);
+	} else {
+		u32 plen;
+
+		plen = octets * 16 / bitrate;
+		if ((octets * 16 % bitrate) > 0) {
+			plen++;
+			if ((bitrate == B43_CCK_RATE_11MB)
+			    && ((octets * 8 % 11) < 4)) {
+				raw[1] = 0x84;
+			} else
+				raw[1] = 0x04;
+		} else
+			raw[1] = 0x04;
+		*data |= cpu_to_le32(plen << 16);
+		raw[0] = b43_plcp_get_ratecode_cck(bitrate);
+	}
+}
+
+static u8 b43_calc_fallback_rate(u8 bitrate)
+{
+	switch (bitrate) {
+	case B43_CCK_RATE_1MB:
+		return B43_CCK_RATE_1MB;
+	case B43_CCK_RATE_2MB:
+		return B43_CCK_RATE_1MB;
+	case B43_CCK_RATE_5MB:
+		return B43_CCK_RATE_2MB;
+	case B43_CCK_RATE_11MB:
+		return B43_CCK_RATE_5MB;
+	case B43_OFDM_RATE_6MB:
+		return B43_CCK_RATE_5MB;
+	case B43_OFDM_RATE_9MB:
+		return B43_OFDM_RATE_6MB;
+	case B43_OFDM_RATE_12MB:
+		return B43_OFDM_RATE_9MB;
+	case B43_OFDM_RATE_18MB:
+		return B43_OFDM_RATE_12MB;
+	case B43_OFDM_RATE_24MB:
+		return B43_OFDM_RATE_18MB;
+	case B43_OFDM_RATE_36MB:
+		return B43_OFDM_RATE_24MB;
+	case B43_OFDM_RATE_48MB:
+		return B43_OFDM_RATE_36MB;
+	case B43_OFDM_RATE_54MB:
+		return B43_OFDM_RATE_48MB;
+	}
+	B43_WARN_ON(1);
+	return 0;
+}
+
+static void generate_txhdr_fw4(struct b43_wldev *dev,
+			       struct b43_txhdr_fw4 *txhdr,
+			       const unsigned char *fragment_data,
+			       unsigned int fragment_len,
+			       const struct ieee80211_tx_control *txctl,
+			       u16 cookie)
+{
+	const struct b43_phy *phy = &dev->phy;
+	const struct ieee80211_hdr *wlhdr =
+	    (const struct ieee80211_hdr *)fragment_data;
+	int use_encryption = (!(txctl->flags & IEEE80211_TXCTL_DO_NOT_ENCRYPT));
+	u16 fctl = le16_to_cpu(wlhdr->frame_control);
+	u8 rate, rate_fb;
+	int rate_ofdm, rate_fb_ofdm;
+	unsigned int plcp_fragment_len;
+	u32 mac_ctl = 0;
+	u16 phy_ctl = 0;
+	u8 extra_ft = 0;
+
+	memset(txhdr, 0, sizeof(*txhdr));
+
+	rate = txctl->tx_rate;
+	rate_ofdm = b43_is_ofdm_rate(rate);
+	rate_fb = (txctl->alt_retry_rate == -1) ? rate : txctl->alt_retry_rate;
+	rate_fb_ofdm = b43_is_ofdm_rate(rate_fb);
+
+	if (rate_ofdm)
+		txhdr->phy_rate = b43_plcp_get_ratecode_ofdm(rate);
+	else
+		txhdr->phy_rate = b43_plcp_get_ratecode_cck(rate);
+	txhdr->mac_frame_ctl = wlhdr->frame_control;
+	memcpy(txhdr->tx_receiver, wlhdr->addr1, 6);
+
+	/* Calculate duration for fallback rate */
+	if ((rate_fb == rate) ||
+	    (wlhdr->duration_id & cpu_to_le16(0x8000)) ||
+	    (wlhdr->duration_id == cpu_to_le16(0))) {
+		/* If the fallback rate equals the normal rate or the
+		 * dur_id field contains an AID, CFP magic or 0,
+		 * use the original dur_id field. */
+		txhdr->dur_fb = wlhdr->duration_id;
+	} else {
+		int fbrate_base100kbps = B43_RATE_TO_BASE100KBPS(rate_fb);
+		txhdr->dur_fb = ieee80211_generic_frame_duration(dev->wl->hw,
+								 dev->wl->if_id,
+								 fragment_len,
+								 fbrate_base100kbps);
+	}
+
+	plcp_fragment_len = fragment_len + FCS_LEN;
+	if (use_encryption) {
+		u8 key_idx = (u16) (txctl->key_idx);
+		struct b43_key *key;
+		int wlhdr_len;
+		size_t iv_len;
+
+		B43_WARN_ON(key_idx >= dev->max_nr_keys);
+		key = &(dev->key[key_idx]);
+		B43_WARN_ON(!key->keyconf);
+
+		/* Hardware appends ICV. */
+		plcp_fragment_len += txctl->icv_len;
+
+		key_idx = b43_kidx_to_fw(dev, key_idx);
+		mac_ctl |= (key_idx << B43_TX4_MAC_KEYIDX_SHIFT) &
+			   B43_TX4_MAC_KEYIDX;
+		mac_ctl |= (key->algorithm << B43_TX4_MAC_KEYALG_SHIFT) &
+			   B43_TX4_MAC_KEYALG;
+		wlhdr_len = ieee80211_get_hdrlen(fctl);
+		iv_len = min((size_t) txctl->iv_len,
+			     ARRAY_SIZE(txhdr->iv));
+		memcpy(txhdr->iv, ((u8 *) wlhdr) + wlhdr_len, iv_len);
+	}
+	b43_generate_plcp_hdr((struct b43_plcp_hdr4 *)(&txhdr->plcp),
+			      plcp_fragment_len, rate);
+	b43_generate_plcp_hdr((struct b43_plcp_hdr4 *)(&txhdr->plcp_fb),
+			      plcp_fragment_len, rate_fb);
+
+	/* Extra Frame Types */
+	if (rate_fb_ofdm)
+		extra_ft |= B43_TX4_EFT_FBOFDM;
+
+	/* Set channel radio code. Note that the micrcode ORs 0x100 to
+	 * this value before comparing it to the value in SHM, if this
+	 * is a 5Ghz packet.
+	 */
+	txhdr->chan_radio_code = phy->channel;
+
+	/* PHY TX Control word */
+	if (rate_ofdm)
+		phy_ctl |= B43_TX4_PHY_OFDM;
+	if (dev->short_preamble)
+		phy_ctl |= B43_TX4_PHY_SHORTPRMBL;
+	switch (txctl->antenna_sel_tx) {
+	case 0:
+		phy_ctl |= B43_TX4_PHY_ANTLAST;
+		break;
+	case 1:
+		phy_ctl |= B43_TX4_PHY_ANT0;
+		break;
+	case 2:
+		phy_ctl |= B43_TX4_PHY_ANT1;
+		break;
+	default:
+		B43_WARN_ON(1);
+	}
+
+	/* MAC control */
+	if (!(txctl->flags & IEEE80211_TXCTL_NO_ACK))
+		mac_ctl |= B43_TX4_MAC_ACK;
+	if (!(((fctl & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_CTL) &&
+	      ((fctl & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_PSPOLL)))
+		mac_ctl |= B43_TX4_MAC_HWSEQ;
+	if (txctl->flags & IEEE80211_TXCTL_FIRST_FRAGMENT)
+		mac_ctl |= B43_TX4_MAC_STMSDU;
+	if (phy->type == B43_PHYTYPE_A)
+		mac_ctl |= B43_TX4_MAC_5GHZ;
+
+	/* Generate the RTS or CTS-to-self frame */
+	if ((txctl->flags & IEEE80211_TXCTL_USE_RTS_CTS) ||
+	    (txctl->flags & IEEE80211_TXCTL_USE_CTS_PROTECT)) {
+		unsigned int len;
+		struct ieee80211_hdr *hdr;
+		int rts_rate, rts_rate_fb;
+		int rts_rate_ofdm, rts_rate_fb_ofdm;
+
+		rts_rate = txctl->rts_cts_rate;
+		rts_rate_ofdm = b43_is_ofdm_rate(rts_rate);
+		rts_rate_fb = b43_calc_fallback_rate(rts_rate);
+		rts_rate_fb_ofdm = b43_is_ofdm_rate(rts_rate_fb);
+
+		if (txctl->flags & IEEE80211_TXCTL_USE_CTS_PROTECT) {
+			ieee80211_ctstoself_get(dev->wl->hw, dev->wl->if_id,
+						fragment_data, fragment_len,
+						txctl,
+						(struct ieee80211_cts *)(txhdr->
+									 rts_frame));
+			mac_ctl |= B43_TX4_MAC_SENDCTS;
+			len = sizeof(struct ieee80211_cts);
+		} else {
+			ieee80211_rts_get(dev->wl->hw, dev->wl->if_id,
+					  fragment_data, fragment_len, txctl,
+					  (struct ieee80211_rts *)(txhdr->
+								   rts_frame));
+			mac_ctl |= B43_TX4_MAC_SENDRTS;
+			len = sizeof(struct ieee80211_rts);
+		}
+		len += FCS_LEN;
+		b43_generate_plcp_hdr((struct b43_plcp_hdr4 *)(&txhdr->
+							       rts_plcp), len,
+				      rts_rate);
+		b43_generate_plcp_hdr((struct b43_plcp_hdr4 *)(&txhdr->
+							       rts_plcp_fb),
+				      len, rts_rate_fb);
+		hdr = (struct ieee80211_hdr *)(&txhdr->rts_frame);
+		txhdr->rts_dur_fb = hdr->duration_id;
+		if (rts_rate_ofdm) {
+			extra_ft |= B43_TX4_EFT_RTSOFDM;
+			txhdr->phy_rate_rts =
+			    b43_plcp_get_ratecode_ofdm(rts_rate);
+		} else
+			txhdr->phy_rate_rts =
+			    b43_plcp_get_ratecode_cck(rts_rate);
+		if (rts_rate_fb_ofdm)
+			extra_ft |= B43_TX4_EFT_RTSFBOFDM;
+		mac_ctl |= B43_TX4_MAC_LONGFRAME;
+	}
+
+	/* Magic cookie */
+	txhdr->cookie = cpu_to_le16(cookie);
+
+	/* Apply the bitfields */
+	txhdr->mac_ctl = cpu_to_le32(mac_ctl);
+	txhdr->phy_ctl = cpu_to_le16(phy_ctl);
+	txhdr->extra_ft = extra_ft;
+}
+
+void b43_generate_txhdr(struct b43_wldev *dev,
+			u8 * txhdr,
+			const unsigned char *fragment_data,
+			unsigned int fragment_len,
+			const struct ieee80211_tx_control *txctl, u16 cookie)
+{
+	generate_txhdr_fw4(dev, (struct b43_txhdr_fw4 *)txhdr,
+			   fragment_data, fragment_len, txctl, cookie);
+}
+
+static s8 b43_rssi_postprocess(struct b43_wldev *dev,
+			       u8 in_rssi, int ofdm,
+			       int adjust_2053, int adjust_2050)
+{
+	struct b43_phy *phy = &dev->phy;
+	s32 tmp;
+
+	switch (phy->radio_ver) {
+	case 0x2050:
+		if (ofdm) {
+			tmp = in_rssi;
+			if (tmp > 127)
+				tmp -= 256;
+			tmp *= 73;
+			tmp /= 64;
+			if (adjust_2050)
+				tmp += 25;
+			else
+				tmp -= 3;
+		} else {
+			if (dev->dev->bus->sprom.r1.
+			    boardflags_lo & B43_BFL_RSSI) {
+				if (in_rssi > 63)
+					in_rssi = 63;
+				tmp = phy->nrssi_lt[in_rssi];
+				tmp = 31 - tmp;
+				tmp *= -131;
+				tmp /= 128;
+				tmp -= 57;
+			} else {
+				tmp = in_rssi;
+				tmp = 31 - tmp;
+				tmp *= -149;
+				tmp /= 128;
+				tmp -= 68;
+			}
+			if (phy->type == B43_PHYTYPE_G && adjust_2050)
+				tmp += 25;
+		}
+		break;
+	case 0x2060:
+		if (in_rssi > 127)
+			tmp = in_rssi - 256;
+		else
+			tmp = in_rssi;
+		break;
+	default:
+		tmp = in_rssi;
+		tmp -= 11;
+		tmp *= 103;
+		tmp /= 64;
+		if (adjust_2053)
+			tmp -= 109;
+		else
+			tmp -= 83;
+	}
+
+	return (s8) tmp;
+}
+
+//TODO
+#if 0
+static s8 b43_rssinoise_postprocess(struct b43_wldev *dev, u8 in_rssi)
+{
+	struct b43_phy *phy = &dev->phy;
+	s8 ret;
+
+	if (phy->type == B43_PHYTYPE_A) {
+		//TODO: Incomplete specs.
+		ret = 0;
+	} else
+		ret = b43_rssi_postprocess(dev, in_rssi, 0, 1, 1);
+
+	return ret;
+}
+#endif
+
+void b43_rx(struct b43_wldev *dev, struct sk_buff *skb, const void *_rxhdr)
+{
+	struct ieee80211_rx_status status;
+	struct b43_plcp_hdr6 *plcp;
+	struct ieee80211_hdr *wlhdr;
+	const struct b43_rxhdr_fw4 *rxhdr = _rxhdr;
+	u16 fctl;
+	u16 phystat0, phystat3, chanstat, mactime;
+	u32 macstat;
+	u16 chanid;
+	u8 jssi;
+	int padding;
+
+	memset(&status, 0, sizeof(status));
+
+	/* Get metadata about the frame from the header. */
+	phystat0 = le16_to_cpu(rxhdr->phy_status0);
+	phystat3 = le16_to_cpu(rxhdr->phy_status3);
+	jssi = rxhdr->jssi;
+	macstat = le32_to_cpu(rxhdr->mac_status);
+	mactime = le16_to_cpu(rxhdr->mac_time);
+	chanstat = le16_to_cpu(rxhdr->channel);
+
+	if (macstat & B43_RX_MAC_FCSERR)
+		dev->wl->ieee_stats.dot11FCSErrorCount++;
+	if (macstat & B43_RX_MAC_DECERR) {
+		/* Decryption with the given key failed.
+		 * Drop the packet. We also won't be able to decrypt it with
+		 * the key in software. */
+		goto drop;
+	}
+
+	/* Skip PLCP and padding */
+	padding = (macstat & B43_RX_MAC_PADDING) ? 2 : 0;
+	if (unlikely(skb->len < (sizeof(struct b43_plcp_hdr6) + padding))) {
+		b43dbg(dev->wl, "RX: Packet size underrun (1)\n");
+		goto drop;
+	}
+	plcp = (struct b43_plcp_hdr6 *)(skb->data + padding);
+	skb_pull(skb, sizeof(struct b43_plcp_hdr6) + padding);
+	/* The skb contains the Wireless Header + payload data now */
+	if (unlikely(skb->len < (2 + 2 + 6 /*minimum hdr */  + FCS_LEN))) {
+		b43dbg(dev->wl, "RX: Packet size underrun (2)\n");
+		goto drop;
+	}
+	wlhdr = (struct ieee80211_hdr *)(skb->data);
+	fctl = le16_to_cpu(wlhdr->frame_control);
+	skb_trim(skb, skb->len - FCS_LEN);
+
+	if (macstat & B43_RX_MAC_DEC) {
+		unsigned int keyidx;
+		int wlhdr_len;
+
+		keyidx = ((macstat & B43_RX_MAC_KEYIDX)
+			  >> B43_RX_MAC_KEYIDX_SHIFT);
+		/* We must adjust the key index here. We want the "physical"
+		 * key index, but the ucode passed it slightly different.
+		 */
+		keyidx = b43_kidx_to_raw(dev, keyidx);
+		B43_WARN_ON(keyidx >= dev->max_nr_keys);
+
+		if (dev->key[keyidx].algorithm != B43_SEC_ALGO_NONE) {
+			wlhdr_len = ieee80211_get_hdrlen(fctl);
+			if (unlikely(skb->len < (wlhdr_len + 3))) {
+				b43dbg(dev->wl,
+				       "RX: Packet size underrun (3)\n");
+				goto drop;
+			}
+			status.flag |= RX_FLAG_DECRYPTED;
+		}
+	}
+
+	status.ssi = b43_rssi_postprocess(dev, jssi,
+					  (phystat0 & B43_RX_PHYST0_OFDM),
+					  (phystat0 & B43_RX_PHYST0_GAINCTL),
+					  (phystat3 & B43_RX_PHYST3_TRSTATE));
+	status.noise = dev->stats.link_noise;
+	/* the next line looks wrong, but is what mac80211 wants */
+	status.signal = (jssi * 100) / B43_RX_MAX_SSI;
+	if (phystat0 & B43_RX_PHYST0_OFDM)
+		status.rate = b43_plcp_get_bitrate_ofdm(plcp);
+	else
+		status.rate = b43_plcp_get_bitrate_cck(plcp);
+	status.antenna = !!(phystat0 & B43_RX_PHYST0_ANT);
+	status.mactime = mactime;
+
+	chanid = (chanstat & B43_RX_CHAN_ID) >> B43_RX_CHAN_ID_SHIFT;
+	switch (chanstat & B43_RX_CHAN_PHYTYPE) {
+	case B43_PHYTYPE_A:
+		status.phymode = MODE_IEEE80211A;
+		status.freq = chanid;
+		status.channel = b43_freq_to_channel_a(chanid);
+		break;
+	case B43_PHYTYPE_B:
+		status.phymode = MODE_IEEE80211B;
+		status.freq = chanid + 2400;
+		status.channel = b43_freq_to_channel_bg(chanid + 2400);
+		break;
+	case B43_PHYTYPE_G:
+		status.phymode = MODE_IEEE80211G;
+		status.freq = chanid + 2400;
+		status.channel = b43_freq_to_channel_bg(chanid + 2400);
+		break;
+	default:
+		B43_WARN_ON(1);
+	}
+
+	dev->stats.last_rx = jiffies;
+	ieee80211_rx_irqsafe(dev->wl->hw, skb, &status);
+
+	return;
+drop:
+	b43dbg(dev->wl, "RX: Packet dropped\n");
+	dev_kfree_skb_any(skb);
+}
+
+void b43_handle_txstatus(struct b43_wldev *dev,
+			 const struct b43_txstatus *status)
+{
+	b43_debugfs_log_txstat(dev, status);
+
+	if (status->intermediate)
+		return;
+	if (status->for_ampdu)
+		return;
+	if (!status->acked)
+		dev->wl->ieee_stats.dot11ACKFailureCount++;
+	if (status->rts_count) {
+		if (status->rts_count == 0xF)	//FIXME
+			dev->wl->ieee_stats.dot11RTSFailureCount++;
+		else
+			dev->wl->ieee_stats.dot11RTSSuccessCount++;
+	}
+
+	if (b43_using_pio(dev))
+		b43_pio_handle_txstatus(dev, status);
+	else
+		b43_dma_handle_txstatus(dev, status);
+}
+
+/* Handle TX status report as received through DMA/PIO queues */
+void b43_handle_hwtxstatus(struct b43_wldev *dev,
+			   const struct b43_hwtxstatus *hw)
+{
+	struct b43_txstatus status;
+	u8 tmp;
+
+	status.cookie = le16_to_cpu(hw->cookie);
+	status.seq = le16_to_cpu(hw->seq);
+	status.phy_stat = hw->phy_stat;
+	tmp = hw->count;
+	status.frame_count = (tmp >> 4);
+	status.rts_count = (tmp & 0x0F);
+	tmp = hw->flags;
+	status.supp_reason = ((tmp & 0x1C) >> 2);
+	status.pm_indicated = !!(tmp & 0x80);
+	status.intermediate = !!(tmp & 0x40);
+	status.for_ampdu = !!(tmp & 0x20);
+	status.acked = !!(tmp & 0x02);
+
+	b43_handle_txstatus(dev, &status);
+}
+
+/* Stop any TX operation on the device (suspend the hardware queues) */
+void b43_tx_suspend(struct b43_wldev *dev)
+{
+	if (b43_using_pio(dev))
+		b43_pio_freeze_txqueues(dev);
+	else
+		b43_dma_tx_suspend(dev);
+}
+
+/* Resume any TX operation on the device (resume the hardware queues) */
+void b43_tx_resume(struct b43_wldev *dev)
+{
+	if (b43_using_pio(dev))
+		b43_pio_thaw_txqueues(dev);
+	else
+		b43_dma_tx_resume(dev);
+}
+
+#if 0
+static void upload_qos_parms(struct b43_wldev *dev,
+			     const u16 * parms, u16 offset)
+{
+	int i;
+
+	for (i = 0; i < B43_NR_QOSPARMS; i++) {
+		b43_shm_write16(dev, B43_SHM_SHARED,
+				offset + (i * 2), parms[i]);
+	}
+}
+#endif
+
+/* Initialize the QoS parameters */
+void b43_qos_init(struct b43_wldev *dev)
+{
+	/* FIXME: This function must probably be called from the mac80211
+	 * config callback. */
+	return;
+
+	b43_hf_write(dev, b43_hf_read(dev) | B43_HF_EDCF);
+	//FIXME kill magic
+	b43_write16(dev, 0x688, b43_read16(dev, 0x688) | 0x4);
+
+	/*TODO: We might need some stack support here to get the values. */
+}
diff --git a/drivers/net/wireless/b43/xmit.h b/drivers/net/wireless/b43/xmit.h
new file mode 100644
index 000000000000..03bddd251618
--- /dev/null
+++ b/drivers/net/wireless/b43/xmit.h
@@ -0,0 +1,250 @@
+#ifndef B43_XMIT_H_
+#define B43_XMIT_H_
+
+#include "main.h"
+
+#define _b43_declare_plcp_hdr(size) \
+	struct b43_plcp_hdr##size {		\
+		union {				\
+			__le32 data;		\
+			__u8 raw[size];		\
+		} __attribute__((__packed__));	\
+	} __attribute__((__packed__))
+
+/* struct b43_plcp_hdr4 */
+_b43_declare_plcp_hdr(4);
+/* struct b43_plcp_hdr6 */
+_b43_declare_plcp_hdr(6);
+
+#undef _b43_declare_plcp_hdr
+
+/* TX header for v4 firmware */
+struct b43_txhdr_fw4 {
+	__le32 mac_ctl;		/* MAC TX control */
+	__le16 mac_frame_ctl;	/* Copy of the FrameControl field */
+	__le16 tx_fes_time_norm;	/* TX FES Time Normal */
+	__le16 phy_ctl;		/* PHY TX control */
+	__le16 phy_ctl_0;	/* Unused */
+	__le16 phy_ctl_1;	/* Unused */
+	__le16 phy_ctl_rts_0;	/* Unused */
+	__le16 phy_ctl_rts_1;	/* Unused */
+	__u8 phy_rate;		/* PHY rate */
+	__u8 phy_rate_rts;	/* PHY rate for RTS/CTS */
+	__u8 extra_ft;		/* Extra Frame Types */
+	__u8 chan_radio_code;	/* Channel Radio Code */
+	__u8 iv[16];		/* Encryption IV */
+	__u8 tx_receiver[6];	/* TX Frame Receiver address */
+	__le16 tx_fes_time_fb;	/* TX FES Time Fallback */
+	struct b43_plcp_hdr6 rts_plcp_fb;	/* RTS fallback PLCP */
+	__le16 rts_dur_fb;	/* RTS fallback duration */
+	struct b43_plcp_hdr6 plcp_fb;	/* Fallback PLCP */
+	__le16 dur_fb;		/* Fallback duration */
+	__le16 mm_dur_time;	/* Unused */
+	__le16 mm_dur_time_fb;	/* Unused */
+	__le32 time_stamp;	/* Timestamp */
+	 PAD_BYTES(2);
+	__le16 cookie;		/* TX frame cookie */
+	__le16 tx_status;	/* TX status */
+	struct b43_plcp_hdr6 rts_plcp;	/* RTS PLCP */
+	__u8 rts_frame[16];	/* The RTS frame (if used) */
+	 PAD_BYTES(2);
+	struct b43_plcp_hdr6 plcp;	/* Main PLCP */
+} __attribute__ ((__packed__));
+
+/* MAC TX control */
+#define B43_TX4_MAC_KEYIDX		0x0FF00000	/* Security key index */
+#define B43_TX4_MAC_KEYIDX_SHIFT	20
+#define B43_TX4_MAC_KEYALG		0x00070000	/* Security key algorithm */
+#define B43_TX4_MAC_KEYALG_SHIFT	16
+#define B43_TX4_MAC_LIFETIME	0x00001000
+#define B43_TX4_MAC_FRAMEBURST	0x00000800
+#define B43_TX4_MAC_SENDCTS		0x00000400
+#define B43_TX4_MAC_AMPDU		0x00000300
+#define B43_TX4_MAC_AMPDU_SHIFT	8
+#define B43_TX4_MAC_5GHZ		0x00000080
+#define B43_TX4_MAC_IGNPMQ		0x00000020
+#define B43_TX4_MAC_HWSEQ		0x00000010	/* Use Hardware Sequence Number */
+#define B43_TX4_MAC_STMSDU		0x00000008	/* Start MSDU */
+#define B43_TX4_MAC_SENDRTS		0x00000004
+#define B43_TX4_MAC_LONGFRAME	0x00000002
+#define B43_TX4_MAC_ACK		0x00000001
+
+/* Extra Frame Types */
+#define B43_TX4_EFT_FBOFDM		0x0001	/* Data frame fallback rate type */
+#define B43_TX4_EFT_RTSOFDM		0x0004	/* RTS/CTS rate type */
+#define B43_TX4_EFT_RTSFBOFDM	0x0010	/* RTS/CTS fallback rate type */
+
+/* PHY TX control word */
+#define B43_TX4_PHY_OFDM		0x0001	/* Data frame rate type */
+#define B43_TX4_PHY_SHORTPRMBL	0x0010	/* Use short preamble */
+#define B43_TX4_PHY_ANT		0x03C0	/* Antenna selection */
+#define  B43_TX4_PHY_ANT0		0x0000	/* Use antenna 0 */
+#define  B43_TX4_PHY_ANT1		0x0100	/* Use antenna 1 */
+#define  B43_TX4_PHY_ANTLAST	0x0300	/* Use last used antenna */
+
+void b43_generate_txhdr(struct b43_wldev *dev,
+			u8 * txhdr,
+			const unsigned char *fragment_data,
+			unsigned int fragment_len,
+			const struct ieee80211_tx_control *txctl, u16 cookie);
+
+/* Transmit Status */
+struct b43_txstatus {
+	u16 cookie;		/* The cookie from the txhdr */
+	u16 seq;		/* Sequence number */
+	u8 phy_stat;		/* PHY TX status */
+	u8 frame_count;		/* Frame transmit count */
+	u8 rts_count;		/* RTS transmit count */
+	u8 supp_reason;		/* Suppression reason */
+	/* flags */
+	u8 pm_indicated;	/* PM mode indicated to AP */
+	u8 intermediate;	/* Intermediate status notification (not final) */
+	u8 for_ampdu;		/* Status is for an AMPDU (afterburner) */
+	u8 acked;		/* Wireless ACK received */
+};
+
+/* txstatus supp_reason values */
+enum {
+	B43_TXST_SUPP_NONE,	/* Not suppressed */
+	B43_TXST_SUPP_PMQ,	/* Suppressed due to PMQ entry */
+	B43_TXST_SUPP_FLUSH,	/* Suppressed due to flush request */
+	B43_TXST_SUPP_PREV,	/* Previous fragment failed */
+	B43_TXST_SUPP_CHAN,	/* Channel mismatch */
+	B43_TXST_SUPP_LIFE,	/* Lifetime expired */
+	B43_TXST_SUPP_UNDER,	/* Buffer underflow */
+	B43_TXST_SUPP_ABNACK,	/* Afterburner NACK */
+};
+
+/* Transmit Status as received through DMA/PIO on old chips */
+struct b43_hwtxstatus {
+	PAD_BYTES(4);
+	__le16 cookie;
+	u8 flags;
+	u8 count;
+	 PAD_BYTES(2);
+	__le16 seq;
+	u8 phy_stat;
+	 PAD_BYTES(1);
+} __attribute__ ((__packed__));
+
+/* Receive header for v4 firmware. */
+struct b43_rxhdr_fw4 {
+	__le16 frame_len;	/* Frame length */
+	 PAD_BYTES(2);
+	__le16 phy_status0;	/* PHY RX Status 0 */
+	__u8 jssi;		/* PHY RX Status 1: JSSI */
+	__u8 sig_qual;		/* PHY RX Status 1: Signal Quality */
+	__le16 phy_status2;	/* PHY RX Status 2 */
+	__le16 phy_status3;	/* PHY RX Status 3 */
+	__le32 mac_status;	/* MAC RX status */
+	__le16 mac_time;
+	__le16 channel;
+} __attribute__ ((__packed__));
+
+/* PHY RX Status 0 */
+#define B43_RX_PHYST0_GAINCTL	0x4000	/* Gain Control */
+#define B43_RX_PHYST0_PLCPHCF	0x0200
+#define B43_RX_PHYST0_PLCPFV	0x0100
+#define B43_RX_PHYST0_SHORTPRMBL	0x0080	/* Received with Short Preamble */
+#define B43_RX_PHYST0_LCRS		0x0040
+#define B43_RX_PHYST0_ANT		0x0020	/* Antenna */
+#define B43_RX_PHYST0_UNSRATE	0x0010
+#define B43_RX_PHYST0_CLIP		0x000C
+#define B43_RX_PHYST0_CLIP_SHIFT	2
+#define B43_RX_PHYST0_FTYPE		0x0003	/* Frame type */
+#define  B43_RX_PHYST0_CCK		0x0000	/* Frame type: CCK */
+#define  B43_RX_PHYST0_OFDM		0x0001	/* Frame type: OFDM */
+#define  B43_RX_PHYST0_PRE_N	0x0002	/* Pre-standard N-PHY frame */
+#define  B43_RX_PHYST0_STD_N	0x0003	/* Standard N-PHY frame */
+
+/* PHY RX Status 2 */
+#define B43_RX_PHYST2_LNAG		0xC000	/* LNA Gain */
+#define B43_RX_PHYST2_LNAG_SHIFT	14
+#define B43_RX_PHYST2_PNAG		0x3C00	/* PNA Gain */
+#define B43_RX_PHYST2_PNAG_SHIFT	10
+#define B43_RX_PHYST2_FOFF		0x03FF	/* F offset */
+
+/* PHY RX Status 3 */
+#define B43_RX_PHYST3_DIGG		0x1800	/* DIG Gain */
+#define B43_RX_PHYST3_DIGG_SHIFT	11
+#define B43_RX_PHYST3_TRSTATE	0x0400	/* TR state */
+
+/* MAC RX Status */
+#define B43_RX_MAC_BEACONSENT	0x00008000	/* Beacon send flag */
+#define B43_RX_MAC_KEYIDX		0x000007E0	/* Key index */
+#define B43_RX_MAC_KEYIDX_SHIFT	5
+#define B43_RX_MAC_DECERR		0x00000010	/* Decrypt error */
+#define B43_RX_MAC_DEC		0x00000008	/* Decryption attempted */
+#define B43_RX_MAC_PADDING		0x00000004	/* Pad bytes present */
+#define B43_RX_MAC_RESP		0x00000002	/* Response frame transmitted */
+#define B43_RX_MAC_FCSERR		0x00000001	/* FCS error */
+
+/* RX channel */
+#define B43_RX_CHAN_GAIN		0xFC00	/* Gain */
+#define B43_RX_CHAN_GAIN_SHIFT	10
+#define B43_RX_CHAN_ID		0x03FC	/* Channel ID */
+#define B43_RX_CHAN_ID_SHIFT	2
+#define B43_RX_CHAN_PHYTYPE		0x0003	/* PHY type */
+
+u8 b43_plcp_get_ratecode_cck(const u8 bitrate);
+u8 b43_plcp_get_ratecode_ofdm(const u8 bitrate);
+
+void b43_generate_plcp_hdr(struct b43_plcp_hdr4 *plcp,
+			   const u16 octets, const u8 bitrate);
+
+void b43_rx(struct b43_wldev *dev, struct sk_buff *skb, const void *_rxhdr);
+
+void b43_handle_txstatus(struct b43_wldev *dev,
+			 const struct b43_txstatus *status);
+
+void b43_handle_hwtxstatus(struct b43_wldev *dev,
+			   const struct b43_hwtxstatus *hw);
+
+void b43_tx_suspend(struct b43_wldev *dev);
+void b43_tx_resume(struct b43_wldev *dev);
+
+#define B43_NR_QOSPARMS		22
+enum {
+	B43_QOSPARM_TXOP = 0,
+	B43_QOSPARM_CWMIN,
+	B43_QOSPARM_CWMAX,
+	B43_QOSPARM_CWCUR,
+	B43_QOSPARM_AIFS,
+	B43_QOSPARM_BSLOTS,
+	B43_QOSPARM_REGGAP,
+	B43_QOSPARM_STATUS,
+};
+void b43_qos_init(struct b43_wldev *dev);
+
+/* Helper functions for converting the key-table index from "firmware-format"
+ * to "raw-format" and back. The firmware API changed for this at some revision.
+ * We need to account for that here. */
+static inline int b43_new_kidx_api(struct b43_wldev *dev)
+{
+	/* FIXME: Not sure the change was at rev 351 */
+	return (dev->fw.rev >= 351);
+}
+static inline u8 b43_kidx_to_fw(struct b43_wldev *dev, u8 raw_kidx)
+{
+	u8 firmware_kidx;
+	if (b43_new_kidx_api(dev)) {
+		firmware_kidx = raw_kidx;
+	} else {
+		if (raw_kidx >= 4)	/* Is per STA key? */
+			firmware_kidx = raw_kidx - 4;
+		else
+			firmware_kidx = raw_kidx;	/* TX default key */
+	}
+	return firmware_kidx;
+}
+static inline u8 b43_kidx_to_raw(struct b43_wldev *dev, u8 firmware_kidx)
+{
+	u8 raw_kidx;
+	if (b43_new_kidx_api(dev))
+		raw_kidx = firmware_kidx;
+	else
+		raw_kidx = firmware_kidx + 4;	/* RX default keys or per STA keys */
+	return raw_kidx;
+}
+
+#endif /* B43_XMIT_H_ */
diff --git a/drivers/net/wireless/b43legacy/Kconfig b/drivers/net/wireless/b43legacy/Kconfig
new file mode 100644
index 000000000000..7e23ec23fc98
--- /dev/null
+++ b/drivers/net/wireless/b43legacy/Kconfig
@@ -0,0 +1,89 @@
+config B43LEGACY
+	tristate "Broadcom 43xx-legacy wireless support (mac80211 stack)"
+	depends on SSB_POSSIBLE && MAC80211 && WLAN_80211
+	select SSB
+	select FW_LOADER
+	select HW_RANDOM
+	---help---
+	  b43legacy is a driver for 802.11b devices from Broadcom (BCM4301 and
+	  BCM4303) and early model 802.11g chips (BCM4306 Ver. 2) used in the
+	  Linksys WPC54G V1 PCMCIA devices.
+
+	  Newer 802.11g and 802.11a devices need b43.
+
+	  It is safe to include both b43 and b43legacy as the underlying glue
+	  layer will automatically load the correct version for your device.
+
+	  This driver uses V3 firmware, which must be installed separately using
+	  b43-fwcutter.
+
+	  This driver can be built as a module (recommended) that will be
+	  called "b43legacy". If unsure, say M.
+
+# Auto-select SSB PCI-HOST support, if possible
+config B43LEGACY_PCI_AUTOSELECT
+	bool
+	depends on B43LEGACY && SSB_PCIHOST_POSSIBLE
+	select SSB_PCIHOST
+	default y
+
+# Auto-select SSB PCICORE driver, if possible
+config B43LEGACY_PCICORE_AUTOSELECT
+	bool
+	depends on B43LEGACY && SSB_DRIVER_PCICORE_POSSIBLE
+	select SSB_DRIVER_PCICORE
+	default y
+
+config B43LEGACY_DEBUG
+	bool "Broadcom 43xx-legacy debugging"
+	depends on B43LEGACY
+	default y
+	---help---
+	  Say Y, because this information will help you get the driver running.
+	  This option generates a minimum of log output.
+
+config B43LEGACY_DMA
+	bool
+	depends on B43LEGACY
+
+config B43LEGACY_PIO
+	bool
+	depends on B43LEGACY
+
+choice
+	prompt "Broadcom 43xx-legacy data transfer mode"
+	depends on B43LEGACY
+	default B43LEGACY_DMA_AND_PIO_MODE
+
+config B43LEGACY_DMA_AND_PIO_MODE
+	bool "DMA + PIO"
+	select B43LEGACY_DMA
+	select B43LEGACY_PIO
+	---help---
+	  Include both, Direct Memory Access (DMA) and Programmed I/O (PIO)
+	  data transfer modes. The mode actually used is selectable through
+	  the module parameter "pio". With pio=0 as a module parameter, the
+	  default DMA is used, otherwise PIO is used.
+
+	  If unsure, choose this option.
+
+config B43LEGACY_DMA_MODE
+	bool "DMA (Direct Memory Access) only"
+	select B43LEGACY_DMA
+	---help---
+	  Only include Direct Memory Access (DMA).
+	  This reduces the size of the driver module, by omitting the PIO code.
+
+config B43LEGACY_PIO_MODE
+	bool "PIO (Programmed I/O) only"
+	select B43LEGACY_PIO
+	---help---
+	  Only include Programmed I/O (PIO).
+	  This reduces the size of the driver module, by omitting the DMA code.
+	  Please note that PIO transfers are slow (compared to DMA).
+
+	  Also note that not all devices of the b43legacy series support PIO.
+
+	  You should use PIO only if DMA does not work for you.
+
+endchoice
diff --git a/drivers/net/wireless/b43legacy/Makefile b/drivers/net/wireless/b43legacy/Makefile
new file mode 100644
index 000000000000..ec3a2482bbad
--- /dev/null
+++ b/drivers/net/wireless/b43legacy/Makefile
@@ -0,0 +1,14 @@
+obj-$(CONFIG_B43LEGACY) += b43legacy.o
+b43legacy-obj-$(CONFIG_B43LEGACY_DEBUG) += debugfs.o
+
+b43legacy-obj-$(CONFIG_B43LEGACY_DMA) += dma.o
+b43legacy-obj-$(CONFIG_B43LEGACY_PIO) += pio.o
+
+b43legacy-objs := main.o \
+		ilt.o \
+		leds.o \
+		phy.o \
+		radio.o \
+		sysfs.o \
+		xmit.o \
+		$(b43legacy-obj-y)
diff --git a/drivers/net/wireless/b43legacy/b43legacy.h b/drivers/net/wireless/b43legacy/b43legacy.h
new file mode 100644
index 000000000000..afe145cec067
--- /dev/null
+++ b/drivers/net/wireless/b43legacy/b43legacy.h
@@ -0,0 +1,832 @@
+#ifndef B43legacy_H_
+#define B43legacy_H_
+
+#include <linux/hw_random.h>
+#include <linux/kernel.h>
+#include <linux/spinlock.h>
+#include <linux/interrupt.h>
+#include <linux/stringify.h>
+#include <linux/netdevice.h>
+#include <linux/pci.h>
+#include <asm/atomic.h>
+#include <linux/io.h>
+
+#include <linux/ssb/ssb.h>
+#include <linux/ssb/ssb_driver_chipcommon.h>
+
+#include <linux/wireless.h>
+#include <net/mac80211.h>
+
+#include "debugfs.h"
+#include "leds.h"
+#include "phy.h"
+
+
+#define B43legacy_IRQWAIT_MAX_RETRIES	100
+
+#define B43legacy_RX_MAX_SSI		60 /* best guess at max ssi */
+
+/* MMIO offsets */
+#define B43legacy_MMIO_DMA0_REASON	0x20
+#define B43legacy_MMIO_DMA0_IRQ_MASK	0x24
+#define B43legacy_MMIO_DMA1_REASON	0x28
+#define B43legacy_MMIO_DMA1_IRQ_MASK	0x2C
+#define B43legacy_MMIO_DMA2_REASON	0x30
+#define B43legacy_MMIO_DMA2_IRQ_MASK	0x34
+#define B43legacy_MMIO_DMA3_REASON	0x38
+#define B43legacy_MMIO_DMA3_IRQ_MASK	0x3C
+#define B43legacy_MMIO_DMA4_REASON	0x40
+#define B43legacy_MMIO_DMA4_IRQ_MASK	0x44
+#define B43legacy_MMIO_DMA5_REASON	0x48
+#define B43legacy_MMIO_DMA5_IRQ_MASK	0x4C
+#define B43legacy_MMIO_MACCTL		0x120
+#define B43legacy_MMIO_STATUS_BITFIELD	0x120
+#define B43legacy_MMIO_STATUS2_BITFIELD	0x124
+#define B43legacy_MMIO_GEN_IRQ_REASON	0x128
+#define B43legacy_MMIO_GEN_IRQ_MASK	0x12C
+#define B43legacy_MMIO_RAM_CONTROL	0x130
+#define B43legacy_MMIO_RAM_DATA		0x134
+#define B43legacy_MMIO_PS_STATUS		0x140
+#define B43legacy_MMIO_RADIO_HWENABLED_HI	0x158
+#define B43legacy_MMIO_SHM_CONTROL	0x160
+#define B43legacy_MMIO_SHM_DATA		0x164
+#define B43legacy_MMIO_SHM_DATA_UNALIGNED	0x166
+#define B43legacy_MMIO_XMITSTAT_0		0x170
+#define B43legacy_MMIO_XMITSTAT_1		0x174
+#define B43legacy_MMIO_REV3PLUS_TSF_LOW	0x180 /* core rev >= 3 only */
+#define B43legacy_MMIO_REV3PLUS_TSF_HIGH	0x184 /* core rev >= 3 only */
+
+/* 32-bit DMA */
+#define B43legacy_MMIO_DMA32_BASE0	0x200
+#define B43legacy_MMIO_DMA32_BASE1	0x220
+#define B43legacy_MMIO_DMA32_BASE2	0x240
+#define B43legacy_MMIO_DMA32_BASE3	0x260
+#define B43legacy_MMIO_DMA32_BASE4	0x280
+#define B43legacy_MMIO_DMA32_BASE5	0x2A0
+/* 64-bit DMA */
+#define B43legacy_MMIO_DMA64_BASE0	0x200
+#define B43legacy_MMIO_DMA64_BASE1	0x240
+#define B43legacy_MMIO_DMA64_BASE2	0x280
+#define B43legacy_MMIO_DMA64_BASE3	0x2C0
+#define B43legacy_MMIO_DMA64_BASE4	0x300
+#define B43legacy_MMIO_DMA64_BASE5	0x340
+/* PIO */
+#define B43legacy_MMIO_PIO1_BASE		0x300
+#define B43legacy_MMIO_PIO2_BASE		0x310
+#define B43legacy_MMIO_PIO3_BASE		0x320
+#define B43legacy_MMIO_PIO4_BASE		0x330
+
+#define B43legacy_MMIO_PHY_VER		0x3E0
+#define B43legacy_MMIO_PHY_RADIO		0x3E2
+#define B43legacy_MMIO_PHY0		0x3E6
+#define B43legacy_MMIO_ANTENNA		0x3E8
+#define B43legacy_MMIO_CHANNEL		0x3F0
+#define B43legacy_MMIO_CHANNEL_EXT	0x3F4
+#define B43legacy_MMIO_RADIO_CONTROL	0x3F6
+#define B43legacy_MMIO_RADIO_DATA_HIGH	0x3F8
+#define B43legacy_MMIO_RADIO_DATA_LOW	0x3FA
+#define B43legacy_MMIO_PHY_CONTROL	0x3FC
+#define B43legacy_MMIO_PHY_DATA		0x3FE
+#define B43legacy_MMIO_MACFILTER_CONTROL	0x420
+#define B43legacy_MMIO_MACFILTER_DATA	0x422
+#define B43legacy_MMIO_RCMTA_COUNT	0x43C /* Receive Match Transmitter Addr */
+#define B43legacy_MMIO_RADIO_HWENABLED_LO	0x49A
+#define B43legacy_MMIO_GPIO_CONTROL	0x49C
+#define B43legacy_MMIO_GPIO_MASK		0x49E
+#define B43legacy_MMIO_TSF_0		0x632 /* core rev < 3 only */
+#define B43legacy_MMIO_TSF_1		0x634 /* core rev < 3 only */
+#define B43legacy_MMIO_TSF_2		0x636 /* core rev < 3 only */
+#define B43legacy_MMIO_TSF_3		0x638 /* core rev < 3 only */
+#define B43legacy_MMIO_RNG		0x65A
+#define B43legacy_MMIO_POWERUP_DELAY	0x6A8
+
+/* SPROM boardflags_lo values */
+#define B43legacy_BFL_PACTRL		0x0002
+#define B43legacy_BFL_RSSI		0x0008
+#define B43legacy_BFL_EXTLNA		0x1000
+
+/* GPIO register offset, in both ChipCommon and PCI core. */
+#define B43legacy_GPIO_CONTROL		0x6c
+
+/* SHM Routing */
+#define	B43legacy_SHM_SHARED		0x0001
+#define	B43legacy_SHM_WIRELESS		0x0002
+#define	B43legacy_SHM_HW		0x0004
+#define	B43legacy_SHM_UCODE		0x0300
+
+/* SHM Routing modifiers */
+#define B43legacy_SHM_AUTOINC_R		0x0200 /* Read Auto-increment */
+#define B43legacy_SHM_AUTOINC_W		0x0100 /* Write Auto-increment */
+#define B43legacy_SHM_AUTOINC_RW	(B43legacy_SHM_AUTOINC_R | \
+					 B43legacy_SHM_AUTOINC_W)
+
+/* Misc SHM_SHARED offsets */
+#define B43legacy_SHM_SH_WLCOREREV	0x0016 /* 802.11 core revision */
+#define B43legacy_SHM_SH_HOSTFLO	0x005E /* Hostflags ucode opts (low) */
+#define B43legacy_SHM_SH_HOSTFHI	0x0060 /* Hostflags ucode opts (high) */
+/* SHM_SHARED crypto engine */
+#define B43legacy_SHM_SH_KEYIDXBLOCK	0x05D4 /* Key index/algorithm block */
+/* SHM_SHARED beacon variables */
+#define B43legacy_SHM_SH_BEACPHYCTL	0x0054 /* Beacon PHY TX control word */
+/* SHM_SHARED ACK/CTS control */
+#define B43legacy_SHM_SH_ACKCTSPHYCTL	0x0022 /* ACK/CTS PHY control word */
+/* SHM_SHARED probe response variables */
+#define B43legacy_SHM_SH_PRPHYCTL	0x0188 /* Probe Resp PHY TX control */
+#define B43legacy_SHM_SH_PRMAXTIME	0x0074 /* Probe Response max time */
+/* SHM_SHARED rate tables */
+/* SHM_SHARED microcode soft registers */
+#define B43legacy_SHM_SH_UCODEREV	0x0000 /* Microcode revision */
+#define B43legacy_SHM_SH_UCODEPATCH	0x0002 /* Microcode patchlevel */
+#define B43legacy_SHM_SH_UCODEDATE	0x0004 /* Microcode date */
+#define B43legacy_SHM_SH_UCODETIME	0x0006 /* Microcode time */
+
+#define B43legacy_UCODEFLAGS_OFFSET     0x005E
+
+/* Hardware Radio Enable masks */
+#define B43legacy_MMIO_RADIO_HWENABLED_HI_MASK (1 << 16)
+#define B43legacy_MMIO_RADIO_HWENABLED_LO_MASK (1 << 4)
+
+/* HostFlags. See b43legacy_hf_read/write() */
+#define B43legacy_HF_SYMW		0x00000002 /* G-PHY SYM workaround */
+#define B43legacy_HF_GDCW		0x00000020 /* G-PHY DV cancel filter */
+#define B43legacy_HF_OFDMPABOOST	0x00000040 /* Enable PA boost OFDM */
+#define B43legacy_HF_EDCF		0x00000100 /* on if WME/MAC suspended */
+
+/* MacFilter offsets. */
+#define B43legacy_MACFILTER_SELF	0x0000
+#define B43legacy_MACFILTER_BSSID	0x0003
+#define B43legacy_MACFILTER_MAC		0x0010
+
+/* PHYVersioning */
+#define B43legacy_PHYTYPE_B		0x01
+#define B43legacy_PHYTYPE_G		0x02
+
+/* PHYRegisters */
+#define B43legacy_PHY_G_LO_CONTROL	0x0810
+#define B43legacy_PHY_ILT_G_CTRL	0x0472
+#define B43legacy_PHY_ILT_G_DATA1	0x0473
+#define B43legacy_PHY_ILT_G_DATA2	0x0474
+#define B43legacy_PHY_G_PCTL		0x0029
+#define B43legacy_PHY_RADIO_BITFIELD	0x0401
+#define B43legacy_PHY_G_CRS		0x0429
+#define B43legacy_PHY_NRSSILT_CTRL	0x0803
+#define B43legacy_PHY_NRSSILT_DATA	0x0804
+
+/* RadioRegisters */
+#define B43legacy_RADIOCTL_ID		0x01
+
+/* MAC Control bitfield */
+#define B43legacy_MACCTL_IHR_ENABLED	0x00000400 /* IHR Region Enabled */
+#define B43legacy_MACCTL_INFRA		0x00020000 /* Infrastructure mode */
+#define B43legacy_MACCTL_AP		0x00040000 /* AccessPoint mode */
+#define B43legacy_MACCTL_BEACPROMISC	0x00100000 /* Beacon Promiscuous */
+#define B43legacy_MACCTL_KEEP_BADPLCP	0x00200000 /* Keep bad PLCP frames */
+#define B43legacy_MACCTL_KEEP_CTL	0x00400000 /* Keep control frames */
+#define B43legacy_MACCTL_KEEP_BAD	0x00800000 /* Keep bad frames (FCS) */
+#define B43legacy_MACCTL_PROMISC	0x01000000 /* Promiscuous mode */
+#define B43legacy_MACCTL_GMODE		0x80000000 /* G Mode */
+
+/* StatusBitField */
+#define B43legacy_SBF_MAC_ENABLED	0x00000001
+#define B43legacy_SBF_CORE_READY	0x00000004
+#define B43legacy_SBF_400		0x00000400 /*FIXME: fix name*/
+#define B43legacy_SBF_XFER_REG_BYTESWAP	0x00010000
+#define B43legacy_SBF_MODE_NOTADHOC	0x00020000
+#define B43legacy_SBF_MODE_AP		0x00040000
+#define B43legacy_SBF_RADIOREG_LOCK	0x00080000
+#define B43legacy_SBF_MODE_MONITOR	0x00400000
+#define B43legacy_SBF_MODE_PROMISC	0x01000000
+#define B43legacy_SBF_PS1		0x02000000
+#define B43legacy_SBF_PS2		0x04000000
+#define B43legacy_SBF_NO_SSID_BCAST	0x08000000
+#define B43legacy_SBF_TIME_UPDATE	0x10000000
+
+/* 802.11 core specific TM State Low flags */
+#define B43legacy_TMSLOW_GMODE		0x20000000 /* G Mode Enable */
+#define B43legacy_TMSLOW_PLLREFSEL	0x00200000 /* PLL Freq Ref Select */
+#define B43legacy_TMSLOW_MACPHYCLKEN	0x00100000 /* MAC PHY Clock Ctrl Enbl */
+#define B43legacy_TMSLOW_PHYRESET	0x00080000 /* PHY Reset */
+#define B43legacy_TMSLOW_PHYCLKEN	0x00040000 /* PHY Clock Enable */
+
+/* 802.11 core specific TM State High flags */
+#define B43legacy_TMSHIGH_FCLOCK	0x00040000 /* Fast Clock Available */
+#define B43legacy_TMSHIGH_GPHY		0x00010000 /* G-PHY avail (rev >= 5) */
+
+#define B43legacy_UCODEFLAG_AUTODIV       0x0001
+
+/* Generic-Interrupt reasons. */
+#define B43legacy_IRQ_MAC_SUSPENDED	0x00000001
+#define B43legacy_IRQ_BEACON		0x00000002
+#define B43legacy_IRQ_TBTT_INDI		0x00000004 /* Target Beacon Transmit Time */
+#define B43legacy_IRQ_BEACON_TX_OK	0x00000008
+#define B43legacy_IRQ_BEACON_CANCEL	0x00000010
+#define B43legacy_IRQ_ATIM_END		0x00000020
+#define B43legacy_IRQ_PMQ		0x00000040
+#define B43legacy_IRQ_PIO_WORKAROUND	0x00000100
+#define B43legacy_IRQ_MAC_TXERR		0x00000200
+#define B43legacy_IRQ_PHY_TXERR		0x00000800
+#define B43legacy_IRQ_PMEVENT		0x00001000
+#define B43legacy_IRQ_TIMER0		0x00002000
+#define B43legacy_IRQ_TIMER1		0x00004000
+#define B43legacy_IRQ_DMA		0x00008000
+#define B43legacy_IRQ_TXFIFO_FLUSH_OK	0x00010000
+#define B43legacy_IRQ_CCA_MEASURE_OK	0x00020000
+#define B43legacy_IRQ_NOISESAMPLE_OK	0x00040000
+#define B43legacy_IRQ_UCODE_DEBUG	0x08000000
+#define B43legacy_IRQ_RFKILL		0x10000000
+#define B43legacy_IRQ_TX_OK		0x20000000
+#define B43legacy_IRQ_PHY_G_CHANGED	0x40000000
+#define B43legacy_IRQ_TIMEOUT		0x80000000
+
+#define B43legacy_IRQ_ALL		0xFFFFFFFF
+#define B43legacy_IRQ_MASKTEMPLATE	(B43legacy_IRQ_MAC_SUSPENDED |	\
+					 B43legacy_IRQ_BEACON |		\
+					 B43legacy_IRQ_TBTT_INDI |	\
+					 B43legacy_IRQ_ATIM_END |	\
+					 B43legacy_IRQ_PMQ |		\
+					 B43legacy_IRQ_MAC_TXERR |	\
+					 B43legacy_IRQ_PHY_TXERR |	\
+					 B43legacy_IRQ_DMA |		\
+					 B43legacy_IRQ_TXFIFO_FLUSH_OK | \
+					 B43legacy_IRQ_NOISESAMPLE_OK | \
+					 B43legacy_IRQ_UCODE_DEBUG |	\
+					 B43legacy_IRQ_RFKILL |		\
+					 B43legacy_IRQ_TX_OK)
+
+/* Device specific rate values.
+ * The actual values defined here are (rate_in_mbps * 2).
+ * Some code depends on this. Don't change it. */
+#define B43legacy_CCK_RATE_1MB		2
+#define B43legacy_CCK_RATE_2MB		4
+#define B43legacy_CCK_RATE_5MB		11
+#define B43legacy_CCK_RATE_11MB		22
+#define B43legacy_OFDM_RATE_6MB		12
+#define B43legacy_OFDM_RATE_9MB		18
+#define B43legacy_OFDM_RATE_12MB	24
+#define B43legacy_OFDM_RATE_18MB	36
+#define B43legacy_OFDM_RATE_24MB	48
+#define B43legacy_OFDM_RATE_36MB	72
+#define B43legacy_OFDM_RATE_48MB	96
+#define B43legacy_OFDM_RATE_54MB	108
+/* Convert a b43legacy rate value to a rate in 100kbps */
+#define B43legacy_RATE_TO_100KBPS(rate)	(((rate) * 10) / 2)
+
+
+#define B43legacy_DEFAULT_SHORT_RETRY_LIMIT	7
+#define B43legacy_DEFAULT_LONG_RETRY_LIMIT	4
+
+/* Max size of a security key */
+#define B43legacy_SEC_KEYSIZE		16
+/* Security algorithms. */
+enum {
+	B43legacy_SEC_ALGO_NONE = 0, /* unencrypted, as of TX header. */
+	B43legacy_SEC_ALGO_WEP40,
+	B43legacy_SEC_ALGO_TKIP,
+	B43legacy_SEC_ALGO_AES,
+	B43legacy_SEC_ALGO_WEP104,
+	B43legacy_SEC_ALGO_AES_LEGACY,
+};
+
+/* Core Information Registers */
+#define B43legacy_CIR_BASE                0xf00
+#define B43legacy_CIR_SBTPSFLAG           (B43legacy_CIR_BASE + 0x18)
+#define B43legacy_CIR_SBIMSTATE           (B43legacy_CIR_BASE + 0x90)
+#define B43legacy_CIR_SBINTVEC            (B43legacy_CIR_BASE + 0x94)
+#define B43legacy_CIR_SBTMSTATELOW        (B43legacy_CIR_BASE + 0x98)
+#define B43legacy_CIR_SBTMSTATEHIGH       (B43legacy_CIR_BASE + 0x9c)
+#define B43legacy_CIR_SBIMCONFIGLOW       (B43legacy_CIR_BASE + 0xa8)
+#define B43legacy_CIR_SB_ID_HI            (B43legacy_CIR_BASE + 0xfc)
+
+/* sbtmstatehigh state flags */
+#define B43legacy_SBTMSTATEHIGH_SERROR		0x00000001
+#define B43legacy_SBTMSTATEHIGH_BUSY		0x00000004
+#define B43legacy_SBTMSTATEHIGH_TIMEOUT		0x00000020
+#define B43legacy_SBTMSTATEHIGH_G_PHY_AVAIL	0x00010000
+#define B43legacy_SBTMSTATEHIGH_COREFLAGS	0x1FFF0000
+#define B43legacy_SBTMSTATEHIGH_DMA64BIT	0x10000000
+#define B43legacy_SBTMSTATEHIGH_GATEDCLK	0x20000000
+#define B43legacy_SBTMSTATEHIGH_BISTFAILED	0x40000000
+#define B43legacy_SBTMSTATEHIGH_BISTCOMPLETE	0x80000000
+
+/* sbimstate flags */
+#define B43legacy_SBIMSTATE_IB_ERROR		0x20000
+#define B43legacy_SBIMSTATE_TIMEOUT		0x40000
+
+#define PFX		KBUILD_MODNAME ": "
+#ifdef assert
+# undef assert
+#endif
+#ifdef CONFIG_B43LEGACY_DEBUG
+# define B43legacy_WARN_ON(expr)					\
+	do {								\
+		if (unlikely((expr))) {					\
+			printk(KERN_INFO PFX "Test (%s) failed at:"	\
+					      " %s:%d:%s()\n",		\
+					      #expr, __FILE__,		\
+					      __LINE__, __FUNCTION__);	\
+		}							\
+	} while (0)
+# define B43legacy_BUG_ON(expr)						\
+	do {								\
+		if (unlikely((expr))) {					\
+			printk(KERN_INFO PFX "Test (%s) failed\n",	\
+					      #expr);			\
+			BUG_ON(expr);					\
+		}							\
+	} while (0)
+# define B43legacy_DEBUG	1
+#else
+# define B43legacy_WARN_ON(x)	do { /* nothing */ } while (0)
+# define B43legacy_BUG_ON(x)	do { /* nothing */ } while (0)
+# define B43legacy_DEBUG	0
+#endif
+
+
+struct net_device;
+struct pci_dev;
+struct b43legacy_dmaring;
+struct b43legacy_pioqueue;
+
+/* The firmware file header */
+#define B43legacy_FW_TYPE_UCODE	'u'
+#define B43legacy_FW_TYPE_PCM	'p'
+#define B43legacy_FW_TYPE_IV	'i'
+struct b43legacy_fw_header {
+	/* File type */
+	u8 type;
+	/* File format version */
+	u8 ver;
+	u8 __padding[2];
+	/* Size of the data. For ucode and PCM this is in bytes.
+	 * For IV this is number-of-ivs. */
+	__be32 size;
+} __attribute__((__packed__));
+
+/* Initial Value file format */
+#define B43legacy_IV_OFFSET_MASK	0x7FFF
+#define B43legacy_IV_32BIT		0x8000
+struct b43legacy_iv {
+	__be16 offset_size;
+	union {
+		__be16 d16;
+		__be32 d32;
+	} data __attribute__((__packed__));
+} __attribute__((__packed__));
+
+#define B43legacy_PHYMODE(phytype)	(1 << (phytype))
+#define B43legacy_PHYMODE_B		B43legacy_PHYMODE	\
+					((B43legacy_PHYTYPE_B))
+#define B43legacy_PHYMODE_G		B43legacy_PHYMODE	\
+					((B43legacy_PHYTYPE_G))
+
+/* Value pair to measure the LocalOscillator. */
+struct b43legacy_lopair {
+	s8 low;
+	s8 high;
+	u8 used:1;
+};
+#define B43legacy_LO_COUNT	(14*4)
+
+struct b43legacy_phy {
+	/* Possible PHYMODEs on this PHY */
+	u8 possible_phymodes;
+	/* GMODE bit enabled in MACCTL? */
+	bool gmode;
+	/* Possible ieee80211 subsystem hwmodes for this PHY.
+	 * Which mode is selected, depends on thr GMODE enabled bit */
+#define B43legacy_MAX_PHYHWMODES	2
+	struct ieee80211_hw_mode hwmodes[B43legacy_MAX_PHYHWMODES];
+
+	/* Analog Type */
+	u8 analog;
+	/* B43legacy_PHYTYPE_ */
+	u8 type;
+	/* PHY revision number. */
+	u8 rev;
+
+	u16 antenna_diversity;
+	u16 savedpctlreg;
+	/* Radio versioning */
+	u16 radio_manuf;	/* Radio manufacturer */
+	u16 radio_ver;		/* Radio version */
+	u8 calibrated:1;
+	u8 radio_rev;		/* Radio revision */
+
+	bool locked;		/* Only used in b43legacy_phy_{un}lock() */
+	bool dyn_tssi_tbl;	/* tssi2dbm is kmalloc()ed. */
+
+	/* ACI (adjacent channel interference) flags. */
+	bool aci_enable;
+	bool aci_wlan_automatic;
+	bool aci_hw_rssi;
+
+	/* Radio switched on/off */
+	bool radio_on;
+	struct {
+		/* Values saved when turning the radio off.
+		 * They are needed when turning it on again. */
+		bool valid;
+		u16 rfover;
+		u16 rfoverval;
+	} radio_off_context;
+
+	u16 minlowsig[2];
+	u16 minlowsigpos[2];
+
+	/* LO Measurement Data.
+	 * Use b43legacy_get_lopair() to get a value.
+	 */
+	struct b43legacy_lopair *_lo_pairs;
+	/* TSSI to dBm table in use */
+	const s8 *tssi2dbm;
+	/* idle TSSI value */
+	s8 idle_tssi;
+	/* Target idle TSSI */
+	int tgt_idle_tssi;
+	/* Current idle TSSI */
+	int cur_idle_tssi;
+
+	/* LocalOscillator control values. */
+	struct b43legacy_txpower_lo_control *lo_control;
+	/* Values from b43legacy_calc_loopback_gain() */
+	s16 max_lb_gain;	/* Maximum Loopback gain in hdB */
+	s16 trsw_rx_gain;	/* TRSW RX gain in hdB */
+	s16 lna_lod_gain;	/* LNA lod */
+	s16 lna_gain;		/* LNA */
+	s16 pga_gain;		/* PGA */
+
+	/* PHY lock for core.rev < 3
+	 * This lock is only used by b43legacy_phy_{un}lock()
+	 */
+	spinlock_t lock;
+
+	/* Desired TX power level (in dBm). This is set by the user and
+	 * adjusted in b43legacy_phy_xmitpower(). */
+	u8 power_level;
+
+	/* Values from b43legacy_calc_loopback_gain() */
+	u16 loopback_gain[2];
+
+	/* TX Power control values. */
+	/* B/G PHY */
+	struct {
+		/* Current Radio Attenuation for TXpower recalculation. */
+		u16 rfatt;
+		/* Current Baseband Attenuation for TXpower recalculation. */
+		u16 bbatt;
+		/* Current TXpower control value for TXpower recalculation. */
+		u16 txctl1;
+		u16 txctl2;
+	};
+	/* A PHY */
+	struct {
+		u16 txpwr_offset;
+	};
+
+#ifdef CONFIG_B43LEGACY_DEBUG
+	bool manual_txpower_control; /* Manual TX-power control enabled? */
+#endif
+	/* Current Interference Mitigation mode */
+	int interfmode;
+	/* Stack of saved values from the Interference Mitigation code.
+	 * Each value in the stack is layed out as follows:
+	 * bit 0-11:  offset
+	 * bit 12-15: register ID
+	 * bit 16-32: value
+	 * register ID is: 0x1 PHY, 0x2 Radio, 0x3 ILT
+	 */
+#define B43legacy_INTERFSTACK_SIZE	26
+	u32 interfstack[B43legacy_INTERFSTACK_SIZE];
+
+	/* Saved values from the NRSSI Slope calculation */
+	s16 nrssi[2];
+	s32 nrssislope;
+	/* In memory nrssi lookup table. */
+	s8 nrssi_lt[64];
+
+	/* current channel */
+	u8 channel;
+
+	u16 lofcal;
+
+	u16 initval;
+};
+
+/* Data structures for DMA transmission, per 80211 core. */
+struct b43legacy_dma {
+	struct b43legacy_dmaring *tx_ring0;
+	struct b43legacy_dmaring *tx_ring1;
+	struct b43legacy_dmaring *tx_ring2;
+	struct b43legacy_dmaring *tx_ring3;
+	struct b43legacy_dmaring *tx_ring4;
+	struct b43legacy_dmaring *tx_ring5;
+
+	struct b43legacy_dmaring *rx_ring0;
+	struct b43legacy_dmaring *rx_ring3; /* only on core.rev < 5 */
+};
+
+/* Data structures for PIO transmission, per 80211 core. */
+struct b43legacy_pio {
+	struct b43legacy_pioqueue *queue0;
+	struct b43legacy_pioqueue *queue1;
+	struct b43legacy_pioqueue *queue2;
+	struct b43legacy_pioqueue *queue3;
+};
+
+/* Context information for a noise calculation (Link Quality). */
+struct b43legacy_noise_calculation {
+	u8 channel_at_start;
+	bool calculation_running;
+	u8 nr_samples;
+	s8 samples[8][4];
+};
+
+struct b43legacy_stats {
+	u8 link_noise;
+	/* Store the last TX/RX times here for updating the leds. */
+	unsigned long last_tx;
+	unsigned long last_rx;
+};
+
+struct b43legacy_key {
+	void *keyconf;
+	bool enabled;
+	u8 algorithm;
+};
+
+struct b43legacy_wldev;
+
+/* Data structure for the WLAN parts (802.11 cores) of the b43legacy chip. */
+struct b43legacy_wl {
+	/* Pointer to the active wireless device on this chip */
+	struct b43legacy_wldev *current_dev;
+	/* Pointer to the ieee80211 hardware data structure */
+	struct ieee80211_hw *hw;
+
+	spinlock_t irq_lock;		/* locks IRQ */
+	struct mutex mutex;		/* locks wireless core state */
+	spinlock_t leds_lock;		/* lock for leds */
+
+	/* We can only have one operating interface (802.11 core)
+	 * at a time. General information about this interface follows.
+	 */
+
+	/* Opaque ID of the operating interface from the ieee80211
+	 * subsystem. Do not modify.
+	 */
+	int if_id;
+	/* MAC address (can be NULL). */
+	u8 mac_addr[ETH_ALEN];
+	/* Current BSSID (can be NULL). */
+	u8 bssid[ETH_ALEN];
+	/* Interface type. (IEEE80211_IF_TYPE_XXX) */
+	int if_type;
+	/* Is the card operating in AP, STA or IBSS mode? */
+	bool operating;
+	/* filter flags */
+	unsigned int filter_flags;
+	/* Stats about the wireless interface */
+	struct ieee80211_low_level_stats ieee_stats;
+
+	struct hwrng rng;
+	u8 rng_initialized;
+	char rng_name[30 + 1];
+
+	/* List of all wireless devices on this chip */
+	struct list_head devlist;
+	u8 nr_devs;
+};
+
+/* Pointers to the firmware data and meta information about it. */
+struct b43legacy_firmware {
+	/* Microcode */
+	const struct firmware *ucode;
+	/* PCM code */
+	const struct firmware *pcm;
+	/* Initial MMIO values for the firmware */
+	const struct firmware *initvals;
+	/* Initial MMIO values for the firmware, band-specific */
+	const struct firmware *initvals_band;
+	/* Firmware revision */
+	u16 rev;
+	/* Firmware patchlevel */
+	u16 patch;
+};
+
+/* Device (802.11 core) initialization status. */
+enum {
+	B43legacy_STAT_UNINIT		= 0, /* Uninitialized. */
+	B43legacy_STAT_INITIALIZED	= 1, /* Initialized, not yet started. */
+	B43legacy_STAT_STARTED	= 2, /* Up and running. */
+};
+#define b43legacy_status(wldev)	atomic_read(&(wldev)->__init_status)
+#define b43legacy_set_status(wldev, stat)	do {		\
+		atomic_set(&(wldev)->__init_status, (stat));	\
+		smp_wmb();					\
+					} while (0)
+
+/* *** ---   HOW LOCKING WORKS IN B43legacy   --- ***
+ *
+ * You should always acquire both, wl->mutex and wl->irq_lock unless:
+ * - You don't need to acquire wl->irq_lock, if the interface is stopped.
+ * - You don't need to acquire wl->mutex in the IRQ handler, IRQ tasklet
+ *   and packet TX path (and _ONLY_ there.)
+ */
+
+/* Data structure for one wireless device (802.11 core) */
+struct b43legacy_wldev {
+	struct ssb_device *dev;
+	struct b43legacy_wl *wl;
+
+	/* The device initialization status.
+	 * Use b43legacy_status() to query. */
+	atomic_t __init_status;
+	/* Saved init status for handling suspend. */
+	int suspend_init_status;
+
+	bool __using_pio;	/* Using pio rather than dma. */
+	bool bad_frames_preempt;/* Use "Bad Frames Preemption". */
+	bool reg124_set_0x4;	/* Variable to keep track of IRQ. */
+	bool short_preamble;	/* TRUE if using short preamble. */
+	bool short_slot;	/* TRUE if using short slot timing. */
+	bool radio_hw_enable;	/* State of radio hardware enable bit. */
+
+	/* PHY/Radio device. */
+	struct b43legacy_phy phy;
+	union {
+		/* DMA engines. */
+		struct b43legacy_dma dma;
+		/* PIO engines. */
+		struct b43legacy_pio pio;
+	};
+
+	/* Various statistics about the physical device. */
+	struct b43legacy_stats stats;
+
+#define B43legacy_NR_LEDS		4
+	struct b43legacy_led leds[B43legacy_NR_LEDS];
+
+	/* Reason code of the last interrupt. */
+	u32 irq_reason;
+	u32 dma_reason[6];
+	/* saved irq enable/disable state bitfield. */
+	u32 irq_savedstate;
+	/* Link Quality calculation context. */
+	struct b43legacy_noise_calculation noisecalc;
+	/* if > 0 MAC is suspended. if == 0 MAC is enabled. */
+	int mac_suspended;
+
+	/* Interrupt Service Routine tasklet (bottom-half) */
+	struct tasklet_struct isr_tasklet;
+
+	/* Periodic tasks */
+	struct delayed_work periodic_work;
+	unsigned int periodic_state;
+
+	struct work_struct restart_work;
+
+	/* encryption/decryption */
+	u16 ktp; /* Key table pointer */
+	u8 max_nr_keys;
+	struct b43legacy_key key[58];
+
+	/* Cached beacon template while uploading the template. */
+	struct sk_buff *cached_beacon;
+
+	/* Firmware data */
+	struct b43legacy_firmware fw;
+
+	/* Devicelist in struct b43legacy_wl (all 802.11 cores) */
+	struct list_head list;
+
+	/* Debugging stuff follows. */
+#ifdef CONFIG_B43LEGACY_DEBUG
+	struct b43legacy_dfsentry *dfsentry;
+#endif
+};
+
+
+static inline
+struct b43legacy_wl *hw_to_b43legacy_wl(struct ieee80211_hw *hw)
+{
+	return hw->priv;
+}
+
+/* Helper function, which returns a boolean.
+ * TRUE, if PIO is used; FALSE, if DMA is used.
+ */
+#if defined(CONFIG_B43LEGACY_DMA) && defined(CONFIG_B43LEGACY_PIO)
+static inline
+int b43legacy_using_pio(struct b43legacy_wldev *dev)
+{
+	return dev->__using_pio;
+}
+#elif defined(CONFIG_B43LEGACY_DMA)
+static inline
+int b43legacy_using_pio(struct b43legacy_wldev *dev)
+{
+	return 0;
+}
+#elif defined(CONFIG_B43LEGACY_PIO)
+static inline
+int b43legacy_using_pio(struct b43legacy_wldev *dev)
+{
+	return 1;
+}
+#else
+# error "Using neither DMA nor PIO? Confused..."
+#endif
+
+
+static inline
+struct b43legacy_wldev *dev_to_b43legacy_wldev(struct device *dev)
+{
+	struct ssb_device *ssb_dev = dev_to_ssb_dev(dev);
+	return ssb_get_drvdata(ssb_dev);
+}
+
+/* Is the device operating in a specified mode (IEEE80211_IF_TYPE_XXX). */
+static inline
+int b43legacy_is_mode(struct b43legacy_wl *wl, int type)
+{
+	return (wl->operating &&
+		wl->if_type == type);
+}
+
+static inline
+bool is_bcm_board_vendor(struct b43legacy_wldev *dev)
+{
+	return  (dev->dev->bus->boardinfo.vendor == PCI_VENDOR_ID_BROADCOM);
+}
+
+static inline
+u16 b43legacy_read16(struct b43legacy_wldev *dev, u16 offset)
+{
+	return ssb_read16(dev->dev, offset);
+}
+
+static inline
+void b43legacy_write16(struct b43legacy_wldev *dev, u16 offset, u16 value)
+{
+	ssb_write16(dev->dev, offset, value);
+}
+
+static inline
+u32 b43legacy_read32(struct b43legacy_wldev *dev, u16 offset)
+{
+	return ssb_read32(dev->dev, offset);
+}
+
+static inline
+void b43legacy_write32(struct b43legacy_wldev *dev, u16 offset, u32 value)
+{
+	ssb_write32(dev->dev, offset, value);
+}
+
+static inline
+struct b43legacy_lopair *b43legacy_get_lopair(struct b43legacy_phy *phy,
+					      u16 radio_attenuation,
+					      u16 baseband_attenuation)
+{
+	return phy->_lo_pairs + (radio_attenuation
+			+ 14 * (baseband_attenuation / 2));
+}
+
+
+
+/* Message printing */
+void b43legacyinfo(struct b43legacy_wl *wl, const char *fmt, ...)
+		__attribute__((format(printf, 2, 3)));
+void b43legacyerr(struct b43legacy_wl *wl, const char *fmt, ...)
+		__attribute__((format(printf, 2, 3)));
+void b43legacywarn(struct b43legacy_wl *wl, const char *fmt, ...)
+		__attribute__((format(printf, 2, 3)));
+#if B43legacy_DEBUG
+void b43legacydbg(struct b43legacy_wl *wl, const char *fmt, ...)
+		__attribute__((format(printf, 2, 3)));
+#else /* DEBUG */
+# define b43legacydbg(wl, fmt...) do { /* nothing */ } while (0)
+#endif /* DEBUG */
+
+
+/** Limit a value between two limits */
+#ifdef limit_value
+# undef limit_value
+#endif
+#define limit_value(value, min, max)  \
+	({						\
+		typeof(value) __value = (value);	\
+		typeof(value) __min = (min);		\
+		typeof(value) __max = (max);		\
+		if (__value < __min)			\
+			__value = __min;		\
+		else if (__value > __max)		\
+			__value = __max;		\
+		__value;				\
+	})
+
+/* Macros for printing a value in Q5.2 format */
+#define Q52_FMT		"%u.%u"
+#define Q52_ARG(q52)	((q52) / 4), (((q52) & 3) * 100 / 4)
+
+#endif /* B43legacy_H_ */
diff --git a/drivers/net/wireless/b43legacy/debugfs.c b/drivers/net/wireless/b43legacy/debugfs.c
new file mode 100644
index 000000000000..eefa6fb79685
--- /dev/null
+++ b/drivers/net/wireless/b43legacy/debugfs.c
@@ -0,0 +1,505 @@
+/*
+
+  Broadcom B43legacy wireless driver
+
+  debugfs driver debugging code
+
+  Copyright (c) 2005-2007 Michael Buesch <mb@bu3sch.de>
+
+  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, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with this program; see the file COPYING.  If not, write to
+  the Free Software Foundation, Inc., 51 Franklin Steet, Fifth Floor,
+  Boston, MA 02110-1301, USA.
+
+*/
+
+#include <linux/fs.h>
+#include <linux/debugfs.h>
+#include <linux/slab.h>
+#include <linux/netdevice.h>
+#include <linux/pci.h>
+#include <linux/mutex.h>
+
+#include "b43legacy.h"
+#include "main.h"
+#include "debugfs.h"
+#include "dma.h"
+#include "pio.h"
+#include "xmit.h"
+
+
+/* The root directory. */
+static struct dentry *rootdir;
+
+struct b43legacy_debugfs_fops {
+	ssize_t (*read)(struct b43legacy_wldev *dev, char *buf, size_t bufsize);
+	int (*write)(struct b43legacy_wldev *dev, const char *buf, size_t count);
+	struct file_operations fops;
+	/* Offset of struct b43legacy_dfs_file in struct b43legacy_dfsentry */
+	size_t file_struct_offset;
+	/* Take wl->irq_lock before calling read/write? */
+	bool take_irqlock;
+};
+
+static inline
+struct b43legacy_dfs_file * fops_to_dfs_file(struct b43legacy_wldev *dev,
+				       const struct b43legacy_debugfs_fops *dfops)
+{
+	void *p;
+
+	p = dev->dfsentry;
+	p += dfops->file_struct_offset;
+
+	return p;
+}
+
+
+#define fappend(fmt, x...)	\
+	do {							\
+		if (bufsize - count)				\
+			count += snprintf(buf + count,		\
+					  bufsize - count,	\
+					  fmt , ##x);		\
+		else						\
+			printk(KERN_ERR "b43legacy: fappend overflow\n"); \
+	} while (0)
+
+
+/* wl->irq_lock is locked */
+static ssize_t tsf_read_file(struct b43legacy_wldev *dev, char *buf, size_t bufsize)
+{
+	ssize_t count = 0;
+	u64 tsf;
+
+	b43legacy_tsf_read(dev, &tsf);
+	fappend("0x%08x%08x\n",
+		(unsigned int)((tsf & 0xFFFFFFFF00000000ULL) >> 32),
+		(unsigned int)(tsf & 0xFFFFFFFFULL));
+
+	return count;
+}
+
+/* wl->irq_lock is locked */
+static int tsf_write_file(struct b43legacy_wldev *dev, const char *buf, size_t count)
+{
+	u64 tsf;
+
+	if (sscanf(buf, "%llu", (unsigned long long *)(&tsf)) != 1)
+		return -EINVAL;
+	b43legacy_tsf_write(dev, tsf);
+
+	return 0;
+}
+
+/* wl->irq_lock is locked */
+static ssize_t ucode_regs_read_file(struct b43legacy_wldev *dev, char *buf, size_t bufsize)
+{
+	ssize_t count = 0;
+	int i;
+
+	for (i = 0; i < 64; i++) {
+		fappend("r%d = 0x%04x\n", i,
+			b43legacy_shm_read16(dev, B43legacy_SHM_WIRELESS, i));
+	}
+
+	return count;
+}
+
+/* wl->irq_lock is locked */
+static ssize_t shm_read_file(struct b43legacy_wldev *dev, char *buf, size_t bufsize)
+{
+	ssize_t count = 0;
+	int i;
+	u16 tmp;
+	__le16 *le16buf = (__le16 *)buf;
+
+	for (i = 0; i < 0x1000; i++) {
+		if (bufsize <= 0)
+			break;
+		tmp = b43legacy_shm_read16(dev, B43legacy_SHM_SHARED, 2 * i);
+		le16buf[i] = cpu_to_le16(tmp);
+		count += sizeof(tmp);
+		bufsize -= sizeof(tmp);
+	}
+
+	return count;
+}
+
+static ssize_t txstat_read_file(struct b43legacy_wldev *dev, char *buf, size_t bufsize)
+{
+	struct b43legacy_txstatus_log *log = &dev->dfsentry->txstatlog;
+	ssize_t count = 0;
+	unsigned long flags;
+	int i, idx;
+	struct b43legacy_txstatus *stat;
+
+	spin_lock_irqsave(&log->lock, flags);
+	if (log->end < 0) {
+		fappend("Nothing transmitted, yet\n");
+		goto out_unlock;
+	}
+	fappend("b43legacy TX status reports:\n\n"
+		"index | cookie | seq | phy_stat | frame_count | "
+		"rts_count | supp_reason | pm_indicated | "
+		"intermediate | for_ampdu | acked\n" "---\n");
+	i = log->end + 1;
+	idx = 0;
+	while (1) {
+		if (i == B43legacy_NR_LOGGED_TXSTATUS)
+			i = 0;
+		stat = &(log->log[i]);
+		if (stat->cookie) {
+			fappend("%03d | "
+				"0x%04X | 0x%04X | 0x%02X | "
+				"0x%X | 0x%X | "
+				"%u | %u | "
+				"%u | %u | %u\n",
+				idx,
+				stat->cookie, stat->seq, stat->phy_stat,
+				stat->frame_count, stat->rts_count,
+				stat->supp_reason, stat->pm_indicated,
+				stat->intermediate, stat->for_ampdu,
+				stat->acked);
+			idx++;
+		}
+		if (i == log->end)
+			break;
+		i++;
+	}
+out_unlock:
+	spin_unlock_irqrestore(&log->lock, flags);
+
+	return count;
+}
+
+/* wl->irq_lock is locked */
+static int restart_write_file(struct b43legacy_wldev *dev, const char *buf, size_t count)
+{
+	int err = 0;
+
+	if (count > 0 && buf[0] == '1') {
+		b43legacy_controller_restart(dev, "manually restarted");
+	} else
+		err = -EINVAL;
+
+	return err;
+}
+
+#undef fappend
+
+static int b43legacy_debugfs_open(struct inode *inode, struct file *file)
+{
+	file->private_data = inode->i_private;
+	return 0;
+}
+
+static ssize_t b43legacy_debugfs_read(struct file *file, char __user *userbuf,
+				size_t count, loff_t *ppos)
+{
+	struct b43legacy_wldev *dev;
+	struct b43legacy_debugfs_fops *dfops;
+	struct b43legacy_dfs_file *dfile;
+	ssize_t ret = 0;
+	char *buf;
+	const size_t bufsize = 1024 * 128;
+	const size_t buforder = get_order(bufsize);
+	int err = 0;
+
+	if (!count)
+		return 0;
+	dev = file->private_data;
+	if (!dev)
+		return -ENODEV;
+
+	mutex_lock(&dev->wl->mutex);
+	if (b43legacy_status(dev) < B43legacy_STAT_INITIALIZED) {
+		err = -ENODEV;
+		goto out_unlock;
+	}
+
+	dfops = container_of(file->f_op, struct b43legacy_debugfs_fops, fops);
+	if (!dfops->read) {
+		err = -ENOSYS;
+		goto out_unlock;
+	}
+	dfile = fops_to_dfs_file(dev, dfops);
+
+	if (!dfile->buffer) {
+		buf = (char *)__get_free_pages(GFP_KERNEL, buforder);
+		if (!buf) {
+			err = -ENOMEM;
+			goto out_unlock;
+		}
+		memset(buf, 0, bufsize);
+		if (dfops->take_irqlock) {
+			spin_lock_irq(&dev->wl->irq_lock);
+			ret = dfops->read(dev, buf, bufsize);
+			spin_unlock_irq(&dev->wl->irq_lock);
+		} else
+			ret = dfops->read(dev, buf, bufsize);
+		if (ret <= 0) {
+			free_pages((unsigned long)buf, buforder);
+			err = ret;
+			goto out_unlock;
+		}
+		dfile->data_len = ret;
+		dfile->buffer = buf;
+	}
+
+	ret = simple_read_from_buffer(userbuf, count, ppos,
+				      dfile->buffer,
+				      dfile->data_len);
+	if (*ppos >= dfile->data_len) {
+		free_pages((unsigned long)dfile->buffer, buforder);
+		dfile->buffer = NULL;
+		dfile->data_len = 0;
+	}
+out_unlock:
+	mutex_unlock(&dev->wl->mutex);
+
+	return err ? err : ret;
+}
+
+static ssize_t b43legacy_debugfs_write(struct file *file,
+				 const char __user *userbuf,
+				 size_t count, loff_t *ppos)
+{
+	struct b43legacy_wldev *dev;
+	struct b43legacy_debugfs_fops *dfops;
+	char *buf;
+	int err = 0;
+
+	if (!count)
+		return 0;
+	if (count > PAGE_SIZE)
+		return -E2BIG;
+	dev = file->private_data;
+	if (!dev)
+		return -ENODEV;
+
+	mutex_lock(&dev->wl->mutex);
+	if (b43legacy_status(dev) < B43legacy_STAT_INITIALIZED) {
+		err = -ENODEV;
+		goto out_unlock;
+	}
+
+	dfops = container_of(file->f_op, struct b43legacy_debugfs_fops, fops);
+	if (!dfops->write) {
+		err = -ENOSYS;
+		goto out_unlock;
+	}
+
+	buf = (char *)get_zeroed_page(GFP_KERNEL);
+	if (!buf) {
+		err = -ENOMEM;
+		goto out_unlock;
+	}
+	if (copy_from_user(buf, userbuf, count)) {
+		err = -EFAULT;
+		goto out_freepage;
+	}
+	if (dfops->take_irqlock) {
+		spin_lock_irq(&dev->wl->irq_lock);
+		err = dfops->write(dev, buf, count);
+		spin_unlock_irq(&dev->wl->irq_lock);
+	} else
+		err = dfops->write(dev, buf, count);
+	if (err)
+		goto out_freepage;
+
+out_freepage:
+	free_page((unsigned long)buf);
+out_unlock:
+	mutex_unlock(&dev->wl->mutex);
+
+	return err ? err : count;
+}
+
+
+#define B43legacy_DEBUGFS_FOPS(name, _read, _write, _take_irqlock)	\
+	static struct b43legacy_debugfs_fops fops_##name = {		\
+		.read	= _read,				\
+		.write	= _write,				\
+		.fops	= {					\
+			.open	= b43legacy_debugfs_open,		\
+			.read	= b43legacy_debugfs_read,		\
+			.write	= b43legacy_debugfs_write,		\
+		},						\
+		.file_struct_offset = offsetof(struct b43legacy_dfsentry, \
+					       file_##name),	\
+		.take_irqlock	= _take_irqlock,		\
+	}
+
+B43legacy_DEBUGFS_FOPS(tsf, tsf_read_file, tsf_write_file, 1);
+B43legacy_DEBUGFS_FOPS(ucode_regs, ucode_regs_read_file, NULL, 1);
+B43legacy_DEBUGFS_FOPS(shm, shm_read_file, NULL, 1);
+B43legacy_DEBUGFS_FOPS(txstat, txstat_read_file, NULL, 0);
+B43legacy_DEBUGFS_FOPS(restart, NULL, restart_write_file, 1);
+
+
+int b43legacy_debug(struct b43legacy_wldev *dev, enum b43legacy_dyndbg feature)
+{
+	return !!(dev->dfsentry && dev->dfsentry->dyn_debug[feature]);
+}
+
+static void b43legacy_remove_dynamic_debug(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_dfsentry *e = dev->dfsentry;
+	int i;
+
+	for (i = 0; i < __B43legacy_NR_DYNDBG; i++)
+		debugfs_remove(e->dyn_debug_dentries[i]);
+}
+
+static void b43legacy_add_dynamic_debug(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_dfsentry *e = dev->dfsentry;
+	struct dentry *d;
+
+#define add_dyn_dbg(name, id, initstate) do {		\
+	e->dyn_debug[id] = (initstate);			\
+	d = debugfs_create_bool(name, 0600, e->subdir,	\
+				&(e->dyn_debug[id]));	\
+	if (!IS_ERR(d))					\
+		e->dyn_debug_dentries[id] = d;		\
+				} while (0)
+
+	add_dyn_dbg("debug_xmitpower", B43legacy_DBG_XMITPOWER, 0);
+	add_dyn_dbg("debug_dmaoverflow", B43legacy_DBG_DMAOVERFLOW, 0);
+	add_dyn_dbg("debug_dmaverbose", B43legacy_DBG_DMAVERBOSE, 0);
+	add_dyn_dbg("debug_pwork_fast", B43legacy_DBG_PWORK_FAST, 0);
+	add_dyn_dbg("debug_pwork_stop", B43legacy_DBG_PWORK_STOP, 0);
+
+#undef add_dyn_dbg
+}
+
+void b43legacy_debugfs_add_device(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_dfsentry *e;
+	struct b43legacy_txstatus_log *log;
+	char devdir[16];
+
+	B43legacy_WARN_ON(!dev);
+	e = kzalloc(sizeof(*e), GFP_KERNEL);
+	if (!e) {
+		b43legacyerr(dev->wl, "debugfs: add device OOM\n");
+		return;
+	}
+	e->dev = dev;
+	log = &e->txstatlog;
+	log->log = kcalloc(B43legacy_NR_LOGGED_TXSTATUS,
+			   sizeof(struct b43legacy_txstatus), GFP_KERNEL);
+	if (!log->log) {
+		b43legacyerr(dev->wl, "debugfs: add device txstatus OOM\n");
+		kfree(e);
+		return;
+	}
+	log->end = -1;
+	spin_lock_init(&log->lock);
+
+	dev->dfsentry = e;
+
+	snprintf(devdir, sizeof(devdir), "%s", wiphy_name(dev->wl->hw->wiphy));
+	e->subdir = debugfs_create_dir(devdir, rootdir);
+	if (!e->subdir || IS_ERR(e->subdir)) {
+		if (e->subdir == ERR_PTR(-ENODEV)) {
+			b43legacydbg(dev->wl, "DebugFS (CONFIG_DEBUG_FS) not "
+			       "enabled in kernel config\n");
+		} else {
+			b43legacyerr(dev->wl, "debugfs: cannot create %s directory\n",
+			       devdir);
+		}
+		dev->dfsentry = NULL;
+		kfree(log->log);
+		kfree(e);
+		return;
+	}
+
+#define ADD_FILE(name, mode)	\
+	do {							\
+		struct dentry *d;				\
+		d = debugfs_create_file(__stringify(name),	\
+					mode, e->subdir, dev,	\
+					&fops_##name.fops);	\
+		e->file_##name.dentry = NULL;			\
+		if (!IS_ERR(d))					\
+			e->file_##name.dentry = d;		\
+	} while (0)
+
+
+	ADD_FILE(tsf, 0600);
+	ADD_FILE(ucode_regs, 0400);
+	ADD_FILE(shm, 0400);
+	ADD_FILE(txstat, 0400);
+	ADD_FILE(restart, 0200);
+
+#undef ADD_FILE
+
+	b43legacy_add_dynamic_debug(dev);
+}
+
+void b43legacy_debugfs_remove_device(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_dfsentry *e;
+
+	if (!dev)
+		return;
+	e = dev->dfsentry;
+	if (!e)
+		return;
+	b43legacy_remove_dynamic_debug(dev);
+
+	debugfs_remove(e->file_tsf.dentry);
+	debugfs_remove(e->file_ucode_regs.dentry);
+	debugfs_remove(e->file_shm.dentry);
+	debugfs_remove(e->file_txstat.dentry);
+	debugfs_remove(e->file_restart.dentry);
+
+	debugfs_remove(e->subdir);
+	kfree(e->txstatlog.log);
+	kfree(e);
+}
+
+void b43legacy_debugfs_log_txstat(struct b43legacy_wldev *dev,
+			    const struct b43legacy_txstatus *status)
+{
+	struct b43legacy_dfsentry *e = dev->dfsentry;
+	struct b43legacy_txstatus_log *log;
+	struct b43legacy_txstatus *cur;
+	int i;
+
+	if (!e)
+		return;
+	log = &e->txstatlog;
+	B43legacy_WARN_ON(!irqs_disabled());
+	spin_lock(&log->lock);
+	i = log->end + 1;
+	if (i == B43legacy_NR_LOGGED_TXSTATUS)
+		i = 0;
+	log->end = i;
+	cur = &(log->log[i]);
+	memcpy(cur, status, sizeof(*cur));
+	spin_unlock(&log->lock);
+}
+
+void b43legacy_debugfs_init(void)
+{
+	rootdir = debugfs_create_dir(KBUILD_MODNAME, NULL);
+	if (IS_ERR(rootdir))
+		rootdir = NULL;
+}
+
+void b43legacy_debugfs_exit(void)
+{
+	debugfs_remove(rootdir);
+}
diff --git a/drivers/net/wireless/b43legacy/debugfs.h b/drivers/net/wireless/b43legacy/debugfs.h
new file mode 100644
index 000000000000..ae3b0d0fa849
--- /dev/null
+++ b/drivers/net/wireless/b43legacy/debugfs.h
@@ -0,0 +1,89 @@
+#ifndef B43legacy_DEBUGFS_H_
+#define B43legacy_DEBUGFS_H_
+
+struct b43legacy_wldev;
+struct b43legacy_txstatus;
+
+enum b43legacy_dyndbg { /* Dynamic debugging features */
+	B43legacy_DBG_XMITPOWER,
+	B43legacy_DBG_DMAOVERFLOW,
+	B43legacy_DBG_DMAVERBOSE,
+	B43legacy_DBG_PWORK_FAST,
+	B43legacy_DBG_PWORK_STOP,
+	__B43legacy_NR_DYNDBG,
+};
+
+
+#ifdef CONFIG_B43LEGACY_DEBUG
+
+struct dentry;
+
+#define B43legacy_NR_LOGGED_TXSTATUS	100
+
+struct b43legacy_txstatus_log {
+	struct b43legacy_txstatus *log;
+	int end;
+	spinlock_t lock;	/* lock for debugging */
+};
+
+struct b43legacy_dfs_file {
+	struct dentry *dentry;
+	char *buffer;
+	size_t data_len;
+};
+
+struct b43legacy_dfsentry {
+	struct b43legacy_wldev *dev;
+	struct dentry *subdir;
+
+	struct b43legacy_dfs_file file_tsf;
+	struct b43legacy_dfs_file file_ucode_regs;
+	struct b43legacy_dfs_file file_shm;
+	struct b43legacy_dfs_file file_txstat;
+	struct b43legacy_dfs_file file_txpower_g;
+	struct b43legacy_dfs_file file_restart;
+	struct b43legacy_dfs_file file_loctls;
+
+	struct b43legacy_txstatus_log txstatlog;
+
+	/* Enabled/Disabled list for the dynamic debugging features. */
+	u32 dyn_debug[__B43legacy_NR_DYNDBG];
+	/* Dentries for the dynamic debugging entries. */
+	struct dentry *dyn_debug_dentries[__B43legacy_NR_DYNDBG];
+};
+
+int b43legacy_debug(struct b43legacy_wldev *dev,
+		    enum b43legacy_dyndbg feature);
+
+void b43legacy_debugfs_init(void);
+void b43legacy_debugfs_exit(void);
+void b43legacy_debugfs_add_device(struct b43legacy_wldev *dev);
+void b43legacy_debugfs_remove_device(struct b43legacy_wldev *dev);
+void b43legacy_debugfs_log_txstat(struct b43legacy_wldev *dev,
+				  const struct b43legacy_txstatus *status);
+
+#else /* CONFIG_B43LEGACY_DEBUG*/
+
+static inline
+int b43legacy_debug(struct b43legacy_wldev *dev,
+		    enum b43legacy_dyndbg feature)
+{
+	return 0;
+}
+
+static inline
+void b43legacy_debugfs_init(void) { }
+static inline
+void b43legacy_debugfs_exit(void) { }
+static inline
+void b43legacy_debugfs_add_device(struct b43legacy_wldev *dev) { }
+static inline
+void b43legacy_debugfs_remove_device(struct b43legacy_wldev *dev) { }
+static inline
+void b43legacy_debugfs_log_txstat(struct b43legacy_wldev *dev,
+				  const struct b43legacy_txstatus *status)
+				  { }
+
+#endif /* CONFIG_B43LEGACY_DEBUG*/
+
+#endif /* B43legacy_DEBUGFS_H_ */
diff --git a/drivers/net/wireless/b43legacy/dma.c b/drivers/net/wireless/b43legacy/dma.c
new file mode 100644
index 000000000000..8cb3dc4c4745
--- /dev/null
+++ b/drivers/net/wireless/b43legacy/dma.c
@@ -0,0 +1,1565 @@
+/*
+
+  Broadcom B43legacy wireless driver
+
+  DMA ringbuffer and descriptor allocation/management
+
+  Copyright (c) 2005, 2006 Michael Buesch <mb@bu3sch.de>
+
+  Some code in this file is derived from the b44.c driver
+  Copyright (C) 2002 David S. Miller
+  Copyright (C) Pekka Pietikainen
+
+  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, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with this program; see the file COPYING.  If not, write to
+  the Free Software Foundation, Inc., 51 Franklin Steet, Fifth Floor,
+  Boston, MA 02110-1301, USA.
+
+*/
+
+#include "b43legacy.h"
+#include "dma.h"
+#include "main.h"
+#include "debugfs.h"
+#include "xmit.h"
+
+#include <linux/dma-mapping.h>
+#include <linux/pci.h>
+#include <linux/delay.h>
+#include <linux/skbuff.h>
+#include <net/dst.h>
+
+/* 32bit DMA ops. */
+static
+struct b43legacy_dmadesc_generic *op32_idx2desc(
+					struct b43legacy_dmaring *ring,
+					int slot,
+					struct b43legacy_dmadesc_meta **meta)
+{
+	struct b43legacy_dmadesc32 *desc;
+
+	*meta = &(ring->meta[slot]);
+	desc = ring->descbase;
+	desc = &(desc[slot]);
+
+	return (struct b43legacy_dmadesc_generic *)desc;
+}
+
+static void op32_fill_descriptor(struct b43legacy_dmaring *ring,
+				 struct b43legacy_dmadesc_generic *desc,
+				 dma_addr_t dmaaddr, u16 bufsize,
+				 int start, int end, int irq)
+{
+	struct b43legacy_dmadesc32 *descbase = ring->descbase;
+	int slot;
+	u32 ctl;
+	u32 addr;
+	u32 addrext;
+
+	slot = (int)(&(desc->dma32) - descbase);
+	B43legacy_WARN_ON(!(slot >= 0 && slot < ring->nr_slots));
+
+	addr = (u32)(dmaaddr & ~SSB_DMA_TRANSLATION_MASK);
+	addrext = (u32)(dmaaddr & SSB_DMA_TRANSLATION_MASK)
+		   >> SSB_DMA_TRANSLATION_SHIFT;
+	addr |= ssb_dma_translation(ring->dev->dev);
+	ctl = (bufsize - ring->frameoffset)
+	      & B43legacy_DMA32_DCTL_BYTECNT;
+	if (slot == ring->nr_slots - 1)
+		ctl |= B43legacy_DMA32_DCTL_DTABLEEND;
+	if (start)
+		ctl |= B43legacy_DMA32_DCTL_FRAMESTART;
+	if (end)
+		ctl |= B43legacy_DMA32_DCTL_FRAMEEND;
+	if (irq)
+		ctl |= B43legacy_DMA32_DCTL_IRQ;
+	ctl |= (addrext << B43legacy_DMA32_DCTL_ADDREXT_SHIFT)
+	       & B43legacy_DMA32_DCTL_ADDREXT_MASK;
+
+	desc->dma32.control = cpu_to_le32(ctl);
+	desc->dma32.address = cpu_to_le32(addr);
+}
+
+static void op32_poke_tx(struct b43legacy_dmaring *ring, int slot)
+{
+	b43legacy_dma_write(ring, B43legacy_DMA32_TXINDEX,
+			    (u32)(slot * sizeof(struct b43legacy_dmadesc32)));
+}
+
+static void op32_tx_suspend(struct b43legacy_dmaring *ring)
+{
+	b43legacy_dma_write(ring, B43legacy_DMA32_TXCTL,
+			    b43legacy_dma_read(ring, B43legacy_DMA32_TXCTL)
+			    | B43legacy_DMA32_TXSUSPEND);
+}
+
+static void op32_tx_resume(struct b43legacy_dmaring *ring)
+{
+	b43legacy_dma_write(ring, B43legacy_DMA32_TXCTL,
+			    b43legacy_dma_read(ring, B43legacy_DMA32_TXCTL)
+			    & ~B43legacy_DMA32_TXSUSPEND);
+}
+
+static int op32_get_current_rxslot(struct b43legacy_dmaring *ring)
+{
+	u32 val;
+
+	val = b43legacy_dma_read(ring, B43legacy_DMA32_RXSTATUS);
+	val &= B43legacy_DMA32_RXDPTR;
+
+	return (val / sizeof(struct b43legacy_dmadesc32));
+}
+
+static void op32_set_current_rxslot(struct b43legacy_dmaring *ring,
+				    int slot)
+{
+	b43legacy_dma_write(ring, B43legacy_DMA32_RXINDEX,
+			    (u32)(slot * sizeof(struct b43legacy_dmadesc32)));
+}
+
+static const struct b43legacy_dma_ops dma32_ops = {
+	.idx2desc		= op32_idx2desc,
+	.fill_descriptor	= op32_fill_descriptor,
+	.poke_tx		= op32_poke_tx,
+	.tx_suspend		= op32_tx_suspend,
+	.tx_resume		= op32_tx_resume,
+	.get_current_rxslot	= op32_get_current_rxslot,
+	.set_current_rxslot	= op32_set_current_rxslot,
+};
+
+/* 64bit DMA ops. */
+static
+struct b43legacy_dmadesc_generic *op64_idx2desc(
+					struct b43legacy_dmaring *ring,
+					int slot,
+					struct b43legacy_dmadesc_meta
+					**meta)
+{
+	struct b43legacy_dmadesc64 *desc;
+
+	*meta = &(ring->meta[slot]);
+	desc = ring->descbase;
+	desc = &(desc[slot]);
+
+	return (struct b43legacy_dmadesc_generic *)desc;
+}
+
+static void op64_fill_descriptor(struct b43legacy_dmaring *ring,
+				 struct b43legacy_dmadesc_generic *desc,
+				 dma_addr_t dmaaddr, u16 bufsize,
+				 int start, int end, int irq)
+{
+	struct b43legacy_dmadesc64 *descbase = ring->descbase;
+	int slot;
+	u32 ctl0 = 0;
+	u32 ctl1 = 0;
+	u32 addrlo;
+	u32 addrhi;
+	u32 addrext;
+
+	slot = (int)(&(desc->dma64) - descbase);
+	B43legacy_WARN_ON(!(slot >= 0 && slot < ring->nr_slots));
+
+	addrlo = (u32)(dmaaddr & 0xFFFFFFFF);
+	addrhi = (((u64)dmaaddr >> 32) & ~SSB_DMA_TRANSLATION_MASK);
+	addrext = (((u64)dmaaddr >> 32) & SSB_DMA_TRANSLATION_MASK)
+		  >> SSB_DMA_TRANSLATION_SHIFT;
+	addrhi |= ssb_dma_translation(ring->dev->dev);
+	if (slot == ring->nr_slots - 1)
+		ctl0 |= B43legacy_DMA64_DCTL0_DTABLEEND;
+	if (start)
+		ctl0 |= B43legacy_DMA64_DCTL0_FRAMESTART;
+	if (end)
+		ctl0 |= B43legacy_DMA64_DCTL0_FRAMEEND;
+	if (irq)
+		ctl0 |= B43legacy_DMA64_DCTL0_IRQ;
+	ctl1 |= (bufsize - ring->frameoffset)
+		& B43legacy_DMA64_DCTL1_BYTECNT;
+	ctl1 |= (addrext << B43legacy_DMA64_DCTL1_ADDREXT_SHIFT)
+		& B43legacy_DMA64_DCTL1_ADDREXT_MASK;
+
+	desc->dma64.control0 = cpu_to_le32(ctl0);
+	desc->dma64.control1 = cpu_to_le32(ctl1);
+	desc->dma64.address_low = cpu_to_le32(addrlo);
+	desc->dma64.address_high = cpu_to_le32(addrhi);
+}
+
+static void op64_poke_tx(struct b43legacy_dmaring *ring, int slot)
+{
+	b43legacy_dma_write(ring, B43legacy_DMA64_TXINDEX,
+			    (u32)(slot * sizeof(struct b43legacy_dmadesc64)));
+}
+
+static void op64_tx_suspend(struct b43legacy_dmaring *ring)
+{
+	b43legacy_dma_write(ring, B43legacy_DMA64_TXCTL,
+			    b43legacy_dma_read(ring, B43legacy_DMA64_TXCTL)
+			    | B43legacy_DMA64_TXSUSPEND);
+}
+
+static void op64_tx_resume(struct b43legacy_dmaring *ring)
+{
+	b43legacy_dma_write(ring, B43legacy_DMA64_TXCTL,
+			    b43legacy_dma_read(ring, B43legacy_DMA64_TXCTL)
+			    & ~B43legacy_DMA64_TXSUSPEND);
+}
+
+static int op64_get_current_rxslot(struct b43legacy_dmaring *ring)
+{
+	u32 val;
+
+	val = b43legacy_dma_read(ring, B43legacy_DMA64_RXSTATUS);
+	val &= B43legacy_DMA64_RXSTATDPTR;
+
+	return (val / sizeof(struct b43legacy_dmadesc64));
+}
+
+static void op64_set_current_rxslot(struct b43legacy_dmaring *ring,
+				    int slot)
+{
+	b43legacy_dma_write(ring, B43legacy_DMA64_RXINDEX,
+			    (u32)(slot * sizeof(struct b43legacy_dmadesc64)));
+}
+
+static const struct b43legacy_dma_ops dma64_ops = {
+	.idx2desc		= op64_idx2desc,
+	.fill_descriptor	= op64_fill_descriptor,
+	.poke_tx		= op64_poke_tx,
+	.tx_suspend		= op64_tx_suspend,
+	.tx_resume		= op64_tx_resume,
+	.get_current_rxslot	= op64_get_current_rxslot,
+	.set_current_rxslot	= op64_set_current_rxslot,
+};
+
+
+static inline int free_slots(struct b43legacy_dmaring *ring)
+{
+	return (ring->nr_slots - ring->used_slots);
+}
+
+static inline int next_slot(struct b43legacy_dmaring *ring, int slot)
+{
+	B43legacy_WARN_ON(!(slot >= -1 && slot <= ring->nr_slots - 1));
+	if (slot == ring->nr_slots - 1)
+		return 0;
+	return slot + 1;
+}
+
+static inline int prev_slot(struct b43legacy_dmaring *ring, int slot)
+{
+	B43legacy_WARN_ON(!(slot >= 0 && slot <= ring->nr_slots - 1));
+	if (slot == 0)
+		return ring->nr_slots - 1;
+	return slot - 1;
+}
+
+#ifdef CONFIG_B43LEGACY_DEBUG
+static void update_max_used_slots(struct b43legacy_dmaring *ring,
+				  int current_used_slots)
+{
+	if (current_used_slots <= ring->max_used_slots)
+		return;
+	ring->max_used_slots = current_used_slots;
+	if (b43legacy_debug(ring->dev, B43legacy_DBG_DMAVERBOSE))
+		b43legacydbg(ring->dev->wl,
+		       "max_used_slots increased to %d on %s ring %d\n",
+		       ring->max_used_slots,
+		       ring->tx ? "TX" : "RX",
+		       ring->index);
+}
+#else
+static inline
+void update_max_used_slots(struct b43legacy_dmaring *ring,
+			   int current_used_slots)
+{ }
+#endif /* DEBUG */
+
+/* Request a slot for usage. */
+static inline
+int request_slot(struct b43legacy_dmaring *ring)
+{
+	int slot;
+
+	B43legacy_WARN_ON(!ring->tx);
+	B43legacy_WARN_ON(ring->stopped);
+	B43legacy_WARN_ON(free_slots(ring) == 0);
+
+	slot = next_slot(ring, ring->current_slot);
+	ring->current_slot = slot;
+	ring->used_slots++;
+
+	update_max_used_slots(ring, ring->used_slots);
+
+	return slot;
+}
+
+/* Mac80211-queue to b43legacy-ring mapping */
+static struct b43legacy_dmaring *priority_to_txring(
+						struct b43legacy_wldev *dev,
+						int queue_priority)
+{
+	struct b43legacy_dmaring *ring;
+
+/*FIXME: For now we always run on TX-ring-1 */
+return dev->dma.tx_ring1;
+
+	/* 0 = highest priority */
+	switch (queue_priority) {
+	default:
+		B43legacy_WARN_ON(1);
+		/* fallthrough */
+	case 0:
+		ring = dev->dma.tx_ring3;
+		break;
+	case 1:
+		ring = dev->dma.tx_ring2;
+		break;
+	case 2:
+		ring = dev->dma.tx_ring1;
+		break;
+	case 3:
+		ring = dev->dma.tx_ring0;
+		break;
+	case 4:
+		ring = dev->dma.tx_ring4;
+		break;
+	case 5:
+		ring = dev->dma.tx_ring5;
+		break;
+	}
+
+	return ring;
+}
+
+/* Bcm4301-ring to mac80211-queue mapping */
+static inline int txring_to_priority(struct b43legacy_dmaring *ring)
+{
+	static const u8 idx_to_prio[] =
+		{ 3, 2, 1, 0, 4, 5, };
+
+/*FIXME: have only one queue, for now */
+return 0;
+
+	return idx_to_prio[ring->index];
+}
+
+
+u16 b43legacy_dmacontroller_base(int dma64bit, int controller_idx)
+{
+	static const u16 map64[] = {
+		B43legacy_MMIO_DMA64_BASE0,
+		B43legacy_MMIO_DMA64_BASE1,
+		B43legacy_MMIO_DMA64_BASE2,
+		B43legacy_MMIO_DMA64_BASE3,
+		B43legacy_MMIO_DMA64_BASE4,
+		B43legacy_MMIO_DMA64_BASE5,
+	};
+	static const u16 map32[] = {
+		B43legacy_MMIO_DMA32_BASE0,
+		B43legacy_MMIO_DMA32_BASE1,
+		B43legacy_MMIO_DMA32_BASE2,
+		B43legacy_MMIO_DMA32_BASE3,
+		B43legacy_MMIO_DMA32_BASE4,
+		B43legacy_MMIO_DMA32_BASE5,
+	};
+
+	if (dma64bit) {
+		B43legacy_WARN_ON(!(controller_idx >= 0 &&
+				  controller_idx < ARRAY_SIZE(map64)));
+		return map64[controller_idx];
+	}
+	B43legacy_WARN_ON(!(controller_idx >= 0 &&
+			  controller_idx < ARRAY_SIZE(map32)));
+	return map32[controller_idx];
+}
+
+static inline
+dma_addr_t map_descbuffer(struct b43legacy_dmaring *ring,
+			  unsigned char *buf,
+			  size_t len,
+			  int tx)
+{
+	dma_addr_t dmaaddr;
+
+	if (tx)
+		dmaaddr = dma_map_single(ring->dev->dev->dev,
+					 buf, len,
+					 DMA_TO_DEVICE);
+	else
+		dmaaddr = dma_map_single(ring->dev->dev->dev,
+					 buf, len,
+					 DMA_FROM_DEVICE);
+
+	return dmaaddr;
+}
+
+static inline
+void unmap_descbuffer(struct b43legacy_dmaring *ring,
+		      dma_addr_t addr,
+		      size_t len,
+		      int tx)
+{
+	if (tx)
+		dma_unmap_single(ring->dev->dev->dev,
+				 addr, len,
+				 DMA_TO_DEVICE);
+	else
+		dma_unmap_single(ring->dev->dev->dev,
+				 addr, len,
+				 DMA_FROM_DEVICE);
+}
+
+static inline
+void sync_descbuffer_for_cpu(struct b43legacy_dmaring *ring,
+			     dma_addr_t addr,
+			     size_t len)
+{
+	B43legacy_WARN_ON(ring->tx);
+
+	dma_sync_single_for_cpu(ring->dev->dev->dev,
+				addr, len, DMA_FROM_DEVICE);
+}
+
+static inline
+void sync_descbuffer_for_device(struct b43legacy_dmaring *ring,
+				dma_addr_t addr,
+				size_t len)
+{
+	B43legacy_WARN_ON(ring->tx);
+
+	dma_sync_single_for_device(ring->dev->dev->dev,
+				   addr, len, DMA_FROM_DEVICE);
+}
+
+static inline
+void free_descriptor_buffer(struct b43legacy_dmaring *ring,
+			    struct b43legacy_dmadesc_meta *meta,
+			    int irq_context)
+{
+	if (meta->skb) {
+		if (irq_context)
+			dev_kfree_skb_irq(meta->skb);
+		else
+			dev_kfree_skb(meta->skb);
+		meta->skb = NULL;
+	}
+}
+
+static int alloc_ringmemory(struct b43legacy_dmaring *ring)
+{
+	struct device *dev = ring->dev->dev->dev;
+
+	ring->descbase = dma_alloc_coherent(dev, B43legacy_DMA_RINGMEMSIZE,
+					    &(ring->dmabase), GFP_KERNEL);
+	if (!ring->descbase) {
+		b43legacyerr(ring->dev->wl, "DMA ringmemory allocation"
+			     " failed\n");
+		return -ENOMEM;
+	}
+	memset(ring->descbase, 0, B43legacy_DMA_RINGMEMSIZE);
+
+	return 0;
+}
+
+static void free_ringmemory(struct b43legacy_dmaring *ring)
+{
+	struct device *dev = ring->dev->dev->dev;
+
+	dma_free_coherent(dev, B43legacy_DMA_RINGMEMSIZE,
+			  ring->descbase, ring->dmabase);
+}
+
+/* Reset the RX DMA channel */
+int b43legacy_dmacontroller_rx_reset(struct b43legacy_wldev *dev,
+				     u16 mmio_base, int dma64)
+{
+	int i;
+	u32 value;
+	u16 offset;
+
+	might_sleep();
+
+	offset = dma64 ? B43legacy_DMA64_RXCTL : B43legacy_DMA32_RXCTL;
+	b43legacy_write32(dev, mmio_base + offset, 0);
+	for (i = 0; i < 10; i++) {
+		offset = dma64 ? B43legacy_DMA64_RXSTATUS :
+			 B43legacy_DMA32_RXSTATUS;
+		value = b43legacy_read32(dev, mmio_base + offset);
+		if (dma64) {
+			value &= B43legacy_DMA64_RXSTAT;
+			if (value == B43legacy_DMA64_RXSTAT_DISABLED) {
+				i = -1;
+				break;
+			}
+		} else {
+			value &= B43legacy_DMA32_RXSTATE;
+			if (value == B43legacy_DMA32_RXSTAT_DISABLED) {
+				i = -1;
+				break;
+			}
+		}
+		msleep(1);
+	}
+	if (i != -1) {
+		b43legacyerr(dev->wl, "DMA RX reset timed out\n");
+		return -ENODEV;
+	}
+
+	return 0;
+}
+
+/* Reset the RX DMA channel */
+int b43legacy_dmacontroller_tx_reset(struct b43legacy_wldev *dev,
+				     u16 mmio_base, int dma64)
+{
+	int i;
+	u32 value;
+	u16 offset;
+
+	might_sleep();
+
+	for (i = 0; i < 10; i++) {
+		offset = dma64 ? B43legacy_DMA64_TXSTATUS :
+			 B43legacy_DMA32_TXSTATUS;
+		value = b43legacy_read32(dev, mmio_base + offset);
+		if (dma64) {
+			value &= B43legacy_DMA64_TXSTAT;
+			if (value == B43legacy_DMA64_TXSTAT_DISABLED ||
+			    value == B43legacy_DMA64_TXSTAT_IDLEWAIT ||
+			    value == B43legacy_DMA64_TXSTAT_STOPPED)
+				break;
+		} else {
+			value &= B43legacy_DMA32_TXSTATE;
+			if (value == B43legacy_DMA32_TXSTAT_DISABLED ||
+			    value == B43legacy_DMA32_TXSTAT_IDLEWAIT ||
+			    value == B43legacy_DMA32_TXSTAT_STOPPED)
+				break;
+		}
+		msleep(1);
+	}
+	offset = dma64 ? B43legacy_DMA64_TXCTL : B43legacy_DMA32_TXCTL;
+	b43legacy_write32(dev, mmio_base + offset, 0);
+	for (i = 0; i < 10; i++) {
+		offset = dma64 ? B43legacy_DMA64_TXSTATUS :
+			 B43legacy_DMA32_TXSTATUS;
+		value = b43legacy_read32(dev, mmio_base + offset);
+		if (dma64) {
+			value &= B43legacy_DMA64_TXSTAT;
+			if (value == B43legacy_DMA64_TXSTAT_DISABLED) {
+				i = -1;
+				break;
+			}
+		} else {
+			value &= B43legacy_DMA32_TXSTATE;
+			if (value == B43legacy_DMA32_TXSTAT_DISABLED) {
+				i = -1;
+				break;
+			}
+		}
+		msleep(1);
+	}
+	if (i != -1) {
+		b43legacyerr(dev->wl, "DMA TX reset timed out\n");
+		return -ENODEV;
+	}
+	/* ensure the reset is completed. */
+	msleep(1);
+
+	return 0;
+}
+
+static int setup_rx_descbuffer(struct b43legacy_dmaring *ring,
+			       struct b43legacy_dmadesc_generic *desc,
+			       struct b43legacy_dmadesc_meta *meta,
+			       gfp_t gfp_flags)
+{
+	struct b43legacy_rxhdr_fw3 *rxhdr;
+	struct b43legacy_hwtxstatus *txstat;
+	dma_addr_t dmaaddr;
+	struct sk_buff *skb;
+
+	B43legacy_WARN_ON(ring->tx);
+
+	skb = __dev_alloc_skb(ring->rx_buffersize, gfp_flags);
+	if (unlikely(!skb))
+		return -ENOMEM;
+	dmaaddr = map_descbuffer(ring, skb->data,
+				 ring->rx_buffersize, 0);
+	if (dma_mapping_error(dmaaddr)) {
+		/* ugh. try to realloc in zone_dma */
+		gfp_flags |= GFP_DMA;
+
+		dev_kfree_skb_any(skb);
+
+		skb = __dev_alloc_skb(ring->rx_buffersize, gfp_flags);
+		if (unlikely(!skb))
+			return -ENOMEM;
+		dmaaddr = map_descbuffer(ring, skb->data,
+					 ring->rx_buffersize, 0);
+	}
+
+	if (dma_mapping_error(dmaaddr)) {
+		dev_kfree_skb_any(skb);
+		return -EIO;
+	}
+
+	meta->skb = skb;
+	meta->dmaaddr = dmaaddr;
+	ring->ops->fill_descriptor(ring, desc, dmaaddr,
+				   ring->rx_buffersize, 0, 0, 0);
+
+	rxhdr = (struct b43legacy_rxhdr_fw3 *)(skb->data);
+	rxhdr->frame_len = 0;
+	txstat = (struct b43legacy_hwtxstatus *)(skb->data);
+	txstat->cookie = 0;
+
+	return 0;
+}
+
+/* Allocate the initial descbuffers.
+ * This is used for an RX ring only.
+ */
+static int alloc_initial_descbuffers(struct b43legacy_dmaring *ring)
+{
+	int i;
+	int err = -ENOMEM;
+	struct b43legacy_dmadesc_generic *desc;
+	struct b43legacy_dmadesc_meta *meta;
+
+	for (i = 0; i < ring->nr_slots; i++) {
+		desc = ring->ops->idx2desc(ring, i, &meta);
+
+		err = setup_rx_descbuffer(ring, desc, meta, GFP_KERNEL);
+		if (err) {
+			b43legacyerr(ring->dev->wl,
+			       "Failed to allocate initial descbuffers\n");
+			goto err_unwind;
+		}
+	}
+	mb(); /* all descbuffer setup before next line */
+	ring->used_slots = ring->nr_slots;
+	err = 0;
+out:
+	return err;
+
+err_unwind:
+	for (i--; i >= 0; i--) {
+		desc = ring->ops->idx2desc(ring, i, &meta);
+
+		unmap_descbuffer(ring, meta->dmaaddr, ring->rx_buffersize, 0);
+		dev_kfree_skb(meta->skb);
+	}
+	goto out;
+}
+
+/* Do initial setup of the DMA controller.
+ * Reset the controller, write the ring busaddress
+ * and switch the "enable" bit on.
+ */
+static int dmacontroller_setup(struct b43legacy_dmaring *ring)
+{
+	int err = 0;
+	u32 value;
+	u32 addrext;
+	u32 trans = ssb_dma_translation(ring->dev->dev);
+
+	if (ring->tx) {
+		if (ring->dma64) {
+			u64 ringbase = (u64)(ring->dmabase);
+
+			addrext = ((ringbase >> 32) & SSB_DMA_TRANSLATION_MASK)
+				  >> SSB_DMA_TRANSLATION_SHIFT;
+			value = B43legacy_DMA64_TXENABLE;
+			value |= (addrext << B43legacy_DMA64_TXADDREXT_SHIFT)
+				& B43legacy_DMA64_TXADDREXT_MASK;
+			b43legacy_dma_write(ring, B43legacy_DMA64_TXCTL,
+					    value);
+			b43legacy_dma_write(ring, B43legacy_DMA64_TXRINGLO,
+					    (ringbase & 0xFFFFFFFF));
+			b43legacy_dma_write(ring, B43legacy_DMA64_TXRINGHI,
+					    ((ringbase >> 32)
+					    & ~SSB_DMA_TRANSLATION_MASK)
+					    | trans);
+		} else {
+			u32 ringbase = (u32)(ring->dmabase);
+
+			addrext = (ringbase & SSB_DMA_TRANSLATION_MASK)
+				  >> SSB_DMA_TRANSLATION_SHIFT;
+			value = B43legacy_DMA32_TXENABLE;
+			value |= (addrext << B43legacy_DMA32_TXADDREXT_SHIFT)
+				& B43legacy_DMA32_TXADDREXT_MASK;
+			b43legacy_dma_write(ring, B43legacy_DMA32_TXCTL,
+					    value);
+			b43legacy_dma_write(ring, B43legacy_DMA32_TXRING,
+					    (ringbase &
+					    ~SSB_DMA_TRANSLATION_MASK)
+					    | trans);
+		}
+	} else {
+		err = alloc_initial_descbuffers(ring);
+		if (err)
+			goto out;
+		if (ring->dma64) {
+			u64 ringbase = (u64)(ring->dmabase);
+
+			addrext = ((ringbase >> 32) & SSB_DMA_TRANSLATION_MASK)
+				  >> SSB_DMA_TRANSLATION_SHIFT;
+			value = (ring->frameoffset <<
+				 B43legacy_DMA64_RXFROFF_SHIFT);
+			value |= B43legacy_DMA64_RXENABLE;
+			value |= (addrext << B43legacy_DMA64_RXADDREXT_SHIFT)
+				& B43legacy_DMA64_RXADDREXT_MASK;
+			b43legacy_dma_write(ring, B43legacy_DMA64_RXCTL,
+					    value);
+			b43legacy_dma_write(ring, B43legacy_DMA64_RXRINGLO,
+					    (ringbase & 0xFFFFFFFF));
+			b43legacy_dma_write(ring, B43legacy_DMA64_RXRINGHI,
+					    ((ringbase >> 32) &
+					    ~SSB_DMA_TRANSLATION_MASK) |
+					    trans);
+			b43legacy_dma_write(ring, B43legacy_DMA64_RXINDEX,
+					    200);
+		} else {
+			u32 ringbase = (u32)(ring->dmabase);
+
+			addrext = (ringbase & SSB_DMA_TRANSLATION_MASK)
+				  >> SSB_DMA_TRANSLATION_SHIFT;
+			value = (ring->frameoffset <<
+				 B43legacy_DMA32_RXFROFF_SHIFT);
+			value |= B43legacy_DMA32_RXENABLE;
+			value |= (addrext <<
+				 B43legacy_DMA32_RXADDREXT_SHIFT)
+				 & B43legacy_DMA32_RXADDREXT_MASK;
+			b43legacy_dma_write(ring, B43legacy_DMA32_RXCTL,
+					    value);
+			b43legacy_dma_write(ring, B43legacy_DMA32_RXRING,
+					    (ringbase &
+					    ~SSB_DMA_TRANSLATION_MASK)
+					    | trans);
+			b43legacy_dma_write(ring, B43legacy_DMA32_RXINDEX,
+					    200);
+		}
+	}
+
+out:
+	return err;
+}
+
+/* Shutdown the DMA controller. */
+static void dmacontroller_cleanup(struct b43legacy_dmaring *ring)
+{
+	if (ring->tx) {
+		b43legacy_dmacontroller_tx_reset(ring->dev, ring->mmio_base,
+						 ring->dma64);
+		if (ring->dma64) {
+			b43legacy_dma_write(ring, B43legacy_DMA64_TXRINGLO, 0);
+			b43legacy_dma_write(ring, B43legacy_DMA64_TXRINGHI, 0);
+		} else
+			b43legacy_dma_write(ring, B43legacy_DMA32_TXRING, 0);
+	} else {
+		b43legacy_dmacontroller_rx_reset(ring->dev, ring->mmio_base,
+						 ring->dma64);
+		if (ring->dma64) {
+			b43legacy_dma_write(ring, B43legacy_DMA64_RXRINGLO, 0);
+			b43legacy_dma_write(ring, B43legacy_DMA64_RXRINGHI, 0);
+		} else
+			b43legacy_dma_write(ring, B43legacy_DMA32_RXRING, 0);
+	}
+}
+
+static void free_all_descbuffers(struct b43legacy_dmaring *ring)
+{
+	struct b43legacy_dmadesc_generic *desc;
+	struct b43legacy_dmadesc_meta *meta;
+	int i;
+
+	if (!ring->used_slots)
+		return;
+	for (i = 0; i < ring->nr_slots; i++) {
+		desc = ring->ops->idx2desc(ring, i, &meta);
+
+		if (!meta->skb) {
+			B43legacy_WARN_ON(!ring->tx);
+			continue;
+		}
+		if (ring->tx)
+			unmap_descbuffer(ring, meta->dmaaddr,
+					 meta->skb->len, 1);
+		else
+			unmap_descbuffer(ring, meta->dmaaddr,
+					 ring->rx_buffersize, 0);
+		free_descriptor_buffer(ring, meta, 0);
+	}
+}
+
+static u64 supported_dma_mask(struct b43legacy_wldev *dev)
+{
+	u32 tmp;
+	u16 mmio_base;
+
+	tmp = b43legacy_read32(dev, SSB_TMSHIGH);
+	if (tmp & SSB_TMSHIGH_DMA64)
+		return DMA_64BIT_MASK;
+	mmio_base = b43legacy_dmacontroller_base(0, 0);
+	b43legacy_write32(dev,
+			mmio_base + B43legacy_DMA32_TXCTL,
+			B43legacy_DMA32_TXADDREXT_MASK);
+	tmp = b43legacy_read32(dev, mmio_base +
+			       B43legacy_DMA32_TXCTL);
+	if (tmp & B43legacy_DMA32_TXADDREXT_MASK)
+		return DMA_32BIT_MASK;
+
+	return DMA_30BIT_MASK;
+}
+
+/* Main initialization function. */
+static
+struct b43legacy_dmaring *b43legacy_setup_dmaring(
+					struct b43legacy_wldev *dev,
+					int controller_index,
+					int for_tx,
+					int dma64)
+{
+	struct b43legacy_dmaring *ring;
+	int err;
+	int nr_slots;
+	dma_addr_t dma_test;
+
+	ring = kzalloc(sizeof(*ring), GFP_KERNEL);
+	if (!ring)
+		goto out;
+
+	nr_slots = B43legacy_RXRING_SLOTS;
+	if (for_tx)
+		nr_slots = B43legacy_TXRING_SLOTS;
+
+	ring->meta = kcalloc(nr_slots, sizeof(struct b43legacy_dmadesc_meta),
+			     GFP_KERNEL);
+	if (!ring->meta)
+		goto err_kfree_ring;
+	if (for_tx) {
+		ring->txhdr_cache = kcalloc(nr_slots,
+					sizeof(struct b43legacy_txhdr_fw3),
+					GFP_KERNEL);
+		if (!ring->txhdr_cache)
+			goto err_kfree_meta;
+
+		/* test for ability to dma to txhdr_cache */
+		dma_test = dma_map_single(dev->dev->dev,
+				ring->txhdr_cache,
+				sizeof(struct b43legacy_txhdr_fw3),
+				DMA_TO_DEVICE);
+
+		if (dma_mapping_error(dma_test)) {
+			/* ugh realloc */
+			kfree(ring->txhdr_cache);
+			ring->txhdr_cache = kcalloc(nr_slots,
+					sizeof(struct b43legacy_txhdr_fw3),
+					GFP_KERNEL | GFP_DMA);
+			if (!ring->txhdr_cache)
+				goto err_kfree_meta;
+
+			dma_test = dma_map_single(dev->dev->dev,
+					ring->txhdr_cache,
+					sizeof(struct b43legacy_txhdr_fw3),
+					DMA_TO_DEVICE);
+
+			if (dma_mapping_error(dma_test))
+				goto err_kfree_txhdr_cache;
+		}
+
+		dma_unmap_single(dev->dev->dev,
+				 dma_test, sizeof(struct b43legacy_txhdr_fw3),
+				 DMA_TO_DEVICE);
+	}
+
+	ring->dev = dev;
+	ring->nr_slots = nr_slots;
+	ring->mmio_base = b43legacy_dmacontroller_base(dma64,
+						       controller_index);
+	ring->index = controller_index;
+	ring->dma64 = !!dma64;
+	if (dma64)
+		ring->ops = &dma64_ops;
+	else
+		ring->ops = &dma32_ops;
+	if (for_tx) {
+		ring->tx = 1;
+		ring->current_slot = -1;
+	} else {
+		if (ring->index == 0) {
+			ring->rx_buffersize = B43legacy_DMA0_RX_BUFFERSIZE;
+			ring->frameoffset = B43legacy_DMA0_RX_FRAMEOFFSET;
+		} else if (ring->index == 3) {
+			ring->rx_buffersize = B43legacy_DMA3_RX_BUFFERSIZE;
+			ring->frameoffset = B43legacy_DMA3_RX_FRAMEOFFSET;
+		} else
+			B43legacy_WARN_ON(1);
+	}
+	spin_lock_init(&ring->lock);
+#ifdef CONFIG_B43LEGACY_DEBUG
+	ring->last_injected_overflow = jiffies;
+#endif
+
+	err = alloc_ringmemory(ring);
+	if (err)
+		goto err_kfree_txhdr_cache;
+	err = dmacontroller_setup(ring);
+	if (err)
+		goto err_free_ringmemory;
+
+out:
+	return ring;
+
+err_free_ringmemory:
+	free_ringmemory(ring);
+err_kfree_txhdr_cache:
+	kfree(ring->txhdr_cache);
+err_kfree_meta:
+	kfree(ring->meta);
+err_kfree_ring:
+	kfree(ring);
+	ring = NULL;
+	goto out;
+}
+
+/* Main cleanup function. */
+static void b43legacy_destroy_dmaring(struct b43legacy_dmaring *ring)
+{
+	if (!ring)
+		return;
+
+	b43legacydbg(ring->dev->wl, "DMA-%s 0x%04X (%s) max used slots:"
+		     " %d/%d\n", (ring->dma64) ? "64" : "32", ring->mmio_base,
+		     (ring->tx) ? "TX" : "RX",
+		     ring->max_used_slots, ring->nr_slots);
+	/* Device IRQs are disabled prior entering this function,
+	 * so no need to take care of concurrency with rx handler stuff.
+	 */
+	dmacontroller_cleanup(ring);
+	free_all_descbuffers(ring);
+	free_ringmemory(ring);
+
+	kfree(ring->txhdr_cache);
+	kfree(ring->meta);
+	kfree(ring);
+}
+
+void b43legacy_dma_free(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_dma *dma;
+
+	if (b43legacy_using_pio(dev))
+		return;
+	dma = &dev->dma;
+
+	b43legacy_destroy_dmaring(dma->rx_ring3);
+	dma->rx_ring3 = NULL;
+	b43legacy_destroy_dmaring(dma->rx_ring0);
+	dma->rx_ring0 = NULL;
+
+	b43legacy_destroy_dmaring(dma->tx_ring5);
+	dma->tx_ring5 = NULL;
+	b43legacy_destroy_dmaring(dma->tx_ring4);
+	dma->tx_ring4 = NULL;
+	b43legacy_destroy_dmaring(dma->tx_ring3);
+	dma->tx_ring3 = NULL;
+	b43legacy_destroy_dmaring(dma->tx_ring2);
+	dma->tx_ring2 = NULL;
+	b43legacy_destroy_dmaring(dma->tx_ring1);
+	dma->tx_ring1 = NULL;
+	b43legacy_destroy_dmaring(dma->tx_ring0);
+	dma->tx_ring0 = NULL;
+}
+
+int b43legacy_dma_init(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_dma *dma = &dev->dma;
+	struct b43legacy_dmaring *ring;
+	int err;
+	u64 dmamask;
+	int dma64 = 0;
+
+	dmamask = supported_dma_mask(dev);
+	if (dmamask == DMA_64BIT_MASK)
+		dma64 = 1;
+
+	err = ssb_dma_set_mask(dev->dev, dmamask);
+	if (err) {
+#ifdef BCM43XX_PIO
+		b43legacywarn(dev->wl, "DMA for this device not supported. "
+			"Falling back to PIO\n");
+		dev->__using_pio = 1;
+		return -EAGAIN;
+#else
+		b43legacyerr(dev->wl, "DMA for this device not supported and "
+		       "no PIO support compiled in\n");
+		return -EOPNOTSUPP;
+#endif
+	}
+
+	err = -ENOMEM;
+	/* setup TX DMA channels. */
+	ring = b43legacy_setup_dmaring(dev, 0, 1, dma64);
+	if (!ring)
+		goto out;
+	dma->tx_ring0 = ring;
+
+	ring = b43legacy_setup_dmaring(dev, 1, 1, dma64);
+	if (!ring)
+		goto err_destroy_tx0;
+	dma->tx_ring1 = ring;
+
+	ring = b43legacy_setup_dmaring(dev, 2, 1, dma64);
+	if (!ring)
+		goto err_destroy_tx1;
+	dma->tx_ring2 = ring;
+
+	ring = b43legacy_setup_dmaring(dev, 3, 1, dma64);
+	if (!ring)
+		goto err_destroy_tx2;
+	dma->tx_ring3 = ring;
+
+	ring = b43legacy_setup_dmaring(dev, 4, 1, dma64);
+	if (!ring)
+		goto err_destroy_tx3;
+	dma->tx_ring4 = ring;
+
+	ring = b43legacy_setup_dmaring(dev, 5, 1, dma64);
+	if (!ring)
+		goto err_destroy_tx4;
+	dma->tx_ring5 = ring;
+
+	/* setup RX DMA channels. */
+	ring = b43legacy_setup_dmaring(dev, 0, 0, dma64);
+	if (!ring)
+		goto err_destroy_tx5;
+	dma->rx_ring0 = ring;
+
+	if (dev->dev->id.revision < 5) {
+		ring = b43legacy_setup_dmaring(dev, 3, 0, dma64);
+		if (!ring)
+			goto err_destroy_rx0;
+		dma->rx_ring3 = ring;
+	}
+
+	b43legacydbg(dev->wl, "%d-bit DMA initialized\n",
+	       (dmamask == DMA_64BIT_MASK) ? 64 :
+	       (dmamask == DMA_32BIT_MASK) ? 32 : 30);
+	err = 0;
+out:
+	return err;
+
+err_destroy_rx0:
+	b43legacy_destroy_dmaring(dma->rx_ring0);
+	dma->rx_ring0 = NULL;
+err_destroy_tx5:
+	b43legacy_destroy_dmaring(dma->tx_ring5);
+	dma->tx_ring5 = NULL;
+err_destroy_tx4:
+	b43legacy_destroy_dmaring(dma->tx_ring4);
+	dma->tx_ring4 = NULL;
+err_destroy_tx3:
+	b43legacy_destroy_dmaring(dma->tx_ring3);
+	dma->tx_ring3 = NULL;
+err_destroy_tx2:
+	b43legacy_destroy_dmaring(dma->tx_ring2);
+	dma->tx_ring2 = NULL;
+err_destroy_tx1:
+	b43legacy_destroy_dmaring(dma->tx_ring1);
+	dma->tx_ring1 = NULL;
+err_destroy_tx0:
+	b43legacy_destroy_dmaring(dma->tx_ring0);
+	dma->tx_ring0 = NULL;
+	goto out;
+}
+
+/* Generate a cookie for the TX header. */
+static u16 generate_cookie(struct b43legacy_dmaring *ring,
+			   int slot)
+{
+	u16 cookie = 0x1000;
+
+	/* Use the upper 4 bits of the cookie as
+	 * DMA controller ID and store the slot number
+	 * in the lower 12 bits.
+	 * Note that the cookie must never be 0, as this
+	 * is a special value used in RX path.
+	 */
+	switch (ring->index) {
+	case 0:
+		cookie = 0xA000;
+		break;
+	case 1:
+		cookie = 0xB000;
+		break;
+	case 2:
+		cookie = 0xC000;
+		break;
+	case 3:
+		cookie = 0xD000;
+		break;
+	case 4:
+		cookie = 0xE000;
+		break;
+	case 5:
+		cookie = 0xF000;
+		break;
+	}
+	B43legacy_WARN_ON(!(((u16)slot & 0xF000) == 0x0000));
+	cookie |= (u16)slot;
+
+	return cookie;
+}
+
+/* Inspect a cookie and find out to which controller/slot it belongs. */
+static
+struct b43legacy_dmaring *parse_cookie(struct b43legacy_wldev *dev,
+				      u16 cookie, int *slot)
+{
+	struct b43legacy_dma *dma = &dev->dma;
+	struct b43legacy_dmaring *ring = NULL;
+
+	switch (cookie & 0xF000) {
+	case 0xA000:
+		ring = dma->tx_ring0;
+		break;
+	case 0xB000:
+		ring = dma->tx_ring1;
+		break;
+	case 0xC000:
+		ring = dma->tx_ring2;
+		break;
+	case 0xD000:
+		ring = dma->tx_ring3;
+		break;
+	case 0xE000:
+		ring = dma->tx_ring4;
+		break;
+	case 0xF000:
+		ring = dma->tx_ring5;
+		break;
+	default:
+		B43legacy_WARN_ON(1);
+	}
+	*slot = (cookie & 0x0FFF);
+	B43legacy_WARN_ON(!(ring && *slot >= 0 && *slot < ring->nr_slots));
+
+	return ring;
+}
+
+static int dma_tx_fragment(struct b43legacy_dmaring *ring,
+			    struct sk_buff *skb,
+			    struct ieee80211_tx_control *ctl)
+{
+	const struct b43legacy_dma_ops *ops = ring->ops;
+	u8 *header;
+	int slot;
+	int err;
+	struct b43legacy_dmadesc_generic *desc;
+	struct b43legacy_dmadesc_meta *meta;
+	struct b43legacy_dmadesc_meta *meta_hdr;
+	struct sk_buff *bounce_skb;
+
+#define SLOTS_PER_PACKET  2
+	B43legacy_WARN_ON(skb_shinfo(skb)->nr_frags != 0);
+
+	/* Get a slot for the header. */
+	slot = request_slot(ring);
+	desc = ops->idx2desc(ring, slot, &meta_hdr);
+	memset(meta_hdr, 0, sizeof(*meta_hdr));
+
+	header = &(ring->txhdr_cache[slot * sizeof(
+			       struct b43legacy_txhdr_fw3)]);
+	b43legacy_generate_txhdr(ring->dev, header,
+				 skb->data, skb->len, ctl,
+				 generate_cookie(ring, slot));
+
+	meta_hdr->dmaaddr = map_descbuffer(ring, (unsigned char *)header,
+				       sizeof(struct b43legacy_txhdr_fw3), 1);
+	if (dma_mapping_error(meta_hdr->dmaaddr))
+		return -EIO;
+	ops->fill_descriptor(ring, desc, meta_hdr->dmaaddr,
+			     sizeof(struct b43legacy_txhdr_fw3), 1, 0, 0);
+
+	/* Get a slot for the payload. */
+	slot = request_slot(ring);
+	desc = ops->idx2desc(ring, slot, &meta);
+	memset(meta, 0, sizeof(*meta));
+
+	memcpy(&meta->txstat.control, ctl, sizeof(*ctl));
+	meta->skb = skb;
+	meta->is_last_fragment = 1;
+
+	meta->dmaaddr = map_descbuffer(ring, skb->data, skb->len, 1);
+	/* create a bounce buffer in zone_dma on mapping failure. */
+	if (dma_mapping_error(meta->dmaaddr)) {
+		bounce_skb = __dev_alloc_skb(skb->len, GFP_ATOMIC | GFP_DMA);
+		if (!bounce_skb) {
+			err = -ENOMEM;
+			goto out_unmap_hdr;
+		}
+
+		memcpy(skb_put(bounce_skb, skb->len), skb->data, skb->len);
+		dev_kfree_skb_any(skb);
+		skb = bounce_skb;
+		meta->skb = skb;
+		meta->dmaaddr = map_descbuffer(ring, skb->data, skb->len, 1);
+		if (dma_mapping_error(meta->dmaaddr)) {
+			err = -EIO;
+			goto out_free_bounce;
+		}
+	}
+
+	ops->fill_descriptor(ring, desc, meta->dmaaddr,
+			     skb->len, 0, 1, 1);
+
+	wmb();	/* previous stuff MUST be done */
+	/* Now transfer the whole frame. */
+	ops->poke_tx(ring, next_slot(ring, slot));
+	return 0;
+
+out_free_bounce:
+	dev_kfree_skb_any(skb);
+out_unmap_hdr:
+	unmap_descbuffer(ring, meta_hdr->dmaaddr,
+			 sizeof(struct b43legacy_txhdr_fw3), 1);
+	return err;
+}
+
+static inline
+int should_inject_overflow(struct b43legacy_dmaring *ring)
+{
+#ifdef CONFIG_B43LEGACY_DEBUG
+	if (unlikely(b43legacy_debug(ring->dev,
+				     B43legacy_DBG_DMAOVERFLOW))) {
+		/* Check if we should inject another ringbuffer overflow
+		 * to test handling of this situation in the stack. */
+		unsigned long next_overflow;
+
+		next_overflow = ring->last_injected_overflow + HZ;
+		if (time_after(jiffies, next_overflow)) {
+			ring->last_injected_overflow = jiffies;
+			b43legacydbg(ring->dev->wl,
+			       "Injecting TX ring overflow on "
+			       "DMA controller %d\n", ring->index);
+			return 1;
+		}
+	}
+#endif /* CONFIG_B43LEGACY_DEBUG */
+	return 0;
+}
+
+int b43legacy_dma_tx(struct b43legacy_wldev *dev,
+		     struct sk_buff *skb,
+		     struct ieee80211_tx_control *ctl)
+{
+	struct b43legacy_dmaring *ring;
+	int err = 0;
+	unsigned long flags;
+
+	ring = priority_to_txring(dev, ctl->queue);
+	spin_lock_irqsave(&ring->lock, flags);
+	B43legacy_WARN_ON(!ring->tx);
+	if (unlikely(free_slots(ring) < SLOTS_PER_PACKET)) {
+		b43legacywarn(dev->wl, "DMA queue overflow\n");
+		err = -ENOSPC;
+		goto out_unlock;
+	}
+	/* Check if the queue was stopped in mac80211,
+	 * but we got called nevertheless.
+	 * That would be a mac80211 bug. */
+	B43legacy_BUG_ON(ring->stopped);
+
+	err = dma_tx_fragment(ring, skb, ctl);
+	if (unlikely(err)) {
+		b43legacyerr(dev->wl, "DMA tx mapping failure\n");
+		goto out_unlock;
+	}
+	ring->nr_tx_packets++;
+	if ((free_slots(ring) < SLOTS_PER_PACKET) ||
+	    should_inject_overflow(ring)) {
+		/* This TX ring is full. */
+		ieee80211_stop_queue(dev->wl->hw, txring_to_priority(ring));
+		ring->stopped = 1;
+		if (b43legacy_debug(dev, B43legacy_DBG_DMAVERBOSE))
+			b43legacydbg(dev->wl, "Stopped TX ring %d\n",
+			       ring->index);
+	}
+out_unlock:
+	spin_unlock_irqrestore(&ring->lock, flags);
+
+	return err;
+}
+
+void b43legacy_dma_handle_txstatus(struct b43legacy_wldev *dev,
+				 const struct b43legacy_txstatus *status)
+{
+	const struct b43legacy_dma_ops *ops;
+	struct b43legacy_dmaring *ring;
+	struct b43legacy_dmadesc_generic *desc;
+	struct b43legacy_dmadesc_meta *meta;
+	int slot;
+
+	ring = parse_cookie(dev, status->cookie, &slot);
+	if (unlikely(!ring))
+		return;
+	B43legacy_WARN_ON(!irqs_disabled());
+	spin_lock(&ring->lock);
+
+	B43legacy_WARN_ON(!ring->tx);
+	ops = ring->ops;
+	while (1) {
+		B43legacy_WARN_ON(!(slot >= 0 && slot < ring->nr_slots));
+		desc = ops->idx2desc(ring, slot, &meta);
+
+		if (meta->skb)
+			unmap_descbuffer(ring, meta->dmaaddr,
+					 meta->skb->len, 1);
+		else
+			unmap_descbuffer(ring, meta->dmaaddr,
+					 sizeof(struct b43legacy_txhdr_fw3),
+					 1);
+
+		if (meta->is_last_fragment) {
+			B43legacy_WARN_ON(!meta->skb);
+			/* Call back to inform the ieee80211 subsystem about the
+			 * status of the transmission.
+			 * Some fields of txstat are already filled in dma_tx().
+			 */
+			if (status->acked) {
+				meta->txstat.flags |= IEEE80211_TX_STATUS_ACK;
+			} else {
+				if (!(meta->txstat.control.flags
+				      & IEEE80211_TXCTL_NO_ACK))
+					 meta->txstat.excessive_retries = 1;
+			}
+			if (status->frame_count == 0) {
+				/* The frame was not transmitted at all. */
+				meta->txstat.retry_count = 0;
+			} else
+				meta->txstat.retry_count = status->frame_count
+							   - 1;
+			ieee80211_tx_status_irqsafe(dev->wl->hw, meta->skb,
+						    &(meta->txstat));
+			/* skb is freed by ieee80211_tx_status_irqsafe() */
+			meta->skb = NULL;
+		} else {
+			/* No need to call free_descriptor_buffer here, as
+			 * this is only the txhdr, which is not allocated.
+			 */
+			B43legacy_WARN_ON(meta->skb != NULL);
+		}
+
+		/* Everything unmapped and free'd. So it's not used anymore. */
+		ring->used_slots--;
+
+		if (meta->is_last_fragment)
+			break;
+		slot = next_slot(ring, slot);
+	}
+	dev->stats.last_tx = jiffies;
+	if (ring->stopped) {
+		B43legacy_WARN_ON(free_slots(ring) < SLOTS_PER_PACKET);
+		ieee80211_wake_queue(dev->wl->hw, txring_to_priority(ring));
+		ring->stopped = 0;
+		if (b43legacy_debug(dev, B43legacy_DBG_DMAVERBOSE))
+			b43legacydbg(dev->wl, "Woke up TX ring %d\n",
+			       ring->index);
+	}
+
+	spin_unlock(&ring->lock);
+}
+
+void b43legacy_dma_get_tx_stats(struct b43legacy_wldev *dev,
+			      struct ieee80211_tx_queue_stats *stats)
+{
+	const int nr_queues = dev->wl->hw->queues;
+	struct b43legacy_dmaring *ring;
+	struct ieee80211_tx_queue_stats_data *data;
+	unsigned long flags;
+	int i;
+
+	for (i = 0; i < nr_queues; i++) {
+		data = &(stats->data[i]);
+		ring = priority_to_txring(dev, i);
+
+		spin_lock_irqsave(&ring->lock, flags);
+		data->len = ring->used_slots / SLOTS_PER_PACKET;
+		data->limit = ring->nr_slots / SLOTS_PER_PACKET;
+		data->count = ring->nr_tx_packets;
+		spin_unlock_irqrestore(&ring->lock, flags);
+	}
+}
+
+static void dma_rx(struct b43legacy_dmaring *ring,
+		   int *slot)
+{
+	const struct b43legacy_dma_ops *ops = ring->ops;
+	struct b43legacy_dmadesc_generic *desc;
+	struct b43legacy_dmadesc_meta *meta;
+	struct b43legacy_rxhdr_fw3 *rxhdr;
+	struct sk_buff *skb;
+	u16 len;
+	int err;
+	dma_addr_t dmaaddr;
+
+	desc = ops->idx2desc(ring, *slot, &meta);
+
+	sync_descbuffer_for_cpu(ring, meta->dmaaddr, ring->rx_buffersize);
+	skb = meta->skb;
+
+	if (ring->index == 3) {
+		/* We received an xmit status. */
+		struct b43legacy_hwtxstatus *hw =
+				(struct b43legacy_hwtxstatus *)skb->data;
+		int i = 0;
+
+		while (hw->cookie == 0) {
+			if (i > 100)
+				break;
+			i++;
+			udelay(2);
+			barrier();
+		}
+		b43legacy_handle_hwtxstatus(ring->dev, hw);
+		/* recycle the descriptor buffer. */
+		sync_descbuffer_for_device(ring, meta->dmaaddr,
+					   ring->rx_buffersize);
+
+		return;
+	}
+	rxhdr = (struct b43legacy_rxhdr_fw3 *)skb->data;
+	len = le16_to_cpu(rxhdr->frame_len);
+	if (len == 0) {
+		int i = 0;
+
+		do {
+			udelay(2);
+			barrier();
+			len = le16_to_cpu(rxhdr->frame_len);
+		} while (len == 0 && i++ < 5);
+		if (unlikely(len == 0)) {
+			/* recycle the descriptor buffer. */
+			sync_descbuffer_for_device(ring, meta->dmaaddr,
+						   ring->rx_buffersize);
+			goto drop;
+		}
+	}
+	if (unlikely(len > ring->rx_buffersize)) {
+		/* The data did not fit into one descriptor buffer
+		 * and is split over multiple buffers.
+		 * This should never happen, as we try to allocate buffers
+		 * big enough. So simply ignore this packet.
+		 */
+		int cnt = 0;
+		s32 tmp = len;
+
+		while (1) {
+			desc = ops->idx2desc(ring, *slot, &meta);
+			/* recycle the descriptor buffer. */
+			sync_descbuffer_for_device(ring, meta->dmaaddr,
+						   ring->rx_buffersize);
+			*slot = next_slot(ring, *slot);
+			cnt++;
+			tmp -= ring->rx_buffersize;
+			if (tmp <= 0)
+				break;
+		}
+		b43legacyerr(ring->dev->wl, "DMA RX buffer too small "
+		       "(len: %u, buffer: %u, nr-dropped: %d)\n",
+		       len, ring->rx_buffersize, cnt);
+		goto drop;
+	}
+
+	dmaaddr = meta->dmaaddr;
+	err = setup_rx_descbuffer(ring, desc, meta, GFP_ATOMIC);
+	if (unlikely(err)) {
+		b43legacydbg(ring->dev->wl, "DMA RX: setup_rx_descbuffer()"
+			     " failed\n");
+		sync_descbuffer_for_device(ring, dmaaddr,
+					   ring->rx_buffersize);
+		goto drop;
+	}
+
+	unmap_descbuffer(ring, dmaaddr, ring->rx_buffersize, 0);
+	skb_put(skb, len + ring->frameoffset);
+	skb_pull(skb, ring->frameoffset);
+
+	b43legacy_rx(ring->dev, skb, rxhdr);
+drop:
+	return;
+}
+
+void b43legacy_dma_rx(struct b43legacy_dmaring *ring)
+{
+	const struct b43legacy_dma_ops *ops = ring->ops;
+	int slot;
+	int current_slot;
+	int used_slots = 0;
+
+	B43legacy_WARN_ON(ring->tx);
+	current_slot = ops->get_current_rxslot(ring);
+	B43legacy_WARN_ON(!(current_slot >= 0 && current_slot <
+			   ring->nr_slots));
+
+	slot = ring->current_slot;
+	for (; slot != current_slot; slot = next_slot(ring, slot)) {
+		dma_rx(ring, &slot);
+		update_max_used_slots(ring, ++used_slots);
+	}
+	ops->set_current_rxslot(ring, slot);
+	ring->current_slot = slot;
+}
+
+static void b43legacy_dma_tx_suspend_ring(struct b43legacy_dmaring *ring)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&ring->lock, flags);
+	B43legacy_WARN_ON(!ring->tx);
+	ring->ops->tx_suspend(ring);
+	spin_unlock_irqrestore(&ring->lock, flags);
+}
+
+static void b43legacy_dma_tx_resume_ring(struct b43legacy_dmaring *ring)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&ring->lock, flags);
+	B43legacy_WARN_ON(!ring->tx);
+	ring->ops->tx_resume(ring);
+	spin_unlock_irqrestore(&ring->lock, flags);
+}
+
+void b43legacy_dma_tx_suspend(struct b43legacy_wldev *dev)
+{
+	b43legacy_power_saving_ctl_bits(dev, -1, 1);
+	b43legacy_dma_tx_suspend_ring(dev->dma.tx_ring0);
+	b43legacy_dma_tx_suspend_ring(dev->dma.tx_ring1);
+	b43legacy_dma_tx_suspend_ring(dev->dma.tx_ring2);
+	b43legacy_dma_tx_suspend_ring(dev->dma.tx_ring3);
+	b43legacy_dma_tx_suspend_ring(dev->dma.tx_ring4);
+	b43legacy_dma_tx_suspend_ring(dev->dma.tx_ring5);
+}
+
+void b43legacy_dma_tx_resume(struct b43legacy_wldev *dev)
+{
+	b43legacy_dma_tx_resume_ring(dev->dma.tx_ring5);
+	b43legacy_dma_tx_resume_ring(dev->dma.tx_ring4);
+	b43legacy_dma_tx_resume_ring(dev->dma.tx_ring3);
+	b43legacy_dma_tx_resume_ring(dev->dma.tx_ring2);
+	b43legacy_dma_tx_resume_ring(dev->dma.tx_ring1);
+	b43legacy_dma_tx_resume_ring(dev->dma.tx_ring0);
+	b43legacy_power_saving_ctl_bits(dev, -1, -1);
+}
diff --git a/drivers/net/wireless/b43legacy/dma.h b/drivers/net/wireless/b43legacy/dma.h
new file mode 100644
index 000000000000..26f6ab08de75
--- /dev/null
+++ b/drivers/net/wireless/b43legacy/dma.h
@@ -0,0 +1,367 @@
+#ifndef B43legacy_DMA_H_
+#define B43legacy_DMA_H_
+
+#include <linux/list.h>
+#include <linux/spinlock.h>
+#include <linux/workqueue.h>
+#include <linux/linkage.h>
+#include <asm/atomic.h>
+
+#include "b43legacy.h"
+
+
+/* DMA-Interrupt reasons. */
+#define B43legacy_DMAIRQ_FATALMASK	((1 << 10) | (1 << 11) | (1 << 12) \
+					 | (1 << 14) | (1 << 15))
+#define B43legacy_DMAIRQ_NONFATALMASK	(1 << 13)
+#define B43legacy_DMAIRQ_RX_DONE		(1 << 16)
+
+
+/*** 32-bit DMA Engine. ***/
+
+/* 32-bit DMA controller registers. */
+#define B43legacy_DMA32_TXCTL				0x00
+#define		B43legacy_DMA32_TXENABLE		0x00000001
+#define		B43legacy_DMA32_TXSUSPEND		0x00000002
+#define		B43legacy_DMA32_TXLOOPBACK		0x00000004
+#define		B43legacy_DMA32_TXFLUSH			0x00000010
+#define		B43legacy_DMA32_TXADDREXT_MASK		0x00030000
+#define		B43legacy_DMA32_TXADDREXT_SHIFT		16
+#define B43legacy_DMA32_TXRING				0x04
+#define B43legacy_DMA32_TXINDEX				0x08
+#define B43legacy_DMA32_TXSTATUS			0x0C
+#define		B43legacy_DMA32_TXDPTR			0x00000FFF
+#define		B43legacy_DMA32_TXSTATE			0x0000F000
+#define			B43legacy_DMA32_TXSTAT_DISABLED	0x00000000
+#define			B43legacy_DMA32_TXSTAT_ACTIVE	0x00001000
+#define			B43legacy_DMA32_TXSTAT_IDLEWAIT	0x00002000
+#define			B43legacy_DMA32_TXSTAT_STOPPED	0x00003000
+#define			B43legacy_DMA32_TXSTAT_SUSP	0x00004000
+#define		B43legacy_DMA32_TXERROR			0x000F0000
+#define			B43legacy_DMA32_TXERR_NOERR	0x00000000
+#define			B43legacy_DMA32_TXERR_PROT	0x00010000
+#define			B43legacy_DMA32_TXERR_UNDERRUN	0x00020000
+#define			B43legacy_DMA32_TXERR_BUFREAD	0x00030000
+#define			B43legacy_DMA32_TXERR_DESCREAD	0x00040000
+#define		B43legacy_DMA32_TXACTIVE		0xFFF00000
+#define B43legacy_DMA32_RXCTL				0x10
+#define		B43legacy_DMA32_RXENABLE		0x00000001
+#define		B43legacy_DMA32_RXFROFF_MASK		0x000000FE
+#define		B43legacy_DMA32_RXFROFF_SHIFT		1
+#define		B43legacy_DMA32_RXDIRECTFIFO		0x00000100
+#define		B43legacy_DMA32_RXADDREXT_MASK		0x00030000
+#define		B43legacy_DMA32_RXADDREXT_SHIFT		16
+#define B43legacy_DMA32_RXRING				0x14
+#define B43legacy_DMA32_RXINDEX				0x18
+#define B43legacy_DMA32_RXSTATUS			0x1C
+#define		B43legacy_DMA32_RXDPTR			0x00000FFF
+#define		B43legacy_DMA32_RXSTATE			0x0000F000
+#define			B43legacy_DMA32_RXSTAT_DISABLED	0x00000000
+#define			B43legacy_DMA32_RXSTAT_ACTIVE	0x00001000
+#define			B43legacy_DMA32_RXSTAT_IDLEWAIT	0x00002000
+#define			B43legacy_DMA32_RXSTAT_STOPPED	0x00003000
+#define		B43legacy_DMA32_RXERROR			0x000F0000
+#define			B43legacy_DMA32_RXERR_NOERR	0x00000000
+#define			B43legacy_DMA32_RXERR_PROT	0x00010000
+#define			B43legacy_DMA32_RXERR_OVERFLOW	0x00020000
+#define			B43legacy_DMA32_RXERR_BUFWRITE	0x00030000
+#define			B43legacy_DMA32_RXERR_DESCREAD	0x00040000
+#define		B43legacy_DMA32_RXACTIVE		0xFFF00000
+
+/* 32-bit DMA descriptor. */
+struct b43legacy_dmadesc32 {
+	__le32 control;
+	__le32 address;
+} __attribute__((__packed__));
+#define B43legacy_DMA32_DCTL_BYTECNT		0x00001FFF
+#define B43legacy_DMA32_DCTL_ADDREXT_MASK	0x00030000
+#define B43legacy_DMA32_DCTL_ADDREXT_SHIFT	16
+#define B43legacy_DMA32_DCTL_DTABLEEND		0x10000000
+#define B43legacy_DMA32_DCTL_IRQ		0x20000000
+#define B43legacy_DMA32_DCTL_FRAMEEND		0x40000000
+#define B43legacy_DMA32_DCTL_FRAMESTART		0x80000000
+
+
+
+/*** 64-bit DMA Engine. ***/
+
+/* 64-bit DMA controller registers. */
+#define B43legacy_DMA64_TXCTL				0x00
+#define		B43legacy_DMA64_TXENABLE		0x00000001
+#define		B43legacy_DMA64_TXSUSPEND		0x00000002
+#define		B43legacy_DMA64_TXLOOPBACK		0x00000004
+#define		B43legacy_DMA64_TXFLUSH			0x00000010
+#define		B43legacy_DMA64_TXADDREXT_MASK		0x00030000
+#define		B43legacy_DMA64_TXADDREXT_SHIFT		16
+#define B43legacy_DMA64_TXINDEX				0x04
+#define B43legacy_DMA64_TXRINGLO			0x08
+#define B43legacy_DMA64_TXRINGHI			0x0C
+#define B43legacy_DMA64_TXSTATUS			0x10
+#define		B43legacy_DMA64_TXSTATDPTR		0x00001FFF
+#define		B43legacy_DMA64_TXSTAT			0xF0000000
+#define			B43legacy_DMA64_TXSTAT_DISABLED	0x00000000
+#define			B43legacy_DMA64_TXSTAT_ACTIVE	0x10000000
+#define			B43legacy_DMA64_TXSTAT_IDLEWAIT	0x20000000
+#define			B43legacy_DMA64_TXSTAT_STOPPED	0x30000000
+#define			B43legacy_DMA64_TXSTAT_SUSP	0x40000000
+#define B43legacy_DMA64_TXERROR				0x14
+#define		B43legacy_DMA64_TXERRDPTR		0x0001FFFF
+#define		B43legacy_DMA64_TXERR			0xF0000000
+#define			B43legacy_DMA64_TXERR_NOERR	0x00000000
+#define			B43legacy_DMA64_TXERR_PROT	0x10000000
+#define			B43legacy_DMA64_TXERR_UNDERRUN	0x20000000
+#define			B43legacy_DMA64_TXERR_TRANSFER	0x30000000
+#define			B43legacy_DMA64_TXERR_DESCREAD	0x40000000
+#define			B43legacy_DMA64_TXERR_CORE	0x50000000
+#define B43legacy_DMA64_RXCTL				0x20
+#define		B43legacy_DMA64_RXENABLE		0x00000001
+#define		B43legacy_DMA64_RXFROFF_MASK		0x000000FE
+#define		B43legacy_DMA64_RXFROFF_SHIFT		1
+#define		B43legacy_DMA64_RXDIRECTFIFO		0x00000100
+#define		B43legacy_DMA64_RXADDREXT_MASK		0x00030000
+#define		B43legacy_DMA64_RXADDREXT_SHIFT		16
+#define B43legacy_DMA64_RXINDEX				0x24
+#define B43legacy_DMA64_RXRINGLO			0x28
+#define B43legacy_DMA64_RXRINGHI			0x2C
+#define B43legacy_DMA64_RXSTATUS			0x30
+#define		B43legacy_DMA64_RXSTATDPTR		0x00001FFF
+#define		B43legacy_DMA64_RXSTAT			0xF0000000
+#define			B43legacy_DMA64_RXSTAT_DISABLED	0x00000000
+#define			B43legacy_DMA64_RXSTAT_ACTIVE	0x10000000
+#define			B43legacy_DMA64_RXSTAT_IDLEWAIT	0x20000000
+#define			B43legacy_DMA64_RXSTAT_STOPPED	0x30000000
+#define			B43legacy_DMA64_RXSTAT_SUSP	0x40000000
+#define B43legacy_DMA64_RXERROR				0x34
+#define		B43legacy_DMA64_RXERRDPTR		0x0001FFFF
+#define		B43legacy_DMA64_RXERR			0xF0000000
+#define			B43legacy_DMA64_RXERR_NOERR	0x00000000
+#define			B43legacy_DMA64_RXERR_PROT	0x10000000
+#define			B43legacy_DMA64_RXERR_UNDERRUN	0x20000000
+#define			B43legacy_DMA64_RXERR_TRANSFER	0x30000000
+#define			B43legacy_DMA64_RXERR_DESCREAD	0x40000000
+#define			B43legacy_DMA64_RXERR_CORE	0x50000000
+
+/* 64-bit DMA descriptor. */
+struct b43legacy_dmadesc64 {
+	__le32 control0;
+	__le32 control1;
+	__le32 address_low;
+	__le32 address_high;
+} __attribute__((__packed__));
+#define B43legacy_DMA64_DCTL0_DTABLEEND		0x10000000
+#define B43legacy_DMA64_DCTL0_IRQ		0x20000000
+#define B43legacy_DMA64_DCTL0_FRAMEEND		0x40000000
+#define B43legacy_DMA64_DCTL0_FRAMESTART	0x80000000
+#define B43legacy_DMA64_DCTL1_BYTECNT		0x00001FFF
+#define B43legacy_DMA64_DCTL1_ADDREXT_MASK	0x00030000
+#define B43legacy_DMA64_DCTL1_ADDREXT_SHIFT	16
+
+
+
+struct b43legacy_dmadesc_generic {
+	union {
+		struct b43legacy_dmadesc32 dma32;
+		struct b43legacy_dmadesc64 dma64;
+	} __attribute__((__packed__));
+} __attribute__((__packed__));
+
+
+/* Misc DMA constants */
+#define B43legacy_DMA_RINGMEMSIZE	PAGE_SIZE
+#define B43legacy_DMA0_RX_FRAMEOFFSET	30
+#define B43legacy_DMA3_RX_FRAMEOFFSET	0
+
+
+/* DMA engine tuning knobs */
+#define B43legacy_TXRING_SLOTS		128
+#define B43legacy_RXRING_SLOTS		64
+#define B43legacy_DMA0_RX_BUFFERSIZE	(2304 + 100)
+#define B43legacy_DMA3_RX_BUFFERSIZE	16
+
+
+
+#ifdef CONFIG_B43LEGACY_DMA
+
+
+struct sk_buff;
+struct b43legacy_private;
+struct b43legacy_txstatus;
+
+
+struct b43legacy_dmadesc_meta {
+	/* The kernel DMA-able buffer. */
+	struct sk_buff *skb;
+	/* DMA base bus-address of the descriptor buffer. */
+	dma_addr_t dmaaddr;
+	/* ieee80211 TX status. Only used once per 802.11 frag. */
+	bool is_last_fragment;
+	struct ieee80211_tx_status txstat;
+};
+
+struct b43legacy_dmaring;
+
+/* Lowlevel DMA operations that differ between 32bit and 64bit DMA. */
+struct b43legacy_dma_ops {
+	struct b43legacy_dmadesc_generic * (*idx2desc)
+					   (struct b43legacy_dmaring *ring,
+					   int slot,
+					   struct b43legacy_dmadesc_meta
+					   **meta);
+	void (*fill_descriptor)(struct b43legacy_dmaring *ring,
+				struct b43legacy_dmadesc_generic *desc,
+				dma_addr_t dmaaddr, u16 bufsize,
+				int start, int end, int irq);
+	void (*poke_tx)(struct b43legacy_dmaring *ring, int slot);
+	void (*tx_suspend)(struct b43legacy_dmaring *ring);
+	void (*tx_resume)(struct b43legacy_dmaring *ring);
+	int (*get_current_rxslot)(struct b43legacy_dmaring *ring);
+	void (*set_current_rxslot)(struct b43legacy_dmaring *ring, int slot);
+};
+
+struct b43legacy_dmaring {
+	/* Lowlevel DMA ops. */
+	const struct b43legacy_dma_ops *ops;
+	/* Kernel virtual base address of the ring memory. */
+	void *descbase;
+	/* Meta data about all descriptors. */
+	struct b43legacy_dmadesc_meta *meta;
+	/* Cache of TX headers for each slot.
+	 * This is to avoid an allocation on each TX.
+	 * This is NULL for an RX ring.
+	 */
+	u8 *txhdr_cache;
+	/* (Unadjusted) DMA base bus-address of the ring memory. */
+	dma_addr_t dmabase;
+	/* Number of descriptor slots in the ring. */
+	int nr_slots;
+	/* Number of used descriptor slots. */
+	int used_slots;
+	/* Currently used slot in the ring. */
+	int current_slot;
+	/* Total number of packets sent. Statistics only. */
+	unsigned int nr_tx_packets;
+	/* Frameoffset in octets. */
+	u32 frameoffset;
+	/* Descriptor buffer size. */
+	u16 rx_buffersize;
+	/* The MMIO base register of the DMA controller. */
+	u16 mmio_base;
+	/* DMA controller index number (0-5). */
+	int index;
+	/* Boolean. Is this a TX ring? */
+	bool tx;
+	/* Boolean. 64bit DMA if true, 32bit DMA otherwise. */
+	bool dma64;
+	/* Boolean. Is this ring stopped at ieee80211 level? */
+	bool stopped;
+	/* Lock, only used for TX. */
+	spinlock_t lock;
+	struct b43legacy_wldev *dev;
+#ifdef CONFIG_B43LEGACY_DEBUG
+	/* Maximum number of used slots. */
+	int max_used_slots;
+	/* Last time we injected a ring overflow. */
+	unsigned long last_injected_overflow;
+#endif /* CONFIG_B43LEGACY_DEBUG*/
+};
+
+
+static inline
+u32 b43legacy_dma_read(struct b43legacy_dmaring *ring,
+		       u16 offset)
+{
+	return b43legacy_read32(ring->dev, ring->mmio_base + offset);
+}
+
+static inline
+void b43legacy_dma_write(struct b43legacy_dmaring *ring,
+			 u16 offset, u32 value)
+{
+	b43legacy_write32(ring->dev, ring->mmio_base + offset, value);
+}
+
+
+int b43legacy_dma_init(struct b43legacy_wldev *dev);
+void b43legacy_dma_free(struct b43legacy_wldev *dev);
+
+int b43legacy_dmacontroller_rx_reset(struct b43legacy_wldev *dev,
+				     u16 dmacontroller_mmio_base,
+				     int dma64);
+int b43legacy_dmacontroller_tx_reset(struct b43legacy_wldev *dev,
+				     u16 dmacontroller_mmio_base,
+				     int dma64);
+
+u16 b43legacy_dmacontroller_base(int dma64bit, int dmacontroller_idx);
+
+void b43legacy_dma_tx_suspend(struct b43legacy_wldev *dev);
+void b43legacy_dma_tx_resume(struct b43legacy_wldev *dev);
+
+void b43legacy_dma_get_tx_stats(struct b43legacy_wldev *dev,
+				struct ieee80211_tx_queue_stats *stats);
+
+int b43legacy_dma_tx(struct b43legacy_wldev *dev,
+		     struct sk_buff *skb,
+		     struct ieee80211_tx_control *ctl);
+void b43legacy_dma_handle_txstatus(struct b43legacy_wldev *dev,
+				   const struct b43legacy_txstatus *status);
+
+void b43legacy_dma_rx(struct b43legacy_dmaring *ring);
+
+#else /* CONFIG_B43LEGACY_DMA */
+
+
+static inline
+int b43legacy_dma_init(struct b43legacy_wldev *dev)
+{
+	return 0;
+}
+static inline
+void b43legacy_dma_free(struct b43legacy_wldev *dev)
+{
+}
+static inline
+int b43legacy_dmacontroller_rx_reset(struct b43legacy_wldev *dev,
+				     u16 dmacontroller_mmio_base,
+				     int dma64)
+{
+	return 0;
+}
+static inline
+int b43legacy_dmacontroller_tx_reset(struct b43legacy_wldev *dev,
+				     u16 dmacontroller_mmio_base,
+				     int dma64)
+{
+	return 0;
+}
+static inline
+void b43legacy_dma_get_tx_stats(struct b43legacy_wldev *dev,
+				struct ieee80211_tx_queue_stats *stats)
+{
+}
+static inline
+int b43legacy_dma_tx(struct b43legacy_wldev *dev,
+		     struct sk_buff *skb,
+		     struct ieee80211_tx_control *ctl)
+{
+	return 0;
+}
+static inline
+void b43legacy_dma_handle_txstatus(struct b43legacy_wldev *dev,
+				   const struct b43legacy_txstatus *status)
+{
+}
+static inline
+void b43legacy_dma_rx(struct b43legacy_dmaring *ring)
+{
+}
+static inline
+void b43legacy_dma_tx_suspend(struct b43legacy_wldev *dev)
+{
+}
+static inline
+void b43legacy_dma_tx_resume(struct b43legacy_wldev *dev)
+{
+}
+
+#endif /* CONFIG_B43LEGACY_DMA */
+#endif /* B43legacy_DMA_H_ */
diff --git a/drivers/net/wireless/b43legacy/ilt.c b/drivers/net/wireless/b43legacy/ilt.c
new file mode 100644
index 000000000000..247fc780ffdb
--- /dev/null
+++ b/drivers/net/wireless/b43legacy/ilt.c
@@ -0,0 +1,336 @@
+/*
+
+  Broadcom B43legacy wireless driver
+
+  Copyright (c) 2005 Martin Langer <martin-langer@gmx.de>,
+		     Stefano Brivio <st3@riseup.net>
+		     Michael Buesch <mbuesch@freenet.de>
+		     Danny van Dyk <kugelfang@gentoo.org>
+		     Andreas Jaggi <andreas.jaggi@waterwave.ch>
+
+  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, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with this program; see the file COPYING.  If not, write to
+  the Free Software Foundation, Inc., 51 Franklin Steet, Fifth Floor,
+  Boston, MA 02110-1301, USA.
+
+*/
+
+#include "b43legacy.h"
+#include "ilt.h"
+#include "phy.h"
+
+
+/**** Initial Internal Lookup Tables ****/
+
+const u32 b43legacy_ilt_rotor[B43legacy_ILT_ROTOR_SIZE] = {
+	0xFEB93FFD, 0xFEC63FFD, /* 0 */
+	0xFED23FFD, 0xFEDF3FFD,
+	0xFEEC3FFE, 0xFEF83FFE,
+	0xFF053FFE, 0xFF113FFE,
+	0xFF1E3FFE, 0xFF2A3FFF, /* 8 */
+	0xFF373FFF, 0xFF443FFF,
+	0xFF503FFF, 0xFF5D3FFF,
+	0xFF693FFF, 0xFF763FFF,
+	0xFF824000, 0xFF8F4000, /* 16 */
+	0xFF9B4000, 0xFFA84000,
+	0xFFB54000, 0xFFC14000,
+	0xFFCE4000, 0xFFDA4000,
+	0xFFE74000, 0xFFF34000, /* 24 */
+	0x00004000, 0x000D4000,
+	0x00194000, 0x00264000,
+	0x00324000, 0x003F4000,
+	0x004B4000, 0x00584000, /* 32 */
+	0x00654000, 0x00714000,
+	0x007E4000, 0x008A3FFF,
+	0x00973FFF, 0x00A33FFF,
+	0x00B03FFF, 0x00BC3FFF, /* 40 */
+	0x00C93FFF, 0x00D63FFF,
+	0x00E23FFE, 0x00EF3FFE,
+	0x00FB3FFE, 0x01083FFE,
+	0x01143FFE, 0x01213FFD, /* 48 */
+	0x012E3FFD, 0x013A3FFD,
+	0x01473FFD,
+};
+
+const u32 b43legacy_ilt_retard[B43legacy_ILT_RETARD_SIZE] = {
+	0xDB93CB87, 0xD666CF64, /* 0 */
+	0xD1FDD358, 0xCDA6D826,
+	0xCA38DD9F, 0xC729E2B4,
+	0xC469E88E, 0xC26AEE2B,
+	0xC0DEF46C, 0xC073FA62, /* 8 */
+	0xC01D00D5, 0xC0760743,
+	0xC1560D1E, 0xC2E51369,
+	0xC4ED18FF, 0xC7AC1ED7,
+	0xCB2823B2, 0xCEFA28D9, /* 16 */
+	0xD2F62D3F, 0xD7BB3197,
+	0xDCE53568, 0xE1FE3875,
+	0xE7D13B35, 0xED663D35,
+	0xF39B3EC4, 0xF98E3FA7, /* 24 */
+	0x00004000, 0x06723FA7,
+	0x0C653EC4, 0x129A3D35,
+	0x182F3B35, 0x1E023875,
+	0x231B3568, 0x28453197, /* 32 */
+	0x2D0A2D3F, 0x310628D9,
+	0x34D823B2, 0x38541ED7,
+	0x3B1318FF, 0x3D1B1369,
+	0x3EAA0D1E, 0x3F8A0743, /* 40 */
+	0x3FE300D5, 0x3F8DFA62,
+	0x3F22F46C, 0x3D96EE2B,
+	0x3B97E88E, 0x38D7E2B4,
+	0x35C8DD9F, 0x325AD826, /* 48 */
+	0x2E03D358, 0x299ACF64,
+	0x246DCB87,
+};
+
+const u16 b43legacy_ilt_finefreqa[B43legacy_ILT_FINEFREQA_SIZE] = {
+	0x0082, 0x0082, 0x0102, 0x0182, /* 0 */
+	0x0202, 0x0282, 0x0302, 0x0382,
+	0x0402, 0x0482, 0x0502, 0x0582,
+	0x05E2, 0x0662, 0x06E2, 0x0762,
+	0x07E2, 0x0842, 0x08C2, 0x0942, /* 16 */
+	0x09C2, 0x0A22, 0x0AA2, 0x0B02,
+	0x0B82, 0x0BE2, 0x0C62, 0x0CC2,
+	0x0D42, 0x0DA2, 0x0E02, 0x0E62,
+	0x0EE2, 0x0F42, 0x0FA2, 0x1002, /* 32 */
+	0x1062, 0x10C2, 0x1122, 0x1182,
+	0x11E2, 0x1242, 0x12A2, 0x12E2,
+	0x1342, 0x13A2, 0x1402, 0x1442,
+	0x14A2, 0x14E2, 0x1542, 0x1582, /* 48 */
+	0x15E2, 0x1622, 0x1662, 0x16C1,
+	0x1701, 0x1741, 0x1781, 0x17E1,
+	0x1821, 0x1861, 0x18A1, 0x18E1,
+	0x1921, 0x1961, 0x19A1, 0x19E1, /* 64 */
+	0x1A21, 0x1A61, 0x1AA1, 0x1AC1,
+	0x1B01, 0x1B41, 0x1B81, 0x1BA1,
+	0x1BE1, 0x1C21, 0x1C41, 0x1C81,
+	0x1CA1, 0x1CE1, 0x1D01, 0x1D41, /* 80 */
+	0x1D61, 0x1DA1, 0x1DC1, 0x1E01,
+	0x1E21, 0x1E61, 0x1E81, 0x1EA1,
+	0x1EE1, 0x1F01, 0x1F21, 0x1F41,
+	0x1F81, 0x1FA1, 0x1FC1, 0x1FE1, /* 96 */
+	0x2001, 0x2041, 0x2061, 0x2081,
+	0x20A1, 0x20C1, 0x20E1, 0x2101,
+	0x2121, 0x2141, 0x2161, 0x2181,
+	0x21A1, 0x21C1, 0x21E1, 0x2201, /* 112 */
+	0x2221, 0x2241, 0x2261, 0x2281,
+	0x22A1, 0x22C1, 0x22C1, 0x22E1,
+	0x2301, 0x2321, 0x2341, 0x2361,
+	0x2361, 0x2381, 0x23A1, 0x23C1, /* 128 */
+	0x23E1, 0x23E1, 0x2401, 0x2421,
+	0x2441, 0x2441, 0x2461, 0x2481,
+	0x2481, 0x24A1, 0x24C1, 0x24C1,
+	0x24E1, 0x2501, 0x2501, 0x2521, /* 144 */
+	0x2541, 0x2541, 0x2561, 0x2561,
+	0x2581, 0x25A1, 0x25A1, 0x25C1,
+	0x25C1, 0x25E1, 0x2601, 0x2601,
+	0x2621, 0x2621, 0x2641, 0x2641, /* 160 */
+	0x2661, 0x2661, 0x2681, 0x2681,
+	0x26A1, 0x26A1, 0x26C1, 0x26C1,
+	0x26E1, 0x26E1, 0x2701, 0x2701,
+	0x2721, 0x2721, 0x2740, 0x2740, /* 176 */
+	0x2760, 0x2760, 0x2780, 0x2780,
+	0x2780, 0x27A0, 0x27A0, 0x27C0,
+	0x27C0, 0x27E0, 0x27E0, 0x27E0,
+	0x2800, 0x2800, 0x2820, 0x2820, /* 192 */
+	0x2820, 0x2840, 0x2840, 0x2840,
+	0x2860, 0x2860, 0x2880, 0x2880,
+	0x2880, 0x28A0, 0x28A0, 0x28A0,
+	0x28C0, 0x28C0, 0x28C0, 0x28E0, /* 208 */
+	0x28E0, 0x28E0, 0x2900, 0x2900,
+	0x2900, 0x2920, 0x2920, 0x2920,
+	0x2940, 0x2940, 0x2940, 0x2960,
+	0x2960, 0x2960, 0x2960, 0x2980, /* 224 */
+	0x2980, 0x2980, 0x29A0, 0x29A0,
+	0x29A0, 0x29A0, 0x29C0, 0x29C0,
+	0x29C0, 0x29E0, 0x29E0, 0x29E0,
+	0x29E0, 0x2A00, 0x2A00, 0x2A00, /* 240 */
+	0x2A00, 0x2A20, 0x2A20, 0x2A20,
+	0x2A20, 0x2A40, 0x2A40, 0x2A40,
+	0x2A40, 0x2A60, 0x2A60, 0x2A60,
+};
+
+const u16 b43legacy_ilt_finefreqg[B43legacy_ILT_FINEFREQG_SIZE] = {
+	0x0089, 0x02E9, 0x0409, 0x04E9, /* 0 */
+	0x05A9, 0x0669, 0x0709, 0x0789,
+	0x0829, 0x08A9, 0x0929, 0x0989,
+	0x0A09, 0x0A69, 0x0AC9, 0x0B29,
+	0x0BA9, 0x0BE9, 0x0C49, 0x0CA9, /* 16 */
+	0x0D09, 0x0D69, 0x0DA9, 0x0E09,
+	0x0E69, 0x0EA9, 0x0F09, 0x0F49,
+	0x0FA9, 0x0FE9, 0x1029, 0x1089,
+	0x10C9, 0x1109, 0x1169, 0x11A9, /* 32 */
+	0x11E9, 0x1229, 0x1289, 0x12C9,
+	0x1309, 0x1349, 0x1389, 0x13C9,
+	0x1409, 0x1449, 0x14A9, 0x14E9,
+	0x1529, 0x1569, 0x15A9, 0x15E9, /* 48 */
+	0x1629, 0x1669, 0x16A9, 0x16E8,
+	0x1728, 0x1768, 0x17A8, 0x17E8,
+	0x1828, 0x1868, 0x18A8, 0x18E8,
+	0x1928, 0x1968, 0x19A8, 0x19E8, /* 64 */
+	0x1A28, 0x1A68, 0x1AA8, 0x1AE8,
+	0x1B28, 0x1B68, 0x1BA8, 0x1BE8,
+	0x1C28, 0x1C68, 0x1CA8, 0x1CE8,
+	0x1D28, 0x1D68, 0x1DC8, 0x1E08, /* 80 */
+	0x1E48, 0x1E88, 0x1EC8, 0x1F08,
+	0x1F48, 0x1F88, 0x1FE8, 0x2028,
+	0x2068, 0x20A8, 0x2108, 0x2148,
+	0x2188, 0x21C8, 0x2228, 0x2268, /* 96 */
+	0x22C8, 0x2308, 0x2348, 0x23A8,
+	0x23E8, 0x2448, 0x24A8, 0x24E8,
+	0x2548, 0x25A8, 0x2608, 0x2668,
+	0x26C8, 0x2728, 0x2787, 0x27E7, /* 112 */
+	0x2847, 0x28C7, 0x2947, 0x29A7,
+	0x2A27, 0x2AC7, 0x2B47, 0x2BE7,
+	0x2CA7, 0x2D67, 0x2E47, 0x2F67,
+	0x3247, 0x3526, 0x3646, 0x3726, /* 128 */
+	0x3806, 0x38A6, 0x3946, 0x39E6,
+	0x3A66, 0x3AE6, 0x3B66, 0x3BC6,
+	0x3C45, 0x3CA5, 0x3D05, 0x3D85,
+	0x3DE5, 0x3E45, 0x3EA5, 0x3EE5, /* 144 */
+	0x3F45, 0x3FA5, 0x4005, 0x4045,
+	0x40A5, 0x40E5, 0x4145, 0x4185,
+	0x41E5, 0x4225, 0x4265, 0x42C5,
+	0x4305, 0x4345, 0x43A5, 0x43E5, /* 160 */
+	0x4424, 0x4464, 0x44C4, 0x4504,
+	0x4544, 0x4584, 0x45C4, 0x4604,
+	0x4644, 0x46A4, 0x46E4, 0x4724,
+	0x4764, 0x47A4, 0x47E4, 0x4824, /* 176 */
+	0x4864, 0x48A4, 0x48E4, 0x4924,
+	0x4964, 0x49A4, 0x49E4, 0x4A24,
+	0x4A64, 0x4AA4, 0x4AE4, 0x4B23,
+	0x4B63, 0x4BA3, 0x4BE3, 0x4C23, /* 192 */
+	0x4C63, 0x4CA3, 0x4CE3, 0x4D23,
+	0x4D63, 0x4DA3, 0x4DE3, 0x4E23,
+	0x4E63, 0x4EA3, 0x4EE3, 0x4F23,
+	0x4F63, 0x4FC3, 0x5003, 0x5043, /* 208 */
+	0x5083, 0x50C3, 0x5103, 0x5143,
+	0x5183, 0x51E2, 0x5222, 0x5262,
+	0x52A2, 0x52E2, 0x5342, 0x5382,
+	0x53C2, 0x5402, 0x5462, 0x54A2, /* 224 */
+	0x5502, 0x5542, 0x55A2, 0x55E2,
+	0x5642, 0x5682, 0x56E2, 0x5722,
+	0x5782, 0x57E1, 0x5841, 0x58A1,
+	0x5901, 0x5961, 0x59C1, 0x5A21, /* 240 */
+	0x5AA1, 0x5B01, 0x5B81, 0x5BE1,
+	0x5C61, 0x5D01, 0x5D80, 0x5E20,
+	0x5EE0, 0x5FA0, 0x6080, 0x61C0,
+};
+
+const u16 b43legacy_ilt_noisea2[B43legacy_ILT_NOISEA2_SIZE] = {
+	0x0001, 0x0001, 0x0001, 0xFFFE,
+	0xFFFE, 0x3FFF, 0x1000, 0x0393,
+};
+
+const u16 b43legacy_ilt_noisea3[B43legacy_ILT_NOISEA3_SIZE] = {
+	0x4C4C, 0x4C4C, 0x4C4C, 0x2D36,
+	0x4C4C, 0x4C4C, 0x4C4C, 0x2D36,
+};
+
+const u16 b43legacy_ilt_noiseg1[B43legacy_ILT_NOISEG1_SIZE] = {
+	0x013C, 0x01F5, 0x031A, 0x0631,
+	0x0001, 0x0001, 0x0001, 0x0001,
+};
+
+const u16 b43legacy_ilt_noiseg2[B43legacy_ILT_NOISEG2_SIZE] = {
+	0x5484, 0x3C40, 0x0000, 0x0000,
+	0x0000, 0x0000, 0x0000, 0x0000,
+};
+
+const u16 b43legacy_ilt_noisescaleg1[B43legacy_ILT_NOISESCALEG_SIZE] = {
+	0x6C77, 0x5162, 0x3B40, 0x3335, /* 0 */
+	0x2F2D, 0x2A2A, 0x2527, 0x1F21,
+	0x1A1D, 0x1719, 0x1616, 0x1414,
+	0x1414, 0x1400, 0x1414, 0x1614,
+	0x1716, 0x1A19, 0x1F1D, 0x2521, /* 16 */
+	0x2A27, 0x2F2A, 0x332D, 0x3B35,
+	0x5140, 0x6C62, 0x0077,
+};
+
+const u16 b43legacy_ilt_noisescaleg2[B43legacy_ILT_NOISESCALEG_SIZE] = {
+	0xD8DD, 0xCBD4, 0xBCC0, 0XB6B7, /* 0 */
+	0xB2B0, 0xADAD, 0xA7A9, 0x9FA1,
+	0x969B, 0x9195, 0x8F8F, 0x8A8A,
+	0x8A8A, 0x8A00, 0x8A8A, 0x8F8A,
+	0x918F, 0x9695, 0x9F9B, 0xA7A1, /* 16 */
+	0xADA9, 0xB2AD, 0xB6B0, 0xBCB7,
+	0xCBC0, 0xD8D4, 0x00DD,
+};
+
+const u16 b43legacy_ilt_noisescaleg3[B43legacy_ILT_NOISESCALEG_SIZE] = {
+	0xA4A4, 0xA4A4, 0xA4A4, 0xA4A4, /* 0 */
+	0xA4A4, 0xA4A4, 0xA4A4, 0xA4A4,
+	0xA4A4, 0xA4A4, 0xA4A4, 0xA4A4,
+	0xA4A4, 0xA400, 0xA4A4, 0xA4A4,
+	0xA4A4, 0xA4A4, 0xA4A4, 0xA4A4, /* 16 */
+	0xA4A4, 0xA4A4, 0xA4A4, 0xA4A4,
+	0xA4A4, 0xA4A4, 0x00A4,
+};
+
+const u16 b43legacy_ilt_sigmasqr1[B43legacy_ILT_SIGMASQR_SIZE] = {
+	0x007A, 0x0075, 0x0071, 0x006C, /* 0 */
+	0x0067, 0x0063, 0x005E, 0x0059,
+	0x0054, 0x0050, 0x004B, 0x0046,
+	0x0042, 0x003D, 0x003D, 0x003D,
+	0x003D, 0x003D, 0x003D, 0x003D, /* 16 */
+	0x003D, 0x003D, 0x003D, 0x003D,
+	0x003D, 0x003D, 0x0000, 0x003D,
+	0x003D, 0x003D, 0x003D, 0x003D,
+	0x003D, 0x003D, 0x003D, 0x003D, /* 32 */
+	0x003D, 0x003D, 0x003D, 0x003D,
+	0x0042, 0x0046, 0x004B, 0x0050,
+	0x0054, 0x0059, 0x005E, 0x0063,
+	0x0067, 0x006C, 0x0071, 0x0075, /* 48 */
+	0x007A,
+};
+
+const u16 b43legacy_ilt_sigmasqr2[B43legacy_ILT_SIGMASQR_SIZE] = {
+	0x00DE, 0x00DC, 0x00DA, 0x00D8, /* 0 */
+	0x00D6, 0x00D4, 0x00D2, 0x00CF,
+	0x00CD, 0x00CA, 0x00C7, 0x00C4,
+	0x00C1, 0x00BE, 0x00BE, 0x00BE,
+	0x00BE, 0x00BE, 0x00BE, 0x00BE, /* 16 */
+	0x00BE, 0x00BE, 0x00BE, 0x00BE,
+	0x00BE, 0x00BE, 0x0000, 0x00BE,
+	0x00BE, 0x00BE, 0x00BE, 0x00BE,
+	0x00BE, 0x00BE, 0x00BE, 0x00BE, /* 32 */
+	0x00BE, 0x00BE, 0x00BE, 0x00BE,
+	0x00C1, 0x00C4, 0x00C7, 0x00CA,
+	0x00CD, 0x00CF, 0x00D2, 0x00D4,
+	0x00D6, 0x00D8, 0x00DA, 0x00DC, /* 48 */
+	0x00DE,
+};
+
+/**** Helper functions to access the device Internal Lookup Tables ****/
+
+void b43legacy_ilt_write(struct b43legacy_wldev *dev, u16 offset, u16 val)
+{
+	b43legacy_phy_write(dev, B43legacy_PHY_ILT_G_CTRL, offset);
+	mmiowb();
+	b43legacy_phy_write(dev, B43legacy_PHY_ILT_G_DATA1, val);
+}
+
+void b43legacy_ilt_write32(struct b43legacy_wldev *dev, u16 offset, u32 val)
+{
+	b43legacy_phy_write(dev, B43legacy_PHY_ILT_G_CTRL, offset);
+	mmiowb();
+	b43legacy_phy_write(dev, B43legacy_PHY_ILT_G_DATA2,
+			    (val & 0xFFFF0000) >> 16);
+	b43legacy_phy_write(dev, B43legacy_PHY_ILT_G_DATA1,
+			    val & 0x0000FFFF);
+}
+
+u16 b43legacy_ilt_read(struct b43legacy_wldev *dev, u16 offset)
+{
+	b43legacy_phy_write(dev, B43legacy_PHY_ILT_G_CTRL, offset);
+	return b43legacy_phy_read(dev, B43legacy_PHY_ILT_G_DATA1);
+}
diff --git a/drivers/net/wireless/b43legacy/ilt.h b/drivers/net/wireless/b43legacy/ilt.h
new file mode 100644
index 000000000000..48bcf37eccb8
--- /dev/null
+++ b/drivers/net/wireless/b43legacy/ilt.h
@@ -0,0 +1,34 @@
+#ifndef B43legacy_ILT_H_
+#define B43legacy_ILT_H_
+
+#define B43legacy_ILT_ROTOR_SIZE	53
+extern const u32 b43legacy_ilt_rotor[B43legacy_ILT_ROTOR_SIZE];
+#define B43legacy_ILT_RETARD_SIZE	53
+extern const u32 b43legacy_ilt_retard[B43legacy_ILT_RETARD_SIZE];
+#define B43legacy_ILT_FINEFREQA_SIZE	256
+extern const u16 b43legacy_ilt_finefreqa[B43legacy_ILT_FINEFREQA_SIZE];
+#define B43legacy_ILT_FINEFREQG_SIZE	256
+extern const u16 b43legacy_ilt_finefreqg[B43legacy_ILT_FINEFREQG_SIZE];
+#define B43legacy_ILT_NOISEA2_SIZE	8
+extern const u16 b43legacy_ilt_noisea2[B43legacy_ILT_NOISEA2_SIZE];
+#define B43legacy_ILT_NOISEA3_SIZE	8
+extern const u16 b43legacy_ilt_noisea3[B43legacy_ILT_NOISEA3_SIZE];
+#define B43legacy_ILT_NOISEG1_SIZE	8
+extern const u16 b43legacy_ilt_noiseg1[B43legacy_ILT_NOISEG1_SIZE];
+#define B43legacy_ILT_NOISEG2_SIZE	8
+extern const u16 b43legacy_ilt_noiseg2[B43legacy_ILT_NOISEG2_SIZE];
+#define B43legacy_ILT_NOISESCALEG_SIZE	27
+extern const u16 b43legacy_ilt_noisescaleg1[B43legacy_ILT_NOISESCALEG_SIZE];
+extern const u16 b43legacy_ilt_noisescaleg2[B43legacy_ILT_NOISESCALEG_SIZE];
+extern const u16 b43legacy_ilt_noisescaleg3[B43legacy_ILT_NOISESCALEG_SIZE];
+#define B43legacy_ILT_SIGMASQR_SIZE	53
+extern const u16 b43legacy_ilt_sigmasqr1[B43legacy_ILT_SIGMASQR_SIZE];
+extern const u16 b43legacy_ilt_sigmasqr2[B43legacy_ILT_SIGMASQR_SIZE];
+
+
+void b43legacy_ilt_write(struct b43legacy_wldev *dev, u16 offset, u16 val);
+void b43legacy_ilt_write32(struct b43legacy_wldev *dev, u16 offset,
+			   u32 val);
+u16 b43legacy_ilt_read(struct b43legacy_wldev *dev, u16 offset);
+
+#endif /* B43legacy_ILT_H_ */
diff --git a/drivers/net/wireless/b43legacy/leds.c b/drivers/net/wireless/b43legacy/leds.c
new file mode 100644
index 000000000000..a584ea810502
--- /dev/null
+++ b/drivers/net/wireless/b43legacy/leds.c
@@ -0,0 +1,298 @@
+/*
+
+  Broadcom B43legacy wireless driver
+
+  Copyright (c) 2005 Martin Langer <martin-langer@gmx.de>,
+		     Stefano Brivio <st3@riseup.net>
+		     Michael Buesch <mb@bu3sch.de>
+		     Danny van Dyk <kugelfang@gentoo.org>
+		     Andreas Jaggi <andreas.jaggi@waterwave.ch>
+  Copyright (c) 2007 Larry Finger <Larry.Finger@lwfinger.net>
+
+  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, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with this program; see the file COPYING.  If not, write to
+  the Free Software Foundation, Inc., 51 Franklin Steet, Fifth Floor,
+  Boston, MA 02110-1301, USA.
+
+*/
+
+#include "leds.h"
+#include "b43legacy.h"
+#include "main.h"
+
+static void b43legacy_led_changestate(struct b43legacy_led *led)
+{
+	struct b43legacy_wldev *dev = led->dev;
+	const int index = led->index;
+	u16 ledctl;
+
+	B43legacy_WARN_ON(!(index >= 0 && index < B43legacy_NR_LEDS));
+	B43legacy_WARN_ON(!led->blink_interval);
+	ledctl = b43legacy_read16(dev, B43legacy_MMIO_GPIO_CONTROL);
+	ledctl ^= (1 << index);
+	b43legacy_write16(dev, B43legacy_MMIO_GPIO_CONTROL, ledctl);
+}
+
+static void b43legacy_led_blink(unsigned long d)
+{
+	struct b43legacy_led *led = (struct b43legacy_led *)d;
+	struct b43legacy_wldev *dev = led->dev;
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev->wl->leds_lock, flags);
+	if (led->blink_interval) {
+		b43legacy_led_changestate(led);
+		mod_timer(&led->blink_timer, jiffies + led->blink_interval);
+	}
+	spin_unlock_irqrestore(&dev->wl->leds_lock, flags);
+}
+
+static void b43legacy_led_blink_start(struct b43legacy_led *led,
+				      unsigned long interval)
+{
+	if (led->blink_interval)
+		return;
+	led->blink_interval = interval;
+	b43legacy_led_changestate(led);
+	led->blink_timer.expires = jiffies + interval;
+	add_timer(&led->blink_timer);
+}
+
+static void b43legacy_led_blink_stop(struct b43legacy_led *led, int sync)
+{
+	struct b43legacy_wldev *dev = led->dev;
+	const int index = led->index;
+	u16 ledctl;
+
+	if (!led->blink_interval)
+		return;
+	if (unlikely(sync))
+		del_timer_sync(&led->blink_timer);
+	else
+		del_timer(&led->blink_timer);
+	led->blink_interval = 0;
+
+	/* Make sure the LED is turned off. */
+	B43legacy_WARN_ON(!(index >= 0 && index < B43legacy_NR_LEDS));
+	ledctl = b43legacy_read16(dev, B43legacy_MMIO_GPIO_CONTROL);
+	if (led->activelow)
+		ledctl |= (1 << index);
+	else
+		ledctl &= ~(1 << index);
+	b43legacy_write16(dev, B43legacy_MMIO_GPIO_CONTROL, ledctl);
+}
+
+static void b43legacy_led_init_hardcoded(struct b43legacy_wldev *dev,
+					 struct b43legacy_led *led,
+					 int led_index)
+{
+	struct ssb_bus *bus = dev->dev->bus;
+
+	/* This function is called, if the behaviour (and activelow)
+	 * information for a LED is missing in the SPROM.
+	 * We hardcode the behaviour values for various devices here.
+	 * Note that the B43legacy_LED_TEST_XXX behaviour values can
+	 * be used to figure out which led is mapped to which index.
+	 */
+
+	switch (led_index) {
+	case 0:
+		led->behaviour = B43legacy_LED_ACTIVITY;
+		led->activelow = 1;
+		if (bus->boardinfo.vendor == PCI_VENDOR_ID_COMPAQ)
+			led->behaviour = B43legacy_LED_RADIO_ALL;
+		break;
+	case 1:
+		led->behaviour = B43legacy_LED_RADIO_B;
+		if (bus->boardinfo.vendor == PCI_VENDOR_ID_ASUSTEK)
+			led->behaviour = B43legacy_LED_ASSOC;
+		break;
+	case 2:
+		led->behaviour = B43legacy_LED_RADIO_A;
+		break;
+	case 3:
+		led->behaviour = B43legacy_LED_OFF;
+		break;
+	default:
+		B43legacy_BUG_ON(1);
+	}
+}
+
+int b43legacy_leds_init(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_led *led;
+	u8 sprom[4];
+	int i;
+
+	sprom[0] = dev->dev->bus->sprom.r1.gpio0;
+	sprom[1] = dev->dev->bus->sprom.r1.gpio1;
+	sprom[2] = dev->dev->bus->sprom.r1.gpio2;
+	sprom[3] = dev->dev->bus->sprom.r1.gpio3;
+
+	for (i = 0; i < B43legacy_NR_LEDS; i++) {
+		led = &(dev->leds[i]);
+		led->index = i;
+		led->dev = dev;
+		setup_timer(&led->blink_timer,
+			    b43legacy_led_blink,
+			    (unsigned long)led);
+
+		if (sprom[i] == 0xFF)
+			b43legacy_led_init_hardcoded(dev, led, i);
+		else {
+			led->behaviour = sprom[i] & B43legacy_LED_BEHAVIOUR;
+			led->activelow = !!(sprom[i] &
+					   B43legacy_LED_ACTIVELOW);
+		}
+	}
+
+	return 0;
+}
+
+void b43legacy_leds_exit(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_led *led;
+	int i;
+
+	for (i = 0; i < B43legacy_NR_LEDS; i++) {
+		led = &(dev->leds[i]);
+		b43legacy_led_blink_stop(led, 1);
+	}
+	b43legacy_leds_switch_all(dev, 0);
+}
+
+void b43legacy_leds_update(struct b43legacy_wldev *dev, int activity)
+{
+	struct b43legacy_led *led;
+	struct b43legacy_phy *phy = &dev->phy;
+	const int transferring = (jiffies - dev->stats.last_tx)
+				  < B43legacy_LED_XFER_THRES;
+	int i;
+	int turn_on;
+	unsigned long interval = 0;
+	u16 ledctl;
+	unsigned long flags;
+	bool radio_enabled = (phy->radio_on && dev->radio_hw_enable);
+
+	spin_lock_irqsave(&dev->wl->leds_lock, flags);
+	ledctl = b43legacy_read16(dev, B43legacy_MMIO_GPIO_CONTROL);
+	for (i = 0; i < B43legacy_NR_LEDS; i++) {
+		led = &(dev->leds[i]);
+
+		turn_on = 0;
+		switch (led->behaviour) {
+		case B43legacy_LED_INACTIVE:
+			continue;
+		case B43legacy_LED_OFF:
+			break;
+		case B43legacy_LED_ON:
+			turn_on = 1;
+			break;
+		case B43legacy_LED_ACTIVITY:
+			turn_on = activity;
+			break;
+		case B43legacy_LED_RADIO_ALL:
+			turn_on = radio_enabled;
+			break;
+		case B43legacy_LED_RADIO_A:
+			break;
+		case B43legacy_LED_RADIO_B:
+			turn_on = radio_enabled;
+			break;
+		case B43legacy_LED_MODE_BG:
+			if (phy->type == B43legacy_PHYTYPE_G && radio_enabled)
+				turn_on = 1;
+			break;
+		case B43legacy_LED_TRANSFER:
+			if (transferring)
+				b43legacy_led_blink_start(led,
+						B43legacy_LEDBLINK_MEDIUM);
+			else
+				b43legacy_led_blink_stop(led, 0);
+			continue;
+		case B43legacy_LED_APTRANSFER:
+			if (b43legacy_is_mode(dev->wl,
+						IEEE80211_IF_TYPE_AP)) {
+				if (transferring) {
+					interval = B43legacy_LEDBLINK_FAST;
+					turn_on = 1;
+				}
+			} else {
+				turn_on = 1;
+				if (transferring)
+					interval = B43legacy_LEDBLINK_FAST;
+				else
+					turn_on = 0;
+			}
+			if (turn_on)
+				b43legacy_led_blink_start(led, interval);
+			else
+				b43legacy_led_blink_stop(led, 0);
+			continue;
+		case B43legacy_LED_WEIRD:
+			break;
+		case B43legacy_LED_ASSOC:
+			turn_on = 1;
+#ifdef CONFIG_B43LEGACY_DEBUG
+		case B43legacy_LED_TEST_BLINKSLOW:
+			b43legacy_led_blink_start(led, B43legacy_LEDBLINK_SLOW);
+			continue;
+		case B43legacy_LED_TEST_BLINKMEDIUM:
+			b43legacy_led_blink_start(led,
+						   B43legacy_LEDBLINK_MEDIUM);
+			continue;
+		case B43legacy_LED_TEST_BLINKFAST:
+			b43legacy_led_blink_start(led, B43legacy_LEDBLINK_FAST);
+			continue;
+#endif /* CONFIG_B43LEGACY_DEBUG */
+		default:
+			B43legacy_BUG_ON(1);
+		};
+
+		if (led->activelow)
+			turn_on = !turn_on;
+		if (turn_on)
+			ledctl |= (1 << i);
+		else
+			ledctl &= ~(1 << i);
+	}
+	b43legacy_write16(dev, B43legacy_MMIO_GPIO_CONTROL, ledctl);
+	spin_unlock_irqrestore(&dev->wl->leds_lock, flags);
+}
+
+void b43legacy_leds_switch_all(struct b43legacy_wldev *dev, int on)
+{
+	struct b43legacy_led *led;
+	u16 ledctl;
+	int i;
+	int bit_on;
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev->wl->leds_lock, flags);
+	ledctl = b43legacy_read16(dev, B43legacy_MMIO_GPIO_CONTROL);
+	for (i = 0; i < B43legacy_NR_LEDS; i++) {
+		led = &(dev->leds[i]);
+		if (led->behaviour == B43legacy_LED_INACTIVE)
+			continue;
+		if (on)
+			bit_on = led->activelow ? 0 : 1;
+		else
+			bit_on = led->activelow ? 1 : 0;
+		if (bit_on)
+			ledctl |= (1 << i);
+		else
+			ledctl &= ~(1 << i);
+	}
+	b43legacy_write16(dev, B43legacy_MMIO_GPIO_CONTROL, ledctl);
+	spin_unlock_irqrestore(&dev->wl->leds_lock, flags);
+}
diff --git a/drivers/net/wireless/b43legacy/leds.h b/drivers/net/wireless/b43legacy/leds.h
new file mode 100644
index 000000000000..b989f503e684
--- /dev/null
+++ b/drivers/net/wireless/b43legacy/leds.h
@@ -0,0 +1,56 @@
+#ifndef B43legacy_LEDS_H_
+#define B43legacy_LEDS_H_
+
+#include <linux/types.h>
+#include <linux/timer.h>
+
+
+struct b43legacy_led {
+	u8 behaviour;
+	bool activelow;
+	/* Index in the "leds" array in b43legacy_wldev */
+	u8 index;
+	struct b43legacy_wldev *dev;
+	struct timer_list blink_timer;
+	unsigned long blink_interval;
+};
+
+/* Delay between state changes when blinking in jiffies */
+#define B43legacy_LEDBLINK_SLOW		(HZ / 1)
+#define B43legacy_LEDBLINK_MEDIUM	(HZ / 4)
+#define B43legacy_LEDBLINK_FAST		(HZ / 8)
+
+#define B43legacy_LED_XFER_THRES	(HZ / 100)
+
+#define B43legacy_LED_BEHAVIOUR		0x7F
+#define B43legacy_LED_ACTIVELOW		0x80
+enum { /* LED behaviour values */
+	B43legacy_LED_OFF,
+	B43legacy_LED_ON,
+	B43legacy_LED_ACTIVITY,
+	B43legacy_LED_RADIO_ALL,
+	B43legacy_LED_RADIO_A,
+	B43legacy_LED_RADIO_B,
+	B43legacy_LED_MODE_BG,
+	B43legacy_LED_TRANSFER,
+	B43legacy_LED_APTRANSFER,
+	B43legacy_LED_WEIRD,
+	B43legacy_LED_ASSOC,
+	B43legacy_LED_INACTIVE,
+
+	/* Behaviour values for testing.
+	 * With these values it is easier to figure out
+	 * the real behaviour of leds, in case the SPROM
+	 * is missing information.
+	 */
+	B43legacy_LED_TEST_BLINKSLOW,
+	B43legacy_LED_TEST_BLINKMEDIUM,
+	B43legacy_LED_TEST_BLINKFAST,
+};
+
+int b43legacy_leds_init(struct b43legacy_wldev *dev);
+void b43legacy_leds_exit(struct b43legacy_wldev *dev);
+void b43legacy_leds_update(struct b43legacy_wldev *dev, int activity);
+void b43legacy_leds_switch_all(struct b43legacy_wldev *dev, int on);
+
+#endif /* B43legacy_LEDS_H_ */
diff --git a/drivers/net/wireless/b43legacy/main.c b/drivers/net/wireless/b43legacy/main.c
new file mode 100644
index 000000000000..f0749510bcd7
--- /dev/null
+++ b/drivers/net/wireless/b43legacy/main.c
@@ -0,0 +1,3856 @@
+/*
+ *
+ *  Broadcom B43legacy wireless driver
+ *
+ *  Copyright (c) 2005 Martin Langer <martin-langer@gmx.de>
+ *  Copyright (c) 2005 Stefano Brivio <st3@riseup.net>
+ *  Copyright (c) 2005, 2006 Michael Buesch <mb@bu3sch.de>
+ *  Copyright (c) 2005 Danny van Dyk <kugelfang@gentoo.org>
+ *  Copyright (c) 2005 Andreas Jaggi <andreas.jaggi@waterwave.ch>
+ *  Copyright (c) 2007 Larry Finger <Larry.Finger@lwfinger.net>
+ *
+ *  Some parts of the code in this file are derived from the ipw2200
+ *  driver  Copyright(c) 2003 - 2004 Intel Corporation.
+
+ *  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, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program; see the file COPYING.  If not, write to
+ *  the Free Software Foundation, Inc., 51 Franklin Steet, Fifth Floor,
+ *  Boston, MA 02110-1301, USA.
+ *
+ */
+
+#include <linux/delay.h>
+#include <linux/init.h>
+#include <linux/moduleparam.h>
+#include <linux/if_arp.h>
+#include <linux/etherdevice.h>
+#include <linux/version.h>
+#include <linux/firmware.h>
+#include <linux/wireless.h>
+#include <linux/workqueue.h>
+#include <linux/skbuff.h>
+#include <linux/dma-mapping.h>
+#include <net/dst.h>
+#include <asm/unaligned.h>
+
+#include "b43legacy.h"
+#include "main.h"
+#include "debugfs.h"
+#include "phy.h"
+#include "dma.h"
+#include "pio.h"
+#include "sysfs.h"
+#include "xmit.h"
+#include "radio.h"
+
+
+MODULE_DESCRIPTION("Broadcom B43legacy wireless driver");
+MODULE_AUTHOR("Martin Langer");
+MODULE_AUTHOR("Stefano Brivio");
+MODULE_AUTHOR("Michael Buesch");
+MODULE_LICENSE("GPL");
+
+#if defined(CONFIG_B43LEGACY_DMA) && defined(CONFIG_B43LEGACY_PIO)
+static int modparam_pio;
+module_param_named(pio, modparam_pio, int, 0444);
+MODULE_PARM_DESC(pio, "enable(1) / disable(0) PIO mode");
+#elif defined(CONFIG_B43LEGACY_DMA)
+# define modparam_pio	0
+#elif defined(CONFIG_B43LEGACY_PIO)
+# define modparam_pio	1
+#endif
+
+static int modparam_bad_frames_preempt;
+module_param_named(bad_frames_preempt, modparam_bad_frames_preempt, int, 0444);
+MODULE_PARM_DESC(bad_frames_preempt, "enable(1) / disable(0) Bad Frames"
+		 " Preemption");
+
+static int modparam_short_retry = B43legacy_DEFAULT_SHORT_RETRY_LIMIT;
+module_param_named(short_retry, modparam_short_retry, int, 0444);
+MODULE_PARM_DESC(short_retry, "Short-Retry-Limit (0 - 15)");
+
+static int modparam_long_retry = B43legacy_DEFAULT_LONG_RETRY_LIMIT;
+module_param_named(long_retry, modparam_long_retry, int, 0444);
+MODULE_PARM_DESC(long_retry, "Long-Retry-Limit (0 - 15)");
+
+static int modparam_noleds;
+module_param_named(noleds, modparam_noleds, int, 0444);
+MODULE_PARM_DESC(noleds, "Turn off all LED activity");
+
+static char modparam_fwpostfix[16];
+module_param_string(fwpostfix, modparam_fwpostfix, 16, 0444);
+MODULE_PARM_DESC(fwpostfix, "Postfix for the firmware files to load.");
+
+/* The following table supports BCM4301, BCM4303 and BCM4306/2 devices. */
+static const struct ssb_device_id b43legacy_ssb_tbl[] = {
+	SSB_DEVICE(SSB_VENDOR_BROADCOM, SSB_DEV_80211, 2),
+	SSB_DEVICE(SSB_VENDOR_BROADCOM, SSB_DEV_80211, 4),
+	SSB_DEVTABLE_END
+};
+MODULE_DEVICE_TABLE(ssb, b43legacy_ssb_tbl);
+
+
+/* Channel and ratetables are shared for all devices.
+ * They can't be const, because ieee80211 puts some precalculated
+ * data in there. This data is the same for all devices, so we don't
+ * get concurrency issues */
+#define RATETAB_ENT(_rateid, _flags) \
+	{							\
+		.rate	= B43legacy_RATE_TO_100KBPS(_rateid),	\
+		.val	= (_rateid),				\
+		.val2	= (_rateid),				\
+		.flags	= (_flags),				\
+	}
+static struct ieee80211_rate __b43legacy_ratetable[] = {
+	RATETAB_ENT(B43legacy_CCK_RATE_1MB, IEEE80211_RATE_CCK),
+	RATETAB_ENT(B43legacy_CCK_RATE_2MB, IEEE80211_RATE_CCK_2),
+	RATETAB_ENT(B43legacy_CCK_RATE_5MB, IEEE80211_RATE_CCK_2),
+	RATETAB_ENT(B43legacy_CCK_RATE_11MB, IEEE80211_RATE_CCK_2),
+	RATETAB_ENT(B43legacy_OFDM_RATE_6MB, IEEE80211_RATE_OFDM),
+	RATETAB_ENT(B43legacy_OFDM_RATE_9MB, IEEE80211_RATE_OFDM),
+	RATETAB_ENT(B43legacy_OFDM_RATE_12MB, IEEE80211_RATE_OFDM),
+	RATETAB_ENT(B43legacy_OFDM_RATE_18MB, IEEE80211_RATE_OFDM),
+	RATETAB_ENT(B43legacy_OFDM_RATE_24MB, IEEE80211_RATE_OFDM),
+	RATETAB_ENT(B43legacy_OFDM_RATE_36MB, IEEE80211_RATE_OFDM),
+	RATETAB_ENT(B43legacy_OFDM_RATE_48MB, IEEE80211_RATE_OFDM),
+	RATETAB_ENT(B43legacy_OFDM_RATE_54MB, IEEE80211_RATE_OFDM),
+};
+#define b43legacy_a_ratetable		(__b43legacy_ratetable + 4)
+#define b43legacy_a_ratetable_size	8
+#define b43legacy_b_ratetable		(__b43legacy_ratetable + 0)
+#define b43legacy_b_ratetable_size	4
+#define b43legacy_g_ratetable		(__b43legacy_ratetable + 0)
+#define b43legacy_g_ratetable_size	12
+
+#define CHANTAB_ENT(_chanid, _freq) \
+	{							\
+		.chan	= (_chanid),				\
+		.freq	= (_freq),				\
+		.val	= (_chanid),				\
+		.flag	= IEEE80211_CHAN_W_SCAN |		\
+			  IEEE80211_CHAN_W_ACTIVE_SCAN |	\
+			  IEEE80211_CHAN_W_IBSS,		\
+		.power_level	= 0x0A,				\
+		.antenna_max	= 0xFF,				\
+	}
+static struct ieee80211_channel b43legacy_bg_chantable[] = {
+	CHANTAB_ENT(1, 2412),
+	CHANTAB_ENT(2, 2417),
+	CHANTAB_ENT(3, 2422),
+	CHANTAB_ENT(4, 2427),
+	CHANTAB_ENT(5, 2432),
+	CHANTAB_ENT(6, 2437),
+	CHANTAB_ENT(7, 2442),
+	CHANTAB_ENT(8, 2447),
+	CHANTAB_ENT(9, 2452),
+	CHANTAB_ENT(10, 2457),
+	CHANTAB_ENT(11, 2462),
+	CHANTAB_ENT(12, 2467),
+	CHANTAB_ENT(13, 2472),
+	CHANTAB_ENT(14, 2484),
+};
+#define b43legacy_bg_chantable_size	ARRAY_SIZE(b43legacy_bg_chantable)
+
+static void b43legacy_wireless_core_exit(struct b43legacy_wldev *dev);
+static int b43legacy_wireless_core_init(struct b43legacy_wldev *dev);
+static void b43legacy_wireless_core_stop(struct b43legacy_wldev *dev);
+static int b43legacy_wireless_core_start(struct b43legacy_wldev *dev);
+
+
+static int b43legacy_ratelimit(struct b43legacy_wl *wl)
+{
+	if (!wl || !wl->current_dev)
+		return 1;
+	if (b43legacy_status(wl->current_dev) < B43legacy_STAT_STARTED)
+		return 1;
+	/* We are up and running.
+	 * Ratelimit the messages to avoid DoS over the net. */
+	return net_ratelimit();
+}
+
+void b43legacyinfo(struct b43legacy_wl *wl, const char *fmt, ...)
+{
+	va_list args;
+
+	if (!b43legacy_ratelimit(wl))
+		return;
+	va_start(args, fmt);
+	printk(KERN_INFO "b43legacy-%s: ",
+	       (wl && wl->hw) ? wiphy_name(wl->hw->wiphy) : "wlan");
+	vprintk(fmt, args);
+	va_end(args);
+}
+
+void b43legacyerr(struct b43legacy_wl *wl, const char *fmt, ...)
+{
+	va_list args;
+
+	if (!b43legacy_ratelimit(wl))
+		return;
+	va_start(args, fmt);
+	printk(KERN_ERR "b43legacy-%s ERROR: ",
+	       (wl && wl->hw) ? wiphy_name(wl->hw->wiphy) : "wlan");
+	vprintk(fmt, args);
+	va_end(args);
+}
+
+void b43legacywarn(struct b43legacy_wl *wl, const char *fmt, ...)
+{
+	va_list args;
+
+	if (!b43legacy_ratelimit(wl))
+		return;
+	va_start(args, fmt);
+	printk(KERN_WARNING "b43legacy-%s warning: ",
+	       (wl && wl->hw) ? wiphy_name(wl->hw->wiphy) : "wlan");
+	vprintk(fmt, args);
+	va_end(args);
+}
+
+#if B43legacy_DEBUG
+void b43legacydbg(struct b43legacy_wl *wl, const char *fmt, ...)
+{
+	va_list args;
+
+	va_start(args, fmt);
+	printk(KERN_DEBUG "b43legacy-%s debug: ",
+	       (wl && wl->hw) ? wiphy_name(wl->hw->wiphy) : "wlan");
+	vprintk(fmt, args);
+	va_end(args);
+}
+#endif /* DEBUG */
+
+static void b43legacy_ram_write(struct b43legacy_wldev *dev, u16 offset,
+				u32 val)
+{
+	u32 status;
+
+	B43legacy_WARN_ON(offset % 4 != 0);
+
+	status = b43legacy_read32(dev, B43legacy_MMIO_STATUS_BITFIELD);
+	if (status & B43legacy_SBF_XFER_REG_BYTESWAP)
+		val = swab32(val);
+
+	b43legacy_write32(dev, B43legacy_MMIO_RAM_CONTROL, offset);
+	mmiowb();
+	b43legacy_write32(dev, B43legacy_MMIO_RAM_DATA, val);
+}
+
+static inline
+void b43legacy_shm_control_word(struct b43legacy_wldev *dev,
+				u16 routing, u16 offset)
+{
+	u32 control;
+
+	/* "offset" is the WORD offset. */
+
+	control = routing;
+	control <<= 16;
+	control |= offset;
+	b43legacy_write32(dev, B43legacy_MMIO_SHM_CONTROL, control);
+}
+
+u32 b43legacy_shm_read32(struct b43legacy_wldev *dev,
+		       u16 routing, u16 offset)
+{
+	u32 ret;
+
+	if (routing == B43legacy_SHM_SHARED) {
+		B43legacy_WARN_ON((offset & 0x0001) != 0);
+		if (offset & 0x0003) {
+			/* Unaligned access */
+			b43legacy_shm_control_word(dev, routing, offset >> 2);
+			ret = b43legacy_read16(dev,
+				B43legacy_MMIO_SHM_DATA_UNALIGNED);
+			ret <<= 16;
+			b43legacy_shm_control_word(dev, routing,
+						     (offset >> 2) + 1);
+			ret |= b43legacy_read16(dev, B43legacy_MMIO_SHM_DATA);
+
+			return ret;
+		}
+		offset >>= 2;
+	}
+	b43legacy_shm_control_word(dev, routing, offset);
+	ret = b43legacy_read32(dev, B43legacy_MMIO_SHM_DATA);
+
+	return ret;
+}
+
+u16 b43legacy_shm_read16(struct b43legacy_wldev *dev,
+			   u16 routing, u16 offset)
+{
+	u16 ret;
+
+	if (routing == B43legacy_SHM_SHARED) {
+		B43legacy_WARN_ON((offset & 0x0001) != 0);
+		if (offset & 0x0003) {
+			/* Unaligned access */
+			b43legacy_shm_control_word(dev, routing, offset >> 2);
+			ret = b43legacy_read16(dev,
+					     B43legacy_MMIO_SHM_DATA_UNALIGNED);
+
+			return ret;
+		}
+		offset >>= 2;
+	}
+	b43legacy_shm_control_word(dev, routing, offset);
+	ret = b43legacy_read16(dev, B43legacy_MMIO_SHM_DATA);
+
+	return ret;
+}
+
+void b43legacy_shm_write32(struct b43legacy_wldev *dev,
+			   u16 routing, u16 offset,
+			   u32 value)
+{
+	if (routing == B43legacy_SHM_SHARED) {
+		B43legacy_WARN_ON((offset & 0x0001) != 0);
+		if (offset & 0x0003) {
+			/* Unaligned access */
+			b43legacy_shm_control_word(dev, routing, offset >> 2);
+			mmiowb();
+			b43legacy_write16(dev,
+					  B43legacy_MMIO_SHM_DATA_UNALIGNED,
+					  (value >> 16) & 0xffff);
+			mmiowb();
+			b43legacy_shm_control_word(dev, routing,
+						   (offset >> 2) + 1);
+			mmiowb();
+			b43legacy_write16(dev, B43legacy_MMIO_SHM_DATA,
+					  value & 0xffff);
+			return;
+		}
+		offset >>= 2;
+	}
+	b43legacy_shm_control_word(dev, routing, offset);
+	mmiowb();
+	b43legacy_write32(dev, B43legacy_MMIO_SHM_DATA, value);
+}
+
+void b43legacy_shm_write16(struct b43legacy_wldev *dev, u16 routing, u16 offset,
+			   u16 value)
+{
+	if (routing == B43legacy_SHM_SHARED) {
+		B43legacy_WARN_ON((offset & 0x0001) != 0);
+		if (offset & 0x0003) {
+			/* Unaligned access */
+			b43legacy_shm_control_word(dev, routing, offset >> 2);
+			mmiowb();
+			b43legacy_write16(dev,
+					  B43legacy_MMIO_SHM_DATA_UNALIGNED,
+					  value);
+			return;
+		}
+		offset >>= 2;
+	}
+	b43legacy_shm_control_word(dev, routing, offset);
+	mmiowb();
+	b43legacy_write16(dev, B43legacy_MMIO_SHM_DATA, value);
+}
+
+/* Read HostFlags */
+u32 b43legacy_hf_read(struct b43legacy_wldev *dev)
+{
+	u32 ret;
+
+	ret = b43legacy_shm_read16(dev, B43legacy_SHM_SHARED,
+				   B43legacy_SHM_SH_HOSTFHI);
+	ret <<= 16;
+	ret |= b43legacy_shm_read16(dev, B43legacy_SHM_SHARED,
+				    B43legacy_SHM_SH_HOSTFLO);
+
+	return ret;
+}
+
+/* Write HostFlags */
+void b43legacy_hf_write(struct b43legacy_wldev *dev, u32 value)
+{
+	b43legacy_shm_write16(dev, B43legacy_SHM_SHARED,
+			      B43legacy_SHM_SH_HOSTFLO,
+			      (value & 0x0000FFFF));
+	b43legacy_shm_write16(dev, B43legacy_SHM_SHARED,
+			      B43legacy_SHM_SH_HOSTFHI,
+			      ((value & 0xFFFF0000) >> 16));
+}
+
+void b43legacy_tsf_read(struct b43legacy_wldev *dev, u64 *tsf)
+{
+	/* We need to be careful. As we read the TSF from multiple
+	 * registers, we should take care of register overflows.
+	 * In theory, the whole tsf read process should be atomic.
+	 * We try to be atomic here, by restaring the read process,
+	 * if any of the high registers changed (overflew).
+	 */
+	if (dev->dev->id.revision >= 3) {
+		u32 low;
+		u32 high;
+		u32 high2;
+
+		do {
+			high = b43legacy_read32(dev,
+					B43legacy_MMIO_REV3PLUS_TSF_HIGH);
+			low = b43legacy_read32(dev,
+					B43legacy_MMIO_REV3PLUS_TSF_LOW);
+			high2 = b43legacy_read32(dev,
+					B43legacy_MMIO_REV3PLUS_TSF_HIGH);
+		} while (unlikely(high != high2));
+
+		*tsf = high;
+		*tsf <<= 32;
+		*tsf |= low;
+	} else {
+		u64 tmp;
+		u16 v0;
+		u16 v1;
+		u16 v2;
+		u16 v3;
+		u16 test1;
+		u16 test2;
+		u16 test3;
+
+		do {
+			v3 = b43legacy_read16(dev, B43legacy_MMIO_TSF_3);
+			v2 = b43legacy_read16(dev, B43legacy_MMIO_TSF_2);
+			v1 = b43legacy_read16(dev, B43legacy_MMIO_TSF_1);
+			v0 = b43legacy_read16(dev, B43legacy_MMIO_TSF_0);
+
+			test3 = b43legacy_read16(dev, B43legacy_MMIO_TSF_3);
+			test2 = b43legacy_read16(dev, B43legacy_MMIO_TSF_2);
+			test1 = b43legacy_read16(dev, B43legacy_MMIO_TSF_1);
+		} while (v3 != test3 || v2 != test2 || v1 != test1);
+
+		*tsf = v3;
+		*tsf <<= 48;
+		tmp = v2;
+		tmp <<= 32;
+		*tsf |= tmp;
+		tmp = v1;
+		tmp <<= 16;
+		*tsf |= tmp;
+		*tsf |= v0;
+	}
+}
+
+static void b43legacy_time_lock(struct b43legacy_wldev *dev)
+{
+	u32 status;
+
+	status = b43legacy_read32(dev, B43legacy_MMIO_STATUS_BITFIELD);
+	status |= B43legacy_SBF_TIME_UPDATE;
+	b43legacy_write32(dev, B43legacy_MMIO_STATUS_BITFIELD, status);
+	mmiowb();
+}
+
+static void b43legacy_time_unlock(struct b43legacy_wldev *dev)
+{
+	u32 status;
+
+	status = b43legacy_read32(dev, B43legacy_MMIO_STATUS_BITFIELD);
+	status &= ~B43legacy_SBF_TIME_UPDATE;
+	b43legacy_write32(dev, B43legacy_MMIO_STATUS_BITFIELD, status);
+}
+
+static void b43legacy_tsf_write_locked(struct b43legacy_wldev *dev, u64 tsf)
+{
+	/* Be careful with the in-progress timer.
+	 * First zero out the low register, so we have a full
+	 * register-overflow duration to complete the operation.
+	 */
+	if (dev->dev->id.revision >= 3) {
+		u32 lo = (tsf & 0x00000000FFFFFFFFULL);
+		u32 hi = (tsf & 0xFFFFFFFF00000000ULL) >> 32;
+
+		b43legacy_write32(dev, B43legacy_MMIO_REV3PLUS_TSF_LOW, 0);
+		mmiowb();
+		b43legacy_write32(dev, B43legacy_MMIO_REV3PLUS_TSF_HIGH,
+				    hi);
+		mmiowb();
+		b43legacy_write32(dev, B43legacy_MMIO_REV3PLUS_TSF_LOW,
+				    lo);
+	} else {
+		u16 v0 = (tsf & 0x000000000000FFFFULL);
+		u16 v1 = (tsf & 0x00000000FFFF0000ULL) >> 16;
+		u16 v2 = (tsf & 0x0000FFFF00000000ULL) >> 32;
+		u16 v3 = (tsf & 0xFFFF000000000000ULL) >> 48;
+
+		b43legacy_write16(dev, B43legacy_MMIO_TSF_0, 0);
+		mmiowb();
+		b43legacy_write16(dev, B43legacy_MMIO_TSF_3, v3);
+		mmiowb();
+		b43legacy_write16(dev, B43legacy_MMIO_TSF_2, v2);
+		mmiowb();
+		b43legacy_write16(dev, B43legacy_MMIO_TSF_1, v1);
+		mmiowb();
+		b43legacy_write16(dev, B43legacy_MMIO_TSF_0, v0);
+	}
+}
+
+void b43legacy_tsf_write(struct b43legacy_wldev *dev, u64 tsf)
+{
+	b43legacy_time_lock(dev);
+	b43legacy_tsf_write_locked(dev, tsf);
+	b43legacy_time_unlock(dev);
+}
+
+static
+void b43legacy_macfilter_set(struct b43legacy_wldev *dev,
+			     u16 offset, const u8 *mac)
+{
+	static const u8 zero_addr[ETH_ALEN] = { 0 };
+	u16 data;
+
+	if (!mac)
+		mac = zero_addr;
+
+	offset |= 0x0020;
+	b43legacy_write16(dev, B43legacy_MMIO_MACFILTER_CONTROL, offset);
+
+	data = mac[0];
+	data |= mac[1] << 8;
+	b43legacy_write16(dev, B43legacy_MMIO_MACFILTER_DATA, data);
+	data = mac[2];
+	data |= mac[3] << 8;
+	b43legacy_write16(dev, B43legacy_MMIO_MACFILTER_DATA, data);
+	data = mac[4];
+	data |= mac[5] << 8;
+	b43legacy_write16(dev, B43legacy_MMIO_MACFILTER_DATA, data);
+}
+
+static void b43legacy_write_mac_bssid_templates(struct b43legacy_wldev *dev)
+{
+	static const u8 zero_addr[ETH_ALEN] = { 0 };
+	const u8 *mac = dev->wl->mac_addr;
+	const u8 *bssid = dev->wl->bssid;
+	u8 mac_bssid[ETH_ALEN * 2];
+	int i;
+	u32 tmp;
+
+	if (!bssid)
+		bssid = zero_addr;
+	if (!mac)
+		mac = zero_addr;
+
+	b43legacy_macfilter_set(dev, B43legacy_MACFILTER_BSSID, bssid);
+
+	memcpy(mac_bssid, mac, ETH_ALEN);
+	memcpy(mac_bssid + ETH_ALEN, bssid, ETH_ALEN);
+
+	/* Write our MAC address and BSSID to template ram */
+	for (i = 0; i < ARRAY_SIZE(mac_bssid); i += sizeof(u32)) {
+		tmp =  (u32)(mac_bssid[i + 0]);
+		tmp |= (u32)(mac_bssid[i + 1]) << 8;
+		tmp |= (u32)(mac_bssid[i + 2]) << 16;
+		tmp |= (u32)(mac_bssid[i + 3]) << 24;
+		b43legacy_ram_write(dev, 0x20 + i, tmp);
+		b43legacy_ram_write(dev, 0x78 + i, tmp);
+		b43legacy_ram_write(dev, 0x478 + i, tmp);
+	}
+}
+
+static void b43legacy_upload_card_macaddress(struct b43legacy_wldev *dev)
+{
+	b43legacy_write_mac_bssid_templates(dev);
+	b43legacy_macfilter_set(dev, B43legacy_MACFILTER_SELF,
+				dev->wl->mac_addr);
+}
+
+static void b43legacy_set_slot_time(struct b43legacy_wldev *dev,
+				    u16 slot_time)
+{
+	/* slot_time is in usec. */
+	if (dev->phy.type != B43legacy_PHYTYPE_G)
+		return;
+	b43legacy_write16(dev, 0x684, 510 + slot_time);
+	b43legacy_shm_write16(dev, B43legacy_SHM_SHARED, 0x0010,
+			      slot_time);
+}
+
+static void b43legacy_short_slot_timing_enable(struct b43legacy_wldev *dev)
+{
+	b43legacy_set_slot_time(dev, 9);
+	dev->short_slot = 1;
+}
+
+static void b43legacy_short_slot_timing_disable(struct b43legacy_wldev *dev)
+{
+	b43legacy_set_slot_time(dev, 20);
+	dev->short_slot = 0;
+}
+
+/* Enable a Generic IRQ. "mask" is the mask of which IRQs to enable.
+ * Returns the _previously_ enabled IRQ mask.
+ */
+static inline u32 b43legacy_interrupt_enable(struct b43legacy_wldev *dev,
+					     u32 mask)
+{
+	u32 old_mask;
+
+	old_mask = b43legacy_read32(dev, B43legacy_MMIO_GEN_IRQ_MASK);
+	b43legacy_write32(dev, B43legacy_MMIO_GEN_IRQ_MASK, old_mask |
+			  mask);
+
+	return old_mask;
+}
+
+/* Disable a Generic IRQ. "mask" is the mask of which IRQs to disable.
+ * Returns the _previously_ enabled IRQ mask.
+ */
+static inline u32 b43legacy_interrupt_disable(struct b43legacy_wldev *dev,
+					      u32 mask)
+{
+	u32 old_mask;
+
+	old_mask = b43legacy_read32(dev, B43legacy_MMIO_GEN_IRQ_MASK);
+	b43legacy_write32(dev, B43legacy_MMIO_GEN_IRQ_MASK, old_mask & ~mask);
+
+	return old_mask;
+}
+
+/* Synchronize IRQ top- and bottom-half.
+ * IRQs must be masked before calling this.
+ * This must not be called with the irq_lock held.
+ */
+static void b43legacy_synchronize_irq(struct b43legacy_wldev *dev)
+{
+	synchronize_irq(dev->dev->irq);
+	tasklet_kill(&dev->isr_tasklet);
+}
+
+/* DummyTransmission function, as documented on
+ * http://bcm-specs.sipsolutions.net/DummyTransmission
+ */
+void b43legacy_dummy_transmission(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+	unsigned int i;
+	unsigned int max_loop;
+	u16 value;
+	u32 buffer[5] = {
+		0x00000000,
+		0x00D40000,
+		0x00000000,
+		0x01000000,
+		0x00000000,
+	};
+
+	switch (phy->type) {
+	case B43legacy_PHYTYPE_B:
+	case B43legacy_PHYTYPE_G:
+		max_loop = 0xFA;
+		buffer[0] = 0x000B846E;
+		break;
+	default:
+		B43legacy_BUG_ON(1);
+		return;
+	}
+
+	for (i = 0; i < 5; i++)
+		b43legacy_ram_write(dev, i * 4, buffer[i]);
+
+	/* dummy read follows */
+	b43legacy_read32(dev, B43legacy_MMIO_STATUS_BITFIELD);
+
+	b43legacy_write16(dev, 0x0568, 0x0000);
+	b43legacy_write16(dev, 0x07C0, 0x0000);
+	b43legacy_write16(dev, 0x050C, 0x0000);
+	b43legacy_write16(dev, 0x0508, 0x0000);
+	b43legacy_write16(dev, 0x050A, 0x0000);
+	b43legacy_write16(dev, 0x054C, 0x0000);
+	b43legacy_write16(dev, 0x056A, 0x0014);
+	b43legacy_write16(dev, 0x0568, 0x0826);
+	b43legacy_write16(dev, 0x0500, 0x0000);
+	b43legacy_write16(dev, 0x0502, 0x0030);
+
+	if (phy->radio_ver == 0x2050 && phy->radio_rev <= 0x5)
+		b43legacy_radio_write16(dev, 0x0051, 0x0017);
+	for (i = 0x00; i < max_loop; i++) {
+		value = b43legacy_read16(dev, 0x050E);
+		if (value & 0x0080)
+			break;
+		udelay(10);
+	}
+	for (i = 0x00; i < 0x0A; i++) {
+		value = b43legacy_read16(dev, 0x050E);
+		if (value & 0x0400)
+			break;
+		udelay(10);
+	}
+	for (i = 0x00; i < 0x0A; i++) {
+		value = b43legacy_read16(dev, 0x0690);
+		if (!(value & 0x0100))
+			break;
+		udelay(10);
+	}
+	if (phy->radio_ver == 0x2050 && phy->radio_rev <= 0x5)
+		b43legacy_radio_write16(dev, 0x0051, 0x0037);
+}
+
+/* Turn the Analog ON/OFF */
+static void b43legacy_switch_analog(struct b43legacy_wldev *dev, int on)
+{
+	b43legacy_write16(dev, B43legacy_MMIO_PHY0, on ? 0 : 0xF4);
+}
+
+void b43legacy_wireless_core_reset(struct b43legacy_wldev *dev, u32 flags)
+{
+	u32 tmslow;
+	u32 macctl;
+
+	flags |= B43legacy_TMSLOW_PHYCLKEN;
+	flags |= B43legacy_TMSLOW_PHYRESET;
+	ssb_device_enable(dev->dev, flags);
+	msleep(2); /* Wait for the PLL to turn on. */
+
+	/* Now take the PHY out of Reset again */
+	tmslow = ssb_read32(dev->dev, SSB_TMSLOW);
+	tmslow |= SSB_TMSLOW_FGC;
+	tmslow &= ~B43legacy_TMSLOW_PHYRESET;
+	ssb_write32(dev->dev, SSB_TMSLOW, tmslow);
+	ssb_read32(dev->dev, SSB_TMSLOW); /* flush */
+	msleep(1);
+	tmslow &= ~SSB_TMSLOW_FGC;
+	ssb_write32(dev->dev, SSB_TMSLOW, tmslow);
+	ssb_read32(dev->dev, SSB_TMSLOW); /* flush */
+	msleep(1);
+
+	/* Turn Analog ON */
+	b43legacy_switch_analog(dev, 1);
+
+	macctl = b43legacy_read32(dev, B43legacy_MMIO_MACCTL);
+	macctl &= ~B43legacy_MACCTL_GMODE;
+	if (flags & B43legacy_TMSLOW_GMODE) {
+		macctl |= B43legacy_MACCTL_GMODE;
+		dev->phy.gmode = 1;
+	} else
+		dev->phy.gmode = 0;
+	macctl |= B43legacy_MACCTL_IHR_ENABLED;
+	b43legacy_write32(dev, B43legacy_MMIO_MACCTL, macctl);
+}
+
+static void handle_irq_transmit_status(struct b43legacy_wldev *dev)
+{
+	u32 v0;
+	u32 v1;
+	u16 tmp;
+	struct b43legacy_txstatus stat;
+
+	while (1) {
+		v0 = b43legacy_read32(dev, B43legacy_MMIO_XMITSTAT_0);
+		if (!(v0 & 0x00000001))
+			break;
+		v1 = b43legacy_read32(dev, B43legacy_MMIO_XMITSTAT_1);
+
+		stat.cookie = (v0 >> 16);
+		stat.seq = (v1 & 0x0000FFFF);
+		stat.phy_stat = ((v1 & 0x00FF0000) >> 16);
+		tmp = (v0 & 0x0000FFFF);
+		stat.frame_count = ((tmp & 0xF000) >> 12);
+		stat.rts_count = ((tmp & 0x0F00) >> 8);
+		stat.supp_reason = ((tmp & 0x001C) >> 2);
+		stat.pm_indicated = !!(tmp & 0x0080);
+		stat.intermediate = !!(tmp & 0x0040);
+		stat.for_ampdu = !!(tmp & 0x0020);
+		stat.acked = !!(tmp & 0x0002);
+
+		b43legacy_handle_txstatus(dev, &stat);
+	}
+}
+
+static void drain_txstatus_queue(struct b43legacy_wldev *dev)
+{
+	u32 dummy;
+
+	if (dev->dev->id.revision < 5)
+		return;
+	/* Read all entries from the microcode TXstatus FIFO
+	 * and throw them away.
+	 */
+	while (1) {
+		dummy = b43legacy_read32(dev, B43legacy_MMIO_XMITSTAT_0);
+		if (!(dummy & 0x00000001))
+			break;
+		dummy = b43legacy_read32(dev, B43legacy_MMIO_XMITSTAT_1);
+	}
+}
+
+static u32 b43legacy_jssi_read(struct b43legacy_wldev *dev)
+{
+	u32 val = 0;
+
+	val = b43legacy_shm_read16(dev, B43legacy_SHM_SHARED, 0x40A);
+	val <<= 16;
+	val |= b43legacy_shm_read16(dev, B43legacy_SHM_SHARED, 0x408);
+
+	return val;
+}
+
+static void b43legacy_jssi_write(struct b43legacy_wldev *dev, u32 jssi)
+{
+	b43legacy_shm_write16(dev, B43legacy_SHM_SHARED, 0x408,
+			      (jssi & 0x0000FFFF));
+	b43legacy_shm_write16(dev, B43legacy_SHM_SHARED, 0x40A,
+			      (jssi & 0xFFFF0000) >> 16);
+}
+
+static void b43legacy_generate_noise_sample(struct b43legacy_wldev *dev)
+{
+	b43legacy_jssi_write(dev, 0x7F7F7F7F);
+	b43legacy_write32(dev, B43legacy_MMIO_STATUS2_BITFIELD,
+			  b43legacy_read32(dev,
+			  B43legacy_MMIO_STATUS2_BITFIELD)
+			  | (1 << 4));
+	B43legacy_WARN_ON(dev->noisecalc.channel_at_start !=
+			    dev->phy.channel);
+}
+
+static void b43legacy_calculate_link_quality(struct b43legacy_wldev *dev)
+{
+	/* Top half of Link Quality calculation. */
+
+	if (dev->noisecalc.calculation_running)
+		return;
+	dev->noisecalc.channel_at_start = dev->phy.channel;
+	dev->noisecalc.calculation_running = 1;
+	dev->noisecalc.nr_samples = 0;
+
+	b43legacy_generate_noise_sample(dev);
+}
+
+static void handle_irq_noise(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+	u16 tmp;
+	u8 noise[4];
+	u8 i;
+	u8 j;
+	s32 average;
+
+	/* Bottom half of Link Quality calculation. */
+
+	B43legacy_WARN_ON(!dev->noisecalc.calculation_running);
+	if (dev->noisecalc.channel_at_start != phy->channel)
+		goto drop_calculation;
+	*((__le32 *)noise) = cpu_to_le32(b43legacy_jssi_read(dev));
+	if (noise[0] == 0x7F || noise[1] == 0x7F ||
+	    noise[2] == 0x7F || noise[3] == 0x7F)
+		goto generate_new;
+
+	/* Get the noise samples. */
+	B43legacy_WARN_ON(dev->noisecalc.nr_samples >= 8);
+	i = dev->noisecalc.nr_samples;
+	noise[0] = limit_value(noise[0], 0, ARRAY_SIZE(phy->nrssi_lt) - 1);
+	noise[1] = limit_value(noise[1], 0, ARRAY_SIZE(phy->nrssi_lt) - 1);
+	noise[2] = limit_value(noise[2], 0, ARRAY_SIZE(phy->nrssi_lt) - 1);
+	noise[3] = limit_value(noise[3], 0, ARRAY_SIZE(phy->nrssi_lt) - 1);
+	dev->noisecalc.samples[i][0] = phy->nrssi_lt[noise[0]];
+	dev->noisecalc.samples[i][1] = phy->nrssi_lt[noise[1]];
+	dev->noisecalc.samples[i][2] = phy->nrssi_lt[noise[2]];
+	dev->noisecalc.samples[i][3] = phy->nrssi_lt[noise[3]];
+	dev->noisecalc.nr_samples++;
+	if (dev->noisecalc.nr_samples == 8) {
+		/* Calculate the Link Quality by the noise samples. */
+		average = 0;
+		for (i = 0; i < 8; i++) {
+			for (j = 0; j < 4; j++)
+				average += dev->noisecalc.samples[i][j];
+		}
+		average /= (8 * 4);
+		average *= 125;
+		average += 64;
+		average /= 128;
+		tmp = b43legacy_shm_read16(dev, B43legacy_SHM_SHARED,
+					     0x40C);
+		tmp = (tmp / 128) & 0x1F;
+		if (tmp >= 8)
+			average += 2;
+		else
+			average -= 25;
+		if (tmp == 8)
+			average -= 72;
+		else
+			average -= 48;
+
+		dev->stats.link_noise = average;
+drop_calculation:
+		dev->noisecalc.calculation_running = 0;
+		return;
+	}
+generate_new:
+	b43legacy_generate_noise_sample(dev);
+}
+
+static void handle_irq_tbtt_indication(struct b43legacy_wldev *dev)
+{
+	if (b43legacy_is_mode(dev->wl, IEEE80211_IF_TYPE_AP)) {
+		/* TODO: PS TBTT */
+	} else {
+		if (1/*FIXME: the last PSpoll frame was sent successfully */)
+			b43legacy_power_saving_ctl_bits(dev, -1, -1);
+	}
+	dev->reg124_set_0x4 = 0;
+	if (b43legacy_is_mode(dev->wl, IEEE80211_IF_TYPE_IBSS))
+		dev->reg124_set_0x4 = 1;
+}
+
+static void handle_irq_atim_end(struct b43legacy_wldev *dev)
+{
+	if (!dev->reg124_set_0x4) /*FIXME rename this variable*/
+		return;
+	b43legacy_write32(dev, B43legacy_MMIO_STATUS2_BITFIELD,
+			  b43legacy_read32(dev, B43legacy_MMIO_STATUS2_BITFIELD)
+			  | 0x4);
+}
+
+static void handle_irq_pmq(struct b43legacy_wldev *dev)
+{
+	u32 tmp;
+
+	/* TODO: AP mode. */
+
+	while (1) {
+		tmp = b43legacy_read32(dev, B43legacy_MMIO_PS_STATUS);
+		if (!(tmp & 0x00000008))
+			break;
+	}
+	/* 16bit write is odd, but correct. */
+	b43legacy_write16(dev, B43legacy_MMIO_PS_STATUS, 0x0002);
+}
+
+static void b43legacy_write_template_common(struct b43legacy_wldev *dev,
+					    const u8 *data, u16 size,
+					    u16 ram_offset,
+					    u16 shm_size_offset, u8 rate)
+{
+	u32 i;
+	u32 tmp;
+	struct b43legacy_plcp_hdr4 plcp;
+
+	plcp.data = 0;
+	b43legacy_generate_plcp_hdr(&plcp, size + FCS_LEN, rate);
+	b43legacy_ram_write(dev, ram_offset, le32_to_cpu(plcp.data));
+	ram_offset += sizeof(u32);
+	/* The PLCP is 6 bytes long, but we only wrote 4 bytes, yet.
+	 * So leave the first two bytes of the next write blank.
+	 */
+	tmp = (u32)(data[0]) << 16;
+	tmp |= (u32)(data[1]) << 24;
+	b43legacy_ram_write(dev, ram_offset, tmp);
+	ram_offset += sizeof(u32);
+	for (i = 2; i < size; i += sizeof(u32)) {
+		tmp = (u32)(data[i + 0]);
+		if (i + 1 < size)
+			tmp |= (u32)(data[i + 1]) << 8;
+		if (i + 2 < size)
+			tmp |= (u32)(data[i + 2]) << 16;
+		if (i + 3 < size)
+			tmp |= (u32)(data[i + 3]) << 24;
+		b43legacy_ram_write(dev, ram_offset + i - 2, tmp);
+	}
+	b43legacy_shm_write16(dev, B43legacy_SHM_SHARED, shm_size_offset,
+			      size + sizeof(struct b43legacy_plcp_hdr6));
+}
+
+static void b43legacy_write_beacon_template(struct b43legacy_wldev *dev,
+					    u16 ram_offset,
+					    u16 shm_size_offset, u8 rate)
+{
+	int len;
+	const u8 *data;
+
+	B43legacy_WARN_ON(!dev->cached_beacon);
+	len = min((size_t)dev->cached_beacon->len,
+		  0x200 - sizeof(struct b43legacy_plcp_hdr6));
+	data = (const u8 *)(dev->cached_beacon->data);
+	b43legacy_write_template_common(dev, data,
+					len, ram_offset,
+					shm_size_offset, rate);
+}
+
+static void b43legacy_write_probe_resp_plcp(struct b43legacy_wldev *dev,
+					    u16 shm_offset, u16 size,
+					    u8 rate)
+{
+	struct b43legacy_plcp_hdr4 plcp;
+	u32 tmp;
+	__le16 dur;
+
+	plcp.data = 0;
+	b43legacy_generate_plcp_hdr(&plcp, size + FCS_LEN, rate);
+	dur = ieee80211_generic_frame_duration(dev->wl->hw,
+					       dev->wl->if_id,
+					       size,
+					       B43legacy_RATE_TO_100KBPS(rate));
+	/* Write PLCP in two parts and timing for packet transfer */
+	tmp = le32_to_cpu(plcp.data);
+	b43legacy_shm_write16(dev, B43legacy_SHM_SHARED, shm_offset,
+			      tmp & 0xFFFF);
+	b43legacy_shm_write16(dev, B43legacy_SHM_SHARED, shm_offset + 2,
+			      tmp >> 16);
+	b43legacy_shm_write16(dev, B43legacy_SHM_SHARED, shm_offset + 6,
+			      le16_to_cpu(dur));
+}
+
+/* Instead of using custom probe response template, this function
+ * just patches custom beacon template by:
+ * 1) Changing packet type
+ * 2) Patching duration field
+ * 3) Stripping TIM
+ */
+static u8 *b43legacy_generate_probe_resp(struct b43legacy_wldev *dev,
+					 u16 *dest_size, u8 rate)
+{
+	const u8 *src_data;
+	u8 *dest_data;
+	u16 src_size;
+	u16 elem_size;
+	u16 src_pos;
+	u16 dest_pos;
+	__le16 dur;
+	struct ieee80211_hdr *hdr;
+
+	B43legacy_WARN_ON(!dev->cached_beacon);
+	src_size = dev->cached_beacon->len;
+	src_data = (const u8 *)dev->cached_beacon->data;
+
+	if (unlikely(src_size < 0x24)) {
+		b43legacydbg(dev->wl, "b43legacy_generate_probe_resp: "
+		       "invalid beacon\n");
+		return NULL;
+	}
+
+	dest_data = kmalloc(src_size, GFP_ATOMIC);
+	if (unlikely(!dest_data))
+		return NULL;
+
+	/* 0x24 is offset of first variable-len Information-Element
+	 * in beacon frame.
+	 */
+	memcpy(dest_data, src_data, 0x24);
+	src_pos = 0x24;
+	dest_pos = 0x24;
+	for (; src_pos < src_size - 2; src_pos += elem_size) {
+		elem_size = src_data[src_pos + 1] + 2;
+		if (src_data[src_pos] != 0x05) { /* TIM */
+			memcpy(dest_data + dest_pos, src_data + src_pos,
+			       elem_size);
+			dest_pos += elem_size;
+		}
+	}
+	*dest_size = dest_pos;
+	hdr = (struct ieee80211_hdr *)dest_data;
+
+	/* Set the frame control. */
+	hdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
+					 IEEE80211_STYPE_PROBE_RESP);
+	dur = ieee80211_generic_frame_duration(dev->wl->hw,
+					       dev->wl->if_id,
+					       *dest_size,
+					       B43legacy_RATE_TO_100KBPS(rate));
+	hdr->duration_id = dur;
+
+	return dest_data;
+}
+
+static void b43legacy_write_probe_resp_template(struct b43legacy_wldev *dev,
+						u16 ram_offset,
+						u16 shm_size_offset, u8 rate)
+{
+	u8 *probe_resp_data;
+	u16 size;
+
+	B43legacy_WARN_ON(!dev->cached_beacon);
+	size = dev->cached_beacon->len;
+	probe_resp_data = b43legacy_generate_probe_resp(dev, &size, rate);
+	if (unlikely(!probe_resp_data))
+		return;
+
+	/* Looks like PLCP headers plus packet timings are stored for
+	 * all possible basic rates
+	 */
+	b43legacy_write_probe_resp_plcp(dev, 0x31A, size,
+					B43legacy_CCK_RATE_1MB);
+	b43legacy_write_probe_resp_plcp(dev, 0x32C, size,
+					B43legacy_CCK_RATE_2MB);
+	b43legacy_write_probe_resp_plcp(dev, 0x33E, size,
+					B43legacy_CCK_RATE_5MB);
+	b43legacy_write_probe_resp_plcp(dev, 0x350, size,
+					B43legacy_CCK_RATE_11MB);
+
+	size = min((size_t)size,
+		   0x200 - sizeof(struct b43legacy_plcp_hdr6));
+	b43legacy_write_template_common(dev, probe_resp_data,
+					size, ram_offset,
+					shm_size_offset, rate);
+	kfree(probe_resp_data);
+}
+
+static int b43legacy_refresh_cached_beacon(struct b43legacy_wldev *dev,
+					   struct sk_buff *beacon)
+{
+	if (dev->cached_beacon)
+		kfree_skb(dev->cached_beacon);
+	dev->cached_beacon = beacon;
+
+	return 0;
+}
+
+static void b43legacy_update_templates(struct b43legacy_wldev *dev)
+{
+	u32 status;
+
+	B43legacy_WARN_ON(!dev->cached_beacon);
+
+	b43legacy_write_beacon_template(dev, 0x68, 0x18,
+					B43legacy_CCK_RATE_1MB);
+	b43legacy_write_beacon_template(dev, 0x468, 0x1A,
+					B43legacy_CCK_RATE_1MB);
+	b43legacy_write_probe_resp_template(dev, 0x268, 0x4A,
+					    B43legacy_CCK_RATE_11MB);
+
+	status = b43legacy_read32(dev, B43legacy_MMIO_STATUS2_BITFIELD);
+	status |= 0x03;
+	b43legacy_write32(dev, B43legacy_MMIO_STATUS2_BITFIELD, status);
+}
+
+static void b43legacy_refresh_templates(struct b43legacy_wldev *dev,
+					struct sk_buff *beacon)
+{
+	int err;
+
+	err = b43legacy_refresh_cached_beacon(dev, beacon);
+	if (unlikely(err))
+		return;
+	b43legacy_update_templates(dev);
+}
+
+static void b43legacy_set_ssid(struct b43legacy_wldev *dev,
+			       const u8 *ssid, u8 ssid_len)
+{
+	u32 tmp;
+	u16 i;
+	u16 len;
+
+	len = min((u16)ssid_len, (u16)0x100);
+	for (i = 0; i < len; i += sizeof(u32)) {
+		tmp = (u32)(ssid[i + 0]);
+		if (i + 1 < len)
+			tmp |= (u32)(ssid[i + 1]) << 8;
+		if (i + 2 < len)
+			tmp |= (u32)(ssid[i + 2]) << 16;
+		if (i + 3 < len)
+			tmp |= (u32)(ssid[i + 3]) << 24;
+		b43legacy_shm_write32(dev, B43legacy_SHM_SHARED,
+				      0x380 + i, tmp);
+	}
+	b43legacy_shm_write16(dev, B43legacy_SHM_SHARED,
+			      0x48, len);
+}
+
+static void b43legacy_set_beacon_int(struct b43legacy_wldev *dev,
+				     u16 beacon_int)
+{
+	b43legacy_time_lock(dev);
+	if (dev->dev->id.revision >= 3)
+		b43legacy_write32(dev, 0x188, (beacon_int << 16));
+	else {
+		b43legacy_write16(dev, 0x606, (beacon_int >> 6));
+		b43legacy_write16(dev, 0x610, beacon_int);
+	}
+	b43legacy_time_unlock(dev);
+}
+
+static void handle_irq_beacon(struct b43legacy_wldev *dev)
+{
+	u32 status;
+
+	if (!b43legacy_is_mode(dev->wl, IEEE80211_IF_TYPE_AP))
+		return;
+
+	dev->irq_savedstate &= ~B43legacy_IRQ_BEACON;
+	status = b43legacy_read32(dev, B43legacy_MMIO_STATUS2_BITFIELD);
+
+	if (!dev->cached_beacon || ((status & 0x1) && (status & 0x2))) {
+		/* ACK beacon IRQ. */
+		b43legacy_write32(dev, B43legacy_MMIO_GEN_IRQ_REASON,
+				  B43legacy_IRQ_BEACON);
+		dev->irq_savedstate |= B43legacy_IRQ_BEACON;
+		if (dev->cached_beacon)
+			kfree_skb(dev->cached_beacon);
+		dev->cached_beacon = NULL;
+		return;
+	}
+	if (!(status & 0x1)) {
+		b43legacy_write_beacon_template(dev, 0x68, 0x18,
+						B43legacy_CCK_RATE_1MB);
+		status |= 0x1;
+		b43legacy_write32(dev, B43legacy_MMIO_STATUS2_BITFIELD,
+				  status);
+	}
+	if (!(status & 0x2)) {
+		b43legacy_write_beacon_template(dev, 0x468, 0x1A,
+						B43legacy_CCK_RATE_1MB);
+		status |= 0x2;
+		b43legacy_write32(dev, B43legacy_MMIO_STATUS2_BITFIELD,
+				  status);
+	}
+}
+
+static void handle_irq_ucode_debug(struct b43legacy_wldev *dev)
+{
+}
+
+/* Interrupt handler bottom-half */
+static void b43legacy_interrupt_tasklet(struct b43legacy_wldev *dev)
+{
+	u32 reason;
+	u32 dma_reason[ARRAY_SIZE(dev->dma_reason)];
+	u32 merged_dma_reason = 0;
+	int i;
+	int activity = 0;
+	unsigned long flags;
+
+	spin_lock_irqsave(&dev->wl->irq_lock, flags);
+
+	B43legacy_WARN_ON(b43legacy_status(dev) <
+			  B43legacy_STAT_INITIALIZED);
+
+	reason = dev->irq_reason;
+	for (i = 0; i < ARRAY_SIZE(dma_reason); i++) {
+		dma_reason[i] = dev->dma_reason[i];
+		merged_dma_reason |= dma_reason[i];
+	}
+
+	if (unlikely(reason & B43legacy_IRQ_MAC_TXERR))
+		b43legacyerr(dev->wl, "MAC transmission error\n");
+
+	if (unlikely(reason & B43legacy_IRQ_PHY_TXERR))
+		b43legacyerr(dev->wl, "PHY transmission error\n");
+
+	if (unlikely(merged_dma_reason & (B43legacy_DMAIRQ_FATALMASK |
+					  B43legacy_DMAIRQ_NONFATALMASK))) {
+		if (merged_dma_reason & B43legacy_DMAIRQ_FATALMASK) {
+			b43legacyerr(dev->wl, "Fatal DMA error: "
+			       "0x%08X, 0x%08X, 0x%08X, "
+			       "0x%08X, 0x%08X, 0x%08X\n",
+			       dma_reason[0], dma_reason[1],
+			       dma_reason[2], dma_reason[3],
+			       dma_reason[4], dma_reason[5]);
+			b43legacy_controller_restart(dev, "DMA error");
+			mmiowb();
+			spin_unlock_irqrestore(&dev->wl->irq_lock, flags);
+			return;
+		}
+		if (merged_dma_reason & B43legacy_DMAIRQ_NONFATALMASK)
+			b43legacyerr(dev->wl, "DMA error: "
+			       "0x%08X, 0x%08X, 0x%08X, "
+			       "0x%08X, 0x%08X, 0x%08X\n",
+			       dma_reason[0], dma_reason[1],
+			       dma_reason[2], dma_reason[3],
+			       dma_reason[4], dma_reason[5]);
+	}
+
+	if (unlikely(reason & B43legacy_IRQ_UCODE_DEBUG))
+		handle_irq_ucode_debug(dev);
+	if (reason & B43legacy_IRQ_TBTT_INDI)
+		handle_irq_tbtt_indication(dev);
+	if (reason & B43legacy_IRQ_ATIM_END)
+		handle_irq_atim_end(dev);
+	if (reason & B43legacy_IRQ_BEACON)
+		handle_irq_beacon(dev);
+	if (reason & B43legacy_IRQ_PMQ)
+		handle_irq_pmq(dev);
+	if (reason & B43legacy_IRQ_TXFIFO_FLUSH_OK)
+		;/*TODO*/
+	if (reason & B43legacy_IRQ_NOISESAMPLE_OK)
+		handle_irq_noise(dev);
+
+	/* Check the DMA reason registers for received data. */
+	if (dma_reason[0] & B43legacy_DMAIRQ_RX_DONE) {
+		if (b43legacy_using_pio(dev))
+			b43legacy_pio_rx(dev->pio.queue0);
+		else
+			b43legacy_dma_rx(dev->dma.rx_ring0);
+		/* We intentionally don't set "activity" to 1, here. */
+	}
+	B43legacy_WARN_ON(dma_reason[1] & B43legacy_DMAIRQ_RX_DONE);
+	B43legacy_WARN_ON(dma_reason[2] & B43legacy_DMAIRQ_RX_DONE);
+	if (dma_reason[3] & B43legacy_DMAIRQ_RX_DONE) {
+		if (b43legacy_using_pio(dev))
+			b43legacy_pio_rx(dev->pio.queue3);
+		else
+			b43legacy_dma_rx(dev->dma.rx_ring3);
+		activity = 1;
+	}
+	B43legacy_WARN_ON(dma_reason[4] & B43legacy_DMAIRQ_RX_DONE);
+	B43legacy_WARN_ON(dma_reason[5] & B43legacy_DMAIRQ_RX_DONE);
+
+	if (reason & B43legacy_IRQ_TX_OK) {
+		handle_irq_transmit_status(dev);
+		activity = 1;
+		/* TODO: In AP mode, this also causes sending of powersave
+			 responses. */
+	}
+
+	if (!modparam_noleds)
+		b43legacy_leds_update(dev, activity);
+	b43legacy_interrupt_enable(dev, dev->irq_savedstate);
+	mmiowb();
+	spin_unlock_irqrestore(&dev->wl->irq_lock, flags);
+}
+
+static void pio_irq_workaround(struct b43legacy_wldev *dev,
+			       u16 base, int queueidx)
+{
+	u16 rxctl;
+
+	rxctl = b43legacy_read16(dev, base + B43legacy_PIO_RXCTL);
+	if (rxctl & B43legacy_PIO_RXCTL_DATAAVAILABLE)
+		dev->dma_reason[queueidx] |= B43legacy_DMAIRQ_RX_DONE;
+	else
+		dev->dma_reason[queueidx] &= ~B43legacy_DMAIRQ_RX_DONE;
+}
+
+static void b43legacy_interrupt_ack(struct b43legacy_wldev *dev, u32 reason)
+{
+	if (b43legacy_using_pio(dev) &&
+	    (dev->dev->id.revision < 3) &&
+	    (!(reason & B43legacy_IRQ_PIO_WORKAROUND))) {
+		/* Apply a PIO specific workaround to the dma_reasons */
+		pio_irq_workaround(dev, B43legacy_MMIO_PIO1_BASE, 0);
+		pio_irq_workaround(dev, B43legacy_MMIO_PIO2_BASE, 1);
+		pio_irq_workaround(dev, B43legacy_MMIO_PIO3_BASE, 2);
+		pio_irq_workaround(dev, B43legacy_MMIO_PIO4_BASE, 3);
+	}
+
+	b43legacy_write32(dev, B43legacy_MMIO_GEN_IRQ_REASON, reason);
+
+	b43legacy_write32(dev, B43legacy_MMIO_DMA0_REASON,
+			  dev->dma_reason[0]);
+	b43legacy_write32(dev, B43legacy_MMIO_DMA1_REASON,
+			  dev->dma_reason[1]);
+	b43legacy_write32(dev, B43legacy_MMIO_DMA2_REASON,
+			  dev->dma_reason[2]);
+	b43legacy_write32(dev, B43legacy_MMIO_DMA3_REASON,
+			  dev->dma_reason[3]);
+	b43legacy_write32(dev, B43legacy_MMIO_DMA4_REASON,
+			  dev->dma_reason[4]);
+	b43legacy_write32(dev, B43legacy_MMIO_DMA5_REASON,
+			  dev->dma_reason[5]);
+}
+
+/* Interrupt handler top-half */
+static irqreturn_t b43legacy_interrupt_handler(int irq, void *dev_id)
+{
+	irqreturn_t ret = IRQ_NONE;
+	struct b43legacy_wldev *dev = dev_id;
+	u32 reason;
+
+	if (!dev)
+		return IRQ_NONE;
+
+	spin_lock(&dev->wl->irq_lock);
+
+	if (b43legacy_status(dev) < B43legacy_STAT_STARTED)
+		goto out;
+	reason = b43legacy_read32(dev, B43legacy_MMIO_GEN_IRQ_REASON);
+	if (reason == 0xffffffff) /* shared IRQ */
+		goto out;
+	ret = IRQ_HANDLED;
+	reason &= b43legacy_read32(dev, B43legacy_MMIO_GEN_IRQ_MASK);
+	if (!reason)
+		goto out;
+
+	dev->dma_reason[0] = b43legacy_read32(dev,
+					      B43legacy_MMIO_DMA0_REASON)
+					      & 0x0001DC00;
+	dev->dma_reason[1] = b43legacy_read32(dev,
+					      B43legacy_MMIO_DMA1_REASON)
+					      & 0x0000DC00;
+	dev->dma_reason[2] = b43legacy_read32(dev,
+					      B43legacy_MMIO_DMA2_REASON)
+					      & 0x0000DC00;
+	dev->dma_reason[3] = b43legacy_read32(dev,
+					      B43legacy_MMIO_DMA3_REASON)
+					      & 0x0001DC00;
+	dev->dma_reason[4] = b43legacy_read32(dev,
+					      B43legacy_MMIO_DMA4_REASON)
+					      & 0x0000DC00;
+	dev->dma_reason[5] = b43legacy_read32(dev,
+					      B43legacy_MMIO_DMA5_REASON)
+					      & 0x0000DC00;
+
+	b43legacy_interrupt_ack(dev, reason);
+	/* disable all IRQs. They are enabled again in the bottom half. */
+	dev->irq_savedstate = b43legacy_interrupt_disable(dev,
+							  B43legacy_IRQ_ALL);
+	/* save the reason code and call our bottom half. */
+	dev->irq_reason = reason;
+	tasklet_schedule(&dev->isr_tasklet);
+out:
+	mmiowb();
+	spin_unlock(&dev->wl->irq_lock);
+
+	return ret;
+}
+
+static void b43legacy_release_firmware(struct b43legacy_wldev *dev)
+{
+	release_firmware(dev->fw.ucode);
+	dev->fw.ucode = NULL;
+	release_firmware(dev->fw.pcm);
+	dev->fw.pcm = NULL;
+	release_firmware(dev->fw.initvals);
+	dev->fw.initvals = NULL;
+	release_firmware(dev->fw.initvals_band);
+	dev->fw.initvals_band = NULL;
+}
+
+static void b43legacy_print_fw_helptext(struct b43legacy_wl *wl)
+{
+	b43legacyerr(wl, "You must go to http://linuxwireless.org/en/users/"
+		     "Drivers/bcm43xx#devicefirmware "
+		     "and download the correct firmware (version 3).\n");
+}
+
+static int do_request_fw(struct b43legacy_wldev *dev,
+			 const char *name,
+			 const struct firmware **fw)
+{
+	char path[sizeof(modparam_fwpostfix) + 32];
+	struct b43legacy_fw_header *hdr;
+	u32 size;
+	int err;
+
+	if (!name)
+		return 0;
+
+	snprintf(path, ARRAY_SIZE(path),
+		 "b43legacy%s/%s.fw",
+		 modparam_fwpostfix, name);
+	err = request_firmware(fw, path, dev->dev->dev);
+	if (err) {
+		b43legacyerr(dev->wl, "Firmware file \"%s\" not found "
+		       "or load failed.\n", path);
+		return err;
+	}
+	if ((*fw)->size < sizeof(struct b43legacy_fw_header))
+		goto err_format;
+	hdr = (struct b43legacy_fw_header *)((*fw)->data);
+	switch (hdr->type) {
+	case B43legacy_FW_TYPE_UCODE:
+	case B43legacy_FW_TYPE_PCM:
+		size = be32_to_cpu(hdr->size);
+		if (size != (*fw)->size - sizeof(struct b43legacy_fw_header))
+			goto err_format;
+		/* fallthrough */
+	case B43legacy_FW_TYPE_IV:
+		if (hdr->ver != 1)
+			goto err_format;
+		break;
+	default:
+		goto err_format;
+	}
+
+	return err;
+
+err_format:
+	b43legacyerr(dev->wl, "Firmware file \"%s\" format error.\n", path);
+	return -EPROTO;
+}
+
+static int b43legacy_request_firmware(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_firmware *fw = &dev->fw;
+	const u8 rev = dev->dev->id.revision;
+	const char *filename;
+	u32 tmshigh;
+	int err;
+
+	tmshigh = ssb_read32(dev->dev, SSB_TMSHIGH);
+	if (!fw->ucode) {
+		if (rev == 2)
+			filename = "ucode2";
+		else if (rev == 4)
+			filename = "ucode4";
+		else
+			filename = "ucode5";
+		err = do_request_fw(dev, filename, &fw->ucode);
+		if (err)
+			goto err_load;
+	}
+	if (!fw->pcm) {
+		if (rev < 5)
+			filename = "pcm4";
+		else
+			filename = "pcm5";
+		err = do_request_fw(dev, filename, &fw->pcm);
+		if (err)
+			goto err_load;
+	}
+	if (!fw->initvals) {
+		switch (dev->phy.type) {
+		case B43legacy_PHYTYPE_G:
+			if ((rev >= 5) && (rev <= 10))
+				filename = "b0g0initvals5";
+			else if (rev == 2 || rev == 4)
+				filename = "b0g0initvals2";
+			else
+				goto err_no_initvals;
+			break;
+		default:
+			goto err_no_initvals;
+		}
+		err = do_request_fw(dev, filename, &fw->initvals);
+		if (err)
+			goto err_load;
+	}
+	if (!fw->initvals_band) {
+		switch (dev->phy.type) {
+		case B43legacy_PHYTYPE_G:
+			if ((rev >= 5) && (rev <= 10))
+				filename = "b0g0bsinitvals5";
+			else if (rev >= 11)
+				filename = NULL;
+			else if (rev == 2 || rev == 4)
+				filename = NULL;
+			else
+				goto err_no_initvals;
+			break;
+		default:
+			goto err_no_initvals;
+		}
+		err = do_request_fw(dev, filename, &fw->initvals_band);
+		if (err)
+			goto err_load;
+	}
+
+	return 0;
+
+err_load:
+	b43legacy_print_fw_helptext(dev->wl);
+	goto error;
+
+err_no_initvals:
+	err = -ENODEV;
+	b43legacyerr(dev->wl, "No Initial Values firmware file for PHY %u, "
+	       "core rev %u\n", dev->phy.type, rev);
+	goto error;
+
+error:
+	b43legacy_release_firmware(dev);
+	return err;
+}
+
+static int b43legacy_upload_microcode(struct b43legacy_wldev *dev)
+{
+	const size_t hdr_len = sizeof(struct b43legacy_fw_header);
+	const __be32 *data;
+	unsigned int i;
+	unsigned int len;
+	u16 fwrev;
+	u16 fwpatch;
+	u16 fwdate;
+	u16 fwtime;
+	u32 tmp;
+	int err = 0;
+
+	/* Upload Microcode. */
+	data = (__be32 *) (dev->fw.ucode->data + hdr_len);
+	len = (dev->fw.ucode->size - hdr_len) / sizeof(__be32);
+	b43legacy_shm_control_word(dev,
+				   B43legacy_SHM_UCODE |
+				   B43legacy_SHM_AUTOINC_W,
+				   0x0000);
+	for (i = 0; i < len; i++) {
+		b43legacy_write32(dev, B43legacy_MMIO_SHM_DATA,
+				    be32_to_cpu(data[i]));
+		udelay(10);
+	}
+
+	if (dev->fw.pcm) {
+		/* Upload PCM data. */
+		data = (__be32 *) (dev->fw.pcm->data + hdr_len);
+		len = (dev->fw.pcm->size - hdr_len) / sizeof(__be32);
+		b43legacy_shm_control_word(dev, B43legacy_SHM_HW, 0x01EA);
+		b43legacy_write32(dev, B43legacy_MMIO_SHM_DATA, 0x00004000);
+		/* No need for autoinc bit in SHM_HW */
+		b43legacy_shm_control_word(dev, B43legacy_SHM_HW, 0x01EB);
+		for (i = 0; i < len; i++) {
+			b43legacy_write32(dev, B43legacy_MMIO_SHM_DATA,
+					  be32_to_cpu(data[i]));
+			udelay(10);
+		}
+	}
+
+	b43legacy_write32(dev, B43legacy_MMIO_GEN_IRQ_REASON,
+			  B43legacy_IRQ_ALL);
+	b43legacy_write32(dev, B43legacy_MMIO_STATUS_BITFIELD, 0x00020402);
+
+	/* Wait for the microcode to load and respond */
+	i = 0;
+	while (1) {
+		tmp = b43legacy_read32(dev, B43legacy_MMIO_GEN_IRQ_REASON);
+		if (tmp == B43legacy_IRQ_MAC_SUSPENDED)
+			break;
+		i++;
+		if (i >= B43legacy_IRQWAIT_MAX_RETRIES) {
+			b43legacyerr(dev->wl, "Microcode not responding\n");
+			b43legacy_print_fw_helptext(dev->wl);
+			err = -ENODEV;
+			goto out;
+		}
+		udelay(10);
+	}
+	/* dummy read follows */
+	b43legacy_read32(dev, B43legacy_MMIO_GEN_IRQ_REASON);
+
+	/* Get and check the revisions. */
+	fwrev = b43legacy_shm_read16(dev, B43legacy_SHM_SHARED,
+				     B43legacy_SHM_SH_UCODEREV);
+	fwpatch = b43legacy_shm_read16(dev, B43legacy_SHM_SHARED,
+				       B43legacy_SHM_SH_UCODEPATCH);
+	fwdate = b43legacy_shm_read16(dev, B43legacy_SHM_SHARED,
+				      B43legacy_SHM_SH_UCODEDATE);
+	fwtime = b43legacy_shm_read16(dev, B43legacy_SHM_SHARED,
+				      B43legacy_SHM_SH_UCODETIME);
+
+	if (fwrev > 0x128) {
+		b43legacyerr(dev->wl, "YOU ARE TRYING TO LOAD V4 FIRMWARE."
+			     " Only firmware from binary drivers version 3.x"
+			     " is supported. You must change your firmware"
+			     " files.\n");
+		b43legacy_print_fw_helptext(dev->wl);
+		b43legacy_write32(dev, B43legacy_MMIO_STATUS_BITFIELD, 0);
+		err = -EOPNOTSUPP;
+		goto out;
+	}
+	b43legacydbg(dev->wl, "Loading firmware version 0x%X, patch level %u "
+	       "(20%.2i-%.2i-%.2i %.2i:%.2i:%.2i)\n", fwrev, fwpatch,
+	       (fwdate >> 12) & 0xF, (fwdate >> 8) & 0xF, fwdate & 0xFF,
+	       (fwtime >> 11) & 0x1F, (fwtime >> 5) & 0x3F, fwtime & 0x1F);
+
+	dev->fw.rev = fwrev;
+	dev->fw.patch = fwpatch;
+
+out:
+	return err;
+}
+
+static int b43legacy_write_initvals(struct b43legacy_wldev *dev,
+				    const struct b43legacy_iv *ivals,
+				    size_t count,
+				    size_t array_size)
+{
+	const struct b43legacy_iv *iv;
+	u16 offset;
+	size_t i;
+	bool bit32;
+
+	BUILD_BUG_ON(sizeof(struct b43legacy_iv) != 6);
+	iv = ivals;
+	for (i = 0; i < count; i++) {
+		if (array_size < sizeof(iv->offset_size))
+			goto err_format;
+		array_size -= sizeof(iv->offset_size);
+		offset = be16_to_cpu(iv->offset_size);
+		bit32 = !!(offset & B43legacy_IV_32BIT);
+		offset &= B43legacy_IV_OFFSET_MASK;
+		if (offset >= 0x1000)
+			goto err_format;
+		if (bit32) {
+			u32 value;
+
+			if (array_size < sizeof(iv->data.d32))
+				goto err_format;
+			array_size -= sizeof(iv->data.d32);
+
+			value = be32_to_cpu(get_unaligned(&iv->data.d32));
+			b43legacy_write32(dev, offset, value);
+
+			iv = (const struct b43legacy_iv *)((const uint8_t *)iv +
+							sizeof(__be16) +
+							sizeof(__be32));
+		} else {
+			u16 value;
+
+			if (array_size < sizeof(iv->data.d16))
+				goto err_format;
+			array_size -= sizeof(iv->data.d16);
+
+			value = be16_to_cpu(iv->data.d16);
+			b43legacy_write16(dev, offset, value);
+
+			iv = (const struct b43legacy_iv *)((const uint8_t *)iv +
+							sizeof(__be16) +
+							sizeof(__be16));
+		}
+	}
+	if (array_size)
+		goto err_format;
+
+	return 0;
+
+err_format:
+	b43legacyerr(dev->wl, "Initial Values Firmware file-format error.\n");
+	b43legacy_print_fw_helptext(dev->wl);
+
+	return -EPROTO;
+}
+
+static int b43legacy_upload_initvals(struct b43legacy_wldev *dev)
+{
+	const size_t hdr_len = sizeof(struct b43legacy_fw_header);
+	const struct b43legacy_fw_header *hdr;
+	struct b43legacy_firmware *fw = &dev->fw;
+	const struct b43legacy_iv *ivals;
+	size_t count;
+	int err;
+
+	hdr = (const struct b43legacy_fw_header *)(fw->initvals->data);
+	ivals = (const struct b43legacy_iv *)(fw->initvals->data + hdr_len);
+	count = be32_to_cpu(hdr->size);
+	err = b43legacy_write_initvals(dev, ivals, count,
+				 fw->initvals->size - hdr_len);
+	if (err)
+		goto out;
+	if (fw->initvals_band) {
+		hdr = (const struct b43legacy_fw_header *)
+		      (fw->initvals_band->data);
+		ivals = (const struct b43legacy_iv *)(fw->initvals_band->data
+			+ hdr_len);
+		count = be32_to_cpu(hdr->size);
+		err = b43legacy_write_initvals(dev, ivals, count,
+					 fw->initvals_band->size - hdr_len);
+		if (err)
+			goto out;
+	}
+out:
+
+	return err;
+}
+
+/* Initialize the GPIOs
+ * http://bcm-specs.sipsolutions.net/GPIO
+ */
+static int b43legacy_gpio_init(struct b43legacy_wldev *dev)
+{
+	struct ssb_bus *bus = dev->dev->bus;
+	struct ssb_device *gpiodev, *pcidev = NULL;
+	u32 mask;
+	u32 set;
+
+	b43legacy_write32(dev, B43legacy_MMIO_STATUS_BITFIELD,
+			  b43legacy_read32(dev,
+			  B43legacy_MMIO_STATUS_BITFIELD)
+			  & 0xFFFF3FFF);
+
+	b43legacy_leds_switch_all(dev, 0);
+	b43legacy_write16(dev, B43legacy_MMIO_GPIO_MASK,
+			  b43legacy_read16(dev,
+			  B43legacy_MMIO_GPIO_MASK)
+			  | 0x000F);
+
+	mask = 0x0000001F;
+	set = 0x0000000F;
+	if (dev->dev->bus->chip_id == 0x4301) {
+		mask |= 0x0060;
+		set |= 0x0060;
+	}
+	if (dev->dev->bus->sprom.r1.boardflags_lo & B43legacy_BFL_PACTRL) {
+		b43legacy_write16(dev, B43legacy_MMIO_GPIO_MASK,
+				  b43legacy_read16(dev,
+				  B43legacy_MMIO_GPIO_MASK)
+				  | 0x0200);
+		mask |= 0x0200;
+		set |= 0x0200;
+	}
+	if (dev->dev->id.revision >= 2)
+		mask  |= 0x0010; /* FIXME: This is redundant. */
+
+#ifdef CONFIG_SSB_DRIVER_PCICORE
+	pcidev = bus->pcicore.dev;
+#endif
+	gpiodev = bus->chipco.dev ? : pcidev;
+	if (!gpiodev)
+		return 0;
+	ssb_write32(gpiodev, B43legacy_GPIO_CONTROL,
+		    (ssb_read32(gpiodev, B43legacy_GPIO_CONTROL)
+		     & mask) | set);
+
+	return 0;
+}
+
+/* Turn off all GPIO stuff. Call this on module unload, for example. */
+static void b43legacy_gpio_cleanup(struct b43legacy_wldev *dev)
+{
+	struct ssb_bus *bus = dev->dev->bus;
+	struct ssb_device *gpiodev, *pcidev = NULL;
+
+#ifdef CONFIG_SSB_DRIVER_PCICORE
+	pcidev = bus->pcicore.dev;
+#endif
+	gpiodev = bus->chipco.dev ? : pcidev;
+	if (!gpiodev)
+		return;
+	ssb_write32(gpiodev, B43legacy_GPIO_CONTROL, 0);
+}
+
+/* http://bcm-specs.sipsolutions.net/EnableMac */
+void b43legacy_mac_enable(struct b43legacy_wldev *dev)
+{
+	dev->mac_suspended--;
+	B43legacy_WARN_ON(dev->mac_suspended < 0);
+	if (dev->mac_suspended == 0) {
+		b43legacy_write32(dev, B43legacy_MMIO_STATUS_BITFIELD,
+				  b43legacy_read32(dev,
+				  B43legacy_MMIO_STATUS_BITFIELD)
+				  | B43legacy_SBF_MAC_ENABLED);
+		b43legacy_write32(dev, B43legacy_MMIO_GEN_IRQ_REASON,
+				  B43legacy_IRQ_MAC_SUSPENDED);
+		/* the next two are dummy reads */
+		b43legacy_read32(dev, B43legacy_MMIO_STATUS_BITFIELD);
+		b43legacy_read32(dev, B43legacy_MMIO_GEN_IRQ_REASON);
+		b43legacy_power_saving_ctl_bits(dev, -1, -1);
+	}
+}
+
+/* http://bcm-specs.sipsolutions.net/SuspendMAC */
+void b43legacy_mac_suspend(struct b43legacy_wldev *dev)
+{
+	int i;
+	u32 tmp;
+
+	B43legacy_WARN_ON(dev->mac_suspended < 0);
+	if (dev->mac_suspended == 0) {
+		b43legacy_power_saving_ctl_bits(dev, -1, 1);
+		b43legacy_write32(dev, B43legacy_MMIO_STATUS_BITFIELD,
+				  b43legacy_read32(dev,
+				  B43legacy_MMIO_STATUS_BITFIELD)
+				  & ~B43legacy_SBF_MAC_ENABLED);
+		b43legacy_read32(dev, B43legacy_MMIO_GEN_IRQ_REASON);
+		for (i = 10000; i; i--) {
+			tmp = b43legacy_read32(dev,
+					       B43legacy_MMIO_GEN_IRQ_REASON);
+			if (tmp & B43legacy_IRQ_MAC_SUSPENDED)
+				goto out;
+			udelay(1);
+		}
+		b43legacyerr(dev->wl, "MAC suspend failed\n");
+	}
+out:
+	dev->mac_suspended++;
+}
+
+static void b43legacy_adjust_opmode(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_wl *wl = dev->wl;
+	u32 ctl;
+	u16 cfp_pretbtt;
+
+	ctl = b43legacy_read32(dev, B43legacy_MMIO_MACCTL);
+	/* Reset status to STA infrastructure mode. */
+	ctl &= ~B43legacy_MACCTL_AP;
+	ctl &= ~B43legacy_MACCTL_KEEP_CTL;
+	ctl &= ~B43legacy_MACCTL_KEEP_BADPLCP;
+	ctl &= ~B43legacy_MACCTL_KEEP_BAD;
+	ctl &= ~B43legacy_MACCTL_PROMISC;
+	ctl &= ~B43legacy_MACCTL_BEACPROMISC;
+	ctl |= B43legacy_MACCTL_INFRA;
+
+	if (b43legacy_is_mode(wl, IEEE80211_IF_TYPE_AP))
+		ctl |= B43legacy_MACCTL_AP;
+	else if (b43legacy_is_mode(wl, IEEE80211_IF_TYPE_IBSS))
+		ctl &= ~B43legacy_MACCTL_INFRA;
+
+	if (wl->filter_flags & FIF_CONTROL)
+		ctl |= B43legacy_MACCTL_KEEP_CTL;
+	if (wl->filter_flags & FIF_FCSFAIL)
+		ctl |= B43legacy_MACCTL_KEEP_BAD;
+	if (wl->filter_flags & FIF_PLCPFAIL)
+		ctl |= B43legacy_MACCTL_KEEP_BADPLCP;
+	if (wl->filter_flags & FIF_PROMISC_IN_BSS)
+		ctl |= B43legacy_MACCTL_PROMISC;
+	if (wl->filter_flags & FIF_BCN_PRBRESP_PROMISC)
+		ctl |= B43legacy_MACCTL_BEACPROMISC;
+
+	/* Workaround: On old hardware the HW-MAC-address-filter
+	 * doesn't work properly, so always run promisc in filter
+	 * it in software. */
+	if (dev->dev->id.revision <= 4)
+		ctl |= B43legacy_MACCTL_PROMISC;
+
+	b43legacy_write32(dev, B43legacy_MMIO_MACCTL, ctl);
+
+	cfp_pretbtt = 2;
+	if ((ctl & B43legacy_MACCTL_INFRA) &&
+	    !(ctl & B43legacy_MACCTL_AP)) {
+		if (dev->dev->bus->chip_id == 0x4306 &&
+		    dev->dev->bus->chip_rev == 3)
+			cfp_pretbtt = 100;
+		else
+			cfp_pretbtt = 50;
+	}
+	b43legacy_write16(dev, 0x612, cfp_pretbtt);
+}
+
+static void b43legacy_rate_memory_write(struct b43legacy_wldev *dev,
+					u16 rate,
+					int is_ofdm)
+{
+	u16 offset;
+
+	if (is_ofdm) {
+		offset = 0x480;
+		offset += (b43legacy_plcp_get_ratecode_ofdm(rate) & 0x000F) * 2;
+	} else {
+		offset = 0x4C0;
+		offset += (b43legacy_plcp_get_ratecode_cck(rate) & 0x000F) * 2;
+	}
+	b43legacy_shm_write16(dev, B43legacy_SHM_SHARED, offset + 0x20,
+			      b43legacy_shm_read16(dev,
+			      B43legacy_SHM_SHARED, offset));
+}
+
+static void b43legacy_rate_memory_init(struct b43legacy_wldev *dev)
+{
+	switch (dev->phy.type) {
+	case B43legacy_PHYTYPE_G:
+		b43legacy_rate_memory_write(dev, B43legacy_OFDM_RATE_6MB, 1);
+		b43legacy_rate_memory_write(dev, B43legacy_OFDM_RATE_12MB, 1);
+		b43legacy_rate_memory_write(dev, B43legacy_OFDM_RATE_18MB, 1);
+		b43legacy_rate_memory_write(dev, B43legacy_OFDM_RATE_24MB, 1);
+		b43legacy_rate_memory_write(dev, B43legacy_OFDM_RATE_36MB, 1);
+		b43legacy_rate_memory_write(dev, B43legacy_OFDM_RATE_48MB, 1);
+		b43legacy_rate_memory_write(dev, B43legacy_OFDM_RATE_54MB, 1);
+		/* fallthrough */
+	case B43legacy_PHYTYPE_B:
+		b43legacy_rate_memory_write(dev, B43legacy_CCK_RATE_1MB, 0);
+		b43legacy_rate_memory_write(dev, B43legacy_CCK_RATE_2MB, 0);
+		b43legacy_rate_memory_write(dev, B43legacy_CCK_RATE_5MB, 0);
+		b43legacy_rate_memory_write(dev, B43legacy_CCK_RATE_11MB, 0);
+		break;
+	default:
+		B43legacy_BUG_ON(1);
+	}
+}
+
+/* Set the TX-Antenna for management frames sent by firmware. */
+static void b43legacy_mgmtframe_txantenna(struct b43legacy_wldev *dev,
+					  int antenna)
+{
+	u16 ant = 0;
+	u16 tmp;
+
+	switch (antenna) {
+	case B43legacy_ANTENNA0:
+		ant |= B43legacy_TX4_PHY_ANT0;
+		break;
+	case B43legacy_ANTENNA1:
+		ant |= B43legacy_TX4_PHY_ANT1;
+		break;
+	case B43legacy_ANTENNA_AUTO:
+		ant |= B43legacy_TX4_PHY_ANTLAST;
+		break;
+	default:
+		B43legacy_BUG_ON(1);
+	}
+
+	/* FIXME We also need to set the other flags of the PHY control
+	 * field somewhere. */
+
+	/* For Beacons */
+	tmp = b43legacy_shm_read16(dev, B43legacy_SHM_SHARED,
+				   B43legacy_SHM_SH_BEACPHYCTL);
+	tmp = (tmp & ~B43legacy_TX4_PHY_ANT) | ant;
+	b43legacy_shm_write16(dev, B43legacy_SHM_SHARED,
+			      B43legacy_SHM_SH_BEACPHYCTL, tmp);
+	/* For ACK/CTS */
+	tmp = b43legacy_shm_read16(dev, B43legacy_SHM_SHARED,
+				   B43legacy_SHM_SH_ACKCTSPHYCTL);
+	tmp = (tmp & ~B43legacy_TX4_PHY_ANT) | ant;
+	b43legacy_shm_write16(dev, B43legacy_SHM_SHARED,
+			      B43legacy_SHM_SH_ACKCTSPHYCTL, tmp);
+	/* For Probe Resposes */
+	tmp = b43legacy_shm_read16(dev, B43legacy_SHM_SHARED,
+				   B43legacy_SHM_SH_PRPHYCTL);
+	tmp = (tmp & ~B43legacy_TX4_PHY_ANT) | ant;
+	b43legacy_shm_write16(dev, B43legacy_SHM_SHARED,
+			      B43legacy_SHM_SH_PRPHYCTL, tmp);
+}
+
+/* Returns TRUE, if the radio is enabled in hardware. */
+static bool b43legacy_is_hw_radio_enabled(struct b43legacy_wldev *dev)
+{
+	if (dev->phy.rev >= 3) {
+		if (!(b43legacy_read32(dev, B43legacy_MMIO_RADIO_HWENABLED_HI)
+		      & B43legacy_MMIO_RADIO_HWENABLED_HI_MASK))
+			return 1;
+	} else {
+		if (b43legacy_read16(dev, B43legacy_MMIO_RADIO_HWENABLED_LO)
+		    & B43legacy_MMIO_RADIO_HWENABLED_LO_MASK)
+			return 1;
+	}
+	return 0;
+}
+
+/* This is the opposite of b43legacy_chip_init() */
+static void b43legacy_chip_exit(struct b43legacy_wldev *dev)
+{
+	b43legacy_radio_turn_off(dev);
+	if (!modparam_noleds)
+		b43legacy_leds_exit(dev);
+	b43legacy_gpio_cleanup(dev);
+	/* firmware is released later */
+}
+
+/* Initialize the chip
+ * http://bcm-specs.sipsolutions.net/ChipInit
+ */
+static int b43legacy_chip_init(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+	int err;
+	int tmp;
+	u32 value32;
+	u16 value16;
+
+	b43legacy_write32(dev, B43legacy_MMIO_STATUS_BITFIELD,
+			  B43legacy_SBF_CORE_READY
+			  | B43legacy_SBF_400);
+
+	err = b43legacy_request_firmware(dev);
+	if (err)
+		goto out;
+	err = b43legacy_upload_microcode(dev);
+	if (err)
+		goto out; /* firmware is released later */
+
+	err = b43legacy_gpio_init(dev);
+	if (err)
+		goto out; /* firmware is released later */
+	err = b43legacy_upload_initvals(dev);
+	if (err)
+		goto err_gpio_cleanup;
+	b43legacy_radio_turn_on(dev);
+
+	b43legacy_write16(dev, 0x03E6, 0x0000);
+	err = b43legacy_phy_init(dev);
+	if (err)
+		goto err_radio_off;
+
+	/* Select initial Interference Mitigation. */
+	tmp = phy->interfmode;
+	phy->interfmode = B43legacy_INTERFMODE_NONE;
+	b43legacy_radio_set_interference_mitigation(dev, tmp);
+
+	b43legacy_phy_set_antenna_diversity(dev);
+	b43legacy_mgmtframe_txantenna(dev, B43legacy_ANTENNA_DEFAULT);
+
+	if (phy->type == B43legacy_PHYTYPE_B) {
+		value16 = b43legacy_read16(dev, 0x005E);
+		value16 |= 0x0004;
+		b43legacy_write16(dev, 0x005E, value16);
+	}
+	b43legacy_write32(dev, 0x0100, 0x01000000);
+	if (dev->dev->id.revision < 5)
+		b43legacy_write32(dev, 0x010C, 0x01000000);
+
+	value32 = b43legacy_read32(dev, B43legacy_MMIO_STATUS_BITFIELD);
+	value32 &= ~B43legacy_SBF_MODE_NOTADHOC;
+	b43legacy_write32(dev, B43legacy_MMIO_STATUS_BITFIELD, value32);
+	value32 = b43legacy_read32(dev, B43legacy_MMIO_STATUS_BITFIELD);
+	value32 |= B43legacy_SBF_MODE_NOTADHOC;
+	b43legacy_write32(dev, B43legacy_MMIO_STATUS_BITFIELD, value32);
+
+	if (b43legacy_using_pio(dev)) {
+		b43legacy_write32(dev, 0x0210, 0x00000100);
+		b43legacy_write32(dev, 0x0230, 0x00000100);
+		b43legacy_write32(dev, 0x0250, 0x00000100);
+		b43legacy_write32(dev, 0x0270, 0x00000100);
+		b43legacy_shm_write16(dev, B43legacy_SHM_SHARED, 0x0034,
+				      0x0000);
+	}
+
+	/* Probe Response Timeout value */
+	/* FIXME: Default to 0, has to be set by ioctl probably... :-/ */
+	b43legacy_shm_write16(dev, B43legacy_SHM_SHARED, 0x0074, 0x0000);
+
+	/* Initially set the wireless operation mode. */
+	b43legacy_adjust_opmode(dev);
+
+	if (dev->dev->id.revision < 3) {
+		b43legacy_write16(dev, 0x060E, 0x0000);
+		b43legacy_write16(dev, 0x0610, 0x8000);
+		b43legacy_write16(dev, 0x0604, 0x0000);
+		b43legacy_write16(dev, 0x0606, 0x0200);
+	} else {
+		b43legacy_write32(dev, 0x0188, 0x80000000);
+		b43legacy_write32(dev, 0x018C, 0x02000000);
+	}
+	b43legacy_write32(dev, B43legacy_MMIO_GEN_IRQ_REASON, 0x00004000);
+	b43legacy_write32(dev, B43legacy_MMIO_DMA0_IRQ_MASK, 0x0001DC00);
+	b43legacy_write32(dev, B43legacy_MMIO_DMA1_IRQ_MASK, 0x0000DC00);
+	b43legacy_write32(dev, B43legacy_MMIO_DMA2_IRQ_MASK, 0x0000DC00);
+	b43legacy_write32(dev, B43legacy_MMIO_DMA3_IRQ_MASK, 0x0001DC00);
+	b43legacy_write32(dev, B43legacy_MMIO_DMA4_IRQ_MASK, 0x0000DC00);
+	b43legacy_write32(dev, B43legacy_MMIO_DMA5_IRQ_MASK, 0x0000DC00);
+
+	value32 = ssb_read32(dev->dev, SSB_TMSLOW);
+	value32 |= 0x00100000;
+	ssb_write32(dev->dev, SSB_TMSLOW, value32);
+
+	b43legacy_write16(dev, B43legacy_MMIO_POWERUP_DELAY,
+			  dev->dev->bus->chipco.fast_pwrup_delay);
+
+	B43legacy_WARN_ON(err != 0);
+	b43legacydbg(dev->wl, "Chip initialized\n");
+out:
+	return err;
+
+err_radio_off:
+	b43legacy_radio_turn_off(dev);
+err_gpio_cleanup:
+	b43legacy_gpio_cleanup(dev);
+	goto out;
+}
+
+static void b43legacy_periodic_every120sec(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+
+	if (phy->type != B43legacy_PHYTYPE_G || phy->rev < 2)
+		return;
+
+	b43legacy_mac_suspend(dev);
+	b43legacy_phy_lo_g_measure(dev);
+	b43legacy_mac_enable(dev);
+}
+
+static void b43legacy_periodic_every60sec(struct b43legacy_wldev *dev)
+{
+	b43legacy_phy_lo_mark_all_unused(dev);
+	if (dev->dev->bus->sprom.r1.boardflags_lo & B43legacy_BFL_RSSI) {
+		b43legacy_mac_suspend(dev);
+		b43legacy_calc_nrssi_slope(dev);
+		b43legacy_mac_enable(dev);
+	}
+}
+
+static void b43legacy_periodic_every30sec(struct b43legacy_wldev *dev)
+{
+	/* Update device statistics. */
+	b43legacy_calculate_link_quality(dev);
+}
+
+static void b43legacy_periodic_every15sec(struct b43legacy_wldev *dev)
+{
+	b43legacy_phy_xmitpower(dev); /* FIXME: unless scanning? */
+}
+
+static void b43legacy_periodic_every1sec(struct b43legacy_wldev *dev)
+{
+	bool radio_hw_enable;
+
+	/* check if radio hardware enabled status changed */
+	radio_hw_enable = b43legacy_is_hw_radio_enabled(dev);
+	if (unlikely(dev->radio_hw_enable != radio_hw_enable)) {
+		dev->radio_hw_enable = radio_hw_enable;
+		b43legacyinfo(dev->wl, "Radio hardware status changed to %s\n",
+		       (radio_hw_enable) ? "enabled" : "disabled");
+		b43legacy_leds_update(dev, 0);
+	}
+}
+
+static void do_periodic_work(struct b43legacy_wldev *dev)
+{
+	unsigned int state;
+
+	state = dev->periodic_state;
+	if (state % 120 == 0)
+		b43legacy_periodic_every120sec(dev);
+	if (state % 60 == 0)
+		b43legacy_periodic_every60sec(dev);
+	if (state % 30 == 0)
+		b43legacy_periodic_every30sec(dev);
+	if (state % 15 == 0)
+		b43legacy_periodic_every15sec(dev);
+	b43legacy_periodic_every1sec(dev);
+}
+
+/* Estimate a "Badness" value based on the periodic work
+ * state-machine state. "Badness" is worse (bigger), if the
+ * periodic work will take longer.
+ */
+static int estimate_periodic_work_badness(unsigned int state)
+{
+	int badness = 0;
+
+	if (state % 120 == 0) /* every 120 sec */
+		badness += 10;
+	if (state % 60 == 0) /* every 60 sec */
+		badness += 5;
+	if (state % 30 == 0) /* every 30 sec */
+		badness += 1;
+	if (state % 15 == 0) /* every 15 sec */
+		badness += 1;
+
+#define BADNESS_LIMIT	4
+	return badness;
+}
+
+static void b43legacy_periodic_work_handler(struct work_struct *work)
+{
+	struct b43legacy_wldev *dev =
+			     container_of(work, struct b43legacy_wldev,
+			     periodic_work.work);
+	unsigned long flags;
+	unsigned long delay;
+	u32 savedirqs = 0;
+	int badness;
+
+	mutex_lock(&dev->wl->mutex);
+
+	if (unlikely(b43legacy_status(dev) != B43legacy_STAT_STARTED))
+		goto out;
+	if (b43legacy_debug(dev, B43legacy_DBG_PWORK_STOP))
+		goto out_requeue;
+
+	badness = estimate_periodic_work_badness(dev->periodic_state);
+	if (badness > BADNESS_LIMIT) {
+		spin_lock_irqsave(&dev->wl->irq_lock, flags);
+		/* Suspend TX as we don't want to transmit packets while
+		 * we recalibrate the hardware. */
+		b43legacy_tx_suspend(dev);
+		savedirqs = b43legacy_interrupt_disable(dev,
+							  B43legacy_IRQ_ALL);
+		/* Periodic work will take a long time, so we want it to
+		 * be preemtible and release the spinlock. */
+		spin_unlock_irqrestore(&dev->wl->irq_lock, flags);
+		b43legacy_synchronize_irq(dev);
+
+		do_periodic_work(dev);
+
+		spin_lock_irqsave(&dev->wl->irq_lock, flags);
+		b43legacy_interrupt_enable(dev, savedirqs);
+		b43legacy_tx_resume(dev);
+		mmiowb();
+		spin_unlock_irqrestore(&dev->wl->irq_lock, flags);
+	} else {
+		/* Take the global driver lock. This will lock any operation. */
+		spin_lock_irqsave(&dev->wl->irq_lock, flags);
+
+		do_periodic_work(dev);
+
+		mmiowb();
+		spin_unlock_irqrestore(&dev->wl->irq_lock, flags);
+	}
+	dev->periodic_state++;
+out_requeue:
+	if (b43legacy_debug(dev, B43legacy_DBG_PWORK_FAST))
+		delay = msecs_to_jiffies(50);
+	else
+		delay = round_jiffies(HZ);
+	queue_delayed_work(dev->wl->hw->workqueue,
+			   &dev->periodic_work, delay);
+out:
+	mutex_unlock(&dev->wl->mutex);
+}
+
+static void b43legacy_periodic_tasks_setup(struct b43legacy_wldev *dev)
+{
+	struct delayed_work *work = &dev->periodic_work;
+
+	dev->periodic_state = 0;
+	INIT_DELAYED_WORK(work, b43legacy_periodic_work_handler);
+	queue_delayed_work(dev->wl->hw->workqueue, work, 0);
+}
+
+/* Validate access to the chip (SHM) */
+static int b43legacy_validate_chipaccess(struct b43legacy_wldev *dev)
+{
+	u32 value;
+	u32 shm_backup;
+
+	shm_backup = b43legacy_shm_read32(dev, B43legacy_SHM_SHARED, 0);
+	b43legacy_shm_write32(dev, B43legacy_SHM_SHARED, 0, 0xAA5555AA);
+	if (b43legacy_shm_read32(dev, B43legacy_SHM_SHARED, 0) !=
+				 0xAA5555AA)
+		goto error;
+	b43legacy_shm_write32(dev, B43legacy_SHM_SHARED, 0, 0x55AAAA55);
+	if (b43legacy_shm_read32(dev, B43legacy_SHM_SHARED, 0) !=
+				 0x55AAAA55)
+		goto error;
+	b43legacy_shm_write32(dev, B43legacy_SHM_SHARED, 0, shm_backup);
+
+	value = b43legacy_read32(dev, B43legacy_MMIO_MACCTL);
+	if ((value | B43legacy_MACCTL_GMODE) !=
+	    (B43legacy_MACCTL_GMODE | B43legacy_MACCTL_IHR_ENABLED))
+		goto error;
+
+	value = b43legacy_read32(dev, B43legacy_MMIO_GEN_IRQ_REASON);
+	if (value)
+		goto error;
+
+	return 0;
+error:
+	b43legacyerr(dev->wl, "Failed to validate the chipaccess\n");
+	return -ENODEV;
+}
+
+static void b43legacy_security_init(struct b43legacy_wldev *dev)
+{
+	dev->max_nr_keys = (dev->dev->id.revision >= 5) ? 58 : 20;
+	B43legacy_WARN_ON(dev->max_nr_keys > ARRAY_SIZE(dev->key));
+	dev->ktp = b43legacy_shm_read16(dev, B43legacy_SHM_SHARED,
+					0x0056);
+	/* KTP is a word address, but we address SHM bytewise.
+	 * So multiply by two.
+	 */
+	dev->ktp *= 2;
+	if (dev->dev->id.revision >= 5)
+		/* Number of RCMTA address slots */
+		b43legacy_write16(dev, B43legacy_MMIO_RCMTA_COUNT,
+				  dev->max_nr_keys - 8);
+}
+
+static int b43legacy_rng_read(struct hwrng *rng, u32 *data)
+{
+	struct b43legacy_wl *wl = (struct b43legacy_wl *)rng->priv;
+	unsigned long flags;
+
+	/* Don't take wl->mutex here, as it could deadlock with
+	 * hwrng internal locking. It's not needed to take
+	 * wl->mutex here, anyway. */
+
+	spin_lock_irqsave(&wl->irq_lock, flags);
+	*data = b43legacy_read16(wl->current_dev, B43legacy_MMIO_RNG);
+	spin_unlock_irqrestore(&wl->irq_lock, flags);
+
+	return (sizeof(u16));
+}
+
+static void b43legacy_rng_exit(struct b43legacy_wl *wl)
+{
+	if (wl->rng_initialized)
+		hwrng_unregister(&wl->rng);
+}
+
+static int b43legacy_rng_init(struct b43legacy_wl *wl)
+{
+	int err;
+
+	snprintf(wl->rng_name, ARRAY_SIZE(wl->rng_name),
+		 "%s_%s", KBUILD_MODNAME, wiphy_name(wl->hw->wiphy));
+	wl->rng.name = wl->rng_name;
+	wl->rng.data_read = b43legacy_rng_read;
+	wl->rng.priv = (unsigned long)wl;
+	wl->rng_initialized = 1;
+	err = hwrng_register(&wl->rng);
+	if (err) {
+		wl->rng_initialized = 0;
+		b43legacyerr(wl, "Failed to register the random "
+		       "number generator (%d)\n", err);
+	}
+
+	return err;
+}
+
+static int b43legacy_tx(struct ieee80211_hw *hw,
+			struct sk_buff *skb,
+			struct ieee80211_tx_control *ctl)
+{
+	struct b43legacy_wl *wl = hw_to_b43legacy_wl(hw);
+	struct b43legacy_wldev *dev = wl->current_dev;
+	int err = -ENODEV;
+	unsigned long flags;
+
+	if (unlikely(!dev))
+		goto out;
+	if (unlikely(b43legacy_status(dev) < B43legacy_STAT_STARTED))
+		goto out;
+	/* DMA-TX is done without a global lock. */
+	if (b43legacy_using_pio(dev)) {
+		spin_lock_irqsave(&wl->irq_lock, flags);
+		err = b43legacy_pio_tx(dev, skb, ctl);
+		spin_unlock_irqrestore(&wl->irq_lock, flags);
+	} else
+		err = b43legacy_dma_tx(dev, skb, ctl);
+out:
+	if (unlikely(err))
+		return NETDEV_TX_BUSY;
+	return NETDEV_TX_OK;
+}
+
+static int b43legacy_conf_tx(struct ieee80211_hw *hw,
+			     int queue,
+			     const struct ieee80211_tx_queue_params *params)
+{
+	return 0;
+}
+
+static int b43legacy_get_tx_stats(struct ieee80211_hw *hw,
+				  struct ieee80211_tx_queue_stats *stats)
+{
+	struct b43legacy_wl *wl = hw_to_b43legacy_wl(hw);
+	struct b43legacy_wldev *dev = wl->current_dev;
+	unsigned long flags;
+	int err = -ENODEV;
+
+	if (!dev)
+		goto out;
+	spin_lock_irqsave(&wl->irq_lock, flags);
+	if (likely(b43legacy_status(dev) >= B43legacy_STAT_STARTED)) {
+		if (b43legacy_using_pio(dev))
+			b43legacy_pio_get_tx_stats(dev, stats);
+		else
+			b43legacy_dma_get_tx_stats(dev, stats);
+		err = 0;
+	}
+	spin_unlock_irqrestore(&wl->irq_lock, flags);
+out:
+	return err;
+}
+
+static int b43legacy_get_stats(struct ieee80211_hw *hw,
+			       struct ieee80211_low_level_stats *stats)
+{
+	struct b43legacy_wl *wl = hw_to_b43legacy_wl(hw);
+	unsigned long flags;
+
+	spin_lock_irqsave(&wl->irq_lock, flags);
+	memcpy(stats, &wl->ieee_stats, sizeof(*stats));
+	spin_unlock_irqrestore(&wl->irq_lock, flags);
+
+	return 0;
+}
+
+static const char *phymode_to_string(unsigned int phymode)
+{
+	switch (phymode) {
+	case B43legacy_PHYMODE_B:
+		return "B";
+	case B43legacy_PHYMODE_G:
+		return "G";
+	default:
+		B43legacy_BUG_ON(1);
+	}
+	return "";
+}
+
+static int find_wldev_for_phymode(struct b43legacy_wl *wl,
+				  unsigned int phymode,
+				  struct b43legacy_wldev **dev,
+				  bool *gmode)
+{
+	struct b43legacy_wldev *d;
+
+	list_for_each_entry(d, &wl->devlist, list) {
+		if (d->phy.possible_phymodes & phymode) {
+			/* Ok, this device supports the PHY-mode.
+			 * Set the gmode bit. */
+			*gmode = 1;
+			*dev = d;
+
+			return 0;
+		}
+	}
+
+	return -ESRCH;
+}
+
+static void b43legacy_put_phy_into_reset(struct b43legacy_wldev *dev)
+{
+	struct ssb_device *sdev = dev->dev;
+	u32 tmslow;
+
+	tmslow = ssb_read32(sdev, SSB_TMSLOW);
+	tmslow &= ~B43legacy_TMSLOW_GMODE;
+	tmslow |= B43legacy_TMSLOW_PHYRESET;
+	tmslow |= SSB_TMSLOW_FGC;
+	ssb_write32(sdev, SSB_TMSLOW, tmslow);
+	msleep(1);
+
+	tmslow = ssb_read32(sdev, SSB_TMSLOW);
+	tmslow &= ~SSB_TMSLOW_FGC;
+	tmslow |= B43legacy_TMSLOW_PHYRESET;
+	ssb_write32(sdev, SSB_TMSLOW, tmslow);
+	msleep(1);
+}
+
+/* Expects wl->mutex locked */
+static int b43legacy_switch_phymode(struct b43legacy_wl *wl,
+				      unsigned int new_mode)
+{
+	struct b43legacy_wldev *up_dev;
+	struct b43legacy_wldev *down_dev;
+	int err;
+	bool gmode = 0;
+	int prev_status;
+
+	err = find_wldev_for_phymode(wl, new_mode, &up_dev, &gmode);
+	if (err) {
+		b43legacyerr(wl, "Could not find a device for %s-PHY mode\n",
+		       phymode_to_string(new_mode));
+		return err;
+	}
+	if ((up_dev == wl->current_dev) &&
+	    (!!wl->current_dev->phy.gmode == !!gmode))
+		/* This device is already running. */
+		return 0;
+	b43legacydbg(wl, "Reconfiguring PHYmode to %s-PHY\n",
+	       phymode_to_string(new_mode));
+	down_dev = wl->current_dev;
+
+	prev_status = b43legacy_status(down_dev);
+	/* Shutdown the currently running core. */
+	if (prev_status >= B43legacy_STAT_STARTED)
+		b43legacy_wireless_core_stop(down_dev);
+	if (prev_status >= B43legacy_STAT_INITIALIZED)
+		b43legacy_wireless_core_exit(down_dev);
+
+	if (down_dev != up_dev)
+		/* We switch to a different core, so we put PHY into
+		 * RESET on the old core. */
+		b43legacy_put_phy_into_reset(down_dev);
+
+	/* Now start the new core. */
+	up_dev->phy.gmode = gmode;
+	if (prev_status >= B43legacy_STAT_INITIALIZED) {
+		err = b43legacy_wireless_core_init(up_dev);
+		if (err) {
+			b43legacyerr(wl, "Fatal: Could not initialize device"
+				     " for newly selected %s-PHY mode\n",
+				     phymode_to_string(new_mode));
+			goto init_failure;
+		}
+	}
+	if (prev_status >= B43legacy_STAT_STARTED) {
+		err = b43legacy_wireless_core_start(up_dev);
+		if (err) {
+			b43legacyerr(wl, "Fatal: Coult not start device for "
+			       "newly selected %s-PHY mode\n",
+			       phymode_to_string(new_mode));
+			b43legacy_wireless_core_exit(up_dev);
+			goto init_failure;
+		}
+	}
+	B43legacy_WARN_ON(b43legacy_status(up_dev) != prev_status);
+
+	b43legacy_shm_write32(up_dev, B43legacy_SHM_SHARED, 0x003E, 0);
+
+	wl->current_dev = up_dev;
+
+	return 0;
+init_failure:
+	/* Whoops, failed to init the new core. No core is operating now. */
+	wl->current_dev = NULL;
+	return err;
+}
+
+static int b43legacy_antenna_from_ieee80211(u8 antenna)
+{
+	switch (antenna) {
+	case 0: /* default/diversity */
+		return B43legacy_ANTENNA_DEFAULT;
+	case 1: /* Antenna 0 */
+		return B43legacy_ANTENNA0;
+	case 2: /* Antenna 1 */
+		return B43legacy_ANTENNA1;
+	default:
+		return B43legacy_ANTENNA_DEFAULT;
+	}
+}
+
+static int b43legacy_dev_config(struct ieee80211_hw *hw,
+				struct ieee80211_conf *conf)
+{
+	struct b43legacy_wl *wl = hw_to_b43legacy_wl(hw);
+	struct b43legacy_wldev *dev;
+	struct b43legacy_phy *phy;
+	unsigned long flags;
+	unsigned int new_phymode = 0xFFFF;
+	int antenna_tx;
+	int antenna_rx;
+	int err = 0;
+	u32 savedirqs;
+
+	antenna_tx = b43legacy_antenna_from_ieee80211(conf->antenna_sel_tx);
+	antenna_rx = b43legacy_antenna_from_ieee80211(conf->antenna_sel_rx);
+
+	mutex_lock(&wl->mutex);
+
+	/* Switch the PHY mode (if necessary). */
+	switch (conf->phymode) {
+	case MODE_IEEE80211B:
+		new_phymode = B43legacy_PHYMODE_B;
+		break;
+	case MODE_IEEE80211G:
+		new_phymode = B43legacy_PHYMODE_G;
+		break;
+	default:
+		B43legacy_WARN_ON(1);
+	}
+	err = b43legacy_switch_phymode(wl, new_phymode);
+	if (err)
+		goto out_unlock_mutex;
+	dev = wl->current_dev;
+	phy = &dev->phy;
+
+	/* Disable IRQs while reconfiguring the device.
+	 * This makes it possible to drop the spinlock throughout
+	 * the reconfiguration process. */
+	spin_lock_irqsave(&wl->irq_lock, flags);
+	if (b43legacy_status(dev) < B43legacy_STAT_STARTED) {
+		spin_unlock_irqrestore(&wl->irq_lock, flags);
+		goto out_unlock_mutex;
+	}
+	savedirqs = b43legacy_interrupt_disable(dev, B43legacy_IRQ_ALL);
+	spin_unlock_irqrestore(&wl->irq_lock, flags);
+	b43legacy_synchronize_irq(dev);
+
+	/* Switch to the requested channel.
+	 * The firmware takes care of races with the TX handler. */
+	if (conf->channel_val != phy->channel)
+		b43legacy_radio_selectchannel(dev, conf->channel_val, 0);
+
+	/* Enable/Disable ShortSlot timing. */
+	if ((!!(conf->flags & IEEE80211_CONF_SHORT_SLOT_TIME))
+	     != dev->short_slot) {
+		B43legacy_WARN_ON(phy->type != B43legacy_PHYTYPE_G);
+		if (conf->flags & IEEE80211_CONF_SHORT_SLOT_TIME)
+			b43legacy_short_slot_timing_enable(dev);
+		else
+			b43legacy_short_slot_timing_disable(dev);
+	}
+
+	/* Adjust the desired TX power level. */
+	if (conf->power_level != 0) {
+		if (conf->power_level != phy->power_level) {
+			phy->power_level = conf->power_level;
+			b43legacy_phy_xmitpower(dev);
+		}
+	}
+
+	/* Antennas for RX and management frame TX. */
+	b43legacy_mgmtframe_txantenna(dev, antenna_tx);
+
+	/* Update templates for AP mode. */
+	if (b43legacy_is_mode(wl, IEEE80211_IF_TYPE_AP))
+		b43legacy_set_beacon_int(dev, conf->beacon_int);
+
+
+	if (!!conf->radio_enabled != phy->radio_on) {
+		if (conf->radio_enabled) {
+			b43legacy_radio_turn_on(dev);
+			b43legacyinfo(dev->wl, "Radio turned on by software\n");
+			if (!dev->radio_hw_enable)
+				b43legacyinfo(dev->wl, "The hardware RF-kill"
+					      " button still turns the radio"
+					      " physically off. Press the"
+					      " button to turn it on.\n");
+		} else {
+			b43legacy_radio_turn_off(dev);
+			b43legacyinfo(dev->wl, "Radio turned off by"
+				      " software\n");
+		}
+	}
+
+	spin_lock_irqsave(&wl->irq_lock, flags);
+	b43legacy_interrupt_enable(dev, savedirqs);
+	mmiowb();
+	spin_unlock_irqrestore(&wl->irq_lock, flags);
+out_unlock_mutex:
+	mutex_unlock(&wl->mutex);
+
+	return err;
+}
+
+static int b43legacy_dev_set_key(struct ieee80211_hw *hw,
+				 enum set_key_cmd cmd,
+				 const u8 *local_addr, const u8 *addr,
+				 struct ieee80211_key_conf *key)
+{
+	struct b43legacy_wl *wl = hw_to_b43legacy_wl(hw);
+	struct b43legacy_wldev *dev = wl->current_dev;
+	unsigned long flags;
+	int err = -EOPNOTSUPP;
+	DECLARE_MAC_BUF(mac);
+
+	if (!dev)
+		return -ENODEV;
+	mutex_lock(&wl->mutex);
+	spin_lock_irqsave(&wl->irq_lock, flags);
+
+	if (b43legacy_status(dev) < B43legacy_STAT_INITIALIZED) {
+		err = -ENODEV;
+	}
+	spin_unlock_irqrestore(&wl->irq_lock, flags);
+	mutex_unlock(&wl->mutex);
+	b43legacydbg(wl, "Using software based encryption for "
+		     "mac: %s\n", print_mac(mac, addr));
+	return err;
+}
+
+static void b43legacy_configure_filter(struct ieee80211_hw *hw,
+				       unsigned int changed,
+				       unsigned int *fflags,
+				       int mc_count,
+				       struct dev_addr_list *mc_list)
+{
+	struct b43legacy_wl *wl = hw_to_b43legacy_wl(hw);
+	struct b43legacy_wldev *dev = wl->current_dev;
+	unsigned long flags;
+
+	if (!dev) {
+		*fflags = 0;
+		return;
+	}
+
+	spin_lock_irqsave(&wl->irq_lock, flags);
+	*fflags &= FIF_PROMISC_IN_BSS |
+		  FIF_ALLMULTI |
+		  FIF_FCSFAIL |
+		  FIF_PLCPFAIL |
+		  FIF_CONTROL |
+		  FIF_OTHER_BSS |
+		  FIF_BCN_PRBRESP_PROMISC;
+
+	changed &= FIF_PROMISC_IN_BSS |
+		   FIF_ALLMULTI |
+		   FIF_FCSFAIL |
+		   FIF_PLCPFAIL |
+		   FIF_CONTROL |
+		   FIF_OTHER_BSS |
+		   FIF_BCN_PRBRESP_PROMISC;
+
+	wl->filter_flags = *fflags;
+
+	if (changed && b43legacy_status(dev) >= B43legacy_STAT_INITIALIZED)
+		b43legacy_adjust_opmode(dev);
+	spin_unlock_irqrestore(&wl->irq_lock, flags);
+}
+
+static int b43legacy_config_interface(struct ieee80211_hw *hw,
+				      int if_id,
+				      struct ieee80211_if_conf *conf)
+{
+	struct b43legacy_wl *wl = hw_to_b43legacy_wl(hw);
+	struct b43legacy_wldev *dev = wl->current_dev;
+	unsigned long flags;
+
+	if (!dev)
+		return -ENODEV;
+	mutex_lock(&wl->mutex);
+	spin_lock_irqsave(&wl->irq_lock, flags);
+	B43legacy_WARN_ON(wl->if_id != if_id);
+	if (conf->bssid)
+		memcpy(wl->bssid, conf->bssid, ETH_ALEN);
+	else
+		memset(wl->bssid, 0, ETH_ALEN);
+	if (b43legacy_status(dev) >= B43legacy_STAT_INITIALIZED) {
+		if (b43legacy_is_mode(wl, IEEE80211_IF_TYPE_AP)) {
+			B43legacy_WARN_ON(conf->type != IEEE80211_IF_TYPE_AP);
+			b43legacy_set_ssid(dev, conf->ssid, conf->ssid_len);
+			if (conf->beacon)
+				b43legacy_refresh_templates(dev, conf->beacon);
+		}
+		b43legacy_write_mac_bssid_templates(dev);
+	}
+	spin_unlock_irqrestore(&wl->irq_lock, flags);
+	mutex_unlock(&wl->mutex);
+
+	return 0;
+}
+
+/* Locking: wl->mutex */
+static void b43legacy_wireless_core_stop(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_wl *wl = dev->wl;
+	unsigned long flags;
+
+	if (b43legacy_status(dev) < B43legacy_STAT_STARTED)
+		return;
+	b43legacy_set_status(dev, B43legacy_STAT_INITIALIZED);
+
+	mutex_unlock(&wl->mutex);
+	/* Must unlock as it would otherwise deadlock. No races here.
+	 * Cancel the possibly running self-rearming periodic work. */
+	cancel_delayed_work_sync(&dev->periodic_work);
+	mutex_lock(&wl->mutex);
+
+	ieee80211_stop_queues(wl->hw); /* FIXME this could cause a deadlock */
+
+	/* Disable and sync interrupts. */
+	spin_lock_irqsave(&wl->irq_lock, flags);
+	dev->irq_savedstate = b43legacy_interrupt_disable(dev,
+							  B43legacy_IRQ_ALL);
+	b43legacy_read32(dev, B43legacy_MMIO_GEN_IRQ_MASK); /* flush */
+	spin_unlock_irqrestore(&wl->irq_lock, flags);
+	b43legacy_synchronize_irq(dev);
+
+	b43legacy_mac_suspend(dev);
+	free_irq(dev->dev->irq, dev);
+	b43legacydbg(wl, "Wireless interface stopped\n");
+}
+
+/* Locking: wl->mutex */
+static int b43legacy_wireless_core_start(struct b43legacy_wldev *dev)
+{
+	int err;
+
+	B43legacy_WARN_ON(b43legacy_status(dev) != B43legacy_STAT_INITIALIZED);
+
+	drain_txstatus_queue(dev);
+	err = request_irq(dev->dev->irq, b43legacy_interrupt_handler,
+			  IRQF_SHARED, KBUILD_MODNAME, dev);
+	if (err) {
+		b43legacyerr(dev->wl, "Cannot request IRQ-%d\n",
+		       dev->dev->irq);
+		goto out;
+	}
+	/* We are ready to run. */
+	b43legacy_set_status(dev, B43legacy_STAT_STARTED);
+
+	/* Start data flow (TX/RX) */
+	b43legacy_mac_enable(dev);
+	b43legacy_interrupt_enable(dev, dev->irq_savedstate);
+	ieee80211_start_queues(dev->wl->hw);
+
+	/* Start maintenance work */
+	b43legacy_periodic_tasks_setup(dev);
+
+	b43legacydbg(dev->wl, "Wireless interface started\n");
+out:
+	return err;
+}
+
+/* Get PHY and RADIO versioning numbers */
+static int b43legacy_phy_versioning(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+	u32 tmp;
+	u8 analog_type;
+	u8 phy_type;
+	u8 phy_rev;
+	u16 radio_manuf;
+	u16 radio_ver;
+	u16 radio_rev;
+	int unsupported = 0;
+
+	/* Get PHY versioning */
+	tmp = b43legacy_read16(dev, B43legacy_MMIO_PHY_VER);
+	analog_type = (tmp & B43legacy_PHYVER_ANALOG)
+		      >> B43legacy_PHYVER_ANALOG_SHIFT;
+	phy_type = (tmp & B43legacy_PHYVER_TYPE) >> B43legacy_PHYVER_TYPE_SHIFT;
+	phy_rev = (tmp & B43legacy_PHYVER_VERSION);
+	switch (phy_type) {
+	case B43legacy_PHYTYPE_B:
+		if (phy_rev != 2 && phy_rev != 4
+		    && phy_rev != 6 && phy_rev != 7)
+			unsupported = 1;
+		break;
+	case B43legacy_PHYTYPE_G:
+		if (phy_rev > 8)
+			unsupported = 1;
+		break;
+	default:
+		unsupported = 1;
+	};
+	if (unsupported) {
+		b43legacyerr(dev->wl, "FOUND UNSUPPORTED PHY "
+		       "(Analog %u, Type %u, Revision %u)\n",
+		       analog_type, phy_type, phy_rev);
+		return -EOPNOTSUPP;
+	}
+	b43legacydbg(dev->wl, "Found PHY: Analog %u, Type %u, Revision %u\n",
+	       analog_type, phy_type, phy_rev);
+
+
+	/* Get RADIO versioning */
+	if (dev->dev->bus->chip_id == 0x4317) {
+		if (dev->dev->bus->chip_rev == 0)
+			tmp = 0x3205017F;
+		else if (dev->dev->bus->chip_rev == 1)
+			tmp = 0x4205017F;
+		else
+			tmp = 0x5205017F;
+	} else {
+		b43legacy_write16(dev, B43legacy_MMIO_RADIO_CONTROL,
+				  B43legacy_RADIOCTL_ID);
+		tmp = b43legacy_read16(dev, B43legacy_MMIO_RADIO_DATA_HIGH);
+		tmp <<= 16;
+		b43legacy_write16(dev, B43legacy_MMIO_RADIO_CONTROL,
+				  B43legacy_RADIOCTL_ID);
+		tmp |= b43legacy_read16(dev, B43legacy_MMIO_RADIO_DATA_LOW);
+	}
+	radio_manuf = (tmp & 0x00000FFF);
+	radio_ver = (tmp & 0x0FFFF000) >> 12;
+	radio_rev = (tmp & 0xF0000000) >> 28;
+	switch (phy_type) {
+	case B43legacy_PHYTYPE_B:
+		if ((radio_ver & 0xFFF0) != 0x2050)
+			unsupported = 1;
+		break;
+	case B43legacy_PHYTYPE_G:
+		if (radio_ver != 0x2050)
+			unsupported = 1;
+		break;
+	default:
+		B43legacy_BUG_ON(1);
+	}
+	if (unsupported) {
+		b43legacyerr(dev->wl, "FOUND UNSUPPORTED RADIO "
+		       "(Manuf 0x%X, Version 0x%X, Revision %u)\n",
+		       radio_manuf, radio_ver, radio_rev);
+		return -EOPNOTSUPP;
+	}
+	b43legacydbg(dev->wl, "Found Radio: Manuf 0x%X, Version 0x%X,"
+		     " Revision %u\n", radio_manuf, radio_ver, radio_rev);
+
+
+	phy->radio_manuf = radio_manuf;
+	phy->radio_ver = radio_ver;
+	phy->radio_rev = radio_rev;
+
+	phy->analog = analog_type;
+	phy->type = phy_type;
+	phy->rev = phy_rev;
+
+	return 0;
+}
+
+static void setup_struct_phy_for_init(struct b43legacy_wldev *dev,
+				      struct b43legacy_phy *phy)
+{
+	struct b43legacy_lopair *lo;
+	int i;
+
+	memset(phy->minlowsig, 0xFF, sizeof(phy->minlowsig));
+	memset(phy->minlowsigpos, 0, sizeof(phy->minlowsigpos));
+
+	/* Flags */
+	phy->locked = 0;
+	/* Assume the radio is enabled. If it's not enabled, the state will
+	 * immediately get fixed on the first periodic work run. */
+	dev->radio_hw_enable = 1;
+
+	phy->savedpctlreg = 0xFFFF;
+	phy->aci_enable = 0;
+	phy->aci_wlan_automatic = 0;
+	phy->aci_hw_rssi = 0;
+
+	lo = phy->_lo_pairs;
+	if (lo)
+		memset(lo, 0, sizeof(struct b43legacy_lopair) *
+				     B43legacy_LO_COUNT);
+	phy->max_lb_gain = 0;
+	phy->trsw_rx_gain = 0;
+
+	/* Set default attenuation values. */
+	phy->bbatt = b43legacy_default_baseband_attenuation(dev);
+	phy->rfatt = b43legacy_default_radio_attenuation(dev);
+	phy->txctl1 = b43legacy_default_txctl1(dev);
+	phy->txpwr_offset = 0;
+
+	/* NRSSI */
+	phy->nrssislope = 0;
+	for (i = 0; i < ARRAY_SIZE(phy->nrssi); i++)
+		phy->nrssi[i] = -1000;
+	for (i = 0; i < ARRAY_SIZE(phy->nrssi_lt); i++)
+		phy->nrssi_lt[i] = i;
+
+	phy->lofcal = 0xFFFF;
+	phy->initval = 0xFFFF;
+
+	spin_lock_init(&phy->lock);
+	phy->interfmode = B43legacy_INTERFMODE_NONE;
+	phy->channel = 0xFF;
+}
+
+static void setup_struct_wldev_for_init(struct b43legacy_wldev *dev)
+{
+	/* Flags */
+	dev->reg124_set_0x4 = 0;
+
+	/* Stats */
+	memset(&dev->stats, 0, sizeof(dev->stats));
+
+	setup_struct_phy_for_init(dev, &dev->phy);
+
+	/* IRQ related flags */
+	dev->irq_reason = 0;
+	memset(dev->dma_reason, 0, sizeof(dev->dma_reason));
+	dev->irq_savedstate = B43legacy_IRQ_MASKTEMPLATE;
+
+	dev->mac_suspended = 1;
+
+	/* Noise calculation context */
+	memset(&dev->noisecalc, 0, sizeof(dev->noisecalc));
+}
+
+static void b43legacy_imcfglo_timeouts_workaround(struct b43legacy_wldev *dev)
+{
+#ifdef CONFIG_SSB_DRIVER_PCICORE
+	struct ssb_bus *bus = dev->dev->bus;
+	u32 tmp;
+
+	if (bus->pcicore.dev &&
+	    bus->pcicore.dev->id.coreid == SSB_DEV_PCI &&
+	    bus->pcicore.dev->id.revision <= 5) {
+		/* IMCFGLO timeouts workaround. */
+		tmp = ssb_read32(dev->dev, SSB_IMCFGLO);
+		tmp &= ~SSB_IMCFGLO_REQTO;
+		tmp &= ~SSB_IMCFGLO_SERTO;
+		switch (bus->bustype) {
+		case SSB_BUSTYPE_PCI:
+		case SSB_BUSTYPE_PCMCIA:
+			tmp |= 0x32;
+			break;
+		case SSB_BUSTYPE_SSB:
+			tmp |= 0x53;
+			break;
+		}
+		ssb_write32(dev->dev, SSB_IMCFGLO, tmp);
+	}
+#endif /* CONFIG_SSB_DRIVER_PCICORE */
+}
+
+/* Shutdown a wireless core */
+/* Locking: wl->mutex */
+static void b43legacy_wireless_core_exit(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_wl *wl = dev->wl;
+	struct b43legacy_phy *phy = &dev->phy;
+
+	B43legacy_WARN_ON(b43legacy_status(dev) > B43legacy_STAT_INITIALIZED);
+	if (b43legacy_status(dev) != B43legacy_STAT_INITIALIZED)
+		return;
+	b43legacy_set_status(dev, B43legacy_STAT_UNINIT);
+
+	mutex_unlock(&wl->mutex);
+	/* Must unlock as it would otherwise deadlock. No races here.
+	 * Cancel possibly pending workqueues. */
+	cancel_work_sync(&dev->restart_work);
+	mutex_lock(&wl->mutex);
+
+	b43legacy_rng_exit(dev->wl);
+	b43legacy_pio_free(dev);
+	b43legacy_dma_free(dev);
+	b43legacy_chip_exit(dev);
+	b43legacy_radio_turn_off(dev);
+	b43legacy_switch_analog(dev, 0);
+	if (phy->dyn_tssi_tbl)
+		kfree(phy->tssi2dbm);
+	kfree(phy->lo_control);
+	phy->lo_control = NULL;
+	ssb_device_disable(dev->dev, 0);
+	ssb_bus_may_powerdown(dev->dev->bus);
+}
+
+static void prepare_phy_data_for_init(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+	int i;
+
+	/* Set default attenuation values. */
+	phy->bbatt = b43legacy_default_baseband_attenuation(dev);
+	phy->rfatt = b43legacy_default_radio_attenuation(dev);
+	phy->txctl1 = b43legacy_default_txctl1(dev);
+	phy->txctl2 = 0xFFFF;
+	phy->txpwr_offset = 0;
+
+	/* NRSSI */
+	phy->nrssislope = 0;
+	for (i = 0; i < ARRAY_SIZE(phy->nrssi); i++)
+		phy->nrssi[i] = -1000;
+	for (i = 0; i < ARRAY_SIZE(phy->nrssi_lt); i++)
+		phy->nrssi_lt[i] = i;
+
+	phy->lofcal = 0xFFFF;
+	phy->initval = 0xFFFF;
+
+	phy->aci_enable = 0;
+	phy->aci_wlan_automatic = 0;
+	phy->aci_hw_rssi = 0;
+
+	phy->antenna_diversity = 0xFFFF;
+	memset(phy->minlowsig, 0xFF, sizeof(phy->minlowsig));
+	memset(phy->minlowsigpos, 0, sizeof(phy->minlowsigpos));
+
+	/* Flags */
+	phy->calibrated = 0;
+	phy->locked = 0;
+
+	if (phy->_lo_pairs)
+		memset(phy->_lo_pairs, 0,
+		       sizeof(struct b43legacy_lopair) * B43legacy_LO_COUNT);
+	memset(phy->loopback_gain, 0, sizeof(phy->loopback_gain));
+}
+
+/* Initialize a wireless core */
+static int b43legacy_wireless_core_init(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_wl *wl = dev->wl;
+	struct ssb_bus *bus = dev->dev->bus;
+	struct b43legacy_phy *phy = &dev->phy;
+	struct ssb_sprom *sprom = &dev->dev->bus->sprom;
+	int err;
+	u32 hf;
+	u32 tmp;
+
+	B43legacy_WARN_ON(b43legacy_status(dev) != B43legacy_STAT_UNINIT);
+
+	err = ssb_bus_powerup(bus, 0);
+	if (err)
+		goto out;
+	if (!ssb_device_is_enabled(dev->dev)) {
+		tmp = phy->gmode ? B43legacy_TMSLOW_GMODE : 0;
+		b43legacy_wireless_core_reset(dev, tmp);
+	}
+
+	if ((phy->type == B43legacy_PHYTYPE_B) ||
+	    (phy->type == B43legacy_PHYTYPE_G)) {
+		phy->_lo_pairs = kzalloc(sizeof(struct b43legacy_lopair)
+					 * B43legacy_LO_COUNT,
+					 GFP_KERNEL);
+		if (!phy->_lo_pairs)
+			return -ENOMEM;
+	}
+	setup_struct_wldev_for_init(dev);
+
+	err = b43legacy_phy_init_tssi2dbm_table(dev);
+	if (err)
+		goto err_kfree_lo_control;
+
+	/* Enable IRQ routing to this device. */
+	ssb_pcicore_dev_irqvecs_enable(&bus->pcicore, dev->dev);
+
+	b43legacy_imcfglo_timeouts_workaround(dev);
+	prepare_phy_data_for_init(dev);
+	b43legacy_phy_calibrate(dev);
+	err = b43legacy_chip_init(dev);
+	if (err)
+		goto err_kfree_tssitbl;
+	b43legacy_shm_write16(dev, B43legacy_SHM_SHARED,
+			      B43legacy_SHM_SH_WLCOREREV,
+			      dev->dev->id.revision);
+	hf = b43legacy_hf_read(dev);
+	if (phy->type == B43legacy_PHYTYPE_G) {
+		hf |= B43legacy_HF_SYMW;
+		if (phy->rev == 1)
+			hf |= B43legacy_HF_GDCW;
+		if (sprom->r1.boardflags_lo & B43legacy_BFL_PACTRL)
+			hf |= B43legacy_HF_OFDMPABOOST;
+	} else if (phy->type == B43legacy_PHYTYPE_B) {
+		hf |= B43legacy_HF_SYMW;
+		if (phy->rev >= 2 && phy->radio_ver == 0x2050)
+			hf &= ~B43legacy_HF_GDCW;
+	}
+	b43legacy_hf_write(dev, hf);
+
+	/* Short/Long Retry Limit.
+	 * The retry-limit is a 4-bit counter. Enforce this to avoid overflowing
+	 * the chip-internal counter.
+	 */
+	tmp = limit_value(modparam_short_retry, 0, 0xF);
+	b43legacy_shm_write16(dev, B43legacy_SHM_WIRELESS,
+			      0x0006, tmp);
+	tmp = limit_value(modparam_long_retry, 0, 0xF);
+	b43legacy_shm_write16(dev, B43legacy_SHM_WIRELESS,
+			      0x0007, tmp);
+
+	b43legacy_shm_write16(dev, B43legacy_SHM_SHARED,
+			      0x0044, 3);
+	b43legacy_shm_write16(dev, B43legacy_SHM_SHARED,
+			      0x0046, 2);
+
+	/* Disable sending probe responses from firmware.
+	 * Setting the MaxTime to one usec will always trigger
+	 * a timeout, so we never send any probe resp.
+	 * A timeout of zero is infinite. */
+	b43legacy_shm_write16(dev, B43legacy_SHM_SHARED,
+			      B43legacy_SHM_SH_PRMAXTIME, 1);
+
+	b43legacy_rate_memory_init(dev);
+
+	/* Minimum Contention Window */
+	if (phy->type == B43legacy_PHYTYPE_B)
+		b43legacy_shm_write16(dev, B43legacy_SHM_WIRELESS,
+				      0x0003, 31);
+	else
+		b43legacy_shm_write16(dev, B43legacy_SHM_WIRELESS,
+				      0x0003, 15);
+	/* Maximum Contention Window */
+	b43legacy_shm_write16(dev, B43legacy_SHM_WIRELESS,
+			      0x0004, 1023);
+
+	do {
+		if (b43legacy_using_pio(dev))
+			err = b43legacy_pio_init(dev);
+		else {
+			err = b43legacy_dma_init(dev);
+			if (!err)
+				b43legacy_qos_init(dev);
+		}
+	} while (err == -EAGAIN);
+	if (err)
+		goto err_chip_exit;
+
+	b43legacy_write16(dev, 0x0612, 0x0050);
+	b43legacy_shm_write16(dev, B43legacy_SHM_SHARED, 0x0416, 0x0050);
+	b43legacy_shm_write16(dev, B43legacy_SHM_SHARED, 0x0414, 0x01F4);
+
+	ssb_bus_powerup(bus, 1); /* Enable dynamic PCTL */
+	memset(wl->bssid, 0, ETH_ALEN);
+	memset(wl->mac_addr, 0, ETH_ALEN);
+	b43legacy_upload_card_macaddress(dev);
+	b43legacy_security_init(dev);
+	b43legacy_rng_init(wl);
+
+	b43legacy_set_status(dev, B43legacy_STAT_INITIALIZED);
+
+out:
+	return err;
+
+err_chip_exit:
+	b43legacy_chip_exit(dev);
+err_kfree_tssitbl:
+	if (phy->dyn_tssi_tbl)
+		kfree(phy->tssi2dbm);
+err_kfree_lo_control:
+	kfree(phy->lo_control);
+	phy->lo_control = NULL;
+	ssb_bus_may_powerdown(bus);
+	B43legacy_WARN_ON(b43legacy_status(dev) != B43legacy_STAT_UNINIT);
+	return err;
+}
+
+static int b43legacy_add_interface(struct ieee80211_hw *hw,
+				   struct ieee80211_if_init_conf *conf)
+{
+	struct b43legacy_wl *wl = hw_to_b43legacy_wl(hw);
+	struct b43legacy_wldev *dev;
+	unsigned long flags;
+	int err = -EOPNOTSUPP;
+
+	/* TODO: allow WDS/AP devices to coexist */
+
+	if (conf->type != IEEE80211_IF_TYPE_AP &&
+	    conf->type != IEEE80211_IF_TYPE_STA &&
+	    conf->type != IEEE80211_IF_TYPE_WDS &&
+	    conf->type != IEEE80211_IF_TYPE_IBSS)
+		return -EOPNOTSUPP;
+
+	mutex_lock(&wl->mutex);
+	if (wl->operating)
+		goto out_mutex_unlock;
+
+	b43legacydbg(wl, "Adding Interface type %d\n", conf->type);
+
+	dev = wl->current_dev;
+	wl->operating = 1;
+	wl->if_id = conf->if_id;
+	wl->if_type = conf->type;
+	memcpy(wl->mac_addr, conf->mac_addr, ETH_ALEN);
+
+	spin_lock_irqsave(&wl->irq_lock, flags);
+	b43legacy_adjust_opmode(dev);
+	b43legacy_upload_card_macaddress(dev);
+	spin_unlock_irqrestore(&wl->irq_lock, flags);
+
+	err = 0;
+ out_mutex_unlock:
+	mutex_unlock(&wl->mutex);
+
+	return err;
+}
+
+static void b43legacy_remove_interface(struct ieee80211_hw *hw,
+				       struct ieee80211_if_init_conf *conf)
+{
+	struct b43legacy_wl *wl = hw_to_b43legacy_wl(hw);
+	struct b43legacy_wldev *dev = wl->current_dev;
+	unsigned long flags;
+
+	b43legacydbg(wl, "Removing Interface type %d\n", conf->type);
+
+	mutex_lock(&wl->mutex);
+
+	B43legacy_WARN_ON(!wl->operating);
+	B43legacy_WARN_ON(wl->if_id != conf->if_id);
+
+	wl->operating = 0;
+
+	spin_lock_irqsave(&wl->irq_lock, flags);
+	b43legacy_adjust_opmode(dev);
+	memset(wl->mac_addr, 0, ETH_ALEN);
+	b43legacy_upload_card_macaddress(dev);
+	spin_unlock_irqrestore(&wl->irq_lock, flags);
+
+	mutex_unlock(&wl->mutex);
+}
+
+static int b43legacy_start(struct ieee80211_hw *hw)
+{
+	struct b43legacy_wl *wl = hw_to_b43legacy_wl(hw);
+	struct b43legacy_wldev *dev = wl->current_dev;
+	int did_init = 0;
+	int err;
+
+	mutex_lock(&wl->mutex);
+
+	if (b43legacy_status(dev) < B43legacy_STAT_INITIALIZED) {
+		err = b43legacy_wireless_core_init(dev);
+		if (err)
+			goto out_mutex_unlock;
+		did_init = 1;
+	}
+
+	if (b43legacy_status(dev) < B43legacy_STAT_STARTED) {
+		err = b43legacy_wireless_core_start(dev);
+		if (err) {
+			if (did_init)
+				b43legacy_wireless_core_exit(dev);
+			goto out_mutex_unlock;
+		}
+	}
+
+out_mutex_unlock:
+	mutex_unlock(&wl->mutex);
+
+	return err;
+}
+
+void b43legacy_stop(struct ieee80211_hw *hw)
+{
+	struct b43legacy_wl *wl = hw_to_b43legacy_wl(hw);
+	struct b43legacy_wldev *dev = wl->current_dev;
+
+	mutex_lock(&wl->mutex);
+	if (b43legacy_status(dev) >= B43legacy_STAT_STARTED)
+		b43legacy_wireless_core_stop(dev);
+	b43legacy_wireless_core_exit(dev);
+	mutex_unlock(&wl->mutex);
+}
+
+
+static const struct ieee80211_ops b43legacy_hw_ops = {
+	.tx = b43legacy_tx,
+	.conf_tx = b43legacy_conf_tx,
+	.add_interface = b43legacy_add_interface,
+	.remove_interface = b43legacy_remove_interface,
+	.config = b43legacy_dev_config,
+	.config_interface = b43legacy_config_interface,
+	.set_key = b43legacy_dev_set_key,
+	.configure_filter = b43legacy_configure_filter,
+	.get_stats = b43legacy_get_stats,
+	.get_tx_stats = b43legacy_get_tx_stats,
+	.start = b43legacy_start,
+	.stop = b43legacy_stop,
+};
+
+/* Hard-reset the chip. Do not call this directly.
+ * Use b43legacy_controller_restart()
+ */
+static void b43legacy_chip_reset(struct work_struct *work)
+{
+	struct b43legacy_wldev *dev =
+		container_of(work, struct b43legacy_wldev, restart_work);
+	struct b43legacy_wl *wl = dev->wl;
+	int err = 0;
+	int prev_status;
+
+	mutex_lock(&wl->mutex);
+
+	prev_status = b43legacy_status(dev);
+	/* Bring the device down... */
+	if (prev_status >= B43legacy_STAT_STARTED)
+		b43legacy_wireless_core_stop(dev);
+	if (prev_status >= B43legacy_STAT_INITIALIZED)
+		b43legacy_wireless_core_exit(dev);
+
+	/* ...and up again. */
+	if (prev_status >= B43legacy_STAT_INITIALIZED) {
+		err = b43legacy_wireless_core_init(dev);
+		if (err)
+			goto out;
+	}
+	if (prev_status >= B43legacy_STAT_STARTED) {
+		err = b43legacy_wireless_core_start(dev);
+		if (err) {
+			b43legacy_wireless_core_exit(dev);
+			goto out;
+		}
+	}
+out:
+	mutex_unlock(&wl->mutex);
+	if (err)
+		b43legacyerr(wl, "Controller restart FAILED\n");
+	else
+		b43legacyinfo(wl, "Controller restarted\n");
+}
+
+static int b43legacy_setup_modes(struct b43legacy_wldev *dev,
+				 int have_bphy,
+				 int have_gphy)
+{
+	struct ieee80211_hw *hw = dev->wl->hw;
+	struct ieee80211_hw_mode *mode;
+	struct b43legacy_phy *phy = &dev->phy;
+	int cnt = 0;
+	int err;
+
+	phy->possible_phymodes = 0;
+	for (; 1; cnt++) {
+		if (have_bphy) {
+			B43legacy_WARN_ON(cnt >= B43legacy_MAX_PHYHWMODES);
+			mode = &phy->hwmodes[cnt];
+
+			mode->mode = MODE_IEEE80211B;
+			mode->num_channels = b43legacy_bg_chantable_size;
+			mode->channels = b43legacy_bg_chantable;
+			mode->num_rates = b43legacy_b_ratetable_size;
+			mode->rates = b43legacy_b_ratetable;
+			err = ieee80211_register_hwmode(hw, mode);
+			if (err)
+				return err;
+
+			phy->possible_phymodes |= B43legacy_PHYMODE_B;
+			have_bphy = 0;
+			continue;
+		}
+		if (have_gphy) {
+			B43legacy_WARN_ON(cnt >= B43legacy_MAX_PHYHWMODES);
+			mode = &phy->hwmodes[cnt];
+
+			mode->mode = MODE_IEEE80211G;
+			mode->num_channels = b43legacy_bg_chantable_size;
+			mode->channels = b43legacy_bg_chantable;
+			mode->num_rates = b43legacy_g_ratetable_size;
+			mode->rates = b43legacy_g_ratetable;
+			err = ieee80211_register_hwmode(hw, mode);
+			if (err)
+				return err;
+
+			phy->possible_phymodes |= B43legacy_PHYMODE_G;
+			have_gphy = 0;
+			continue;
+		}
+		break;
+	}
+
+	return 0;
+}
+
+static void b43legacy_wireless_core_detach(struct b43legacy_wldev *dev)
+{
+	/* We release firmware that late to not be required to re-request
+	 * is all the time when we reinit the core. */
+	b43legacy_release_firmware(dev);
+}
+
+static int b43legacy_wireless_core_attach(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_wl *wl = dev->wl;
+	struct ssb_bus *bus = dev->dev->bus;
+	struct pci_dev *pdev = bus->host_pci;
+	int err;
+	int have_bphy = 0;
+	int have_gphy = 0;
+	u32 tmp;
+
+	/* Do NOT do any device initialization here.
+	 * Do it in wireless_core_init() instead.
+	 * This function is for gathering basic information about the HW, only.
+	 * Also some structs may be set up here. But most likely you want to
+	 * have that in core_init(), too.
+	 */
+
+	err = ssb_bus_powerup(bus, 0);
+	if (err) {
+		b43legacyerr(wl, "Bus powerup failed\n");
+		goto out;
+	}
+	/* Get the PHY type. */
+	if (dev->dev->id.revision >= 5) {
+		u32 tmshigh;
+
+		tmshigh = ssb_read32(dev->dev, SSB_TMSHIGH);
+		have_gphy = !!(tmshigh & B43legacy_TMSHIGH_GPHY);
+		if (!have_gphy)
+			have_bphy = 1;
+	} else if (dev->dev->id.revision == 4)
+		have_gphy = 1;
+	else
+		have_bphy = 1;
+
+	/* Initialize LEDs structs. */
+	err = b43legacy_leds_init(dev);
+	if (err)
+		goto err_powerdown;
+
+	dev->phy.gmode = (have_gphy || have_bphy);
+	tmp = dev->phy.gmode ? B43legacy_TMSLOW_GMODE : 0;
+	b43legacy_wireless_core_reset(dev, tmp);
+
+	err = b43legacy_phy_versioning(dev);
+	if (err)
+		goto err_leds_exit;
+	/* Check if this device supports multiband. */
+	if (!pdev ||
+	    (pdev->device != 0x4312 &&
+	     pdev->device != 0x4319 &&
+	     pdev->device != 0x4324)) {
+		/* No multiband support. */
+		have_bphy = 0;
+		have_gphy = 0;
+		switch (dev->phy.type) {
+		case B43legacy_PHYTYPE_B:
+			have_bphy = 1;
+			break;
+		case B43legacy_PHYTYPE_G:
+			have_gphy = 1;
+			break;
+		default:
+			B43legacy_BUG_ON(1);
+		}
+	}
+	dev->phy.gmode = (have_gphy || have_bphy);
+	tmp = dev->phy.gmode ? B43legacy_TMSLOW_GMODE : 0;
+	b43legacy_wireless_core_reset(dev, tmp);
+
+	err = b43legacy_validate_chipaccess(dev);
+	if (err)
+		goto err_leds_exit;
+	err = b43legacy_setup_modes(dev, have_bphy, have_gphy);
+	if (err)
+		goto err_leds_exit;
+
+	/* Now set some default "current_dev" */
+	if (!wl->current_dev)
+		wl->current_dev = dev;
+	INIT_WORK(&dev->restart_work, b43legacy_chip_reset);
+
+	b43legacy_radio_turn_off(dev);
+	b43legacy_switch_analog(dev, 0);
+	ssb_device_disable(dev->dev, 0);
+	ssb_bus_may_powerdown(bus);
+
+out:
+	return err;
+
+err_leds_exit:
+	b43legacy_leds_exit(dev);
+err_powerdown:
+	ssb_bus_may_powerdown(bus);
+	return err;
+}
+
+static void b43legacy_one_core_detach(struct ssb_device *dev)
+{
+	struct b43legacy_wldev *wldev;
+	struct b43legacy_wl *wl;
+
+	wldev = ssb_get_drvdata(dev);
+	wl = wldev->wl;
+	cancel_work_sync(&wldev->restart_work);
+	b43legacy_debugfs_remove_device(wldev);
+	b43legacy_wireless_core_detach(wldev);
+	list_del(&wldev->list);
+	wl->nr_devs--;
+	ssb_set_drvdata(dev, NULL);
+	kfree(wldev);
+}
+
+static int b43legacy_one_core_attach(struct ssb_device *dev,
+				     struct b43legacy_wl *wl)
+{
+	struct b43legacy_wldev *wldev;
+	struct pci_dev *pdev;
+	int err = -ENOMEM;
+
+	if (!list_empty(&wl->devlist)) {
+		/* We are not the first core on this chip. */
+		pdev = dev->bus->host_pci;
+		/* Only special chips support more than one wireless
+		 * core, although some of the other chips have more than
+		 * one wireless core as well. Check for this and
+		 * bail out early.
+		 */
+		if (!pdev ||
+		    ((pdev->device != 0x4321) &&
+		     (pdev->device != 0x4313) &&
+		     (pdev->device != 0x431A))) {
+			b43legacydbg(wl, "Ignoring unconnected 802.11 core\n");
+			return -ENODEV;
+		}
+	}
+
+	wldev = kzalloc(sizeof(*wldev), GFP_KERNEL);
+	if (!wldev)
+		goto out;
+
+	wldev->dev = dev;
+	wldev->wl = wl;
+	b43legacy_set_status(wldev, B43legacy_STAT_UNINIT);
+	wldev->bad_frames_preempt = modparam_bad_frames_preempt;
+	tasklet_init(&wldev->isr_tasklet,
+		     (void (*)(unsigned long))b43legacy_interrupt_tasklet,
+		     (unsigned long)wldev);
+	if (modparam_pio)
+		wldev->__using_pio = 1;
+	INIT_LIST_HEAD(&wldev->list);
+
+	err = b43legacy_wireless_core_attach(wldev);
+	if (err)
+		goto err_kfree_wldev;
+
+	list_add(&wldev->list, &wl->devlist);
+	wl->nr_devs++;
+	ssb_set_drvdata(dev, wldev);
+	b43legacy_debugfs_add_device(wldev);
+out:
+	return err;
+
+err_kfree_wldev:
+	kfree(wldev);
+	return err;
+}
+
+static void b43legacy_sprom_fixup(struct ssb_bus *bus)
+{
+	/* boardflags workarounds */
+	if (bus->boardinfo.vendor == PCI_VENDOR_ID_APPLE &&
+	    bus->boardinfo.type == 0x4E &&
+	    bus->boardinfo.rev > 0x40)
+		bus->sprom.r1.boardflags_lo |= B43legacy_BFL_PACTRL;
+
+	/* Convert Antennagain values to Q5.2 */
+	if (bus->sprom.r1.antenna_gain_bg == 0xFF)
+		bus->sprom.r1.antenna_gain_bg = 2; /* if unset, use 2 dBm */
+	bus->sprom.r1.antenna_gain_bg <<= 2;
+}
+
+static void b43legacy_wireless_exit(struct ssb_device *dev,
+				  struct b43legacy_wl *wl)
+{
+	struct ieee80211_hw *hw = wl->hw;
+
+	ssb_set_devtypedata(dev, NULL);
+	ieee80211_free_hw(hw);
+}
+
+static int b43legacy_wireless_init(struct ssb_device *dev)
+{
+	struct ssb_sprom *sprom = &dev->bus->sprom;
+	struct ieee80211_hw *hw;
+	struct b43legacy_wl *wl;
+	int err = -ENOMEM;
+
+	b43legacy_sprom_fixup(dev->bus);
+
+	hw = ieee80211_alloc_hw(sizeof(*wl), &b43legacy_hw_ops);
+	if (!hw) {
+		b43legacyerr(NULL, "Could not allocate ieee80211 device\n");
+		goto out;
+	}
+
+	/* fill hw info */
+	hw->flags = IEEE80211_HW_HOST_GEN_BEACON_TEMPLATE |
+		    IEEE80211_HW_RX_INCLUDES_FCS;
+	hw->max_signal = 100;
+	hw->max_rssi = -110;
+	hw->max_noise = -110;
+	hw->queues = 1; /* FIXME: hardware has more queues */
+	SET_IEEE80211_DEV(hw, dev->dev);
+	if (is_valid_ether_addr(sprom->r1.et1mac))
+		SET_IEEE80211_PERM_ADDR(hw, sprom->r1.et1mac);
+	else
+		SET_IEEE80211_PERM_ADDR(hw, sprom->r1.il0mac);
+
+	/* Get and initialize struct b43legacy_wl */
+	wl = hw_to_b43legacy_wl(hw);
+	memset(wl, 0, sizeof(*wl));
+	wl->hw = hw;
+	spin_lock_init(&wl->irq_lock);
+	spin_lock_init(&wl->leds_lock);
+	mutex_init(&wl->mutex);
+	INIT_LIST_HEAD(&wl->devlist);
+
+	ssb_set_devtypedata(dev, wl);
+	b43legacyinfo(wl, "Broadcom %04X WLAN found\n", dev->bus->chip_id);
+	err = 0;
+out:
+	return err;
+}
+
+static int b43legacy_probe(struct ssb_device *dev,
+			 const struct ssb_device_id *id)
+{
+	struct b43legacy_wl *wl;
+	int err;
+	int first = 0;
+
+	wl = ssb_get_devtypedata(dev);
+	if (!wl) {
+		/* Probing the first core - setup common struct b43legacy_wl */
+		first = 1;
+		err = b43legacy_wireless_init(dev);
+		if (err)
+			goto out;
+		wl = ssb_get_devtypedata(dev);
+		B43legacy_WARN_ON(!wl);
+	}
+	err = b43legacy_one_core_attach(dev, wl);
+	if (err)
+		goto err_wireless_exit;
+
+	if (first) {
+		err = ieee80211_register_hw(wl->hw);
+		if (err)
+			goto err_one_core_detach;
+	}
+
+out:
+	return err;
+
+err_one_core_detach:
+	b43legacy_one_core_detach(dev);
+err_wireless_exit:
+	if (first)
+		b43legacy_wireless_exit(dev, wl);
+	return err;
+}
+
+static void b43legacy_remove(struct ssb_device *dev)
+{
+	struct b43legacy_wl *wl = ssb_get_devtypedata(dev);
+	struct b43legacy_wldev *wldev = ssb_get_drvdata(dev);
+
+	B43legacy_WARN_ON(!wl);
+	if (wl->current_dev == wldev)
+		ieee80211_unregister_hw(wl->hw);
+
+	b43legacy_one_core_detach(dev);
+
+	if (list_empty(&wl->devlist))
+		/* Last core on the chip unregistered.
+		 * We can destroy common struct b43legacy_wl.
+		 */
+		b43legacy_wireless_exit(dev, wl);
+}
+
+/* Perform a hardware reset. This can be called from any context. */
+void b43legacy_controller_restart(struct b43legacy_wldev *dev,
+				  const char *reason)
+{
+	/* Must avoid requeueing, if we are in shutdown. */
+	if (b43legacy_status(dev) < B43legacy_STAT_INITIALIZED)
+		return;
+	b43legacyinfo(dev->wl, "Controller RESET (%s) ...\n", reason);
+	queue_work(dev->wl->hw->workqueue, &dev->restart_work);
+}
+
+#ifdef CONFIG_PM
+
+static int b43legacy_suspend(struct ssb_device *dev, pm_message_t state)
+{
+	struct b43legacy_wldev *wldev = ssb_get_drvdata(dev);
+	struct b43legacy_wl *wl = wldev->wl;
+
+	b43legacydbg(wl, "Suspending...\n");
+
+	mutex_lock(&wl->mutex);
+	wldev->suspend_init_status = b43legacy_status(wldev);
+	if (wldev->suspend_init_status >= B43legacy_STAT_STARTED)
+		b43legacy_wireless_core_stop(wldev);
+	if (wldev->suspend_init_status >= B43legacy_STAT_INITIALIZED)
+		b43legacy_wireless_core_exit(wldev);
+	mutex_unlock(&wl->mutex);
+
+	b43legacydbg(wl, "Device suspended.\n");
+
+	return 0;
+}
+
+static int b43legacy_resume(struct ssb_device *dev)
+{
+	struct b43legacy_wldev *wldev = ssb_get_drvdata(dev);
+	struct b43legacy_wl *wl = wldev->wl;
+	int err = 0;
+
+	b43legacydbg(wl, "Resuming...\n");
+
+	mutex_lock(&wl->mutex);
+	if (wldev->suspend_init_status >= B43legacy_STAT_INITIALIZED) {
+		err = b43legacy_wireless_core_init(wldev);
+		if (err) {
+			b43legacyerr(wl, "Resume failed at core init\n");
+			goto out;
+		}
+	}
+	if (wldev->suspend_init_status >= B43legacy_STAT_STARTED) {
+		err = b43legacy_wireless_core_start(wldev);
+		if (err) {
+			b43legacy_wireless_core_exit(wldev);
+			b43legacyerr(wl, "Resume failed at core start\n");
+			goto out;
+		}
+	}
+	mutex_unlock(&wl->mutex);
+
+	b43legacydbg(wl, "Device resumed.\n");
+out:
+	return err;
+}
+
+#else	/* CONFIG_PM */
+# define b43legacy_suspend	NULL
+# define b43legacy_resume		NULL
+#endif	/* CONFIG_PM */
+
+static struct ssb_driver b43legacy_ssb_driver = {
+	.name		= KBUILD_MODNAME,
+	.id_table	= b43legacy_ssb_tbl,
+	.probe		= b43legacy_probe,
+	.remove		= b43legacy_remove,
+	.suspend	= b43legacy_suspend,
+	.resume		= b43legacy_resume,
+};
+
+static int __init b43legacy_init(void)
+{
+	int err;
+
+	b43legacy_debugfs_init();
+
+	err = ssb_driver_register(&b43legacy_ssb_driver);
+	if (err)
+		goto err_dfs_exit;
+
+	return err;
+
+err_dfs_exit:
+	b43legacy_debugfs_exit();
+	return err;
+}
+
+static void __exit b43legacy_exit(void)
+{
+	ssb_driver_unregister(&b43legacy_ssb_driver);
+	b43legacy_debugfs_exit();
+}
+
+module_init(b43legacy_init)
+module_exit(b43legacy_exit)
diff --git a/drivers/net/wireless/b43legacy/main.h b/drivers/net/wireless/b43legacy/main.h
new file mode 100644
index 000000000000..68435c50d8e0
--- /dev/null
+++ b/drivers/net/wireless/b43legacy/main.h
@@ -0,0 +1,127 @@
+/*
+
+  Broadcom B43legacy wireless driver
+
+  Copyright (c) 2005 Martin Langer <martin-langer@gmx.de>,
+  Copyright (c) 2005 Stefano Brivio <st3@riseup.net>
+  Copyright (c) 2005, 2006 Michael Buesch <mb@bu3sch.de>
+  Copyright (c) 2005  Danny van Dyk <kugelfang@gentoo.org>
+  Copyright (c) 2005  Andreas Jaggi <andreas.jaggi@waterwave.ch>
+  Copyright (c) 2007  Larry Finger <Larry.Finger@lwfinger.net>
+
+  Some parts of the code in this file are derived from the ipw2200
+  driver  Copyright(c) 2003 - 2004 Intel Corporation.
+
+  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, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with this program; see the file COPYING.  If not, write to
+  the Free Software Foundation, Inc., 51 Franklin Steet, Fifth Floor,
+  Boston, MA 02110-1301, USA.
+
+*/
+
+#ifndef B43legacy_MAIN_H_
+#define B43legacy_MAIN_H_
+
+#include "b43legacy.h"
+
+
+#define P4D_BYT3S(magic, nr_bytes)	u8 __p4dding##magic[nr_bytes]
+#define P4D_BYTES(line, nr_bytes)	P4D_BYT3S(line, nr_bytes)
+/* Magic helper macro to pad structures. Ignore those above. It's magic. */
+#define PAD_BYTES(nr_bytes)		P4D_BYTES(__LINE__ , (nr_bytes))
+
+
+/* Lightweight function to convert a frequency (in Mhz) to a channel number. */
+static inline
+u8 b43legacy_freq_to_channel_bg(int freq)
+{
+	u8 channel;
+
+	if (freq == 2484)
+		channel = 14;
+	else
+		channel = (freq - 2407) / 5;
+
+	return channel;
+}
+static inline
+u8 b43legacy_freq_to_channel(struct b43legacy_wldev *dev,
+			     int freq)
+{
+	return b43legacy_freq_to_channel_bg(freq);
+}
+
+/* Lightweight function to convert a channel number to a frequency (in Mhz). */
+static inline
+int b43legacy_channel_to_freq_bg(u8 channel)
+{
+	int freq;
+
+	if (channel == 14)
+		freq = 2484;
+	else
+		freq = 2407 + (5 * channel);
+
+	return freq;
+}
+
+static inline
+int b43legacy_channel_to_freq(struct b43legacy_wldev *dev,
+			      u8 channel)
+{
+	return b43legacy_channel_to_freq_bg(channel);
+}
+
+static inline
+int b43legacy_is_cck_rate(int rate)
+{
+	return (rate == B43legacy_CCK_RATE_1MB ||
+		rate == B43legacy_CCK_RATE_2MB ||
+		rate == B43legacy_CCK_RATE_5MB ||
+		rate == B43legacy_CCK_RATE_11MB);
+}
+
+static inline
+int b43legacy_is_ofdm_rate(int rate)
+{
+	return !b43legacy_is_cck_rate(rate);
+}
+
+void b43legacy_tsf_read(struct b43legacy_wldev *dev, u64 *tsf);
+void b43legacy_tsf_write(struct b43legacy_wldev *dev, u64 tsf);
+
+u32 b43legacy_shm_read32(struct b43legacy_wldev *dev,
+			 u16 routing, u16 offset);
+u16 b43legacy_shm_read16(struct b43legacy_wldev *dev,
+			 u16 routing, u16 offset);
+void b43legacy_shm_write32(struct b43legacy_wldev *dev,
+			 u16 routing, u16 offset,
+			 u32 value);
+void b43legacy_shm_write16(struct b43legacy_wldev *dev,
+			 u16 routing, u16 offset,
+			 u16 value);
+
+u32 b43legacy_hf_read(struct b43legacy_wldev *dev);
+void b43legacy_hf_write(struct b43legacy_wldev *dev, u32 value);
+
+void b43legacy_dummy_transmission(struct b43legacy_wldev *dev);
+
+void b43legacy_wireless_core_reset(struct b43legacy_wldev *dev, u32 flags);
+
+void b43legacy_mac_suspend(struct b43legacy_wldev *dev);
+void b43legacy_mac_enable(struct b43legacy_wldev *dev);
+
+void b43legacy_controller_restart(struct b43legacy_wldev *dev,
+				  const char *reason);
+
+#endif /* B43legacy_MAIN_H_ */
diff --git a/drivers/net/wireless/b43legacy/phy.c b/drivers/net/wireless/b43legacy/phy.c
new file mode 100644
index 000000000000..22a4b3d0186d
--- /dev/null
+++ b/drivers/net/wireless/b43legacy/phy.c
@@ -0,0 +1,2255 @@
+/*
+
+  Broadcom B43legacy wireless driver
+
+  Copyright (c) 2005 Martin Langer <martin-langer@gmx.de>,
+		     Stefano Brivio <st3@riseup.net>
+		     Michael Buesch <mbuesch@freenet.de>
+		     Danny van Dyk <kugelfang@gentoo.org>
+     Andreas Jaggi <andreas.jaggi@waterwave.ch>
+  Copyright (c) 2007 Larry Finger <Larry.Finger@lwfinger.net>
+
+  Some parts of the code in this file are derived from the ipw2200
+  driver  Copyright(c) 2003 - 2004 Intel Corporation.
+
+  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, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with this program; see the file COPYING.  If not, write to
+  the Free Software Foundation, Inc., 51 Franklin Steet, Fifth Floor,
+  Boston, MA 02110-1301, USA.
+
+*/
+
+#include <linux/delay.h>
+#include <linux/pci.h>
+#include <linux/types.h>
+
+#include "b43legacy.h"
+#include "phy.h"
+#include "main.h"
+#include "radio.h"
+#include "ilt.h"
+
+
+static const s8 b43legacy_tssi2dbm_b_table[] = {
+	0x4D, 0x4C, 0x4B, 0x4A,
+	0x4A, 0x49, 0x48, 0x47,
+	0x47, 0x46, 0x45, 0x45,
+	0x44, 0x43, 0x42, 0x42,
+	0x41, 0x40, 0x3F, 0x3E,
+	0x3D, 0x3C, 0x3B, 0x3A,
+	0x39, 0x38, 0x37, 0x36,
+	0x35, 0x34, 0x32, 0x31,
+	0x30, 0x2F, 0x2D, 0x2C,
+	0x2B, 0x29, 0x28, 0x26,
+	0x25, 0x23, 0x21, 0x1F,
+	0x1D, 0x1A, 0x17, 0x14,
+	0x10, 0x0C, 0x06, 0x00,
+	  -7,   -7,   -7,   -7,
+	  -7,   -7,   -7,   -7,
+	  -7,   -7,   -7,   -7,
+};
+
+static const s8 b43legacy_tssi2dbm_g_table[] = {
+	 77,  77,  77,  76,
+	 76,  76,  75,  75,
+	 74,  74,  73,  73,
+	 73,  72,  72,  71,
+	 71,  70,  70,  69,
+	 68,  68,  67,  67,
+	 66,  65,  65,  64,
+	 63,  63,  62,  61,
+	 60,  59,  58,  57,
+	 56,  55,  54,  53,
+	 52,  50,  49,  47,
+	 45,  43,  40,  37,
+	 33,  28,  22,  14,
+	  5,  -7, -20, -20,
+	-20, -20, -20, -20,
+	-20, -20, -20, -20,
+};
+
+static void b43legacy_phy_initg(struct b43legacy_wldev *dev);
+
+
+static inline
+void b43legacy_voluntary_preempt(void)
+{
+	B43legacy_BUG_ON(!(!in_atomic() && !in_irq() &&
+			  !in_interrupt() && !irqs_disabled()));
+#ifndef CONFIG_PREEMPT
+	cond_resched();
+#endif /* CONFIG_PREEMPT */
+}
+
+void b43legacy_raw_phy_lock(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+
+	B43legacy_WARN_ON(!irqs_disabled());
+	if (b43legacy_read32(dev, B43legacy_MMIO_STATUS_BITFIELD) == 0) {
+		phy->locked = 0;
+		return;
+	}
+	if (dev->dev->id.revision < 3) {
+		b43legacy_mac_suspend(dev);
+		spin_lock(&phy->lock);
+	} else {
+		if (!b43legacy_is_mode(dev->wl, IEEE80211_IF_TYPE_AP))
+			b43legacy_power_saving_ctl_bits(dev, -1, 1);
+	}
+	phy->locked = 1;
+}
+
+void b43legacy_raw_phy_unlock(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+
+	B43legacy_WARN_ON(!irqs_disabled());
+	if (dev->dev->id.revision < 3) {
+		if (phy->locked) {
+			spin_unlock(&phy->lock);
+			b43legacy_mac_enable(dev);
+		}
+	} else {
+		if (!b43legacy_is_mode(dev->wl, IEEE80211_IF_TYPE_AP))
+			b43legacy_power_saving_ctl_bits(dev, -1, -1);
+	}
+	phy->locked = 0;
+}
+
+u16 b43legacy_phy_read(struct b43legacy_wldev *dev, u16 offset)
+{
+	b43legacy_write16(dev, B43legacy_MMIO_PHY_CONTROL, offset);
+	return b43legacy_read16(dev, B43legacy_MMIO_PHY_DATA);
+}
+
+void b43legacy_phy_write(struct b43legacy_wldev *dev, u16 offset, u16 val)
+{
+	b43legacy_write16(dev, B43legacy_MMIO_PHY_CONTROL, offset);
+	mmiowb();
+	b43legacy_write16(dev, B43legacy_MMIO_PHY_DATA, val);
+}
+
+void b43legacy_phy_calibrate(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+
+	b43legacy_read32(dev, B43legacy_MMIO_STATUS_BITFIELD); /* Dummy read. */
+	if (phy->calibrated)
+		return;
+	if (phy->type == B43legacy_PHYTYPE_G && phy->rev == 1) {
+		b43legacy_wireless_core_reset(dev, 0);
+		b43legacy_phy_initg(dev);
+		b43legacy_wireless_core_reset(dev, B43legacy_TMSLOW_GMODE);
+	}
+	phy->calibrated = 1;
+}
+
+/* intialize B PHY power control
+ * as described in http://bcm-specs.sipsolutions.net/InitPowerControl
+ */
+static void b43legacy_phy_init_pctl(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+	u16 saved_batt = 0;
+	u16 saved_ratt = 0;
+	u16 saved_txctl1 = 0;
+	int must_reset_txpower = 0;
+
+	B43legacy_BUG_ON(!(phy->type == B43legacy_PHYTYPE_B ||
+			  phy->type == B43legacy_PHYTYPE_G));
+	if (is_bcm_board_vendor(dev) &&
+	    (dev->dev->bus->boardinfo.type == 0x0416))
+		return;
+
+	b43legacy_phy_write(dev, 0x0028, 0x8018);
+	b43legacy_write16(dev, 0x03E6, b43legacy_read16(dev, 0x03E6) & 0xFFDF);
+
+	if (phy->type == B43legacy_PHYTYPE_G) {
+		if (!phy->gmode)
+			return;
+		b43legacy_phy_write(dev, 0x047A, 0xC111);
+	}
+	if (phy->savedpctlreg != 0xFFFF)
+		return;
+#ifdef CONFIG_B43LEGACY_DEBUG
+	if (phy->manual_txpower_control)
+		return;
+#endif
+
+	if (phy->type == B43legacy_PHYTYPE_B &&
+	    phy->rev >= 2 &&
+	    phy->radio_ver == 0x2050)
+		b43legacy_radio_write16(dev, 0x0076,
+					b43legacy_radio_read16(dev, 0x0076)
+					| 0x0084);
+	else {
+		saved_batt = phy->bbatt;
+		saved_ratt = phy->rfatt;
+		saved_txctl1 = phy->txctl1;
+		if ((phy->radio_rev >= 6) && (phy->radio_rev <= 8)
+		    && /*FIXME: incomplete specs for 5 < revision < 9 */ 0)
+			b43legacy_radio_set_txpower_bg(dev, 0xB, 0x1F, 0);
+		else
+			b43legacy_radio_set_txpower_bg(dev, 0xB, 9, 0);
+		must_reset_txpower = 1;
+	}
+	b43legacy_dummy_transmission(dev);
+
+	phy->savedpctlreg = b43legacy_phy_read(dev, B43legacy_PHY_G_PCTL);
+
+	if (must_reset_txpower)
+		b43legacy_radio_set_txpower_bg(dev, saved_batt, saved_ratt,
+					       saved_txctl1);
+	else
+		b43legacy_radio_write16(dev, 0x0076, b43legacy_radio_read16(dev,
+					0x0076) & 0xFF7B);
+	b43legacy_radio_clear_tssi(dev);
+}
+
+static void b43legacy_phy_agcsetup(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+	u16 offset = 0x0000;
+
+	if (phy->rev == 1)
+		offset = 0x4C00;
+
+	b43legacy_ilt_write(dev, offset, 0x00FE);
+	b43legacy_ilt_write(dev, offset + 1, 0x000D);
+	b43legacy_ilt_write(dev, offset + 2, 0x0013);
+	b43legacy_ilt_write(dev, offset + 3, 0x0019);
+
+	if (phy->rev == 1) {
+		b43legacy_ilt_write(dev, 0x1800, 0x2710);
+		b43legacy_ilt_write(dev, 0x1801, 0x9B83);
+		b43legacy_ilt_write(dev, 0x1802, 0x9B83);
+		b43legacy_ilt_write(dev, 0x1803, 0x0F8D);
+		b43legacy_phy_write(dev, 0x0455, 0x0004);
+	}
+
+	b43legacy_phy_write(dev, 0x04A5, (b43legacy_phy_read(dev, 0x04A5)
+					  & 0x00FF) | 0x5700);
+	b43legacy_phy_write(dev, 0x041A, (b43legacy_phy_read(dev, 0x041A)
+					  & 0xFF80) | 0x000F);
+	b43legacy_phy_write(dev, 0x041A, (b43legacy_phy_read(dev, 0x041A)
+					  & 0xC07F) | 0x2B80);
+	b43legacy_phy_write(dev, 0x048C, (b43legacy_phy_read(dev, 0x048C)
+					  & 0xF0FF) | 0x0300);
+
+	b43legacy_radio_write16(dev, 0x007A,
+				b43legacy_radio_read16(dev, 0x007A)
+				| 0x0008);
+
+	b43legacy_phy_write(dev, 0x04A0, (b43legacy_phy_read(dev, 0x04A0)
+			    & 0xFFF0) | 0x0008);
+	b43legacy_phy_write(dev, 0x04A1, (b43legacy_phy_read(dev, 0x04A1)
+			    & 0xF0FF) | 0x0600);
+	b43legacy_phy_write(dev, 0x04A2, (b43legacy_phy_read(dev, 0x04A2)
+			    & 0xF0FF) | 0x0700);
+	b43legacy_phy_write(dev, 0x04A0, (b43legacy_phy_read(dev, 0x04A0)
+			    & 0xF0FF) | 0x0100);
+
+	if (phy->rev == 1)
+		b43legacy_phy_write(dev, 0x04A2,
+				    (b43legacy_phy_read(dev, 0x04A2)
+				    & 0xFFF0) | 0x0007);
+
+	b43legacy_phy_write(dev, 0x0488, (b43legacy_phy_read(dev, 0x0488)
+			    & 0xFF00) | 0x001C);
+	b43legacy_phy_write(dev, 0x0488, (b43legacy_phy_read(dev, 0x0488)
+			    & 0xC0FF) | 0x0200);
+	b43legacy_phy_write(dev, 0x0496, (b43legacy_phy_read(dev, 0x0496)
+			    & 0xFF00) | 0x001C);
+	b43legacy_phy_write(dev, 0x0489, (b43legacy_phy_read(dev, 0x0489)
+			    & 0xFF00) | 0x0020);
+	b43legacy_phy_write(dev, 0x0489, (b43legacy_phy_read(dev, 0x0489)
+			    & 0xC0FF) | 0x0200);
+	b43legacy_phy_write(dev, 0x0482, (b43legacy_phy_read(dev, 0x0482)
+			    & 0xFF00) | 0x002E);
+	b43legacy_phy_write(dev, 0x0496, (b43legacy_phy_read(dev, 0x0496)
+			    & 0x00FF) | 0x1A00);
+	b43legacy_phy_write(dev, 0x0481, (b43legacy_phy_read(dev, 0x0481)
+			    & 0xFF00) | 0x0028);
+	b43legacy_phy_write(dev, 0x0481, (b43legacy_phy_read(dev, 0x0481)
+			    & 0x00FF) | 0x2C00);
+
+	if (phy->rev == 1) {
+		b43legacy_phy_write(dev, 0x0430, 0x092B);
+		b43legacy_phy_write(dev, 0x041B,
+				    (b43legacy_phy_read(dev, 0x041B)
+				    & 0xFFE1) | 0x0002);
+	} else {
+		b43legacy_phy_write(dev, 0x041B,
+				    b43legacy_phy_read(dev, 0x041B) & 0xFFE1);
+		b43legacy_phy_write(dev, 0x041F, 0x287A);
+		b43legacy_phy_write(dev, 0x0420,
+				    (b43legacy_phy_read(dev, 0x0420)
+				    & 0xFFF0) | 0x0004);
+	}
+
+	if (phy->rev > 2) {
+		b43legacy_phy_write(dev, 0x0422, 0x287A);
+		b43legacy_phy_write(dev, 0x0420,
+				    (b43legacy_phy_read(dev, 0x0420)
+				    & 0x0FFF) | 0x3000);
+	}
+
+	b43legacy_phy_write(dev, 0x04A8, (b43legacy_phy_read(dev, 0x04A8)
+			    & 0x8080) | 0x7874);
+	b43legacy_phy_write(dev, 0x048E, 0x1C00);
+
+	if (phy->rev == 1) {
+		b43legacy_phy_write(dev, 0x04AB,
+				    (b43legacy_phy_read(dev, 0x04AB)
+				    & 0xF0FF) | 0x0600);
+		b43legacy_phy_write(dev, 0x048B, 0x005E);
+		b43legacy_phy_write(dev, 0x048C,
+				    (b43legacy_phy_read(dev, 0x048C) & 0xFF00)
+				    | 0x001E);
+		b43legacy_phy_write(dev, 0x048D, 0x0002);
+	}
+
+	b43legacy_ilt_write(dev, offset + 0x0800, 0);
+	b43legacy_ilt_write(dev, offset + 0x0801, 7);
+	b43legacy_ilt_write(dev, offset + 0x0802, 16);
+	b43legacy_ilt_write(dev, offset + 0x0803, 28);
+
+	if (phy->rev >= 6) {
+		b43legacy_phy_write(dev, 0x0426,
+				    (b43legacy_phy_read(dev, 0x0426) & 0xFFFC));
+		b43legacy_phy_write(dev, 0x0426,
+				    (b43legacy_phy_read(dev, 0x0426) & 0xEFFF));
+	}
+}
+
+static void b43legacy_phy_setupg(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+	u16 i;
+
+	B43legacy_BUG_ON(phy->type != B43legacy_PHYTYPE_G);
+	if (phy->rev == 1) {
+		b43legacy_phy_write(dev, 0x0406, 0x4F19);
+		b43legacy_phy_write(dev, B43legacy_PHY_G_CRS,
+				    (b43legacy_phy_read(dev,
+				    B43legacy_PHY_G_CRS) & 0xFC3F) | 0x0340);
+		b43legacy_phy_write(dev, 0x042C, 0x005A);
+		b43legacy_phy_write(dev, 0x0427, 0x001A);
+
+		for (i = 0; i < B43legacy_ILT_FINEFREQG_SIZE; i++)
+			b43legacy_ilt_write(dev, 0x5800 + i,
+					    b43legacy_ilt_finefreqg[i]);
+		for (i = 0; i < B43legacy_ILT_NOISEG1_SIZE; i++)
+			b43legacy_ilt_write(dev, 0x1800 + i,
+					    b43legacy_ilt_noiseg1[i]);
+		for (i = 0; i < B43legacy_ILT_ROTOR_SIZE; i++)
+			b43legacy_ilt_write32(dev, 0x2000 + i,
+					      b43legacy_ilt_rotor[i]);
+	} else {
+		/* nrssi values are signed 6-bit values. Why 0x7654 here? */
+		b43legacy_nrssi_hw_write(dev, 0xBA98, (s16)0x7654);
+
+		if (phy->rev == 2) {
+			b43legacy_phy_write(dev, 0x04C0, 0x1861);
+			b43legacy_phy_write(dev, 0x04C1, 0x0271);
+		} else if (phy->rev > 2) {
+			b43legacy_phy_write(dev, 0x04C0, 0x0098);
+			b43legacy_phy_write(dev, 0x04C1, 0x0070);
+			b43legacy_phy_write(dev, 0x04C9, 0x0080);
+		}
+		b43legacy_phy_write(dev, 0x042B, b43legacy_phy_read(dev,
+				    0x042B) | 0x800);
+
+		for (i = 0; i < 64; i++)
+			b43legacy_ilt_write(dev, 0x4000 + i, i);
+		for (i = 0; i < B43legacy_ILT_NOISEG2_SIZE; i++)
+			b43legacy_ilt_write(dev, 0x1800 + i,
+					    b43legacy_ilt_noiseg2[i]);
+	}
+
+	if (phy->rev <= 2)
+		for (i = 0; i < B43legacy_ILT_NOISESCALEG_SIZE; i++)
+			b43legacy_ilt_write(dev, 0x1400 + i,
+					    b43legacy_ilt_noisescaleg1[i]);
+	else if ((phy->rev >= 7) && (b43legacy_phy_read(dev, 0x0449) & 0x0200))
+		for (i = 0; i < B43legacy_ILT_NOISESCALEG_SIZE; i++)
+			b43legacy_ilt_write(dev, 0x1400 + i,
+					    b43legacy_ilt_noisescaleg3[i]);
+	else
+		for (i = 0; i < B43legacy_ILT_NOISESCALEG_SIZE; i++)
+			b43legacy_ilt_write(dev, 0x1400 + i,
+					    b43legacy_ilt_noisescaleg2[i]);
+
+	if (phy->rev == 2)
+		for (i = 0; i < B43legacy_ILT_SIGMASQR_SIZE; i++)
+			b43legacy_ilt_write(dev, 0x5000 + i,
+					    b43legacy_ilt_sigmasqr1[i]);
+	else if ((phy->rev > 2) && (phy->rev <= 8))
+		for (i = 0; i < B43legacy_ILT_SIGMASQR_SIZE; i++)
+			b43legacy_ilt_write(dev, 0x5000 + i,
+					    b43legacy_ilt_sigmasqr2[i]);
+
+	if (phy->rev == 1) {
+		for (i = 0; i < B43legacy_ILT_RETARD_SIZE; i++)
+			b43legacy_ilt_write32(dev, 0x2400 + i,
+					      b43legacy_ilt_retard[i]);
+		for (i = 4; i < 20; i++)
+			b43legacy_ilt_write(dev, 0x5400 + i, 0x0020);
+		b43legacy_phy_agcsetup(dev);
+
+		if (is_bcm_board_vendor(dev) &&
+		    (dev->dev->bus->boardinfo.type == 0x0416) &&
+		    (dev->dev->bus->boardinfo.rev == 0x0017))
+			return;
+
+		b43legacy_ilt_write(dev, 0x5001, 0x0002);
+		b43legacy_ilt_write(dev, 0x5002, 0x0001);
+	} else {
+		for (i = 0; i <= 0x20; i++)
+			b43legacy_ilt_write(dev, 0x1000 + i, 0x0820);
+		b43legacy_phy_agcsetup(dev);
+		b43legacy_phy_read(dev, 0x0400); /* dummy read */
+		b43legacy_phy_write(dev, 0x0403, 0x1000);
+		b43legacy_ilt_write(dev, 0x3C02, 0x000F);
+		b43legacy_ilt_write(dev, 0x3C03, 0x0014);
+
+		if (is_bcm_board_vendor(dev) &&
+		    (dev->dev->bus->boardinfo.type == 0x0416) &&
+		    (dev->dev->bus->boardinfo.rev == 0x0017))
+			return;
+
+		b43legacy_ilt_write(dev, 0x0401, 0x0002);
+		b43legacy_ilt_write(dev, 0x0402, 0x0001);
+	}
+}
+
+/* Initialize the APHY portion of a GPHY. */
+static void b43legacy_phy_inita(struct b43legacy_wldev *dev)
+{
+
+	might_sleep();
+
+	b43legacy_phy_setupg(dev);
+	if (dev->dev->bus->sprom.r1.boardflags_lo & B43legacy_BFL_PACTRL)
+		b43legacy_phy_write(dev, 0x046E, 0x03CF);
+}
+
+static void b43legacy_phy_initb2(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+	u16 offset;
+	int val;
+
+	b43legacy_write16(dev, 0x03EC, 0x3F22);
+	b43legacy_phy_write(dev, 0x0020, 0x301C);
+	b43legacy_phy_write(dev, 0x0026, 0x0000);
+	b43legacy_phy_write(dev, 0x0030, 0x00C6);
+	b43legacy_phy_write(dev, 0x0088, 0x3E00);
+	val = 0x3C3D;
+	for (offset = 0x0089; offset < 0x00A7; offset++) {
+		b43legacy_phy_write(dev, offset, val);
+		val -= 0x0202;
+	}
+	b43legacy_phy_write(dev, 0x03E4, 0x3000);
+	b43legacy_radio_selectchannel(dev, phy->channel, 0);
+	if (phy->radio_ver != 0x2050) {
+		b43legacy_radio_write16(dev, 0x0075, 0x0080);
+		b43legacy_radio_write16(dev, 0x0079, 0x0081);
+	}
+	b43legacy_radio_write16(dev, 0x0050, 0x0020);
+	b43legacy_radio_write16(dev, 0x0050, 0x0023);
+	if (phy->radio_ver == 0x2050) {
+		b43legacy_radio_write16(dev, 0x0050, 0x0020);
+		b43legacy_radio_write16(dev, 0x005A, 0x0070);
+		b43legacy_radio_write16(dev, 0x005B, 0x007B);
+		b43legacy_radio_write16(dev, 0x005C, 0x00B0);
+		b43legacy_radio_write16(dev, 0x007A, 0x000F);
+		b43legacy_phy_write(dev, 0x0038, 0x0677);
+		b43legacy_radio_init2050(dev);
+	}
+	b43legacy_phy_write(dev, 0x0014, 0x0080);
+	b43legacy_phy_write(dev, 0x0032, 0x00CA);
+	b43legacy_phy_write(dev, 0x0032, 0x00CC);
+	b43legacy_phy_write(dev, 0x0035, 0x07C2);
+	b43legacy_phy_lo_b_measure(dev);
+	b43legacy_phy_write(dev, 0x0026, 0xCC00);
+	if (phy->radio_ver != 0x2050)
+		b43legacy_phy_write(dev, 0x0026, 0xCE00);
+	b43legacy_write16(dev, B43legacy_MMIO_CHANNEL_EXT, 0x1000);
+	b43legacy_phy_write(dev, 0x002A, 0x88A3);
+	if (phy->radio_ver != 0x2050)
+		b43legacy_phy_write(dev, 0x002A, 0x88C2);
+	b43legacy_radio_set_txpower_bg(dev, 0xFFFF, 0xFFFF, 0xFFFF);
+	b43legacy_phy_init_pctl(dev);
+}
+
+static void b43legacy_phy_initb4(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+	u16 offset;
+	u16 val;
+
+	b43legacy_write16(dev, 0x03EC, 0x3F22);
+	b43legacy_phy_write(dev, 0x0020, 0x301C);
+	b43legacy_phy_write(dev, 0x0026, 0x0000);
+	b43legacy_phy_write(dev, 0x0030, 0x00C6);
+	b43legacy_phy_write(dev, 0x0088, 0x3E00);
+	val = 0x3C3D;
+	for (offset = 0x0089; offset < 0x00A7; offset++) {
+		b43legacy_phy_write(dev, offset, val);
+		val -= 0x0202;
+	}
+	b43legacy_phy_write(dev, 0x03E4, 0x3000);
+	b43legacy_radio_selectchannel(dev, phy->channel, 0);
+	if (phy->radio_ver != 0x2050) {
+		b43legacy_radio_write16(dev, 0x0075, 0x0080);
+		b43legacy_radio_write16(dev, 0x0079, 0x0081);
+	}
+	b43legacy_radio_write16(dev, 0x0050, 0x0020);
+	b43legacy_radio_write16(dev, 0x0050, 0x0023);
+	if (phy->radio_ver == 0x2050) {
+		b43legacy_radio_write16(dev, 0x0050, 0x0020);
+		b43legacy_radio_write16(dev, 0x005A, 0x0070);
+		b43legacy_radio_write16(dev, 0x005B, 0x007B);
+		b43legacy_radio_write16(dev, 0x005C, 0x00B0);
+		b43legacy_radio_write16(dev, 0x007A, 0x000F);
+		b43legacy_phy_write(dev, 0x0038, 0x0677);
+		b43legacy_radio_init2050(dev);
+	}
+	b43legacy_phy_write(dev, 0x0014, 0x0080);
+	b43legacy_phy_write(dev, 0x0032, 0x00CA);
+	if (phy->radio_ver == 0x2050)
+		b43legacy_phy_write(dev, 0x0032, 0x00E0);
+	b43legacy_phy_write(dev, 0x0035, 0x07C2);
+
+	b43legacy_phy_lo_b_measure(dev);
+
+	b43legacy_phy_write(dev, 0x0026, 0xCC00);
+	if (phy->radio_ver == 0x2050)
+		b43legacy_phy_write(dev, 0x0026, 0xCE00);
+	b43legacy_write16(dev, B43legacy_MMIO_CHANNEL_EXT, 0x1100);
+	b43legacy_phy_write(dev, 0x002A, 0x88A3);
+	if (phy->radio_ver == 0x2050)
+		b43legacy_phy_write(dev, 0x002A, 0x88C2);
+	b43legacy_radio_set_txpower_bg(dev, 0xFFFF, 0xFFFF, 0xFFFF);
+	if (dev->dev->bus->sprom.r1.boardflags_lo & B43legacy_BFL_RSSI) {
+		b43legacy_calc_nrssi_slope(dev);
+		b43legacy_calc_nrssi_threshold(dev);
+	}
+	b43legacy_phy_init_pctl(dev);
+}
+
+static void b43legacy_phy_initb5(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+	u16 offset;
+	u16 value;
+	u8 old_channel;
+
+	if (phy->analog == 1)
+		b43legacy_radio_write16(dev, 0x007A,
+					b43legacy_radio_read16(dev, 0x007A)
+					| 0x0050);
+	if (!is_bcm_board_vendor(dev) &&
+	    (dev->dev->bus->boardinfo.type != 0x0416)) {
+		value = 0x2120;
+		for (offset = 0x00A8 ; offset < 0x00C7; offset++) {
+			b43legacy_phy_write(dev, offset, value);
+			value += 0x0202;
+		}
+	}
+	b43legacy_phy_write(dev, 0x0035,
+			    (b43legacy_phy_read(dev, 0x0035) & 0xF0FF)
+			    | 0x0700);
+	if (phy->radio_ver == 0x2050)
+		b43legacy_phy_write(dev, 0x0038, 0x0667);
+
+	if (phy->gmode) {
+		if (phy->radio_ver == 0x2050) {
+			b43legacy_radio_write16(dev, 0x007A,
+					b43legacy_radio_read16(dev, 0x007A)
+					| 0x0020);
+			b43legacy_radio_write16(dev, 0x0051,
+					b43legacy_radio_read16(dev, 0x0051)
+					| 0x0004);
+		}
+		b43legacy_write16(dev, B43legacy_MMIO_PHY_RADIO, 0x0000);
+
+		b43legacy_phy_write(dev, 0x0802, b43legacy_phy_read(dev, 0x0802)
+				    | 0x0100);
+		b43legacy_phy_write(dev, 0x042B, b43legacy_phy_read(dev, 0x042B)
+				    | 0x2000);
+
+		b43legacy_phy_write(dev, 0x001C, 0x186A);
+
+		b43legacy_phy_write(dev, 0x0013, (b43legacy_phy_read(dev,
+				    0x0013) & 0x00FF) | 0x1900);
+		b43legacy_phy_write(dev, 0x0035, (b43legacy_phy_read(dev,
+				    0x0035) & 0xFFC0) | 0x0064);
+		b43legacy_phy_write(dev, 0x005D, (b43legacy_phy_read(dev,
+				    0x005D) & 0xFF80) | 0x000A);
+	}
+
+	if (dev->bad_frames_preempt)
+		b43legacy_phy_write(dev, B43legacy_PHY_RADIO_BITFIELD,
+				    b43legacy_phy_read(dev,
+				    B43legacy_PHY_RADIO_BITFIELD) | (1 << 11));
+
+	if (phy->analog == 1) {
+		b43legacy_phy_write(dev, 0x0026, 0xCE00);
+		b43legacy_phy_write(dev, 0x0021, 0x3763);
+		b43legacy_phy_write(dev, 0x0022, 0x1BC3);
+		b43legacy_phy_write(dev, 0x0023, 0x06F9);
+		b43legacy_phy_write(dev, 0x0024, 0x037E);
+	} else
+		b43legacy_phy_write(dev, 0x0026, 0xCC00);
+	b43legacy_phy_write(dev, 0x0030, 0x00C6);
+	b43legacy_write16(dev, 0x03EC, 0x3F22);
+
+	if (phy->analog == 1)
+		b43legacy_phy_write(dev, 0x0020, 0x3E1C);
+	else
+		b43legacy_phy_write(dev, 0x0020, 0x301C);
+
+	if (phy->analog == 0)
+		b43legacy_write16(dev, 0x03E4, 0x3000);
+
+	old_channel = (phy->channel == 0xFF) ? 1 : phy->channel;
+	/* Force to channel 7, even if not supported. */
+	b43legacy_radio_selectchannel(dev, 7, 0);
+
+	if (phy->radio_ver != 0x2050) {
+		b43legacy_radio_write16(dev, 0x0075, 0x0080);
+		b43legacy_radio_write16(dev, 0x0079, 0x0081);
+	}
+
+	b43legacy_radio_write16(dev, 0x0050, 0x0020);
+	b43legacy_radio_write16(dev, 0x0050, 0x0023);
+
+	if (phy->radio_ver == 0x2050) {
+		b43legacy_radio_write16(dev, 0x0050, 0x0020);
+		b43legacy_radio_write16(dev, 0x005A, 0x0070);
+	}
+
+	b43legacy_radio_write16(dev, 0x005B, 0x007B);
+	b43legacy_radio_write16(dev, 0x005C, 0x00B0);
+
+	b43legacy_radio_write16(dev, 0x007A, b43legacy_radio_read16(dev,
+				0x007A) | 0x0007);
+
+	b43legacy_radio_selectchannel(dev, old_channel, 0);
+
+	b43legacy_phy_write(dev, 0x0014, 0x0080);
+	b43legacy_phy_write(dev, 0x0032, 0x00CA);
+	b43legacy_phy_write(dev, 0x002A, 0x88A3);
+
+	b43legacy_radio_set_txpower_bg(dev, 0xFFFF, 0xFFFF, 0xFFFF);
+
+	if (phy->radio_ver == 0x2050)
+		b43legacy_radio_write16(dev, 0x005D, 0x000D);
+
+	b43legacy_write16(dev, 0x03E4, (b43legacy_read16(dev, 0x03E4) &
+			  0xFFC0) | 0x0004);
+}
+
+static void b43legacy_phy_initb6(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+	u16 offset;
+	u16 val;
+	u8 old_channel;
+
+	b43legacy_phy_write(dev, 0x003E, 0x817A);
+	b43legacy_radio_write16(dev, 0x007A,
+				(b43legacy_radio_read16(dev, 0x007A) | 0x0058));
+	if (phy->radio_rev == 4 ||
+	     phy->radio_rev == 5) {
+		b43legacy_radio_write16(dev, 0x0051, 0x0037);
+		b43legacy_radio_write16(dev, 0x0052, 0x0070);
+		b43legacy_radio_write16(dev, 0x0053, 0x00B3);
+		b43legacy_radio_write16(dev, 0x0054, 0x009B);
+		b43legacy_radio_write16(dev, 0x005A, 0x0088);
+		b43legacy_radio_write16(dev, 0x005B, 0x0088);
+		b43legacy_radio_write16(dev, 0x005D, 0x0088);
+		b43legacy_radio_write16(dev, 0x005E, 0x0088);
+		b43legacy_radio_write16(dev, 0x007D, 0x0088);
+		b43legacy_shm_write32(dev, B43legacy_SHM_SHARED,
+				      B43legacy_UCODEFLAGS_OFFSET,
+				      (b43legacy_shm_read32(dev,
+				      B43legacy_SHM_SHARED,
+				      B43legacy_UCODEFLAGS_OFFSET)
+				      | 0x00000200));
+	}
+	if (phy->radio_rev == 8) {
+		b43legacy_radio_write16(dev, 0x0051, 0x0000);
+		b43legacy_radio_write16(dev, 0x0052, 0x0040);
+		b43legacy_radio_write16(dev, 0x0053, 0x00B7);
+		b43legacy_radio_write16(dev, 0x0054, 0x0098);
+		b43legacy_radio_write16(dev, 0x005A, 0x0088);
+		b43legacy_radio_write16(dev, 0x005B, 0x006B);
+		b43legacy_radio_write16(dev, 0x005C, 0x000F);
+		if (dev->dev->bus->sprom.r1.boardflags_lo & 0x8000) {
+			b43legacy_radio_write16(dev, 0x005D, 0x00FA);
+			b43legacy_radio_write16(dev, 0x005E, 0x00D8);
+		} else {
+			b43legacy_radio_write16(dev, 0x005D, 0x00F5);
+			b43legacy_radio_write16(dev, 0x005E, 0x00B8);
+		}
+		b43legacy_radio_write16(dev, 0x0073, 0x0003);
+		b43legacy_radio_write16(dev, 0x007D, 0x00A8);
+		b43legacy_radio_write16(dev, 0x007C, 0x0001);
+		b43legacy_radio_write16(dev, 0x007E, 0x0008);
+	}
+	val = 0x1E1F;
+	for (offset = 0x0088; offset < 0x0098; offset++) {
+		b43legacy_phy_write(dev, offset, val);
+		val -= 0x0202;
+	}
+	val = 0x3E3F;
+	for (offset = 0x0098; offset < 0x00A8; offset++) {
+		b43legacy_phy_write(dev, offset, val);
+		val -= 0x0202;
+	}
+	val = 0x2120;
+	for (offset = 0x00A8; offset < 0x00C8; offset++) {
+		b43legacy_phy_write(dev, offset, (val & 0x3F3F));
+		val += 0x0202;
+	}
+	if (phy->type == B43legacy_PHYTYPE_G) {
+		b43legacy_radio_write16(dev, 0x007A,
+					b43legacy_radio_read16(dev, 0x007A) |
+					0x0020);
+		b43legacy_radio_write16(dev, 0x0051,
+					b43legacy_radio_read16(dev, 0x0051) |
+					0x0004);
+		b43legacy_phy_write(dev, 0x0802,
+				    b43legacy_phy_read(dev, 0x0802) | 0x0100);
+		b43legacy_phy_write(dev, 0x042B,
+				    b43legacy_phy_read(dev, 0x042B) | 0x2000);
+		b43legacy_phy_write(dev, 0x5B, 0x0000);
+		b43legacy_phy_write(dev, 0x5C, 0x0000);
+	}
+
+	old_channel = phy->channel;
+	if (old_channel >= 8)
+		b43legacy_radio_selectchannel(dev, 1, 0);
+	else
+		b43legacy_radio_selectchannel(dev, 13, 0);
+
+	b43legacy_radio_write16(dev, 0x0050, 0x0020);
+	b43legacy_radio_write16(dev, 0x0050, 0x0023);
+	udelay(40);
+	if (phy->radio_rev < 6 || phy->radio_rev == 8) {
+		b43legacy_radio_write16(dev, 0x007C,
+					(b43legacy_radio_read16(dev, 0x007C)
+					| 0x0002));
+		b43legacy_radio_write16(dev, 0x0050, 0x0020);
+	}
+	if (phy->radio_rev <= 2) {
+		b43legacy_radio_write16(dev, 0x007C, 0x0020);
+		b43legacy_radio_write16(dev, 0x005A, 0x0070);
+		b43legacy_radio_write16(dev, 0x005B, 0x007B);
+		b43legacy_radio_write16(dev, 0x005C, 0x00B0);
+	}
+	b43legacy_radio_write16(dev, 0x007A,
+				(b43legacy_radio_read16(dev,
+				0x007A) & 0x00F8) | 0x0007);
+
+	b43legacy_radio_selectchannel(dev, old_channel, 0);
+
+	b43legacy_phy_write(dev, 0x0014, 0x0200);
+	if (phy->radio_rev >= 6)
+		b43legacy_phy_write(dev, 0x002A, 0x88C2);
+	else
+		b43legacy_phy_write(dev, 0x002A, 0x8AC0);
+	b43legacy_phy_write(dev, 0x0038, 0x0668);
+	b43legacy_radio_set_txpower_bg(dev, 0xFFFF, 0xFFFF, 0xFFFF);
+	if (phy->radio_rev <= 5)
+		b43legacy_phy_write(dev, 0x005D, (b43legacy_phy_read(dev,
+				    0x005D) & 0xFF80) | 0x0003);
+	if (phy->radio_rev <= 2)
+		b43legacy_radio_write16(dev, 0x005D, 0x000D);
+
+	if (phy->analog == 4) {
+		b43legacy_write16(dev, 0x03E4, 0x0009);
+		b43legacy_phy_write(dev, 0x61, b43legacy_phy_read(dev, 0x61)
+				    & 0xFFF);
+	} else
+		b43legacy_phy_write(dev, 0x0002, (b43legacy_phy_read(dev,
+				    0x0002) & 0xFFC0) | 0x0004);
+	if (phy->type == B43legacy_PHYTYPE_G)
+		b43legacy_write16(dev, 0x03E6, 0x0);
+	if (phy->type == B43legacy_PHYTYPE_B) {
+		b43legacy_write16(dev, 0x03E6, 0x8140);
+		b43legacy_phy_write(dev, 0x0016, 0x0410);
+		b43legacy_phy_write(dev, 0x0017, 0x0820);
+		b43legacy_phy_write(dev, 0x0062, 0x0007);
+		b43legacy_radio_init2050(dev);
+		b43legacy_phy_lo_g_measure(dev);
+		if (dev->dev->bus->sprom.r1.boardflags_lo &
+		    B43legacy_BFL_RSSI) {
+			b43legacy_calc_nrssi_slope(dev);
+			b43legacy_calc_nrssi_threshold(dev);
+		}
+		b43legacy_phy_init_pctl(dev);
+	}
+}
+
+static void b43legacy_calc_loopback_gain(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+	u16 backup_phy[15] = {0};
+	u16 backup_radio[3];
+	u16 backup_bband;
+	u16 i;
+	u16 loop1_cnt;
+	u16 loop1_done;
+	u16 loop1_omitted;
+	u16 loop2_done;
+
+	backup_phy[0] = b43legacy_phy_read(dev, 0x0429);
+	backup_phy[1] = b43legacy_phy_read(dev, 0x0001);
+	backup_phy[2] = b43legacy_phy_read(dev, 0x0811);
+	backup_phy[3] = b43legacy_phy_read(dev, 0x0812);
+	if (phy->rev != 1) {
+		backup_phy[4] = b43legacy_phy_read(dev, 0x0814);
+		backup_phy[5] = b43legacy_phy_read(dev, 0x0815);
+	}
+	backup_phy[6] = b43legacy_phy_read(dev, 0x005A);
+	backup_phy[7] = b43legacy_phy_read(dev, 0x0059);
+	backup_phy[8] = b43legacy_phy_read(dev, 0x0058);
+	backup_phy[9] = b43legacy_phy_read(dev, 0x000A);
+	backup_phy[10] = b43legacy_phy_read(dev, 0x0003);
+	backup_phy[11] = b43legacy_phy_read(dev, 0x080F);
+	backup_phy[12] = b43legacy_phy_read(dev, 0x0810);
+	backup_phy[13] = b43legacy_phy_read(dev, 0x002B);
+	backup_phy[14] = b43legacy_phy_read(dev, 0x0015);
+	b43legacy_phy_read(dev, 0x002D); /* dummy read */
+	backup_bband = phy->bbatt;
+	backup_radio[0] = b43legacy_radio_read16(dev, 0x0052);
+	backup_radio[1] = b43legacy_radio_read16(dev, 0x0043);
+	backup_radio[2] = b43legacy_radio_read16(dev, 0x007A);
+
+	b43legacy_phy_write(dev, 0x0429,
+			    b43legacy_phy_read(dev, 0x0429) & 0x3FFF);
+	b43legacy_phy_write(dev, 0x0001,
+			    b43legacy_phy_read(dev, 0x0001) & 0x8000);
+	b43legacy_phy_write(dev, 0x0811,
+			    b43legacy_phy_read(dev, 0x0811) | 0x0002);
+	b43legacy_phy_write(dev, 0x0812,
+			    b43legacy_phy_read(dev, 0x0812) & 0xFFFD);
+	b43legacy_phy_write(dev, 0x0811,
+			    b43legacy_phy_read(dev, 0x0811) | 0x0001);
+	b43legacy_phy_write(dev, 0x0812,
+			    b43legacy_phy_read(dev, 0x0812) & 0xFFFE);
+	if (phy->rev != 1) {
+		b43legacy_phy_write(dev, 0x0814,
+				    b43legacy_phy_read(dev, 0x0814) | 0x0001);
+		b43legacy_phy_write(dev, 0x0815,
+				    b43legacy_phy_read(dev, 0x0815) & 0xFFFE);
+		b43legacy_phy_write(dev, 0x0814,
+				    b43legacy_phy_read(dev, 0x0814) | 0x0002);
+		b43legacy_phy_write(dev, 0x0815,
+				    b43legacy_phy_read(dev, 0x0815) & 0xFFFD);
+	}
+	b43legacy_phy_write(dev, 0x0811, b43legacy_phy_read(dev, 0x0811) |
+			    0x000C);
+	b43legacy_phy_write(dev, 0x0812, b43legacy_phy_read(dev, 0x0812) |
+			    0x000C);
+
+	b43legacy_phy_write(dev, 0x0811, (b43legacy_phy_read(dev, 0x0811)
+			    & 0xFFCF) | 0x0030);
+	b43legacy_phy_write(dev, 0x0812, (b43legacy_phy_read(dev, 0x0812)
+			    & 0xFFCF) | 0x0010);
+
+	b43legacy_phy_write(dev, 0x005A, 0x0780);
+	b43legacy_phy_write(dev, 0x0059, 0xC810);
+	b43legacy_phy_write(dev, 0x0058, 0x000D);
+	if (phy->analog == 0)
+		b43legacy_phy_write(dev, 0x0003, 0x0122);
+	else
+		b43legacy_phy_write(dev, 0x000A,
+				    b43legacy_phy_read(dev, 0x000A)
+				    | 0x2000);
+	if (phy->rev != 1) {
+		b43legacy_phy_write(dev, 0x0814,
+				    b43legacy_phy_read(dev, 0x0814) | 0x0004);
+		b43legacy_phy_write(dev, 0x0815,
+				    b43legacy_phy_read(dev, 0x0815) & 0xFFFB);
+	}
+	b43legacy_phy_write(dev, 0x0003,
+			    (b43legacy_phy_read(dev, 0x0003)
+			     & 0xFF9F) | 0x0040);
+	if (phy->radio_ver == 0x2050 && phy->radio_rev == 2) {
+		b43legacy_radio_write16(dev, 0x0052, 0x0000);
+		b43legacy_radio_write16(dev, 0x0043,
+					(b43legacy_radio_read16(dev, 0x0043)
+					 & 0xFFF0) | 0x0009);
+		loop1_cnt = 9;
+	} else if (phy->radio_rev == 8) {
+		b43legacy_radio_write16(dev, 0x0043, 0x000F);
+		loop1_cnt = 15;
+	} else
+		loop1_cnt = 0;
+
+	b43legacy_phy_set_baseband_attenuation(dev, 11);
+
+	if (phy->rev >= 3)
+		b43legacy_phy_write(dev, 0x080F, 0xC020);
+	else
+		b43legacy_phy_write(dev, 0x080F, 0x8020);
+	b43legacy_phy_write(dev, 0x0810, 0x0000);
+
+	b43legacy_phy_write(dev, 0x002B,
+			    (b43legacy_phy_read(dev, 0x002B)
+			     & 0xFFC0) | 0x0001);
+	b43legacy_phy_write(dev, 0x002B,
+			    (b43legacy_phy_read(dev, 0x002B)
+			     & 0xC0FF) | 0x0800);
+	b43legacy_phy_write(dev, 0x0811,
+			    b43legacy_phy_read(dev, 0x0811) | 0x0100);
+	b43legacy_phy_write(dev, 0x0812,
+			    b43legacy_phy_read(dev, 0x0812) & 0xCFFF);
+	if (dev->dev->bus->sprom.r1.boardflags_lo & B43legacy_BFL_EXTLNA) {
+		if (phy->rev >= 7) {
+			b43legacy_phy_write(dev, 0x0811,
+					    b43legacy_phy_read(dev, 0x0811)
+					    | 0x0800);
+			b43legacy_phy_write(dev, 0x0812,
+					    b43legacy_phy_read(dev, 0x0812)
+					    | 0x8000);
+		}
+	}
+	b43legacy_radio_write16(dev, 0x007A,
+				b43legacy_radio_read16(dev, 0x007A)
+				& 0x00F7);
+
+	for (i = 0; i < loop1_cnt; i++) {
+		b43legacy_radio_write16(dev, 0x0043, loop1_cnt);
+		b43legacy_phy_write(dev, 0x0812,
+				    (b43legacy_phy_read(dev, 0x0812)
+				     & 0xF0FF) | (i << 8));
+		b43legacy_phy_write(dev, 0x0015,
+				    (b43legacy_phy_read(dev, 0x0015)
+				     & 0x0FFF) | 0xA000);
+		b43legacy_phy_write(dev, 0x0015,
+				    (b43legacy_phy_read(dev, 0x0015)
+				     & 0x0FFF) | 0xF000);
+		udelay(20);
+		if (b43legacy_phy_read(dev, 0x002D) >= 0x0DFC)
+			break;
+	}
+	loop1_done = i;
+	loop1_omitted = loop1_cnt - loop1_done;
+
+	loop2_done = 0;
+	if (loop1_done >= 8) {
+		b43legacy_phy_write(dev, 0x0812,
+				    b43legacy_phy_read(dev, 0x0812)
+				    | 0x0030);
+		for (i = loop1_done - 8; i < 16; i++) {
+			b43legacy_phy_write(dev, 0x0812,
+					    (b43legacy_phy_read(dev, 0x0812)
+					     & 0xF0FF) | (i << 8));
+			b43legacy_phy_write(dev, 0x0015,
+					    (b43legacy_phy_read(dev, 0x0015)
+					     & 0x0FFF) | 0xA000);
+			b43legacy_phy_write(dev, 0x0015,
+					    (b43legacy_phy_read(dev, 0x0015)
+					     & 0x0FFF) | 0xF000);
+			udelay(20);
+			if (b43legacy_phy_read(dev, 0x002D) >= 0x0DFC)
+				break;
+		}
+	}
+
+	if (phy->rev != 1) {
+		b43legacy_phy_write(dev, 0x0814, backup_phy[4]);
+		b43legacy_phy_write(dev, 0x0815, backup_phy[5]);
+	}
+	b43legacy_phy_write(dev, 0x005A, backup_phy[6]);
+	b43legacy_phy_write(dev, 0x0059, backup_phy[7]);
+	b43legacy_phy_write(dev, 0x0058, backup_phy[8]);
+	b43legacy_phy_write(dev, 0x000A, backup_phy[9]);
+	b43legacy_phy_write(dev, 0x0003, backup_phy[10]);
+	b43legacy_phy_write(dev, 0x080F, backup_phy[11]);
+	b43legacy_phy_write(dev, 0x0810, backup_phy[12]);
+	b43legacy_phy_write(dev, 0x002B, backup_phy[13]);
+	b43legacy_phy_write(dev, 0x0015, backup_phy[14]);
+
+	b43legacy_phy_set_baseband_attenuation(dev, backup_bband);
+
+	b43legacy_radio_write16(dev, 0x0052, backup_radio[0]);
+	b43legacy_radio_write16(dev, 0x0043, backup_radio[1]);
+	b43legacy_radio_write16(dev, 0x007A, backup_radio[2]);
+
+	b43legacy_phy_write(dev, 0x0811, backup_phy[2] | 0x0003);
+	udelay(10);
+	b43legacy_phy_write(dev, 0x0811, backup_phy[2]);
+	b43legacy_phy_write(dev, 0x0812, backup_phy[3]);
+	b43legacy_phy_write(dev, 0x0429, backup_phy[0]);
+	b43legacy_phy_write(dev, 0x0001, backup_phy[1]);
+
+	phy->loopback_gain[0] = ((loop1_done * 6) - (loop1_omitted * 4)) - 11;
+	phy->loopback_gain[1] = (24 - (3 * loop2_done)) * 2;
+}
+
+static void b43legacy_phy_initg(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+	u16 tmp;
+
+	if (phy->rev == 1)
+		b43legacy_phy_initb5(dev);
+	else
+		b43legacy_phy_initb6(dev);
+	if (phy->rev >= 2 || phy->gmode)
+		b43legacy_phy_inita(dev);
+
+	if (phy->rev >= 2) {
+		b43legacy_phy_write(dev, 0x0814, 0x0000);
+		b43legacy_phy_write(dev, 0x0815, 0x0000);
+	}
+	if (phy->rev == 2) {
+		b43legacy_phy_write(dev, 0x0811, 0x0000);
+		b43legacy_phy_write(dev, 0x0015, 0x00C0);
+	}
+	if (phy->rev > 5) {
+		b43legacy_phy_write(dev, 0x0811, 0x0400);
+		b43legacy_phy_write(dev, 0x0015, 0x00C0);
+	}
+	if (phy->rev >= 2 || phy->gmode) {
+		tmp = b43legacy_phy_read(dev, 0x0400) & 0xFF;
+		if (tmp == 3 || tmp == 5) {
+			b43legacy_phy_write(dev, 0x04C2, 0x1816);
+			b43legacy_phy_write(dev, 0x04C3, 0x8006);
+			if (tmp == 5)
+				b43legacy_phy_write(dev, 0x04CC,
+						    (b43legacy_phy_read(dev,
+						     0x04CC) & 0x00FF) |
+						     0x1F00);
+		}
+		b43legacy_phy_write(dev, 0x047E, 0x0078);
+	}
+	if (phy->radio_rev == 8) {
+		b43legacy_phy_write(dev, 0x0801, b43legacy_phy_read(dev, 0x0801)
+				    | 0x0080);
+		b43legacy_phy_write(dev, 0x043E, b43legacy_phy_read(dev, 0x043E)
+				    | 0x0004);
+	}
+	if (phy->rev >= 2 && phy->gmode)
+		b43legacy_calc_loopback_gain(dev);
+	if (phy->radio_rev != 8) {
+		if (phy->initval == 0xFFFF)
+			phy->initval = b43legacy_radio_init2050(dev);
+		else
+			b43legacy_radio_write16(dev, 0x0078, phy->initval);
+	}
+	if (phy->txctl2 == 0xFFFF)
+		b43legacy_phy_lo_g_measure(dev);
+	else {
+		if (phy->radio_ver == 0x2050 && phy->radio_rev == 8)
+			b43legacy_radio_write16(dev, 0x0052,
+						(phy->txctl1 << 4) |
+						phy->txctl2);
+		else
+			b43legacy_radio_write16(dev, 0x0052,
+						(b43legacy_radio_read16(dev,
+						 0x0052) & 0xFFF0) |
+						 phy->txctl1);
+		if (phy->rev >= 6)
+			b43legacy_phy_write(dev, 0x0036,
+					    (b43legacy_phy_read(dev, 0x0036)
+					     & 0x0FFF) | (phy->txctl2 << 12));
+		if (dev->dev->bus->sprom.r1.boardflags_lo &
+		    B43legacy_BFL_PACTRL)
+			b43legacy_phy_write(dev, 0x002E, 0x8075);
+		else
+			b43legacy_phy_write(dev, 0x002E, 0x807F);
+		if (phy->rev < 2)
+			b43legacy_phy_write(dev, 0x002F, 0x0101);
+		else
+			b43legacy_phy_write(dev, 0x002F, 0x0202);
+	}
+	if (phy->gmode || phy->rev >= 2) {
+		b43legacy_phy_lo_adjust(dev, 0);
+		b43legacy_phy_write(dev, 0x080F, 0x8078);
+	}
+
+	if (!(dev->dev->bus->sprom.r1.boardflags_lo & B43legacy_BFL_RSSI)) {
+		/* The specs state to update the NRSSI LT with
+		 * the value 0x7FFFFFFF here. I think that is some weird
+		 * compiler optimization in the original driver.
+		 * Essentially, what we do here is resetting all NRSSI LT
+		 * entries to -32 (see the limit_value() in nrssi_hw_update())
+		 */
+		b43legacy_nrssi_hw_update(dev, 0xFFFF);
+		b43legacy_calc_nrssi_threshold(dev);
+	} else if (phy->gmode || phy->rev >= 2) {
+		if (phy->nrssi[0] == -1000) {
+			B43legacy_WARN_ON(phy->nrssi[1] != -1000);
+			b43legacy_calc_nrssi_slope(dev);
+		} else {
+			B43legacy_WARN_ON(phy->nrssi[1] == -1000);
+			b43legacy_calc_nrssi_threshold(dev);
+		}
+	}
+	if (phy->radio_rev == 8)
+		b43legacy_phy_write(dev, 0x0805, 0x3230);
+	b43legacy_phy_init_pctl(dev);
+	if (dev->dev->bus->chip_id == 0x4306
+	    && dev->dev->bus->chip_package == 2) {
+		b43legacy_phy_write(dev, 0x0429,
+				    b43legacy_phy_read(dev, 0x0429) & 0xBFFF);
+		b43legacy_phy_write(dev, 0x04C3,
+				    b43legacy_phy_read(dev, 0x04C3) & 0x7FFF);
+	}
+}
+
+static u16 b43legacy_phy_lo_b_r15_loop(struct b43legacy_wldev *dev)
+{
+	int i;
+	u16 ret = 0;
+	unsigned long flags;
+
+	local_irq_save(flags);
+	for (i = 0; i < 10; i++) {
+		b43legacy_phy_write(dev, 0x0015, 0xAFA0);
+		udelay(1);
+		b43legacy_phy_write(dev, 0x0015, 0xEFA0);
+		udelay(10);
+		b43legacy_phy_write(dev, 0x0015, 0xFFA0);
+		udelay(40);
+		ret += b43legacy_phy_read(dev, 0x002C);
+	}
+	local_irq_restore(flags);
+	b43legacy_voluntary_preempt();
+
+	return ret;
+}
+
+void b43legacy_phy_lo_b_measure(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+	u16 regstack[12] = { 0 };
+	u16 mls;
+	u16 fval;
+	int i;
+	int j;
+
+	regstack[0] = b43legacy_phy_read(dev, 0x0015);
+	regstack[1] = b43legacy_radio_read16(dev, 0x0052) & 0xFFF0;
+
+	if (phy->radio_ver == 0x2053) {
+		regstack[2] = b43legacy_phy_read(dev, 0x000A);
+		regstack[3] = b43legacy_phy_read(dev, 0x002A);
+		regstack[4] = b43legacy_phy_read(dev, 0x0035);
+		regstack[5] = b43legacy_phy_read(dev, 0x0003);
+		regstack[6] = b43legacy_phy_read(dev, 0x0001);
+		regstack[7] = b43legacy_phy_read(dev, 0x0030);
+
+		regstack[8] = b43legacy_radio_read16(dev, 0x0043);
+		regstack[9] = b43legacy_radio_read16(dev, 0x007A);
+		regstack[10] = b43legacy_read16(dev, 0x03EC);
+		regstack[11] = b43legacy_radio_read16(dev, 0x0052) & 0x00F0;
+
+		b43legacy_phy_write(dev, 0x0030, 0x00FF);
+		b43legacy_write16(dev, 0x03EC, 0x3F3F);
+		b43legacy_phy_write(dev, 0x0035, regstack[4] & 0xFF7F);
+		b43legacy_radio_write16(dev, 0x007A, regstack[9] & 0xFFF0);
+	}
+	b43legacy_phy_write(dev, 0x0015, 0xB000);
+	b43legacy_phy_write(dev, 0x002B, 0x0004);
+
+	if (phy->radio_ver == 0x2053) {
+		b43legacy_phy_write(dev, 0x002B, 0x0203);
+		b43legacy_phy_write(dev, 0x002A, 0x08A3);
+	}
+
+	phy->minlowsig[0] = 0xFFFF;
+
+	for (i = 0; i < 4; i++) {
+		b43legacy_radio_write16(dev, 0x0052, regstack[1] | i);
+		b43legacy_phy_lo_b_r15_loop(dev);
+	}
+	for (i = 0; i < 10; i++) {
+		b43legacy_radio_write16(dev, 0x0052, regstack[1] | i);
+		mls = b43legacy_phy_lo_b_r15_loop(dev) / 10;
+		if (mls < phy->minlowsig[0]) {
+			phy->minlowsig[0] = mls;
+			phy->minlowsigpos[0] = i;
+		}
+	}
+	b43legacy_radio_write16(dev, 0x0052, regstack[1]
+				| phy->minlowsigpos[0]);
+
+	phy->minlowsig[1] = 0xFFFF;
+
+	for (i = -4; i < 5; i += 2) {
+		for (j = -4; j < 5; j += 2) {
+			if (j < 0)
+				fval = (0x0100 * i) + j + 0x0100;
+			else
+				fval = (0x0100 * i) + j;
+			b43legacy_phy_write(dev, 0x002F, fval);
+			mls = b43legacy_phy_lo_b_r15_loop(dev) / 10;
+			if (mls < phy->minlowsig[1]) {
+				phy->minlowsig[1] = mls;
+				phy->minlowsigpos[1] = fval;
+			}
+		}
+	}
+	phy->minlowsigpos[1] += 0x0101;
+
+	b43legacy_phy_write(dev, 0x002F, phy->minlowsigpos[1]);
+	if (phy->radio_ver == 0x2053) {
+		b43legacy_phy_write(dev, 0x000A, regstack[2]);
+		b43legacy_phy_write(dev, 0x002A, regstack[3]);
+		b43legacy_phy_write(dev, 0x0035, regstack[4]);
+		b43legacy_phy_write(dev, 0x0003, regstack[5]);
+		b43legacy_phy_write(dev, 0x0001, regstack[6]);
+		b43legacy_phy_write(dev, 0x0030, regstack[7]);
+
+		b43legacy_radio_write16(dev, 0x0043, regstack[8]);
+		b43legacy_radio_write16(dev, 0x007A, regstack[9]);
+
+		b43legacy_radio_write16(dev, 0x0052,
+					(b43legacy_radio_read16(dev, 0x0052)
+					& 0x000F) | regstack[11]);
+
+		b43legacy_write16(dev, 0x03EC, regstack[10]);
+	}
+	b43legacy_phy_write(dev, 0x0015, regstack[0]);
+}
+
+static inline
+u16 b43legacy_phy_lo_g_deviation_subval(struct b43legacy_wldev *dev,
+					u16 control)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+	u16 ret;
+	unsigned long flags;
+
+	local_irq_save(flags);
+	if (phy->gmode) {
+		b43legacy_phy_write(dev, 0x15, 0xE300);
+		control <<= 8;
+		b43legacy_phy_write(dev, 0x0812, control | 0x00B0);
+		udelay(5);
+		b43legacy_phy_write(dev, 0x0812, control | 0x00B2);
+		udelay(2);
+		b43legacy_phy_write(dev, 0x0812, control | 0x00B3);
+		udelay(4);
+		b43legacy_phy_write(dev, 0x0015, 0xF300);
+		udelay(8);
+	} else {
+		b43legacy_phy_write(dev, 0x0015, control | 0xEFA0);
+		udelay(2);
+		b43legacy_phy_write(dev, 0x0015, control | 0xEFE0);
+		udelay(4);
+		b43legacy_phy_write(dev, 0x0015, control | 0xFFE0);
+		udelay(8);
+	}
+	ret = b43legacy_phy_read(dev, 0x002D);
+	local_irq_restore(flags);
+	b43legacy_voluntary_preempt();
+
+	return ret;
+}
+
+static u32 b43legacy_phy_lo_g_singledeviation(struct b43legacy_wldev *dev,
+					      u16 control)
+{
+	int i;
+	u32 ret = 0;
+
+	for (i = 0; i < 8; i++)
+		ret += b43legacy_phy_lo_g_deviation_subval(dev, control);
+
+	return ret;
+}
+
+/* Write the LocalOscillator CONTROL */
+static inline
+void b43legacy_lo_write(struct b43legacy_wldev *dev,
+			struct b43legacy_lopair *pair)
+{
+	u16 value;
+
+	value = (u8)(pair->low);
+	value |= ((u8)(pair->high)) << 8;
+
+#ifdef CONFIG_B43LEGACY_DEBUG
+	/* Sanity check. */
+	if (pair->low < -8 || pair->low > 8 ||
+	    pair->high < -8 || pair->high > 8) {
+		struct b43legacy_phy *phy = &dev->phy;
+		b43legacydbg(dev->wl,
+		       "WARNING: Writing invalid LOpair "
+		       "(low: %d, high: %d, index: %lu)\n",
+		       pair->low, pair->high,
+		       (unsigned long)(pair - phy->_lo_pairs));
+		dump_stack();
+	}
+#endif
+
+	b43legacy_phy_write(dev, B43legacy_PHY_G_LO_CONTROL, value);
+}
+
+static inline
+struct b43legacy_lopair *b43legacy_find_lopair(struct b43legacy_wldev *dev,
+					       u16 bbatt,
+					       u16 rfatt,
+					       u16 tx)
+{
+	static const u8 dict[10] = { 11, 10, 11, 12, 13, 12, 13, 12, 13, 12 };
+	struct b43legacy_phy *phy = &dev->phy;
+
+	if (bbatt > 6)
+		bbatt = 6;
+	B43legacy_WARN_ON(rfatt >= 10);
+
+	if (tx == 3)
+		return b43legacy_get_lopair(phy, rfatt, bbatt);
+	return b43legacy_get_lopair(phy, dict[rfatt], bbatt);
+}
+
+static inline
+struct b43legacy_lopair *b43legacy_current_lopair(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+
+	return b43legacy_find_lopair(dev, phy->bbatt,
+				     phy->rfatt, phy->txctl1);
+}
+
+/* Adjust B/G LO */
+void b43legacy_phy_lo_adjust(struct b43legacy_wldev *dev, int fixed)
+{
+	struct b43legacy_lopair *pair;
+
+	if (fixed) {
+		/* Use fixed values. Only for initialization. */
+		pair = b43legacy_find_lopair(dev, 2, 3, 0);
+	} else
+		pair = b43legacy_current_lopair(dev);
+	b43legacy_lo_write(dev, pair);
+}
+
+static void b43legacy_phy_lo_g_measure_txctl2(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+	u16 txctl2 = 0;
+	u16 i;
+	u32 smallest;
+	u32 tmp;
+
+	b43legacy_radio_write16(dev, 0x0052, 0x0000);
+	udelay(10);
+	smallest = b43legacy_phy_lo_g_singledeviation(dev, 0);
+	for (i = 0; i < 16; i++) {
+		b43legacy_radio_write16(dev, 0x0052, i);
+		udelay(10);
+		tmp = b43legacy_phy_lo_g_singledeviation(dev, 0);
+		if (tmp < smallest) {
+			smallest = tmp;
+			txctl2 = i;
+		}
+	}
+	phy->txctl2 = txctl2;
+}
+
+static
+void b43legacy_phy_lo_g_state(struct b43legacy_wldev *dev,
+			      const struct b43legacy_lopair *in_pair,
+			      struct b43legacy_lopair *out_pair,
+			      u16 r27)
+{
+	static const struct b43legacy_lopair transitions[8] = {
+		{ .high =  1,  .low =  1, },
+		{ .high =  1,  .low =  0, },
+		{ .high =  1,  .low = -1, },
+		{ .high =  0,  .low = -1, },
+		{ .high = -1,  .low = -1, },
+		{ .high = -1,  .low =  0, },
+		{ .high = -1,  .low =  1, },
+		{ .high =  0,  .low =  1, },
+	};
+	struct b43legacy_lopair lowest_transition = {
+		.high = in_pair->high,
+		.low = in_pair->low,
+	};
+	struct b43legacy_lopair tmp_pair;
+	struct b43legacy_lopair transition;
+	int i = 12;
+	int state = 0;
+	int found_lower;
+	int j;
+	int begin;
+	int end;
+	u32 lowest_deviation;
+	u32 tmp;
+
+	/* Note that in_pair and out_pair can point to the same pair.
+	 * Be careful. */
+
+	b43legacy_lo_write(dev, &lowest_transition);
+	lowest_deviation = b43legacy_phy_lo_g_singledeviation(dev, r27);
+	do {
+		found_lower = 0;
+		B43legacy_WARN_ON(!(state >= 0 && state <= 8));
+		if (state == 0) {
+			begin = 1;
+			end = 8;
+		} else if (state % 2 == 0) {
+			begin = state - 1;
+			end = state + 1;
+		} else {
+			begin = state - 2;
+			end = state + 2;
+		}
+		if (begin < 1)
+			begin += 8;
+		if (end > 8)
+			end -= 8;
+
+		j = begin;
+		tmp_pair.high = lowest_transition.high;
+		tmp_pair.low = lowest_transition.low;
+		while (1) {
+			B43legacy_WARN_ON(!(j >= 1 && j <= 8));
+			transition.high = tmp_pair.high +
+					  transitions[j - 1].high;
+			transition.low = tmp_pair.low + transitions[j - 1].low;
+			if ((abs(transition.low) < 9)
+			     && (abs(transition.high) < 9)) {
+				b43legacy_lo_write(dev, &transition);
+				tmp = b43legacy_phy_lo_g_singledeviation(dev,
+								       r27);
+				if (tmp < lowest_deviation) {
+					lowest_deviation = tmp;
+					state = j;
+					found_lower = 1;
+
+					lowest_transition.high =
+								transition.high;
+					lowest_transition.low = transition.low;
+				}
+			}
+			if (j == end)
+				break;
+			if (j == 8)
+				j = 1;
+			else
+				j++;
+		}
+	} while (i-- && found_lower);
+
+	out_pair->high = lowest_transition.high;
+	out_pair->low = lowest_transition.low;
+}
+
+/* Set the baseband attenuation value on chip. */
+void b43legacy_phy_set_baseband_attenuation(struct b43legacy_wldev *dev,
+					    u16 bbatt)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+	u16 value;
+
+	if (phy->analog == 0) {
+		value = (b43legacy_read16(dev, 0x03E6) & 0xFFF0);
+		value |= (bbatt & 0x000F);
+		b43legacy_write16(dev, 0x03E6, value);
+		return;
+	}
+
+	if (phy->analog > 1) {
+		value = b43legacy_phy_read(dev, 0x0060) & 0xFFC3;
+		value |= (bbatt << 2) & 0x003C;
+	} else {
+		value = b43legacy_phy_read(dev, 0x0060) & 0xFF87;
+		value |= (bbatt << 3) & 0x0078;
+	}
+	b43legacy_phy_write(dev, 0x0060, value);
+}
+
+/* http://bcm-specs.sipsolutions.net/LocalOscillator/Measure */
+void b43legacy_phy_lo_g_measure(struct b43legacy_wldev *dev)
+{
+	static const u8 pairorder[10] = { 3, 1, 5, 7, 9, 2, 0, 4, 6, 8 };
+	const int is_initializing = (b43legacy_status(dev)
+				     < B43legacy_STAT_STARTED);
+	struct b43legacy_phy *phy = &dev->phy;
+	u16 h;
+	u16 i;
+	u16 oldi = 0;
+	u16 j;
+	struct b43legacy_lopair control;
+	struct b43legacy_lopair *tmp_control;
+	u16 tmp;
+	u16 regstack[16] = { 0 };
+	u8 oldchannel;
+
+	/* XXX: What are these? */
+	u8 r27 = 0;
+	u16 r31;
+
+	oldchannel = phy->channel;
+	/* Setup */
+	if (phy->gmode) {
+		regstack[0] = b43legacy_phy_read(dev, B43legacy_PHY_G_CRS);
+		regstack[1] = b43legacy_phy_read(dev, 0x0802);
+		b43legacy_phy_write(dev, B43legacy_PHY_G_CRS, regstack[0]
+				    & 0x7FFF);
+		b43legacy_phy_write(dev, 0x0802, regstack[1] & 0xFFFC);
+	}
+	regstack[3] = b43legacy_read16(dev, 0x03E2);
+	b43legacy_write16(dev, 0x03E2, regstack[3] | 0x8000);
+	regstack[4] = b43legacy_read16(dev, B43legacy_MMIO_CHANNEL_EXT);
+	regstack[5] = b43legacy_phy_read(dev, 0x15);
+	regstack[6] = b43legacy_phy_read(dev, 0x2A);
+	regstack[7] = b43legacy_phy_read(dev, 0x35);
+	regstack[8] = b43legacy_phy_read(dev, 0x60);
+	regstack[9] = b43legacy_radio_read16(dev, 0x43);
+	regstack[10] = b43legacy_radio_read16(dev, 0x7A);
+	regstack[11] = b43legacy_radio_read16(dev, 0x52);
+	if (phy->gmode) {
+		regstack[12] = b43legacy_phy_read(dev, 0x0811);
+		regstack[13] = b43legacy_phy_read(dev, 0x0812);
+		regstack[14] = b43legacy_phy_read(dev, 0x0814);
+		regstack[15] = b43legacy_phy_read(dev, 0x0815);
+	}
+	b43legacy_radio_selectchannel(dev, 6, 0);
+	if (phy->gmode) {
+		b43legacy_phy_write(dev, B43legacy_PHY_G_CRS, regstack[0]
+				    & 0x7FFF);
+		b43legacy_phy_write(dev, 0x0802, regstack[1] & 0xFFFC);
+		b43legacy_dummy_transmission(dev);
+	}
+	b43legacy_radio_write16(dev, 0x0043, 0x0006);
+
+	b43legacy_phy_set_baseband_attenuation(dev, 2);
+
+	b43legacy_write16(dev, B43legacy_MMIO_CHANNEL_EXT, 0x0000);
+	b43legacy_phy_write(dev, 0x002E, 0x007F);
+	b43legacy_phy_write(dev, 0x080F, 0x0078);
+	b43legacy_phy_write(dev, 0x0035, regstack[7] & ~(1 << 7));
+	b43legacy_radio_write16(dev, 0x007A, regstack[10] & 0xFFF0);
+	b43legacy_phy_write(dev, 0x002B, 0x0203);
+	b43legacy_phy_write(dev, 0x002A, 0x08A3);
+	if (phy->gmode) {
+		b43legacy_phy_write(dev, 0x0814, regstack[14] | 0x0003);
+		b43legacy_phy_write(dev, 0x0815, regstack[15] & 0xFFFC);
+		b43legacy_phy_write(dev, 0x0811, 0x01B3);
+		b43legacy_phy_write(dev, 0x0812, 0x00B2);
+	}
+	if (is_initializing)
+		b43legacy_phy_lo_g_measure_txctl2(dev);
+	b43legacy_phy_write(dev, 0x080F, 0x8078);
+
+	/* Measure */
+	control.low = 0;
+	control.high = 0;
+	for (h = 0; h < 10; h++) {
+		/* Loop over each possible RadioAttenuation (0-9) */
+		i = pairorder[h];
+		if (is_initializing) {
+			if (i == 3) {
+				control.low = 0;
+				control.high = 0;
+			} else if (((i % 2 == 1) && (oldi % 2 == 1)) ||
+				  ((i % 2 == 0) && (oldi % 2 == 0))) {
+				tmp_control = b43legacy_get_lopair(phy, oldi,
+								   0);
+				memcpy(&control, tmp_control, sizeof(control));
+			} else {
+				tmp_control = b43legacy_get_lopair(phy, 3, 0);
+				memcpy(&control, tmp_control, sizeof(control));
+			}
+		}
+		/* Loop over each possible BasebandAttenuation/2 */
+		for (j = 0; j < 4; j++) {
+			if (is_initializing) {
+				tmp = i * 2 + j;
+				r27 = 0;
+				r31 = 0;
+				if (tmp > 14) {
+					r31 = 1;
+					if (tmp > 17)
+						r27 = 1;
+					if (tmp > 19)
+						r27 = 2;
+				}
+			} else {
+				tmp_control = b43legacy_get_lopair(phy, i,
+								   j * 2);
+				if (!tmp_control->used)
+					continue;
+				memcpy(&control, tmp_control, sizeof(control));
+				r27 = 3;
+				r31 = 0;
+			}
+			b43legacy_radio_write16(dev, 0x43, i);
+			b43legacy_radio_write16(dev, 0x52, phy->txctl2);
+			udelay(10);
+			b43legacy_voluntary_preempt();
+
+			b43legacy_phy_set_baseband_attenuation(dev, j * 2);
+
+			tmp = (regstack[10] & 0xFFF0);
+			if (r31)
+				tmp |= 0x0008;
+			b43legacy_radio_write16(dev, 0x007A, tmp);
+
+			tmp_control = b43legacy_get_lopair(phy, i, j * 2);
+			b43legacy_phy_lo_g_state(dev, &control, tmp_control,
+						 r27);
+		}
+		oldi = i;
+	}
+	/* Loop over each possible RadioAttenuation (10-13) */
+	for (i = 10; i < 14; i++) {
+		/* Loop over each possible BasebandAttenuation/2 */
+		for (j = 0; j < 4; j++) {
+			if (is_initializing) {
+				tmp_control = b43legacy_get_lopair(phy, i - 9,
+								 j * 2);
+				memcpy(&control, tmp_control, sizeof(control));
+				/* FIXME: The next line is wrong, as the
+				 * following if statement can never trigger. */
+				tmp = (i - 9) * 2 + j - 5;
+				r27 = 0;
+				r31 = 0;
+				if (tmp > 14) {
+					r31 = 1;
+					if (tmp > 17)
+						r27 = 1;
+					if (tmp > 19)
+						r27 = 2;
+				}
+			} else {
+				tmp_control = b43legacy_get_lopair(phy, i - 9,
+								   j * 2);
+				if (!tmp_control->used)
+					continue;
+				memcpy(&control, tmp_control, sizeof(control));
+				r27 = 3;
+				r31 = 0;
+			}
+			b43legacy_radio_write16(dev, 0x43, i - 9);
+			/* FIXME: shouldn't txctl1 be zero in the next line
+			 * and 3 in the loop above? */
+			b43legacy_radio_write16(dev, 0x52,
+					      phy->txctl2
+					      | (3/*txctl1*/ << 4));
+			udelay(10);
+			b43legacy_voluntary_preempt();
+
+			b43legacy_phy_set_baseband_attenuation(dev, j * 2);
+
+			tmp = (regstack[10] & 0xFFF0);
+			if (r31)
+				tmp |= 0x0008;
+			b43legacy_radio_write16(dev, 0x7A, tmp);
+
+			tmp_control = b43legacy_get_lopair(phy, i, j * 2);
+			b43legacy_phy_lo_g_state(dev, &control, tmp_control,
+						 r27);
+		}
+	}
+
+	/* Restoration */
+	if (phy->gmode) {
+		b43legacy_phy_write(dev, 0x0015, 0xE300);
+		b43legacy_phy_write(dev, 0x0812, (r27 << 8) | 0xA0);
+		udelay(5);
+		b43legacy_phy_write(dev, 0x0812, (r27 << 8) | 0xA2);
+		udelay(2);
+		b43legacy_phy_write(dev, 0x0812, (r27 << 8) | 0xA3);
+		b43legacy_voluntary_preempt();
+	} else
+		b43legacy_phy_write(dev, 0x0015, r27 | 0xEFA0);
+	b43legacy_phy_lo_adjust(dev, is_initializing);
+	b43legacy_phy_write(dev, 0x002E, 0x807F);
+	if (phy->gmode)
+		b43legacy_phy_write(dev, 0x002F, 0x0202);
+	else
+		b43legacy_phy_write(dev, 0x002F, 0x0101);
+	b43legacy_write16(dev, B43legacy_MMIO_CHANNEL_EXT, regstack[4]);
+	b43legacy_phy_write(dev, 0x0015, regstack[5]);
+	b43legacy_phy_write(dev, 0x002A, regstack[6]);
+	b43legacy_phy_write(dev, 0x0035, regstack[7]);
+	b43legacy_phy_write(dev, 0x0060, regstack[8]);
+	b43legacy_radio_write16(dev, 0x0043, regstack[9]);
+	b43legacy_radio_write16(dev, 0x007A, regstack[10]);
+	regstack[11] &= 0x00F0;
+	regstack[11] |= (b43legacy_radio_read16(dev, 0x52) & 0x000F);
+	b43legacy_radio_write16(dev, 0x52, regstack[11]);
+	b43legacy_write16(dev, 0x03E2, regstack[3]);
+	if (phy->gmode) {
+		b43legacy_phy_write(dev, 0x0811, regstack[12]);
+		b43legacy_phy_write(dev, 0x0812, regstack[13]);
+		b43legacy_phy_write(dev, 0x0814, regstack[14]);
+		b43legacy_phy_write(dev, 0x0815, regstack[15]);
+		b43legacy_phy_write(dev, B43legacy_PHY_G_CRS, regstack[0]);
+		b43legacy_phy_write(dev, 0x0802, regstack[1]);
+	}
+	b43legacy_radio_selectchannel(dev, oldchannel, 1);
+
+#ifdef CONFIG_B43LEGACY_DEBUG
+	{
+		/* Sanity check for all lopairs. */
+		for (i = 0; i < B43legacy_LO_COUNT; i++) {
+			tmp_control = phy->_lo_pairs + i;
+			if (tmp_control->low < -8 || tmp_control->low > 8 ||
+			    tmp_control->high < -8 || tmp_control->high > 8)
+				b43legacywarn(dev->wl,
+				       "WARNING: Invalid LOpair (low: %d, high:"
+				       " %d, index: %d)\n",
+				       tmp_control->low, tmp_control->high, i);
+		}
+	}
+#endif /* CONFIG_B43LEGACY_DEBUG */
+}
+
+static
+void b43legacy_phy_lo_mark_current_used(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_lopair *pair;
+
+	pair = b43legacy_current_lopair(dev);
+	pair->used = 1;
+}
+
+void b43legacy_phy_lo_mark_all_unused(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+	struct b43legacy_lopair *pair;
+	int i;
+
+	for (i = 0; i < B43legacy_LO_COUNT; i++) {
+		pair = phy->_lo_pairs + i;
+		pair->used = 0;
+	}
+}
+
+/* http://bcm-specs.sipsolutions.net/EstimatePowerOut
+ * This function converts a TSSI value to dBm in Q5.2
+ */
+static s8 b43legacy_phy_estimate_power_out(struct b43legacy_wldev *dev, s8 tssi)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+	s8 dbm = 0;
+	s32 tmp;
+
+	tmp = phy->idle_tssi;
+	tmp += tssi;
+	tmp -= phy->savedpctlreg;
+
+	switch (phy->type) {
+	case B43legacy_PHYTYPE_B:
+	case B43legacy_PHYTYPE_G:
+		tmp = limit_value(tmp, 0x00, 0x3F);
+		dbm = phy->tssi2dbm[tmp];
+		break;
+	default:
+		B43legacy_BUG_ON(1);
+	}
+
+	return dbm;
+}
+
+/* http://bcm-specs.sipsolutions.net/RecalculateTransmissionPower */
+void b43legacy_phy_xmitpower(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+	u16 tmp;
+	u16 txpower;
+	s8 v0;
+	s8 v1;
+	s8 v2;
+	s8 v3;
+	s8 average;
+	int max_pwr;
+	s16 desired_pwr;
+	s16 estimated_pwr;
+	s16 pwr_adjust;
+	s16 radio_att_delta;
+	s16 baseband_att_delta;
+	s16 radio_attenuation;
+	s16 baseband_attenuation;
+	unsigned long phylock_flags;
+
+	if (phy->savedpctlreg == 0xFFFF)
+		return;
+	if ((dev->dev->bus->boardinfo.type == 0x0416) &&
+	    is_bcm_board_vendor(dev))
+		return;
+#ifdef CONFIG_B43LEGACY_DEBUG
+	if (phy->manual_txpower_control)
+		return;
+#endif
+
+	B43legacy_BUG_ON(!(phy->type == B43legacy_PHYTYPE_B ||
+			 phy->type == B43legacy_PHYTYPE_G));
+	tmp = b43legacy_shm_read16(dev, B43legacy_SHM_SHARED, 0x0058);
+	v0 = (s8)(tmp & 0x00FF);
+	v1 = (s8)((tmp & 0xFF00) >> 8);
+	tmp = b43legacy_shm_read16(dev, B43legacy_SHM_SHARED, 0x005A);
+	v2 = (s8)(tmp & 0x00FF);
+	v3 = (s8)((tmp & 0xFF00) >> 8);
+	tmp = 0;
+
+	if (v0 == 0x7F || v1 == 0x7F || v2 == 0x7F || v3 == 0x7F) {
+		tmp = b43legacy_shm_read16(dev, B43legacy_SHM_SHARED,
+					 0x0070);
+		v0 = (s8)(tmp & 0x00FF);
+		v1 = (s8)((tmp & 0xFF00) >> 8);
+		tmp = b43legacy_shm_read16(dev, B43legacy_SHM_SHARED,
+					 0x0072);
+		v2 = (s8)(tmp & 0x00FF);
+		v3 = (s8)((tmp & 0xFF00) >> 8);
+		if (v0 == 0x7F || v1 == 0x7F || v2 == 0x7F || v3 == 0x7F)
+			return;
+		v0 = (v0 + 0x20) & 0x3F;
+		v1 = (v1 + 0x20) & 0x3F;
+		v2 = (v2 + 0x20) & 0x3F;
+		v3 = (v3 + 0x20) & 0x3F;
+		tmp = 1;
+	}
+	b43legacy_radio_clear_tssi(dev);
+
+	average = (v0 + v1 + v2 + v3 + 2) / 4;
+
+	if (tmp && (b43legacy_shm_read16(dev, B43legacy_SHM_SHARED, 0x005E)
+	    & 0x8))
+		average -= 13;
+
+	estimated_pwr = b43legacy_phy_estimate_power_out(dev, average);
+
+	max_pwr = dev->dev->bus->sprom.r1.maxpwr_bg;
+
+	if ((dev->dev->bus->sprom.r1.boardflags_lo
+	     & B43legacy_BFL_PACTRL) &&
+	    (phy->type == B43legacy_PHYTYPE_G))
+		max_pwr -= 0x3;
+	if (unlikely(max_pwr <= 0)) {
+		b43legacywarn(dev->wl, "Invalid max-TX-power value in SPROM."
+			"\n");
+		max_pwr = 74; /* fake it */
+		dev->dev->bus->sprom.r1.maxpwr_bg = max_pwr;
+	}
+
+	/* Use regulatory information to get the maximum power.
+	 * In the absence of such data from mac80211, we will use 20 dBm, which
+	 * is the value for the EU, US, Canada, and most of the world.
+	 * The regulatory maximum is reduced by the antenna gain (from sprom)
+	 * and 1.5 dBm (a safety factor??). The result is in Q5.2 format
+	 * which accounts for the factor of 4 */
+#define REG_MAX_PWR 20
+	max_pwr = min(REG_MAX_PWR * 4 - dev->dev->bus->sprom.r1.antenna_gain_bg
+		      - 0x6, max_pwr);
+
+	/* find the desired power in Q5.2 - power_level is in dBm
+	 * and limit it - max_pwr is already in Q5.2 */
+	desired_pwr = limit_value(phy->power_level << 2, 0, max_pwr);
+	if (b43legacy_debug(dev, B43legacy_DBG_XMITPOWER))
+		b43legacydbg(dev->wl, "Current TX power output: " Q52_FMT
+		       " dBm, Desired TX power output: " Q52_FMT
+		       " dBm\n", Q52_ARG(estimated_pwr),
+		       Q52_ARG(desired_pwr));
+	/* Check if we need to adjust the current power. The factor of 2 is
+	 * for damping */
+	pwr_adjust = (desired_pwr - estimated_pwr) / 2;
+	/* RF attenuation delta
+	 * The minus sign is because lower attenuation => more power */
+	radio_att_delta = -(pwr_adjust + 7) >> 3;
+	/* Baseband attenuation delta */
+	baseband_att_delta = -(pwr_adjust >> 1) - (4 * radio_att_delta);
+	/* Do we need to adjust anything? */
+	if ((radio_att_delta == 0) && (baseband_att_delta == 0)) {
+		b43legacy_phy_lo_mark_current_used(dev);
+		return;
+	}
+
+	/* Calculate the new attenuation values. */
+	baseband_attenuation = phy->bbatt;
+	baseband_attenuation += baseband_att_delta;
+	radio_attenuation = phy->rfatt;
+	radio_attenuation += radio_att_delta;
+
+	/* Get baseband and radio attenuation values into permitted ranges.
+	 * baseband 0-11, radio 0-9.
+	 * Radio attenuation affects power level 4 times as much as baseband.
+	 */
+	if (radio_attenuation < 0) {
+		baseband_attenuation -= (4 * -radio_attenuation);
+		radio_attenuation = 0;
+	} else if (radio_attenuation > 9) {
+		baseband_attenuation += (4 * (radio_attenuation - 9));
+		radio_attenuation = 9;
+	} else {
+		while (baseband_attenuation < 0 && radio_attenuation > 0) {
+			baseband_attenuation += 4;
+			radio_attenuation--;
+		}
+		while (baseband_attenuation > 11 && radio_attenuation < 9) {
+			baseband_attenuation -= 4;
+			radio_attenuation++;
+		}
+	}
+	baseband_attenuation = limit_value(baseband_attenuation, 0, 11);
+
+	txpower = phy->txctl1;
+	if ((phy->radio_ver == 0x2050) && (phy->radio_rev == 2)) {
+		if (radio_attenuation <= 1) {
+			if (txpower == 0) {
+				txpower = 3;
+				radio_attenuation += 2;
+				baseband_attenuation += 2;
+			} else if (dev->dev->bus->sprom.r1.boardflags_lo
+				   & B43legacy_BFL_PACTRL) {
+				baseband_attenuation += 4 *
+						     (radio_attenuation - 2);
+				radio_attenuation = 2;
+			}
+		} else if (radio_attenuation > 4 && txpower != 0) {
+			txpower = 0;
+			if (baseband_attenuation < 3) {
+				radio_attenuation -= 3;
+				baseband_attenuation += 2;
+			} else {
+				radio_attenuation -= 2;
+				baseband_attenuation -= 2;
+			}
+		}
+	}
+	/* Save the control values */
+	phy->txctl1 = txpower;
+	baseband_attenuation = limit_value(baseband_attenuation, 0, 11);
+	radio_attenuation = limit_value(radio_attenuation, 0, 9);
+	phy->rfatt = radio_attenuation;
+	phy->bbatt = baseband_attenuation;
+
+	/* Adjust the hardware */
+	b43legacy_phy_lock(dev, phylock_flags);
+	b43legacy_radio_lock(dev);
+	b43legacy_radio_set_txpower_bg(dev, baseband_attenuation,
+				       radio_attenuation, txpower);
+	b43legacy_phy_lo_mark_current_used(dev);
+	b43legacy_radio_unlock(dev);
+	b43legacy_phy_unlock(dev, phylock_flags);
+}
+
+static inline
+s32 b43legacy_tssi2dbm_ad(s32 num, s32 den)
+{
+	if (num < 0)
+		return num/den;
+	else
+		return (num+den/2)/den;
+}
+
+static inline
+s8 b43legacy_tssi2dbm_entry(s8 entry [], u8 index, s16 pab0, s16 pab1, s16 pab2)
+{
+	s32 m1;
+	s32 m2;
+	s32 f = 256;
+	s32 q;
+	s32 delta;
+	s8 i = 0;
+
+	m1 = b43legacy_tssi2dbm_ad(16 * pab0 + index * pab1, 32);
+	m2 = max(b43legacy_tssi2dbm_ad(32768 + index * pab2, 256), 1);
+	do {
+		if (i > 15)
+			return -EINVAL;
+		q = b43legacy_tssi2dbm_ad(f * 4096 -
+					  b43legacy_tssi2dbm_ad(m2 * f, 16) *
+					  f, 2048);
+		delta = abs(q - f);
+		f = q;
+		i++;
+	} while (delta >= 2);
+	entry[index] = limit_value(b43legacy_tssi2dbm_ad(m1 * f, 8192),
+				   -127, 128);
+	return 0;
+}
+
+/* http://bcm-specs.sipsolutions.net/TSSI_to_DBM_Table */
+int b43legacy_phy_init_tssi2dbm_table(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+	s16 pab0;
+	s16 pab1;
+	s16 pab2;
+	u8 idx;
+	s8 *dyn_tssi2dbm;
+
+	B43legacy_WARN_ON(!(phy->type == B43legacy_PHYTYPE_B ||
+			  phy->type == B43legacy_PHYTYPE_G));
+	pab0 = (s16)(dev->dev->bus->sprom.r1.pa0b0);
+	pab1 = (s16)(dev->dev->bus->sprom.r1.pa0b1);
+	pab2 = (s16)(dev->dev->bus->sprom.r1.pa0b2);
+
+	if ((dev->dev->bus->chip_id == 0x4301) && (phy->radio_ver != 0x2050)) {
+		phy->idle_tssi = 0x34;
+		phy->tssi2dbm = b43legacy_tssi2dbm_b_table;
+		return 0;
+	}
+
+	if (pab0 != 0 && pab1 != 0 && pab2 != 0 &&
+	    pab0 != -1 && pab1 != -1 && pab2 != -1) {
+		/* The pabX values are set in SPROM. Use them. */
+		if ((s8)dev->dev->bus->sprom.r1.itssi_bg != 0 &&
+		    (s8)dev->dev->bus->sprom.r1.itssi_bg != -1)
+			phy->idle_tssi = (s8)(dev->dev->bus->sprom.r1.itssi_bg);
+		else
+			phy->idle_tssi = 62;
+		dyn_tssi2dbm = kmalloc(64, GFP_KERNEL);
+		if (dyn_tssi2dbm == NULL) {
+			b43legacyerr(dev->wl, "Could not allocate memory"
+			       "for tssi2dbm table\n");
+			return -ENOMEM;
+		}
+		for (idx = 0; idx < 64; idx++)
+			if (b43legacy_tssi2dbm_entry(dyn_tssi2dbm, idx, pab0,
+						     pab1, pab2)) {
+				phy->tssi2dbm = NULL;
+				b43legacyerr(dev->wl, "Could not generate "
+				       "tssi2dBm table\n");
+				kfree(dyn_tssi2dbm);
+				return -ENODEV;
+			}
+		phy->tssi2dbm = dyn_tssi2dbm;
+		phy->dyn_tssi_tbl = 1;
+	} else {
+		/* pabX values not set in SPROM. */
+		switch (phy->type) {
+		case B43legacy_PHYTYPE_B:
+			phy->idle_tssi = 0x34;
+			phy->tssi2dbm = b43legacy_tssi2dbm_b_table;
+			break;
+		case B43legacy_PHYTYPE_G:
+			phy->idle_tssi = 0x34;
+			phy->tssi2dbm = b43legacy_tssi2dbm_g_table;
+			break;
+		}
+	}
+
+	return 0;
+}
+
+int b43legacy_phy_init(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+	int err = -ENODEV;
+
+	switch (phy->type) {
+	case B43legacy_PHYTYPE_B:
+		switch (phy->rev) {
+		case 2:
+			b43legacy_phy_initb2(dev);
+			err = 0;
+			break;
+		case 4:
+			b43legacy_phy_initb4(dev);
+			err = 0;
+			break;
+		case 5:
+			b43legacy_phy_initb5(dev);
+			err = 0;
+			break;
+		case 6:
+			b43legacy_phy_initb6(dev);
+			err = 0;
+			break;
+		}
+		break;
+	case B43legacy_PHYTYPE_G:
+		b43legacy_phy_initg(dev);
+		err = 0;
+		break;
+	}
+	if (err)
+		b43legacyerr(dev->wl, "Unknown PHYTYPE found\n");
+
+	return err;
+}
+
+void b43legacy_phy_set_antenna_diversity(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+	u16 antennadiv;
+	u16 offset;
+	u16 value;
+	u32 ucodeflags;
+
+	antennadiv = phy->antenna_diversity;
+
+	if (antennadiv == 0xFFFF)
+		antennadiv = 3;
+	B43legacy_WARN_ON(antennadiv > 3);
+
+	ucodeflags = b43legacy_shm_read32(dev, B43legacy_SHM_SHARED,
+					  B43legacy_UCODEFLAGS_OFFSET);
+	b43legacy_shm_write32(dev, B43legacy_SHM_SHARED,
+			      B43legacy_UCODEFLAGS_OFFSET,
+			      ucodeflags & ~B43legacy_UCODEFLAG_AUTODIV);
+
+	switch (phy->type) {
+	case B43legacy_PHYTYPE_G:
+		offset = 0x0400;
+
+		if (antennadiv == 2)
+			value = (3/*automatic*/ << 7);
+		else
+			value = (antennadiv << 7);
+		b43legacy_phy_write(dev, offset + 1,
+				    (b43legacy_phy_read(dev, offset + 1)
+				    & 0x7E7F) | value);
+
+		if (antennadiv >= 2) {
+			if (antennadiv == 2)
+				value = (antennadiv << 7);
+			else
+				value = (0/*force0*/ << 7);
+			b43legacy_phy_write(dev, offset + 0x2B,
+					    (b43legacy_phy_read(dev,
+					    offset + 0x2B)
+					    & 0xFEFF) | value);
+		}
+
+		if (phy->type == B43legacy_PHYTYPE_G) {
+			if (antennadiv >= 2)
+				b43legacy_phy_write(dev, 0x048C,
+						    b43legacy_phy_read(dev,
+						    0x048C) | 0x2000);
+			else
+				b43legacy_phy_write(dev, 0x048C,
+						    b43legacy_phy_read(dev,
+						    0x048C) & ~0x2000);
+			if (phy->rev >= 2) {
+				b43legacy_phy_write(dev, 0x0461,
+						    b43legacy_phy_read(dev,
+						    0x0461) | 0x0010);
+				b43legacy_phy_write(dev, 0x04AD,
+						    (b43legacy_phy_read(dev,
+						    0x04AD)
+						    & 0x00FF) | 0x0015);
+				if (phy->rev == 2)
+					b43legacy_phy_write(dev, 0x0427,
+							    0x0008);
+				else
+					b43legacy_phy_write(dev, 0x0427,
+						(b43legacy_phy_read(dev, 0x0427)
+						 & 0x00FF) | 0x0008);
+			} else if (phy->rev >= 6)
+				b43legacy_phy_write(dev, 0x049B, 0x00DC);
+		} else {
+			if (phy->rev < 3)
+				b43legacy_phy_write(dev, 0x002B,
+						    (b43legacy_phy_read(dev,
+						    0x002B) & 0x00FF)
+						    | 0x0024);
+			else {
+				b43legacy_phy_write(dev, 0x0061,
+						    b43legacy_phy_read(dev,
+						    0x0061) | 0x0010);
+				if (phy->rev == 3) {
+					b43legacy_phy_write(dev, 0x0093,
+							    0x001D);
+					b43legacy_phy_write(dev, 0x0027,
+							    0x0008);
+				} else {
+					b43legacy_phy_write(dev, 0x0093,
+							    0x003A);
+					b43legacy_phy_write(dev, 0x0027,
+						(b43legacy_phy_read(dev, 0x0027)
+						 & 0x00FF) | 0x0008);
+				}
+			}
+		}
+		break;
+	case B43legacy_PHYTYPE_B:
+		if (dev->dev->id.revision == 2)
+			value = (3/*automatic*/ << 7);
+		else
+			value = (antennadiv << 7);
+		b43legacy_phy_write(dev, 0x03E2,
+				    (b43legacy_phy_read(dev, 0x03E2)
+				    & 0xFE7F) | value);
+		break;
+	default:
+		B43legacy_WARN_ON(1);
+	}
+
+	if (antennadiv >= 2) {
+		ucodeflags = b43legacy_shm_read32(dev, B43legacy_SHM_SHARED,
+						  B43legacy_UCODEFLAGS_OFFSET);
+		b43legacy_shm_write32(dev, B43legacy_SHM_SHARED,
+				      B43legacy_UCODEFLAGS_OFFSET,
+				      ucodeflags | B43legacy_UCODEFLAG_AUTODIV);
+	}
+
+	phy->antenna_diversity = antennadiv;
+}
+
+/* Set the PowerSavingControlBits.
+ * Bitvalues:
+ *   0  => unset the bit
+ *   1  => set the bit
+ *   -1 => calculate the bit
+ */
+void b43legacy_power_saving_ctl_bits(struct b43legacy_wldev *dev,
+				     int bit25, int bit26)
+{
+	int i;
+	u32 status;
+
+/* FIXME: Force 25 to off and 26 to on for now: */
+bit25 = 0;
+bit26 = 1;
+
+	if (bit25 == -1) {
+		/* TODO: If powersave is not off and FIXME is not set and we
+		 *	are not in adhoc and thus is not an AP and we arei
+		 *	associated, set bit 25 */
+	}
+	if (bit26 == -1) {
+		/* TODO: If the device is awake or this is an AP, or we are
+		 *	scanning, or FIXME, or we are associated, or FIXME,
+		 *	or the latest PS-Poll packet sent was successful,
+		 *	set bit26  */
+	}
+	status = b43legacy_read32(dev, B43legacy_MMIO_STATUS_BITFIELD);
+	if (bit25)
+		status |= B43legacy_SBF_PS1;
+	else
+		status &= ~B43legacy_SBF_PS1;
+	if (bit26)
+		status |= B43legacy_SBF_PS2;
+	else
+		status &= ~B43legacy_SBF_PS2;
+	b43legacy_write32(dev, B43legacy_MMIO_STATUS_BITFIELD, status);
+	if (bit26 && dev->dev->id.revision >= 5) {
+		for (i = 0; i < 100; i++) {
+			if (b43legacy_shm_read32(dev, B43legacy_SHM_SHARED,
+						 0x0040) != 4)
+				break;
+			udelay(10);
+		}
+	}
+}
diff --git a/drivers/net/wireless/b43legacy/phy.h b/drivers/net/wireless/b43legacy/phy.h
new file mode 100644
index 000000000000..f11b4271714c
--- /dev/null
+++ b/drivers/net/wireless/b43legacy/phy.h
@@ -0,0 +1,219 @@
+/*
+
+  Broadcom B43legacy wireless driver
+
+  Copyright (c) 2005 Martin Langer <martin-langer@gmx.de>,
+		     Stefano Brivio <st3@riseup.net>
+		     Michael Buesch <mbuesch@freenet.de>
+		     Danny van Dyk <kugelfang@gentoo.org>
+		     Andreas Jaggi <andreas.jaggi@waterwave.ch>
+  Copyright (c) 2007 Larry Finger <Larry.Finger@lwfinger.net>
+
+  Some parts of the code in this file are derived from the ipw2200
+  driver  Copyright(c) 2003 - 2004 Intel Corporation.
+
+  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, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with this program; see the file COPYING.  If not, write to
+  the Free Software Foundation, Inc., 51 Franklin Steet, Fifth Floor,
+  Boston, MA 02110-1301, USA.
+
+*/
+
+#ifndef B43legacy_PHY_H_
+#define B43legacy_PHY_H_
+
+#include <linux/types.h>
+
+enum {
+	B43legacy_ANTENNA0,	  /* Antenna 0 */
+	B43legacy_ANTENNA1,	  /* Antenna 0 */
+	B43legacy_ANTENNA_AUTO1,  /* Automatic, starting with antenna 1 */
+	B43legacy_ANTENNA_AUTO0,  /* Automatic, starting with antenna 0 */
+
+	B43legacy_ANTENNA_AUTO	= B43legacy_ANTENNA_AUTO0,
+	B43legacy_ANTENNA_DEFAULT = B43legacy_ANTENNA_AUTO,
+};
+
+enum {
+	B43legacy_INTERFMODE_NONE,
+	B43legacy_INTERFMODE_NONWLAN,
+	B43legacy_INTERFMODE_MANUALWLAN,
+	B43legacy_INTERFMODE_AUTOWLAN,
+};
+
+/*** PHY Registers ***/
+
+/* Routing */
+#define B43legacy_PHYROUTE_OFDM_GPHY	0x400
+#define B43legacy_PHYROUTE_EXT_GPHY	0x800
+
+/* Base registers. */
+#define B43legacy_PHY_BASE(reg)	(reg)
+/* OFDM (A) registers of a G-PHY */
+#define B43legacy_PHY_OFDM(reg)	((reg) | B43legacy_PHYROUTE_OFDM_GPHY)
+/* Extended G-PHY registers */
+#define B43legacy_PHY_EXTG(reg)	((reg) | B43legacy_PHYROUTE_EXT_GPHY)
+
+
+/* Extended G-PHY Registers */
+#define B43legacy_PHY_CLASSCTL		B43legacy_PHY_EXTG(0x02)	/* Classify control */
+#define B43legacy_PHY_GTABCTL		B43legacy_PHY_EXTG(0x03)	/* G-PHY table control (see below) */
+#define  B43legacy_PHY_GTABOFF		0x03FF			/* G-PHY table offset (see below) */
+#define  B43legacy_PHY_GTABNR		0xFC00			/* G-PHY table number (see below) */
+#define  B43legacy_PHY_GTABNR_SHIFT	10
+#define B43legacy_PHY_GTABDATA		B43legacy_PHY_EXTG(0x04)	/* G-PHY table data */
+#define B43legacy_PHY_LO_MASK		B43legacy_PHY_EXTG(0x0F)	/* Local Oscillator control mask */
+#define B43legacy_PHY_LO_CTL		B43legacy_PHY_EXTG(0x10)	/* Local Oscillator control */
+#define B43legacy_PHY_RFOVER		B43legacy_PHY_EXTG(0x11)	/* RF override */
+#define B43legacy_PHY_RFOVERVAL		B43legacy_PHY_EXTG(0x12)	/* RF override value */
+/*** OFDM table numbers ***/
+#define B43legacy_OFDMTAB(number, offset)				\
+			  (((number) << B43legacy_PHY_OTABLENR_SHIFT)	\
+			  | (offset))
+#define B43legacy_OFDMTAB_AGC1		B43legacy_OFDMTAB(0x00, 0)
+#define B43legacy_OFDMTAB_GAIN0		B43legacy_OFDMTAB(0x00, 0)
+#define B43legacy_OFDMTAB_GAINX		B43legacy_OFDMTAB(0x01, 0)
+#define B43legacy_OFDMTAB_GAIN1		B43legacy_OFDMTAB(0x01, 4)
+#define B43legacy_OFDMTAB_AGC3		B43legacy_OFDMTAB(0x02, 0)
+#define B43legacy_OFDMTAB_GAIN2		B43legacy_OFDMTAB(0x02, 3)
+#define B43legacy_OFDMTAB_LNAHPFGAIN1	B43legacy_OFDMTAB(0x03, 0)
+#define B43legacy_OFDMTAB_WRSSI		B43legacy_OFDMTAB(0x04, 0)
+#define B43legacy_OFDMTAB_LNAHPFGAIN2	B43legacy_OFDMTAB(0x04, 0)
+#define B43legacy_OFDMTAB_NOISESCALE	B43legacy_OFDMTAB(0x05, 0)
+#define B43legacy_OFDMTAB_AGC2		B43legacy_OFDMTAB(0x06, 0)
+#define B43legacy_OFDMTAB_ROTOR		B43legacy_OFDMTAB(0x08, 0)
+#define B43legacy_OFDMTAB_ADVRETARD	B43legacy_OFDMTAB(0x09, 0)
+#define B43legacy_OFDMTAB_DAC		B43legacy_OFDMTAB(0x0C, 0)
+#define B43legacy_OFDMTAB_DC		B43legacy_OFDMTAB(0x0E, 7)
+#define B43legacy_OFDMTAB_PWRDYN2	B43legacy_OFDMTAB(0x0E, 12)
+#define B43legacy_OFDMTAB_LNAGAIN	B43legacy_OFDMTAB(0x0E, 13)
+
+#define B43legacy_OFDMTAB_LPFGAIN	B43legacy_OFDMTAB(0x0F, 12)
+#define B43legacy_OFDMTAB_RSSI		B43legacy_OFDMTAB(0x10, 0)
+
+#define B43legacy_OFDMTAB_AGC1_R1	B43legacy_OFDMTAB(0x13, 0)
+#define B43legacy_OFDMTAB_GAINX_R1	B43legacy_OFDMTAB(0x14, 0)
+#define B43legacy_OFDMTAB_MINSIGSQ	B43legacy_OFDMTAB(0x14, 1)
+#define B43legacy_OFDMTAB_AGC3_R1	B43legacy_OFDMTAB(0x15, 0)
+#define B43legacy_OFDMTAB_WRSSI_R1	B43legacy_OFDMTAB(0x15, 4)
+#define B43legacy_OFDMTAB_TSSI		B43legacy_OFDMTAB(0x15, 0)
+#define B43legacy_OFDMTAB_DACRFPABB	B43legacy_OFDMTAB(0x16, 0)
+#define B43legacy_OFDMTAB_DACOFF	B43legacy_OFDMTAB(0x17, 0)
+#define B43legacy_OFDMTAB_DCBIAS	B43legacy_OFDMTAB(0x18, 0)
+
+void b43legacy_put_attenuation_into_ranges(int *_bbatt, int *_rfatt);
+
+/* OFDM (A) PHY Registers */
+#define B43legacy_PHY_VERSION_OFDM	B43legacy_PHY_OFDM(0x00)	/* Versioning register for A-PHY */
+#define B43legacy_PHY_BBANDCFG		B43legacy_PHY_OFDM(0x01)	/* Baseband config */
+#define  B43legacy_PHY_BBANDCFG_RXANT	0x180			/* RX Antenna selection */
+#define  B43legacy_PHY_BBANDCFG_RXANT_SHIFT	7
+#define B43legacy_PHY_PWRDOWN		B43legacy_PHY_OFDM(0x03)	/* Powerdown */
+#define B43legacy_PHY_CRSTHRES1		B43legacy_PHY_OFDM(0x06)	/* CRS Threshold 1 */
+#define B43legacy_PHY_LNAHPFCTL		B43legacy_PHY_OFDM(0x1C)	/* LNA/HPF control */
+#define B43legacy_PHY_ADIVRELATED	B43legacy_PHY_OFDM(0x27)	/* FIXME rename */
+#define B43legacy_PHY_CRS0		B43legacy_PHY_OFDM(0x29)
+#define B43legacy_PHY_ANTDWELL		B43legacy_PHY_OFDM(0x2B)	/* Antenna dwell */
+#define  B43legacy_PHY_ANTDWELL_AUTODIV1	0x0100			/* Automatic RX diversity start antenna */
+#define B43legacy_PHY_ENCORE		B43legacy_PHY_OFDM(0x49)	/* "Encore" (RangeMax / BroadRange) */
+#define  B43legacy_PHY_ENCORE_EN	0x0200				/* Encore enable */
+#define B43legacy_PHY_LMS		B43legacy_PHY_OFDM(0x55)
+#define B43legacy_PHY_OFDM61		B43legacy_PHY_OFDM(0x61)	/* FIXME rename */
+#define  B43legacy_PHY_OFDM61_10	0x0010				/* FIXME rename */
+#define B43legacy_PHY_IQBAL		B43legacy_PHY_OFDM(0x69)	/* I/Q balance */
+#define B43legacy_PHY_OTABLECTL		B43legacy_PHY_OFDM(0x72)	/* OFDM table control (see below) */
+#define  B43legacy_PHY_OTABLEOFF	0x03FF				/* OFDM table offset (see below) */
+#define  B43legacy_PHY_OTABLENR		0xFC00				/* OFDM table number (see below) */
+#define  B43legacy_PHY_OTABLENR_SHIFT	10
+#define B43legacy_PHY_OTABLEI		B43legacy_PHY_OFDM(0x73)	/* OFDM table data I */
+#define B43legacy_PHY_OTABLEQ		B43legacy_PHY_OFDM(0x74)	/* OFDM table data Q */
+#define B43legacy_PHY_HPWR_TSSICTL	B43legacy_PHY_OFDM(0x78)	/* Hardware power TSSI control */
+#define B43legacy_PHY_NRSSITHRES	B43legacy_PHY_OFDM(0x8A)	/* NRSSI threshold */
+#define B43legacy_PHY_ANTWRSETT		B43legacy_PHY_OFDM(0x8C)	/* Antenna WR settle */
+#define  B43legacy_PHY_ANTWRSETT_ARXDIV	0x2000				/* Automatic RX diversity enabled */
+#define B43legacy_PHY_CLIPPWRDOWNT	B43legacy_PHY_OFDM(0x93)	/* Clip powerdown threshold */
+#define B43legacy_PHY_OFDM9B		B43legacy_PHY_OFDM(0x9B)	/* FIXME rename */
+#define B43legacy_PHY_N1P1GAIN		B43legacy_PHY_OFDM(0xA0)
+#define B43legacy_PHY_P1P2GAIN		B43legacy_PHY_OFDM(0xA1)
+#define B43legacy_PHY_N1N2GAIN		B43legacy_PHY_OFDM(0xA2)
+#define B43legacy_PHY_CLIPTHRES		B43legacy_PHY_OFDM(0xA3)
+#define B43legacy_PHY_CLIPN1P2THRES	B43legacy_PHY_OFDM(0xA4)
+#define B43legacy_PHY_DIVSRCHIDX	B43legacy_PHY_OFDM(0xA8)	/* Divider search gain/index */
+#define B43legacy_PHY_CLIPP2THRES	B43legacy_PHY_OFDM(0xA9)
+#define B43legacy_PHY_CLIPP3THRES	B43legacy_PHY_OFDM(0xAA)
+#define B43legacy_PHY_DIVP1P2GAIN	B43legacy_PHY_OFDM(0xAB)
+#define B43legacy_PHY_DIVSRCHGAINBACK	B43legacy_PHY_OFDM(0xAD)	/* Divider search gain back */
+#define B43legacy_PHY_DIVSRCHGAINCHNG	B43legacy_PHY_OFDM(0xAE)	/* Divider search gain change */
+#define B43legacy_PHY_CRSTHRES1_R1	B43legacy_PHY_OFDM(0xC0)	/* CRS Threshold 1 (rev 1 only) */
+#define B43legacy_PHY_CRSTHRES2_R1	B43legacy_PHY_OFDM(0xC1)	/* CRS Threshold 2 (rev 1 only) */
+#define B43legacy_PHY_TSSIP_LTBASE	B43legacy_PHY_OFDM(0x380)	/* TSSI power lookup table base */
+#define B43legacy_PHY_DC_LTBASE		B43legacy_PHY_OFDM(0x3A0)	/* DC lookup table base */
+#define B43legacy_PHY_GAIN_LTBASE	B43legacy_PHY_OFDM(0x3C0)	/* Gain lookup table base */
+
+void b43legacy_put_attenuation_into_ranges(int *_bbatt, int *_rfatt);
+
+/* Masks for the different PHY versioning registers. */
+#define B43legacy_PHYVER_ANALOG		0xF000
+#define B43legacy_PHYVER_ANALOG_SHIFT	12
+#define B43legacy_PHYVER_TYPE		0x0F00
+#define B43legacy_PHYVER_TYPE_SHIFT	8
+#define B43legacy_PHYVER_VERSION	0x00FF
+
+struct b43legacy_wldev;
+
+void b43legacy_raw_phy_lock(struct b43legacy_wldev *dev);
+#define b43legacy_phy_lock(bcm, flags) 		\
+	do {					\
+		local_irq_save(flags);		\
+		b43legacy_raw_phy_lock(bcm);	\
+	} while (0)
+void b43legacy_raw_phy_unlock(struct b43legacy_wldev *dev);
+#define b43legacy_phy_unlock(bcm, flags)	\
+	do {					\
+		b43legacy_raw_phy_unlock(bcm);	\
+		local_irq_restore(flags);	\
+	} while (0)
+
+/* Card uses the loopback gain stuff */
+#define has_loopback_gain(phy)			 \
+	(((phy)->rev > 1) || ((phy)->gmode))
+
+u16 b43legacy_phy_read(struct b43legacy_wldev *dev, u16 offset);
+void b43legacy_phy_write(struct b43legacy_wldev *dev, u16 offset, u16 val);
+
+int b43legacy_phy_init_tssi2dbm_table(struct b43legacy_wldev *dev);
+int b43legacy_phy_init(struct b43legacy_wldev *dev);
+
+void b43legacy_set_rx_antenna(struct b43legacy_wldev *dev, int antenna);
+
+void b43legacy_phy_set_antenna_diversity(struct b43legacy_wldev *dev);
+void b43legacy_phy_calibrate(struct b43legacy_wldev *dev);
+int b43legacy_phy_connect(struct b43legacy_wldev *dev, int connect);
+
+void b43legacy_phy_lo_b_measure(struct b43legacy_wldev *dev);
+void b43legacy_phy_lo_g_measure(struct b43legacy_wldev *dev);
+void b43legacy_phy_xmitpower(struct b43legacy_wldev *dev);
+
+/* Adjust the LocalOscillator to the saved values.
+ * "fixed" is only set to 1 once in initialization. Set to 0 otherwise.
+ */
+void b43legacy_phy_lo_adjust(struct b43legacy_wldev *dev, int fixed);
+void b43legacy_phy_lo_mark_all_unused(struct b43legacy_wldev *dev);
+
+void b43legacy_phy_set_baseband_attenuation(struct b43legacy_wldev *dev,
+					    u16 baseband_attenuation);
+
+void b43legacy_power_saving_ctl_bits(struct b43legacy_wldev *dev,
+				     int bit25, int bit26);
+
+#endif /* B43legacy_PHY_H_ */
diff --git a/drivers/net/wireless/b43legacy/pio.c b/drivers/net/wireless/b43legacy/pio.c
new file mode 100644
index 000000000000..de843ac147ae
--- /dev/null
+++ b/drivers/net/wireless/b43legacy/pio.c
@@ -0,0 +1,668 @@
+/*
+
+  Broadcom B43legacy wireless driver
+
+  PIO Transmission
+
+  Copyright (c) 2005 Michael Buesch <mb@bu3sch.de>
+
+  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, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with this program; see the file COPYING.  If not, write to
+  the Free Software Foundation, Inc., 51 Franklin Steet, Fifth Floor,
+  Boston, MA 02110-1301, USA.
+
+*/
+
+#include "b43legacy.h"
+#include "pio.h"
+#include "main.h"
+#include "xmit.h"
+
+#include <linux/delay.h>
+
+
+static void tx_start(struct b43legacy_pioqueue *queue)
+{
+	b43legacy_pio_write(queue, B43legacy_PIO_TXCTL,
+			    B43legacy_PIO_TXCTL_INIT);
+}
+
+static void tx_octet(struct b43legacy_pioqueue *queue,
+		     u8 octet)
+{
+	if (queue->need_workarounds) {
+		b43legacy_pio_write(queue, B43legacy_PIO_TXDATA, octet);
+		b43legacy_pio_write(queue, B43legacy_PIO_TXCTL,
+				    B43legacy_PIO_TXCTL_WRITELO);
+	} else {
+		b43legacy_pio_write(queue, B43legacy_PIO_TXCTL,
+				    B43legacy_PIO_TXCTL_WRITELO);
+		b43legacy_pio_write(queue, B43legacy_PIO_TXDATA, octet);
+	}
+}
+
+static u16 tx_get_next_word(const u8 *txhdr,
+			    const u8 *packet,
+			    size_t txhdr_size,
+			    unsigned int *pos)
+{
+	const u8 *source;
+	unsigned int i = *pos;
+	u16 ret;
+
+	if (i < txhdr_size)
+		source = txhdr;
+	else {
+		source = packet;
+		i -= txhdr_size;
+	}
+	ret = le16_to_cpu(*((__le16 *)(source + i)));
+	*pos += 2;
+
+	return ret;
+}
+
+static void tx_data(struct b43legacy_pioqueue *queue,
+		    u8 *txhdr,
+		    const u8 *packet,
+		    unsigned int octets)
+{
+	u16 data;
+	unsigned int i = 0;
+
+	if (queue->need_workarounds) {
+		data = tx_get_next_word(txhdr, packet,
+					sizeof(struct b43legacy_txhdr_fw3), &i);
+		b43legacy_pio_write(queue, B43legacy_PIO_TXDATA, data);
+	}
+	b43legacy_pio_write(queue, B43legacy_PIO_TXCTL,
+			    B43legacy_PIO_TXCTL_WRITELO |
+			    B43legacy_PIO_TXCTL_WRITEHI);
+	while (i < octets - 1) {
+		data = tx_get_next_word(txhdr, packet,
+					sizeof(struct b43legacy_txhdr_fw3), &i);
+		b43legacy_pio_write(queue, B43legacy_PIO_TXDATA, data);
+	}
+	if (octets % 2)
+		tx_octet(queue, packet[octets -
+			 sizeof(struct b43legacy_txhdr_fw3) - 1]);
+}
+
+static void tx_complete(struct b43legacy_pioqueue *queue,
+			struct sk_buff *skb)
+{
+	if (queue->need_workarounds) {
+		b43legacy_pio_write(queue, B43legacy_PIO_TXDATA,
+				    skb->data[skb->len - 1]);
+		b43legacy_pio_write(queue, B43legacy_PIO_TXCTL,
+				    B43legacy_PIO_TXCTL_WRITELO |
+				    B43legacy_PIO_TXCTL_COMPLETE);
+	} else
+		b43legacy_pio_write(queue, B43legacy_PIO_TXCTL,
+				    B43legacy_PIO_TXCTL_COMPLETE);
+}
+
+static u16 generate_cookie(struct b43legacy_pioqueue *queue,
+			   struct b43legacy_pio_txpacket *packet)
+{
+	u16 cookie = 0x0000;
+	int packetindex;
+
+	/* We use the upper 4 bits for the PIO
+	 * controller ID and the lower 12 bits
+	 * for the packet index (in the cache).
+	 */
+	switch (queue->mmio_base) {
+	case B43legacy_MMIO_PIO1_BASE:
+		break;
+	case B43legacy_MMIO_PIO2_BASE:
+		cookie = 0x1000;
+		break;
+	case B43legacy_MMIO_PIO3_BASE:
+		cookie = 0x2000;
+		break;
+	case B43legacy_MMIO_PIO4_BASE:
+		cookie = 0x3000;
+		break;
+	default:
+		B43legacy_WARN_ON(1);
+	}
+	packetindex = pio_txpacket_getindex(packet);
+	B43legacy_WARN_ON(!(((u16)packetindex & 0xF000) == 0x0000));
+	cookie |= (u16)packetindex;
+
+	return cookie;
+}
+
+static
+struct b43legacy_pioqueue *parse_cookie(struct b43legacy_wldev *dev,
+					u16 cookie,
+					struct b43legacy_pio_txpacket **packet)
+{
+	struct b43legacy_pio *pio = &dev->pio;
+	struct b43legacy_pioqueue *queue = NULL;
+	int packetindex;
+
+	switch (cookie & 0xF000) {
+	case 0x0000:
+		queue = pio->queue0;
+		break;
+	case 0x1000:
+		queue = pio->queue1;
+		break;
+	case 0x2000:
+		queue = pio->queue2;
+		break;
+	case 0x3000:
+		queue = pio->queue3;
+		break;
+	default:
+		B43legacy_WARN_ON(1);
+	}
+	packetindex = (cookie & 0x0FFF);
+	B43legacy_WARN_ON(!(packetindex >= 0 && packetindex
+			  < B43legacy_PIO_MAXTXPACKETS));
+	*packet = &(queue->tx_packets_cache[packetindex]);
+
+	return queue;
+}
+
+union txhdr_union {
+	struct b43legacy_txhdr_fw3 txhdr_fw3;
+};
+
+static void pio_tx_write_fragment(struct b43legacy_pioqueue *queue,
+				  struct sk_buff *skb,
+				  struct b43legacy_pio_txpacket *packet,
+				  size_t txhdr_size)
+{
+	union txhdr_union txhdr_data;
+	u8 *txhdr = NULL;
+	unsigned int octets;
+
+	txhdr = (u8 *)(&txhdr_data.txhdr_fw3);
+
+	B43legacy_WARN_ON(skb_shinfo(skb)->nr_frags != 0);
+	b43legacy_generate_txhdr(queue->dev,
+				 txhdr, skb->data, skb->len,
+				 &packet->txstat.control,
+				 generate_cookie(queue, packet));
+
+	tx_start(queue);
+	octets = skb->len + txhdr_size;
+	if (queue->need_workarounds)
+		octets--;
+	tx_data(queue, txhdr, (u8 *)skb->data, octets);
+	tx_complete(queue, skb);
+}
+
+static void free_txpacket(struct b43legacy_pio_txpacket *packet,
+			  int irq_context)
+{
+	struct b43legacy_pioqueue *queue = packet->queue;
+
+	if (packet->skb) {
+		if (irq_context)
+			dev_kfree_skb_irq(packet->skb);
+		else
+			dev_kfree_skb(packet->skb);
+	}
+	list_move(&packet->list, &queue->txfree);
+	queue->nr_txfree++;
+}
+
+static int pio_tx_packet(struct b43legacy_pio_txpacket *packet)
+{
+	struct b43legacy_pioqueue *queue = packet->queue;
+	struct sk_buff *skb = packet->skb;
+	u16 octets;
+
+	octets = (u16)skb->len + sizeof(struct b43legacy_txhdr_fw3);
+	if (queue->tx_devq_size < octets) {
+		b43legacywarn(queue->dev->wl, "PIO queue too small. "
+			"Dropping packet.\n");
+		/* Drop it silently (return success) */
+		free_txpacket(packet, 1);
+		return 0;
+	}
+	B43legacy_WARN_ON(queue->tx_devq_packets >
+			  B43legacy_PIO_MAXTXDEVQPACKETS);
+	B43legacy_WARN_ON(queue->tx_devq_used > queue->tx_devq_size);
+	/* Check if there is sufficient free space on the device
+	 * TX queue. If not, return and let the TX tasklet
+	 * retry later.
+	 */
+	if (queue->tx_devq_packets == B43legacy_PIO_MAXTXDEVQPACKETS)
+		return -EBUSY;
+	if (queue->tx_devq_used + octets > queue->tx_devq_size)
+		return -EBUSY;
+	/* Now poke the device. */
+	pio_tx_write_fragment(queue, skb, packet,
+			      sizeof(struct b43legacy_txhdr_fw3));
+
+	/* Account for the packet size.
+	 * (We must not overflow the device TX queue)
+	 */
+	queue->tx_devq_packets++;
+	queue->tx_devq_used += octets;
+
+	/* Transmission started, everything ok, move the
+	 * packet to the txrunning list.
+	 */
+	list_move_tail(&packet->list, &queue->txrunning);
+
+	return 0;
+}
+
+static void tx_tasklet(unsigned long d)
+{
+	struct b43legacy_pioqueue *queue = (struct b43legacy_pioqueue *)d;
+	struct b43legacy_wldev *dev = queue->dev;
+	unsigned long flags;
+	struct b43legacy_pio_txpacket *packet, *tmp_packet;
+	int err;
+	u16 txctl;
+
+	spin_lock_irqsave(&dev->wl->irq_lock, flags);
+	if (queue->tx_frozen)
+		goto out_unlock;
+	txctl = b43legacy_pio_read(queue, B43legacy_PIO_TXCTL);
+	if (txctl & B43legacy_PIO_TXCTL_SUSPEND)
+		goto out_unlock;
+
+	list_for_each_entry_safe(packet, tmp_packet, &queue->txqueue, list) {
+		/* Try to transmit the packet. This can fail, if
+		 * the device queue is full. In case of failure, the
+		 * packet is left in the txqueue.
+		 * If transmission succeed, the packet is moved to txrunning.
+		 * If it is impossible to transmit the packet, it
+		 * is dropped.
+		 */
+		err = pio_tx_packet(packet);
+		if (err)
+			break;
+	}
+out_unlock:
+	spin_unlock_irqrestore(&dev->wl->irq_lock, flags);
+}
+
+static void setup_txqueues(struct b43legacy_pioqueue *queue)
+{
+	struct b43legacy_pio_txpacket *packet;
+	int i;
+
+	queue->nr_txfree = B43legacy_PIO_MAXTXPACKETS;
+	for (i = 0; i < B43legacy_PIO_MAXTXPACKETS; i++) {
+		packet = &(queue->tx_packets_cache[i]);
+
+		packet->queue = queue;
+		INIT_LIST_HEAD(&packet->list);
+
+		list_add(&packet->list, &queue->txfree);
+	}
+}
+
+static
+struct b43legacy_pioqueue *b43legacy_setup_pioqueue(struct b43legacy_wldev *dev,
+						    u16 pio_mmio_base)
+{
+	struct b43legacy_pioqueue *queue;
+	u32 value;
+	u16 qsize;
+
+	queue = kzalloc(sizeof(*queue), GFP_KERNEL);
+	if (!queue)
+		goto out;
+
+	queue->dev = dev;
+	queue->mmio_base = pio_mmio_base;
+	queue->need_workarounds = (dev->dev->id.revision < 3);
+
+	INIT_LIST_HEAD(&queue->txfree);
+	INIT_LIST_HEAD(&queue->txqueue);
+	INIT_LIST_HEAD(&queue->txrunning);
+	tasklet_init(&queue->txtask, tx_tasklet,
+		     (unsigned long)queue);
+
+	value = b43legacy_read32(dev, B43legacy_MMIO_STATUS_BITFIELD);
+	value &= ~B43legacy_SBF_XFER_REG_BYTESWAP;
+	b43legacy_write32(dev, B43legacy_MMIO_STATUS_BITFIELD, value);
+
+	qsize = b43legacy_read16(dev, queue->mmio_base
+				 + B43legacy_PIO_TXQBUFSIZE);
+	if (qsize == 0) {
+		b43legacyerr(dev->wl, "This card does not support PIO "
+		       "operation mode. Please use DMA mode "
+		       "(module parameter pio=0).\n");
+		goto err_freequeue;
+	}
+	if (qsize <= B43legacy_PIO_TXQADJUST) {
+		b43legacyerr(dev->wl, "PIO tx device-queue too small (%u)\n",
+		       qsize);
+		goto err_freequeue;
+	}
+	qsize -= B43legacy_PIO_TXQADJUST;
+	queue->tx_devq_size = qsize;
+
+	setup_txqueues(queue);
+
+out:
+	return queue;
+
+err_freequeue:
+	kfree(queue);
+	queue = NULL;
+	goto out;
+}
+
+static void cancel_transfers(struct b43legacy_pioqueue *queue)
+{
+	struct b43legacy_pio_txpacket *packet, *tmp_packet;
+
+	tasklet_disable(&queue->txtask);
+
+	list_for_each_entry_safe(packet, tmp_packet, &queue->txrunning, list)
+		free_txpacket(packet, 0);
+	list_for_each_entry_safe(packet, tmp_packet, &queue->txqueue, list)
+		free_txpacket(packet, 0);
+}
+
+static void b43legacy_destroy_pioqueue(struct b43legacy_pioqueue *queue)
+{
+	if (!queue)
+		return;
+
+	cancel_transfers(queue);
+	kfree(queue);
+}
+
+void b43legacy_pio_free(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_pio *pio;
+
+	if (!b43legacy_using_pio(dev))
+		return;
+	pio = &dev->pio;
+
+	b43legacy_destroy_pioqueue(pio->queue3);
+	pio->queue3 = NULL;
+	b43legacy_destroy_pioqueue(pio->queue2);
+	pio->queue2 = NULL;
+	b43legacy_destroy_pioqueue(pio->queue1);
+	pio->queue1 = NULL;
+	b43legacy_destroy_pioqueue(pio->queue0);
+	pio->queue0 = NULL;
+}
+
+int b43legacy_pio_init(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_pio *pio = &dev->pio;
+	struct b43legacy_pioqueue *queue;
+	int err = -ENOMEM;
+
+	queue = b43legacy_setup_pioqueue(dev, B43legacy_MMIO_PIO1_BASE);
+	if (!queue)
+		goto out;
+	pio->queue0 = queue;
+
+	queue = b43legacy_setup_pioqueue(dev, B43legacy_MMIO_PIO2_BASE);
+	if (!queue)
+		goto err_destroy0;
+	pio->queue1 = queue;
+
+	queue = b43legacy_setup_pioqueue(dev, B43legacy_MMIO_PIO3_BASE);
+	if (!queue)
+		goto err_destroy1;
+	pio->queue2 = queue;
+
+	queue = b43legacy_setup_pioqueue(dev, B43legacy_MMIO_PIO4_BASE);
+	if (!queue)
+		goto err_destroy2;
+	pio->queue3 = queue;
+
+	if (dev->dev->id.revision < 3)
+		dev->irq_savedstate |= B43legacy_IRQ_PIO_WORKAROUND;
+
+	b43legacydbg(dev->wl, "PIO initialized\n");
+	err = 0;
+out:
+	return err;
+
+err_destroy2:
+	b43legacy_destroy_pioqueue(pio->queue2);
+	pio->queue2 = NULL;
+err_destroy1:
+	b43legacy_destroy_pioqueue(pio->queue1);
+	pio->queue1 = NULL;
+err_destroy0:
+	b43legacy_destroy_pioqueue(pio->queue0);
+	pio->queue0 = NULL;
+	goto out;
+}
+
+int b43legacy_pio_tx(struct b43legacy_wldev *dev,
+		     struct sk_buff *skb,
+		     struct ieee80211_tx_control *ctl)
+{
+	struct b43legacy_pioqueue *queue = dev->pio.queue1;
+	struct b43legacy_pio_txpacket *packet;
+
+	B43legacy_WARN_ON(queue->tx_suspended);
+	B43legacy_WARN_ON(list_empty(&queue->txfree));
+
+	packet = list_entry(queue->txfree.next, struct b43legacy_pio_txpacket,
+			    list);
+	packet->skb = skb;
+
+	memset(&packet->txstat, 0, sizeof(packet->txstat));
+	memcpy(&packet->txstat.control, ctl, sizeof(*ctl));
+
+	list_move_tail(&packet->list, &queue->txqueue);
+	queue->nr_txfree--;
+	queue->nr_tx_packets++;
+	B43legacy_WARN_ON(queue->nr_txfree >= B43legacy_PIO_MAXTXPACKETS);
+
+	tasklet_schedule(&queue->txtask);
+
+	return 0;
+}
+
+void b43legacy_pio_handle_txstatus(struct b43legacy_wldev *dev,
+				   const struct b43legacy_txstatus *status)
+{
+	struct b43legacy_pioqueue *queue;
+	struct b43legacy_pio_txpacket *packet;
+
+	queue = parse_cookie(dev, status->cookie, &packet);
+	B43legacy_WARN_ON(!queue);
+
+	queue->tx_devq_packets--;
+	queue->tx_devq_used -= (packet->skb->len +
+				sizeof(struct b43legacy_txhdr_fw3));
+
+	if (status->acked)
+		packet->txstat.flags |= IEEE80211_TX_STATUS_ACK;
+	packet->txstat.retry_count = status->frame_count - 1;
+	ieee80211_tx_status_irqsafe(dev->wl->hw, packet->skb,
+				    &(packet->txstat));
+	packet->skb = NULL;
+
+	free_txpacket(packet, 1);
+	/* If there are packets on the txqueue, poke the tasklet
+	 * to transmit them.
+	 */
+	if (!list_empty(&queue->txqueue))
+		tasklet_schedule(&queue->txtask);
+}
+
+void b43legacy_pio_get_tx_stats(struct b43legacy_wldev *dev,
+				struct ieee80211_tx_queue_stats *stats)
+{
+	struct b43legacy_pio *pio = &dev->pio;
+	struct b43legacy_pioqueue *queue;
+	struct ieee80211_tx_queue_stats_data *data;
+
+	queue = pio->queue1;
+	data = &(stats->data[0]);
+	data->len = B43legacy_PIO_MAXTXPACKETS - queue->nr_txfree;
+	data->limit = B43legacy_PIO_MAXTXPACKETS;
+	data->count = queue->nr_tx_packets;
+}
+
+static void pio_rx_error(struct b43legacy_pioqueue *queue,
+			 int clear_buffers,
+			 const char *error)
+{
+	int i;
+
+	b43legacyerr(queue->dev->wl, "PIO RX error: %s\n", error);
+	b43legacy_pio_write(queue, B43legacy_PIO_RXCTL,
+			    B43legacy_PIO_RXCTL_READY);
+	if (clear_buffers) {
+		B43legacy_WARN_ON(queue->mmio_base != B43legacy_MMIO_PIO1_BASE);
+		for (i = 0; i < 15; i++) {
+			/* Dummy read. */
+			b43legacy_pio_read(queue, B43legacy_PIO_RXDATA);
+		}
+	}
+}
+
+void b43legacy_pio_rx(struct b43legacy_pioqueue *queue)
+{
+	__le16 preamble[21] = { 0 };
+	struct b43legacy_rxhdr_fw3 *rxhdr;
+	u16 tmp;
+	u16 len;
+	u16 macstat;
+	int i;
+	int preamble_readwords;
+	struct sk_buff *skb;
+
+	tmp = b43legacy_pio_read(queue, B43legacy_PIO_RXCTL);
+	if (!(tmp & B43legacy_PIO_RXCTL_DATAAVAILABLE))
+		return;
+	b43legacy_pio_write(queue, B43legacy_PIO_RXCTL,
+			    B43legacy_PIO_RXCTL_DATAAVAILABLE);
+
+	for (i = 0; i < 10; i++) {
+		tmp = b43legacy_pio_read(queue, B43legacy_PIO_RXCTL);
+		if (tmp & B43legacy_PIO_RXCTL_READY)
+			goto data_ready;
+		udelay(10);
+	}
+	b43legacydbg(queue->dev->wl, "PIO RX timed out\n");
+	return;
+data_ready:
+
+	len = b43legacy_pio_read(queue, B43legacy_PIO_RXDATA);
+	if (unlikely(len > 0x700)) {
+		pio_rx_error(queue, 0, "len > 0x700");
+		return;
+	}
+	if (unlikely(len == 0 && queue->mmio_base !=
+		     B43legacy_MMIO_PIO4_BASE)) {
+		pio_rx_error(queue, 0, "len == 0");
+		return;
+	}
+	preamble[0] = cpu_to_le16(len);
+	if (queue->mmio_base == B43legacy_MMIO_PIO4_BASE)
+		preamble_readwords = 14 / sizeof(u16);
+	else
+		preamble_readwords = 18 / sizeof(u16);
+	for (i = 0; i < preamble_readwords; i++) {
+		tmp = b43legacy_pio_read(queue, B43legacy_PIO_RXDATA);
+		preamble[i + 1] = cpu_to_le16(tmp);
+	}
+	rxhdr = (struct b43legacy_rxhdr_fw3 *)preamble;
+	macstat = le16_to_cpu(rxhdr->mac_status);
+	if (macstat & B43legacy_RX_MAC_FCSERR) {
+		pio_rx_error(queue,
+			     (queue->mmio_base == B43legacy_MMIO_PIO1_BASE),
+			     "Frame FCS error");
+		return;
+	}
+	if (queue->mmio_base == B43legacy_MMIO_PIO4_BASE) {
+		/* We received an xmit status. */
+		struct b43legacy_hwtxstatus *hw;
+
+		hw = (struct b43legacy_hwtxstatus *)(preamble + 1);
+		b43legacy_handle_hwtxstatus(queue->dev, hw);
+
+		return;
+	}
+
+	skb = dev_alloc_skb(len);
+	if (unlikely(!skb)) {
+		pio_rx_error(queue, 1, "OOM");
+		return;
+	}
+	skb_put(skb, len);
+	for (i = 0; i < len - 1; i += 2) {
+		tmp = b43legacy_pio_read(queue, B43legacy_PIO_RXDATA);
+		*((__le16 *)(skb->data + i)) = cpu_to_le16(tmp);
+	}
+	if (len % 2) {
+		tmp = b43legacy_pio_read(queue, B43legacy_PIO_RXDATA);
+		skb->data[len - 1] = (tmp & 0x00FF);
+	}
+	b43legacy_rx(queue->dev, skb, rxhdr);
+}
+
+void b43legacy_pio_tx_suspend(struct b43legacy_pioqueue *queue)
+{
+	b43legacy_power_saving_ctl_bits(queue->dev, -1, 1);
+	b43legacy_pio_write(queue, B43legacy_PIO_TXCTL,
+			    b43legacy_pio_read(queue, B43legacy_PIO_TXCTL)
+			    | B43legacy_PIO_TXCTL_SUSPEND);
+}
+
+void b43legacy_pio_tx_resume(struct b43legacy_pioqueue *queue)
+{
+	b43legacy_pio_write(queue, B43legacy_PIO_TXCTL,
+			    b43legacy_pio_read(queue, B43legacy_PIO_TXCTL)
+			    & ~B43legacy_PIO_TXCTL_SUSPEND);
+	b43legacy_power_saving_ctl_bits(queue->dev, -1, -1);
+	tasklet_schedule(&queue->txtask);
+}
+
+void b43legacy_pio_freeze_txqueues(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_pio *pio;
+
+	B43legacy_WARN_ON(!b43legacy_using_pio(dev));
+	pio = &dev->pio;
+	pio->queue0->tx_frozen = 1;
+	pio->queue1->tx_frozen = 1;
+	pio->queue2->tx_frozen = 1;
+	pio->queue3->tx_frozen = 1;
+}
+
+void b43legacy_pio_thaw_txqueues(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_pio *pio;
+
+	B43legacy_WARN_ON(!b43legacy_using_pio(dev));
+	pio = &dev->pio;
+	pio->queue0->tx_frozen = 0;
+	pio->queue1->tx_frozen = 0;
+	pio->queue2->tx_frozen = 0;
+	pio->queue3->tx_frozen = 0;
+	if (!list_empty(&pio->queue0->txqueue))
+		tasklet_schedule(&pio->queue0->txtask);
+	if (!list_empty(&pio->queue1->txqueue))
+		tasklet_schedule(&pio->queue1->txtask);
+	if (!list_empty(&pio->queue2->txqueue))
+		tasklet_schedule(&pio->queue2->txtask);
+	if (!list_empty(&pio->queue3->txqueue))
+		tasklet_schedule(&pio->queue3->txtask);
+}
diff --git a/drivers/net/wireless/b43legacy/pio.h b/drivers/net/wireless/b43legacy/pio.h
new file mode 100644
index 000000000000..5bfed0c40030
--- /dev/null
+++ b/drivers/net/wireless/b43legacy/pio.h
@@ -0,0 +1,172 @@
+#ifndef B43legacy_PIO_H_
+#define B43legacy_PIO_H_
+
+#include "b43legacy.h"
+
+#include <linux/interrupt.h>
+#include <linux/list.h>
+#include <linux/skbuff.h>
+
+
+#define B43legacy_PIO_TXCTL		0x00
+#define B43legacy_PIO_TXDATA		0x02
+#define B43legacy_PIO_TXQBUFSIZE	0x04
+#define B43legacy_PIO_RXCTL		0x08
+#define B43legacy_PIO_RXDATA		0x0A
+
+#define B43legacy_PIO_TXCTL_WRITELO	(1 << 0)
+#define B43legacy_PIO_TXCTL_WRITEHI	(1 << 1)
+#define B43legacy_PIO_TXCTL_COMPLETE	(1 << 2)
+#define B43legacy_PIO_TXCTL_INIT	(1 << 3)
+#define B43legacy_PIO_TXCTL_SUSPEND	(1 << 7)
+
+#define B43legacy_PIO_RXCTL_DATAAVAILABLE	(1 << 0)
+#define B43legacy_PIO_RXCTL_READY		(1 << 1)
+
+/* PIO constants */
+#define B43legacy_PIO_MAXTXDEVQPACKETS	31
+#define B43legacy_PIO_TXQADJUST		80
+
+/* PIO tuning knobs */
+#define B43legacy_PIO_MAXTXPACKETS	256
+
+
+
+#ifdef CONFIG_B43LEGACY_PIO
+
+
+struct b43legacy_pioqueue;
+struct b43legacy_xmitstatus;
+
+struct b43legacy_pio_txpacket {
+	struct b43legacy_pioqueue *queue;
+	struct sk_buff *skb;
+	struct ieee80211_tx_status txstat;
+	struct list_head list;
+};
+
+#define pio_txpacket_getindex(packet) ((int)((packet) - \
+			      (packet)->queue->tx_packets_cache))
+
+struct b43legacy_pioqueue {
+	struct b43legacy_wldev *dev;
+	u16 mmio_base;
+
+	bool tx_suspended;
+	bool tx_frozen;
+	bool need_workarounds; /* Workarounds needed for core.rev < 3 */
+
+	/* Adjusted size of the device internal TX buffer. */
+	u16 tx_devq_size;
+	/* Used octets of the device internal TX buffer. */
+	u16 tx_devq_used;
+	/* Used packet slots in the device internal TX buffer. */
+	u8 tx_devq_packets;
+	/* Packets from the txfree list can
+	 * be taken on incoming TX requests.
+	 */
+	struct list_head txfree;
+	unsigned int nr_txfree;
+	/* Packets on the txqueue are queued,
+	 * but not completely written to the chip, yet.
+	 */
+	struct list_head txqueue;
+	/* Packets on the txrunning queue are completely
+	 * posted to the device. We are waiting for the txstatus.
+	 */
+	struct list_head txrunning;
+	/* Total number or packets sent.
+	 * (This counter can obviously wrap).
+	 */
+	unsigned int nr_tx_packets;
+	struct tasklet_struct txtask;
+	struct b43legacy_pio_txpacket
+			 tx_packets_cache[B43legacy_PIO_MAXTXPACKETS];
+};
+
+static inline
+u16 b43legacy_pio_read(struct b43legacy_pioqueue *queue,
+		     u16 offset)
+{
+	return b43legacy_read16(queue->dev, queue->mmio_base + offset);
+}
+
+static inline
+void b43legacy_pio_write(struct b43legacy_pioqueue *queue,
+		       u16 offset, u16 value)
+{
+	b43legacy_write16(queue->dev, queue->mmio_base + offset, value);
+	mmiowb();
+}
+
+
+int b43legacy_pio_init(struct b43legacy_wldev *dev);
+void b43legacy_pio_free(struct b43legacy_wldev *dev);
+
+int b43legacy_pio_tx(struct b43legacy_wldev *dev,
+		   struct sk_buff *skb,
+		   struct ieee80211_tx_control *ctl);
+void b43legacy_pio_handle_txstatus(struct b43legacy_wldev *dev,
+				 const struct b43legacy_txstatus *status);
+void b43legacy_pio_get_tx_stats(struct b43legacy_wldev *dev,
+			      struct ieee80211_tx_queue_stats *stats);
+void b43legacy_pio_rx(struct b43legacy_pioqueue *queue);
+
+/* Suspend TX queue in hardware. */
+void b43legacy_pio_tx_suspend(struct b43legacy_pioqueue *queue);
+void b43legacy_pio_tx_resume(struct b43legacy_pioqueue *queue);
+/* Suspend (freeze) the TX tasklet (software level). */
+void b43legacy_pio_freeze_txqueues(struct b43legacy_wldev *dev);
+void b43legacy_pio_thaw_txqueues(struct b43legacy_wldev *dev);
+
+#else /* CONFIG_B43LEGACY_PIO */
+
+static inline
+int b43legacy_pio_init(struct b43legacy_wldev *dev)
+{
+	return 0;
+}
+static inline
+void b43legacy_pio_free(struct b43legacy_wldev *dev)
+{
+}
+static inline
+int b43legacy_pio_tx(struct b43legacy_wldev *dev,
+		   struct sk_buff *skb,
+		   struct ieee80211_tx_control *ctl)
+{
+	return 0;
+}
+static inline
+void b43legacy_pio_handle_txstatus(struct b43legacy_wldev *dev,
+				 const struct b43legacy_txstatus *status)
+{
+}
+static inline
+void b43legacy_pio_get_tx_stats(struct b43legacy_wldev *dev,
+			      struct ieee80211_tx_queue_stats *stats)
+{
+}
+static inline
+void b43legacy_pio_rx(struct b43legacy_pioqueue *queue)
+{
+}
+static inline
+void b43legacy_pio_tx_suspend(struct b43legacy_pioqueue *queue)
+{
+}
+static inline
+void b43legacy_pio_tx_resume(struct b43legacy_pioqueue *queue)
+{
+}
+static inline
+void b43legacy_pio_freeze_txqueues(struct b43legacy_wldev *dev)
+{
+}
+static inline
+void b43legacy_pio_thaw_txqueues(struct b43legacy_wldev *dev)
+{
+}
+
+#endif /* CONFIG_B43LEGACY_PIO */
+#endif /* B43legacy_PIO_H_ */
diff --git a/drivers/net/wireless/b43legacy/radio.c b/drivers/net/wireless/b43legacy/radio.c
new file mode 100644
index 000000000000..a361dee664af
--- /dev/null
+++ b/drivers/net/wireless/b43legacy/radio.c
@@ -0,0 +1,2158 @@
+/*
+
+  Broadcom B43legacy wireless driver
+
+  Copyright (c) 2005 Martin Langer <martin-langer@gmx.de>,
+		     Stefano Brivio <st3@riseup.net>
+		     Michael Buesch <mbuesch@freenet.de>
+		     Danny van Dyk <kugelfang@gentoo.org>
+		     Andreas Jaggi <andreas.jaggi@waterwave.ch>
+  Copyright (c) 2007 Larry Finger <Larry.Finger@lwfinger.net>
+
+  Some parts of the code in this file are derived from the ipw2200
+  driver  Copyright(c) 2003 - 2004 Intel Corporation.
+
+  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, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with this program; see the file COPYING.  If not, write to
+  the Free Software Foundation, Inc., 51 Franklin Steet, Fifth Floor,
+  Boston, MA 02110-1301, USA.
+
+*/
+
+#include <linux/delay.h>
+
+#include "b43legacy.h"
+#include "main.h"
+#include "phy.h"
+#include "radio.h"
+#include "ilt.h"
+
+
+/* Table for b43legacy_radio_calibrationvalue() */
+static const u16 rcc_table[16] = {
+	0x0002, 0x0003, 0x0001, 0x000F,
+	0x0006, 0x0007, 0x0005, 0x000F,
+	0x000A, 0x000B, 0x0009, 0x000F,
+	0x000E, 0x000F, 0x000D, 0x000F,
+};
+
+/* Reverse the bits of a 4bit value.
+ * Example:  1101 is flipped 1011
+ */
+static u16 flip_4bit(u16 value)
+{
+	u16 flipped = 0x0000;
+
+	B43legacy_BUG_ON(!((value & ~0x000F) == 0x0000));
+
+	flipped |= (value & 0x0001) << 3;
+	flipped |= (value & 0x0002) << 1;
+	flipped |= (value & 0x0004) >> 1;
+	flipped |= (value & 0x0008) >> 3;
+
+	return flipped;
+}
+
+/* Get the freq, as it has to be written to the device. */
+static inline
+u16 channel2freq_bg(u8 channel)
+{
+	/* Frequencies are given as frequencies_bg[index] + 2.4GHz
+	 * Starting with channel 1
+	 */
+	static const u16 frequencies_bg[14] = {
+		12, 17, 22, 27,
+		32, 37, 42, 47,
+		52, 57, 62, 67,
+		72, 84,
+	};
+
+	if (unlikely(channel < 1 || channel > 14)) {
+		printk(KERN_INFO "b43legacy: Channel %d is out of range\n",
+				  channel);
+		dump_stack();
+		return 2412;
+	}
+
+	return frequencies_bg[channel - 1];
+}
+
+void b43legacy_radio_lock(struct b43legacy_wldev *dev)
+{
+	u32 status;
+
+	status = b43legacy_read32(dev, B43legacy_MMIO_STATUS_BITFIELD);
+	status |= B43legacy_SBF_RADIOREG_LOCK;
+	b43legacy_write32(dev, B43legacy_MMIO_STATUS_BITFIELD, status);
+	mmiowb();
+	udelay(10);
+}
+
+void b43legacy_radio_unlock(struct b43legacy_wldev *dev)
+{
+	u32 status;
+
+	b43legacy_read16(dev, B43legacy_MMIO_PHY_VER); /* dummy read */
+	status = b43legacy_read32(dev, B43legacy_MMIO_STATUS_BITFIELD);
+	status &= ~B43legacy_SBF_RADIOREG_LOCK;
+	b43legacy_write32(dev, B43legacy_MMIO_STATUS_BITFIELD, status);
+	mmiowb();
+}
+
+u16 b43legacy_radio_read16(struct b43legacy_wldev *dev, u16 offset)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+
+	switch (phy->type) {
+	case B43legacy_PHYTYPE_B:
+		if (phy->radio_ver == 0x2053) {
+			if (offset < 0x70)
+				offset += 0x80;
+			else if (offset < 0x80)
+				offset += 0x70;
+		} else if (phy->radio_ver == 0x2050)
+			offset |= 0x80;
+		else
+			B43legacy_WARN_ON(1);
+		break;
+	case B43legacy_PHYTYPE_G:
+		offset |= 0x80;
+		break;
+	default:
+		B43legacy_BUG_ON(1);
+	}
+
+	b43legacy_write16(dev, B43legacy_MMIO_RADIO_CONTROL, offset);
+	return b43legacy_read16(dev, B43legacy_MMIO_RADIO_DATA_LOW);
+}
+
+void b43legacy_radio_write16(struct b43legacy_wldev *dev, u16 offset, u16 val)
+{
+	b43legacy_write16(dev, B43legacy_MMIO_RADIO_CONTROL, offset);
+	mmiowb();
+	b43legacy_write16(dev, B43legacy_MMIO_RADIO_DATA_LOW, val);
+}
+
+static void b43legacy_set_all_gains(struct b43legacy_wldev *dev,
+				  s16 first, s16 second, s16 third)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+	u16 i;
+	u16 start = 0x08;
+	u16 end = 0x18;
+	u16 offset = 0x0400;
+	u16 tmp;
+
+	if (phy->rev <= 1) {
+		offset = 0x5000;
+		start = 0x10;
+		end = 0x20;
+	}
+
+	for (i = 0; i < 4; i++)
+		b43legacy_ilt_write(dev, offset + i, first);
+
+	for (i = start; i < end; i++)
+		b43legacy_ilt_write(dev, offset + i, second);
+
+	if (third != -1) {
+		tmp = ((u16)third << 14) | ((u16)third << 6);
+		b43legacy_phy_write(dev, 0x04A0,
+				    (b43legacy_phy_read(dev, 0x04A0) & 0xBFBF)
+				    | tmp);
+		b43legacy_phy_write(dev, 0x04A1,
+				    (b43legacy_phy_read(dev, 0x04A1) & 0xBFBF)
+				    | tmp);
+		b43legacy_phy_write(dev, 0x04A2,
+				    (b43legacy_phy_read(dev, 0x04A2) & 0xBFBF)
+				    | tmp);
+	}
+	b43legacy_dummy_transmission(dev);
+}
+
+static void b43legacy_set_original_gains(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+	u16 i;
+	u16 tmp;
+	u16 offset = 0x0400;
+	u16 start = 0x0008;
+	u16 end = 0x0018;
+
+	if (phy->rev <= 1) {
+		offset = 0x5000;
+		start = 0x0010;
+		end = 0x0020;
+	}
+
+	for (i = 0; i < 4; i++) {
+		tmp = (i & 0xFFFC);
+		tmp |= (i & 0x0001) << 1;
+		tmp |= (i & 0x0002) >> 1;
+
+		b43legacy_ilt_write(dev, offset + i, tmp);
+	}
+
+	for (i = start; i < end; i++)
+		b43legacy_ilt_write(dev, offset + i, i - start);
+
+	b43legacy_phy_write(dev, 0x04A0,
+			    (b43legacy_phy_read(dev, 0x04A0) & 0xBFBF)
+			    | 0x4040);
+	b43legacy_phy_write(dev, 0x04A1,
+			    (b43legacy_phy_read(dev, 0x04A1) & 0xBFBF)
+			    | 0x4040);
+	b43legacy_phy_write(dev, 0x04A2,
+			    (b43legacy_phy_read(dev, 0x04A2) & 0xBFBF)
+			    | 0x4000);
+	b43legacy_dummy_transmission(dev);
+}
+
+/* Synthetic PU workaround */
+static void b43legacy_synth_pu_workaround(struct b43legacy_wldev *dev,
+					  u8 channel)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+
+	might_sleep();
+
+	if (phy->radio_ver != 0x2050 || phy->radio_rev >= 6)
+		/* We do not need the workaround. */
+		return;
+
+	if (channel <= 10)
+		b43legacy_write16(dev, B43legacy_MMIO_CHANNEL,
+				  channel2freq_bg(channel + 4));
+	else
+		b43legacy_write16(dev, B43legacy_MMIO_CHANNEL,
+				  channel2freq_bg(channel));
+	msleep(1);
+	b43legacy_write16(dev, B43legacy_MMIO_CHANNEL,
+			  channel2freq_bg(channel));
+}
+
+u8 b43legacy_radio_aci_detect(struct b43legacy_wldev *dev, u8 channel)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+	u8 ret = 0;
+	u16 saved;
+	u16 rssi;
+	u16 temp;
+	int i;
+	int j = 0;
+
+	saved = b43legacy_phy_read(dev, 0x0403);
+	b43legacy_radio_selectchannel(dev, channel, 0);
+	b43legacy_phy_write(dev, 0x0403, (saved & 0xFFF8) | 5);
+	if (phy->aci_hw_rssi)
+		rssi = b43legacy_phy_read(dev, 0x048A) & 0x3F;
+	else
+		rssi = saved & 0x3F;
+	/* clamp temp to signed 5bit */
+	if (rssi > 32)
+		rssi -= 64;
+	for (i = 0; i < 100; i++) {
+		temp = (b43legacy_phy_read(dev, 0x047F) >> 8) & 0x3F;
+		if (temp > 32)
+			temp -= 64;
+		if (temp < rssi)
+			j++;
+		if (j >= 20)
+			ret = 1;
+	}
+	b43legacy_phy_write(dev, 0x0403, saved);
+
+	return ret;
+}
+
+u8 b43legacy_radio_aci_scan(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+	u8 ret[13];
+	unsigned int channel = phy->channel;
+	unsigned int i;
+	unsigned int j;
+	unsigned int start;
+	unsigned int end;
+	unsigned long phylock_flags;
+
+	if (!((phy->type == B43legacy_PHYTYPE_G) && (phy->rev > 0)))
+		return 0;
+
+	b43legacy_phy_lock(dev, phylock_flags);
+	b43legacy_radio_lock(dev);
+	b43legacy_phy_write(dev, 0x0802,
+			    b43legacy_phy_read(dev, 0x0802) & 0xFFFC);
+	b43legacy_phy_write(dev, B43legacy_PHY_G_CRS,
+			    b43legacy_phy_read(dev, B43legacy_PHY_G_CRS)
+			    & 0x7FFF);
+	b43legacy_set_all_gains(dev, 3, 8, 1);
+
+	start = (channel - 5 > 0) ? channel - 5 : 1;
+	end = (channel + 5 < 14) ? channel + 5 : 13;
+
+	for (i = start; i <= end; i++) {
+		if (abs(channel - i) > 2)
+			ret[i-1] = b43legacy_radio_aci_detect(dev, i);
+	}
+	b43legacy_radio_selectchannel(dev, channel, 0);
+	b43legacy_phy_write(dev, 0x0802,
+			    (b43legacy_phy_read(dev, 0x0802) & 0xFFFC)
+			    | 0x0003);
+	b43legacy_phy_write(dev, 0x0403,
+			    b43legacy_phy_read(dev, 0x0403) & 0xFFF8);
+	b43legacy_phy_write(dev, B43legacy_PHY_G_CRS,
+			    b43legacy_phy_read(dev, B43legacy_PHY_G_CRS)
+			    | 0x8000);
+	b43legacy_set_original_gains(dev);
+	for (i = 0; i < 13; i++) {
+		if (!ret[i])
+			continue;
+		end = (i + 5 < 13) ? i + 5 : 13;
+		for (j = i; j < end; j++)
+			ret[j] = 1;
+	}
+	b43legacy_radio_unlock(dev);
+	b43legacy_phy_unlock(dev, phylock_flags);
+
+	return ret[channel - 1];
+}
+
+/* http://bcm-specs.sipsolutions.net/NRSSILookupTable */
+void b43legacy_nrssi_hw_write(struct b43legacy_wldev *dev, u16 offset, s16 val)
+{
+	b43legacy_phy_write(dev, B43legacy_PHY_NRSSILT_CTRL, offset);
+	mmiowb();
+	b43legacy_phy_write(dev, B43legacy_PHY_NRSSILT_DATA, (u16)val);
+}
+
+/* http://bcm-specs.sipsolutions.net/NRSSILookupTable */
+s16 b43legacy_nrssi_hw_read(struct b43legacy_wldev *dev, u16 offset)
+{
+	u16 val;
+
+	b43legacy_phy_write(dev, B43legacy_PHY_NRSSILT_CTRL, offset);
+	val = b43legacy_phy_read(dev, B43legacy_PHY_NRSSILT_DATA);
+
+	return (s16)val;
+}
+
+/* http://bcm-specs.sipsolutions.net/NRSSILookupTable */
+void b43legacy_nrssi_hw_update(struct b43legacy_wldev *dev, u16 val)
+{
+	u16 i;
+	s16 tmp;
+
+	for (i = 0; i < 64; i++) {
+		tmp = b43legacy_nrssi_hw_read(dev, i);
+		tmp -= val;
+		tmp = limit_value(tmp, -32, 31);
+		b43legacy_nrssi_hw_write(dev, i, tmp);
+	}
+}
+
+/* http://bcm-specs.sipsolutions.net/NRSSILookupTable */
+void b43legacy_nrssi_mem_update(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+	s16 i;
+	s16 delta;
+	s32 tmp;
+
+	delta = 0x1F - phy->nrssi[0];
+	for (i = 0; i < 64; i++) {
+		tmp = (i - delta) * phy->nrssislope;
+		tmp /= 0x10000;
+		tmp += 0x3A;
+		tmp = limit_value(tmp, 0, 0x3F);
+		phy->nrssi_lt[i] = tmp;
+	}
+}
+
+static void b43legacy_calc_nrssi_offset(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+	u16 backup[20] = { 0 };
+	s16 v47F;
+	u16 i;
+	u16 saved = 0xFFFF;
+
+	backup[0] = b43legacy_phy_read(dev, 0x0001);
+	backup[1] = b43legacy_phy_read(dev, 0x0811);
+	backup[2] = b43legacy_phy_read(dev, 0x0812);
+	backup[3] = b43legacy_phy_read(dev, 0x0814);
+	backup[4] = b43legacy_phy_read(dev, 0x0815);
+	backup[5] = b43legacy_phy_read(dev, 0x005A);
+	backup[6] = b43legacy_phy_read(dev, 0x0059);
+	backup[7] = b43legacy_phy_read(dev, 0x0058);
+	backup[8] = b43legacy_phy_read(dev, 0x000A);
+	backup[9] = b43legacy_phy_read(dev, 0x0003);
+	backup[10] = b43legacy_radio_read16(dev, 0x007A);
+	backup[11] = b43legacy_radio_read16(dev, 0x0043);
+
+	b43legacy_phy_write(dev, 0x0429,
+			    b43legacy_phy_read(dev, 0x0429) & 0x7FFF);
+	b43legacy_phy_write(dev, 0x0001,
+			    (b43legacy_phy_read(dev, 0x0001) & 0x3FFF)
+			    | 0x4000);
+	b43legacy_phy_write(dev, 0x0811,
+			    b43legacy_phy_read(dev, 0x0811) | 0x000C);
+	b43legacy_phy_write(dev, 0x0812,
+			    (b43legacy_phy_read(dev, 0x0812) & 0xFFF3)
+			    | 0x0004);
+	b43legacy_phy_write(dev, 0x0802,
+			    b43legacy_phy_read(dev, 0x0802) & ~(0x1 | 0x2));
+	if (phy->rev >= 6) {
+		backup[12] = b43legacy_phy_read(dev, 0x002E);
+		backup[13] = b43legacy_phy_read(dev, 0x002F);
+		backup[14] = b43legacy_phy_read(dev, 0x080F);
+		backup[15] = b43legacy_phy_read(dev, 0x0810);
+		backup[16] = b43legacy_phy_read(dev, 0x0801);
+		backup[17] = b43legacy_phy_read(dev, 0x0060);
+		backup[18] = b43legacy_phy_read(dev, 0x0014);
+		backup[19] = b43legacy_phy_read(dev, 0x0478);
+
+		b43legacy_phy_write(dev, 0x002E, 0);
+		b43legacy_phy_write(dev, 0x002F, 0);
+		b43legacy_phy_write(dev, 0x080F, 0);
+		b43legacy_phy_write(dev, 0x0810, 0);
+		b43legacy_phy_write(dev, 0x0478,
+				    b43legacy_phy_read(dev, 0x0478) | 0x0100);
+		b43legacy_phy_write(dev, 0x0801,
+				    b43legacy_phy_read(dev, 0x0801) | 0x0040);
+		b43legacy_phy_write(dev, 0x0060,
+				    b43legacy_phy_read(dev, 0x0060) | 0x0040);
+		b43legacy_phy_write(dev, 0x0014,
+				    b43legacy_phy_read(dev, 0x0014) | 0x0200);
+	}
+	b43legacy_radio_write16(dev, 0x007A,
+				b43legacy_radio_read16(dev, 0x007A) | 0x0070);
+	b43legacy_radio_write16(dev, 0x007A,
+				b43legacy_radio_read16(dev, 0x007A) | 0x0080);
+	udelay(30);
+
+	v47F = (s16)((b43legacy_phy_read(dev, 0x047F) >> 8) & 0x003F);
+	if (v47F >= 0x20)
+		v47F -= 0x40;
+	if (v47F == 31) {
+		for (i = 7; i >= 4; i--) {
+			b43legacy_radio_write16(dev, 0x007B, i);
+			udelay(20);
+			v47F = (s16)((b43legacy_phy_read(dev, 0x047F) >> 8)
+							 & 0x003F);
+			if (v47F >= 0x20)
+				v47F -= 0x40;
+			if (v47F < 31 && saved == 0xFFFF)
+				saved = i;
+		}
+		if (saved == 0xFFFF)
+			saved = 4;
+	} else {
+		b43legacy_radio_write16(dev, 0x007A,
+					b43legacy_radio_read16(dev, 0x007A)
+					& 0x007F);
+		b43legacy_phy_write(dev, 0x0814,
+				    b43legacy_phy_read(dev, 0x0814) | 0x0001);
+		b43legacy_phy_write(dev, 0x0815,
+				    b43legacy_phy_read(dev, 0x0815) & 0xFFFE);
+		b43legacy_phy_write(dev, 0x0811,
+				    b43legacy_phy_read(dev, 0x0811) | 0x000C);
+		b43legacy_phy_write(dev, 0x0812,
+				    b43legacy_phy_read(dev, 0x0812) | 0x000C);
+		b43legacy_phy_write(dev, 0x0811,
+				    b43legacy_phy_read(dev, 0x0811) | 0x0030);
+		b43legacy_phy_write(dev, 0x0812,
+				    b43legacy_phy_read(dev, 0x0812) | 0x0030);
+		b43legacy_phy_write(dev, 0x005A, 0x0480);
+		b43legacy_phy_write(dev, 0x0059, 0x0810);
+		b43legacy_phy_write(dev, 0x0058, 0x000D);
+		if (phy->analog == 0)
+			b43legacy_phy_write(dev, 0x0003, 0x0122);
+		else
+			b43legacy_phy_write(dev, 0x000A,
+					    b43legacy_phy_read(dev, 0x000A)
+					    | 0x2000);
+		b43legacy_phy_write(dev, 0x0814,
+				    b43legacy_phy_read(dev, 0x0814) | 0x0004);
+		b43legacy_phy_write(dev, 0x0815,
+				    b43legacy_phy_read(dev, 0x0815) & 0xFFFB);
+		b43legacy_phy_write(dev, 0x0003,
+				    (b43legacy_phy_read(dev, 0x0003) & 0xFF9F)
+				    | 0x0040);
+		b43legacy_radio_write16(dev, 0x007A,
+					b43legacy_radio_read16(dev, 0x007A)
+					| 0x000F);
+		b43legacy_set_all_gains(dev, 3, 0, 1);
+		b43legacy_radio_write16(dev, 0x0043,
+					(b43legacy_radio_read16(dev, 0x0043)
+					& 0x00F0) | 0x000F);
+		udelay(30);
+		v47F = (s16)((b43legacy_phy_read(dev, 0x047F) >> 8) & 0x003F);
+		if (v47F >= 0x20)
+			v47F -= 0x40;
+		if (v47F == -32) {
+			for (i = 0; i < 4; i++) {
+				b43legacy_radio_write16(dev, 0x007B, i);
+				udelay(20);
+				v47F = (s16)((b43legacy_phy_read(dev, 0x047F) >>
+								 8) & 0x003F);
+				if (v47F >= 0x20)
+					v47F -= 0x40;
+				if (v47F > -31 && saved == 0xFFFF)
+					saved = i;
+			}
+			if (saved == 0xFFFF)
+				saved = 3;
+		} else
+			saved = 0;
+	}
+	b43legacy_radio_write16(dev, 0x007B, saved);
+
+	if (phy->rev >= 6) {
+		b43legacy_phy_write(dev, 0x002E, backup[12]);
+		b43legacy_phy_write(dev, 0x002F, backup[13]);
+		b43legacy_phy_write(dev, 0x080F, backup[14]);
+		b43legacy_phy_write(dev, 0x0810, backup[15]);
+	}
+	b43legacy_phy_write(dev, 0x0814, backup[3]);
+	b43legacy_phy_write(dev, 0x0815, backup[4]);
+	b43legacy_phy_write(dev, 0x005A, backup[5]);
+	b43legacy_phy_write(dev, 0x0059, backup[6]);
+	b43legacy_phy_write(dev, 0x0058, backup[7]);
+	b43legacy_phy_write(dev, 0x000A, backup[8]);
+	b43legacy_phy_write(dev, 0x0003, backup[9]);
+	b43legacy_radio_write16(dev, 0x0043, backup[11]);
+	b43legacy_radio_write16(dev, 0x007A, backup[10]);
+	b43legacy_phy_write(dev, 0x0802,
+			    b43legacy_phy_read(dev, 0x0802) | 0x1 | 0x2);
+	b43legacy_phy_write(dev, 0x0429,
+			    b43legacy_phy_read(dev, 0x0429) | 0x8000);
+	b43legacy_set_original_gains(dev);
+	if (phy->rev >= 6) {
+		b43legacy_phy_write(dev, 0x0801, backup[16]);
+		b43legacy_phy_write(dev, 0x0060, backup[17]);
+		b43legacy_phy_write(dev, 0x0014, backup[18]);
+		b43legacy_phy_write(dev, 0x0478, backup[19]);
+	}
+	b43legacy_phy_write(dev, 0x0001, backup[0]);
+	b43legacy_phy_write(dev, 0x0812, backup[2]);
+	b43legacy_phy_write(dev, 0x0811, backup[1]);
+}
+
+void b43legacy_calc_nrssi_slope(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+	u16 backup[18] = { 0 };
+	u16 tmp;
+	s16 nrssi0;
+	s16 nrssi1;
+
+	switch (phy->type) {
+	case B43legacy_PHYTYPE_B:
+		backup[0] = b43legacy_radio_read16(dev, 0x007A);
+		backup[1] = b43legacy_radio_read16(dev, 0x0052);
+		backup[2] = b43legacy_radio_read16(dev, 0x0043);
+		backup[3] = b43legacy_phy_read(dev, 0x0030);
+		backup[4] = b43legacy_phy_read(dev, 0x0026);
+		backup[5] = b43legacy_phy_read(dev, 0x0015);
+		backup[6] = b43legacy_phy_read(dev, 0x002A);
+		backup[7] = b43legacy_phy_read(dev, 0x0020);
+		backup[8] = b43legacy_phy_read(dev, 0x005A);
+		backup[9] = b43legacy_phy_read(dev, 0x0059);
+		backup[10] = b43legacy_phy_read(dev, 0x0058);
+		backup[11] = b43legacy_read16(dev, 0x03E2);
+		backup[12] = b43legacy_read16(dev, 0x03E6);
+		backup[13] = b43legacy_read16(dev, B43legacy_MMIO_CHANNEL_EXT);
+
+		tmp  = b43legacy_radio_read16(dev, 0x007A);
+		tmp &= (phy->rev >= 5) ? 0x007F : 0x000F;
+		b43legacy_radio_write16(dev, 0x007A, tmp);
+		b43legacy_phy_write(dev, 0x0030, 0x00FF);
+		b43legacy_write16(dev, 0x03EC, 0x7F7F);
+		b43legacy_phy_write(dev, 0x0026, 0x0000);
+		b43legacy_phy_write(dev, 0x0015,
+				    b43legacy_phy_read(dev, 0x0015) | 0x0020);
+		b43legacy_phy_write(dev, 0x002A, 0x08A3);
+		b43legacy_radio_write16(dev, 0x007A,
+					b43legacy_radio_read16(dev, 0x007A)
+					| 0x0080);
+
+		nrssi0 = (s16)b43legacy_phy_read(dev, 0x0027);
+		b43legacy_radio_write16(dev, 0x007A,
+					b43legacy_radio_read16(dev, 0x007A)
+					& 0x007F);
+		if (phy->analog >= 2)
+			b43legacy_write16(dev, 0x03E6, 0x0040);
+		else if (phy->analog == 0)
+			b43legacy_write16(dev, 0x03E6, 0x0122);
+		else
+			b43legacy_write16(dev, B43legacy_MMIO_CHANNEL_EXT,
+					  b43legacy_read16(dev,
+					  B43legacy_MMIO_CHANNEL_EXT) & 0x2000);
+		b43legacy_phy_write(dev, 0x0020, 0x3F3F);
+		b43legacy_phy_write(dev, 0x0015, 0xF330);
+		b43legacy_radio_write16(dev, 0x005A, 0x0060);
+		b43legacy_radio_write16(dev, 0x0043,
+					b43legacy_radio_read16(dev, 0x0043)
+					& 0x00F0);
+		b43legacy_phy_write(dev, 0x005A, 0x0480);
+		b43legacy_phy_write(dev, 0x0059, 0x0810);
+		b43legacy_phy_write(dev, 0x0058, 0x000D);
+		udelay(20);
+
+		nrssi1 = (s16)b43legacy_phy_read(dev, 0x0027);
+		b43legacy_phy_write(dev, 0x0030, backup[3]);
+		b43legacy_radio_write16(dev, 0x007A, backup[0]);
+		b43legacy_write16(dev, 0x03E2, backup[11]);
+		b43legacy_phy_write(dev, 0x0026, backup[4]);
+		b43legacy_phy_write(dev, 0x0015, backup[5]);
+		b43legacy_phy_write(dev, 0x002A, backup[6]);
+		b43legacy_synth_pu_workaround(dev, phy->channel);
+		if (phy->analog != 0)
+			b43legacy_write16(dev, 0x03F4, backup[13]);
+
+		b43legacy_phy_write(dev, 0x0020, backup[7]);
+		b43legacy_phy_write(dev, 0x005A, backup[8]);
+		b43legacy_phy_write(dev, 0x0059, backup[9]);
+		b43legacy_phy_write(dev, 0x0058, backup[10]);
+		b43legacy_radio_write16(dev, 0x0052, backup[1]);
+		b43legacy_radio_write16(dev, 0x0043, backup[2]);
+
+		if (nrssi0 == nrssi1)
+			phy->nrssislope = 0x00010000;
+		else
+			phy->nrssislope = 0x00400000 / (nrssi0 - nrssi1);
+
+		if (nrssi0 <= -4) {
+			phy->nrssi[0] = nrssi0;
+			phy->nrssi[1] = nrssi1;
+		}
+		break;
+	case B43legacy_PHYTYPE_G:
+		if (phy->radio_rev >= 9)
+			return;
+		if (phy->radio_rev == 8)
+			b43legacy_calc_nrssi_offset(dev);
+
+		b43legacy_phy_write(dev, B43legacy_PHY_G_CRS,
+				    b43legacy_phy_read(dev, B43legacy_PHY_G_CRS)
+				    & 0x7FFF);
+		b43legacy_phy_write(dev, 0x0802,
+				    b43legacy_phy_read(dev, 0x0802) & 0xFFFC);
+		backup[7] = b43legacy_read16(dev, 0x03E2);
+		b43legacy_write16(dev, 0x03E2,
+				  b43legacy_read16(dev, 0x03E2) | 0x8000);
+		backup[0] = b43legacy_radio_read16(dev, 0x007A);
+		backup[1] = b43legacy_radio_read16(dev, 0x0052);
+		backup[2] = b43legacy_radio_read16(dev, 0x0043);
+		backup[3] = b43legacy_phy_read(dev, 0x0015);
+		backup[4] = b43legacy_phy_read(dev, 0x005A);
+		backup[5] = b43legacy_phy_read(dev, 0x0059);
+		backup[6] = b43legacy_phy_read(dev, 0x0058);
+		backup[8] = b43legacy_read16(dev, 0x03E6);
+		backup[9] = b43legacy_read16(dev, B43legacy_MMIO_CHANNEL_EXT);
+		if (phy->rev >= 3) {
+			backup[10] = b43legacy_phy_read(dev, 0x002E);
+			backup[11] = b43legacy_phy_read(dev, 0x002F);
+			backup[12] = b43legacy_phy_read(dev, 0x080F);
+			backup[13] = b43legacy_phy_read(dev,
+						B43legacy_PHY_G_LO_CONTROL);
+			backup[14] = b43legacy_phy_read(dev, 0x0801);
+			backup[15] = b43legacy_phy_read(dev, 0x0060);
+			backup[16] = b43legacy_phy_read(dev, 0x0014);
+			backup[17] = b43legacy_phy_read(dev, 0x0478);
+			b43legacy_phy_write(dev, 0x002E, 0);
+			b43legacy_phy_write(dev, B43legacy_PHY_G_LO_CONTROL, 0);
+			switch (phy->rev) {
+			case 4: case 6: case 7:
+				b43legacy_phy_write(dev, 0x0478,
+						    b43legacy_phy_read(dev,
+						    0x0478) | 0x0100);
+				b43legacy_phy_write(dev, 0x0801,
+						    b43legacy_phy_read(dev,
+						    0x0801) | 0x0040);
+				break;
+			case 3: case 5:
+				b43legacy_phy_write(dev, 0x0801,
+						    b43legacy_phy_read(dev,
+						    0x0801) & 0xFFBF);
+				break;
+			}
+			b43legacy_phy_write(dev, 0x0060,
+					    b43legacy_phy_read(dev, 0x0060)
+					    | 0x0040);
+			b43legacy_phy_write(dev, 0x0014,
+					    b43legacy_phy_read(dev, 0x0014)
+					    | 0x0200);
+		}
+		b43legacy_radio_write16(dev, 0x007A,
+					b43legacy_radio_read16(dev, 0x007A)
+					| 0x0070);
+		b43legacy_set_all_gains(dev, 0, 8, 0);
+		b43legacy_radio_write16(dev, 0x007A,
+					b43legacy_radio_read16(dev, 0x007A)
+					& 0x00F7);
+		if (phy->rev >= 2) {
+			b43legacy_phy_write(dev, 0x0811,
+					    (b43legacy_phy_read(dev, 0x0811)
+					    & 0xFFCF) | 0x0030);
+			b43legacy_phy_write(dev, 0x0812,
+					    (b43legacy_phy_read(dev, 0x0812)
+					    & 0xFFCF) | 0x0010);
+		}
+		b43legacy_radio_write16(dev, 0x007A,
+					b43legacy_radio_read16(dev, 0x007A)
+					| 0x0080);
+		udelay(20);
+
+		nrssi0 = (s16)((b43legacy_phy_read(dev, 0x047F) >> 8) & 0x003F);
+		if (nrssi0 >= 0x0020)
+			nrssi0 -= 0x0040;
+
+		b43legacy_radio_write16(dev, 0x007A,
+					b43legacy_radio_read16(dev, 0x007A)
+					& 0x007F);
+		if (phy->analog >= 2)
+			b43legacy_phy_write(dev, 0x0003,
+					    (b43legacy_phy_read(dev, 0x0003)
+					    & 0xFF9F) | 0x0040);
+
+		b43legacy_write16(dev, B43legacy_MMIO_CHANNEL_EXT,
+				  b43legacy_read16(dev,
+				  B43legacy_MMIO_CHANNEL_EXT) | 0x2000);
+		b43legacy_radio_write16(dev, 0x007A,
+					b43legacy_radio_read16(dev, 0x007A)
+					| 0x000F);
+		b43legacy_phy_write(dev, 0x0015, 0xF330);
+		if (phy->rev >= 2) {
+			b43legacy_phy_write(dev, 0x0812,
+					    (b43legacy_phy_read(dev, 0x0812)
+					    & 0xFFCF) | 0x0020);
+			b43legacy_phy_write(dev, 0x0811,
+					    (b43legacy_phy_read(dev, 0x0811)
+					    & 0xFFCF) | 0x0020);
+		}
+
+		b43legacy_set_all_gains(dev, 3, 0, 1);
+		if (phy->radio_rev == 8)
+			b43legacy_radio_write16(dev, 0x0043, 0x001F);
+		else {
+			tmp = b43legacy_radio_read16(dev, 0x0052) & 0xFF0F;
+			b43legacy_radio_write16(dev, 0x0052, tmp | 0x0060);
+			tmp = b43legacy_radio_read16(dev, 0x0043) & 0xFFF0;
+			b43legacy_radio_write16(dev, 0x0043, tmp | 0x0009);
+		}
+		b43legacy_phy_write(dev, 0x005A, 0x0480);
+		b43legacy_phy_write(dev, 0x0059, 0x0810);
+		b43legacy_phy_write(dev, 0x0058, 0x000D);
+		udelay(20);
+		nrssi1 = (s16)((b43legacy_phy_read(dev, 0x047F) >> 8) & 0x003F);
+		if (nrssi1 >= 0x0020)
+			nrssi1 -= 0x0040;
+		if (nrssi0 == nrssi1)
+			phy->nrssislope = 0x00010000;
+		else
+			phy->nrssislope = 0x00400000 / (nrssi0 - nrssi1);
+		if (nrssi0 >= -4) {
+			phy->nrssi[0] = nrssi1;
+			phy->nrssi[1] = nrssi0;
+		}
+		if (phy->rev >= 3) {
+			b43legacy_phy_write(dev, 0x002E, backup[10]);
+			b43legacy_phy_write(dev, 0x002F, backup[11]);
+			b43legacy_phy_write(dev, 0x080F, backup[12]);
+			b43legacy_phy_write(dev, B43legacy_PHY_G_LO_CONTROL,
+					    backup[13]);
+		}
+		if (phy->rev >= 2) {
+			b43legacy_phy_write(dev, 0x0812,
+					    b43legacy_phy_read(dev, 0x0812)
+					    & 0xFFCF);
+			b43legacy_phy_write(dev, 0x0811,
+					    b43legacy_phy_read(dev, 0x0811)
+					    & 0xFFCF);
+		}
+
+		b43legacy_radio_write16(dev, 0x007A, backup[0]);
+		b43legacy_radio_write16(dev, 0x0052, backup[1]);
+		b43legacy_radio_write16(dev, 0x0043, backup[2]);
+		b43legacy_write16(dev, 0x03E2, backup[7]);
+		b43legacy_write16(dev, 0x03E6, backup[8]);
+		b43legacy_write16(dev, B43legacy_MMIO_CHANNEL_EXT, backup[9]);
+		b43legacy_phy_write(dev, 0x0015, backup[3]);
+		b43legacy_phy_write(dev, 0x005A, backup[4]);
+		b43legacy_phy_write(dev, 0x0059, backup[5]);
+		b43legacy_phy_write(dev, 0x0058, backup[6]);
+		b43legacy_synth_pu_workaround(dev, phy->channel);
+		b43legacy_phy_write(dev, 0x0802,
+				    b43legacy_phy_read(dev, 0x0802) | 0x0003);
+		b43legacy_set_original_gains(dev);
+		b43legacy_phy_write(dev, B43legacy_PHY_G_CRS,
+				    b43legacy_phy_read(dev, B43legacy_PHY_G_CRS)
+				    | 0x8000);
+		if (phy->rev >= 3) {
+			b43legacy_phy_write(dev, 0x0801, backup[14]);
+			b43legacy_phy_write(dev, 0x0060, backup[15]);
+			b43legacy_phy_write(dev, 0x0014, backup[16]);
+			b43legacy_phy_write(dev, 0x0478, backup[17]);
+		}
+		b43legacy_nrssi_mem_update(dev);
+		b43legacy_calc_nrssi_threshold(dev);
+		break;
+	default:
+		B43legacy_BUG_ON(1);
+	}
+}
+
+void b43legacy_calc_nrssi_threshold(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+	s32 threshold;
+	s32 a;
+	s32 b;
+	s16 tmp16;
+	u16 tmp_u16;
+
+	switch (phy->type) {
+	case B43legacy_PHYTYPE_B: {
+		if (phy->radio_ver != 0x2050)
+			return;
+		if (!(dev->dev->bus->sprom.r1.boardflags_lo &
+		    B43legacy_BFL_RSSI))
+			return;
+
+		if (phy->radio_rev >= 6) {
+			threshold = (phy->nrssi[1] - phy->nrssi[0]) * 32;
+			threshold += 20 * (phy->nrssi[0] + 1);
+			threshold /= 40;
+		} else
+			threshold = phy->nrssi[1] - 5;
+
+		threshold = limit_value(threshold, 0, 0x3E);
+		b43legacy_phy_read(dev, 0x0020); /* dummy read */
+		b43legacy_phy_write(dev, 0x0020, (((u16)threshold) << 8)
+				    | 0x001C);
+
+		if (phy->radio_rev >= 6) {
+			b43legacy_phy_write(dev, 0x0087, 0x0E0D);
+			b43legacy_phy_write(dev, 0x0086, 0x0C0B);
+			b43legacy_phy_write(dev, 0x0085, 0x0A09);
+			b43legacy_phy_write(dev, 0x0084, 0x0808);
+			b43legacy_phy_write(dev, 0x0083, 0x0808);
+			b43legacy_phy_write(dev, 0x0082, 0x0604);
+			b43legacy_phy_write(dev, 0x0081, 0x0302);
+			b43legacy_phy_write(dev, 0x0080, 0x0100);
+		}
+		break;
+	}
+	case B43legacy_PHYTYPE_G:
+		if (!phy->gmode ||
+		    !(dev->dev->bus->sprom.r1.boardflags_lo &
+		    B43legacy_BFL_RSSI)) {
+			tmp16 = b43legacy_nrssi_hw_read(dev, 0x20);
+			if (tmp16 >= 0x20)
+				tmp16 -= 0x40;
+			if (tmp16 < 3)
+				b43legacy_phy_write(dev, 0x048A,
+						    (b43legacy_phy_read(dev,
+						    0x048A) & 0xF000) | 0x09EB);
+			else
+				b43legacy_phy_write(dev, 0x048A,
+						    (b43legacy_phy_read(dev,
+						    0x048A) & 0xF000) | 0x0AED);
+		} else {
+			if (phy->interfmode ==
+			    B43legacy_RADIO_INTERFMODE_NONWLAN) {
+				a = 0xE;
+				b = 0xA;
+			} else if (!phy->aci_wlan_automatic &&
+				    phy->aci_enable) {
+				a = 0x13;
+				b = 0x12;
+			} else {
+				a = 0xE;
+				b = 0x11;
+			}
+
+			a = a * (phy->nrssi[1] - phy->nrssi[0]);
+			a += (phy->nrssi[0] << 6);
+			if (a < 32)
+				a += 31;
+			else
+				a += 32;
+			a = a >> 6;
+			a = limit_value(a, -31, 31);
+
+			b = b * (phy->nrssi[1] - phy->nrssi[0]);
+			b += (phy->nrssi[0] << 6);
+			if (b < 32)
+				b += 31;
+			else
+				b += 32;
+			b = b >> 6;
+			b = limit_value(b, -31, 31);
+
+			tmp_u16 = b43legacy_phy_read(dev, 0x048A) & 0xF000;
+			tmp_u16 |= ((u32)b & 0x0000003F);
+			tmp_u16 |= (((u32)a & 0x0000003F) << 6);
+			b43legacy_phy_write(dev, 0x048A, tmp_u16);
+		}
+		break;
+	default:
+		B43legacy_BUG_ON(1);
+	}
+}
+
+/* Stack implementation to save/restore values from the
+ * interference mitigation code.
+ * It is save to restore values in random order.
+ */
+static void _stack_save(u32 *_stackptr, size_t *stackidx,
+			u8 id, u16 offset, u16 value)
+{
+	u32 *stackptr = &(_stackptr[*stackidx]);
+
+	B43legacy_WARN_ON(!((offset & 0xE000) == 0x0000));
+	B43legacy_WARN_ON(!((id & 0xF8) == 0x00));
+	*stackptr = offset;
+	*stackptr |= ((u32)id) << 13;
+	*stackptr |= ((u32)value) << 16;
+	(*stackidx)++;
+	B43legacy_WARN_ON(!(*stackidx < B43legacy_INTERFSTACK_SIZE));
+}
+
+static u16 _stack_restore(u32 *stackptr,
+			  u8 id, u16 offset)
+{
+	size_t i;
+
+	B43legacy_WARN_ON(!((offset & 0xE000) == 0x0000));
+	B43legacy_WARN_ON(!((id & 0xF8) == 0x00));
+	for (i = 0; i < B43legacy_INTERFSTACK_SIZE; i++, stackptr++) {
+		if ((*stackptr & 0x00001FFF) != offset)
+			continue;
+		if (((*stackptr & 0x00007000) >> 13) != id)
+			continue;
+		return ((*stackptr & 0xFFFF0000) >> 16);
+	}
+	B43legacy_BUG_ON(1);
+
+	return 0;
+}
+
+#define phy_stacksave(offset)					\
+	do {							\
+		_stack_save(stack, &stackidx, 0x1, (offset),	\
+			    b43legacy_phy_read(dev, (offset)));	\
+	} while (0)
+#define phy_stackrestore(offset)				\
+	do {							\
+		b43legacy_phy_write(dev, (offset),		\
+				    _stack_restore(stack, 0x1,	\
+				    (offset)));			\
+	} while (0)
+#define radio_stacksave(offset)						\
+	do {								\
+		_stack_save(stack, &stackidx, 0x2, (offset),		\
+			    b43legacy_radio_read16(dev, (offset)));	\
+	} while (0)
+#define radio_stackrestore(offset)					\
+	do {								\
+		b43legacy_radio_write16(dev, (offset),			\
+					_stack_restore(stack, 0x2,	\
+					(offset)));			\
+	} while (0)
+#define ilt_stacksave(offset)					\
+	do {							\
+		_stack_save(stack, &stackidx, 0x3, (offset),	\
+			    b43legacy_ilt_read(dev, (offset)));	\
+	} while (0)
+#define ilt_stackrestore(offset)				\
+	do {							\
+		b43legacy_ilt_write(dev, (offset),		\
+				  _stack_restore(stack, 0x3,	\
+						 (offset)));	\
+	} while (0)
+
+static void
+b43legacy_radio_interference_mitigation_enable(struct b43legacy_wldev *dev,
+					       int mode)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+	u16 tmp;
+	u16 flipped;
+	u32 tmp32;
+	size_t stackidx = 0;
+	u32 *stack = phy->interfstack;
+
+	switch (mode) {
+	case B43legacy_RADIO_INTERFMODE_NONWLAN:
+		if (phy->rev != 1) {
+			b43legacy_phy_write(dev, 0x042B,
+					    b43legacy_phy_read(dev, 0x042B)
+					    | 0x0800);
+			b43legacy_phy_write(dev, B43legacy_PHY_G_CRS,
+					    b43legacy_phy_read(dev,
+					    B43legacy_PHY_G_CRS) & ~0x4000);
+			break;
+		}
+		radio_stacksave(0x0078);
+		tmp = (b43legacy_radio_read16(dev, 0x0078) & 0x001E);
+		flipped = flip_4bit(tmp);
+		if (flipped < 10 && flipped >= 8)
+			flipped = 7;
+		else if (flipped >= 10)
+			flipped -= 3;
+		flipped = flip_4bit(flipped);
+		flipped = (flipped << 1) | 0x0020;
+		b43legacy_radio_write16(dev, 0x0078, flipped);
+
+		b43legacy_calc_nrssi_threshold(dev);
+
+		phy_stacksave(0x0406);
+		b43legacy_phy_write(dev, 0x0406, 0x7E28);
+
+		b43legacy_phy_write(dev, 0x042B,
+				    b43legacy_phy_read(dev, 0x042B) | 0x0800);
+		b43legacy_phy_write(dev, B43legacy_PHY_RADIO_BITFIELD,
+				    b43legacy_phy_read(dev,
+				    B43legacy_PHY_RADIO_BITFIELD) | 0x1000);
+
+		phy_stacksave(0x04A0);
+		b43legacy_phy_write(dev, 0x04A0,
+				    (b43legacy_phy_read(dev, 0x04A0) & 0xC0C0)
+				    | 0x0008);
+		phy_stacksave(0x04A1);
+		b43legacy_phy_write(dev, 0x04A1,
+				    (b43legacy_phy_read(dev, 0x04A1) & 0xC0C0)
+				    | 0x0605);
+		phy_stacksave(0x04A2);
+		b43legacy_phy_write(dev, 0x04A2,
+				    (b43legacy_phy_read(dev, 0x04A2) & 0xC0C0)
+				    | 0x0204);
+		phy_stacksave(0x04A8);
+		b43legacy_phy_write(dev, 0x04A8,
+				    (b43legacy_phy_read(dev, 0x04A8) & 0xC0C0)
+				    | 0x0803);
+		phy_stacksave(0x04AB);
+		b43legacy_phy_write(dev, 0x04AB,
+				    (b43legacy_phy_read(dev, 0x04AB) & 0xC0C0)
+				    | 0x0605);
+
+		phy_stacksave(0x04A7);
+		b43legacy_phy_write(dev, 0x04A7, 0x0002);
+		phy_stacksave(0x04A3);
+		b43legacy_phy_write(dev, 0x04A3, 0x287A);
+		phy_stacksave(0x04A9);
+		b43legacy_phy_write(dev, 0x04A9, 0x2027);
+		phy_stacksave(0x0493);
+		b43legacy_phy_write(dev, 0x0493, 0x32F5);
+		phy_stacksave(0x04AA);
+		b43legacy_phy_write(dev, 0x04AA, 0x2027);
+		phy_stacksave(0x04AC);
+		b43legacy_phy_write(dev, 0x04AC, 0x32F5);
+		break;
+	case B43legacy_RADIO_INTERFMODE_MANUALWLAN:
+		if (b43legacy_phy_read(dev, 0x0033) & 0x0800)
+			break;
+
+		phy->aci_enable = 1;
+
+		phy_stacksave(B43legacy_PHY_RADIO_BITFIELD);
+		phy_stacksave(B43legacy_PHY_G_CRS);
+		if (phy->rev < 2)
+			phy_stacksave(0x0406);
+		else {
+			phy_stacksave(0x04C0);
+			phy_stacksave(0x04C1);
+		}
+		phy_stacksave(0x0033);
+		phy_stacksave(0x04A7);
+		phy_stacksave(0x04A3);
+		phy_stacksave(0x04A9);
+		phy_stacksave(0x04AA);
+		phy_stacksave(0x04AC);
+		phy_stacksave(0x0493);
+		phy_stacksave(0x04A1);
+		phy_stacksave(0x04A0);
+		phy_stacksave(0x04A2);
+		phy_stacksave(0x048A);
+		phy_stacksave(0x04A8);
+		phy_stacksave(0x04AB);
+		if (phy->rev == 2) {
+			phy_stacksave(0x04AD);
+			phy_stacksave(0x04AE);
+		} else if (phy->rev >= 3) {
+			phy_stacksave(0x04AD);
+			phy_stacksave(0x0415);
+			phy_stacksave(0x0416);
+			phy_stacksave(0x0417);
+			ilt_stacksave(0x1A00 + 0x2);
+			ilt_stacksave(0x1A00 + 0x3);
+		}
+		phy_stacksave(0x042B);
+		phy_stacksave(0x048C);
+
+		b43legacy_phy_write(dev, B43legacy_PHY_RADIO_BITFIELD,
+				    b43legacy_phy_read(dev,
+				    B43legacy_PHY_RADIO_BITFIELD) & ~0x1000);
+		b43legacy_phy_write(dev, B43legacy_PHY_G_CRS,
+				    (b43legacy_phy_read(dev,
+				    B43legacy_PHY_G_CRS)
+				    & 0xFFFC) | 0x0002);
+
+		b43legacy_phy_write(dev, 0x0033, 0x0800);
+		b43legacy_phy_write(dev, 0x04A3, 0x2027);
+		b43legacy_phy_write(dev, 0x04A9, 0x1CA8);
+		b43legacy_phy_write(dev, 0x0493, 0x287A);
+		b43legacy_phy_write(dev, 0x04AA, 0x1CA8);
+		b43legacy_phy_write(dev, 0x04AC, 0x287A);
+
+		b43legacy_phy_write(dev, 0x04A0,
+				    (b43legacy_phy_read(dev, 0x04A0)
+				    & 0xFFC0) | 0x001A);
+		b43legacy_phy_write(dev, 0x04A7, 0x000D);
+
+		if (phy->rev < 2)
+			b43legacy_phy_write(dev, 0x0406, 0xFF0D);
+		else if (phy->rev == 2) {
+			b43legacy_phy_write(dev, 0x04C0, 0xFFFF);
+			b43legacy_phy_write(dev, 0x04C1, 0x00A9);
+		} else {
+			b43legacy_phy_write(dev, 0x04C0, 0x00C1);
+			b43legacy_phy_write(dev, 0x04C1, 0x0059);
+		}
+
+		b43legacy_phy_write(dev, 0x04A1,
+				    (b43legacy_phy_read(dev, 0x04A1)
+				    & 0xC0FF) | 0x1800);
+		b43legacy_phy_write(dev, 0x04A1,
+				    (b43legacy_phy_read(dev, 0x04A1)
+				    & 0xFFC0) | 0x0015);
+		b43legacy_phy_write(dev, 0x04A8,
+				    (b43legacy_phy_read(dev, 0x04A8)
+				    & 0xCFFF) | 0x1000);
+		b43legacy_phy_write(dev, 0x04A8,
+				    (b43legacy_phy_read(dev, 0x04A8)
+				    & 0xF0FF) | 0x0A00);
+		b43legacy_phy_write(dev, 0x04AB,
+				    (b43legacy_phy_read(dev, 0x04AB)
+				    & 0xCFFF) | 0x1000);
+		b43legacy_phy_write(dev, 0x04AB,
+				    (b43legacy_phy_read(dev, 0x04AB)
+				    & 0xF0FF) | 0x0800);
+		b43legacy_phy_write(dev, 0x04AB,
+				    (b43legacy_phy_read(dev, 0x04AB)
+				    & 0xFFCF) | 0x0010);
+		b43legacy_phy_write(dev, 0x04AB,
+				    (b43legacy_phy_read(dev, 0x04AB)
+				    & 0xFFF0) | 0x0005);
+		b43legacy_phy_write(dev, 0x04A8,
+				    (b43legacy_phy_read(dev, 0x04A8)
+				    & 0xFFCF) | 0x0010);
+		b43legacy_phy_write(dev, 0x04A8,
+				    (b43legacy_phy_read(dev, 0x04A8)
+				    & 0xFFF0) | 0x0006);
+		b43legacy_phy_write(dev, 0x04A2,
+				    (b43legacy_phy_read(dev, 0x04A2)
+				    & 0xF0FF) | 0x0800);
+		b43legacy_phy_write(dev, 0x04A0,
+				    (b43legacy_phy_read(dev, 0x04A0)
+				    & 0xF0FF) | 0x0500);
+		b43legacy_phy_write(dev, 0x04A2,
+				    (b43legacy_phy_read(dev, 0x04A2)
+				    & 0xFFF0) | 0x000B);
+
+		if (phy->rev >= 3) {
+			b43legacy_phy_write(dev, 0x048A,
+					    b43legacy_phy_read(dev, 0x048A)
+					    & ~0x8000);
+			b43legacy_phy_write(dev, 0x0415,
+					    (b43legacy_phy_read(dev, 0x0415)
+					    & 0x8000) | 0x36D8);
+			b43legacy_phy_write(dev, 0x0416,
+					    (b43legacy_phy_read(dev, 0x0416)
+					    & 0x8000) | 0x36D8);
+			b43legacy_phy_write(dev, 0x0417,
+					    (b43legacy_phy_read(dev, 0x0417)
+					    & 0xFE00) | 0x016D);
+		} else {
+			b43legacy_phy_write(dev, 0x048A,
+					    b43legacy_phy_read(dev, 0x048A)
+					    | 0x1000);
+			b43legacy_phy_write(dev, 0x048A,
+					    (b43legacy_phy_read(dev, 0x048A)
+					    & 0x9FFF) | 0x2000);
+			tmp32 = b43legacy_shm_read32(dev, B43legacy_SHM_SHARED,
+					    B43legacy_UCODEFLAGS_OFFSET);
+			if (!(tmp32 & 0x800)) {
+				tmp32 |= 0x800;
+				b43legacy_shm_write32(dev, B43legacy_SHM_SHARED,
+					    B43legacy_UCODEFLAGS_OFFSET,
+					    tmp32);
+			}
+		}
+		if (phy->rev >= 2)
+			b43legacy_phy_write(dev, 0x042B,
+					    b43legacy_phy_read(dev, 0x042B)
+					    | 0x0800);
+		b43legacy_phy_write(dev, 0x048C,
+				    (b43legacy_phy_read(dev, 0x048C)
+				    & 0xF0FF) | 0x0200);
+		if (phy->rev == 2) {
+			b43legacy_phy_write(dev, 0x04AE,
+					    (b43legacy_phy_read(dev, 0x04AE)
+					    & 0xFF00) | 0x007F);
+			b43legacy_phy_write(dev, 0x04AD,
+					    (b43legacy_phy_read(dev, 0x04AD)
+					    & 0x00FF) | 0x1300);
+		} else if (phy->rev >= 6) {
+			b43legacy_ilt_write(dev, 0x1A00 + 0x3, 0x007F);
+			b43legacy_ilt_write(dev, 0x1A00 + 0x2, 0x007F);
+			b43legacy_phy_write(dev, 0x04AD,
+					    b43legacy_phy_read(dev, 0x04AD)
+					    & 0x00FF);
+		}
+		b43legacy_calc_nrssi_slope(dev);
+		break;
+	default:
+		B43legacy_BUG_ON(1);
+	}
+}
+
+static void
+b43legacy_radio_interference_mitigation_disable(struct b43legacy_wldev *dev,
+						int mode)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+	u32 tmp32;
+	u32 *stack = phy->interfstack;
+
+	switch (mode) {
+	case B43legacy_RADIO_INTERFMODE_NONWLAN:
+		if (phy->rev != 1) {
+			b43legacy_phy_write(dev, 0x042B,
+					    b43legacy_phy_read(dev, 0x042B)
+					    & ~0x0800);
+			b43legacy_phy_write(dev, B43legacy_PHY_G_CRS,
+					    b43legacy_phy_read(dev,
+					    B43legacy_PHY_G_CRS) | 0x4000);
+			break;
+		}
+		phy_stackrestore(0x0078);
+		b43legacy_calc_nrssi_threshold(dev);
+		phy_stackrestore(0x0406);
+		b43legacy_phy_write(dev, 0x042B,
+				    b43legacy_phy_read(dev, 0x042B) & ~0x0800);
+		if (!dev->bad_frames_preempt)
+			b43legacy_phy_write(dev, B43legacy_PHY_RADIO_BITFIELD,
+					    b43legacy_phy_read(dev,
+					    B43legacy_PHY_RADIO_BITFIELD)
+					    & ~(1 << 11));
+		b43legacy_phy_write(dev, B43legacy_PHY_G_CRS,
+				    b43legacy_phy_read(dev, B43legacy_PHY_G_CRS)
+				    | 0x4000);
+		phy_stackrestore(0x04A0);
+		phy_stackrestore(0x04A1);
+		phy_stackrestore(0x04A2);
+		phy_stackrestore(0x04A8);
+		phy_stackrestore(0x04AB);
+		phy_stackrestore(0x04A7);
+		phy_stackrestore(0x04A3);
+		phy_stackrestore(0x04A9);
+		phy_stackrestore(0x0493);
+		phy_stackrestore(0x04AA);
+		phy_stackrestore(0x04AC);
+		break;
+	case B43legacy_RADIO_INTERFMODE_MANUALWLAN:
+		if (!(b43legacy_phy_read(dev, 0x0033) & 0x0800))
+			break;
+
+		phy->aci_enable = 0;
+
+		phy_stackrestore(B43legacy_PHY_RADIO_BITFIELD);
+		phy_stackrestore(B43legacy_PHY_G_CRS);
+		phy_stackrestore(0x0033);
+		phy_stackrestore(0x04A3);
+		phy_stackrestore(0x04A9);
+		phy_stackrestore(0x0493);
+		phy_stackrestore(0x04AA);
+		phy_stackrestore(0x04AC);
+		phy_stackrestore(0x04A0);
+		phy_stackrestore(0x04A7);
+		if (phy->rev >= 2) {
+			phy_stackrestore(0x04C0);
+			phy_stackrestore(0x04C1);
+		} else
+			phy_stackrestore(0x0406);
+		phy_stackrestore(0x04A1);
+		phy_stackrestore(0x04AB);
+		phy_stackrestore(0x04A8);
+		if (phy->rev == 2) {
+			phy_stackrestore(0x04AD);
+			phy_stackrestore(0x04AE);
+		} else if (phy->rev >= 3) {
+			phy_stackrestore(0x04AD);
+			phy_stackrestore(0x0415);
+			phy_stackrestore(0x0416);
+			phy_stackrestore(0x0417);
+			ilt_stackrestore(0x1A00 + 0x2);
+			ilt_stackrestore(0x1A00 + 0x3);
+		}
+		phy_stackrestore(0x04A2);
+		phy_stackrestore(0x04A8);
+		phy_stackrestore(0x042B);
+		phy_stackrestore(0x048C);
+		tmp32 = b43legacy_shm_read32(dev, B43legacy_SHM_SHARED,
+					     B43legacy_UCODEFLAGS_OFFSET);
+		if (tmp32 & 0x800) {
+			tmp32 &= ~0x800;
+			b43legacy_shm_write32(dev, B43legacy_SHM_SHARED,
+					      B43legacy_UCODEFLAGS_OFFSET,
+					      tmp32);
+		}
+		b43legacy_calc_nrssi_slope(dev);
+		break;
+	default:
+		B43legacy_BUG_ON(1);
+	}
+}
+
+#undef phy_stacksave
+#undef phy_stackrestore
+#undef radio_stacksave
+#undef radio_stackrestore
+#undef ilt_stacksave
+#undef ilt_stackrestore
+
+int b43legacy_radio_set_interference_mitigation(struct b43legacy_wldev *dev,
+						int mode)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+	int currentmode;
+
+	if ((phy->type != B43legacy_PHYTYPE_G) ||
+	    (phy->rev == 0) || (!phy->gmode))
+		return -ENODEV;
+
+	phy->aci_wlan_automatic = 0;
+	switch (mode) {
+	case B43legacy_RADIO_INTERFMODE_AUTOWLAN:
+		phy->aci_wlan_automatic = 1;
+		if (phy->aci_enable)
+			mode = B43legacy_RADIO_INTERFMODE_MANUALWLAN;
+		else
+			mode = B43legacy_RADIO_INTERFMODE_NONE;
+		break;
+	case B43legacy_RADIO_INTERFMODE_NONE:
+	case B43legacy_RADIO_INTERFMODE_NONWLAN:
+	case B43legacy_RADIO_INTERFMODE_MANUALWLAN:
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	currentmode = phy->interfmode;
+	if (currentmode == mode)
+		return 0;
+	if (currentmode != B43legacy_RADIO_INTERFMODE_NONE)
+		b43legacy_radio_interference_mitigation_disable(dev,
+								currentmode);
+
+	if (mode == B43legacy_RADIO_INTERFMODE_NONE) {
+		phy->aci_enable = 0;
+		phy->aci_hw_rssi = 0;
+	} else
+		b43legacy_radio_interference_mitigation_enable(dev, mode);
+	phy->interfmode = mode;
+
+	return 0;
+}
+
+u16 b43legacy_radio_calibrationvalue(struct b43legacy_wldev *dev)
+{
+	u16 reg;
+	u16 index;
+	u16 ret;
+
+	reg = b43legacy_radio_read16(dev, 0x0060);
+	index = (reg & 0x001E) >> 1;
+	ret = rcc_table[index] << 1;
+	ret |= (reg & 0x0001);
+	ret |= 0x0020;
+
+	return ret;
+}
+
+#define LPD(L, P, D)    (((L) << 2) | ((P) << 1) | ((D) << 0))
+static u16 b43legacy_get_812_value(struct b43legacy_wldev *dev, u8 lpd)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+	u16 loop_or = 0;
+	u16 adj_loopback_gain = phy->loopback_gain[0];
+	u8 loop;
+	u16 extern_lna_control;
+
+	if (!phy->gmode)
+		return 0;
+	if (!has_loopback_gain(phy)) {
+		if (phy->rev < 7 || !(dev->dev->bus->sprom.r1.boardflags_lo
+		    & B43legacy_BFL_EXTLNA)) {
+			switch (lpd) {
+			case LPD(0, 1, 1):
+				return 0x0FB2;
+			case LPD(0, 0, 1):
+				return 0x00B2;
+			case LPD(1, 0, 1):
+				return 0x30B2;
+			case LPD(1, 0, 0):
+				return 0x30B3;
+			default:
+				B43legacy_BUG_ON(1);
+			}
+		} else {
+			switch (lpd) {
+			case LPD(0, 1, 1):
+				return 0x8FB2;
+			case LPD(0, 0, 1):
+				return 0x80B2;
+			case LPD(1, 0, 1):
+				return 0x20B2;
+			case LPD(1, 0, 0):
+				return 0x20B3;
+			default:
+				B43legacy_BUG_ON(1);
+			}
+		}
+	} else {
+		if (phy->radio_rev == 8)
+			adj_loopback_gain += 0x003E;
+		else
+			adj_loopback_gain += 0x0026;
+		if (adj_loopback_gain >= 0x46) {
+			adj_loopback_gain -= 0x46;
+			extern_lna_control = 0x3000;
+		} else if (adj_loopback_gain >= 0x3A) {
+			adj_loopback_gain -= 0x3A;
+			extern_lna_control = 0x2000;
+		} else if (adj_loopback_gain >= 0x2E) {
+			adj_loopback_gain -= 0x2E;
+			extern_lna_control = 0x1000;
+		} else {
+			adj_loopback_gain -= 0x10;
+			extern_lna_control = 0x0000;
+		}
+		for (loop = 0; loop < 16; loop++) {
+			u16 tmp = adj_loopback_gain - 6 * loop;
+			if (tmp < 6)
+				break;
+		}
+
+		loop_or = (loop << 8) | extern_lna_control;
+		if (phy->rev >= 7 && dev->dev->bus->sprom.r1.boardflags_lo
+		    & B43legacy_BFL_EXTLNA) {
+			if (extern_lna_control)
+				loop_or |= 0x8000;
+			switch (lpd) {
+			case LPD(0, 1, 1):
+				return 0x8F92;
+			case LPD(0, 0, 1):
+				return (0x8092 | loop_or);
+			case LPD(1, 0, 1):
+				return (0x2092 | loop_or);
+			case LPD(1, 0, 0):
+				return (0x2093 | loop_or);
+			default:
+				B43legacy_BUG_ON(1);
+			}
+		} else {
+			switch (lpd) {
+			case LPD(0, 1, 1):
+				return 0x0F92;
+			case LPD(0, 0, 1):
+			case LPD(1, 0, 1):
+				return (0x0092 | loop_or);
+			case LPD(1, 0, 0):
+				return (0x0093 | loop_or);
+			default:
+				B43legacy_BUG_ON(1);
+			}
+		}
+	}
+	return 0;
+}
+
+u16 b43legacy_radio_init2050(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+	u16 backup[21] = { 0 };
+	u16 ret;
+	u16 i;
+	u16 j;
+	u32 tmp1 = 0;
+	u32 tmp2 = 0;
+
+	backup[0] = b43legacy_radio_read16(dev, 0x0043);
+	backup[14] = b43legacy_radio_read16(dev, 0x0051);
+	backup[15] = b43legacy_radio_read16(dev, 0x0052);
+	backup[1] = b43legacy_phy_read(dev, 0x0015);
+	backup[16] = b43legacy_phy_read(dev, 0x005A);
+	backup[17] = b43legacy_phy_read(dev, 0x0059);
+	backup[18] = b43legacy_phy_read(dev, 0x0058);
+	if (phy->type == B43legacy_PHYTYPE_B) {
+		backup[2] = b43legacy_phy_read(dev, 0x0030);
+		backup[3] = b43legacy_read16(dev, 0x03EC);
+		b43legacy_phy_write(dev, 0x0030, 0x00FF);
+		b43legacy_write16(dev, 0x03EC, 0x3F3F);
+	} else {
+		if (phy->gmode) {
+			backup[4] = b43legacy_phy_read(dev, 0x0811);
+			backup[5] = b43legacy_phy_read(dev, 0x0812);
+			backup[6] = b43legacy_phy_read(dev, 0x0814);
+			backup[7] = b43legacy_phy_read(dev, 0x0815);
+			backup[8] = b43legacy_phy_read(dev,
+						       B43legacy_PHY_G_CRS);
+			backup[9] = b43legacy_phy_read(dev, 0x0802);
+			b43legacy_phy_write(dev, 0x0814,
+					    (b43legacy_phy_read(dev, 0x0814)
+					    | 0x0003));
+			b43legacy_phy_write(dev, 0x0815,
+					    (b43legacy_phy_read(dev, 0x0815)
+					    & 0xFFFC));
+			b43legacy_phy_write(dev, B43legacy_PHY_G_CRS,
+					    (b43legacy_phy_read(dev,
+					    B43legacy_PHY_G_CRS) & 0x7FFF));
+			b43legacy_phy_write(dev, 0x0802,
+					    (b43legacy_phy_read(dev, 0x0802)
+					    & 0xFFFC));
+			if (phy->rev > 1) { /* loopback gain enabled */
+				backup[19] = b43legacy_phy_read(dev, 0x080F);
+				backup[20] = b43legacy_phy_read(dev, 0x0810);
+				if (phy->rev >= 3)
+					b43legacy_phy_write(dev, 0x080F,
+							    0xC020);
+				else
+					b43legacy_phy_write(dev, 0x080F,
+							    0x8020);
+				b43legacy_phy_write(dev, 0x0810, 0x0000);
+			}
+			b43legacy_phy_write(dev, 0x0812,
+					    b43legacy_get_812_value(dev,
+					    LPD(0, 1, 1)));
+			if (phy->rev < 7 ||
+			    !(dev->dev->bus->sprom.r1.boardflags_lo
+			    & B43legacy_BFL_EXTLNA))
+				b43legacy_phy_write(dev, 0x0811, 0x01B3);
+			else
+				b43legacy_phy_write(dev, 0x0811, 0x09B3);
+		}
+	}
+	b43legacy_write16(dev, B43legacy_MMIO_PHY_RADIO,
+			(b43legacy_read16(dev, B43legacy_MMIO_PHY_RADIO)
+					  | 0x8000));
+	backup[10] = b43legacy_phy_read(dev, 0x0035);
+	b43legacy_phy_write(dev, 0x0035,
+			    (b43legacy_phy_read(dev, 0x0035) & 0xFF7F));
+	backup[11] = b43legacy_read16(dev, 0x03E6);
+	backup[12] = b43legacy_read16(dev, B43legacy_MMIO_CHANNEL_EXT);
+
+	/* Initialization */
+	if (phy->analog == 0)
+		b43legacy_write16(dev, 0x03E6, 0x0122);
+	else {
+		if (phy->analog >= 2)
+			b43legacy_phy_write(dev, 0x0003,
+					    (b43legacy_phy_read(dev, 0x0003)
+					    & 0xFFBF) | 0x0040);
+		b43legacy_write16(dev, B43legacy_MMIO_CHANNEL_EXT,
+				  (b43legacy_read16(dev,
+				  B43legacy_MMIO_CHANNEL_EXT) | 0x2000));
+	}
+
+	ret = b43legacy_radio_calibrationvalue(dev);
+
+	if (phy->type == B43legacy_PHYTYPE_B)
+		b43legacy_radio_write16(dev, 0x0078, 0x0026);
+
+	if (phy->gmode)
+		b43legacy_phy_write(dev, 0x0812,
+				    b43legacy_get_812_value(dev,
+				    LPD(0, 1, 1)));
+	b43legacy_phy_write(dev, 0x0015, 0xBFAF);
+	b43legacy_phy_write(dev, 0x002B, 0x1403);
+	if (phy->gmode)
+		b43legacy_phy_write(dev, 0x0812,
+				    b43legacy_get_812_value(dev,
+				    LPD(0, 0, 1)));
+	b43legacy_phy_write(dev, 0x0015, 0xBFA0);
+	b43legacy_radio_write16(dev, 0x0051,
+				(b43legacy_radio_read16(dev, 0x0051)
+				| 0x0004));
+	if (phy->radio_rev == 8)
+		b43legacy_radio_write16(dev, 0x0043, 0x001F);
+	else {
+		b43legacy_radio_write16(dev, 0x0052, 0x0000);
+		b43legacy_radio_write16(dev, 0x0043,
+					(b43legacy_radio_read16(dev, 0x0043)
+					& 0xFFF0) | 0x0009);
+	}
+	b43legacy_phy_write(dev, 0x0058, 0x0000);
+
+	for (i = 0; i < 16; i++) {
+		b43legacy_phy_write(dev, 0x005A, 0x0480);
+		b43legacy_phy_write(dev, 0x0059, 0xC810);
+		b43legacy_phy_write(dev, 0x0058, 0x000D);
+		if (phy->gmode)
+			b43legacy_phy_write(dev, 0x0812,
+					    b43legacy_get_812_value(dev,
+					    LPD(1, 0, 1)));
+		b43legacy_phy_write(dev, 0x0015, 0xAFB0);
+		udelay(10);
+		if (phy->gmode)
+			b43legacy_phy_write(dev, 0x0812,
+					    b43legacy_get_812_value(dev,
+					    LPD(1, 0, 1)));
+		b43legacy_phy_write(dev, 0x0015, 0xEFB0);
+		udelay(10);
+		if (phy->gmode)
+			b43legacy_phy_write(dev, 0x0812,
+					    b43legacy_get_812_value(dev,
+					    LPD(1, 0, 0)));
+		b43legacy_phy_write(dev, 0x0015, 0xFFF0);
+		udelay(20);
+		tmp1 += b43legacy_phy_read(dev, 0x002D);
+		b43legacy_phy_write(dev, 0x0058, 0x0000);
+		if (phy->gmode)
+			b43legacy_phy_write(dev, 0x0812,
+					    b43legacy_get_812_value(dev,
+					    LPD(1, 0, 1)));
+		b43legacy_phy_write(dev, 0x0015, 0xAFB0);
+	}
+
+	tmp1++;
+	tmp1 >>= 9;
+	udelay(10);
+	b43legacy_phy_write(dev, 0x0058, 0x0000);
+
+	for (i = 0; i < 16; i++) {
+		b43legacy_radio_write16(dev, 0x0078, (flip_4bit(i) << 1)
+					| 0x0020);
+		backup[13] = b43legacy_radio_read16(dev, 0x0078);
+		udelay(10);
+		for (j = 0; j < 16; j++) {
+			b43legacy_phy_write(dev, 0x005A, 0x0D80);
+			b43legacy_phy_write(dev, 0x0059, 0xC810);
+			b43legacy_phy_write(dev, 0x0058, 0x000D);
+			if (phy->gmode)
+				b43legacy_phy_write(dev, 0x0812,
+						    b43legacy_get_812_value(dev,
+						    LPD(1, 0, 1)));
+			b43legacy_phy_write(dev, 0x0015, 0xAFB0);
+			udelay(10);
+			if (phy->gmode)
+				b43legacy_phy_write(dev, 0x0812,
+						    b43legacy_get_812_value(dev,
+						    LPD(1, 0, 1)));
+			b43legacy_phy_write(dev, 0x0015, 0xEFB0);
+			udelay(10);
+			if (phy->gmode)
+				b43legacy_phy_write(dev, 0x0812,
+						    b43legacy_get_812_value(dev,
+						    LPD(1, 0, 0)));
+			b43legacy_phy_write(dev, 0x0015, 0xFFF0);
+			udelay(10);
+			tmp2 += b43legacy_phy_read(dev, 0x002D);
+			b43legacy_phy_write(dev, 0x0058, 0x0000);
+			if (phy->gmode)
+				b43legacy_phy_write(dev, 0x0812,
+						    b43legacy_get_812_value(dev,
+						    LPD(1, 0, 1)));
+			b43legacy_phy_write(dev, 0x0015, 0xAFB0);
+		}
+		tmp2++;
+		tmp2 >>= 8;
+		if (tmp1 < tmp2)
+			break;
+	}
+
+	/* Restore the registers */
+	b43legacy_phy_write(dev, 0x0015, backup[1]);
+	b43legacy_radio_write16(dev, 0x0051, backup[14]);
+	b43legacy_radio_write16(dev, 0x0052, backup[15]);
+	b43legacy_radio_write16(dev, 0x0043, backup[0]);
+	b43legacy_phy_write(dev, 0x005A, backup[16]);
+	b43legacy_phy_write(dev, 0x0059, backup[17]);
+	b43legacy_phy_write(dev, 0x0058, backup[18]);
+	b43legacy_write16(dev, 0x03E6, backup[11]);
+	if (phy->analog != 0)
+		b43legacy_write16(dev, B43legacy_MMIO_CHANNEL_EXT, backup[12]);
+	b43legacy_phy_write(dev, 0x0035, backup[10]);
+	b43legacy_radio_selectchannel(dev, phy->channel, 1);
+	if (phy->type == B43legacy_PHYTYPE_B) {
+		b43legacy_phy_write(dev, 0x0030, backup[2]);
+		b43legacy_write16(dev, 0x03EC, backup[3]);
+	} else {
+		if (phy->gmode) {
+			b43legacy_write16(dev, B43legacy_MMIO_PHY_RADIO,
+					  (b43legacy_read16(dev,
+					  B43legacy_MMIO_PHY_RADIO) & 0x7FFF));
+			b43legacy_phy_write(dev, 0x0811, backup[4]);
+			b43legacy_phy_write(dev, 0x0812, backup[5]);
+			b43legacy_phy_write(dev, 0x0814, backup[6]);
+			b43legacy_phy_write(dev, 0x0815, backup[7]);
+			b43legacy_phy_write(dev, B43legacy_PHY_G_CRS,
+					    backup[8]);
+			b43legacy_phy_write(dev, 0x0802, backup[9]);
+			if (phy->rev > 1) {
+				b43legacy_phy_write(dev, 0x080F, backup[19]);
+				b43legacy_phy_write(dev, 0x0810, backup[20]);
+			}
+		}
+	}
+	if (i >= 15)
+		ret = backup[13];
+
+	return ret;
+}
+
+static inline
+u16 freq_r3A_value(u16 frequency)
+{
+	u16 value;
+
+	if (frequency < 5091)
+		value = 0x0040;
+	else if (frequency < 5321)
+		value = 0x0000;
+	else if (frequency < 5806)
+		value = 0x0080;
+	else
+		value = 0x0040;
+
+	return value;
+}
+
+void b43legacy_radio_set_tx_iq(struct b43legacy_wldev *dev)
+{
+	static const u8 data_high[5] = { 0x00, 0x40, 0x80, 0x90, 0xD0 };
+	static const u8 data_low[5]  = { 0x00, 0x01, 0x05, 0x06, 0x0A };
+	u16 tmp = b43legacy_radio_read16(dev, 0x001E);
+	int i;
+	int j;
+
+	for (i = 0; i < 5; i++) {
+		for (j = 0; j < 5; j++) {
+			if (tmp == (data_high[i] | data_low[j])) {
+				b43legacy_phy_write(dev, 0x0069, (i - j) << 8 |
+						    0x00C0);
+				return;
+			}
+		}
+	}
+}
+
+int b43legacy_radio_selectchannel(struct b43legacy_wldev *dev,
+				  u8 channel,
+				  int synthetic_pu_workaround)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+
+	if (channel == 0xFF) {
+		switch (phy->type) {
+		case B43legacy_PHYTYPE_B:
+		case B43legacy_PHYTYPE_G:
+			channel = B43legacy_RADIO_DEFAULT_CHANNEL_BG;
+			break;
+		default:
+			B43legacy_WARN_ON(1);
+		}
+	}
+
+/* TODO: Check if channel is valid - return -EINVAL if not */
+	if (synthetic_pu_workaround)
+		b43legacy_synth_pu_workaround(dev, channel);
+
+	b43legacy_write16(dev, B43legacy_MMIO_CHANNEL,
+			  channel2freq_bg(channel));
+
+	if (channel == 14) {
+		if (dev->dev->bus->sprom.r1.country_code == 5)   /* JAPAN) */
+			b43legacy_shm_write32(dev, B43legacy_SHM_SHARED,
+					      B43legacy_UCODEFLAGS_OFFSET,
+					      b43legacy_shm_read32(dev,
+					      B43legacy_SHM_SHARED,
+					      B43legacy_UCODEFLAGS_OFFSET)
+					      & ~(1 << 7));
+		else
+			b43legacy_shm_write32(dev, B43legacy_SHM_SHARED,
+					      B43legacy_UCODEFLAGS_OFFSET,
+					      b43legacy_shm_read32(dev,
+					      B43legacy_SHM_SHARED,
+					      B43legacy_UCODEFLAGS_OFFSET)
+					      | (1 << 7));
+		b43legacy_write16(dev, B43legacy_MMIO_CHANNEL_EXT,
+				  b43legacy_read16(dev,
+				  B43legacy_MMIO_CHANNEL_EXT) | (1 << 11));
+	} else
+		b43legacy_write16(dev, B43legacy_MMIO_CHANNEL_EXT,
+				  b43legacy_read16(dev,
+				  B43legacy_MMIO_CHANNEL_EXT) & 0xF7BF);
+
+	phy->channel = channel;
+	/*XXX: Using the longer of 2 timeouts (8000 vs 2000 usecs). Specs states
+	 *     that 2000 usecs might suffice. */
+	msleep(8);
+
+	return 0;
+}
+
+void b43legacy_radio_set_txantenna(struct b43legacy_wldev *dev, u32 val)
+{
+	u16 tmp;
+
+	val <<= 8;
+	tmp = b43legacy_shm_read16(dev, B43legacy_SHM_SHARED, 0x0022) & 0xFCFF;
+	b43legacy_shm_write16(dev, B43legacy_SHM_SHARED, 0x0022, tmp | val);
+	tmp = b43legacy_shm_read16(dev, B43legacy_SHM_SHARED, 0x03A8) & 0xFCFF;
+	b43legacy_shm_write16(dev, B43legacy_SHM_SHARED, 0x03A8, tmp | val);
+	tmp = b43legacy_shm_read16(dev, B43legacy_SHM_SHARED, 0x0054) & 0xFCFF;
+	b43legacy_shm_write16(dev, B43legacy_SHM_SHARED, 0x0054, tmp | val);
+}
+
+/* http://bcm-specs.sipsolutions.net/TX_Gain_Base_Band */
+static u16 b43legacy_get_txgain_base_band(u16 txpower)
+{
+	u16 ret;
+
+	B43legacy_WARN_ON(txpower > 63);
+
+	if (txpower >= 54)
+		ret = 2;
+	else if (txpower >= 49)
+		ret = 4;
+	else if (txpower >= 44)
+		ret = 5;
+	else
+		ret = 6;
+
+	return ret;
+}
+
+/* http://bcm-specs.sipsolutions.net/TX_Gain_Radio_Frequency_Power_Amplifier */
+static u16 b43legacy_get_txgain_freq_power_amp(u16 txpower)
+{
+	u16 ret;
+
+	B43legacy_WARN_ON(txpower > 63);
+
+	if (txpower >= 32)
+		ret = 0;
+	else if (txpower >= 25)
+		ret = 1;
+	else if (txpower >= 20)
+		ret = 2;
+	else if (txpower >= 12)
+		ret = 3;
+	else
+		ret = 4;
+
+	return ret;
+}
+
+/* http://bcm-specs.sipsolutions.net/TX_Gain_Digital_Analog_Converter */
+static u16 b43legacy_get_txgain_dac(u16 txpower)
+{
+	u16 ret;
+
+	B43legacy_WARN_ON(txpower > 63);
+
+	if (txpower >= 54)
+		ret = txpower - 53;
+	else if (txpower >= 49)
+		ret = txpower - 42;
+	else if (txpower >= 44)
+		ret = txpower - 37;
+	else if (txpower >= 32)
+		ret = txpower - 32;
+	else if (txpower >= 25)
+		ret = txpower - 20;
+	else if (txpower >= 20)
+		ret = txpower - 13;
+	else if (txpower >= 12)
+		ret = txpower - 8;
+	else
+		ret = txpower;
+
+	return ret;
+}
+
+void b43legacy_radio_set_txpower_a(struct b43legacy_wldev *dev, u16 txpower)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+	u16 pamp;
+	u16 base;
+	u16 dac;
+	u16 ilt;
+
+	txpower = limit_value(txpower, 0, 63);
+
+	pamp = b43legacy_get_txgain_freq_power_amp(txpower);
+	pamp <<= 5;
+	pamp &= 0x00E0;
+	b43legacy_phy_write(dev, 0x0019, pamp);
+
+	base = b43legacy_get_txgain_base_band(txpower);
+	base &= 0x000F;
+	b43legacy_phy_write(dev, 0x0017, base | 0x0020);
+
+	ilt = b43legacy_ilt_read(dev, 0x3001);
+	ilt &= 0x0007;
+
+	dac = b43legacy_get_txgain_dac(txpower);
+	dac <<= 3;
+	dac |= ilt;
+
+	b43legacy_ilt_write(dev, 0x3001, dac);
+
+	phy->txpwr_offset = txpower;
+
+	/* TODO: FuncPlaceholder (Adjust BB loft cancel) */
+}
+
+void b43legacy_radio_set_txpower_bg(struct b43legacy_wldev *dev,
+				    u16 baseband_attenuation,
+				    u16 radio_attenuation,
+				    u16 txpower)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+
+	if (baseband_attenuation == 0xFFFF)
+		baseband_attenuation = phy->bbatt;
+	if (radio_attenuation == 0xFFFF)
+		radio_attenuation = phy->rfatt;
+	if (txpower == 0xFFFF)
+		txpower = phy->txctl1;
+	phy->bbatt = baseband_attenuation;
+	phy->rfatt = radio_attenuation;
+	phy->txctl1 = txpower;
+
+	B43legacy_WARN_ON(baseband_attenuation > 11);
+	if (phy->radio_rev < 6)
+		B43legacy_WARN_ON(radio_attenuation > 9);
+	else
+		B43legacy_WARN_ON(radio_attenuation > 31);
+	B43legacy_WARN_ON(txpower > 7);
+
+	b43legacy_phy_set_baseband_attenuation(dev, baseband_attenuation);
+	b43legacy_radio_write16(dev, 0x0043, radio_attenuation);
+	b43legacy_shm_write16(dev, B43legacy_SHM_SHARED, 0x0064,
+			      radio_attenuation);
+	if (phy->radio_ver == 0x2050)
+		b43legacy_radio_write16(dev, 0x0052,
+					(b43legacy_radio_read16(dev, 0x0052)
+					& ~0x0070) | ((txpower << 4) & 0x0070));
+	/* FIXME: The spec is very weird and unclear here. */
+	if (phy->type == B43legacy_PHYTYPE_G)
+		b43legacy_phy_lo_adjust(dev, 0);
+}
+
+u16 b43legacy_default_baseband_attenuation(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+
+	if (phy->radio_ver == 0x2050 && phy->radio_rev < 6)
+		return 0;
+	return 2;
+}
+
+u16 b43legacy_default_radio_attenuation(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+	u16 att = 0xFFFF;
+
+	switch (phy->radio_ver) {
+	case 0x2053:
+		switch (phy->radio_rev) {
+		case 1:
+			att = 6;
+			break;
+		}
+		break;
+	case 0x2050:
+		switch (phy->radio_rev) {
+		case 0:
+			att = 5;
+			break;
+		case 1:
+			if (phy->type == B43legacy_PHYTYPE_G) {
+				if (is_bcm_board_vendor(dev) &&
+				    dev->dev->bus->boardinfo.type == 0x421 &&
+				    dev->dev->bus->boardinfo.rev >= 30)
+					att = 3;
+				else if (is_bcm_board_vendor(dev) &&
+					 dev->dev->bus->boardinfo.type == 0x416)
+					att = 3;
+				else
+					att = 1;
+			} else {
+				if (is_bcm_board_vendor(dev) &&
+				    dev->dev->bus->boardinfo.type == 0x421 &&
+				    dev->dev->bus->boardinfo.rev >= 30)
+					att = 7;
+				else
+					att = 6;
+			}
+			break;
+		case 2:
+			if (phy->type == B43legacy_PHYTYPE_G) {
+				if (is_bcm_board_vendor(dev) &&
+				    dev->dev->bus->boardinfo.type == 0x421 &&
+				    dev->dev->bus->boardinfo.rev >= 30)
+					att = 3;
+				else if (is_bcm_board_vendor(dev) &&
+					 dev->dev->bus->boardinfo.type ==
+					 0x416)
+					att = 5;
+				else if (dev->dev->bus->chip_id == 0x4320)
+					att = 4;
+				else
+					att = 3;
+			} else
+				att = 6;
+			break;
+		case 3:
+			att = 5;
+			break;
+		case 4:
+		case 5:
+			att = 1;
+			break;
+		case 6:
+		case 7:
+			att = 5;
+			break;
+		case 8:
+			att = 0x1A;
+			break;
+		case 9:
+		default:
+			att = 5;
+		}
+	}
+	if (is_bcm_board_vendor(dev) &&
+	    dev->dev->bus->boardinfo.type == 0x421) {
+		if (dev->dev->bus->boardinfo.rev < 0x43)
+			att = 2;
+		else if (dev->dev->bus->boardinfo.rev < 0x51)
+			att = 3;
+	}
+	if (att == 0xFFFF)
+		att = 5;
+
+	return att;
+}
+
+u16 b43legacy_default_txctl1(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+
+	if (phy->radio_ver != 0x2050)
+		return 0;
+	if (phy->radio_rev == 1)
+		return 3;
+	if (phy->radio_rev < 6)
+		return 2;
+	if (phy->radio_rev == 8)
+		return 1;
+	return 0;
+}
+
+void b43legacy_radio_turn_on(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+	int err;
+	u8 channel;
+
+	might_sleep();
+
+	if (phy->radio_on)
+		return;
+
+	switch (phy->type) {
+	case B43legacy_PHYTYPE_B:
+	case B43legacy_PHYTYPE_G:
+		b43legacy_phy_write(dev, 0x0015, 0x8000);
+		b43legacy_phy_write(dev, 0x0015, 0xCC00);
+		b43legacy_phy_write(dev, 0x0015,
+				    (phy->gmode ? 0x00C0 : 0x0000));
+		if (phy->radio_off_context.valid) {
+			/* Restore the RFover values. */
+			b43legacy_phy_write(dev, B43legacy_PHY_RFOVER,
+					    phy->radio_off_context.rfover);
+			b43legacy_phy_write(dev, B43legacy_PHY_RFOVERVAL,
+					    phy->radio_off_context.rfoverval);
+			phy->radio_off_context.valid = 0;
+		}
+		channel = phy->channel;
+		err = b43legacy_radio_selectchannel(dev,
+					B43legacy_RADIO_DEFAULT_CHANNEL_BG, 1);
+		err |= b43legacy_radio_selectchannel(dev, channel, 0);
+		B43legacy_WARN_ON(err);
+		break;
+	default:
+		B43legacy_BUG_ON(1);
+	}
+	phy->radio_on = 1;
+	b43legacy_leds_update(dev, 0);
+}
+
+void b43legacy_radio_turn_off(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+
+	if (phy->type == B43legacy_PHYTYPE_G && dev->dev->id.revision >= 5) {
+		u16 rfover, rfoverval;
+
+		rfover = b43legacy_phy_read(dev, B43legacy_PHY_RFOVER);
+		rfoverval = b43legacy_phy_read(dev, B43legacy_PHY_RFOVERVAL);
+		phy->radio_off_context.rfover = rfover;
+		phy->radio_off_context.rfoverval = rfoverval;
+		phy->radio_off_context.valid = 1;
+		b43legacy_phy_write(dev, B43legacy_PHY_RFOVER, rfover | 0x008C);
+		b43legacy_phy_write(dev, B43legacy_PHY_RFOVERVAL,
+				    rfoverval & 0xFF73);
+	} else
+		b43legacy_phy_write(dev, 0x0015, 0xAA00);
+	phy->radio_on = 0;
+	b43legacydbg(dev->wl, "Radio initialized\n");
+	b43legacy_leds_update(dev, 0);
+}
+
+void b43legacy_radio_clear_tssi(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+
+	switch (phy->type) {
+	case B43legacy_PHYTYPE_B:
+	case B43legacy_PHYTYPE_G:
+		b43legacy_shm_write16(dev, B43legacy_SHM_SHARED, 0x0058,
+				      0x7F7F);
+		b43legacy_shm_write16(dev, B43legacy_SHM_SHARED, 0x005a,
+				      0x7F7F);
+		b43legacy_shm_write16(dev, B43legacy_SHM_SHARED, 0x0070,
+				      0x7F7F);
+		b43legacy_shm_write16(dev, B43legacy_SHM_SHARED, 0x0072,
+				      0x7F7F);
+		break;
+	}
+}
diff --git a/drivers/net/wireless/b43legacy/radio.h b/drivers/net/wireless/b43legacy/radio.h
new file mode 100644
index 000000000000..6c6a203439e1
--- /dev/null
+++ b/drivers/net/wireless/b43legacy/radio.h
@@ -0,0 +1,98 @@
+/*
+
+  Broadcom B43legacy wireless driver
+
+  Copyright (c) 2005 Martin Langer <martin-langer@gmx.de>,
+		     Stefano Brivio <st3@riseup.net>
+		     Michael Buesch <mbuesch@freenet.de>
+		     Danny van Dyk <kugelfang@gentoo.org>
+		     Andreas Jaggi <andreas.jaggi@waterwave.ch>
+
+  Some parts of the code in this file are derived from the ipw2200
+  driver  Copyright(c) 2003 - 2004 Intel Corporation.
+
+  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, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with this program; see the file COPYING.  If not, write to
+  the Free Software Foundation, Inc., 51 Franklin Steet, Fifth Floor,
+  Boston, MA 02110-1301, USA.
+
+*/
+
+#ifndef B43legacy_RADIO_H_
+#define B43legacy_RADIO_H_
+
+#include "b43legacy.h"
+
+
+#define B43legacy_RADIO_DEFAULT_CHANNEL_BG	6
+
+/* Force antenna 0. */
+#define B43legacy_RADIO_TXANTENNA_0		0
+/* Force antenna 1. */
+#define B43legacy_RADIO_TXANTENNA_1		1
+/* Use the RX antenna, that was selected for the most recently
+ * received good PLCP header.
+ */
+#define B43legacy_RADIO_TXANTENNA_LASTPLCP	3
+#define B43legacy_RADIO_TXANTENNA_DEFAULT	B43legacy_RADIO_TXANTENNA_LASTPLCP
+
+#define B43legacy_RADIO_INTERFMODE_NONE		0
+#define B43legacy_RADIO_INTERFMODE_NONWLAN	1
+#define B43legacy_RADIO_INTERFMODE_MANUALWLAN	2
+#define B43legacy_RADIO_INTERFMODE_AUTOWLAN	3
+
+
+void b43legacy_radio_lock(struct b43legacy_wldev *dev);
+void b43legacy_radio_unlock(struct b43legacy_wldev *dev);
+
+u16 b43legacy_radio_read16(struct b43legacy_wldev *dev, u16 offset);
+void b43legacy_radio_write16(struct b43legacy_wldev *dev, u16 offset, u16 val);
+
+u16 b43legacy_radio_init2050(struct b43legacy_wldev *dev);
+
+void b43legacy_radio_turn_on(struct b43legacy_wldev *dev);
+void b43legacy_radio_turn_off(struct b43legacy_wldev *dev);
+
+int b43legacy_radio_selectchannel(struct b43legacy_wldev *dev, u8 channel,
+				  int synthetic_pu_workaround);
+
+void b43legacy_radio_set_txpower_a(struct b43legacy_wldev *dev, u16 txpower);
+void b43legacy_radio_set_txpower_bg(struct b43legacy_wldev *dev,
+				    u16 baseband_attenuation, u16 attenuation,
+				    u16 txpower);
+
+u16 b43legacy_default_baseband_attenuation(struct b43legacy_wldev *dev);
+u16 b43legacy_default_radio_attenuation(struct b43legacy_wldev *dev);
+u16 b43legacy_default_txctl1(struct b43legacy_wldev *dev);
+
+void b43legacy_radio_set_txantenna(struct b43legacy_wldev *dev, u32 val);
+
+void b43legacy_radio_clear_tssi(struct b43legacy_wldev *dev);
+
+u8 b43legacy_radio_aci_detect(struct b43legacy_wldev *dev, u8 channel);
+u8 b43legacy_radio_aci_scan(struct b43legacy_wldev *dev);
+
+int b43legacy_radio_set_interference_mitigation(struct b43legacy_wldev *dev,
+						int mode);
+
+void b43legacy_calc_nrssi_slope(struct b43legacy_wldev *dev);
+void b43legacy_calc_nrssi_threshold(struct b43legacy_wldev *dev);
+s16 b43legacy_nrssi_hw_read(struct b43legacy_wldev *dev, u16 offset);
+void b43legacy_nrssi_hw_write(struct b43legacy_wldev *dev, u16 offset, s16 val);
+void b43legacy_nrssi_hw_update(struct b43legacy_wldev *dev, u16 val);
+void b43legacy_nrssi_mem_update(struct b43legacy_wldev *dev);
+
+void b43legacy_radio_set_tx_iq(struct b43legacy_wldev *dev);
+u16 b43legacy_radio_calibrationvalue(struct b43legacy_wldev *dev);
+
+#endif /* B43legacy_RADIO_H_ */
diff --git a/drivers/net/wireless/b43legacy/sysfs.c b/drivers/net/wireless/b43legacy/sysfs.c
new file mode 100644
index 000000000000..56c384fa9b1f
--- /dev/null
+++ b/drivers/net/wireless/b43legacy/sysfs.c
@@ -0,0 +1,238 @@
+/*
+
+  Broadcom B43legacy wireless driver
+
+  SYSFS support routines
+
+  Copyright (c) 2006 Michael Buesch <mb@bu3sch.de>
+
+  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, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with this program; see the file COPYING.  If not, write to
+  the Free Software Foundation, Inc., 51 Franklin Steet, Fifth Floor,
+  Boston, MA 02110-1301, USA.
+
+*/
+
+#include "sysfs.h"
+#include "b43legacy.h"
+#include "main.h"
+#include "phy.h"
+#include "radio.h"
+
+#include <linux/capability.h>
+
+
+#define GENERIC_FILESIZE	64
+
+
+static int get_integer(const char *buf, size_t count)
+{
+	char tmp[10 + 1] = { 0 };
+	int ret = -EINVAL;
+
+	if (count == 0)
+		goto out;
+	count = min(count, (size_t)10);
+	memcpy(tmp, buf, count);
+	ret = simple_strtol(tmp, NULL, 10);
+out:
+	return ret;
+}
+
+static int get_boolean(const char *buf, size_t count)
+{
+	if (count != 0) {
+		if (buf[0] == '1')
+			return 1;
+		if (buf[0] == '0')
+			return 0;
+		if (count >= 4 && memcmp(buf, "true", 4) == 0)
+			return 1;
+		if (count >= 5 && memcmp(buf, "false", 5) == 0)
+			return 0;
+		if (count >= 3 && memcmp(buf, "yes", 3) == 0)
+			return 1;
+		if (count >= 2 && memcmp(buf, "no", 2) == 0)
+			return 0;
+		if (count >= 2 && memcmp(buf, "on", 2) == 0)
+			return 1;
+		if (count >= 3 && memcmp(buf, "off", 3) == 0)
+			return 0;
+	}
+	return -EINVAL;
+}
+
+static ssize_t b43legacy_attr_interfmode_show(struct device *dev,
+					      struct device_attribute *attr,
+					      char *buf)
+{
+	struct b43legacy_wldev *wldev = dev_to_b43legacy_wldev(dev);
+	ssize_t count = 0;
+
+	if (!capable(CAP_NET_ADMIN))
+		return -EPERM;
+
+	mutex_lock(&wldev->wl->mutex);
+
+	switch (wldev->phy.interfmode) {
+	case B43legacy_INTERFMODE_NONE:
+		count = snprintf(buf, PAGE_SIZE, "0 (No Interference"
+				 " Mitigation)\n");
+		break;
+	case B43legacy_INTERFMODE_NONWLAN:
+		count = snprintf(buf, PAGE_SIZE, "1 (Non-WLAN Interference"
+				 " Mitigation)\n");
+		break;
+	case B43legacy_INTERFMODE_MANUALWLAN:
+		count = snprintf(buf, PAGE_SIZE, "2 (WLAN Interference"
+				 " Mitigation)\n");
+		break;
+	default:
+		B43legacy_WARN_ON(1);
+	}
+
+	mutex_unlock(&wldev->wl->mutex);
+
+	return count;
+}
+
+static ssize_t b43legacy_attr_interfmode_store(struct device *dev,
+					       struct device_attribute *attr,
+					       const char *buf, size_t count)
+{
+	struct b43legacy_wldev *wldev = dev_to_b43legacy_wldev(dev);
+	unsigned long flags;
+	int err;
+	int mode;
+
+	if (!capable(CAP_NET_ADMIN))
+		return -EPERM;
+
+	mode = get_integer(buf, count);
+	switch (mode) {
+	case 0:
+		mode = B43legacy_INTERFMODE_NONE;
+		break;
+	case 1:
+		mode = B43legacy_INTERFMODE_NONWLAN;
+		break;
+	case 2:
+		mode = B43legacy_INTERFMODE_MANUALWLAN;
+		break;
+	case 3:
+		mode = B43legacy_INTERFMODE_AUTOWLAN;
+		break;
+	default:
+		return -EINVAL;
+	}
+
+	mutex_lock(&wldev->wl->mutex);
+	spin_lock_irqsave(&wldev->wl->irq_lock, flags);
+
+	err = b43legacy_radio_set_interference_mitigation(wldev, mode);
+	if (err)
+		b43legacyerr(wldev->wl, "Interference Mitigation not "
+		       "supported by device\n");
+	mmiowb();
+	spin_unlock_irqrestore(&wldev->wl->irq_lock, flags);
+	mutex_unlock(&wldev->wl->mutex);
+
+	return err ? err : count;
+}
+
+static DEVICE_ATTR(interference, 0644,
+		   b43legacy_attr_interfmode_show,
+		   b43legacy_attr_interfmode_store);
+
+static ssize_t b43legacy_attr_preamble_show(struct device *dev,
+					    struct device_attribute *attr,
+					    char *buf)
+{
+	struct b43legacy_wldev *wldev = dev_to_b43legacy_wldev(dev);
+	ssize_t count;
+
+	if (!capable(CAP_NET_ADMIN))
+		return -EPERM;
+
+	mutex_lock(&wldev->wl->mutex);
+
+	if (wldev->short_preamble)
+		count = snprintf(buf, PAGE_SIZE, "1 (Short Preamble"
+				 " enabled)\n");
+	else
+		count = snprintf(buf, PAGE_SIZE, "0 (Short Preamble"
+				 " disabled)\n");
+
+	mutex_unlock(&wldev->wl->mutex);
+
+	return count;
+}
+
+static ssize_t b43legacy_attr_preamble_store(struct device *dev,
+					     struct device_attribute *attr,
+					     const char *buf, size_t count)
+{
+	struct b43legacy_wldev *wldev = dev_to_b43legacy_wldev(dev);
+	unsigned long flags;
+	int value;
+
+	if (!capable(CAP_NET_ADMIN))
+		return -EPERM;
+
+	value = get_boolean(buf, count);
+	if (value < 0)
+		return value;
+	mutex_lock(&wldev->wl->mutex);
+	spin_lock_irqsave(&wldev->wl->irq_lock, flags);
+
+	wldev->short_preamble = !!value;
+
+	spin_unlock_irqrestore(&wldev->wl->irq_lock, flags);
+	mutex_unlock(&wldev->wl->mutex);
+
+	return count;
+}
+
+static DEVICE_ATTR(shortpreamble, 0644,
+		   b43legacy_attr_preamble_show,
+		   b43legacy_attr_preamble_store);
+
+int b43legacy_sysfs_register(struct b43legacy_wldev *wldev)
+{
+	struct device *dev = wldev->dev->dev;
+	int err;
+
+	B43legacy_WARN_ON(b43legacy_status(wldev) !=
+			  B43legacy_STAT_INITIALIZED);
+
+	err = device_create_file(dev, &dev_attr_interference);
+	if (err)
+		goto out;
+	err = device_create_file(dev, &dev_attr_shortpreamble);
+	if (err)
+		goto err_remove_interfmode;
+
+out:
+	return err;
+err_remove_interfmode:
+	device_remove_file(dev, &dev_attr_interference);
+	goto out;
+}
+
+void b43legacy_sysfs_unregister(struct b43legacy_wldev *wldev)
+{
+	struct device *dev = wldev->dev->dev;
+
+	device_remove_file(dev, &dev_attr_shortpreamble);
+	device_remove_file(dev, &dev_attr_interference);
+}
diff --git a/drivers/net/wireless/b43legacy/sysfs.h b/drivers/net/wireless/b43legacy/sysfs.h
new file mode 100644
index 000000000000..417d509803c7
--- /dev/null
+++ b/drivers/net/wireless/b43legacy/sysfs.h
@@ -0,0 +1,9 @@
+#ifndef B43legacy_SYSFS_H_
+#define B43legacy_SYSFS_H_
+
+struct b43legacy_wldev;
+
+int b43legacy_sysfs_register(struct b43legacy_wldev *dev);
+void b43legacy_sysfs_unregister(struct b43legacy_wldev *dev);
+
+#endif /* B43legacy_SYSFS_H_ */
diff --git a/drivers/net/wireless/b43legacy/xmit.c b/drivers/net/wireless/b43legacy/xmit.c
new file mode 100644
index 000000000000..fa1e65687a63
--- /dev/null
+++ b/drivers/net/wireless/b43legacy/xmit.c
@@ -0,0 +1,642 @@
+/*
+
+  Broadcom B43legacy wireless driver
+
+  Transmission (TX/RX) related functions.
+
+  Copyright (C) 2005 Martin Langer <martin-langer@gmx.de>
+  Copyright (C) 2005 Stefano Brivio <st3@riseup.net>
+  Copyright (C) 2005, 2006 Michael Buesch <mb@bu3sch.de>
+  Copyright (C) 2005 Danny van Dyk <kugelfang@gentoo.org>
+  Copyright (C) 2005 Andreas Jaggi <andreas.jaggi@waterwave.ch>
+  Copyright (C) 2007 Larry Finger <Larry.Finger@lwfinger.net>
+
+  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, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with this program; see the file COPYING.  If not, write to
+  the Free Software Foundation, Inc., 51 Franklin Steet, Fifth Floor,
+  Boston, MA 02110-1301, USA.
+
+*/
+
+#include <net/dst.h>
+
+#include "xmit.h"
+#include "phy.h"
+#include "dma.h"
+#include "pio.h"
+
+
+/* Extract the bitrate out of a CCK PLCP header. */
+static u8 b43legacy_plcp_get_bitrate_cck(struct b43legacy_plcp_hdr6 *plcp)
+{
+	switch (plcp->raw[0]) {
+	case 0x0A:
+		return B43legacy_CCK_RATE_1MB;
+	case 0x14:
+		return B43legacy_CCK_RATE_2MB;
+	case 0x37:
+		return B43legacy_CCK_RATE_5MB;
+	case 0x6E:
+		return B43legacy_CCK_RATE_11MB;
+	}
+	B43legacy_BUG_ON(1);
+	return 0;
+}
+
+/* Extract the bitrate out of an OFDM PLCP header. */
+static u8 b43legacy_plcp_get_bitrate_ofdm(struct b43legacy_plcp_hdr6 *plcp)
+{
+	switch (plcp->raw[0] & 0xF) {
+	case 0xB:
+		return B43legacy_OFDM_RATE_6MB;
+	case 0xF:
+		return B43legacy_OFDM_RATE_9MB;
+	case 0xA:
+		return B43legacy_OFDM_RATE_12MB;
+	case 0xE:
+		return B43legacy_OFDM_RATE_18MB;
+	case 0x9:
+		return B43legacy_OFDM_RATE_24MB;
+	case 0xD:
+		return B43legacy_OFDM_RATE_36MB;
+	case 0x8:
+		return B43legacy_OFDM_RATE_48MB;
+	case 0xC:
+		return B43legacy_OFDM_RATE_54MB;
+	}
+	B43legacy_BUG_ON(1);
+	return 0;
+}
+
+u8 b43legacy_plcp_get_ratecode_cck(const u8 bitrate)
+{
+	switch (bitrate) {
+	case B43legacy_CCK_RATE_1MB:
+		return 0x0A;
+	case B43legacy_CCK_RATE_2MB:
+		return 0x14;
+	case B43legacy_CCK_RATE_5MB:
+		return 0x37;
+	case B43legacy_CCK_RATE_11MB:
+		return 0x6E;
+	}
+	B43legacy_BUG_ON(1);
+	return 0;
+}
+
+u8 b43legacy_plcp_get_ratecode_ofdm(const u8 bitrate)
+{
+	switch (bitrate) {
+	case B43legacy_OFDM_RATE_6MB:
+		return 0xB;
+	case B43legacy_OFDM_RATE_9MB:
+		return 0xF;
+	case B43legacy_OFDM_RATE_12MB:
+		return 0xA;
+	case B43legacy_OFDM_RATE_18MB:
+		return 0xE;
+	case B43legacy_OFDM_RATE_24MB:
+		return 0x9;
+	case B43legacy_OFDM_RATE_36MB:
+		return 0xD;
+	case B43legacy_OFDM_RATE_48MB:
+		return 0x8;
+	case B43legacy_OFDM_RATE_54MB:
+		return 0xC;
+	}
+	B43legacy_BUG_ON(1);
+	return 0;
+}
+
+void b43legacy_generate_plcp_hdr(struct b43legacy_plcp_hdr4 *plcp,
+				 const u16 octets, const u8 bitrate)
+{
+	__le32 *data = &(plcp->data);
+	__u8 *raw = plcp->raw;
+
+	if (b43legacy_is_ofdm_rate(bitrate)) {
+		u16 d;
+
+		d = b43legacy_plcp_get_ratecode_ofdm(bitrate);
+		B43legacy_WARN_ON(octets & 0xF000);
+		d |= (octets << 5);
+		*data = cpu_to_le32(d);
+	} else {
+		u32 plen;
+
+		plen = octets * 16 / bitrate;
+		if ((octets * 16 % bitrate) > 0) {
+			plen++;
+			if ((bitrate == B43legacy_CCK_RATE_11MB)
+			    && ((octets * 8 % 11) < 4))
+				raw[1] = 0x84;
+			else
+				raw[1] = 0x04;
+		} else
+			raw[1] = 0x04;
+		*data |= cpu_to_le32(plen << 16);
+		raw[0] = b43legacy_plcp_get_ratecode_cck(bitrate);
+	}
+}
+
+static u8 b43legacy_calc_fallback_rate(u8 bitrate)
+{
+	switch (bitrate) {
+	case B43legacy_CCK_RATE_1MB:
+		return B43legacy_CCK_RATE_1MB;
+	case B43legacy_CCK_RATE_2MB:
+		return B43legacy_CCK_RATE_1MB;
+	case B43legacy_CCK_RATE_5MB:
+		return B43legacy_CCK_RATE_2MB;
+	case B43legacy_CCK_RATE_11MB:
+		return B43legacy_CCK_RATE_5MB;
+	case B43legacy_OFDM_RATE_6MB:
+		return B43legacy_CCK_RATE_5MB;
+	case B43legacy_OFDM_RATE_9MB:
+		return B43legacy_OFDM_RATE_6MB;
+	case B43legacy_OFDM_RATE_12MB:
+		return B43legacy_OFDM_RATE_9MB;
+	case B43legacy_OFDM_RATE_18MB:
+		return B43legacy_OFDM_RATE_12MB;
+	case B43legacy_OFDM_RATE_24MB:
+		return B43legacy_OFDM_RATE_18MB;
+	case B43legacy_OFDM_RATE_36MB:
+		return B43legacy_OFDM_RATE_24MB;
+	case B43legacy_OFDM_RATE_48MB:
+		return B43legacy_OFDM_RATE_36MB;
+	case B43legacy_OFDM_RATE_54MB:
+		return B43legacy_OFDM_RATE_48MB;
+	}
+	B43legacy_BUG_ON(1);
+	return 0;
+}
+
+static void generate_txhdr_fw3(struct b43legacy_wldev *dev,
+			       struct b43legacy_txhdr_fw3 *txhdr,
+			       const unsigned char *fragment_data,
+			       unsigned int fragment_len,
+			       const struct ieee80211_tx_control *txctl,
+			       u16 cookie)
+{
+	const struct ieee80211_hdr *wlhdr;
+	int use_encryption = (!(txctl->flags & IEEE80211_TXCTL_DO_NOT_ENCRYPT));
+	u16 fctl;
+	u8 rate;
+	u8 rate_fb;
+	int rate_ofdm;
+	int rate_fb_ofdm;
+	unsigned int plcp_fragment_len;
+	u32 mac_ctl = 0;
+	u16 phy_ctl = 0;
+
+	wlhdr = (const struct ieee80211_hdr *)fragment_data;
+	fctl = le16_to_cpu(wlhdr->frame_control);
+
+	memset(txhdr, 0, sizeof(*txhdr));
+
+	rate = txctl->tx_rate;
+	rate_ofdm = b43legacy_is_ofdm_rate(rate);
+	rate_fb = (txctl->alt_retry_rate == -1) ? rate : txctl->alt_retry_rate;
+	rate_fb_ofdm = b43legacy_is_ofdm_rate(rate_fb);
+
+	txhdr->mac_frame_ctl = wlhdr->frame_control;
+	memcpy(txhdr->tx_receiver, wlhdr->addr1, 6);
+
+	/* Calculate duration for fallback rate */
+	if ((rate_fb == rate) ||
+	    (wlhdr->duration_id & cpu_to_le16(0x8000)) ||
+	    (wlhdr->duration_id == cpu_to_le16(0))) {
+		/* If the fallback rate equals the normal rate or the
+		 * dur_id field contains an AID, CFP magic or 0,
+		 * use the original dur_id field. */
+		txhdr->dur_fb = wlhdr->duration_id;
+	} else {
+		int fbrate_base100kbps = B43legacy_RATE_TO_100KBPS(rate_fb);
+		txhdr->dur_fb = ieee80211_generic_frame_duration(dev->wl->hw,
+							 dev->wl->if_id,
+							 fragment_len,
+							 fbrate_base100kbps);
+	}
+
+	plcp_fragment_len = fragment_len + FCS_LEN;
+	if (use_encryption) {
+		u8 key_idx = (u16)(txctl->key_idx);
+		struct b43legacy_key *key;
+		int wlhdr_len;
+		size_t iv_len;
+
+		B43legacy_WARN_ON(key_idx >= dev->max_nr_keys);
+		key = &(dev->key[key_idx]);
+
+		if (key->enabled) {
+			/* Hardware appends ICV. */
+			plcp_fragment_len += txctl->icv_len;
+
+			key_idx = b43legacy_kidx_to_fw(dev, key_idx);
+			mac_ctl |= (key_idx << B43legacy_TX4_MAC_KEYIDX_SHIFT) &
+				   B43legacy_TX4_MAC_KEYIDX;
+			mac_ctl |= (key->algorithm <<
+				   B43legacy_TX4_MAC_KEYALG_SHIFT) &
+				   B43legacy_TX4_MAC_KEYALG;
+			wlhdr_len = ieee80211_get_hdrlen(fctl);
+			iv_len = min((size_t)txctl->iv_len,
+				     ARRAY_SIZE(txhdr->iv));
+			memcpy(txhdr->iv, ((u8 *)wlhdr) + wlhdr_len, iv_len);
+		}
+	}
+	b43legacy_generate_plcp_hdr((struct b43legacy_plcp_hdr4 *)
+				    (&txhdr->plcp), plcp_fragment_len,
+				    rate);
+	b43legacy_generate_plcp_hdr((struct b43legacy_plcp_hdr4 *)
+				    (&txhdr->plcp_fb), plcp_fragment_len,
+				    rate_fb);
+
+	/* PHY TX Control word */
+	if (rate_ofdm)
+		phy_ctl |= B43legacy_TX4_PHY_OFDM;
+	if (dev->short_preamble)
+		phy_ctl |= B43legacy_TX4_PHY_SHORTPRMBL;
+	switch (txctl->antenna_sel_tx) {
+	case 0:
+		phy_ctl |= B43legacy_TX4_PHY_ANTLAST;
+		break;
+	case 1:
+		phy_ctl |= B43legacy_TX4_PHY_ANT0;
+		break;
+	case 2:
+		phy_ctl |= B43legacy_TX4_PHY_ANT1;
+		break;
+	default:
+		B43legacy_BUG_ON(1);
+	}
+
+	/* MAC control */
+	if (!(txctl->flags & IEEE80211_TXCTL_NO_ACK))
+		mac_ctl |= B43legacy_TX4_MAC_ACK;
+	if (!(((fctl & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_CTL) &&
+	      ((fctl & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_PSPOLL)))
+		mac_ctl |= B43legacy_TX4_MAC_HWSEQ;
+	if (txctl->flags & IEEE80211_TXCTL_FIRST_FRAGMENT)
+		mac_ctl |= B43legacy_TX4_MAC_STMSDU;
+	if (rate_fb_ofdm)
+		mac_ctl |= B43legacy_TX4_MAC_FALLBACKOFDM;
+
+	/* Generate the RTS or CTS-to-self frame */
+	if ((txctl->flags & IEEE80211_TXCTL_USE_RTS_CTS) ||
+	    (txctl->flags & IEEE80211_TXCTL_USE_CTS_PROTECT)) {
+		unsigned int len;
+		struct ieee80211_hdr *hdr;
+		int rts_rate;
+		int rts_rate_fb;
+		int rts_rate_ofdm;
+		int rts_rate_fb_ofdm;
+
+		rts_rate = txctl->rts_cts_rate;
+		rts_rate_ofdm = b43legacy_is_ofdm_rate(rts_rate);
+		rts_rate_fb = b43legacy_calc_fallback_rate(rts_rate);
+		rts_rate_fb_ofdm = b43legacy_is_ofdm_rate(rts_rate_fb);
+		if (rts_rate_fb_ofdm)
+			mac_ctl |= B43legacy_TX4_MAC_CTSFALLBACKOFDM;
+
+		if (txctl->flags & IEEE80211_TXCTL_USE_CTS_PROTECT) {
+			ieee80211_ctstoself_get(dev->wl->hw,
+						dev->wl->if_id,
+						fragment_data,
+						fragment_len, txctl,
+						(struct ieee80211_cts *)
+						(txhdr->rts_frame));
+			mac_ctl |= B43legacy_TX4_MAC_SENDCTS;
+			len = sizeof(struct ieee80211_cts);
+		} else {
+			ieee80211_rts_get(dev->wl->hw,
+					  dev->wl->if_id,
+					  fragment_data, fragment_len, txctl,
+					  (struct ieee80211_rts *)
+					  (txhdr->rts_frame));
+			mac_ctl |= B43legacy_TX4_MAC_SENDRTS;
+			len = sizeof(struct ieee80211_rts);
+		}
+		len += FCS_LEN;
+		b43legacy_generate_plcp_hdr((struct b43legacy_plcp_hdr4 *)
+					    (&txhdr->rts_plcp),
+					    len, rts_rate);
+		b43legacy_generate_plcp_hdr((struct b43legacy_plcp_hdr4 *)
+					    (&txhdr->rts_plcp_fb),
+					    len, rts_rate_fb);
+		hdr = (struct ieee80211_hdr *)(&txhdr->rts_frame);
+		txhdr->rts_dur_fb = hdr->duration_id;
+		mac_ctl |= B43legacy_TX4_MAC_LONGFRAME;
+	}
+
+	/* Magic cookie */
+	txhdr->cookie = cpu_to_le16(cookie);
+
+	/* Apply the bitfields */
+	txhdr->mac_ctl = cpu_to_le32(mac_ctl);
+	txhdr->phy_ctl = cpu_to_le16(phy_ctl);
+}
+
+void b43legacy_generate_txhdr(struct b43legacy_wldev *dev,
+			      u8 *txhdr,
+			      const unsigned char *fragment_data,
+			      unsigned int fragment_len,
+			      const struct ieee80211_tx_control *txctl,
+			      u16 cookie)
+{
+	generate_txhdr_fw3(dev, (struct b43legacy_txhdr_fw3 *)txhdr,
+			   fragment_data, fragment_len,
+			   txctl, cookie);
+}
+
+static s8 b43legacy_rssi_postprocess(struct b43legacy_wldev *dev,
+				     u8 in_rssi, int ofdm,
+				     int adjust_2053, int adjust_2050)
+{
+	struct b43legacy_phy *phy = &dev->phy;
+	s32 tmp;
+
+	switch (phy->radio_ver) {
+	case 0x2050:
+		if (ofdm) {
+			tmp = in_rssi;
+			if (tmp > 127)
+				tmp -= 256;
+			tmp *= 73;
+			tmp /= 64;
+			if (adjust_2050)
+				tmp += 25;
+			else
+				tmp -= 3;
+		} else {
+			if (dev->dev->bus->sprom.r1.boardflags_lo
+			    & B43legacy_BFL_RSSI) {
+				if (in_rssi > 63)
+					in_rssi = 63;
+				tmp = phy->nrssi_lt[in_rssi];
+				tmp = 31 - tmp;
+				tmp *= -131;
+				tmp /= 128;
+				tmp -= 57;
+			} else {
+				tmp = in_rssi;
+				tmp = 31 - tmp;
+				tmp *= -149;
+				tmp /= 128;
+				tmp -= 68;
+			}
+			if (phy->type == B43legacy_PHYTYPE_G &&
+			    adjust_2050)
+				tmp += 25;
+		}
+		break;
+	case 0x2060:
+		if (in_rssi > 127)
+			tmp = in_rssi - 256;
+		else
+			tmp = in_rssi;
+		break;
+	default:
+		tmp = in_rssi;
+		tmp -= 11;
+		tmp *= 103;
+		tmp /= 64;
+		if (adjust_2053)
+			tmp -= 109;
+		else
+			tmp -= 83;
+	}
+
+	return (s8)tmp;
+}
+
+void b43legacy_rx(struct b43legacy_wldev *dev,
+		  struct sk_buff *skb,
+		  const void *_rxhdr)
+{
+	struct ieee80211_rx_status status;
+	struct b43legacy_plcp_hdr6 *plcp;
+	struct ieee80211_hdr *wlhdr;
+	const struct b43legacy_rxhdr_fw3 *rxhdr = _rxhdr;
+	u16 fctl;
+	u16 phystat0;
+	u16 phystat3;
+	u16 chanstat;
+	u16 mactime;
+	u32 macstat;
+	u16 chanid;
+	u8 jssi;
+	int padding;
+
+	memset(&status, 0, sizeof(status));
+
+	/* Get metadata about the frame from the header. */
+	phystat0 = le16_to_cpu(rxhdr->phy_status0);
+	phystat3 = le16_to_cpu(rxhdr->phy_status3);
+	jssi = rxhdr->jssi;
+	macstat = le16_to_cpu(rxhdr->mac_status);
+	mactime = le16_to_cpu(rxhdr->mac_time);
+	chanstat = le16_to_cpu(rxhdr->channel);
+
+	if (macstat & B43legacy_RX_MAC_FCSERR)
+		dev->wl->ieee_stats.dot11FCSErrorCount++;
+
+	/* Skip PLCP and padding */
+	padding = (macstat & B43legacy_RX_MAC_PADDING) ? 2 : 0;
+	if (unlikely(skb->len < (sizeof(struct b43legacy_plcp_hdr6) +
+	    padding))) {
+		b43legacydbg(dev->wl, "RX: Packet size underrun (1)\n");
+		goto drop;
+	}
+	plcp = (struct b43legacy_plcp_hdr6 *)(skb->data + padding);
+	skb_pull(skb, sizeof(struct b43legacy_plcp_hdr6) + padding);
+	/* The skb contains the Wireless Header + payload data now */
+	if (unlikely(skb->len < (2+2+6/*minimum hdr*/ + FCS_LEN))) {
+		b43legacydbg(dev->wl, "RX: Packet size underrun (2)\n");
+		goto drop;
+	}
+	wlhdr = (struct ieee80211_hdr *)(skb->data);
+	fctl = le16_to_cpu(wlhdr->frame_control);
+
+	if ((macstat & B43legacy_RX_MAC_DEC) &&
+	    !(macstat & B43legacy_RX_MAC_DECERR)) {
+		unsigned int keyidx;
+		int wlhdr_len;
+		int iv_len;
+		int icv_len;
+
+		keyidx = ((macstat & B43legacy_RX_MAC_KEYIDX)
+			  >> B43legacy_RX_MAC_KEYIDX_SHIFT);
+		/* We must adjust the key index here. We want the "physical"
+		 * key index, but the ucode passed it slightly different.
+		 */
+		keyidx = b43legacy_kidx_to_raw(dev, keyidx);
+		B43legacy_WARN_ON(keyidx >= dev->max_nr_keys);
+
+		if (dev->key[keyidx].algorithm != B43legacy_SEC_ALGO_NONE) {
+			/* Remove PROTECTED flag to mark it as decrypted. */
+			B43legacy_WARN_ON(!(fctl & IEEE80211_FCTL_PROTECTED));
+			fctl &= ~IEEE80211_FCTL_PROTECTED;
+			wlhdr->frame_control = cpu_to_le16(fctl);
+
+			wlhdr_len = ieee80211_get_hdrlen(fctl);
+			if (unlikely(skb->len < (wlhdr_len + 3))) {
+				b43legacydbg(dev->wl, "RX: Packet size"
+					     " underrun3\n");
+				goto drop;
+			}
+			if (skb->data[wlhdr_len + 3] & (1 << 5)) {
+				/* The Ext-IV Bit is set in the "KeyID"
+				 * octet of the IV.
+				 */
+				iv_len = 8;
+				icv_len = 8;
+			} else {
+				iv_len = 4;
+				icv_len = 4;
+			}
+			if (unlikely(skb->len < (wlhdr_len + iv_len +
+			    icv_len))) {
+				b43legacydbg(dev->wl, "RX: Packet size"
+					     " underrun4\n");
+				goto drop;
+			}
+			/* Remove the IV */
+			memmove(skb->data + iv_len, skb->data, wlhdr_len);
+			skb_pull(skb, iv_len);
+			/* Remove the ICV */
+			skb_trim(skb, skb->len - icv_len);
+
+			status.flag |= RX_FLAG_DECRYPTED;
+		}
+	}
+
+	status.ssi = b43legacy_rssi_postprocess(dev, jssi,
+				      (phystat0 & B43legacy_RX_PHYST0_OFDM),
+				      (phystat0 & B43legacy_RX_PHYST0_GAINCTL),
+				      (phystat3 & B43legacy_RX_PHYST3_TRSTATE));
+	status.noise = dev->stats.link_noise;
+	status.signal = (jssi * 100) / B43legacy_RX_MAX_SSI;
+	if (phystat0 & B43legacy_RX_PHYST0_OFDM)
+		status.rate = b43legacy_plcp_get_bitrate_ofdm(plcp);
+	else
+		status.rate = b43legacy_plcp_get_bitrate_cck(plcp);
+	status.antenna = !!(phystat0 & B43legacy_RX_PHYST0_ANT);
+	status.mactime = mactime;
+
+	chanid = (chanstat & B43legacy_RX_CHAN_ID) >>
+		  B43legacy_RX_CHAN_ID_SHIFT;
+	switch (chanstat & B43legacy_RX_CHAN_PHYTYPE) {
+	case B43legacy_PHYTYPE_B:
+		status.phymode = MODE_IEEE80211B;
+		status.freq = chanid + 2400;
+		status.channel = b43legacy_freq_to_channel_bg(chanid + 2400);
+		break;
+	case B43legacy_PHYTYPE_G:
+		status.phymode = MODE_IEEE80211G;
+		status.freq = chanid + 2400;
+		status.channel = b43legacy_freq_to_channel_bg(chanid + 2400);
+		break;
+	default:
+		b43legacywarn(dev->wl, "Unexpected value for chanstat (0x%X)\n",
+		       chanstat);
+	}
+
+	dev->stats.last_rx = jiffies;
+	ieee80211_rx_irqsafe(dev->wl->hw, skb, &status);
+
+	return;
+drop:
+	b43legacydbg(dev->wl, "RX: Packet dropped\n");
+	dev_kfree_skb_any(skb);
+}
+
+void b43legacy_handle_txstatus(struct b43legacy_wldev *dev,
+			     const struct b43legacy_txstatus *status)
+{
+	b43legacy_debugfs_log_txstat(dev, status);
+
+	if (status->intermediate)
+		return;
+	if (status->for_ampdu)
+		return;
+	if (!status->acked)
+		dev->wl->ieee_stats.dot11ACKFailureCount++;
+	if (status->rts_count) {
+		if (status->rts_count == 0xF) /* FIXME */
+			dev->wl->ieee_stats.dot11RTSFailureCount++;
+		else
+			dev->wl->ieee_stats.dot11RTSSuccessCount++;
+	}
+
+	if (b43legacy_using_pio(dev))
+		b43legacy_pio_handle_txstatus(dev, status);
+	else
+		b43legacy_dma_handle_txstatus(dev, status);
+}
+
+/* Handle TX status report as received through DMA/PIO queues */
+void b43legacy_handle_hwtxstatus(struct b43legacy_wldev *dev,
+				 const struct b43legacy_hwtxstatus *hw)
+{
+	struct b43legacy_txstatus status;
+	u8 tmp;
+
+	status.cookie = le16_to_cpu(hw->cookie);
+	status.seq = le16_to_cpu(hw->seq);
+	status.phy_stat = hw->phy_stat;
+	tmp = hw->count;
+	status.frame_count = (tmp >> 4);
+	status.rts_count = (tmp & 0x0F);
+	tmp = hw->flags;
+	status.supp_reason = ((tmp & 0x1C) >> 2);
+	status.pm_indicated = !!(tmp & 0x80);
+	status.intermediate = !!(tmp & 0x40);
+	status.for_ampdu = !!(tmp & 0x20);
+	status.acked = !!(tmp & 0x02);
+
+	b43legacy_handle_txstatus(dev, &status);
+}
+
+/* Stop any TX operation on the device (suspend the hardware queues) */
+void b43legacy_tx_suspend(struct b43legacy_wldev *dev)
+{
+	if (b43legacy_using_pio(dev))
+		b43legacy_pio_freeze_txqueues(dev);
+	else
+		b43legacy_dma_tx_suspend(dev);
+}
+
+/* Resume any TX operation on the device (resume the hardware queues) */
+void b43legacy_tx_resume(struct b43legacy_wldev *dev)
+{
+	if (b43legacy_using_pio(dev))
+		b43legacy_pio_thaw_txqueues(dev);
+	else
+		b43legacy_dma_tx_resume(dev);
+}
+
+/* Initialize the QoS parameters */
+void b43legacy_qos_init(struct b43legacy_wldev *dev)
+{
+	/* FIXME: This function must probably be called from the mac80211
+	 * config callback. */
+return;
+
+	b43legacy_hf_write(dev, b43legacy_hf_read(dev) | B43legacy_HF_EDCF);
+	/* FIXME kill magic */
+	b43legacy_write16(dev, 0x688,
+			  b43legacy_read16(dev, 0x688) | 0x4);
+
+
+	/*TODO: We might need some stack support here to get the values. */
+}
diff --git a/drivers/net/wireless/b43legacy/xmit.h b/drivers/net/wireless/b43legacy/xmit.h
new file mode 100644
index 000000000000..8a155d0a5d1f
--- /dev/null
+++ b/drivers/net/wireless/b43legacy/xmit.h
@@ -0,0 +1,259 @@
+#ifndef B43legacy_XMIT_H_
+#define B43legacy_XMIT_H_
+
+#include "main.h"
+
+
+#define _b43legacy_declare_plcp_hdr(size)	\
+	struct b43legacy_plcp_hdr##size {	\
+		union {				\
+			__le32 data;		\
+			__u8 raw[size];		\
+		} __attribute__((__packed__));	\
+	} __attribute__((__packed__))
+
+/* struct b43legacy_plcp_hdr4 */
+_b43legacy_declare_plcp_hdr(4);
+/* struct b43legacy_plcp_hdr6 */
+_b43legacy_declare_plcp_hdr(6);
+
+#undef _b43legacy_declare_plcp_hdr
+
+
+/* TX header for v3 firmware */
+struct b43legacy_txhdr_fw3 {
+	__le32 mac_ctl;				/* MAC TX control */
+	__le16 mac_frame_ctl;			/* Copy of the FrameControl */
+	__le16 tx_fes_time_norm;		/* TX FES Time Normal */
+	__le16 phy_ctl;				/* PHY TX control */
+	__u8 iv[16];				/* Encryption IV */
+	__u8 tx_receiver[6];			/* TX Frame Receiver address */
+	__le16 tx_fes_time_fb;			/* TX FES Time Fallback */
+	struct b43legacy_plcp_hdr4 rts_plcp_fb;	/* RTS fallback PLCP */
+	__le16 rts_dur_fb;			/* RTS fallback duration */
+	struct b43legacy_plcp_hdr4 plcp_fb;	/* Fallback PLCP */
+	__le16 dur_fb;				/* Fallback duration */
+	PAD_BYTES(2);
+	__le16 cookie;
+	__le16 unknown_scb_stuff;
+	struct b43legacy_plcp_hdr6 rts_plcp;	/* RTS PLCP */
+	__u8 rts_frame[18];			/* The RTS frame (if used) */
+	struct b43legacy_plcp_hdr6 plcp;
+} __attribute__((__packed__));
+
+/* MAC TX control */
+#define B43legacy_TX4_MAC_KEYIDX	0x0FF00000 /* Security key index */
+#define B43legacy_TX4_MAC_KEYIDX_SHIFT	20
+#define B43legacy_TX4_MAC_KEYALG	0x00070000 /* Security key algorithm */
+#define B43legacy_TX4_MAC_KEYALG_SHIFT	16
+#define B43legacy_TX4_MAC_LIFETIME	0x00001000
+#define B43legacy_TX4_MAC_FRAMEBURST	0x00000800
+#define B43legacy_TX4_MAC_SENDCTS	0x00000400
+#define B43legacy_TX4_MAC_AMPDU		0x00000300
+#define B43legacy_TX4_MAC_AMPDU_SHIFT	8
+#define B43legacy_TX4_MAC_CTSFALLBACKOFDM	0x00000200
+#define B43legacy_TX4_MAC_FALLBACKOFDM	0x00000100
+#define B43legacy_TX4_MAC_5GHZ		0x00000080
+#define B43legacy_TX4_MAC_IGNPMQ	0x00000020
+#define B43legacy_TX4_MAC_HWSEQ		0x00000010 /* Use Hardware Seq No */
+#define B43legacy_TX4_MAC_STMSDU	0x00000008 /* Start MSDU */
+#define B43legacy_TX4_MAC_SENDRTS	0x00000004
+#define B43legacy_TX4_MAC_LONGFRAME	0x00000002
+#define B43legacy_TX4_MAC_ACK		0x00000001
+
+/* Extra Frame Types */
+#define B43legacy_TX4_EFT_FBOFDM	0x0001 /* Data frame fb rate type */
+#define B43legacy_TX4_EFT_RTSOFDM	0x0004 /* RTS/CTS rate type */
+#define B43legacy_TX4_EFT_RTSFBOFDM	0x0010 /* RTS/CTS fallback rate type */
+
+/* PHY TX control word */
+#define B43legacy_TX4_PHY_OFDM		0x0001 /* Data frame rate type */
+#define B43legacy_TX4_PHY_SHORTPRMBL	0x0010 /* Use short preamble */
+#define B43legacy_TX4_PHY_ANT		0x03C0 /* Antenna selection */
+#define  B43legacy_TX4_PHY_ANT0		0x0000 /* Use antenna 0 */
+#define  B43legacy_TX4_PHY_ANT1		0x0100 /* Use antenna 1 */
+#define  B43legacy_TX4_PHY_ANTLAST	0x0300 /* Use last used antenna */
+
+
+
+void b43legacy_generate_txhdr(struct b43legacy_wldev *dev,
+			      u8 *txhdr,
+			      const unsigned char *fragment_data,
+			      unsigned int fragment_len,
+			      const struct ieee80211_tx_control *txctl,
+			      u16 cookie);
+
+
+/* Transmit Status */
+struct b43legacy_txstatus {
+	u16 cookie;	/* The cookie from the txhdr */
+	u16 seq;	/* Sequence number */
+	u8 phy_stat;	/* PHY TX status */
+	u8 frame_count;	/* Frame transmit count */
+	u8 rts_count;	/* RTS transmit count */
+	u8 supp_reason;	/* Suppression reason */
+	/* flags */
+	u8 pm_indicated;/* PM mode indicated to AP */
+	u8 intermediate;/* Intermediate status notification */
+	u8 for_ampdu;	/* Status is for an AMPDU (afterburner) */
+	u8 acked;	/* Wireless ACK received */
+};
+
+/* txstatus supp_reason values */
+enum {
+	B43legacy_TXST_SUPP_NONE,	/* Not suppressed */
+	B43legacy_TXST_SUPP_PMQ,	/* Suppressed due to PMQ entry */
+	B43legacy_TXST_SUPP_FLUSH,	/* Suppressed due to flush request */
+	B43legacy_TXST_SUPP_PREV,	/* Previous fragment failed */
+	B43legacy_TXST_SUPP_CHAN,	/* Channel mismatch */
+	B43legacy_TXST_SUPP_LIFE,	/* Lifetime expired */
+	B43legacy_TXST_SUPP_UNDER,	/* Buffer underflow */
+	B43legacy_TXST_SUPP_ABNACK,	/* Afterburner NACK */
+};
+
+/* Transmit Status as received through DMA/PIO on old chips */
+struct b43legacy_hwtxstatus {
+	PAD_BYTES(4);
+	__le16 cookie;
+	u8 flags;
+	u8 count;
+	PAD_BYTES(2);
+	__le16 seq;
+	u8 phy_stat;
+	PAD_BYTES(1);
+} __attribute__((__packed__));
+
+
+/* Receive header for v3 firmware. */
+struct b43legacy_rxhdr_fw3 {
+	__le16 frame_len;	/* Frame length */
+	PAD_BYTES(2);
+	__le16 phy_status0;	/* PHY RX Status 0 */
+	__u8 jssi;		/* PHY RX Status 1: JSSI */
+	__u8 sig_qual;		/* PHY RX Status 1: Signal Quality */
+	PAD_BYTES(2);		/* PHY RX Status 2 */
+	__le16 phy_status3;	/* PHY RX Status 3 */
+	__le16 mac_status;	/* MAC RX status */
+	__le16 mac_time;
+	__le16 channel;
+} __attribute__((__packed__));
+
+
+/* PHY RX Status 0 */
+#define B43legacy_RX_PHYST0_GAINCTL	0x4000 /* Gain Control */
+#define B43legacy_RX_PHYST0_PLCPHCF	0x0200
+#define B43legacy_RX_PHYST0_PLCPFV	0x0100
+#define B43legacy_RX_PHYST0_SHORTPRMBL	0x0080 /* Recvd with Short Preamble */
+#define B43legacy_RX_PHYST0_LCRS	0x0040
+#define B43legacy_RX_PHYST0_ANT		0x0020 /* Antenna */
+#define B43legacy_RX_PHYST0_UNSRATE	0x0010
+#define B43legacy_RX_PHYST0_CLIP	0x000C
+#define B43legacy_RX_PHYST0_CLIP_SHIFT	2
+#define B43legacy_RX_PHYST0_FTYPE	0x0003 /* Frame type */
+#define  B43legacy_RX_PHYST0_CCK	0x0000 /* Frame type: CCK */
+#define  B43legacy_RX_PHYST0_OFDM	0x0001 /* Frame type: OFDM */
+#define  B43legacy_RX_PHYST0_PRE_N	0x0002 /* Pre-standard N-PHY frame */
+#define  B43legacy_RX_PHYST0_STD_N	0x0003 /* Standard N-PHY frame */
+
+/* PHY RX Status 2 */
+#define B43legacy_RX_PHYST2_LNAG	0xC000 /* LNA Gain */
+#define B43legacy_RX_PHYST2_LNAG_SHIFT	14
+#define B43legacy_RX_PHYST2_PNAG	0x3C00 /* PNA Gain */
+#define B43legacy_RX_PHYST2_PNAG_SHIFT	10
+#define B43legacy_RX_PHYST2_FOFF	0x03FF /* F offset */
+
+/* PHY RX Status 3 */
+#define B43legacy_RX_PHYST3_DIGG	0x1800 /* DIG Gain */
+#define B43legacy_RX_PHYST3_DIGG_SHIFT	11
+#define B43legacy_RX_PHYST3_TRSTATE	0x0400 /* TR state */
+
+/* MAC RX Status */
+#define B43legacy_RX_MAC_BEACONSENT	0x00008000 /* Beacon send flag */
+#define B43legacy_RX_MAC_KEYIDX		0x000007E0 /* Key index */
+#define B43legacy_RX_MAC_KEYIDX_SHIFT	5
+#define B43legacy_RX_MAC_DECERR		0x00000010 /* Decrypt error */
+#define B43legacy_RX_MAC_DEC		0x00000008 /* Decryption attempted */
+#define B43legacy_RX_MAC_PADDING	0x00000004 /* Pad bytes present */
+#define B43legacy_RX_MAC_RESP		0x00000002 /* Response frame xmitted */
+#define B43legacy_RX_MAC_FCSERR		0x00000001 /* FCS error */
+
+/* RX channel */
+#define B43legacy_RX_CHAN_GAIN		0xFC00 /* Gain */
+#define B43legacy_RX_CHAN_GAIN_SHIFT	10
+#define B43legacy_RX_CHAN_ID		0x03FC /* Channel ID */
+#define B43legacy_RX_CHAN_ID_SHIFT	2
+#define B43legacy_RX_CHAN_PHYTYPE	0x0003 /* PHY type */
+
+
+
+u8 b43legacy_plcp_get_ratecode_cck(const u8 bitrate);
+u8 b43legacy_plcp_get_ratecode_ofdm(const u8 bitrate);
+
+void b43legacy_generate_plcp_hdr(struct b43legacy_plcp_hdr4 *plcp,
+			       const u16 octets, const u8 bitrate);
+
+void b43legacy_rx(struct b43legacy_wldev *dev,
+		struct sk_buff *skb,
+		const void *_rxhdr);
+
+void b43legacy_handle_txstatus(struct b43legacy_wldev *dev,
+			       const struct b43legacy_txstatus *status);
+
+void b43legacy_handle_hwtxstatus(struct b43legacy_wldev *dev,
+				 const struct b43legacy_hwtxstatus *hw);
+
+void b43legacy_tx_suspend(struct b43legacy_wldev *dev);
+void b43legacy_tx_resume(struct b43legacy_wldev *dev);
+
+
+#define B43legacy_NR_QOSPARMS	22
+enum {
+	B43legacy_QOSPARM_TXOP = 0,
+	B43legacy_QOSPARM_CWMIN,
+	B43legacy_QOSPARM_CWMAX,
+	B43legacy_QOSPARM_CWCUR,
+	B43legacy_QOSPARM_AIFS,
+	B43legacy_QOSPARM_BSLOTS,
+	B43legacy_QOSPARM_REGGAP,
+	B43legacy_QOSPARM_STATUS,
+};
+
+void b43legacy_qos_init(struct b43legacy_wldev *dev);
+
+
+/* Helper functions for converting the key-table index from "firmware-format"
+ * to "raw-format" and back. The firmware API changed for this at some revision.
+ * We need to account for that here. */
+static inline
+int b43legacy_new_kidx_api(struct b43legacy_wldev *dev)
+{
+	/* FIXME: Not sure the change was at rev 351 */
+	return (dev->fw.rev >= 351);
+}
+static inline
+u8 b43legacy_kidx_to_fw(struct b43legacy_wldev *dev, u8 raw_kidx)
+{
+	u8 firmware_kidx;
+	if (b43legacy_new_kidx_api(dev))
+		firmware_kidx = raw_kidx;
+	else {
+		if (raw_kidx >= 4) /* Is per STA key? */
+			firmware_kidx = raw_kidx - 4;
+		else
+			firmware_kidx = raw_kidx; /* TX default key */
+	}
+	return firmware_kidx;
+}
+static inline
+u8 b43legacy_kidx_to_raw(struct b43legacy_wldev *dev, u8 firmware_kidx)
+{
+	u8 raw_kidx;
+	if (b43legacy_new_kidx_api(dev))
+		raw_kidx = firmware_kidx;
+	else
+		/* RX default keys or per STA keys */
+		raw_kidx = firmware_kidx + 4;
+	return raw_kidx;
+}
+
+#endif /* B43legacy_XMIT_H_ */
diff --git a/drivers/net/wireless/bcm43xx/bcm43xx.h b/drivers/net/wireless/bcm43xx/bcm43xx.h
index 10e07e865426..5fdbf24d2443 100644
--- a/drivers/net/wireless/bcm43xx/bcm43xx.h
+++ b/drivers/net/wireless/bcm43xx/bcm43xx.h
@@ -994,10 +994,4 @@ int bcm43xx_pci_write_config32(struct bcm43xx_private *bcm, int offset, u32 valu
 	 	__value;				\
 	})
 
-/** Helpers to print MAC addresses. */
-#define BCM43xx_MACFMT		"%02x:%02x:%02x:%02x:%02x:%02x"
-#define BCM43xx_MACARG(x)	((u8*)(x))[0], ((u8*)(x))[1], \
-				((u8*)(x))[2], ((u8*)(x))[3], \
-				((u8*)(x))[4], ((u8*)(x))[5]
-
 #endif /* BCM43xx_H_ */
diff --git a/drivers/net/wireless/bcm43xx/bcm43xx_main.c b/drivers/net/wireless/bcm43xx/bcm43xx_main.c
index c5d6753a55ea..45683437a98d 100644
--- a/drivers/net/wireless/bcm43xx/bcm43xx_main.c
+++ b/drivers/net/wireless/bcm43xx/bcm43xx_main.c
@@ -2380,7 +2380,7 @@ static int bcm43xx_chip_init(struct bcm43xx_private *bcm)
 		goto err_gpio_cleanup;
 	bcm43xx_radio_turn_on(bcm);
 	bcm->radio_hw_enable = bcm43xx_is_hw_radio_enabled(bcm);
-	dprintk(KERN_INFO PFX "Radio %s by hardware\n",
+	printk(KERN_INFO PFX "Radio %s by hardware\n",
 		(bcm->radio_hw_enable == 0) ? "disabled" : "enabled");
 
 	bcm43xx_write16(bcm, 0x03E6, 0x0000);
@@ -3129,7 +3129,7 @@ static void bcm43xx_periodic_every1sec(struct bcm43xx_private *bcm)
 	radio_hw_enable = bcm43xx_is_hw_radio_enabled(bcm);
 	if (unlikely(bcm->radio_hw_enable != radio_hw_enable)) {
 		bcm->radio_hw_enable = radio_hw_enable;
-		dprintk(KERN_INFO PFX "Radio hardware status changed to %s\n",
+		printk(KERN_INFO PFX "Radio hardware status changed to %s\n",
 		       (radio_hw_enable == 0) ? "disabled" : "enabled");
 		bcm43xx_leds_update(bcm, 0);
 	}
@@ -3183,6 +3183,9 @@ static void bcm43xx_periodic_work_handler(struct work_struct *work)
 	unsigned long orig_trans_start = 0;
 
 	mutex_lock(&bcm->mutex);
+	/* keep from doing and rearming periodic work if shutting down */
+	if (bcm43xx_status(bcm) == BCM43xx_STAT_UNINIT)
+		goto unlock_mutex;
 	if (unlikely(bcm->periodic_state % 60 == 0)) {
 		/* Periodic work will take a long time, so we want it to
 		 * be preemtible.
@@ -3228,14 +3231,10 @@ static void bcm43xx_periodic_work_handler(struct work_struct *work)
 	mmiowb();
 	bcm->periodic_state++;
 	spin_unlock_irqrestore(&bcm->irq_lock, flags);
+unlock_mutex:
 	mutex_unlock(&bcm->mutex);
 }
 
-void bcm43xx_periodic_tasks_delete(struct bcm43xx_private *bcm)
-{
-	cancel_rearming_delayed_work(&bcm->periodic_work);
-}
-
 void bcm43xx_periodic_tasks_setup(struct bcm43xx_private *bcm)
 {
 	struct delayed_work *work = &bcm->periodic_work;
@@ -3285,6 +3284,14 @@ static int bcm43xx_rng_init(struct bcm43xx_private *bcm)
 	return err;
 }
 
+void bcm43xx_cancel_work(struct bcm43xx_private *bcm)
+{
+	/* The system must be unlocked when this routine is entered.
+	 * If not, the next 2 steps may deadlock */
+	cancel_work_sync(&bcm->restart_work);
+	cancel_delayed_work_sync(&bcm->periodic_work);
+}
+
 static int bcm43xx_shutdown_all_wireless_cores(struct bcm43xx_private *bcm)
 {
 	int ret = 0;
@@ -3321,7 +3328,12 @@ static void bcm43xx_free_board(struct bcm43xx_private *bcm)
 {
 	bcm43xx_rng_exit(bcm);
 	bcm43xx_sysfs_unregister(bcm);
-	bcm43xx_periodic_tasks_delete(bcm);
+
+	mutex_lock(&(bcm)->mutex);
+	bcm43xx_set_status(bcm, BCM43xx_STAT_UNINIT);
+	mutex_unlock(&(bcm)->mutex);
+
+	bcm43xx_cancel_work(bcm);
 
 	mutex_lock(&(bcm)->mutex);
 	bcm43xx_shutdown_all_wireless_cores(bcm);
@@ -4016,7 +4028,7 @@ static int bcm43xx_net_stop(struct net_device *net_dev)
 	err = bcm43xx_disable_interrupts_sync(bcm);
 	assert(!err);
 	bcm43xx_free_board(bcm);
-	flush_scheduled_work();
+	bcm43xx_cancel_work(bcm);
 
 	return 0;
 }
@@ -4080,7 +4092,6 @@ static int __devinit bcm43xx_init_one(struct pci_dev *pdev,
 		goto out;
 	}
 	/* initialize the net_device struct */
-	SET_MODULE_OWNER(net_dev);
 	SET_NETDEV_DEV(net_dev, &pdev->dev);
 
 	net_dev->open = bcm43xx_net_open;
@@ -4148,9 +4159,9 @@ static void bcm43xx_chip_reset(struct work_struct *work)
 	struct bcm43xx_phyinfo *phy;
 	int err = -ENODEV;
 
+	bcm43xx_cancel_work(bcm);
 	mutex_lock(&(bcm)->mutex);
 	if (bcm43xx_status(bcm) == BCM43xx_STAT_INITIALIZED) {
-		bcm43xx_periodic_tasks_delete(bcm);
 		phy = bcm43xx_current_phy(bcm);
 		err = bcm43xx_select_wireless_core(bcm, phy->type);
 		if (!err)
diff --git a/drivers/net/wireless/bcm43xx/bcm43xx_main.h b/drivers/net/wireless/bcm43xx/bcm43xx_main.h
index c8f3c532bab5..14cfbeb582ef 100644
--- a/drivers/net/wireless/bcm43xx/bcm43xx_main.h
+++ b/drivers/net/wireless/bcm43xx/bcm43xx_main.h
@@ -122,7 +122,7 @@ void bcm43xx_wireless_core_reset(struct bcm43xx_private *bcm, int connect_phy);
 void bcm43xx_mac_suspend(struct bcm43xx_private *bcm);
 void bcm43xx_mac_enable(struct bcm43xx_private *bcm);
 
-void bcm43xx_periodic_tasks_delete(struct bcm43xx_private *bcm);
+void bcm43xx_cancel_work(struct bcm43xx_private *bcm);
 void bcm43xx_periodic_tasks_setup(struct bcm43xx_private *bcm);
 
 void bcm43xx_controller_restart(struct bcm43xx_private *bcm, const char *reason);
diff --git a/drivers/net/wireless/bcm43xx/bcm43xx_radio.c b/drivers/net/wireless/bcm43xx/bcm43xx_radio.c
index 6a109f4a1b71..c605099c9baf 100644
--- a/drivers/net/wireless/bcm43xx/bcm43xx_radio.c
+++ b/drivers/net/wireless/bcm43xx/bcm43xx_radio.c
@@ -2146,7 +2146,7 @@ void bcm43xx_radio_turn_off(struct bcm43xx_private *bcm)
 	} else
 		bcm43xx_phy_write(bcm, 0x0015, 0xAA00);
 	radio->enabled = 0;
-	dprintk(KERN_INFO PFX "Radio turned off\n");
+	dprintk(KERN_INFO PFX "Radio initialized\n");
 	bcm43xx_leds_update(bcm, 0);
 }
 
diff --git a/drivers/net/wireless/bcm43xx/bcm43xx_sysfs.c b/drivers/net/wireless/bcm43xx/bcm43xx_sysfs.c
index c71b998a3694..8ab5f93d192a 100644
--- a/drivers/net/wireless/bcm43xx/bcm43xx_sysfs.c
+++ b/drivers/net/wireless/bcm43xx/bcm43xx_sysfs.c
@@ -327,7 +327,7 @@ static ssize_t bcm43xx_attr_phymode_store(struct device *dev,
 		goto out;
 	}
 
-	bcm43xx_periodic_tasks_delete(bcm);
+	bcm43xx_cancel_work(bcm);
 	mutex_lock(&(bcm)->mutex);
 	err = bcm43xx_select_wireless_core(bcm, phytype);
 	if (!err)
diff --git a/drivers/net/wireless/bcm43xx/bcm43xx_wx.c b/drivers/net/wireless/bcm43xx/bcm43xx_wx.c
index d6d9413d7f23..6acfdc49dccd 100644
--- a/drivers/net/wireless/bcm43xx/bcm43xx_wx.c
+++ b/drivers/net/wireless/bcm43xx/bcm43xx_wx.c
@@ -444,7 +444,7 @@ static int bcm43xx_wx_set_xmitpower(struct net_device *net_dev,
 	u16 maxpower;
 
 	if ((data->txpower.flags & IW_TXPOW_TYPE) != IW_TXPOW_DBM) {
-		printk(PFX KERN_ERR "TX power not in dBm.\n");
+		printk(KERN_ERR PFX "TX power not in dBm.\n");
 		return -EOPNOTSUPP;
 	}
 
diff --git a/drivers/net/wireless/hostap/hostap.h b/drivers/net/wireless/hostap/hostap.h
index ef37a75d550b..547ba84dc797 100644
--- a/drivers/net/wireless/hostap/hostap.h
+++ b/drivers/net/wireless/hostap/hostap.h
@@ -30,12 +30,11 @@ void hostap_dump_rx_header(const char *name,
 			   const struct hfa384x_rx_frame *rx);
 void hostap_dump_tx_header(const char *name,
 			   const struct hfa384x_tx_frame *tx);
-int hostap_80211_header_parse(struct sk_buff *skb, unsigned char *haddr);
-int hostap_80211_prism_header_parse(struct sk_buff *skb, unsigned char *haddr);
+extern const struct header_ops hostap_80211_ops;
 int hostap_80211_get_hdrlen(u16 fc);
 struct net_device_stats *hostap_get_stats(struct net_device *dev);
 void hostap_setup_dev(struct net_device *dev, local_info_t *local,
-		      int main_dev);
+		      int type);
 void hostap_set_multicast_list_queue(struct work_struct *work);
 int hostap_set_hostapd(local_info_t *local, int val, int rtnl_locked);
 int hostap_set_hostapd_sta(local_info_t *local, int val, int rtnl_locked);
diff --git a/drivers/net/wireless/hostap/hostap_80211_rx.c b/drivers/net/wireless/hostap/hostap_80211_rx.c
index cbedc9ee740a..ef084df3d48e 100644
--- a/drivers/net/wireless/hostap/hostap_80211_rx.c
+++ b/drivers/net/wireless/hostap/hostap_80211_rx.c
@@ -19,6 +19,7 @@ void hostap_dump_rx_80211(const char *name, struct sk_buff *skb,
 {
 	struct ieee80211_hdr_4addr *hdr;
 	u16 fc;
+	DECLARE_MAC_BUF(mac);
 
 	hdr = (struct ieee80211_hdr_4addr *) skb->data;
 
@@ -44,10 +45,11 @@ void hostap_dump_rx_80211(const char *name, struct sk_buff *skb,
 	printk(" dur=0x%04x seq=0x%04x\n", le16_to_cpu(hdr->duration_id),
 	       le16_to_cpu(hdr->seq_ctl));
 
-	printk(KERN_DEBUG "   A1=" MACSTR " A2=" MACSTR " A3=" MACSTR,
-	       MAC2STR(hdr->addr1), MAC2STR(hdr->addr2), MAC2STR(hdr->addr3));
+	printk(KERN_DEBUG "   A1=%s", print_mac(mac, hdr->addr1));
+	printk(" A2=%s", print_mac(mac, hdr->addr2));
+	printk(" A3=%s", print_mac(mac, hdr->addr3));
 	if (skb->len >= 30)
-		printk(" A4=" MACSTR, MAC2STR(hdr->addr4));
+		printk(" A4=%s", print_mac(mac, hdr->addr4));
 	printk("\n");
 }
 
@@ -534,6 +536,7 @@ static int
 hostap_rx_frame_wds(local_info_t *local, struct ieee80211_hdr_4addr *hdr,
 		    u16 fc, struct net_device **wds)
 {
+	DECLARE_MAC_BUF(mac);
 	/* FIX: is this really supposed to accept WDS frames only in Master
 	 * mode? What about Repeater or Managed with WDS frames? */
 	if ((fc & (IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS)) !=
@@ -549,10 +552,10 @@ hostap_rx_frame_wds(local_info_t *local, struct ieee80211_hdr_4addr *hdr,
 	     hdr->addr1[4] != 0xff || hdr->addr1[5] != 0xff)) {
 		/* RA (or BSSID) is not ours - drop */
 		PDEBUG(DEBUG_EXTRA, "%s: received WDS frame with "
-		       "not own or broadcast %s=" MACSTR "\n",
+		       "not own or broadcast %s=%s\n",
 		       local->dev->name,
 		       fc & IEEE80211_FCTL_FROMDS ? "RA" : "BSSID",
-		       MAC2STR(hdr->addr1));
+		       print_mac(mac, hdr->addr1));
 		return -1;
 	}
 
@@ -565,8 +568,8 @@ hostap_rx_frame_wds(local_info_t *local, struct ieee80211_hdr_4addr *hdr,
 		/* require that WDS link has been registered with TA or the
 		 * frame is from current AP when using 'AP client mode' */
 		PDEBUG(DEBUG_EXTRA, "%s: received WDS[4 addr] frame "
-		       "from unknown TA=" MACSTR "\n",
-		       local->dev->name, MAC2STR(hdr->addr2));
+		       "from unknown TA=%s\n",
+		       local->dev->name, print_mac(mac, hdr->addr2));
 		if (local->ap && local->ap->autom_ap_wds)
 			hostap_wds_link_oper(local, hdr->addr2, WDS_ADD);
 		return -1;
@@ -632,6 +635,7 @@ hostap_rx_frame_decrypt(local_info_t *local, struct sk_buff *skb,
 {
 	struct ieee80211_hdr_4addr *hdr;
 	int res, hdrlen;
+	DECLARE_MAC_BUF(mac);
 
 	if (crypt == NULL || crypt->ops->decrypt_mpdu == NULL)
 		return 0;
@@ -643,8 +647,8 @@ hostap_rx_frame_decrypt(local_info_t *local, struct sk_buff *skb,
 	    strcmp(crypt->ops->name, "TKIP") == 0) {
 		if (net_ratelimit()) {
 			printk(KERN_DEBUG "%s: TKIP countermeasures: dropped "
-			       "received packet from " MACSTR "\n",
-			       local->dev->name, MAC2STR(hdr->addr2));
+			       "received packet from %s\n",
+			       local->dev->name, print_mac(mac, hdr->addr2));
 		}
 		return -1;
 	}
@@ -653,9 +657,9 @@ hostap_rx_frame_decrypt(local_info_t *local, struct sk_buff *skb,
 	res = crypt->ops->decrypt_mpdu(skb, hdrlen, crypt->priv);
 	atomic_dec(&crypt->refcnt);
 	if (res < 0) {
-		printk(KERN_DEBUG "%s: decryption failed (SA=" MACSTR
+		printk(KERN_DEBUG "%s: decryption failed (SA=%s"
 		       ") res=%d\n",
-		       local->dev->name, MAC2STR(hdr->addr2), res);
+		       local->dev->name, print_mac(mac, hdr->addr2), res);
 		local->comm_tallies.rx_discards_wep_undecryptable++;
 		return -1;
 	}
@@ -671,6 +675,7 @@ hostap_rx_frame_decrypt_msdu(local_info_t *local, struct sk_buff *skb,
 {
 	struct ieee80211_hdr_4addr *hdr;
 	int res, hdrlen;
+	DECLARE_MAC_BUF(mac);
 
 	if (crypt == NULL || crypt->ops->decrypt_msdu == NULL)
 		return 0;
@@ -683,8 +688,8 @@ hostap_rx_frame_decrypt_msdu(local_info_t *local, struct sk_buff *skb,
 	atomic_dec(&crypt->refcnt);
 	if (res < 0) {
 		printk(KERN_DEBUG "%s: MSDU decryption/MIC verification failed"
-		       " (SA=" MACSTR " keyidx=%d)\n",
-		       local->dev->name, MAC2STR(hdr->addr2), keyidx);
+		       " (SA=%s keyidx=%d)\n",
+		       local->dev->name, print_mac(mac, hdr->addr2), keyidx);
 		return -1;
 	}
 
@@ -716,6 +721,7 @@ void hostap_80211_rx(struct net_device *dev, struct sk_buff *skb,
 	struct ieee80211_crypt_data *crypt = NULL;
 	void *sta = NULL;
 	int keyidx = 0;
+	DECLARE_MAC_BUF(mac);
 
 	iface = netdev_priv(dev);
 	local = iface->local;
@@ -792,8 +798,8 @@ void hostap_80211_rx(struct net_device *dev, struct sk_buff *skb,
 			 * frames silently instead of filling system log with
 			 * these reports. */
 			printk(KERN_DEBUG "%s: WEP decryption failed (not set)"
-			       " (SA=" MACSTR ")\n",
-			       local->dev->name, MAC2STR(hdr->addr2));
+			       " (SA=%s)\n",
+			       local->dev->name, print_mac(mac, hdr->addr2));
 #endif
 			local->comm_tallies.rx_discards_wep_undecryptable++;
 			goto rx_dropped;
@@ -807,8 +813,8 @@ void hostap_80211_rx(struct net_device *dev, struct sk_buff *skb,
 		    (keyidx = hostap_rx_frame_decrypt(local, skb, crypt)) < 0)
 		{
 			printk(KERN_DEBUG "%s: failed to decrypt mgmt::auth "
-			       "from " MACSTR "\n", dev->name,
-			       MAC2STR(hdr->addr2));
+			       "from %s\n", dev->name,
+			       print_mac(mac, hdr->addr2));
 			/* TODO: could inform hostapd about this so that it
 			 * could send auth failure report */
 			goto rx_dropped;
@@ -976,8 +982,8 @@ void hostap_80211_rx(struct net_device *dev, struct sk_buff *skb,
 			       "unencrypted EAPOL frame\n", local->dev->name);
 		} else {
 			printk(KERN_DEBUG "%s: encryption configured, but RX "
-			       "frame not encrypted (SA=" MACSTR ")\n",
-			       local->dev->name, MAC2STR(hdr->addr2));
+			       "frame not encrypted (SA=%s)\n",
+			       local->dev->name, print_mac(mac, hdr->addr2));
 			goto rx_dropped;
 		}
 	}
@@ -986,8 +992,9 @@ void hostap_80211_rx(struct net_device *dev, struct sk_buff *skb,
 	    !hostap_is_eapol_frame(local, skb)) {
 		if (net_ratelimit()) {
 			printk(KERN_DEBUG "%s: dropped unencrypted RX data "
-			       "frame from " MACSTR " (drop_unencrypted=1)\n",
-			       dev->name, MAC2STR(hdr->addr2));
+			       "frame from %s"
+			       " (drop_unencrypted=1)\n",
+			       dev->name, print_mac(mac, hdr->addr2));
 		}
 		goto rx_dropped;
 	}
diff --git a/drivers/net/wireless/hostap/hostap_80211_tx.c b/drivers/net/wireless/hostap/hostap_80211_tx.c
index 3df3c60263d4..e7afc3ec3e6d 100644
--- a/drivers/net/wireless/hostap/hostap_80211_tx.c
+++ b/drivers/net/wireless/hostap/hostap_80211_tx.c
@@ -17,6 +17,7 @@ void hostap_dump_tx_80211(const char *name, struct sk_buff *skb)
 {
 	struct ieee80211_hdr_4addr *hdr;
 	u16 fc;
+	DECLARE_MAC_BUF(mac);
 
 	hdr = (struct ieee80211_hdr_4addr *) skb->data;
 
@@ -40,10 +41,11 @@ void hostap_dump_tx_80211(const char *name, struct sk_buff *skb)
 	printk(" dur=0x%04x seq=0x%04x\n", le16_to_cpu(hdr->duration_id),
 	       le16_to_cpu(hdr->seq_ctl));
 
-	printk(KERN_DEBUG "   A1=" MACSTR " A2=" MACSTR " A3=" MACSTR,
-	       MAC2STR(hdr->addr1), MAC2STR(hdr->addr2), MAC2STR(hdr->addr3));
+	printk(KERN_DEBUG "   A1=%s", print_mac(mac, hdr->addr1));
+	printk(" A2=%s", print_mac(mac, hdr->addr2));
+	printk(" A3=%s", print_mac(mac, hdr->addr3));
 	if (skb->len >= 30)
-		printk(" A4=" MACSTR, MAC2STR(hdr->addr4));
+		printk(" A4=%s", print_mac(mac, hdr->addr4));
 	printk("\n");
 }
 
@@ -312,6 +314,7 @@ static struct sk_buff * hostap_tx_encrypt(struct sk_buff *skb,
 	struct ieee80211_hdr_4addr *hdr;
 	u16 fc;
 	int prefix_len, postfix_len, hdr_len, res;
+	DECLARE_MAC_BUF(mac);
 
 	iface = netdev_priv(skb->dev);
 	local = iface->local;
@@ -326,8 +329,8 @@ static struct sk_buff * hostap_tx_encrypt(struct sk_buff *skb,
 		hdr = (struct ieee80211_hdr_4addr *) skb->data;
 		if (net_ratelimit()) {
 			printk(KERN_DEBUG "%s: TKIP countermeasures: dropped "
-			       "TX packet to " MACSTR "\n",
-			       local->dev->name, MAC2STR(hdr->addr1));
+			       "TX packet to %s\n",
+			       local->dev->name, print_mac(mac, hdr->addr1));
 		}
 		kfree_skb(skb);
 		return NULL;
diff --git a/drivers/net/wireless/hostap/hostap_ap.c b/drivers/net/wireless/hostap/hostap_ap.c
index 90900525379c..6bbdb76b32df 100644
--- a/drivers/net/wireless/hostap/hostap_ap.c
+++ b/drivers/net/wireless/hostap/hostap_ap.c
@@ -94,6 +94,7 @@ static void ap_sta_hash_add(struct ap_data *ap, struct sta_info *sta)
 static void ap_sta_hash_del(struct ap_data *ap, struct sta_info *sta)
 {
 	struct sta_info *s;
+	DECLARE_MAC_BUF(mac);
 
 	s = ap->sta_hash[STA_HASH(sta->addr)];
 	if (s == NULL) return;
@@ -108,18 +109,20 @@ static void ap_sta_hash_del(struct ap_data *ap, struct sta_info *sta)
 	if (s->hnext != NULL)
 		s->hnext = s->hnext->hnext;
 	else
-		printk("AP: could not remove STA " MACSTR " from hash table\n",
-		       MAC2STR(sta->addr));
+		printk("AP: could not remove STA %s"
+		       " from hash table\n",
+		       print_mac(mac, sta->addr));
 }
 
 static void ap_free_sta(struct ap_data *ap, struct sta_info *sta)
 {
+	DECLARE_MAC_BUF(mac);
 	if (sta->ap && sta->local)
 		hostap_event_expired_sta(sta->local->dev, sta);
 
 	if (ap->proc != NULL) {
 		char name[20];
-		sprintf(name, MACSTR, MAC2STR(sta->addr));
+		sprintf(name, "%s", print_mac(mac, sta->addr));
 		remove_proc_entry(name, ap->proc);
 	}
 
@@ -182,6 +185,7 @@ static void ap_handle_timer(unsigned long data)
 	struct ap_data *ap;
 	unsigned long next_time = 0;
 	int was_assoc;
+	DECLARE_MAC_BUF(mac);
 
 	if (sta == NULL || sta->local == NULL || sta->local->ap == NULL) {
 		PDEBUG(DEBUG_AP, "ap_handle_timer() called with NULL data\n");
@@ -238,8 +242,8 @@ static void ap_handle_timer(unsigned long data)
 	if (sta->ap) {
 		if (ap->autom_ap_wds) {
 			PDEBUG(DEBUG_AP, "%s: removing automatic WDS "
-			       "connection to AP " MACSTR "\n",
-			       local->dev->name, MAC2STR(sta->addr));
+			       "connection to AP %s\n",
+			       local->dev->name, print_mac(mac, sta->addr));
 			hostap_wds_link_oper(local, sta->addr, WDS_DEL);
 		}
 	} else if (sta->timeout_next == STA_NULLFUNC) {
@@ -255,11 +259,11 @@ static void ap_handle_timer(unsigned long data)
 	} else {
 		int deauth = sta->timeout_next == STA_DEAUTH;
 		u16 resp;
-		PDEBUG(DEBUG_AP, "%s: sending %s info to STA " MACSTR
+		PDEBUG(DEBUG_AP, "%s: sending %s info to STA %s"
 		       "(last=%lu, jiffies=%lu)\n",
 		       local->dev->name,
 		       deauth ? "deauthentication" : "disassociation",
-		       MAC2STR(sta->addr), sta->last_rx, jiffies);
+		       print_mac(mac, sta->addr), sta->last_rx, jiffies);
 
 		resp = cpu_to_le16(deauth ? WLAN_REASON_PREV_AUTH_NOT_VALID :
 				   WLAN_REASON_DISASSOC_DUE_TO_INACTIVITY);
@@ -271,9 +275,10 @@ static void ap_handle_timer(unsigned long data)
 
 	if (sta->timeout_next == STA_DEAUTH) {
 		if (sta->flags & WLAN_STA_PERM) {
-			PDEBUG(DEBUG_AP, "%s: STA " MACSTR " would have been "
-			       "removed, but it has 'perm' flag\n",
-			       local->dev->name, MAC2STR(sta->addr));
+			PDEBUG(DEBUG_AP, "%s: STA %s"
+			       " would have been removed, "
+			       "but it has 'perm' flag\n",
+			       local->dev->name, print_mac(mac, sta->addr));
 		} else
 			ap_free_sta(ap, sta);
 		return;
@@ -327,6 +332,7 @@ static int ap_control_proc_read(char *page, char **start, off_t off,
 	struct ap_data *ap = (struct ap_data *) data;
 	char *policy_txt;
 	struct mac_entry *entry;
+	DECLARE_MAC_BUF(mac);
 
 	if (off != 0) {
 		*eof = 1;
@@ -357,7 +363,7 @@ static int ap_control_proc_read(char *page, char **start, off_t off,
 			break;
 		}
 
-		p += sprintf(p, MACSTR "\n", MAC2STR(entry->addr));
+		p += sprintf(p, "%s\n", print_mac(mac, entry->addr));
 	}
 	spin_unlock_bh(&ap->mac_restrictions.lock);
 
@@ -514,6 +520,7 @@ static int prism2_ap_proc_read(char *page, char **start, off_t off,
 	struct ap_data *ap = (struct ap_data *) data;
 	struct sta_info *sta;
 	int i;
+	DECLARE_MAC_BUF(mac);
 
 	if (off > PROC_LIMIT) {
 		*eof = 1;
@@ -526,7 +533,8 @@ static int prism2_ap_proc_read(char *page, char **start, off_t off,
 		if (!sta->ap)
 			continue;
 
-		p += sprintf(p, MACSTR " %d %d %d %d '", MAC2STR(sta->addr),
+		p += sprintf(p, "%s %d %d %d %d '",
+			     print_mac(mac, sta->addr),
 			     sta->u.ap.channel, sta->last_rx_signal,
 			     sta->last_rx_silence, sta->last_rx_rate);
 		for (i = 0; i < sta->u.ap.ssid_len; i++)
@@ -623,6 +631,7 @@ static void hostap_ap_tx_cb_auth(struct sk_buff *skb, int ok, void *data)
 	u16 fc, *pos, auth_alg, auth_transaction, status;
 	struct sta_info *sta = NULL;
 	char *txt = NULL;
+	DECLARE_MAC_BUF(mac);
 
 	if (ap->local->hostapd) {
 		dev_kfree_skb(skb);
@@ -674,9 +683,9 @@ static void hostap_ap_tx_cb_auth(struct sk_buff *skb, int ok, void *data)
 	if (sta)
 		atomic_dec(&sta->users);
 	if (txt) {
-		PDEBUG(DEBUG_AP, "%s: " MACSTR " auth_cb - alg=%d trans#=%d "
-		       "status=%d - %s\n",
-		       dev->name, MAC2STR(hdr->addr1), auth_alg,
+		PDEBUG(DEBUG_AP, "%s: %s auth_cb - alg=%d "
+		       "trans#=%d status=%d - %s\n",
+		       dev->name, print_mac(mac, hdr->addr1), auth_alg,
 		       auth_transaction, status, txt);
 	}
 	dev_kfree_skb(skb);
@@ -692,6 +701,7 @@ static void hostap_ap_tx_cb_assoc(struct sk_buff *skb, int ok, void *data)
 	u16 fc, *pos, status;
 	struct sta_info *sta = NULL;
 	char *txt = NULL;
+	DECLARE_MAC_BUF(mac);
 
 	if (ap->local->hostapd) {
 		dev_kfree_skb(skb);
@@ -742,8 +752,8 @@ static void hostap_ap_tx_cb_assoc(struct sk_buff *skb, int ok, void *data)
 	if (sta)
 		atomic_dec(&sta->users);
 	if (txt) {
-		PDEBUG(DEBUG_AP, "%s: " MACSTR " assoc_cb - %s\n",
-		       dev->name, MAC2STR(hdr->addr1), txt);
+		PDEBUG(DEBUG_AP, "%s: %s assoc_cb - %s\n",
+		       dev->name, print_mac(mac, hdr->addr1), txt);
 	}
 	dev_kfree_skb(skb);
 }
@@ -755,6 +765,7 @@ static void hostap_ap_tx_cb_poll(struct sk_buff *skb, int ok, void *data)
 	struct ap_data *ap = data;
 	struct ieee80211_hdr_4addr *hdr;
 	struct sta_info *sta;
+	DECLARE_MAC_BUF(mac);
 
 	if (skb->len < 24)
 		goto fail;
@@ -766,9 +777,9 @@ static void hostap_ap_tx_cb_poll(struct sk_buff *skb, int ok, void *data)
 			sta->flags &= ~WLAN_STA_PENDING_POLL;
 		spin_unlock(&ap->sta_table_lock);
 	} else {
-		PDEBUG(DEBUG_AP, "%s: STA " MACSTR " did not ACK activity "
-		       "poll frame\n", ap->local->dev->name,
-		       MAC2STR(hdr->addr1));
+		PDEBUG(DEBUG_AP, "%s: STA %s"
+		       " did not ACK activity poll frame\n",
+		       ap->local->dev->name, print_mac(mac, hdr->addr1));
 	}
 
  fail:
@@ -985,6 +996,7 @@ static int prism2_sta_proc_read(char *page, char **start, off_t off,
 	char *p = page;
 	struct sta_info *sta = (struct sta_info *) data;
 	int i;
+	DECLARE_MAC_BUF(mac);
 
 	/* FIX: possible race condition.. the STA data could have just expired,
 	 * but proc entry was still here so that the read could have started;
@@ -995,11 +1007,11 @@ static int prism2_sta_proc_read(char *page, char **start, off_t off,
 		return 0;
 	}
 
-	p += sprintf(p, "%s=" MACSTR "\nusers=%d\naid=%d\n"
+	p += sprintf(p, "%s=%s\nusers=%d\naid=%d\n"
 		     "flags=0x%04x%s%s%s%s%s%s%s\n"
 		     "capability=0x%02x\nlisten_interval=%d\nsupported_rates=",
 		     sta->ap ? "AP" : "STA",
-		     MAC2STR(sta->addr), atomic_read(&sta->users), sta->aid,
+		     print_mac(mac, sta->addr), atomic_read(&sta->users), sta->aid,
 		     sta->flags,
 		     sta->flags & WLAN_STA_AUTH ? " AUTH" : "",
 		     sta->flags & WLAN_STA_ASSOC ? " ASSOC" : "",
@@ -1060,6 +1072,7 @@ static void handle_add_proc_queue(struct work_struct *work)
 	struct sta_info *sta;
 	char name[20];
 	struct add_sta_proc_data *entry, *prev;
+	DECLARE_MAC_BUF(mac);
 
 	entry = ap->add_sta_proc_entries;
 	ap->add_sta_proc_entries = NULL;
@@ -1072,7 +1085,7 @@ static void handle_add_proc_queue(struct work_struct *work)
 		spin_unlock_bh(&ap->sta_table_lock);
 
 		if (sta) {
-			sprintf(name, MACSTR, MAC2STR(sta->addr));
+			sprintf(name, "%s", print_mac(mac, sta->addr));
 			sta->proc = create_proc_read_entry(
 				name, 0, ap->proc,
 				prism2_sta_proc_read, sta);
@@ -1290,6 +1303,7 @@ static void handle_authen(local_info_t *local, struct sk_buff *skb,
 	struct sta_info *sta = NULL;
 	struct ieee80211_crypt_data *crypt;
 	char *txt = "";
+	DECLARE_MAC_BUF(mac);
 
 	len = skb->len - IEEE80211_MGMT_HDR_LEN;
 
@@ -1298,8 +1312,8 @@ static void handle_authen(local_info_t *local, struct sk_buff *skb,
 
 	if (len < 6) {
 		PDEBUG(DEBUG_AP, "%s: handle_authen - too short payload "
-		       "(len=%d) from " MACSTR "\n", dev->name, len,
-		       MAC2STR(hdr->addr2));
+		       "(len=%d) from %s\n", dev->name, len,
+		       print_mac(mac, hdr->addr2));
 		return;
 	}
 
@@ -1364,8 +1378,8 @@ static void handle_authen(local_info_t *local, struct sk_buff *skb,
 		if (time_after(jiffies, sta->u.ap.last_beacon +
 			       (10 * sta->listen_interval * HZ) / 1024)) {
 			PDEBUG(DEBUG_AP, "%s: no beacons received for a while,"
-			       " assuming AP " MACSTR " is now STA\n",
-			       dev->name, MAC2STR(sta->addr));
+			       " assuming AP %s is now STA\n",
+			       dev->name, print_mac(mac, sta->addr));
 			sta->ap = 0;
 			sta->flags = 0;
 			sta->u.sta.challenge = NULL;
@@ -1480,9 +1494,9 @@ static void handle_authen(local_info_t *local, struct sk_buff *skb,
 	}
 
 	if (resp) {
-		PDEBUG(DEBUG_AP, "%s: " MACSTR " auth (alg=%d trans#=%d "
-		       "stat=%d len=%d fc=%04x) ==> %d (%s)\n",
-		       dev->name, MAC2STR(hdr->addr2), auth_alg,
+		PDEBUG(DEBUG_AP, "%s: %s auth (alg=%d "
+		       "trans#=%d stat=%d len=%d fc=%04x) ==> %d (%s)\n",
+		       dev->name, print_mac(mac, hdr->addr2), auth_alg,
 		       auth_transaction, status_code, len, fc, resp, txt);
 	}
 }
@@ -1502,13 +1516,14 @@ static void handle_assoc(local_info_t *local, struct sk_buff *skb,
 	int send_deauth = 0;
 	char *txt = "";
 	u8 prev_ap[ETH_ALEN];
+	DECLARE_MAC_BUF(mac);
 
 	left = len = skb->len - IEEE80211_MGMT_HDR_LEN;
 
 	if (len < (reassoc ? 10 : 4)) {
 		PDEBUG(DEBUG_AP, "%s: handle_assoc - too short payload "
-		       "(len=%d, reassoc=%d) from " MACSTR "\n",
-		       dev->name, len, reassoc, MAC2STR(hdr->addr2));
+		       "(len=%d, reassoc=%d) from %s\n",
+		       dev->name, len, reassoc, print_mac(mac, hdr->addr2));
 		return;
 	}
 
@@ -1585,9 +1600,9 @@ static void handle_assoc(local_info_t *local, struct sk_buff *skb,
 		}
 
 		if (left > 0) {
-			PDEBUG(DEBUG_AP, "%s: assoc from " MACSTR " with extra"
-			       " data (%d bytes) [",
-			       dev->name, MAC2STR(hdr->addr2), left);
+			PDEBUG(DEBUG_AP, "%s: assoc from %s"
+			       " with extra data (%d bytes) [",
+			       dev->name, print_mac(mac, hdr->addr2), left);
 			while (left > 0) {
 				PDEBUG2(DEBUG_AP, "<%02x>", *u);
 				u++; left--;
@@ -1687,10 +1702,10 @@ static void handle_assoc(local_info_t *local, struct sk_buff *skb,
 	}
 
 #if 0
-	PDEBUG(DEBUG_AP, "%s: " MACSTR " %sassoc (len=%d prev_ap=" MACSTR
-	       ") => %d(%d) (%s)\n",
-	       dev->name, MAC2STR(hdr->addr2), reassoc ? "re" : "", len,
-	       MAC2STR(prev_ap), resp, send_deauth, txt);
+	PDEBUG(DEBUG_AP, "%s: %s %sassoc (len=%d "
+	       "prev_ap=%s) => %d(%d) (%s)\n",
+	       dev->name, print_mac(mac, hdr->addr2), reassoc ? "re" : "", len,
+	       print_mac(mac, prev_ap), resp, send_deauth, txt);
 #endif
 }
 
@@ -1705,6 +1720,7 @@ static void handle_deauth(local_info_t *local, struct sk_buff *skb,
 	int len;
 	u16 reason_code, *pos;
 	struct sta_info *sta = NULL;
+	DECLARE_MAC_BUF(mac);
 
 	len = skb->len - IEEE80211_MGMT_HDR_LEN;
 
@@ -1716,8 +1732,8 @@ static void handle_deauth(local_info_t *local, struct sk_buff *skb,
 	pos = (u16 *) body;
 	reason_code = __le16_to_cpu(*pos);
 
-	PDEBUG(DEBUG_AP, "%s: deauthentication: " MACSTR " len=%d, "
-	       "reason_code=%d\n", dev->name, MAC2STR(hdr->addr2), len,
+	PDEBUG(DEBUG_AP, "%s: deauthentication: %s len=%d, "
+	       "reason_code=%d\n", dev->name, print_mac(mac, hdr->addr2), len,
 	       reason_code);
 
 	spin_lock_bh(&local->ap->sta_table_lock);
@@ -1729,9 +1745,9 @@ static void handle_deauth(local_info_t *local, struct sk_buff *skb,
 	}
 	spin_unlock_bh(&local->ap->sta_table_lock);
 	if (sta == NULL) {
-		printk("%s: deauthentication from " MACSTR ", "
+		printk("%s: deauthentication from %s, "
 	       "reason_code=%d, but STA not authenticated\n", dev->name,
-		       MAC2STR(hdr->addr2), reason_code);
+		       print_mac(mac, hdr->addr2), reason_code);
 	}
 }
 
@@ -1746,6 +1762,7 @@ static void handle_disassoc(local_info_t *local, struct sk_buff *skb,
 	int len;
 	u16 reason_code, *pos;
 	struct sta_info *sta = NULL;
+	DECLARE_MAC_BUF(mac);
 
 	len = skb->len - IEEE80211_MGMT_HDR_LEN;
 
@@ -1757,8 +1774,8 @@ static void handle_disassoc(local_info_t *local, struct sk_buff *skb,
 	pos = (u16 *) body;
 	reason_code = __le16_to_cpu(*pos);
 
-	PDEBUG(DEBUG_AP, "%s: disassociation: " MACSTR " len=%d, "
-	       "reason_code=%d\n", dev->name, MAC2STR(hdr->addr2), len,
+	PDEBUG(DEBUG_AP, "%s: disassociation: %s len=%d, "
+	       "reason_code=%d\n", dev->name, print_mac(mac, hdr->addr2), len,
 	       reason_code);
 
 	spin_lock_bh(&local->ap->sta_table_lock);
@@ -1770,9 +1787,9 @@ static void handle_disassoc(local_info_t *local, struct sk_buff *skb,
 	}
 	spin_unlock_bh(&local->ap->sta_table_lock);
 	if (sta == NULL) {
-		printk("%s: disassociation from " MACSTR ", "
+		printk("%s: disassociation from %s, "
 		       "reason_code=%d, but STA not authenticated\n",
-		       dev->name, MAC2STR(hdr->addr2), reason_code);
+		       dev->name, print_mac(mac, hdr->addr2), reason_code);
 	}
 }
 
@@ -1862,15 +1879,16 @@ static void handle_pspoll(local_info_t *local,
 	struct sta_info *sta;
 	u16 aid;
 	struct sk_buff *skb;
+	DECLARE_MAC_BUF(mac);
 
-	PDEBUG(DEBUG_PS2, "handle_pspoll: BSSID=" MACSTR ", TA=" MACSTR
-	       " PWRMGT=%d\n",
-	       MAC2STR(hdr->addr1), MAC2STR(hdr->addr2),
+	PDEBUG(DEBUG_PS2, "handle_pspoll: BSSID=%s"
+	       ", TA=%s PWRMGT=%d\n",
+	       print_mac(mac, hdr->addr1), print_mac(mac, hdr->addr2),
 	       !!(le16_to_cpu(hdr->frame_ctl) & IEEE80211_FCTL_PM));
 
 	if (memcmp(hdr->addr1, dev->dev_addr, ETH_ALEN)) {
-		PDEBUG(DEBUG_AP, "handle_pspoll - addr1(BSSID)=" MACSTR
-		       " not own MAC\n", MAC2STR(hdr->addr1));
+		PDEBUG(DEBUG_AP, "handle_pspoll - addr1(BSSID)=%s"
+		       " not own MAC\n", print_mac(mac, hdr->addr1));
 		return;
 	}
 
@@ -1948,6 +1966,7 @@ static void handle_wds_oper_queue(struct work_struct *work)
 					  wds_oper_queue);
 	local_info_t *local = ap->local;
 	struct wds_oper_data *entry, *prev;
+	DECLARE_MAC_BUF(mac);
 
 	spin_lock_bh(&local->lock);
 	entry = local->ap->wds_oper_entries;
@@ -1956,10 +1975,10 @@ static void handle_wds_oper_queue(struct work_struct *work)
 
 	while (entry) {
 		PDEBUG(DEBUG_AP, "%s: %s automatic WDS connection "
-		       "to AP " MACSTR "\n",
+		       "to AP %s\n",
 		       local->dev->name,
 		       entry->type == WDS_ADD ? "adding" : "removing",
-		       MAC2STR(entry->addr));
+		       print_mac(mac, entry->addr));
 		if (entry->type == WDS_ADD)
 			prism2_wds_add(local, entry->addr, 0);
 		else if (entry->type == WDS_DEL)
@@ -2135,6 +2154,7 @@ static void handle_ap_item(local_info_t *local, struct sk_buff *skb,
 #endif /* PRISM2_NO_KERNEL_IEEE80211_MGMT */
 	u16 fc, type, stype;
 	struct ieee80211_hdr_4addr *hdr;
+	DECLARE_MAC_BUF(mac);
 
 	/* FIX: should give skb->len to handler functions and check that the
 	 * buffer is long enough */
@@ -2163,8 +2183,8 @@ static void handle_ap_item(local_info_t *local, struct sk_buff *skb,
 
 		if (memcmp(hdr->addr1, dev->dev_addr, ETH_ALEN)) {
 			PDEBUG(DEBUG_AP, "handle_ap_item - addr1(BSSID)="
-			       MACSTR " not own MAC\n",
-			       MAC2STR(hdr->addr1));
+			       "%s not own MAC\n",
+			       print_mac(mac, hdr->addr1));
 			goto done;
 		}
 
@@ -2200,14 +2220,14 @@ static void handle_ap_item(local_info_t *local, struct sk_buff *skb,
 	}
 
 	if (memcmp(hdr->addr1, dev->dev_addr, ETH_ALEN)) {
-		PDEBUG(DEBUG_AP, "handle_ap_item - addr1(DA)=" MACSTR
-		       " not own MAC\n", MAC2STR(hdr->addr1));
+		PDEBUG(DEBUG_AP, "handle_ap_item - addr1(DA)=%s"
+		       " not own MAC\n", print_mac(mac, hdr->addr1));
 		goto done;
 	}
 
 	if (memcmp(hdr->addr3, dev->dev_addr, ETH_ALEN)) {
-		PDEBUG(DEBUG_AP, "handle_ap_item - addr3(BSSID)=" MACSTR
-		       " not own MAC\n", MAC2STR(hdr->addr3));
+		PDEBUG(DEBUG_AP, "handle_ap_item - addr3(BSSID)=%s"
+		       " not own MAC\n", print_mac(mac, hdr->addr3));
 		goto done;
 	}
 
@@ -2288,6 +2308,7 @@ static void schedule_packet_send(local_info_t *local, struct sta_info *sta)
 	struct sk_buff *skb;
 	struct ieee80211_hdr_4addr *hdr;
 	struct hostap_80211_rx_status rx_stats;
+	DECLARE_MAC_BUF(mac);
 
 	if (skb_queue_empty(&sta->tx_buf))
 		return;
@@ -2308,8 +2329,8 @@ static void schedule_packet_send(local_info_t *local, struct sta_info *sta)
 	memcpy(hdr->addr2, sta->addr, ETH_ALEN);
 	hdr->duration_id = cpu_to_le16(sta->aid | BIT(15) | BIT(14));
 
-	PDEBUG(DEBUG_PS2, "%s: Scheduling buffered packet delivery for "
-	       "STA " MACSTR "\n", local->dev->name, MAC2STR(sta->addr));
+	PDEBUG(DEBUG_PS2, "%s: Scheduling buffered packet delivery for STA "
+	       "%s\n", local->dev->name, print_mac(mac, sta->addr));
 
 	skb->dev = local->dev;
 
@@ -2636,6 +2657,7 @@ static int ap_update_sta_tx_rate(struct sta_info *sta, struct net_device *dev)
 	int ret = sta->tx_rate;
 	struct hostap_interface *iface;
 	local_info_t *local;
+	DECLARE_MAC_BUF(mac);
 
 	iface = netdev_priv(dev);
 	local = iface->local;
@@ -2663,9 +2685,9 @@ static int ap_update_sta_tx_rate(struct sta_info *sta, struct net_device *dev)
 			case 3: sta->tx_rate = 110; break;
 			default: sta->tx_rate = 0; break;
 			}
-			PDEBUG(DEBUG_AP, "%s: STA " MACSTR " TX rate raised to"
-			       " %d\n", dev->name, MAC2STR(sta->addr),
-			       sta->tx_rate);
+			PDEBUG(DEBUG_AP, "%s: STA %s"
+			       " TX rate raised to %d\n",
+			       dev->name, print_mac(mac, sta->addr), sta->tx_rate);
 		}
 		sta->tx_since_last_failure = 0;
 	}
@@ -2683,6 +2705,7 @@ ap_tx_ret hostap_handle_sta_tx(local_info_t *local, struct hostap_tx_data *tx)
 	int set_tim, ret;
 	struct ieee80211_hdr_4addr *hdr;
 	struct hostap_skb_tx_data *meta;
+	DECLARE_MAC_BUF(mac);
 
 	meta = (struct hostap_skb_tx_data *) skb->cb;
 	ret = AP_TX_CONTINUE;
@@ -2718,7 +2741,8 @@ ap_tx_ret hostap_handle_sta_tx(local_info_t *local, struct hostap_tx_data *tx)
 		 * print out any errors here. */
 		if (net_ratelimit()) {
 			printk(KERN_DEBUG "AP: drop packet to non-associated "
-			       "STA " MACSTR "\n", MAC2STR(hdr->addr1));
+			       "STA %s\n",
+			       print_mac(mac, hdr->addr1));
 		}
 #endif
 		local->ap->tx_drop_nonassoc++;
@@ -2756,8 +2780,9 @@ ap_tx_ret hostap_handle_sta_tx(local_info_t *local, struct hostap_tx_data *tx)
 	}
 
 	if (skb_queue_len(&sta->tx_buf) >= STA_MAX_TX_BUFFER) {
-		PDEBUG(DEBUG_PS, "%s: No more space in STA (" MACSTR ")'s PS "
-		       "mode buffer\n", local->dev->name, MAC2STR(sta->addr));
+		PDEBUG(DEBUG_PS, "%s: No more space in STA (%s"
+		       ")'s PS mode buffer\n",
+		       local->dev->name, print_mac(mac, sta->addr));
 		/* Make sure that TIM is set for the station (it might not be
 		 * after AP wlan hw reset). */
 		/* FIX: should fix hw reset to restore bits based on STA
@@ -2821,6 +2846,7 @@ void hostap_handle_sta_tx_exc(local_info_t *local, struct sk_buff *skb)
 	struct sta_info *sta;
 	struct ieee80211_hdr_4addr *hdr;
 	struct hostap_skb_tx_data *meta;
+	DECLARE_MAC_BUF(mac);
 
 	hdr = (struct ieee80211_hdr_4addr *) skb->data;
 	meta = (struct hostap_skb_tx_data *) skb->cb;
@@ -2829,9 +2855,9 @@ void hostap_handle_sta_tx_exc(local_info_t *local, struct sk_buff *skb)
 	sta = ap_get_sta(local->ap, hdr->addr1);
 	if (!sta) {
 		spin_unlock(&local->ap->sta_table_lock);
-		PDEBUG(DEBUG_AP, "%s: Could not find STA " MACSTR " for this "
-		       "TX error (@%lu)\n",
-		       local->dev->name, MAC2STR(hdr->addr1), jiffies);
+		PDEBUG(DEBUG_AP, "%s: Could not find STA %s"
+		       " for this TX error (@%lu)\n",
+		       local->dev->name, print_mac(mac, hdr->addr1), jiffies);
 		return;
 	}
 
@@ -2858,8 +2884,9 @@ void hostap_handle_sta_tx_exc(local_info_t *local, struct sk_buff *skb)
 			case 3: sta->tx_rate = 110; break;
 			default: sta->tx_rate = 0; break;
 			}
-			PDEBUG(DEBUG_AP, "%s: STA " MACSTR " TX rate lowered "
-			       "to %d\n", local->dev->name, MAC2STR(sta->addr),
+			PDEBUG(DEBUG_AP, "%s: STA %s"
+			       " TX rate lowered to %d\n",
+			       local->dev->name, print_mac(mac, sta->addr),
 			       sta->tx_rate);
 		}
 		sta->tx_consecutive_exc = 0;
@@ -2871,16 +2898,17 @@ void hostap_handle_sta_tx_exc(local_info_t *local, struct sk_buff *skb)
 static void hostap_update_sta_ps2(local_info_t *local, struct sta_info *sta,
 				  int pwrmgt, int type, int stype)
 {
+	DECLARE_MAC_BUF(mac);
 	if (pwrmgt && !(sta->flags & WLAN_STA_PS)) {
 		sta->flags |= WLAN_STA_PS;
-		PDEBUG(DEBUG_PS2, "STA " MACSTR " changed to use PS "
+		PDEBUG(DEBUG_PS2, "STA %s changed to use PS "
 		       "mode (type=0x%02X, stype=0x%02X)\n",
-		       MAC2STR(sta->addr), type >> 2, stype >> 4);
+		       print_mac(mac, sta->addr), type >> 2, stype >> 4);
 	} else if (!pwrmgt && (sta->flags & WLAN_STA_PS)) {
 		sta->flags &= ~WLAN_STA_PS;
-		PDEBUG(DEBUG_PS2, "STA " MACSTR " changed to not use "
+		PDEBUG(DEBUG_PS2, "STA %s changed to not use "
 		       "PS mode (type=0x%02X, stype=0x%02X)\n",
-		       MAC2STR(sta->addr), type >> 2, stype >> 4);
+		       print_mac(mac, sta->addr), type >> 2, stype >> 4);
 		if (type != IEEE80211_FTYPE_CTL ||
 		    stype != IEEE80211_STYPE_PSPOLL)
 			schedule_packet_send(local, sta);
@@ -2924,6 +2952,7 @@ ap_rx_ret hostap_handle_sta_rx(local_info_t *local, struct net_device *dev,
 	struct sta_info *sta;
 	u16 fc, type, stype;
 	struct ieee80211_hdr_4addr *hdr;
+	DECLARE_MAC_BUF(mac);
 
 	if (local->ap == NULL)
 		return AP_RX_CONTINUE;
@@ -2954,9 +2983,10 @@ ap_rx_ret hostap_handle_sta_rx(local_info_t *local, struct net_device *dev,
 #ifndef PRISM2_NO_KERNEL_IEEE80211_MGMT
 			} else {
 				printk(KERN_DEBUG "%s: dropped received packet"
-				       " from non-associated STA " MACSTR
+				       " from non-associated STA "
+				       "%s"
 				       " (type=0x%02x, subtype=0x%02x)\n",
-				       dev->name, MAC2STR(hdr->addr2),
+				       dev->name, print_mac(mac, hdr->addr2),
 				       type >> 2, stype >> 4);
 				hostap_rx(dev, skb, rx_stats);
 #endif /* PRISM2_NO_KERNEL_IEEE80211_MGMT */
@@ -2991,8 +3021,8 @@ ap_rx_ret hostap_handle_sta_rx(local_info_t *local, struct net_device *dev,
 			 * being associated. */
 			printk(KERN_DEBUG "%s: rejected received nullfunc "
 			       "frame without ToDS from not associated STA "
-			       MACSTR "\n",
-			       dev->name, MAC2STR(hdr->addr2));
+			       "%s\n",
+			       dev->name, print_mac(mac, hdr->addr2));
 			hostap_rx(dev, skb, rx_stats);
 #endif /* PRISM2_NO_KERNEL_IEEE80211_MGMT */
 		}
@@ -3009,9 +3039,9 @@ ap_rx_ret hostap_handle_sta_rx(local_info_t *local, struct net_device *dev,
 		 * If BSSID is own, report the dropping of this frame. */
 		if (memcmp(hdr->addr3, dev->dev_addr, ETH_ALEN) == 0) {
 			printk(KERN_DEBUG "%s: dropped received packet from "
-			       MACSTR " with no ToDS flag (type=0x%02x, "
-			       "subtype=0x%02x)\n", dev->name,
-			       MAC2STR(hdr->addr2), type >> 2, stype >> 4);
+			       "%s with no ToDS flag "
+			       "(type=0x%02x, subtype=0x%02x)\n", dev->name,
+			       print_mac(mac, hdr->addr2), type >> 2, stype >> 4);
 			hostap_dump_rx_80211(dev->name, skb, rx_stats);
 		}
 		ret = AP_RX_DROP;
diff --git a/drivers/net/wireless/hostap/hostap_common.h b/drivers/net/wireless/hostap/hostap_common.h
index b31e6a05f23c..ceb7f1e5e9e0 100644
--- a/drivers/net/wireless/hostap/hostap_common.h
+++ b/drivers/net/wireless/hostap/hostap_common.h
@@ -6,9 +6,6 @@
 
 #define BIT(x) (1 << (x))
 
-#define MAC2STR(a) (a)[0], (a)[1], (a)[2], (a)[3], (a)[4], (a)[5]
-#define MACSTR "%02x:%02x:%02x:%02x:%02x:%02x"
-
 
 /* IEEE 802.11 defines */
 
diff --git a/drivers/net/wireless/hostap/hostap_cs.c b/drivers/net/wireless/hostap/hostap_cs.c
index 30e723f65979..877d3bdd37a0 100644
--- a/drivers/net/wireless/hostap/hostap_cs.c
+++ b/drivers/net/wireless/hostap/hostap_cs.c
@@ -272,7 +272,7 @@ static int sandisk_enable_wireless(struct net_device *dev)
 {
 	int res, ret = 0;
 	conf_reg_t reg;
-	struct hostap_interface *iface = dev->priv;
+	struct hostap_interface *iface = netdev_priv(dev);
 	local_info_t *local = iface->local;
 	tuple_t tuple;
 	cisparse_t *parse = NULL;
@@ -822,6 +822,7 @@ static struct pcmcia_device_id hostap_cs_ids[] = {
 	PCMCIA_DEVICE_MANF_CARD(0x0101, 0x0777),
 	PCMCIA_DEVICE_MANF_CARD(0x0126, 0x8000),
 	PCMCIA_DEVICE_MANF_CARD(0x0138, 0x0002),
+	PCMCIA_DEVICE_MANF_CARD(0x01bf, 0x3301),
 	PCMCIA_DEVICE_MANF_CARD(0x0250, 0x0002),
 	PCMCIA_DEVICE_MANF_CARD(0x026f, 0x030b),
 	PCMCIA_DEVICE_MANF_CARD(0x0274, 0x1612),
@@ -889,6 +890,10 @@ static struct pcmcia_device_id hostap_cs_ids[] = {
 	PCMCIA_DEVICE_PROD_ID123(
 		"corega", "WL PCCL-11", "ISL37300P",
 		0xa21501a, 0x59868926, 0xc9049a39),
+	PCMCIA_DEVICE_PROD_ID1234(
+		"The Linksys Group, Inc.", "Wireless Network CF Card", "ISL37300P",
+		"RevA",
+		0xa5f472c2, 0x9c05598d, 0xc9049a39, 0x57a66194),
 	PCMCIA_DEVICE_NULL
 };
 MODULE_DEVICE_TABLE(pcmcia, hostap_cs_ids);
diff --git a/drivers/net/wireless/hostap/hostap_hw.c b/drivers/net/wireless/hostap/hostap_hw.c
index 959887b70ca7..c592641e914e 100644
--- a/drivers/net/wireless/hostap/hostap_hw.c
+++ b/drivers/net/wireless/hostap/hostap_hw.c
@@ -825,7 +825,7 @@ static int hfa384x_get_rid(struct net_device *dev, u16 rid, void *buf, int len,
 	    local->hw_downloading)
 		return -ENODEV;
 
-	res = down_interruptible(&local->rid_bap_sem);
+	res = mutex_lock_interruptible(&local->rid_bap_mtx);
 	if (res)
 		return res;
 
@@ -834,7 +834,7 @@ static int hfa384x_get_rid(struct net_device *dev, u16 rid, void *buf, int len,
 		printk(KERN_DEBUG "%s: hfa384x_get_rid: CMDCODE_ACCESS failed "
 		       "(res=%d, rid=%04x, len=%d)\n",
 		       dev->name, res, rid, len);
-		up(&local->rid_bap_sem);
+		mutex_unlock(&local->rid_bap_mtx);
 		return res;
 	}
 
@@ -861,7 +861,7 @@ static int hfa384x_get_rid(struct net_device *dev, u16 rid, void *buf, int len,
 		res = hfa384x_from_bap(dev, BAP0, buf, len);
 
 	spin_unlock_bh(&local->baplock);
-	up(&local->rid_bap_sem);
+	mutex_unlock(&local->rid_bap_mtx);
 
 	if (res) {
 		if (res != -ENODATA)
@@ -902,7 +902,7 @@ static int hfa384x_set_rid(struct net_device *dev, u16 rid, void *buf, int len)
 	/* RID len in words and +1 for rec.rid */
 	rec.len = cpu_to_le16(len / 2 + len % 2 + 1);
 
-	res = down_interruptible(&local->rid_bap_sem);
+	res = mutex_lock_interruptible(&local->rid_bap_mtx);
 	if (res)
 		return res;
 
@@ -917,12 +917,12 @@ static int hfa384x_set_rid(struct net_device *dev, u16 rid, void *buf, int len)
 	if (res) {
 		printk(KERN_DEBUG "%s: hfa384x_set_rid (rid=%04x, len=%d) - "
 		       "failed - res=%d\n", dev->name, rid, len, res);
-		up(&local->rid_bap_sem);
+		mutex_unlock(&local->rid_bap_mtx);
 		return res;
 	}
 
 	res = hfa384x_cmd(dev, HFA384X_CMDCODE_ACCESS_WRITE, rid, NULL, NULL);
-	up(&local->rid_bap_sem);
+	mutex_unlock(&local->rid_bap_mtx);
 
 	if (res) {
 		printk(KERN_DEBUG "%s: hfa384x_set_rid: CMDCODE_ACCESS_WRITE "
@@ -2335,6 +2335,10 @@ static void prism2_txexc(local_info_t *local)
 	int show_dump, res;
 	char *payload = NULL;
 	struct hfa384x_tx_frame txdesc;
+	DECLARE_MAC_BUF(mac);
+	DECLARE_MAC_BUF(mac2);
+	DECLARE_MAC_BUF(mac3);
+	DECLARE_MAC_BUF(mac4);
 
 	show_dump = local->frame_dump & PRISM2_DUMP_TXEXC_HDR;
 	local->stats.tx_errors++;
@@ -2400,10 +2404,9 @@ static void prism2_txexc(local_info_t *local)
 	       WLAN_FC_GET_STYPE(fc) >> 4,
 	       fc & IEEE80211_FCTL_TODS ? " ToDS" : "",
 	       fc & IEEE80211_FCTL_FROMDS ? " FromDS" : "");
-	PDEBUG(DEBUG_EXTRA, "   A1=" MACSTR " A2=" MACSTR " A3="
-	       MACSTR " A4=" MACSTR "\n",
-	       MAC2STR(txdesc.addr1), MAC2STR(txdesc.addr2),
-	       MAC2STR(txdesc.addr3), MAC2STR(txdesc.addr4));
+	PDEBUG(DEBUG_EXTRA, "   A1=%s A2=%s A3=%s A4=%s\n",
+	       print_mac(mac, txdesc.addr1), print_mac(mac2, txdesc.addr2),
+	       print_mac(mac3, txdesc.addr3), print_mac(mac4, txdesc.addr4));
 }
 
 
@@ -3171,7 +3174,7 @@ prism2_init_local_data(struct prism2_helper_functions *funcs, int card_idx,
 	spin_lock_init(&local->cmdlock);
 	spin_lock_init(&local->baplock);
 	spin_lock_init(&local->lock);
-	init_MUTEX(&local->rid_bap_sem);
+	mutex_init(&local->rid_bap_mtx);
 
 	if (card_idx < 0 || card_idx >= MAX_PARM_DEVICES)
 		card_idx = 0;
@@ -3254,12 +3257,11 @@ while (0)
 
 	INIT_LIST_HEAD(&local->bss_list);
 
-	hostap_setup_dev(dev, local, 1);
-	local->saved_eth_header_parse = dev->hard_header_parse;
+	hostap_setup_dev(dev, local, HOSTAP_INTERFACE_MASTER);
 
 	dev->hard_start_xmit = hostap_master_start_xmit;
 	dev->type = ARPHRD_IEEE80211;
-	dev->hard_header_parse = hostap_80211_header_parse;
+	dev->header_ops = &hostap_80211_ops;
 
 	rtnl_lock();
 	ret = dev_alloc_name(dev, "wifi%d");
@@ -3424,7 +3426,7 @@ static void prism2_suspend(struct net_device *dev)
 	struct local_info *local;
 	union iwreq_data wrqu;
 
-	iface = dev->priv;
+	iface = netdev_priv(dev);
 	local = iface->local;
 
 	/* Send disconnect event, e.g., to trigger reassociation after resume
diff --git a/drivers/net/wireless/hostap/hostap_info.c b/drivers/net/wireless/hostap/hostap_info.c
index b6a02a02da74..636f4b2382ea 100644
--- a/drivers/net/wireless/hostap/hostap_info.c
+++ b/drivers/net/wireless/hostap/hostap_info.c
@@ -166,6 +166,7 @@ static void prism2_host_roaming(local_info_t *local)
 	struct hfa384x_hostscan_result *selected, *entry;
 	int i;
 	unsigned long flags;
+	DECLARE_MAC_BUF(mac);
 
 	if (local->last_join_time &&
 	    time_before(jiffies, local->last_join_time + 10 * HZ)) {
@@ -198,8 +199,9 @@ static void prism2_host_roaming(local_info_t *local)
 	    local->preferred_ap[2] || local->preferred_ap[3] ||
 	    local->preferred_ap[4] || local->preferred_ap[5]) {
 		/* Try to find preferred AP */
-		PDEBUG(DEBUG_EXTRA, "%s: Preferred AP BSSID " MACSTR "\n",
-		       dev->name, MAC2STR(local->preferred_ap));
+		PDEBUG(DEBUG_EXTRA, "%s: Preferred AP BSSID "
+		       "%s\n",
+		       dev->name, print_mac(mac, local->preferred_ap));
 		for (i = 0; i < local->last_scan_results_count; i++) {
 			entry = &local->last_scan_results[i];
 			if (memcmp(local->preferred_ap, entry->bssid, 6) == 0)
@@ -216,8 +218,9 @@ static void prism2_host_roaming(local_info_t *local)
 	req.channel = selected->chid;
 	spin_unlock_irqrestore(&local->lock, flags);
 
-	PDEBUG(DEBUG_EXTRA, "%s: JoinRequest: BSSID=" MACSTR " channel=%d\n",
-	       dev->name, MAC2STR(req.bssid), le16_to_cpu(req.channel));
+	PDEBUG(DEBUG_EXTRA, "%s: JoinRequest: BSSID=%s"
+	       " channel=%d\n",
+	       dev->name, print_mac(mac, req.bssid), le16_to_cpu(req.channel));
 	if (local->func->set_rid(dev, HFA384X_RID_JOINREQUEST, &req,
 				 sizeof(req))) {
 		printk(KERN_DEBUG "%s: JoinRequest failed\n", dev->name);
@@ -409,6 +412,7 @@ static void handle_info_queue_linkstatus(local_info_t *local)
 	int val = local->prev_link_status;
 	int connected;
 	union iwreq_data wrqu;
+	DECLARE_MAC_BUF(mac);
 
 	connected =
 		val == HFA384X_LINKSTATUS_CONNECTED ||
@@ -420,9 +424,10 @@ static void handle_info_queue_linkstatus(local_info_t *local)
 		printk(KERN_DEBUG "%s: could not read CURRENTBSSID after "
 		       "LinkStatus event\n", local->dev->name);
 	} else {
-		PDEBUG(DEBUG_EXTRA, "%s: LinkStatus: BSSID=" MACSTR "\n",
+		PDEBUG(DEBUG_EXTRA, "%s: LinkStatus: BSSID="
+		       "%s\n",
 		       local->dev->name,
-		       MAC2STR((unsigned char *) local->bssid));
+		       print_mac(mac, (unsigned char *) local->bssid));
 		if (local->wds_type & HOSTAP_WDS_AP_CLIENT)
 			hostap_add_sta(local->ap, local->bssid);
 	}
diff --git a/drivers/net/wireless/hostap/hostap_ioctl.c b/drivers/net/wireless/hostap/hostap_ioctl.c
index 8c71077d653c..40f516d42c5e 100644
--- a/drivers/net/wireless/hostap/hostap_ioctl.c
+++ b/drivers/net/wireless/hostap/hostap_ioctl.c
@@ -664,6 +664,7 @@ static int hostap_join_ap(struct net_device *dev)
 	unsigned long flags;
 	int i;
 	struct hfa384x_hostscan_result *entry;
+	DECLARE_MAC_BUF(mac);
 
 	iface = netdev_priv(dev);
 	local = iface->local;
@@ -685,14 +686,14 @@ static int hostap_join_ap(struct net_device *dev)
 
 	if (local->func->set_rid(dev, HFA384X_RID_JOINREQUEST, &req,
 				 sizeof(req))) {
-		printk(KERN_DEBUG "%s: JoinRequest " MACSTR
+		printk(KERN_DEBUG "%s: JoinRequest %s"
 		       " failed\n",
-		       dev->name, MAC2STR(local->preferred_ap));
+		       dev->name, print_mac(mac, local->preferred_ap));
 		return -1;
 	}
 
-	printk(KERN_DEBUG "%s: Trying to join BSSID " MACSTR "\n",
-	       dev->name, MAC2STR(local->preferred_ap));
+	printk(KERN_DEBUG "%s: Trying to join BSSID %s\n",
+	       dev->name, print_mac(mac, local->preferred_ap));
 
 	return 0;
 }
@@ -896,11 +897,8 @@ static void hostap_monitor_set_type(local_info_t *local)
 	if (local->monitor_type == PRISM2_MONITOR_PRISM ||
 	    local->monitor_type == PRISM2_MONITOR_CAPHDR) {
 		dev->type = ARPHRD_IEEE80211_PRISM;
-		dev->hard_header_parse =
-			hostap_80211_prism_header_parse;
 	} else {
 		dev->type = ARPHRD_IEEE80211;
-		dev->hard_header_parse = hostap_80211_header_parse;
 	}
 }
 
@@ -1140,7 +1138,7 @@ static int hostap_monitor_mode_disable(local_info_t *local)
 
 	printk(KERN_DEBUG "%s: Disabling monitor mode\n", dev->name);
 	dev->type = ARPHRD_ETHER;
-	dev->hard_header_parse = local->saved_eth_header_parse;
+
 	if (local->func->cmd(dev, HFA384X_CMDCODE_TEST |
 			     (HFA384X_TEST_STOP << 8),
 			     0, NULL, NULL))
@@ -3088,7 +3086,7 @@ static int prism2_ioctl_priv_download(local_info_t *local, struct iw_point *p)
 static int prism2_set_genericelement(struct net_device *dev, u8 *elem,
 				     size_t len)
 {
-	struct hostap_interface *iface = dev->priv;
+	struct hostap_interface *iface = netdev_priv(dev);
 	local_info_t *local = iface->local;
 	u8 *buf;
 
@@ -3116,7 +3114,7 @@ static int prism2_ioctl_siwauth(struct net_device *dev,
 				struct iw_request_info *info,
 				struct iw_param *data, char *extra)
 {
-	struct hostap_interface *iface = dev->priv;
+	struct hostap_interface *iface = netdev_priv(dev);
 	local_info_t *local = iface->local;
 
 	switch (data->flags & IW_AUTH_INDEX) {
@@ -3182,7 +3180,7 @@ static int prism2_ioctl_giwauth(struct net_device *dev,
 				struct iw_request_info *info,
 				struct iw_param *data, char *extra)
 {
-	struct hostap_interface *iface = dev->priv;
+	struct hostap_interface *iface = netdev_priv(dev);
 	local_info_t *local = iface->local;
 
 	switch (data->flags & IW_AUTH_INDEX) {
@@ -3221,7 +3219,7 @@ static int prism2_ioctl_siwencodeext(struct net_device *dev,
 				     struct iw_request_info *info,
 				     struct iw_point *erq, char *extra)
 {
-	struct hostap_interface *iface = dev->priv;
+	struct hostap_interface *iface = netdev_priv(dev);
 	local_info_t *local = iface->local;
 	struct iw_encode_ext *ext = (struct iw_encode_ext *) extra;
 	int i, ret = 0;
@@ -3395,7 +3393,7 @@ static int prism2_ioctl_giwencodeext(struct net_device *dev,
 				     struct iw_request_info *info,
 				     struct iw_point *erq, char *extra)
 {
-	struct hostap_interface *iface = dev->priv;
+	struct hostap_interface *iface = netdev_priv(dev);
 	local_info_t *local = iface->local;
 	struct ieee80211_crypt_data **crypt;
 	void *sta_ptr;
@@ -3697,8 +3695,10 @@ static int prism2_ioctl_set_assoc_ap_addr(local_info_t *local,
 					  struct prism2_hostapd_param *param,
 					  int param_len)
 {
-	printk(KERN_DEBUG "%ssta: associated as client with AP " MACSTR "\n",
-	       local->dev->name, MAC2STR(param->sta_addr));
+	DECLARE_MAC_BUF(mac);
+	printk(KERN_DEBUG "%ssta: associated as client with AP "
+	       "%s\n",
+	       local->dev->name, print_mac(mac, param->sta_addr));
 	memcpy(local->assoc_ap_addr, param->sta_addr, ETH_ALEN);
 	return 0;
 }
@@ -3716,7 +3716,7 @@ static int prism2_ioctl_giwgenie(struct net_device *dev,
 				 struct iw_request_info *info,
 				 struct iw_point *data, char *extra)
 {
-	struct hostap_interface *iface = dev->priv;
+	struct hostap_interface *iface = netdev_priv(dev);
 	local_info_t *local = iface->local;
 	int len = local->generic_elem_len - 2;
 
@@ -3755,7 +3755,7 @@ static int prism2_ioctl_siwmlme(struct net_device *dev,
 				struct iw_request_info *info,
 				struct iw_point *data, char *extra)
 {
-	struct hostap_interface *iface = dev->priv;
+	struct hostap_interface *iface = netdev_priv(dev);
 	local_info_t *local = iface->local;
 	struct iw_mlme *mlme = (struct iw_mlme *) extra;
 	u16 reason;
@@ -3976,9 +3976,9 @@ static const iw_handler prism2_private_handler[] =
 
 const struct iw_handler_def hostap_iw_handler_def =
 {
-	.num_standard	= sizeof(prism2_handler) / sizeof(iw_handler),
-	.num_private	= sizeof(prism2_private_handler) / sizeof(iw_handler),
-	.num_private_args = sizeof(prism2_priv) / sizeof(struct iw_priv_args),
+	.num_standard	= ARRAY_SIZE(prism2_handler),
+	.num_private	= ARRAY_SIZE(prism2_private_handler),
+	.num_private_args = ARRAY_SIZE(prism2_priv),
 	.standard	= (iw_handler *) prism2_handler,
 	.private	= (iw_handler *) prism2_private_handler,
 	.private_args	= (struct iw_priv_args *) prism2_priv,
diff --git a/drivers/net/wireless/hostap/hostap_main.c b/drivers/net/wireless/hostap/hostap_main.c
index 446de51bab74..17c58e9bdad5 100644
--- a/drivers/net/wireless/hostap/hostap_main.c
+++ b/drivers/net/wireless/hostap/hostap_main.c
@@ -24,6 +24,7 @@
 #include <linux/rtnetlink.h>
 #include <linux/wireless.h>
 #include <linux/etherdevice.h>
+#include <net/net_namespace.h>
 #include <net/iw_handler.h>
 #include <net/ieee80211.h>
 #include <net/ieee80211_crypt.h>
@@ -72,7 +73,7 @@ struct net_device * hostap_add_interface(struct local_info *local,
 	dev->mem_start = mdev->mem_start;
 	dev->mem_end = mdev->mem_end;
 
-	hostap_setup_dev(dev, local, 0);
+	hostap_setup_dev(dev, local, type);
 	dev->destructor = free_netdev;
 
 	sprintf(dev->name, "%s%s", prefix, name);
@@ -529,6 +530,10 @@ int hostap_set_auth_algs(local_info_t *local)
 void hostap_dump_rx_header(const char *name, const struct hfa384x_rx_frame *rx)
 {
 	u16 status, fc;
+	DECLARE_MAC_BUF(mac);
+	DECLARE_MAC_BUF(mac2);
+	DECLARE_MAC_BUF(mac3);
+	DECLARE_MAC_BUF(mac4);
 
 	status = __le16_to_cpu(rx->status);
 
@@ -547,13 +552,12 @@ void hostap_dump_rx_header(const char *name, const struct hfa384x_rx_frame *rx)
 	       fc & IEEE80211_FCTL_TODS ? " [ToDS]" : "",
 	       fc & IEEE80211_FCTL_FROMDS ? " [FromDS]" : "");
 
-	printk(KERN_DEBUG "   A1=" MACSTR " A2=" MACSTR " A3=" MACSTR " A4="
-	       MACSTR "\n",
-	       MAC2STR(rx->addr1), MAC2STR(rx->addr2), MAC2STR(rx->addr3),
-	       MAC2STR(rx->addr4));
+	printk(KERN_DEBUG "   A1=%s A2=%s A3=%s A4=%s\n",
+	       print_mac(mac, rx->addr1), print_mac(mac2, rx->addr2),
+	       print_mac(mac3, rx->addr3), print_mac(mac4, rx->addr4));
 
-	printk(KERN_DEBUG "   dst=" MACSTR " src=" MACSTR " len=%d\n",
-	       MAC2STR(rx->dst_addr), MAC2STR(rx->src_addr),
+	printk(KERN_DEBUG "   dst=%s src=%s len=%d\n",
+	       print_mac(mac, rx->dst_addr), print_mac(mac2, rx->src_addr),
 	       __be16_to_cpu(rx->len));
 }
 
@@ -561,6 +565,10 @@ void hostap_dump_rx_header(const char *name, const struct hfa384x_rx_frame *rx)
 void hostap_dump_tx_header(const char *name, const struct hfa384x_tx_frame *tx)
 {
 	u16 fc;
+	DECLARE_MAC_BUF(mac);
+	DECLARE_MAC_BUF(mac2);
+	DECLARE_MAC_BUF(mac3);
+	DECLARE_MAC_BUF(mac4);
 
 	printk(KERN_DEBUG "%s: TX status=0x%04x retry_count=%d tx_rate=%d "
 	       "tx_control=0x%04x; jiffies=%ld\n",
@@ -576,35 +584,37 @@ void hostap_dump_tx_header(const char *name, const struct hfa384x_tx_frame *tx)
 	       fc & IEEE80211_FCTL_TODS ? " [ToDS]" : "",
 	       fc & IEEE80211_FCTL_FROMDS ? " [FromDS]" : "");
 
-	printk(KERN_DEBUG "   A1=" MACSTR " A2=" MACSTR " A3=" MACSTR " A4="
-	       MACSTR "\n",
-	       MAC2STR(tx->addr1), MAC2STR(tx->addr2), MAC2STR(tx->addr3),
-	       MAC2STR(tx->addr4));
+	printk(KERN_DEBUG "   A1=%s A2=%s A3=%s A4=%s\n",
+	       print_mac(mac, tx->addr1), print_mac(mac2, tx->addr2),
+	       print_mac(mac3, tx->addr3), print_mac(mac4, tx->addr4));
 
-	printk(KERN_DEBUG "   dst=" MACSTR " src=" MACSTR " len=%d\n",
-	       MAC2STR(tx->dst_addr), MAC2STR(tx->src_addr),
+	printk(KERN_DEBUG "   dst=%s src=%s len=%d\n",
+	       print_mac(mac, tx->dst_addr), print_mac(mac2, tx->src_addr),
 	       __be16_to_cpu(tx->len));
 }
 
 
-int hostap_80211_header_parse(struct sk_buff *skb, unsigned char *haddr)
+int hostap_80211_header_parse(const struct sk_buff *skb, unsigned char *haddr)
 {
-	memcpy(haddr, skb_mac_header(skb) + 10, ETH_ALEN); /* addr2 */
-	return ETH_ALEN;
-}
-
+	struct hostap_interface *iface = netdev_priv(skb->dev);
+	local_info_t *local = iface->local;
+
+	if (local->monitor_type == PRISM2_MONITOR_PRISM ||
+	    local->monitor_type == PRISM2_MONITOR_CAPHDR) {
+		const unsigned char *mac = skb_mac_header(skb);
+
+		if (*(u32 *)mac == LWNG_CAP_DID_BASE) {
+			memcpy(haddr,
+			       mac + sizeof(struct linux_wlan_ng_prism_hdr) + 10,
+			       ETH_ALEN); /* addr2 */
+		} else { /* (*(u32 *)mac == htonl(LWNG_CAPHDR_VERSION)) */
+			memcpy(haddr,
+			       mac + sizeof(struct linux_wlan_ng_cap_hdr) + 10,
+			       ETH_ALEN); /* addr2 */
+		}
+	} else
+		memcpy(haddr, skb_mac_header(skb) + 10, ETH_ALEN); /* addr2 */
 
-int hostap_80211_prism_header_parse(struct sk_buff *skb, unsigned char *haddr)
-{
-	const unsigned char *mac = skb_mac_header(skb);
-
-	if (*(u32 *)mac == LWNG_CAP_DID_BASE) {
-		memcpy(haddr, mac + sizeof(struct linux_wlan_ng_prism_hdr) + 10,
-		       ETH_ALEN); /* addr2 */
-	} else { /* (*(u32 *)mac == htonl(LWNG_CAPHDR_VERSION)) */
-		memcpy(haddr, mac + sizeof(struct linux_wlan_ng_cap_hdr) + 10,
-		       ETH_ALEN); /* addr2 */
-	}
 	return ETH_ALEN;
 }
 
@@ -836,9 +846,18 @@ static void prism2_tx_timeout(struct net_device *dev)
 	local->func->schedule_reset(local);
 }
 
+const struct header_ops hostap_80211_ops = {
+	.create		= eth_header,
+	.rebuild	= eth_rebuild_header,
+	.cache		= eth_header_cache,
+	.cache_update	= eth_header_cache_update,
+
+	.parse		= hostap_80211_header_parse,
+};
+EXPORT_SYMBOL(hostap_80211_ops);
 
 void hostap_setup_dev(struct net_device *dev, local_info_t *local,
-		      int main_dev)
+		      int type)
 {
 	struct hostap_interface *iface;
 
@@ -858,15 +877,22 @@ void hostap_setup_dev(struct net_device *dev, local_info_t *local,
 	dev->do_ioctl = hostap_ioctl;
 	dev->open = prism2_open;
 	dev->stop = prism2_close;
-	dev->hard_start_xmit = hostap_data_start_xmit;
 	dev->set_mac_address = prism2_set_mac_address;
 	dev->set_multicast_list = hostap_set_multicast_list;
 	dev->change_mtu = prism2_change_mtu;
 	dev->tx_timeout = prism2_tx_timeout;
 	dev->watchdog_timeo = TX_TIMEOUT;
 
+	if (type == HOSTAP_INTERFACE_AP) {
+		dev->hard_start_xmit = hostap_mgmt_start_xmit;
+		dev->type = ARPHRD_IEEE80211;
+		dev->header_ops = &hostap_80211_ops;
+	} else {
+		dev->hard_start_xmit = hostap_data_start_xmit;
+	}
+
 	dev->mtu = local->mtu;
-	if (!main_dev) {
+	if (type != HOSTAP_INTERFACE_MASTER) {
 		/* use main radio device queue */
 		dev->tx_queue_len = 0;
 	}
@@ -876,7 +902,6 @@ void hostap_setup_dev(struct net_device *dev, local_info_t *local,
 	netif_stop_queue(dev);
 }
 
-
 static int hostap_enable_hostapd(local_info_t *local, int rtnl_locked)
 {
 	struct net_device *dev = local->dev;
@@ -892,10 +917,6 @@ static int hostap_enable_hostapd(local_info_t *local, int rtnl_locked)
 	if (local->apdev == NULL)
 		return -ENOMEM;
 
-	local->apdev->hard_start_xmit = hostap_mgmt_start_xmit;
-	local->apdev->type = ARPHRD_IEEE80211;
-	local->apdev->hard_header_parse = hostap_80211_header_parse;
-
 	return 0;
 }
 
@@ -1093,8 +1114,8 @@ struct proc_dir_entry *hostap_proc;
 
 static int __init hostap_init(void)
 {
-	if (proc_net != NULL) {
-		hostap_proc = proc_mkdir("hostap", proc_net);
+	if (init_net.proc_net != NULL) {
+		hostap_proc = proc_mkdir("hostap", init_net.proc_net);
 		if (!hostap_proc)
 			printk(KERN_WARNING "Failed to mkdir "
 			       "/proc/net/hostap\n");
@@ -1109,7 +1130,7 @@ static void __exit hostap_exit(void)
 {
 	if (hostap_proc != NULL) {
 		hostap_proc = NULL;
-		remove_proc_entry("hostap", proc_net);
+		remove_proc_entry("hostap", init_net.proc_net);
 	}
 }
 
diff --git a/drivers/net/wireless/hostap/hostap_proc.c b/drivers/net/wireless/hostap/hostap_proc.c
index d1d8ce022e63..b03536008ad9 100644
--- a/drivers/net/wireless/hostap/hostap_proc.c
+++ b/drivers/net/wireless/hostap/hostap_proc.c
@@ -106,6 +106,7 @@ static int prism2_wds_proc_read(char *page, char **start, off_t off,
 	local_info_t *local = (local_info_t *) data;
 	struct list_head *ptr;
 	struct hostap_interface *iface;
+	DECLARE_MAC_BUF(mac);
 
 	if (off > PROC_LIMIT) {
 		*eof = 1;
@@ -117,9 +118,9 @@ static int prism2_wds_proc_read(char *page, char **start, off_t off,
 		iface = list_entry(ptr, struct hostap_interface, list);
 		if (iface->type != HOSTAP_INTERFACE_WDS)
 			continue;
-		p += sprintf(p, "%s\t" MACSTR "\n",
+		p += sprintf(p, "%s\t%s\n",
 			     iface->dev->name,
-			     MAC2STR(iface->u.wds.remote_addr));
+			     print_mac(mac, iface->u.wds.remote_addr));
 		if ((p - page) > PROC_LIMIT) {
 			printk(KERN_DEBUG "%s: wds proc did not fit\n",
 			       local->dev->name);
@@ -147,6 +148,7 @@ static int prism2_bss_list_proc_read(char *page, char **start, off_t off,
 	struct list_head *ptr;
 	struct hostap_bss_info *bss;
 	int i;
+	DECLARE_MAC_BUF(mac);
 
 	if (off > PROC_LIMIT) {
 		*eof = 1;
@@ -158,8 +160,8 @@ static int prism2_bss_list_proc_read(char *page, char **start, off_t off,
 	spin_lock_bh(&local->lock);
 	list_for_each(ptr, &local->bss_list) {
 		bss = list_entry(ptr, struct hostap_bss_info, list);
-		p += sprintf(p, MACSTR "\t%lu\t%u\t0x%x\t",
-			     MAC2STR(bss->bssid), bss->last_update,
+		p += sprintf(p, "%s\t%lu\t%u\t0x%x\t",
+			     print_mac(mac, bss->bssid), bss->last_update,
 			     bss->count, bss->capab_info);
 		for (i = 0; i < bss->ssid_len; i++) {
 			p += sprintf(p, "%c",
@@ -312,6 +314,7 @@ static int prism2_scan_results_proc_read(char *page, char **start, off_t off,
 	int entry, i, len, total = 0;
 	struct hfa384x_hostscan_result *scanres;
 	u8 *pos;
+	DECLARE_MAC_BUF(mac);
 
 	p += sprintf(p, "CHID ANL SL BcnInt Capab Rate BSSID ATIM SupRates "
 		     "SSID\n");
@@ -329,14 +332,14 @@ static int prism2_scan_results_proc_read(char *page, char **start, off_t off,
 		if ((p - page) > (PAGE_SIZE - 200))
 			break;
 
-		p += sprintf(p, "%d %d %d %d 0x%02x %d " MACSTR " %d ",
+		p += sprintf(p, "%d %d %d %d 0x%02x %d %s %d ",
 			     le16_to_cpu(scanres->chid),
 			     (s16) le16_to_cpu(scanres->anl),
 			     (s16) le16_to_cpu(scanres->sl),
 			     le16_to_cpu(scanres->beacon_interval),
 			     le16_to_cpu(scanres->capability),
 			     le16_to_cpu(scanres->rate),
-			     MAC2STR(scanres->bssid),
+			     print_mac(mac, scanres->bssid),
 			     le16_to_cpu(scanres->atim));
 
 		pos = scanres->sup_rates;
diff --git a/drivers/net/wireless/hostap/hostap_wlan.h b/drivers/net/wireless/hostap/hostap_wlan.h
index 87a54aa6f4dd..e6516a186d0e 100644
--- a/drivers/net/wireless/hostap/hostap_wlan.h
+++ b/drivers/net/wireless/hostap/hostap_wlan.h
@@ -3,6 +3,7 @@
 
 #include <linux/wireless.h>
 #include <linux/netdevice.h>
+#include <linux/mutex.h>
 #include <net/iw_handler.h>
 
 #include "hostap_config.h"
@@ -641,7 +642,7 @@ struct local_info {
 			      * when removing entries from the list.
 			      * TX and RX paths can use read lock. */
 	spinlock_t cmdlock, baplock, lock;
-	struct semaphore rid_bap_sem;
+	struct mutex rid_bap_mtx;
 	u16 infofid; /* MAC buffer id for info frame */
 	/* txfid, intransmitfid, next_txtid, and next_alloc are protected by
 	 * txfidlock */
@@ -660,7 +661,7 @@ struct local_info {
 #define HOSTAP_BITS_TRANSMIT 0
 #define HOSTAP_BITS_BAP_TASKLET 1
 #define HOSTAP_BITS_BAP_TASKLET2 2
-	long bits;
+	unsigned long bits;
 
 	struct ap_data *ap;
 
@@ -735,8 +736,6 @@ struct local_info {
 		PRISM2_MONITOR_80211 = 0, PRISM2_MONITOR_PRISM = 1,
 		PRISM2_MONITOR_CAPHDR = 2
 	} monitor_type;
-	int (*saved_eth_header_parse)(struct sk_buff *skb,
-				      unsigned char *haddr);
 	int monitor_allow_fcserr;
 
 	int hostapd; /* whether user space daemon, hostapd, is used for AP
diff --git a/drivers/net/wireless/ipw2100.c b/drivers/net/wireless/ipw2100.c
index 8990585bd228..2d46a16c0945 100644
--- a/drivers/net/wireless/ipw2100.c
+++ b/drivers/net/wireless/ipw2100.c
@@ -1922,6 +1922,7 @@ static void isr_indicate_associated(struct ipw2100_priv *priv, u32 status)
 	u32 chan;
 	char *txratename;
 	u8 bssid[ETH_ALEN];
+	DECLARE_MAC_BUF(mac);
 
 	/*
 	 * TBD: BSSID is usually 00:00:00:00:00:00 here and not
@@ -1983,9 +1984,9 @@ static void isr_indicate_associated(struct ipw2100_priv *priv, u32 status)
 	}
 
 	IPW_DEBUG_INFO("%s: Associated with '%s' at %s, channel %d (BSSID="
-		       MAC_FMT ")\n",
+		       "%s)\n",
 		       priv->net_dev->name, escape_essid(essid, essid_len),
-		       txratename, chan, MAC_ARG(bssid));
+		       txratename, chan, print_mac(mac, bssid));
 
 	/* now we copy read ssid into dev */
 	if (!(priv->config & CFG_STATIC_ESSID)) {
@@ -2053,10 +2054,12 @@ static int ipw2100_set_essid(struct ipw2100_priv *priv, char *essid,
 
 static void isr_indicate_association_lost(struct ipw2100_priv *priv, u32 status)
 {
+	DECLARE_MAC_BUF(mac);
+
 	IPW_DEBUG(IPW_DL_NOTIF | IPW_DL_STATE | IPW_DL_ASSOC,
-		  "disassociated: '%s' " MAC_FMT " \n",
+		  "disassociated: '%s' %s \n",
 		  escape_essid(priv->essid, priv->essid_len),
-		  MAC_ARG(priv->bssid));
+		  print_mac(mac, priv->bssid));
 
 	priv->status &= ~(STATUS_ASSOCIATED | STATUS_ASSOCIATING);
 
@@ -4049,6 +4052,7 @@ static ssize_t show_bssinfo(struct device *d, struct device_attribute *attr,
 	char *out = buf;
 	int length;
 	int ret;
+	DECLARE_MAC_BUF(mac);
 
 	if (priv->status & STATUS_RF_KILL_MASK)
 		return 0;
@@ -4076,9 +4080,7 @@ static ssize_t show_bssinfo(struct device *d, struct device_attribute *attr,
 			       __LINE__);
 
 	out += sprintf(out, "ESSID: %s\n", essid);
-	out += sprintf(out, "BSSID:   %02x:%02x:%02x:%02x:%02x:%02x\n",
-		       bssid[0], bssid[1], bssid[2],
-		       bssid[3], bssid[4], bssid[5]);
+	out += sprintf(out, "BSSID:   %s\n", print_mac(mac, bssid));
 	out += sprintf(out, "Channel: %d\n", chan);
 
 	return out - buf;
@@ -4652,19 +4654,20 @@ static void ipw2100_rx_free(struct ipw2100_priv *priv)
 static int ipw2100_read_mac_address(struct ipw2100_priv *priv)
 {
 	u32 length = ETH_ALEN;
-	u8 mac[ETH_ALEN];
+	u8 addr[ETH_ALEN];
+	DECLARE_MAC_BUF(mac);
 
 	int err;
 
-	err = ipw2100_get_ordinal(priv, IPW_ORD_STAT_ADAPTER_MAC, mac, &length);
+	err = ipw2100_get_ordinal(priv, IPW_ORD_STAT_ADAPTER_MAC, addr, &length);
 	if (err) {
 		IPW_DEBUG_INFO("MAC address read failed\n");
 		return -EIO;
 	}
-	IPW_DEBUG_INFO("card MAC is %02X:%02X:%02X:%02X:%02X:%02X\n",
-		       mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
 
-	memcpy(priv->net_dev->dev_addr, mac, ETH_ALEN);
+	memcpy(priv->net_dev->dev_addr, addr, ETH_ALEN);
+	IPW_DEBUG_INFO("card MAC is %s\n",
+		       print_mac(mac, priv->net_dev->dev_addr));
 
 	return 0;
 }
@@ -5043,10 +5046,10 @@ static int ipw2100_set_mandatory_bssid(struct ipw2100_priv *priv, u8 * bssid,
 	int err;
 
 #ifdef CONFIG_IPW2100_DEBUG
+	DECLARE_MAC_BUF(mac);
 	if (bssid != NULL)
-		IPW_DEBUG_HC("MANDATORY_BSSID: %02X:%02X:%02X:%02X:%02X:%02X\n",
-			     bssid[0], bssid[1], bssid[2], bssid[3], bssid[4],
-			     bssid[5]);
+		IPW_DEBUG_HC("MANDATORY_BSSID: %s\n",
+			     print_mac(mac, bssid));
 	else
 		IPW_DEBUG_HC("MANDATORY_BSSID: <clear>\n");
 #endif
@@ -6239,8 +6242,6 @@ static int ipw2100_pci_init_one(struct pci_dev *pci_dev,
 
 	IPW_DEBUG_INFO("Attempting to register device...\n");
 
-	SET_MODULE_OWNER(dev);
-
 	printk(KERN_INFO DRV_NAME
 	       ": Detected Intel PRO/Wireless 2100 Network Connection\n");
 
@@ -6894,6 +6895,7 @@ static int ipw2100_wx_set_wap(struct net_device *dev,
 	static const unsigned char off[] = {
 		0x00, 0x00, 0x00, 0x00, 0x00, 0x00
 	};
+	DECLARE_MAC_BUF(mac);
 
 	// sanity checks
 	if (wrqu->ap_addr.sa_family != ARPHRD_ETHER)
@@ -6919,13 +6921,8 @@ static int ipw2100_wx_set_wap(struct net_device *dev,
 
 	err = ipw2100_set_mandatory_bssid(priv, wrqu->ap_addr.sa_data, 0);
 
-	IPW_DEBUG_WX("SET BSSID -> %02X:%02X:%02X:%02X:%02X:%02X\n",
-		     wrqu->ap_addr.sa_data[0] & 0xff,
-		     wrqu->ap_addr.sa_data[1] & 0xff,
-		     wrqu->ap_addr.sa_data[2] & 0xff,
-		     wrqu->ap_addr.sa_data[3] & 0xff,
-		     wrqu->ap_addr.sa_data[4] & 0xff,
-		     wrqu->ap_addr.sa_data[5] & 0xff);
+	IPW_DEBUG_WX("SET BSSID -> %s\n",
+		     print_mac(mac, wrqu->ap_addr.sa_data));
 
       done:
 	mutex_unlock(&priv->action_mutex);
@@ -6941,6 +6938,7 @@ static int ipw2100_wx_get_wap(struct net_device *dev,
 	 */
 
 	struct ipw2100_priv *priv = ieee80211_priv(dev);
+	DECLARE_MAC_BUF(mac);
 
 	/* If we are associated, trying to associate, or have a statically
 	 * configured BSSID then return that; otherwise return ANY */
@@ -6950,8 +6948,8 @@ static int ipw2100_wx_get_wap(struct net_device *dev,
 	} else
 		memset(wrqu->ap_addr.sa_data, 0, ETH_ALEN);
 
-	IPW_DEBUG_WX("Getting WAP BSSID: " MAC_FMT "\n",
-		     MAC_ARG(wrqu->ap_addr.sa_data));
+	IPW_DEBUG_WX("Getting WAP BSSID: %s\n",
+		     print_mac(mac, wrqu->ap_addr.sa_data));
 	return 0;
 }
 
@@ -8279,10 +8277,9 @@ static struct iw_statistics *ipw2100_wx_wireless_stats(struct net_device *dev)
 
 static struct iw_handler_def ipw2100_wx_handler_def = {
 	.standard = ipw2100_wx_handlers,
-	.num_standard = sizeof(ipw2100_wx_handlers) / sizeof(iw_handler),
-	.num_private = sizeof(ipw2100_private_handler) / sizeof(iw_handler),
-	.num_private_args = sizeof(ipw2100_private_args) /
-	    sizeof(struct iw_priv_args),
+	.num_standard = ARRAY_SIZE(ipw2100_wx_handlers),
+	.num_private = ARRAY_SIZE(ipw2100_private_handler),
+	.num_private_args = ARRAY_SIZE(ipw2100_private_args),
 	.private = (iw_handler *) ipw2100_private_handler,
 	.private_args = (struct iw_priv_args *)ipw2100_private_args,
 	.get_wireless_stats = ipw2100_wx_wireless_stats,
diff --git a/drivers/net/wireless/ipw2200.c b/drivers/net/wireless/ipw2200.c
index 61497c467467..feb8fcbab2d5 100644
--- a/drivers/net/wireless/ipw2200.c
+++ b/drivers/net/wireless/ipw2200.c
@@ -1740,8 +1740,10 @@ static int ipw_radio_kill_sw(struct ipw_priv *priv, int disable_radio)
 	if (disable_radio) {
 		priv->status |= STATUS_RF_KILL_SW;
 
-		if (priv->workqueue)
+		if (priv->workqueue) {
 			cancel_delayed_work(&priv->request_scan);
+			cancel_delayed_work(&priv->scan_event);
+		}
 		queue_work(priv->workqueue, &priv->down);
 	} else {
 		priv->status &= ~STATUS_RF_KILL_SW;
@@ -1992,6 +1994,7 @@ static void ipw_irq_tasklet(struct ipw_priv *priv)
 		wake_up_interruptible(&priv->wait_command_queue);
 		priv->status &= ~(STATUS_ASSOCIATED | STATUS_ASSOCIATING);
 		cancel_delayed_work(&priv->request_scan);
+		cancel_delayed_work(&priv->scan_event);
 		schedule_work(&priv->link_down);
 		queue_delayed_work(priv->workqueue, &priv->rf_kill, 2 * HZ);
 		handled |= IPW_INTA_BIT_RF_KILL_DONE;
@@ -2247,8 +2250,8 @@ static int ipw_send_adapter_address(struct ipw_priv *priv, u8 * mac)
 		return -1;
 	}
 
-	IPW_DEBUG_INFO("%s: Setting MAC to " MAC_FMT "\n",
-		       priv->net_dev->name, MAC_ARG(mac));
+	IPW_DEBUG_INFO("%s: Setting MAC to %s\n",
+		       priv->net_dev->name, print_mac(mac, mac));
 
 	return ipw_send_cmd_pdu(priv, IPW_CMD_ADAPTER_ADDRESS, ETH_ALEN, mac);
 }
@@ -3796,6 +3799,7 @@ static u8 ipw_add_station(struct ipw_priv *priv, u8 * bssid)
 {
 	struct ipw_station_entry entry;
 	int i;
+	DECLARE_MAC_BUF(mac);
 
 	for (i = 0; i < priv->num_stations; i++) {
 		if (!memcmp(priv->stations[i], bssid, ETH_ALEN)) {
@@ -3812,7 +3816,7 @@ static u8 ipw_add_station(struct ipw_priv *priv, u8 * bssid)
 	if (i == MAX_STATIONS)
 		return IPW_INVALID_STATION;
 
-	IPW_DEBUG_SCAN("Adding AdHoc station: " MAC_FMT "\n", MAC_ARG(bssid));
+	IPW_DEBUG_SCAN("Adding AdHoc station: %s\n", print_mac(mac, bssid));
 
 	entry.reserved = 0;
 	entry.support_mode = 0;
@@ -3839,6 +3843,7 @@ static u8 ipw_find_station(struct ipw_priv *priv, u8 * bssid)
 static void ipw_send_disassociate(struct ipw_priv *priv, int quiet)
 {
 	int err;
+	DECLARE_MAC_BUF(mac);
 
 	if (priv->status & STATUS_ASSOCIATING) {
 		IPW_DEBUG_ASSOC("Disassociating while associating.\n");
@@ -3851,9 +3856,9 @@ static void ipw_send_disassociate(struct ipw_priv *priv, int quiet)
 		return;
 	}
 
-	IPW_DEBUG_ASSOC("Disassocation attempt from " MAC_FMT " "
+	IPW_DEBUG_ASSOC("Disassocation attempt from %s "
 			"on channel %d.\n",
-			MAC_ARG(priv->assoc_request.bssid),
+			print_mac(mac, priv->assoc_request.bssid),
 			priv->assoc_request.channel);
 
 	priv->status &= ~(STATUS_ASSOCIATING | STATUS_ASSOCIATED);
@@ -4341,6 +4346,37 @@ static void ipw_handle_missed_beacon(struct ipw_priv *priv,
 	IPW_DEBUG_NOTIF("Missed beacon: %d\n", missed_count);
 }
 
+static void ipw_scan_event(struct work_struct *work)
+{
+	union iwreq_data wrqu;
+
+	struct ipw_priv *priv =
+		container_of(work, struct ipw_priv, scan_event.work);
+
+	wrqu.data.length = 0;
+	wrqu.data.flags = 0;
+	wireless_send_event(priv->net_dev, SIOCGIWSCAN, &wrqu, NULL);
+}
+
+static void handle_scan_event(struct ipw_priv *priv)
+{
+	/* Only userspace-requested scan completion events go out immediately */
+	if (!priv->user_requested_scan) {
+		if (!delayed_work_pending(&priv->scan_event))
+			queue_delayed_work(priv->workqueue, &priv->scan_event,
+					 round_jiffies(msecs_to_jiffies(4000)));
+	} else {
+		union iwreq_data wrqu;
+
+		priv->user_requested_scan = 0;
+		cancel_delayed_work(&priv->scan_event);
+
+		wrqu.data.length = 0;
+		wrqu.data.flags = 0;
+		wireless_send_event(priv->net_dev, SIOCGIWSCAN, &wrqu, NULL);
+	}
+}
+
 /**
  * Handle host notification packet.
  * Called from interrupt routine
@@ -4348,6 +4384,7 @@ static void ipw_handle_missed_beacon(struct ipw_priv *priv,
 static void ipw_rx_notification(struct ipw_priv *priv,
 				       struct ipw_rx_notification *notif)
 {
+	DECLARE_MAC_BUF(mac);
 	notif->size = le16_to_cpu(notif->size);
 
 	IPW_DEBUG_NOTIF("type = %i (%d bytes)\n", notif->subtype, notif->size);
@@ -4360,11 +4397,11 @@ static void ipw_rx_notification(struct ipw_priv *priv,
 			case CMAS_ASSOCIATED:{
 					IPW_DEBUG(IPW_DL_NOTIF | IPW_DL_STATE |
 						  IPW_DL_ASSOC,
-						  "associated: '%s' " MAC_FMT
+						  "associated: '%s' %s"
 						  " \n",
 						  escape_essid(priv->essid,
 							       priv->essid_len),
-						  MAC_ARG(priv->bssid));
+						  print_mac(mac, priv->bssid));
 
 					switch (priv->ieee->iw_mode) {
 					case IW_MODE_INFRA:
@@ -4444,13 +4481,13 @@ static void ipw_rx_notification(struct ipw_priv *priv,
 							  IPW_DL_STATE |
 							  IPW_DL_ASSOC,
 							  "deauthenticated: '%s' "
-							  MAC_FMT
+							  "%s"
 							  ": (0x%04X) - %s \n",
 							  escape_essid(priv->
 								       essid,
 								       priv->
 								       essid_len),
-							  MAC_ARG(priv->bssid),
+							  print_mac(mac, priv->bssid),
 							  ntohs(auth->status),
 							  ipw_get_status_code
 							  (ntohs
@@ -4467,11 +4504,11 @@ static void ipw_rx_notification(struct ipw_priv *priv,
 
 					IPW_DEBUG(IPW_DL_NOTIF | IPW_DL_STATE |
 						  IPW_DL_ASSOC,
-						  "authenticated: '%s' " MAC_FMT
+						  "authenticated: '%s' %s"
 						  "\n",
 						  escape_essid(priv->essid,
 							       priv->essid_len),
-						  MAC_ARG(priv->bssid));
+						  print_mac(mac, priv->bssid));
 					break;
 				}
 
@@ -4496,11 +4533,11 @@ static void ipw_rx_notification(struct ipw_priv *priv,
 
 					IPW_DEBUG(IPW_DL_NOTIF | IPW_DL_STATE |
 						  IPW_DL_ASSOC,
-						  "disassociated: '%s' " MAC_FMT
+						  "disassociated: '%s' %s"
 						  " \n",
 						  escape_essid(priv->essid,
 							       priv->essid_len),
-						  MAC_ARG(priv->bssid));
+						  print_mac(mac, priv->bssid));
 
 					priv->status &=
 					    ~(STATUS_DISASSOCIATING |
@@ -4535,10 +4572,10 @@ static void ipw_rx_notification(struct ipw_priv *priv,
 			switch (auth->state) {
 			case CMAS_AUTHENTICATED:
 				IPW_DEBUG(IPW_DL_NOTIF | IPW_DL_STATE,
-					  "authenticated: '%s' " MAC_FMT " \n",
+					  "authenticated: '%s' %s \n",
 					  escape_essid(priv->essid,
 						       priv->essid_len),
-					  MAC_ARG(priv->bssid));
+					  print_mac(mac, priv->bssid));
 				priv->status |= STATUS_AUTH;
 				break;
 
@@ -4554,10 +4591,10 @@ static void ipw_rx_notification(struct ipw_priv *priv,
 				}
 				IPW_DEBUG(IPW_DL_NOTIF | IPW_DL_STATE |
 					  IPW_DL_ASSOC,
-					  "deauthenticated: '%s' " MAC_FMT "\n",
+					  "deauthenticated: '%s' %s\n",
 					  escape_essid(priv->essid,
 						       priv->essid_len),
-					  MAC_ARG(priv->bssid));
+					  print_mac(mac, priv->bssid));
 
 				priv->status &= ~(STATUS_ASSOCIATING |
 						  STATUS_AUTH |
@@ -4702,14 +4739,8 @@ static void ipw_rx_notification(struct ipw_priv *priv,
 			 * on how the scan was initiated. User space can just
 			 * sync on periodic scan to get fresh data...
 			 * Jean II */
-			if (x->status == SCAN_COMPLETED_STATUS_COMPLETE) {
-				union iwreq_data wrqu;
-
-				wrqu.data.length = 0;
-				wrqu.data.flags = 0;
-				wireless_send_event(priv->net_dev, SIOCGIWSCAN,
-						    &wrqu, NULL);
-			}
+			if (x->status == SCAN_COMPLETED_STATUS_COMPLETE)
+				handle_scan_event(priv);
 			break;
 		}
 
@@ -5383,25 +5414,27 @@ static int ipw_find_adhoc_network(struct ipw_priv *priv,
 				  int roaming)
 {
 	struct ipw_supported_rates rates;
+	DECLARE_MAC_BUF(mac);
+	DECLARE_MAC_BUF(mac2);
 
 	/* Verify that this network's capability is compatible with the
 	 * current mode (AdHoc or Infrastructure) */
 	if ((priv->ieee->iw_mode == IW_MODE_ADHOC &&
 	     !(network->capability & WLAN_CAPABILITY_IBSS))) {
-		IPW_DEBUG_MERGE("Network '%s (" MAC_FMT ")' excluded due to "
+		IPW_DEBUG_MERGE("Network '%s (%s)' excluded due to "
 				"capability mismatch.\n",
 				escape_essid(network->ssid, network->ssid_len),
-				MAC_ARG(network->bssid));
+				print_mac(mac, network->bssid));
 		return 0;
 	}
 
 	/* If we do not have an ESSID for this AP, we can not associate with
 	 * it */
 	if (network->flags & NETWORK_EMPTY_ESSID) {
-		IPW_DEBUG_MERGE("Network '%s (" MAC_FMT ")' excluded "
+		IPW_DEBUG_MERGE("Network '%s (%s)' excluded "
 				"because of hidden ESSID.\n",
 				escape_essid(network->ssid, network->ssid_len),
-				MAC_ARG(network->bssid));
+				print_mac(mac, network->bssid));
 		return 0;
 	}
 
@@ -5411,11 +5444,11 @@ static int ipw_find_adhoc_network(struct ipw_priv *priv,
 		if ((network->ssid_len != match->network->ssid_len) ||
 		    memcmp(network->ssid, match->network->ssid,
 			   network->ssid_len)) {
-			IPW_DEBUG_MERGE("Netowrk '%s (" MAC_FMT ")' excluded "
+			IPW_DEBUG_MERGE("Network '%s (%s)' excluded "
 					"because of non-network ESSID.\n",
 					escape_essid(network->ssid,
 						     network->ssid_len),
-					MAC_ARG(network->bssid));
+					print_mac(mac, network->bssid));
 			return 0;
 		}
 	} else {
@@ -5430,9 +5463,9 @@ static int ipw_find_adhoc_network(struct ipw_priv *priv,
 			strncpy(escaped,
 				escape_essid(network->ssid, network->ssid_len),
 				sizeof(escaped));
-			IPW_DEBUG_MERGE("Network '%s (" MAC_FMT ")' excluded "
+			IPW_DEBUG_MERGE("Network '%s (%s)' excluded "
 					"because of ESSID mismatch: '%s'.\n",
-					escaped, MAC_ARG(network->bssid),
+					escaped, print_mac(mac, network->bssid),
 					escape_essid(priv->essid,
 						     priv->essid_len));
 			return 0;
@@ -5459,10 +5492,10 @@ static int ipw_find_adhoc_network(struct ipw_priv *priv,
 	/* Now go through and see if the requested network is valid... */
 	if (priv->ieee->scan_age != 0 &&
 	    time_after(jiffies, network->last_scanned + priv->ieee->scan_age)) {
-		IPW_DEBUG_MERGE("Network '%s (" MAC_FMT ")' excluded "
+		IPW_DEBUG_MERGE("Network '%s (%s)' excluded "
 				"because of age: %ums.\n",
 				escape_essid(network->ssid, network->ssid_len),
-				MAC_ARG(network->bssid),
+				print_mac(mac, network->bssid),
 				jiffies_to_msecs(jiffies -
 						 network->last_scanned));
 		return 0;
@@ -5470,10 +5503,10 @@ static int ipw_find_adhoc_network(struct ipw_priv *priv,
 
 	if ((priv->config & CFG_STATIC_CHANNEL) &&
 	    (network->channel != priv->channel)) {
-		IPW_DEBUG_MERGE("Network '%s (" MAC_FMT ")' excluded "
+		IPW_DEBUG_MERGE("Network '%s (%s)' excluded "
 				"because of channel mismatch: %d != %d.\n",
 				escape_essid(network->ssid, network->ssid_len),
-				MAC_ARG(network->bssid),
+				print_mac(mac, network->bssid),
 				network->channel, priv->channel);
 		return 0;
 	}
@@ -5481,10 +5514,10 @@ static int ipw_find_adhoc_network(struct ipw_priv *priv,
 	/* Verify privacy compatability */
 	if (((priv->capability & CAP_PRIVACY_ON) ? 1 : 0) !=
 	    ((network->capability & WLAN_CAPABILITY_PRIVACY) ? 1 : 0)) {
-		IPW_DEBUG_MERGE("Network '%s (" MAC_FMT ")' excluded "
+		IPW_DEBUG_MERGE("Network '%s (%s)' excluded "
 				"because of privacy mismatch: %s != %s.\n",
 				escape_essid(network->ssid, network->ssid_len),
-				MAC_ARG(network->bssid),
+				print_mac(mac, network->bssid),
 				priv->
 				capability & CAP_PRIVACY_ON ? "on" : "off",
 				network->
@@ -5494,40 +5527,41 @@ static int ipw_find_adhoc_network(struct ipw_priv *priv,
 	}
 
 	if (!memcmp(network->bssid, priv->bssid, ETH_ALEN)) {
-		IPW_DEBUG_MERGE("Network '%s (" MAC_FMT ")' excluded "
-				"because of the same BSSID match: " MAC_FMT
+		IPW_DEBUG_MERGE("Network '%s (%s)' excluded "
+				"because of the same BSSID match: %s"
 				".\n", escape_essid(network->ssid,
 						    network->ssid_len),
-				MAC_ARG(network->bssid), MAC_ARG(priv->bssid));
+				print_mac(mac, network->bssid),
+				print_mac(mac2, priv->bssid));
 		return 0;
 	}
 
 	/* Filter out any incompatible freq / mode combinations */
 	if (!ieee80211_is_valid_mode(priv->ieee, network->mode)) {
-		IPW_DEBUG_MERGE("Network '%s (" MAC_FMT ")' excluded "
+		IPW_DEBUG_MERGE("Network '%s (%s)' excluded "
 				"because of invalid frequency/mode "
 				"combination.\n",
 				escape_essid(network->ssid, network->ssid_len),
-				MAC_ARG(network->bssid));
+				print_mac(mac, network->bssid));
 		return 0;
 	}
 
 	/* Ensure that the rates supported by the driver are compatible with
 	 * this AP, including verification of basic rates (mandatory) */
 	if (!ipw_compatible_rates(priv, network, &rates)) {
-		IPW_DEBUG_MERGE("Network '%s (" MAC_FMT ")' excluded "
+		IPW_DEBUG_MERGE("Network '%s (%s)' excluded "
 				"because configured rate mask excludes "
 				"AP mandatory rate.\n",
 				escape_essid(network->ssid, network->ssid_len),
-				MAC_ARG(network->bssid));
+				print_mac(mac, network->bssid));
 		return 0;
 	}
 
 	if (rates.num_rates == 0) {
-		IPW_DEBUG_MERGE("Network '%s (" MAC_FMT ")' excluded "
+		IPW_DEBUG_MERGE("Network '%s (%s)' excluded "
 				"because of no compatible rates.\n",
 				escape_essid(network->ssid, network->ssid_len),
-				MAC_ARG(network->bssid));
+				print_mac(mac, network->bssid));
 		return 0;
 	}
 
@@ -5538,9 +5572,9 @@ static int ipw_find_adhoc_network(struct ipw_priv *priv,
 	/* Set up 'new' AP to this network */
 	ipw_copy_rates(&match->rates, &rates);
 	match->network = network;
-	IPW_DEBUG_MERGE("Network '%s (" MAC_FMT ")' is a viable match.\n",
+	IPW_DEBUG_MERGE("Network '%s (%s)' is a viable match.\n",
 			escape_essid(network->ssid, network->ssid_len),
-			MAC_ARG(network->bssid));
+			print_mac(mac, network->bssid));
 
 	return 1;
 }
@@ -5594,6 +5628,7 @@ static int ipw_best_network(struct ipw_priv *priv,
 			    struct ieee80211_network *network, int roaming)
 {
 	struct ipw_supported_rates rates;
+	DECLARE_MAC_BUF(mac);
 
 	/* Verify that this network's capability is compatible with the
 	 * current mode (AdHoc or Infrastructure) */
@@ -5601,20 +5636,20 @@ static int ipw_best_network(struct ipw_priv *priv,
 	     !(network->capability & WLAN_CAPABILITY_ESS)) ||
 	    (priv->ieee->iw_mode == IW_MODE_ADHOC &&
 	     !(network->capability & WLAN_CAPABILITY_IBSS))) {
-		IPW_DEBUG_ASSOC("Network '%s (" MAC_FMT ")' excluded due to "
+		IPW_DEBUG_ASSOC("Network '%s (%s)' excluded due to "
 				"capability mismatch.\n",
 				escape_essid(network->ssid, network->ssid_len),
-				MAC_ARG(network->bssid));
+				print_mac(mac, network->bssid));
 		return 0;
 	}
 
 	/* If we do not have an ESSID for this AP, we can not associate with
 	 * it */
 	if (network->flags & NETWORK_EMPTY_ESSID) {
-		IPW_DEBUG_ASSOC("Network '%s (" MAC_FMT ")' excluded "
+		IPW_DEBUG_ASSOC("Network '%s (%s)' excluded "
 				"because of hidden ESSID.\n",
 				escape_essid(network->ssid, network->ssid_len),
-				MAC_ARG(network->bssid));
+				print_mac(mac, network->bssid));
 		return 0;
 	}
 
@@ -5624,11 +5659,11 @@ static int ipw_best_network(struct ipw_priv *priv,
 		if ((network->ssid_len != match->network->ssid_len) ||
 		    memcmp(network->ssid, match->network->ssid,
 			   network->ssid_len)) {
-			IPW_DEBUG_ASSOC("Netowrk '%s (" MAC_FMT ")' excluded "
+			IPW_DEBUG_ASSOC("Network '%s (%s)' excluded "
 					"because of non-network ESSID.\n",
 					escape_essid(network->ssid,
 						     network->ssid_len),
-					MAC_ARG(network->bssid));
+					print_mac(mac, network->bssid));
 			return 0;
 		}
 	} else {
@@ -5642,9 +5677,9 @@ static int ipw_best_network(struct ipw_priv *priv,
 			strncpy(escaped,
 				escape_essid(network->ssid, network->ssid_len),
 				sizeof(escaped));
-			IPW_DEBUG_ASSOC("Network '%s (" MAC_FMT ")' excluded "
+			IPW_DEBUG_ASSOC("Network '%s (%s)' excluded "
 					"because of ESSID mismatch: '%s'.\n",
-					escaped, MAC_ARG(network->bssid),
+					escaped, print_mac(mac, network->bssid),
 					escape_essid(priv->essid,
 						     priv->essid_len));
 			return 0;
@@ -5658,12 +5693,12 @@ static int ipw_best_network(struct ipw_priv *priv,
 		strncpy(escaped,
 			escape_essid(network->ssid, network->ssid_len),
 			sizeof(escaped));
-		IPW_DEBUG_ASSOC("Network '%s (" MAC_FMT ")' excluded because "
-				"'%s (" MAC_FMT ")' has a stronger signal.\n",
-				escaped, MAC_ARG(network->bssid),
+		IPW_DEBUG_ASSOC("Network '%s (%s)' excluded because "
+				"'%s (%s)' has a stronger signal.\n",
+				escaped, print_mac(mac, network->bssid),
 				escape_essid(match->network->ssid,
 					     match->network->ssid_len),
-				MAC_ARG(match->network->bssid));
+				print_mac(mac, match->network->bssid));
 		return 0;
 	}
 
@@ -5671,11 +5706,11 @@ static int ipw_best_network(struct ipw_priv *priv,
 	 * last 3 seconds, do not try and associate again... */
 	if (network->last_associate &&
 	    time_after(network->last_associate + (HZ * 3UL), jiffies)) {
-		IPW_DEBUG_ASSOC("Network '%s (" MAC_FMT ")' excluded "
+		IPW_DEBUG_ASSOC("Network '%s (%s)' excluded "
 				"because of storming (%ums since last "
 				"assoc attempt).\n",
 				escape_essid(network->ssid, network->ssid_len),
-				MAC_ARG(network->bssid),
+				print_mac(mac, network->bssid),
 				jiffies_to_msecs(jiffies -
 						 network->last_associate));
 		return 0;
@@ -5684,10 +5719,10 @@ static int ipw_best_network(struct ipw_priv *priv,
 	/* Now go through and see if the requested network is valid... */
 	if (priv->ieee->scan_age != 0 &&
 	    time_after(jiffies, network->last_scanned + priv->ieee->scan_age)) {
-		IPW_DEBUG_ASSOC("Network '%s (" MAC_FMT ")' excluded "
+		IPW_DEBUG_ASSOC("Network '%s (%s)' excluded "
 				"because of age: %ums.\n",
 				escape_essid(network->ssid, network->ssid_len),
-				MAC_ARG(network->bssid),
+				print_mac(mac, network->bssid),
 				jiffies_to_msecs(jiffies -
 						 network->last_scanned));
 		return 0;
@@ -5695,10 +5730,10 @@ static int ipw_best_network(struct ipw_priv *priv,
 
 	if ((priv->config & CFG_STATIC_CHANNEL) &&
 	    (network->channel != priv->channel)) {
-		IPW_DEBUG_ASSOC("Network '%s (" MAC_FMT ")' excluded "
+		IPW_DEBUG_ASSOC("Network '%s (%s)' excluded "
 				"because of channel mismatch: %d != %d.\n",
 				escape_essid(network->ssid, network->ssid_len),
-				MAC_ARG(network->bssid),
+				print_mac(mac, network->bssid),
 				network->channel, priv->channel);
 		return 0;
 	}
@@ -5706,10 +5741,10 @@ static int ipw_best_network(struct ipw_priv *priv,
 	/* Verify privacy compatability */
 	if (((priv->capability & CAP_PRIVACY_ON) ? 1 : 0) !=
 	    ((network->capability & WLAN_CAPABILITY_PRIVACY) ? 1 : 0)) {
-		IPW_DEBUG_ASSOC("Network '%s (" MAC_FMT ")' excluded "
+		IPW_DEBUG_ASSOC("Network '%s (%s)' excluded "
 				"because of privacy mismatch: %s != %s.\n",
 				escape_essid(network->ssid, network->ssid_len),
-				MAC_ARG(network->bssid),
+				print_mac(mac, network->bssid),
 				priv->capability & CAP_PRIVACY_ON ? "on" :
 				"off",
 				network->capability &
@@ -5719,48 +5754,48 @@ static int ipw_best_network(struct ipw_priv *priv,
 
 	if ((priv->config & CFG_STATIC_BSSID) &&
 	    memcmp(network->bssid, priv->bssid, ETH_ALEN)) {
-		IPW_DEBUG_ASSOC("Network '%s (" MAC_FMT ")' excluded "
-				"because of BSSID mismatch: " MAC_FMT ".\n",
+		IPW_DEBUG_ASSOC("Network '%s (%s)' excluded "
+				"because of BSSID mismatch: %s.\n",
 				escape_essid(network->ssid, network->ssid_len),
-				MAC_ARG(network->bssid), MAC_ARG(priv->bssid));
+				print_mac(mac, network->bssid), print_mac(mac, priv->bssid));
 		return 0;
 	}
 
 	/* Filter out any incompatible freq / mode combinations */
 	if (!ieee80211_is_valid_mode(priv->ieee, network->mode)) {
-		IPW_DEBUG_ASSOC("Network '%s (" MAC_FMT ")' excluded "
+		IPW_DEBUG_ASSOC("Network '%s (%s)' excluded "
 				"because of invalid frequency/mode "
 				"combination.\n",
 				escape_essid(network->ssid, network->ssid_len),
-				MAC_ARG(network->bssid));
+				print_mac(mac, network->bssid));
 		return 0;
 	}
 
 	/* Filter out invalid channel in current GEO */
 	if (!ieee80211_is_valid_channel(priv->ieee, network->channel)) {
-		IPW_DEBUG_ASSOC("Network '%s (" MAC_FMT ")' excluded "
+		IPW_DEBUG_ASSOC("Network '%s (%s)' excluded "
 				"because of invalid channel in current GEO\n",
 				escape_essid(network->ssid, network->ssid_len),
-				MAC_ARG(network->bssid));
+				print_mac(mac, network->bssid));
 		return 0;
 	}
 
 	/* Ensure that the rates supported by the driver are compatible with
 	 * this AP, including verification of basic rates (mandatory) */
 	if (!ipw_compatible_rates(priv, network, &rates)) {
-		IPW_DEBUG_ASSOC("Network '%s (" MAC_FMT ")' excluded "
+		IPW_DEBUG_ASSOC("Network '%s (%s)' excluded "
 				"because configured rate mask excludes "
 				"AP mandatory rate.\n",
 				escape_essid(network->ssid, network->ssid_len),
-				MAC_ARG(network->bssid));
+				print_mac(mac, network->bssid));
 		return 0;
 	}
 
 	if (rates.num_rates == 0) {
-		IPW_DEBUG_ASSOC("Network '%s (" MAC_FMT ")' excluded "
+		IPW_DEBUG_ASSOC("Network '%s (%s)' excluded "
 				"because of no compatible rates.\n",
 				escape_essid(network->ssid, network->ssid_len),
-				MAC_ARG(network->bssid));
+				print_mac(mac, network->bssid));
 		return 0;
 	}
 
@@ -5772,9 +5807,9 @@ static int ipw_best_network(struct ipw_priv *priv,
 	ipw_copy_rates(&match->rates, &rates);
 	match->network = network;
 
-	IPW_DEBUG_ASSOC("Network '%s (" MAC_FMT ")' is a viable match.\n",
+	IPW_DEBUG_ASSOC("Network '%s (%s)' is a viable match.\n",
 			escape_essid(network->ssid, network->ssid_len),
-			MAC_ARG(network->bssid));
+			print_mac(mac, network->bssid));
 
 	return 1;
 }
@@ -6016,6 +6051,7 @@ static void ipw_bg_adhoc_check(struct work_struct *work)
 
 static void ipw_debug_config(struct ipw_priv *priv)
 {
+	DECLARE_MAC_BUF(mac);
 	IPW_DEBUG_INFO("Scan completed, no valid APs matched "
 		       "[CFG 0x%08X]\n", priv->config);
 	if (priv->config & CFG_STATIC_CHANNEL)
@@ -6028,8 +6064,8 @@ static void ipw_debug_config(struct ipw_priv *priv)
 	else
 		IPW_DEBUG_INFO("ESSID unlocked.\n");
 	if (priv->config & CFG_STATIC_BSSID)
-		IPW_DEBUG_INFO("BSSID locked to " MAC_FMT "\n",
-			       MAC_ARG(priv->bssid));
+		IPW_DEBUG_INFO("BSSID locked to %s\n",
+			       print_mac(mac, priv->bssid));
 	else
 		IPW_DEBUG_INFO("BSSID unlocked.\n");
 	if (priv->capability & CAP_PRIVACY_ON)
@@ -7221,6 +7257,7 @@ static int ipw_associate_network(struct ipw_priv *priv,
 				 struct ipw_supported_rates *rates, int roaming)
 {
 	int err;
+	DECLARE_MAC_BUF(mac);
 
 	if (priv->config & CFG_FIXED_RATE)
 		ipw_set_fixed_rate(priv, network->mode);
@@ -7388,9 +7425,9 @@ static int ipw_associate_network(struct ipw_priv *priv,
 		return err;
 	}
 
-	IPW_DEBUG(IPW_DL_STATE, "associating: '%s' " MAC_FMT " \n",
+	IPW_DEBUG(IPW_DL_STATE, "associating: '%s' %s \n",
 		  escape_essid(priv->essid, priv->essid_len),
-		  MAC_ARG(priv->bssid));
+		  print_mac(mac, priv->bssid));
 
 	return 0;
 }
@@ -8202,6 +8239,9 @@ static void ipw_rx(struct ipw_priv *priv)
 	struct ieee80211_hdr_4addr *header;
 	u32 r, w, i;
 	u8 network_packet;
+	DECLARE_MAC_BUF(mac);
+	DECLARE_MAC_BUF(mac2);
+	DECLARE_MAC_BUF(mac3);
 
 	r = ipw_read32(priv, IPW_RX_READ_INDEX);
 	w = ipw_read32(priv, IPW_RX_WRITE_INDEX);
@@ -8328,14 +8368,17 @@ static void ipw_rx(struct ipw_priv *priv)
 									 header)))
 					{
 						IPW_DEBUG_DROP("Dropping: "
-							       MAC_FMT ", "
-							       MAC_FMT ", "
-							       MAC_FMT "\n",
-							       MAC_ARG(header->
+							       "%s, "
+							       "%s, "
+							       "%s\n",
+							       print_mac(mac,
+									 header->
 								       addr1),
-							       MAC_ARG(header->
+							       print_mac(mac2,
+									 header->
 								       addr2),
-							       MAC_ARG(header->
+							       print_mac(mac3,
+									 header->
 								       addr3));
 						break;
 					}
@@ -8867,6 +8910,7 @@ static int ipw_wx_set_wap(struct net_device *dev,
 			  union iwreq_data *wrqu, char *extra)
 {
 	struct ipw_priv *priv = ieee80211_priv(dev);
+	DECLARE_MAC_BUF(mac);
 
 	static const unsigned char any[] = {
 		0xff, 0xff, 0xff, 0xff, 0xff, 0xff
@@ -8897,8 +8941,8 @@ static int ipw_wx_set_wap(struct net_device *dev,
 		return 0;
 	}
 
-	IPW_DEBUG_WX("Setting mandatory BSSID to " MAC_FMT "\n",
-		     MAC_ARG(wrqu->ap_addr.sa_data));
+	IPW_DEBUG_WX("Setting mandatory BSSID to %s\n",
+		     print_mac(mac, wrqu->ap_addr.sa_data));
 
 	memcpy(priv->bssid, wrqu->ap_addr.sa_data, ETH_ALEN);
 
@@ -8916,6 +8960,8 @@ static int ipw_wx_get_wap(struct net_device *dev,
 			  union iwreq_data *wrqu, char *extra)
 {
 	struct ipw_priv *priv = ieee80211_priv(dev);
+	DECLARE_MAC_BUF(mac);
+
 	/* If we are associated, trying to associate, or have a statically
 	 * configured BSSID then return that; otherwise return ANY */
 	mutex_lock(&priv->mutex);
@@ -8926,8 +8972,8 @@ static int ipw_wx_get_wap(struct net_device *dev,
 	} else
 		memset(wrqu->ap_addr.sa_data, 0, ETH_ALEN);
 
-	IPW_DEBUG_WX("Getting WAP BSSID: " MAC_FMT "\n",
-		     MAC_ARG(wrqu->ap_addr.sa_data));
+	IPW_DEBUG_WX("Getting WAP BSSID: %s\n",
+		     print_mac(mac, wrqu->ap_addr.sa_data));
 	mutex_unlock(&priv->mutex);
 	return 0;
 }
@@ -9472,6 +9518,10 @@ static int ipw_wx_set_scan(struct net_device *dev,
 	struct ipw_priv *priv = ieee80211_priv(dev);
 	struct iw_scan_req *req = (struct iw_scan_req *)extra;
 
+	mutex_lock(&priv->mutex);
+	priv->user_requested_scan = 1;
+	mutex_unlock(&priv->mutex);
+
 	if (wrqu->data.length == sizeof(struct iw_scan_req)) {
 		if (wrqu->data.flags & IW_SCAN_THIS_ESSID) {
 			ipw_request_direct_scan(priv, req->essid,
@@ -10133,6 +10183,7 @@ static int ipw_tx_skb(struct ipw_priv *priv, struct ieee80211_txb *txb,
 	u8 id, hdr_len, unicast;
 	u16 remaining_bytes;
 	int fc;
+	DECLARE_MAC_BUF(mac);
 
 	hdr_len = ieee80211_get_hdrlen(le16_to_cpu(hdr->frame_ctl));
 	switch (priv->ieee->iw_mode) {
@@ -10143,8 +10194,8 @@ static int ipw_tx_skb(struct ipw_priv *priv, struct ieee80211_txb *txb,
 			id = ipw_add_station(priv, hdr->addr1);
 			if (id == IPW_INVALID_STATION) {
 				IPW_WARNING("Attempt to send data to "
-					    "invalid cell: " MAC_FMT "\n",
-					    MAC_ARG(hdr->addr1));
+					    "invalid cell: %s\n",
+					    print_mac(mac, hdr->addr1));
 				goto drop;
 			}
 		}
@@ -10460,13 +10511,15 @@ static int ipw_net_set_mac_address(struct net_device *dev, void *p)
 {
 	struct ipw_priv *priv = ieee80211_priv(dev);
 	struct sockaddr *addr = p;
+	DECLARE_MAC_BUF(mac);
+
 	if (!is_valid_ether_addr(addr->sa_data))
 		return -EADDRNOTAVAIL;
 	mutex_lock(&priv->mutex);
 	priv->config |= CFG_CUSTOM_MAC;
 	memcpy(priv->mac_addr, addr->sa_data, ETH_ALEN);
-	printk(KERN_INFO "%s: Setting MAC to " MAC_FMT "\n",
-	       priv->net_dev->name, MAC_ARG(priv->mac_addr));
+	printk(KERN_INFO "%s: Setting MAC to %s\n",
+	       priv->net_dev->name, print_mac(mac, priv->mac_addr));
 	queue_work(priv->workqueue, &priv->adapter_restart);
 	mutex_unlock(&priv->mutex);
 	return 0;
@@ -10647,6 +10700,7 @@ static void ipw_link_up(struct ipw_priv *priv)
 	}
 
 	cancel_delayed_work(&priv->request_scan);
+	cancel_delayed_work(&priv->scan_event);
 	ipw_reset_stats(priv);
 	/* Ensure the rate is updated immediately */
 	priv->last_rate = ipw_get_current_rate(priv);
@@ -10684,7 +10738,8 @@ static void ipw_link_down(struct ipw_priv *priv)
 	if (!(priv->status & STATUS_EXIT_PENDING)) {
 		/* Queue up another scan... */
 		queue_delayed_work(priv->workqueue, &priv->request_scan, 0);
-	}
+	} else
+		cancel_delayed_work(&priv->scan_event);
 }
 
 static void ipw_bg_link_down(struct work_struct *work)
@@ -10714,6 +10769,7 @@ static int ipw_setup_deferred_work(struct ipw_priv *priv)
 	INIT_WORK(&priv->up, ipw_bg_up);
 	INIT_WORK(&priv->down, ipw_bg_down);
 	INIT_DELAYED_WORK(&priv->request_scan, ipw_request_scan);
+	INIT_DELAYED_WORK(&priv->scan_event, ipw_scan_event);
 	INIT_WORK(&priv->request_passive_scan, ipw_request_passive_scan);
 	INIT_DELAYED_WORK(&priv->gather_stats, ipw_bg_gather_stats);
 	INIT_WORK(&priv->abort_scan, ipw_bg_abort_scan);
@@ -11625,7 +11681,6 @@ static int ipw_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 		goto out_destroy_workqueue;
 	}
 
-	SET_MODULE_OWNER(net_dev);
 	SET_NETDEV_DEV(net_dev, &pdev->dev);
 
 	mutex_lock(&priv->mutex);
@@ -11746,6 +11801,7 @@ static void ipw_pci_remove(struct pci_dev *pdev)
 	cancel_delayed_work(&priv->adhoc_check);
 	cancel_delayed_work(&priv->gather_stats);
 	cancel_delayed_work(&priv->request_scan);
+	cancel_delayed_work(&priv->scan_event);
 	cancel_delayed_work(&priv->rf_kill);
 	cancel_delayed_work(&priv->scan_check);
 	destroy_workqueue(priv->workqueue);
diff --git a/drivers/net/wireless/ipw2200.h b/drivers/net/wireless/ipw2200.h
index 626a240a87d8..bec8e37a1738 100644
--- a/drivers/net/wireless/ipw2200.h
+++ b/drivers/net/wireless/ipw2200.h
@@ -45,7 +45,6 @@
 
 #include <linux/firmware.h>
 #include <linux/wireless.h>
-#include <linux/dma-mapping.h>
 #include <linux/jiffies.h>
 #include <asm/io.h>
 
@@ -1288,6 +1287,8 @@ struct ipw_priv {
 
 	struct iw_public_data wireless_data;
 
+	int user_requested_scan;
+
 	struct workqueue_struct *workqueue;
 
 	struct delayed_work adhoc_check;
@@ -1296,6 +1297,7 @@ struct ipw_priv {
 	struct work_struct system_config;
 	struct work_struct rx_replenish;
 	struct delayed_work request_scan;
+	struct delayed_work scan_event;
   	struct work_struct request_passive_scan;
 	struct work_struct adapter_restart;
 	struct delayed_work rf_kill;
diff --git a/drivers/net/wireless/iwlwifi/Kconfig b/drivers/net/wireless/iwlwifi/Kconfig
new file mode 100644
index 000000000000..25cfc6c32509
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/Kconfig
@@ -0,0 +1,128 @@
+config IWLWIFI
+	bool "Intel Wireless WiFi Link Drivers"
+	depends on PCI && MAC80211 && WLAN_80211 && EXPERIMENTAL
+	select FW_LOADER
+	default n
+	---help---
+	  Select to enable drivers based on the iwlwifi project.  This
+	  project provides a common foundation for Intel's wireless
+	  drivers designed to use the mac80211 subsystem.
+
+	  See <file:Documentation/networking/README.iwlwifi> for
+	  information on the capabilities currently enabled in this
+	  driver and for tips for debugging issues and problems.
+
+config IWLWIFI_DEBUG
+	bool "Enable full debugging output in iwlwifi drivers"
+	depends on IWLWIFI
+	default y
+	---help---
+	  This option will enable debug tracing output for the iwlwifi
+	  drivers.
+
+	  This will result in the kernel module being ~100k larger.  You can
+	  control which debug output is sent to the kernel log by setting the
+	  value in
+
+	          /sys/bus/pci/drivers/${DRIVER}/debug_level
+
+	  This entry will only exist if this option is enabled.
+
+	  To set a value, simply echo an 8-byte hex value to the same file:
+
+		  % echo 0x43fff > /sys/bus/pci/drivers/${DRIVER}/debug_level
+
+	  You can find the list of debug mask values in:
+		  drivers/net/wireless/mac80211/iwlwifi/iwl-debug.h
+
+	  If this is your first time using this driver, you should say Y here
+	  as the debug information can assist others in helping you resolve
+	  any problems you may encounter.
+
+config IWLWIFI_SENSITIVITY
+	bool "Enable Sensitivity Calibration in iwlwifi drivers"
+	depends on IWLWIFI
+	default y
+	---help---
+	  This option will enable sensitivity calibration for the iwlwifi
+	  drivers.
+
+config IWLWIFI_SPECTRUM_MEASUREMENT
+	bool "Enable Spectrum Measurement in iwlwifi drivers"
+	depends on IWLWIFI
+	default y
+	---help---
+	  This option will enable spectrum measurement for the iwlwifi drivers.
+
+config IWLWIFI_QOS
+	bool "Enable Wireless QoS in iwlwifi drivers"
+	depends on IWLWIFI
+	default y
+	---help---
+	  This option will enable wireless quality of service (QoS) for the
+	  iwlwifi drivers.
+
+config IWLWIFI_HT
+	bool "Enable 802.11n HT features in iwlwifi drivers"
+	depends on EXPERIMENTAL
+	depends on IWLWIFI && MAC80211_HT
+	default n
+	---help---
+	  This option enables IEEE 802.11n High Throughput features
+	  for the iwlwifi drivers.
+
+config IWL4965
+	tristate "Intel Wireless WiFi 4965AGN"
+	depends on m && IWLWIFI && EXPERIMENTAL
+	default m
+	---help---
+	  Select to build the driver supporting the:
+
+	  Intel Wireless WiFi Link 4965AGN
+
+	  This driver uses the kernel's mac80211 subsystem.
+
+	  See <file:Documentation/networking/README.iwlwifi> for
+	  information on the capabilities currently enabled in this
+	  driver and for tips for debugging any issues or problems.
+
+	  In order to use this driver, you will need a microcode (uCode)
+	  image for it. You can obtain the microcode from:
+
+	          <http://intellinuxwireless.org/>.
+
+	  See the above referenced README.iwlwifi for information on where
+	  to install the microcode images.
+
+	  If you want to compile the driver as a module ( = code which can be
+	  inserted in and remvoed from the running kernel whenever you want),
+	  say M here and read <file:Documentation/modules.txt>.  The module
+	  will be called iwl4965.ko.
+
+config IWL3945
+	tristate "Intel PRO/Wireless 3945ABG/BG Network Connection"
+	depends on m && IWLWIFI && EXPERIMENTAL
+	default m
+	---help---
+	  Select to build the driver supporting the:
+
+	  Intel PRO/Wireless 3945ABG/BG Network Connection
+
+	  This driver uses the kernel's mac80211 subsystem.
+
+	  See <file:Documentation/networking/README.iwlwifi> for
+	  information on the capabilities currently enabled in this
+	  driver and for tips for debugging any issues or problems.
+
+	  In order to use this driver, you will need a microcode (uCode)
+	  image for it. You can obtain the microcode from:
+
+	          <http://intellinuxwireless.org/>.
+
+	  See the above referenced README.iwlwifi for information on where
+	  to install the microcode images.
+
+	  If you want to compile the driver as a module ( = code which can be
+	  inserted in and remvoed from the running kernel whenever you want),
+	  say M here and read <file:Documentation/modules.txt>.  The module
+	  will be called iwl3945.ko.
diff --git a/drivers/net/wireless/iwlwifi/Makefile b/drivers/net/wireless/iwlwifi/Makefile
new file mode 100644
index 000000000000..3bbd38358d53
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/Makefile
@@ -0,0 +1,5 @@
+obj-$(CONFIG_IWL3945)	+= iwl3945.o
+iwl3945-objs		= iwl3945-base.o iwl-3945.o iwl-3945-rs.o
+
+obj-$(CONFIG_IWL4965)	+= iwl4965.o
+iwl4965-objs		= iwl4965-base.o iwl-4965.o iwl-4965-rs.o
diff --git a/drivers/net/wireless/iwlwifi/iwl-3945-hw.h b/drivers/net/wireless/iwlwifi/iwl-3945-hw.h
new file mode 100644
index 000000000000..fb5f0649f4f6
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-3945-hw.h
@@ -0,0 +1,118 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual BSD/GPLv2 license.  When using or
+ * redistributing this file, you may do so under either license.
+ *
+ * GPL LICENSE SUMMARY
+ *
+ * Copyright(c) 2005 - 2007 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU Geeral Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110,
+ * USA
+ *
+ * The full GNU General Public License is included in this distribution
+ * in the file called LICENSE.GPL.
+ *
+ * Contact Information:
+ * James P. Ketrenos <ipw2100-admin@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ * BSD LICENSE
+ *
+ * Copyright(c) 2005 - 2007 Intel Corporation. All rights reserved.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *  * Neither the name Intel Corporation nor the names of its
+ *    contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+
+#ifndef __iwl_3945_hw__
+#define __iwl_3945_hw__
+
+#define IWL_RX_BUF_SIZE 3000
+/* card static random access memory (SRAM) for processor data and instructs */
+#define ALM_RTC_INST_UPPER_BOUND		(0x014000)
+#define ALM_RTC_DATA_UPPER_BOUND		(0x808000)
+
+#define ALM_RTC_INST_SIZE (ALM_RTC_INST_UPPER_BOUND - RTC_INST_LOWER_BOUND)
+#define ALM_RTC_DATA_SIZE (ALM_RTC_DATA_UPPER_BOUND - RTC_DATA_LOWER_BOUND)
+
+#define IWL_MAX_BSM_SIZE ALM_RTC_INST_SIZE
+#define IWL_MAX_INST_SIZE ALM_RTC_INST_SIZE
+#define IWL_MAX_DATA_SIZE ALM_RTC_DATA_SIZE
+#define IWL_MAX_NUM_QUEUES	8
+
+static inline int iwl_hw_valid_rtc_data_addr(u32 addr)
+{
+	return (addr >= RTC_DATA_LOWER_BOUND) &&
+	       (addr < ALM_RTC_DATA_UPPER_BOUND);
+}
+
+/* Base physical address of iwl_shared is provided to FH_TSSR_CBB_BASE
+ * and &iwl_shared.rx_read_ptr[0] is provided to FH_RCSR_RPTR_ADDR(0) */
+struct iwl_shared {
+	__le32 tx_base_ptr[8];
+	__le32 rx_read_ptr[3];
+} __attribute__ ((packed));
+
+struct iwl_tfd_frame_data {
+	__le32 addr;
+	__le32 len;
+} __attribute__ ((packed));
+
+struct iwl_tfd_frame {
+	__le32 control_flags;
+	struct iwl_tfd_frame_data pa[4];
+	u8 reserved[28];
+} __attribute__ ((packed));
+
+static inline u8 iwl_hw_get_rate(__le16 rate_n_flags)
+{
+	return le16_to_cpu(rate_n_flags) & 0xFF;
+}
+
+static inline u16 iwl_hw_get_rate_n_flags(__le16 rate_n_flags)
+{
+	return le16_to_cpu(rate_n_flags);
+}
+
+static inline __le16 iwl_hw_set_rate_n_flags(u8 rate, u16 flags)
+{
+	return cpu_to_le16((u16)rate|flags);
+}
+#endif
diff --git a/drivers/net/wireless/iwlwifi/iwl-3945-rs.c b/drivers/net/wireless/iwlwifi/iwl-3945-rs.c
new file mode 100644
index 000000000000..f4aabcf480e4
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-3945-rs.c
@@ -0,0 +1,982 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2005 - 2007 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * James P. Ketrenos <ipw2100-admin@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/skbuff.h>
+#include <linux/wireless.h>
+#include <net/mac80211.h>
+#include <net/ieee80211.h>
+
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/delay.h>
+
+#include <linux/workqueue.h>
+
+#include <net/mac80211.h>
+#include <linux/wireless.h>
+
+#define IWL 3945
+
+#include "../net/mac80211/ieee80211_rate.h"
+
+#include "iwlwifi.h"
+
+#define RS_NAME "iwl-3945-rs"
+
+struct iwl_rate_scale_data {
+	u64 data;
+	s32 success_counter;
+	s32 success_ratio;
+	s32 counter;
+	s32 average_tpt;
+	unsigned long stamp;
+};
+
+struct iwl_rate_scale_priv {
+	spinlock_t lock;
+	s32 *expected_tpt;
+	unsigned long last_partial_flush;
+	unsigned long last_flush;
+	u32 flush_time;
+	u32 last_tx_packets;
+	u32 tx_packets;
+	u8 tgg;
+	u8 flush_pending;
+	u8 start_rate;
+	u8 ibss_sta_added;
+	struct timer_list rate_scale_flush;
+	struct iwl_rate_scale_data win[IWL_RATE_COUNT];
+};
+
+static s32 iwl_expected_tpt_g[IWL_RATE_COUNT] = {
+	0, 0, 76, 104, 130, 168, 191, 202, 7, 13, 35, 58
+};
+
+static s32 iwl_expected_tpt_g_prot[IWL_RATE_COUNT] = {
+	0, 0, 0, 80, 93, 113, 123, 125, 7, 13, 35, 58
+};
+
+static s32 iwl_expected_tpt_a[IWL_RATE_COUNT] = {
+	40, 57, 72, 98, 121, 154, 177, 186, 0, 0, 0, 0
+};
+
+static s32 iwl_expected_tpt_b[IWL_RATE_COUNT] = {
+	0, 0, 0, 0, 0, 0, 0, 0, 7, 13, 35, 58
+};
+
+struct iwl_tpt_entry {
+	s8 min_rssi;
+	u8 index;
+};
+
+static struct iwl_tpt_entry iwl_tpt_table_a[] = {
+	{-60, IWL_RATE_54M_INDEX},
+	{-64, IWL_RATE_48M_INDEX},
+	{-72, IWL_RATE_36M_INDEX},
+	{-80, IWL_RATE_24M_INDEX},
+	{-84, IWL_RATE_18M_INDEX},
+	{-85, IWL_RATE_12M_INDEX},
+	{-87, IWL_RATE_9M_INDEX},
+	{-89, IWL_RATE_6M_INDEX}
+};
+
+static struct iwl_tpt_entry iwl_tpt_table_b[] = {
+	{-86, IWL_RATE_11M_INDEX},
+	{-88, IWL_RATE_5M_INDEX},
+	{-90, IWL_RATE_2M_INDEX},
+	{-92, IWL_RATE_1M_INDEX}
+
+};
+
+static struct iwl_tpt_entry iwl_tpt_table_g[] = {
+	{-60, IWL_RATE_54M_INDEX},
+	{-64, IWL_RATE_48M_INDEX},
+	{-68, IWL_RATE_36M_INDEX},
+	{-80, IWL_RATE_24M_INDEX},
+	{-84, IWL_RATE_18M_INDEX},
+	{-85, IWL_RATE_12M_INDEX},
+	{-86, IWL_RATE_11M_INDEX},
+	{-88, IWL_RATE_5M_INDEX},
+	{-90, IWL_RATE_2M_INDEX},
+	{-92, IWL_RATE_1M_INDEX}
+};
+
+#define IWL_RATE_MAX_WINDOW          62
+#define IWL_RATE_FLUSH        (3*HZ/10)
+#define IWL_RATE_WIN_FLUSH       (HZ/2)
+#define IWL_RATE_HIGH_TH          11520
+#define IWL_RATE_MIN_FAILURE_TH       8
+#define IWL_RATE_MIN_SUCCESS_TH       8
+#define IWL_RATE_DECREASE_TH       1920
+
+static u8 iwl_get_rate_index_by_rssi(s32 rssi, u8 mode)
+{
+	u32 index = 0;
+	u32 table_size = 0;
+	struct iwl_tpt_entry *tpt_table = NULL;
+
+	if ((rssi < IWL_MIN_RSSI_VAL) || (rssi > IWL_MAX_RSSI_VAL))
+		rssi = IWL_MIN_RSSI_VAL;
+
+	switch (mode) {
+	case MODE_IEEE80211G:
+		tpt_table = iwl_tpt_table_g;
+		table_size = ARRAY_SIZE(iwl_tpt_table_g);
+		break;
+
+	case MODE_IEEE80211A:
+		tpt_table = iwl_tpt_table_a;
+		table_size = ARRAY_SIZE(iwl_tpt_table_a);
+		break;
+
+	default:
+	case MODE_IEEE80211B:
+		tpt_table = iwl_tpt_table_b;
+		table_size = ARRAY_SIZE(iwl_tpt_table_b);
+		break;
+	}
+
+	while ((index < table_size) && (rssi < tpt_table[index].min_rssi))
+		index++;
+
+	index = min(index, (table_size - 1));
+
+	return tpt_table[index].index;
+}
+
+static void iwl_clear_window(struct iwl_rate_scale_data *window)
+{
+	window->data = 0;
+	window->success_counter = 0;
+	window->success_ratio = IWL_INVALID_VALUE;
+	window->counter = 0;
+	window->average_tpt = IWL_INVALID_VALUE;
+	window->stamp = 0;
+}
+
+/**
+ * iwl_rate_scale_flush_windows - flush out the rate scale windows
+ *
+ * Returns the number of windows that have gathered data but were
+ * not flushed.  If there were any that were not flushed, then
+ * reschedule the rate flushing routine.
+ */
+static int iwl_rate_scale_flush_windows(struct iwl_rate_scale_priv *rs_priv)
+{
+	int unflushed = 0;
+	int i;
+	unsigned long flags;
+
+	/*
+	 * For each rate, if we have collected data on that rate
+	 * and it has been more than IWL_RATE_WIN_FLUSH
+	 * since we flushed, clear out the gathered statistics
+	 */
+	for (i = 0; i < IWL_RATE_COUNT; i++) {
+		if (!rs_priv->win[i].counter)
+			continue;
+
+		spin_lock_irqsave(&rs_priv->lock, flags);
+		if (time_after(jiffies, rs_priv->win[i].stamp +
+			       IWL_RATE_WIN_FLUSH)) {
+			IWL_DEBUG_RATE("flushing %d samples of rate "
+				       "index %d\n",
+				       rs_priv->win[i].counter, i);
+			iwl_clear_window(&rs_priv->win[i]);
+		} else
+			unflushed++;
+		spin_unlock_irqrestore(&rs_priv->lock, flags);
+	}
+
+	return unflushed;
+}
+
+#define IWL_RATE_FLUSH_MAX              5000	/* msec */
+#define IWL_RATE_FLUSH_MIN              50	/* msec */
+
+static void iwl_bg_rate_scale_flush(unsigned long data)
+{
+	struct iwl_rate_scale_priv *rs_priv = (void *)data;
+	int unflushed = 0;
+	unsigned long flags;
+	u32 packet_count, duration, pps;
+
+	IWL_DEBUG_RATE("enter\n");
+
+	unflushed = iwl_rate_scale_flush_windows(rs_priv);
+
+	spin_lock_irqsave(&rs_priv->lock, flags);
+
+	rs_priv->flush_pending = 0;
+
+	/* Number of packets Rx'd since last time this timer ran */
+	packet_count = (rs_priv->tx_packets - rs_priv->last_tx_packets) + 1;
+
+	rs_priv->last_tx_packets = rs_priv->tx_packets + 1;
+
+	if (unflushed) {
+		duration =
+		    jiffies_to_msecs(jiffies - rs_priv->last_partial_flush);
+/*              duration = jiffies_to_msecs(rs_priv->flush_time); */
+
+		IWL_DEBUG_RATE("Tx'd %d packets in %dms\n",
+			       packet_count, duration);
+
+		/* Determine packets per second */
+		if (duration)
+			pps = (packet_count * 1000) / duration;
+		else
+			pps = 0;
+
+		if (pps) {
+			duration = IWL_RATE_FLUSH_MAX / pps;
+			if (duration < IWL_RATE_FLUSH_MIN)
+				duration = IWL_RATE_FLUSH_MIN;
+		} else
+			duration = IWL_RATE_FLUSH_MAX;
+
+		rs_priv->flush_time = msecs_to_jiffies(duration);
+
+		IWL_DEBUG_RATE("new flush period: %d msec ave %d\n",
+			       duration, packet_count);
+
+		mod_timer(&rs_priv->rate_scale_flush, jiffies +
+			  rs_priv->flush_time);
+
+		rs_priv->last_partial_flush = jiffies;
+	}
+
+	/* If there weren't any unflushed entries, we don't schedule the timer
+	 * to run again */
+
+	rs_priv->last_flush = jiffies;
+
+	spin_unlock_irqrestore(&rs_priv->lock, flags);
+
+	IWL_DEBUG_RATE("leave\n");
+}
+
+/**
+ * iwl_collect_tx_data - Update the success/failure sliding window
+ *
+ * We keep a sliding window of the last 64 packets transmitted
+ * at this rate.  window->data contains the bitmask of successful
+ * packets.
+ */
+static void iwl_collect_tx_data(struct iwl_rate_scale_priv *rs_priv,
+				struct iwl_rate_scale_data *window,
+				int success, int retries)
+{
+	unsigned long flags;
+
+	if (!retries) {
+		IWL_DEBUG_RATE("leave: retries == 0 -- should be at least 1\n");
+		return;
+	}
+
+	while (retries--) {
+		spin_lock_irqsave(&rs_priv->lock, flags);
+
+		/* If we have filled up the window then subtract one from the
+		 * success counter if the high-bit is counting toward
+		 * success */
+		if (window->counter == IWL_RATE_MAX_WINDOW) {
+			if (window->data & (1ULL << (IWL_RATE_MAX_WINDOW - 1)))
+				window->success_counter--;
+		} else
+			window->counter++;
+
+		/* Slide the window to the left one bit */
+		window->data = (window->data << 1);
+
+		/* If this packet was a success then set the low bit high */
+		if (success) {
+			window->success_counter++;
+			window->data |= 1;
+		}
+
+		/* window->counter can't be 0 -- it is either >0 or
+		 * IWL_RATE_MAX_WINDOW */
+		window->success_ratio = 12800 * window->success_counter /
+		    window->counter;
+
+		/* Tag this window as having been updated */
+		window->stamp = jiffies;
+
+		spin_unlock_irqrestore(&rs_priv->lock, flags);
+	}
+}
+
+static void rs_rate_init(void *priv_rate, void *priv_sta,
+			 struct ieee80211_local *local, struct sta_info *sta)
+{
+	int i;
+
+	IWL_DEBUG_RATE("enter\n");
+
+	/* TODO: what is a good starting rate for STA? About middle? Maybe not
+	 * the lowest or the highest rate.. Could consider using RSSI from
+	 * previous packets? Need to have IEEE 802.1X auth succeed immediately
+	 * after assoc.. */
+
+	for (i = IWL_RATE_COUNT - 1; i >= 0; i--) {
+		if (sta->supp_rates & (1 << i)) {
+			sta->txrate = i;
+			break;
+		}
+	}
+
+	sta->last_txrate = sta->txrate;
+
+	IWL_DEBUG_RATE("leave\n");
+}
+
+static void *rs_alloc(struct ieee80211_local *local)
+{
+	return local->hw.priv;
+}
+
+/* rate scale requires free function to be implmented */
+static void rs_free(void *priv)
+{
+	return;
+}
+static void rs_clear(void *priv)
+{
+	return;
+}
+
+
+static void *rs_alloc_sta(void *priv, gfp_t gfp)
+{
+	struct iwl_rate_scale_priv *rs_priv;
+	int i;
+
+	IWL_DEBUG_RATE("enter\n");
+
+	rs_priv = kzalloc(sizeof(struct iwl_rate_scale_priv), gfp);
+	if (!rs_priv) {
+		IWL_DEBUG_RATE("leave: ENOMEM\n");
+		return NULL;
+	}
+
+	spin_lock_init(&rs_priv->lock);
+
+	rs_priv->start_rate = IWL_RATE_INVALID;
+
+	/* default to just 802.11b */
+	rs_priv->expected_tpt = iwl_expected_tpt_b;
+
+	rs_priv->last_partial_flush = jiffies;
+	rs_priv->last_flush = jiffies;
+	rs_priv->flush_time = IWL_RATE_FLUSH;
+	rs_priv->last_tx_packets = 0;
+	rs_priv->ibss_sta_added = 0;
+
+	init_timer(&rs_priv->rate_scale_flush);
+	rs_priv->rate_scale_flush.data = (unsigned long)rs_priv;
+	rs_priv->rate_scale_flush.function = &iwl_bg_rate_scale_flush;
+
+	for (i = 0; i < IWL_RATE_COUNT; i++)
+		iwl_clear_window(&rs_priv->win[i]);
+
+	IWL_DEBUG_RATE("leave\n");
+
+	return rs_priv;
+}
+
+static void rs_free_sta(void *priv, void *priv_sta)
+{
+	struct iwl_rate_scale_priv *rs_priv = priv_sta;
+
+	IWL_DEBUG_RATE("enter\n");
+	del_timer_sync(&rs_priv->rate_scale_flush);
+	kfree(rs_priv);
+	IWL_DEBUG_RATE("leave\n");
+}
+
+/**
+ * rs_tx_status - Update rate control values based on Tx results
+ *
+ * NOTE: Uses iwl_priv->retry_rate for the # of retries attempted by
+ * the hardware for each rate.
+ */
+static void rs_tx_status(void *priv_rate,
+			 struct net_device *dev,
+			 struct sk_buff *skb,
+			 struct ieee80211_tx_status *tx_resp)
+{
+	u8 retries, current_count;
+	int scale_rate_index, first_index, last_index;
+	unsigned long flags;
+	struct sta_info *sta;
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
+	struct iwl_priv *priv = (struct iwl_priv *)priv_rate;
+	struct ieee80211_local *local = wdev_priv(dev->ieee80211_ptr);
+	struct iwl_rate_scale_priv *rs_priv;
+
+	IWL_DEBUG_RATE("enter\n");
+
+	retries = tx_resp->retry_count;
+
+	first_index = tx_resp->control.tx_rate;
+	if ((first_index < 0) || (first_index >= IWL_RATE_COUNT)) {
+		IWL_DEBUG_RATE("leave: Rate out of bounds: %0x for %d\n",
+			       tx_resp->control.tx_rate, first_index);
+		return;
+	}
+
+	sta = sta_info_get(local, hdr->addr1);
+	if (!sta || !sta->rate_ctrl_priv) {
+		if (sta)
+			sta_info_put(sta);
+		IWL_DEBUG_RATE("leave: No STA priv data to update!\n");
+		return;
+	}
+
+	rs_priv = (void *)sta->rate_ctrl_priv;
+
+	rs_priv->tx_packets++;
+
+	scale_rate_index = first_index;
+	last_index = first_index;
+
+	/*
+	 * Update the window for each rate.  We determine which rates
+	 * were Tx'd based on the total number of retries vs. the number
+	 * of retries configured for each rate -- currently set to the
+	 * priv value 'retry_rate' vs. rate specific
+	 *
+	 * On exit from this while loop last_index indicates the rate
+	 * at which the frame was finally transmitted (or failed if no
+	 * ACK)
+	 */
+	while (retries > 0) {
+		if (retries < priv->retry_rate) {
+			current_count = retries;
+			last_index = scale_rate_index;
+		} else {
+			current_count = priv->retry_rate;
+			last_index = iwl_get_prev_ieee_rate(scale_rate_index);
+		}
+
+		/* Update this rate accounting for as many retries
+		 * as was used for it (per current_count) */
+		iwl_collect_tx_data(rs_priv,
+				    &rs_priv->win[scale_rate_index],
+				    0, current_count);
+		IWL_DEBUG_RATE("Update rate %d for %d retries.\n",
+			       scale_rate_index, current_count);
+
+		retries -= current_count;
+
+		if (retries)
+			scale_rate_index =
+			    iwl_get_prev_ieee_rate(scale_rate_index);
+	}
+
+	/* Update the last index window with success/failure based on ACK */
+	IWL_DEBUG_RATE("Update rate %d with %s.\n",
+		       last_index,
+		       (tx_resp->flags & IEEE80211_TX_STATUS_ACK) ?
+		       "success" : "failure");
+	iwl_collect_tx_data(rs_priv,
+			    &rs_priv->win[last_index],
+			    tx_resp->flags & IEEE80211_TX_STATUS_ACK, 1);
+
+	/* We updated the rate scale window -- if its been more than
+	 * flush_time since the last run, schedule the flush
+	 * again */
+	spin_lock_irqsave(&rs_priv->lock, flags);
+
+	if (!rs_priv->flush_pending &&
+	    time_after(jiffies, rs_priv->last_partial_flush +
+		       rs_priv->flush_time)) {
+
+		rs_priv->flush_pending = 1;
+		mod_timer(&rs_priv->rate_scale_flush,
+			  jiffies + rs_priv->flush_time);
+	}
+
+	spin_unlock_irqrestore(&rs_priv->lock, flags);
+
+	sta_info_put(sta);
+
+	IWL_DEBUG_RATE("leave\n");
+
+	return;
+}
+
+static struct ieee80211_rate *iwl_get_lowest_rate(struct ieee80211_local
+						  *local)
+{
+	struct ieee80211_hw_mode *mode = local->oper_hw_mode;
+	int i;
+
+	for (i = 0; i < mode->num_rates; i++) {
+		struct ieee80211_rate *rate = &mode->rates[i];
+
+		if (rate->flags & IEEE80211_RATE_SUPPORTED)
+			return rate;
+	}
+
+	return &mode->rates[0];
+}
+
+static u16 iwl_get_adjacent_rate(struct iwl_rate_scale_priv *rs_priv,
+				 u8 index, u16 rate_mask, int phymode)
+{
+	u8 high = IWL_RATE_INVALID;
+	u8 low = IWL_RATE_INVALID;
+
+	/* 802.11A walks to the next literal adjascent rate in
+	 * the rate table */
+	if (unlikely(phymode == MODE_IEEE80211A)) {
+		int i;
+		u32 mask;
+
+		/* Find the previous rate that is in the rate mask */
+		i = index - 1;
+		for (mask = (1 << i); i >= 0; i--, mask >>= 1) {
+			if (rate_mask & mask) {
+				low = i;
+				break;
+			}
+		}
+
+		/* Find the next rate that is in the rate mask */
+		i = index + 1;
+		for (mask = (1 << i); i < IWL_RATE_COUNT; i++, mask <<= 1) {
+			if (rate_mask & mask) {
+				high = i;
+				break;
+			}
+		}
+
+		return (high << 8) | low;
+	}
+
+	low = index;
+	while (low != IWL_RATE_INVALID) {
+		if (rs_priv->tgg)
+			low = iwl_rates[low].prev_rs_tgg;
+		else
+			low = iwl_rates[low].prev_rs;
+		if (low == IWL_RATE_INVALID)
+			break;
+		if (rate_mask & (1 << low))
+			break;
+		IWL_DEBUG_RATE("Skipping masked lower rate: %d\n", low);
+	}
+
+	high = index;
+	while (high != IWL_RATE_INVALID) {
+		if (rs_priv->tgg)
+			high = iwl_rates[high].next_rs_tgg;
+		else
+			high = iwl_rates[high].next_rs;
+		if (high == IWL_RATE_INVALID)
+			break;
+		if (rate_mask & (1 << high))
+			break;
+		IWL_DEBUG_RATE("Skipping masked higher rate: %d\n", high);
+	}
+
+	return (high << 8) | low;
+}
+
+/**
+ * rs_get_rate - find the rate for the requested packet
+ *
+ * Returns the ieee80211_rate structure allocated by the driver.
+ *
+ * The rate control algorithm has no internal mapping between hw_mode's
+ * rate ordering and the rate ordering used by the rate control algorithm.
+ *
+ * The rate control algorithm uses a single table of rates that goes across
+ * the entire A/B/G spectrum vs. being limited to just one particular
+ * hw_mode.
+ *
+ * As such, we can't convert the index obtained below into the hw_mode's
+ * rate table and must reference the driver allocated rate table
+ *
+ */
+static struct ieee80211_rate *rs_get_rate(void *priv_rate,
+					  struct net_device *dev,
+					  struct sk_buff *skb,
+					  struct rate_control_extra *extra)
+{
+	u8 low = IWL_RATE_INVALID;
+	u8 high = IWL_RATE_INVALID;
+	u16 high_low;
+	int index;
+	struct iwl_rate_scale_priv *rs_priv;
+	struct iwl_rate_scale_data *window = NULL;
+	int current_tpt = IWL_INVALID_VALUE;
+	int low_tpt = IWL_INVALID_VALUE;
+	int high_tpt = IWL_INVALID_VALUE;
+	u32 fail_count;
+	s8 scale_action = 0;
+	unsigned long flags;
+	struct ieee80211_local *local = wdev_priv(dev->ieee80211_ptr);
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
+	struct sta_info *sta;
+	u16 fc, rate_mask;
+	struct iwl_priv *priv = (struct iwl_priv *)priv_rate;
+	DECLARE_MAC_BUF(mac);
+
+	IWL_DEBUG_RATE("enter\n");
+
+	memset(extra, 0, sizeof(*extra));
+
+	fc = le16_to_cpu(hdr->frame_control);
+	if (((fc & IEEE80211_FCTL_FTYPE) != IEEE80211_FTYPE_DATA) ||
+	    (is_multicast_ether_addr(hdr->addr1))) {
+		/* Send management frames and broadcast/multicast data using
+		 * lowest rate. */
+		/* TODO: this could probably be improved.. */
+		IWL_DEBUG_RATE("leave: lowest rate (not data or is "
+			       "multicast)\n");
+
+		return iwl_get_lowest_rate(local);
+	}
+
+	sta = sta_info_get(local, hdr->addr1);
+	if (!sta || !sta->rate_ctrl_priv) {
+		IWL_DEBUG_RATE("leave: No STA priv data to update!\n");
+		if (sta)
+			sta_info_put(sta);
+		return NULL;
+	}
+
+	rate_mask = sta->supp_rates;
+	index = min(sta->txrate & 0xffff, IWL_RATE_COUNT - 1);
+
+	rs_priv = (void *)sta->rate_ctrl_priv;
+
+	if ((priv->iw_mode == IEEE80211_IF_TYPE_IBSS) &&
+	    !rs_priv->ibss_sta_added) {
+		u8 sta_id = iwl_hw_find_station(priv, hdr->addr1);
+
+		if (sta_id == IWL_INVALID_STATION) {
+			IWL_DEBUG_RATE("LQ: ADD station %s\n",
+				       print_mac(mac, hdr->addr1));
+			sta_id = iwl_add_station(priv,
+				    hdr->addr1, 0, CMD_ASYNC);
+		}
+		if (sta_id != IWL_INVALID_STATION)
+			rs_priv->ibss_sta_added = 1;
+	}
+
+	spin_lock_irqsave(&rs_priv->lock, flags);
+
+	if (rs_priv->start_rate != IWL_RATE_INVALID) {
+		index = rs_priv->start_rate;
+		rs_priv->start_rate = IWL_RATE_INVALID;
+	}
+
+	window = &(rs_priv->win[index]);
+
+	fail_count = window->counter - window->success_counter;
+
+	if (((fail_count <= IWL_RATE_MIN_FAILURE_TH) &&
+	     (window->success_counter < IWL_RATE_MIN_SUCCESS_TH))) {
+		window->average_tpt = IWL_INVALID_VALUE;
+		spin_unlock_irqrestore(&rs_priv->lock, flags);
+
+		IWL_DEBUG_RATE("Invalid average_tpt on rate %d: "
+			       "counter: %d, success_counter: %d, "
+			       "expected_tpt is %sNULL\n",
+			       index,
+			       window->counter,
+			       window->success_counter,
+			       rs_priv->expected_tpt ? "not " : "");
+		goto out;
+
+	}
+
+	window->average_tpt = ((window->success_ratio *
+				rs_priv->expected_tpt[index] + 64) / 128);
+	current_tpt = window->average_tpt;
+
+	high_low = iwl_get_adjacent_rate(rs_priv, index, rate_mask,
+					 local->hw.conf.phymode);
+	low = high_low & 0xff;
+	high = (high_low >> 8) & 0xff;
+
+	if (low != IWL_RATE_INVALID)
+		low_tpt = rs_priv->win[low].average_tpt;
+
+	if (high != IWL_RATE_INVALID)
+		high_tpt = rs_priv->win[high].average_tpt;
+
+	spin_unlock_irqrestore(&rs_priv->lock, flags);
+
+	scale_action = 1;
+
+	if ((window->success_ratio < IWL_RATE_DECREASE_TH) || !current_tpt) {
+		IWL_DEBUG_RATE("decrease rate because of low success_ratio\n");
+		scale_action = -1;
+	} else if ((low_tpt == IWL_INVALID_VALUE) &&
+		   (high_tpt == IWL_INVALID_VALUE))
+		scale_action = 1;
+	else if ((low_tpt != IWL_INVALID_VALUE) &&
+		   (high_tpt != IWL_INVALID_VALUE)
+		   && (low_tpt < current_tpt)
+		   && (high_tpt < current_tpt)) {
+		IWL_DEBUG_RATE("No action -- low [%d] & high [%d] < "
+			       "current_tpt [%d]\n",
+			       low_tpt, high_tpt, current_tpt);
+		scale_action = 0;
+	} else {
+		if (high_tpt != IWL_INVALID_VALUE) {
+			if (high_tpt > current_tpt)
+				scale_action = 1;
+			else {
+				IWL_DEBUG_RATE
+				    ("decrease rate because of high tpt\n");
+				scale_action = -1;
+			}
+		} else if (low_tpt != IWL_INVALID_VALUE) {
+			if (low_tpt > current_tpt) {
+				IWL_DEBUG_RATE
+				    ("decrease rate because of low tpt\n");
+				scale_action = -1;
+			} else
+				scale_action = 1;
+		}
+	}
+
+	if ((window->success_ratio > IWL_RATE_HIGH_TH) ||
+	    (current_tpt > window->average_tpt)) {
+		IWL_DEBUG_RATE("No action -- success_ratio [%d] > HIGH_TH or "
+			       "current_tpt [%d] > average_tpt [%d]\n",
+			       window->success_ratio,
+			       current_tpt, window->average_tpt);
+		scale_action = 0;
+	}
+
+	switch (scale_action) {
+	case -1:
+		if (low != IWL_RATE_INVALID)
+			index = low;
+		break;
+
+	case 1:
+		if (high != IWL_RATE_INVALID)
+			index = high;
+
+		break;
+
+	case 0:
+	default:
+		break;
+	}
+
+	IWL_DEBUG_RATE("Selected %d (action %d) - low %d high %d\n",
+		       index, scale_action, low, high);
+
+ out:
+
+	sta->last_txrate = index;
+	sta->txrate = sta->last_txrate;
+	sta_info_put(sta);
+
+	IWL_DEBUG_RATE("leave: %d\n", index);
+
+	return &priv->ieee_rates[index];
+}
+
+static struct rate_control_ops rs_ops = {
+	.module = NULL,
+	.name = RS_NAME,
+	.tx_status = rs_tx_status,
+	.get_rate = rs_get_rate,
+	.rate_init = rs_rate_init,
+	.clear = rs_clear,
+	.alloc = rs_alloc,
+	.free = rs_free,
+	.alloc_sta = rs_alloc_sta,
+	.free_sta = rs_free_sta,
+};
+
+int iwl_fill_rs_info(struct ieee80211_hw *hw, char *buf, u8 sta_id)
+{
+	struct ieee80211_local *local = hw_to_local(hw);
+	struct iwl_priv *priv = hw->priv;
+	struct iwl_rate_scale_priv *rs_priv;
+	struct sta_info *sta;
+	unsigned long flags;
+	int count = 0, i;
+	u32 samples = 0, success = 0, good = 0;
+	unsigned long now = jiffies;
+	u32 max_time = 0;
+
+	sta = sta_info_get(local, priv->stations[sta_id].sta.sta.addr);
+	if (!sta || !sta->rate_ctrl_priv) {
+		if (sta) {
+			sta_info_put(sta);
+			IWL_DEBUG_RATE("leave - no private rate data!\n");
+		} else
+			IWL_DEBUG_RATE("leave - no station!\n");
+		return sprintf(buf, "station %d not found\n", sta_id);
+	}
+
+	rs_priv = (void *)sta->rate_ctrl_priv;
+	spin_lock_irqsave(&rs_priv->lock, flags);
+	i = IWL_RATE_54M_INDEX;
+	while (1) {
+		u64 mask;
+		int j;
+
+		count +=
+		    sprintf(&buf[count], " %2dMbs: ", iwl_rates[i].ieee / 2);
+
+		mask = (1ULL << (IWL_RATE_MAX_WINDOW - 1));
+		for (j = 0; j < IWL_RATE_MAX_WINDOW; j++, mask >>= 1)
+			buf[count++] =
+			    (rs_priv->win[i].data & mask) ? '1' : '0';
+
+		samples += rs_priv->win[i].counter;
+		good += rs_priv->win[i].success_counter;
+		success += rs_priv->win[i].success_counter * iwl_rates[i].ieee;
+
+		if (rs_priv->win[i].stamp) {
+			int delta =
+			    jiffies_to_msecs(now - rs_priv->win[i].stamp);
+
+			if (delta > max_time)
+				max_time = delta;
+
+			count += sprintf(&buf[count], "%5dms\n", delta);
+		} else
+			buf[count++] = '\n';
+
+		j = iwl_get_prev_ieee_rate(i);
+		if (j == i)
+			break;
+		i = j;
+	}
+	spin_unlock_irqrestore(&rs_priv->lock, flags);
+	sta_info_put(sta);
+
+	/* Display the average rate of all samples taken.
+	 *
+	 * NOTE:  We multiple # of samples by 2 since the IEEE measurement
+	 * added from iwl_rates is actually 2X the rate */
+	if (samples)
+		count += sprintf(
+			&buf[count],
+			"\nAverage rate is %3d.%02dMbs over last %4dms\n"
+			"%3d%% success (%d good packets over %d tries)\n",
+			success / (2 * samples), (success * 5 / samples) % 10,
+			max_time, good * 100 / samples, good, samples);
+	else
+		count += sprintf(&buf[count], "\nAverage rate: 0Mbs\n");
+
+	return count;
+}
+
+void iwl_rate_scale_init(struct ieee80211_hw *hw, s32 sta_id)
+{
+	struct iwl_priv *priv = hw->priv;
+	s32 rssi = 0;
+	unsigned long flags;
+	struct ieee80211_local *local = hw_to_local(hw);
+	struct iwl_rate_scale_priv *rs_priv;
+	struct sta_info *sta;
+
+	IWL_DEBUG_RATE("enter\n");
+
+	if (!local->rate_ctrl->ops->name ||
+	    strcmp(local->rate_ctrl->ops->name, RS_NAME)) {
+		IWL_WARNING("iwl-3945-rs not selected as rate control algo!\n");
+		IWL_DEBUG_RATE("leave - mac80211 picked the wrong RC algo.\n");
+		return;
+	}
+
+	sta = sta_info_get(local, priv->stations[sta_id].sta.sta.addr);
+	if (!sta || !sta->rate_ctrl_priv) {
+		if (sta)
+			sta_info_put(sta);
+		IWL_DEBUG_RATE("leave - no private rate data!\n");
+		return;
+	}
+
+	rs_priv = (void *)sta->rate_ctrl_priv;
+
+	spin_lock_irqsave(&rs_priv->lock, flags);
+
+	rs_priv->tgg = 0;
+	switch (priv->phymode) {
+	case MODE_IEEE80211G:
+		if (priv->active_rxon.flags & RXON_FLG_TGG_PROTECT_MSK) {
+			rs_priv->tgg = 1;
+			rs_priv->expected_tpt = iwl_expected_tpt_g_prot;
+		} else
+			rs_priv->expected_tpt = iwl_expected_tpt_g;
+		break;
+
+	case MODE_IEEE80211A:
+		rs_priv->expected_tpt = iwl_expected_tpt_a;
+		break;
+
+	default:
+		IWL_WARNING("Invalid phymode.  Defaulting to 802.11b\n");
+	case MODE_IEEE80211B:
+		rs_priv->expected_tpt = iwl_expected_tpt_b;
+		break;
+	}
+
+	sta_info_put(sta);
+	spin_unlock_irqrestore(&rs_priv->lock, flags);
+
+	rssi = priv->last_rx_rssi;
+	if (rssi == 0)
+		rssi = IWL_MIN_RSSI_VAL;
+
+	IWL_DEBUG(IWL_DL_INFO | IWL_DL_RATE, "Network RSSI: %d\n", rssi);
+
+	rs_priv->start_rate = iwl_get_rate_index_by_rssi(rssi, priv->phymode);
+
+	IWL_DEBUG_RATE("leave: rssi %d assign rate index: "
+		       "%d (plcp 0x%x)\n", rssi, rs_priv->start_rate,
+		       iwl_rates[rs_priv->start_rate].plcp);
+}
+
+void iwl_rate_control_register(struct ieee80211_hw *hw)
+{
+	ieee80211_rate_control_register(&rs_ops);
+}
+
+void iwl_rate_control_unregister(struct ieee80211_hw *hw)
+{
+	ieee80211_rate_control_unregister(&rs_ops);
+}
+
+
diff --git a/drivers/net/wireless/iwlwifi/iwl-3945-rs.h b/drivers/net/wireless/iwlwifi/iwl-3945-rs.h
new file mode 100644
index 000000000000..b926738e0ea1
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-3945-rs.h
@@ -0,0 +1,191 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2005 - 2007 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * James P. Ketrenos <ipw2100-admin@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+
+#ifndef __iwl_3945_rs_h__
+#define __iwl_3945_rs_h__
+
+struct iwl_rate_info {
+	u8 plcp;
+	u8 ieee;
+	u8 prev_ieee;		/* previous rate in IEEE speeds */
+	u8 next_ieee;		/* next rate in IEEE speeds */
+	u8 prev_rs;		/* previous rate used in rs algo */
+	u8 next_rs;		/* next rate used in rs algo */
+	u8 prev_rs_tgg;		/* previous rate used in TGG rs algo */
+	u8 next_rs_tgg;		/* next rate used in TGG rs algo */
+};
+
+enum {
+	IWL_RATE_6M_INDEX = 0,
+	IWL_RATE_9M_INDEX,
+	IWL_RATE_12M_INDEX,
+	IWL_RATE_18M_INDEX,
+	IWL_RATE_24M_INDEX,
+	IWL_RATE_36M_INDEX,
+	IWL_RATE_48M_INDEX,
+	IWL_RATE_54M_INDEX,
+	IWL_RATE_1M_INDEX,
+	IWL_RATE_2M_INDEX,
+	IWL_RATE_5M_INDEX,
+	IWL_RATE_11M_INDEX,
+	IWL_RATE_COUNT,
+	IWL_RATE_INVM_INDEX,
+	IWL_RATE_INVALID = IWL_RATE_INVM_INDEX
+};
+
+enum {
+	IWL_FIRST_OFDM_RATE = IWL_RATE_6M_INDEX,
+	IWL_LAST_OFDM_RATE = IWL_RATE_54M_INDEX,
+	IWL_FIRST_CCK_RATE = IWL_RATE_1M_INDEX,
+	IWL_LAST_CCK_RATE = IWL_RATE_11M_INDEX,
+};
+
+/* #define vs. enum to keep from defaulting to 'large integer' */
+#define	IWL_RATE_6M_MASK   (1<<IWL_RATE_6M_INDEX)
+#define	IWL_RATE_9M_MASK   (1<<IWL_RATE_9M_INDEX)
+#define	IWL_RATE_12M_MASK  (1<<IWL_RATE_12M_INDEX)
+#define	IWL_RATE_18M_MASK  (1<<IWL_RATE_18M_INDEX)
+#define	IWL_RATE_24M_MASK  (1<<IWL_RATE_24M_INDEX)
+#define	IWL_RATE_36M_MASK  (1<<IWL_RATE_36M_INDEX)
+#define	IWL_RATE_48M_MASK  (1<<IWL_RATE_48M_INDEX)
+#define	IWL_RATE_54M_MASK  (1<<IWL_RATE_54M_INDEX)
+#define	IWL_RATE_1M_MASK   (1<<IWL_RATE_1M_INDEX)
+#define	IWL_RATE_2M_MASK   (1<<IWL_RATE_2M_INDEX)
+#define	IWL_RATE_5M_MASK   (1<<IWL_RATE_5M_INDEX)
+#define	IWL_RATE_11M_MASK  (1<<IWL_RATE_11M_INDEX)
+
+enum {
+	IWL_RATE_6M_PLCP = 13,
+	IWL_RATE_9M_PLCP = 15,
+	IWL_RATE_12M_PLCP = 5,
+	IWL_RATE_18M_PLCP = 7,
+	IWL_RATE_24M_PLCP = 9,
+	IWL_RATE_36M_PLCP = 11,
+	IWL_RATE_48M_PLCP = 1,
+	IWL_RATE_54M_PLCP = 3,
+	IWL_RATE_1M_PLCP = 10,
+	IWL_RATE_2M_PLCP = 20,
+	IWL_RATE_5M_PLCP = 55,
+	IWL_RATE_11M_PLCP = 110,
+};
+
+enum {
+	IWL_RATE_6M_IEEE = 12,
+	IWL_RATE_9M_IEEE = 18,
+	IWL_RATE_12M_IEEE = 24,
+	IWL_RATE_18M_IEEE = 36,
+	IWL_RATE_24M_IEEE = 48,
+	IWL_RATE_36M_IEEE = 72,
+	IWL_RATE_48M_IEEE = 96,
+	IWL_RATE_54M_IEEE = 108,
+	IWL_RATE_1M_IEEE = 2,
+	IWL_RATE_2M_IEEE = 4,
+	IWL_RATE_5M_IEEE = 11,
+	IWL_RATE_11M_IEEE = 22,
+};
+
+#define IWL_CCK_BASIC_RATES_MASK    \
+       (IWL_RATE_1M_MASK          | \
+	IWL_RATE_2M_MASK)
+
+#define IWL_CCK_RATES_MASK          \
+       (IWL_BASIC_RATES_MASK      | \
+	IWL_RATE_5M_MASK          | \
+	IWL_RATE_11M_MASK)
+
+#define IWL_OFDM_BASIC_RATES_MASK   \
+	(IWL_RATE_6M_MASK         | \
+	IWL_RATE_12M_MASK         | \
+	IWL_RATE_24M_MASK)
+
+#define IWL_OFDM_RATES_MASK         \
+       (IWL_OFDM_BASIC_RATES_MASK | \
+	IWL_RATE_9M_MASK          | \
+	IWL_RATE_18M_MASK         | \
+	IWL_RATE_36M_MASK         | \
+	IWL_RATE_48M_MASK         | \
+	IWL_RATE_54M_MASK)
+
+#define IWL_BASIC_RATES_MASK         \
+	(IWL_OFDM_BASIC_RATES_MASK | \
+	 IWL_CCK_BASIC_RATES_MASK)
+
+#define IWL_RATES_MASK ((1<<IWL_RATE_COUNT)-1)
+
+#define IWL_INVALID_VALUE    -1
+
+#define IWL_MIN_RSSI_VAL                 -100
+#define IWL_MAX_RSSI_VAL                    0
+
+extern const struct iwl_rate_info iwl_rates[IWL_RATE_COUNT];
+
+static inline u8 iwl_get_prev_ieee_rate(u8 rate_index)
+{
+	u8 rate = iwl_rates[rate_index].prev_ieee;
+
+	if (rate == IWL_RATE_INVALID)
+		rate = rate_index;
+	return rate;
+}
+
+/**
+ * iwl_fill_rs_info - Fill an output text buffer with the rate representation
+ *
+ * NOTE:  This is provided as a quick mechanism for a user to visualize
+ * the performance of the rate control alogirthm and is not meant to be
+ * parsed software.
+ */
+extern int iwl_fill_rs_info(struct ieee80211_hw *, char *buf, u8 sta_id);
+
+/**
+ * iwl_rate_scale_init - Initialize the rate scale table based on assoc info
+ *
+ * The specific througput table used is based on the type of network
+ * the associated with, including A, B, G, and G w/ TGG protection
+ */
+extern void iwl_rate_scale_init(struct ieee80211_hw *hw, s32 sta_id);
+
+/**
+ * iwl_rate_control_register - Register the rate control algorithm callbacks
+ *
+ * Since the rate control algorithm is hardware specific, there is no need
+ * or reason to place it as a stand alone module.  The driver can call
+ * iwl_rate_control_register in order to register the rate control callbacks
+ * with the mac80211 subsystem.  This should be performed prior to calling
+ * ieee80211_register_hw
+ *
+ */
+extern void iwl_rate_control_register(struct ieee80211_hw *hw);
+
+/**
+ * iwl_rate_control_unregister - Unregister the rate control callbacks
+ *
+ * This should be called after calling ieee80211_unregister_hw, but before
+ * the driver is unloaded.
+ */
+extern void iwl_rate_control_unregister(struct ieee80211_hw *hw);
+
+#endif
diff --git a/drivers/net/wireless/iwlwifi/iwl-3945.c b/drivers/net/wireless/iwlwifi/iwl-3945.c
new file mode 100644
index 000000000000..acb38750535f
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-3945.c
@@ -0,0 +1,2300 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2003 - 2007 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * James P. Ketrenos <ipw2100-admin@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/version.h>
+#include <linux/init.h>
+#include <linux/pci.h>
+#include <linux/dma-mapping.h>
+#include <linux/delay.h>
+#include <linux/skbuff.h>
+#include <linux/netdevice.h>
+#include <linux/wireless.h>
+#include <linux/firmware.h>
+#include <net/mac80211.h>
+
+#include <linux/etherdevice.h>
+#include <linux/delay.h>
+
+#define IWL 3945
+
+#include "iwlwifi.h"
+#include "iwl-helpers.h"
+#include "iwl-3945.h"
+#include "iwl-3945-rs.h"
+
+#define IWL_DECLARE_RATE_INFO(r, ip, in, rp, rn, pp, np)    \
+	[IWL_RATE_##r##M_INDEX] = { IWL_RATE_##r##M_PLCP,   \
+				    IWL_RATE_##r##M_IEEE,   \
+				    IWL_RATE_##ip##M_INDEX, \
+				    IWL_RATE_##in##M_INDEX, \
+				    IWL_RATE_##rp##M_INDEX, \
+				    IWL_RATE_##rn##M_INDEX, \
+				    IWL_RATE_##pp##M_INDEX, \
+				    IWL_RATE_##np##M_INDEX }
+
+/*
+ * Parameter order:
+ *   rate, prev rate, next rate, prev tgg rate, next tgg rate
+ *
+ * If there isn't a valid next or previous rate then INV is used which
+ * maps to IWL_RATE_INVALID
+ *
+ */
+const struct iwl_rate_info iwl_rates[IWL_RATE_COUNT] = {
+	IWL_DECLARE_RATE_INFO(6, 5, 9, 5, 11, 5, 11),        /*  6mbps */
+	IWL_DECLARE_RATE_INFO(9, 6, 11, 5, 11, 5, 11),       /*  9mbps */
+	IWL_DECLARE_RATE_INFO(12, 11, 18, 11, 18, 11, 18),   /* 12mbps */
+	IWL_DECLARE_RATE_INFO(18, 12, 24, 12, 24, 11, 24),   /* 18mbps */
+	IWL_DECLARE_RATE_INFO(24, 18, 36, 18, 36, 18, 36),   /* 24mbps */
+	IWL_DECLARE_RATE_INFO(36, 24, 48, 24, 48, 24, 48),   /* 36mbps */
+	IWL_DECLARE_RATE_INFO(48, 36, 54, 36, 54, 36, 54),   /* 48mbps */
+	IWL_DECLARE_RATE_INFO(54, 48, INV, 48, INV, 48, INV),/* 54mbps */
+	IWL_DECLARE_RATE_INFO(1, INV, 2, INV, 2, INV, 2),    /*  1mbps */
+	IWL_DECLARE_RATE_INFO(2, 1, 5, 1, 5, 1, 5),          /*  2mbps */
+	IWL_DECLARE_RATE_INFO(5, 2, 6, 2, 11, 2, 11),        /*5.5mbps */
+	IWL_DECLARE_RATE_INFO(11, 9, 12, 5, 12, 5, 18),      /* 11mbps */
+};
+
+/* 1 = enable the iwl_disable_events() function */
+#define IWL_EVT_DISABLE (0)
+#define IWL_EVT_DISABLE_SIZE (1532/32)
+
+/**
+ * iwl_disable_events - Disable selected events in uCode event log
+ *
+ * Disable an event by writing "1"s into "disable"
+ *   bitmap in SRAM.  Bit position corresponds to Event # (id/type).
+ *   Default values of 0 enable uCode events to be logged.
+ * Use for only special debugging.  This function is just a placeholder as-is,
+ *   you'll need to provide the special bits! ...
+ *   ... and set IWL_EVT_DISABLE to 1. */
+void iwl_disable_events(struct iwl_priv *priv)
+{
+	int rc;
+	int i;
+	u32 base;		/* SRAM address of event log header */
+	u32 disable_ptr;	/* SRAM address of event-disable bitmap array */
+	u32 array_size;		/* # of u32 entries in array */
+	u32 evt_disable[IWL_EVT_DISABLE_SIZE] = {
+		0x00000000,	/*   31 -    0  Event id numbers */
+		0x00000000,	/*   63 -   32 */
+		0x00000000,	/*   95 -   64 */
+		0x00000000,	/*  127 -   96 */
+		0x00000000,	/*  159 -  128 */
+		0x00000000,	/*  191 -  160 */
+		0x00000000,	/*  223 -  192 */
+		0x00000000,	/*  255 -  224 */
+		0x00000000,	/*  287 -  256 */
+		0x00000000,	/*  319 -  288 */
+		0x00000000,	/*  351 -  320 */
+		0x00000000,	/*  383 -  352 */
+		0x00000000,	/*  415 -  384 */
+		0x00000000,	/*  447 -  416 */
+		0x00000000,	/*  479 -  448 */
+		0x00000000,	/*  511 -  480 */
+		0x00000000,	/*  543 -  512 */
+		0x00000000,	/*  575 -  544 */
+		0x00000000,	/*  607 -  576 */
+		0x00000000,	/*  639 -  608 */
+		0x00000000,	/*  671 -  640 */
+		0x00000000,	/*  703 -  672 */
+		0x00000000,	/*  735 -  704 */
+		0x00000000,	/*  767 -  736 */
+		0x00000000,	/*  799 -  768 */
+		0x00000000,	/*  831 -  800 */
+		0x00000000,	/*  863 -  832 */
+		0x00000000,	/*  895 -  864 */
+		0x00000000,	/*  927 -  896 */
+		0x00000000,	/*  959 -  928 */
+		0x00000000,	/*  991 -  960 */
+		0x00000000,	/* 1023 -  992 */
+		0x00000000,	/* 1055 - 1024 */
+		0x00000000,	/* 1087 - 1056 */
+		0x00000000,	/* 1119 - 1088 */
+		0x00000000,	/* 1151 - 1120 */
+		0x00000000,	/* 1183 - 1152 */
+		0x00000000,	/* 1215 - 1184 */
+		0x00000000,	/* 1247 - 1216 */
+		0x00000000,	/* 1279 - 1248 */
+		0x00000000,	/* 1311 - 1280 */
+		0x00000000,	/* 1343 - 1312 */
+		0x00000000,	/* 1375 - 1344 */
+		0x00000000,	/* 1407 - 1376 */
+		0x00000000,	/* 1439 - 1408 */
+		0x00000000,	/* 1471 - 1440 */
+		0x00000000,	/* 1503 - 1472 */
+	};
+
+	base = le32_to_cpu(priv->card_alive.log_event_table_ptr);
+	if (!iwl_hw_valid_rtc_data_addr(base)) {
+		IWL_ERROR("Invalid event log pointer 0x%08X\n", base);
+		return;
+	}
+
+	rc = iwl_grab_restricted_access(priv);
+	if (rc) {
+		IWL_WARNING("Can not read from adapter at this time.\n");
+		return;
+	}
+
+	disable_ptr = iwl_read_restricted_mem(priv, base + (4 * sizeof(u32)));
+	array_size = iwl_read_restricted_mem(priv, base + (5 * sizeof(u32)));
+	iwl_release_restricted_access(priv);
+
+	if (IWL_EVT_DISABLE && (array_size == IWL_EVT_DISABLE_SIZE)) {
+		IWL_DEBUG_INFO("Disabling selected uCode log events at 0x%x\n",
+			       disable_ptr);
+		rc = iwl_grab_restricted_access(priv);
+		for (i = 0; i < IWL_EVT_DISABLE_SIZE; i++)
+			iwl_write_restricted_mem(priv,
+						 disable_ptr +
+						 (i * sizeof(u32)),
+						 evt_disable[i]);
+
+		iwl_release_restricted_access(priv);
+	} else {
+		IWL_DEBUG_INFO("Selected uCode log events may be disabled\n");
+		IWL_DEBUG_INFO("  by writing \"1\"s into disable bitmap\n");
+		IWL_DEBUG_INFO("  in SRAM at 0x%x, size %d u32s\n",
+			       disable_ptr, array_size);
+	}
+
+}
+
+/**
+ * iwl3945_get_antenna_flags - Get antenna flags for RXON command
+ * @priv: eeprom and antenna fields are used to determine antenna flags
+ *
+ * priv->eeprom  is used to determine if antenna AUX/MAIN are reversed
+ * priv->antenna specifies the antenna diversity mode:
+ *
+ * IWL_ANTENNA_DIVERISTY - NIC selects best antenna by itself
+ * IWL_ANTENNA_MAIN      - Force MAIN antenna
+ * IWL_ANTENNA_AUX       - Force AUX antenna
+ */
+__le32 iwl3945_get_antenna_flags(const struct iwl_priv *priv)
+{
+	switch (priv->antenna) {
+	case IWL_ANTENNA_DIVERSITY:
+		return 0;
+
+	case IWL_ANTENNA_MAIN:
+		if (priv->eeprom.antenna_switch_type)
+			return RXON_FLG_DIS_DIV_MSK | RXON_FLG_ANT_B_MSK;
+		return RXON_FLG_DIS_DIV_MSK | RXON_FLG_ANT_A_MSK;
+
+	case IWL_ANTENNA_AUX:
+		if (priv->eeprom.antenna_switch_type)
+			return RXON_FLG_DIS_DIV_MSK | RXON_FLG_ANT_A_MSK;
+		return RXON_FLG_DIS_DIV_MSK | RXON_FLG_ANT_B_MSK;
+	}
+
+	/* bad antenna selector value */
+	IWL_ERROR("Bad antenna selector value (0x%x)\n", priv->antenna);
+	return 0;		/* "diversity" is default if error */
+}
+
+/*****************************************************************************
+ *
+ * Intel PRO/Wireless 3945ABG/BG Network Connection
+ *
+ *  RX handler implementations
+ *
+ *  Used by iwl-base.c
+ *
+ *****************************************************************************/
+
+void iwl_hw_rx_statistics(struct iwl_priv *priv, struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = (void *)rxb->skb->data;
+	IWL_DEBUG_RX("Statistics notification received (%d vs %d).\n",
+		     (int)sizeof(struct iwl_notif_statistics),
+		     le32_to_cpu(pkt->len));
+
+	memcpy(&priv->statistics, pkt->u.raw, sizeof(priv->statistics));
+
+	priv->last_statistics_time = jiffies;
+}
+
+static void iwl3945_handle_data_packet(struct iwl_priv *priv, int is_data,
+				   struct iwl_rx_mem_buffer *rxb,
+				   struct ieee80211_rx_status *stats,
+				   u16 phy_flags)
+{
+	struct ieee80211_hdr *hdr;
+	struct iwl_rx_packet *pkt = (struct iwl_rx_packet *)rxb->skb->data;
+	struct iwl_rx_frame_hdr *rx_hdr = IWL_RX_HDR(pkt);
+	struct iwl_rx_frame_end *rx_end = IWL_RX_END(pkt);
+	short len = le16_to_cpu(rx_hdr->len);
+
+	/* We received data from the HW, so stop the watchdog */
+	if (unlikely((len + IWL_RX_FRAME_SIZE) > skb_tailroom(rxb->skb))) {
+		IWL_DEBUG_DROP("Corruption detected!\n");
+		return;
+	}
+
+	/* We only process data packets if the interface is open */
+	if (unlikely(!priv->is_open)) {
+		IWL_DEBUG_DROP_LIMIT
+		    ("Dropping packet while interface is not open.\n");
+		return;
+	}
+	if (priv->iw_mode == IEEE80211_IF_TYPE_MNTR) {
+		if (iwl_param_hwcrypto)
+			iwl_set_decrypted_flag(priv, rxb->skb,
+					       le32_to_cpu(rx_end->status),
+					       stats);
+		iwl_handle_data_packet_monitor(priv, rxb, IWL_RX_DATA(pkt),
+					       len, stats, phy_flags);
+		return;
+	}
+
+	skb_reserve(rxb->skb, (void *)rx_hdr->payload - (void *)pkt);
+	/* Set the size of the skb to the size of the frame */
+	skb_put(rxb->skb, le16_to_cpu(rx_hdr->len));
+
+	hdr = (void *)rxb->skb->data;
+
+	if (iwl_param_hwcrypto)
+		iwl_set_decrypted_flag(priv, rxb->skb,
+				       le32_to_cpu(rx_end->status), stats);
+
+	ieee80211_rx_irqsafe(priv->hw, rxb->skb, stats);
+	rxb->skb = NULL;
+}
+
+static void iwl3945_rx_reply_rx(struct iwl_priv *priv,
+				struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = (void *)rxb->skb->data;
+	struct iwl_rx_frame_stats *rx_stats = IWL_RX_STATS(pkt);
+	struct iwl_rx_frame_hdr *rx_hdr = IWL_RX_HDR(pkt);
+	struct iwl_rx_frame_end *rx_end = IWL_RX_END(pkt);
+	struct ieee80211_hdr *header;
+	u16 phy_flags = le16_to_cpu(rx_hdr->phy_flags);
+	u16 rx_stats_sig_avg = le16_to_cpu(rx_stats->sig_avg);
+	u16 rx_stats_noise_diff = le16_to_cpu(rx_stats->noise_diff);
+	struct ieee80211_rx_status stats = {
+		.mactime = le64_to_cpu(rx_end->timestamp),
+		.freq = ieee80211chan2mhz(le16_to_cpu(rx_hdr->channel)),
+		.channel = le16_to_cpu(rx_hdr->channel),
+		.phymode = (rx_hdr->phy_flags & RX_RES_PHY_FLAGS_BAND_24_MSK) ?
+		MODE_IEEE80211G : MODE_IEEE80211A,
+		.antenna = 0,
+		.rate = rx_hdr->rate,
+		.flag = 0,
+	};
+	u8 network_packet;
+	int snr;
+
+	if ((unlikely(rx_stats->phy_count > 20))) {
+		IWL_DEBUG_DROP
+		    ("dsp size out of range [0,20]: "
+		     "%d/n", rx_stats->phy_count);
+		return;
+	}
+
+	if (!(rx_end->status & RX_RES_STATUS_NO_CRC32_ERROR)
+	    || !(rx_end->status & RX_RES_STATUS_NO_RXE_OVERFLOW)) {
+		IWL_DEBUG_RX("Bad CRC or FIFO: 0x%08X.\n", rx_end->status);
+		return;
+	}
+
+	if (priv->iw_mode == IEEE80211_IF_TYPE_MNTR) {
+		iwl3945_handle_data_packet(priv, 1, rxb, &stats, phy_flags);
+		return;
+	}
+
+	/* Convert 3945's rssi indicator to dBm */
+	stats.ssi = rx_stats->rssi - IWL_RSSI_OFFSET;
+
+	/* Set default noise value to -127 */
+	if (priv->last_rx_noise == 0)
+		priv->last_rx_noise = IWL_NOISE_MEAS_NOT_AVAILABLE;
+
+	/* 3945 provides noise info for OFDM frames only.
+	 * sig_avg and noise_diff are measured by the 3945's digital signal
+	 *   processor (DSP), and indicate linear levels of signal level and
+	 *   distortion/noise within the packet preamble after
+	 *   automatic gain control (AGC).  sig_avg should stay fairly
+	 *   constant if the radio's AGC is working well.
+	 * Since these values are linear (not dB or dBm), linear
+	 *   signal-to-noise ratio (SNR) is (sig_avg / noise_diff).
+	 * Convert linear SNR to dB SNR, then subtract that from rssi dBm
+	 *   to obtain noise level in dBm.
+	 * Calculate stats.signal (quality indicator in %) based on SNR. */
+	if (rx_stats_noise_diff) {
+		snr = rx_stats_sig_avg / rx_stats_noise_diff;
+		stats.noise = stats.ssi - iwl_calc_db_from_ratio(snr);
+		stats.signal = iwl_calc_sig_qual(stats.ssi, stats.noise);
+
+	/* If noise info not available, calculate signal quality indicator (%)
+	 *   using just the dBm signal level. */
+	} else {
+		stats.noise = priv->last_rx_noise;
+		stats.signal = iwl_calc_sig_qual(stats.ssi, 0);
+	}
+
+
+	IWL_DEBUG_STATS("Rssi %d noise %d qual %d sig_avg %d noise_diff %d\n",
+			stats.ssi, stats.noise, stats.signal,
+			rx_stats_sig_avg, rx_stats_noise_diff);
+
+	stats.freq = ieee80211chan2mhz(stats.channel);
+
+	/* can be covered by iwl_report_frame() in most cases */
+/*      IWL_DEBUG_RX("RX status: 0x%08X\n", rx_end->status); */
+
+	header = (struct ieee80211_hdr *)IWL_RX_DATA(pkt);
+
+	network_packet = iwl_is_network_packet(priv, header);
+
+#ifdef CONFIG_IWLWIFI_DEBUG
+	if (iwl_debug_level & IWL_DL_STATS && net_ratelimit())
+		IWL_DEBUG_STATS
+		    ("[%c] %d RSSI: %d Signal: %u, Noise: %u, Rate: %u\n",
+		     network_packet ? '*' : ' ',
+		     stats.channel, stats.ssi, stats.ssi,
+		     stats.ssi, stats.rate);
+
+	if (iwl_debug_level & (IWL_DL_RX))
+		/* Set "1" to report good data frames in groups of 100 */
+		iwl_report_frame(priv, pkt, header, 1);
+#endif
+
+	if (network_packet) {
+		priv->last_beacon_time = le32_to_cpu(rx_end->beacon_timestamp);
+		priv->last_tsf = le64_to_cpu(rx_end->timestamp);
+		priv->last_rx_rssi = stats.ssi;
+		priv->last_rx_noise = stats.noise;
+	}
+
+	switch (le16_to_cpu(header->frame_control) & IEEE80211_FCTL_FTYPE) {
+	case IEEE80211_FTYPE_MGMT:
+		switch (le16_to_cpu(header->frame_control) &
+			IEEE80211_FCTL_STYPE) {
+		case IEEE80211_STYPE_PROBE_RESP:
+		case IEEE80211_STYPE_BEACON:{
+				/* If this is a beacon or probe response for
+				 * our network then cache the beacon
+				 * timestamp */
+				if ((((priv->iw_mode == IEEE80211_IF_TYPE_STA)
+				      && !compare_ether_addr(header->addr2,
+							     priv->bssid)) ||
+				     ((priv->iw_mode == IEEE80211_IF_TYPE_IBSS)
+				      && !compare_ether_addr(header->addr3,
+							     priv->bssid)))) {
+					struct ieee80211_mgmt *mgmt =
+					    (struct ieee80211_mgmt *)header;
+					__le32 *pos;
+					pos =
+					    (__le32 *) & mgmt->u.beacon.
+					    timestamp;
+					priv->timestamp0 = le32_to_cpu(pos[0]);
+					priv->timestamp1 = le32_to_cpu(pos[1]);
+					priv->beacon_int = le16_to_cpu(
+					    mgmt->u.beacon.beacon_int);
+					if (priv->call_post_assoc_from_beacon &&
+					    (priv->iw_mode ==
+						IEEE80211_IF_TYPE_STA))
+						queue_work(priv->workqueue,
+						    &priv->post_associate.work);
+
+					priv->call_post_assoc_from_beacon = 0;
+				}
+
+				break;
+			}
+
+		case IEEE80211_STYPE_ACTION:
+			/* TODO: Parse 802.11h frames for CSA... */
+			break;
+
+			/*
+			 * TODO: There is no callback function from upper
+			 * stack to inform us when associated status. this
+			 * work around to sniff assoc_resp management frame
+			 * and finish the association process.
+			 */
+		case IEEE80211_STYPE_ASSOC_RESP:
+		case IEEE80211_STYPE_REASSOC_RESP:{
+				struct ieee80211_mgmt *mgnt =
+				    (struct ieee80211_mgmt *)header;
+				priv->assoc_id = (~((1 << 15) | (1 << 14)) &
+						  le16_to_cpu(mgnt->u.
+							      assoc_resp.aid));
+				priv->assoc_capability =
+				    le16_to_cpu(mgnt->u.assoc_resp.capab_info);
+				if (priv->beacon_int)
+					queue_work(priv->workqueue,
+					    &priv->post_associate.work);
+				else
+					priv->call_post_assoc_from_beacon = 1;
+				break;
+			}
+
+		case IEEE80211_STYPE_PROBE_REQ:{
+				DECLARE_MAC_BUF(mac1);
+				DECLARE_MAC_BUF(mac2);
+				DECLARE_MAC_BUF(mac3);
+				if (priv->iw_mode == IEEE80211_IF_TYPE_IBSS)
+					IWL_DEBUG_DROP
+					    ("Dropping (non network): %s"
+					     ", %s, %s\n",
+					     print_mac(mac1, header->addr1),
+					     print_mac(mac2, header->addr2),
+					     print_mac(mac3, header->addr3));
+				return;
+			}
+		}
+
+		iwl3945_handle_data_packet(priv, 0, rxb, &stats, phy_flags);
+		break;
+
+	case IEEE80211_FTYPE_CTL:
+		break;
+
+	case IEEE80211_FTYPE_DATA: {
+		DECLARE_MAC_BUF(mac1);
+		DECLARE_MAC_BUF(mac2);
+		DECLARE_MAC_BUF(mac3);
+
+		if (unlikely(is_duplicate_packet(priv, header)))
+			IWL_DEBUG_DROP("Dropping (dup): %s, %s, %s\n",
+				       print_mac(mac1, header->addr1),
+				       print_mac(mac2, header->addr2),
+				       print_mac(mac3, header->addr3));
+		else
+			iwl3945_handle_data_packet(priv, 1, rxb, &stats,
+						   phy_flags);
+		break;
+	}
+	}
+}
+
+int iwl_hw_txq_attach_buf_to_tfd(struct iwl_priv *priv, void *ptr,
+				 dma_addr_t addr, u16 len)
+{
+	int count;
+	u32 pad;
+	struct iwl_tfd_frame *tfd = (struct iwl_tfd_frame *)ptr;
+
+	count = TFD_CTL_COUNT_GET(le32_to_cpu(tfd->control_flags));
+	pad = TFD_CTL_PAD_GET(le32_to_cpu(tfd->control_flags));
+
+	if ((count >= NUM_TFD_CHUNKS) || (count < 0)) {
+		IWL_ERROR("Error can not send more than %d chunks\n",
+			  NUM_TFD_CHUNKS);
+		return -EINVAL;
+	}
+
+	tfd->pa[count].addr = cpu_to_le32(addr);
+	tfd->pa[count].len = cpu_to_le32(len);
+
+	count++;
+
+	tfd->control_flags = cpu_to_le32(TFD_CTL_COUNT_SET(count) |
+					 TFD_CTL_PAD_SET(pad));
+
+	return 0;
+}
+
+/**
+ * iwl_hw_txq_free_tfd - Free one TFD, those at index [txq->q.last_used]
+ *
+ * Does NOT advance any indexes
+ */
+int iwl_hw_txq_free_tfd(struct iwl_priv *priv, struct iwl_tx_queue *txq)
+{
+	struct iwl_tfd_frame *bd_tmp = (struct iwl_tfd_frame *)&txq->bd[0];
+	struct iwl_tfd_frame *bd = &bd_tmp[txq->q.last_used];
+	struct pci_dev *dev = priv->pci_dev;
+	int i;
+	int counter;
+
+	/* classify bd */
+	if (txq->q.id == IWL_CMD_QUEUE_NUM)
+		/* nothing to cleanup after for host commands */
+		return 0;
+
+	/* sanity check */
+	counter = TFD_CTL_COUNT_GET(le32_to_cpu(bd->control_flags));
+	if (counter > NUM_TFD_CHUNKS) {
+		IWL_ERROR("Too many chunks: %i\n", counter);
+		/* @todo issue fatal error, it is quite serious situation */
+		return 0;
+	}
+
+	/* unmap chunks if any */
+
+	for (i = 1; i < counter; i++) {
+		pci_unmap_single(dev, le32_to_cpu(bd->pa[i].addr),
+				 le32_to_cpu(bd->pa[i].len), PCI_DMA_TODEVICE);
+		if (txq->txb[txq->q.last_used].skb[0]) {
+			struct sk_buff *skb = txq->txb[txq->q.last_used].skb[0];
+			if (txq->txb[txq->q.last_used].skb[0]) {
+				/* Can be called from interrupt context */
+				dev_kfree_skb_any(skb);
+				txq->txb[txq->q.last_used].skb[0] = NULL;
+			}
+		}
+	}
+	return 0;
+}
+
+u8 iwl_hw_find_station(struct iwl_priv *priv, const u8 *addr)
+{
+	int i;
+	int ret = IWL_INVALID_STATION;
+	unsigned long flags;
+	DECLARE_MAC_BUF(mac);
+
+	spin_lock_irqsave(&priv->sta_lock, flags);
+	for (i = IWL_STA_ID; i < priv->hw_setting.max_stations; i++)
+		if ((priv->stations[i].used) &&
+		    (!compare_ether_addr
+		     (priv->stations[i].sta.sta.addr, addr))) {
+			ret = i;
+			goto out;
+		}
+
+	IWL_DEBUG_INFO("can not find STA %s (total %d)\n",
+		       print_mac(mac, addr), priv->num_stations);
+ out:
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+	return ret;
+}
+
+/**
+ * iwl_hw_build_tx_cmd_rate - Add rate portion to TX_CMD:
+ *
+*/
+void iwl_hw_build_tx_cmd_rate(struct iwl_priv *priv,
+			      struct iwl_cmd *cmd,
+			      struct ieee80211_tx_control *ctrl,
+			      struct ieee80211_hdr *hdr, int sta_id, int tx_id)
+{
+	unsigned long flags;
+	u16 rate_index = min(ctrl->tx_rate & 0xffff, IWL_RATE_COUNT - 1);
+	u16 rate_mask;
+	int rate;
+	u8 rts_retry_limit;
+	u8 data_retry_limit;
+	__le32 tx_flags;
+	u16 fc = le16_to_cpu(hdr->frame_control);
+
+	rate = iwl_rates[rate_index].plcp;
+	tx_flags = cmd->cmd.tx.tx_flags;
+
+	/* We need to figure out how to get the sta->supp_rates while
+	 * in this running context; perhaps encoding into ctrl->tx_rate? */
+	rate_mask = IWL_RATES_MASK;
+
+	spin_lock_irqsave(&priv->sta_lock, flags);
+
+	priv->stations[sta_id].current_rate.rate_n_flags = rate;
+
+	if ((priv->iw_mode == IEEE80211_IF_TYPE_IBSS) &&
+	    (sta_id != IWL3945_BROADCAST_ID) &&
+		(sta_id != IWL_MULTICAST_ID))
+		priv->stations[IWL_STA_ID].current_rate.rate_n_flags = rate;
+
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+
+	if (tx_id >= IWL_CMD_QUEUE_NUM)
+		rts_retry_limit = 3;
+	else
+		rts_retry_limit = 7;
+
+	if (ieee80211_is_probe_response(fc)) {
+		data_retry_limit = 3;
+		if (data_retry_limit < rts_retry_limit)
+			rts_retry_limit = data_retry_limit;
+	} else
+		data_retry_limit = IWL_DEFAULT_TX_RETRY;
+
+	if (priv->data_retry_limit != -1)
+		data_retry_limit = priv->data_retry_limit;
+
+	if ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT) {
+		switch (fc & IEEE80211_FCTL_STYPE) {
+		case IEEE80211_STYPE_AUTH:
+		case IEEE80211_STYPE_DEAUTH:
+		case IEEE80211_STYPE_ASSOC_REQ:
+		case IEEE80211_STYPE_REASSOC_REQ:
+			if (tx_flags & TX_CMD_FLG_RTS_MSK) {
+				tx_flags &= ~TX_CMD_FLG_RTS_MSK;
+				tx_flags |= TX_CMD_FLG_CTS_MSK;
+			}
+			break;
+		default:
+			break;
+		}
+	}
+
+	cmd->cmd.tx.rts_retry_limit = rts_retry_limit;
+	cmd->cmd.tx.data_retry_limit = data_retry_limit;
+	cmd->cmd.tx.rate = rate;
+	cmd->cmd.tx.tx_flags = tx_flags;
+
+	/* OFDM */
+	cmd->cmd.tx.supp_rates[0] = rate_mask & IWL_OFDM_RATES_MASK;
+
+	/* CCK */
+	cmd->cmd.tx.supp_rates[1] = (rate_mask >> 8) & 0xF;
+
+	IWL_DEBUG_RATE("Tx sta id: %d, rate: %d (plcp), flags: 0x%4X "
+		       "cck/ofdm mask: 0x%x/0x%x\n", sta_id,
+		       cmd->cmd.tx.rate, le32_to_cpu(cmd->cmd.tx.tx_flags),
+		       cmd->cmd.tx.supp_rates[1], cmd->cmd.tx.supp_rates[0]);
+}
+
+u8 iwl3945_sync_sta(struct iwl_priv *priv, int sta_id, u16 tx_rate, u8 flags)
+{
+	unsigned long flags_spin;
+	struct iwl_station_entry *station;
+
+	if (sta_id == IWL_INVALID_STATION)
+		return IWL_INVALID_STATION;
+
+	spin_lock_irqsave(&priv->sta_lock, flags_spin);
+	station = &priv->stations[sta_id];
+
+	station->sta.sta.modify_mask = STA_MODIFY_TX_RATE_MSK;
+	station->sta.rate_n_flags = cpu_to_le16(tx_rate);
+	station->current_rate.rate_n_flags = tx_rate;
+	station->sta.mode = STA_CONTROL_MODIFY_MSK;
+
+	spin_unlock_irqrestore(&priv->sta_lock, flags_spin);
+
+	iwl_send_add_station(priv, &station->sta, flags);
+	IWL_DEBUG_RATE("SCALE sync station %d to rate %d\n",
+			sta_id, tx_rate);
+	return sta_id;
+}
+
+void iwl_hw_card_show_info(struct iwl_priv *priv)
+{
+	IWL_DEBUG_INFO("3945ABG HW Version %u.%u.%u\n",
+		       ((priv->eeprom.board_revision >> 8) & 0x0F),
+		       ((priv->eeprom.board_revision >> 8) >> 4),
+		       (priv->eeprom.board_revision & 0x00FF));
+
+	IWL_DEBUG_INFO("3945ABG PBA Number %.*s\n",
+		       (int)sizeof(priv->eeprom.board_pba_number),
+		       priv->eeprom.board_pba_number);
+
+	IWL_DEBUG_INFO("EEPROM_ANTENNA_SWITCH_TYPE is 0x%02X\n",
+		       priv->eeprom.antenna_switch_type);
+}
+
+static int iwl3945_nic_set_pwr_src(struct iwl_priv *priv, int pwr_max)
+{
+	int rc;
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	rc = iwl_grab_restricted_access(priv);
+	if (rc) {
+		spin_unlock_irqrestore(&priv->lock, flags);
+		return rc;
+	}
+
+	if (!pwr_max) {
+		u32 val;
+
+		rc = pci_read_config_dword(priv->pci_dev,
+				PCI_POWER_SOURCE, &val);
+		if (val & PCI_CFG_PMC_PME_FROM_D3COLD_SUPPORT) {
+			iwl_set_bits_mask_restricted_reg(priv, APMG_PS_CTRL_REG,
+					APMG_PS_CTRL_VAL_PWR_SRC_VAUX,
+					~APMG_PS_CTRL_MSK_PWR_SRC);
+			iwl_release_restricted_access(priv);
+
+			iwl_poll_bit(priv, CSR_GPIO_IN,
+				     CSR_GPIO_IN_VAL_VAUX_PWR_SRC,
+				     CSR_GPIO_IN_BIT_AUX_POWER, 5000);
+		} else
+			iwl_release_restricted_access(priv);
+	} else {
+		iwl_set_bits_mask_restricted_reg(priv, APMG_PS_CTRL_REG,
+				APMG_PS_CTRL_VAL_PWR_SRC_VMAIN,
+				~APMG_PS_CTRL_MSK_PWR_SRC);
+
+		iwl_release_restricted_access(priv);
+		iwl_poll_bit(priv, CSR_GPIO_IN, CSR_GPIO_IN_VAL_VMAIN_PWR_SRC,
+			     CSR_GPIO_IN_BIT_AUX_POWER, 5000);	/* uS */
+	}
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return rc;
+}
+
+static int iwl3945_rx_init(struct iwl_priv *priv, struct iwl_rx_queue *rxq)
+{
+	int rc;
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	rc = iwl_grab_restricted_access(priv);
+	if (rc) {
+		spin_unlock_irqrestore(&priv->lock, flags);
+		return rc;
+	}
+
+	iwl_write_restricted(priv, FH_RCSR_RBD_BASE(0), rxq->dma_addr);
+	iwl_write_restricted(priv, FH_RCSR_RPTR_ADDR(0),
+			     priv->hw_setting.shared_phys +
+			     offsetof(struct iwl_shared, rx_read_ptr[0]));
+	iwl_write_restricted(priv, FH_RCSR_WPTR(0), 0);
+	iwl_write_restricted(priv, FH_RCSR_CONFIG(0),
+		ALM_FH_RCSR_RX_CONFIG_REG_VAL_DMA_CHNL_EN_ENABLE |
+		ALM_FH_RCSR_RX_CONFIG_REG_VAL_RDRBD_EN_ENABLE |
+		ALM_FH_RCSR_RX_CONFIG_REG_BIT_WR_STTS_EN |
+		ALM_FH_RCSR_RX_CONFIG_REG_VAL_MAX_FRAG_SIZE_128 |
+		(RX_QUEUE_SIZE_LOG << ALM_FH_RCSR_RX_CONFIG_REG_POS_RBDC_SIZE) |
+		ALM_FH_RCSR_RX_CONFIG_REG_VAL_IRQ_DEST_INT_HOST |
+		(1 << ALM_FH_RCSR_RX_CONFIG_REG_POS_IRQ_RBTH) |
+		ALM_FH_RCSR_RX_CONFIG_REG_VAL_MSG_MODE_FH);
+
+	/* fake read to flush all prev I/O */
+	iwl_read_restricted(priv, FH_RSSR_CTRL);
+
+	iwl_release_restricted_access(priv);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return 0;
+}
+
+static int iwl3945_tx_reset(struct iwl_priv *priv)
+{
+	int rc;
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	rc = iwl_grab_restricted_access(priv);
+	if (rc) {
+		spin_unlock_irqrestore(&priv->lock, flags);
+		return rc;
+	}
+
+	/* bypass mode */
+	iwl_write_restricted_reg(priv, SCD_MODE_REG, 0x2);
+
+	/* RA 0 is active */
+	iwl_write_restricted_reg(priv, SCD_ARASTAT_REG, 0x01);
+
+	/* all 6 fifo are active */
+	iwl_write_restricted_reg(priv, SCD_TXFACT_REG, 0x3f);
+
+	iwl_write_restricted_reg(priv, SCD_SBYP_MODE_1_REG, 0x010000);
+	iwl_write_restricted_reg(priv, SCD_SBYP_MODE_2_REG, 0x030002);
+	iwl_write_restricted_reg(priv, SCD_TXF4MF_REG, 0x000004);
+	iwl_write_restricted_reg(priv, SCD_TXF5MF_REG, 0x000005);
+
+	iwl_write_restricted(priv, FH_TSSR_CBB_BASE,
+			     priv->hw_setting.shared_phys);
+
+	iwl_write_restricted(priv, FH_TSSR_MSG_CONFIG,
+		ALM_FH_TSSR_TX_MSG_CONFIG_REG_VAL_SNOOP_RD_TXPD_ON |
+		ALM_FH_TSSR_TX_MSG_CONFIG_REG_VAL_ORDER_RD_TXPD_ON |
+		ALM_FH_TSSR_TX_MSG_CONFIG_REG_VAL_MAX_FRAG_SIZE_128B |
+		ALM_FH_TSSR_TX_MSG_CONFIG_REG_VAL_SNOOP_RD_TFD_ON |
+		ALM_FH_TSSR_TX_MSG_CONFIG_REG_VAL_ORDER_RD_CBB_ON |
+		ALM_FH_TSSR_TX_MSG_CONFIG_REG_VAL_ORDER_RSP_WAIT_TH |
+		ALM_FH_TSSR_TX_MSG_CONFIG_REG_VAL_RSP_WAIT_TH);
+
+	iwl_release_restricted_access(priv);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return 0;
+}
+
+/**
+ * iwl3945_txq_ctx_reset - Reset TX queue context
+ *
+ * Destroys all DMA structures and initialize them again
+ */
+static int iwl3945_txq_ctx_reset(struct iwl_priv *priv)
+{
+	int rc;
+	int txq_id, slots_num;
+
+	iwl_hw_txq_ctx_free(priv);
+
+	/* Tx CMD queue */
+	rc = iwl3945_tx_reset(priv);
+	if (rc)
+		goto error;
+
+	/* Tx queue(s) */
+	for (txq_id = 0; txq_id < TFD_QUEUE_MAX; txq_id++) {
+		slots_num = (txq_id == IWL_CMD_QUEUE_NUM) ?
+				TFD_CMD_SLOTS : TFD_TX_CMD_SLOTS;
+		rc = iwl_tx_queue_init(priv, &priv->txq[txq_id], slots_num,
+				txq_id);
+		if (rc) {
+			IWL_ERROR("Tx %d queue init failed\n", txq_id);
+			goto error;
+		}
+	}
+
+	return rc;
+
+ error:
+	iwl_hw_txq_ctx_free(priv);
+	return rc;
+}
+
+int iwl_hw_nic_init(struct iwl_priv *priv)
+{
+	u8 rev_id;
+	int rc;
+	unsigned long flags;
+	struct iwl_rx_queue *rxq = &priv->rxq;
+
+	iwl_power_init_handle(priv);
+
+	spin_lock_irqsave(&priv->lock, flags);
+	iwl_set_bit(priv, CSR_ANA_PLL_CFG, (1 << 24));
+	iwl_set_bit(priv, CSR_GIO_CHICKEN_BITS,
+		    CSR_GIO_CHICKEN_BITS_REG_BIT_L1A_NO_L0S_RX);
+
+	iwl_set_bit(priv, CSR_GP_CNTRL, CSR_GP_CNTRL_REG_FLAG_INIT_DONE);
+	rc = iwl_poll_bit(priv, CSR_GP_CNTRL,
+			  CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY,
+			  CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY, 25000);
+	if (rc < 0) {
+		spin_unlock_irqrestore(&priv->lock, flags);
+		IWL_DEBUG_INFO("Failed to init the card\n");
+		return rc;
+	}
+
+	rc = iwl_grab_restricted_access(priv);
+	if (rc) {
+		spin_unlock_irqrestore(&priv->lock, flags);
+		return rc;
+	}
+	iwl_write_restricted_reg(priv, APMG_CLK_EN_REG,
+				 APMG_CLK_VAL_DMA_CLK_RQT |
+				 APMG_CLK_VAL_BSM_CLK_RQT);
+	udelay(20);
+	iwl_set_bits_restricted_reg(priv, APMG_PCIDEV_STT_REG,
+				    APMG_PCIDEV_STT_VAL_L1_ACT_DIS);
+	iwl_release_restricted_access(priv);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	/* Determine HW type */
+	rc = pci_read_config_byte(priv->pci_dev, PCI_REVISION_ID, &rev_id);
+	if (rc)
+		return rc;
+	IWL_DEBUG_INFO("HW Revision ID = 0x%X\n", rev_id);
+
+	iwl3945_nic_set_pwr_src(priv, 1);
+	spin_lock_irqsave(&priv->lock, flags);
+
+	if (rev_id & PCI_CFG_REV_ID_BIT_RTP)
+		IWL_DEBUG_INFO("RTP type \n");
+	else if (rev_id & PCI_CFG_REV_ID_BIT_BASIC_SKU) {
+		IWL_DEBUG_INFO("ALM-MB type\n");
+		iwl_set_bit(priv, CSR_HW_IF_CONFIG_REG,
+			    CSR_HW_IF_CONFIG_REG_BIT_ALMAGOR_MB);
+	} else {
+		IWL_DEBUG_INFO("ALM-MM type\n");
+		iwl_set_bit(priv, CSR_HW_IF_CONFIG_REG,
+			    CSR_HW_IF_CONFIG_REG_BIT_ALMAGOR_MM);
+	}
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	/* Initialize the EEPROM */
+	rc = iwl_eeprom_init(priv);
+	if (rc)
+		return rc;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	if (EEPROM_SKU_CAP_OP_MODE_MRC == priv->eeprom.sku_cap) {
+		IWL_DEBUG_INFO("SKU OP mode is mrc\n");
+		iwl_set_bit(priv, CSR_HW_IF_CONFIG_REG,
+			    CSR_HW_IF_CONFIG_REG_BIT_SKU_MRC);
+	} else
+		IWL_DEBUG_INFO("SKU OP mode is basic\n");
+
+	if ((priv->eeprom.board_revision & 0xF0) == 0xD0) {
+		IWL_DEBUG_INFO("3945ABG revision is 0x%X\n",
+			       priv->eeprom.board_revision);
+		iwl_set_bit(priv, CSR_HW_IF_CONFIG_REG,
+			    CSR_HW_IF_CONFIG_REG_BIT_BOARD_TYPE);
+	} else {
+		IWL_DEBUG_INFO("3945ABG revision is 0x%X\n",
+			       priv->eeprom.board_revision);
+		iwl_clear_bit(priv, CSR_HW_IF_CONFIG_REG,
+			      CSR_HW_IF_CONFIG_REG_BIT_BOARD_TYPE);
+	}
+
+	if (priv->eeprom.almgor_m_version <= 1) {
+		iwl_set_bit(priv, CSR_HW_IF_CONFIG_REG,
+			    CSR_HW_IF_CONFIG_REG_BITS_SILICON_TYPE_A);
+		IWL_DEBUG_INFO("Card M type A version is 0x%X\n",
+			       priv->eeprom.almgor_m_version);
+	} else {
+		IWL_DEBUG_INFO("Card M type B version is 0x%X\n",
+			       priv->eeprom.almgor_m_version);
+		iwl_set_bit(priv, CSR_HW_IF_CONFIG_REG,
+			    CSR_HW_IF_CONFIG_REG_BITS_SILICON_TYPE_B);
+	}
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	if (priv->eeprom.sku_cap & EEPROM_SKU_CAP_SW_RF_KILL_ENABLE)
+		IWL_DEBUG_RF_KILL("SW RF KILL supported in EEPROM.\n");
+
+	if (priv->eeprom.sku_cap & EEPROM_SKU_CAP_HW_RF_KILL_ENABLE)
+		IWL_DEBUG_RF_KILL("HW RF KILL supported in EEPROM.\n");
+
+	/* Allocate the RX queue, or reset if it is already allocated */
+	if (!rxq->bd) {
+		rc = iwl_rx_queue_alloc(priv);
+		if (rc) {
+			IWL_ERROR("Unable to initialize Rx queue\n");
+			return -ENOMEM;
+		}
+	} else
+		iwl_rx_queue_reset(priv, rxq);
+
+	iwl_rx_replenish(priv);
+
+	iwl3945_rx_init(priv, rxq);
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	/* Look at using this instead:
+	rxq->need_update = 1;
+	iwl_rx_queue_update_write_ptr(priv, rxq);
+	*/
+
+	rc = iwl_grab_restricted_access(priv);
+	if (rc) {
+		spin_unlock_irqrestore(&priv->lock, flags);
+		return rc;
+	}
+	iwl_write_restricted(priv, FH_RCSR_WPTR(0), rxq->write & ~7);
+	iwl_release_restricted_access(priv);
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	rc = iwl3945_txq_ctx_reset(priv);
+	if (rc)
+		return rc;
+
+	set_bit(STATUS_INIT, &priv->status);
+
+	return 0;
+}
+
+/**
+ * iwl_hw_txq_ctx_free - Free TXQ Context
+ *
+ * Destroy all TX DMA queues and structures
+ */
+void iwl_hw_txq_ctx_free(struct iwl_priv *priv)
+{
+	int txq_id;
+
+	/* Tx queues */
+	for (txq_id = 0; txq_id < TFD_QUEUE_MAX; txq_id++)
+		iwl_tx_queue_free(priv, &priv->txq[txq_id]);
+}
+
+void iwl_hw_txq_ctx_stop(struct iwl_priv *priv)
+{
+	int queue;
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	if (iwl_grab_restricted_access(priv)) {
+		spin_unlock_irqrestore(&priv->lock, flags);
+		iwl_hw_txq_ctx_free(priv);
+		return;
+	}
+
+	/* stop SCD */
+	iwl_write_restricted_reg(priv, SCD_MODE_REG, 0);
+
+	/* reset TFD queues */
+	for (queue = TFD_QUEUE_MIN; queue < TFD_QUEUE_MAX; queue++) {
+		iwl_write_restricted(priv, FH_TCSR_CONFIG(queue), 0x0);
+		iwl_poll_restricted_bit(priv, FH_TSSR_TX_STATUS,
+				ALM_FH_TSSR_TX_STATUS_REG_MSK_CHNL_IDLE(queue),
+				1000);
+	}
+
+	iwl_release_restricted_access(priv);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	iwl_hw_txq_ctx_free(priv);
+}
+
+int iwl_hw_nic_stop_master(struct iwl_priv *priv)
+{
+	int rc = 0;
+	u32 reg_val;
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	/* set stop master bit */
+	iwl_set_bit(priv, CSR_RESET, CSR_RESET_REG_FLAG_STOP_MASTER);
+
+	reg_val = iwl_read32(priv, CSR_GP_CNTRL);
+
+	if (CSR_GP_CNTRL_REG_FLAG_MAC_POWER_SAVE ==
+	    (reg_val & CSR_GP_CNTRL_REG_MSK_POWER_SAVE_TYPE))
+		IWL_DEBUG_INFO("Card in power save, master is already "
+			       "stopped\n");
+	else {
+		rc = iwl_poll_bit(priv, CSR_RESET,
+				  CSR_RESET_REG_FLAG_MASTER_DISABLED,
+				  CSR_RESET_REG_FLAG_MASTER_DISABLED, 100);
+		if (rc < 0) {
+			spin_unlock_irqrestore(&priv->lock, flags);
+			return rc;
+		}
+	}
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+	IWL_DEBUG_INFO("stop master\n");
+
+	return rc;
+}
+
+int iwl_hw_nic_reset(struct iwl_priv *priv)
+{
+	int rc;
+	unsigned long flags;
+
+	iwl_hw_nic_stop_master(priv);
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	iwl_set_bit(priv, CSR_RESET, CSR_RESET_REG_FLAG_SW_RESET);
+
+	rc = iwl_poll_bit(priv, CSR_GP_CNTRL,
+			  CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY,
+			  CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY, 25000);
+
+	rc = iwl_grab_restricted_access(priv);
+	if (!rc) {
+		iwl_write_restricted_reg(priv, APMG_CLK_CTRL_REG,
+					 APMG_CLK_VAL_BSM_CLK_RQT);
+
+		udelay(10);
+
+		iwl_set_bit(priv, CSR_GP_CNTRL,
+			    CSR_GP_CNTRL_REG_FLAG_INIT_DONE);
+
+		iwl_write_restricted_reg(priv, APMG_RTC_INT_MSK_REG, 0x0);
+		iwl_write_restricted_reg(priv, APMG_RTC_INT_STT_REG,
+					0xFFFFFFFF);
+
+		/* enable DMA */
+		iwl_write_restricted_reg(priv, APMG_CLK_EN_REG,
+					 APMG_CLK_VAL_DMA_CLK_RQT |
+					 APMG_CLK_VAL_BSM_CLK_RQT);
+		udelay(10);
+
+		iwl_set_bits_restricted_reg(priv, APMG_PS_CTRL_REG,
+				APMG_PS_CTRL_VAL_RESET_REQ);
+		udelay(5);
+		iwl_clear_bits_restricted_reg(priv, APMG_PS_CTRL_REG,
+				APMG_PS_CTRL_VAL_RESET_REQ);
+		iwl_release_restricted_access(priv);
+	}
+
+	/* Clear the 'host command active' bit... */
+	clear_bit(STATUS_HCMD_ACTIVE, &priv->status);
+
+	wake_up_interruptible(&priv->wait_command_queue);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return rc;
+}
+
+/**
+ * iwl_hw_reg_adjust_power_by_temp - return index delta into power gain settings table
+ */
+static int iwl_hw_reg_adjust_power_by_temp(int new_reading, int old_reading)
+{
+	return (new_reading - old_reading) * (-11) / 100;
+}
+
+/**
+ * iwl_hw_reg_temp_out_of_range - Keep temperature in sane range
+ */
+static inline int iwl_hw_reg_temp_out_of_range(int temperature)
+{
+	return (((temperature < -260) || (temperature > 25)) ? 1 : 0);
+}
+
+int iwl_hw_get_temperature(struct iwl_priv *priv)
+{
+	return iwl_read32(priv, CSR_UCODE_DRV_GP2);
+}
+
+/**
+ * iwl_hw_reg_txpower_get_temperature - get current temperature by reading from NIC
+ */
+static int iwl_hw_reg_txpower_get_temperature(struct iwl_priv *priv)
+{
+	int temperature;
+
+	temperature = iwl_hw_get_temperature(priv);
+
+	/* driver's okay range is -260 to +25.
+	 *   human readable okay range is 0 to +285 */
+	IWL_DEBUG_INFO("Temperature: %d\n", temperature + IWL_TEMP_CONVERT);
+
+	/* handle insane temp reading */
+	if (iwl_hw_reg_temp_out_of_range(temperature)) {
+		IWL_ERROR("Error bad temperature value  %d\n", temperature);
+
+		/* if really really hot(?),
+		 *   substitute the 3rd band/group's temp measured at factory */
+		if (priv->last_temperature > 100)
+			temperature = priv->eeprom.groups[2].temperature;
+		else /* else use most recent "sane" value from driver */
+			temperature = priv->last_temperature;
+	}
+
+	return temperature;	/* raw, not "human readable" */
+}
+
+/* Adjust Txpower only if temperature variance is greater than threshold.
+ *
+ * Both are lower than older versions' 9 degrees */
+#define IWL_TEMPERATURE_LIMIT_TIMER   6
+
+/**
+ * is_temp_calib_needed - determines if new calibration is needed
+ *
+ * records new temperature in tx_mgr->temperature.
+ * replaces tx_mgr->last_temperature *only* if calib needed
+ *    (assumes caller will actually do the calibration!). */
+static int is_temp_calib_needed(struct iwl_priv *priv)
+{
+	int temp_diff;
+
+	priv->temperature = iwl_hw_reg_txpower_get_temperature(priv);
+	temp_diff = priv->temperature - priv->last_temperature;
+
+	/* get absolute value */
+	if (temp_diff < 0) {
+		IWL_DEBUG_POWER("Getting cooler, delta %d,\n", temp_diff);
+		temp_diff = -temp_diff;
+	} else if (temp_diff == 0)
+		IWL_DEBUG_POWER("Same temp,\n");
+	else
+		IWL_DEBUG_POWER("Getting warmer, delta %d,\n", temp_diff);
+
+	/* if we don't need calibration, *don't* update last_temperature */
+	if (temp_diff < IWL_TEMPERATURE_LIMIT_TIMER) {
+		IWL_DEBUG_POWER("Timed thermal calib not needed\n");
+		return 0;
+	}
+
+	IWL_DEBUG_POWER("Timed thermal calib needed\n");
+
+	/* assume that caller will actually do calib ...
+	 *   update the "last temperature" value */
+	priv->last_temperature = priv->temperature;
+	return 1;
+}
+
+#define IWL_MAX_GAIN_ENTRIES 78
+#define IWL_CCK_FROM_OFDM_POWER_DIFF  -5
+#define IWL_CCK_FROM_OFDM_INDEX_DIFF (10)
+
+/* radio and DSP power table, each step is 1/2 dB.
+ * 1st number is for RF analog gain, 2nd number is for DSP pre-DAC gain. */
+static struct iwl_tx_power power_gain_table[2][IWL_MAX_GAIN_ENTRIES] = {
+	{
+	 {251, 127},		/* 2.4 GHz, highest power */
+	 {251, 127},
+	 {251, 127},
+	 {251, 127},
+	 {251, 125},
+	 {251, 110},
+	 {251, 105},
+	 {251, 98},
+	 {187, 125},
+	 {187, 115},
+	 {187, 108},
+	 {187, 99},
+	 {243, 119},
+	 {243, 111},
+	 {243, 105},
+	 {243, 97},
+	 {243, 92},
+	 {211, 106},
+	 {211, 100},
+	 {179, 120},
+	 {179, 113},
+	 {179, 107},
+	 {147, 125},
+	 {147, 119},
+	 {147, 112},
+	 {147, 106},
+	 {147, 101},
+	 {147, 97},
+	 {147, 91},
+	 {115, 107},
+	 {235, 121},
+	 {235, 115},
+	 {235, 109},
+	 {203, 127},
+	 {203, 121},
+	 {203, 115},
+	 {203, 108},
+	 {203, 102},
+	 {203, 96},
+	 {203, 92},
+	 {171, 110},
+	 {171, 104},
+	 {171, 98},
+	 {139, 116},
+	 {227, 125},
+	 {227, 119},
+	 {227, 113},
+	 {227, 107},
+	 {227, 101},
+	 {227, 96},
+	 {195, 113},
+	 {195, 106},
+	 {195, 102},
+	 {195, 95},
+	 {163, 113},
+	 {163, 106},
+	 {163, 102},
+	 {163, 95},
+	 {131, 113},
+	 {131, 106},
+	 {131, 102},
+	 {131, 95},
+	 {99, 113},
+	 {99, 106},
+	 {99, 102},
+	 {99, 95},
+	 {67, 113},
+	 {67, 106},
+	 {67, 102},
+	 {67, 95},
+	 {35, 113},
+	 {35, 106},
+	 {35, 102},
+	 {35, 95},
+	 {3, 113},
+	 {3, 106},
+	 {3, 102},
+	 {3, 95} },		/* 2.4 GHz, lowest power */
+	{
+	 {251, 127},		/* 5.x GHz, highest power */
+	 {251, 120},
+	 {251, 114},
+	 {219, 119},
+	 {219, 101},
+	 {187, 113},
+	 {187, 102},
+	 {155, 114},
+	 {155, 103},
+	 {123, 117},
+	 {123, 107},
+	 {123, 99},
+	 {123, 92},
+	 {91, 108},
+	 {59, 125},
+	 {59, 118},
+	 {59, 109},
+	 {59, 102},
+	 {59, 96},
+	 {59, 90},
+	 {27, 104},
+	 {27, 98},
+	 {27, 92},
+	 {115, 118},
+	 {115, 111},
+	 {115, 104},
+	 {83, 126},
+	 {83, 121},
+	 {83, 113},
+	 {83, 105},
+	 {83, 99},
+	 {51, 118},
+	 {51, 111},
+	 {51, 104},
+	 {51, 98},
+	 {19, 116},
+	 {19, 109},
+	 {19, 102},
+	 {19, 98},
+	 {19, 93},
+	 {171, 113},
+	 {171, 107},
+	 {171, 99},
+	 {139, 120},
+	 {139, 113},
+	 {139, 107},
+	 {139, 99},
+	 {107, 120},
+	 {107, 113},
+	 {107, 107},
+	 {107, 99},
+	 {75, 120},
+	 {75, 113},
+	 {75, 107},
+	 {75, 99},
+	 {43, 120},
+	 {43, 113},
+	 {43, 107},
+	 {43, 99},
+	 {11, 120},
+	 {11, 113},
+	 {11, 107},
+	 {11, 99},
+	 {131, 107},
+	 {131, 99},
+	 {99, 120},
+	 {99, 113},
+	 {99, 107},
+	 {99, 99},
+	 {67, 120},
+	 {67, 113},
+	 {67, 107},
+	 {67, 99},
+	 {35, 120},
+	 {35, 113},
+	 {35, 107},
+	 {35, 99},
+	 {3, 120} }		/* 5.x GHz, lowest power */
+};
+
+static inline u8 iwl_hw_reg_fix_power_index(int index)
+{
+	if (index < 0)
+		return 0;
+	if (index >= IWL_MAX_GAIN_ENTRIES)
+		return IWL_MAX_GAIN_ENTRIES - 1;
+	return (u8) index;
+}
+
+/* Kick off thermal recalibration check every 60 seconds */
+#define REG_RECALIB_PERIOD (60)
+
+/**
+ * iwl_hw_reg_set_scan_power - Set Tx power for scan probe requests
+ *
+ * Set (in our channel info database) the direct scan Tx power for 1 Mbit (CCK)
+ * or 6 Mbit (OFDM) rates.
+ */
+static void iwl_hw_reg_set_scan_power(struct iwl_priv *priv, u32 scan_tbl_index,
+			       s32 rate_index, const s8 *clip_pwrs,
+			       struct iwl_channel_info *ch_info,
+			       int band_index)
+{
+	struct iwl_scan_power_info *scan_power_info;
+	s8 power;
+	u8 power_index;
+
+	scan_power_info = &ch_info->scan_pwr_info[scan_tbl_index];
+
+	/* use this channel group's 6Mbit clipping/saturation pwr,
+	 *   but cap at regulatory scan power restriction (set during init
+	 *   based on eeprom channel data) for this channel.  */
+	power = min(ch_info->scan_power, clip_pwrs[IWL_RATE_6M_INDEX]);
+
+	/* further limit to user's max power preference.
+	 * FIXME:  Other spectrum management power limitations do not
+	 *   seem to apply?? */
+	power = min(power, priv->user_txpower_limit);
+	scan_power_info->requested_power = power;
+
+	/* find difference between new scan *power* and current "normal"
+	 *   Tx *power* for 6Mb.  Use this difference (x2) to adjust the
+	 *   current "normal" temperature-compensated Tx power *index* for
+	 *   this rate (1Mb or 6Mb) to yield new temp-compensated scan power
+	 *   *index*. */
+	power_index = ch_info->power_info[rate_index].power_table_index
+	    - (power - ch_info->power_info
+	       [IWL_RATE_6M_INDEX].requested_power) * 2;
+
+	/* store reference index that we use when adjusting *all* scan
+	 *   powers.  So we can accommodate user (all channel) or spectrum
+	 *   management (single channel) power changes "between" temperature
+	 *   feedback compensation procedures.
+	 * don't force fit this reference index into gain table; it may be a
+	 *   negative number.  This will help avoid errors when we're at
+	 *   the lower bounds (highest gains, for warmest temperatures)
+	 *   of the table. */
+
+	/* don't exceed table bounds for "real" setting */
+	power_index = iwl_hw_reg_fix_power_index(power_index);
+
+	scan_power_info->power_table_index = power_index;
+	scan_power_info->tpc.tx_gain =
+	    power_gain_table[band_index][power_index].tx_gain;
+	scan_power_info->tpc.dsp_atten =
+	    power_gain_table[band_index][power_index].dsp_atten;
+}
+
+/**
+ * iwl_hw_reg_send_txpower - fill in Tx Power command with gain settings
+ *
+ * Configures power settings for all rates for the current channel,
+ * using values from channel info struct, and send to NIC
+ */
+int iwl_hw_reg_send_txpower(struct iwl_priv *priv)
+{
+	int rate_idx;
+	const struct iwl_channel_info *ch_info = NULL;
+	struct iwl_txpowertable_cmd txpower = {
+		.channel = priv->active_rxon.channel,
+	};
+
+	txpower.band = (priv->phymode == MODE_IEEE80211A) ? 0 : 1;
+	ch_info = iwl_get_channel_info(priv,
+				       priv->phymode,
+				       le16_to_cpu(priv->active_rxon.channel));
+	if (!ch_info) {
+		IWL_ERROR
+		    ("Failed to get channel info for channel %d [%d]\n",
+		     le16_to_cpu(priv->active_rxon.channel), priv->phymode);
+		return -EINVAL;
+	}
+
+	if (!is_channel_valid(ch_info)) {
+		IWL_DEBUG_POWER("Not calling TX_PWR_TABLE_CMD on "
+				"non-Tx channel.\n");
+		return 0;
+	}
+
+	/* fill cmd with power settings for all rates for current channel */
+	for (rate_idx = 0; rate_idx < IWL_RATE_COUNT; rate_idx++) {
+		txpower.power[rate_idx].tpc = ch_info->power_info[rate_idx].tpc;
+		txpower.power[rate_idx].rate = iwl_rates[rate_idx].plcp;
+
+		IWL_DEBUG_POWER("ch %d:%d rf %d dsp %3d rate code 0x%02x\n",
+				le16_to_cpu(txpower.channel),
+				txpower.band,
+				txpower.power[rate_idx].tpc.tx_gain,
+				txpower.power[rate_idx].tpc.dsp_atten,
+				txpower.power[rate_idx].rate);
+	}
+
+	return iwl_send_cmd_pdu(priv, REPLY_TX_PWR_TABLE_CMD,
+				sizeof(struct iwl_txpowertable_cmd), &txpower);
+
+}
+
+/**
+ * iwl_hw_reg_set_new_power - Configures power tables at new levels
+ * @ch_info: Channel to update.  Uses power_info.requested_power.
+ *
+ * Replace requested_power and base_power_index ch_info fields for
+ * one channel.
+ *
+ * Called if user or spectrum management changes power preferences.
+ * Takes into account h/w and modulation limitations (clip power).
+ *
+ * This does *not* send anything to NIC, just sets up ch_info for one channel.
+ *
+ * NOTE: reg_compensate_for_temperature_dif() *must* be run after this to
+ *	 properly fill out the scan powers, and actual h/w gain settings,
+ *	 and send changes to NIC
+ */
+static int iwl_hw_reg_set_new_power(struct iwl_priv *priv,
+			     struct iwl_channel_info *ch_info)
+{
+	struct iwl_channel_power_info *power_info;
+	int power_changed = 0;
+	int i;
+	const s8 *clip_pwrs;
+	int power;
+
+	/* Get this chnlgrp's rate-to-max/clip-powers table */
+	clip_pwrs = priv->clip_groups[ch_info->group_index].clip_powers;
+
+	/* Get this channel's rate-to-current-power settings table */
+	power_info = ch_info->power_info;
+
+	/* update OFDM Txpower settings */
+	for (i = IWL_FIRST_OFDM_RATE; i <= IWL_LAST_OFDM_RATE;
+	     i++, ++power_info) {
+		int delta_idx;
+
+		/* limit new power to be no more than h/w capability */
+		power = min(ch_info->curr_txpow, clip_pwrs[i]);
+		if (power == power_info->requested_power)
+			continue;
+
+		/* find difference between old and new requested powers,
+		 *    update base (non-temp-compensated) power index */
+		delta_idx = (power - power_info->requested_power) * 2;
+		power_info->base_power_index -= delta_idx;
+
+		/* save new requested power value */
+		power_info->requested_power = power;
+
+		power_changed = 1;
+	}
+
+	/* update CCK Txpower settings, based on OFDM 12M setting ...
+	 *    ... all CCK power settings for a given channel are the *same*. */
+	if (power_changed) {
+		power =
+		    ch_info->power_info[IWL_RATE_12M_INDEX].
+		    requested_power + IWL_CCK_FROM_OFDM_POWER_DIFF;
+
+		/* do all CCK rates' iwl_channel_power_info structures */
+		for (i = IWL_FIRST_CCK_RATE; i <= IWL_LAST_CCK_RATE; i++) {
+			power_info->requested_power = power;
+			power_info->base_power_index =
+			    ch_info->power_info[IWL_RATE_12M_INDEX].
+			    base_power_index + IWL_CCK_FROM_OFDM_INDEX_DIFF;
+			++power_info;
+		}
+	}
+
+	return 0;
+}
+
+/**
+ * iwl_hw_reg_get_ch_txpower_limit - returns new power limit for channel
+ *
+ * NOTE: Returned power limit may be less (but not more) than requested,
+ *	 based strictly on regulatory (eeprom and spectrum mgt) limitations
+ *	 (no consideration for h/w clipping limitations).
+ */
+static int iwl_hw_reg_get_ch_txpower_limit(struct iwl_channel_info *ch_info)
+{
+	s8 max_power;
+
+#if 0
+	/* if we're using TGd limits, use lower of TGd or EEPROM */
+	if (ch_info->tgd_data.max_power != 0)
+		max_power = min(ch_info->tgd_data.max_power,
+				ch_info->eeprom.max_power_avg);
+
+	/* else just use EEPROM limits */
+	else
+#endif
+		max_power = ch_info->eeprom.max_power_avg;
+
+	return min(max_power, ch_info->max_power_avg);
+}
+
+/**
+ * iwl_hw_reg_comp_txpower_temp - Compensate for temperature
+ *
+ * Compensate txpower settings of *all* channels for temperature.
+ * This only accounts for the difference between current temperature
+ *   and the factory calibration temperatures, and bases the new settings
+ *   on the channel's base_power_index.
+ *
+ * If RxOn is "associated", this sends the new Txpower to NIC!
+ */
+static int iwl_hw_reg_comp_txpower_temp(struct iwl_priv *priv)
+{
+	struct iwl_channel_info *ch_info = NULL;
+	int delta_index;
+	const s8 *clip_pwrs; /* array of h/w max power levels for each rate */
+	u8 a_band;
+	u8 rate_index;
+	u8 scan_tbl_index;
+	u8 i;
+	int ref_temp;
+	int temperature = priv->temperature;
+
+	/* set up new Tx power info for each and every channel, 2.4 and 5.x */
+	for (i = 0; i < priv->channel_count; i++) {
+		ch_info = &priv->channel_info[i];
+		a_band = is_channel_a_band(ch_info);
+
+		/* Get this chnlgrp's factory calibration temperature */
+		ref_temp = (s16)priv->eeprom.groups[ch_info->group_index].
+		    temperature;
+
+		/* get power index adjustment based on curr and factory
+		 * temps */
+		delta_index = iwl_hw_reg_adjust_power_by_temp(temperature,
+							      ref_temp);
+
+		/* set tx power value for all rates, OFDM and CCK */
+		for (rate_index = 0; rate_index < IWL_RATE_COUNT;
+		     rate_index++) {
+			int power_idx =
+			    ch_info->power_info[rate_index].base_power_index;
+
+			/* temperature compensate */
+			power_idx += delta_index;
+
+			/* stay within table range */
+			power_idx = iwl_hw_reg_fix_power_index(power_idx);
+			ch_info->power_info[rate_index].
+			    power_table_index = (u8) power_idx;
+			ch_info->power_info[rate_index].tpc =
+			    power_gain_table[a_band][power_idx];
+		}
+
+		/* Get this chnlgrp's rate-to-max/clip-powers table */
+		clip_pwrs = priv->clip_groups[ch_info->group_index].clip_powers;
+
+		/* set scan tx power, 1Mbit for CCK, 6Mbit for OFDM */
+		for (scan_tbl_index = 0;
+		     scan_tbl_index < IWL_NUM_SCAN_RATES; scan_tbl_index++) {
+			s32 actual_index = (scan_tbl_index == 0) ?
+			    IWL_RATE_1M_INDEX : IWL_RATE_6M_INDEX;
+			iwl_hw_reg_set_scan_power(priv, scan_tbl_index,
+					   actual_index, clip_pwrs,
+					   ch_info, a_band);
+		}
+	}
+
+	/* send Txpower command for current channel to ucode */
+	return iwl_hw_reg_send_txpower(priv);
+}
+
+int iwl_hw_reg_set_txpower(struct iwl_priv *priv, s8 power)
+{
+	struct iwl_channel_info *ch_info;
+	s8 max_power;
+	u8 a_band;
+	u8 i;
+
+	if (priv->user_txpower_limit == power) {
+		IWL_DEBUG_POWER("Requested Tx power same as current "
+				"limit: %ddBm.\n", power);
+		return 0;
+	}
+
+	IWL_DEBUG_POWER("Setting upper limit clamp to %ddBm.\n", power);
+	priv->user_txpower_limit = power;
+
+	/* set up new Tx powers for each and every channel, 2.4 and 5.x */
+
+	for (i = 0; i < priv->channel_count; i++) {
+		ch_info = &priv->channel_info[i];
+		a_band = is_channel_a_band(ch_info);
+
+		/* find minimum power of all user and regulatory constraints
+		 *    (does not consider h/w clipping limitations) */
+		max_power = iwl_hw_reg_get_ch_txpower_limit(ch_info);
+		max_power = min(power, max_power);
+		if (max_power != ch_info->curr_txpow) {
+			ch_info->curr_txpow = max_power;
+
+			/* this considers the h/w clipping limitations */
+			iwl_hw_reg_set_new_power(priv, ch_info);
+		}
+	}
+
+	/* update txpower settings for all channels,
+	 *   send to NIC if associated. */
+	is_temp_calib_needed(priv);
+	iwl_hw_reg_comp_txpower_temp(priv);
+
+	return 0;
+}
+
+/* will add 3945 channel switch cmd handling later */
+int iwl_hw_channel_switch(struct iwl_priv *priv, u16 channel)
+{
+	return 0;
+}
+
+/**
+ * iwl3945_reg_txpower_periodic -  called when time to check our temperature.
+ *
+ * -- reset periodic timer
+ * -- see if temp has changed enough to warrant re-calibration ... if so:
+ *     -- correct coeffs for temp (can reset temp timer)
+ *     -- save this temp as "last",
+ *     -- send new set of gain settings to NIC
+ * NOTE:  This should continue working, even when we're not associated,
+ *   so we can keep our internal table of scan powers current. */
+void iwl3945_reg_txpower_periodic(struct iwl_priv *priv)
+{
+	/* This will kick in the "brute force"
+	 * iwl_hw_reg_comp_txpower_temp() below */
+	if (!is_temp_calib_needed(priv))
+		goto reschedule;
+
+	/* Set up a new set of temp-adjusted TxPowers, send to NIC.
+	 * This is based *only* on current temperature,
+	 * ignoring any previous power measurements */
+	iwl_hw_reg_comp_txpower_temp(priv);
+
+ reschedule:
+	queue_delayed_work(priv->workqueue,
+			   &priv->thermal_periodic, REG_RECALIB_PERIOD * HZ);
+}
+
+void iwl3945_bg_reg_txpower_periodic(struct work_struct *work)
+{
+	struct iwl_priv *priv = container_of(work, struct iwl_priv,
+					     thermal_periodic.work);
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+		return;
+
+	mutex_lock(&priv->mutex);
+	iwl3945_reg_txpower_periodic(priv);
+	mutex_unlock(&priv->mutex);
+}
+
+/**
+ * iwl_hw_reg_get_ch_grp_index - find the channel-group index (0-4)
+ * 				   for the channel.
+ *
+ * This function is used when initializing channel-info structs.
+ *
+ * NOTE: These channel groups do *NOT* match the bands above!
+ *	 These channel groups are based on factory-tested channels;
+ *	 on A-band, EEPROM's "group frequency" entries represent the top
+ *	 channel in each group 1-4.  Group 5 All B/G channels are in group 0.
+ */
+static u16 iwl_hw_reg_get_ch_grp_index(struct iwl_priv *priv,
+				       const struct iwl_channel_info *ch_info)
+{
+	struct iwl_eeprom_txpower_group *ch_grp = &priv->eeprom.groups[0];
+	u8 group;
+	u16 group_index = 0;	/* based on factory calib frequencies */
+	u8 grp_channel;
+
+	/* Find the group index for the channel ... don't use index 1(?) */
+	if (is_channel_a_band(ch_info)) {
+		for (group = 1; group < 5; group++) {
+			grp_channel = ch_grp[group].group_channel;
+			if (ch_info->channel <= grp_channel) {
+				group_index = group;
+				break;
+			}
+		}
+		/* group 4 has a few channels *above* its factory cal freq */
+		if (group == 5)
+			group_index = 4;
+	} else
+		group_index = 0;	/* 2.4 GHz, group 0 */
+
+	IWL_DEBUG_POWER("Chnl %d mapped to grp %d\n", ch_info->channel,
+			group_index);
+	return group_index;
+}
+
+/**
+ * iwl_hw_reg_get_matched_power_index - Interpolate to get nominal index
+ *
+ * Interpolate to get nominal (i.e. at factory calibration temperature) index
+ *   into radio/DSP gain settings table for requested power.
+ */
+static int iwl_hw_reg_get_matched_power_index(struct iwl_priv *priv,
+				       s8 requested_power,
+				       s32 setting_index, s32 *new_index)
+{
+	const struct iwl_eeprom_txpower_group *chnl_grp = NULL;
+	s32 index0, index1;
+	s32 power = 2 * requested_power;
+	s32 i;
+	const struct iwl_eeprom_txpower_sample *samples;
+	s32 gains0, gains1;
+	s32 res;
+	s32 denominator;
+
+	chnl_grp = &priv->eeprom.groups[setting_index];
+	samples = chnl_grp->samples;
+	for (i = 0; i < 5; i++) {
+		if (power == samples[i].power) {
+			*new_index = samples[i].gain_index;
+			return 0;
+		}
+	}
+
+	if (power > samples[1].power) {
+		index0 = 0;
+		index1 = 1;
+	} else if (power > samples[2].power) {
+		index0 = 1;
+		index1 = 2;
+	} else if (power > samples[3].power) {
+		index0 = 2;
+		index1 = 3;
+	} else {
+		index0 = 3;
+		index1 = 4;
+	}
+
+	denominator = (s32) samples[index1].power - (s32) samples[index0].power;
+	if (denominator == 0)
+		return -EINVAL;
+	gains0 = (s32) samples[index0].gain_index * (1 << 19);
+	gains1 = (s32) samples[index1].gain_index * (1 << 19);
+	res = gains0 + (gains1 - gains0) *
+	    ((s32) power - (s32) samples[index0].power) / denominator +
+	    (1 << 18);
+	*new_index = res >> 19;
+	return 0;
+}
+
+static void iwl_hw_reg_init_channel_groups(struct iwl_priv *priv)
+{
+	u32 i;
+	s32 rate_index;
+	const struct iwl_eeprom_txpower_group *group;
+
+	IWL_DEBUG_POWER("Initializing factory calib info from EEPROM\n");
+
+	for (i = 0; i < IWL_NUM_TX_CALIB_GROUPS; i++) {
+		s8 *clip_pwrs;	/* table of power levels for each rate */
+		s8 satur_pwr;	/* saturation power for each chnl group */
+		group = &priv->eeprom.groups[i];
+
+		/* sanity check on factory saturation power value */
+		if (group->saturation_power < 40) {
+			IWL_WARNING("Error: saturation power is %d, "
+				    "less than minimum expected 40\n",
+				    group->saturation_power);
+			return;
+		}
+
+		/*
+		 * Derive requested power levels for each rate, based on
+		 *   hardware capabilities (saturation power for band).
+		 * Basic value is 3dB down from saturation, with further
+		 *   power reductions for highest 3 data rates.  These
+		 *   backoffs provide headroom for high rate modulation
+		 *   power peaks, without too much distortion (clipping).
+		 */
+		/* we'll fill in this array with h/w max power levels */
+		clip_pwrs = (s8 *) priv->clip_groups[i].clip_powers;
+
+		/* divide factory saturation power by 2 to find -3dB level */
+		satur_pwr = (s8) (group->saturation_power >> 1);
+
+		/* fill in channel group's nominal powers for each rate */
+		for (rate_index = 0;
+		     rate_index < IWL_RATE_COUNT; rate_index++, clip_pwrs++) {
+			switch (rate_index) {
+			case IWL_RATE_36M_INDEX:
+				if (i == 0)	/* B/G */
+					*clip_pwrs = satur_pwr;
+				else	/* A */
+					*clip_pwrs = satur_pwr - 5;
+				break;
+			case IWL_RATE_48M_INDEX:
+				if (i == 0)
+					*clip_pwrs = satur_pwr - 7;
+				else
+					*clip_pwrs = satur_pwr - 10;
+				break;
+			case IWL_RATE_54M_INDEX:
+				if (i == 0)
+					*clip_pwrs = satur_pwr - 9;
+				else
+					*clip_pwrs = satur_pwr - 12;
+				break;
+			default:
+				*clip_pwrs = satur_pwr;
+				break;
+			}
+		}
+	}
+}
+
+/**
+ * iwl3945_txpower_set_from_eeprom - Set channel power info based on EEPROM
+ *
+ * Second pass (during init) to set up priv->channel_info
+ *
+ * Set up Tx-power settings in our channel info database for each VALID
+ * (for this geo/SKU) channel, at all Tx data rates, based on eeprom values
+ * and current temperature.
+ *
+ * Since this is based on current temperature (at init time), these values may
+ * not be valid for very long, but it gives us a starting/default point,
+ * and allows us to active (i.e. using Tx) scan.
+ *
+ * This does *not* write values to NIC, just sets up our internal table.
+ */
+int iwl3945_txpower_set_from_eeprom(struct iwl_priv *priv)
+{
+	struct iwl_channel_info *ch_info = NULL;
+	struct iwl_channel_power_info *pwr_info;
+	int delta_index;
+	u8 rate_index;
+	u8 scan_tbl_index;
+	const s8 *clip_pwrs;	/* array of power levels for each rate */
+	u8 gain, dsp_atten;
+	s8 power;
+	u8 pwr_index, base_pwr_index, a_band;
+	u8 i;
+	int temperature;
+
+	/* save temperature reference,
+	 *   so we can determine next time to calibrate */
+	temperature = iwl_hw_reg_txpower_get_temperature(priv);
+	priv->last_temperature = temperature;
+
+	iwl_hw_reg_init_channel_groups(priv);
+
+	/* initialize Tx power info for each and every channel, 2.4 and 5.x */
+	for (i = 0, ch_info = priv->channel_info; i < priv->channel_count;
+	     i++, ch_info++) {
+		a_band = is_channel_a_band(ch_info);
+		if (!is_channel_valid(ch_info))
+			continue;
+
+		/* find this channel's channel group (*not* "band") index */
+		ch_info->group_index =
+			iwl_hw_reg_get_ch_grp_index(priv, ch_info);
+
+		/* Get this chnlgrp's rate->max/clip-powers table */
+		clip_pwrs = priv->clip_groups[ch_info->group_index].clip_powers;
+
+		/* calculate power index *adjustment* value according to
+		 *  diff between current temperature and factory temperature */
+		delta_index = iwl_hw_reg_adjust_power_by_temp(temperature,
+				priv->eeprom.groups[ch_info->group_index].
+				temperature);
+
+		IWL_DEBUG_POWER("Delta index for channel %d: %d [%d]\n",
+				ch_info->channel, delta_index, temperature +
+				IWL_TEMP_CONVERT);
+
+		/* set tx power value for all OFDM rates */
+		for (rate_index = 0; rate_index < IWL_OFDM_RATES;
+		     rate_index++) {
+			s32 power_idx;
+			int rc;
+
+			/* use channel group's clip-power table,
+			 *   but don't exceed channel's max power */
+			s8 pwr = min(ch_info->max_power_avg,
+				     clip_pwrs[rate_index]);
+
+			pwr_info = &ch_info->power_info[rate_index];
+
+			/* get base (i.e. at factory-measured temperature)
+			 *    power table index for this rate's power */
+			rc = iwl_hw_reg_get_matched_power_index(priv, pwr,
+							 ch_info->group_index,
+							 &power_idx);
+			if (rc) {
+				IWL_ERROR("Invalid power index\n");
+				return rc;
+			}
+			pwr_info->base_power_index = (u8) power_idx;
+
+			/* temperature compensate */
+			power_idx += delta_index;
+
+			/* stay within range of gain table */
+			power_idx = iwl_hw_reg_fix_power_index(power_idx);
+
+			/* fill 1 OFDM rate's iwl_channel_power_info struct */
+			pwr_info->requested_power = pwr;
+			pwr_info->power_table_index = (u8) power_idx;
+			pwr_info->tpc.tx_gain =
+			    power_gain_table[a_band][power_idx].tx_gain;
+			pwr_info->tpc.dsp_atten =
+			    power_gain_table[a_band][power_idx].dsp_atten;
+		}
+
+		/* set tx power for CCK rates, based on OFDM 12 Mbit settings*/
+		pwr_info = &ch_info->power_info[IWL_RATE_12M_INDEX];
+		power = pwr_info->requested_power +
+			IWL_CCK_FROM_OFDM_POWER_DIFF;
+		pwr_index = pwr_info->power_table_index +
+			IWL_CCK_FROM_OFDM_INDEX_DIFF;
+		base_pwr_index = pwr_info->base_power_index +
+			IWL_CCK_FROM_OFDM_INDEX_DIFF;
+
+		/* stay within table range */
+		pwr_index = iwl_hw_reg_fix_power_index(pwr_index);
+		gain = power_gain_table[a_band][pwr_index].tx_gain;
+		dsp_atten = power_gain_table[a_band][pwr_index].dsp_atten;
+
+		/* fill each CCK rate's iwl_channel_power_info structure
+		 * NOTE:  All CCK-rate Txpwrs are the same for a given chnl!
+		 * NOTE:  CCK rates start at end of OFDM rates! */
+		for (rate_index = IWL_OFDM_RATES;
+		     rate_index < IWL_RATE_COUNT; rate_index++) {
+			pwr_info = &ch_info->power_info[rate_index];
+			pwr_info->requested_power = power;
+			pwr_info->power_table_index = pwr_index;
+			pwr_info->base_power_index = base_pwr_index;
+			pwr_info->tpc.tx_gain = gain;
+			pwr_info->tpc.dsp_atten = dsp_atten;
+		}
+
+		/* set scan tx power, 1Mbit for CCK, 6Mbit for OFDM */
+		for (scan_tbl_index = 0;
+		     scan_tbl_index < IWL_NUM_SCAN_RATES; scan_tbl_index++) {
+			s32 actual_index = (scan_tbl_index == 0) ?
+				IWL_RATE_1M_INDEX : IWL_RATE_6M_INDEX;
+			iwl_hw_reg_set_scan_power(priv, scan_tbl_index,
+				actual_index, clip_pwrs, ch_info, a_band);
+		}
+	}
+
+	return 0;
+}
+
+int iwl_hw_rxq_stop(struct iwl_priv *priv)
+{
+	int rc;
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	rc = iwl_grab_restricted_access(priv);
+	if (rc) {
+		spin_unlock_irqrestore(&priv->lock, flags);
+		return rc;
+	}
+
+	iwl_write_restricted(priv, FH_RCSR_CONFIG(0), 0);
+	rc = iwl_poll_restricted_bit(priv, FH_RSSR_STATUS, (1 << 24), 1000);
+	if (rc < 0)
+		IWL_ERROR("Can't stop Rx DMA.\n");
+
+	iwl_release_restricted_access(priv);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return 0;
+}
+
+int iwl_hw_tx_queue_init(struct iwl_priv *priv, struct iwl_tx_queue *txq)
+{
+	int rc;
+	unsigned long flags;
+	int txq_id = txq->q.id;
+
+	struct iwl_shared *shared_data = priv->hw_setting.shared_virt;
+
+	shared_data->tx_base_ptr[txq_id] = cpu_to_le32((u32)txq->q.dma_addr);
+
+	spin_lock_irqsave(&priv->lock, flags);
+	rc = iwl_grab_restricted_access(priv);
+	if (rc) {
+		spin_unlock_irqrestore(&priv->lock, flags);
+		return rc;
+	}
+	iwl_write_restricted(priv, FH_CBCC_CTRL(txq_id), 0);
+	iwl_write_restricted(priv, FH_CBCC_BASE(txq_id), 0);
+
+	iwl_write_restricted(priv, FH_TCSR_CONFIG(txq_id),
+		ALM_FH_TCSR_TX_CONFIG_REG_VAL_CIRQ_RTC_NOINT |
+		ALM_FH_TCSR_TX_CONFIG_REG_VAL_MSG_MODE_TXF |
+		ALM_FH_TCSR_TX_CONFIG_REG_VAL_CIRQ_HOST_IFTFD |
+		ALM_FH_TCSR_TX_CONFIG_REG_VAL_DMA_CREDIT_ENABLE_VAL |
+		ALM_FH_TCSR_TX_CONFIG_REG_VAL_DMA_CHNL_ENABLE);
+	iwl_release_restricted_access(priv);
+
+	/* fake read to flush all prev. writes */
+	iwl_read32(priv, FH_TSSR_CBB_BASE);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return 0;
+}
+
+int iwl_hw_get_rx_read(struct iwl_priv *priv)
+{
+	struct iwl_shared *shared_data = priv->hw_setting.shared_virt;
+
+	return le32_to_cpu(shared_data->rx_read_ptr[0]);
+}
+
+/**
+ * iwl3945_init_hw_rate_table - Initialize the hardware rate fallback table
+ */
+int iwl3945_init_hw_rate_table(struct iwl_priv *priv)
+{
+	int rc, i;
+	struct iwl_rate_scaling_cmd rate_cmd = {
+		.reserved = {0, 0, 0},
+	};
+	struct iwl_rate_scaling_info *table = rate_cmd.table;
+
+	for (i = 0; i < ARRAY_SIZE(iwl_rates); i++) {
+		table[i].rate_n_flags =
+			iwl_hw_set_rate_n_flags(iwl_rates[i].plcp, 0);
+		table[i].try_cnt = priv->retry_rate;
+		table[i].next_rate_index = iwl_get_prev_ieee_rate(i);
+	}
+
+	switch (priv->phymode) {
+	case MODE_IEEE80211A:
+		IWL_DEBUG_RATE("Select A mode rate scale\n");
+		/* If one of the following CCK rates is used,
+		 * have it fall back to the 6M OFDM rate */
+		for (i = IWL_FIRST_CCK_RATE; i <= IWL_LAST_CCK_RATE; i++)
+			table[i].next_rate_index = IWL_FIRST_OFDM_RATE;
+
+		/* Don't fall back to CCK rates */
+		table[IWL_RATE_12M_INDEX].next_rate_index = IWL_RATE_9M_INDEX;
+
+		/* Don't drop out of OFDM rates */
+		table[IWL_FIRST_OFDM_RATE].next_rate_index =
+		    IWL_FIRST_OFDM_RATE;
+		break;
+
+	case MODE_IEEE80211B:
+		IWL_DEBUG_RATE("Select B mode rate scale\n");
+		/* If an OFDM rate is used, have it fall back to the
+		 * 1M CCK rates */
+		for (i = IWL_FIRST_OFDM_RATE; i <= IWL_LAST_OFDM_RATE; i++)
+			table[i].next_rate_index = IWL_FIRST_CCK_RATE;
+
+		/* CCK shouldn't fall back to OFDM... */
+		table[IWL_RATE_11M_INDEX].next_rate_index = IWL_RATE_5M_INDEX;
+		break;
+
+	default:
+		IWL_DEBUG_RATE("Select G mode rate scale\n");
+		break;
+	}
+
+	/* Update the rate scaling for control frame Tx */
+	rate_cmd.table_id = 0;
+	rc = iwl_send_cmd_pdu(priv, REPLY_RATE_SCALE, sizeof(rate_cmd),
+			      &rate_cmd);
+	if (rc)
+		return rc;
+
+	/* Update the rate scaling for data frame Tx */
+	rate_cmd.table_id = 1;
+	return iwl_send_cmd_pdu(priv, REPLY_RATE_SCALE, sizeof(rate_cmd),
+				&rate_cmd);
+}
+
+int iwl_hw_set_hw_setting(struct iwl_priv *priv)
+{
+	memset((void *)&priv->hw_setting, 0,
+	       sizeof(struct iwl_driver_hw_info));
+
+	priv->hw_setting.shared_virt =
+	    pci_alloc_consistent(priv->pci_dev,
+				 sizeof(struct iwl_shared),
+				 &priv->hw_setting.shared_phys);
+
+	if (!priv->hw_setting.shared_virt) {
+		IWL_ERROR("failed to allocate pci memory\n");
+		mutex_unlock(&priv->mutex);
+		return -ENOMEM;
+	}
+
+	priv->hw_setting.ac_queue_count = AC_NUM;
+	priv->hw_setting.rx_buffer_size = IWL_RX_BUF_SIZE;
+	priv->hw_setting.tx_cmd_len = sizeof(struct iwl_tx_cmd);
+	priv->hw_setting.max_rxq_size = RX_QUEUE_SIZE;
+	priv->hw_setting.max_rxq_log = RX_QUEUE_SIZE_LOG;
+	priv->hw_setting.cck_flag = 0;
+	priv->hw_setting.max_stations = IWL3945_STATION_COUNT;
+	priv->hw_setting.bcast_sta_id = IWL3945_BROADCAST_ID;
+	return 0;
+}
+
+unsigned int iwl_hw_get_beacon_cmd(struct iwl_priv *priv,
+			  struct iwl_frame *frame, u8 rate)
+{
+	struct iwl_tx_beacon_cmd *tx_beacon_cmd;
+	unsigned int frame_size;
+
+	tx_beacon_cmd = (struct iwl_tx_beacon_cmd *)&frame->u;
+	memset(tx_beacon_cmd, 0, sizeof(*tx_beacon_cmd));
+
+	tx_beacon_cmd->tx.sta_id = IWL3945_BROADCAST_ID;
+	tx_beacon_cmd->tx.stop_time.life_time = TX_CMD_LIFE_TIME_INFINITE;
+
+	frame_size = iwl_fill_beacon_frame(priv,
+				tx_beacon_cmd->frame,
+				BROADCAST_ADDR,
+				sizeof(frame->u) - sizeof(*tx_beacon_cmd));
+
+	BUG_ON(frame_size > MAX_MPDU_SIZE);
+	tx_beacon_cmd->tx.len = cpu_to_le16((u16)frame_size);
+
+	tx_beacon_cmd->tx.rate = rate;
+	tx_beacon_cmd->tx.tx_flags = (TX_CMD_FLG_SEQ_CTL_MSK |
+				      TX_CMD_FLG_TSF_MSK);
+
+	/* supp_rates[0] == OFDM  */
+	tx_beacon_cmd->tx.supp_rates[0] = IWL_OFDM_BASIC_RATES_MASK;
+
+	/* supp_rates[1] == CCK
+	 *
+	 * NOTE:  IWL_*_RATES_MASK are not in the order that supp_rates
+	 * expects so we have to shift them around.
+	 *
+	 * supp_rates expects:
+	 * CCK rates are bit0..3
+	 *
+	 * However IWL_*_RATES_MASK has:
+	 * CCK rates are bit8..11
+	 */
+	tx_beacon_cmd->tx.supp_rates[1] =
+		(IWL_CCK_BASIC_RATES_MASK >> 8) & 0xF;
+
+	return (sizeof(struct iwl_tx_beacon_cmd) + frame_size);
+}
+
+void iwl_hw_rx_handler_setup(struct iwl_priv *priv)
+{
+	priv->rx_handlers[REPLY_3945_RX] = iwl3945_rx_reply_rx;
+}
+
+void iwl_hw_setup_deferred_work(struct iwl_priv *priv)
+{
+	INIT_DELAYED_WORK(&priv->thermal_periodic,
+			  iwl3945_bg_reg_txpower_periodic);
+}
+
+void iwl_hw_cancel_deferred_work(struct iwl_priv *priv)
+{
+	cancel_delayed_work(&priv->thermal_periodic);
+}
+
+struct pci_device_id iwl_hw_card_ids[] = {
+	{0x8086, 0x4222, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{0x8086, 0x4227, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{0}
+};
+
+inline int iwl_eeprom_aqcuire_semaphore(struct iwl_priv *priv)
+{
+	_iwl_clear_bit(priv, CSR_EEPROM_GP, CSR_EEPROM_GP_IF_OWNER_MSK);
+	return 0;
+}
+
+MODULE_DEVICE_TABLE(pci, iwl_hw_card_ids);
diff --git a/drivers/net/wireless/iwlwifi/iwl-3945.h b/drivers/net/wireless/iwlwifi/iwl-3945.h
new file mode 100644
index 000000000000..813902e9f8c2
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-3945.h
@@ -0,0 +1,41 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2003 - 2007 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * James P. Ketrenos <ipw2100-admin@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+
+#ifndef __iwl_3945_h__
+#define __iwl_3945_h__
+
+/*
+ * Forward declare iwl-3945.c functions for iwl-base.c
+ */
+extern int iwl_eeprom_aqcuire_semaphore(struct iwl_priv *priv);
+extern __le32 iwl3945_get_antenna_flags(const struct iwl_priv *priv);
+extern int iwl3945_init_hw_rate_table(struct iwl_priv *priv);
+extern void iwl3945_reg_txpower_periodic(struct iwl_priv *priv);
+extern void iwl3945_bg_reg_txpower_periodic(struct work_struct *work);
+extern int iwl3945_txpower_set_from_eeprom(struct iwl_priv *priv);
+extern u8 iwl3945_sync_sta(struct iwl_priv *priv, int sta_id,
+		 u16 tx_rate, u8 flags);
+#endif
diff --git a/drivers/net/wireless/iwlwifi/iwl-4965-hw.h b/drivers/net/wireless/iwlwifi/iwl-4965-hw.h
new file mode 100644
index 000000000000..99a19ef4c743
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-4965-hw.h
@@ -0,0 +1,581 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual BSD/GPLv2 license.  When using or
+ * redistributing this file, you may do so under either license.
+ *
+ * GPL LICENSE SUMMARY
+ *
+ * Copyright(c) 2005 - 2007 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU Geeral Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110,
+ * USA
+ *
+ * The full GNU General Public License is included in this distribution
+ * in the file called LICENSE.GPL.
+ *
+ * Contact Information:
+ * James P. Ketrenos <ipw2100-admin@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ * BSD LICENSE
+ *
+ * Copyright(c) 2005 - 2007 Intel Corporation. All rights reserved.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *  * Neither the name Intel Corporation nor the names of its
+ *    contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+
+#ifndef __iwl_4965_hw_h__
+#define __iwl_4965_hw_h__
+
+#define IWL_RX_BUF_SIZE (4 * 1024)
+#define IWL_MAX_BSM_SIZE BSM_SRAM_SIZE
+#define KDR_RTC_INST_UPPER_BOUND		(0x018000)
+#define KDR_RTC_DATA_UPPER_BOUND		(0x80A000)
+#define KDR_RTC_INST_SIZE    (KDR_RTC_INST_UPPER_BOUND - RTC_INST_LOWER_BOUND)
+#define KDR_RTC_DATA_SIZE    (KDR_RTC_DATA_UPPER_BOUND - RTC_DATA_LOWER_BOUND)
+
+#define IWL_MAX_INST_SIZE KDR_RTC_INST_SIZE
+#define IWL_MAX_DATA_SIZE KDR_RTC_DATA_SIZE
+
+static inline int iwl_hw_valid_rtc_data_addr(u32 addr)
+{
+	return (addr >= RTC_DATA_LOWER_BOUND) &&
+	       (addr < KDR_RTC_DATA_UPPER_BOUND);
+}
+
+/********************* START TXPOWER *****************************************/
+enum {
+	HT_IE_EXT_CHANNEL_NONE = 0,
+	HT_IE_EXT_CHANNEL_ABOVE,
+	HT_IE_EXT_CHANNEL_INVALID,
+	HT_IE_EXT_CHANNEL_BELOW,
+	HT_IE_EXT_CHANNEL_MAX
+};
+
+enum {
+	CALIB_CH_GROUP_1 = 0,
+	CALIB_CH_GROUP_2 = 1,
+	CALIB_CH_GROUP_3 = 2,
+	CALIB_CH_GROUP_4 = 3,
+	CALIB_CH_GROUP_5 = 4,
+	CALIB_CH_GROUP_MAX
+};
+
+/* Temperature calibration offset is 3% 0C in Kelvin */
+#define TEMPERATURE_CALIB_KELVIN_OFFSET 8
+#define TEMPERATURE_CALIB_A_VAL 259
+
+#define IWL_TX_POWER_TEMPERATURE_MIN  (263)
+#define IWL_TX_POWER_TEMPERATURE_MAX  (410)
+
+#define IWL_TX_POWER_TEMPERATURE_OUT_OF_RANGE(t) \
+	(((t) < IWL_TX_POWER_TEMPERATURE_MIN) || \
+	 ((t) > IWL_TX_POWER_TEMPERATURE_MAX))
+
+#define IWL_TX_POWER_ILLEGAL_TEMPERATURE (300)
+
+#define IWL_TX_POWER_TEMPERATURE_DIFFERENCE (2)
+
+#define IWL_TX_POWER_MIMO_REGULATORY_COMPENSATION (6)
+
+#define IWL_TX_POWER_TARGET_POWER_MIN       (0)	/* 0 dBm = 1 milliwatt */
+#define IWL_TX_POWER_TARGET_POWER_MAX      (16)	/* 16 dBm */
+
+/* timeout equivalent to 3 minutes */
+#define IWL_TX_POWER_TIMELIMIT_NOCALIB 1800000000
+
+#define IWL_TX_POWER_CCK_COMPENSATION (9)
+
+#define MIN_TX_GAIN_INDEX		(0)
+#define MIN_TX_GAIN_INDEX_52GHZ_EXT	(-9)
+#define MAX_TX_GAIN_INDEX_52GHZ		(98)
+#define MIN_TX_GAIN_52GHZ		(98)
+#define MAX_TX_GAIN_INDEX_24GHZ		(98)
+#define MIN_TX_GAIN_24GHZ		(98)
+#define MAX_TX_GAIN			(0)
+#define MAX_TX_GAIN_52GHZ_EXT		(-9)
+
+#define IWL_TX_POWER_DEFAULT_REGULATORY_24   (34)
+#define IWL_TX_POWER_DEFAULT_REGULATORY_52   (34)
+#define IWL_TX_POWER_REGULATORY_MIN          (0)
+#define IWL_TX_POWER_REGULATORY_MAX          (34)
+#define IWL_TX_POWER_DEFAULT_SATURATION_24   (38)
+#define IWL_TX_POWER_DEFAULT_SATURATION_52   (38)
+#define IWL_TX_POWER_SATURATION_MIN          (20)
+#define IWL_TX_POWER_SATURATION_MAX          (50)
+
+/* dv *0.4 = dt; so that 5 degrees temperature diff equals
+ * 12.5 in voltage diff */
+#define IWL_TX_TEMPERATURE_UPDATE_LIMIT 9
+
+#define IWL_INVALID_CHANNEL                 (0xffffffff)
+#define IWL_TX_POWER_REGITRY_BIT            (2)
+
+#define MIN_IWL_TX_POWER_CALIB_DUR          (100)
+#define IWL_CCK_FROM_OFDM_POWER_DIFF        (-5)
+#define IWL_CCK_FROM_OFDM_INDEX_DIFF (9)
+
+/* Number of entries in the gain table */
+#define POWER_GAIN_NUM_ENTRIES 78
+#define TX_POW_MAX_SESSION_NUM 5
+/*  timeout equivalent to 3 minutes */
+#define TX_IWL_TIMELIMIT_NOCALIB 1800000000
+
+/* Kedron TX_CALIB_STATES */
+#define IWL_TX_CALIB_STATE_SEND_TX        0x00000001
+#define IWL_TX_CALIB_WAIT_TX_RESPONSE     0x00000002
+#define IWL_TX_CALIB_ENABLED              0x00000004
+#define IWL_TX_CALIB_XVT_ON               0x00000008
+#define IWL_TX_CALIB_TEMPERATURE_CORRECT  0x00000010
+#define IWL_TX_CALIB_WORKING_WITH_XVT     0x00000020
+#define IWL_TX_CALIB_XVT_PERIODICAL       0x00000040
+
+#define NUM_IWL_TX_CALIB_SETTINS 5	/* Number of tx correction groups */
+
+#define IWL_MIN_POWER_IN_VP_TABLE 1	/* 0.5dBm multiplied by 2 */
+#define IWL_MAX_POWER_IN_VP_TABLE 40	/* 20dBm - multiplied by 2 (because
+					 * entries are for each 0.5dBm) */
+#define IWL_STEP_IN_VP_TABLE 1	/* 0.5dB - multiplied by 2 */
+#define IWL_NUM_POINTS_IN_VPTABLE \
+	(1 + IWL_MAX_POWER_IN_VP_TABLE - IWL_MIN_POWER_IN_VP_TABLE)
+
+#define MIN_TX_GAIN_INDEX         (0)
+#define MAX_TX_GAIN_INDEX_52GHZ   (98)
+#define MIN_TX_GAIN_52GHZ         (98)
+#define MAX_TX_GAIN_INDEX_24GHZ   (98)
+#define MIN_TX_GAIN_24GHZ         (98)
+#define MAX_TX_GAIN               (0)
+
+/* First and last channels of all groups */
+#define CALIB_IWL_TX_ATTEN_GR1_FCH 34
+#define CALIB_IWL_TX_ATTEN_GR1_LCH 43
+#define CALIB_IWL_TX_ATTEN_GR2_FCH 44
+#define CALIB_IWL_TX_ATTEN_GR2_LCH 70
+#define CALIB_IWL_TX_ATTEN_GR3_FCH 71
+#define CALIB_IWL_TX_ATTEN_GR3_LCH 124
+#define CALIB_IWL_TX_ATTEN_GR4_FCH 125
+#define CALIB_IWL_TX_ATTEN_GR4_LCH 200
+#define CALIB_IWL_TX_ATTEN_GR5_FCH 1
+#define CALIB_IWL_TX_ATTEN_GR5_LCH 20
+
+
+union iwl_tx_power_dual_stream {
+	struct {
+		u8 radio_tx_gain[2];
+		u8 dsp_predis_atten[2];
+	} s;
+	u32 dw;
+};
+
+/********************* END TXPOWER *****************************************/
+
+/* HT flags */
+#define RXON_FLG_CTRL_CHANNEL_LOC_POS		(22)
+#define RXON_FLG_CTRL_CHANNEL_LOC_HI_MSK	__constant_cpu_to_le32(0x1<<22)
+
+#define RXON_FLG_HT_OPERATING_MODE_POS		(23)
+
+#define RXON_FLG_HT_PROT_MSK			__constant_cpu_to_le32(0x1<<23)
+#define RXON_FLG_FAT_PROT_MSK			__constant_cpu_to_le32(0x2<<23)
+
+#define RXON_FLG_CHANNEL_MODE_POS		(25)
+#define RXON_FLG_CHANNEL_MODE_MSK		__constant_cpu_to_le32(0x3<<25)
+#define RXON_FLG_CHANNEL_MODE_PURE_40_MSK	__constant_cpu_to_le32(0x1<<25)
+#define RXON_FLG_CHANNEL_MODE_MIXED_MSK		__constant_cpu_to_le32(0x2<<25)
+
+#define RXON_RX_CHAIN_DRIVER_FORCE_MSK		__constant_cpu_to_le16(0x1<<0)
+#define RXON_RX_CHAIN_VALID_MSK			__constant_cpu_to_le16(0x7<<1)
+#define RXON_RX_CHAIN_VALID_POS			(1)
+#define RXON_RX_CHAIN_FORCE_SEL_MSK		__constant_cpu_to_le16(0x7<<4)
+#define RXON_RX_CHAIN_FORCE_SEL_POS		(4)
+#define RXON_RX_CHAIN_FORCE_MIMO_SEL_MSK	__constant_cpu_to_le16(0x7<<7)
+#define RXON_RX_CHAIN_FORCE_MIMO_SEL_POS	(7)
+#define RXON_RX_CHAIN_CNT_MSK			__constant_cpu_to_le16(0x3<<10)
+#define RXON_RX_CHAIN_CNT_POS			(10)
+#define RXON_RX_CHAIN_MIMO_CNT_MSK		__constant_cpu_to_le16(0x3<<12)
+#define RXON_RX_CHAIN_MIMO_CNT_POS		(12)
+#define RXON_RX_CHAIN_MIMO_FORCE_MSK		__constant_cpu_to_le16(0x1<<14)
+#define RXON_RX_CHAIN_MIMO_FORCE_POS		(14)
+
+
+#define MCS_DUP_6M_PLCP 0x20
+
+/* OFDM HT rate masks */
+/* ***************************************** */
+#define R_MCS_6M_MSK 0x1
+#define R_MCS_12M_MSK 0x2
+#define R_MCS_18M_MSK 0x4
+#define R_MCS_24M_MSK 0x8
+#define R_MCS_36M_MSK 0x10
+#define R_MCS_48M_MSK 0x20
+#define R_MCS_54M_MSK 0x40
+#define R_MCS_60M_MSK 0x80
+#define R_MCS_12M_DUAL_MSK 0x100
+#define R_MCS_24M_DUAL_MSK 0x200
+#define R_MCS_36M_DUAL_MSK 0x400
+#define R_MCS_48M_DUAL_MSK 0x800
+
+#define is_legacy(tbl) (((tbl) == LQ_G) || ((tbl) == LQ_A))
+#define is_siso(tbl) (((tbl) == LQ_SISO))
+#define is_mimo(tbl) (((tbl) == LQ_MIMO))
+#define is_Ht(tbl) (is_siso(tbl) || is_mimo(tbl))
+#define is_a_band(tbl) (((tbl) == LQ_A))
+#define is_g_and(tbl) (((tbl) == LQ_G))
+
+/* Flow Handler Definitions */
+
+/**********************/
+/*     Addresses      */
+/**********************/
+
+#define FH_MEM_LOWER_BOUND                   (0x1000)
+#define FH_MEM_UPPER_BOUND                   (0x1EF0)
+
+#define IWL_FH_REGS_LOWER_BOUND		     (0x1000)
+#define IWL_FH_REGS_UPPER_BOUND		     (0x2000)
+
+#define IWL_FH_KW_MEM_ADDR_REG		     (FH_MEM_LOWER_BOUND + 0x97C)
+
+/* CBBC Area - Circular buffers base address cache pointers table */
+#define FH_MEM_CBBC_LOWER_BOUND              (FH_MEM_LOWER_BOUND + 0x9D0)
+#define FH_MEM_CBBC_UPPER_BOUND              (FH_MEM_LOWER_BOUND + 0xA10)
+/* queues 0 - 15 */
+#define FH_MEM_CBBC_QUEUE(x)  (FH_MEM_CBBC_LOWER_BOUND + (x) * 0x4)
+
+/* RSCSR Area */
+#define FH_MEM_RSCSR_LOWER_BOUND	(FH_MEM_LOWER_BOUND + 0xBC0)
+#define FH_MEM_RSCSR_UPPER_BOUND	(FH_MEM_LOWER_BOUND + 0xC00)
+#define FH_MEM_RSCSR_CHNL0		(FH_MEM_RSCSR_LOWER_BOUND)
+
+#define FH_RSCSR_CHNL0_STTS_WPTR_REG		(FH_MEM_RSCSR_CHNL0)
+#define FH_RSCSR_CHNL0_RBDCB_BASE_REG		(FH_MEM_RSCSR_CHNL0 + 0x004)
+#define FH_RSCSR_CHNL0_RBDCB_WPTR_REG		(FH_MEM_RSCSR_CHNL0 + 0x008)
+
+/* RCSR Area - Registers address map */
+#define FH_MEM_RCSR_LOWER_BOUND      (FH_MEM_LOWER_BOUND + 0xC00)
+#define FH_MEM_RCSR_UPPER_BOUND      (FH_MEM_LOWER_BOUND + 0xCC0)
+#define FH_MEM_RCSR_CHNL0            (FH_MEM_RCSR_LOWER_BOUND)
+
+#define FH_MEM_RCSR_CHNL0_CONFIG_REG	(FH_MEM_RCSR_CHNL0)
+
+/* RSSR Area - Rx shared ctrl & status registers */
+#define FH_MEM_RSSR_LOWER_BOUND                	(FH_MEM_LOWER_BOUND + 0xC40)
+#define FH_MEM_RSSR_UPPER_BOUND               	(FH_MEM_LOWER_BOUND + 0xD00)
+#define FH_MEM_RSSR_SHARED_CTRL_REG           	(FH_MEM_RSSR_LOWER_BOUND)
+#define FH_MEM_RSSR_RX_STATUS_REG	(FH_MEM_RSSR_LOWER_BOUND + 0x004)
+#define FH_MEM_RSSR_RX_ENABLE_ERR_IRQ2DRV  (FH_MEM_RSSR_LOWER_BOUND + 0x008)
+
+/* TCSR */
+#define IWL_FH_TCSR_LOWER_BOUND  (IWL_FH_REGS_LOWER_BOUND + 0xD00)
+#define IWL_FH_TCSR_UPPER_BOUND  (IWL_FH_REGS_LOWER_BOUND + 0xE60)
+
+#define IWL_FH_TCSR_CHNL_NUM                            (7)
+#define IWL_FH_TCSR_CHNL_TX_CONFIG_REG(_chnl) \
+	(IWL_FH_TCSR_LOWER_BOUND + 0x20 * _chnl)
+
+/* TSSR Area - Tx shared status registers */
+/* TSSR */
+#define IWL_FH_TSSR_LOWER_BOUND		(IWL_FH_REGS_LOWER_BOUND + 0xEA0)
+#define IWL_FH_TSSR_UPPER_BOUND		(IWL_FH_REGS_LOWER_BOUND + 0xEC0)
+
+#define IWL_FH_TSSR_TX_MSG_CONFIG_REG	(IWL_FH_TSSR_LOWER_BOUND + 0x008)
+#define IWL_FH_TSSR_TX_STATUS_REG	(IWL_FH_TSSR_LOWER_BOUND + 0x010)
+
+#define IWL_FH_TSSR_TX_MSG_CONFIG_REG_VAL_SNOOP_RD_TXPD_ON	(0xFF000000)
+#define IWL_FH_TSSR_TX_MSG_CONFIG_REG_VAL_ORDER_RD_TXPD_ON	(0x00FF0000)
+
+#define IWL_FH_TSSR_TX_MSG_CONFIG_REG_VAL_MAX_FRAG_SIZE_64B	(0x00000000)
+#define IWL_FH_TSSR_TX_MSG_CONFIG_REG_VAL_MAX_FRAG_SIZE_128B	(0x00000400)
+#define IWL_FH_TSSR_TX_MSG_CONFIG_REG_VAL_MAX_FRAG_SIZE_256B	(0x00000800)
+#define IWL_FH_TSSR_TX_MSG_CONFIG_REG_VAL_MAX_FRAG_SIZE_512B	(0x00000C00)
+
+#define IWL_FH_TSSR_TX_MSG_CONFIG_REG_VAL_SNOOP_RD_TFD_ON	(0x00000100)
+#define IWL_FH_TSSR_TX_MSG_CONFIG_REG_VAL_ORDER_RD_CBB_ON	(0x00000080)
+
+#define IWL_FH_TSSR_TX_MSG_CONFIG_REG_VAL_ORDER_RSP_WAIT_TH	(0x00000020)
+#define IWL_FH_TSSR_TX_MSG_CONFIG_REG_VAL_RSP_WAIT_TH		(0x00000005)
+
+#define IWL_FH_TSSR_TX_STATUS_REG_BIT_BUFS_EMPTY(_chnl)	\
+	((1 << (_chnl)) << 24)
+#define IWL_FH_TSSR_TX_STATUS_REG_BIT_NO_PEND_REQ(_chnl) \
+	((1 << (_chnl)) << 16)
+
+#define IWL_FH_TSSR_TX_STATUS_REG_MSK_CHNL_IDLE(_chnl) \
+	(IWL_FH_TSSR_TX_STATUS_REG_BIT_BUFS_EMPTY(_chnl) | \
+	IWL_FH_TSSR_TX_STATUS_REG_BIT_NO_PEND_REQ(_chnl))
+
+/* TCSR: tx_config register values */
+#define IWL_FH_TCSR_TX_CONFIG_REG_VAL_MSG_MODE_TXF              (0x00000000)
+#define IWL_FH_TCSR_TX_CONFIG_REG_VAL_MSG_MODE_DRIVER           (0x00000001)
+#define IWL_FH_TCSR_TX_CONFIG_REG_VAL_MSG_MODE_ARC              (0x00000002)
+
+#define IWL_FH_TCSR_TX_CONFIG_REG_VAL_DMA_CREDIT_DISABLE_VAL    (0x00000000)
+#define IWL_FH_TCSR_TX_CONFIG_REG_VAL_DMA_CREDIT_ENABLE_VAL     (0x00000008)
+
+#define IWL_FH_TCSR_TX_CONFIG_REG_VAL_CIRQ_HOST_NOINT           (0x00000000)
+#define IWL_FH_TCSR_TX_CONFIG_REG_VAL_CIRQ_HOST_ENDTFD          (0x00100000)
+#define IWL_FH_TCSR_TX_CONFIG_REG_VAL_CIRQ_HOST_IFTFD           (0x00200000)
+
+#define IWL_FH_TCSR_TX_CONFIG_REG_VAL_CIRQ_RTC_NOINT            (0x00000000)
+#define IWL_FH_TCSR_TX_CONFIG_REG_VAL_CIRQ_RTC_ENDTFD           (0x00400000)
+#define IWL_FH_TCSR_TX_CONFIG_REG_VAL_CIRQ_RTC_IFTFD            (0x00800000)
+
+#define IWL_FH_TCSR_TX_CONFIG_REG_VAL_DMA_CHNL_PAUSE            (0x00000000)
+#define IWL_FH_TCSR_TX_CONFIG_REG_VAL_DMA_CHNL_PAUSE_EOF        (0x40000000)
+#define IWL_FH_TCSR_TX_CONFIG_REG_VAL_DMA_CHNL_ENABLE           (0x80000000)
+
+#define IWL_FH_TCSR_CHNL_TX_BUF_STS_REG_VAL_TFDB_EMPTY          (0x00000000)
+#define IWL_FH_TCSR_CHNL_TX_BUF_STS_REG_VAL_TFDB_WAIT           (0x00002000)
+#define IWL_FH_TCSR_CHNL_TX_BUF_STS_REG_VAL_TFDB_VALID          (0x00000003)
+
+#define IWL_FH_TCSR_CHNL_TX_BUF_STS_REG_BIT_TFDB_WPTR           (0x00000001)
+
+#define IWL_FH_TCSR_CHNL_TX_BUF_STS_REG_POS_TB_NUM              (20)
+#define IWL_FH_TCSR_CHNL_TX_BUF_STS_REG_POS_TB_IDX              (12)
+
+/* RCSR:  channel 0 rx_config register defines */
+#define FH_RCSR_CHNL0_RX_CONFIG_DMA_CHNL_EN_MASK  (0xC0000000) /* bits 30-31 */
+#define FH_RCSR_CHNL0_RX_CONFIG_RBDBC_SIZE_MASK   (0x00F00000) /* bits 20-23 */
+#define FH_RCSR_CHNL0_RX_CONFIG_RB_SIZE_MASK	  (0x00030000) /* bits 16-17 */
+#define FH_RCSR_CHNL0_RX_CONFIG_SINGLE_FRAME_MASK (0x00008000) /* bit 15 */
+#define FH_RCSR_CHNL0_RX_CONFIG_IRQ_DEST_MASK     (0x00001000) /* bit 12 */
+#define FH_RCSR_CHNL0_RX_CONFIG_RB_TIMEOUT_MASK   (0x00000FF0) /* bit 4-11 */
+
+#define FH_RCSR_RX_CONFIG_RBDCB_SIZE_BITSHIFT       (20)
+#define FH_RCSR_RX_CONFIG_RB_SIZE_BITSHIFT			(16)
+
+/* RCSR: rx_config register values */
+#define FH_RCSR_RX_CONFIG_CHNL_EN_PAUSE_VAL         (0x00000000)
+#define FH_RCSR_RX_CONFIG_CHNL_EN_PAUSE_EOF_VAL     (0x40000000)
+#define FH_RCSR_RX_CONFIG_CHNL_EN_ENABLE_VAL        (0x80000000)
+
+#define IWL_FH_RCSR_RX_CONFIG_REG_VAL_RB_SIZE_4K    (0x00000000)
+
+/* RCSR channel 0 config register values */
+#define FH_RCSR_CHNL0_RX_CONFIG_IRQ_DEST_NO_INT_VAL       (0x00000000)
+#define FH_RCSR_CHNL0_RX_CONFIG_IRQ_DEST_INT_HOST_VAL     (0x00001000)
+
+/* RSCSR: defs used in normal mode */
+#define FH_RSCSR_CHNL0_RBDCB_WPTR_MASK		(0x00000FFF)	/* bits 0-11 */
+
+#define SCD_WIN_SIZE				64
+#define SCD_FRAME_LIMIT				64
+
+/* memory mapped registers */
+#define SCD_START_OFFSET		0xa02c00
+
+#define SCD_SRAM_BASE_ADDR           (SCD_START_OFFSET + 0x0)
+#define SCD_EMPTY_BITS               (SCD_START_OFFSET + 0x4)
+#define SCD_DRAM_BASE_ADDR           (SCD_START_OFFSET + 0x10)
+#define SCD_AIT                      (SCD_START_OFFSET + 0x18)
+#define SCD_TXFACT                   (SCD_START_OFFSET + 0x1c)
+#define SCD_QUEUE_WRPTR(x)           (SCD_START_OFFSET + 0x24 + (x) * 4)
+#define SCD_QUEUE_RDPTR(x)           (SCD_START_OFFSET + 0x64 + (x) * 4)
+#define SCD_SETQUEUENUM              (SCD_START_OFFSET + 0xa4)
+#define SCD_SET_TXSTAT_TXED          (SCD_START_OFFSET + 0xa8)
+#define SCD_SET_TXSTAT_DONE          (SCD_START_OFFSET + 0xac)
+#define SCD_SET_TXSTAT_NOT_SCHD      (SCD_START_OFFSET + 0xb0)
+#define SCD_DECREASE_CREDIT          (SCD_START_OFFSET + 0xb4)
+#define SCD_DECREASE_SCREDIT         (SCD_START_OFFSET + 0xb8)
+#define SCD_LOAD_CREDIT              (SCD_START_OFFSET + 0xbc)
+#define SCD_LOAD_SCREDIT             (SCD_START_OFFSET + 0xc0)
+#define SCD_BAR                      (SCD_START_OFFSET + 0xc4)
+#define SCD_BAR_DW0                  (SCD_START_OFFSET + 0xc8)
+#define SCD_BAR_DW1                  (SCD_START_OFFSET + 0xcc)
+#define SCD_QUEUECHAIN_SEL           (SCD_START_OFFSET + 0xd0)
+#define SCD_QUERY_REQ                (SCD_START_OFFSET + 0xd8)
+#define SCD_QUERY_RES                (SCD_START_OFFSET + 0xdc)
+#define SCD_PENDING_FRAMES           (SCD_START_OFFSET + 0xe0)
+#define SCD_INTERRUPT_MASK           (SCD_START_OFFSET + 0xe4)
+#define SCD_INTERRUPT_THRESHOLD      (SCD_START_OFFSET + 0xe8)
+#define SCD_QUERY_MIN_FRAME_SIZE     (SCD_START_OFFSET + 0x100)
+#define SCD_QUEUE_STATUS_BITS(x)     (SCD_START_OFFSET + 0x104 + (x) * 4)
+
+/* SRAM structures */
+#define SCD_CONTEXT_DATA_OFFSET			0x380
+#define SCD_TX_STTS_BITMAP_OFFSET		0x400
+#define SCD_TRANSLATE_TBL_OFFSET		0x500
+#define SCD_CONTEXT_QUEUE_OFFSET(x)	(SCD_CONTEXT_DATA_OFFSET + ((x) * 8))
+#define SCD_TRANSLATE_TBL_OFFSET_QUEUE(x) \
+	((SCD_TRANSLATE_TBL_OFFSET + ((x) * 2)) & 0xfffffffc)
+
+#define SCD_TXFACT_REG_TXFIFO_MASK(lo, hi) \
+       ((1<<(hi))|((1<<(hi))-(1<<(lo))))
+
+
+#define SCD_MODE_REG_BIT_SEARCH_MODE		(1<<0)
+#define SCD_MODE_REG_BIT_SBYP_MODE		(1<<1)
+
+#define SCD_TXFIFO_POS_TID			(0)
+#define SCD_TXFIFO_POS_RA			(4)
+#define SCD_QUEUE_STTS_REG_POS_ACTIVE		(0)
+#define SCD_QUEUE_STTS_REG_POS_TXF		(1)
+#define SCD_QUEUE_STTS_REG_POS_WSL		(5)
+#define SCD_QUEUE_STTS_REG_POS_SCD_ACK		(8)
+#define SCD_QUEUE_STTS_REG_POS_SCD_ACT_EN	(10)
+#define SCD_QUEUE_STTS_REG_MSK			(0x0007FC00)
+
+#define SCD_QUEUE_RA_TID_MAP_RATID_MSK		(0x01FF)
+
+#define SCD_QUEUE_CTX_REG1_WIN_SIZE_POS		(0)
+#define SCD_QUEUE_CTX_REG1_WIN_SIZE_MSK		(0x0000007F)
+#define SCD_QUEUE_CTX_REG1_CREDIT_POS		(8)
+#define SCD_QUEUE_CTX_REG1_CREDIT_MSK		(0x00FFFF00)
+#define SCD_QUEUE_CTX_REG1_SUPER_CREDIT_POS	(24)
+#define SCD_QUEUE_CTX_REG1_SUPER_CREDIT_MSK	(0xFF000000)
+#define SCD_QUEUE_CTX_REG2_FRAME_LIMIT_POS	(16)
+#define SCD_QUEUE_CTX_REG2_FRAME_LIMIT_MSK	(0x007F0000)
+
+#define CSR_HW_IF_CONFIG_REG_BIT_KEDRON_R	(0x00000010)
+#define CSR_HW_IF_CONFIG_REG_MSK_BOARD_VER	(0x00000C00)
+#define CSR_HW_IF_CONFIG_REG_BIT_MAC_SI		(0x00000100)
+#define CSR_HW_IF_CONFIG_REG_BIT_RADIO_SI	(0x00000200)
+
+static inline u8 iwl_hw_get_rate(__le32 rate_n_flags)
+{
+	return le32_to_cpu(rate_n_flags) & 0xFF;
+}
+static inline u16 iwl_hw_get_rate_n_flags(__le32 rate_n_flags)
+{
+	return le32_to_cpu(rate_n_flags) & 0xFFFF;
+}
+static inline __le32 iwl_hw_set_rate_n_flags(u8 rate, u16 flags)
+{
+	return cpu_to_le32(flags|(u16)rate);
+}
+
+struct iwl_tfd_frame_data {
+	__le32 tb1_addr;
+
+	__le32 val1;
+	/* __le32 ptb1_32_35:4; */
+#define IWL_tb1_addr_hi_POS 0
+#define IWL_tb1_addr_hi_LEN 4
+#define IWL_tb1_addr_hi_SYM val1
+	/* __le32 tb_len1:12; */
+#define IWL_tb1_len_POS 4
+#define IWL_tb1_len_LEN 12
+#define IWL_tb1_len_SYM val1
+	/* __le32 ptb2_0_15:16; */
+#define IWL_tb2_addr_lo16_POS 16
+#define IWL_tb2_addr_lo16_LEN 16
+#define IWL_tb2_addr_lo16_SYM val1
+
+	__le32 val2;
+	/* __le32 ptb2_16_35:20; */
+#define IWL_tb2_addr_hi20_POS 0
+#define IWL_tb2_addr_hi20_LEN 20
+#define IWL_tb2_addr_hi20_SYM val2
+	/* __le32 tb_len2:12; */
+#define IWL_tb2_len_POS 20
+#define IWL_tb2_len_LEN 12
+#define IWL_tb2_len_SYM val2
+} __attribute__ ((packed));
+
+struct iwl_tfd_frame {
+	__le32 val0;
+	/* __le32 rsvd1:24; */
+	/* __le32 num_tbs:5; */
+#define IWL_num_tbs_POS 24
+#define IWL_num_tbs_LEN 5
+#define IWL_num_tbs_SYM val0
+	/* __le32 rsvd2:1; */
+	/* __le32 padding:2; */
+	struct iwl_tfd_frame_data pa[10];
+	__le32 reserved;
+} __attribute__ ((packed));
+
+#define IWL4965_MAX_WIN_SIZE              64
+#define IWL4965_QUEUE_SIZE               256
+#define IWL4965_NUM_FIFOS                  7
+#define IWL_MAX_NUM_QUEUES                16
+
+struct iwl4965_queue_byte_cnt_entry {
+	__le16 val;
+	/* __le16 byte_cnt:12; */
+#define IWL_byte_cnt_POS 0
+#define IWL_byte_cnt_LEN 12
+#define IWL_byte_cnt_SYM val
+	/* __le16 rsvd:4; */
+} __attribute__ ((packed));
+
+struct iwl4965_sched_queue_byte_cnt_tbl {
+	struct iwl4965_queue_byte_cnt_entry tfd_offset[IWL4965_QUEUE_SIZE +
+						       IWL4965_MAX_WIN_SIZE];
+	u8 dont_care[1024 -
+		     (IWL4965_QUEUE_SIZE + IWL4965_MAX_WIN_SIZE) *
+		     sizeof(__le16)];
+} __attribute__ ((packed));
+
+/* Base physical address of iwl_shared is provided to SCD_DRAM_BASE_ADDR
+ * and &iwl_shared.val0 is provided to FH_RSCSR_CHNL0_STTS_WPTR_REG */
+struct iwl_shared {
+	struct iwl4965_sched_queue_byte_cnt_tbl
+	 queues_byte_cnt_tbls[IWL_MAX_NUM_QUEUES];
+	__le32 val0;
+
+	/* __le32 rb_closed_stts_rb_num:12; */
+#define IWL_rb_closed_stts_rb_num_POS 0
+#define IWL_rb_closed_stts_rb_num_LEN 12
+#define IWL_rb_closed_stts_rb_num_SYM val0
+	/* __le32 rsrv1:4; */
+	/* __le32 rb_closed_stts_rx_frame_num:12; */
+#define IWL_rb_closed_stts_rx_frame_num_POS 16
+#define IWL_rb_closed_stts_rx_frame_num_LEN 12
+#define IWL_rb_closed_stts_rx_frame_num_SYM val0
+	/* __le32 rsrv2:4; */
+
+	__le32 val1;
+	/* __le32 frame_finished_stts_rb_num:12; */
+#define IWL_frame_finished_stts_rb_num_POS 0
+#define IWL_frame_finished_stts_rb_num_LEN 12
+#define IWL_frame_finished_stts_rb_num_SYM val1
+	/* __le32 rsrv3:4; */
+	/* __le32 frame_finished_stts_rx_frame_num:12; */
+#define IWL_frame_finished_stts_rx_frame_num_POS 16
+#define IWL_frame_finished_stts_rx_frame_num_LEN 12
+#define IWL_frame_finished_stts_rx_frame_num_SYM val1
+	/* __le32 rsrv4:4; */
+
+	__le32 padding1;  /* so that allocation will be aligned to 16B */
+	__le32 padding2;
+} __attribute__ ((packed));
+
+#endif /* __iwl_4965_hw_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-4965-rs.c b/drivers/net/wireless/iwlwifi/iwl-4965-rs.c
new file mode 100644
index 000000000000..287c75705c44
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-4965-rs.c
@@ -0,0 +1,2295 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2005 - 2007 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * James P. Ketrenos <ipw2100-admin@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/skbuff.h>
+#include <linux/wireless.h>
+#include <net/mac80211.h>
+#include <net/ieee80211.h>
+
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/delay.h>
+
+#include <linux/workqueue.h>
+
+#include <net/mac80211.h>
+#include <linux/wireless.h>
+
+#define IWL 4965
+
+#include "../net/mac80211/ieee80211_rate.h"
+
+#include "iwlwifi.h"
+#include "iwl-helpers.h"
+
+#define RS_NAME "iwl-4965-rs"
+
+#define NUM_TRY_BEFORE_ANTENNA_TOGGLE 1
+#define IWL_NUMBER_TRY      1
+#define IWL_HT_NUMBER_TRY   3
+
+#define IWL_RATE_MAX_WINDOW		62
+#define IWL_RATE_HIGH_TH		10880
+#define IWL_RATE_MIN_FAILURE_TH		6
+#define IWL_RATE_MIN_SUCCESS_TH		8
+#define IWL_RATE_DECREASE_TH		1920
+#define IWL_RATE_INCREASE_TH            8960
+#define IWL_RATE_SCALE_FLUSH_INTVL   (2*HZ)        /*2 seconds */
+
+static u8 rs_ht_to_legacy[] = {
+	IWL_RATE_6M_INDEX, IWL_RATE_6M_INDEX,
+	IWL_RATE_6M_INDEX, IWL_RATE_6M_INDEX,
+	IWL_RATE_6M_INDEX,
+	IWL_RATE_6M_INDEX, IWL_RATE_9M_INDEX,
+	IWL_RATE_12M_INDEX, IWL_RATE_18M_INDEX,
+	IWL_RATE_24M_INDEX, IWL_RATE_36M_INDEX,
+	IWL_RATE_48M_INDEX, IWL_RATE_54M_INDEX
+};
+
+struct iwl_rate {
+	u32 rate_n_flags;
+} __attribute__ ((packed));
+
+struct iwl_rate_scale_data {
+	u64 data;
+	s32 success_counter;
+	s32 success_ratio;
+	s32 counter;
+	s32 average_tpt;
+	unsigned long stamp;
+};
+
+struct iwl_scale_tbl_info {
+	enum iwl_table_type lq_type;
+	enum iwl_antenna_type antenna_type;
+	u8 is_SGI;
+	u8 is_fat;
+	u8 is_dup;
+	u8 action;
+	s32 *expected_tpt;
+	struct iwl_rate current_rate;
+	struct iwl_rate_scale_data win[IWL_RATE_COUNT];
+};
+
+struct iwl_rate_scale_priv {
+	u8 active_tbl;
+	u8 enable_counter;
+	u8 stay_in_tbl;
+	u8 search_better_tbl;
+	s32 last_tpt;
+	u32 table_count_limit;
+	u32 max_failure_limit;
+	u32 max_success_limit;
+	u32 table_count;
+	u32 total_failed;
+	u32 total_success;
+	u32 flush_timer;
+	u8 action_counter;
+	u8 antenna;
+	u8 valid_antenna;
+	u8 is_green;
+	u8 is_dup;
+	u8 phymode;
+	u8 ibss_sta_added;
+	u32 supp_rates;
+	u16 active_rate;
+	u16 active_siso_rate;
+	u16 active_mimo_rate;
+	u16 active_rate_basic;
+	struct iwl_link_quality_cmd lq;
+	struct iwl_scale_tbl_info lq_info[LQ_SIZE];
+#ifdef CONFIG_MAC80211_DEBUGFS
+	struct dentry *rs_sta_dbgfs_scale_table_file;
+	struct dentry *rs_sta_dbgfs_stats_table_file;
+	struct iwl_rate dbg_fixed;
+	struct iwl_priv *drv;
+#endif
+};
+
+static void rs_rate_scale_perform(struct iwl_priv *priv,
+				   struct net_device *dev,
+				   struct ieee80211_hdr *hdr,
+				   struct sta_info *sta);
+static void rs_fill_link_cmd(struct iwl_rate_scale_priv *lq_data,
+			     struct iwl_rate *tx_mcs,
+			     struct iwl_link_quality_cmd *tbl);
+
+
+#ifdef CONFIG_MAC80211_DEBUGFS
+static void rs_dbgfs_set_mcs(struct iwl_rate_scale_priv *rs_priv,
+				struct iwl_rate *mcs, int index);
+#else
+static void rs_dbgfs_set_mcs(struct iwl_rate_scale_priv *rs_priv,
+				struct iwl_rate *mcs, int index)
+{}
+#endif
+static s32 expected_tpt_A[IWL_RATE_COUNT] = {
+	0, 0, 0, 0, 40, 57, 72, 98, 121, 154, 177, 186, 186
+};
+
+static s32 expected_tpt_G[IWL_RATE_COUNT] = {
+	7, 13, 35, 58, 40, 57, 72, 98, 121, 154, 177, 186, 186
+};
+
+static s32 expected_tpt_siso20MHz[IWL_RATE_COUNT] = {
+	0, 0, 0, 0, 42, 42, 76, 102, 124, 159, 183, 193, 202
+};
+
+static s32 expected_tpt_siso20MHzSGI[IWL_RATE_COUNT] = {
+	0, 0, 0, 0, 46, 46, 82, 110, 132, 168, 192, 202, 211
+};
+
+static s32 expected_tpt_mimo20MHz[IWL_RATE_COUNT] = {
+	0, 0, 0, 0, 74, 74, 123, 155, 179, 214, 236, 244, 251
+};
+
+static s32 expected_tpt_mimo20MHzSGI[IWL_RATE_COUNT] = {
+	0, 0, 0, 0, 81, 81, 131, 164, 188, 222, 243, 251, 257
+};
+
+static s32 expected_tpt_siso40MHz[IWL_RATE_COUNT] = {
+	0, 0, 0, 0, 77, 77, 127, 160, 184, 220, 242, 250, 257
+};
+
+static s32 expected_tpt_siso40MHzSGI[IWL_RATE_COUNT] = {
+	0, 0, 0, 0, 83, 83, 135, 169, 193, 229, 250, 257, 264
+};
+
+static s32 expected_tpt_mimo40MHz[IWL_RATE_COUNT] = {
+	0, 0, 0, 0, 123, 123, 182, 214, 235, 264, 279, 285, 289
+};
+
+static s32 expected_tpt_mimo40MHzSGI[IWL_RATE_COUNT] = {
+	0, 0, 0, 0, 131, 131, 191, 222, 242, 270, 284, 289, 293
+};
+
+static int iwl_lq_sync_callback(struct iwl_priv *priv,
+				struct iwl_cmd *cmd, struct sk_buff *skb)
+{
+	/*We didn't cache the SKB; let the caller free it */
+	return 1;
+}
+
+static inline u8 iwl_rate_get_rate(u32 rate_n_flags)
+{
+	return (u8)(rate_n_flags & 0xFF);
+}
+
+static int rs_send_lq_cmd(struct iwl_priv *priv,
+			  struct iwl_link_quality_cmd *lq, u8 flags)
+{
+#ifdef CONFIG_IWLWIFI_DEBUG
+	int i;
+#endif
+	int rc = -1;
+
+	struct iwl_host_cmd cmd = {
+		.id = REPLY_TX_LINK_QUALITY_CMD,
+		.len = sizeof(struct iwl_link_quality_cmd),
+		.meta.flags = flags,
+		.data = lq,
+	};
+
+	if ((lq->sta_id == 0xFF) &&
+	    (priv->iw_mode == IEEE80211_IF_TYPE_IBSS))
+		return rc;
+
+	if (lq->sta_id == 0xFF)
+		lq->sta_id = IWL_AP_ID;
+
+	IWL_DEBUG_RATE("lq station id 0x%x\n", lq->sta_id);
+	IWL_DEBUG_RATE("lq dta 0x%X 0x%X\n",
+		       lq->general_params.single_stream_ant_msk,
+		       lq->general_params.dual_stream_ant_msk);
+#ifdef CONFIG_IWLWIFI_DEBUG
+	for (i = 0; i < LINK_QUAL_MAX_RETRY_NUM; i++)
+		IWL_DEBUG_RATE("lq index %d 0x%X\n",
+				i, lq->rs_table[i].rate_n_flags);
+#endif
+
+	if (flags & CMD_ASYNC)
+		cmd.meta.u.callback = iwl_lq_sync_callback;
+
+	if (iwl_is_associated(priv) && priv->assoc_station_added &&
+	    priv->lq_mngr.lq_ready)
+		rc = iwl_send_cmd(priv, &cmd);
+
+	return rc;
+}
+
+static int rs_rate_scale_clear_window(struct iwl_rate_scale_data *window)
+{
+	window->data = 0;
+	window->success_counter = 0;
+	window->success_ratio = IWL_INVALID_VALUE;
+	window->counter = 0;
+	window->average_tpt = IWL_INVALID_VALUE;
+	window->stamp = 0;
+
+	return 0;
+}
+
+static int rs_collect_tx_data(struct iwl_rate_scale_data *windows,
+			      int scale_index, s32 tpt, u32 status)
+{
+	int rc = 0;
+	struct iwl_rate_scale_data *window = NULL;
+	u64 mask;
+	u8 win_size = IWL_RATE_MAX_WINDOW;
+	s32 fail_count;
+
+	if (scale_index < 0)
+		return -1;
+
+	if (scale_index >= IWL_RATE_COUNT)
+		return -1;
+
+	window = &(windows[scale_index]);
+
+	if (window->counter >= win_size) {
+
+		window->counter = win_size - 1;
+		mask = 1;
+		mask = (mask << (win_size - 1));
+		if ((window->data & mask)) {
+			window->data &= ~mask;
+			window->success_counter = window->success_counter - 1;
+		}
+	}
+
+	window->counter = window->counter + 1;
+	mask = window->data;
+	window->data = (mask << 1);
+	if (status != 0) {
+		window->success_counter = window->success_counter + 1;
+		window->data |= 0x1;
+	}
+
+	if (window->counter > 0)
+		window->success_ratio = 128 * (100 * window->success_counter)
+					/ window->counter;
+	else
+		window->success_ratio = IWL_INVALID_VALUE;
+
+	fail_count = window->counter - window->success_counter;
+
+	if ((fail_count >= IWL_RATE_MIN_FAILURE_TH) ||
+	    (window->success_counter >= IWL_RATE_MIN_SUCCESS_TH))
+		window->average_tpt = (window->success_ratio * tpt + 64) / 128;
+	else
+		window->average_tpt = IWL_INVALID_VALUE;
+
+	window->stamp = jiffies;
+
+	return rc;
+}
+
+int static rs_mcs_from_tbl(struct iwl_rate *mcs_rate,
+			   struct iwl_scale_tbl_info *tbl,
+			   int index, u8 use_green)
+{
+	int rc = 0;
+
+	if (is_legacy(tbl->lq_type)) {
+		mcs_rate->rate_n_flags = iwl_rates[index].plcp;
+		if (index >= IWL_FIRST_CCK_RATE && index <= IWL_LAST_CCK_RATE)
+			mcs_rate->rate_n_flags |= RATE_MCS_CCK_MSK;
+
+	} else if (is_siso(tbl->lq_type)) {
+		if (index > IWL_LAST_OFDM_RATE)
+			index = IWL_LAST_OFDM_RATE;
+		 mcs_rate->rate_n_flags = iwl_rates[index].plcp_siso |
+					  RATE_MCS_HT_MSK;
+	} else {
+		if (index > IWL_LAST_OFDM_RATE)
+			index = IWL_LAST_OFDM_RATE;
+		mcs_rate->rate_n_flags = iwl_rates[index].plcp_mimo |
+					 RATE_MCS_HT_MSK;
+	}
+
+	switch (tbl->antenna_type) {
+	case ANT_BOTH:
+		mcs_rate->rate_n_flags |= RATE_MCS_ANT_AB_MSK;
+		break;
+	case ANT_MAIN:
+		mcs_rate->rate_n_flags |= RATE_MCS_ANT_A_MSK;
+		break;
+	case ANT_AUX:
+		mcs_rate->rate_n_flags |= RATE_MCS_ANT_B_MSK;
+		break;
+	case ANT_NONE:
+		break;
+	}
+
+	if (is_legacy(tbl->lq_type))
+		return rc;
+
+	if (tbl->is_fat) {
+		if (tbl->is_dup)
+			mcs_rate->rate_n_flags |= RATE_MCS_DUP_MSK;
+		else
+			mcs_rate->rate_n_flags |= RATE_MCS_FAT_MSK;
+	}
+	if (tbl->is_SGI)
+		mcs_rate->rate_n_flags |= RATE_MCS_SGI_MSK;
+
+	if (use_green) {
+		mcs_rate->rate_n_flags |= RATE_MCS_GF_MSK;
+		if (is_siso(tbl->lq_type))
+			mcs_rate->rate_n_flags &= ~RATE_MCS_SGI_MSK;
+	}
+	return rc;
+}
+
+static int rs_get_tbl_info_from_mcs(const struct iwl_rate *mcs_rate,
+				    int phymode, struct iwl_scale_tbl_info *tbl,
+				    int *rate_idx)
+{
+	int index;
+	u32 ant_msk;
+
+	index = iwl_rate_index_from_plcp(mcs_rate->rate_n_flags);
+
+	if (index  == IWL_RATE_INVALID) {
+		*rate_idx = -1;
+		return -1;
+	}
+	tbl->is_SGI = 0;
+	tbl->is_fat = 0;
+	tbl->is_dup = 0;
+	tbl->antenna_type = ANT_BOTH;
+
+	if (!(mcs_rate->rate_n_flags & RATE_MCS_HT_MSK)) {
+		ant_msk = (mcs_rate->rate_n_flags & RATE_MCS_ANT_AB_MSK);
+
+		if (ant_msk == RATE_MCS_ANT_AB_MSK)
+			tbl->lq_type = LQ_NONE;
+		else {
+
+			if (phymode == MODE_IEEE80211A)
+				tbl->lq_type = LQ_A;
+			else
+				tbl->lq_type = LQ_G;
+
+			if (mcs_rate->rate_n_flags & RATE_MCS_ANT_A_MSK)
+				tbl->antenna_type = ANT_MAIN;
+			else
+				tbl->antenna_type = ANT_AUX;
+		}
+		*rate_idx = index;
+
+	} else if (iwl_rate_get_rate(mcs_rate->rate_n_flags)
+					<= IWL_RATE_SISO_60M_PLCP) {
+		tbl->lq_type = LQ_SISO;
+
+		ant_msk = (mcs_rate->rate_n_flags & RATE_MCS_ANT_AB_MSK);
+		if (ant_msk == RATE_MCS_ANT_AB_MSK)
+			tbl->lq_type = LQ_NONE;
+		else {
+			if (mcs_rate->rate_n_flags & RATE_MCS_ANT_A_MSK)
+				tbl->antenna_type = ANT_MAIN;
+			else
+				tbl->antenna_type = ANT_AUX;
+		}
+		if (mcs_rate->rate_n_flags & RATE_MCS_SGI_MSK)
+			tbl->is_SGI = 1;
+
+		if ((mcs_rate->rate_n_flags & RATE_MCS_FAT_MSK) ||
+		    (mcs_rate->rate_n_flags & RATE_MCS_DUP_MSK))
+			tbl->is_fat = 1;
+
+		if (mcs_rate->rate_n_flags & RATE_MCS_DUP_MSK)
+			tbl->is_dup = 1;
+
+		*rate_idx = index;
+	} else {
+		tbl->lq_type = LQ_MIMO;
+		if (mcs_rate->rate_n_flags & RATE_MCS_SGI_MSK)
+			tbl->is_SGI = 1;
+
+		if ((mcs_rate->rate_n_flags & RATE_MCS_FAT_MSK) ||
+		    (mcs_rate->rate_n_flags & RATE_MCS_DUP_MSK))
+			tbl->is_fat = 1;
+
+		if (mcs_rate->rate_n_flags & RATE_MCS_DUP_MSK)
+			tbl->is_dup = 1;
+		*rate_idx = index;
+	}
+	return 0;
+}
+
+static inline void rs_toggle_antenna(struct iwl_rate *new_rate,
+				     struct iwl_scale_tbl_info *tbl)
+{
+	if (tbl->antenna_type == ANT_AUX) {
+		tbl->antenna_type = ANT_MAIN;
+		new_rate->rate_n_flags &= ~RATE_MCS_ANT_B_MSK;
+		new_rate->rate_n_flags |= RATE_MCS_ANT_A_MSK;
+	} else {
+		tbl->antenna_type = ANT_AUX;
+		new_rate->rate_n_flags &= ~RATE_MCS_ANT_A_MSK;
+		new_rate->rate_n_flags |= RATE_MCS_ANT_B_MSK;
+	}
+}
+
+static inline s8 rs_use_green(struct iwl_priv *priv)
+{
+	s8 rc = 0;
+#ifdef CONFIG_IWLWIFI_HT
+	if (!priv->is_ht_enabled || !priv->current_assoc_ht.is_ht)
+		return 0;
+
+	if ((priv->current_assoc_ht.is_green_field) &&
+	    !(priv->current_assoc_ht.operating_mode & 0x4))
+		rc = 1;
+#endif	/*CONFIG_IWLWIFI_HT */
+	return rc;
+}
+
+/**
+ * rs_get_supported_rates - get the available rates
+ *
+ * if management frame or broadcast frame only return
+ * basic available rates.
+ *
+ */
+static void rs_get_supported_rates(struct iwl_rate_scale_priv *lq_data,
+				   struct ieee80211_hdr *hdr,
+				   enum iwl_table_type rate_type,
+				   u16 *data_rate)
+{
+	if (is_legacy(rate_type))
+		*data_rate = lq_data->active_rate;
+	else {
+		if (is_siso(rate_type))
+			*data_rate = lq_data->active_siso_rate;
+		else
+			*data_rate = lq_data->active_mimo_rate;
+	}
+
+	if (hdr && is_multicast_ether_addr(hdr->addr1) &&
+	    lq_data->active_rate_basic)
+		*data_rate = lq_data->active_rate_basic;
+}
+
+static u16 rs_get_adjacent_rate(u8 index, u16 rate_mask, int rate_type)
+{
+	u8 high = IWL_RATE_INVALID;
+	u8 low = IWL_RATE_INVALID;
+
+	/* 802.11A or ht walks to the next literal adjascent rate in
+	 * the rate table */
+	if (is_a_band(rate_type) || !is_legacy(rate_type)) {
+		int i;
+		u32 mask;
+
+		/* Find the previous rate that is in the rate mask */
+		i = index - 1;
+		for (mask = (1 << i); i >= 0; i--, mask >>= 1) {
+			if (rate_mask & mask) {
+				low = i;
+				break;
+			}
+		}
+
+		/* Find the next rate that is in the rate mask */
+		i = index + 1;
+		for (mask = (1 << i); i < IWL_RATE_COUNT; i++, mask <<= 1) {
+			if (rate_mask & mask) {
+				high = i;
+				break;
+			}
+		}
+
+		return (high << 8) | low;
+	}
+
+	low = index;
+	while (low != IWL_RATE_INVALID) {
+		low = iwl_rates[low].prev_rs;
+		if (low == IWL_RATE_INVALID)
+			break;
+		if (rate_mask & (1 << low))
+			break;
+		IWL_DEBUG_RATE("Skipping masked lower rate: %d\n", low);
+	}
+
+	high = index;
+	while (high != IWL_RATE_INVALID) {
+		high = iwl_rates[high].next_rs;
+		if (high == IWL_RATE_INVALID)
+			break;
+		if (rate_mask & (1 << high))
+			break;
+		IWL_DEBUG_RATE("Skipping masked higher rate: %d\n", high);
+	}
+
+	return (high << 8) | low;
+}
+
+static int rs_get_lower_rate(struct iwl_rate_scale_priv *lq_data,
+			     struct iwl_scale_tbl_info *tbl, u8 scale_index,
+			     u8 ht_possible, struct iwl_rate *mcs_rate)
+{
+	s32 low;
+	u16 rate_mask;
+	u16 high_low;
+	u8 switch_to_legacy = 0;
+	u8 is_green = lq_data->is_green;
+
+	/* check if we need to switch from HT to legacy rates.
+	 * assumption is that mandatory rates (1Mbps or 6Mbps)
+	 * are always supported (spec demand) */
+	if (!is_legacy(tbl->lq_type) && (!ht_possible || !scale_index)) {
+		switch_to_legacy = 1;
+		scale_index = rs_ht_to_legacy[scale_index];
+		if (lq_data->phymode == MODE_IEEE80211A)
+			tbl->lq_type = LQ_A;
+		else
+			tbl->lq_type = LQ_G;
+
+		if ((tbl->antenna_type == ANT_BOTH) ||
+		    (tbl->antenna_type == ANT_NONE))
+			tbl->antenna_type = ANT_MAIN;
+
+		tbl->is_fat = 0;
+		tbl->is_SGI = 0;
+	}
+
+	rs_get_supported_rates(lq_data, NULL, tbl->lq_type, &rate_mask);
+
+	/* mask with station rate restriction */
+	if (is_legacy(tbl->lq_type)) {
+		if (lq_data->phymode == (u8) MODE_IEEE80211A)
+			rate_mask  = (u16)(rate_mask &
+			   (lq_data->supp_rates << IWL_FIRST_OFDM_RATE));
+		else
+			rate_mask = (u16)(rate_mask & lq_data->supp_rates);
+	}
+
+	/* if we did switched from HT to legacy check current rate */
+	if ((switch_to_legacy) &&
+	    (rate_mask & (1 << scale_index))) {
+		rs_mcs_from_tbl(mcs_rate, tbl, scale_index, is_green);
+		return 0;
+	}
+
+	high_low = rs_get_adjacent_rate(scale_index, rate_mask, tbl->lq_type);
+	low = high_low & 0xff;
+
+	if (low != IWL_RATE_INVALID)
+		rs_mcs_from_tbl(mcs_rate, tbl, low, is_green);
+	else
+		rs_mcs_from_tbl(mcs_rate, tbl, scale_index, is_green);
+
+	return 0;
+}
+
+static void rs_tx_status(void *priv_rate,
+			 struct net_device *dev,
+			 struct sk_buff *skb,
+			 struct ieee80211_tx_status *tx_resp)
+{
+	int status;
+	u8 retries;
+	int rs_index, index = 0;
+	struct iwl_rate_scale_priv *lq;
+	struct iwl_link_quality_cmd *table;
+	struct sta_info *sta;
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
+	struct iwl_priv *priv = (struct iwl_priv *)priv_rate;
+	struct ieee80211_local *local = wdev_priv(dev->ieee80211_ptr);
+	struct iwl_rate_scale_data *window = NULL;
+	struct iwl_rate_scale_data *search_win = NULL;
+	struct iwl_rate tx_mcs;
+	struct iwl_scale_tbl_info tbl_type;
+	struct iwl_scale_tbl_info *curr_tbl, *search_tbl;
+	u8 active_index = 0;
+	u16 fc = le16_to_cpu(hdr->frame_control);
+	s32 tpt = 0;
+
+	IWL_DEBUG_RATE_LIMIT("get frame ack response, update rate scale window\n");
+
+	if (!ieee80211_is_data(fc) || is_multicast_ether_addr(hdr->addr1))
+		return;
+
+	retries = tx_resp->retry_count;
+
+	if (retries > 15)
+		retries = 15;
+
+
+	sta = sta_info_get(local, hdr->addr1);
+
+	if (!sta || !sta->rate_ctrl_priv) {
+		if (sta)
+			sta_info_put(sta);
+		return;
+	}
+
+	lq = (struct iwl_rate_scale_priv *)sta->rate_ctrl_priv;
+
+	if (!priv->lq_mngr.lq_ready)
+		return;
+
+	if ((priv->iw_mode == IEEE80211_IF_TYPE_IBSS) && !lq->ibss_sta_added)
+		return;
+
+	table = &lq->lq;
+	active_index = lq->active_tbl;
+
+	lq->antenna = (lq->valid_antenna & local->hw.conf.antenna_sel_tx);
+	if (!lq->antenna)
+		lq->antenna = lq->valid_antenna;
+
+	lq->antenna = lq->valid_antenna;
+	curr_tbl = &(lq->lq_info[active_index]);
+	search_tbl = &(lq->lq_info[(1 - active_index)]);
+	window = (struct iwl_rate_scale_data *)
+	    &(curr_tbl->win[0]);
+	search_win = (struct iwl_rate_scale_data *)
+	    &(search_tbl->win[0]);
+
+	tx_mcs.rate_n_flags = tx_resp->control.tx_rate;
+
+	rs_get_tbl_info_from_mcs(&tx_mcs, priv->phymode,
+				  &tbl_type, &rs_index);
+	if ((rs_index < 0) || (rs_index >= IWL_RATE_COUNT)) {
+		IWL_DEBUG_RATE("bad rate index at: %d rate 0x%X\n",
+			     rs_index, tx_mcs.rate_n_flags);
+		sta_info_put(sta);
+		return;
+	}
+
+	if (retries &&
+	    (tx_mcs.rate_n_flags !=
+				le32_to_cpu(table->rs_table[0].rate_n_flags))) {
+		IWL_DEBUG_RATE("initial rate does not match 0x%x 0x%x\n",
+				tx_mcs.rate_n_flags,
+				le32_to_cpu(table->rs_table[0].rate_n_flags));
+		sta_info_put(sta);
+		return;
+	}
+
+	while (retries) {
+		tx_mcs.rate_n_flags =
+		    le32_to_cpu(table->rs_table[index].rate_n_flags);
+		rs_get_tbl_info_from_mcs(&tx_mcs, priv->phymode,
+					  &tbl_type, &rs_index);
+
+		if ((tbl_type.lq_type == search_tbl->lq_type) &&
+		    (tbl_type.antenna_type == search_tbl->antenna_type) &&
+		    (tbl_type.is_SGI == search_tbl->is_SGI)) {
+			if (search_tbl->expected_tpt)
+				tpt = search_tbl->expected_tpt[rs_index];
+			else
+				tpt = 0;
+			rs_collect_tx_data(search_win,
+					    rs_index, tpt, 0);
+		} else if ((tbl_type.lq_type == curr_tbl->lq_type) &&
+			   (tbl_type.antenna_type == curr_tbl->antenna_type) &&
+			   (tbl_type.is_SGI == curr_tbl->is_SGI)) {
+			if (curr_tbl->expected_tpt)
+				tpt = curr_tbl->expected_tpt[rs_index];
+			else
+				tpt = 0;
+			rs_collect_tx_data(window, rs_index, tpt, 0);
+		}
+		if (lq->stay_in_tbl)
+			lq->total_failed++;
+		--retries;
+		index++;
+
+	}
+
+	if (!tx_resp->retry_count)
+		tx_mcs.rate_n_flags = tx_resp->control.tx_rate;
+	else
+		tx_mcs.rate_n_flags =
+			le32_to_cpu(table->rs_table[index].rate_n_flags);
+
+	rs_get_tbl_info_from_mcs(&tx_mcs, priv->phymode,
+				  &tbl_type, &rs_index);
+
+	if (tx_resp->flags & IEEE80211_TX_STATUS_ACK)
+		status = 1;
+	else
+		status = 0;
+
+	if ((tbl_type.lq_type == search_tbl->lq_type) &&
+	    (tbl_type.antenna_type == search_tbl->antenna_type) &&
+	    (tbl_type.is_SGI == search_tbl->is_SGI)) {
+		if (search_tbl->expected_tpt)
+			tpt = search_tbl->expected_tpt[rs_index];
+		else
+			tpt = 0;
+		rs_collect_tx_data(search_win,
+				    rs_index, tpt, status);
+	} else if ((tbl_type.lq_type == curr_tbl->lq_type) &&
+		   (tbl_type.antenna_type == curr_tbl->antenna_type) &&
+		   (tbl_type.is_SGI == curr_tbl->is_SGI)) {
+		if (curr_tbl->expected_tpt)
+			tpt = curr_tbl->expected_tpt[rs_index];
+		else
+			tpt = 0;
+		rs_collect_tx_data(window, rs_index, tpt, status);
+	}
+
+	if (lq->stay_in_tbl) {
+		if (status)
+			lq->total_success++;
+		else
+			lq->total_failed++;
+	}
+
+	rs_rate_scale_perform(priv, dev, hdr, sta);
+	sta_info_put(sta);
+	return;
+}
+
+static u8 rs_is_ant_connected(u8 valid_antenna,
+			      enum iwl_antenna_type antenna_type)
+{
+	if (antenna_type == ANT_AUX)
+		return ((valid_antenna & 0x2) ? 1:0);
+	else if (antenna_type == ANT_MAIN)
+		return ((valid_antenna & 0x1) ? 1:0);
+	else if (antenna_type == ANT_BOTH) {
+		if ((valid_antenna & 0x3) == 0x3)
+			return 1;
+		else
+			return 0;
+	}
+
+	return 1;
+}
+
+static u8 rs_is_other_ant_connected(u8 valid_antenna,
+				    enum iwl_antenna_type antenna_type)
+{
+	if (antenna_type == ANT_AUX)
+		return (rs_is_ant_connected(valid_antenna, ANT_MAIN));
+	else
+		return (rs_is_ant_connected(valid_antenna, ANT_AUX));
+
+	return 0;
+}
+
+static void rs_set_stay_in_table(u8 is_legacy,
+				 struct iwl_rate_scale_priv *lq_data)
+{
+	IWL_DEBUG_HT("we are staying in the same table\n");
+	lq_data->stay_in_tbl = 1;
+	if (is_legacy) {
+		lq_data->table_count_limit = IWL_LEGACY_TABLE_COUNT;
+		lq_data->max_failure_limit = IWL_LEGACY_FAILURE_LIMIT;
+		lq_data->max_success_limit = IWL_LEGACY_TABLE_COUNT;
+	} else {
+		lq_data->table_count_limit = IWL_NONE_LEGACY_TABLE_COUNT;
+		lq_data->max_failure_limit = IWL_NONE_LEGACY_FAILURE_LIMIT;
+		lq_data->max_success_limit = IWL_NONE_LEGACY_SUCCESS_LIMIT;
+	}
+	lq_data->table_count = 0;
+	lq_data->total_failed = 0;
+	lq_data->total_success = 0;
+}
+
+static void rs_get_expected_tpt_table(struct iwl_rate_scale_priv *lq_data,
+				      struct iwl_scale_tbl_info *tbl)
+{
+	if (is_legacy(tbl->lq_type)) {
+		if (!is_a_band(tbl->lq_type))
+			tbl->expected_tpt = expected_tpt_G;
+		else
+			tbl->expected_tpt = expected_tpt_A;
+	} else if (is_siso(tbl->lq_type)) {
+		if (tbl->is_fat && !lq_data->is_dup)
+			if (tbl->is_SGI)
+				tbl->expected_tpt = expected_tpt_siso40MHzSGI;
+			else
+				tbl->expected_tpt = expected_tpt_siso40MHz;
+		else if (tbl->is_SGI)
+			tbl->expected_tpt = expected_tpt_siso20MHzSGI;
+		else
+			tbl->expected_tpt = expected_tpt_siso20MHz;
+
+	} else if (is_mimo(tbl->lq_type)) {
+		if (tbl->is_fat && !lq_data->is_dup)
+			if (tbl->is_SGI)
+				tbl->expected_tpt = expected_tpt_mimo40MHzSGI;
+			else
+				tbl->expected_tpt = expected_tpt_mimo40MHz;
+		else if (tbl->is_SGI)
+			tbl->expected_tpt = expected_tpt_mimo20MHzSGI;
+		else
+			tbl->expected_tpt = expected_tpt_mimo20MHz;
+	} else
+		tbl->expected_tpt = expected_tpt_G;
+}
+
+#ifdef CONFIG_IWLWIFI_HT
+static s32 rs_get_best_rate(struct iwl_priv *priv,
+			    struct iwl_rate_scale_priv *lq_data,
+			    struct iwl_scale_tbl_info *tbl,
+			    u16 rate_mask, s8 index, s8 rate)
+{
+	struct iwl_scale_tbl_info *active_tbl =
+	    &(lq_data->lq_info[lq_data->active_tbl]);
+	s32 new_rate, high, low, start_hi;
+	s32 active_sr = active_tbl->win[index].success_ratio;
+	s32 *tpt_tbl = tbl->expected_tpt;
+	s32 active_tpt = active_tbl->expected_tpt[index];
+	u16 high_low;
+
+	new_rate = high = low = start_hi = IWL_RATE_INVALID;
+
+	for (; ;) {
+		high_low = rs_get_adjacent_rate(rate, rate_mask, tbl->lq_type);
+
+		low = high_low & 0xff;
+		high = (high_low >> 8) & 0xff;
+
+		if ((((100 * tpt_tbl[rate]) > lq_data->last_tpt) &&
+		     ((active_sr > IWL_RATE_DECREASE_TH) &&
+		      (active_sr <= IWL_RATE_HIGH_TH) &&
+		      (tpt_tbl[rate] <= active_tpt))) ||
+		    ((active_sr >= IWL_RATE_SCALE_SWITCH) &&
+		     (tpt_tbl[rate] > active_tpt))) {
+
+			if (start_hi != IWL_RATE_INVALID) {
+				new_rate = start_hi;
+				break;
+			}
+			new_rate = rate;
+			if (low != IWL_RATE_INVALID)
+				rate = low;
+			else
+				break;
+		} else {
+			if (new_rate != IWL_RATE_INVALID)
+				break;
+			else if (high != IWL_RATE_INVALID) {
+				start_hi = high;
+				rate = high;
+			} else {
+				new_rate = rate;
+				break;
+			}
+		}
+	}
+
+	return new_rate;
+}
+#endif				/* CONFIG_IWLWIFI_HT */
+
+static inline u8 rs_is_both_ant_supp(u8 valid_antenna)
+{
+	return (rs_is_ant_connected(valid_antenna, ANT_BOTH));
+}
+
+static int rs_switch_to_mimo(struct iwl_priv *priv,
+			     struct iwl_rate_scale_priv *lq_data,
+			     struct iwl_scale_tbl_info *tbl, int index)
+{
+	int rc = -1;
+#ifdef CONFIG_IWLWIFI_HT
+	u16 rate_mask;
+	s32 rate;
+	s8 is_green = lq_data->is_green;
+
+	if (!priv->is_ht_enabled || !priv->current_assoc_ht.is_ht)
+		return -1;
+
+	IWL_DEBUG_HT("LQ: try to switch to MIMO\n");
+	tbl->lq_type = LQ_MIMO;
+	rs_get_supported_rates(lq_data, NULL, tbl->lq_type,
+				&rate_mask);
+
+	if (priv->current_assoc_ht.tx_mimo_ps_mode == IWL_MIMO_PS_STATIC)
+		return -1;
+
+	if (!rs_is_both_ant_supp(lq_data->antenna))
+		return -1;
+
+	rc = 0;
+	tbl->is_dup = lq_data->is_dup;
+	tbl->action = 0;
+	if (priv->current_channel_width == IWL_CHANNEL_WIDTH_40MHZ)
+		tbl->is_fat = 1;
+	else
+		tbl->is_fat = 0;
+
+	if (tbl->is_fat) {
+		if (priv->current_assoc_ht.sgf & HT_SHORT_GI_40MHZ_ONLY)
+			tbl->is_SGI = 1;
+		else
+			tbl->is_SGI = 0;
+	} else if (priv->current_assoc_ht.sgf & HT_SHORT_GI_20MHZ_ONLY)
+		tbl->is_SGI = 1;
+	else
+		tbl->is_SGI = 0;
+
+	rs_get_expected_tpt_table(lq_data, tbl);
+
+	rate = rs_get_best_rate(priv, lq_data, tbl, rate_mask, index, index);
+
+	IWL_DEBUG_HT("LQ: MIMO best rate %d mask %X\n", rate, rate_mask);
+	if ((rate == IWL_RATE_INVALID) || !((1 << rate) & rate_mask))
+		return -1;
+	rs_mcs_from_tbl(&tbl->current_rate, tbl, rate, is_green);
+
+	IWL_DEBUG_HT("LQ: Switch to new mcs %X index is green %X\n",
+		     tbl->current_rate.rate_n_flags, is_green);
+
+#endif				/*CONFIG_IWLWIFI_HT */
+	return rc;
+}
+
+static int rs_switch_to_siso(struct iwl_priv *priv,
+			     struct iwl_rate_scale_priv *lq_data,
+			     struct iwl_scale_tbl_info *tbl, int index)
+{
+	int rc = -1;
+#ifdef CONFIG_IWLWIFI_HT
+	u16 rate_mask;
+	u8 is_green = lq_data->is_green;
+	s32 rate;
+
+	IWL_DEBUG_HT("LQ: try to switch to SISO\n");
+	if (!priv->is_ht_enabled || !priv->current_assoc_ht.is_ht)
+		return -1;
+
+	rc = 0;
+	tbl->is_dup = lq_data->is_dup;
+	tbl->lq_type = LQ_SISO;
+	tbl->action = 0;
+	rs_get_supported_rates(lq_data, NULL, tbl->lq_type,
+				&rate_mask);
+
+	if (priv->current_channel_width == IWL_CHANNEL_WIDTH_40MHZ)
+		tbl->is_fat = 1;
+	else
+		tbl->is_fat = 0;
+
+	if (tbl->is_fat) {
+		if (priv->current_assoc_ht.sgf & HT_SHORT_GI_40MHZ_ONLY)
+			tbl->is_SGI = 1;
+		else
+			tbl->is_SGI = 0;
+	} else if (priv->current_assoc_ht.sgf & HT_SHORT_GI_20MHZ_ONLY)
+		tbl->is_SGI = 1;
+	else
+		tbl->is_SGI = 0;
+
+	if (is_green)
+		tbl->is_SGI = 0;
+
+	rs_get_expected_tpt_table(lq_data, tbl);
+	rate = rs_get_best_rate(priv, lq_data, tbl, rate_mask, index, index);
+
+	IWL_DEBUG_HT("LQ: get best rate %d mask %X\n", rate, rate_mask);
+	if ((rate == IWL_RATE_INVALID) || !((1 << rate) & rate_mask)) {
+		IWL_DEBUG_HT("can not switch with index %d rate mask %x\n",
+			     rate, rate_mask);
+		return -1;
+	}
+	rs_mcs_from_tbl(&tbl->current_rate, tbl, rate, is_green);
+	IWL_DEBUG_HT("LQ: Switch to new mcs %X index is green %X\n",
+		     tbl->current_rate.rate_n_flags, is_green);
+
+#endif				/*CONFIG_IWLWIFI_HT */
+	return rc;
+}
+
+static int rs_move_legacy_other(struct iwl_priv *priv,
+				struct iwl_rate_scale_priv *lq_data,
+				int index)
+{
+	int rc = 0;
+	struct iwl_scale_tbl_info *tbl =
+	    &(lq_data->lq_info[lq_data->active_tbl]);
+	struct iwl_scale_tbl_info *search_tbl =
+	    &(lq_data->lq_info[(1 - lq_data->active_tbl)]);
+	struct iwl_rate_scale_data *window = &(tbl->win[index]);
+	u32 sz = (sizeof(struct iwl_scale_tbl_info) -
+		  (sizeof(struct iwl_rate_scale_data) * IWL_RATE_COUNT));
+	u8 start_action = tbl->action;
+
+	for (; ;) {
+		switch (tbl->action) {
+		case IWL_LEGACY_SWITCH_ANTENNA:
+			IWL_DEBUG_HT("LQ Legacy switch Antenna\n");
+
+			search_tbl->lq_type = LQ_NONE;
+			lq_data->action_counter++;
+			if (window->success_ratio >= IWL_RS_GOOD_RATIO)
+				break;
+			if (!rs_is_other_ant_connected(lq_data->antenna,
+							tbl->antenna_type))
+				break;
+
+			memcpy(search_tbl, tbl, sz);
+
+			rs_toggle_antenna(&(search_tbl->current_rate),
+					   search_tbl);
+			rs_get_expected_tpt_table(lq_data, search_tbl);
+			lq_data->search_better_tbl = 1;
+			goto out;
+
+		case IWL_LEGACY_SWITCH_SISO:
+			IWL_DEBUG_HT("LQ: Legacy switch to SISO\n");
+			memcpy(search_tbl, tbl, sz);
+			search_tbl->lq_type = LQ_SISO;
+			search_tbl->is_SGI = 0;
+			search_tbl->is_fat = 0;
+			rc = rs_switch_to_siso(priv, lq_data, search_tbl,
+					       index);
+			if (!rc) {
+				lq_data->search_better_tbl = 1;
+				lq_data->action_counter = 0;
+			}
+			if (!rc)
+				goto out;
+
+			break;
+		case IWL_LEGACY_SWITCH_MIMO:
+			IWL_DEBUG_HT("LQ: Legacy switch MIMO\n");
+			memcpy(search_tbl, tbl, sz);
+			search_tbl->lq_type = LQ_MIMO;
+			search_tbl->is_SGI = 0;
+			search_tbl->is_fat = 0;
+			search_tbl->antenna_type = ANT_BOTH;
+			rc = rs_switch_to_mimo(priv, lq_data, search_tbl,
+					       index);
+			if (!rc) {
+				lq_data->search_better_tbl = 1;
+				lq_data->action_counter = 0;
+			}
+			if (!rc)
+				goto out;
+			break;
+		}
+		tbl->action++;
+		if (tbl->action > IWL_LEGACY_SWITCH_MIMO)
+			tbl->action = IWL_LEGACY_SWITCH_ANTENNA;
+
+		if (tbl->action == start_action)
+			break;
+
+	}
+	return 0;
+
+ out:
+	tbl->action++;
+	if (tbl->action > IWL_LEGACY_SWITCH_MIMO)
+		tbl->action = IWL_LEGACY_SWITCH_ANTENNA;
+	return 0;
+
+}
+
+static int rs_move_siso_to_other(struct iwl_priv *priv,
+				 struct iwl_rate_scale_priv *lq_data,
+				 int index)
+{
+	int rc = -1;
+	u8 is_green = lq_data->is_green;
+	struct iwl_scale_tbl_info *tbl =
+	    &(lq_data->lq_info[lq_data->active_tbl]);
+	struct iwl_scale_tbl_info *search_tbl =
+	    &(lq_data->lq_info[(1 - lq_data->active_tbl)]);
+	struct iwl_rate_scale_data *window = &(tbl->win[index]);
+	u32 sz = (sizeof(struct iwl_scale_tbl_info) -
+		  (sizeof(struct iwl_rate_scale_data) * IWL_RATE_COUNT));
+	u8 start_action = tbl->action;
+
+	for (;;) {
+		lq_data->action_counter++;
+		switch (tbl->action) {
+		case IWL_SISO_SWITCH_ANTENNA:
+			IWL_DEBUG_HT("LQ: SISO SWITCH ANTENNA SISO\n");
+			search_tbl->lq_type = LQ_NONE;
+			if (window->success_ratio >= IWL_RS_GOOD_RATIO)
+				break;
+			if (!rs_is_other_ant_connected(lq_data->antenna,
+						       tbl->antenna_type))
+				break;
+
+			memcpy(search_tbl, tbl, sz);
+			search_tbl->action = IWL_SISO_SWITCH_MIMO;
+			rs_toggle_antenna(&(search_tbl->current_rate),
+					   search_tbl);
+			lq_data->search_better_tbl = 1;
+
+			goto out;
+
+		case IWL_SISO_SWITCH_MIMO:
+			IWL_DEBUG_HT("LQ: SISO SWITCH TO MIMO FROM SISO\n");
+			memcpy(search_tbl, tbl, sz);
+			search_tbl->lq_type = LQ_MIMO;
+			search_tbl->is_SGI = 0;
+			search_tbl->is_fat = 0;
+			search_tbl->antenna_type = ANT_BOTH;
+			rc = rs_switch_to_mimo(priv, lq_data, search_tbl,
+					       index);
+			if (!rc)
+				lq_data->search_better_tbl = 1;
+
+			if (!rc)
+				goto out;
+			break;
+		case IWL_SISO_SWITCH_GI:
+			IWL_DEBUG_HT("LQ: SISO SWITCH TO GI\n");
+			memcpy(search_tbl, tbl, sz);
+			search_tbl->action = 0;
+			if (search_tbl->is_SGI)
+				search_tbl->is_SGI = 0;
+			else if (!is_green)
+				search_tbl->is_SGI = 1;
+			else
+				break;
+			lq_data->search_better_tbl = 1;
+			if ((tbl->lq_type == LQ_SISO) &&
+			    (tbl->is_SGI)) {
+				s32 tpt = lq_data->last_tpt / 100;
+				if (((!tbl->is_fat) &&
+				     (tpt >= expected_tpt_siso20MHz[index])) ||
+				    ((tbl->is_fat) &&
+				     (tpt >= expected_tpt_siso40MHz[index])))
+					lq_data->search_better_tbl = 0;
+			}
+			rs_get_expected_tpt_table(lq_data, search_tbl);
+			rs_mcs_from_tbl(&search_tbl->current_rate,
+					     search_tbl, index, is_green);
+			goto out;
+		}
+		tbl->action++;
+		if (tbl->action > IWL_SISO_SWITCH_GI)
+			tbl->action = IWL_SISO_SWITCH_ANTENNA;
+
+		if (tbl->action == start_action)
+			break;
+	}
+	return 0;
+
+ out:
+	tbl->action++;
+	if (tbl->action > IWL_SISO_SWITCH_GI)
+		tbl->action = IWL_SISO_SWITCH_ANTENNA;
+	return 0;
+}
+
+static int rs_move_mimo_to_other(struct iwl_priv *priv,
+				 struct iwl_rate_scale_priv *lq_data,
+				 int index)
+{
+	int rc = -1;
+	s8 is_green = lq_data->is_green;
+	struct iwl_scale_tbl_info *tbl =
+	    &(lq_data->lq_info[lq_data->active_tbl]);
+	struct iwl_scale_tbl_info *search_tbl =
+	    &(lq_data->lq_info[(1 - lq_data->active_tbl)]);
+	u32 sz = (sizeof(struct iwl_scale_tbl_info) -
+		  (sizeof(struct iwl_rate_scale_data) * IWL_RATE_COUNT));
+	u8 start_action = tbl->action;
+
+	for (;;) {
+		lq_data->action_counter++;
+		switch (tbl->action) {
+		case IWL_MIMO_SWITCH_ANTENNA_A:
+		case IWL_MIMO_SWITCH_ANTENNA_B:
+			IWL_DEBUG_HT("LQ: MIMO SWITCH TO SISO\n");
+			memcpy(search_tbl, tbl, sz);
+			search_tbl->lq_type = LQ_SISO;
+			search_tbl->is_SGI = 0;
+			search_tbl->is_fat = 0;
+			if (tbl->action == IWL_MIMO_SWITCH_ANTENNA_A)
+				search_tbl->antenna_type = ANT_MAIN;
+			else
+				search_tbl->antenna_type = ANT_AUX;
+
+			rc = rs_switch_to_siso(priv, lq_data, search_tbl,
+					       index);
+			if (!rc) {
+				lq_data->search_better_tbl = 1;
+				goto out;
+			}
+			break;
+
+		case IWL_MIMO_SWITCH_GI:
+			IWL_DEBUG_HT("LQ: MIMO SWITCH TO GI\n");
+			memcpy(search_tbl, tbl, sz);
+			search_tbl->lq_type = LQ_MIMO;
+			search_tbl->antenna_type = ANT_BOTH;
+			search_tbl->action = 0;
+			if (search_tbl->is_SGI)
+				search_tbl->is_SGI = 0;
+			else
+				search_tbl->is_SGI = 1;
+			lq_data->search_better_tbl = 1;
+			if ((tbl->lq_type == LQ_MIMO) &&
+			    (tbl->is_SGI)) {
+				s32 tpt = lq_data->last_tpt / 100;
+				if (((!tbl->is_fat) &&
+				     (tpt >= expected_tpt_mimo20MHz[index])) ||
+				    ((tbl->is_fat) &&
+				     (tpt >= expected_tpt_mimo40MHz[index])))
+					lq_data->search_better_tbl = 0;
+			}
+			rs_get_expected_tpt_table(lq_data, search_tbl);
+			rs_mcs_from_tbl(&search_tbl->current_rate,
+					     search_tbl, index, is_green);
+			goto out;
+
+		}
+		tbl->action++;
+		if (tbl->action > IWL_MIMO_SWITCH_GI)
+			tbl->action = IWL_MIMO_SWITCH_ANTENNA_A;
+
+		if (tbl->action == start_action)
+			break;
+	}
+
+	return 0;
+ out:
+	tbl->action++;
+	if (tbl->action > IWL_MIMO_SWITCH_GI)
+		tbl->action = IWL_MIMO_SWITCH_ANTENNA_A;
+	return 0;
+
+}
+
+static void rs_stay_in_table(struct iwl_rate_scale_priv *lq_data)
+{
+	struct iwl_scale_tbl_info *tbl;
+	int i;
+	int active_tbl;
+	int flush_interval_passed = 0;
+
+	active_tbl = lq_data->active_tbl;
+
+	tbl = &(lq_data->lq_info[active_tbl]);
+
+	if (lq_data->stay_in_tbl) {
+
+		if (lq_data->flush_timer)
+			flush_interval_passed =
+			    time_after(jiffies,
+				       (unsigned long)(lq_data->flush_timer +
+					IWL_RATE_SCALE_FLUSH_INTVL));
+
+		flush_interval_passed = 0;
+		if ((lq_data->total_failed > lq_data->max_failure_limit) ||
+		    (lq_data->total_success > lq_data->max_success_limit) ||
+		    ((!lq_data->search_better_tbl) && (lq_data->flush_timer)
+		     && (flush_interval_passed))) {
+			IWL_DEBUG_HT("LQ: stay is expired %d %d %d\n:",
+				     lq_data->total_failed,
+				     lq_data->total_success,
+				     flush_interval_passed);
+			lq_data->stay_in_tbl = 0;
+			lq_data->total_failed = 0;
+			lq_data->total_success = 0;
+			lq_data->flush_timer = 0;
+		} else if (lq_data->table_count > 0) {
+			lq_data->table_count++;
+			if (lq_data->table_count >=
+			    lq_data->table_count_limit) {
+				lq_data->table_count = 0;
+
+				IWL_DEBUG_HT("LQ: stay in table clear win\n");
+				for (i = 0; i < IWL_RATE_COUNT; i++)
+					rs_rate_scale_clear_window(
+						&(tbl->win[i]));
+			}
+		}
+
+		if (!lq_data->stay_in_tbl) {
+			for (i = 0; i < IWL_RATE_COUNT; i++)
+				rs_rate_scale_clear_window(&(tbl->win[i]));
+		}
+	}
+}
+
+static void rs_rate_scale_perform(struct iwl_priv *priv,
+				  struct net_device *dev,
+				  struct ieee80211_hdr *hdr,
+				  struct sta_info *sta)
+{
+	int low = IWL_RATE_INVALID;
+	int high = IWL_RATE_INVALID;
+	int index;
+	int i;
+	struct iwl_rate_scale_data *window = NULL;
+	int current_tpt = IWL_INVALID_VALUE;
+	int low_tpt = IWL_INVALID_VALUE;
+	int high_tpt = IWL_INVALID_VALUE;
+	u32 fail_count;
+	s8 scale_action = 0;
+	u16 fc, rate_mask;
+	u8 update_lq = 0;
+	struct iwl_rate_scale_priv *lq_data;
+	struct iwl_scale_tbl_info *tbl, *tbl1;
+	u16 rate_scale_index_msk = 0;
+	struct iwl_rate mcs_rate;
+	u8 is_green = 0;
+	u8 active_tbl = 0;
+	u8 done_search = 0;
+	u16 high_low;
+
+	IWL_DEBUG_RATE("rate scale calculate new rate for skb\n");
+
+	fc = le16_to_cpu(hdr->frame_control);
+	if (!ieee80211_is_data(fc) || is_multicast_ether_addr(hdr->addr1)) {
+		/* Send management frames and broadcast/multicast data using
+		 * lowest rate. */
+		/* TODO: this could probably be improved.. */
+		return;
+	}
+
+	if (!sta || !sta->rate_ctrl_priv)
+		return;
+
+	if (!priv->lq_mngr.lq_ready) {
+		IWL_DEBUG_RATE("still rate scaling not ready\n");
+		return;
+	}
+	lq_data = (struct iwl_rate_scale_priv *)sta->rate_ctrl_priv;
+
+	if (!lq_data->search_better_tbl)
+		active_tbl = lq_data->active_tbl;
+	else
+		active_tbl = 1 - lq_data->active_tbl;
+
+	tbl = &(lq_data->lq_info[active_tbl]);
+	is_green = lq_data->is_green;
+
+	index = sta->last_txrate;
+
+	IWL_DEBUG_RATE("Rate scale index %d for type %d\n", index,
+		       tbl->lq_type);
+
+	rs_get_supported_rates(lq_data, hdr, tbl->lq_type,
+				&rate_mask);
+
+	IWL_DEBUG_RATE("mask 0x%04X \n", rate_mask);
+
+	/* mask with station rate restriction */
+	if (is_legacy(tbl->lq_type)) {
+		if (lq_data->phymode == (u8) MODE_IEEE80211A)
+			rate_scale_index_msk = (u16) (rate_mask &
+				(lq_data->supp_rates << IWL_FIRST_OFDM_RATE));
+		else
+			rate_scale_index_msk = (u16) (rate_mask &
+						      lq_data->supp_rates);
+
+	} else
+		rate_scale_index_msk = rate_mask;
+
+	if (!rate_scale_index_msk)
+		rate_scale_index_msk = rate_mask;
+
+	if (index < 0 || !((1 << index) & rate_scale_index_msk)) {
+		index = IWL_INVALID_VALUE;
+		update_lq = 1;
+
+		/* get the lowest availabe rate */
+		for (i = 0; i <= IWL_RATE_COUNT; i++) {
+			if ((1 << i) & rate_scale_index_msk)
+				index = i;
+		}
+
+		if (index == IWL_INVALID_VALUE) {
+			IWL_WARNING("Can not find a suitable rate\n");
+			return;
+		}
+	}
+
+	if (!tbl->expected_tpt)
+		rs_get_expected_tpt_table(lq_data, tbl);
+
+	window = &(tbl->win[index]);
+
+	fail_count = window->counter - window->success_counter;
+	if (((fail_count < IWL_RATE_MIN_FAILURE_TH) &&
+	     (window->success_counter < IWL_RATE_MIN_SUCCESS_TH))
+	    || (tbl->expected_tpt == NULL)) {
+		IWL_DEBUG_RATE("LQ: still below TH succ %d total %d "
+			       "for index %d\n",
+			       window->success_counter, window->counter, index);
+		window->average_tpt = IWL_INVALID_VALUE;
+		rs_stay_in_table(lq_data);
+		if (update_lq) {
+			rs_mcs_from_tbl(&mcs_rate, tbl, index, is_green);
+			rs_fill_link_cmd(lq_data, &mcs_rate, &lq_data->lq);
+			rs_send_lq_cmd(priv, &lq_data->lq, CMD_ASYNC);
+		}
+		goto out;
+
+	} else
+		window->average_tpt = ((window->success_ratio *
+					tbl->expected_tpt[index] + 64) / 128);
+
+	if (lq_data->search_better_tbl) {
+		int success_limit = IWL_RATE_SCALE_SWITCH;
+
+		if ((window->success_ratio > success_limit) ||
+		    (window->average_tpt > lq_data->last_tpt)) {
+			if (!is_legacy(tbl->lq_type)) {
+				IWL_DEBUG_HT("LQ: we are switching to HT"
+					     " rate suc %d current tpt %d"
+					     " old tpt %d\n",
+					     window->success_ratio,
+					     window->average_tpt,
+					     lq_data->last_tpt);
+				lq_data->enable_counter = 1;
+			}
+			lq_data->active_tbl = active_tbl;
+			current_tpt = window->average_tpt;
+		} else {
+			tbl->lq_type = LQ_NONE;
+			active_tbl = lq_data->active_tbl;
+			tbl = &(lq_data->lq_info[active_tbl]);
+
+			index = iwl_rate_index_from_plcp(
+				tbl->current_rate.rate_n_flags);
+
+			update_lq = 1;
+			current_tpt = lq_data->last_tpt;
+			IWL_DEBUG_HT("XXY GO BACK TO OLD TABLE\n");
+		}
+		lq_data->search_better_tbl = 0;
+		done_search = 1;
+		goto lq_update;
+	}
+
+	high_low = rs_get_adjacent_rate(index, rate_scale_index_msk,
+					tbl->lq_type);
+	low = high_low & 0xff;
+	high = (high_low >> 8) & 0xff;
+
+	current_tpt = window->average_tpt;
+
+	if (low != IWL_RATE_INVALID)
+		low_tpt = tbl->win[low].average_tpt;
+
+	if (high != IWL_RATE_INVALID)
+		high_tpt = tbl->win[high].average_tpt;
+
+
+	scale_action = 1;
+
+	if ((window->success_ratio <= IWL_RATE_DECREASE_TH) ||
+	    (current_tpt == 0)) {
+		IWL_DEBUG_RATE("decrease rate because of low success_ratio\n");
+		scale_action = -1;
+	} else if ((low_tpt == IWL_INVALID_VALUE) &&
+		   (high_tpt == IWL_INVALID_VALUE))
+		scale_action = 1;
+	else if ((low_tpt != IWL_INVALID_VALUE) &&
+		 (high_tpt != IWL_INVALID_VALUE) &&
+		 (low_tpt < current_tpt) &&
+		 (high_tpt < current_tpt))
+		scale_action = 0;
+	else {
+		if (high_tpt != IWL_INVALID_VALUE) {
+			if (high_tpt > current_tpt)
+				scale_action = 1;
+			else {
+				IWL_DEBUG_RATE
+				    ("decrease rate because of high tpt\n");
+				scale_action = -1;
+			}
+		} else if (low_tpt != IWL_INVALID_VALUE) {
+			if (low_tpt > current_tpt) {
+				IWL_DEBUG_RATE
+				    ("decrease rate because of low tpt\n");
+				scale_action = -1;
+			} else
+				scale_action = 1;
+		}
+	}
+
+	if (scale_action == -1) {
+		if ((low != IWL_RATE_INVALID) &&
+		    ((window->success_ratio > IWL_RATE_HIGH_TH) ||
+		     (current_tpt > (100 * tbl->expected_tpt[low]))))
+			scale_action = 0;
+	} else if ((scale_action == 1) &&
+		   (window->success_ratio < IWL_RATE_INCREASE_TH))
+		scale_action = 0;
+
+	switch (scale_action) {
+	case -1:
+		if (low != IWL_RATE_INVALID) {
+			update_lq = 1;
+			index = low;
+		}
+		break;
+	case 1:
+		if (high != IWL_RATE_INVALID) {
+			update_lq = 1;
+			index = high;
+		}
+
+		break;
+	case 0:
+	default:
+		break;
+	}
+
+	IWL_DEBUG_HT("choose rate scale index %d action %d low %d "
+		    "high %d type %d\n",
+		     index, scale_action, low, high, tbl->lq_type);
+
+ lq_update:
+	if (update_lq) {
+		rs_mcs_from_tbl(&mcs_rate, tbl, index, is_green);
+		rs_fill_link_cmd(lq_data, &mcs_rate, &lq_data->lq);
+		rs_send_lq_cmd(priv, &lq_data->lq, CMD_ASYNC);
+	}
+	rs_stay_in_table(lq_data);
+
+	if (!update_lq && !done_search && !lq_data->stay_in_tbl) {
+		lq_data->last_tpt = current_tpt;
+
+		if (is_legacy(tbl->lq_type))
+			rs_move_legacy_other(priv, lq_data, index);
+		else if (is_siso(tbl->lq_type))
+			rs_move_siso_to_other(priv, lq_data, index);
+		else
+			rs_move_mimo_to_other(priv, lq_data, index);
+
+		if (lq_data->search_better_tbl) {
+			tbl = &(lq_data->lq_info[(1 - lq_data->active_tbl)]);
+			for (i = 0; i < IWL_RATE_COUNT; i++)
+				rs_rate_scale_clear_window(&(tbl->win[i]));
+
+			index = iwl_rate_index_from_plcp(
+					tbl->current_rate.rate_n_flags);
+
+			IWL_DEBUG_HT("Switch current  mcs: %X index: %d\n",
+				     tbl->current_rate.rate_n_flags, index);
+			rs_fill_link_cmd(lq_data, &tbl->current_rate,
+					 &lq_data->lq);
+			rs_send_lq_cmd(priv, &lq_data->lq, CMD_ASYNC);
+		}
+		tbl1 = &(lq_data->lq_info[lq_data->active_tbl]);
+
+		if (is_legacy(tbl1->lq_type) &&
+#ifdef CONFIG_IWLWIFI_HT
+		    !priv->current_assoc_ht.is_ht &&
+#endif
+		    (lq_data->action_counter >= 1)) {
+			lq_data->action_counter = 0;
+			IWL_DEBUG_HT("LQ: STAY in legacy table\n");
+			rs_set_stay_in_table(1, lq_data);
+		}
+
+		if (lq_data->enable_counter &&
+		    (lq_data->action_counter >= IWL_ACTION_LIMIT)) {
+#ifdef CONFIG_IWLWIFI_HT_AGG
+			if ((lq_data->last_tpt > TID_AGG_TPT_THREHOLD) &&
+			    (priv->lq_mngr.agg_ctrl.auto_agg)) {
+				priv->lq_mngr.agg_ctrl.tid_retry =
+				    TID_ALL_SPECIFIED;
+				schedule_work(&priv->agg_work);
+			}
+#endif /*CONFIG_IWLWIFI_HT_AGG */
+			lq_data->action_counter = 0;
+			rs_set_stay_in_table(0, lq_data);
+		}
+	} else {
+		if ((!update_lq) && (!done_search) && (!lq_data->flush_timer))
+			lq_data->flush_timer = jiffies;
+	}
+
+out:
+	rs_mcs_from_tbl(&tbl->current_rate, tbl, index, is_green);
+	i = index;
+	sta->last_txrate = i;
+
+	/* sta->txrate is an index to A mode rates which start
+	 * at IWL_FIRST_OFDM_RATE
+	 */
+	if (lq_data->phymode == (u8) MODE_IEEE80211A)
+		sta->txrate = i - IWL_FIRST_OFDM_RATE;
+	else
+		sta->txrate = i;
+
+	return;
+}
+
+
+static void rs_initialize_lq(struct iwl_priv *priv,
+			     struct sta_info *sta)
+{
+	int i;
+	struct iwl_rate_scale_priv *lq;
+	struct iwl_scale_tbl_info *tbl;
+	u8 active_tbl = 0;
+	int rate_idx;
+	u8 use_green = rs_use_green(priv);
+	struct iwl_rate mcs_rate;
+
+	if (!sta || !sta->rate_ctrl_priv)
+		goto out;
+
+	lq = (struct iwl_rate_scale_priv *)sta->rate_ctrl_priv;
+	i = sta->last_txrate;
+
+	if ((lq->lq.sta_id == 0xff) &&
+	    (priv->iw_mode == IEEE80211_IF_TYPE_IBSS))
+		goto out;
+
+	if (!lq->search_better_tbl)
+		active_tbl = lq->active_tbl;
+	else
+		active_tbl = 1 - lq->active_tbl;
+
+	tbl = &(lq->lq_info[active_tbl]);
+
+	if ((i < 0) || (i >= IWL_RATE_COUNT))
+		i = 0;
+
+	mcs_rate.rate_n_flags = iwl_rates[i].plcp ;
+	mcs_rate.rate_n_flags |= RATE_MCS_ANT_B_MSK;
+	mcs_rate.rate_n_flags &= ~RATE_MCS_ANT_A_MSK;
+
+	if (i >= IWL_FIRST_CCK_RATE && i <= IWL_LAST_CCK_RATE)
+		mcs_rate.rate_n_flags |= RATE_MCS_CCK_MSK;
+
+	tbl->antenna_type = ANT_AUX;
+	rs_get_tbl_info_from_mcs(&mcs_rate, priv->phymode, tbl, &rate_idx);
+	if (!rs_is_ant_connected(priv->valid_antenna, tbl->antenna_type))
+	    rs_toggle_antenna(&mcs_rate, tbl);
+
+	rs_mcs_from_tbl(&mcs_rate, tbl, rate_idx, use_green);
+	tbl->current_rate.rate_n_flags = mcs_rate.rate_n_flags;
+	rs_get_expected_tpt_table(lq, tbl);
+	rs_fill_link_cmd(lq, &mcs_rate, &lq->lq);
+	rs_send_lq_cmd(priv, &lq->lq, CMD_ASYNC);
+ out:
+	return;
+}
+
+static struct ieee80211_rate *rs_get_lowest_rate(struct ieee80211_local
+						 *local)
+{
+	struct ieee80211_hw_mode *mode = local->oper_hw_mode;
+	int i;
+
+	for (i = 0; i < mode->num_rates; i++) {
+		struct ieee80211_rate *rate = &mode->rates[i];
+
+		if (rate->flags & IEEE80211_RATE_SUPPORTED)
+			return rate;
+	}
+
+	return &mode->rates[0];
+}
+
+static struct ieee80211_rate *rs_get_rate(void *priv_rate,
+					       struct net_device *dev,
+					       struct sk_buff *skb,
+					       struct rate_control_extra
+					       *extra)
+{
+
+	int i;
+	struct ieee80211_local *local = wdev_priv(dev->ieee80211_ptr);
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
+	struct sta_info *sta;
+	u16 fc;
+	struct iwl_priv *priv = (struct iwl_priv *)priv_rate;
+	struct iwl_rate_scale_priv *lq;
+
+	IWL_DEBUG_RATE_LIMIT("rate scale calculate new rate for skb\n");
+
+	memset(extra, 0, sizeof(*extra));
+
+	fc = le16_to_cpu(hdr->frame_control);
+	if (!ieee80211_is_data(fc) || is_multicast_ether_addr(hdr->addr1)) {
+		/* Send management frames and broadcast/multicast data using
+		 * lowest rate. */
+		/* TODO: this could probably be improved.. */
+		return rs_get_lowest_rate(local);
+	}
+
+	sta = sta_info_get(local, hdr->addr1);
+
+	if (!sta || !sta->rate_ctrl_priv) {
+		if (sta)
+			sta_info_put(sta);
+		return rs_get_lowest_rate(local);
+	}
+
+	lq = (struct iwl_rate_scale_priv *)sta->rate_ctrl_priv;
+	i = sta->last_txrate;
+
+	if ((priv->iw_mode == IEEE80211_IF_TYPE_IBSS) && !lq->ibss_sta_added) {
+		u8 sta_id = iwl_hw_find_station(priv, hdr->addr1);
+		DECLARE_MAC_BUF(mac);
+
+		if (sta_id == IWL_INVALID_STATION) {
+			IWL_DEBUG_RATE("LQ: ADD station %s\n",
+				       print_mac(mac, hdr->addr1));
+			sta_id = iwl_add_station(priv,
+						 hdr->addr1, 0, CMD_ASYNC);
+		}
+		if ((sta_id != IWL_INVALID_STATION)) {
+			lq->lq.sta_id = sta_id;
+			lq->lq.rs_table[0].rate_n_flags = 0;
+			lq->ibss_sta_added = 1;
+			rs_initialize_lq(priv, sta);
+		}
+		if (!lq->ibss_sta_added)
+			goto done;
+	}
+
+ done:
+	sta_info_put(sta);
+	if ((i < 0) || (i > IWL_RATE_COUNT))
+		return rs_get_lowest_rate(local);
+
+	return &priv->ieee_rates[i];
+}
+
+static void *rs_alloc_sta(void *priv, gfp_t gfp)
+{
+	struct iwl_rate_scale_priv *crl;
+	int i, j;
+
+	IWL_DEBUG_RATE("create station rate scale window\n");
+
+	crl = kzalloc(sizeof(struct iwl_rate_scale_priv), gfp);
+
+	if (crl == NULL)
+		return NULL;
+	crl->lq.sta_id = 0xff;
+
+
+	for (j = 0; j < LQ_SIZE; j++)
+		for (i = 0; i < IWL_RATE_COUNT; i++)
+			rs_rate_scale_clear_window(&(crl->lq_info[j].win[i]));
+
+	return crl;
+}
+
+static void rs_rate_init(void *priv_rate, void *priv_sta,
+			 struct ieee80211_local *local,
+			 struct sta_info *sta)
+{
+	int i, j;
+	struct ieee80211_hw_mode *mode = local->oper_hw_mode;
+	struct iwl_priv *priv = (struct iwl_priv *)priv_rate;
+	struct iwl_rate_scale_priv *crl = priv_sta;
+
+	crl->flush_timer = 0;
+	crl->supp_rates = sta->supp_rates;
+	sta->txrate = 3;
+	for (j = 0; j < LQ_SIZE; j++)
+		for (i = 0; i < IWL_RATE_COUNT; i++)
+			rs_rate_scale_clear_window(&(crl->lq_info[j].win[i]));
+
+	IWL_DEBUG_RATE("rate scale global init\n");
+	/* TODO: what is a good starting rate for STA? About middle? Maybe not
+	 * the lowest or the highest rate.. Could consider using RSSI from
+	 * previous packets? Need to have IEEE 802.1X auth succeed immediately
+	 * after assoc.. */
+
+	crl->ibss_sta_added = 0;
+	if (priv->iw_mode == IEEE80211_IF_TYPE_AP) {
+		u8 sta_id = iwl_hw_find_station(priv, sta->addr);
+		DECLARE_MAC_BUF(mac);
+
+		/* for IBSS the call are from tasklet */
+		IWL_DEBUG_HT("LQ: ADD station %s\n",
+			     print_mac(mac, sta->addr));
+
+		if (sta_id == IWL_INVALID_STATION) {
+			IWL_DEBUG_RATE("LQ: ADD station %s\n",
+				       print_mac(mac, sta->addr));
+			sta_id = iwl_add_station(priv,
+						 sta->addr, 0, CMD_ASYNC);
+		}
+		if ((sta_id != IWL_INVALID_STATION)) {
+			crl->lq.sta_id = sta_id;
+			crl->lq.rs_table[0].rate_n_flags = 0;
+		}
+		/* FIXME: this is w/a remove it later */
+		priv->assoc_station_added = 1;
+	}
+
+	for (i = 0; i < mode->num_rates; i++) {
+		if ((sta->supp_rates & BIT(i)) &&
+		    (mode->rates[i].flags & IEEE80211_RATE_SUPPORTED))
+			sta->txrate = i;
+	}
+	sta->last_txrate = sta->txrate;
+	/* For MODE_IEEE80211A mode cck rate are at end
+	 * rate table
+	 */
+	if (local->hw.conf.phymode == MODE_IEEE80211A)
+		sta->last_txrate += IWL_FIRST_OFDM_RATE;
+
+	crl->is_dup = priv->is_dup;
+	crl->valid_antenna = priv->valid_antenna;
+	crl->antenna = priv->antenna;
+	crl->is_green = rs_use_green(priv);
+	crl->active_rate = priv->active_rate;
+	crl->active_rate &= ~(0x1000);
+	crl->active_rate_basic = priv->active_rate_basic;
+	crl->phymode = priv->phymode;
+#ifdef CONFIG_IWLWIFI_HT
+	crl->active_siso_rate = (priv->current_assoc_ht.supp_rates[0] << 1);
+	crl->active_siso_rate |= (priv->current_assoc_ht.supp_rates[0] & 0x1);
+	crl->active_siso_rate &= ~((u16)0x2);
+	crl->active_siso_rate = crl->active_siso_rate << IWL_FIRST_OFDM_RATE;
+
+	crl->active_mimo_rate = (priv->current_assoc_ht.supp_rates[1] << 1);
+	crl->active_mimo_rate |= (priv->current_assoc_ht.supp_rates[1] & 0x1);
+	crl->active_mimo_rate &= ~((u16)0x2);
+	crl->active_mimo_rate = crl->active_mimo_rate << IWL_FIRST_OFDM_RATE;
+	IWL_DEBUG_HT("MIMO RATE 0x%X SISO MASK 0x%X\n", crl->active_siso_rate,
+		     crl->active_mimo_rate);
+#endif /*CONFIG_IWLWIFI_HT*/
+#ifdef CONFIG_MAC80211_DEBUGFS
+	crl->drv = priv;
+#endif
+
+	if (priv->assoc_station_added)
+		priv->lq_mngr.lq_ready = 1;
+
+	rs_initialize_lq(priv, sta);
+}
+
+static void rs_fill_link_cmd(struct iwl_rate_scale_priv *lq_data,
+			    struct iwl_rate *tx_mcs,
+			    struct iwl_link_quality_cmd *lq_cmd)
+{
+	int index = 0;
+	int rate_idx;
+	int repeat_rate = 0;
+	u8 ant_toggle_count = 0;
+	u8 use_ht_possible = 1;
+	struct iwl_rate new_rate;
+	struct iwl_scale_tbl_info tbl_type = { 0 };
+
+	rs_dbgfs_set_mcs(lq_data, tx_mcs, index);
+
+	rs_get_tbl_info_from_mcs(tx_mcs, lq_data->phymode,
+				  &tbl_type, &rate_idx);
+
+	if (is_legacy(tbl_type.lq_type)) {
+		ant_toggle_count = 1;
+		repeat_rate = IWL_NUMBER_TRY;
+	} else
+		repeat_rate = IWL_HT_NUMBER_TRY;
+
+	lq_cmd->general_params.mimo_delimiter =
+			is_mimo(tbl_type.lq_type) ? 1 : 0;
+	lq_cmd->rs_table[index].rate_n_flags =
+			cpu_to_le32(tx_mcs->rate_n_flags);
+	new_rate.rate_n_flags = tx_mcs->rate_n_flags;
+
+	if (is_mimo(tbl_type.lq_type) || (tbl_type.antenna_type == ANT_MAIN))
+		lq_cmd->general_params.single_stream_ant_msk = 1;
+	else
+		lq_cmd->general_params.single_stream_ant_msk = 2;
+
+	index++;
+	repeat_rate--;
+
+	while (index < LINK_QUAL_MAX_RETRY_NUM) {
+		while (repeat_rate > 0 && (index < LINK_QUAL_MAX_RETRY_NUM)) {
+			if (is_legacy(tbl_type.lq_type)) {
+				if (ant_toggle_count <
+				    NUM_TRY_BEFORE_ANTENNA_TOGGLE)
+					ant_toggle_count++;
+				else {
+					rs_toggle_antenna(&new_rate, &tbl_type);
+					ant_toggle_count = 1;
+				}
+			}
+
+			rs_dbgfs_set_mcs(lq_data, &new_rate, index);
+			lq_cmd->rs_table[index].rate_n_flags =
+					cpu_to_le32(new_rate.rate_n_flags);
+			repeat_rate--;
+			index++;
+		}
+
+		rs_get_tbl_info_from_mcs(&new_rate, lq_data->phymode, &tbl_type,
+						&rate_idx);
+
+		if (is_mimo(tbl_type.lq_type))
+			lq_cmd->general_params.mimo_delimiter = index;
+
+		rs_get_lower_rate(lq_data, &tbl_type, rate_idx,
+				  use_ht_possible, &new_rate);
+
+		if (is_legacy(tbl_type.lq_type)) {
+			if (ant_toggle_count < NUM_TRY_BEFORE_ANTENNA_TOGGLE)
+				ant_toggle_count++;
+			else {
+				rs_toggle_antenna(&new_rate, &tbl_type);
+				ant_toggle_count = 1;
+			}
+			repeat_rate = IWL_NUMBER_TRY;
+		} else
+			repeat_rate = IWL_HT_NUMBER_TRY;
+
+		use_ht_possible = 0;
+
+		rs_dbgfs_set_mcs(lq_data, &new_rate, index);
+		lq_cmd->rs_table[index].rate_n_flags =
+				cpu_to_le32(new_rate.rate_n_flags);
+
+		index++;
+		repeat_rate--;
+	}
+
+	lq_cmd->general_params.dual_stream_ant_msk = 3;
+	lq_cmd->agg_params.agg_dis_start_th = 3;
+	lq_cmd->agg_params.agg_time_limit = cpu_to_le16(4000);
+}
+
+static void *rs_alloc(struct ieee80211_local *local)
+{
+	return local->hw.priv;
+}
+/* rate scale requires free function to be implemented */
+static void rs_free(void *priv_rate)
+{
+	return;
+}
+
+static void rs_clear(void *priv_rate)
+{
+	struct iwl_priv *priv = (struct iwl_priv *) priv_rate;
+
+	IWL_DEBUG_RATE("enter\n");
+
+	priv->lq_mngr.lq_ready = 0;
+#ifdef CONFIG_IWLWIFI_HT
+#ifdef CONFIG_IWLWIFI_HT_AGG
+	if (priv->lq_mngr.agg_ctrl.granted_ba)
+		iwl4965_turn_off_agg(priv, TID_ALL_SPECIFIED);
+#endif /*CONFIG_IWLWIFI_HT_AGG */
+#endif /* CONFIG_IWLWIFI_HT */
+
+	IWL_DEBUG_RATE("leave\n");
+}
+
+static void rs_free_sta(void *priv, void *priv_sta)
+{
+	struct iwl_rate_scale_priv *rs_priv = priv_sta;
+
+	IWL_DEBUG_RATE("enter\n");
+	kfree(rs_priv);
+	IWL_DEBUG_RATE("leave\n");
+}
+
+
+#ifdef CONFIG_MAC80211_DEBUGFS
+static int open_file_generic(struct inode *inode, struct file *file)
+{
+	file->private_data = inode->i_private;
+	return 0;
+}
+static void rs_dbgfs_set_mcs(struct iwl_rate_scale_priv *rs_priv,
+				struct iwl_rate *mcs, int index)
+{
+	const u32 cck_rate = 0x820A;
+	if (rs_priv->dbg_fixed.rate_n_flags) {
+		if (index < 12)
+			mcs->rate_n_flags = rs_priv->dbg_fixed.rate_n_flags;
+		else
+			mcs->rate_n_flags = cck_rate;
+		IWL_DEBUG_RATE("Fixed rate ON\n");
+		return;
+	}
+
+	IWL_DEBUG_RATE("Fixed rate OFF\n");
+}
+
+static ssize_t rs_sta_dbgfs_scale_table_write(struct file *file,
+			const char __user *user_buf, size_t count, loff_t *ppos)
+{
+	struct iwl_rate_scale_priv *rs_priv = file->private_data;
+	char buf[64];
+	int buf_size;
+	u32 parsed_rate;
+
+	memset(buf, 0, sizeof(buf));
+	buf_size = min(count, sizeof(buf) -  1);
+	if (copy_from_user(buf, user_buf, buf_size))
+		return -EFAULT;
+
+	if (sscanf(buf, "%x", &parsed_rate) == 1)
+		rs_priv->dbg_fixed.rate_n_flags = parsed_rate;
+	else
+		rs_priv->dbg_fixed.rate_n_flags = 0;
+
+	rs_priv->active_rate = 0x0FFF;
+	rs_priv->active_siso_rate = 0x1FD0;
+	rs_priv->active_mimo_rate = 0x1FD0;
+
+	IWL_DEBUG_RATE("sta_id %d rate 0x%X\n",
+		rs_priv->lq.sta_id, rs_priv->dbg_fixed.rate_n_flags);
+
+	if (rs_priv->dbg_fixed.rate_n_flags) {
+		rs_fill_link_cmd(rs_priv, &rs_priv->dbg_fixed, &rs_priv->lq);
+		rs_send_lq_cmd(rs_priv->drv, &rs_priv->lq, CMD_ASYNC);
+	}
+
+	return count;
+}
+
+static ssize_t rs_sta_dbgfs_scale_table_read(struct file *file,
+			char __user *user_buf, size_t count, loff_t *ppos)
+{
+	char buff[1024];
+	int desc = 0;
+	int i = 0;
+
+	struct iwl_rate_scale_priv *rs_priv = file->private_data;
+
+	desc += sprintf(buff+desc, "sta_id %d\n", rs_priv->lq.sta_id);
+	desc += sprintf(buff+desc, "failed=%d success=%d rate=0%X\n",
+			rs_priv->total_failed, rs_priv->total_success,
+			rs_priv->active_rate);
+	desc += sprintf(buff+desc, "fixed rate 0x%X\n",
+			rs_priv->dbg_fixed.rate_n_flags);
+	desc += sprintf(buff+desc, "general:"
+		"flags=0x%X mimo-d=%d s-ant0x%x d-ant=0x%x\n",
+		rs_priv->lq.general_params.flags,
+		rs_priv->lq.general_params.mimo_delimiter,
+		rs_priv->lq.general_params.single_stream_ant_msk,
+		rs_priv->lq.general_params.dual_stream_ant_msk);
+
+	desc += sprintf(buff+desc, "agg:"
+			"time_limit=%d dist_start_th=%d frame_cnt_limit=%d\n",
+			le16_to_cpu(rs_priv->lq.agg_params.agg_time_limit),
+			rs_priv->lq.agg_params.agg_dis_start_th,
+			rs_priv->lq.agg_params.agg_frame_cnt_limit);
+
+	desc += sprintf(buff+desc,
+			"Start idx [0]=0x%x [1]=0x%x [2]=0x%x [3]=0x%x\n",
+			rs_priv->lq.general_params.start_rate_index[0],
+			rs_priv->lq.general_params.start_rate_index[1],
+			rs_priv->lq.general_params.start_rate_index[2],
+			rs_priv->lq.general_params.start_rate_index[3]);
+
+
+	for (i = 0; i < LINK_QUAL_MAX_RETRY_NUM; i++)
+		desc += sprintf(buff+desc, " rate[%d] 0x%X\n",
+			i, le32_to_cpu(rs_priv->lq.rs_table[i].rate_n_flags));
+
+	return simple_read_from_buffer(user_buf, count, ppos, buff, desc);
+}
+
+static const struct file_operations rs_sta_dbgfs_scale_table_ops = {
+	.write = rs_sta_dbgfs_scale_table_write,
+	.read = rs_sta_dbgfs_scale_table_read,
+	.open = open_file_generic,
+};
+static ssize_t rs_sta_dbgfs_stats_table_read(struct file *file,
+			char __user *user_buf, size_t count, loff_t *ppos)
+{
+	char buff[1024];
+	int desc = 0;
+	int i, j;
+
+	struct iwl_rate_scale_priv *rs_priv = file->private_data;
+	for (i = 0; i < LQ_SIZE; i++) {
+		desc += sprintf(buff+desc, "%s type=%d SGI=%d FAT=%d DUP=%d\n"
+				"rate=0x%X\n",
+				rs_priv->active_tbl == i?"*":"x",
+				rs_priv->lq_info[i].lq_type,
+				rs_priv->lq_info[i].is_SGI,
+				rs_priv->lq_info[i].is_fat,
+				rs_priv->lq_info[i].is_dup,
+				rs_priv->lq_info[i].current_rate.rate_n_flags);
+		for (j = 0; j < IWL_RATE_COUNT; j++) {
+			desc += sprintf(buff+desc,
+					"counter=%d success=%d %%=%d\n",
+					rs_priv->lq_info[i].win[j].counter,
+					rs_priv->lq_info[i].win[j].success_counter,
+					rs_priv->lq_info[i].win[j].success_ratio);
+		}
+	}
+	return simple_read_from_buffer(user_buf, count, ppos, buff, desc);
+}
+
+static const struct file_operations rs_sta_dbgfs_stats_table_ops = {
+	.read = rs_sta_dbgfs_stats_table_read,
+	.open = open_file_generic,
+};
+
+static void rs_add_debugfs(void *priv, void *priv_sta,
+					struct dentry *dir)
+{
+	struct iwl_rate_scale_priv *rs_priv = priv_sta;
+	rs_priv->rs_sta_dbgfs_scale_table_file =
+		debugfs_create_file("rate_scale_table", 0600, dir,
+				rs_priv, &rs_sta_dbgfs_scale_table_ops);
+	rs_priv->rs_sta_dbgfs_stats_table_file =
+		debugfs_create_file("rate_stats_table", 0600, dir,
+			rs_priv, &rs_sta_dbgfs_stats_table_ops);
+}
+
+static void rs_remove_debugfs(void *priv, void *priv_sta)
+{
+	struct iwl_rate_scale_priv *rs_priv = priv_sta;
+	debugfs_remove(rs_priv->rs_sta_dbgfs_scale_table_file);
+	debugfs_remove(rs_priv->rs_sta_dbgfs_stats_table_file);
+}
+#endif
+
+static struct rate_control_ops rs_ops = {
+	.module = NULL,
+	.name = RS_NAME,
+	.tx_status = rs_tx_status,
+	.get_rate = rs_get_rate,
+	.rate_init = rs_rate_init,
+	.clear = rs_clear,
+	.alloc = rs_alloc,
+	.free = rs_free,
+	.alloc_sta = rs_alloc_sta,
+	.free_sta = rs_free_sta,
+#ifdef CONFIG_MAC80211_DEBUGFS
+	.add_sta_debugfs = rs_add_debugfs,
+	.remove_sta_debugfs = rs_remove_debugfs,
+#endif
+};
+
+int iwl_fill_rs_info(struct ieee80211_hw *hw, char *buf, u8 sta_id)
+{
+	struct ieee80211_local *local = hw_to_local(hw);
+	struct iwl_priv *priv = hw->priv;
+	struct iwl_rate_scale_priv *rs_priv;
+	struct sta_info *sta;
+	int count = 0, i;
+	u32 samples = 0, success = 0, good = 0;
+	unsigned long now = jiffies;
+	u32 max_time = 0;
+	u8 lq_type, antenna;
+
+	sta = sta_info_get(local, priv->stations[sta_id].sta.sta.addr);
+	if (!sta || !sta->rate_ctrl_priv) {
+		if (sta) {
+			sta_info_put(sta);
+			IWL_DEBUG_RATE("leave - no private rate data!\n");
+		} else
+			IWL_DEBUG_RATE("leave - no station!\n");
+		return sprintf(buf, "station %d not found\n", sta_id);
+	}
+
+	rs_priv = (void *)sta->rate_ctrl_priv;
+
+	lq_type = rs_priv->lq_info[rs_priv->active_tbl].lq_type;
+	antenna = rs_priv->lq_info[rs_priv->active_tbl].antenna_type;
+
+	if (is_legacy(lq_type))
+		i = IWL_RATE_54M_INDEX;
+	else
+		i = IWL_RATE_60M_INDEX;
+	while (1) {
+		u64 mask;
+		int j;
+		int active = rs_priv->active_tbl;
+
+		count +=
+		    sprintf(&buf[count], " %2dMbs: ", iwl_rates[i].ieee / 2);
+
+		mask = (1ULL << (IWL_RATE_MAX_WINDOW - 1));
+		for (j = 0; j < IWL_RATE_MAX_WINDOW; j++, mask >>= 1)
+			buf[count++] =
+				(rs_priv->lq_info[active].win[i].data & mask)
+				? '1' : '0';
+
+		samples += rs_priv->lq_info[active].win[i].counter;
+		good += rs_priv->lq_info[active].win[i].success_counter;
+		success += rs_priv->lq_info[active].win[i].success_counter *
+			   iwl_rates[i].ieee;
+
+		if (rs_priv->lq_info[active].win[i].stamp) {
+			int delta =
+				   jiffies_to_msecs(now -
+				   rs_priv->lq_info[active].win[i].stamp);
+
+			if (delta > max_time)
+				max_time = delta;
+
+			count += sprintf(&buf[count], "%5dms\n", delta);
+		} else
+			buf[count++] = '\n';
+
+		j = iwl_get_prev_ieee_rate(i);
+		if (j == i)
+			break;
+		i = j;
+	}
+
+	/* Display the average rate of all samples taken.
+	 *
+	 * NOTE:  We multiple # of samples by 2 since the IEEE measurement
+	 * added from iwl_rates is actually 2X the rate */
+	if (samples)
+		count += sprintf(&buf[count],
+			 "\nAverage rate is %3d.%02dMbs over last %4dms\n"
+			 "%3d%% success (%d good packets over %d tries)\n",
+			 success / (2 * samples), (success * 5 / samples) % 10,
+			 max_time, good * 100 / samples, good, samples);
+	else
+		count += sprintf(&buf[count], "\nAverage rate: 0Mbs\n");
+	count += sprintf(&buf[count], "\nrate scale type %d anntena %d "
+			 "active_search %d rate index %d\n", lq_type, antenna,
+			 rs_priv->search_better_tbl, sta->last_txrate);
+
+	sta_info_put(sta);
+	return count;
+}
+
+void iwl_rate_scale_init(struct ieee80211_hw *hw, s32 sta_id)
+{
+	struct iwl_priv *priv = hw->priv;
+
+	priv->lq_mngr.lq_ready = 1;
+}
+
+void iwl_rate_control_register(struct ieee80211_hw *hw)
+{
+	ieee80211_rate_control_register(&rs_ops);
+}
+
+void iwl_rate_control_unregister(struct ieee80211_hw *hw)
+{
+	ieee80211_rate_control_unregister(&rs_ops);
+}
+
diff --git a/drivers/net/wireless/iwlwifi/iwl-4965-rs.h b/drivers/net/wireless/iwlwifi/iwl-4965-rs.h
new file mode 100644
index 000000000000..c6325f72df68
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-4965-rs.h
@@ -0,0 +1,266 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2003 - 2007 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * James P. Ketrenos <ipw2100-admin@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+
+#ifndef __iwl_4965_rs_h__
+#define __iwl_4965_rs_h__
+
+#include "iwl-4965.h"
+
+struct iwl_rate_info {
+	u8 plcp;
+	u8 plcp_siso;
+	u8 plcp_mimo;
+	u8 ieee;
+	u8 prev_ieee;    /* previous rate in IEEE speeds */
+	u8 next_ieee;    /* next rate in IEEE speeds */
+	u8 prev_rs;      /* previous rate used in rs algo */
+	u8 next_rs;      /* next rate used in rs algo */
+	u8 prev_rs_tgg;  /* previous rate used in TGG rs algo */
+	u8 next_rs_tgg;  /* next rate used in TGG rs algo */
+};
+
+enum {
+	IWL_RATE_1M_INDEX = 0,
+	IWL_RATE_2M_INDEX,
+	IWL_RATE_5M_INDEX,
+	IWL_RATE_11M_INDEX,
+	IWL_RATE_6M_INDEX,
+	IWL_RATE_9M_INDEX,
+	IWL_RATE_12M_INDEX,
+	IWL_RATE_18M_INDEX,
+	IWL_RATE_24M_INDEX,
+	IWL_RATE_36M_INDEX,
+	IWL_RATE_48M_INDEX,
+	IWL_RATE_54M_INDEX,
+	IWL_RATE_60M_INDEX,
+	IWL_RATE_COUNT,
+	IWL_RATE_INVM_INDEX = IWL_RATE_COUNT,
+	IWL_RATE_INVALID = IWL_RATE_INVM_INDEX
+};
+
+enum {
+	IWL_FIRST_OFDM_RATE = IWL_RATE_6M_INDEX,
+	IWL_LAST_OFDM_RATE = IWL_RATE_60M_INDEX,
+	IWL_FIRST_CCK_RATE = IWL_RATE_1M_INDEX,
+	IWL_LAST_CCK_RATE = IWL_RATE_11M_INDEX,
+};
+
+/* #define vs. enum to keep from defaulting to 'large integer' */
+#define	IWL_RATE_6M_MASK   (1<<IWL_RATE_6M_INDEX)
+#define	IWL_RATE_9M_MASK   (1<<IWL_RATE_9M_INDEX)
+#define	IWL_RATE_12M_MASK  (1<<IWL_RATE_12M_INDEX)
+#define	IWL_RATE_18M_MASK  (1<<IWL_RATE_18M_INDEX)
+#define	IWL_RATE_24M_MASK  (1<<IWL_RATE_24M_INDEX)
+#define	IWL_RATE_36M_MASK  (1<<IWL_RATE_36M_INDEX)
+#define	IWL_RATE_48M_MASK  (1<<IWL_RATE_48M_INDEX)
+#define	IWL_RATE_54M_MASK  (1<<IWL_RATE_54M_INDEX)
+#define IWL_RATE_60M_MASK  (1<<IWL_RATE_60M_INDEX)
+#define	IWL_RATE_1M_MASK   (1<<IWL_RATE_1M_INDEX)
+#define	IWL_RATE_2M_MASK   (1<<IWL_RATE_2M_INDEX)
+#define	IWL_RATE_5M_MASK   (1<<IWL_RATE_5M_INDEX)
+#define	IWL_RATE_11M_MASK  (1<<IWL_RATE_11M_INDEX)
+
+enum {
+	IWL_RATE_6M_PLCP  = 13,
+	IWL_RATE_9M_PLCP  = 15,
+	IWL_RATE_12M_PLCP = 5,
+	IWL_RATE_18M_PLCP = 7,
+	IWL_RATE_24M_PLCP = 9,
+	IWL_RATE_36M_PLCP = 11,
+	IWL_RATE_48M_PLCP = 1,
+	IWL_RATE_54M_PLCP = 3,
+	IWL_RATE_60M_PLCP = 3,
+	IWL_RATE_1M_PLCP  = 10,
+	IWL_RATE_2M_PLCP  = 20,
+	IWL_RATE_5M_PLCP  = 55,
+	IWL_RATE_11M_PLCP = 110,
+};
+
+/* OFDM HT rate plcp */
+enum {
+	IWL_RATE_SISO_6M_PLCP = 0,
+	IWL_RATE_SISO_12M_PLCP = 1,
+	IWL_RATE_SISO_18M_PLCP = 2,
+	IWL_RATE_SISO_24M_PLCP = 3,
+	IWL_RATE_SISO_36M_PLCP = 4,
+	IWL_RATE_SISO_48M_PLCP = 5,
+	IWL_RATE_SISO_54M_PLCP = 6,
+	IWL_RATE_SISO_60M_PLCP = 7,
+	IWL_RATE_MIMO_6M_PLCP  = 0x8,
+	IWL_RATE_MIMO_12M_PLCP = 0x9,
+	IWL_RATE_MIMO_18M_PLCP = 0xa,
+	IWL_RATE_MIMO_24M_PLCP = 0xb,
+	IWL_RATE_MIMO_36M_PLCP = 0xc,
+	IWL_RATE_MIMO_48M_PLCP = 0xd,
+	IWL_RATE_MIMO_54M_PLCP = 0xe,
+	IWL_RATE_MIMO_60M_PLCP = 0xf,
+	IWL_RATE_SISO_INVM_PLCP,
+	IWL_RATE_MIMO_INVM_PLCP = IWL_RATE_SISO_INVM_PLCP,
+};
+
+enum {
+	IWL_RATE_6M_IEEE  = 12,
+	IWL_RATE_9M_IEEE  = 18,
+	IWL_RATE_12M_IEEE = 24,
+	IWL_RATE_18M_IEEE = 36,
+	IWL_RATE_24M_IEEE = 48,
+	IWL_RATE_36M_IEEE = 72,
+	IWL_RATE_48M_IEEE = 96,
+	IWL_RATE_54M_IEEE = 108,
+	IWL_RATE_60M_IEEE = 120,
+	IWL_RATE_1M_IEEE  = 2,
+	IWL_RATE_2M_IEEE  = 4,
+	IWL_RATE_5M_IEEE  = 11,
+	IWL_RATE_11M_IEEE = 22,
+};
+
+#define IWL_CCK_BASIC_RATES_MASK    \
+       (IWL_RATE_1M_MASK          | \
+	IWL_RATE_2M_MASK)
+
+#define IWL_CCK_RATES_MASK          \
+       (IWL_BASIC_RATES_MASK      | \
+	IWL_RATE_5M_MASK          | \
+	IWL_RATE_11M_MASK)
+
+#define IWL_OFDM_BASIC_RATES_MASK   \
+	(IWL_RATE_6M_MASK         | \
+	IWL_RATE_12M_MASK         | \
+	IWL_RATE_24M_MASK)
+
+#define IWL_OFDM_RATES_MASK         \
+       (IWL_OFDM_BASIC_RATES_MASK | \
+	IWL_RATE_9M_MASK          | \
+	IWL_RATE_18M_MASK         | \
+	IWL_RATE_36M_MASK         | \
+	IWL_RATE_48M_MASK         | \
+	IWL_RATE_54M_MASK)
+
+#define IWL_BASIC_RATES_MASK         \
+	(IWL_OFDM_BASIC_RATES_MASK | \
+	 IWL_CCK_BASIC_RATES_MASK)
+
+#define IWL_RATES_MASK ((1<<IWL_RATE_COUNT)-1)
+
+#define IWL_INVALID_VALUE    -1
+
+#define IWL_MIN_RSSI_VAL                 -100
+#define IWL_MAX_RSSI_VAL                    0
+
+#define IWL_LEGACY_SWITCH_ANTENNA	0
+#define IWL_LEGACY_SWITCH_SISO		1
+#define IWL_LEGACY_SWITCH_MIMO	        2
+
+#define IWL_RS_GOOD_RATIO		12800
+
+#define IWL_ACTION_LIMIT		3
+#define IWL_LEGACY_FAILURE_LIMIT	160
+#define IWL_LEGACY_SUCCESS_LIMIT	480
+#define IWL_LEGACY_TABLE_COUNT		160
+
+#define IWL_NONE_LEGACY_FAILURE_LIMIT	400
+#define IWL_NONE_LEGACY_SUCCESS_LIMIT	4500
+#define IWL_NONE_LEGACY_TABLE_COUNT	1500
+
+#define IWL_RATE_SCALE_SWITCH		(10880)
+
+#define IWL_SISO_SWITCH_ANTENNA		0
+#define IWL_SISO_SWITCH_MIMO		1
+#define IWL_SISO_SWITCH_GI		2
+
+#define IWL_MIMO_SWITCH_ANTENNA_A	0
+#define IWL_MIMO_SWITCH_ANTENNA_B	1
+#define IWL_MIMO_SWITCH_GI		2
+
+#define LQ_SIZE		2
+
+extern const struct iwl_rate_info iwl_rates[IWL_RATE_COUNT];
+
+enum iwl_table_type {
+	LQ_NONE,
+	LQ_G,
+	LQ_A,
+	LQ_SISO,
+	LQ_MIMO,
+	LQ_MAX,
+};
+
+enum iwl_antenna_type {
+	ANT_NONE,
+	ANT_MAIN,
+	ANT_AUX,
+	ANT_BOTH,
+};
+
+static inline u8 iwl_get_prev_ieee_rate(u8 rate_index)
+{
+	u8 rate = iwl_rates[rate_index].prev_ieee;
+
+	if (rate == IWL_RATE_INVALID)
+		rate = rate_index;
+	return rate;
+}
+
+extern int iwl_rate_index_from_plcp(int plcp);
+
+/**
+ * iwl_fill_rs_info - Fill an output text buffer with the rate representation
+ *
+ * NOTE:  This is provided as a quick mechanism for a user to visualize
+ * the performance of the rate control alogirthm and is not meant to be
+ * parsed software.
+ */
+extern int iwl_fill_rs_info(struct ieee80211_hw *, char *buf, u8 sta_id);
+
+/**
+ * iwl_rate_scale_init - Initialize the rate scale table based on assoc info
+ *
+ * The specific througput table used is based on the type of network
+ * the associated with, including A, B, G, and G w/ TGG protection
+ */
+extern void iwl_rate_scale_init(struct ieee80211_hw *hw, s32 sta_id);
+
+/**
+ * iwl_rate_control_register - Register the rate control algorithm callbacks
+ *
+ * Since the rate control algorithm is hardware specific, there is no need
+ * or reason to place it as a stand alone module.  The driver can call
+ * iwl_rate_control_register in order to register the rate control callbacks
+ * with the mac80211 subsystem.  This should be performed prior to calling
+ * ieee80211_register_hw
+ *
+ */
+extern void iwl_rate_control_register(struct ieee80211_hw *hw);
+
+/**
+ * iwl_rate_control_unregister - Unregister the rate control callbacks
+ *
+ * This should be called after calling ieee80211_unregister_hw, but before
+ * the driver is unloaded.
+ */
+extern void iwl_rate_control_unregister(struct ieee80211_hw *hw);
+
+#endif
diff --git a/drivers/net/wireless/iwlwifi/iwl-4965.c b/drivers/net/wireless/iwlwifi/iwl-4965.c
new file mode 100644
index 000000000000..b50d20267c8a
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-4965.c
@@ -0,0 +1,4736 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2003 - 2007 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * James P. Ketrenos <ipw2100-admin@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/version.h>
+#include <linux/init.h>
+#include <linux/pci.h>
+#include <linux/dma-mapping.h>
+#include <linux/delay.h>
+#include <linux/skbuff.h>
+#include <linux/netdevice.h>
+#include <linux/wireless.h>
+#include <net/mac80211.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/delay.h>
+
+#define IWL 4965
+
+#include "iwlwifi.h"
+#include "iwl-4965.h"
+#include "iwl-helpers.h"
+
+#define IWL_DECLARE_RATE_INFO(r, s, ip, in, rp, rn, pp, np)    \
+	[IWL_RATE_##r##M_INDEX] = { IWL_RATE_##r##M_PLCP,      \
+				    IWL_RATE_SISO_##s##M_PLCP, \
+				    IWL_RATE_MIMO_##s##M_PLCP, \
+				    IWL_RATE_##r##M_IEEE,      \
+				    IWL_RATE_##ip##M_INDEX,    \
+				    IWL_RATE_##in##M_INDEX,    \
+				    IWL_RATE_##rp##M_INDEX,    \
+				    IWL_RATE_##rn##M_INDEX,    \
+				    IWL_RATE_##pp##M_INDEX,    \
+				    IWL_RATE_##np##M_INDEX }
+
+/*
+ * Parameter order:
+ *   rate, ht rate, prev rate, next rate, prev tgg rate, next tgg rate
+ *
+ * If there isn't a valid next or previous rate then INV is used which
+ * maps to IWL_RATE_INVALID
+ *
+ */
+const struct iwl_rate_info iwl_rates[IWL_RATE_COUNT] = {
+	IWL_DECLARE_RATE_INFO(1, INV, INV, 2, INV, 2, INV, 2),    /*  1mbps */
+	IWL_DECLARE_RATE_INFO(2, INV, 1, 5, 1, 5, 1, 5),          /*  2mbps */
+	IWL_DECLARE_RATE_INFO(5, INV, 2, 6, 2, 11, 2, 11),        /*5.5mbps */
+	IWL_DECLARE_RATE_INFO(11, INV, 9, 12, 9, 12, 5, 18),      /* 11mbps */
+	IWL_DECLARE_RATE_INFO(6, 6, 5, 9, 5, 11, 5, 11),        /*  6mbps */
+	IWL_DECLARE_RATE_INFO(9, 6, 6, 11, 6, 11, 5, 11),       /*  9mbps */
+	IWL_DECLARE_RATE_INFO(12, 12, 11, 18, 11, 18, 11, 18),   /* 12mbps */
+	IWL_DECLARE_RATE_INFO(18, 18, 12, 24, 12, 24, 11, 24),   /* 18mbps */
+	IWL_DECLARE_RATE_INFO(24, 24, 18, 36, 18, 36, 18, 36),   /* 24mbps */
+	IWL_DECLARE_RATE_INFO(36, 36, 24, 48, 24, 48, 24, 48),   /* 36mbps */
+	IWL_DECLARE_RATE_INFO(48, 48, 36, 54, 36, 54, 36, 54),   /* 48mbps */
+	IWL_DECLARE_RATE_INFO(54, 54, 48, INV, 48, INV, 48, INV),/* 54mbps */
+	IWL_DECLARE_RATE_INFO(60, 60, 48, INV, 48, INV, 48, INV),/* 60mbps */
+};
+
+static int is_fat_channel(__le32 rxon_flags)
+{
+	return (rxon_flags & RXON_FLG_CHANNEL_MODE_PURE_40_MSK) ||
+		(rxon_flags & RXON_FLG_CHANNEL_MODE_MIXED_MSK);
+}
+
+static u8 is_single_stream(struct iwl_priv *priv)
+{
+#ifdef CONFIG_IWLWIFI_HT
+	if (!priv->is_ht_enabled || !priv->current_assoc_ht.is_ht ||
+	    (priv->active_rate_ht[1] == 0) ||
+	    (priv->ps_mode == IWL_MIMO_PS_STATIC))
+		return 1;
+#else
+	return 1;
+#endif	/*CONFIG_IWLWIFI_HT */
+	return 0;
+}
+
+/*
+ * Determine how many receiver/antenna chains to use.
+ * More provides better reception via diversity.  Fewer saves power.
+ * MIMO (dual stream) requires at least 2, but works better with 3.
+ * This does not determine *which* chains to use, just how many.
+ */
+static int iwl4965_get_rx_chain_counter(struct iwl_priv *priv,
+					u8 *idle_state, u8 *rx_state)
+{
+	u8 is_single = is_single_stream(priv);
+	u8 is_cam = test_bit(STATUS_POWER_PMI, &priv->status) ? 0 : 1;
+
+	/* # of Rx chains to use when expecting MIMO. */
+	if (is_single || (!is_cam && (priv->ps_mode == IWL_MIMO_PS_STATIC)))
+		*rx_state = 2;
+	else
+		*rx_state = 3;
+
+	/* # Rx chains when idling and maybe trying to save power */
+	switch (priv->ps_mode) {
+	case IWL_MIMO_PS_STATIC:
+	case IWL_MIMO_PS_DYNAMIC:
+		*idle_state = (is_cam) ? 2 : 1;
+		break;
+	case IWL_MIMO_PS_NONE:
+		*idle_state = (is_cam) ? *rx_state : 1;
+		break;
+	default:
+		*idle_state = 1;
+		break;
+	}
+
+	return 0;
+}
+
+int iwl_hw_rxq_stop(struct iwl_priv *priv)
+{
+	int rc;
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	rc = iwl_grab_restricted_access(priv);
+	if (rc) {
+		spin_unlock_irqrestore(&priv->lock, flags);
+		return rc;
+	}
+
+	/* stop HW */
+	iwl_write_restricted(priv, FH_MEM_RCSR_CHNL0_CONFIG_REG, 0);
+	rc = iwl_poll_restricted_bit(priv, FH_MEM_RSSR_RX_STATUS_REG,
+				     (1 << 24), 1000);
+	if (rc < 0)
+		IWL_ERROR("Can't stop Rx DMA.\n");
+
+	iwl_release_restricted_access(priv);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return 0;
+}
+
+u8 iwl_hw_find_station(struct iwl_priv *priv, const u8 *addr)
+{
+	int i;
+	int start = 0;
+	int ret = IWL_INVALID_STATION;
+	unsigned long flags;
+	DECLARE_MAC_BUF(mac);
+
+	if ((priv->iw_mode == IEEE80211_IF_TYPE_IBSS) ||
+	    (priv->iw_mode == IEEE80211_IF_TYPE_AP))
+		start = IWL_STA_ID;
+
+	if (is_broadcast_ether_addr(addr))
+		return IWL4965_BROADCAST_ID;
+
+	spin_lock_irqsave(&priv->sta_lock, flags);
+	for (i = start; i < priv->hw_setting.max_stations; i++)
+		if ((priv->stations[i].used) &&
+		    (!compare_ether_addr
+		     (priv->stations[i].sta.sta.addr, addr))) {
+			ret = i;
+			goto out;
+		}
+
+	IWL_DEBUG_ASSOC_LIMIT("can not find STA %s total %d\n",
+			print_mac(mac, addr), priv->num_stations);
+
+ out:
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+	return ret;
+}
+
+static int iwl4965_nic_set_pwr_src(struct iwl_priv *priv, int pwr_max)
+{
+	int rc = 0;
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	rc = iwl_grab_restricted_access(priv);
+	if (rc) {
+		spin_unlock_irqrestore(&priv->lock, flags);
+		return rc;
+	}
+
+	if (!pwr_max) {
+		u32 val;
+
+		rc = pci_read_config_dword(priv->pci_dev, PCI_POWER_SOURCE,
+					   &val);
+
+		if (val & PCI_CFG_PMC_PME_FROM_D3COLD_SUPPORT)
+			iwl_set_bits_mask_restricted_reg(
+				priv, APMG_PS_CTRL_REG,
+				APMG_PS_CTRL_VAL_PWR_SRC_VAUX,
+				~APMG_PS_CTRL_MSK_PWR_SRC);
+	} else
+		iwl_set_bits_mask_restricted_reg(
+			priv, APMG_PS_CTRL_REG,
+			APMG_PS_CTRL_VAL_PWR_SRC_VMAIN,
+			~APMG_PS_CTRL_MSK_PWR_SRC);
+
+	iwl_release_restricted_access(priv);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return rc;
+}
+
+static int iwl4965_rx_init(struct iwl_priv *priv, struct iwl_rx_queue *rxq)
+{
+	int rc;
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	rc = iwl_grab_restricted_access(priv);
+	if (rc) {
+		spin_unlock_irqrestore(&priv->lock, flags);
+		return rc;
+	}
+
+	/* stop HW */
+	iwl_write_restricted(priv, FH_MEM_RCSR_CHNL0_CONFIG_REG, 0);
+
+	iwl_write_restricted(priv, FH_RSCSR_CHNL0_RBDCB_WPTR_REG, 0);
+	iwl_write_restricted(priv, FH_RSCSR_CHNL0_RBDCB_BASE_REG,
+			     rxq->dma_addr >> 8);
+
+	iwl_write_restricted(priv, FH_RSCSR_CHNL0_STTS_WPTR_REG,
+			     (priv->hw_setting.shared_phys +
+			      offsetof(struct iwl_shared, val0)) >> 4);
+
+	iwl_write_restricted(priv, FH_MEM_RCSR_CHNL0_CONFIG_REG,
+			     FH_RCSR_RX_CONFIG_CHNL_EN_ENABLE_VAL |
+			     FH_RCSR_CHNL0_RX_CONFIG_IRQ_DEST_INT_HOST_VAL |
+			     IWL_FH_RCSR_RX_CONFIG_REG_VAL_RB_SIZE_4K |
+			     /*0x10 << 4 | */
+			     (RX_QUEUE_SIZE_LOG <<
+			      FH_RCSR_RX_CONFIG_RBDCB_SIZE_BITSHIFT));
+
+	/*
+	 * iwl_write32(priv,CSR_INT_COAL_REG,0);
+	 */
+
+	iwl_release_restricted_access(priv);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return 0;
+}
+
+static int iwl4965_kw_init(struct iwl_priv *priv)
+{
+	unsigned long flags;
+	int rc;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	rc = iwl_grab_restricted_access(priv);
+	if (rc)
+		goto out;
+
+	iwl_write_restricted(priv, IWL_FH_KW_MEM_ADDR_REG,
+			     priv->kw.dma_addr >> 4);
+	iwl_release_restricted_access(priv);
+out:
+	spin_unlock_irqrestore(&priv->lock, flags);
+	return rc;
+}
+
+static int iwl4965_kw_alloc(struct iwl_priv *priv)
+{
+	struct pci_dev *dev = priv->pci_dev;
+	struct iwl_kw *kw = &priv->kw;
+
+	kw->size = IWL4965_KW_SIZE;	/* TBW need set somewhere else */
+	kw->v_addr = pci_alloc_consistent(dev, kw->size, &kw->dma_addr);
+	if (!kw->v_addr)
+		return -ENOMEM;
+
+	return 0;
+}
+
+#define CHECK_AND_PRINT(x) ((eeprom_ch->flags & EEPROM_CHANNEL_##x) \
+			    ? # x " " : "")
+
+int iwl4965_set_fat_chan_info(struct iwl_priv *priv, int phymode, u16 channel,
+			      const struct iwl_eeprom_channel *eeprom_ch,
+			      u8 fat_extension_channel)
+{
+	struct iwl_channel_info *ch_info;
+
+	ch_info = (struct iwl_channel_info *)
+			iwl_get_channel_info(priv, phymode, channel);
+
+	if (!is_channel_valid(ch_info))
+		return -1;
+
+	IWL_DEBUG_INFO("FAT Ch. %d [%sGHz] %s%s%s%s%s%s(0x%02x"
+			" %ddBm): Ad-Hoc %ssupported\n",
+			ch_info->channel,
+			is_channel_a_band(ch_info) ?
+			"5.2" : "2.4",
+			CHECK_AND_PRINT(IBSS),
+			CHECK_AND_PRINT(ACTIVE),
+			CHECK_AND_PRINT(RADAR),
+			CHECK_AND_PRINT(WIDE),
+			CHECK_AND_PRINT(NARROW),
+			CHECK_AND_PRINT(DFS),
+			eeprom_ch->flags,
+			eeprom_ch->max_power_avg,
+			((eeprom_ch->flags & EEPROM_CHANNEL_IBSS)
+			 && !(eeprom_ch->flags & EEPROM_CHANNEL_RADAR)) ?
+			"" : "not ");
+
+	ch_info->fat_eeprom = *eeprom_ch;
+	ch_info->fat_max_power_avg = eeprom_ch->max_power_avg;
+	ch_info->fat_curr_txpow = eeprom_ch->max_power_avg;
+	ch_info->fat_min_power = 0;
+	ch_info->fat_scan_power = eeprom_ch->max_power_avg;
+	ch_info->fat_flags = eeprom_ch->flags;
+	ch_info->fat_extension_channel = fat_extension_channel;
+
+	return 0;
+}
+
+static void iwl4965_kw_free(struct iwl_priv *priv)
+{
+	struct pci_dev *dev = priv->pci_dev;
+	struct iwl_kw *kw = &priv->kw;
+
+	if (kw->v_addr) {
+		pci_free_consistent(dev, kw->size, kw->v_addr, kw->dma_addr);
+		memset(kw, 0, sizeof(*kw));
+	}
+}
+
+/**
+ * iwl4965_txq_ctx_reset - Reset TX queue context
+ * Destroys all DMA structures and initialise them again
+ *
+ * @param priv
+ * @return error code
+ */
+static int iwl4965_txq_ctx_reset(struct iwl_priv *priv)
+{
+	int rc = 0;
+	int txq_id, slots_num;
+	unsigned long flags;
+
+	iwl4965_kw_free(priv);
+
+	iwl_hw_txq_ctx_free(priv);
+
+	/* Tx CMD queue */
+	rc = iwl4965_kw_alloc(priv);
+	if (rc) {
+		IWL_ERROR("Keep Warm allocation failed");
+		goto error_kw;
+	}
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	rc = iwl_grab_restricted_access(priv);
+	if (unlikely(rc)) {
+		IWL_ERROR("TX reset failed");
+		spin_unlock_irqrestore(&priv->lock, flags);
+		goto error_reset;
+	}
+
+	iwl_write_restricted_reg(priv, SCD_TXFACT, 0);
+	iwl_release_restricted_access(priv);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	rc = iwl4965_kw_init(priv);
+	if (rc) {
+		IWL_ERROR("kw_init failed\n");
+		goto error_reset;
+	}
+
+	/* Tx queue(s) */
+	for (txq_id = 0; txq_id < priv->hw_setting.max_txq_num; txq_id++) {
+		slots_num = (txq_id == IWL_CMD_QUEUE_NUM) ?
+					TFD_CMD_SLOTS : TFD_TX_CMD_SLOTS;
+		rc = iwl_tx_queue_init(priv, &priv->txq[txq_id], slots_num,
+				       txq_id);
+		if (rc) {
+			IWL_ERROR("Tx %d queue init failed\n", txq_id);
+			goto error;
+		}
+	}
+
+	return rc;
+
+ error:
+	iwl_hw_txq_ctx_free(priv);
+ error_reset:
+	iwl4965_kw_free(priv);
+ error_kw:
+	return rc;
+}
+
+int iwl_hw_nic_init(struct iwl_priv *priv)
+{
+	int rc;
+	unsigned long flags;
+	struct iwl_rx_queue *rxq = &priv->rxq;
+	u8 rev_id;
+	u32 val;
+	u8 val_link;
+
+	iwl_power_init_handle(priv);
+
+	/* nic_init */
+	spin_lock_irqsave(&priv->lock, flags);
+
+	iwl_set_bit(priv, CSR_GIO_CHICKEN_BITS,
+		    CSR_GIO_CHICKEN_BITS_REG_BIT_DIS_L0S_EXIT_TIMER);
+
+	iwl_set_bit(priv, CSR_GP_CNTRL, CSR_GP_CNTRL_REG_FLAG_INIT_DONE);
+	rc = iwl_poll_bit(priv, CSR_GP_CNTRL,
+			  CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY,
+			  CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY, 25000);
+	if (rc < 0) {
+		spin_unlock_irqrestore(&priv->lock, flags);
+		IWL_DEBUG_INFO("Failed to init the card\n");
+		return rc;
+	}
+
+	rc = iwl_grab_restricted_access(priv);
+	if (rc) {
+		spin_unlock_irqrestore(&priv->lock, flags);
+		return rc;
+	}
+
+	iwl_read_restricted_reg(priv, APMG_CLK_CTRL_REG);
+
+	iwl_write_restricted_reg(priv, APMG_CLK_CTRL_REG,
+				 APMG_CLK_VAL_DMA_CLK_RQT |
+				 APMG_CLK_VAL_BSM_CLK_RQT);
+	iwl_read_restricted_reg(priv, APMG_CLK_CTRL_REG);
+
+	udelay(20);
+
+	iwl_set_bits_restricted_reg(priv, APMG_PCIDEV_STT_REG,
+				    APMG_PCIDEV_STT_VAL_L1_ACT_DIS);
+
+	iwl_release_restricted_access(priv);
+	iwl_write32(priv, CSR_INT_COALESCING, 512 / 32);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	/* Determine HW type */
+	rc = pci_read_config_byte(priv->pci_dev, PCI_REVISION_ID, &rev_id);
+	if (rc)
+		return rc;
+
+	IWL_DEBUG_INFO("HW Revision ID = 0x%X\n", rev_id);
+
+	iwl4965_nic_set_pwr_src(priv, 1);
+	spin_lock_irqsave(&priv->lock, flags);
+
+	if ((rev_id & 0x80) == 0x80 && (rev_id & 0x7f) < 8) {
+		pci_read_config_dword(priv->pci_dev, PCI_REG_WUM8, &val);
+		/* Enable No Snoop field */
+		pci_write_config_dword(priv->pci_dev, PCI_REG_WUM8,
+				       val & ~(1 << 11));
+	}
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	/* Read the EEPROM */
+	rc = iwl_eeprom_init(priv);
+	if (rc)
+		return rc;
+
+	if (priv->eeprom.calib_version < EEPROM_TX_POWER_VERSION_NEW) {
+		IWL_ERROR("Older EEPROM detected!  Aborting.\n");
+		return -EINVAL;
+	}
+
+	pci_read_config_byte(priv->pci_dev, PCI_LINK_CTRL, &val_link);
+
+	/* disable L1 entry -- workaround for pre-B1 */
+	pci_write_config_byte(priv->pci_dev, PCI_LINK_CTRL, val_link & ~0x02);
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	/* set CSR_HW_CONFIG_REG for uCode use */
+
+	iwl_set_bit(priv, CSR_SW_VER, CSR_HW_IF_CONFIG_REG_BIT_KEDRON_R |
+		    CSR_HW_IF_CONFIG_REG_BIT_RADIO_SI |
+		    CSR_HW_IF_CONFIG_REG_BIT_MAC_SI);
+
+	rc = iwl_grab_restricted_access(priv);
+	if (rc < 0) {
+		spin_unlock_irqrestore(&priv->lock, flags);
+		IWL_DEBUG_INFO("Failed to init the card\n");
+		return rc;
+	}
+
+	iwl_read_restricted_reg(priv, APMG_PS_CTRL_REG);
+	iwl_set_bits_restricted_reg(priv, APMG_PS_CTRL_REG,
+				    APMG_PS_CTRL_VAL_RESET_REQ);
+	udelay(5);
+	iwl_clear_bits_restricted_reg(priv, APMG_PS_CTRL_REG,
+				      APMG_PS_CTRL_VAL_RESET_REQ);
+
+	iwl_release_restricted_access(priv);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	iwl_hw_card_show_info(priv);
+
+	/* end nic_init */
+
+	/* Allocate the RX queue, or reset if it is already allocated */
+	if (!rxq->bd) {
+		rc = iwl_rx_queue_alloc(priv);
+		if (rc) {
+			IWL_ERROR("Unable to initialize Rx queue\n");
+			return -ENOMEM;
+		}
+	} else
+		iwl_rx_queue_reset(priv, rxq);
+
+	iwl_rx_replenish(priv);
+
+	iwl4965_rx_init(priv, rxq);
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	rxq->need_update = 1;
+	iwl_rx_queue_update_write_ptr(priv, rxq);
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+	rc = iwl4965_txq_ctx_reset(priv);
+	if (rc)
+		return rc;
+
+	if (priv->eeprom.sku_cap & EEPROM_SKU_CAP_SW_RF_KILL_ENABLE)
+		IWL_DEBUG_RF_KILL("SW RF KILL supported in EEPROM.\n");
+
+	if (priv->eeprom.sku_cap & EEPROM_SKU_CAP_HW_RF_KILL_ENABLE)
+		IWL_DEBUG_RF_KILL("HW RF KILL supported in EEPROM.\n");
+
+	set_bit(STATUS_INIT, &priv->status);
+
+	return 0;
+}
+
+int iwl_hw_nic_stop_master(struct iwl_priv *priv)
+{
+	int rc = 0;
+	u32 reg_val;
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	/* set stop master bit */
+	iwl_set_bit(priv, CSR_RESET, CSR_RESET_REG_FLAG_STOP_MASTER);
+
+	reg_val = iwl_read32(priv, CSR_GP_CNTRL);
+
+	if (CSR_GP_CNTRL_REG_FLAG_MAC_POWER_SAVE ==
+	    (reg_val & CSR_GP_CNTRL_REG_MSK_POWER_SAVE_TYPE))
+		IWL_DEBUG_INFO("Card in power save, master is already "
+			       "stopped\n");
+	else {
+		rc = iwl_poll_bit(priv, CSR_RESET,
+				  CSR_RESET_REG_FLAG_MASTER_DISABLED,
+				  CSR_RESET_REG_FLAG_MASTER_DISABLED, 100);
+		if (rc < 0) {
+			spin_unlock_irqrestore(&priv->lock, flags);
+			return rc;
+		}
+	}
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+	IWL_DEBUG_INFO("stop master\n");
+
+	return rc;
+}
+
+void iwl_hw_txq_ctx_stop(struct iwl_priv *priv)
+{
+
+	int txq_id;
+	unsigned long flags;
+
+	/* reset TFD queues */
+	for (txq_id = 0; txq_id < priv->hw_setting.max_txq_num; txq_id++) {
+		spin_lock_irqsave(&priv->lock, flags);
+		if (iwl_grab_restricted_access(priv)) {
+			spin_unlock_irqrestore(&priv->lock, flags);
+			continue;
+		}
+
+		iwl_write_restricted(priv,
+				     IWL_FH_TCSR_CHNL_TX_CONFIG_REG(txq_id),
+				     0x0);
+		iwl_poll_restricted_bit(priv, IWL_FH_TSSR_TX_STATUS_REG,
+					IWL_FH_TSSR_TX_STATUS_REG_MSK_CHNL_IDLE
+					(txq_id), 200);
+		iwl_release_restricted_access(priv);
+		spin_unlock_irqrestore(&priv->lock, flags);
+	}
+
+	iwl_hw_txq_ctx_free(priv);
+}
+
+int iwl_hw_nic_reset(struct iwl_priv *priv)
+{
+	int rc = 0;
+	unsigned long flags;
+
+	iwl_hw_nic_stop_master(priv);
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	iwl_set_bit(priv, CSR_RESET, CSR_RESET_REG_FLAG_SW_RESET);
+
+	udelay(10);
+
+	iwl_set_bit(priv, CSR_GP_CNTRL, CSR_GP_CNTRL_REG_FLAG_INIT_DONE);
+	rc = iwl_poll_bit(priv, CSR_RESET,
+			  CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY,
+			  CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY, 25);
+
+	udelay(10);
+
+	rc = iwl_grab_restricted_access(priv);
+	if (!rc) {
+		iwl_write_restricted_reg(priv, APMG_CLK_EN_REG,
+					 APMG_CLK_VAL_DMA_CLK_RQT |
+					 APMG_CLK_VAL_BSM_CLK_RQT);
+
+		udelay(10);
+
+		iwl_set_bits_restricted_reg(priv, APMG_PCIDEV_STT_REG,
+				APMG_PCIDEV_STT_VAL_L1_ACT_DIS);
+
+		iwl_release_restricted_access(priv);
+	}
+
+	clear_bit(STATUS_HCMD_ACTIVE, &priv->status);
+	wake_up_interruptible(&priv->wait_command_queue);
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return rc;
+
+}
+
+#define REG_RECALIB_PERIOD (60)
+
+/**
+ * iwl4965_bg_statistics_periodic - Timer callback to queue statistics
+ *
+ * This callback is provided in order to queue the statistics_work
+ * in work_queue context (v. softirq)
+ *
+ * This timer function is continually reset to execute within
+ * REG_RECALIB_PERIOD seconds since the last STATISTICS_NOTIFICATION
+ * was received.  We need to ensure we receive the statistics in order
+ * to update the temperature used for calibrating the TXPOWER.  However,
+ * we can't send the statistics command from softirq context (which
+ * is the context which timers run at) so we have to queue off the
+ * statistics_work to actually send the command to the hardware.
+ */
+static void iwl4965_bg_statistics_periodic(unsigned long data)
+{
+	struct iwl_priv *priv = (struct iwl_priv *)data;
+
+	queue_work(priv->workqueue, &priv->statistics_work);
+}
+
+/**
+ * iwl4965_bg_statistics_work - Send the statistics request to the hardware.
+ *
+ * This is queued by iwl_bg_statistics_periodic.
+ */
+static void iwl4965_bg_statistics_work(struct work_struct *work)
+{
+	struct iwl_priv *priv = container_of(work, struct iwl_priv,
+					     statistics_work);
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+		return;
+
+	mutex_lock(&priv->mutex);
+	iwl_send_statistics_request(priv);
+	mutex_unlock(&priv->mutex);
+}
+
+#define CT_LIMIT_CONST		259
+#define TM_CT_KILL_THRESHOLD	110
+
+void iwl4965_rf_kill_ct_config(struct iwl_priv *priv)
+{
+	struct iwl_ct_kill_config cmd;
+	u32 R1, R2, R3;
+	u32 temp_th;
+	u32 crit_temperature;
+	unsigned long flags;
+	int rc = 0;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	iwl_write32(priv, CSR_UCODE_DRV_GP1_CLR,
+		    CSR_UCODE_DRV_GP1_REG_BIT_CT_KILL_EXIT);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	if (priv->statistics.flag & STATISTICS_REPLY_FLG_FAT_MODE_MSK) {
+		R1 = (s32)le32_to_cpu(priv->card_alive_init.therm_r1[1]);
+		R2 = (s32)le32_to_cpu(priv->card_alive_init.therm_r2[1]);
+		R3 = (s32)le32_to_cpu(priv->card_alive_init.therm_r3[1]);
+	} else {
+		R1 = (s32)le32_to_cpu(priv->card_alive_init.therm_r1[0]);
+		R2 = (s32)le32_to_cpu(priv->card_alive_init.therm_r2[0]);
+		R3 = (s32)le32_to_cpu(priv->card_alive_init.therm_r3[0]);
+	}
+
+	temp_th = CELSIUS_TO_KELVIN(TM_CT_KILL_THRESHOLD);
+
+	crit_temperature = ((temp_th * (R3-R1))/CT_LIMIT_CONST) + R2;
+	cmd.critical_temperature_R =  cpu_to_le32(crit_temperature);
+	rc = iwl_send_cmd_pdu(priv,
+			      REPLY_CT_KILL_CONFIG_CMD, sizeof(cmd), &cmd);
+	if (rc)
+		IWL_ERROR("REPLY_CT_KILL_CONFIG_CMD failed\n");
+	else
+		IWL_DEBUG_INFO("REPLY_CT_KILL_CONFIG_CMD succeeded\n");
+}
+
+#ifdef CONFIG_IWLWIFI_SENSITIVITY
+
+/* "false alarms" are signals that our DSP tries to lock onto,
+ *   but then determines that they are either noise, or transmissions
+ *   from a distant wireless network (also "noise", really) that get
+ *   "stepped on" by stronger transmissions within our own network.
+ * This algorithm attempts to set a sensitivity level that is high
+ *   enough to receive all of our own network traffic, but not so
+ *   high that our DSP gets too busy trying to lock onto non-network
+ *   activity/noise. */
+static int iwl4965_sens_energy_cck(struct iwl_priv *priv,
+				   u32 norm_fa,
+				   u32 rx_enable_time,
+				   struct statistics_general_data *rx_info)
+{
+	u32 max_nrg_cck = 0;
+	int i = 0;
+	u8 max_silence_rssi = 0;
+	u32 silence_ref = 0;
+	u8 silence_rssi_a = 0;
+	u8 silence_rssi_b = 0;
+	u8 silence_rssi_c = 0;
+	u32 val;
+
+	/* "false_alarms" values below are cross-multiplications to assess the
+	 *   numbers of false alarms within the measured period of actual Rx
+	 *   (Rx is off when we're txing), vs the min/max expected false alarms
+	 *   (some should be expected if rx is sensitive enough) in a
+	 *   hypothetical listening period of 200 time units (TU), 204.8 msec:
+	 *
+	 * MIN_FA/fixed-time < false_alarms/actual-rx-time < MAX_FA/beacon-time
+	 *
+	 * */
+	u32 false_alarms = norm_fa * 200 * 1024;
+	u32 max_false_alarms = MAX_FA_CCK * rx_enable_time;
+	u32 min_false_alarms = MIN_FA_CCK * rx_enable_time;
+	struct iwl_sensitivity_data *data = NULL;
+
+	data = &(priv->sensitivity_data);
+
+	data->nrg_auto_corr_silence_diff = 0;
+
+	/* Find max silence rssi among all 3 receivers.
+	 * This is background noise, which may include transmissions from other
+	 *    networks, measured during silence before our network's beacon */
+	silence_rssi_a = (u8)((rx_info->beacon_silence_rssi_a &
+			    ALL_BAND_FILTER)>>8);
+	silence_rssi_b = (u8)((rx_info->beacon_silence_rssi_b &
+			    ALL_BAND_FILTER)>>8);
+	silence_rssi_c = (u8)((rx_info->beacon_silence_rssi_c &
+			    ALL_BAND_FILTER)>>8);
+
+	val = max(silence_rssi_b, silence_rssi_c);
+	max_silence_rssi = max(silence_rssi_a, (u8) val);
+
+	/* Store silence rssi in 20-beacon history table */
+	data->nrg_silence_rssi[data->nrg_silence_idx] = max_silence_rssi;
+	data->nrg_silence_idx++;
+	if (data->nrg_silence_idx >= NRG_NUM_PREV_STAT_L)
+		data->nrg_silence_idx = 0;
+
+	/* Find max silence rssi across 20 beacon history */
+	for (i = 0; i < NRG_NUM_PREV_STAT_L; i++) {
+		val = data->nrg_silence_rssi[i];
+		silence_ref = max(silence_ref, val);
+	}
+	IWL_DEBUG_CALIB("silence a %u, b %u, c %u, 20-bcn max %u\n",
+			silence_rssi_a, silence_rssi_b, silence_rssi_c,
+			silence_ref);
+
+	/* Find max rx energy (min value!) among all 3 receivers,
+	 *   measured during beacon frame.
+	 * Save it in 10-beacon history table. */
+	i = data->nrg_energy_idx;
+	val = min(rx_info->beacon_energy_b, rx_info->beacon_energy_c);
+	data->nrg_value[i] = min(rx_info->beacon_energy_a, val);
+
+	data->nrg_energy_idx++;
+	if (data->nrg_energy_idx >= 10)
+		data->nrg_energy_idx = 0;
+
+	/* Find min rx energy (max value) across 10 beacon history.
+	 * This is the minimum signal level that we want to receive well.
+	 * Add backoff (margin so we don't miss slightly lower energy frames).
+	 * This establishes an upper bound (min value) for energy threshold. */
+	max_nrg_cck = data->nrg_value[0];
+	for (i = 1; i < 10; i++)
+		max_nrg_cck = (u32) max(max_nrg_cck, (data->nrg_value[i]));
+	max_nrg_cck += 6;
+
+	IWL_DEBUG_CALIB("rx energy a %u, b %u, c %u, 10-bcn max/min %u\n",
+			rx_info->beacon_energy_a, rx_info->beacon_energy_b,
+			rx_info->beacon_energy_c, max_nrg_cck - 6);
+
+	/* Count number of consecutive beacons with fewer-than-desired
+	 *   false alarms. */
+	if (false_alarms < min_false_alarms)
+		data->num_in_cck_no_fa++;
+	else
+		data->num_in_cck_no_fa = 0;
+	IWL_DEBUG_CALIB("consecutive bcns with few false alarms = %u\n",
+			data->num_in_cck_no_fa);
+
+	/* If we got too many false alarms this time, reduce sensitivity */
+	if (false_alarms > max_false_alarms) {
+		IWL_DEBUG_CALIB("norm FA %u > max FA %u\n",
+			     false_alarms, max_false_alarms);
+		IWL_DEBUG_CALIB("... reducing sensitivity\n");
+		data->nrg_curr_state = IWL_FA_TOO_MANY;
+
+		if (data->auto_corr_cck > AUTO_CORR_MAX_TH_CCK) {
+			/* Store for "fewer than desired" on later beacon */
+			data->nrg_silence_ref = silence_ref;
+
+			/* increase energy threshold (reduce nrg value)
+			 *   to decrease sensitivity */
+			if (data->nrg_th_cck > (NRG_MAX_CCK + NRG_STEP_CCK))
+				data->nrg_th_cck = data->nrg_th_cck
+							 - NRG_STEP_CCK;
+		}
+
+		/* increase auto_corr values to decrease sensitivity */
+		if (data->auto_corr_cck < AUTO_CORR_MAX_TH_CCK)
+			data->auto_corr_cck = AUTO_CORR_MAX_TH_CCK + 1;
+		else {
+			val = data->auto_corr_cck + AUTO_CORR_STEP_CCK;
+			data->auto_corr_cck = min((u32)AUTO_CORR_MAX_CCK, val);
+		}
+		val = data->auto_corr_cck_mrc + AUTO_CORR_STEP_CCK;
+		data->auto_corr_cck_mrc = min((u32)AUTO_CORR_MAX_CCK_MRC, val);
+
+	/* Else if we got fewer than desired, increase sensitivity */
+	} else if (false_alarms < min_false_alarms) {
+		data->nrg_curr_state = IWL_FA_TOO_FEW;
+
+		/* Compare silence level with silence level for most recent
+		 *   healthy number or too many false alarms */
+		data->nrg_auto_corr_silence_diff = (s32)data->nrg_silence_ref -
+						   (s32)silence_ref;
+
+		IWL_DEBUG_CALIB("norm FA %u < min FA %u, silence diff %d\n",
+			 false_alarms, min_false_alarms,
+			 data->nrg_auto_corr_silence_diff);
+
+		/* Increase value to increase sensitivity, but only if:
+		 * 1a) previous beacon did *not* have *too many* false alarms
+		 * 1b) AND there's a significant difference in Rx levels
+		 *      from a previous beacon with too many, or healthy # FAs
+		 * OR 2) We've seen a lot of beacons (100) with too few
+		 *       false alarms */
+		if ((data->nrg_prev_state != IWL_FA_TOO_MANY) &&
+			((data->nrg_auto_corr_silence_diff > NRG_DIFF) ||
+			(data->num_in_cck_no_fa > MAX_NUMBER_CCK_NO_FA))) {
+
+			IWL_DEBUG_CALIB("... increasing sensitivity\n");
+			/* Increase nrg value to increase sensitivity */
+			val = data->nrg_th_cck + NRG_STEP_CCK;
+			data->nrg_th_cck = min((u32)NRG_MIN_CCK, val);
+
+			/* Decrease auto_corr values to increase sensitivity */
+			val = data->auto_corr_cck - AUTO_CORR_STEP_CCK;
+			data->auto_corr_cck = max((u32)AUTO_CORR_MIN_CCK, val);
+
+			val = data->auto_corr_cck_mrc - AUTO_CORR_STEP_CCK;
+			data->auto_corr_cck_mrc =
+					 max((u32)AUTO_CORR_MIN_CCK_MRC, val);
+
+		} else
+			IWL_DEBUG_CALIB("... but not changing sensitivity\n");
+
+	/* Else we got a healthy number of false alarms, keep status quo */
+	} else {
+		IWL_DEBUG_CALIB(" FA in safe zone\n");
+		data->nrg_curr_state = IWL_FA_GOOD_RANGE;
+
+		/* Store for use in "fewer than desired" with later beacon */
+		data->nrg_silence_ref = silence_ref;
+
+		/* If previous beacon had too many false alarms,
+		 *   give it some extra margin by reducing sensitivity again
+		 *   (but don't go below measured energy of desired Rx) */
+		if (IWL_FA_TOO_MANY == data->nrg_prev_state) {
+			IWL_DEBUG_CALIB("... increasing margin\n");
+			data->nrg_th_cck -= NRG_MARGIN;
+		}
+	}
+
+	/* Make sure the energy threshold does not go above the measured
+	 * energy of the desired Rx signals (reduced by backoff margin),
+	 * or else we might start missing Rx frames.
+	 * Lower value is higher energy, so we use max()!
+	 */
+	data->nrg_th_cck = max(max_nrg_cck, data->nrg_th_cck);
+	IWL_DEBUG_CALIB("new nrg_th_cck %u\n", data->nrg_th_cck);
+
+	data->nrg_prev_state = data->nrg_curr_state;
+
+	return 0;
+}
+
+
+static int iwl4965_sens_auto_corr_ofdm(struct iwl_priv *priv,
+				       u32 norm_fa,
+				       u32 rx_enable_time)
+{
+	u32 val;
+	u32 false_alarms = norm_fa * 200 * 1024;
+	u32 max_false_alarms = MAX_FA_OFDM * rx_enable_time;
+	u32 min_false_alarms = MIN_FA_OFDM * rx_enable_time;
+	struct iwl_sensitivity_data *data = NULL;
+
+	data = &(priv->sensitivity_data);
+
+	/* If we got too many false alarms this time, reduce sensitivity */
+	if (false_alarms > max_false_alarms) {
+
+		IWL_DEBUG_CALIB("norm FA %u > max FA %u)\n",
+			     false_alarms, max_false_alarms);
+
+		val = data->auto_corr_ofdm + AUTO_CORR_STEP_OFDM;
+		data->auto_corr_ofdm =
+				min((u32)AUTO_CORR_MAX_OFDM, val);
+
+		val = data->auto_corr_ofdm_mrc + AUTO_CORR_STEP_OFDM;
+		data->auto_corr_ofdm_mrc =
+				min((u32)AUTO_CORR_MAX_OFDM_MRC, val);
+
+		val = data->auto_corr_ofdm_x1 + AUTO_CORR_STEP_OFDM;
+		data->auto_corr_ofdm_x1 =
+				min((u32)AUTO_CORR_MAX_OFDM_X1, val);
+
+		val = data->auto_corr_ofdm_mrc_x1 + AUTO_CORR_STEP_OFDM;
+		data->auto_corr_ofdm_mrc_x1 =
+				min((u32)AUTO_CORR_MAX_OFDM_MRC_X1, val);
+	}
+
+	/* Else if we got fewer than desired, increase sensitivity */
+	else if (false_alarms < min_false_alarms) {
+
+		IWL_DEBUG_CALIB("norm FA %u < min FA %u\n",
+			     false_alarms, min_false_alarms);
+
+		val = data->auto_corr_ofdm - AUTO_CORR_STEP_OFDM;
+		data->auto_corr_ofdm =
+				max((u32)AUTO_CORR_MIN_OFDM, val);
+
+		val = data->auto_corr_ofdm_mrc - AUTO_CORR_STEP_OFDM;
+		data->auto_corr_ofdm_mrc =
+				max((u32)AUTO_CORR_MIN_OFDM_MRC, val);
+
+		val = data->auto_corr_ofdm_x1 - AUTO_CORR_STEP_OFDM;
+		data->auto_corr_ofdm_x1 =
+				max((u32)AUTO_CORR_MIN_OFDM_X1, val);
+
+		val = data->auto_corr_ofdm_mrc_x1 - AUTO_CORR_STEP_OFDM;
+		data->auto_corr_ofdm_mrc_x1 =
+				max((u32)AUTO_CORR_MIN_OFDM_MRC_X1, val);
+	}
+
+	else
+		IWL_DEBUG_CALIB("min FA %u < norm FA %u < max FA %u OK\n",
+			 min_false_alarms, false_alarms, max_false_alarms);
+
+	return 0;
+}
+
+static int iwl_sensitivity_callback(struct iwl_priv *priv,
+				    struct iwl_cmd *cmd, struct sk_buff *skb)
+{
+	/* We didn't cache the SKB; let the caller free it */
+	return 1;
+}
+
+/* Prepare a SENSITIVITY_CMD, send to uCode if values have changed */
+static int iwl4965_sensitivity_write(struct iwl_priv *priv, u8 flags)
+{
+	int rc = 0;
+	struct iwl_sensitivity_cmd cmd ;
+	struct iwl_sensitivity_data *data = NULL;
+	struct iwl_host_cmd cmd_out = {
+		.id = SENSITIVITY_CMD,
+		.len = sizeof(struct iwl_sensitivity_cmd),
+		.meta.flags = flags,
+		.data = &cmd,
+	};
+
+	data = &(priv->sensitivity_data);
+
+	memset(&cmd, 0, sizeof(cmd));
+
+	cmd.table[HD_AUTO_CORR32_X4_TH_ADD_MIN_INDEX] =
+				cpu_to_le16((u16)data->auto_corr_ofdm);
+	cmd.table[HD_AUTO_CORR32_X4_TH_ADD_MIN_MRC_INDEX] =
+				cpu_to_le16((u16)data->auto_corr_ofdm_mrc);
+	cmd.table[HD_AUTO_CORR32_X1_TH_ADD_MIN_INDEX] =
+				cpu_to_le16((u16)data->auto_corr_ofdm_x1);
+	cmd.table[HD_AUTO_CORR32_X1_TH_ADD_MIN_MRC_INDEX] =
+				cpu_to_le16((u16)data->auto_corr_ofdm_mrc_x1);
+
+	cmd.table[HD_AUTO_CORR40_X4_TH_ADD_MIN_INDEX] =
+				cpu_to_le16((u16)data->auto_corr_cck);
+	cmd.table[HD_AUTO_CORR40_X4_TH_ADD_MIN_MRC_INDEX] =
+				cpu_to_le16((u16)data->auto_corr_cck_mrc);
+
+	cmd.table[HD_MIN_ENERGY_CCK_DET_INDEX] =
+				cpu_to_le16((u16)data->nrg_th_cck);
+	cmd.table[HD_MIN_ENERGY_OFDM_DET_INDEX] =
+				cpu_to_le16((u16)data->nrg_th_ofdm);
+
+	cmd.table[HD_BARKER_CORR_TH_ADD_MIN_INDEX] =
+				__constant_cpu_to_le16(190);
+	cmd.table[HD_BARKER_CORR_TH_ADD_MIN_MRC_INDEX] =
+				__constant_cpu_to_le16(390);
+	cmd.table[HD_OFDM_ENERGY_TH_IN_INDEX] =
+				__constant_cpu_to_le16(62);
+
+	IWL_DEBUG_CALIB("ofdm: ac %u mrc %u x1 %u mrc_x1 %u thresh %u\n",
+			data->auto_corr_ofdm, data->auto_corr_ofdm_mrc,
+			data->auto_corr_ofdm_x1, data->auto_corr_ofdm_mrc_x1,
+			data->nrg_th_ofdm);
+
+	IWL_DEBUG_CALIB("cck: ac %u mrc %u thresh %u\n",
+			data->auto_corr_cck, data->auto_corr_cck_mrc,
+			data->nrg_th_cck);
+
+	cmd.control = SENSITIVITY_CMD_CONTROL_WORK_TABLE;
+
+	if (flags & CMD_ASYNC)
+		cmd_out.meta.u.callback = iwl_sensitivity_callback;
+
+	/* Don't send command to uCode if nothing has changed */
+	if (!memcmp(&cmd.table[0], &(priv->sensitivity_tbl[0]),
+		    sizeof(u16)*HD_TABLE_SIZE)) {
+		IWL_DEBUG_CALIB("No change in SENSITIVITY_CMD\n");
+		return 0;
+	}
+
+	/* Copy table for comparison next time */
+	memcpy(&(priv->sensitivity_tbl[0]), &(cmd.table[0]),
+	       sizeof(u16)*HD_TABLE_SIZE);
+
+	rc = iwl_send_cmd(priv, &cmd_out);
+	if (!rc) {
+		IWL_DEBUG_CALIB("SENSITIVITY_CMD succeeded\n");
+		return rc;
+	}
+
+	return 0;
+}
+
+void iwl4965_init_sensitivity(struct iwl_priv *priv, u8 flags, u8 force)
+{
+	int rc = 0;
+	int i;
+	struct iwl_sensitivity_data *data = NULL;
+
+	IWL_DEBUG_CALIB("Start iwl4965_init_sensitivity\n");
+
+	if (force)
+		memset(&(priv->sensitivity_tbl[0]), 0,
+			sizeof(u16)*HD_TABLE_SIZE);
+
+	/* Clear driver's sensitivity algo data */
+	data = &(priv->sensitivity_data);
+	memset(data, 0, sizeof(struct iwl_sensitivity_data));
+
+	data->num_in_cck_no_fa = 0;
+	data->nrg_curr_state = IWL_FA_TOO_MANY;
+	data->nrg_prev_state = IWL_FA_TOO_MANY;
+	data->nrg_silence_ref = 0;
+	data->nrg_silence_idx = 0;
+	data->nrg_energy_idx = 0;
+
+	for (i = 0; i < 10; i++)
+		data->nrg_value[i] = 0;
+
+	for (i = 0; i < NRG_NUM_PREV_STAT_L; i++)
+		data->nrg_silence_rssi[i] = 0;
+
+	data->auto_corr_ofdm = 90;
+	data->auto_corr_ofdm_mrc = 170;
+	data->auto_corr_ofdm_x1  = 105;
+	data->auto_corr_ofdm_mrc_x1 = 220;
+	data->auto_corr_cck = AUTO_CORR_CCK_MIN_VAL_DEF;
+	data->auto_corr_cck_mrc = 200;
+	data->nrg_th_cck = 100;
+	data->nrg_th_ofdm = 100;
+
+	data->last_bad_plcp_cnt_ofdm = 0;
+	data->last_fa_cnt_ofdm = 0;
+	data->last_bad_plcp_cnt_cck = 0;
+	data->last_fa_cnt_cck = 0;
+
+	/* Clear prior Sensitivity command data to force send to uCode */
+	if (force)
+		memset(&(priv->sensitivity_tbl[0]), 0,
+		    sizeof(u16)*HD_TABLE_SIZE);
+
+	rc |= iwl4965_sensitivity_write(priv, flags);
+	IWL_DEBUG_CALIB("<<return 0x%X\n", rc);
+
+	return;
+}
+
+
+/* Reset differential Rx gains in NIC to prepare for chain noise calibration.
+ * Called after every association, but this runs only once!
+ *  ... once chain noise is calibrated the first time, it's good forever.  */
+void iwl4965_chain_noise_reset(struct iwl_priv *priv)
+{
+	struct iwl_chain_noise_data *data = NULL;
+	int rc = 0;
+
+	data = &(priv->chain_noise_data);
+	if ((data->state == IWL_CHAIN_NOISE_ALIVE) && iwl_is_associated(priv)) {
+		struct iwl_calibration_cmd cmd;
+
+		memset(&cmd, 0, sizeof(cmd));
+		cmd.opCode = PHY_CALIBRATE_DIFF_GAIN_CMD;
+		cmd.diff_gain_a = 0;
+		cmd.diff_gain_b = 0;
+		cmd.diff_gain_c = 0;
+		rc = iwl_send_cmd_pdu(priv, REPLY_PHY_CALIBRATION_CMD,
+				 sizeof(cmd), &cmd);
+		msleep(4);
+		data->state = IWL_CHAIN_NOISE_ACCUMULATE;
+		IWL_DEBUG_CALIB("Run chain_noise_calibrate\n");
+	}
+	return;
+}
+
+/*
+ * Accumulate 20 beacons of signal and noise statistics for each of
+ *   3 receivers/antennas/rx-chains, then figure out:
+ * 1)  Which antennas are connected.
+ * 2)  Differential rx gain settings to balance the 3 receivers.
+ */
+static void iwl4965_noise_calibration(struct iwl_priv *priv,
+				      struct iwl_notif_statistics *stat_resp)
+{
+	struct iwl_chain_noise_data *data = NULL;
+	int rc = 0;
+
+	u32 chain_noise_a;
+	u32 chain_noise_b;
+	u32 chain_noise_c;
+	u32 chain_sig_a;
+	u32 chain_sig_b;
+	u32 chain_sig_c;
+	u32 average_sig[NUM_RX_CHAINS] = {INITIALIZATION_VALUE};
+	u32 average_noise[NUM_RX_CHAINS] = {INITIALIZATION_VALUE};
+	u32 max_average_sig;
+	u16 max_average_sig_antenna_i;
+	u32 min_average_noise = MIN_AVERAGE_NOISE_MAX_VALUE;
+	u16 min_average_noise_antenna_i = INITIALIZATION_VALUE;
+	u16 i = 0;
+	u16 chan_num = INITIALIZATION_VALUE;
+	u32 band = INITIALIZATION_VALUE;
+	u32 active_chains = 0;
+	unsigned long flags;
+	struct statistics_rx_non_phy *rx_info = &(stat_resp->rx.general);
+
+	data = &(priv->chain_noise_data);
+
+	/* Accumulate just the first 20 beacons after the first association,
+	 *   then we're done forever. */
+	if (data->state != IWL_CHAIN_NOISE_ACCUMULATE) {
+		if (data->state == IWL_CHAIN_NOISE_ALIVE)
+			IWL_DEBUG_CALIB("Wait for noise calib reset\n");
+		return;
+	}
+
+	spin_lock_irqsave(&priv->lock, flags);
+	if (rx_info->interference_data_flag != INTERFERENCE_DATA_AVAILABLE) {
+		IWL_DEBUG_CALIB(" << Interference data unavailable\n");
+		spin_unlock_irqrestore(&priv->lock, flags);
+		return;
+	}
+
+	band = (priv->staging_rxon.flags & RXON_FLG_BAND_24G_MSK) ? 0 : 1;
+	chan_num = le16_to_cpu(priv->staging_rxon.channel);
+
+	/* Make sure we accumulate data for just the associated channel
+	 *   (even if scanning). */
+	if ((chan_num != (le32_to_cpu(stat_resp->flag) >> 16)) ||
+	    ((STATISTICS_REPLY_FLG_BAND_24G_MSK ==
+	     (stat_resp->flag & STATISTICS_REPLY_FLG_BAND_24G_MSK)) && band)) {
+		IWL_DEBUG_CALIB("Stats not from chan=%d, band=%d\n",
+				chan_num, band);
+		spin_unlock_irqrestore(&priv->lock, flags);
+		return;
+	}
+
+	/* Accumulate beacon statistics values across 20 beacons */
+	chain_noise_a = le32_to_cpu(rx_info->beacon_silence_rssi_a) &
+				IN_BAND_FILTER;
+	chain_noise_b = le32_to_cpu(rx_info->beacon_silence_rssi_b) &
+				IN_BAND_FILTER;
+	chain_noise_c = le32_to_cpu(rx_info->beacon_silence_rssi_c) &
+				IN_BAND_FILTER;
+
+	chain_sig_a = le32_to_cpu(rx_info->beacon_rssi_a) & IN_BAND_FILTER;
+	chain_sig_b = le32_to_cpu(rx_info->beacon_rssi_b) & IN_BAND_FILTER;
+	chain_sig_c = le32_to_cpu(rx_info->beacon_rssi_c) & IN_BAND_FILTER;
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	data->beacon_count++;
+
+	data->chain_noise_a = (chain_noise_a + data->chain_noise_a);
+	data->chain_noise_b = (chain_noise_b + data->chain_noise_b);
+	data->chain_noise_c = (chain_noise_c + data->chain_noise_c);
+
+	data->chain_signal_a = (chain_sig_a + data->chain_signal_a);
+	data->chain_signal_b = (chain_sig_b + data->chain_signal_b);
+	data->chain_signal_c = (chain_sig_c + data->chain_signal_c);
+
+	IWL_DEBUG_CALIB("chan=%d, band=%d, beacon=%d\n", chan_num, band,
+			data->beacon_count);
+	IWL_DEBUG_CALIB("chain_sig: a %d b %d c %d\n",
+			chain_sig_a, chain_sig_b, chain_sig_c);
+	IWL_DEBUG_CALIB("chain_noise: a %d b %d c %d\n",
+			chain_noise_a, chain_noise_b, chain_noise_c);
+
+	/* If this is the 20th beacon, determine:
+	 * 1)  Disconnected antennas (using signal strengths)
+	 * 2)  Differential gain (using silence noise) to balance receivers */
+	if (data->beacon_count == CAL_NUM_OF_BEACONS) {
+
+		/* Analyze signal for disconnected antenna */
+		average_sig[0] = (data->chain_signal_a) / CAL_NUM_OF_BEACONS;
+		average_sig[1] = (data->chain_signal_b) / CAL_NUM_OF_BEACONS;
+		average_sig[2] = (data->chain_signal_c) / CAL_NUM_OF_BEACONS;
+
+		if (average_sig[0] >= average_sig[1]) {
+			max_average_sig = average_sig[0];
+			max_average_sig_antenna_i = 0;
+			active_chains = (1 << max_average_sig_antenna_i);
+		} else {
+			max_average_sig = average_sig[1];
+			max_average_sig_antenna_i = 1;
+			active_chains = (1 << max_average_sig_antenna_i);
+		}
+
+		if (average_sig[2] >= max_average_sig) {
+			max_average_sig = average_sig[2];
+			max_average_sig_antenna_i = 2;
+			active_chains = (1 << max_average_sig_antenna_i);
+		}
+
+		IWL_DEBUG_CALIB("average_sig: a %d b %d c %d\n",
+			     average_sig[0], average_sig[1], average_sig[2]);
+		IWL_DEBUG_CALIB("max_average_sig = %d, antenna %d\n",
+			     max_average_sig, max_average_sig_antenna_i);
+
+		/* Compare signal strengths for all 3 receivers. */
+		for (i = 0; i < NUM_RX_CHAINS; i++) {
+			if (i != max_average_sig_antenna_i) {
+				s32 rssi_delta = (max_average_sig -
+						  average_sig[i]);
+
+				/* If signal is very weak, compared with
+				 * strongest, mark it as disconnected. */
+				if (rssi_delta > MAXIMUM_ALLOWED_PATHLOSS)
+					data->disconn_array[i] = 1;
+				else
+					active_chains |= (1 << i);
+			IWL_DEBUG_CALIB("i = %d  rssiDelta = %d  "
+				     "disconn_array[i] = %d\n",
+				     i, rssi_delta, data->disconn_array[i]);
+			}
+		}
+
+		/*If both chains A & B are disconnected -
+		 * connect B and leave A as is */
+		if (data->disconn_array[CHAIN_A] &&
+		    data->disconn_array[CHAIN_B]) {
+			data->disconn_array[CHAIN_B] = 0;
+			active_chains |= (1 << CHAIN_B);
+			IWL_DEBUG_CALIB("both A & B chains are disconnected! "
+				     "W/A - declare B as connected\n");
+		}
+
+		IWL_DEBUG_CALIB("active_chains (bitwise) = 0x%x\n",
+				active_chains);
+
+		/* Save for use within RXON, TX, SCAN commands, etc. */
+		priv->valid_antenna = active_chains;
+
+		/* Analyze noise for rx balance */
+		average_noise[0] = ((data->chain_noise_a)/CAL_NUM_OF_BEACONS);
+		average_noise[1] = ((data->chain_noise_b)/CAL_NUM_OF_BEACONS);
+		average_noise[2] = ((data->chain_noise_c)/CAL_NUM_OF_BEACONS);
+
+		for (i = 0; i < NUM_RX_CHAINS; i++) {
+			if (!(data->disconn_array[i]) &&
+			   (average_noise[i] <= min_average_noise)) {
+				/* This means that chain i is active and has
+				 * lower noise values so far: */
+				min_average_noise = average_noise[i];
+				min_average_noise_antenna_i = i;
+			}
+		}
+
+		data->delta_gain_code[min_average_noise_antenna_i] = 0;
+
+		IWL_DEBUG_CALIB("average_noise: a %d b %d c %d\n",
+				average_noise[0], average_noise[1],
+				average_noise[2]);
+
+		IWL_DEBUG_CALIB("min_average_noise = %d, antenna %d\n",
+				min_average_noise, min_average_noise_antenna_i);
+
+		for (i = 0; i < NUM_RX_CHAINS; i++) {
+			s32 delta_g = 0;
+
+			if (!(data->disconn_array[i]) &&
+			    (data->delta_gain_code[i] ==
+			     CHAIN_NOISE_DELTA_GAIN_INIT_VAL)) {
+				delta_g = average_noise[i] - min_average_noise;
+				data->delta_gain_code[i] = (u8)((delta_g *
+								    10) / 15);
+				if (CHAIN_NOISE_MAX_DELTA_GAIN_CODE <
+				   data->delta_gain_code[i])
+					data->delta_gain_code[i] =
+					  CHAIN_NOISE_MAX_DELTA_GAIN_CODE;
+
+				data->delta_gain_code[i] =
+					(data->delta_gain_code[i] | (1 << 2));
+			} else
+				data->delta_gain_code[i] = 0;
+		}
+		IWL_DEBUG_CALIB("delta_gain_codes: a %d b %d c %d\n",
+			     data->delta_gain_code[0],
+			     data->delta_gain_code[1],
+			     data->delta_gain_code[2]);
+
+		/* Differential gain gets sent to uCode only once */
+		if (!data->radio_write) {
+			struct iwl_calibration_cmd cmd;
+			data->radio_write = 1;
+
+			memset(&cmd, 0, sizeof(cmd));
+			cmd.opCode = PHY_CALIBRATE_DIFF_GAIN_CMD;
+			cmd.diff_gain_a = data->delta_gain_code[0];
+			cmd.diff_gain_b = data->delta_gain_code[1];
+			cmd.diff_gain_c = data->delta_gain_code[2];
+			rc = iwl_send_cmd_pdu(priv, REPLY_PHY_CALIBRATION_CMD,
+					      sizeof(cmd), &cmd);
+			if (rc)
+				IWL_DEBUG_CALIB("fail sending cmd "
+					     "REPLY_PHY_CALIBRATION_CMD \n");
+
+			/* TODO we might want recalculate
+			 * rx_chain in rxon cmd */
+
+			/* Mark so we run this algo only once! */
+			data->state = IWL_CHAIN_NOISE_CALIBRATED;
+		}
+		data->chain_noise_a = 0;
+		data->chain_noise_b = 0;
+		data->chain_noise_c = 0;
+		data->chain_signal_a = 0;
+		data->chain_signal_b = 0;
+		data->chain_signal_c = 0;
+		data->beacon_count = 0;
+	}
+	return;
+}
+
+static void iwl4965_sensitivity_calibration(struct iwl_priv *priv,
+					    struct iwl_notif_statistics *resp)
+{
+	int rc = 0;
+	u32 rx_enable_time;
+	u32 fa_cck;
+	u32 fa_ofdm;
+	u32 bad_plcp_cck;
+	u32 bad_plcp_ofdm;
+	u32 norm_fa_ofdm;
+	u32 norm_fa_cck;
+	struct iwl_sensitivity_data *data = NULL;
+	struct statistics_rx_non_phy *rx_info = &(resp->rx.general);
+	struct statistics_rx *statistics = &(resp->rx);
+	unsigned long flags;
+	struct statistics_general_data statis;
+
+	data = &(priv->sensitivity_data);
+
+	if (!iwl_is_associated(priv)) {
+		IWL_DEBUG_CALIB("<< - not associated\n");
+		return;
+	}
+
+	spin_lock_irqsave(&priv->lock, flags);
+	if (rx_info->interference_data_flag != INTERFERENCE_DATA_AVAILABLE) {
+		IWL_DEBUG_CALIB("<< invalid data.\n");
+		spin_unlock_irqrestore(&priv->lock, flags);
+		return;
+	}
+
+	/* Extract Statistics: */
+	rx_enable_time = le32_to_cpu(rx_info->channel_load);
+	fa_cck = le32_to_cpu(statistics->cck.false_alarm_cnt);
+	fa_ofdm = le32_to_cpu(statistics->ofdm.false_alarm_cnt);
+	bad_plcp_cck = le32_to_cpu(statistics->cck.plcp_err);
+	bad_plcp_ofdm = le32_to_cpu(statistics->ofdm.plcp_err);
+
+	statis.beacon_silence_rssi_a =
+			le32_to_cpu(statistics->general.beacon_silence_rssi_a);
+	statis.beacon_silence_rssi_b =
+			le32_to_cpu(statistics->general.beacon_silence_rssi_b);
+	statis.beacon_silence_rssi_c =
+			le32_to_cpu(statistics->general.beacon_silence_rssi_c);
+	statis.beacon_energy_a =
+			le32_to_cpu(statistics->general.beacon_energy_a);
+	statis.beacon_energy_b =
+			le32_to_cpu(statistics->general.beacon_energy_b);
+	statis.beacon_energy_c =
+			le32_to_cpu(statistics->general.beacon_energy_c);
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	IWL_DEBUG_CALIB("rx_enable_time = %u usecs\n", rx_enable_time);
+
+	if (!rx_enable_time) {
+		IWL_DEBUG_CALIB("<< RX Enable Time == 0! \n");
+		return;
+	}
+
+	/* These statistics increase monotonically, and do not reset
+	 *   at each beacon.  Calculate difference from last value, or just
+	 *   use the new statistics value if it has reset or wrapped around. */
+	if (data->last_bad_plcp_cnt_cck > bad_plcp_cck)
+		data->last_bad_plcp_cnt_cck = bad_plcp_cck;
+	else {
+		bad_plcp_cck -= data->last_bad_plcp_cnt_cck;
+		data->last_bad_plcp_cnt_cck += bad_plcp_cck;
+	}
+
+	if (data->last_bad_plcp_cnt_ofdm > bad_plcp_ofdm)
+		data->last_bad_plcp_cnt_ofdm = bad_plcp_ofdm;
+	else {
+		bad_plcp_ofdm -= data->last_bad_plcp_cnt_ofdm;
+		data->last_bad_plcp_cnt_ofdm += bad_plcp_ofdm;
+	}
+
+	if (data->last_fa_cnt_ofdm > fa_ofdm)
+		data->last_fa_cnt_ofdm = fa_ofdm;
+	else {
+		fa_ofdm -= data->last_fa_cnt_ofdm;
+		data->last_fa_cnt_ofdm += fa_ofdm;
+	}
+
+	if (data->last_fa_cnt_cck > fa_cck)
+		data->last_fa_cnt_cck = fa_cck;
+	else {
+		fa_cck -= data->last_fa_cnt_cck;
+		data->last_fa_cnt_cck += fa_cck;
+	}
+
+	/* Total aborted signal locks */
+	norm_fa_ofdm = fa_ofdm + bad_plcp_ofdm;
+	norm_fa_cck = fa_cck + bad_plcp_cck;
+
+	IWL_DEBUG_CALIB("cck: fa %u badp %u  ofdm: fa %u badp %u\n", fa_cck,
+			bad_plcp_cck, fa_ofdm, bad_plcp_ofdm);
+
+	iwl4965_sens_auto_corr_ofdm(priv, norm_fa_ofdm, rx_enable_time);
+	iwl4965_sens_energy_cck(priv, norm_fa_cck, rx_enable_time, &statis);
+	rc |= iwl4965_sensitivity_write(priv, CMD_ASYNC);
+
+	return;
+}
+
+static void iwl4965_bg_sensitivity_work(struct work_struct *work)
+{
+	struct iwl_priv *priv = container_of(work, struct iwl_priv,
+			sensitivity_work);
+
+	mutex_lock(&priv->mutex);
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status) ||
+	    test_bit(STATUS_SCANNING, &priv->status)) {
+		mutex_unlock(&priv->mutex);
+		return;
+	}
+
+	if (priv->start_calib) {
+		iwl4965_noise_calibration(priv, &priv->statistics);
+
+		if (priv->sensitivity_data.state ==
+					IWL_SENS_CALIB_NEED_REINIT) {
+			iwl4965_init_sensitivity(priv, CMD_ASYNC, 0);
+			priv->sensitivity_data.state = IWL_SENS_CALIB_ALLOWED;
+		} else
+			iwl4965_sensitivity_calibration(priv,
+					&priv->statistics);
+	}
+
+	mutex_unlock(&priv->mutex);
+	return;
+}
+#endif /*CONFIG_IWLWIFI_SENSITIVITY*/
+
+static void iwl4965_bg_txpower_work(struct work_struct *work)
+{
+	struct iwl_priv *priv = container_of(work, struct iwl_priv,
+			txpower_work);
+
+	/* If a scan happened to start before we got here
+	 * then just return; the statistics notification will
+	 * kick off another scheduled work to compensate for
+	 * any temperature delta we missed here. */
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status) ||
+	    test_bit(STATUS_SCANNING, &priv->status))
+		return;
+
+	mutex_lock(&priv->mutex);
+
+	/* Regardless of if we are assocaited, we must reconfigure the
+	 * TX power since frames can be sent on non-radar channels while
+	 * not associated */
+	iwl_hw_reg_send_txpower(priv);
+
+	/* Update last_temperature to keep is_calib_needed from running
+	 * when it isn't needed... */
+	priv->last_temperature = priv->temperature;
+
+	mutex_unlock(&priv->mutex);
+}
+
+/*
+ * Acquire priv->lock before calling this function !
+ */
+static void iwl4965_set_wr_ptrs(struct iwl_priv *priv, int txq_id, u32 index)
+{
+	iwl_write_restricted(priv, HBUS_TARG_WRPTR,
+			     (index & 0xff) | (txq_id << 8));
+	iwl_write_restricted_reg(priv, SCD_QUEUE_RDPTR(txq_id), index);
+}
+
+/*
+ * Acquire priv->lock before calling this function !
+ */
+static void iwl4965_tx_queue_set_status(struct iwl_priv *priv,
+					struct iwl_tx_queue *txq,
+					int tx_fifo_id, int scd_retry)
+{
+	int txq_id = txq->q.id;
+	int active = test_bit(txq_id, &priv->txq_ctx_active_msk)?1:0;
+
+	iwl_write_restricted_reg(priv, SCD_QUEUE_STATUS_BITS(txq_id),
+				 (active << SCD_QUEUE_STTS_REG_POS_ACTIVE) |
+				 (tx_fifo_id << SCD_QUEUE_STTS_REG_POS_TXF) |
+				 (scd_retry << SCD_QUEUE_STTS_REG_POS_WSL) |
+				 (scd_retry << SCD_QUEUE_STTS_REG_POS_SCD_ACK) |
+				 SCD_QUEUE_STTS_REG_MSK);
+
+	txq->sched_retry = scd_retry;
+
+	IWL_DEBUG_INFO("%s %s Queue %d on AC %d\n",
+		       active ? "Activete" : "Deactivate",
+		       scd_retry ? "BA" : "AC", txq_id, tx_fifo_id);
+}
+
+static const u16 default_queue_to_tx_fifo[] = {
+	IWL_TX_FIFO_AC3,
+	IWL_TX_FIFO_AC2,
+	IWL_TX_FIFO_AC1,
+	IWL_TX_FIFO_AC0,
+	IWL_CMD_FIFO_NUM,
+	IWL_TX_FIFO_HCCA_1,
+	IWL_TX_FIFO_HCCA_2
+};
+
+static inline void iwl4965_txq_ctx_activate(struct iwl_priv *priv, int txq_id)
+{
+	set_bit(txq_id, &priv->txq_ctx_active_msk);
+}
+
+static inline void iwl4965_txq_ctx_deactivate(struct iwl_priv *priv, int txq_id)
+{
+	clear_bit(txq_id, &priv->txq_ctx_active_msk);
+}
+
+int iwl4965_alive_notify(struct iwl_priv *priv)
+{
+	u32 a;
+	int i = 0;
+	unsigned long flags;
+	int rc;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+#ifdef CONFIG_IWLWIFI_SENSITIVITY
+	memset(&(priv->sensitivity_data), 0,
+	       sizeof(struct iwl_sensitivity_data));
+	memset(&(priv->chain_noise_data), 0,
+	       sizeof(struct iwl_chain_noise_data));
+	for (i = 0; i < NUM_RX_CHAINS; i++)
+		priv->chain_noise_data.delta_gain_code[i] =
+				CHAIN_NOISE_DELTA_GAIN_INIT_VAL;
+#endif /* CONFIG_IWLWIFI_SENSITIVITY*/
+	rc = iwl_grab_restricted_access(priv);
+	if (rc) {
+		spin_unlock_irqrestore(&priv->lock, flags);
+		return rc;
+	}
+
+	priv->scd_base_addr = iwl_read_restricted_reg(priv, SCD_SRAM_BASE_ADDR);
+	a = priv->scd_base_addr + SCD_CONTEXT_DATA_OFFSET;
+	for (; a < priv->scd_base_addr + SCD_TX_STTS_BITMAP_OFFSET; a += 4)
+		iwl_write_restricted_mem(priv, a, 0);
+	for (; a < priv->scd_base_addr + SCD_TRANSLATE_TBL_OFFSET; a += 4)
+		iwl_write_restricted_mem(priv, a, 0);
+	for (; a < sizeof(u16) * priv->hw_setting.max_txq_num; a += 4)
+		iwl_write_restricted_mem(priv, a, 0);
+
+	iwl_write_restricted_reg(priv, SCD_DRAM_BASE_ADDR,
+		(priv->hw_setting.shared_phys +
+		 offsetof(struct iwl_shared, queues_byte_cnt_tbls)) >> 10);
+	iwl_write_restricted_reg(priv, SCD_QUEUECHAIN_SEL, 0);
+
+	/* initiate the queues */
+	for (i = 0; i < priv->hw_setting.max_txq_num; i++) {
+		iwl_write_restricted_reg(priv, SCD_QUEUE_RDPTR(i), 0);
+		iwl_write_restricted(priv, HBUS_TARG_WRPTR, 0 | (i << 8));
+		iwl_write_restricted_mem(priv, priv->scd_base_addr +
+					SCD_CONTEXT_QUEUE_OFFSET(i),
+					(SCD_WIN_SIZE <<
+					SCD_QUEUE_CTX_REG1_WIN_SIZE_POS) &
+					SCD_QUEUE_CTX_REG1_WIN_SIZE_MSK);
+		iwl_write_restricted_mem(priv, priv->scd_base_addr +
+					SCD_CONTEXT_QUEUE_OFFSET(i) +
+					sizeof(u32),
+					(SCD_FRAME_LIMIT <<
+					SCD_QUEUE_CTX_REG2_FRAME_LIMIT_POS) &
+					SCD_QUEUE_CTX_REG2_FRAME_LIMIT_MSK);
+
+	}
+	iwl_write_restricted_reg(priv, SCD_INTERRUPT_MASK,
+				 (1 << priv->hw_setting.max_txq_num) - 1);
+
+	iwl_write_restricted_reg(priv, SCD_TXFACT,
+				 SCD_TXFACT_REG_TXFIFO_MASK(0, 7));
+
+	iwl4965_set_wr_ptrs(priv, IWL_CMD_QUEUE_NUM, 0);
+	/* map qos queues to fifos one-to-one */
+	for (i = 0; i < ARRAY_SIZE(default_queue_to_tx_fifo); i++) {
+		int ac = default_queue_to_tx_fifo[i];
+		iwl4965_txq_ctx_activate(priv, i);
+		iwl4965_tx_queue_set_status(priv, &priv->txq[i], ac, 0);
+	}
+
+	iwl_release_restricted_access(priv);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return 0;
+}
+
+int iwl_hw_set_hw_setting(struct iwl_priv *priv)
+{
+	priv->hw_setting.shared_virt =
+	    pci_alloc_consistent(priv->pci_dev,
+				 sizeof(struct iwl_shared),
+				 &priv->hw_setting.shared_phys);
+
+	if (!priv->hw_setting.shared_virt)
+		return -1;
+
+	memset(priv->hw_setting.shared_virt, 0, sizeof(struct iwl_shared));
+
+	priv->hw_setting.max_txq_num = iwl_param_queues_num;
+	priv->hw_setting.ac_queue_count = AC_NUM;
+
+	priv->hw_setting.cck_flag = RATE_MCS_CCK_MSK;
+	priv->hw_setting.tx_cmd_len = sizeof(struct iwl_tx_cmd);
+	priv->hw_setting.max_rxq_size = RX_QUEUE_SIZE;
+	priv->hw_setting.max_rxq_log = RX_QUEUE_SIZE_LOG;
+
+	priv->hw_setting.max_stations = IWL4965_STATION_COUNT;
+	priv->hw_setting.bcast_sta_id = IWL4965_BROADCAST_ID;
+	return 0;
+}
+
+/**
+ * iwl_hw_txq_ctx_free - Free TXQ Context
+ *
+ * Destroy all TX DMA queues and structures
+ */
+void iwl_hw_txq_ctx_free(struct iwl_priv *priv)
+{
+	int txq_id;
+
+	/* Tx queues */
+	for (txq_id = 0; txq_id < priv->hw_setting.max_txq_num; txq_id++)
+		iwl_tx_queue_free(priv, &priv->txq[txq_id]);
+
+	iwl4965_kw_free(priv);
+}
+
+/**
+ * iwl_hw_txq_free_tfd -  Free one TFD, those at index [txq->q.last_used]
+ *
+ * Does NOT advance any indexes
+ */
+int iwl_hw_txq_free_tfd(struct iwl_priv *priv, struct iwl_tx_queue *txq)
+{
+	struct iwl_tfd_frame *bd_tmp = (struct iwl_tfd_frame *)&txq->bd[0];
+	struct iwl_tfd_frame *bd = &bd_tmp[txq->q.last_used];
+	struct pci_dev *dev = priv->pci_dev;
+	int i;
+	int counter = 0;
+	int index, is_odd;
+
+	/* classify bd */
+	if (txq->q.id == IWL_CMD_QUEUE_NUM)
+		/* nothing to cleanup after for host commands */
+		return 0;
+
+	/* sanity check */
+	counter = IWL_GET_BITS(*bd, num_tbs);
+	if (counter > MAX_NUM_OF_TBS) {
+		IWL_ERROR("Too many chunks: %i\n", counter);
+		/* @todo issue fatal error, it is quite serious situation */
+		return 0;
+	}
+
+	/* unmap chunks if any */
+
+	for (i = 0; i < counter; i++) {
+		index = i / 2;
+		is_odd = i & 0x1;
+
+		if (is_odd)
+			pci_unmap_single(
+				dev,
+				IWL_GET_BITS(bd->pa[index], tb2_addr_lo16) |
+				(IWL_GET_BITS(bd->pa[index],
+					      tb2_addr_hi20) << 16),
+				IWL_GET_BITS(bd->pa[index], tb2_len),
+				PCI_DMA_TODEVICE);
+
+		else if (i > 0)
+			pci_unmap_single(dev,
+					 le32_to_cpu(bd->pa[index].tb1_addr),
+					 IWL_GET_BITS(bd->pa[index], tb1_len),
+					 PCI_DMA_TODEVICE);
+
+		if (txq->txb[txq->q.last_used].skb[i]) {
+			struct sk_buff *skb = txq->txb[txq->q.last_used].skb[i];
+
+			dev_kfree_skb(skb);
+			txq->txb[txq->q.last_used].skb[i] = NULL;
+		}
+	}
+	return 0;
+}
+
+int iwl_hw_reg_set_txpower(struct iwl_priv *priv, s8 power)
+{
+	IWL_ERROR("TODO: Implement iwl_hw_reg_set_txpower!\n");
+	return -EINVAL;
+}
+
+static s32 iwl4965_math_div_round(s32 num, s32 denom, s32 *res)
+{
+	s32 sign = 1;
+
+	if (num < 0) {
+		sign = -sign;
+		num = -num;
+	}
+	if (denom < 0) {
+		sign = -sign;
+		denom = -denom;
+	}
+	*res = 1;
+	*res = ((num * 2 + denom) / (denom * 2)) * sign;
+
+	return 1;
+}
+
+static s32 iwl4965_get_voltage_compensation(s32 eeprom_voltage,
+					    s32 current_voltage)
+{
+	s32 comp = 0;
+
+	if ((TX_POWER_IWL_ILLEGAL_VOLTAGE == eeprom_voltage) ||
+	    (TX_POWER_IWL_ILLEGAL_VOLTAGE == current_voltage))
+		return 0;
+
+	iwl4965_math_div_round(current_voltage - eeprom_voltage,
+			       TX_POWER_IWL_VOLTAGE_CODES_PER_03V, &comp);
+
+	if (current_voltage > eeprom_voltage)
+		comp *= 2;
+	if ((comp < -2) || (comp > 2))
+		comp = 0;
+
+	return comp;
+}
+
+static const struct iwl_channel_info *
+iwl4965_get_channel_txpower_info(struct iwl_priv *priv, u8 phymode, u16 channel)
+{
+	const struct iwl_channel_info *ch_info;
+
+	ch_info = iwl_get_channel_info(priv, phymode, channel);
+
+	if (!is_channel_valid(ch_info))
+		return NULL;
+
+	return ch_info;
+}
+
+static s32 iwl4965_get_tx_atten_grp(u16 channel)
+{
+	if (channel >= CALIB_IWL_TX_ATTEN_GR5_FCH &&
+	    channel <= CALIB_IWL_TX_ATTEN_GR5_LCH)
+		return CALIB_CH_GROUP_5;
+
+	if (channel >= CALIB_IWL_TX_ATTEN_GR1_FCH &&
+	    channel <= CALIB_IWL_TX_ATTEN_GR1_LCH)
+		return CALIB_CH_GROUP_1;
+
+	if (channel >= CALIB_IWL_TX_ATTEN_GR2_FCH &&
+	    channel <= CALIB_IWL_TX_ATTEN_GR2_LCH)
+		return CALIB_CH_GROUP_2;
+
+	if (channel >= CALIB_IWL_TX_ATTEN_GR3_FCH &&
+	    channel <= CALIB_IWL_TX_ATTEN_GR3_LCH)
+		return CALIB_CH_GROUP_3;
+
+	if (channel >= CALIB_IWL_TX_ATTEN_GR4_FCH &&
+	    channel <= CALIB_IWL_TX_ATTEN_GR4_LCH)
+		return CALIB_CH_GROUP_4;
+
+	IWL_ERROR("Can't find txatten group for channel %d.\n", channel);
+	return -1;
+}
+
+static u32 iwl4965_get_sub_band(const struct iwl_priv *priv, u32 channel)
+{
+	s32 b = -1;
+
+	for (b = 0; b < EEPROM_TX_POWER_BANDS; b++) {
+		if (priv->eeprom.calib_info.band_info[b].ch_from == 0)
+			continue;
+
+		if ((channel >= priv->eeprom.calib_info.band_info[b].ch_from)
+		    && (channel <= priv->eeprom.calib_info.band_info[b].ch_to))
+			break;
+	}
+
+	return b;
+}
+
+static s32 iwl4965_interpolate_value(s32 x, s32 x1, s32 y1, s32 x2, s32 y2)
+{
+	s32 val;
+
+	if (x2 == x1)
+		return y1;
+	else {
+		iwl4965_math_div_round((x2 - x) * (y1 - y2), (x2 - x1), &val);
+		return val + y2;
+	}
+}
+
+static int iwl4965_interpolate_chan(struct iwl_priv *priv, u32 channel,
+				    struct iwl_eeprom_calib_ch_info *chan_info)
+{
+	s32 s = -1;
+	u32 c;
+	u32 m;
+	const struct iwl_eeprom_calib_measure *m1;
+	const struct iwl_eeprom_calib_measure *m2;
+	struct iwl_eeprom_calib_measure *omeas;
+	u32 ch_i1;
+	u32 ch_i2;
+
+	s = iwl4965_get_sub_band(priv, channel);
+	if (s >= EEPROM_TX_POWER_BANDS) {
+		IWL_ERROR("Tx Power can not find channel %d ", channel);
+		return -1;
+	}
+
+	ch_i1 = priv->eeprom.calib_info.band_info[s].ch1.ch_num;
+	ch_i2 = priv->eeprom.calib_info.band_info[s].ch2.ch_num;
+	chan_info->ch_num = (u8) channel;
+
+	IWL_DEBUG_TXPOWER("channel %d subband %d factory cal ch %d & %d\n",
+			  channel, s, ch_i1, ch_i2);
+
+	for (c = 0; c < EEPROM_TX_POWER_TX_CHAINS; c++) {
+		for (m = 0; m < EEPROM_TX_POWER_MEASUREMENTS; m++) {
+			m1 = &(priv->eeprom.calib_info.band_info[s].ch1.
+			       measurements[c][m]);
+			m2 = &(priv->eeprom.calib_info.band_info[s].ch2.
+			       measurements[c][m]);
+			omeas = &(chan_info->measurements[c][m]);
+
+			omeas->actual_pow =
+			    (u8) iwl4965_interpolate_value(channel, ch_i1,
+							   m1->actual_pow,
+							   ch_i2,
+							   m2->actual_pow);
+			omeas->gain_idx =
+			    (u8) iwl4965_interpolate_value(channel, ch_i1,
+							   m1->gain_idx, ch_i2,
+							   m2->gain_idx);
+			omeas->temperature =
+			    (u8) iwl4965_interpolate_value(channel, ch_i1,
+							   m1->temperature,
+							   ch_i2,
+							   m2->temperature);
+			omeas->pa_det =
+			    (s8) iwl4965_interpolate_value(channel, ch_i1,
+							   m1->pa_det, ch_i2,
+							   m2->pa_det);
+
+			IWL_DEBUG_TXPOWER
+			    ("chain %d meas %d AP1=%d AP2=%d AP=%d\n", c, m,
+			     m1->actual_pow, m2->actual_pow, omeas->actual_pow);
+			IWL_DEBUG_TXPOWER
+			    ("chain %d meas %d NI1=%d NI2=%d NI=%d\n", c, m,
+			     m1->gain_idx, m2->gain_idx, omeas->gain_idx);
+			IWL_DEBUG_TXPOWER
+			    ("chain %d meas %d PA1=%d PA2=%d PA=%d\n", c, m,
+			     m1->pa_det, m2->pa_det, omeas->pa_det);
+			IWL_DEBUG_TXPOWER
+			    ("chain %d meas %d  T1=%d  T2=%d  T=%d\n", c, m,
+			     m1->temperature, m2->temperature,
+			     omeas->temperature);
+		}
+	}
+
+	return 0;
+}
+
+/* bit-rate-dependent table to prevent Tx distortion, in half-dB units,
+ * for OFDM 6, 12, 18, 24, 36, 48, 54, 60 MBit, and CCK all rates. */
+static s32 back_off_table[] = {
+	10, 10, 10, 10, 10, 15, 17, 20,	/* OFDM SISO 20 MHz */
+	10, 10, 10, 10, 10, 15, 17, 20,	/* OFDM MIMO 20 MHz */
+	10, 10, 10, 10, 10, 15, 17, 20,	/* OFDM SISO 40 MHz */
+	10, 10, 10, 10, 10, 15, 17, 20,	/* OFDM MIMO 40 MHz */
+	10			/* CCK */
+};
+
+/* Thermal compensation values for txpower for various frequency ranges ...
+ *   ratios from 3:1 to 4.5:1 of degrees (Celsius) per half-dB gain adjust */
+static struct iwl_txpower_comp_entry {
+	s32 degrees_per_05db_a;
+	s32 degrees_per_05db_a_denom;
+} tx_power_cmp_tble[CALIB_CH_GROUP_MAX] = {
+	{9, 2},			/* group 0 5.2, ch  34-43 */
+	{4, 1},			/* group 1 5.2, ch  44-70 */
+	{4, 1},			/* group 2 5.2, ch  71-124 */
+	{4, 1},			/* group 3 5.2, ch 125-200 */
+	{3, 1}			/* group 4 2.4, ch   all */
+};
+
+static s32 get_min_power_index(s32 rate_power_index, u32 band)
+{
+	if (!band) {
+		if ((rate_power_index & 7) <= 4)
+			return MIN_TX_GAIN_INDEX_52GHZ_EXT;
+	}
+	return MIN_TX_GAIN_INDEX;
+}
+
+struct gain_entry {
+	u8 dsp;
+	u8 radio;
+};
+
+static const struct gain_entry gain_table[2][108] = {
+	/* 5.2GHz power gain index table */
+	{
+	 {123, 0x3F},		/* highest txpower */
+	 {117, 0x3F},
+	 {110, 0x3F},
+	 {104, 0x3F},
+	 {98, 0x3F},
+	 {110, 0x3E},
+	 {104, 0x3E},
+	 {98, 0x3E},
+	 {110, 0x3D},
+	 {104, 0x3D},
+	 {98, 0x3D},
+	 {110, 0x3C},
+	 {104, 0x3C},
+	 {98, 0x3C},
+	 {110, 0x3B},
+	 {104, 0x3B},
+	 {98, 0x3B},
+	 {110, 0x3A},
+	 {104, 0x3A},
+	 {98, 0x3A},
+	 {110, 0x39},
+	 {104, 0x39},
+	 {98, 0x39},
+	 {110, 0x38},
+	 {104, 0x38},
+	 {98, 0x38},
+	 {110, 0x37},
+	 {104, 0x37},
+	 {98, 0x37},
+	 {110, 0x36},
+	 {104, 0x36},
+	 {98, 0x36},
+	 {110, 0x35},
+	 {104, 0x35},
+	 {98, 0x35},
+	 {110, 0x34},
+	 {104, 0x34},
+	 {98, 0x34},
+	 {110, 0x33},
+	 {104, 0x33},
+	 {98, 0x33},
+	 {110, 0x32},
+	 {104, 0x32},
+	 {98, 0x32},
+	 {110, 0x31},
+	 {104, 0x31},
+	 {98, 0x31},
+	 {110, 0x30},
+	 {104, 0x30},
+	 {98, 0x30},
+	 {110, 0x25},
+	 {104, 0x25},
+	 {98, 0x25},
+	 {110, 0x24},
+	 {104, 0x24},
+	 {98, 0x24},
+	 {110, 0x23},
+	 {104, 0x23},
+	 {98, 0x23},
+	 {110, 0x22},
+	 {104, 0x18},
+	 {98, 0x18},
+	 {110, 0x17},
+	 {104, 0x17},
+	 {98, 0x17},
+	 {110, 0x16},
+	 {104, 0x16},
+	 {98, 0x16},
+	 {110, 0x15},
+	 {104, 0x15},
+	 {98, 0x15},
+	 {110, 0x14},
+	 {104, 0x14},
+	 {98, 0x14},
+	 {110, 0x13},
+	 {104, 0x13},
+	 {98, 0x13},
+	 {110, 0x12},
+	 {104, 0x08},
+	 {98, 0x08},
+	 {110, 0x07},
+	 {104, 0x07},
+	 {98, 0x07},
+	 {110, 0x06},
+	 {104, 0x06},
+	 {98, 0x06},
+	 {110, 0x05},
+	 {104, 0x05},
+	 {98, 0x05},
+	 {110, 0x04},
+	 {104, 0x04},
+	 {98, 0x04},
+	 {110, 0x03},
+	 {104, 0x03},
+	 {98, 0x03},
+	 {110, 0x02},
+	 {104, 0x02},
+	 {98, 0x02},
+	 {110, 0x01},
+	 {104, 0x01},
+	 {98, 0x01},
+	 {110, 0x00},
+	 {104, 0x00},
+	 {98, 0x00},
+	 {93, 0x00},
+	 {88, 0x00},
+	 {83, 0x00},
+	 {78, 0x00},
+	 },
+	/* 2.4GHz power gain index table */
+	{
+	 {110, 0x3f},		/* highest txpower */
+	 {104, 0x3f},
+	 {98, 0x3f},
+	 {110, 0x3e},
+	 {104, 0x3e},
+	 {98, 0x3e},
+	 {110, 0x3d},
+	 {104, 0x3d},
+	 {98, 0x3d},
+	 {110, 0x3c},
+	 {104, 0x3c},
+	 {98, 0x3c},
+	 {110, 0x3b},
+	 {104, 0x3b},
+	 {98, 0x3b},
+	 {110, 0x3a},
+	 {104, 0x3a},
+	 {98, 0x3a},
+	 {110, 0x39},
+	 {104, 0x39},
+	 {98, 0x39},
+	 {110, 0x38},
+	 {104, 0x38},
+	 {98, 0x38},
+	 {110, 0x37},
+	 {104, 0x37},
+	 {98, 0x37},
+	 {110, 0x36},
+	 {104, 0x36},
+	 {98, 0x36},
+	 {110, 0x35},
+	 {104, 0x35},
+	 {98, 0x35},
+	 {110, 0x34},
+	 {104, 0x34},
+	 {98, 0x34},
+	 {110, 0x33},
+	 {104, 0x33},
+	 {98, 0x33},
+	 {110, 0x32},
+	 {104, 0x32},
+	 {98, 0x32},
+	 {110, 0x31},
+	 {104, 0x31},
+	 {98, 0x31},
+	 {110, 0x30},
+	 {104, 0x30},
+	 {98, 0x30},
+	 {110, 0x6},
+	 {104, 0x6},
+	 {98, 0x6},
+	 {110, 0x5},
+	 {104, 0x5},
+	 {98, 0x5},
+	 {110, 0x4},
+	 {104, 0x4},
+	 {98, 0x4},
+	 {110, 0x3},
+	 {104, 0x3},
+	 {98, 0x3},
+	 {110, 0x2},
+	 {104, 0x2},
+	 {98, 0x2},
+	 {110, 0x1},
+	 {104, 0x1},
+	 {98, 0x1},
+	 {110, 0x0},
+	 {104, 0x0},
+	 {98, 0x0},
+	 {97, 0},
+	 {96, 0},
+	 {95, 0},
+	 {94, 0},
+	 {93, 0},
+	 {92, 0},
+	 {91, 0},
+	 {90, 0},
+	 {89, 0},
+	 {88, 0},
+	 {87, 0},
+	 {86, 0},
+	 {85, 0},
+	 {84, 0},
+	 {83, 0},
+	 {82, 0},
+	 {81, 0},
+	 {80, 0},
+	 {79, 0},
+	 {78, 0},
+	 {77, 0},
+	 {76, 0},
+	 {75, 0},
+	 {74, 0},
+	 {73, 0},
+	 {72, 0},
+	 {71, 0},
+	 {70, 0},
+	 {69, 0},
+	 {68, 0},
+	 {67, 0},
+	 {66, 0},
+	 {65, 0},
+	 {64, 0},
+	 {63, 0},
+	 {62, 0},
+	 {61, 0},
+	 {60, 0},
+	 {59, 0},
+	 }
+};
+
+static int iwl4965_fill_txpower_tbl(struct iwl_priv *priv, u8 band, u16 channel,
+				    u8 is_fat, u8 ctrl_chan_high,
+				    struct iwl_tx_power_db *tx_power_tbl)
+{
+	u8 saturation_power;
+	s32 target_power;
+	s32 user_target_power;
+	s32 power_limit;
+	s32 current_temp;
+	s32 reg_limit;
+	s32 current_regulatory;
+	s32 txatten_grp = CALIB_CH_GROUP_MAX;
+	int i;
+	int c;
+	const struct iwl_channel_info *ch_info = NULL;
+	struct iwl_eeprom_calib_ch_info ch_eeprom_info;
+	const struct iwl_eeprom_calib_measure *measurement;
+	s16 voltage;
+	s32 init_voltage;
+	s32 voltage_compensation;
+	s32 degrees_per_05db_num;
+	s32 degrees_per_05db_denom;
+	s32 factory_temp;
+	s32 temperature_comp[2];
+	s32 factory_gain_index[2];
+	s32 factory_actual_pwr[2];
+	s32 power_index;
+
+	/* Sanity check requested level (dBm) */
+	if (priv->user_txpower_limit < IWL_TX_POWER_TARGET_POWER_MIN) {
+		IWL_WARNING("Requested user TXPOWER %d below limit.\n",
+			    priv->user_txpower_limit);
+		return -EINVAL;
+	}
+	if (priv->user_txpower_limit > IWL_TX_POWER_TARGET_POWER_MAX) {
+		IWL_WARNING("Requested user TXPOWER %d above limit.\n",
+			    priv->user_txpower_limit);
+		return -EINVAL;
+	}
+
+	/* user_txpower_limit is in dBm, convert to half-dBm (half-dB units
+	 *   are used for indexing into txpower table) */
+	user_target_power = 2 * priv->user_txpower_limit;
+
+	/* Get current (RXON) channel, band, width */
+	ch_info =
+		iwl4965_get_channel_txpower_info(priv, priv->phymode, channel);
+
+	IWL_DEBUG_TXPOWER("chan %d band %d is_fat %d\n", channel, band,
+			  is_fat);
+
+	if (!ch_info)
+		return -EINVAL;
+
+	/* get txatten group, used to select 1) thermal txpower adjustment
+	 *   and 2) mimo txpower balance between Tx chains. */
+	txatten_grp = iwl4965_get_tx_atten_grp(channel);
+	if (txatten_grp < 0)
+		return -EINVAL;
+
+	IWL_DEBUG_TXPOWER("channel %d belongs to txatten group %d\n",
+			  channel, txatten_grp);
+
+	if (is_fat) {
+		if (ctrl_chan_high)
+			channel -= 2;
+		else
+			channel += 2;
+	}
+
+	/* hardware txpower limits ...
+	 * saturation (clipping distortion) txpowers are in half-dBm */
+	if (band)
+		saturation_power = priv->eeprom.calib_info.saturation_power24;
+	else
+		saturation_power = priv->eeprom.calib_info.saturation_power52;
+
+	if (saturation_power < IWL_TX_POWER_SATURATION_MIN ||
+	    saturation_power > IWL_TX_POWER_SATURATION_MAX) {
+		if (band)
+			saturation_power = IWL_TX_POWER_DEFAULT_SATURATION_24;
+		else
+			saturation_power = IWL_TX_POWER_DEFAULT_SATURATION_52;
+	}
+
+	/* regulatory txpower limits ... reg_limit values are in half-dBm,
+	 *   max_power_avg values are in dBm, convert * 2 */
+	if (is_fat)
+		reg_limit = ch_info->fat_max_power_avg * 2;
+	else
+		reg_limit = ch_info->max_power_avg * 2;
+
+	if ((reg_limit < IWL_TX_POWER_REGULATORY_MIN) ||
+	    (reg_limit > IWL_TX_POWER_REGULATORY_MAX)) {
+		if (band)
+			reg_limit = IWL_TX_POWER_DEFAULT_REGULATORY_24;
+		else
+			reg_limit = IWL_TX_POWER_DEFAULT_REGULATORY_52;
+	}
+
+	/* Interpolate txpower calibration values for this channel,
+	 *   based on factory calibration tests on spaced channels. */
+	iwl4965_interpolate_chan(priv, channel, &ch_eeprom_info);
+
+	/* calculate tx gain adjustment based on power supply voltage */
+	voltage = priv->eeprom.calib_info.voltage;
+	init_voltage = (s32)le32_to_cpu(priv->card_alive_init.voltage);
+	voltage_compensation =
+	    iwl4965_get_voltage_compensation(voltage, init_voltage);
+
+	IWL_DEBUG_TXPOWER("curr volt %d eeprom volt %d volt comp %d\n",
+			  init_voltage,
+			  voltage, voltage_compensation);
+
+	/* get current temperature (Celsius) */
+	current_temp = max(priv->temperature, IWL_TX_POWER_TEMPERATURE_MIN);
+	current_temp = min(priv->temperature, IWL_TX_POWER_TEMPERATURE_MAX);
+	current_temp = KELVIN_TO_CELSIUS(current_temp);
+
+	/* select thermal txpower adjustment params, based on channel group
+	 *   (same frequency group used for mimo txatten adjustment) */
+	degrees_per_05db_num =
+	    tx_power_cmp_tble[txatten_grp].degrees_per_05db_a;
+	degrees_per_05db_denom =
+	    tx_power_cmp_tble[txatten_grp].degrees_per_05db_a_denom;
+
+	/* get per-chain txpower values from factory measurements */
+	for (c = 0; c < 2; c++) {
+		measurement = &ch_eeprom_info.measurements[c][1];
+
+		/* txgain adjustment (in half-dB steps) based on difference
+		 *   between factory and current temperature */
+		factory_temp = measurement->temperature;
+		iwl4965_math_div_round((current_temp - factory_temp) *
+				       degrees_per_05db_denom,
+				       degrees_per_05db_num,
+				       &temperature_comp[c]);
+
+		factory_gain_index[c] = measurement->gain_idx;
+		factory_actual_pwr[c] = measurement->actual_pow;
+
+		IWL_DEBUG_TXPOWER("chain = %d\n", c);
+		IWL_DEBUG_TXPOWER("fctry tmp %d, "
+				  "curr tmp %d, comp %d steps\n",
+				  factory_temp, current_temp,
+				  temperature_comp[c]);
+
+		IWL_DEBUG_TXPOWER("fctry idx %d, fctry pwr %d\n",
+				  factory_gain_index[c],
+				  factory_actual_pwr[c]);
+	}
+
+	/* for each of 33 bit-rates (including 1 for CCK) */
+	for (i = 0; i < POWER_TABLE_NUM_ENTRIES; i++) {
+		u8 is_mimo_rate;
+		union iwl_tx_power_dual_stream tx_power;
+
+		/* for mimo, reduce each chain's txpower by half
+		 * (3dB, 6 steps), so total output power is regulatory
+		 * compliant. */
+		if (i & 0x8) {
+			current_regulatory = reg_limit -
+			    IWL_TX_POWER_MIMO_REGULATORY_COMPENSATION;
+			is_mimo_rate = 1;
+		} else {
+			current_regulatory = reg_limit;
+			is_mimo_rate = 0;
+		}
+
+		/* find txpower limit, either hardware or regulatory */
+		power_limit = saturation_power - back_off_table[i];
+		if (power_limit > current_regulatory)
+			power_limit = current_regulatory;
+
+		/* reduce user's txpower request if necessary
+		 * for this rate on this channel */
+		target_power = user_target_power;
+		if (target_power > power_limit)
+			target_power = power_limit;
+
+		IWL_DEBUG_TXPOWER("rate %d sat %d reg %d usr %d tgt %d\n",
+				  i, saturation_power - back_off_table[i],
+				  current_regulatory, user_target_power,
+				  target_power);
+
+		/* for each of 2 Tx chains (radio transmitters) */
+		for (c = 0; c < 2; c++) {
+			s32 atten_value;
+
+			if (is_mimo_rate)
+				atten_value =
+				    (s32)le32_to_cpu(priv->card_alive_init.
+				    tx_atten[txatten_grp][c]);
+			else
+				atten_value = 0;
+
+			/* calculate index; higher index means lower txpower */
+			power_index = (u8) (factory_gain_index[c] -
+					    (target_power -
+					     factory_actual_pwr[c]) -
+					    temperature_comp[c] -
+					    voltage_compensation +
+					    atten_value);
+
+/*			IWL_DEBUG_TXPOWER("calculated txpower index %d\n",
+						power_index); */
+
+			if (power_index < get_min_power_index(i, band))
+				power_index = get_min_power_index(i, band);
+
+			/* adjust 5 GHz index to support negative indexes */
+			if (!band)
+				power_index += 9;
+
+			/* CCK, rate 32, reduce txpower for CCK */
+			if (i == POWER_TABLE_CCK_ENTRY)
+				power_index +=
+				    IWL_TX_POWER_CCK_COMPENSATION_C_STEP;
+
+			/* stay within the table! */
+			if (power_index > 107) {
+				IWL_WARNING("txpower index %d > 107\n",
+					    power_index);
+				power_index = 107;
+			}
+			if (power_index < 0) {
+				IWL_WARNING("txpower index %d < 0\n",
+					    power_index);
+				power_index = 0;
+			}
+
+			/* fill txpower command for this rate/chain */
+			tx_power.s.radio_tx_gain[c] =
+				gain_table[band][power_index].radio;
+			tx_power.s.dsp_predis_atten[c] =
+				gain_table[band][power_index].dsp;
+
+			IWL_DEBUG_TXPOWER("chain %d mimo %d index %d "
+					  "gain 0x%02x dsp %d\n",
+					  c, atten_value, power_index,
+					tx_power.s.radio_tx_gain[c],
+					tx_power.s.dsp_predis_atten[c]);
+		}/* for each chain */
+
+		tx_power_tbl->power_tbl[i].dw = cpu_to_le32(tx_power.dw);
+
+	}/* for each rate */
+
+	return 0;
+}
+
+/**
+ * iwl_hw_reg_send_txpower - Configure the TXPOWER level user limit
+ *
+ * Uses the active RXON for channel, band, and characteristics (fat, high)
+ * The power limit is taken from priv->user_txpower_limit.
+ */
+int iwl_hw_reg_send_txpower(struct iwl_priv *priv)
+{
+	struct iwl_txpowertable_cmd cmd = { 0 };
+	int rc = 0;
+	u8 band = 0;
+	u8 is_fat = 0;
+	u8 ctrl_chan_high = 0;
+
+	if (test_bit(STATUS_SCANNING, &priv->status)) {
+		/* If this gets hit a lot, switch it to a BUG() and catch
+		 * the stack trace to find out who is calling this during
+		 * a scan. */
+		IWL_WARNING("TX Power requested while scanning!\n");
+		return -EAGAIN;
+	}
+
+	band = ((priv->phymode == MODE_IEEE80211B) ||
+		(priv->phymode == MODE_IEEE80211G));
+
+	is_fat =  is_fat_channel(priv->active_rxon.flags);
+
+	if (is_fat &&
+	    (priv->active_rxon.flags & RXON_FLG_CTRL_CHANNEL_LOC_HI_MSK))
+		ctrl_chan_high = 1;
+
+	cmd.band = band;
+	cmd.channel = priv->active_rxon.channel;
+
+	rc = iwl4965_fill_txpower_tbl(priv, band,
+				le16_to_cpu(priv->active_rxon.channel),
+				is_fat, ctrl_chan_high, &cmd.tx_power);
+	if (rc)
+		return rc;
+
+	rc = iwl_send_cmd_pdu(priv, REPLY_TX_PWR_TABLE_CMD, sizeof(cmd), &cmd);
+	return rc;
+}
+
+int iwl_hw_channel_switch(struct iwl_priv *priv, u16 channel)
+{
+	int rc;
+	u8 band = 0;
+	u8 is_fat = 0;
+	u8 ctrl_chan_high = 0;
+	struct iwl_channel_switch_cmd cmd = { 0 };
+	const struct iwl_channel_info *ch_info;
+
+	band = ((priv->phymode == MODE_IEEE80211B) ||
+		(priv->phymode == MODE_IEEE80211G));
+
+	ch_info = iwl_get_channel_info(priv, priv->phymode, channel);
+
+	is_fat = is_fat_channel(priv->staging_rxon.flags);
+
+	if (is_fat &&
+	    (priv->active_rxon.flags & RXON_FLG_CTRL_CHANNEL_LOC_HI_MSK))
+		ctrl_chan_high = 1;
+
+	cmd.band = band;
+	cmd.expect_beacon = 0;
+	cmd.channel = cpu_to_le16(channel);
+	cmd.rxon_flags = priv->active_rxon.flags;
+	cmd.rxon_filter_flags = priv->active_rxon.filter_flags;
+	cmd.switch_time = cpu_to_le32(priv->ucode_beacon_time);
+	if (ch_info)
+		cmd.expect_beacon = is_channel_radar(ch_info);
+	else
+		cmd.expect_beacon = 1;
+
+	rc = iwl4965_fill_txpower_tbl(priv, band, channel, is_fat,
+				      ctrl_chan_high, &cmd.tx_power);
+	if (rc) {
+		IWL_DEBUG_11H("error:%d  fill txpower_tbl\n", rc);
+		return rc;
+	}
+
+	rc = iwl_send_cmd_pdu(priv, REPLY_CHANNEL_SWITCH, sizeof(cmd), &cmd);
+	return rc;
+}
+
+#define RTS_HCCA_RETRY_LIMIT		3
+#define RTS_DFAULT_RETRY_LIMIT		60
+
+void iwl_hw_build_tx_cmd_rate(struct iwl_priv *priv,
+			      struct iwl_cmd *cmd,
+			      struct ieee80211_tx_control *ctrl,
+			      struct ieee80211_hdr *hdr, int sta_id,
+			      int is_hcca)
+{
+	u8 rate;
+	u8 rts_retry_limit = 0;
+	u8 data_retry_limit = 0;
+	__le32 tx_flags;
+	u16 fc = le16_to_cpu(hdr->frame_control);
+
+	tx_flags = cmd->cmd.tx.tx_flags;
+
+	rate = iwl_rates[ctrl->tx_rate].plcp;
+
+	rts_retry_limit = (is_hcca) ?
+	    RTS_HCCA_RETRY_LIMIT : RTS_DFAULT_RETRY_LIMIT;
+
+	if (ieee80211_is_probe_response(fc)) {
+		data_retry_limit = 3;
+		if (data_retry_limit < rts_retry_limit)
+			rts_retry_limit = data_retry_limit;
+	} else
+		data_retry_limit = IWL_DEFAULT_TX_RETRY;
+
+	if (priv->data_retry_limit != -1)
+		data_retry_limit = priv->data_retry_limit;
+
+	if ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT) {
+		switch (fc & IEEE80211_FCTL_STYPE) {
+		case IEEE80211_STYPE_AUTH:
+		case IEEE80211_STYPE_DEAUTH:
+		case IEEE80211_STYPE_ASSOC_REQ:
+		case IEEE80211_STYPE_REASSOC_REQ:
+			if (tx_flags & TX_CMD_FLG_RTS_MSK) {
+				tx_flags &= ~TX_CMD_FLG_RTS_MSK;
+				tx_flags |= TX_CMD_FLG_CTS_MSK;
+			}
+			break;
+		default:
+			break;
+		}
+	}
+
+	cmd->cmd.tx.rts_retry_limit = rts_retry_limit;
+	cmd->cmd.tx.data_retry_limit = data_retry_limit;
+	cmd->cmd.tx.rate_n_flags = iwl_hw_set_rate_n_flags(rate, 0);
+	cmd->cmd.tx.tx_flags = tx_flags;
+}
+
+int iwl_hw_get_rx_read(struct iwl_priv *priv)
+{
+	struct iwl_shared *shared_data = priv->hw_setting.shared_virt;
+
+	return IWL_GET_BITS(*shared_data, rb_closed_stts_rb_num);
+}
+
+int iwl_hw_get_temperature(struct iwl_priv *priv)
+{
+	return priv->temperature;
+}
+
+unsigned int iwl_hw_get_beacon_cmd(struct iwl_priv *priv,
+			  struct iwl_frame *frame, u8 rate)
+{
+	struct iwl_tx_beacon_cmd *tx_beacon_cmd;
+	unsigned int frame_size;
+
+	tx_beacon_cmd = &frame->u.beacon;
+	memset(tx_beacon_cmd, 0, sizeof(*tx_beacon_cmd));
+
+	tx_beacon_cmd->tx.sta_id = IWL4965_BROADCAST_ID;
+	tx_beacon_cmd->tx.stop_time.life_time = TX_CMD_LIFE_TIME_INFINITE;
+
+	frame_size = iwl_fill_beacon_frame(priv,
+				tx_beacon_cmd->frame,
+				BROADCAST_ADDR,
+				sizeof(frame->u) - sizeof(*tx_beacon_cmd));
+
+	BUG_ON(frame_size > MAX_MPDU_SIZE);
+	tx_beacon_cmd->tx.len = cpu_to_le16((u16)frame_size);
+
+	if ((rate == IWL_RATE_1M_PLCP) || (rate >= IWL_RATE_2M_PLCP))
+		tx_beacon_cmd->tx.rate_n_flags =
+			iwl_hw_set_rate_n_flags(rate, RATE_MCS_CCK_MSK);
+	else
+		tx_beacon_cmd->tx.rate_n_flags =
+			iwl_hw_set_rate_n_flags(rate, 0);
+
+	tx_beacon_cmd->tx.tx_flags = (TX_CMD_FLG_SEQ_CTL_MSK |
+				TX_CMD_FLG_TSF_MSK | TX_CMD_FLG_STA_RATE_MSK);
+	return (sizeof(*tx_beacon_cmd) + frame_size);
+}
+
+int iwl_hw_tx_queue_init(struct iwl_priv *priv, struct iwl_tx_queue *txq)
+{
+	int rc;
+	unsigned long flags;
+	int txq_id = txq->q.id;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	rc = iwl_grab_restricted_access(priv);
+	if (rc) {
+		spin_unlock_irqrestore(&priv->lock, flags);
+		return rc;
+	}
+
+	iwl_write_restricted(priv, FH_MEM_CBBC_QUEUE(txq_id),
+			     txq->q.dma_addr >> 8);
+	iwl_write_restricted(
+		priv, IWL_FH_TCSR_CHNL_TX_CONFIG_REG(txq_id),
+		IWL_FH_TCSR_TX_CONFIG_REG_VAL_DMA_CHNL_ENABLE |
+		IWL_FH_TCSR_TX_CONFIG_REG_VAL_DMA_CREDIT_ENABLE_VAL);
+	iwl_release_restricted_access(priv);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return 0;
+}
+
+static inline u8 iwl4965_get_dma_hi_address(dma_addr_t addr)
+{
+	return sizeof(addr) > sizeof(u32) ? (addr >> 16) >> 16 : 0;
+}
+
+int iwl_hw_txq_attach_buf_to_tfd(struct iwl_priv *priv, void *ptr,
+				 dma_addr_t addr, u16 len)
+{
+	int index, is_odd;
+	struct iwl_tfd_frame *tfd = ptr;
+	u32 num_tbs = IWL_GET_BITS(*tfd, num_tbs);
+
+	if ((num_tbs >= MAX_NUM_OF_TBS) || (num_tbs < 0)) {
+		IWL_ERROR("Error can not send more than %d chunks\n",
+			  MAX_NUM_OF_TBS);
+		return -EINVAL;
+	}
+
+	index = num_tbs / 2;
+	is_odd = num_tbs & 0x1;
+
+	if (!is_odd) {
+		tfd->pa[index].tb1_addr = cpu_to_le32(addr);
+		IWL_SET_BITS(tfd->pa[index], tb1_addr_hi,
+			     iwl4965_get_dma_hi_address(addr));
+		IWL_SET_BITS(tfd->pa[index], tb1_len, len);
+	} else {
+		IWL_SET_BITS(tfd->pa[index], tb2_addr_lo16,
+			     (u32) (addr & 0xffff));
+		IWL_SET_BITS(tfd->pa[index], tb2_addr_hi20, addr >> 16);
+		IWL_SET_BITS(tfd->pa[index], tb2_len, len);
+	}
+
+	IWL_SET_BITS(*tfd, num_tbs, num_tbs + 1);
+
+	return 0;
+}
+
+void iwl_hw_card_show_info(struct iwl_priv *priv)
+{
+	u16 hw_version = priv->eeprom.board_revision_4965;
+
+	IWL_DEBUG_INFO("4965ABGN HW Version %u.%u.%u\n",
+		       ((hw_version >> 8) & 0x0F),
+		       ((hw_version >> 8) >> 4), (hw_version & 0x00FF));
+
+	IWL_DEBUG_INFO("4965ABGN PBA Number %.16s\n",
+		       priv->eeprom.board_pba_number_4965);
+}
+
+#define IWL_TX_CRC_SIZE		4
+#define IWL_TX_DELIMITER_SIZE	4
+
+int iwl4965_tx_queue_update_wr_ptr(struct iwl_priv *priv,
+				   struct iwl_tx_queue *txq, u16 byte_cnt)
+{
+	int len;
+	int txq_id = txq->q.id;
+	struct iwl_shared *shared_data = priv->hw_setting.shared_virt;
+
+	if (txq->need_update == 0)
+		return 0;
+
+	len = byte_cnt + IWL_TX_CRC_SIZE + IWL_TX_DELIMITER_SIZE;
+
+	IWL_SET_BITS16(shared_data->queues_byte_cnt_tbls[txq_id].
+		       tfd_offset[txq->q.first_empty], byte_cnt, len);
+
+	if (txq->q.first_empty < IWL4965_MAX_WIN_SIZE)
+		IWL_SET_BITS16(shared_data->queues_byte_cnt_tbls[txq_id].
+			tfd_offset[IWL4965_QUEUE_SIZE + txq->q.first_empty],
+			byte_cnt, len);
+
+	return 0;
+}
+
+/* Set up Rx receiver/antenna/chain usage in "staging" RXON image.
+ * This should not be used for scan command ... it puts data in wrong place.  */
+void iwl4965_set_rxon_chain(struct iwl_priv *priv)
+{
+	u8 is_single = is_single_stream(priv);
+	u8 idle_state, rx_state;
+
+	priv->staging_rxon.rx_chain = 0;
+	rx_state = idle_state = 3;
+
+	/* Tell uCode which antennas are actually connected.
+	 * Before first association, we assume all antennas are connected.
+	 * Just after first association, iwl4965_noise_calibration()
+	 *    checks which antennas actually *are* connected. */
+	priv->staging_rxon.rx_chain |=
+	    cpu_to_le16(priv->valid_antenna << RXON_RX_CHAIN_VALID_POS);
+
+	/* How many receivers should we use? */
+	iwl4965_get_rx_chain_counter(priv, &idle_state, &rx_state);
+	priv->staging_rxon.rx_chain |=
+		cpu_to_le16(rx_state << RXON_RX_CHAIN_MIMO_CNT_POS);
+	priv->staging_rxon.rx_chain |=
+		cpu_to_le16(idle_state << RXON_RX_CHAIN_CNT_POS);
+
+	if (!is_single && (rx_state >= 2) &&
+	    !test_bit(STATUS_POWER_PMI, &priv->status))
+		priv->staging_rxon.rx_chain |= RXON_RX_CHAIN_MIMO_FORCE_MSK;
+	else
+		priv->staging_rxon.rx_chain &= ~RXON_RX_CHAIN_MIMO_FORCE_MSK;
+
+	IWL_DEBUG_ASSOC("rx chain %X\n", priv->staging_rxon.rx_chain);
+}
+
+#ifdef CONFIG_IWLWIFI_HT
+#ifdef CONFIG_IWLWIFI_HT_AGG
+/*
+	get the traffic load value for tid
+*/
+static u32 iwl4965_tl_get_load(struct iwl_priv *priv, u8 tid)
+{
+	u32 load = 0;
+	u32 current_time = jiffies_to_msecs(jiffies);
+	u32 time_diff;
+	s32 index;
+	unsigned long flags;
+	struct iwl_traffic_load *tid_ptr = NULL;
+
+	if (tid >= TID_MAX_LOAD_COUNT)
+		return 0;
+
+	tid_ptr = &(priv->lq_mngr.agg_ctrl.traffic_load[tid]);
+
+	current_time -= current_time % TID_ROUND_VALUE;
+
+	spin_lock_irqsave(&priv->lq_mngr.lock, flags);
+	if (!(tid_ptr->queue_count))
+		goto out;
+
+	time_diff = TIME_WRAP_AROUND(tid_ptr->time_stamp, current_time);
+	index = time_diff / TID_QUEUE_CELL_SPACING;
+
+	if (index >= TID_QUEUE_MAX_SIZE) {
+		u32 oldest_time = current_time - TID_MAX_TIME_DIFF;
+
+		while (tid_ptr->queue_count &&
+		       (tid_ptr->time_stamp < oldest_time)) {
+			tid_ptr->total -= tid_ptr->packet_count[tid_ptr->head];
+			tid_ptr->packet_count[tid_ptr->head] = 0;
+			tid_ptr->time_stamp += TID_QUEUE_CELL_SPACING;
+			tid_ptr->queue_count--;
+			tid_ptr->head++;
+			if (tid_ptr->head >= TID_QUEUE_MAX_SIZE)
+				tid_ptr->head = 0;
+		}
+	}
+	load = tid_ptr->total;
+
+ out:
+	spin_unlock_irqrestore(&priv->lq_mngr.lock, flags);
+	return load;
+}
+
+/*
+	increment traffic load value for tid and also remove
+	any old values if passed the certian time period
+*/
+static void iwl4965_tl_add_packet(struct iwl_priv *priv, u8 tid)
+{
+	u32 current_time = jiffies_to_msecs(jiffies);
+	u32 time_diff;
+	s32 index;
+	unsigned long flags;
+	struct iwl_traffic_load *tid_ptr = NULL;
+
+	if (tid >= TID_MAX_LOAD_COUNT)
+		return;
+
+	tid_ptr = &(priv->lq_mngr.agg_ctrl.traffic_load[tid]);
+
+	current_time -= current_time % TID_ROUND_VALUE;
+
+	spin_lock_irqsave(&priv->lq_mngr.lock, flags);
+	if (!(tid_ptr->queue_count)) {
+		tid_ptr->total = 1;
+		tid_ptr->time_stamp = current_time;
+		tid_ptr->queue_count = 1;
+		tid_ptr->head = 0;
+		tid_ptr->packet_count[0] = 1;
+		goto out;
+	}
+
+	time_diff = TIME_WRAP_AROUND(tid_ptr->time_stamp, current_time);
+	index = time_diff / TID_QUEUE_CELL_SPACING;
+
+	if (index >= TID_QUEUE_MAX_SIZE) {
+		u32 oldest_time = current_time - TID_MAX_TIME_DIFF;
+
+		while (tid_ptr->queue_count &&
+		       (tid_ptr->time_stamp < oldest_time)) {
+			tid_ptr->total -= tid_ptr->packet_count[tid_ptr->head];
+			tid_ptr->packet_count[tid_ptr->head] = 0;
+			tid_ptr->time_stamp += TID_QUEUE_CELL_SPACING;
+			tid_ptr->queue_count--;
+			tid_ptr->head++;
+			if (tid_ptr->head >= TID_QUEUE_MAX_SIZE)
+				tid_ptr->head = 0;
+		}
+	}
+
+	index = (tid_ptr->head + index) % TID_QUEUE_MAX_SIZE;
+	tid_ptr->packet_count[index] = tid_ptr->packet_count[index] + 1;
+	tid_ptr->total = tid_ptr->total + 1;
+
+	if ((index + 1) > tid_ptr->queue_count)
+		tid_ptr->queue_count = index + 1;
+ out:
+	spin_unlock_irqrestore(&priv->lq_mngr.lock, flags);
+
+}
+
+#define MMAC_SCHED_MAX_NUMBER_OF_HT_BACK_FLOWS   7
+enum HT_STATUS {
+	BA_STATUS_FAILURE = 0,
+	BA_STATUS_INITIATOR_DELBA,
+	BA_STATUS_RECIPIENT_DELBA,
+	BA_STATUS_RENEW_ADDBA_REQUEST,
+	BA_STATUS_ACTIVE,
+};
+
+static u8 iwl4964_tl_ba_avail(struct iwl_priv *priv)
+{
+	int i;
+	struct iwl_lq_mngr *lq;
+	u8 count = 0;
+	u16 msk;
+
+	lq = (struct iwl_lq_mngr *)&(priv->lq_mngr);
+	for (i = 0; i < TID_MAX_LOAD_COUNT ; i++) {
+		msk = 1 << i;
+		if ((lq->agg_ctrl.granted_ba & msk) ||
+		    (lq->agg_ctrl.wait_for_agg_status & msk))
+			count++;
+	}
+
+	if (count < MMAC_SCHED_MAX_NUMBER_OF_HT_BACK_FLOWS)
+		return 1;
+
+	return 0;
+}
+
+static void iwl4965_ba_status(struct iwl_priv *priv,
+			      u8 tid, enum HT_STATUS status);
+
+static int iwl4965_perform_addba(struct iwl_priv *priv, u8 tid, u32 length,
+				 u32 ba_timeout)
+{
+	int rc;
+
+	rc = ieee80211_start_BA_session(priv->hw, priv->bssid, tid);
+	if (rc)
+		iwl4965_ba_status(priv, tid, BA_STATUS_FAILURE);
+
+	return rc;
+}
+
+static int iwl4965_perform_delba(struct iwl_priv *priv, u8 tid)
+{
+	int rc;
+
+	rc = ieee80211_stop_BA_session(priv->hw, priv->bssid, tid);
+	if (rc)
+		iwl4965_ba_status(priv, tid, BA_STATUS_FAILURE);
+
+	return rc;
+}
+
+static void iwl4965_turn_on_agg_for_tid(struct iwl_priv *priv,
+					struct iwl_lq_mngr *lq,
+					u8 auto_agg, u8 tid)
+{
+	u32 tid_msk = (1 << tid);
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->lq_mngr.lock, flags);
+/*
+	if ((auto_agg) && (!lq->enable_counter)){
+		lq->agg_ctrl.next_retry = 0;
+		lq->agg_ctrl.tid_retry = 0;
+		spin_unlock_irqrestore(&priv->lq_mngr.lock, flags);
+		return;
+	}
+*/
+	if (!(lq->agg_ctrl.granted_ba & tid_msk) &&
+	    (lq->agg_ctrl.requested_ba & tid_msk)) {
+		u8 available_queues;
+		u32 load;
+
+		spin_unlock_irqrestore(&priv->lq_mngr.lock, flags);
+		available_queues = iwl4964_tl_ba_avail(priv);
+		load = iwl4965_tl_get_load(priv, tid);
+
+		spin_lock_irqsave(&priv->lq_mngr.lock, flags);
+		if (!available_queues) {
+			if (auto_agg)
+				lq->agg_ctrl.tid_retry |= tid_msk;
+			else {
+				lq->agg_ctrl.requested_ba &= ~tid_msk;
+				lq->agg_ctrl.wait_for_agg_status &= ~tid_msk;
+			}
+		} else if ((auto_agg) &&
+			   ((load <= lq->agg_ctrl.tid_traffic_load_threshold) ||
+			    ((lq->agg_ctrl.wait_for_agg_status & tid_msk))))
+			lq->agg_ctrl.tid_retry |= tid_msk;
+		else {
+			lq->agg_ctrl.wait_for_agg_status |= tid_msk;
+			spin_unlock_irqrestore(&priv->lq_mngr.lock, flags);
+			iwl4965_perform_addba(priv, tid, 0x40,
+					      lq->agg_ctrl.ba_timeout);
+			spin_lock_irqsave(&priv->lq_mngr.lock, flags);
+		}
+	}
+	spin_unlock_irqrestore(&priv->lq_mngr.lock, flags);
+}
+
+static void iwl4965_turn_on_agg(struct iwl_priv *priv, u8 tid)
+{
+	struct iwl_lq_mngr *lq;
+	unsigned long flags;
+
+	lq = (struct iwl_lq_mngr *)&(priv->lq_mngr);
+
+	if ((tid < TID_MAX_LOAD_COUNT))
+		iwl4965_turn_on_agg_for_tid(priv, lq, lq->agg_ctrl.auto_agg,
+					    tid);
+	else if (tid == TID_ALL_SPECIFIED) {
+		if (lq->agg_ctrl.requested_ba) {
+			for (tid = 0; tid < TID_MAX_LOAD_COUNT; tid++)
+				iwl4965_turn_on_agg_for_tid(priv, lq,
+					lq->agg_ctrl.auto_agg, tid);
+		} else {
+			spin_lock_irqsave(&priv->lq_mngr.lock, flags);
+			lq->agg_ctrl.tid_retry = 0;
+			lq->agg_ctrl.next_retry = 0;
+			spin_unlock_irqrestore(&priv->lq_mngr.lock, flags);
+		}
+	}
+
+}
+
+void iwl4965_turn_off_agg(struct iwl_priv *priv, u8 tid)
+{
+	u32 tid_msk;
+	struct iwl_lq_mngr *lq;
+	unsigned long flags;
+
+	lq = (struct iwl_lq_mngr *)&(priv->lq_mngr);
+
+	if ((tid < TID_MAX_LOAD_COUNT)) {
+		tid_msk = 1 << tid;
+		spin_lock_irqsave(&priv->lq_mngr.lock, flags);
+		lq->agg_ctrl.wait_for_agg_status |= tid_msk;
+		lq->agg_ctrl.requested_ba &= ~tid_msk;
+		spin_unlock_irqrestore(&priv->lq_mngr.lock, flags);
+		iwl4965_perform_delba(priv, tid);
+	} else if (tid == TID_ALL_SPECIFIED) {
+		spin_lock_irqsave(&priv->lq_mngr.lock, flags);
+		for (tid = 0; tid < TID_MAX_LOAD_COUNT; tid++) {
+			tid_msk = 1 << tid;
+			lq->agg_ctrl.wait_for_agg_status |= tid_msk;
+			spin_unlock_irqrestore(&priv->lq_mngr.lock, flags);
+			iwl4965_perform_delba(priv, tid);
+			spin_lock_irqsave(&priv->lq_mngr.lock, flags);
+		}
+		lq->agg_ctrl.requested_ba = 0;
+		spin_unlock_irqrestore(&priv->lq_mngr.lock, flags);
+	}
+}
+
+static void iwl4965_ba_status(struct iwl_priv *priv,
+				u8 tid, enum HT_STATUS status)
+{
+	struct iwl_lq_mngr *lq;
+	u32 tid_msk = (1 << tid);
+	unsigned long flags;
+
+	lq = (struct iwl_lq_mngr *)&(priv->lq_mngr);
+
+	if ((tid >= TID_MAX_LOAD_COUNT))
+		goto out;
+
+	spin_lock_irqsave(&priv->lq_mngr.lock, flags);
+	switch (status) {
+	case BA_STATUS_ACTIVE:
+		if (!(lq->agg_ctrl.granted_ba & tid_msk))
+			lq->agg_ctrl.granted_ba |= tid_msk;
+		break;
+	default:
+		if ((lq->agg_ctrl.granted_ba & tid_msk))
+			lq->agg_ctrl.granted_ba &= ~tid_msk;
+		break;
+	}
+
+	lq->agg_ctrl.wait_for_agg_status &= ~tid_msk;
+	if (status != BA_STATUS_ACTIVE) {
+		if (lq->agg_ctrl.auto_agg) {
+			lq->agg_ctrl.tid_retry |= tid_msk;
+			lq->agg_ctrl.next_retry =
+			    jiffies + msecs_to_jiffies(500);
+		} else
+			lq->agg_ctrl.requested_ba &= ~tid_msk;
+	}
+	spin_unlock_irqrestore(&priv->lq_mngr.lock, flags);
+ out:
+	return;
+}
+
+static void iwl4965_bg_agg_work(struct work_struct *work)
+{
+	struct iwl_priv *priv = container_of(work, struct iwl_priv,
+					agg_work);
+
+	u32 tid;
+	u32 retry_tid;
+	u32 tid_msk;
+	unsigned long flags;
+	struct iwl_lq_mngr *lq = (struct iwl_lq_mngr *)&(priv->lq_mngr);
+
+	spin_lock_irqsave(&priv->lq_mngr.lock, flags);
+	retry_tid = lq->agg_ctrl.tid_retry;
+	lq->agg_ctrl.tid_retry = 0;
+	spin_unlock_irqrestore(&priv->lq_mngr.lock, flags);
+
+	if (retry_tid == TID_ALL_SPECIFIED)
+		iwl4965_turn_on_agg(priv, TID_ALL_SPECIFIED);
+	else {
+		for (tid = 0; tid < TID_MAX_LOAD_COUNT; tid++) {
+			tid_msk = (1 << tid);
+			if (retry_tid & tid_msk)
+				iwl4965_turn_on_agg(priv, tid);
+		}
+	}
+
+	spin_lock_irqsave(&priv->lq_mngr.lock, flags);
+	if (lq->agg_ctrl.tid_retry)
+		lq->agg_ctrl.next_retry = jiffies + msecs_to_jiffies(500);
+	spin_unlock_irqrestore(&priv->lq_mngr.lock, flags);
+	return;
+}
+#endif /*CONFIG_IWLWIFI_HT_AGG */
+#endif /* CONFIG_IWLWIFI_HT */
+
+int iwl4965_tx_cmd(struct iwl_priv *priv, struct iwl_cmd *out_cmd,
+		   u8 sta_id, dma_addr_t txcmd_phys,
+		   struct ieee80211_hdr *hdr, u8 hdr_len,
+		   struct ieee80211_tx_control *ctrl, void *sta_in)
+{
+	struct iwl_tx_cmd cmd;
+	struct iwl_tx_cmd *tx = (struct iwl_tx_cmd *)&out_cmd->cmd.payload[0];
+	dma_addr_t scratch_phys;
+	u8 unicast = 0;
+	u8 is_data = 1;
+	u16 fc;
+	u16 rate_flags;
+	int rate_index = min(ctrl->tx_rate & 0xffff, IWL_RATE_COUNT - 1);
+#ifdef CONFIG_IWLWIFI_HT
+#ifdef CONFIG_IWLWIFI_HT_AGG
+	__le16 *qc;
+#endif /*CONFIG_IWLWIFI_HT_AGG */
+#endif /* CONFIG_IWLWIFI_HT */
+
+	unicast = !is_multicast_ether_addr(hdr->addr1);
+
+	fc = le16_to_cpu(hdr->frame_control);
+	if ((fc & IEEE80211_FCTL_FTYPE) != IEEE80211_FTYPE_DATA)
+		is_data = 0;
+
+	memcpy(&cmd, &(out_cmd->cmd.tx), sizeof(struct iwl_tx_cmd));
+	memset(tx, 0, sizeof(struct iwl_tx_cmd));
+	memcpy(tx->hdr, hdr, hdr_len);
+
+	tx->len = cmd.len;
+	tx->driver_txop = cmd.driver_txop;
+	tx->stop_time.life_time = cmd.stop_time.life_time;
+	tx->tx_flags = cmd.tx_flags;
+	tx->sta_id = cmd.sta_id;
+	tx->tid_tspec = cmd.tid_tspec;
+	tx->timeout.pm_frame_timeout = cmd.timeout.pm_frame_timeout;
+	tx->next_frame_len = cmd.next_frame_len;
+
+	tx->sec_ctl = cmd.sec_ctl;
+	memcpy(&(tx->key[0]), &(cmd.key[0]), 16);
+	tx->tx_flags = cmd.tx_flags;
+
+	tx->rts_retry_limit = cmd.rts_retry_limit;
+	tx->data_retry_limit = cmd.data_retry_limit;
+
+	scratch_phys = txcmd_phys + sizeof(struct iwl_cmd_header) +
+	    offsetof(struct iwl_tx_cmd, scratch);
+	tx->dram_lsb_ptr = cpu_to_le32(scratch_phys);
+	tx->dram_msb_ptr = iwl4965_get_dma_hi_address(scratch_phys);
+
+	/* Hard coded to start at the highest retry fallback position
+	 * until the 4965 specific rate control algorithm is tied in */
+	tx->initial_rate_index = LINK_QUAL_MAX_RETRY_NUM - 1;
+
+	/* Alternate between antenna A and B for successive frames */
+	if (priv->use_ant_b_for_management_frame) {
+		priv->use_ant_b_for_management_frame = 0;
+		rate_flags = RATE_MCS_ANT_B_MSK;
+	} else {
+		priv->use_ant_b_for_management_frame = 1;
+		rate_flags = RATE_MCS_ANT_A_MSK;
+	}
+
+	if (!unicast || !is_data) {
+		if ((rate_index >= IWL_FIRST_CCK_RATE) &&
+		    (rate_index <= IWL_LAST_CCK_RATE))
+			rate_flags |= RATE_MCS_CCK_MSK;
+	} else {
+		tx->initial_rate_index = 0;
+		tx->tx_flags |= TX_CMD_FLG_STA_RATE_MSK;
+	}
+
+	tx->rate_n_flags = iwl_hw_set_rate_n_flags(iwl_rates[rate_index].plcp,
+						rate_flags);
+
+	if (ieee80211_is_probe_request(fc))
+		tx->tx_flags |= TX_CMD_FLG_TSF_MSK;
+	else if (ieee80211_is_back_request(fc))
+		tx->tx_flags |= TX_CMD_FLG_ACK_MSK |
+			TX_CMD_FLG_IMM_BA_RSP_MASK;
+#ifdef CONFIG_IWLWIFI_HT
+#ifdef CONFIG_IWLWIFI_HT_AGG
+	qc = ieee80211_get_qos_ctrl(hdr);
+	if (qc &&
+	    (priv->iw_mode != IEEE80211_IF_TYPE_IBSS)) {
+		u8 tid = 0;
+		tid = (u8) (le16_to_cpu(*qc) & 0xF);
+		if (tid < TID_MAX_LOAD_COUNT)
+			iwl4965_tl_add_packet(priv, tid);
+	}
+
+	if (priv->lq_mngr.agg_ctrl.next_retry &&
+	    (time_after(priv->lq_mngr.agg_ctrl.next_retry, jiffies))) {
+		unsigned long flags;
+
+		spin_lock_irqsave(&priv->lq_mngr.lock, flags);
+		priv->lq_mngr.agg_ctrl.next_retry = 0;
+		spin_unlock_irqrestore(&priv->lq_mngr.lock, flags);
+		schedule_work(&priv->agg_work);
+	}
+#endif
+#endif
+	return 0;
+}
+
+/**
+ * sign_extend - Sign extend a value using specified bit as sign-bit
+ *
+ * Example: sign_extend(9, 3) would return -7 as bit3 of 1001b is 1
+ * and bit0..2 is 001b which when sign extended to 1111111111111001b is -7.
+ *
+ * @param oper value to sign extend
+ * @param index 0 based bit index (0<=index<32) to sign bit
+ */
+static s32 sign_extend(u32 oper, int index)
+{
+	u8 shift = 31 - index;
+
+	return (s32)(oper << shift) >> shift;
+}
+
+/**
+ * iwl4965_get_temperature - return the calibrated temperature (in Kelvin)
+ * @statistics: Provides the temperature reading from the uCode
+ *
+ * A return of <0 indicates bogus data in the statistics
+ */
+int iwl4965_get_temperature(const struct iwl_priv *priv)
+{
+	s32 temperature;
+	s32 vt;
+	s32 R1, R2, R3;
+	u32 R4;
+
+	if (test_bit(STATUS_TEMPERATURE, &priv->status) &&
+		(priv->statistics.flag & STATISTICS_REPLY_FLG_FAT_MODE_MSK)) {
+		IWL_DEBUG_TEMP("Running FAT temperature calibration\n");
+		R1 = (s32)le32_to_cpu(priv->card_alive_init.therm_r1[1]);
+		R2 = (s32)le32_to_cpu(priv->card_alive_init.therm_r2[1]);
+		R3 = (s32)le32_to_cpu(priv->card_alive_init.therm_r3[1]);
+		R4 = le32_to_cpu(priv->card_alive_init.therm_r4[1]);
+	} else {
+		IWL_DEBUG_TEMP("Running temperature calibration\n");
+		R1 = (s32)le32_to_cpu(priv->card_alive_init.therm_r1[0]);
+		R2 = (s32)le32_to_cpu(priv->card_alive_init.therm_r2[0]);
+		R3 = (s32)le32_to_cpu(priv->card_alive_init.therm_r3[0]);
+		R4 = le32_to_cpu(priv->card_alive_init.therm_r4[0]);
+	}
+
+	/*
+	 * Temperature is only 23 bits so sign extend out to 32
+	 *
+	 * NOTE If we haven't received a statistics notification yet
+	 * with an updated temperature, use R4 provided to us in the
+	 * ALIVE response. */
+	if (!test_bit(STATUS_TEMPERATURE, &priv->status))
+		vt = sign_extend(R4, 23);
+	else
+		vt = sign_extend(
+			le32_to_cpu(priv->statistics.general.temperature), 23);
+
+	IWL_DEBUG_TEMP("Calib values R[1-3]: %d %d %d R4: %d\n",
+		       R1, R2, R3, vt);
+
+	if (R3 == R1) {
+		IWL_ERROR("Calibration conflict R1 == R3\n");
+		return -1;
+	}
+
+	/* Calculate temperature in degrees Kelvin, adjust by 97%.
+	 * Add offset to center the adjustment around 0 degrees Centigrade. */
+	temperature = TEMPERATURE_CALIB_A_VAL * (vt - R2);
+	temperature /= (R3 - R1);
+	temperature = (temperature * 97) / 100 +
+	    TEMPERATURE_CALIB_KELVIN_OFFSET;
+
+	IWL_DEBUG_TEMP("Calibrated temperature: %dK, %dC\n", temperature,
+	    KELVIN_TO_CELSIUS(temperature));
+
+	return temperature;
+}
+
+/* Adjust Txpower only if temperature variance is greater than threshold. */
+#define IWL_TEMPERATURE_THRESHOLD   3
+
+/**
+ * iwl4965_is_temp_calib_needed - determines if new calibration is needed
+ *
+ * If the temperature changed has changed sufficiently, then a recalibration
+ * is needed.
+ *
+ * Assumes caller will replace priv->last_temperature once calibration
+ * executed.
+ */
+static int iwl4965_is_temp_calib_needed(struct iwl_priv *priv)
+{
+	int temp_diff;
+
+	if (!test_bit(STATUS_STATISTICS, &priv->status)) {
+		IWL_DEBUG_TEMP("Temperature not updated -- no statistics.\n");
+		return 0;
+	}
+
+	temp_diff = priv->temperature - priv->last_temperature;
+
+	/* get absolute value */
+	if (temp_diff < 0) {
+		IWL_DEBUG_POWER("Getting cooler, delta %d, \n", temp_diff);
+		temp_diff = -temp_diff;
+	} else if (temp_diff == 0)
+		IWL_DEBUG_POWER("Same temp, \n");
+	else
+		IWL_DEBUG_POWER("Getting warmer, delta %d, \n", temp_diff);
+
+	if (temp_diff < IWL_TEMPERATURE_THRESHOLD) {
+		IWL_DEBUG_POWER("Thermal txpower calib not needed\n");
+		return 0;
+	}
+
+	IWL_DEBUG_POWER("Thermal txpower calib needed\n");
+
+	return 1;
+}
+
+/* Calculate noise level, based on measurements during network silence just
+ *   before arriving beacon.  This measurement can be done only if we know
+ *   exactly when to expect beacons, therefore only when we're associated. */
+static void iwl4965_rx_calc_noise(struct iwl_priv *priv)
+{
+	struct statistics_rx_non_phy *rx_info
+				= &(priv->statistics.rx.general);
+	int num_active_rx = 0;
+	int total_silence = 0;
+	int bcn_silence_a =
+		le32_to_cpu(rx_info->beacon_silence_rssi_a) & IN_BAND_FILTER;
+	int bcn_silence_b =
+		le32_to_cpu(rx_info->beacon_silence_rssi_b) & IN_BAND_FILTER;
+	int bcn_silence_c =
+		le32_to_cpu(rx_info->beacon_silence_rssi_c) & IN_BAND_FILTER;
+
+	if (bcn_silence_a) {
+		total_silence += bcn_silence_a;
+		num_active_rx++;
+	}
+	if (bcn_silence_b) {
+		total_silence += bcn_silence_b;
+		num_active_rx++;
+	}
+	if (bcn_silence_c) {
+		total_silence += bcn_silence_c;
+		num_active_rx++;
+	}
+
+	/* Average among active antennas */
+	if (num_active_rx)
+		priv->last_rx_noise = (total_silence / num_active_rx) - 107;
+	else
+		priv->last_rx_noise = IWL_NOISE_MEAS_NOT_AVAILABLE;
+
+	IWL_DEBUG_CALIB("inband silence a %u, b %u, c %u, dBm %d\n",
+			bcn_silence_a, bcn_silence_b, bcn_silence_c,
+			priv->last_rx_noise);
+}
+
+void iwl_hw_rx_statistics(struct iwl_priv *priv, struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = (void *)rxb->skb->data;
+	int change;
+	s32 temp;
+
+	IWL_DEBUG_RX("Statistics notification received (%d vs %d).\n",
+		     (int)sizeof(priv->statistics), pkt->len);
+
+	change = ((priv->statistics.general.temperature !=
+		   pkt->u.stats.general.temperature) ||
+		  ((priv->statistics.flag &
+		    STATISTICS_REPLY_FLG_FAT_MODE_MSK) !=
+		   (pkt->u.stats.flag & STATISTICS_REPLY_FLG_FAT_MODE_MSK)));
+
+	memcpy(&priv->statistics, &pkt->u.stats, sizeof(priv->statistics));
+
+	set_bit(STATUS_STATISTICS, &priv->status);
+
+	/* Reschedule the statistics timer to occur in
+	 * REG_RECALIB_PERIOD seconds to ensure we get a
+	 * thermal update even if the uCode doesn't give
+	 * us one */
+	mod_timer(&priv->statistics_periodic, jiffies +
+		  msecs_to_jiffies(REG_RECALIB_PERIOD * 1000));
+
+	if (unlikely(!test_bit(STATUS_SCANNING, &priv->status)) &&
+	    (pkt->hdr.cmd == STATISTICS_NOTIFICATION)) {
+		iwl4965_rx_calc_noise(priv);
+#ifdef CONFIG_IWLWIFI_SENSITIVITY
+		queue_work(priv->workqueue, &priv->sensitivity_work);
+#endif
+	}
+
+	/* If the hardware hasn't reported a change in
+	 * temperature then don't bother computing a
+	 * calibrated temperature value */
+	if (!change)
+		return;
+
+	temp = iwl4965_get_temperature(priv);
+	if (temp < 0)
+		return;
+
+	if (priv->temperature != temp) {
+		if (priv->temperature)
+			IWL_DEBUG_TEMP("Temperature changed "
+				       "from %dC to %dC\n",
+				       KELVIN_TO_CELSIUS(priv->temperature),
+				       KELVIN_TO_CELSIUS(temp));
+		else
+			IWL_DEBUG_TEMP("Temperature "
+				       "initialized to %dC\n",
+				       KELVIN_TO_CELSIUS(temp));
+	}
+
+	priv->temperature = temp;
+	set_bit(STATUS_TEMPERATURE, &priv->status);
+
+	if (unlikely(!test_bit(STATUS_SCANNING, &priv->status)) &&
+		     iwl4965_is_temp_calib_needed(priv))
+		queue_work(priv->workqueue, &priv->txpower_work);
+}
+
+static void iwl4965_handle_data_packet(struct iwl_priv *priv, int is_data,
+				       int include_phy,
+				       struct iwl_rx_mem_buffer *rxb,
+				       struct ieee80211_rx_status *stats)
+{
+	struct iwl_rx_packet *pkt = (struct iwl_rx_packet *)rxb->skb->data;
+	struct iwl4965_rx_phy_res *rx_start = (include_phy) ?
+	    (struct iwl4965_rx_phy_res *)&(pkt->u.raw[0]) : NULL;
+	struct ieee80211_hdr *hdr;
+	u16 len;
+	__le32 *rx_end;
+	unsigned int skblen;
+	u32 ampdu_status;
+
+	if (!include_phy && priv->last_phy_res[0])
+		rx_start = (struct iwl4965_rx_phy_res *)&priv->last_phy_res[1];
+
+	if (!rx_start) {
+		IWL_ERROR("MPDU frame without a PHY data\n");
+		return;
+	}
+	if (include_phy) {
+		hdr = (struct ieee80211_hdr *)((u8 *) & rx_start[1] +
+					       rx_start->cfg_phy_cnt);
+
+		len = le16_to_cpu(rx_start->byte_count);
+
+		rx_end = (__le32 *) ((u8 *) & pkt->u.raw[0] +
+				  sizeof(struct iwl4965_rx_phy_res) +
+				  rx_start->cfg_phy_cnt + len);
+
+	} else {
+		struct iwl4965_rx_mpdu_res_start *amsdu =
+		    (struct iwl4965_rx_mpdu_res_start *)pkt->u.raw;
+
+		hdr = (struct ieee80211_hdr *)(pkt->u.raw +
+			       sizeof(struct iwl4965_rx_mpdu_res_start));
+		len =  le16_to_cpu(amsdu->byte_count);
+		rx_start->byte_count = amsdu->byte_count;
+		rx_end = (__le32 *) (((u8 *) hdr) + len);
+	}
+	if (len > 2342 || len < 16) {
+		IWL_DEBUG_DROP("byte count out of range [16,2342]"
+			       " : %d\n", len);
+		return;
+	}
+
+	ampdu_status = le32_to_cpu(*rx_end);
+	skblen = ((u8 *) rx_end - (u8 *) & pkt->u.raw[0]) + sizeof(u32);
+
+	/* start from MAC */
+	skb_reserve(rxb->skb, (void *)hdr - (void *)pkt);
+	skb_put(rxb->skb, len);	/* end where data ends */
+
+	/* We only process data packets if the interface is open */
+	if (unlikely(!priv->is_open)) {
+		IWL_DEBUG_DROP_LIMIT
+		    ("Dropping packet while interface is not open.\n");
+		return;
+	}
+
+	if (priv->iw_mode == IEEE80211_IF_TYPE_MNTR) {
+		if (iwl_param_hwcrypto)
+			iwl_set_decrypted_flag(priv, rxb->skb,
+					       ampdu_status, stats);
+		iwl_handle_data_packet_monitor(priv, rxb, hdr, len, stats, 0);
+		return;
+	}
+
+	stats->flag = 0;
+	hdr = (struct ieee80211_hdr *)rxb->skb->data;
+
+	if (iwl_param_hwcrypto)
+		iwl_set_decrypted_flag(priv, rxb->skb, ampdu_status, stats);
+
+	ieee80211_rx_irqsafe(priv->hw, rxb->skb, stats);
+	priv->alloc_rxb_skb--;
+	rxb->skb = NULL;
+#ifdef LED
+	priv->led_packets += len;
+	iwl_setup_activity_timer(priv);
+#endif
+}
+
+/* Calc max signal level (dBm) among 3 possible receivers */
+static int iwl4965_calc_rssi(struct iwl4965_rx_phy_res *rx_resp)
+{
+	/* data from PHY/DSP regarding signal strength, etc.,
+	 *   contents are always there, not configurable by host.  */
+	struct iwl4965_rx_non_cfg_phy *ncphy =
+	    (struct iwl4965_rx_non_cfg_phy *)rx_resp->non_cfg_phy;
+	u32 agc = (le16_to_cpu(ncphy->agc_info) & IWL_AGC_DB_MASK)
+			>> IWL_AGC_DB_POS;
+
+	u32 valid_antennae =
+	    (le16_to_cpu(rx_resp->phy_flags) & RX_PHY_FLAGS_ANTENNAE_MASK)
+			>> RX_PHY_FLAGS_ANTENNAE_OFFSET;
+	u8 max_rssi = 0;
+	u32 i;
+
+	/* Find max rssi among 3 possible receivers.
+	 * These values are measured by the digital signal processor (DSP).
+	 * They should stay fairly constant even as the signal strength varies,
+	 *   if the radio's automatic gain control (AGC) is working right.
+	 * AGC value (see below) will provide the "interesting" info. */
+	for (i = 0; i < 3; i++)
+		if (valid_antennae & (1 << i))
+			max_rssi = max(ncphy->rssi_info[i << 1], max_rssi);
+
+	IWL_DEBUG_STATS("Rssi In A %d B %d C %d Max %d AGC dB %d\n",
+		ncphy->rssi_info[0], ncphy->rssi_info[2], ncphy->rssi_info[4],
+		max_rssi, agc);
+
+	/* dBm = max_rssi dB - agc dB - constant.
+	 * Higher AGC (higher radio gain) means lower signal. */
+	return (max_rssi - agc - IWL_RSSI_OFFSET);
+}
+
+#ifdef CONFIG_IWLWIFI_HT
+
+/* Parsed Information Elements */
+struct ieee802_11_elems {
+	u8 *ds_params;
+	u8 ds_params_len;
+	u8 *tim;
+	u8 tim_len;
+	u8 *ibss_params;
+	u8 ibss_params_len;
+	u8 *erp_info;
+	u8 erp_info_len;
+	u8 *ht_cap_param;
+	u8 ht_cap_param_len;
+	u8 *ht_extra_param;
+	u8 ht_extra_param_len;
+};
+
+static int parse_elems(u8 *start, size_t len, struct ieee802_11_elems *elems)
+{
+	size_t left = len;
+	u8 *pos = start;
+	int unknown = 0;
+
+	memset(elems, 0, sizeof(*elems));
+
+	while (left >= 2) {
+		u8 id, elen;
+
+		id = *pos++;
+		elen = *pos++;
+		left -= 2;
+
+		if (elen > left)
+			return -1;
+
+		switch (id) {
+		case WLAN_EID_DS_PARAMS:
+			elems->ds_params = pos;
+			elems->ds_params_len = elen;
+			break;
+		case WLAN_EID_TIM:
+			elems->tim = pos;
+			elems->tim_len = elen;
+			break;
+		case WLAN_EID_IBSS_PARAMS:
+			elems->ibss_params = pos;
+			elems->ibss_params_len = elen;
+			break;
+		case WLAN_EID_ERP_INFO:
+			elems->erp_info = pos;
+			elems->erp_info_len = elen;
+			break;
+		case WLAN_EID_HT_CAPABILITY:
+			elems->ht_cap_param = pos;
+			elems->ht_cap_param_len = elen;
+			break;
+		case WLAN_EID_HT_EXTRA_INFO:
+			elems->ht_extra_param = pos;
+			elems->ht_extra_param_len = elen;
+			break;
+		default:
+			unknown++;
+			break;
+		}
+
+		left -= elen;
+		pos += elen;
+	}
+
+	return 0;
+}
+#endif /* CONFIG_IWLWIFI_HT */
+
+static void iwl4965_sta_modify_ps_wake(struct iwl_priv *priv, int sta_id)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->sta_lock, flags);
+	priv->stations[sta_id].sta.station_flags &= ~STA_FLG_PWR_SAVE_MSK;
+	priv->stations[sta_id].sta.station_flags_msk = STA_FLG_PWR_SAVE_MSK;
+	priv->stations[sta_id].sta.sta.modify_mask = 0;
+	priv->stations[sta_id].sta.mode = STA_CONTROL_MODIFY_MSK;
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+
+	iwl_send_add_station(priv, &priv->stations[sta_id].sta, CMD_ASYNC);
+}
+
+static void iwl4965_update_ps_mode(struct iwl_priv *priv, u16 ps_bit, u8 *addr)
+{
+	/* FIXME: need locking over ps_status ??? */
+	u8 sta_id = iwl_hw_find_station(priv, addr);
+
+	if (sta_id != IWL_INVALID_STATION) {
+		u8 sta_awake = priv->stations[sta_id].
+				ps_status == STA_PS_STATUS_WAKE;
+
+		if (sta_awake && ps_bit)
+			priv->stations[sta_id].ps_status = STA_PS_STATUS_SLEEP;
+		else if (!sta_awake && !ps_bit) {
+			iwl4965_sta_modify_ps_wake(priv, sta_id);
+			priv->stations[sta_id].ps_status = STA_PS_STATUS_WAKE;
+		}
+	}
+}
+
+/* Called for REPLY_4965_RX (legacy ABG frames), or
+ * REPLY_RX_MPDU_CMD (HT high-throughput N frames). */
+static void iwl4965_rx_reply_rx(struct iwl_priv *priv,
+				struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = (void *)rxb->skb->data;
+	/* Use phy data (Rx signal strength, etc.) contained within
+	 *   this rx packet for legacy frames,
+	 *   or phy data cached from REPLY_RX_PHY_CMD for HT frames. */
+	int include_phy = (pkt->hdr.cmd == REPLY_4965_RX);
+	struct iwl4965_rx_phy_res *rx_start = (include_phy) ?
+		(struct iwl4965_rx_phy_res *)&(pkt->u.raw[0]) :
+		(struct iwl4965_rx_phy_res *)&priv->last_phy_res[1];
+	__le32 *rx_end;
+	unsigned int len = 0;
+	struct ieee80211_hdr *header;
+	u16 fc;
+	struct ieee80211_rx_status stats = {
+		.mactime = le64_to_cpu(rx_start->timestamp),
+		.channel = le16_to_cpu(rx_start->channel),
+		.phymode =
+			(rx_start->phy_flags & RX_RES_PHY_FLAGS_BAND_24_MSK) ?
+			MODE_IEEE80211G : MODE_IEEE80211A,
+		.antenna = 0,
+		.rate = iwl_hw_get_rate(rx_start->rate_n_flags),
+		.flag = 0,
+#ifdef CONFIG_IWLWIFI_HT_AGG
+		.ordered = 0
+#endif /* CONFIG_IWLWIFI_HT_AGG */
+	};
+	u8 network_packet;
+
+	if ((unlikely(rx_start->cfg_phy_cnt > 20))) {
+		IWL_DEBUG_DROP
+			("dsp size out of range [0,20]: "
+			 "%d/n", rx_start->cfg_phy_cnt);
+		return;
+	}
+	if (!include_phy) {
+		if (priv->last_phy_res[0])
+			rx_start = (struct iwl4965_rx_phy_res *)
+				&priv->last_phy_res[1];
+		else
+			rx_start = NULL;
+	}
+
+	if (!rx_start) {
+		IWL_ERROR("MPDU frame without a PHY data\n");
+		return;
+	}
+
+	if (include_phy) {
+		header = (struct ieee80211_hdr *)((u8 *) & rx_start[1]
+						  + rx_start->cfg_phy_cnt);
+
+		len = le16_to_cpu(rx_start->byte_count);
+		rx_end = (__le32 *) (pkt->u.raw + rx_start->cfg_phy_cnt +
+				  sizeof(struct iwl4965_rx_phy_res) + len);
+	} else {
+		struct iwl4965_rx_mpdu_res_start *amsdu =
+			(struct iwl4965_rx_mpdu_res_start *)pkt->u.raw;
+
+		header = (void *)(pkt->u.raw +
+			sizeof(struct iwl4965_rx_mpdu_res_start));
+		len = le16_to_cpu(amsdu->byte_count);
+		rx_end = (__le32 *) (pkt->u.raw +
+			sizeof(struct iwl4965_rx_mpdu_res_start) + len);
+	}
+
+	if (!(*rx_end & RX_RES_STATUS_NO_CRC32_ERROR) ||
+	    !(*rx_end & RX_RES_STATUS_NO_RXE_OVERFLOW)) {
+		IWL_DEBUG_RX("Bad CRC or FIFO: 0x%08X.\n",
+				le32_to_cpu(*rx_end));
+		return;
+	}
+
+	priv->ucode_beacon_time = le32_to_cpu(rx_start->beacon_time_stamp);
+
+	stats.freq = ieee80211chan2mhz(stats.channel);
+
+	/* Find max signal strength (dBm) among 3 antenna/receiver chains */
+	stats.ssi = iwl4965_calc_rssi(rx_start);
+
+	/* Meaningful noise values are available only from beacon statistics,
+	 *   which are gathered only when associated, and indicate noise
+	 *   only for the associated network channel ...
+	 * Ignore these noise values while scanning (other channels) */
+	if (iwl_is_associated(priv) &&
+	    !test_bit(STATUS_SCANNING, &priv->status)) {
+		stats.noise = priv->last_rx_noise;
+		stats.signal = iwl_calc_sig_qual(stats.ssi, stats.noise);
+	} else {
+		stats.noise = IWL_NOISE_MEAS_NOT_AVAILABLE;
+		stats.signal = iwl_calc_sig_qual(stats.ssi, 0);
+	}
+
+	/* Reset beacon noise level if not associated. */
+	if (!iwl_is_associated(priv))
+		priv->last_rx_noise = IWL_NOISE_MEAS_NOT_AVAILABLE;
+
+#ifdef CONFIG_IWLWIFI_DEBUG
+	/* TODO:  Parts of iwl_report_frame are broken for 4965 */
+	if (iwl_debug_level & (IWL_DL_RX))
+		/* Set "1" to report good data frames in groups of 100 */
+		iwl_report_frame(priv, pkt, header, 1);
+
+	if (iwl_debug_level & (IWL_DL_RX | IWL_DL_STATS))
+	IWL_DEBUG_RX("Rssi %d, noise %d, qual %d, TSF %lu\n",
+		stats.ssi, stats.noise, stats.signal,
+		 (long unsigned int)le64_to_cpu(rx_start->timestamp));
+#endif
+
+	network_packet = iwl_is_network_packet(priv, header);
+	if (network_packet) {
+		priv->last_rx_rssi = stats.ssi;
+		priv->last_beacon_time =  priv->ucode_beacon_time;
+		priv->last_tsf = le64_to_cpu(rx_start->timestamp);
+	}
+
+	fc = le16_to_cpu(header->frame_control);
+	switch (fc & IEEE80211_FCTL_FTYPE) {
+	case IEEE80211_FTYPE_MGMT:
+
+		if (priv->iw_mode == IEEE80211_IF_TYPE_AP)
+			iwl4965_update_ps_mode(priv, fc  & IEEE80211_FCTL_PM,
+						header->addr2);
+		switch (fc & IEEE80211_FCTL_STYPE) {
+		case IEEE80211_STYPE_PROBE_RESP:
+		case IEEE80211_STYPE_BEACON:
+			if ((priv->iw_mode == IEEE80211_IF_TYPE_STA &&
+			     !compare_ether_addr(header->addr2, priv->bssid)) ||
+			    (priv->iw_mode == IEEE80211_IF_TYPE_IBSS &&
+			     !compare_ether_addr(header->addr3, priv->bssid))) {
+				struct ieee80211_mgmt *mgmt =
+					(struct ieee80211_mgmt *)header;
+				u64 timestamp =
+					le64_to_cpu(mgmt->u.beacon.timestamp);
+
+				priv->timestamp0 = timestamp & 0xFFFFFFFF;
+				priv->timestamp1 =
+					(timestamp >> 32) & 0xFFFFFFFF;
+				priv->beacon_int = le16_to_cpu(
+				    mgmt->u.beacon.beacon_int);
+				if (priv->call_post_assoc_from_beacon &&
+				    (priv->iw_mode == IEEE80211_IF_TYPE_STA)) {
+					priv->call_post_assoc_from_beacon = 0;
+					queue_work(priv->workqueue,
+					    &priv->post_associate.work);
+				}
+			}
+			break;
+
+		case IEEE80211_STYPE_ACTION:
+			break;
+
+			/*
+			 * TODO: There is no callback function from upper
+			 * stack to inform us when associated status. this
+			 * work around to sniff assoc_resp management frame
+			 * and finish the association process.
+			 */
+		case IEEE80211_STYPE_ASSOC_RESP:
+		case IEEE80211_STYPE_REASSOC_RESP:
+			if (network_packet && iwl_is_associated(priv)) {
+#ifdef CONFIG_IWLWIFI_HT
+				u8 *pos = NULL;
+				struct ieee802_11_elems elems;
+#endif				/*CONFIG_IWLWIFI_HT */
+				struct ieee80211_mgmt *mgnt =
+					(struct ieee80211_mgmt *)header;
+
+				priv->assoc_id = (~((1 << 15) | (1 << 14))
+					& le16_to_cpu(mgnt->u.assoc_resp.aid));
+				priv->assoc_capability =
+					le16_to_cpu(
+						mgnt->u.assoc_resp.capab_info);
+#ifdef CONFIG_IWLWIFI_HT
+				pos = mgnt->u.assoc_resp.variable;
+				if (!parse_elems(pos,
+						 len - (pos - (u8 *) mgnt),
+						 &elems)) {
+					if (elems.ht_extra_param &&
+					    elems.ht_cap_param)
+						break;
+				}
+#endif				/*CONFIG_IWLWIFI_HT */
+				/* assoc_id is 0 no association */
+				if (!priv->assoc_id)
+					break;
+				if (priv->beacon_int)
+					queue_work(priv->workqueue,
+					    &priv->post_associate.work);
+				else
+					priv->call_post_assoc_from_beacon = 1;
+			}
+
+			break;
+
+		case IEEE80211_STYPE_PROBE_REQ:
+			if ((priv->iw_mode == IEEE80211_IF_TYPE_IBSS) &&
+			    !iwl_is_associated(priv)) {
+				DECLARE_MAC_BUF(mac1);
+				DECLARE_MAC_BUF(mac2);
+				DECLARE_MAC_BUF(mac3);
+
+				IWL_DEBUG_DROP("Dropping (non network): "
+					       "%s, %s, %s\n",
+					       print_mac(mac1, header->addr1),
+					       print_mac(mac2, header->addr2),
+					       print_mac(mac3, header->addr3));
+				return;
+			}
+		}
+		iwl4965_handle_data_packet(priv, 0, include_phy, rxb, &stats);
+		break;
+
+	case IEEE80211_FTYPE_CTL:
+#ifdef CONFIG_IWLWIFI_HT_AGG
+		switch (fc & IEEE80211_FCTL_STYPE) {
+		case IEEE80211_STYPE_BACK_REQ:
+			IWL_DEBUG_HT("IEEE80211_STYPE_BACK_REQ arrived\n");
+			iwl4965_handle_data_packet(priv, 0, include_phy,
+						rxb, &stats);
+			break;
+		default:
+			break;
+		}
+#endif
+
+		break;
+
+	case IEEE80211_FTYPE_DATA: {
+		DECLARE_MAC_BUF(mac1);
+		DECLARE_MAC_BUF(mac2);
+		DECLARE_MAC_BUF(mac3);
+
+		if (priv->iw_mode == IEEE80211_IF_TYPE_AP)
+			iwl4965_update_ps_mode(priv, fc  & IEEE80211_FCTL_PM,
+						header->addr2);
+
+		if (unlikely(!network_packet))
+			IWL_DEBUG_DROP("Dropping (non network): "
+				       "%s, %s, %s\n",
+				       print_mac(mac1, header->addr1),
+				       print_mac(mac2, header->addr2),
+				       print_mac(mac3, header->addr3));
+		else if (unlikely(is_duplicate_packet(priv, header)))
+			IWL_DEBUG_DROP("Dropping (dup): %s, %s, %s\n",
+				       print_mac(mac1, header->addr1),
+				       print_mac(mac2, header->addr2),
+				       print_mac(mac3, header->addr3));
+		else
+			iwl4965_handle_data_packet(priv, 1, include_phy, rxb,
+						   &stats);
+		break;
+	}
+	default:
+		break;
+
+	}
+}
+
+/* Cache phy data (Rx signal strength, etc) for HT frame (REPLY_RX_PHY_CMD).
+ * This will be used later in iwl4965_rx_reply_rx() for REPLY_RX_MPDU_CMD. */
+static void iwl4965_rx_reply_rx_phy(struct iwl_priv *priv,
+				    struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = (void *)rxb->skb->data;
+	priv->last_phy_res[0] = 1;
+	memcpy(&priv->last_phy_res[1], &(pkt->u.raw[0]),
+	       sizeof(struct iwl4965_rx_phy_res));
+}
+
+static void iwl4965_rx_missed_beacon_notif(struct iwl_priv *priv,
+					   struct iwl_rx_mem_buffer *rxb)
+
+{
+#ifdef CONFIG_IWLWIFI_SENSITIVITY
+	struct iwl_rx_packet *pkt = (void *)rxb->skb->data;
+	struct iwl_missed_beacon_notif *missed_beacon;
+
+	missed_beacon = &pkt->u.missed_beacon;
+	if (le32_to_cpu(missed_beacon->consequtive_missed_beacons) > 5) {
+		IWL_DEBUG_CALIB("missed bcn cnsq %d totl %d rcd %d expctd %d\n",
+		    le32_to_cpu(missed_beacon->consequtive_missed_beacons),
+		    le32_to_cpu(missed_beacon->total_missed_becons),
+		    le32_to_cpu(missed_beacon->num_recvd_beacons),
+		    le32_to_cpu(missed_beacon->num_expected_beacons));
+		priv->sensitivity_data.state = IWL_SENS_CALIB_NEED_REINIT;
+		if (unlikely(!test_bit(STATUS_SCANNING, &priv->status)))
+			queue_work(priv->workqueue, &priv->sensitivity_work);
+	}
+#endif /*CONFIG_IWLWIFI_SENSITIVITY*/
+}
+
+#ifdef CONFIG_IWLWIFI_HT
+#ifdef CONFIG_IWLWIFI_HT_AGG
+
+static void iwl4965_set_tx_status(struct iwl_priv *priv, int txq_id, int idx,
+				  u32 status, u32 retry_count, u32 rate)
+{
+	struct ieee80211_tx_status *tx_status =
+		&(priv->txq[txq_id].txb[idx].status);
+
+	tx_status->flags = status ? IEEE80211_TX_STATUS_ACK : 0;
+	tx_status->retry_count += retry_count;
+	tx_status->control.tx_rate = rate;
+}
+
+
+static void iwl_sta_modify_enable_tid_tx(struct iwl_priv *priv,
+					 int sta_id, int tid)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->sta_lock, flags);
+	priv->stations[sta_id].sta.sta.modify_mask = STA_MODIFY_TID_DISABLE_TX;
+	priv->stations[sta_id].sta.tid_disable_tx &= cpu_to_le16(~(1 << tid));
+	priv->stations[sta_id].sta.mode = STA_CONTROL_MODIFY_MSK;
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+
+	iwl_send_add_station(priv, &priv->stations[sta_id].sta, CMD_ASYNC);
+}
+
+
+static int iwl4965_tx_status_reply_compressed_ba(struct iwl_priv *priv,
+						 struct iwl_ht_agg *agg,
+						 struct iwl_compressed_ba_resp*
+						 ba_resp)
+
+{
+	int i, sh, ack;
+	u16 ba_seq_ctl = le16_to_cpu(ba_resp->ba_seq_ctl);
+	u32 bitmap0, bitmap1;
+	u32 resp_bitmap0 = le32_to_cpu(ba_resp->ba_bitmap0);
+	u32 resp_bitmap1 = le32_to_cpu(ba_resp->ba_bitmap1);
+
+	if (unlikely(!agg->wait_for_ba))  {
+		IWL_ERROR("Received BA when not expected\n");
+		return -EINVAL;
+	}
+	agg->wait_for_ba = 0;
+	IWL_DEBUG_TX_REPLY("BA %d %d\n", agg->start_idx, ba_resp->ba_seq_ctl);
+	sh = agg->start_idx - SEQ_TO_INDEX(ba_seq_ctl>>4);
+	if (sh < 0) /* tbw something is wrong with indeces */
+		sh += 0x100;
+
+	/* don't use 64 bits for now */
+	bitmap0 = resp_bitmap0 >> sh;
+	bitmap1 = resp_bitmap1 >> sh;
+	bitmap0 |= (resp_bitmap1 & ((1<<sh)|((1<<sh)-1))) << (32 - sh);
+
+	if (agg->frame_count > (64 - sh)) {
+		IWL_DEBUG_TX_REPLY("more frames than bitmap size");
+		return -1;
+	}
+
+	/* check for success or failure according to the
+	 * transmitted bitmap and back bitmap */
+	bitmap0 &= agg->bitmap0;
+	bitmap1 &= agg->bitmap1;
+
+	for (i = 0; i < agg->frame_count ; i++) {
+		int idx = (agg->start_idx + i) & 0xff;
+		ack = bitmap0 & (1 << i);
+		IWL_DEBUG_TX_REPLY("%s ON i=%d idx=%d raw=%d\n",
+			ack? "ACK":"NACK", i, idx, agg->start_idx + i);
+		iwl4965_set_tx_status(priv, agg->txq_id, idx, ack, 0,
+			agg->rate_n_flags);
+
+	}
+
+	IWL_DEBUG_TX_REPLY("Bitmap %x%x\n", bitmap0, bitmap1);
+
+	return 0;
+}
+
+static inline int iwl_queue_dec_wrap(int index, int n_bd)
+{
+	return (index == 0) ? n_bd - 1 : index - 1;
+}
+
+static void iwl4965_rx_reply_compressed_ba(struct iwl_priv *priv,
+					   struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = (void *)rxb->skb->data;
+	struct iwl_compressed_ba_resp *ba_resp = &pkt->u.compressed_ba;
+	int index;
+	struct iwl_tx_queue *txq = NULL;
+	struct iwl_ht_agg *agg;
+	u16 ba_resp_scd_flow = le16_to_cpu(ba_resp->scd_flow);
+	u16 ba_resp_scd_ssn = le16_to_cpu(ba_resp->scd_ssn);
+
+	if (ba_resp_scd_flow >= ARRAY_SIZE(priv->txq)) {
+		IWL_ERROR("BUG_ON scd_flow is bigger than number of queues");
+		return;
+	}
+
+	txq = &priv->txq[ba_resp_scd_flow];
+	agg = &priv->stations[ba_resp->sta_id].tid[ba_resp->tid].agg;
+	index = iwl_queue_dec_wrap(ba_resp_scd_ssn & 0xff, txq->q.n_bd);
+
+	/* TODO: Need to get this copy more sefely - now good for debug */
+/*
+	{
+	DECLARE_MAC_BUF(mac);
+	IWL_DEBUG_TX_REPLY("REPLY_COMPRESSED_BA [%d]Received from %s, "
+			   "sta_id = %d\n",
+			   agg->wait_for_ba,
+			   print_mac(mac, (u8*) &ba_resp->sta_addr_lo32),
+			   ba_resp->sta_id);
+	IWL_DEBUG_TX_REPLY("TID = %d, SeqCtl = %d, bitmap = 0x%X%X, scd_flow = "
+			   "%d, scd_ssn = %d\n",
+			   ba_resp->tid,
+			   ba_resp->ba_seq_ctl,
+			   ba_resp->ba_bitmap1,
+			   ba_resp->ba_bitmap0,
+			   ba_resp->scd_flow,
+			   ba_resp->scd_ssn);
+	IWL_DEBUG_TX_REPLY("DAT start_idx = %d, bitmap = 0x%X%X \n",
+			   agg->start_idx,
+			   agg->bitmap1,
+			   agg->bitmap0);
+	}
+*/
+	iwl4965_tx_status_reply_compressed_ba(priv, agg, ba_resp);
+	/* releases all the TFDs until the SSN */
+	if (txq->q.last_used != (ba_resp_scd_ssn & 0xff))
+		iwl_tx_queue_reclaim(priv, ba_resp_scd_flow, index);
+
+}
+
+
+static void iwl4965_tx_queue_stop_scheduler(struct iwl_priv *priv, u16 txq_id)
+{
+	iwl_write_restricted_reg(priv,
+		SCD_QUEUE_STATUS_BITS(txq_id),
+		(0 << SCD_QUEUE_STTS_REG_POS_ACTIVE)|
+		(1 << SCD_QUEUE_STTS_REG_POS_SCD_ACT_EN));
+}
+
+static int iwl4965_tx_queue_set_q2ratid(struct iwl_priv *priv, u16 ra_tid,
+					u16 txq_id)
+{
+	u32 tbl_dw_addr;
+	u32 tbl_dw;
+	u16 scd_q2ratid;
+
+	scd_q2ratid = ra_tid & SCD_QUEUE_RA_TID_MAP_RATID_MSK;
+
+	tbl_dw_addr = priv->scd_base_addr +
+			SCD_TRANSLATE_TBL_OFFSET_QUEUE(txq_id);
+
+	tbl_dw = iwl_read_restricted_mem(priv, tbl_dw_addr);
+
+	if (txq_id & 0x1)
+		tbl_dw = (scd_q2ratid << 16) | (tbl_dw & 0x0000FFFF);
+	else
+		tbl_dw = scd_q2ratid | (tbl_dw & 0xFFFF0000);
+
+	iwl_write_restricted_mem(priv, tbl_dw_addr, tbl_dw);
+
+	return 0;
+}
+
+/**
+ * txq_id must be greater than IWL_BACK_QUEUE_FIRST_ID
+ */
+static int iwl4965_tx_queue_agg_enable(struct iwl_priv *priv, int txq_id,
+				       int tx_fifo, int sta_id, int tid,
+				       u16 ssn_idx)
+{
+	unsigned long flags;
+	int rc;
+	u16 ra_tid;
+
+	if (IWL_BACK_QUEUE_FIRST_ID > txq_id)
+		IWL_WARNING("queue number too small: %d, must be > %d\n",
+			txq_id, IWL_BACK_QUEUE_FIRST_ID);
+
+	ra_tid = BUILD_RAxTID(sta_id, tid);
+
+	iwl_sta_modify_enable_tid_tx(priv, sta_id, tid);
+
+	spin_lock_irqsave(&priv->lock, flags);
+	rc = iwl_grab_restricted_access(priv);
+	if (rc) {
+		spin_unlock_irqrestore(&priv->lock, flags);
+		return rc;
+	}
+
+	iwl4965_tx_queue_stop_scheduler(priv, txq_id);
+
+	iwl4965_tx_queue_set_q2ratid(priv, ra_tid, txq_id);
+
+
+	iwl_set_bits_restricted_reg(priv, SCD_QUEUECHAIN_SEL, (1<<txq_id));
+
+	priv->txq[txq_id].q.last_used = (ssn_idx & 0xff);
+	priv->txq[txq_id].q.first_empty = (ssn_idx & 0xff);
+
+	/* supposes that ssn_idx is valid (!= 0xFFF) */
+	iwl4965_set_wr_ptrs(priv, txq_id, ssn_idx);
+
+	iwl_write_restricted_mem(priv,
+			priv->scd_base_addr + SCD_CONTEXT_QUEUE_OFFSET(txq_id),
+			(SCD_WIN_SIZE << SCD_QUEUE_CTX_REG1_WIN_SIZE_POS) &
+			SCD_QUEUE_CTX_REG1_WIN_SIZE_MSK);
+
+	iwl_write_restricted_mem(priv, priv->scd_base_addr +
+			SCD_CONTEXT_QUEUE_OFFSET(txq_id) + sizeof(u32),
+			(SCD_FRAME_LIMIT << SCD_QUEUE_CTX_REG2_FRAME_LIMIT_POS)
+			& SCD_QUEUE_CTX_REG2_FRAME_LIMIT_MSK);
+
+	iwl_set_bits_restricted_reg(priv, SCD_INTERRUPT_MASK, (1 << txq_id));
+
+	iwl4965_tx_queue_set_status(priv, &priv->txq[txq_id], tx_fifo, 1);
+
+	iwl_release_restricted_access(priv);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return 0;
+}
+
+/**
+ * txq_id must be greater than IWL_BACK_QUEUE_FIRST_ID
+ */
+static int iwl4965_tx_queue_agg_disable(struct iwl_priv *priv, u16 txq_id,
+					u16 ssn_idx, u8 tx_fifo)
+{
+	unsigned long flags;
+	int rc;
+
+	if (IWL_BACK_QUEUE_FIRST_ID > txq_id) {
+		IWL_WARNING("queue number too small: %d, must be > %d\n",
+				txq_id, IWL_BACK_QUEUE_FIRST_ID);
+		return -EINVAL;
+	}
+
+	spin_lock_irqsave(&priv->lock, flags);
+	rc = iwl_grab_restricted_access(priv);
+	if (rc) {
+		spin_unlock_irqrestore(&priv->lock, flags);
+		return rc;
+	}
+
+	iwl4965_tx_queue_stop_scheduler(priv, txq_id);
+
+	iwl_clear_bits_restricted_reg(priv, SCD_QUEUECHAIN_SEL, (1 << txq_id));
+
+	priv->txq[txq_id].q.last_used = (ssn_idx & 0xff);
+	priv->txq[txq_id].q.first_empty = (ssn_idx & 0xff);
+	/* supposes that ssn_idx is valid (!= 0xFFF) */
+	iwl4965_set_wr_ptrs(priv, txq_id, ssn_idx);
+
+	iwl_clear_bits_restricted_reg(priv, SCD_INTERRUPT_MASK, (1 << txq_id));
+	iwl4965_txq_ctx_deactivate(priv, txq_id);
+	iwl4965_tx_queue_set_status(priv, &priv->txq[txq_id], tx_fifo, 0);
+
+	iwl_release_restricted_access(priv);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return 0;
+}
+
+#endif/* CONFIG_IWLWIFI_HT_AGG */
+#endif /* CONFIG_IWLWIFI_HT */
+/*
+ * RATE SCALE CODE
+ */
+int iwl4965_init_hw_rates(struct iwl_priv *priv, struct ieee80211_rate *rates)
+{
+	return 0;
+}
+
+
+/**
+ * iwl4965_add_station - Initialize a station's hardware rate table
+ *
+ * The uCode contains a table of fallback rates and retries per rate
+ * for automatic fallback during transmission.
+ *
+ * NOTE: This initializes the table for a single retry per data rate
+ * which is not optimal.  Setting up an intelligent retry per rate
+ * requires feedback from transmission, which isn't exposed through
+ * rc80211_simple which is what this driver is currently using.
+ *
+ */
+void iwl4965_add_station(struct iwl_priv *priv, const u8 *addr, int is_ap)
+{
+	int i, r;
+	struct iwl_link_quality_cmd link_cmd = {
+		.reserved1 = 0,
+	};
+	u16 rate_flags;
+
+	/* Set up the rate scaling to start at 54M and fallback
+	 * all the way to 1M in IEEE order and then spin on IEEE */
+	if (is_ap)
+		r = IWL_RATE_54M_INDEX;
+	else if (priv->phymode == MODE_IEEE80211A)
+		r = IWL_RATE_6M_INDEX;
+	else
+		r = IWL_RATE_1M_INDEX;
+
+	for (i = 0; i < LINK_QUAL_MAX_RETRY_NUM; i++) {
+		rate_flags = 0;
+		if (r >= IWL_FIRST_CCK_RATE && r <= IWL_LAST_CCK_RATE)
+			rate_flags |= RATE_MCS_CCK_MSK;
+
+		rate_flags |= RATE_MCS_ANT_B_MSK;
+		rate_flags &= ~RATE_MCS_ANT_A_MSK;
+		link_cmd.rs_table[i].rate_n_flags =
+			iwl_hw_set_rate_n_flags(iwl_rates[r].plcp, rate_flags);
+		r = iwl_get_prev_ieee_rate(r);
+	}
+
+	link_cmd.general_params.single_stream_ant_msk = 2;
+	link_cmd.general_params.dual_stream_ant_msk = 3;
+	link_cmd.agg_params.agg_dis_start_th = 3;
+	link_cmd.agg_params.agg_time_limit = cpu_to_le16(4000);
+
+	/* Update the rate scaling for control frame Tx to AP */
+	link_cmd.sta_id = is_ap ? IWL_AP_ID : IWL4965_BROADCAST_ID;
+
+	iwl_send_cmd_pdu(priv, REPLY_TX_LINK_QUALITY_CMD, sizeof(link_cmd),
+			 &link_cmd);
+}
+
+#ifdef CONFIG_IWLWIFI_HT
+
+static u8 iwl_is_channel_extension(struct iwl_priv *priv, int phymode,
+				   u16 channel, u8 extension_chan_offset)
+{
+	const struct iwl_channel_info *ch_info;
+
+	ch_info = iwl_get_channel_info(priv, phymode, channel);
+	if (!is_channel_valid(ch_info))
+		return 0;
+
+	if (extension_chan_offset == IWL_EXT_CHANNEL_OFFSET_AUTO)
+		return 0;
+
+	if ((ch_info->fat_extension_channel == extension_chan_offset) ||
+	    (ch_info->fat_extension_channel == HT_IE_EXT_CHANNEL_MAX))
+		return 1;
+
+	return 0;
+}
+
+static u8 iwl_is_fat_tx_allowed(struct iwl_priv *priv,
+				const struct sta_ht_info *ht_info)
+{
+
+	if (priv->channel_width != IWL_CHANNEL_WIDTH_40MHZ)
+		return 0;
+
+	if (ht_info->supported_chan_width != IWL_CHANNEL_WIDTH_40MHZ)
+		return 0;
+
+	if (ht_info->extension_chan_offset == IWL_EXT_CHANNEL_OFFSET_AUTO)
+		return 0;
+
+	/* no fat tx allowed on 2.4GHZ */
+	if (priv->phymode != MODE_IEEE80211A)
+		return 0;
+	return (iwl_is_channel_extension(priv, priv->phymode,
+					 ht_info->control_channel,
+					 ht_info->extension_chan_offset));
+}
+
+void iwl4965_set_rxon_ht(struct iwl_priv *priv, struct sta_ht_info *ht_info)
+{
+	struct iwl_rxon_cmd *rxon = &priv->staging_rxon;
+	u32 val;
+
+	if (!ht_info->is_ht)
+		return;
+
+	if (iwl_is_fat_tx_allowed(priv, ht_info))
+		rxon->flags |= RXON_FLG_CHANNEL_MODE_MIXED_MSK;
+	else
+		rxon->flags &= ~(RXON_FLG_CHANNEL_MODE_MIXED_MSK |
+				 RXON_FLG_CHANNEL_MODE_PURE_40_MSK);
+
+	if (le16_to_cpu(rxon->channel) != ht_info->control_channel) {
+		IWL_DEBUG_ASSOC("control diff than current %d %d\n",
+				le16_to_cpu(rxon->channel),
+				ht_info->control_channel);
+		rxon->channel = cpu_to_le16(ht_info->control_channel);
+		return;
+	}
+
+	/* Note: control channel is oposit to extension channel */
+	switch (ht_info->extension_chan_offset) {
+	case IWL_EXT_CHANNEL_OFFSET_ABOVE:
+		rxon->flags &= ~(RXON_FLG_CTRL_CHANNEL_LOC_HI_MSK);
+		break;
+	case IWL_EXT_CHANNEL_OFFSET_BELOW:
+		rxon->flags |= RXON_FLG_CTRL_CHANNEL_LOC_HI_MSK;
+		break;
+	case IWL_EXT_CHANNEL_OFFSET_AUTO:
+		rxon->flags &= ~RXON_FLG_CHANNEL_MODE_MIXED_MSK;
+		break;
+	default:
+		rxon->flags &= ~RXON_FLG_CHANNEL_MODE_MIXED_MSK;
+		break;
+	}
+
+	val = ht_info->operating_mode;
+
+	rxon->flags |= cpu_to_le32(val << RXON_FLG_HT_OPERATING_MODE_POS);
+
+	priv->active_rate_ht[0] = ht_info->supp_rates[0];
+	priv->active_rate_ht[1] = ht_info->supp_rates[1];
+	iwl4965_set_rxon_chain(priv);
+
+	IWL_DEBUG_ASSOC("supported HT rate 0x%X %X "
+			"rxon flags 0x%X operation mode :0x%X "
+			"extension channel offset 0x%x "
+			"control chan %d\n",
+			priv->active_rate_ht[0], priv->active_rate_ht[1],
+			le32_to_cpu(rxon->flags), ht_info->operating_mode,
+			ht_info->extension_chan_offset,
+			ht_info->control_channel);
+	return;
+}
+
+void iwl4965_set_ht_add_station(struct iwl_priv *priv, u8 index)
+{
+	__le32 sta_flags;
+	struct sta_ht_info *ht_info = &priv->current_assoc_ht;
+
+	priv->current_channel_width = IWL_CHANNEL_WIDTH_20MHZ;
+	if (!ht_info->is_ht)
+		goto done;
+
+	sta_flags = priv->stations[index].sta.station_flags;
+
+	if (ht_info->tx_mimo_ps_mode == IWL_MIMO_PS_DYNAMIC)
+		sta_flags |= STA_FLG_RTS_MIMO_PROT_MSK;
+	else
+		sta_flags &= ~STA_FLG_RTS_MIMO_PROT_MSK;
+
+	sta_flags |= cpu_to_le32(
+		(u32)ht_info->ampdu_factor << STA_FLG_MAX_AGG_SIZE_POS);
+
+	sta_flags |= cpu_to_le32(
+		(u32)ht_info->mpdu_density << STA_FLG_AGG_MPDU_DENSITY_POS);
+
+	sta_flags &= (~STA_FLG_FAT_EN_MSK);
+	ht_info->tx_chan_width = IWL_CHANNEL_WIDTH_20MHZ;
+	ht_info->chan_width_cap = IWL_CHANNEL_WIDTH_20MHZ;
+
+	if (iwl_is_fat_tx_allowed(priv, ht_info)) {
+		sta_flags |= STA_FLG_FAT_EN_MSK;
+		ht_info->chan_width_cap = IWL_CHANNEL_WIDTH_40MHZ;
+		if (ht_info->supported_chan_width == IWL_CHANNEL_WIDTH_40MHZ)
+			ht_info->tx_chan_width = IWL_CHANNEL_WIDTH_40MHZ;
+	}
+	priv->current_channel_width = ht_info->tx_chan_width;
+	priv->stations[index].sta.station_flags = sta_flags;
+ done:
+	return;
+}
+
+#ifdef CONFIG_IWLWIFI_HT_AGG
+
+static void iwl4965_sta_modify_add_ba_tid(struct iwl_priv *priv,
+					  int sta_id, int tid, u16 ssn)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->sta_lock, flags);
+	priv->stations[sta_id].sta.station_flags_msk = 0;
+	priv->stations[sta_id].sta.sta.modify_mask = STA_MODIFY_ADDBA_TID_MSK;
+	priv->stations[sta_id].sta.add_immediate_ba_tid = (u8)tid;
+	priv->stations[sta_id].sta.add_immediate_ba_ssn = cpu_to_le16(ssn);
+	priv->stations[sta_id].sta.mode = STA_CONTROL_MODIFY_MSK;
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+
+	iwl_send_add_station(priv, &priv->stations[sta_id].sta, CMD_ASYNC);
+}
+
+static void iwl4965_sta_modify_del_ba_tid(struct iwl_priv *priv,
+					  int sta_id, int tid)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->sta_lock, flags);
+	priv->stations[sta_id].sta.station_flags_msk = 0;
+	priv->stations[sta_id].sta.sta.modify_mask = STA_MODIFY_DELBA_TID_MSK;
+	priv->stations[sta_id].sta.remove_immediate_ba_tid = (u8)tid;
+	priv->stations[sta_id].sta.mode = STA_CONTROL_MODIFY_MSK;
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+
+	iwl_send_add_station(priv, &priv->stations[sta_id].sta, CMD_ASYNC);
+}
+
+static const u16 default_tid_to_tx_fifo[] = {
+	IWL_TX_FIFO_AC1,
+	IWL_TX_FIFO_AC0,
+	IWL_TX_FIFO_AC0,
+	IWL_TX_FIFO_AC1,
+	IWL_TX_FIFO_AC2,
+	IWL_TX_FIFO_AC2,
+	IWL_TX_FIFO_AC3,
+	IWL_TX_FIFO_AC3,
+	IWL_TX_FIFO_NONE,
+	IWL_TX_FIFO_NONE,
+	IWL_TX_FIFO_NONE,
+	IWL_TX_FIFO_NONE,
+	IWL_TX_FIFO_NONE,
+	IWL_TX_FIFO_NONE,
+	IWL_TX_FIFO_NONE,
+	IWL_TX_FIFO_NONE,
+	IWL_TX_FIFO_AC3
+};
+
+static int iwl_txq_ctx_activate_free(struct iwl_priv *priv)
+{
+	int txq_id;
+
+	for (txq_id = 0; txq_id < priv->hw_setting.max_txq_num; txq_id++)
+		if (!test_and_set_bit(txq_id, &priv->txq_ctx_active_msk))
+			return txq_id;
+	return -1;
+}
+
+int iwl_mac_ht_tx_agg_start(struct ieee80211_hw *hw, u8 *da, u16 tid,
+			    u16 *start_seq_num)
+{
+
+	struct iwl_priv *priv = hw->priv;
+	int sta_id;
+	int tx_fifo;
+	int txq_id;
+	int ssn = -1;
+	unsigned long flags;
+	struct iwl_tid_data *tid_data;
+	DECLARE_MAC_BUF(mac);
+
+	if (likely(tid < ARRAY_SIZE(default_tid_to_tx_fifo)))
+		tx_fifo = default_tid_to_tx_fifo[tid];
+	else
+		return -EINVAL;
+
+	IWL_WARNING("iwl-AGG iwl_mac_ht_tx_agg_start on da=%s"
+		    " tid=%d\n", print_mac(mac, da), tid);
+
+	sta_id = iwl_hw_find_station(priv, da);
+	if (sta_id == IWL_INVALID_STATION)
+		return -ENXIO;
+
+	txq_id = iwl_txq_ctx_activate_free(priv);
+	if (txq_id == -1)
+		return -ENXIO;
+
+	spin_lock_irqsave(&priv->sta_lock, flags);
+	tid_data = &priv->stations[sta_id].tid[tid];
+	ssn = SEQ_TO_SN(tid_data->seq_number);
+	tid_data->agg.txq_id = txq_id;
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+
+	*start_seq_num = ssn;
+	iwl4965_ba_status(priv, tid, BA_STATUS_ACTIVE);
+	return iwl4965_tx_queue_agg_enable(priv, txq_id, tx_fifo,
+					   sta_id, tid, ssn);
+}
+
+
+int iwl_mac_ht_tx_agg_stop(struct ieee80211_hw *hw, u8 *da, u16 tid,
+			   int generator)
+{
+
+	struct iwl_priv *priv = hw->priv;
+	int tx_fifo_id, txq_id, sta_id, ssn = -1;
+	struct iwl_tid_data *tid_data;
+	int rc;
+	DECLARE_MAC_BUF(mac);
+
+	if (!da) {
+		IWL_ERROR("%s: da = NULL\n", __func__);
+		return -EINVAL;
+	}
+
+	if (likely(tid < ARRAY_SIZE(default_tid_to_tx_fifo)))
+		tx_fifo_id = default_tid_to_tx_fifo[tid];
+	else
+		return -EINVAL;
+
+	sta_id = iwl_hw_find_station(priv, da);
+
+	if (sta_id == IWL_INVALID_STATION)
+		return -ENXIO;
+
+	tid_data = &priv->stations[sta_id].tid[tid];
+	ssn = (tid_data->seq_number & IEEE80211_SCTL_SEQ) >> 4;
+	txq_id = tid_data->agg.txq_id;
+
+	rc = iwl4965_tx_queue_agg_disable(priv, txq_id, ssn, tx_fifo_id);
+	/* FIXME: need more safe way to handle error condition */
+	if (rc)
+		return rc;
+
+	iwl4965_ba_status(priv, tid, BA_STATUS_INITIATOR_DELBA);
+	IWL_DEBUG_INFO("iwl_mac_ht_tx_agg_stop on da=%s tid=%d\n",
+		       print_mac(mac, da), tid);
+
+	return 0;
+}
+
+int iwl_mac_ht_rx_agg_start(struct ieee80211_hw *hw, u8 *da,
+			    u16 tid, u16 start_seq_num)
+{
+	struct iwl_priv *priv = hw->priv;
+	int sta_id;
+	DECLARE_MAC_BUF(mac);
+
+	IWL_WARNING("iwl-AGG iwl_mac_ht_rx_agg_start on da=%s"
+		    " tid=%d\n", print_mac(mac, da), tid);
+	sta_id = iwl_hw_find_station(priv, da);
+	iwl4965_sta_modify_add_ba_tid(priv, sta_id, tid, start_seq_num);
+	return 0;
+}
+
+int iwl_mac_ht_rx_agg_stop(struct ieee80211_hw *hw, u8 *da,
+			   u16 tid, int generator)
+{
+	struct iwl_priv *priv = hw->priv;
+	int sta_id;
+	DECLARE_MAC_BUF(mac);
+
+	IWL_WARNING("iwl-AGG iwl_mac_ht_rx_agg_stop on da=%s tid=%d\n",
+		    print_mac(mac, da), tid);
+	sta_id = iwl_hw_find_station(priv, da);
+	iwl4965_sta_modify_del_ba_tid(priv, sta_id, tid);
+	return 0;
+}
+
+#endif /* CONFIG_IWLWIFI_HT_AGG */
+#endif /* CONFIG_IWLWIFI_HT */
+
+/* Set up 4965-specific Rx frame reply handlers */
+void iwl_hw_rx_handler_setup(struct iwl_priv *priv)
+{
+	/* Legacy Rx frames */
+	priv->rx_handlers[REPLY_4965_RX] = iwl4965_rx_reply_rx;
+
+	/* High-throughput (HT) Rx frames */
+	priv->rx_handlers[REPLY_RX_PHY_CMD] = iwl4965_rx_reply_rx_phy;
+	priv->rx_handlers[REPLY_RX_MPDU_CMD] = iwl4965_rx_reply_rx;
+
+	priv->rx_handlers[MISSED_BEACONS_NOTIFICATION] =
+	    iwl4965_rx_missed_beacon_notif;
+
+#ifdef CONFIG_IWLWIFI_HT
+#ifdef CONFIG_IWLWIFI_HT_AGG
+	priv->rx_handlers[REPLY_COMPRESSED_BA] = iwl4965_rx_reply_compressed_ba;
+#endif /* CONFIG_IWLWIFI_AGG */
+#endif /* CONFIG_IWLWIFI */
+}
+
+void iwl_hw_setup_deferred_work(struct iwl_priv *priv)
+{
+	INIT_WORK(&priv->txpower_work, iwl4965_bg_txpower_work);
+	INIT_WORK(&priv->statistics_work, iwl4965_bg_statistics_work);
+#ifdef CONFIG_IWLWIFI_SENSITIVITY
+	INIT_WORK(&priv->sensitivity_work, iwl4965_bg_sensitivity_work);
+#endif
+#ifdef CONFIG_IWLWIFI_HT
+#ifdef CONFIG_IWLWIFI_HT_AGG
+	INIT_WORK(&priv->agg_work, iwl4965_bg_agg_work);
+#endif /* CONFIG_IWLWIFI_AGG */
+#endif /* CONFIG_IWLWIFI_HT */
+	init_timer(&priv->statistics_periodic);
+	priv->statistics_periodic.data = (unsigned long)priv;
+	priv->statistics_periodic.function = iwl4965_bg_statistics_periodic;
+}
+
+void iwl_hw_cancel_deferred_work(struct iwl_priv *priv)
+{
+	del_timer_sync(&priv->statistics_periodic);
+
+	cancel_delayed_work(&priv->init_alive_start);
+}
+
+struct pci_device_id iwl_hw_card_ids[] = {
+	{0x8086, 0x4229, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{0x8086, 0x4230, PCI_ANY_ID, PCI_ANY_ID, 0, 0, 0},
+	{0}
+};
+
+int iwl_eeprom_aqcuire_semaphore(struct iwl_priv *priv)
+{
+	u16 count;
+	int rc;
+
+	for (count = 0; count < EEPROM_SEM_RETRY_LIMIT; count++) {
+		iwl_set_bit(priv, CSR_HW_IF_CONFIG_REG,
+			CSR_HW_IF_CONFIG_REG_BIT_EEPROM_OWN_SEM);
+		rc = iwl_poll_bit(priv, CSR_HW_IF_CONFIG_REG,
+					CSR_HW_IF_CONFIG_REG_BIT_EEPROM_OWN_SEM,
+					CSR_HW_IF_CONFIG_REG_BIT_EEPROM_OWN_SEM,
+					EEPROM_SEM_TIMEOUT);
+		if (rc >= 0) {
+			IWL_DEBUG_IO("Aqcuired semaphore after %d tries.\n",
+				count+1);
+			return rc;
+		}
+	}
+
+	return rc;
+}
+
+inline void iwl_eeprom_release_semaphore(struct iwl_priv *priv)
+{
+	iwl_clear_bit(priv, CSR_HW_IF_CONFIG_REG,
+		CSR_HW_IF_CONFIG_REG_BIT_EEPROM_OWN_SEM);
+}
+
+
+MODULE_DEVICE_TABLE(pci, iwl_hw_card_ids);
diff --git a/drivers/net/wireless/iwlwifi/iwl-4965.h b/drivers/net/wireless/iwlwifi/iwl-4965.h
new file mode 100644
index 000000000000..4c700812b45b
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-4965.h
@@ -0,0 +1,341 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2003 - 2007 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * James P. Ketrenos <ipw2100-admin@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+#ifndef __iwl_4965_h__
+#define __iwl_4965_h__
+
+struct iwl_priv;
+struct sta_ht_info;
+
+/*
+ * Forward declare iwl-4965.c functions for iwl-base.c
+ */
+extern int iwl_eeprom_aqcuire_semaphore(struct iwl_priv *priv);
+extern void iwl_eeprom_release_semaphore(struct iwl_priv *priv);
+
+extern int iwl4965_tx_queue_update_wr_ptr(struct iwl_priv *priv,
+					  struct iwl_tx_queue *txq,
+					  u16 byte_cnt);
+extern void iwl4965_add_station(struct iwl_priv *priv, const u8 *addr,
+				int is_ap);
+extern void iwl4965_set_rxon_ht(struct iwl_priv *priv,
+				struct sta_ht_info *ht_info);
+
+extern void iwl4965_set_rxon_chain(struct iwl_priv *priv);
+extern int iwl4965_tx_cmd(struct iwl_priv *priv, struct iwl_cmd *out_cmd,
+			  u8 sta_id, dma_addr_t txcmd_phys,
+			  struct ieee80211_hdr *hdr, u8 hdr_len,
+			  struct ieee80211_tx_control *ctrl, void *sta_in);
+extern int iwl4965_init_hw_rates(struct iwl_priv *priv,
+				 struct ieee80211_rate *rates);
+extern int iwl4965_alive_notify(struct iwl_priv *priv);
+extern void iwl4965_update_rate_scaling(struct iwl_priv *priv, u8 mode);
+extern void iwl4965_set_ht_add_station(struct iwl_priv *priv, u8 index);
+
+extern void iwl4965_chain_noise_reset(struct iwl_priv *priv);
+extern void iwl4965_init_sensitivity(struct iwl_priv *priv, u8 flags,
+				     u8 force);
+extern int iwl4965_set_fat_chan_info(struct iwl_priv *priv, int phymode,
+				u16 channel,
+				const struct iwl_eeprom_channel *eeprom_ch,
+				u8 fat_extension_channel);
+extern void iwl4965_rf_kill_ct_config(struct iwl_priv *priv);
+
+#ifdef CONFIG_IWLWIFI_HT
+#ifdef CONFIG_IWLWIFI_HT_AGG
+extern int iwl_mac_ht_tx_agg_start(struct ieee80211_hw *hw, u8 *da,
+				   u16 tid, u16 *start_seq_num);
+extern int iwl_mac_ht_rx_agg_start(struct ieee80211_hw *hw, u8 *da,
+				   u16 tid, u16 start_seq_num);
+extern int iwl_mac_ht_rx_agg_stop(struct ieee80211_hw *hw, u8 *da,
+				  u16 tid, int generator);
+extern int iwl_mac_ht_tx_agg_stop(struct ieee80211_hw *hw, u8 *da,
+				  u16 tid, int generator);
+extern void iwl4965_turn_off_agg(struct iwl_priv *priv, u8 tid);
+#endif /* CONFIG_IWLWIFI_HT_AGG */
+#endif /*CONFIG_IWLWIFI_HT */
+/* Structures, enum, and defines specific to the 4965 */
+
+#define IWL4965_KW_SIZE 0x1000	/*4k */
+
+struct iwl_kw {
+	dma_addr_t dma_addr;
+	void *v_addr;
+	size_t size;
+};
+
+#define TID_QUEUE_CELL_SPACING 50	/*mS */
+#define TID_QUEUE_MAX_SIZE     20
+#define TID_ROUND_VALUE        5	/* mS */
+#define TID_MAX_LOAD_COUNT     8
+
+#define TID_MAX_TIME_DIFF ((TID_QUEUE_MAX_SIZE - 1) * TID_QUEUE_CELL_SPACING)
+#define TIME_WRAP_AROUND(x, y) (((y) > (x)) ? (y) - (x) : (0-(x)) + (y))
+
+#define TID_ALL_ENABLED		0x7f
+#define TID_ALL_SPECIFIED       0xff
+#define TID_AGG_TPT_THREHOLD    0x0
+
+#define IWL_CHANNEL_WIDTH_20MHZ   0
+#define IWL_CHANNEL_WIDTH_40MHZ   1
+
+#define IWL_MIMO_PS_STATIC        0
+#define IWL_MIMO_PS_NONE          3
+#define IWL_MIMO_PS_DYNAMIC       1
+#define IWL_MIMO_PS_INVALID       2
+
+#define IWL_OPERATION_MODE_AUTO     0
+#define IWL_OPERATION_MODE_HT_ONLY  1
+#define IWL_OPERATION_MODE_MIXED    2
+#define IWL_OPERATION_MODE_20MHZ    3
+
+#define IWL_EXT_CHANNEL_OFFSET_AUTO   0
+#define IWL_EXT_CHANNEL_OFFSET_ABOVE  1
+#define IWL_EXT_CHANNEL_OFFSET_       2
+#define IWL_EXT_CHANNEL_OFFSET_BELOW  3
+#define IWL_EXT_CHANNEL_OFFSET_MAX    4
+
+#define NRG_NUM_PREV_STAT_L     20
+#define NUM_RX_CHAINS           (3)
+
+#define TX_POWER_IWL_ILLEGAL_VDET    -100000
+#define TX_POWER_IWL_ILLEGAL_VOLTAGE -10000
+#define TX_POWER_IWL_CLOSED_LOOP_MIN_POWER 18
+#define TX_POWER_IWL_CLOSED_LOOP_MAX_POWER 34
+#define TX_POWER_IWL_VDET_SLOPE_BELOW_NOMINAL 17
+#define TX_POWER_IWL_VDET_SLOPE_ABOVE_NOMINAL 20
+#define TX_POWER_IWL_NOMINAL_POWER            26
+#define TX_POWER_IWL_CLOSED_LOOP_ITERATION_LIMIT 1
+#define TX_POWER_IWL_VOLTAGE_CODES_PER_03V       7
+#define TX_POWER_IWL_DEGREES_PER_VDET_CODE       11
+#define IWL_TX_POWER_MAX_NUM_PA_MEASUREMENTS 1
+#define IWL_TX_POWER_CCK_COMPENSATION_B_STEP (9)
+#define IWL_TX_POWER_CCK_COMPENSATION_C_STEP (5)
+
+struct iwl_traffic_load {
+	unsigned long time_stamp;
+	u32 packet_count[TID_QUEUE_MAX_SIZE];
+	u8 queue_count;
+	u8 head;
+	u32 total;
+};
+
+#ifdef CONFIG_IWLWIFI_HT_AGG
+struct iwl_agg_control {
+	unsigned long next_retry;
+	u32 wait_for_agg_status;
+	u32 tid_retry;
+	u32 requested_ba;
+	u32 granted_ba;
+	u8 auto_agg;
+	u32 tid_traffic_load_threshold;
+	u32 ba_timeout;
+	struct iwl_traffic_load traffic_load[TID_MAX_LOAD_COUNT];
+};
+#endif				/*CONFIG_IWLWIFI_HT_AGG */
+
+struct iwl_lq_mngr {
+#ifdef CONFIG_IWLWIFI_HT_AGG
+	struct iwl_agg_control agg_ctrl;
+#endif
+	spinlock_t lock;
+	s32 max_window_size;
+	s32 *expected_tpt;
+	u8 *next_higher_rate;
+	u8 *next_lower_rate;
+	unsigned long stamp;
+	unsigned long stamp_last;
+	u32 flush_time;
+	u32 tx_packets;
+	u8 lq_ready;
+};
+
+
+/* Sensitivity and chain noise calibration */
+#define INTERFERENCE_DATA_AVAILABLE	__constant_cpu_to_le32(1)
+#define INITIALIZATION_VALUE		0xFFFF
+#define CAL_NUM_OF_BEACONS		20
+#define MAXIMUM_ALLOWED_PATHLOSS	15
+
+/* Param table within SENSITIVITY_CMD */
+#define HD_MIN_ENERGY_CCK_DET_INDEX                 (0)
+#define HD_MIN_ENERGY_OFDM_DET_INDEX                (1)
+#define HD_AUTO_CORR32_X1_TH_ADD_MIN_INDEX          (2)
+#define HD_AUTO_CORR32_X1_TH_ADD_MIN_MRC_INDEX      (3)
+#define HD_AUTO_CORR40_X4_TH_ADD_MIN_MRC_INDEX      (4)
+#define HD_AUTO_CORR32_X4_TH_ADD_MIN_INDEX          (5)
+#define HD_AUTO_CORR32_X4_TH_ADD_MIN_MRC_INDEX      (6)
+#define HD_BARKER_CORR_TH_ADD_MIN_INDEX             (7)
+#define HD_BARKER_CORR_TH_ADD_MIN_MRC_INDEX         (8)
+#define HD_AUTO_CORR40_X4_TH_ADD_MIN_INDEX          (9)
+#define HD_OFDM_ENERGY_TH_IN_INDEX                  (10)
+
+#define SENSITIVITY_CMD_CONTROL_DEFAULT_TABLE	__constant_cpu_to_le16(0)
+#define SENSITIVITY_CMD_CONTROL_WORK_TABLE	__constant_cpu_to_le16(1)
+
+#define CHAIN_NOISE_MAX_DELTA_GAIN_CODE 3
+
+#define MAX_FA_OFDM  50
+#define MIN_FA_OFDM  5
+#define MAX_FA_CCK   50
+#define MIN_FA_CCK   5
+
+#define NRG_MIN_CCK  97
+#define NRG_MAX_CCK  0
+
+#define AUTO_CORR_MIN_OFDM        85
+#define AUTO_CORR_MIN_OFDM_MRC    170
+#define AUTO_CORR_MIN_OFDM_X1     105
+#define AUTO_CORR_MIN_OFDM_MRC_X1 220
+#define AUTO_CORR_MAX_OFDM        120
+#define AUTO_CORR_MAX_OFDM_MRC    210
+#define AUTO_CORR_MAX_OFDM_X1     140
+#define AUTO_CORR_MAX_OFDM_MRC_X1 270
+#define AUTO_CORR_STEP_OFDM       1
+
+#define AUTO_CORR_MIN_CCK      (125)
+#define AUTO_CORR_MAX_CCK      (200)
+#define AUTO_CORR_MIN_CCK_MRC  200
+#define AUTO_CORR_MAX_CCK_MRC  400
+#define AUTO_CORR_STEP_CCK     3
+#define AUTO_CORR_MAX_TH_CCK   160
+
+#define NRG_ALG                0
+#define AUTO_CORR_ALG          1
+#define NRG_DIFF               2
+#define NRG_STEP_CCK           2
+#define NRG_MARGIN             8
+#define MAX_NUMBER_CCK_NO_FA 100
+
+#define AUTO_CORR_CCK_MIN_VAL_DEF    (125)
+
+#define CHAIN_A             0
+#define CHAIN_B             1
+#define CHAIN_C             2
+#define CHAIN_NOISE_DELTA_GAIN_INIT_VAL 4
+#define ALL_BAND_FILTER			0xFF00
+#define IN_BAND_FILTER			0xFF
+#define MIN_AVERAGE_NOISE_MAX_VALUE	0xFFFFFFFF
+
+enum iwl_false_alarm_state {
+	IWL_FA_TOO_MANY = 0,
+	IWL_FA_TOO_FEW = 1,
+	IWL_FA_GOOD_RANGE = 2,
+};
+
+enum iwl_chain_noise_state {
+	IWL_CHAIN_NOISE_ALIVE = 0,  /* must be 0 */
+	IWL_CHAIN_NOISE_ACCUMULATE = 1,
+	IWL_CHAIN_NOISE_CALIBRATED = 2,
+};
+
+enum iwl_sensitivity_state {
+	IWL_SENS_CALIB_ALLOWED = 0,
+	IWL_SENS_CALIB_NEED_REINIT = 1,
+};
+
+enum iwl_calib_enabled_state {
+	IWL_CALIB_DISABLED = 0,  /* must be 0 */
+	IWL_CALIB_ENABLED = 1,
+};
+
+struct statistics_general_data {
+	u32 beacon_silence_rssi_a;
+	u32 beacon_silence_rssi_b;
+	u32 beacon_silence_rssi_c;
+	u32 beacon_energy_a;
+	u32 beacon_energy_b;
+	u32 beacon_energy_c;
+};
+
+/* Sensitivity calib data */
+struct iwl_sensitivity_data {
+	u32 auto_corr_ofdm;
+	u32 auto_corr_ofdm_mrc;
+	u32 auto_corr_ofdm_x1;
+	u32 auto_corr_ofdm_mrc_x1;
+	u32 auto_corr_cck;
+	u32 auto_corr_cck_mrc;
+
+	u32 last_bad_plcp_cnt_ofdm;
+	u32 last_fa_cnt_ofdm;
+	u32 last_bad_plcp_cnt_cck;
+	u32 last_fa_cnt_cck;
+
+	u32 nrg_curr_state;
+	u32 nrg_prev_state;
+	u32 nrg_value[10];
+	u8  nrg_silence_rssi[NRG_NUM_PREV_STAT_L];
+	u32 nrg_silence_ref;
+	u32 nrg_energy_idx;
+	u32 nrg_silence_idx;
+	u32 nrg_th_cck;
+	s32 nrg_auto_corr_silence_diff;
+	u32 num_in_cck_no_fa;
+	u32 nrg_th_ofdm;
+
+	u8 state;
+};
+
+/* Chain noise (differential Rx gain) calib data */
+struct iwl_chain_noise_data {
+	u8 state;
+	u16 beacon_count;
+	u32 chain_noise_a;
+	u32 chain_noise_b;
+	u32 chain_noise_c;
+	u32 chain_signal_a;
+	u32 chain_signal_b;
+	u32 chain_signal_c;
+	u8 disconn_array[NUM_RX_CHAINS];
+	u8 delta_gain_code[NUM_RX_CHAINS];
+	u8 radio_write;
+};
+
+/* IWL4965 */
+#define RATE_MCS_CODE_MSK 0x7
+#define RATE_MCS_MIMO_POS 3
+#define RATE_MCS_MIMO_MSK 0x8
+#define RATE_MCS_HT_DUP_POS 5
+#define RATE_MCS_HT_DUP_MSK 0x20
+#define RATE_MCS_FLAGS_POS 8
+#define RATE_MCS_HT_POS 8
+#define RATE_MCS_HT_MSK 0x100
+#define RATE_MCS_CCK_POS 9
+#define RATE_MCS_CCK_MSK 0x200
+#define RATE_MCS_GF_POS 10
+#define RATE_MCS_GF_MSK 0x400
+
+#define RATE_MCS_FAT_POS 11
+#define RATE_MCS_FAT_MSK 0x800
+#define RATE_MCS_DUP_POS 12
+#define RATE_MCS_DUP_MSK 0x1000
+#define RATE_MCS_SGI_POS 13
+#define RATE_MCS_SGI_MSK 0x2000
+
+#define	EEPROM_SEM_TIMEOUT 10
+#define EEPROM_SEM_RETRY_LIMIT 1000
+
+#endif				/* __iwl_4965_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-channel.h b/drivers/net/wireless/iwlwifi/iwl-channel.h
new file mode 100644
index 000000000000..023c3f240cea
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-channel.h
@@ -0,0 +1,161 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2005 - 2007 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * James P. Ketrenos <ipw2100-admin@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+#ifndef __iwl_channel_h__
+#define __iwl_channel_h__
+
+#define IWL_NUM_SCAN_RATES         (2)
+
+struct iwl_channel_tgd_info {
+	u8 type;
+	s8 max_power;
+};
+
+struct iwl_channel_tgh_info {
+	s64 last_radar_time;
+};
+
+/* current Tx power values to use, one for each rate for each channel.
+ * requested power is limited by:
+ * -- regulatory EEPROM limits for this channel
+ * -- hardware capabilities (clip-powers)
+ * -- spectrum management
+ * -- user preference (e.g. iwconfig)
+ * when requested power is set, base power index must also be set. */
+struct iwl_channel_power_info {
+	struct iwl_tx_power tpc;	/* actual radio and DSP gain settings */
+	s8 power_table_index;	/* actual (compenst'd) index into gain table */
+	s8 base_power_index;	/* gain index for power at factory temp. */
+	s8 requested_power;	/* power (dBm) requested for this chnl/rate */
+};
+
+/* current scan Tx power values to use, one for each scan rate for each
+ * channel. */
+struct iwl_scan_power_info {
+	struct iwl_tx_power tpc;	/* actual radio and DSP gain settings */
+	s8 power_table_index;	/* actual (compenst'd) index into gain table */
+	s8 requested_power;	/* scan pwr (dBm) requested for chnl/rate */
+};
+
+/* Channel unlock period is 15 seconds. If no beacon or probe response
+ * has been received within 15 seconds on a locked channel then the channel
+ * remains locked. */
+#define TX_UNLOCK_PERIOD 15
+
+/* CSA lock period is 15 seconds.  If a CSA has been received on a channel in
+ * the last 15 seconds, the channel is locked */
+#define CSA_LOCK_PERIOD 15
+/*
+ * One for each channel, holds all channel setup data
+ * Some of the fields (e.g. eeprom and flags/max_power_avg) are redundant
+ *     with one another!
+ */
+#define IWL4965_MAX_RATE (33)
+
+struct iwl_channel_info {
+	struct iwl_channel_tgd_info tgd;
+	struct iwl_channel_tgh_info tgh;
+	struct iwl_eeprom_channel eeprom;	/* EEPROM regulatory limit */
+	struct iwl_eeprom_channel fat_eeprom;	/* EEPROM regulatory limit for
+						 * FAT channel */
+
+	u8 channel;	  /* channel number */
+	u8 flags;	  /* flags copied from EEPROM */
+	s8 max_power_avg; /* (dBm) regul. eeprom, normal Tx, any rate */
+	s8 curr_txpow;	  /* (dBm) regulatory/spectrum/user (not h/w) */
+	s8 min_power;	  /* always 0 */
+	s8 scan_power;	  /* (dBm) regul. eeprom, direct scans, any rate */
+
+	u8 group_index;	  /* 0-4, maps channel to group1/2/3/4/5 */
+	u8 band_index;	  /* 0-4, maps channel to band1/2/3/4/5 */
+	u8 phymode;	  /* MODE_IEEE80211{A,B,G} */
+
+	/* Radio/DSP gain settings for each "normal" data Tx rate.
+	 * These include, in addition to RF and DSP gain, a few fields for
+	 *   remembering/modifying gain settings (indexes). */
+	struct iwl_channel_power_info power_info[IWL4965_MAX_RATE];
+
+#if IWL == 4965
+	/* FAT channel info */
+	s8 fat_max_power_avg;	/* (dBm) regul. eeprom, normal Tx, any rate */
+	s8 fat_curr_txpow;	/* (dBm) regulatory/spectrum/user (not h/w) */
+	s8 fat_min_power;	/* always 0 */
+	s8 fat_scan_power;	/* (dBm) eeprom, direct scans, any rate */
+	u8 fat_flags;		/* flags copied from EEPROM */
+	u8 fat_extension_channel;
+#endif
+
+	/* Radio/DSP gain settings for each scan rate, for directed scans. */
+	struct iwl_scan_power_info scan_pwr_info[IWL_NUM_SCAN_RATES];
+};
+
+struct iwl_clip_group {
+	/* maximum power level to prevent clipping for each rate, derived by
+	 *   us from this band's saturation power in EEPROM */
+	const s8 clip_powers[IWL_MAX_RATES];
+};
+
+static inline int is_channel_valid(const struct iwl_channel_info *ch_info)
+{
+	if (ch_info == NULL)
+		return 0;
+	return (ch_info->flags & EEPROM_CHANNEL_VALID) ? 1 : 0;
+}
+
+static inline int is_channel_narrow(const struct iwl_channel_info *ch_info)
+{
+	return (ch_info->flags & EEPROM_CHANNEL_NARROW) ? 1 : 0;
+}
+
+static inline int is_channel_radar(const struct iwl_channel_info *ch_info)
+{
+	return (ch_info->flags & EEPROM_CHANNEL_RADAR) ? 1 : 0;
+}
+
+static inline u8 is_channel_a_band(const struct iwl_channel_info *ch_info)
+{
+	return ch_info->phymode == MODE_IEEE80211A;
+}
+
+static inline u8 is_channel_bg_band(const struct iwl_channel_info *ch_info)
+{
+	return ((ch_info->phymode == MODE_IEEE80211B) ||
+		(ch_info->phymode == MODE_IEEE80211G));
+}
+
+static inline int is_channel_passive(const struct iwl_channel_info *ch)
+{
+	return (!(ch->flags & EEPROM_CHANNEL_ACTIVE)) ? 1 : 0;
+}
+
+static inline int is_channel_ibss(const struct iwl_channel_info *ch)
+{
+	return ((ch->flags & EEPROM_CHANNEL_IBSS)) ? 1 : 0;
+}
+
+extern const struct iwl_channel_info *iwl_get_channel_info(
+	const struct iwl_priv *priv, int phymode, u16 channel);
+
+#endif
diff --git a/drivers/net/wireless/iwlwifi/iwl-commands.h b/drivers/net/wireless/iwlwifi/iwl-commands.h
new file mode 100644
index 000000000000..9de8d7f6efa3
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-commands.h
@@ -0,0 +1,1734 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual BSD/GPLv2 license.  When using or
+ * redistributing this file, you may do so under either license.
+ *
+ * GPL LICENSE SUMMARY
+ *
+ * Copyright(c) 2005 - 2007 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU Geeral Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110,
+ * USA
+ *
+ * The full GNU General Public License is included in this distribution
+ * in the file called LICENSE.GPL.
+ *
+ * Contact Information:
+ * James P. Ketrenos <ipw2100-admin@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ * BSD LICENSE
+ *
+ * Copyright(c) 2005 - 2007 Intel Corporation. All rights reserved.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *  * Neither the name Intel Corporation nor the names of its
+ *    contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+
+#ifndef __iwl_commands_h__
+#define __iwl_commands_h__
+
+enum {
+	REPLY_ALIVE = 0x1,
+	REPLY_ERROR = 0x2,
+
+	/* RXON and QOS commands */
+	REPLY_RXON = 0x10,
+	REPLY_RXON_ASSOC = 0x11,
+	REPLY_QOS_PARAM = 0x13,
+	REPLY_RXON_TIMING = 0x14,
+
+	/* Multi-Station support */
+	REPLY_ADD_STA = 0x18,
+	REPLY_REMOVE_STA = 0x19,	/* not used */
+	REPLY_REMOVE_ALL_STA = 0x1a,	/* not used */
+
+	/* RX, TX, LEDs */
+#if IWL == 3945
+	REPLY_3945_RX = 0x1b,		/* 3945 only */
+#endif
+	REPLY_TX = 0x1c,
+	REPLY_RATE_SCALE = 0x47,	/* 3945 only */
+	REPLY_LEDS_CMD = 0x48,
+	REPLY_TX_LINK_QUALITY_CMD = 0x4e, /* 4965 only */
+
+	/* 802.11h related */
+	RADAR_NOTIFICATION = 0x70,	/* not used */
+	REPLY_QUIET_CMD = 0x71,		/* not used */
+	REPLY_CHANNEL_SWITCH = 0x72,
+	CHANNEL_SWITCH_NOTIFICATION = 0x73,
+	REPLY_SPECTRUM_MEASUREMENT_CMD = 0x74,
+	SPECTRUM_MEASURE_NOTIFICATION = 0x75,
+
+	/* Power Management */
+	POWER_TABLE_CMD = 0x77,
+	PM_SLEEP_NOTIFICATION = 0x7A,
+	PM_DEBUG_STATISTIC_NOTIFIC = 0x7B,
+
+	/* Scan commands and notifications */
+	REPLY_SCAN_CMD = 0x80,
+	REPLY_SCAN_ABORT_CMD = 0x81,
+	SCAN_START_NOTIFICATION = 0x82,
+	SCAN_RESULTS_NOTIFICATION = 0x83,
+	SCAN_COMPLETE_NOTIFICATION = 0x84,
+
+	/* IBSS/AP commands */
+	BEACON_NOTIFICATION = 0x90,
+	REPLY_TX_BEACON = 0x91,
+	WHO_IS_AWAKE_NOTIFICATION = 0x94,	/* not used */
+
+	/* Miscellaneous commands */
+	QUIET_NOTIFICATION = 0x96,		/* not used */
+	REPLY_TX_PWR_TABLE_CMD = 0x97,
+	MEASURE_ABORT_NOTIFICATION = 0x99,	/* not used */
+
+	/* BT config command */
+	REPLY_BT_CONFIG = 0x9b,
+
+	/* 4965 Statistics */
+	REPLY_STATISTICS_CMD = 0x9c,
+	STATISTICS_NOTIFICATION = 0x9d,
+
+	/* RF-KILL commands and notifications */
+	REPLY_CARD_STATE_CMD = 0xa0,
+	CARD_STATE_NOTIFICATION = 0xa1,
+
+	/* Missed beacons notification */
+	MISSED_BEACONS_NOTIFICATION = 0xa2,
+
+#if IWL == 4965
+	REPLY_CT_KILL_CONFIG_CMD = 0xa4,
+	SENSITIVITY_CMD = 0xa8,
+	REPLY_PHY_CALIBRATION_CMD = 0xb0,
+	REPLY_RX_PHY_CMD = 0xc0,
+	REPLY_RX_MPDU_CMD = 0xc1,
+	REPLY_4965_RX = 0xc3,
+	REPLY_COMPRESSED_BA = 0xc5,
+#endif
+	REPLY_MAX = 0xff
+};
+
+/******************************************************************************
+ * (0)
+ * Header
+ *
+ *****************************************************************************/
+
+#define IWL_CMD_FAILED_MSK 0x40
+
+struct iwl_cmd_header {
+	u8 cmd;
+	u8 flags;
+	/* We have 15 LSB to use as we please (MSB indicates
+	 * a frame Rx'd from the HW).  We encode the following
+	 * information into the sequence field:
+	 *
+	 *  0:7    index in fifo
+	 *  8:13   fifo selection
+	 * 14:14   bit indicating if this packet references the 'extra'
+	 *         storage at the end of the memory queue
+	 * 15:15   (Rx indication)
+	 *
+	 */
+	__le16 sequence;
+
+	/* command data follows immediately */
+	u8 data[0];
+} __attribute__ ((packed));
+
+/******************************************************************************
+ * (0a)
+ * Alive and Error Commands & Responses:
+ *
+ *****************************************************************************/
+
+#define UCODE_VALID_OK	__constant_cpu_to_le32(0x1)
+#define INITIALIZE_SUBTYPE    (9)
+
+/*
+ * REPLY_ALIVE = 0x1 (response only, not a command)
+ */
+struct iwl_alive_resp {
+	u8 ucode_minor;
+	u8 ucode_major;
+	__le16 reserved1;
+	u8 sw_rev[8];
+	u8 ver_type;
+	u8 ver_subtype;
+	__le16 reserved2;
+	__le32 log_event_table_ptr;
+	__le32 error_event_table_ptr;
+	__le32 timestamp;
+	__le32 is_valid;
+} __attribute__ ((packed));
+
+struct iwl_init_alive_resp {
+	u8 ucode_minor;
+	u8 ucode_major;
+	__le16 reserved1;
+	u8 sw_rev[8];
+	u8 ver_type;
+	u8 ver_subtype;
+	__le16 reserved2;
+	__le32 log_event_table_ptr;
+	__le32 error_event_table_ptr;
+	__le32 timestamp;
+	__le32 is_valid;
+
+#if IWL == 4965
+	/* calibration values from "initialize" uCode */
+	__le32 voltage;		/* signed */
+	__le32 therm_r1[2];	/* signed 1st for normal, 2nd for FAT channel */
+	__le32 therm_r2[2];	/* signed */
+	__le32 therm_r3[2];	/* signed */
+	__le32 therm_r4[2];	/* signed */
+	__le32 tx_atten[5][2];	/* signed MIMO gain comp, 5 freq groups,
+				 * 2 Tx chains */
+#endif
+} __attribute__ ((packed));
+
+union tsf {
+	u8 byte[8];
+	__le16 word[4];
+	__le32 dw[2];
+};
+
+/*
+ * REPLY_ERROR = 0x2 (response only, not a command)
+ */
+struct iwl_error_resp {
+	__le32 error_type;
+	u8 cmd_id;
+	u8 reserved1;
+	__le16 bad_cmd_seq_num;
+#if IWL == 3945
+	__le16 reserved2;
+#endif
+	__le32 error_info;
+	union tsf timestamp;
+} __attribute__ ((packed));
+
+/******************************************************************************
+ * (1)
+ * RXON Commands & Responses:
+ *
+ *****************************************************************************/
+
+/*
+ * Rx config defines & structure
+ */
+/* rx_config device types  */
+enum {
+	RXON_DEV_TYPE_AP = 1,
+	RXON_DEV_TYPE_ESS = 3,
+	RXON_DEV_TYPE_IBSS = 4,
+	RXON_DEV_TYPE_SNIFFER = 6,
+};
+
+/* rx_config flags */
+/* band & modulation selection */
+#define RXON_FLG_BAND_24G_MSK           __constant_cpu_to_le32(1 << 0)
+#define RXON_FLG_CCK_MSK                __constant_cpu_to_le32(1 << 1)
+/* auto detection enable */
+#define RXON_FLG_AUTO_DETECT_MSK        __constant_cpu_to_le32(1 << 2)
+/* TGg protection when tx */
+#define RXON_FLG_TGG_PROTECT_MSK        __constant_cpu_to_le32(1 << 3)
+/* cck short slot & preamble */
+#define RXON_FLG_SHORT_SLOT_MSK          __constant_cpu_to_le32(1 << 4)
+#define RXON_FLG_SHORT_PREAMBLE_MSK     __constant_cpu_to_le32(1 << 5)
+/* antenna selection */
+#define RXON_FLG_DIS_DIV_MSK            __constant_cpu_to_le32(1 << 7)
+#define RXON_FLG_ANT_SEL_MSK            __constant_cpu_to_le32(0x0f00)
+#define RXON_FLG_ANT_A_MSK              __constant_cpu_to_le32(1 << 8)
+#define RXON_FLG_ANT_B_MSK              __constant_cpu_to_le32(1 << 9)
+/* radar detection enable */
+#define RXON_FLG_RADAR_DETECT_MSK       __constant_cpu_to_le32(1 << 12)
+#define RXON_FLG_TGJ_NARROW_BAND_MSK    __constant_cpu_to_le32(1 << 13)
+/* rx response to host with 8-byte TSF
+* (according to ON_AIR deassertion) */
+#define RXON_FLG_TSF2HOST_MSK           __constant_cpu_to_le32(1 << 15)
+
+/* rx_config filter flags */
+/* accept all data frames */
+#define RXON_FILTER_PROMISC_MSK         __constant_cpu_to_le32(1 << 0)
+/* pass control & management to host */
+#define RXON_FILTER_CTL2HOST_MSK        __constant_cpu_to_le32(1 << 1)
+/* accept multi-cast */
+#define RXON_FILTER_ACCEPT_GRP_MSK      __constant_cpu_to_le32(1 << 2)
+/* don't decrypt uni-cast frames */
+#define RXON_FILTER_DIS_DECRYPT_MSK     __constant_cpu_to_le32(1 << 3)
+/* don't decrypt multi-cast frames */
+#define RXON_FILTER_DIS_GRP_DECRYPT_MSK __constant_cpu_to_le32(1 << 4)
+/* STA is associated */
+#define RXON_FILTER_ASSOC_MSK           __constant_cpu_to_le32(1 << 5)
+/* transfer to host non bssid beacons in associated state */
+#define RXON_FILTER_BCON_AWARE_MSK      __constant_cpu_to_le32(1 << 6)
+
+/*
+ * REPLY_RXON = 0x10 (command, has simple generic response)
+ */
+struct iwl_rxon_cmd {
+	u8 node_addr[6];
+	__le16 reserved1;
+	u8 bssid_addr[6];
+	__le16 reserved2;
+	u8 wlap_bssid_addr[6];
+	__le16 reserved3;
+	u8 dev_type;
+	u8 air_propagation;
+#if IWL == 3945
+	__le16 reserved4;
+#elif IWL == 4965
+	__le16 rx_chain;
+#endif
+	u8 ofdm_basic_rates;
+	u8 cck_basic_rates;
+	__le16 assoc_id;
+	__le32 flags;
+	__le32 filter_flags;
+	__le16 channel;
+#if IWL == 3945
+	__le16 reserved5;
+#elif IWL == 4965
+	u8 ofdm_ht_single_stream_basic_rates;
+	u8 ofdm_ht_dual_stream_basic_rates;
+#endif
+} __attribute__ ((packed));
+
+/*
+ * REPLY_RXON_ASSOC = 0x11 (command, has simple generic response)
+ */
+struct iwl_rxon_assoc_cmd {
+	__le32 flags;
+	__le32 filter_flags;
+	u8 ofdm_basic_rates;
+	u8 cck_basic_rates;
+#if IWL == 4965
+	u8 ofdm_ht_single_stream_basic_rates;
+	u8 ofdm_ht_dual_stream_basic_rates;
+	__le16 rx_chain_select_flags;
+#endif
+	__le16 reserved;
+} __attribute__ ((packed));
+
+/*
+ * REPLY_RXON_TIMING = 0x14 (command, has simple generic response)
+ */
+struct iwl_rxon_time_cmd {
+	union tsf timestamp;
+	__le16 beacon_interval;
+	__le16 atim_window;
+	__le32 beacon_init_val;
+	__le16 listen_interval;
+	__le16 reserved;
+} __attribute__ ((packed));
+
+struct iwl_tx_power {
+	u8 tx_gain;		/* gain for analog radio */
+	u8 dsp_atten;		/* gain for DSP */
+} __attribute__ ((packed));
+
+#if IWL == 3945
+struct iwl_power_per_rate {
+	u8 rate;		/* plcp */
+	struct iwl_tx_power tpc;
+	u8 reserved;
+} __attribute__ ((packed));
+
+#elif IWL == 4965
+#define POWER_TABLE_NUM_ENTRIES			33
+#define POWER_TABLE_NUM_HT_OFDM_ENTRIES		32
+#define POWER_TABLE_CCK_ENTRY			32
+struct tx_power_dual_stream {
+	__le32 dw;
+} __attribute__ ((packed));
+
+struct iwl_tx_power_db {
+	struct tx_power_dual_stream power_tbl[POWER_TABLE_NUM_ENTRIES];
+} __attribute__ ((packed));
+#endif
+
+/*
+ * REPLY_CHANNEL_SWITCH = 0x72 (command, has simple generic response)
+ */
+struct iwl_channel_switch_cmd {
+	u8 band;
+	u8 expect_beacon;
+	__le16 channel;
+	__le32 rxon_flags;
+	__le32 rxon_filter_flags;
+	__le32 switch_time;
+#if IWL == 3945
+	struct iwl_power_per_rate power[IWL_MAX_RATES];
+#elif IWL == 4965
+	struct iwl_tx_power_db tx_power;
+#endif
+} __attribute__ ((packed));
+
+/*
+ * CHANNEL_SWITCH_NOTIFICATION = 0x73 (notification only, not a command)
+ */
+struct iwl_csa_notification {
+	__le16 band;
+	__le16 channel;
+	__le32 status;		/* 0 - OK, 1 - fail */
+} __attribute__ ((packed));
+
+/******************************************************************************
+ * (2)
+ * Quality-of-Service (QOS) Commands & Responses:
+ *
+ *****************************************************************************/
+struct iwl_ac_qos {
+	__le16 cw_min;
+	__le16 cw_max;
+	u8 aifsn;
+	u8 reserved1;
+	__le16 edca_txop;
+} __attribute__ ((packed));
+
+/* QoS flags defines */
+#define QOS_PARAM_FLG_UPDATE_EDCA_MSK	__constant_cpu_to_le32(0x01)
+#define QOS_PARAM_FLG_TGN_MSK		__constant_cpu_to_le32(0x02)
+#define QOS_PARAM_FLG_TXOP_TYPE_MSK	__constant_cpu_to_le32(0x10)
+
+/*
+ *  TXFIFO Queue number defines
+ */
+/* number of Access categories (AC) (EDCA), queues 0..3 */
+#define AC_NUM                4
+
+/*
+ * REPLY_QOS_PARAM = 0x13 (command, has simple generic response)
+ */
+struct iwl_qosparam_cmd {
+	__le32 qos_flags;
+	struct iwl_ac_qos ac[AC_NUM];
+} __attribute__ ((packed));
+
+/******************************************************************************
+ * (3)
+ * Add/Modify Stations Commands & Responses:
+ *
+ *****************************************************************************/
+/*
+ * Multi station support
+ */
+#define	IWL_AP_ID		0
+#define IWL_MULTICAST_ID	1
+#define	IWL_STA_ID		2
+
+#define	IWL3945_BROADCAST_ID	24
+#define IWL3945_STATION_COUNT	25
+
+#define IWL4965_BROADCAST_ID	31
+#define	IWL4965_STATION_COUNT	32
+
+#define	IWL_STATION_COUNT	32 	/* MAX(3945,4965)*/
+#define	IWL_INVALID_STATION 	255
+
+#if IWL == 3945
+#define STA_FLG_TX_RATE_MSK		__constant_cpu_to_le32(1<<2);
+#endif
+#define STA_FLG_PWR_SAVE_MSK		__constant_cpu_to_le32(1<<8);
+
+#define STA_CONTROL_MODIFY_MSK		0x01
+
+/* key flags __le16*/
+#define STA_KEY_FLG_ENCRYPT_MSK	__constant_cpu_to_le16(0x7)
+#define STA_KEY_FLG_NO_ENC	__constant_cpu_to_le16(0x0)
+#define STA_KEY_FLG_WEP		__constant_cpu_to_le16(0x1)
+#define STA_KEY_FLG_CCMP	__constant_cpu_to_le16(0x2)
+#define STA_KEY_FLG_TKIP	__constant_cpu_to_le16(0x3)
+
+#define STA_KEY_FLG_KEYID_POS	8
+#define STA_KEY_FLG_INVALID 	__constant_cpu_to_le16(0x0800)
+
+/* modify flags  */
+#define	STA_MODIFY_KEY_MASK		0x01
+#define	STA_MODIFY_TID_DISABLE_TX	0x02
+#define	STA_MODIFY_TX_RATE_MSK		0x04
+#define STA_MODIFY_ADDBA_TID_MSK	0x08
+#define STA_MODIFY_DELBA_TID_MSK	0x10
+#define BUILD_RAxTID(sta_id, tid)	(((sta_id) << 4) + (tid))
+
+/*
+ * Antenna masks:
+ * bit14:15 01 B inactive, A active
+ *          10 B active, A inactive
+ *          11 Both active
+ */
+#define RATE_MCS_ANT_A_POS	14
+#define RATE_MCS_ANT_B_POS	15
+#define RATE_MCS_ANT_A_MSK	0x4000
+#define RATE_MCS_ANT_B_MSK	0x8000
+#define RATE_MCS_ANT_AB_MSK	0xc000
+
+struct iwl_keyinfo {
+	__le16 key_flags;
+	u8 tkip_rx_tsc_byte2;	/* TSC[2] for key mix ph1 detection */
+	u8 reserved1;
+	__le16 tkip_rx_ttak[5];	/* 10-byte unicast TKIP TTAK */
+	__le16 reserved2;
+	u8 key[16];		/* 16-byte unicast decryption key */
+} __attribute__ ((packed));
+
+struct sta_id_modify {
+	u8 addr[ETH_ALEN];
+	__le16 reserved1;
+	u8 sta_id;
+	u8 modify_mask;
+	__le16 reserved2;
+} __attribute__ ((packed));
+
+/*
+ * REPLY_ADD_STA = 0x18 (command)
+ */
+struct iwl_addsta_cmd {
+	u8 mode;
+	u8 reserved[3];
+	struct sta_id_modify sta;
+	struct iwl_keyinfo key;
+	__le32 station_flags;
+	__le32 station_flags_msk;
+	__le16 tid_disable_tx;
+#if IWL == 3945
+	__le16 rate_n_flags;
+#else
+	__le16	reserved1;
+#endif
+	u8 add_immediate_ba_tid;
+	u8 remove_immediate_ba_tid;
+	__le16 add_immediate_ba_ssn;
+#if IWL == 4965
+	__le32 reserved2;
+#endif
+} __attribute__ ((packed));
+
+/*
+ * REPLY_ADD_STA = 0x18 (response)
+ */
+struct iwl_add_sta_resp {
+	u8 status;
+} __attribute__ ((packed));
+
+#define ADD_STA_SUCCESS_MSK              0x1
+
+/******************************************************************************
+ * (4)
+ * Rx Responses:
+ *
+ *****************************************************************************/
+
+struct iwl_rx_frame_stats {
+	u8 phy_count;
+	u8 id;
+	u8 rssi;
+	u8 agc;
+	__le16 sig_avg;
+	__le16 noise_diff;
+	u8 payload[0];
+} __attribute__ ((packed));
+
+struct iwl_rx_frame_hdr {
+	__le16 channel;
+	__le16 phy_flags;
+	u8 reserved1;
+	u8 rate;
+	__le16 len;
+	u8 payload[0];
+} __attribute__ ((packed));
+
+#define	RX_RES_STATUS_NO_CRC32_ERROR	__constant_cpu_to_le32(1 << 0)
+#define	RX_RES_STATUS_NO_RXE_OVERFLOW	__constant_cpu_to_le32(1 << 1)
+
+#define	RX_RES_PHY_FLAGS_BAND_24_MSK	__constant_cpu_to_le16(1 << 0)
+#define	RX_RES_PHY_FLAGS_MOD_CCK_MSK		__constant_cpu_to_le16(1 << 1)
+#define	RX_RES_PHY_FLAGS_SHORT_PREAMBLE_MSK	__constant_cpu_to_le16(1 << 2)
+#define	RX_RES_PHY_FLAGS_NARROW_BAND_MSK	__constant_cpu_to_le16(1 << 3)
+#define	RX_RES_PHY_FLAGS_ANTENNA_MSK		__constant_cpu_to_le16(0xf0)
+
+#define	RX_RES_STATUS_SEC_TYPE_MSK	(0x7 << 8)
+#define	RX_RES_STATUS_SEC_TYPE_NONE	(0x0 << 8)
+#define	RX_RES_STATUS_SEC_TYPE_WEP	(0x1 << 8)
+#define	RX_RES_STATUS_SEC_TYPE_CCMP	(0x2 << 8)
+#define	RX_RES_STATUS_SEC_TYPE_TKIP	(0x3 << 8)
+
+#define	RX_RES_STATUS_DECRYPT_TYPE_MSK	(0x3 << 11)
+#define	RX_RES_STATUS_NOT_DECRYPT	(0x0 << 11)
+#define	RX_RES_STATUS_DECRYPT_OK	(0x3 << 11)
+#define	RX_RES_STATUS_BAD_ICV_MIC	(0x1 << 11)
+#define	RX_RES_STATUS_BAD_KEY_TTAK	(0x2 << 11)
+
+struct iwl_rx_frame_end {
+	__le32 status;
+	__le64 timestamp;
+	__le32 beacon_timestamp;
+} __attribute__ ((packed));
+
+/*
+ * REPLY_3945_RX = 0x1b (response only, not a command)
+ *
+ * NOTE:  DO NOT dereference from casts to this structure
+ * It is provided only for calculating minimum data set size.
+ * The actual offsets of the hdr and end are dynamic based on
+ * stats.phy_count
+ */
+struct iwl_rx_frame {
+	struct iwl_rx_frame_stats stats;
+	struct iwl_rx_frame_hdr hdr;
+	struct iwl_rx_frame_end end;
+} __attribute__ ((packed));
+
+/* Fixed (non-configurable) rx data from phy */
+#define RX_PHY_FLAGS_ANTENNAE_OFFSET		(4)
+#define RX_PHY_FLAGS_ANTENNAE_MASK		(0x70)
+#define IWL_AGC_DB_MASK 	(0x3f80)	/* MASK(7,13) */
+#define IWL_AGC_DB_POS		(7)
+struct iwl4965_rx_non_cfg_phy {
+	__le16 ant_selection;	/* ant A bit 4, ant B bit 5, ant C bit 6 */
+	__le16 agc_info;	/* agc code 0:6, agc dB 7:13, reserved 14:15 */
+	u8 rssi_info[6];	/* we use even entries, 0/2/4 for A/B/C rssi */
+	u8 pad[0];
+} __attribute__ ((packed));
+
+/*
+ * REPLY_4965_RX = 0xc3 (response only, not a command)
+ * Used only for legacy (non 11n) frames.
+ */
+#define RX_RES_PHY_CNT 14
+struct iwl4965_rx_phy_res {
+	u8 non_cfg_phy_cnt;     /* non configurable DSP phy data byte count */
+	u8 cfg_phy_cnt;		/* configurable DSP phy data byte count */
+	u8 stat_id;		/* configurable DSP phy data set ID */
+	u8 reserved1;
+	__le64 timestamp;	/* TSF at on air rise */
+	__le32 beacon_time_stamp; /* beacon at on-air rise */
+	__le16 phy_flags;	/* general phy flags: band, modulation, ... */
+	__le16 channel;		/* channel number */
+	__le16 non_cfg_phy[RX_RES_PHY_CNT];	/* upto 14 phy entries */
+	__le32 reserved2;
+	__le32 rate_n_flags;
+	__le16 byte_count;		/* frame's byte-count */
+	__le16 reserved3;
+} __attribute__ ((packed));
+
+struct iwl4965_rx_mpdu_res_start {
+	__le16 byte_count;
+	__le16 reserved;
+} __attribute__ ((packed));
+
+
+/******************************************************************************
+ * (5)
+ * Tx Commands & Responses:
+ *
+ *****************************************************************************/
+
+/* Tx flags */
+#define TX_CMD_FLG_RTS_MSK __constant_cpu_to_le32(1 << 1)
+#define TX_CMD_FLG_CTS_MSK __constant_cpu_to_le32(1 << 2)
+#define TX_CMD_FLG_ACK_MSK __constant_cpu_to_le32(1 << 3)
+#define TX_CMD_FLG_STA_RATE_MSK __constant_cpu_to_le32(1 << 4)
+#define TX_CMD_FLG_IMM_BA_RSP_MASK  __constant_cpu_to_le32(1 << 6)
+#define TX_CMD_FLG_FULL_TXOP_PROT_MSK __constant_cpu_to_le32(1 << 7)
+#define TX_CMD_FLG_ANT_SEL_MSK __constant_cpu_to_le32(0xf00)
+#define TX_CMD_FLG_ANT_A_MSK __constant_cpu_to_le32(1 << 8)
+#define TX_CMD_FLG_ANT_B_MSK __constant_cpu_to_le32(1 << 9)
+
+/* ucode ignores BT priority for this frame */
+#define TX_CMD_FLG_BT_DIS_MSK __constant_cpu_to_le32(1 << 12)
+
+/* ucode overrides sequence control */
+#define TX_CMD_FLG_SEQ_CTL_MSK __constant_cpu_to_le32(1 << 13)
+
+/* signal that this frame is non-last MPDU */
+#define TX_CMD_FLG_MORE_FRAG_MSK __constant_cpu_to_le32(1 << 14)
+
+/* calculate TSF in outgoing frame */
+#define TX_CMD_FLG_TSF_MSK __constant_cpu_to_le32(1 << 16)
+
+/* activate TX calibration. */
+#define TX_CMD_FLG_CALIB_MSK __constant_cpu_to_le32(1 << 17)
+
+/* signals that 2 bytes pad was inserted
+   after the MAC header */
+#define TX_CMD_FLG_MH_PAD_MSK __constant_cpu_to_le32(1 << 20)
+
+/* HCCA-AP - disable duration overwriting. */
+#define TX_CMD_FLG_DUR_MSK __constant_cpu_to_le32(1 << 25)
+
+/*
+ * TX command security control
+ */
+#define TX_CMD_SEC_WEP  	0x01
+#define TX_CMD_SEC_CCM  	0x02
+#define TX_CMD_SEC_TKIP		0x03
+#define TX_CMD_SEC_MSK		0x03
+#define TX_CMD_SEC_SHIFT	6
+#define TX_CMD_SEC_KEY128	0x08
+
+/*
+ * TX command Frame life time
+ */
+
+struct iwl_dram_scratch {
+	u8 try_cnt;
+	u8 bt_kill_cnt;
+	__le16 reserved;
+} __attribute__ ((packed));
+
+/*
+ * REPLY_TX = 0x1c (command)
+ */
+struct iwl_tx_cmd {
+	__le16 len;
+	__le16 next_frame_len;
+	__le32 tx_flags;
+#if IWL == 3945
+	u8 rate;
+	u8 sta_id;
+	u8 tid_tspec;
+#elif IWL == 4965
+	struct iwl_dram_scratch scratch;
+	__le32 rate_n_flags;
+	u8 sta_id;
+#endif
+	u8 sec_ctl;
+#if IWL == 4965
+	u8 initial_rate_index;
+	u8 reserved;
+#endif
+	u8 key[16];
+#if IWL == 3945
+	union {
+		u8 byte[8];
+		__le16 word[4];
+		__le32 dw[2];
+	} tkip_mic;
+	__le32 next_frame_info;
+#elif IWL == 4965
+	__le16 next_frame_flags;
+	__le16 reserved2;
+#endif
+	union {
+		__le32 life_time;
+		__le32 attempt;
+	} stop_time;
+#if IWL == 3945
+	u8 supp_rates[2];
+#elif IWL == 4965
+	__le32 dram_lsb_ptr;
+	u8 dram_msb_ptr;
+#endif
+	u8 rts_retry_limit;	/*byte 50 */
+	u8 data_retry_limit;	/*byte 51 */
+#if IWL == 4965
+	u8 tid_tspec;
+#endif
+	union {
+		__le16 pm_frame_timeout;
+		__le16 attempt_duration;
+	} timeout;
+	__le16 driver_txop;
+	u8 payload[0];
+	struct ieee80211_hdr hdr[0];
+} __attribute__ ((packed));
+
+/* TX command response is sent after *all* transmission attempts.
+ *
+ * NOTES:
+ *
+ * TX_STATUS_FAIL_NEXT_FRAG
+ *
+ * If the fragment flag in the MAC header for the frame being transmitted
+ * is set and there is insufficient time to transmit the next frame, the
+ * TX status will be returned with 'TX_STATUS_FAIL_NEXT_FRAG'.
+ *
+ * TX_STATUS_FIFO_UNDERRUN
+ *
+ * Indicates the host did not provide bytes to the FIFO fast enough while
+ * a TX was in progress.
+ *
+ * TX_STATUS_FAIL_MGMNT_ABORT
+ *
+ * This status is only possible if the ABORT ON MGMT RX parameter was
+ * set to true with the TX command.
+ *
+ * If the MSB of the status parameter is set then an abort sequence is
+ * required.  This sequence consists of the host activating the TX Abort
+ * control line, and then waiting for the TX Abort command response.  This
+ * indicates that a the device is no longer in a transmit state, and that the
+ * command FIFO has been cleared.  The host must then deactivate the TX Abort
+ * control line.  Receiving is still allowed in this case.
+ */
+enum {
+	TX_STATUS_SUCCESS = 0x01,
+	TX_STATUS_DIRECT_DONE = 0x02,
+	TX_STATUS_FAIL_SHORT_LIMIT = 0x82,
+	TX_STATUS_FAIL_LONG_LIMIT = 0x83,
+	TX_STATUS_FAIL_FIFO_UNDERRUN = 0x84,
+	TX_STATUS_FAIL_MGMNT_ABORT = 0x85,
+	TX_STATUS_FAIL_NEXT_FRAG = 0x86,
+	TX_STATUS_FAIL_LIFE_EXPIRE = 0x87,
+	TX_STATUS_FAIL_DEST_PS = 0x88,
+	TX_STATUS_FAIL_ABORTED = 0x89,
+	TX_STATUS_FAIL_BT_RETRY = 0x8a,
+	TX_STATUS_FAIL_STA_INVALID = 0x8b,
+	TX_STATUS_FAIL_FRAG_DROPPED = 0x8c,
+	TX_STATUS_FAIL_TID_DISABLE = 0x8d,
+	TX_STATUS_FAIL_FRAME_FLUSHED = 0x8e,
+	TX_STATUS_FAIL_INSUFFICIENT_CF_POLL = 0x8f,
+	TX_STATUS_FAIL_TX_LOCKED = 0x90,
+	TX_STATUS_FAIL_NO_BEACON_ON_RADAR = 0x91,
+};
+
+#define	TX_PACKET_MODE_REGULAR		0x0000
+#define	TX_PACKET_MODE_BURST_SEQ	0x0100
+#define	TX_PACKET_MODE_BURST_FIRST	0x0200
+
+enum {
+	TX_POWER_PA_NOT_ACTIVE = 0x0,
+};
+
+enum {
+	TX_STATUS_MSK = 0x000000ff,	/* bits 0:7 */
+	TX_STATUS_DELAY_MSK = 0x00000040,
+	TX_STATUS_ABORT_MSK = 0x00000080,
+	TX_PACKET_MODE_MSK = 0x0000ff00,	/* bits 8:15 */
+	TX_FIFO_NUMBER_MSK = 0x00070000,	/* bits 16:18 */
+	TX_RESERVED = 0x00780000,	/* bits 19:22 */
+	TX_POWER_PA_DETECT_MSK = 0x7f800000,	/* bits 23:30 */
+	TX_ABORT_REQUIRED_MSK = 0x80000000,	/* bits 31:31 */
+};
+
+/* *******************************
+ * TX aggregation state
+ ******************************* */
+
+enum {
+	AGG_TX_STATE_TRANSMITTED = 0x00,
+	AGG_TX_STATE_UNDERRUN_MSK = 0x01,
+	AGG_TX_STATE_BT_PRIO_MSK = 0x02,
+	AGG_TX_STATE_FEW_BYTES_MSK = 0x04,
+	AGG_TX_STATE_ABORT_MSK = 0x08,
+	AGG_TX_STATE_LAST_SENT_TTL_MSK = 0x10,
+	AGG_TX_STATE_LAST_SENT_TRY_CNT_MSK = 0x20,
+	AGG_TX_STATE_LAST_SENT_BT_KILL_MSK = 0x40,
+	AGG_TX_STATE_SCD_QUERY_MSK = 0x80,
+	AGG_TX_STATE_TEST_BAD_CRC32_MSK = 0x100,
+	AGG_TX_STATE_RESPONSE_MSK = 0x1ff,
+	AGG_TX_STATE_DUMP_TX_MSK = 0x200,
+	AGG_TX_STATE_DELAY_TX_MSK = 0x400
+};
+
+#define AGG_TX_STATE_LAST_SENT_MSK \
+(AGG_TX_STATE_LAST_SENT_TTL_MSK | \
+ AGG_TX_STATE_LAST_SENT_TRY_CNT_MSK | \
+ AGG_TX_STATE_LAST_SENT_BT_KILL_MSK)
+
+#define AGG_TX_STATE_TRY_CNT_POS 12
+#define AGG_TX_STATE_TRY_CNT_MSK 0xf000
+
+#define AGG_TX_STATE_SEQ_NUM_POS 16
+#define AGG_TX_STATE_SEQ_NUM_MSK 0xffff0000
+
+/*
+ * REPLY_TX = 0x1c (response)
+ */
+#if IWL == 4965
+struct iwl_tx_resp {
+	u8 frame_count;		/* 1 no aggregation, >1 aggregation */
+	u8 bt_kill_count;
+	u8 failure_rts;
+	u8 failure_frame;
+	__le32 rate_n_flags;
+	__le16 wireless_media_time;
+	__le16 reserved;
+	__le32 pa_power1;
+	__le32 pa_power2;
+	__le32 status;	/* TX status (for aggregation status of 1st frame) */
+} __attribute__ ((packed));
+
+#elif IWL == 3945
+struct iwl_tx_resp {
+	u8 failure_rts;
+	u8 failure_frame;
+	u8 bt_kill_count;
+	u8 rate;
+	__le32 wireless_media_time;
+	__le32 status;	/* TX status (for aggregation status of 1st frame) */
+} __attribute__ ((packed));
+#endif
+
+/*
+ * REPLY_COMPRESSED_BA = 0xc5 (response only, not a command)
+ */
+struct iwl_compressed_ba_resp {
+	__le32 sta_addr_lo32;
+	__le16 sta_addr_hi16;
+	__le16 reserved;
+	u8 sta_id;
+	u8 tid;
+	__le16 ba_seq_ctl;
+	__le32 ba_bitmap0;
+	__le32 ba_bitmap1;
+	__le16 scd_flow;
+	__le16 scd_ssn;
+} __attribute__ ((packed));
+
+/*
+ * REPLY_TX_PWR_TABLE_CMD = 0x97 (command, has simple generic response)
+ */
+struct iwl_txpowertable_cmd {
+	u8 band;		/* 0: 5 GHz, 1: 2.4 GHz */
+	u8 reserved;
+	__le16 channel;
+#if IWL == 3945
+	struct iwl_power_per_rate power[IWL_MAX_RATES];
+#elif IWL == 4965
+	struct iwl_tx_power_db tx_power;
+#endif
+} __attribute__ ((packed));
+
+#if IWL == 3945
+struct iwl_rate_scaling_info {
+	__le16 rate_n_flags;
+	u8 try_cnt;
+	u8 next_rate_index;
+} __attribute__ ((packed));
+
+/**
+ * struct iwl_rate_scaling_cmd - Rate Scaling Command & Response
+ *
+ * REPLY_RATE_SCALE = 0x47 (command, has simple generic response)
+ *
+ * NOTE: The table of rates passed to the uCode via the
+ * RATE_SCALE command sets up the corresponding order of
+ * rates used for all related commands, including rate
+ * masks, etc.
+ *
+ * For example, if you set 9MB (PLCP 0x0f) as the first
+ * rate in the rate table, the bit mask for that rate
+ * when passed through ofdm_basic_rates on the REPLY_RXON
+ * command would be bit 0 (1<<0)
+ */
+struct iwl_rate_scaling_cmd {
+	u8 table_id;
+	u8 reserved[3];
+	struct iwl_rate_scaling_info table[IWL_MAX_RATES];
+} __attribute__ ((packed));
+
+#elif IWL == 4965
+
+/*RS_NEW_API: only TLC_RTS remains and moved to bit 0 */
+#define  LINK_QUAL_FLAGS_SET_STA_TLC_RTS_MSK	(1<<0)
+
+#define  LINK_QUAL_AC_NUM AC_NUM
+#define  LINK_QUAL_MAX_RETRY_NUM 16
+
+#define  LINK_QUAL_ANT_A_MSK (1<<0)
+#define  LINK_QUAL_ANT_B_MSK (1<<1)
+#define  LINK_QUAL_ANT_MSK   (LINK_QUAL_ANT_A_MSK|LINK_QUAL_ANT_B_MSK)
+
+struct iwl_link_qual_general_params {
+	u8 flags;
+	u8 mimo_delimiter;
+	u8 single_stream_ant_msk;
+	u8 dual_stream_ant_msk;
+	u8 start_rate_index[LINK_QUAL_AC_NUM];
+} __attribute__ ((packed));
+
+struct iwl_link_qual_agg_params {
+	__le16 agg_time_limit;
+	u8 agg_dis_start_th;
+	u8 agg_frame_cnt_limit;
+	__le32 reserved;
+} __attribute__ ((packed));
+
+/*
+ * REPLY_TX_LINK_QUALITY_CMD = 0x4e (command, has simple generic response)
+ */
+struct iwl_link_quality_cmd {
+	u8 sta_id;
+	u8 reserved1;
+	__le16 control;
+	struct iwl_link_qual_general_params general_params;
+	struct iwl_link_qual_agg_params agg_params;
+	struct {
+		__le32 rate_n_flags;
+	} rs_table[LINK_QUAL_MAX_RETRY_NUM];
+	__le32 reserved2;
+} __attribute__ ((packed));
+#endif
+
+/*
+ * REPLY_BT_CONFIG = 0x9b (command, has simple generic response)
+ */
+struct iwl_bt_cmd {
+	u8 flags;
+	u8 lead_time;
+	u8 max_kill;
+	u8 reserved;
+	__le32 kill_ack_mask;
+	__le32 kill_cts_mask;
+} __attribute__ ((packed));
+
+/******************************************************************************
+ * (6)
+ * Spectrum Management (802.11h) Commands, Responses, Notifications:
+ *
+ *****************************************************************************/
+
+/*
+ * Spectrum Management
+ */
+#define MEASUREMENT_FILTER_FLAG (RXON_FILTER_PROMISC_MSK         | \
+				 RXON_FILTER_CTL2HOST_MSK        | \
+				 RXON_FILTER_ACCEPT_GRP_MSK      | \
+				 RXON_FILTER_DIS_DECRYPT_MSK     | \
+				 RXON_FILTER_DIS_GRP_DECRYPT_MSK | \
+				 RXON_FILTER_ASSOC_MSK           | \
+				 RXON_FILTER_BCON_AWARE_MSK)
+
+struct iwl_measure_channel {
+	__le32 duration;	/* measurement duration in extended beacon
+				 * format */
+	u8 channel;		/* channel to measure */
+	u8 type;		/* see enum iwl_measure_type */
+	__le16 reserved;
+} __attribute__ ((packed));
+
+/*
+ * REPLY_SPECTRUM_MEASUREMENT_CMD = 0x74 (command)
+ */
+struct iwl_spectrum_cmd {
+	__le16 len;		/* number of bytes starting from token */
+	u8 token;		/* token id */
+	u8 id;			/* measurement id -- 0 or 1 */
+	u8 origin;		/* 0 = TGh, 1 = other, 2 = TGk */
+	u8 periodic;		/* 1 = periodic */
+	__le16 path_loss_timeout;
+	__le32 start_time;	/* start time in extended beacon format */
+	__le32 reserved2;
+	__le32 flags;		/* rxon flags */
+	__le32 filter_flags;	/* rxon filter flags */
+	__le16 channel_count;	/* minimum 1, maximum 10 */
+	__le16 reserved3;
+	struct iwl_measure_channel channels[10];
+} __attribute__ ((packed));
+
+/*
+ * REPLY_SPECTRUM_MEASUREMENT_CMD = 0x74 (response)
+ */
+struct iwl_spectrum_resp {
+	u8 token;
+	u8 id;			/* id of the prior command replaced, or 0xff */
+	__le16 status;		/* 0 - command will be handled
+				 * 1 - cannot handle (conflicts with another
+				 *     measurement) */
+} __attribute__ ((packed));
+
+enum iwl_measurement_state {
+	IWL_MEASUREMENT_START = 0,
+	IWL_MEASUREMENT_STOP = 1,
+};
+
+enum iwl_measurement_status {
+	IWL_MEASUREMENT_OK = 0,
+	IWL_MEASUREMENT_CONCURRENT = 1,
+	IWL_MEASUREMENT_CSA_CONFLICT = 2,
+	IWL_MEASUREMENT_TGH_CONFLICT = 3,
+	/* 4-5 reserved */
+	IWL_MEASUREMENT_STOPPED = 6,
+	IWL_MEASUREMENT_TIMEOUT = 7,
+	IWL_MEASUREMENT_PERIODIC_FAILED = 8,
+};
+
+#define NUM_ELEMENTS_IN_HISTOGRAM 8
+
+struct iwl_measurement_histogram {
+	__le32 ofdm[NUM_ELEMENTS_IN_HISTOGRAM];	/* in 0.8usec counts */
+	__le32 cck[NUM_ELEMENTS_IN_HISTOGRAM];	/* in 1usec counts */
+} __attribute__ ((packed));
+
+/* clear channel availability counters */
+struct iwl_measurement_cca_counters {
+	__le32 ofdm;
+	__le32 cck;
+} __attribute__ ((packed));
+
+enum iwl_measure_type {
+	IWL_MEASURE_BASIC = (1 << 0),
+	IWL_MEASURE_CHANNEL_LOAD = (1 << 1),
+	IWL_MEASURE_HISTOGRAM_RPI = (1 << 2),
+	IWL_MEASURE_HISTOGRAM_NOISE = (1 << 3),
+	IWL_MEASURE_FRAME = (1 << 4),
+	/* bits 5:6 are reserved */
+	IWL_MEASURE_IDLE = (1 << 7),
+};
+
+/*
+ * SPECTRUM_MEASURE_NOTIFICATION = 0x75 (notification only, not a command)
+ */
+struct iwl_spectrum_notification {
+	u8 id;			/* measurement id -- 0 or 1 */
+	u8 token;
+	u8 channel_index;	/* index in measurement channel list */
+	u8 state;		/* 0 - start, 1 - stop */
+	__le32 start_time;	/* lower 32-bits of TSF */
+	u8 band;		/* 0 - 5.2GHz, 1 - 2.4GHz */
+	u8 channel;
+	u8 type;		/* see enum iwl_measurement_type */
+	u8 reserved1;
+	/* NOTE:  cca_ofdm, cca_cck, basic_type, and histogram are only only
+	 * valid if applicable for measurement type requested. */
+	__le32 cca_ofdm;	/* cca fraction time in 40Mhz clock periods */
+	__le32 cca_cck;		/* cca fraction time in 44Mhz clock periods */
+	__le32 cca_time;	/* channel load time in usecs */
+	u8 basic_type;		/* 0 - bss, 1 - ofdm preamble, 2 -
+				 * unidentified */
+	u8 reserved2[3];
+	struct iwl_measurement_histogram histogram;
+	__le32 stop_time;	/* lower 32-bits of TSF */
+	__le32 status;		/* see iwl_measurement_status */
+} __attribute__ ((packed));
+
+/******************************************************************************
+ * (7)
+ * Power Management Commands, Responses, Notifications:
+ *
+ *****************************************************************************/
+
+/**
+ * struct iwl_powertable_cmd - Power Table Command
+ * @flags: See below:
+ *
+ * POWER_TABLE_CMD = 0x77 (command, has simple generic response)
+ *
+ * PM allow:
+ *   bit 0 - '0' Driver not allow power management
+ *           '1' Driver allow PM (use rest of parameters)
+ * uCode send sleep notifications:
+ *   bit 1 - '0' Don't send sleep notification
+ *           '1' send sleep notification (SEND_PM_NOTIFICATION)
+ * Sleep over DTIM
+ *   bit 2 - '0' PM have to walk up every DTIM
+ *           '1' PM could sleep over DTIM till listen Interval.
+ * PCI power managed
+ *   bit 3 - '0' (PCI_LINK_CTRL & 0x1)
+ *           '1' !(PCI_LINK_CTRL & 0x1)
+ * Force sleep Modes
+ *   bit 31/30- '00' use both mac/xtal sleeps
+ *              '01' force Mac sleep
+ *              '10' force xtal sleep
+ *              '11' Illegal set
+ *
+ * NOTE: if sleep_interval[SLEEP_INTRVL_TABLE_SIZE-1] > DTIM period then
+ * ucode assume sleep over DTIM is allowed and we don't need to wakeup
+ * for every DTIM.
+ */
+#define IWL_POWER_VEC_SIZE 5
+
+
+#if IWL == 3945
+
+#define IWL_POWER_DRIVER_ALLOW_SLEEP_MSK	__constant_cpu_to_le32(1<<0)
+#define IWL_POWER_SLEEP_OVER_DTIM_MSK		__constant_cpu_to_le32(1<<2)
+#define IWL_POWER_PCI_PM_MSK			__constant_cpu_to_le32(1<<3)
+struct iwl_powertable_cmd {
+	__le32 flags;
+	__le32 rx_data_timeout;
+	__le32 tx_data_timeout;
+	__le32 sleep_interval[IWL_POWER_VEC_SIZE];
+} __attribute__((packed));
+
+#elif IWL == 4965
+
+#define IWL_POWER_DRIVER_ALLOW_SLEEP_MSK	__constant_cpu_to_le16(1<<0)
+#define IWL_POWER_SLEEP_OVER_DTIM_MSK		__constant_cpu_to_le16(1<<2)
+#define IWL_POWER_PCI_PM_MSK			__constant_cpu_to_le16(1<<3)
+
+struct iwl_powertable_cmd {
+	__le16 flags;
+	u8 keep_alive_seconds;
+	u8 debug_flags;
+	__le32 rx_data_timeout;
+	__le32 tx_data_timeout;
+	__le32 sleep_interval[IWL_POWER_VEC_SIZE];
+	__le32 keep_alive_beacons;
+} __attribute__ ((packed));
+#endif
+
+/*
+ * PM_SLEEP_NOTIFICATION = 0x7A (notification only, not a command)
+ * 3945 and 4965 identical.
+ */
+struct iwl_sleep_notification {
+	u8 pm_sleep_mode;
+	u8 pm_wakeup_src;
+	__le16 reserved;
+	__le32 sleep_time;
+	__le32 tsf_low;
+	__le32 bcon_timer;
+} __attribute__ ((packed));
+
+/* Sleep states.  3945 and 4965 identical. */
+enum {
+	IWL_PM_NO_SLEEP = 0,
+	IWL_PM_SLP_MAC = 1,
+	IWL_PM_SLP_FULL_MAC_UNASSOCIATE = 2,
+	IWL_PM_SLP_FULL_MAC_CARD_STATE = 3,
+	IWL_PM_SLP_PHY = 4,
+	IWL_PM_SLP_REPENT = 5,
+	IWL_PM_WAKEUP_BY_TIMER = 6,
+	IWL_PM_WAKEUP_BY_DRIVER = 7,
+	IWL_PM_WAKEUP_BY_RFKILL = 8,
+	/* 3 reserved */
+	IWL_PM_NUM_OF_MODES = 12,
+};
+
+/*
+ * REPLY_CARD_STATE_CMD = 0xa0 (command, has simple generic response)
+ */
+#define CARD_STATE_CMD_DISABLE 0x00	/* Put card to sleep */
+#define CARD_STATE_CMD_ENABLE  0x01	/* Wake up card */
+#define CARD_STATE_CMD_HALT    0x02	/* Power down permanently */
+struct iwl_card_state_cmd {
+	__le32 status;		/* CARD_STATE_CMD_* request new power state */
+} __attribute__ ((packed));
+
+/*
+ * CARD_STATE_NOTIFICATION = 0xa1 (notification only, not a command)
+ */
+struct iwl_card_state_notif {
+	__le32 flags;
+} __attribute__ ((packed));
+
+#define HW_CARD_DISABLED   0x01
+#define SW_CARD_DISABLED   0x02
+#define RF_CARD_DISABLED   0x04
+#define RXON_CARD_DISABLED 0x10
+
+struct iwl_ct_kill_config {
+	__le32   reserved;
+	__le32   critical_temperature_M;
+	__le32   critical_temperature_R;
+}  __attribute__ ((packed));
+
+/******************************************************************************
+ * (8)
+ * Scan Commands, Responses, Notifications:
+ *
+ *****************************************************************************/
+
+struct iwl_scan_channel {
+	/* type is defined as:
+	 * 0:0 active (0 - passive)
+	 * 1:4 SSID direct
+	 *     If 1 is set then corresponding SSID IE is transmitted in probe
+	 * 5:7 reserved
+	 */
+	u8 type;
+	u8 channel;
+	struct iwl_tx_power tpc;
+	__le16 active_dwell;
+	__le16 passive_dwell;
+} __attribute__ ((packed));
+
+struct iwl_ssid_ie {
+	u8 id;
+	u8 len;
+	u8 ssid[32];
+} __attribute__ ((packed));
+
+#define PROBE_OPTION_MAX        0x4
+#define TX_CMD_LIFE_TIME_INFINITE	__constant_cpu_to_le32(0xFFFFFFFF)
+#define IWL_GOOD_CRC_TH		__constant_cpu_to_le16(1)
+#define IWL_MAX_SCAN_SIZE 1024
+
+/*
+ * REPLY_SCAN_CMD = 0x80 (command)
+ */
+struct iwl_scan_cmd {
+	__le16 len;
+	u8 reserved0;
+	u8 channel_count;
+	__le16 quiet_time;     /* dwell only this long on quiet chnl
+				* (active scan) */
+	__le16 quiet_plcp_th;  /* quiet chnl is < this # pkts (typ. 1) */
+	__le16 good_CRC_th;    /* passive -> active promotion threshold */
+#if IWL == 3945
+	__le16 reserved1;
+#elif IWL == 4965
+	__le16 rx_chain;
+#endif
+	__le32 max_out_time;   /* max usec to be out of associated (service)
+				* chnl */
+	__le32 suspend_time;   /* pause scan this long when returning to svc
+				* chnl.
+				* 3945 -- 31:24 # beacons, 19:0 additional usec,
+				* 4965 -- 31:22 # beacons, 21:0 additional usec.
+				*/
+	__le32 flags;
+	__le32 filter_flags;
+
+	struct iwl_tx_cmd tx_cmd;
+	struct iwl_ssid_ie direct_scan[PROBE_OPTION_MAX];
+
+	u8 data[0];
+	/*
+	 * The channels start after the probe request payload and are of type:
+	 *
+	 * struct iwl_scan_channel channels[0];
+	 *
+	 * NOTE:  Only one band of channels can be scanned per pass.  You
+	 * can not mix 2.4GHz channels and 5.2GHz channels and must
+	 * request a scan multiple times (not concurrently)
+	 *
+	 */
+} __attribute__ ((packed));
+
+/* Can abort will notify by complete notification with abort status. */
+#define CAN_ABORT_STATUS	__constant_cpu_to_le32(0x1)
+/* complete notification statuses */
+#define ABORT_STATUS            0x2
+
+/*
+ * REPLY_SCAN_CMD = 0x80 (response)
+ */
+struct iwl_scanreq_notification {
+	__le32 status;		/* 1: okay, 2: cannot fulfill request */
+} __attribute__ ((packed));
+
+/*
+ * SCAN_START_NOTIFICATION = 0x82 (notification only, not a command)
+ */
+struct iwl_scanstart_notification {
+	__le32 tsf_low;
+	__le32 tsf_high;
+	__le32 beacon_timer;
+	u8 channel;
+	u8 band;
+	u8 reserved[2];
+	__le32 status;
+} __attribute__ ((packed));
+
+#define  SCAN_OWNER_STATUS 0x1;
+#define  MEASURE_OWNER_STATUS 0x2;
+
+#define NUMBER_OF_STATISTICS 1	/* first __le32 is good CRC */
+/*
+ * SCAN_RESULTS_NOTIFICATION = 0x83 (notification only, not a command)
+ */
+struct iwl_scanresults_notification {
+	u8 channel;
+	u8 band;
+	u8 reserved[2];
+	__le32 tsf_low;
+	__le32 tsf_high;
+	__le32 statistics[NUMBER_OF_STATISTICS];
+} __attribute__ ((packed));
+
+/*
+ * SCAN_COMPLETE_NOTIFICATION = 0x84 (notification only, not a command)
+ */
+struct iwl_scancomplete_notification {
+	u8 scanned_channels;
+	u8 status;
+	u8 reserved;
+	u8 last_channel;
+	__le32 tsf_low;
+	__le32 tsf_high;
+} __attribute__ ((packed));
+
+
+/******************************************************************************
+ * (9)
+ * IBSS/AP Commands and Notifications:
+ *
+ *****************************************************************************/
+
+/*
+ * BEACON_NOTIFICATION = 0x90 (notification only, not a command)
+ */
+struct iwl_beacon_notif {
+	struct iwl_tx_resp beacon_notify_hdr;
+	__le32 low_tsf;
+	__le32 high_tsf;
+	__le32 ibss_mgr_status;
+} __attribute__ ((packed));
+
+/*
+ * REPLY_TX_BEACON = 0x91 (command, has simple generic response)
+ */
+struct iwl_tx_beacon_cmd {
+	struct iwl_tx_cmd tx;
+	__le16 tim_idx;
+	u8 tim_size;
+	u8 reserved1;
+	struct ieee80211_hdr frame[0];	/* beacon frame */
+} __attribute__ ((packed));
+
+/******************************************************************************
+ * (10)
+ * Statistics Commands and Notifications:
+ *
+ *****************************************************************************/
+
+#define IWL_TEMP_CONVERT 260
+
+#define SUP_RATE_11A_MAX_NUM_CHANNELS  8
+#define SUP_RATE_11B_MAX_NUM_CHANNELS  4
+#define SUP_RATE_11G_MAX_NUM_CHANNELS  12
+
+/* Used for passing to driver number of successes and failures per rate */
+struct rate_histogram {
+	union {
+		__le32 a[SUP_RATE_11A_MAX_NUM_CHANNELS];
+		__le32 b[SUP_RATE_11B_MAX_NUM_CHANNELS];
+		__le32 g[SUP_RATE_11G_MAX_NUM_CHANNELS];
+	} success;
+	union {
+		__le32 a[SUP_RATE_11A_MAX_NUM_CHANNELS];
+		__le32 b[SUP_RATE_11B_MAX_NUM_CHANNELS];
+		__le32 g[SUP_RATE_11G_MAX_NUM_CHANNELS];
+	} failed;
+} __attribute__ ((packed));
+
+/* statistics command response */
+
+struct statistics_rx_phy {
+	__le32 ina_cnt;
+	__le32 fina_cnt;
+	__le32 plcp_err;
+	__le32 crc32_err;
+	__le32 overrun_err;
+	__le32 early_overrun_err;
+	__le32 crc32_good;
+	__le32 false_alarm_cnt;
+	__le32 fina_sync_err_cnt;
+	__le32 sfd_timeout;
+	__le32 fina_timeout;
+	__le32 unresponded_rts;
+	__le32 rxe_frame_limit_overrun;
+	__le32 sent_ack_cnt;
+	__le32 sent_cts_cnt;
+#if IWL == 4965
+	__le32 sent_ba_rsp_cnt;
+	__le32 dsp_self_kill;
+	__le32 mh_format_err;
+	__le32 re_acq_main_rssi_sum;
+	__le32 reserved3;
+#endif
+} __attribute__ ((packed));
+
+#if IWL == 4965
+struct statistics_rx_ht_phy {
+	__le32 plcp_err;
+	__le32 overrun_err;
+	__le32 early_overrun_err;
+	__le32 crc32_good;
+	__le32 crc32_err;
+	__le32 mh_format_err;
+	__le32 agg_crc32_good;
+	__le32 agg_mpdu_cnt;
+	__le32 agg_cnt;
+	__le32 reserved2;
+} __attribute__ ((packed));
+#endif
+
+struct statistics_rx_non_phy {
+	__le32 bogus_cts;	/* CTS received when not expecting CTS */
+	__le32 bogus_ack;	/* ACK received when not expecting ACK */
+	__le32 non_bssid_frames;	/* number of frames with BSSID that
+					 * doesn't belong to the STA BSSID */
+	__le32 filtered_frames;	/* count frames that were dumped in the
+				 * filtering process */
+	__le32 non_channel_beacons;	/* beacons with our bss id but not on
+					 * our serving channel */
+#if IWL == 4965
+	__le32 channel_beacons;	/* beacons with our bss id and in our
+				 * serving channel */
+	__le32 num_missed_bcon;	/* number of missed beacons */
+	__le32 adc_rx_saturation_time;	/* count in 0.8us units the time the
+					 * ADC was in saturation */
+	__le32 ina_detection_search_time;/* total time (in 0.8us) searched
+					  * for INA */
+	__le32 beacon_silence_rssi_a;	/* RSSI silence after beacon frame */
+	__le32 beacon_silence_rssi_b;	/* RSSI silence after beacon frame */
+	__le32 beacon_silence_rssi_c;	/* RSSI silence after beacon frame */
+	__le32 interference_data_flag;	/* flag for interference data
+					 * availability. 1 when data is
+					 * available. */
+	__le32 channel_load;	/* counts RX Enable time */
+	__le32 dsp_false_alarms;	/* DSP false alarm (both OFDM
+					 * and CCK) counter */
+	__le32 beacon_rssi_a;
+	__le32 beacon_rssi_b;
+	__le32 beacon_rssi_c;
+	__le32 beacon_energy_a;
+	__le32 beacon_energy_b;
+	__le32 beacon_energy_c;
+#endif
+} __attribute__ ((packed));
+
+struct statistics_rx {
+	struct statistics_rx_phy ofdm;
+	struct statistics_rx_phy cck;
+	struct statistics_rx_non_phy general;
+#if IWL == 4965
+	struct statistics_rx_ht_phy ofdm_ht;
+#endif
+} __attribute__ ((packed));
+
+#if IWL == 4965
+struct statistics_tx_non_phy_agg {
+	__le32 ba_timeout;
+	__le32 ba_reschedule_frames;
+	__le32 scd_query_agg_frame_cnt;
+	__le32 scd_query_no_agg;
+	__le32 scd_query_agg;
+	__le32 scd_query_mismatch;
+	__le32 frame_not_ready;
+	__le32 underrun;
+	__le32 bt_prio_kill;
+	__le32 rx_ba_rsp_cnt;
+	__le32 reserved2;
+	__le32 reserved3;
+} __attribute__ ((packed));
+#endif
+
+struct statistics_tx {
+	__le32 preamble_cnt;
+	__le32 rx_detected_cnt;
+	__le32 bt_prio_defer_cnt;
+	__le32 bt_prio_kill_cnt;
+	__le32 few_bytes_cnt;
+	__le32 cts_timeout;
+	__le32 ack_timeout;
+	__le32 expected_ack_cnt;
+	__le32 actual_ack_cnt;
+#if IWL == 4965
+	__le32 dump_msdu_cnt;
+	__le32 burst_abort_next_frame_mismatch_cnt;
+	__le32 burst_abort_missing_next_frame_cnt;
+	__le32 cts_timeout_collision;
+	__le32 ack_or_ba_timeout_collision;
+	struct statistics_tx_non_phy_agg agg;
+#endif
+} __attribute__ ((packed));
+
+struct statistics_dbg {
+	__le32 burst_check;
+	__le32 burst_count;
+	__le32 reserved[4];
+} __attribute__ ((packed));
+
+struct statistics_div {
+	__le32 tx_on_a;
+	__le32 tx_on_b;
+	__le32 exec_time;
+	__le32 probe_time;
+#if IWL == 4965
+	__le32 reserved1;
+	__le32 reserved2;
+#endif
+} __attribute__ ((packed));
+
+struct statistics_general {
+	__le32 temperature;
+#if IWL == 4965
+	__le32 temperature_m;
+#endif
+	struct statistics_dbg dbg;
+	__le32 sleep_time;
+	__le32 slots_out;
+	__le32 slots_idle;
+	__le32 ttl_timestamp;
+	struct statistics_div div;
+#if IWL == 4965
+	__le32 rx_enable_counter;
+	__le32 reserved1;
+	__le32 reserved2;
+	__le32 reserved3;
+#endif
+} __attribute__ ((packed));
+
+/*
+ * REPLY_STATISTICS_CMD = 0x9c,
+ * 3945 and 4965 identical.
+ *
+ * This command triggers an immediate response containing uCode statistics.
+ * The response is in the same format as STATISTICS_NOTIFICATION 0x9d, below.
+ *
+ * If the CLEAR_STATS configuration flag is set, uCode will clear its
+ * internal copy of the statistics (counters) after issuing the response.
+ * This flag does not affect STATISTICS_NOTIFICATIONs after beacons (see below).
+ *
+ * If the DISABLE_NOTIF configuration flag is set, uCode will not issue
+ * STATISTICS_NOTIFICATIONs after received beacons (see below).  This flag
+ * does not affect the response to the REPLY_STATISTICS_CMD 0x9c itself.
+ */
+#define IWL_STATS_CONF_CLEAR_STATS __constant_cpu_to_le32(0x1)	/* see above */
+#define IWL_STATS_CONF_DISABLE_NOTIF __constant_cpu_to_le32(0x2)/* see above */
+struct iwl_statistics_cmd {
+	__le32 configuration_flags;	/* IWL_STATS_CONF_* */
+} __attribute__ ((packed));
+
+/*
+ * STATISTICS_NOTIFICATION = 0x9d (notification only, not a command)
+ *
+ * By default, uCode issues this notification after receiving a beacon
+ * while associated.  To disable this behavior, set DISABLE_NOTIF flag in the
+ * REPLY_STATISTICS_CMD 0x9c, above.
+ *
+ * Statistics counters continue to increment beacon after beacon, but are
+ * cleared when changing channels or when driver issues REPLY_STATISTICS_CMD
+ * 0x9c with CLEAR_STATS bit set (see above).
+ *
+ * uCode also issues this notification during scans.  uCode clears statistics
+ * appropriately so that each notification contains statistics for only the
+ * one channel that has just been scanned.
+ */
+#define STATISTICS_REPLY_FLG_BAND_24G_MSK         __constant_cpu_to_le32(0x2)
+#define STATISTICS_REPLY_FLG_FAT_MODE_MSK         __constant_cpu_to_le32(0x8)
+struct iwl_notif_statistics {
+	__le32 flag;
+	struct statistics_rx rx;
+	struct statistics_tx tx;
+	struct statistics_general general;
+} __attribute__ ((packed));
+
+
+/*
+ * MISSED_BEACONS_NOTIFICATION = 0xa2 (notification only, not a command)
+ */
+/* if ucode missed CONSECUTIVE_MISSED_BCONS_TH beacons in a row,
+ * then this notification will be sent. */
+#define CONSECUTIVE_MISSED_BCONS_TH 20
+
+struct iwl_missed_beacon_notif {
+	__le32 consequtive_missed_beacons;
+	__le32 total_missed_becons;
+	__le32 num_expected_beacons;
+	__le32 num_recvd_beacons;
+} __attribute__ ((packed));
+
+/******************************************************************************
+ * (11)
+ * Rx Calibration Commands:
+ *
+ *****************************************************************************/
+
+#define PHY_CALIBRATE_DIFF_GAIN_CMD (7)
+#define HD_TABLE_SIZE  (11)
+
+struct iwl_sensitivity_cmd {
+	__le16 control;
+	__le16 table[HD_TABLE_SIZE];
+} __attribute__ ((packed));
+
+struct iwl_calibration_cmd {
+	u8 opCode;
+	u8 flags;
+	__le16 reserved;
+	s8 diff_gain_a;
+	s8 diff_gain_b;
+	s8 diff_gain_c;
+	u8 reserved1;
+} __attribute__ ((packed));
+
+/******************************************************************************
+ * (12)
+ * Miscellaneous Commands:
+ *
+ *****************************************************************************/
+
+/*
+ * LEDs Command & Response
+ * REPLY_LEDS_CMD = 0x48 (command, has simple generic response)
+ *
+ * For each of 3 possible LEDs (Activity/Link/Tech, selected by "id" field),
+ * this command turns it on or off, or sets up a periodic blinking cycle.
+ */
+struct iwl_led_cmd {
+	__le32 interval;	/* "interval" in uSec */
+	u8 id;			/* 1: Activity, 2: Link, 3: Tech */
+	u8 off;			/* # intervals off while blinking;
+				 * "0", with >0 "on" value, turns LED on */
+	u8 on;			/* # intervals on while blinking;
+				 * "0", regardless of "off", turns LED off */
+	u8 reserved;
+} __attribute__ ((packed));
+
+/******************************************************************************
+ * (13)
+ * Union of all expected notifications/responses:
+ *
+ *****************************************************************************/
+
+struct iwl_rx_packet {
+	__le32 len;
+	struct iwl_cmd_header hdr;
+	union {
+		struct iwl_alive_resp alive_frame;
+		struct iwl_rx_frame rx_frame;
+		struct iwl_tx_resp tx_resp;
+		struct iwl_spectrum_notification spectrum_notif;
+		struct iwl_csa_notification csa_notif;
+		struct iwl_error_resp err_resp;
+		struct iwl_card_state_notif card_state_notif;
+		struct iwl_beacon_notif beacon_status;
+		struct iwl_add_sta_resp add_sta;
+		struct iwl_sleep_notification sleep_notif;
+		struct iwl_spectrum_resp spectrum;
+		struct iwl_notif_statistics stats;
+#if IWL == 4965
+		struct iwl_compressed_ba_resp compressed_ba;
+		struct iwl_missed_beacon_notif missed_beacon;
+#endif
+		__le32 status;
+		u8 raw[0];
+	} u;
+} __attribute__ ((packed));
+
+#define IWL_RX_FRAME_SIZE        (4 + sizeof(struct iwl_rx_frame))
+
+#endif				/* __iwl_commands_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-debug.h b/drivers/net/wireless/iwlwifi/iwl-debug.h
new file mode 100644
index 000000000000..72318d78957e
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-debug.h
@@ -0,0 +1,152 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2003 - 2007 Intel Corporation. All rights reserved.
+ *
+ * Portions of this file are derived from the ipw3945 project.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * James P. Ketrenos <ipw2100-admin@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+
+#ifndef __iwl_debug_h__
+#define __iwl_debug_h__
+
+#ifdef CONFIG_IWLWIFI_DEBUG
+extern u32 iwl_debug_level;
+#define IWL_DEBUG(level, fmt, args...) \
+do { if (iwl_debug_level & (level)) \
+  printk(KERN_ERR DRV_NAME": %c %s " fmt, \
+	 in_interrupt() ? 'I' : 'U', __FUNCTION__ , ## args); } while (0)
+
+#define IWL_DEBUG_LIMIT(level, fmt, args...) \
+do { if ((iwl_debug_level & (level)) && net_ratelimit()) \
+  printk(KERN_ERR DRV_NAME": %c %s " fmt, \
+	 in_interrupt() ? 'I' : 'U', __FUNCTION__ , ## args); } while (0)
+#else
+static inline void IWL_DEBUG(int level, const char *fmt, ...)
+{
+}
+static inline void IWL_DEBUG_LIMIT(int level, const char *fmt, ...)
+{
+}
+#endif				/* CONFIG_IWLWIFI_DEBUG */
+
+/*
+ * To use the debug system;
+ *
+ * If you are defining a new debug classification, simply add it to the #define
+ * list here in the form of:
+ *
+ * #define IWL_DL_xxxx VALUE
+ *
+ * shifting value to the left one bit from the previous entry.  xxxx should be
+ * the name of the classification (for example, WEP)
+ *
+ * You then need to either add a IWL_xxxx_DEBUG() macro definition for your
+ * classification, or use IWL_DEBUG(IWL_DL_xxxx, ...) whenever you want
+ * to send output to that classification.
+ *
+ * To add your debug level to the list of levels seen when you perform
+ *
+ * % cat /proc/net/iwl/debug_level
+ *
+ * you simply need to add your entry to the iwl_debug_levels array.
+ *
+ * If you do not see debug_level in /proc/net/iwl then you do not have
+ * CONFIG_IWLWIFI_DEBUG defined in your kernel configuration
+ *
+ */
+
+#define IWL_DL_INFO          (1<<0)
+#define IWL_DL_MAC80211      (1<<1)
+#define IWL_DL_HOST_COMMAND  (1<<2)
+#define IWL_DL_STATE         (1<<3)
+
+#define IWL_DL_RADIO         (1<<7)
+#define IWL_DL_POWER         (1<<8)
+#define IWL_DL_TEMP          (1<<9)
+
+#define IWL_DL_NOTIF         (1<<10)
+#define IWL_DL_SCAN          (1<<11)
+#define IWL_DL_ASSOC         (1<<12)
+#define IWL_DL_DROP          (1<<13)
+
+#define IWL_DL_TXPOWER       (1<<14)
+
+#define IWL_DL_AP            (1<<15)
+
+#define IWL_DL_FW            (1<<16)
+#define IWL_DL_RF_KILL       (1<<17)
+#define IWL_DL_FW_ERRORS     (1<<18)
+
+#define IWL_DL_LED           (1<<19)
+
+#define IWL_DL_RATE          (1<<20)
+
+#define IWL_DL_CALIB         (1<<21)
+#define IWL_DL_WEP           (1<<22)
+#define IWL_DL_TX            (1<<23)
+#define IWL_DL_RX            (1<<24)
+#define IWL_DL_ISR           (1<<25)
+#define IWL_DL_HT            (1<<26)
+#define IWL_DL_IO            (1<<27)
+#define IWL_DL_11H           (1<<28)
+
+#define IWL_DL_STATS         (1<<29)
+#define IWL_DL_TX_REPLY      (1<<30)
+#define IWL_DL_QOS           (1<<31)
+
+#define IWL_ERROR(f, a...) printk(KERN_ERR DRV_NAME ": " f, ## a)
+#define IWL_WARNING(f, a...) printk(KERN_WARNING DRV_NAME ": " f, ## a)
+#define IWL_DEBUG_INFO(f, a...)    IWL_DEBUG(IWL_DL_INFO, f, ## a)
+
+#define IWL_DEBUG_MAC80211(f, a...)     IWL_DEBUG(IWL_DL_MAC80211, f, ## a)
+#define IWL_DEBUG_TEMP(f, a...)   IWL_DEBUG(IWL_DL_TEMP, f, ## a)
+#define IWL_DEBUG_SCAN(f, a...)   IWL_DEBUG(IWL_DL_SCAN, f, ## a)
+#define IWL_DEBUG_RX(f, a...)     IWL_DEBUG(IWL_DL_RX, f, ## a)
+#define IWL_DEBUG_TX(f, a...)     IWL_DEBUG(IWL_DL_TX, f, ## a)
+#define IWL_DEBUG_ISR(f, a...)    IWL_DEBUG(IWL_DL_ISR, f, ## a)
+#define IWL_DEBUG_LED(f, a...) IWL_DEBUG(IWL_DL_LED, f, ## a)
+#define IWL_DEBUG_WEP(f, a...)    IWL_DEBUG(IWL_DL_WEP, f, ## a)
+#define IWL_DEBUG_HC(f, a...) IWL_DEBUG(IWL_DL_HOST_COMMAND, f, ## a)
+#define IWL_DEBUG_CALIB(f, a...) IWL_DEBUG(IWL_DL_CALIB, f, ## a)
+#define IWL_DEBUG_FW(f, a...) IWL_DEBUG(IWL_DL_FW, f, ## a)
+#define IWL_DEBUG_RF_KILL(f, a...) IWL_DEBUG(IWL_DL_RF_KILL, f, ## a)
+#define IWL_DEBUG_DROP(f, a...) IWL_DEBUG(IWL_DL_DROP, f, ## a)
+#define IWL_DEBUG_DROP_LIMIT(f, a...) IWL_DEBUG_LIMIT(IWL_DL_DROP, f, ## a)
+#define IWL_DEBUG_AP(f, a...) IWL_DEBUG(IWL_DL_AP, f, ## a)
+#define IWL_DEBUG_TXPOWER(f, a...) IWL_DEBUG(IWL_DL_TXPOWER, f, ## a)
+#define IWL_DEBUG_IO(f, a...) IWL_DEBUG(IWL_DL_IO, f, ## a)
+#define IWL_DEBUG_RATE(f, a...) IWL_DEBUG(IWL_DL_RATE, f, ## a)
+#define IWL_DEBUG_RATE_LIMIT(f, a...) IWL_DEBUG_LIMIT(IWL_DL_RATE, f, ## a)
+#define IWL_DEBUG_NOTIF(f, a...) IWL_DEBUG(IWL_DL_NOTIF, f, ## a)
+#define IWL_DEBUG_ASSOC(f, a...) IWL_DEBUG(IWL_DL_ASSOC | IWL_DL_INFO, f, ## a)
+#define IWL_DEBUG_ASSOC_LIMIT(f, a...) \
+	IWL_DEBUG_LIMIT(IWL_DL_ASSOC | IWL_DL_INFO, f, ## a)
+#define IWL_DEBUG_HT(f, a...) IWL_DEBUG(IWL_DL_HT, f, ## a)
+#define IWL_DEBUG_STATS(f, a...) IWL_DEBUG(IWL_DL_STATS, f, ## a)
+#define IWL_DEBUG_TX_REPLY(f, a...) IWL_DEBUG(IWL_DL_TX_REPLY, f, ## a)
+#define IWL_DEBUG_QOS(f, a...)   IWL_DEBUG(IWL_DL_QOS, f, ## a)
+#define IWL_DEBUG_RADIO(f, a...)  IWL_DEBUG(IWL_DL_RADIO, f, ## a)
+#define IWL_DEBUG_POWER(f, a...)  IWL_DEBUG(IWL_DL_POWER, f, ## a)
+#define IWL_DEBUG_11H(f, a...)  IWL_DEBUG(IWL_DL_11H, f, ## a)
+
+#endif
diff --git a/drivers/net/wireless/iwlwifi/iwl-eeprom.h b/drivers/net/wireless/iwlwifi/iwl-eeprom.h
new file mode 100644
index 000000000000..e473c97e3f4f
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-eeprom.h
@@ -0,0 +1,336 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual BSD/GPLv2 license.  When using or
+ * redistributing this file, you may do so under either license.
+ *
+ * GPL LICENSE SUMMARY
+ *
+ * Copyright(c) 2005 - 2007 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU Geeral Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110,
+ * USA
+ *
+ * The full GNU General Public License is included in this distribution
+ * in the file called LICENSE.GPL.
+ *
+ * Contact Information:
+ * James P. Ketrenos <ipw2100-admin@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ * BSD LICENSE
+ *
+ * Copyright(c) 2005 - 2007 Intel Corporation. All rights reserved.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *  * Neither the name Intel Corporation nor the names of its
+ *    contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ *****************************************************************************/
+
+#ifndef __iwl_eeprom_h__
+#define __iwl_eeprom_h__
+
+/*
+ * This file defines EEPROM related constants, enums, and inline functions.
+ *
+ */
+
+#define IWL_EEPROM_ACCESS_TIMEOUT	5000 /* uSec */
+#define IWL_EEPROM_ACCESS_DELAY		10   /* uSec */
+/* EEPROM field values */
+#define ANTENNA_SWITCH_NORMAL     0
+#define ANTENNA_SWITCH_INVERSE    1
+
+enum {
+	EEPROM_CHANNEL_VALID = (1 << 0),	/* usable for this SKU/geo */
+	EEPROM_CHANNEL_IBSS = (1 << 1),	/* usable as an IBSS channel */
+	/* Bit 2 Reserved */
+	EEPROM_CHANNEL_ACTIVE = (1 << 3),	/* active scanning allowed */
+	EEPROM_CHANNEL_RADAR = (1 << 4),	/* radar detection required */
+	EEPROM_CHANNEL_WIDE = (1 << 5),
+	EEPROM_CHANNEL_NARROW = (1 << 6),
+	EEPROM_CHANNEL_DFS = (1 << 7),	/* dynamic freq selection candidate */
+};
+
+/* EEPROM field lengths */
+#define EEPROM_BOARD_PBA_NUMBER_LENGTH                  11
+
+/* EEPROM field lengths */
+#define EEPROM_BOARD_PBA_NUMBER_LENGTH                  11
+#define EEPROM_REGULATORY_SKU_ID_LENGTH                 4
+#define EEPROM_REGULATORY_BAND1_CHANNELS_LENGTH         14
+#define EEPROM_REGULATORY_BAND2_CHANNELS_LENGTH         13
+#define EEPROM_REGULATORY_BAND3_CHANNELS_LENGTH         12
+#define EEPROM_REGULATORY_BAND4_CHANNELS_LENGTH         11
+#define EEPROM_REGULATORY_BAND5_CHANNELS_LENGTH         6
+
+#if IWL == 3945
+#define EEPROM_REGULATORY_CHANNELS_LENGTH ( \
+	EEPROM_REGULATORY_BAND1_CHANNELS_LENGTH + \
+	EEPROM_REGULATORY_BAND2_CHANNELS_LENGTH + \
+	EEPROM_REGULATORY_BAND3_CHANNELS_LENGTH + \
+	EEPROM_REGULATORY_BAND4_CHANNELS_LENGTH + \
+	EEPROM_REGULATORY_BAND5_CHANNELS_LENGTH)
+#elif IWL == 4965
+#define EEPROM_REGULATORY_BAND_24_FAT_CHANNELS_LENGTH 7
+#define EEPROM_REGULATORY_BAND_52_FAT_CHANNELS_LENGTH 11
+#define EEPROM_REGULATORY_CHANNELS_LENGTH ( \
+	EEPROM_REGULATORY_BAND1_CHANNELS_LENGTH + \
+	EEPROM_REGULATORY_BAND2_CHANNELS_LENGTH + \
+	EEPROM_REGULATORY_BAND3_CHANNELS_LENGTH + \
+	EEPROM_REGULATORY_BAND4_CHANNELS_LENGTH + \
+	EEPROM_REGULATORY_BAND5_CHANNELS_LENGTH + \
+	EEPROM_REGULATORY_BAND_24_FAT_CHANNELS_LENGTH + \
+	EEPROM_REGULATORY_BAND_52_FAT_CHANNELS_LENGTH)
+#endif
+
+#define EEPROM_REGULATORY_NUMBER_OF_BANDS               5
+
+/* SKU Capabilities */
+#define EEPROM_SKU_CAP_SW_RF_KILL_ENABLE                (1 << 0)
+#define EEPROM_SKU_CAP_HW_RF_KILL_ENABLE                (1 << 1)
+#define EEPROM_SKU_CAP_OP_MODE_MRC                      (1 << 7)
+
+/* *regulatory* channel data from eeprom, one for each channel */
+struct iwl_eeprom_channel {
+	u8 flags;		/* flags copied from EEPROM */
+	s8 max_power_avg;	/* max power (dBm) on this chnl, limit 31 */
+} __attribute__ ((packed));
+
+/*
+ * Mapping of a Tx power level, at factory calibration temperature,
+ *   to a radio/DSP gain table index.
+ * One for each of 5 "sample" power levels in each band.
+ * v_det is measured at the factory, using the 3945's built-in power amplifier
+ *   (PA) output voltage detector.  This same detector is used during Tx of
+ *   long packets in normal operation to provide feedback as to proper output
+ *   level.
+ * Data copied from EEPROM.
+ */
+struct iwl_eeprom_txpower_sample {
+	u8 gain_index;		/* index into power (gain) setup table ... */
+	s8 power;		/* ... for this pwr level for this chnl group */
+	u16 v_det;		/* PA output voltage */
+} __attribute__ ((packed));
+
+/*
+ * Mappings of Tx power levels -> nominal radio/DSP gain table indexes.
+ * One for each channel group (a.k.a. "band") (1 for BG, 4 for A).
+ * Tx power setup code interpolates between the 5 "sample" power levels
+ *    to determine the nominal setup for a requested power level.
+ * Data copied from EEPROM.
+ * DO NOT ALTER THIS STRUCTURE!!!
+ */
+struct iwl_eeprom_txpower_group {
+	struct iwl_eeprom_txpower_sample samples[5];	/* 5 power levels */
+	s32 a, b, c, d, e;	/* coefficients for voltage->power
+				 * formula (signed) */
+	s32 Fa, Fb, Fc, Fd, Fe;	/* these modify coeffs based on
+					 * frequency (signed) */
+	s8 saturation_power;	/* highest power possible by h/w in this
+				 * band */
+	u8 group_channel;	/* "representative" channel # in this band */
+	s16 temperature;	/* h/w temperature at factory calib this band
+				 * (signed) */
+} __attribute__ ((packed));
+
+/*
+ * Temperature-based Tx-power compensation data, not band-specific.
+ * These coefficients are use to modify a/b/c/d/e coeffs based on
+ *   difference between current temperature and factory calib temperature.
+ * Data copied from EEPROM.
+ */
+struct iwl_eeprom_temperature_corr {
+	u32 Ta;
+	u32 Tb;
+	u32 Tc;
+	u32 Td;
+	u32 Te;
+} __attribute__ ((packed));
+
+#if IWL == 4965
+#define EEPROM_TX_POWER_TX_CHAINS      (2)
+#define EEPROM_TX_POWER_BANDS          (8)
+#define EEPROM_TX_POWER_MEASUREMENTS   (3)
+#define EEPROM_TX_POWER_VERSION        (2)
+#define EEPROM_TX_POWER_VERSION_NEW    (5)
+
+struct iwl_eeprom_calib_measure {
+	u8 temperature;
+	u8 gain_idx;
+	u8 actual_pow;
+	s8 pa_det;
+} __attribute__ ((packed));
+
+struct iwl_eeprom_calib_ch_info {
+	u8 ch_num;
+	struct iwl_eeprom_calib_measure measurements[EEPROM_TX_POWER_TX_CHAINS]
+		[EEPROM_TX_POWER_MEASUREMENTS];
+} __attribute__ ((packed));
+
+struct iwl_eeprom_calib_subband_info {
+	u8 ch_from;
+	u8 ch_to;
+	struct iwl_eeprom_calib_ch_info ch1;
+	struct iwl_eeprom_calib_ch_info ch2;
+} __attribute__ ((packed));
+
+struct iwl_eeprom_calib_info {
+	u8 saturation_power24;
+	u8 saturation_power52;
+	s16 voltage;		/* signed */
+	struct iwl_eeprom_calib_subband_info band_info[EEPROM_TX_POWER_BANDS];
+} __attribute__ ((packed));
+
+#endif
+
+struct iwl_eeprom {
+	u8 reserved0[16];
+#define EEPROM_DEVICE_ID                    (2*0x08)	/* 2 bytes */
+	u16 device_id;	/* abs.ofs: 16 */
+	u8 reserved1[2];
+#define EEPROM_PMC                          (2*0x0A)	/* 2 bytes */
+	u16 pmc;		/* abs.ofs: 20 */
+	u8 reserved2[20];
+#define EEPROM_MAC_ADDRESS                  (2*0x15)	/* 6  bytes */
+	u8 mac_address[6];	/* abs.ofs: 42 */
+	u8 reserved3[58];
+#define EEPROM_BOARD_REVISION               (2*0x35)	/* 2  bytes */
+	u16 board_revision;	/* abs.ofs: 106 */
+	u8 reserved4[11];
+#define EEPROM_BOARD_PBA_NUMBER             (2*0x3B+1)	/* 9  bytes */
+	u8 board_pba_number[9];	/* abs.ofs: 119 */
+	u8 reserved5[8];
+#define EEPROM_VERSION                      (2*0x44)	/* 2  bytes */
+	u16 version;		/* abs.ofs: 136 */
+#define EEPROM_SKU_CAP                      (2*0x45)	/* 1  bytes */
+	u8 sku_cap;		/* abs.ofs: 138 */
+#define EEPROM_LEDS_MODE                    (2*0x45+1)	/* 1  bytes */
+	u8 leds_mode;		/* abs.ofs: 139 */
+#define EEPROM_OEM_MODE                     (2*0x46)	/* 2  bytes */
+	u16 oem_mode;
+#define EEPROM_WOWLAN_MODE                  (2*0x47)	/* 2  bytes */
+	u16 wowlan_mode;	/* abs.ofs: 142 */
+#define EEPROM_LEDS_TIME_INTERVAL           (2*0x48)	/* 2  bytes */
+	u16 leds_time_interval;	/* abs.ofs: 144 */
+#define EEPROM_LEDS_OFF_TIME                (2*0x49)	/* 1  bytes */
+	u8 leds_off_time;	/* abs.ofs: 146 */
+#define EEPROM_LEDS_ON_TIME                 (2*0x49+1)	/* 1  bytes */
+	u8 leds_on_time;	/* abs.ofs: 147 */
+#define EEPROM_ALMGOR_M_VERSION             (2*0x4A)	/* 1  bytes */
+	u8 almgor_m_version;	/* abs.ofs: 148 */
+#define EEPROM_ANTENNA_SWITCH_TYPE          (2*0x4A+1)	/* 1  bytes */
+	u8 antenna_switch_type;	/* abs.ofs: 149 */
+#if IWL == 3945
+	u8 reserved6[42];
+#else
+	u8 reserved6[8];
+#define EEPROM_4965_BOARD_REVISION          (2*0x4F)	/* 2 bytes */
+	u16 board_revision_4965;	/* abs.ofs: 158 */
+	u8 reserved7[13];
+#define EEPROM_4965_BOARD_PBA               (2*0x56+1)	/* 9 bytes */
+	u8 board_pba_number_4965[9];	/* abs.ofs: 173 */
+	u8 reserved8[10];
+#endif
+#define EEPROM_REGULATORY_SKU_ID            (2*0x60)	/* 4  bytes */
+	u8 sku_id[4];		/* abs.ofs: 192 */
+#define EEPROM_REGULATORY_BAND_1            (2*0x62)	/* 2  bytes */
+	u16 band_1_count;	/* abs.ofs: 196 */
+#define EEPROM_REGULATORY_BAND_1_CHANNELS   (2*0x63)	/* 28 bytes */
+	struct iwl_eeprom_channel band_1_channels[14];	/* abs.ofs: 196 */
+#define EEPROM_REGULATORY_BAND_2            (2*0x71)	/* 2  bytes */
+	u16 band_2_count;	/* abs.ofs: 226 */
+#define EEPROM_REGULATORY_BAND_2_CHANNELS   (2*0x72)	/* 26 bytes */
+	struct iwl_eeprom_channel band_2_channels[13];	/* abs.ofs: 228 */
+#define EEPROM_REGULATORY_BAND_3            (2*0x7F)	/* 2  bytes */
+	u16 band_3_count;	/* abs.ofs: 254 */
+#define EEPROM_REGULATORY_BAND_3_CHANNELS   (2*0x80)	/* 24 bytes */
+	struct iwl_eeprom_channel band_3_channels[12];	/* abs.ofs: 256 */
+#define EEPROM_REGULATORY_BAND_4            (2*0x8C)	/* 2  bytes */
+	u16 band_4_count;	/* abs.ofs: 280 */
+#define EEPROM_REGULATORY_BAND_4_CHANNELS   (2*0x8D)	/* 22 bytes */
+	struct iwl_eeprom_channel band_4_channels[11];	/* abs.ofs: 282 */
+#define EEPROM_REGULATORY_BAND_5            (2*0x98)	/* 2  bytes */
+	u16 band_5_count;	/* abs.ofs: 304 */
+#define EEPROM_REGULATORY_BAND_5_CHANNELS   (2*0x99)	/* 12 bytes */
+	struct iwl_eeprom_channel band_5_channels[6];	/* abs.ofs: 306 */
+
+/* From here on out the EEPROM diverges between the 4965 and the 3945 */
+#if IWL == 3945
+
+	u8 reserved9[194];
+
+#define EEPROM_TXPOWER_CALIB_GROUP0 0x200
+#define EEPROM_TXPOWER_CALIB_GROUP1 0x240
+#define EEPROM_TXPOWER_CALIB_GROUP2 0x280
+#define EEPROM_TXPOWER_CALIB_GROUP3 0x2c0
+#define EEPROM_TXPOWER_CALIB_GROUP4 0x300
+#define IWL_NUM_TX_CALIB_GROUPS 5
+	struct iwl_eeprom_txpower_group groups[IWL_NUM_TX_CALIB_GROUPS];
+/* abs.ofs: 512 */
+#define EEPROM_CALIB_TEMPERATURE_CORRECT 0x340
+	struct iwl_eeprom_temperature_corr corrections;	/* abs.ofs: 832 */
+	u8 reserved16[172];	/* fill out to full 1024 byte block */
+
+/* 4965AGN adds fat channel support */
+#elif IWL == 4965
+
+	u8 reserved10[2];
+#define EEPROM_REGULATORY_BAND_24_FAT_CHANNELS (2*0xA0)	/* 14 bytes */
+	struct iwl_eeprom_channel band_24_channels[7];	/* abs.ofs: 320 */
+	u8 reserved11[2];
+#define EEPROM_REGULATORY_BAND_52_FAT_CHANNELS (2*0xA8)	/* 22 bytes */
+	struct iwl_eeprom_channel band_52_channels[11];	/* abs.ofs: 336 */
+	u8 reserved12[6];
+#define EEPROM_CALIB_VERSION_OFFSET            (2*0xB6)	/* 2 bytes */
+	u16 calib_version;	/* abs.ofs: 364 */
+	u8 reserved13[2];
+#define EEPROM_SATURATION_POWER_OFFSET         (2*0xB8)	/* 2 bytes */
+	u16 satruation_power;	/* abs.ofs: 368 */
+	u8 reserved14[94];
+#define EEPROM_IWL_CALIB_TXPOWER_OFFSET        (2*0xE8)	/* 48  bytes */
+	struct iwl_eeprom_calib_info calib_info;	/* abs.ofs: 464 */
+
+	u8 reserved16[140];	/* fill out to full 1024 byte block */
+
+#endif
+
+} __attribute__ ((packed));
+
+#define IWL_EEPROM_IMAGE_SIZE 1024
+
+#endif
diff --git a/drivers/net/wireless/iwlwifi/iwl-helpers.h b/drivers/net/wireless/iwlwifi/iwl-helpers.h
new file mode 100644
index 000000000000..e2a8d95ad9cd
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-helpers.h
@@ -0,0 +1,255 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2003 - 2007 Intel Corporation. All rights reserved.
+ *
+ * Portions of this file are derived from the ipw3945 project, as well
+ * as portions of the ieee80211 subsystem header files.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * James P. Ketrenos <ipw2100-admin@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+
+#ifndef __iwl_helpers_h__
+#define __iwl_helpers_h__
+
+#include <linux/ctype.h>
+
+/*
+ * The structures defined by the hardware/uCode interface
+ * have bit-wise operations.  For each bit-field there is
+ * a data symbol in the structure, the start bit position
+ * and the length of the bit-field.
+ *
+ * iwl_get_bits and iwl_set_bits will return or set the
+ * appropriate bits on a 32-bit value.
+ *
+ * IWL_GET_BITS and IWL_SET_BITS use symbol expansion to
+ * expand out to the appropriate call to iwl_get_bits
+ * and iwl_set_bits without having to reference all of the
+ * numerical constants and defines provided in the hardware
+ * definition
+ */
+
+/**
+ * iwl_get_bits - Extract a hardware bit-field value
+ * @src: source hardware value (__le32)
+ * @pos: bit-position (0-based) of first bit of value
+ * @len: length of bit-field
+ *
+ * iwl_get_bits will return the bit-field in cpu endian ordering.
+ *
+ * NOTE:  If used from IWL_GET_BITS then pos and len are compile-constants and
+ *        will collapse to minimal code by the compiler.
+ */
+static inline u32 iwl_get_bits(__le32 src, u8 pos, u8 len)
+{
+	u32 tmp = le32_to_cpu(src);
+
+	tmp >>= pos;
+	tmp &= (1UL << len) - 1;
+	return tmp;
+}
+
+/**
+ * iwl_set_bits - Set a hardware bit-field value
+ * @dst: Address of __le32 hardware value
+ * @pos: bit-position (0-based) of first bit of value
+ * @len: length of bit-field
+ * @val: cpu endian value to encode into the bit-field
+ *
+ * iwl_set_bits will encode val into dst, masked to be len bits long at bit
+ * position pos.
+ *
+ * NOTE:  If used IWL_SET_BITS pos and len will be compile-constants and
+ *        will collapse to minimal code by the compiler.
+ */
+static inline void iwl_set_bits(__le32 *dst, u8 pos, u8 len, int val)
+{
+	u32 tmp = le32_to_cpu(*dst);
+
+	tmp &= ~(((1UL << len) - 1) << pos);
+	tmp |= (val & ((1UL << len) - 1)) << pos;
+	*dst = cpu_to_le32(tmp);
+}
+
+static inline void iwl_set_bits16(__le16 *dst, u8 pos, u8 len, int val)
+{
+	u16 tmp = le16_to_cpu(*dst);
+
+	tmp &= ~((1UL << (pos + len)) - (1UL << pos));
+	tmp |= (val & ((1UL << len) - 1)) << pos;
+	*dst = cpu_to_le16(tmp);
+}
+
+/*
+ * The bit-field definitions in iwl-xxxx-hw.h are in the form of:
+ *
+ * struct example {
+ *         __le32 val1;
+ * #define IWL_name_POS 8
+ * #define IWL_name_LEN 4
+ * #define IWL_name_SYM val1
+ * };
+ *
+ * The IWL_SET_BITS and IWL_GET_BITS macros are provided to allow the driver
+ * to call:
+ *
+ * struct example bar;
+ * u32 val = IWL_GET_BITS(bar, name);
+ * val = val * 2;
+ * IWL_SET_BITS(bar, name, val);
+ *
+ * All cpu / host ordering, masking, and shifts are performed by the macros
+ * and iwl_{get,set}_bits.
+ *
+ */
+#define IWL_SET_BITS(s, sym, v) \
+	iwl_set_bits(&(s).IWL_ ## sym ## _SYM, IWL_ ## sym ## _POS, \
+		     IWL_ ## sym ## _LEN, (v))
+
+#define IWL_SET_BITS16(s, sym, v) \
+	iwl_set_bits16(&(s).IWL_ ## sym ## _SYM, IWL_ ## sym ## _POS, \
+		       IWL_ ## sym ## _LEN, (v))
+
+#define IWL_GET_BITS(s, sym) \
+	iwl_get_bits((s).IWL_ ## sym ## _SYM, IWL_ ## sym ## _POS, \
+		      IWL_ ## sym ## _LEN)
+
+
+#define KELVIN_TO_CELSIUS(x) ((x)-273)
+#define CELSIUS_TO_KELVIN(x) ((x)+273)
+
+#define IEEE80211_CHAN_W_RADAR_DETECT 0x00000010
+
+static inline struct ieee80211_conf *ieee80211_get_hw_conf(
+	struct ieee80211_hw *hw)
+{
+	return &hw->conf;
+}
+
+#define QOS_CONTROL_LEN 2
+
+#define IEEE80211_STYPE_BACK_REQ	0x0080
+#define IEEE80211_STYPE_BACK		0x0090
+
+
+static inline int ieee80211_is_management(u16 fc)
+{
+	return (fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT;
+}
+
+static inline int ieee80211_is_control(u16 fc)
+{
+	return (fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_CTL;
+}
+
+static inline int ieee80211_is_data(u16 fc)
+{
+	return (fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_DATA;
+}
+
+static inline int ieee80211_is_back_request(u16 fc)
+{
+	return ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_CTL) &&
+	       ((fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_BACK_REQ);
+}
+
+static inline int ieee80211_is_probe_response(u16 fc)
+{
+	return ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT) &&
+	       ((fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_PROBE_RESP);
+}
+
+static inline int ieee80211_is_probe_request(u16 fc)
+{
+	return ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT) &&
+	       ((fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_PROBE_REQ);
+}
+
+static inline int ieee80211_is_beacon(u16 fc)
+{
+	return ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT) &&
+	       ((fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_BEACON);
+}
+
+static inline int ieee80211_is_atim(u16 fc)
+{
+	return ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT) &&
+	       ((fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_ATIM);
+}
+
+static inline int ieee80211_is_assoc_request(u16 fc)
+{
+	return ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT) &&
+	       ((fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_ASSOC_REQ);
+}
+
+static inline int ieee80211_is_assoc_response(u16 fc)
+{
+	return ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT) &&
+	       ((fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_ASSOC_RESP);
+}
+
+static inline int ieee80211_is_auth(u16 fc)
+{
+	return ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT) &&
+	       ((fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_ASSOC_REQ);
+}
+
+static inline int ieee80211_is_deauth(u16 fc)
+{
+	return ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT) &&
+	       ((fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_ASSOC_REQ);
+}
+
+static inline int ieee80211_is_disassoc(u16 fc)
+{
+	return ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT) &&
+	       ((fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_ASSOC_REQ);
+}
+
+static inline int ieee80211_is_reassoc_request(u16 fc)
+{
+	return ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT) &&
+	       ((fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_REASSOC_REQ);
+}
+
+static inline int ieee80211_is_reassoc_response(u16 fc)
+{
+	return ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT) &&
+	       ((fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_REASSOC_RESP);
+}
+
+static inline int iwl_check_bits(unsigned long field, unsigned long mask)
+{
+	return ((field & mask) == mask) ? 1 : 0;
+}
+
+static inline unsigned long elapsed_jiffies(unsigned long start,
+					    unsigned long end)
+{
+	if (end > start)
+		return end - start;
+
+	return end + (MAX_JIFFY_OFFSET - start);
+}
+
+#endif				/* __iwl_helpers_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-hw.h b/drivers/net/wireless/iwlwifi/iwl-hw.h
new file mode 100644
index 000000000000..1aa6fcd39a5e
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-hw.h
@@ -0,0 +1,537 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual BSD/GPLv2 license.  When using or
+ * redistributing this file, you may do so under either license.
+ *
+ * GPL LICENSE SUMMARY
+ *
+ * Copyright(c) 2005 - 2007 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU Geeral Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110,
+ * USA
+ *
+ * The full GNU General Public License is included in this distribution
+ * in the file called LICENSE.GPL.
+ *
+ * Contact Information:
+ * James P. Ketrenos <ipw2100-admin@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ * BSD LICENSE
+ *
+ * Copyright(c) 2005 - 2007 Intel Corporation. All rights reserved.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *  * Neither the name Intel Corporation nor the names of its
+ *    contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *****************************************************************************/
+
+#ifndef	__iwlwifi_hw_h__
+#define __iwlwifi_hw_h__
+
+/*
+ * This file defines hardware constants common to 3945 and 4965.
+ *
+ * Device-specific constants are defined in iwl-3945-hw.h and iwl-4965-hw.h,
+ * although this file contains a few definitions for which the .c
+ * implementation is the same for 3945 and 4965, except for the value of
+ * a constant.
+ *
+ * uCode API constants are defined in iwl-commands.h.
+ *
+ * NOTE:  DO NOT PUT OS IMPLEMENTATION-SPECIFIC DECLARATIONS HERE
+ *
+ * The iwl-*hw.h (and files they include) files should remain OS/driver
+ * implementation independent, declaring only the hardware interface.
+ */
+
+/* uCode queue management definitions */
+#define IWL_CMD_QUEUE_NUM       4
+#define IWL_CMD_FIFO_NUM        4
+#define IWL_BACK_QUEUE_FIRST_ID 7
+
+/* Tx rates */
+#define IWL_CCK_RATES 4
+#define IWL_OFDM_RATES 8
+
+#if IWL == 3945
+#define IWL_HT_RATES 0
+#elif IWL == 4965
+#define IWL_HT_RATES 16
+#endif
+
+#define IWL_MAX_RATES  (IWL_CCK_RATES+IWL_OFDM_RATES+IWL_HT_RATES)
+
+/* Time constants */
+#define SHORT_SLOT_TIME 9
+#define LONG_SLOT_TIME 20
+
+/* RSSI to dBm */
+#if IWL == 3945
+#define IWL_RSSI_OFFSET	95
+#elif IWL == 4965
+#define IWL_RSSI_OFFSET	44
+#endif
+
+#include "iwl-eeprom.h"
+#include "iwl-commands.h"
+
+#define PCI_LINK_CTRL      0x0F0
+#define PCI_POWER_SOURCE   0x0C8
+#define PCI_REG_WUM8       0x0E8
+#define PCI_CFG_PMC_PME_FROM_D3COLD_SUPPORT         (0x80000000)
+
+/*=== CSR (control and status registers) ===*/
+#define CSR_BASE    (0x000)
+
+#define CSR_SW_VER              (CSR_BASE+0x000)
+#define CSR_HW_IF_CONFIG_REG    (CSR_BASE+0x000) /* hardware interface config */
+#define CSR_INT_COALESCING      (CSR_BASE+0x004) /* accum ints, 32-usec units */
+#define CSR_INT                 (CSR_BASE+0x008) /* host interrupt status/ack */
+#define CSR_INT_MASK            (CSR_BASE+0x00c) /* host interrupt enable */
+#define CSR_FH_INT_STATUS       (CSR_BASE+0x010) /* busmaster int status/ack*/
+#define CSR_GPIO_IN             (CSR_BASE+0x018) /* read external chip pins */
+#define CSR_RESET               (CSR_BASE+0x020) /* busmaster enable, NMI, etc*/
+#define CSR_GP_CNTRL            (CSR_BASE+0x024)
+#define CSR_HW_REV              (CSR_BASE+0x028)
+#define CSR_EEPROM_REG          (CSR_BASE+0x02c)
+#define CSR_EEPROM_GP           (CSR_BASE+0x030)
+#define CSR_GP_UCODE		(CSR_BASE+0x044)
+#define CSR_UCODE_DRV_GP1       (CSR_BASE+0x054)
+#define CSR_UCODE_DRV_GP1_SET   (CSR_BASE+0x058)
+#define CSR_UCODE_DRV_GP1_CLR   (CSR_BASE+0x05c)
+#define CSR_UCODE_DRV_GP2       (CSR_BASE+0x060)
+#define CSR_LED_REG		(CSR_BASE+0x094)
+#define CSR_DRAM_INT_TBL_CTL	(CSR_BASE+0x0A0)
+#define CSR_GIO_CHICKEN_BITS    (CSR_BASE+0x100)
+#define CSR_ANA_PLL_CFG         (CSR_BASE+0x20c)
+#define CSR_HW_REV_WA_REG	(CSR_BASE+0x22C)
+
+/* HW I/F configuration */
+#define CSR_HW_IF_CONFIG_REG_BIT_ALMAGOR_MB         (0x00000100)
+#define CSR_HW_IF_CONFIG_REG_BIT_ALMAGOR_MM         (0x00000200)
+#define CSR_HW_IF_CONFIG_REG_BIT_SKU_MRC            (0x00000400)
+#define CSR_HW_IF_CONFIG_REG_BIT_BOARD_TYPE         (0x00000800)
+#define CSR_HW_IF_CONFIG_REG_BITS_SILICON_TYPE_A    (0x00000000)
+#define CSR_HW_IF_CONFIG_REG_BITS_SILICON_TYPE_B    (0x00001000)
+#define CSR_HW_IF_CONFIG_REG_BIT_EEPROM_OWN_SEM     (0x00200000)
+
+/* interrupt flags in INTA, set by uCode or hardware (e.g. dma),
+ * acknowledged (reset) by host writing "1" to flagged bits. */
+#define CSR_INT_BIT_FH_RX        (1<<31) /* Rx DMA, cmd responses, FH_INT[17:16] */
+#define CSR_INT_BIT_HW_ERR       (1<<29) /* DMA hardware error FH_INT[31] */
+#define CSR_INT_BIT_DNLD         (1<<28) /* uCode Download */
+#define CSR_INT_BIT_FH_TX        (1<<27) /* Tx DMA FH_INT[1:0] */
+#define CSR_INT_BIT_MAC_CLK_ACTV (1<<26) /* NIC controller's clock toggled on/off */
+#define CSR_INT_BIT_SW_ERR       (1<<25) /* uCode error */
+#define CSR_INT_BIT_RF_KILL      (1<<7)  /* HW RFKILL switch GP_CNTRL[27] toggled */
+#define CSR_INT_BIT_CT_KILL      (1<<6)  /* Critical temp (chip too hot) rfkill */
+#define CSR_INT_BIT_SW_RX        (1<<3)  /* Rx, command responses, 3945 */
+#define CSR_INT_BIT_WAKEUP       (1<<1)  /* NIC controller waking up (pwr mgmt) */
+#define CSR_INT_BIT_ALIVE        (1<<0)  /* uCode interrupts once it initializes */
+
+#define CSR_INI_SET_MASK	(CSR_INT_BIT_FH_RX   | \
+				 CSR_INT_BIT_HW_ERR  | \
+				 CSR_INT_BIT_FH_TX   | \
+				 CSR_INT_BIT_SW_ERR  | \
+				 CSR_INT_BIT_RF_KILL | \
+				 CSR_INT_BIT_SW_RX   | \
+				 CSR_INT_BIT_WAKEUP  | \
+				 CSR_INT_BIT_ALIVE)
+
+/* interrupt flags in FH (flow handler) (PCI busmaster DMA) */
+#define CSR_FH_INT_BIT_ERR       (1<<31) /* Error */
+#define CSR_FH_INT_BIT_HI_PRIOR  (1<<30) /* High priority Rx, bypass coalescing */
+#define CSR_FH_INT_BIT_RX_CHNL2  (1<<18) /* Rx channel 2 (3945 only) */
+#define CSR_FH_INT_BIT_RX_CHNL1  (1<<17) /* Rx channel 1 */
+#define CSR_FH_INT_BIT_RX_CHNL0  (1<<16) /* Rx channel 0 */
+#define CSR_FH_INT_BIT_TX_CHNL6  (1<<6)  /* Tx channel 6 (3945 only) */
+#define CSR_FH_INT_BIT_TX_CHNL1  (1<<1)  /* Tx channel 1 */
+#define CSR_FH_INT_BIT_TX_CHNL0  (1<<0)  /* Tx channel 0 */
+
+#define CSR_FH_INT_RX_MASK	(CSR_FH_INT_BIT_HI_PRIOR | \
+				 CSR_FH_INT_BIT_RX_CHNL2 | \
+				 CSR_FH_INT_BIT_RX_CHNL1 | \
+				 CSR_FH_INT_BIT_RX_CHNL0)
+
+#define CSR_FH_INT_TX_MASK	(CSR_FH_INT_BIT_TX_CHNL6 | \
+				 CSR_FH_INT_BIT_TX_CHNL1 | \
+				 CSR_FH_INT_BIT_TX_CHNL0 )
+
+
+/* RESET */
+#define CSR_RESET_REG_FLAG_NEVO_RESET                (0x00000001)
+#define CSR_RESET_REG_FLAG_FORCE_NMI                 (0x00000002)
+#define CSR_RESET_REG_FLAG_SW_RESET                  (0x00000080)
+#define CSR_RESET_REG_FLAG_MASTER_DISABLED           (0x00000100)
+#define CSR_RESET_REG_FLAG_STOP_MASTER               (0x00000200)
+
+/* GP (general purpose) CONTROL */
+#define CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY        (0x00000001)
+#define CSR_GP_CNTRL_REG_FLAG_INIT_DONE              (0x00000004)
+#define CSR_GP_CNTRL_REG_FLAG_MAC_ACCESS_REQ         (0x00000008)
+#define CSR_GP_CNTRL_REG_FLAG_GOING_TO_SLEEP         (0x00000010)
+
+#define CSR_GP_CNTRL_REG_VAL_MAC_ACCESS_EN           (0x00000001)
+
+#define CSR_GP_CNTRL_REG_MSK_POWER_SAVE_TYPE         (0x07000000)
+#define CSR_GP_CNTRL_REG_FLAG_MAC_POWER_SAVE         (0x04000000)
+#define CSR_GP_CNTRL_REG_FLAG_HW_RF_KILL_SW          (0x08000000)
+
+
+/* EEPROM REG */
+#define CSR_EEPROM_REG_READ_VALID_MSK	(0x00000001)
+#define CSR_EEPROM_REG_BIT_CMD		(0x00000002)
+
+/* EEPROM GP */
+#define CSR_EEPROM_GP_VALID_MSK		(0x00000006)
+#define CSR_EEPROM_GP_BAD_SIGNATURE	(0x00000000)
+#define CSR_EEPROM_GP_IF_OWNER_MSK	(0x00000180)
+
+/* UCODE DRV GP */
+#define CSR_UCODE_DRV_GP1_BIT_MAC_SLEEP             (0x00000001)
+#define CSR_UCODE_SW_BIT_RFKILL                     (0x00000002)
+#define CSR_UCODE_DRV_GP1_BIT_CMD_BLOCKED           (0x00000004)
+#define CSR_UCODE_DRV_GP1_REG_BIT_CT_KILL_EXIT      (0x00000008)
+
+/* GPIO */
+#define CSR_GPIO_IN_BIT_AUX_POWER                   (0x00000200)
+#define CSR_GPIO_IN_VAL_VAUX_PWR_SRC                (0x00000000)
+#define CSR_GPIO_IN_VAL_VMAIN_PWR_SRC		CSR_GPIO_IN_BIT_AUX_POWER
+
+/* GI Chicken Bits */
+#define CSR_GIO_CHICKEN_BITS_REG_BIT_L1A_NO_L0S_RX  (0x00800000)
+#define CSR_GIO_CHICKEN_BITS_REG_BIT_DIS_L0S_EXIT_TIMER  (0x20000000)
+
+/* CSR_ANA_PLL_CFG */
+#define CSR_ANA_PLL_CFG_SH		(0x00880300)
+
+#define CSR_LED_REG_TRUN_ON		(0x00000078)
+#define CSR_LED_REG_TRUN_OFF		(0x00000038)
+#define CSR_LED_BSM_CTRL_MSK		(0xFFFFFFDF)
+
+/* DRAM_INT_TBL_CTRL */
+#define CSR_DRAM_INT_TBL_CTRL_EN	(1<<31)
+#define CSR_DRAM_INT_TBL_CTRL_WRAP_CHK	(1<<27)
+
+/*=== HBUS (Host-side Bus) ===*/
+#define HBUS_BASE	(0x400)
+
+#define HBUS_TARG_MEM_RADDR     (HBUS_BASE+0x00c)
+#define HBUS_TARG_MEM_WADDR     (HBUS_BASE+0x010)
+#define HBUS_TARG_MEM_WDAT      (HBUS_BASE+0x018)
+#define HBUS_TARG_MEM_RDAT      (HBUS_BASE+0x01c)
+#define HBUS_TARG_PRPH_WADDR    (HBUS_BASE+0x044)
+#define HBUS_TARG_PRPH_RADDR    (HBUS_BASE+0x048)
+#define HBUS_TARG_PRPH_WDAT     (HBUS_BASE+0x04c)
+#define HBUS_TARG_PRPH_RDAT     (HBUS_BASE+0x050)
+#define HBUS_TARG_WRPTR         (HBUS_BASE+0x060)
+
+#define HBUS_TARG_MBX_C         (HBUS_BASE+0x030)
+
+
+/* SCD (Scheduler) */
+#define SCD_BASE                        (CSR_BASE + 0x2E00)
+
+#define SCD_MODE_REG                    (SCD_BASE + 0x000)
+#define SCD_ARASTAT_REG                 (SCD_BASE + 0x004)
+#define SCD_TXFACT_REG                  (SCD_BASE + 0x010)
+#define SCD_TXF4MF_REG                  (SCD_BASE + 0x014)
+#define SCD_TXF5MF_REG                  (SCD_BASE + 0x020)
+#define SCD_SBYP_MODE_1_REG             (SCD_BASE + 0x02C)
+#define SCD_SBYP_MODE_2_REG             (SCD_BASE + 0x030)
+
+/*=== FH (data Flow Handler) ===*/
+#define FH_BASE     (0x800)
+
+#define FH_CBCC_TABLE           (FH_BASE+0x140)
+#define FH_TFDB_TABLE           (FH_BASE+0x180)
+#define FH_RCSR_TABLE           (FH_BASE+0x400)
+#define FH_RSSR_TABLE           (FH_BASE+0x4c0)
+#define FH_TCSR_TABLE           (FH_BASE+0x500)
+#define FH_TSSR_TABLE           (FH_BASE+0x680)
+
+/* TFDB (Transmit Frame Buffer Descriptor) */
+#define FH_TFDB(_channel, buf) \
+	(FH_TFDB_TABLE+((_channel)*2+(buf))*0x28)
+#define ALM_FH_TFDB_CHNL_BUF_CTRL_REG(_channel) \
+	(FH_TFDB_TABLE + 0x50 * _channel)
+/* CBCC _channel is [0,2] */
+#define FH_CBCC(_channel)           (FH_CBCC_TABLE+(_channel)*0x8)
+#define FH_CBCC_CTRL(_channel)      (FH_CBCC(_channel)+0x00)
+#define FH_CBCC_BASE(_channel)      (FH_CBCC(_channel)+0x04)
+
+/* RCSR _channel is [0,2] */
+#define FH_RCSR(_channel)           (FH_RCSR_TABLE+(_channel)*0x40)
+#define FH_RCSR_CONFIG(_channel)    (FH_RCSR(_channel)+0x00)
+#define FH_RCSR_RBD_BASE(_channel)  (FH_RCSR(_channel)+0x04)
+#define FH_RCSR_WPTR(_channel)      (FH_RCSR(_channel)+0x20)
+#define FH_RCSR_RPTR_ADDR(_channel) (FH_RCSR(_channel)+0x24)
+
+#if IWL == 3945
+#define FH_RSCSR_CHNL0_WPTR        (FH_RCSR_WPTR(0))
+#elif IWL == 4965
+#define FH_RSCSR_CHNL0_WPTR        (FH_RSCSR_CHNL0_RBDCB_WPTR_REG)
+#endif
+
+/* RSSR */
+#define FH_RSSR_CTRL            (FH_RSSR_TABLE+0x000)
+#define FH_RSSR_STATUS          (FH_RSSR_TABLE+0x004)
+/* TCSR */
+#define FH_TCSR(_channel)           (FH_TCSR_TABLE+(_channel)*0x20)
+#define FH_TCSR_CONFIG(_channel)    (FH_TCSR(_channel)+0x00)
+#define FH_TCSR_CREDIT(_channel)    (FH_TCSR(_channel)+0x04)
+#define FH_TCSR_BUFF_STTS(_channel) (FH_TCSR(_channel)+0x08)
+/* TSSR */
+#define FH_TSSR_CBB_BASE        (FH_TSSR_TABLE+0x000)
+#define FH_TSSR_MSG_CONFIG      (FH_TSSR_TABLE+0x008)
+#define FH_TSSR_TX_STATUS       (FH_TSSR_TABLE+0x010)
+/* 18 - reserved */
+
+/* card static random access memory (SRAM) for processor data and instructs */
+#define RTC_INST_LOWER_BOUND			(0x000000)
+#define RTC_DATA_LOWER_BOUND			(0x800000)
+
+
+/* DBM */
+
+#define ALM_FH_SRVC_CHNL                            (6)
+
+#define ALM_FH_RCSR_RX_CONFIG_REG_POS_RBDC_SIZE     (20)
+#define ALM_FH_RCSR_RX_CONFIG_REG_POS_IRQ_RBTH      (4)
+
+#define ALM_FH_RCSR_RX_CONFIG_REG_BIT_WR_STTS_EN    (0x08000000)
+
+#define ALM_FH_RCSR_RX_CONFIG_REG_VAL_DMA_CHNL_EN_ENABLE        (0x80000000)
+
+#define ALM_FH_RCSR_RX_CONFIG_REG_VAL_RDRBD_EN_ENABLE           (0x20000000)
+
+#define ALM_FH_RCSR_RX_CONFIG_REG_VAL_MAX_FRAG_SIZE_128         (0x01000000)
+
+#define ALM_FH_RCSR_RX_CONFIG_REG_VAL_IRQ_DEST_INT_HOST         (0x00001000)
+
+#define ALM_FH_RCSR_RX_CONFIG_REG_VAL_MSG_MODE_FH               (0x00000000)
+
+#define ALM_FH_TCSR_TX_CONFIG_REG_VAL_MSG_MODE_TXF              (0x00000000)
+#define ALM_FH_TCSR_TX_CONFIG_REG_VAL_MSG_MODE_DRIVER           (0x00000001)
+
+#define ALM_FH_TCSR_TX_CONFIG_REG_VAL_DMA_CREDIT_DISABLE_VAL    (0x00000000)
+#define ALM_FH_TCSR_TX_CONFIG_REG_VAL_DMA_CREDIT_ENABLE_VAL     (0x00000008)
+
+#define ALM_FH_TCSR_TX_CONFIG_REG_VAL_CIRQ_HOST_IFTFD           (0x00200000)
+
+#define ALM_FH_TCSR_TX_CONFIG_REG_VAL_CIRQ_RTC_NOINT            (0x00000000)
+
+#define ALM_FH_TCSR_TX_CONFIG_REG_VAL_DMA_CHNL_PAUSE            (0x00000000)
+#define ALM_FH_TCSR_TX_CONFIG_REG_VAL_DMA_CHNL_ENABLE           (0x80000000)
+
+#define ALM_FH_TCSR_CHNL_TX_BUF_STS_REG_VAL_TFDB_VALID          (0x00004000)
+
+#define ALM_FH_TCSR_CHNL_TX_BUF_STS_REG_BIT_TFDB_WPTR           (0x00000001)
+
+#define ALM_FH_TSSR_TX_MSG_CONFIG_REG_VAL_SNOOP_RD_TXPD_ON      (0xFF000000)
+#define ALM_FH_TSSR_TX_MSG_CONFIG_REG_VAL_ORDER_RD_TXPD_ON      (0x00FF0000)
+
+#define ALM_FH_TSSR_TX_MSG_CONFIG_REG_VAL_MAX_FRAG_SIZE_128B    (0x00000400)
+
+#define ALM_FH_TSSR_TX_MSG_CONFIG_REG_VAL_SNOOP_RD_TFD_ON       (0x00000100)
+#define ALM_FH_TSSR_TX_MSG_CONFIG_REG_VAL_ORDER_RD_CBB_ON       (0x00000080)
+
+#define ALM_FH_TSSR_TX_MSG_CONFIG_REG_VAL_ORDER_RSP_WAIT_TH     (0x00000020)
+#define ALM_FH_TSSR_TX_MSG_CONFIG_REG_VAL_RSP_WAIT_TH           (0x00000005)
+
+#define ALM_TB_MAX_BYTES_COUNT      (0xFFF0)
+
+#define ALM_FH_TSSR_TX_STATUS_REG_BIT_BUFS_EMPTY(_channel) \
+	((1LU << _channel) << 24)
+#define ALM_FH_TSSR_TX_STATUS_REG_BIT_NO_PEND_REQ(_channel) \
+	((1LU << _channel) << 16)
+
+#define ALM_FH_TSSR_TX_STATUS_REG_MSK_CHNL_IDLE(_channel) \
+	(ALM_FH_TSSR_TX_STATUS_REG_BIT_BUFS_EMPTY(_channel) | \
+	 ALM_FH_TSSR_TX_STATUS_REG_BIT_NO_PEND_REQ(_channel))
+#define PCI_CFG_REV_ID_BIT_BASIC_SKU                (0x40)	/* bit 6    */
+#define PCI_CFG_REV_ID_BIT_RTP                      (0x80)	/* bit 7    */
+
+#define HBUS_TARG_MBX_C_REG_BIT_CMD_BLOCKED         (0x00000004)
+
+#define TFD_QUEUE_MIN           0
+#define TFD_QUEUE_MAX           6
+#define TFD_QUEUE_SIZE_MAX      (256)
+
+/* spectrum and channel data structures */
+#define IWL_NUM_SCAN_RATES         (2)
+
+#define IWL_SCAN_FLAG_24GHZ  (1<<0)
+#define IWL_SCAN_FLAG_52GHZ  (1<<1)
+#define IWL_SCAN_FLAG_ACTIVE (1<<2)
+#define IWL_SCAN_FLAG_DIRECT (1<<3)
+
+#define IWL_MAX_CMD_SIZE 1024
+
+#define IWL_DEFAULT_TX_RETRY  15
+#define IWL_MAX_TX_RETRY      16
+
+/*********************************************/
+
+#define RFD_SIZE                              4
+#define NUM_TFD_CHUNKS                        4
+
+#define RX_QUEUE_SIZE                         256
+#define RX_QUEUE_MASK                         255
+#define RX_QUEUE_SIZE_LOG                     8
+
+/* QoS  definitions */
+
+#define CW_MIN_OFDM          15
+#define CW_MAX_OFDM          1023
+#define CW_MIN_CCK           31
+#define CW_MAX_CCK           1023
+
+#define QOS_TX0_CW_MIN_OFDM      CW_MIN_OFDM
+#define QOS_TX1_CW_MIN_OFDM      CW_MIN_OFDM
+#define QOS_TX2_CW_MIN_OFDM      ((CW_MIN_OFDM + 1) / 2 - 1)
+#define QOS_TX3_CW_MIN_OFDM      ((CW_MIN_OFDM + 1) / 4 - 1)
+
+#define QOS_TX0_CW_MIN_CCK       CW_MIN_CCK
+#define QOS_TX1_CW_MIN_CCK       CW_MIN_CCK
+#define QOS_TX2_CW_MIN_CCK       ((CW_MIN_CCK + 1) / 2 - 1)
+#define QOS_TX3_CW_MIN_CCK       ((CW_MIN_CCK + 1) / 4 - 1)
+
+#define QOS_TX0_CW_MAX_OFDM      CW_MAX_OFDM
+#define QOS_TX1_CW_MAX_OFDM      CW_MAX_OFDM
+#define QOS_TX2_CW_MAX_OFDM      CW_MIN_OFDM
+#define QOS_TX3_CW_MAX_OFDM      ((CW_MIN_OFDM + 1) / 2 - 1)
+
+#define QOS_TX0_CW_MAX_CCK       CW_MAX_CCK
+#define QOS_TX1_CW_MAX_CCK       CW_MAX_CCK
+#define QOS_TX2_CW_MAX_CCK       CW_MIN_CCK
+#define QOS_TX3_CW_MAX_CCK       ((CW_MIN_CCK + 1) / 2 - 1)
+
+#define QOS_TX0_AIFS            3
+#define QOS_TX1_AIFS            7
+#define QOS_TX2_AIFS            2
+#define QOS_TX3_AIFS            2
+
+#define QOS_TX0_ACM             0
+#define QOS_TX1_ACM             0
+#define QOS_TX2_ACM             0
+#define QOS_TX3_ACM             0
+
+#define QOS_TX0_TXOP_LIMIT_CCK          0
+#define QOS_TX1_TXOP_LIMIT_CCK          0
+#define QOS_TX2_TXOP_LIMIT_CCK          6016
+#define QOS_TX3_TXOP_LIMIT_CCK          3264
+
+#define QOS_TX0_TXOP_LIMIT_OFDM      0
+#define QOS_TX1_TXOP_LIMIT_OFDM      0
+#define QOS_TX2_TXOP_LIMIT_OFDM      3008
+#define QOS_TX3_TXOP_LIMIT_OFDM      1504
+
+#define DEF_TX0_CW_MIN_OFDM      CW_MIN_OFDM
+#define DEF_TX1_CW_MIN_OFDM      CW_MIN_OFDM
+#define DEF_TX2_CW_MIN_OFDM      CW_MIN_OFDM
+#define DEF_TX3_CW_MIN_OFDM      CW_MIN_OFDM
+
+#define DEF_TX0_CW_MIN_CCK       CW_MIN_CCK
+#define DEF_TX1_CW_MIN_CCK       CW_MIN_CCK
+#define DEF_TX2_CW_MIN_CCK       CW_MIN_CCK
+#define DEF_TX3_CW_MIN_CCK       CW_MIN_CCK
+
+#define DEF_TX0_CW_MAX_OFDM      CW_MAX_OFDM
+#define DEF_TX1_CW_MAX_OFDM      CW_MAX_OFDM
+#define DEF_TX2_CW_MAX_OFDM      CW_MAX_OFDM
+#define DEF_TX3_CW_MAX_OFDM      CW_MAX_OFDM
+
+#define DEF_TX0_CW_MAX_CCK       CW_MAX_CCK
+#define DEF_TX1_CW_MAX_CCK       CW_MAX_CCK
+#define DEF_TX2_CW_MAX_CCK       CW_MAX_CCK
+#define DEF_TX3_CW_MAX_CCK       CW_MAX_CCK
+
+#define DEF_TX0_AIFS            (2)
+#define DEF_TX1_AIFS            (2)
+#define DEF_TX2_AIFS            (2)
+#define DEF_TX3_AIFS            (2)
+
+#define DEF_TX0_ACM             0
+#define DEF_TX1_ACM             0
+#define DEF_TX2_ACM             0
+#define DEF_TX3_ACM             0
+
+#define DEF_TX0_TXOP_LIMIT_CCK        0
+#define DEF_TX1_TXOP_LIMIT_CCK        0
+#define DEF_TX2_TXOP_LIMIT_CCK        0
+#define DEF_TX3_TXOP_LIMIT_CCK        0
+
+#define DEF_TX0_TXOP_LIMIT_OFDM       0
+#define DEF_TX1_TXOP_LIMIT_OFDM       0
+#define DEF_TX2_TXOP_LIMIT_OFDM       0
+#define DEF_TX3_TXOP_LIMIT_OFDM       0
+
+#define QOS_QOS_SETS                  3
+#define QOS_PARAM_SET_ACTIVE          0
+#define QOS_PARAM_SET_DEF_CCK         1
+#define QOS_PARAM_SET_DEF_OFDM        2
+
+#define CTRL_QOS_NO_ACK               (0x0020)
+#define DCT_FLAG_EXT_QOS_ENABLED      (0x10)
+
+#define U32_PAD(n)		((4-(n))&0x3)
+
+/*
+ * Generic queue structure
+ *
+ * Contains common data for Rx and Tx queues
+ */
+#define TFD_CTL_COUNT_SET(n)       (n<<24)
+#define TFD_CTL_COUNT_GET(ctl)     ((ctl>>24) & 7)
+#define TFD_CTL_PAD_SET(n)         (n<<28)
+#define TFD_CTL_PAD_GET(ctl)       (ctl>>28)
+
+#define TFD_TX_CMD_SLOTS 256
+#define TFD_CMD_SLOTS 32
+
+#define TFD_MAX_PAYLOAD_SIZE (sizeof(struct iwl_cmd) - \
+			      sizeof(struct iwl_cmd_meta))
+
+/*
+ * RX related structures and functions
+ */
+#define RX_FREE_BUFFERS 64
+#define RX_LOW_WATERMARK 8
+
+#endif				/* __iwlwifi_hw_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-io.h b/drivers/net/wireless/iwlwifi/iwl-io.h
new file mode 100644
index 000000000000..8a8b96fcf48d
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-io.h
@@ -0,0 +1,470 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2003 - 2007 Intel Corporation. All rights reserved.
+ *
+ * Portions of this file are derived from the ipw3945 project.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * James P. Ketrenos <ipw2100-admin@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+
+#ifndef __iwl_io_h__
+#define __iwl_io_h__
+
+#include <linux/io.h>
+
+#include "iwl-debug.h"
+
+/*
+ * IO, register, and NIC memory access functions
+ *
+ * NOTE on naming convention and macro usage for these
+ *
+ * A single _ prefix before a an access function means that no state
+ * check or debug information is printed when that function is called.
+ *
+ * A double __ prefix before an access function means that state is checked
+ * (in the case of *restricted calls) and the current line number is printed
+ * in addition to any other debug output.
+ *
+ * The non-prefixed name is the #define that maps the caller into a
+ * #define that provides the caller's __LINE__ to the double prefix version.
+ *
+ * If you wish to call the function without any debug or state checking,
+ * you should use the single _ prefix version (as is used by dependent IO
+ * routines, for example _iwl_read_restricted calls the non-check version of
+ * _iwl_read32.)
+ *
+ * These declarations are *extremely* useful in quickly isolating code deltas
+ * which result in misconfiguring of the hardware I/O.  In combination with
+ * git-bisect and the IO debug level you can quickly determine the specific
+ * commit which breaks the IO sequence to the hardware.
+ *
+ */
+
+#define _iwl_write32(iwl, ofs, val) writel((val), (iwl)->hw_base + (ofs))
+#ifdef CONFIG_IWLWIFI_DEBUG
+static inline void __iwl_write32(const char *f, u32 l, struct iwl_priv *iwl,
+				 u32 ofs, u32 val)
+{
+	IWL_DEBUG_IO("write_direct32(0x%08X, 0x%08X) - %s %d\n",
+		     (u32) (ofs), (u32) (val), f, l);
+	_iwl_write32(iwl, ofs, val);
+}
+#define iwl_write32(iwl, ofs, val) \
+	__iwl_write32(__FILE__, __LINE__, iwl, ofs, val)
+#else
+#define iwl_write32(iwl, ofs, val) _iwl_write32(iwl, ofs, val)
+#endif
+
+#define _iwl_read32(iwl, ofs) readl((iwl)->hw_base + (ofs))
+#ifdef CONFIG_IWLWIFI_DEBUG
+static inline u32 __iwl_read32(char *f, u32 l, struct iwl_priv *iwl, u32 ofs)
+{
+	IWL_DEBUG_IO("read_direct32(0x%08X) - %s %d\n", ofs, f, l);
+	return _iwl_read32(iwl, ofs);
+}
+#define iwl_read32(iwl, ofs) __iwl_read32(__FILE__, __LINE__, iwl, ofs)
+#else
+#define iwl_read32(p, o) _iwl_read32(p, o)
+#endif
+
+static inline int _iwl_poll_bit(struct iwl_priv *priv, u32 addr,
+				u32 bits, u32 mask, int timeout)
+{
+	int i = 0;
+
+	do {
+		if ((_iwl_read32(priv, addr) & mask) == (bits & mask))
+			return i;
+		mdelay(10);
+		i += 10;
+	} while (i < timeout);
+
+	return -ETIMEDOUT;
+}
+#ifdef CONFIG_IWLWIFI_DEBUG
+static inline int __iwl_poll_bit(const char *f, u32 l,
+				 struct iwl_priv *priv, u32 addr,
+				 u32 bits, u32 mask, int timeout)
+{
+	int rc = _iwl_poll_bit(priv, addr, bits, mask, timeout);
+	if (unlikely(rc == -ETIMEDOUT))
+		IWL_DEBUG_IO
+		    ("poll_bit(0x%08X, 0x%08X, 0x%08X) - timedout - %s %d\n",
+		     addr, bits, mask, f, l);
+	else
+		IWL_DEBUG_IO
+		    ("poll_bit(0x%08X, 0x%08X, 0x%08X) = 0x%08X - %s %d\n",
+		     addr, bits, mask, rc, f, l);
+	return rc;
+}
+#define iwl_poll_bit(iwl, addr, bits, mask, timeout) \
+	__iwl_poll_bit(__FILE__, __LINE__, iwl, addr, bits, mask, timeout)
+#else
+#define iwl_poll_bit(p, a, b, m, t) _iwl_poll_bit(p, a, b, m, t)
+#endif
+
+static inline void _iwl_set_bit(struct iwl_priv *priv, u32 reg, u32 mask)
+{
+	_iwl_write32(priv, reg, _iwl_read32(priv, reg) | mask);
+}
+#ifdef CONFIG_IWLWIFI_DEBUG
+static inline void __iwl_set_bit(const char *f, u32 l,
+				 struct iwl_priv *priv, u32 reg, u32 mask)
+{
+	u32 val = _iwl_read32(priv, reg) | mask;
+	IWL_DEBUG_IO("set_bit(0x%08X, 0x%08X) = 0x%08X\n", reg, mask, val);
+	_iwl_write32(priv, reg, val);
+}
+#define iwl_set_bit(p, r, m) __iwl_set_bit(__FILE__, __LINE__, p, r, m)
+#else
+#define iwl_set_bit(p, r, m) _iwl_set_bit(p, r, m)
+#endif
+
+static inline void _iwl_clear_bit(struct iwl_priv *priv, u32 reg, u32 mask)
+{
+	_iwl_write32(priv, reg, _iwl_read32(priv, reg) & ~mask);
+}
+#ifdef CONFIG_IWLWIFI_DEBUG
+static inline void __iwl_clear_bit(const char *f, u32 l,
+				   struct iwl_priv *priv, u32 reg, u32 mask)
+{
+	u32 val = _iwl_read32(priv, reg) & ~mask;
+	IWL_DEBUG_IO("clear_bit(0x%08X, 0x%08X) = 0x%08X\n", reg, mask, val);
+	_iwl_write32(priv, reg, val);
+}
+#define iwl_clear_bit(p, r, m) __iwl_clear_bit(__FILE__, __LINE__, p, r, m)
+#else
+#define iwl_clear_bit(p, r, m) _iwl_clear_bit(p, r, m)
+#endif
+
+static inline int _iwl_grab_restricted_access(struct iwl_priv *priv)
+{
+	int rc;
+	u32 gp_ctl;
+
+#ifdef CONFIG_IWLWIFI_DEBUG
+	if (atomic_read(&priv->restrict_refcnt))
+		return 0;
+#endif
+	if (test_bit(STATUS_RF_KILL_HW, &priv->status) ||
+	    test_bit(STATUS_RF_KILL_SW, &priv->status)) {
+		IWL_WARNING("WARNING: Requesting MAC access during RFKILL "
+			"wakes up NIC\n");
+
+		/* 10 msec allows time for NIC to complete its data save */
+		gp_ctl = _iwl_read32(priv, CSR_GP_CNTRL);
+		if (gp_ctl & CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY) {
+			IWL_DEBUG_RF_KILL("Wait for complete power-down, "
+				"gpctl = 0x%08x\n", gp_ctl);
+			mdelay(10);
+		} else
+			IWL_DEBUG_RF_KILL("power-down complete, "
+					  "gpctl = 0x%08x\n", gp_ctl);
+	}
+
+	/* this bit wakes up the NIC */
+	_iwl_set_bit(priv, CSR_GP_CNTRL, CSR_GP_CNTRL_REG_FLAG_MAC_ACCESS_REQ);
+	rc = _iwl_poll_bit(priv, CSR_GP_CNTRL,
+			   CSR_GP_CNTRL_REG_VAL_MAC_ACCESS_EN,
+			   (CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY |
+			    CSR_GP_CNTRL_REG_FLAG_GOING_TO_SLEEP), 50);
+	if (rc < 0) {
+		IWL_ERROR("MAC is in deep sleep!\n");
+		return -EIO;
+	}
+
+#ifdef CONFIG_IWLWIFI_DEBUG
+	atomic_inc(&priv->restrict_refcnt);
+#endif
+	return 0;
+}
+
+#ifdef CONFIG_IWLWIFI_DEBUG
+static inline int __iwl_grab_restricted_access(const char *f, u32 l,
+					       struct iwl_priv *priv)
+{
+	if (atomic_read(&priv->restrict_refcnt))
+		IWL_DEBUG_INFO("Grabbing access while already held at "
+			       "line %d.\n", l);
+
+	IWL_DEBUG_IO("grabbing restricted access - %s %d\n", f, l);
+
+	return _iwl_grab_restricted_access(priv);
+}
+#define iwl_grab_restricted_access(priv) \
+	__iwl_grab_restricted_access(__FILE__, __LINE__, priv)
+#else
+#define iwl_grab_restricted_access(priv) \
+	_iwl_grab_restricted_access(priv)
+#endif
+
+static inline void _iwl_release_restricted_access(struct iwl_priv *priv)
+{
+#ifdef CONFIG_IWLWIFI_DEBUG
+	if (atomic_dec_and_test(&priv->restrict_refcnt))
+#endif
+		_iwl_clear_bit(priv, CSR_GP_CNTRL,
+			       CSR_GP_CNTRL_REG_FLAG_MAC_ACCESS_REQ);
+}
+#ifdef CONFIG_IWLWIFI_DEBUG
+static inline void __iwl_release_restricted_access(const char *f, u32 l,
+						   struct iwl_priv *priv)
+{
+	if (atomic_read(&priv->restrict_refcnt) <= 0)
+		IWL_ERROR("Release unheld restricted access at line %d.\n", l);
+
+	IWL_DEBUG_IO("releasing restricted access - %s %d\n", f, l);
+	_iwl_release_restricted_access(priv);
+}
+#define iwl_release_restricted_access(priv) \
+	__iwl_release_restricted_access(__FILE__, __LINE__, priv)
+#else
+#define iwl_release_restricted_access(priv) \
+	_iwl_release_restricted_access(priv)
+#endif
+
+static inline u32 _iwl_read_restricted(struct iwl_priv *priv, u32 reg)
+{
+	return _iwl_read32(priv, reg);
+}
+#ifdef CONFIG_IWLWIFI_DEBUG
+static inline u32 __iwl_read_restricted(const char *f, u32 l,
+					struct iwl_priv *priv, u32 reg)
+{
+	u32 value = _iwl_read_restricted(priv, reg);
+	if (!atomic_read(&priv->restrict_refcnt))
+		IWL_ERROR("Unrestricted access from %s %d\n", f, l);
+	IWL_DEBUG_IO("read_restricted(0x%4X) = 0x%08x - %s %d \n", reg, value,
+		     f, l);
+	return value;
+}
+#define iwl_read_restricted(priv, reg) \
+	__iwl_read_restricted(__FILE__, __LINE__, priv, reg)
+#else
+#define iwl_read_restricted _iwl_read_restricted
+#endif
+
+static inline void _iwl_write_restricted(struct iwl_priv *priv,
+					 u32 reg, u32 value)
+{
+	_iwl_write32(priv, reg, value);
+}
+#ifdef CONFIG_IWLWIFI_DEBUG
+static void __iwl_write_restricted(u32 line,
+				   struct iwl_priv *priv, u32 reg, u32 value)
+{
+	if (!atomic_read(&priv->restrict_refcnt))
+		IWL_ERROR("Unrestricted access from line %d\n", line);
+	_iwl_write_restricted(priv, reg, value);
+}
+#define iwl_write_restricted(priv, reg, value) \
+	__iwl_write_restricted(__LINE__, priv, reg, value)
+#else
+#define iwl_write_restricted _iwl_write_restricted
+#endif
+
+static inline void iwl_write_buffer_restricted(struct iwl_priv *priv,
+					       u32 reg, u32 len, u32 *values)
+{
+	u32 count = sizeof(u32);
+
+	if ((priv != NULL) && (values != NULL)) {
+		for (; 0 < len; len -= count, reg += count, values++)
+			_iwl_write_restricted(priv, reg, *values);
+	}
+}
+
+static inline int _iwl_poll_restricted_bit(struct iwl_priv *priv,
+					   u32 addr, u32 mask, int timeout)
+{
+	int i = 0;
+
+	do {
+		if ((_iwl_read_restricted(priv, addr) & mask) == mask)
+			return i;
+		mdelay(10);
+		i += 10;
+	} while (i < timeout);
+
+	return -ETIMEDOUT;
+}
+
+#ifdef CONFIG_IWLWIFI_DEBUG
+static inline int __iwl_poll_restricted_bit(const char *f, u32 l,
+					    struct iwl_priv *priv,
+					    u32 addr, u32 mask, int timeout)
+{
+	int rc = _iwl_poll_restricted_bit(priv, addr, mask, timeout);
+
+	if (unlikely(rc == -ETIMEDOUT))
+		IWL_DEBUG_IO("poll_restricted_bit(0x%08X, 0x%08X) - "
+			     "timedout - %s %d\n", addr, mask, f, l);
+	else
+		IWL_DEBUG_IO("poll_restricted_bit(0x%08X, 0x%08X) = 0x%08X "
+			     "- %s %d\n", addr, mask, rc, f, l);
+	return rc;
+}
+#define iwl_poll_restricted_bit(iwl, addr, mask, timeout) \
+	__iwl_poll_restricted_bit(__FILE__, __LINE__, iwl, addr, mask, timeout)
+#else
+#define iwl_poll_restricted_bit _iwl_poll_restricted_bit
+#endif
+
+static inline u32 _iwl_read_restricted_reg(struct iwl_priv *priv, u32 reg)
+{
+	_iwl_write_restricted(priv, HBUS_TARG_PRPH_RADDR, reg | (3 << 24));
+	return _iwl_read_restricted(priv, HBUS_TARG_PRPH_RDAT);
+}
+#ifdef CONFIG_IWLWIFI_DEBUG
+static inline u32 __iwl_read_restricted_reg(u32 line,
+					    struct iwl_priv *priv, u32 reg)
+{
+	if (!atomic_read(&priv->restrict_refcnt))
+		IWL_ERROR("Unrestricted access from line %d\n", line);
+	return _iwl_read_restricted_reg(priv, reg);
+}
+
+#define iwl_read_restricted_reg(priv, reg) \
+	__iwl_read_restricted_reg(__LINE__, priv, reg)
+#else
+#define iwl_read_restricted_reg _iwl_read_restricted_reg
+#endif
+
+static inline void _iwl_write_restricted_reg(struct iwl_priv *priv,
+					     u32 addr, u32 val)
+{
+	_iwl_write_restricted(priv, HBUS_TARG_PRPH_WADDR,
+			      ((addr & 0x0000FFFF) | (3 << 24)));
+	_iwl_write_restricted(priv, HBUS_TARG_PRPH_WDAT, val);
+}
+#ifdef CONFIG_IWLWIFI_DEBUG
+static inline void __iwl_write_restricted_reg(u32 line,
+					      struct iwl_priv *priv,
+					      u32 addr, u32 val)
+{
+	if (!atomic_read(&priv->restrict_refcnt))
+		IWL_ERROR("Unrestricted access from line %d\n", line);
+	_iwl_write_restricted_reg(priv, addr, val);
+}
+
+#define iwl_write_restricted_reg(priv, addr, val) \
+	__iwl_write_restricted_reg(__LINE__, priv, addr, val);
+#else
+#define iwl_write_restricted_reg _iwl_write_restricted_reg
+#endif
+
+#define _iwl_set_bits_restricted_reg(priv, reg, mask) \
+	_iwl_write_restricted_reg(priv, reg, \
+				  (_iwl_read_restricted_reg(priv, reg) | mask))
+#ifdef CONFIG_IWLWIFI_DEBUG
+static inline void __iwl_set_bits_restricted_reg(u32 line, struct iwl_priv
+						 *priv, u32 reg, u32 mask)
+{
+	if (!atomic_read(&priv->restrict_refcnt))
+		IWL_ERROR("Unrestricted access from line %d\n", line);
+	_iwl_set_bits_restricted_reg(priv, reg, mask);
+}
+#define iwl_set_bits_restricted_reg(priv, reg, mask) \
+	__iwl_set_bits_restricted_reg(__LINE__, priv, reg, mask)
+#else
+#define iwl_set_bits_restricted_reg _iwl_set_bits_restricted_reg
+#endif
+
+#define _iwl_set_bits_mask_restricted_reg(priv, reg, bits, mask) \
+	_iwl_write_restricted_reg( \
+	    priv, reg, ((_iwl_read_restricted_reg(priv, reg) & mask) | bits))
+#ifdef CONFIG_IWLWIFI_DEBUG
+static inline void __iwl_set_bits_mask_restricted_reg(u32 line,
+		struct iwl_priv *priv, u32 reg, u32 bits, u32 mask)
+{
+	if (!atomic_read(&priv->restrict_refcnt))
+		IWL_ERROR("Unrestricted access from line %d\n", line);
+	_iwl_set_bits_mask_restricted_reg(priv, reg, bits, mask);
+}
+
+#define iwl_set_bits_mask_restricted_reg(priv, reg, bits, mask) \
+	__iwl_set_bits_mask_restricted_reg(__LINE__, priv, reg, bits, mask)
+#else
+#define iwl_set_bits_mask_restricted_reg _iwl_set_bits_mask_restricted_reg
+#endif
+
+static inline void iwl_clear_bits_restricted_reg(struct iwl_priv
+						 *priv, u32 reg, u32 mask)
+{
+	u32 val = _iwl_read_restricted_reg(priv, reg);
+	_iwl_write_restricted_reg(priv, reg, (val & ~mask));
+}
+
+static inline u32 iwl_read_restricted_mem(struct iwl_priv *priv, u32 addr)
+{
+	iwl_write_restricted(priv, HBUS_TARG_MEM_RADDR, addr);
+	return iwl_read_restricted(priv, HBUS_TARG_MEM_RDAT);
+}
+
+static inline void iwl_write_restricted_mem(struct iwl_priv *priv, u32 addr,
+					    u32 val)
+{
+	iwl_write_restricted(priv, HBUS_TARG_MEM_WADDR, addr);
+	iwl_write_restricted(priv, HBUS_TARG_MEM_WDAT, val);
+}
+
+static inline void iwl_write_restricted_mems(struct iwl_priv *priv, u32 addr,
+					     u32 len, u32 *values)
+{
+	iwl_write_restricted(priv, HBUS_TARG_MEM_WADDR, addr);
+	for (; 0 < len; len -= sizeof(u32), values++)
+		iwl_write_restricted(priv, HBUS_TARG_MEM_WDAT, *values);
+}
+
+static inline void iwl_write_restricted_regs(struct iwl_priv *priv, u32 reg,
+					     u32 len, u8 *values)
+{
+	u32 reg_offset = reg;
+	u32 aligment = reg & 0x3;
+
+	/* write any non-dword-aligned stuff at the beginning */
+	if (len < sizeof(u32)) {
+		if ((aligment + len) <= sizeof(u32)) {
+			u8 size;
+			u32 value = 0;
+			size = len - 1;
+			memcpy(&value, values, len);
+			reg_offset = (reg_offset & 0x0000FFFF);
+
+			_iwl_write_restricted(priv,
+					      HBUS_TARG_PRPH_WADDR,
+					      (reg_offset | (size << 24)));
+			_iwl_write_restricted(priv, HBUS_TARG_PRPH_WDAT,
+					      value);
+		}
+
+		return;
+	}
+
+	/* now write all the dword-aligned stuff */
+	for (; reg_offset < (reg + len);
+	     reg_offset += sizeof(u32), values += sizeof(u32))
+		_iwl_write_restricted_reg(priv, reg_offset, *((u32 *) values));
+}
+
+#endif
diff --git a/drivers/net/wireless/iwlwifi/iwl-priv.h b/drivers/net/wireless/iwlwifi/iwl-priv.h
new file mode 100644
index 000000000000..6b490d08fea9
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-priv.h
@@ -0,0 +1,308 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2003 - 2007 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * James P. Ketrenos <ipw2100-admin@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+
+#ifndef __iwl_priv_h__
+#define __iwl_priv_h__
+
+#include <linux/workqueue.h>
+
+#ifdef CONFIG_IWLWIFI_SPECTRUM_MEASUREMENT
+
+enum {
+	MEASUREMENT_READY = (1 << 0),
+	MEASUREMENT_ACTIVE = (1 << 1),
+};
+
+#endif
+
+struct iwl_priv {
+
+	/* ieee device used by generic ieee processing code */
+	struct ieee80211_hw *hw;
+	struct ieee80211_channel *ieee_channels;
+	struct ieee80211_rate *ieee_rates;
+
+	/* temporary frame storage list */
+	struct list_head free_frames;
+	int frames_count;
+
+	u8 phymode;
+	int alloc_rxb_skb;
+
+	void (*rx_handlers[REPLY_MAX])(struct iwl_priv *priv,
+				       struct iwl_rx_mem_buffer *rxb);
+
+	const struct ieee80211_hw_mode *modes;
+
+#ifdef CONFIG_IWLWIFI_SPECTRUM_MEASUREMENT
+	/* spectrum measurement report caching */
+	struct iwl_spectrum_notification measure_report;
+	u8 measurement_status;
+#endif
+	/* ucode beacon time */
+	u32 ucode_beacon_time;
+
+	/* we allocate array of iwl_channel_info for NIC's valid channels.
+	 *    Access via channel # using indirect index array */
+	struct iwl_channel_info *channel_info;	/* channel info array */
+	u8 channel_count;	/* # of channels */
+
+	/* each calibration channel group in the EEPROM has a derived
+	 * clip setting for each rate. */
+	const struct iwl_clip_group clip_groups[5];
+
+	/* thermal calibration */
+	s32 temperature;	/* degrees Kelvin */
+	s32 last_temperature;
+
+	/* Scan related variables */
+	unsigned long last_scan_jiffies;
+	unsigned long scan_start;
+	unsigned long scan_pass_start;
+	unsigned long scan_start_tsf;
+	int scan_bands;
+	int one_direct_scan;
+	u8 direct_ssid_len;
+	u8 direct_ssid[IW_ESSID_MAX_SIZE];
+	struct iwl_scan_cmd *scan;
+	u8 only_active_channel;
+
+	/* spinlock */
+	spinlock_t lock;	/* protect general shared data */
+	spinlock_t hcmd_lock;	/* protect hcmd */
+	struct mutex mutex;
+
+	/* basic pci-network driver stuff */
+	struct pci_dev *pci_dev;
+
+	/* pci hardware address support */
+	void __iomem *hw_base;
+
+	/* uCode images, save to reload in case of failure */
+	struct fw_image_desc ucode_code;	/* runtime inst */
+	struct fw_image_desc ucode_data;	/* runtime data original */
+	struct fw_image_desc ucode_data_backup;	/* runtime data save/restore */
+	struct fw_image_desc ucode_init;	/* initialization inst */
+	struct fw_image_desc ucode_init_data;	/* initialization data */
+	struct fw_image_desc ucode_boot;	/* bootstrap inst */
+
+
+	struct iwl_rxon_time_cmd rxon_timing;
+
+	/* We declare this const so it can only be
+	 * changed via explicit cast within the
+	 * routines that actually update the physical
+	 * hardware */
+	const struct iwl_rxon_cmd active_rxon;
+	struct iwl_rxon_cmd staging_rxon;
+
+	int error_recovering;
+	struct iwl_rxon_cmd recovery_rxon;
+
+	/* 1st responses from initialize and runtime uCode images.
+	 * 4965's initialize alive response contains some calibration data. */
+	struct iwl_init_alive_resp card_alive_init;
+	struct iwl_alive_resp card_alive;
+
+#ifdef LED
+	/* LED related variables */
+	struct iwl_activity_blink activity;
+	unsigned long led_packets;
+	int led_state;
+#endif
+
+	u16 active_rate;
+	u16 active_rate_basic;
+
+	u8 call_post_assoc_from_beacon;
+	u8 assoc_station_added;
+#if IWL == 4965
+	u8 use_ant_b_for_management_frame;	/* Tx antenna selection */
+	/* HT variables */
+	u8 is_dup;
+	u8 is_ht_enabled;
+	u8 channel_width;	/* 0=20MHZ, 1=40MHZ */
+	u8 current_channel_width;
+	u8 valid_antenna;	/* Bit mask of antennas actually connected */
+#ifdef CONFIG_IWLWIFI_SENSITIVITY
+	struct iwl_sensitivity_data sensitivity_data;
+	struct iwl_chain_noise_data chain_noise_data;
+	u8 start_calib;
+	__le16 sensitivity_tbl[HD_TABLE_SIZE];
+#endif /*CONFIG_IWLWIFI_SENSITIVITY*/
+
+#ifdef CONFIG_IWLWIFI_HT
+	struct sta_ht_info current_assoc_ht;
+#endif
+	u8 active_rate_ht[2];
+	u8 last_phy_res[100];
+
+	/* Rate scaling data */
+	struct iwl_lq_mngr lq_mngr;
+#endif
+
+	/* Rate scaling data */
+	s8 data_retry_limit;
+	u8 retry_rate;
+
+	wait_queue_head_t wait_command_queue;
+
+	int activity_timer_active;
+
+	/* Rx and Tx DMA processing queues */
+	struct iwl_rx_queue rxq;
+	struct iwl_tx_queue txq[IWL_MAX_NUM_QUEUES];
+#if IWL == 4965
+	unsigned long txq_ctx_active_msk;
+	struct iwl_kw kw;	/* keep warm address */
+	u32 scd_base_addr;	/* scheduler sram base address */
+#endif
+
+	unsigned long status;
+	u32 config;
+
+	int last_rx_rssi;	/* From Rx packet statisitics */
+	int last_rx_noise;	/* From beacon statistics */
+
+	struct iwl_power_mgr power_data;
+
+	struct iwl_notif_statistics statistics;
+	unsigned long last_statistics_time;
+
+	/* context information */
+	u8 essid[IW_ESSID_MAX_SIZE];
+	u8 essid_len;
+	u16 rates_mask;
+
+	u32 power_mode;
+	u32 antenna;
+	u8 bssid[ETH_ALEN];
+	u16 rts_threshold;
+	u8 mac_addr[ETH_ALEN];
+
+	/*station table variables */
+	spinlock_t sta_lock;
+	int num_stations;
+	struct iwl_station_entry stations[IWL_STATION_COUNT];
+
+	/* Indication if ieee80211_ops->open has been called */
+	int is_open;
+
+	u8 mac80211_registered;
+	int is_abg;
+
+	u32 notif_missed_beacons;
+
+	/* Rx'd packet timing information */
+	u32 last_beacon_time;
+	u64 last_tsf;
+
+	/* Duplicate packet detection */
+	u16 last_seq_num;
+	u16 last_frag_num;
+	unsigned long last_packet_time;
+	struct list_head ibss_mac_hash[IWL_IBSS_MAC_HASH_SIZE];
+
+	/* eeprom */
+	struct iwl_eeprom eeprom;
+
+	int iw_mode;
+
+	struct sk_buff *ibss_beacon;
+
+	/* Last Rx'd beacon timestamp */
+	u32 timestamp0;
+	u32 timestamp1;
+	u16 beacon_int;
+	struct iwl_driver_hw_info hw_setting;
+	int interface_id;
+
+	/* Current association information needed to configure the
+	 * hardware */
+	u16 assoc_id;
+	u16 assoc_capability;
+	u8 ps_mode;
+
+#ifdef CONFIG_IWLWIFI_QOS
+	struct iwl_qos_info qos_data;
+#endif /*CONFIG_IWLWIFI_QOS */
+
+	struct workqueue_struct *workqueue;
+
+	struct work_struct up;
+	struct work_struct restart;
+	struct work_struct calibrated_work;
+	struct work_struct scan_completed;
+	struct work_struct rx_replenish;
+	struct work_struct rf_kill;
+	struct work_struct abort_scan;
+	struct work_struct update_link_led;
+	struct work_struct auth_work;
+	struct work_struct report_work;
+	struct work_struct request_scan;
+	struct work_struct beacon_update;
+
+	struct tasklet_struct irq_tasklet;
+
+	struct delayed_work init_alive_start;
+	struct delayed_work alive_start;
+	struct delayed_work activity_timer;
+	struct delayed_work thermal_periodic;
+	struct delayed_work gather_stats;
+	struct delayed_work scan_check;
+	struct delayed_work post_associate;
+
+#define IWL_DEFAULT_TX_POWER 0x0F
+	s8 user_txpower_limit;
+	s8 max_channel_txpower_limit;
+	u32 cck_power_index_compensation;
+
+#ifdef CONFIG_PM
+	u32 pm_state[16];
+#endif
+
+#ifdef CONFIG_IWLWIFI_DEBUG
+	/* debugging info */
+	u32 framecnt_to_us;
+	atomic_t restrict_refcnt;
+#endif
+
+#if IWL == 4965
+	struct work_struct txpower_work;
+#ifdef CONFIG_IWLWIFI_SENSITIVITY
+	struct work_struct sensitivity_work;
+#endif
+	struct work_struct statistics_work;
+	struct timer_list statistics_periodic;
+
+#ifdef CONFIG_IWLWIFI_HT_AGG
+	struct work_struct agg_work;
+#endif
+
+#endif /* 4965 */
+};				/*iwl_priv */
+
+#endif /* __iwl_priv_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-prph.h b/drivers/net/wireless/iwlwifi/iwl-prph.h
new file mode 100644
index 000000000000..0df41148eadc
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-prph.h
@@ -0,0 +1,229 @@
+/******************************************************************************
+ *
+ * This file is provided under a dual BSD/GPLv2 license.  When using or
+ * redistributing this file, you may do so under either license.
+ *
+ * GPL LICENSE SUMMARY
+ *
+ * Copyright(c) 2005 - 2007 Intel Corporation. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of version 2 of the GNU Geeral Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110,
+ * USA
+ *
+ * The full GNU General Public License is included in this distribution
+ * in the file called LICENSE.GPL.
+ *
+ * Contact Information:
+ * James P. Ketrenos <ipw2100-admin@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ * BSD LICENSE
+ *
+ * Copyright(c) 2005 - 2007 Intel Corporation. All rights reserved.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *  * Neither the name Intel Corporation nor the names of its
+ *    contributors may be used to endorse or promote products derived
+ *    from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+ * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+ * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+ * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *****************************************************************************/
+
+#ifndef	__iwl_prph_h__
+#define __iwl_prph_h__
+
+
+#define PRPH_BASE	(0x00000)
+#define PRPH_END	(0xFFFFF)
+
+/* APMG (power management) constants */
+#define APMG_BASE			(PRPH_BASE + 0x3000)
+#define APMG_CLK_CTRL_REG		(APMG_BASE + 0x0000)
+#define APMG_CLK_EN_REG			(APMG_BASE + 0x0004)
+#define APMG_CLK_DIS_REG		(APMG_BASE + 0x0008)
+#define APMG_PS_CTRL_REG		(APMG_BASE + 0x000c)
+#define APMG_PCIDEV_STT_REG		(APMG_BASE + 0x0010)
+#define APMG_RFKILL_REG			(APMG_BASE + 0x0014)
+#define APMG_RTC_INT_STT_REG		(APMG_BASE + 0x001c)
+#define APMG_RTC_INT_MSK_REG		(APMG_BASE + 0x0020)
+
+#define APMG_CLK_VAL_DMA_CLK_RQT	(0x00000200)
+#define APMG_CLK_VAL_BSM_CLK_RQT	(0x00000800)
+
+#define APMG_PS_CTRL_VAL_RESET_REQ	(0x04000000)
+
+#define APMG_PCIDEV_STT_VAL_L1_ACT_DIS	(0x00000800)
+
+#define APMG_PS_CTRL_MSK_PWR_SRC              (0x03000000)
+#define APMG_PS_CTRL_VAL_PWR_SRC_VMAIN        (0x00000000)
+#define APMG_PS_CTRL_VAL_PWR_SRC_VAUX         (0x01000000)
+
+
+/**
+ * BSM (Bootstrap State Machine)
+ *
+ * The Bootstrap State Machine (BSM) stores a short bootstrap uCode program
+ * in special SRAM that does not power down when the embedded control
+ * processor is sleeping (e.g. for periodic power-saving shutdowns of radio).
+ *
+ * When powering back up after sleeps (or during initial uCode load), the BSM
+ * internally loads the short bootstrap program from the special SRAM into the
+ * embedded processor's instruction SRAM, and starts the processor so it runs
+ * the bootstrap program.
+ *
+ * This bootstrap program loads (via PCI busmaster DMA) instructions and data
+ * images for a uCode program from host DRAM locations.  The host driver
+ * indicates DRAM locations and sizes for instruction and data images via the
+ * four BSM_DRAM_* registers.  Once the bootstrap program loads the new program,
+ * the new program starts automatically.
+ *
+ * The uCode used for open-source drivers includes two programs:
+ *
+ * 1)  Initialization -- performs hardware calibration and sets up some
+ *     internal data, then notifies host via "initialize alive" notification
+ *     (struct iwl_init_alive_resp) that it has completed all of its work.
+ *     After signal from host, it then loads and starts the runtime program.
+ *     The initialization program must be used when initially setting up the
+ *     NIC after loading the driver.
+ *
+ * 2)  Runtime/Protocol -- performs all normal runtime operations.  This
+ *     notifies host via "alive" notification (struct iwl_alive_resp) that it
+ *     is ready to be used.
+ *
+ * When initializing the NIC, the host driver does the following procedure:
+ *
+ * 1)  Load bootstrap program (instructions only, no data image for bootstrap)
+ *     into bootstrap memory.  Use dword writes starting at BSM_SRAM_LOWER_BOUND
+ *
+ * 2)  Point (via BSM_DRAM_*) to the "initialize" uCode data and instruction
+ *     images in host DRAM.
+ *
+ * 3)  Set up BSM to copy from BSM SRAM into uCode instruction SRAM when asked:
+ *     BSM_WR_MEM_SRC_REG = 0
+ *     BSM_WR_MEM_DST_REG = RTC_INST_LOWER_BOUND
+ *     BSM_WR_MEM_DWCOUNT_REG = # dwords in bootstrap instruction image
+ *
+ * 4)  Load bootstrap into instruction SRAM:
+ *     BSM_WR_CTRL_REG = BSM_WR_CTRL_REG_BIT_START
+ *
+ * 5)  Wait for load completion:
+ *     Poll BSM_WR_CTRL_REG for BSM_WR_CTRL_REG_BIT_START = 0
+ *
+ * 6)  Enable future boot loads whenever NIC's power management triggers it:
+ *     BSM_WR_CTRL_REG = BSM_WR_CTRL_REG_BIT_START_EN
+ *
+ * 7)  Start the NIC by removing all reset bits:
+ *     CSR_RESET = 0
+ *
+ *     The bootstrap uCode (already in instruction SRAM) loads initialization
+ *     uCode.  Initialization uCode performs data initialization, sends
+ *     "initialize alive" notification to host, and waits for a signal from
+ *     host to load runtime code.
+ *
+ * 4)  Point (via BSM_DRAM_*) to the "runtime" uCode data and instruction
+ *     images in host DRAM.  The last register loaded must be the instruction
+ *     bytecount register ("1" in MSbit tells initialization uCode to load
+ *     the runtime uCode):
+ *     BSM_DRAM_INST_BYTECOUNT_REG = bytecount | BSM_DRAM_INST_LOAD
+ *
+ * 5)  Wait for "alive" notification, then issue normal runtime commands.
+ *
+ * Data caching during power-downs:
+ *
+ * Just before the embedded controller powers down (e.g for automatic
+ * power-saving modes, or for RFKILL), uCode stores (via PCI busmaster DMA)
+ * a current snapshot of the embedded processor's data SRAM into host DRAM.
+ * This caches the data while the embedded processor's memory is powered down.
+ * Location and size are controlled by BSM_DRAM_DATA_* registers.
+ *
+ * NOTE:  Instruction SRAM does not need to be saved, since that doesn't
+ *        change during operation; the original image (from uCode distribution
+ *        file) can be used for reload.
+ *
+ * When powering back up, the BSM loads the bootstrap program.  Bootstrap looks
+ * at the BSM_DRAM_* registers, which now point to the runtime instruction
+ * image and the cached (modified) runtime data (*not* the initialization
+ * uCode).  Bootstrap reloads these runtime images into SRAM, and restarts the
+ * uCode from where it left off before the power-down.
+ *
+ * NOTE:  Initialization uCode does *not* run as part of the save/restore
+ *        procedure.
+ *
+ * This save/restore method is mostly for autonomous power management during
+ * normal operation (result of POWER_TABLE_CMD).  Platform suspend/resume and
+ * RFKILL should use complete restarts (with total re-initialization) of uCode,
+ * allowing total shutdown (including BSM memory).
+ *
+ * Note that, during normal operation, the host DRAM that held the initial
+ * startup data for the runtime code is now being used as a backup data cache
+ * for modified data!  If you need to completely re-initialize the NIC, make
+ * sure that you use the runtime data image from the uCode distribution file,
+ * not the modified/saved runtime data.  You may want to store a separate
+ * "clean" runtime data image in DRAM to avoid disk reads of distribution file.
+ */
+
+/* BSM bit fields */
+#define BSM_WR_CTRL_REG_BIT_START     (0x80000000) /* start boot load now */
+#define BSM_WR_CTRL_REG_BIT_START_EN  (0x40000000) /* enable boot after pwrup*/
+#define BSM_DRAM_INST_LOAD            (0x80000000) /* start program load now */
+
+/* BSM addresses */
+#define BSM_BASE                     (PRPH_BASE + 0x3400)
+#define BSM_END                      (PRPH_BASE + 0x3800)
+
+#define BSM_WR_CTRL_REG              (BSM_BASE + 0x000) /* ctl and status */
+#define BSM_WR_MEM_SRC_REG           (BSM_BASE + 0x004) /* source in BSM mem */
+#define BSM_WR_MEM_DST_REG           (BSM_BASE + 0x008) /* dest in SRAM mem */
+#define BSM_WR_DWCOUNT_REG           (BSM_BASE + 0x00C) /* bytes */
+#define BSM_WR_STATUS_REG            (BSM_BASE + 0x010) /* bit 0:  1 == done */
+
+/*
+ * Pointers and size regs for bootstrap load and data SRAM save/restore.
+ * NOTE:  3945 pointers use bits 31:0 of DRAM address.
+ *        4965 pointers use bits 35:4 of DRAM address.
+ */
+#define BSM_DRAM_INST_PTR_REG        (BSM_BASE + 0x090)
+#define BSM_DRAM_INST_BYTECOUNT_REG  (BSM_BASE + 0x094)
+#define BSM_DRAM_DATA_PTR_REG        (BSM_BASE + 0x098)
+#define BSM_DRAM_DATA_BYTECOUNT_REG  (BSM_BASE + 0x09C)
+
+/*
+ * BSM special memory, stays powered on during power-save sleeps.
+ * Read/write, address range from LOWER_BOUND to (LOWER_BOUND + SIZE -1)
+ */
+#define BSM_SRAM_LOWER_BOUND         (PRPH_BASE + 0x3800)
+#define BSM_SRAM_SIZE			(1024) /* bytes */
+
+
+#endif				/* __iwl_prph_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-spectrum.h b/drivers/net/wireless/iwlwifi/iwl-spectrum.h
new file mode 100644
index 000000000000..b576ff24eb4f
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-spectrum.h
@@ -0,0 +1,91 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2003 - 2007 Intel Corporation. All rights reserved.
+ *
+ * Portions of this file are derived from the ieee80211 subsystem header files.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * James P. Ketrenos <ipw2100-admin@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+
+#ifndef __iwl_spectrum_h__
+#define __iwl_spectrum_h__
+enum {				/* ieee80211_basic_report.map */
+	IEEE80211_BASIC_MAP_BSS = (1 << 0),
+	IEEE80211_BASIC_MAP_OFDM = (1 << 1),
+	IEEE80211_BASIC_MAP_UNIDENTIFIED = (1 << 2),
+	IEEE80211_BASIC_MAP_RADAR = (1 << 3),
+	IEEE80211_BASIC_MAP_UNMEASURED = (1 << 4),
+	/* Bits 5-7 are reserved */
+
+};
+struct ieee80211_basic_report {
+	u8 channel;
+	__le64 start_time;
+	__le16 duration;
+	u8 map;
+} __attribute__ ((packed));
+
+enum {				/* ieee80211_measurement_request.mode */
+	/* Bit 0 is reserved */
+	IEEE80211_MEASUREMENT_ENABLE = (1 << 1),
+	IEEE80211_MEASUREMENT_REQUEST = (1 << 2),
+	IEEE80211_MEASUREMENT_REPORT = (1 << 3),
+	/* Bits 4-7 are reserved */
+};
+
+enum {
+	IEEE80211_REPORT_BASIC = 0,	/* required */
+	IEEE80211_REPORT_CCA = 1,	/* optional */
+	IEEE80211_REPORT_RPI = 2,	/* optional */
+	/* 3-255 reserved */
+};
+
+struct ieee80211_measurement_params {
+	u8 channel;
+	__le64 start_time;
+	__le16 duration;
+} __attribute__ ((packed));
+
+struct ieee80211_info_element {
+	u8 id;
+	u8 len;
+	u8 data[0];
+} __attribute__ ((packed));
+
+struct ieee80211_measurement_request {
+	struct ieee80211_info_element ie;
+	u8 token;
+	u8 mode;
+	u8 type;
+	struct ieee80211_measurement_params params[0];
+} __attribute__ ((packed));
+
+struct ieee80211_measurement_report {
+	struct ieee80211_info_element ie;
+	u8 token;
+	u8 mode;
+	u8 type;
+	union {
+		struct ieee80211_basic_report basic[0];
+	} u;
+} __attribute__ ((packed));
+#endif
diff --git a/drivers/net/wireless/iwlwifi/iwl3945-base.c b/drivers/net/wireless/iwlwifi/iwl3945-base.c
new file mode 100644
index 000000000000..75e3b5c3f155
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl3945-base.c
@@ -0,0 +1,8746 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2003 - 2007 Intel Corporation. All rights reserved.
+ *
+ * Portions of this file are derived from the ipw3945 project, as well
+ * as portions of the ieee80211 subsystem header files.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * James P. Ketrenos <ipw2100-admin@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+
+/*
+ * NOTE:  This file (iwl-base.c) is used to build to multiple hardware targets
+ * by defining IWL to either 3945 or 4965.  The Makefile used when building
+ * the base targets will create base-3945.o and base-4965.o
+ *
+ * The eventual goal is to move as many of the #if IWL / #endif blocks out of
+ * this file and into the hardware specific implementation files (iwl-XXXX.c)
+ * and leave only the common (non #ifdef sprinkled) code in this file
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/version.h>
+#include <linux/init.h>
+#include <linux/pci.h>
+#include <linux/dma-mapping.h>
+#include <linux/delay.h>
+#include <linux/skbuff.h>
+#include <linux/netdevice.h>
+#include <linux/wireless.h>
+#include <linux/firmware.h>
+#include <linux/skbuff.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/if_arp.h>
+
+#include <net/ieee80211_radiotap.h>
+#include <net/mac80211.h>
+
+#include <asm/div64.h>
+
+#define IWL 3945
+
+#include "iwlwifi.h"
+#include "iwl-3945.h"
+#include "iwl-helpers.h"
+
+#ifdef CONFIG_IWLWIFI_DEBUG
+u32 iwl_debug_level;
+#endif
+
+/******************************************************************************
+ *
+ * module boiler plate
+ *
+ ******************************************************************************/
+
+/* module parameters */
+int iwl_param_disable_hw_scan;
+int iwl_param_debug;
+int iwl_param_disable;      /* def: enable radio */
+int iwl_param_antenna;      /* def: 0 = both antennas (use diversity) */
+int iwl_param_hwcrypto;     /* def: using software encryption */
+int iwl_param_qos_enable = 1;
+int iwl_param_queues_num = IWL_MAX_NUM_QUEUES;
+
+/*
+ * module name, copyright, version, etc.
+ * NOTE: DRV_NAME is defined in iwlwifi.h for use by iwl-debug.h and printk
+ */
+
+#define DRV_DESCRIPTION	\
+"Intel(R) PRO/Wireless 3945ABG/BG Network Connection driver for Linux"
+
+#ifdef CONFIG_IWLWIFI_DEBUG
+#define VD "d"
+#else
+#define VD
+#endif
+
+#ifdef CONFIG_IWLWIFI_SPECTRUM_MEASUREMENT
+#define VS "s"
+#else
+#define VS
+#endif
+
+#define IWLWIFI_VERSION "1.1.17k" VD VS
+#define DRV_COPYRIGHT	"Copyright(c) 2003-2007 Intel Corporation"
+#define DRV_VERSION     IWLWIFI_VERSION
+
+/* Change firmware file name, using "-" and incrementing number,
+ *   *only* when uCode interface or architecture changes so that it
+ *   is not compatible with earlier drivers.
+ * This number will also appear in << 8 position of 1st dword of uCode file */
+#define IWL3945_UCODE_API "-1"
+
+MODULE_DESCRIPTION(DRV_DESCRIPTION);
+MODULE_VERSION(DRV_VERSION);
+MODULE_AUTHOR(DRV_COPYRIGHT);
+MODULE_LICENSE("GPL");
+
+__le16 *ieee80211_get_qos_ctrl(struct ieee80211_hdr *hdr)
+{
+	u16 fc = le16_to_cpu(hdr->frame_control);
+	int hdr_len = ieee80211_get_hdrlen(fc);
+
+	if ((fc & 0x00cc) == (IEEE80211_STYPE_QOS_DATA | IEEE80211_FTYPE_DATA))
+		return (__le16 *) ((u8 *) hdr + hdr_len - QOS_CONTROL_LEN);
+	return NULL;
+}
+
+static const struct ieee80211_hw_mode *iwl_get_hw_mode(
+		struct iwl_priv *priv, int mode)
+{
+	int i;
+
+	for (i = 0; i < 3; i++)
+		if (priv->modes[i].mode == mode)
+			return &priv->modes[i];
+
+	return NULL;
+}
+
+static int iwl_is_empty_essid(const char *essid, int essid_len)
+{
+	/* Single white space is for Linksys APs */
+	if (essid_len == 1 && essid[0] == ' ')
+		return 1;
+
+	/* Otherwise, if the entire essid is 0, we assume it is hidden */
+	while (essid_len) {
+		essid_len--;
+		if (essid[essid_len] != '\0')
+			return 0;
+	}
+
+	return 1;
+}
+
+static const char *iwl_escape_essid(const char *essid, u8 essid_len)
+{
+	static char escaped[IW_ESSID_MAX_SIZE * 2 + 1];
+	const char *s = essid;
+	char *d = escaped;
+
+	if (iwl_is_empty_essid(essid, essid_len)) {
+		memcpy(escaped, "<hidden>", sizeof("<hidden>"));
+		return escaped;
+	}
+
+	essid_len = min(essid_len, (u8) IW_ESSID_MAX_SIZE);
+	while (essid_len--) {
+		if (*s == '\0') {
+			*d++ = '\\';
+			*d++ = '0';
+			s++;
+		} else
+			*d++ = *s++;
+	}
+	*d = '\0';
+	return escaped;
+}
+
+static void iwl_print_hex_dump(int level, void *p, u32 len)
+{
+#ifdef CONFIG_IWLWIFI_DEBUG
+	if (!(iwl_debug_level & level))
+		return;
+
+	print_hex_dump(KERN_DEBUG, "iwl data: ", DUMP_PREFIX_OFFSET, 16, 1,
+			p, len, 1);
+#endif
+}
+
+/*************** DMA-QUEUE-GENERAL-FUNCTIONS  *****
+ * DMA services
+ *
+ * Theory of operation
+ *
+ * A queue is a circular buffers with 'Read' and 'Write' pointers.
+ * 2 empty entries always kept in the buffer to protect from overflow.
+ *
+ * For Tx queue, there are low mark and high mark limits. If, after queuing
+ * the packet for Tx, free space become < low mark, Tx queue stopped. When
+ * reclaiming packets (on 'tx done IRQ), if free space become > high mark,
+ * Tx queue resumed.
+ *
+ * The IWL operates with six queues, one receive queue in the device's
+ * sram, one transmit queue for sending commands to the device firmware,
+ * and four transmit queues for data.
+ ***************************************************/
+
+static int iwl_queue_space(const struct iwl_queue *q)
+{
+	int s = q->last_used - q->first_empty;
+
+	if (q->last_used > q->first_empty)
+		s -= q->n_bd;
+
+	if (s <= 0)
+		s += q->n_window;
+	/* keep some reserve to not confuse empty and full situations */
+	s -= 2;
+	if (s < 0)
+		s = 0;
+	return s;
+}
+
+/* XXX: n_bd must be power-of-two size */
+static inline int iwl_queue_inc_wrap(int index, int n_bd)
+{
+	return ++index & (n_bd - 1);
+}
+
+/* XXX: n_bd must be power-of-two size */
+static inline int iwl_queue_dec_wrap(int index, int n_bd)
+{
+	return --index & (n_bd - 1);
+}
+
+static inline int x2_queue_used(const struct iwl_queue *q, int i)
+{
+	return q->first_empty > q->last_used ?
+		(i >= q->last_used && i < q->first_empty) :
+		!(i < q->last_used && i >= q->first_empty);
+}
+
+static inline u8 get_cmd_index(struct iwl_queue *q, u32 index, int is_huge)
+{
+	if (is_huge)
+		return q->n_window;
+
+	return index & (q->n_window - 1);
+}
+
+static int iwl_queue_init(struct iwl_priv *priv, struct iwl_queue *q,
+			  int count, int slots_num, u32 id)
+{
+	q->n_bd = count;
+	q->n_window = slots_num;
+	q->id = id;
+
+	/* count must be power-of-two size, otherwise iwl_queue_inc_wrap
+	 * and iwl_queue_dec_wrap are broken. */
+	BUG_ON(!is_power_of_2(count));
+
+	/* slots_num must be power-of-two size, otherwise
+	 * get_cmd_index is broken. */
+	BUG_ON(!is_power_of_2(slots_num));
+
+	q->low_mark = q->n_window / 4;
+	if (q->low_mark < 4)
+		q->low_mark = 4;
+
+	q->high_mark = q->n_window / 8;
+	if (q->high_mark < 2)
+		q->high_mark = 2;
+
+	q->first_empty = q->last_used = 0;
+
+	return 0;
+}
+
+static int iwl_tx_queue_alloc(struct iwl_priv *priv,
+			      struct iwl_tx_queue *txq, u32 id)
+{
+	struct pci_dev *dev = priv->pci_dev;
+
+	if (id != IWL_CMD_QUEUE_NUM) {
+		txq->txb = kmalloc(sizeof(txq->txb[0]) *
+				   TFD_QUEUE_SIZE_MAX, GFP_KERNEL);
+		if (!txq->txb) {
+			IWL_ERROR("kmalloc for auxilary BD "
+				  "structures failed\n");
+			goto error;
+		}
+	} else
+		txq->txb = NULL;
+
+	txq->bd = pci_alloc_consistent(dev,
+			sizeof(txq->bd[0]) * TFD_QUEUE_SIZE_MAX,
+			&txq->q.dma_addr);
+
+	if (!txq->bd) {
+		IWL_ERROR("pci_alloc_consistent(%zd) failed\n",
+			  sizeof(txq->bd[0]) * TFD_QUEUE_SIZE_MAX);
+		goto error;
+	}
+	txq->q.id = id;
+
+	return 0;
+
+ error:
+	if (txq->txb) {
+		kfree(txq->txb);
+		txq->txb = NULL;
+	}
+
+	return -ENOMEM;
+}
+
+int iwl_tx_queue_init(struct iwl_priv *priv,
+		      struct iwl_tx_queue *txq, int slots_num, u32 txq_id)
+{
+	struct pci_dev *dev = priv->pci_dev;
+	int len;
+	int rc = 0;
+
+	/* alocate command space + one big command for scan since scan
+	 * command is very huge the system will not have two scan at the
+	 * same time */
+	len = sizeof(struct iwl_cmd) * slots_num;
+	if (txq_id == IWL_CMD_QUEUE_NUM)
+		len +=  IWL_MAX_SCAN_SIZE;
+	txq->cmd = pci_alloc_consistent(dev, len, &txq->dma_addr_cmd);
+	if (!txq->cmd)
+		return -ENOMEM;
+
+	rc = iwl_tx_queue_alloc(priv, txq, txq_id);
+	if (rc) {
+		pci_free_consistent(dev, len, txq->cmd, txq->dma_addr_cmd);
+
+		return -ENOMEM;
+	}
+	txq->need_update = 0;
+
+	/* TFD_QUEUE_SIZE_MAX must be power-of-two size, otherwise
+	 * iwl_queue_inc_wrap and iwl_queue_dec_wrap are broken. */
+	BUILD_BUG_ON(TFD_QUEUE_SIZE_MAX & (TFD_QUEUE_SIZE_MAX - 1));
+	iwl_queue_init(priv, &txq->q, TFD_QUEUE_SIZE_MAX, slots_num, txq_id);
+
+	iwl_hw_tx_queue_init(priv, txq);
+
+	return 0;
+}
+
+/**
+ * iwl_tx_queue_free - Deallocate DMA queue.
+ * @txq: Transmit queue to deallocate.
+ *
+ * Empty queue by removing and destroying all BD's.
+ * Free all buffers.  txq itself is not freed.
+ *
+ */
+void iwl_tx_queue_free(struct iwl_priv *priv, struct iwl_tx_queue *txq)
+{
+	struct iwl_queue *q = &txq->q;
+	struct pci_dev *dev = priv->pci_dev;
+	int len;
+
+	if (q->n_bd == 0)
+		return;
+
+	/* first, empty all BD's */
+	for (; q->first_empty != q->last_used;
+	     q->last_used = iwl_queue_inc_wrap(q->last_used, q->n_bd))
+		iwl_hw_txq_free_tfd(priv, txq);
+
+	len = sizeof(struct iwl_cmd) * q->n_window;
+	if (q->id == IWL_CMD_QUEUE_NUM)
+		len += IWL_MAX_SCAN_SIZE;
+
+	pci_free_consistent(dev, len, txq->cmd, txq->dma_addr_cmd);
+
+	/* free buffers belonging to queue itself */
+	if (txq->q.n_bd)
+		pci_free_consistent(dev, sizeof(struct iwl_tfd_frame) *
+				    txq->q.n_bd, txq->bd, txq->q.dma_addr);
+
+	if (txq->txb) {
+		kfree(txq->txb);
+		txq->txb = NULL;
+	}
+
+	/* 0 fill whole structure */
+	memset(txq, 0, sizeof(*txq));
+}
+
+const u8 BROADCAST_ADDR[ETH_ALEN] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };
+
+/*************** STATION TABLE MANAGEMENT ****
+ *
+ * NOTE:  This needs to be overhauled to better synchronize between
+ * how the iwl-4965.c is using iwl_hw_find_station vs. iwl-3945.c
+ *
+ * mac80211 should also be examined to determine if sta_info is duplicating
+ * the functionality provided here
+ */
+
+/**************************************************************/
+#if 0 /* temparary disable till we add real remove station */
+static u8 iwl_remove_station(struct iwl_priv *priv, const u8 *addr, int is_ap)
+{
+	int index = IWL_INVALID_STATION;
+	int i;
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->sta_lock, flags);
+
+	if (is_ap)
+		index = IWL_AP_ID;
+	else if (is_broadcast_ether_addr(addr))
+		index = priv->hw_setting.bcast_sta_id;
+	else
+		for (i = IWL_STA_ID; i < priv->hw_setting.max_stations; i++)
+			if (priv->stations[i].used &&
+			    !compare_ether_addr(priv->stations[i].sta.sta.addr,
+						addr)) {
+				index = i;
+				break;
+			}
+
+	if (unlikely(index == IWL_INVALID_STATION))
+		goto out;
+
+	if (priv->stations[index].used) {
+		priv->stations[index].used = 0;
+		priv->num_stations--;
+	}
+
+	BUG_ON(priv->num_stations < 0);
+
+out:
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+	return 0;
+}
+#endif
+static void iwl_clear_stations_table(struct iwl_priv *priv)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->sta_lock, flags);
+
+	priv->num_stations = 0;
+	memset(priv->stations, 0, sizeof(priv->stations));
+
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+}
+
+
+u8 iwl_add_station(struct iwl_priv *priv, const u8 *addr, int is_ap, u8 flags)
+{
+	int i;
+	int index = IWL_INVALID_STATION;
+	struct iwl_station_entry *station;
+	unsigned long flags_spin;
+	DECLARE_MAC_BUF(mac);
+	u8 rate;
+
+	spin_lock_irqsave(&priv->sta_lock, flags_spin);
+	if (is_ap)
+		index = IWL_AP_ID;
+	else if (is_broadcast_ether_addr(addr))
+		index = priv->hw_setting.bcast_sta_id;
+	else
+		for (i = IWL_STA_ID; i < priv->hw_setting.max_stations; i++) {
+			if (!compare_ether_addr(priv->stations[i].sta.sta.addr,
+						addr)) {
+				index = i;
+				break;
+			}
+
+			if (!priv->stations[i].used &&
+			    index == IWL_INVALID_STATION)
+				index = i;
+		}
+
+	/* These twh conditions has the same outcome but keep them separate
+	  since they have different meaning */
+	if (unlikely(index == IWL_INVALID_STATION)) {
+		spin_unlock_irqrestore(&priv->sta_lock, flags_spin);
+		return index;
+	}
+
+	if (priv->stations[index].used &&
+	   !compare_ether_addr(priv->stations[index].sta.sta.addr, addr)) {
+		spin_unlock_irqrestore(&priv->sta_lock, flags_spin);
+		return index;
+	}
+
+	IWL_DEBUG_ASSOC("Add STA ID %d: %s\n", index, print_mac(mac, addr));
+	station = &priv->stations[index];
+	station->used = 1;
+	priv->num_stations++;
+
+	memset(&station->sta, 0, sizeof(struct iwl_addsta_cmd));
+	memcpy(station->sta.sta.addr, addr, ETH_ALEN);
+	station->sta.mode = 0;
+	station->sta.sta.sta_id = index;
+	station->sta.station_flags = 0;
+
+	rate = (priv->phymode == MODE_IEEE80211A) ? IWL_RATE_6M_PLCP :
+				IWL_RATE_1M_PLCP | priv->hw_setting.cck_flag;
+
+	/* Turn on both antennas for the station... */
+	station->sta.rate_n_flags =
+			iwl_hw_set_rate_n_flags(rate, RATE_MCS_ANT_AB_MSK);
+	station->current_rate.rate_n_flags =
+			le16_to_cpu(station->sta.rate_n_flags);
+
+	spin_unlock_irqrestore(&priv->sta_lock, flags_spin);
+	iwl_send_add_station(priv, &station->sta, flags);
+	return index;
+
+}
+
+/*************** DRIVER STATUS FUNCTIONS   *****/
+
+static inline int iwl_is_ready(struct iwl_priv *priv)
+{
+	/* The adapter is 'ready' if READY and GEO_CONFIGURED bits are
+	 * set but EXIT_PENDING is not */
+	return test_bit(STATUS_READY, &priv->status) &&
+	       test_bit(STATUS_GEO_CONFIGURED, &priv->status) &&
+	       !test_bit(STATUS_EXIT_PENDING, &priv->status);
+}
+
+static inline int iwl_is_alive(struct iwl_priv *priv)
+{
+	return test_bit(STATUS_ALIVE, &priv->status);
+}
+
+static inline int iwl_is_init(struct iwl_priv *priv)
+{
+	return test_bit(STATUS_INIT, &priv->status);
+}
+
+static inline int iwl_is_rfkill(struct iwl_priv *priv)
+{
+	return test_bit(STATUS_RF_KILL_HW, &priv->status) ||
+	       test_bit(STATUS_RF_KILL_SW, &priv->status);
+}
+
+static inline int iwl_is_ready_rf(struct iwl_priv *priv)
+{
+
+	if (iwl_is_rfkill(priv))
+		return 0;
+
+	return iwl_is_ready(priv);
+}
+
+/*************** HOST COMMAND QUEUE FUNCTIONS   *****/
+
+#define IWL_CMD(x) case x : return #x
+
+static const char *get_cmd_string(u8 cmd)
+{
+	switch (cmd) {
+		IWL_CMD(REPLY_ALIVE);
+		IWL_CMD(REPLY_ERROR);
+		IWL_CMD(REPLY_RXON);
+		IWL_CMD(REPLY_RXON_ASSOC);
+		IWL_CMD(REPLY_QOS_PARAM);
+		IWL_CMD(REPLY_RXON_TIMING);
+		IWL_CMD(REPLY_ADD_STA);
+		IWL_CMD(REPLY_REMOVE_STA);
+		IWL_CMD(REPLY_REMOVE_ALL_STA);
+		IWL_CMD(REPLY_3945_RX);
+		IWL_CMD(REPLY_TX);
+		IWL_CMD(REPLY_RATE_SCALE);
+		IWL_CMD(REPLY_LEDS_CMD);
+		IWL_CMD(REPLY_TX_LINK_QUALITY_CMD);
+		IWL_CMD(RADAR_NOTIFICATION);
+		IWL_CMD(REPLY_QUIET_CMD);
+		IWL_CMD(REPLY_CHANNEL_SWITCH);
+		IWL_CMD(CHANNEL_SWITCH_NOTIFICATION);
+		IWL_CMD(REPLY_SPECTRUM_MEASUREMENT_CMD);
+		IWL_CMD(SPECTRUM_MEASURE_NOTIFICATION);
+		IWL_CMD(POWER_TABLE_CMD);
+		IWL_CMD(PM_SLEEP_NOTIFICATION);
+		IWL_CMD(PM_DEBUG_STATISTIC_NOTIFIC);
+		IWL_CMD(REPLY_SCAN_CMD);
+		IWL_CMD(REPLY_SCAN_ABORT_CMD);
+		IWL_CMD(SCAN_START_NOTIFICATION);
+		IWL_CMD(SCAN_RESULTS_NOTIFICATION);
+		IWL_CMD(SCAN_COMPLETE_NOTIFICATION);
+		IWL_CMD(BEACON_NOTIFICATION);
+		IWL_CMD(REPLY_TX_BEACON);
+		IWL_CMD(WHO_IS_AWAKE_NOTIFICATION);
+		IWL_CMD(QUIET_NOTIFICATION);
+		IWL_CMD(REPLY_TX_PWR_TABLE_CMD);
+		IWL_CMD(MEASURE_ABORT_NOTIFICATION);
+		IWL_CMD(REPLY_BT_CONFIG);
+		IWL_CMD(REPLY_STATISTICS_CMD);
+		IWL_CMD(STATISTICS_NOTIFICATION);
+		IWL_CMD(REPLY_CARD_STATE_CMD);
+		IWL_CMD(CARD_STATE_NOTIFICATION);
+		IWL_CMD(MISSED_BEACONS_NOTIFICATION);
+	default:
+		return "UNKNOWN";
+
+	}
+}
+
+#define HOST_COMPLETE_TIMEOUT (HZ / 2)
+
+/**
+ * iwl_enqueue_hcmd - enqueue a uCode command
+ * @priv: device private data point
+ * @cmd: a point to the ucode command structure
+ *
+ * The function returns < 0 values to indicate the operation is
+ * failed. On success, it turns the index (> 0) of command in the
+ * command queue.
+ */
+static int iwl_enqueue_hcmd(struct iwl_priv *priv, struct iwl_host_cmd *cmd)
+{
+	struct iwl_tx_queue *txq = &priv->txq[IWL_CMD_QUEUE_NUM];
+	struct iwl_queue *q = &txq->q;
+	struct iwl_tfd_frame *tfd;
+	u32 *control_flags;
+	struct iwl_cmd *out_cmd;
+	u32 idx;
+	u16 fix_size = (u16)(cmd->len + sizeof(out_cmd->hdr));
+	dma_addr_t phys_addr;
+	int pad;
+	u16 count;
+	int ret;
+	unsigned long flags;
+
+	/* If any of the command structures end up being larger than
+	 * the TFD_MAX_PAYLOAD_SIZE, and it sent as a 'small' command then
+	 * we will need to increase the size of the TFD entries */
+	BUG_ON((fix_size > TFD_MAX_PAYLOAD_SIZE) &&
+	       !(cmd->meta.flags & CMD_SIZE_HUGE));
+
+	if (iwl_queue_space(q) < ((cmd->meta.flags & CMD_ASYNC) ? 2 : 1)) {
+		IWL_ERROR("No space for Tx\n");
+		return -ENOSPC;
+	}
+
+	spin_lock_irqsave(&priv->hcmd_lock, flags);
+
+	tfd = &txq->bd[q->first_empty];
+	memset(tfd, 0, sizeof(*tfd));
+
+	control_flags = (u32 *) tfd;
+
+	idx = get_cmd_index(q, q->first_empty, cmd->meta.flags & CMD_SIZE_HUGE);
+	out_cmd = &txq->cmd[idx];
+
+	out_cmd->hdr.cmd = cmd->id;
+	memcpy(&out_cmd->meta, &cmd->meta, sizeof(cmd->meta));
+	memcpy(&out_cmd->cmd.payload, cmd->data, cmd->len);
+
+	/* At this point, the out_cmd now has all of the incoming cmd
+	 * information */
+
+	out_cmd->hdr.flags = 0;
+	out_cmd->hdr.sequence = cpu_to_le16(QUEUE_TO_SEQ(IWL_CMD_QUEUE_NUM) |
+			INDEX_TO_SEQ(q->first_empty));
+	if (out_cmd->meta.flags & CMD_SIZE_HUGE)
+		out_cmd->hdr.sequence |= cpu_to_le16(SEQ_HUGE_FRAME);
+
+	phys_addr = txq->dma_addr_cmd + sizeof(txq->cmd[0]) * idx +
+			offsetof(struct iwl_cmd, hdr);
+	iwl_hw_txq_attach_buf_to_tfd(priv, tfd, phys_addr, fix_size);
+
+	pad = U32_PAD(cmd->len);
+	count = TFD_CTL_COUNT_GET(*control_flags);
+	*control_flags = TFD_CTL_COUNT_SET(count) | TFD_CTL_PAD_SET(pad);
+
+	IWL_DEBUG_HC("Sending command %s (#%x), seq: 0x%04X, "
+		     "%d bytes at %d[%d]:%d\n",
+		     get_cmd_string(out_cmd->hdr.cmd),
+		     out_cmd->hdr.cmd, le16_to_cpu(out_cmd->hdr.sequence),
+		     fix_size, q->first_empty, idx, IWL_CMD_QUEUE_NUM);
+
+	txq->need_update = 1;
+	q->first_empty = iwl_queue_inc_wrap(q->first_empty, q->n_bd);
+	ret = iwl_tx_queue_update_write_ptr(priv, txq);
+
+	spin_unlock_irqrestore(&priv->hcmd_lock, flags);
+	return ret ? ret : idx;
+}
+
+int iwl_send_cmd_async(struct iwl_priv *priv, struct iwl_host_cmd *cmd)
+{
+	int ret;
+
+	BUG_ON(!(cmd->meta.flags & CMD_ASYNC));
+
+	/* An asynchronous command can not expect an SKB to be set. */
+	BUG_ON(cmd->meta.flags & CMD_WANT_SKB);
+
+	/* An asynchronous command MUST have a callback. */
+	BUG_ON(!cmd->meta.u.callback);
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+		return -EBUSY;
+
+	ret = iwl_enqueue_hcmd(priv, cmd);
+	if (ret < 0) {
+		IWL_ERROR("Error sending %s: iwl_enqueue_hcmd failed: %d\n",
+			  get_cmd_string(cmd->id), ret);
+		return ret;
+	}
+	return 0;
+}
+
+int iwl_send_cmd_sync(struct iwl_priv *priv, struct iwl_host_cmd *cmd)
+{
+	int cmd_idx;
+	int ret;
+	static atomic_t entry = ATOMIC_INIT(0); /* reentrance protection */
+
+	BUG_ON(cmd->meta.flags & CMD_ASYNC);
+
+	 /* A synchronous command can not have a callback set. */
+	BUG_ON(cmd->meta.u.callback != NULL);
+
+	if (atomic_xchg(&entry, 1)) {
+		IWL_ERROR("Error sending %s: Already sending a host command\n",
+			  get_cmd_string(cmd->id));
+		return -EBUSY;
+	}
+
+	set_bit(STATUS_HCMD_ACTIVE, &priv->status);
+
+	if (cmd->meta.flags & CMD_WANT_SKB)
+		cmd->meta.source = &cmd->meta;
+
+	cmd_idx = iwl_enqueue_hcmd(priv, cmd);
+	if (cmd_idx < 0) {
+		ret = cmd_idx;
+		IWL_ERROR("Error sending %s: iwl_enqueue_hcmd failed: %d\n",
+			  get_cmd_string(cmd->id), ret);
+		goto out;
+	}
+
+	ret = wait_event_interruptible_timeout(priv->wait_command_queue,
+			!test_bit(STATUS_HCMD_ACTIVE, &priv->status),
+			HOST_COMPLETE_TIMEOUT);
+	if (!ret) {
+		if (test_bit(STATUS_HCMD_ACTIVE, &priv->status)) {
+			IWL_ERROR("Error sending %s: time out after %dms.\n",
+				  get_cmd_string(cmd->id),
+				  jiffies_to_msecs(HOST_COMPLETE_TIMEOUT));
+
+			clear_bit(STATUS_HCMD_ACTIVE, &priv->status);
+			ret = -ETIMEDOUT;
+			goto cancel;
+		}
+	}
+
+	if (test_bit(STATUS_RF_KILL_HW, &priv->status)) {
+		IWL_DEBUG_INFO("Command %s aborted: RF KILL Switch\n",
+			       get_cmd_string(cmd->id));
+		ret = -ECANCELED;
+		goto fail;
+	}
+	if (test_bit(STATUS_FW_ERROR, &priv->status)) {
+		IWL_DEBUG_INFO("Command %s failed: FW Error\n",
+			       get_cmd_string(cmd->id));
+		ret = -EIO;
+		goto fail;
+	}
+	if ((cmd->meta.flags & CMD_WANT_SKB) && !cmd->meta.u.skb) {
+		IWL_ERROR("Error: Response NULL in '%s'\n",
+			  get_cmd_string(cmd->id));
+		ret = -EIO;
+		goto out;
+	}
+
+	ret = 0;
+	goto out;
+
+cancel:
+	if (cmd->meta.flags & CMD_WANT_SKB) {
+		struct iwl_cmd *qcmd;
+
+		/* Cancel the CMD_WANT_SKB flag for the cmd in the
+		 * TX cmd queue. Otherwise in case the cmd comes
+		 * in later, it will possibly set an invalid
+		 * address (cmd->meta.source). */
+		qcmd = &priv->txq[IWL_CMD_QUEUE_NUM].cmd[cmd_idx];
+		qcmd->meta.flags &= ~CMD_WANT_SKB;
+	}
+fail:
+	if (cmd->meta.u.skb) {
+		dev_kfree_skb_any(cmd->meta.u.skb);
+		cmd->meta.u.skb = NULL;
+	}
+out:
+	atomic_set(&entry, 0);
+	return ret;
+}
+
+int iwl_send_cmd(struct iwl_priv *priv, struct iwl_host_cmd *cmd)
+{
+	/* A command can not be asynchronous AND expect an SKB to be set. */
+	BUG_ON((cmd->meta.flags & CMD_ASYNC) &&
+	       (cmd->meta.flags & CMD_WANT_SKB));
+
+	if (cmd->meta.flags & CMD_ASYNC)
+		return iwl_send_cmd_async(priv, cmd);
+
+	return iwl_send_cmd_sync(priv, cmd);
+}
+
+int iwl_send_cmd_pdu(struct iwl_priv *priv, u8 id, u16 len, const void *data)
+{
+	struct iwl_host_cmd cmd = {
+		.id = id,
+		.len = len,
+		.data = data,
+	};
+
+	return iwl_send_cmd_sync(priv, &cmd);
+}
+
+static int __must_check iwl_send_cmd_u32(struct iwl_priv *priv, u8 id, u32 val)
+{
+	struct iwl_host_cmd cmd = {
+		.id = id,
+		.len = sizeof(val),
+		.data = &val,
+	};
+
+	return iwl_send_cmd_sync(priv, &cmd);
+}
+
+int iwl_send_statistics_request(struct iwl_priv *priv)
+{
+	return iwl_send_cmd_u32(priv, REPLY_STATISTICS_CMD, 0);
+}
+
+/**
+ * iwl_set_rxon_channel - Set the phymode and channel values in staging RXON
+ * @phymode: MODE_IEEE80211A sets to 5.2GHz; all else set to 2.4GHz
+ * @channel: Any channel valid for the requested phymode
+
+ * In addition to setting the staging RXON, priv->phymode is also set.
+ *
+ * NOTE:  Does not commit to the hardware; it sets appropriate bit fields
+ * in the staging RXON flag structure based on the phymode
+ */
+static int iwl_set_rxon_channel(struct iwl_priv *priv, u8 phymode, u16 channel)
+{
+	if (!iwl_get_channel_info(priv, phymode, channel)) {
+		IWL_DEBUG_INFO("Could not set channel to %d [%d]\n",
+			       channel, phymode);
+		return -EINVAL;
+	}
+
+	if ((le16_to_cpu(priv->staging_rxon.channel) == channel) &&
+	    (priv->phymode == phymode))
+		return 0;
+
+	priv->staging_rxon.channel = cpu_to_le16(channel);
+	if (phymode == MODE_IEEE80211A)
+		priv->staging_rxon.flags &= ~RXON_FLG_BAND_24G_MSK;
+	else
+		priv->staging_rxon.flags |= RXON_FLG_BAND_24G_MSK;
+
+	priv->phymode = phymode;
+
+	IWL_DEBUG_INFO("Staging channel set to %d [%d]\n", channel, phymode);
+
+	return 0;
+}
+
+/**
+ * iwl_check_rxon_cmd - validate RXON structure is valid
+ *
+ * NOTE:  This is really only useful during development and can eventually
+ * be #ifdef'd out once the driver is stable and folks aren't actively
+ * making changes
+ */
+static int iwl_check_rxon_cmd(struct iwl_rxon_cmd *rxon)
+{
+	int error = 0;
+	int counter = 1;
+
+	if (rxon->flags & RXON_FLG_BAND_24G_MSK) {
+		error |= le32_to_cpu(rxon->flags &
+				(RXON_FLG_TGJ_NARROW_BAND_MSK |
+				 RXON_FLG_RADAR_DETECT_MSK));
+		if (error)
+			IWL_WARNING("check 24G fields %d | %d\n",
+				    counter++, error);
+	} else {
+		error |= (rxon->flags & RXON_FLG_SHORT_SLOT_MSK) ?
+				0 : le32_to_cpu(RXON_FLG_SHORT_SLOT_MSK);
+		if (error)
+			IWL_WARNING("check 52 fields %d | %d\n",
+				    counter++, error);
+		error |= le32_to_cpu(rxon->flags & RXON_FLG_CCK_MSK);
+		if (error)
+			IWL_WARNING("check 52 CCK %d | %d\n",
+				    counter++, error);
+	}
+	error |= (rxon->node_addr[0] | rxon->bssid_addr[0]) & 0x1;
+	if (error)
+		IWL_WARNING("check mac addr %d | %d\n", counter++, error);
+
+	/* make sure basic rates 6Mbps and 1Mbps are supported */
+	error |= (((rxon->ofdm_basic_rates & IWL_RATE_6M_MASK) == 0) &&
+		  ((rxon->cck_basic_rates & IWL_RATE_1M_MASK) == 0));
+	if (error)
+		IWL_WARNING("check basic rate %d | %d\n", counter++, error);
+
+	error |= (le16_to_cpu(rxon->assoc_id) > 2007);
+	if (error)
+		IWL_WARNING("check assoc id %d | %d\n", counter++, error);
+
+	error |= ((rxon->flags & (RXON_FLG_CCK_MSK | RXON_FLG_SHORT_SLOT_MSK))
+			== (RXON_FLG_CCK_MSK | RXON_FLG_SHORT_SLOT_MSK));
+	if (error)
+		IWL_WARNING("check CCK and short slot %d | %d\n",
+			    counter++, error);
+
+	error |= ((rxon->flags & (RXON_FLG_CCK_MSK | RXON_FLG_AUTO_DETECT_MSK))
+			== (RXON_FLG_CCK_MSK | RXON_FLG_AUTO_DETECT_MSK));
+	if (error)
+		IWL_WARNING("check CCK & auto detect %d | %d\n",
+			    counter++, error);
+
+	error |= ((rxon->flags & (RXON_FLG_AUTO_DETECT_MSK |
+			RXON_FLG_TGG_PROTECT_MSK)) == RXON_FLG_TGG_PROTECT_MSK);
+	if (error)
+		IWL_WARNING("check TGG and auto detect %d | %d\n",
+			    counter++, error);
+
+	if ((rxon->flags & RXON_FLG_DIS_DIV_MSK))
+		error |= ((rxon->flags & (RXON_FLG_ANT_B_MSK |
+				RXON_FLG_ANT_A_MSK)) == 0);
+	if (error)
+		IWL_WARNING("check antenna %d %d\n", counter++, error);
+
+	if (error)
+		IWL_WARNING("Tuning to channel %d\n",
+			    le16_to_cpu(rxon->channel));
+
+	if (error) {
+		IWL_ERROR("Not a valid iwl_rxon_assoc_cmd field values\n");
+		return -1;
+	}
+	return 0;
+}
+
+/**
+ * iwl_full_rxon_required - determine if RXON_ASSOC can be used in RXON commit
+ * @priv: staging_rxon is comapred to active_rxon
+ *
+ * If the RXON structure is changing sufficient to require a new
+ * tune or to clear and reset the RXON_FILTER_ASSOC_MSK then return 1
+ * to indicate a new tune is required.
+ */
+static int iwl_full_rxon_required(struct iwl_priv *priv)
+{
+
+	/* These items are only settable from the full RXON command */
+	if (!(priv->active_rxon.filter_flags & RXON_FILTER_ASSOC_MSK) ||
+	    compare_ether_addr(priv->staging_rxon.bssid_addr,
+			       priv->active_rxon.bssid_addr) ||
+	    compare_ether_addr(priv->staging_rxon.node_addr,
+			       priv->active_rxon.node_addr) ||
+	    compare_ether_addr(priv->staging_rxon.wlap_bssid_addr,
+			       priv->active_rxon.wlap_bssid_addr) ||
+	    (priv->staging_rxon.dev_type != priv->active_rxon.dev_type) ||
+	    (priv->staging_rxon.channel != priv->active_rxon.channel) ||
+	    (priv->staging_rxon.air_propagation !=
+	     priv->active_rxon.air_propagation) ||
+	    (priv->staging_rxon.assoc_id != priv->active_rxon.assoc_id))
+		return 1;
+
+	/* flags, filter_flags, ofdm_basic_rates, and cck_basic_rates can
+	 * be updated with the RXON_ASSOC command -- however only some
+	 * flag transitions are allowed using RXON_ASSOC */
+
+	/* Check if we are not switching bands */
+	if ((priv->staging_rxon.flags & RXON_FLG_BAND_24G_MSK) !=
+	    (priv->active_rxon.flags & RXON_FLG_BAND_24G_MSK))
+		return 1;
+
+	/* Check if we are switching association toggle */
+	if ((priv->staging_rxon.filter_flags & RXON_FILTER_ASSOC_MSK) !=
+		(priv->active_rxon.filter_flags & RXON_FILTER_ASSOC_MSK))
+		return 1;
+
+	return 0;
+}
+
+static int iwl_send_rxon_assoc(struct iwl_priv *priv)
+{
+	int rc = 0;
+	struct iwl_rx_packet *res = NULL;
+	struct iwl_rxon_assoc_cmd rxon_assoc;
+	struct iwl_host_cmd cmd = {
+		.id = REPLY_RXON_ASSOC,
+		.len = sizeof(rxon_assoc),
+		.meta.flags = CMD_WANT_SKB,
+		.data = &rxon_assoc,
+	};
+	const struct iwl_rxon_cmd *rxon1 = &priv->staging_rxon;
+	const struct iwl_rxon_cmd *rxon2 = &priv->active_rxon;
+
+	if ((rxon1->flags == rxon2->flags) &&
+	    (rxon1->filter_flags == rxon2->filter_flags) &&
+	    (rxon1->cck_basic_rates == rxon2->cck_basic_rates) &&
+	    (rxon1->ofdm_basic_rates == rxon2->ofdm_basic_rates)) {
+		IWL_DEBUG_INFO("Using current RXON_ASSOC.  Not resending.\n");
+		return 0;
+	}
+
+	rxon_assoc.flags = priv->staging_rxon.flags;
+	rxon_assoc.filter_flags = priv->staging_rxon.filter_flags;
+	rxon_assoc.ofdm_basic_rates = priv->staging_rxon.ofdm_basic_rates;
+	rxon_assoc.cck_basic_rates = priv->staging_rxon.cck_basic_rates;
+	rxon_assoc.reserved = 0;
+
+	rc = iwl_send_cmd_sync(priv, &cmd);
+	if (rc)
+		return rc;
+
+	res = (struct iwl_rx_packet *)cmd.meta.u.skb->data;
+	if (res->hdr.flags & IWL_CMD_FAILED_MSK) {
+		IWL_ERROR("Bad return from REPLY_RXON_ASSOC command\n");
+		rc = -EIO;
+	}
+
+	priv->alloc_rxb_skb--;
+	dev_kfree_skb_any(cmd.meta.u.skb);
+
+	return rc;
+}
+
+/**
+ * iwl_commit_rxon - commit staging_rxon to hardware
+ *
+ * The RXON command in staging_rxon is commited to the hardware and
+ * the active_rxon structure is updated with the new data.  This
+ * function correctly transitions out of the RXON_ASSOC_MSK state if
+ * a HW tune is required based on the RXON structure changes.
+ */
+static int iwl_commit_rxon(struct iwl_priv *priv)
+{
+	/* cast away the const for active_rxon in this function */
+	struct iwl_rxon_cmd *active_rxon = (void *)&priv->active_rxon;
+	int rc = 0;
+	DECLARE_MAC_BUF(mac);
+
+	if (!iwl_is_alive(priv))
+		return -1;
+
+	/* always get timestamp with Rx frame */
+	priv->staging_rxon.flags |= RXON_FLG_TSF2HOST_MSK;
+
+	/* select antenna */
+	priv->staging_rxon.flags &=
+	    ~(RXON_FLG_DIS_DIV_MSK | RXON_FLG_ANT_SEL_MSK);
+	priv->staging_rxon.flags |= iwl3945_get_antenna_flags(priv);
+
+	rc = iwl_check_rxon_cmd(&priv->staging_rxon);
+	if (rc) {
+		IWL_ERROR("Invalid RXON configuration.  Not committing.\n");
+		return -EINVAL;
+	}
+
+	/* If we don't need to send a full RXON, we can use
+	 * iwl_rxon_assoc_cmd which is used to reconfigure filter
+	 * and other flags for the current radio configuration. */
+	if (!iwl_full_rxon_required(priv)) {
+		rc = iwl_send_rxon_assoc(priv);
+		if (rc) {
+			IWL_ERROR("Error setting RXON_ASSOC "
+				  "configuration (%d).\n", rc);
+			return rc;
+		}
+
+		memcpy(active_rxon, &priv->staging_rxon, sizeof(*active_rxon));
+
+		return 0;
+	}
+
+	/* If we are currently associated and the new config requires
+	 * an RXON_ASSOC and the new config wants the associated mask enabled,
+	 * we must clear the associated from the active configuration
+	 * before we apply the new config */
+	if (iwl_is_associated(priv) &&
+	    (priv->staging_rxon.filter_flags & RXON_FILTER_ASSOC_MSK)) {
+		IWL_DEBUG_INFO("Toggling associated bit on current RXON\n");
+		active_rxon->filter_flags &= ~RXON_FILTER_ASSOC_MSK;
+
+		rc = iwl_send_cmd_pdu(priv, REPLY_RXON,
+				      sizeof(struct iwl_rxon_cmd),
+				      &priv->active_rxon);
+
+		/* If the mask clearing failed then we set
+		 * active_rxon back to what it was previously */
+		if (rc) {
+			active_rxon->filter_flags |= RXON_FILTER_ASSOC_MSK;
+			IWL_ERROR("Error clearing ASSOC_MSK on current "
+				  "configuration (%d).\n", rc);
+			return rc;
+		}
+	}
+
+	IWL_DEBUG_INFO("Sending RXON\n"
+		       "* with%s RXON_FILTER_ASSOC_MSK\n"
+		       "* channel = %d\n"
+		       "* bssid = %s\n",
+		       ((priv->staging_rxon.filter_flags &
+			 RXON_FILTER_ASSOC_MSK) ? "" : "out"),
+		       le16_to_cpu(priv->staging_rxon.channel),
+		       print_mac(mac, priv->staging_rxon.bssid_addr));
+
+	/* Apply the new configuration */
+	rc = iwl_send_cmd_pdu(priv, REPLY_RXON,
+			      sizeof(struct iwl_rxon_cmd), &priv->staging_rxon);
+	if (rc) {
+		IWL_ERROR("Error setting new configuration (%d).\n", rc);
+		return rc;
+	}
+
+	memcpy(active_rxon, &priv->staging_rxon, sizeof(*active_rxon));
+
+	iwl_clear_stations_table(priv);
+
+	/* If we issue a new RXON command which required a tune then we must
+	 * send a new TXPOWER command or we won't be able to Tx any frames */
+	rc = iwl_hw_reg_send_txpower(priv);
+	if (rc) {
+		IWL_ERROR("Error setting Tx power (%d).\n", rc);
+		return rc;
+	}
+
+	/* Add the broadcast address so we can send broadcast frames */
+	if (iwl_add_station(priv, BROADCAST_ADDR, 0, 0) ==
+	    IWL_INVALID_STATION) {
+		IWL_ERROR("Error adding BROADCAST address for transmit.\n");
+		return -EIO;
+	}
+
+	/* If we have set the ASSOC_MSK and we are in BSS mode then
+	 * add the IWL_AP_ID to the station rate table */
+	if (iwl_is_associated(priv) &&
+	    (priv->iw_mode == IEEE80211_IF_TYPE_STA))
+		if (iwl_add_station(priv, priv->active_rxon.bssid_addr, 1, 0)
+		    == IWL_INVALID_STATION) {
+			IWL_ERROR("Error adding AP address for transmit.\n");
+			return -EIO;
+		}
+
+	/* Init the hardware's rate fallback order based on the
+	 * phymode */
+	rc = iwl3945_init_hw_rate_table(priv);
+	if (rc) {
+		IWL_ERROR("Error setting HW rate table: %02X\n", rc);
+		return -EIO;
+	}
+
+	return 0;
+}
+
+static int iwl_send_bt_config(struct iwl_priv *priv)
+{
+	struct iwl_bt_cmd bt_cmd = {
+		.flags = 3,
+		.lead_time = 0xAA,
+		.max_kill = 1,
+		.kill_ack_mask = 0,
+		.kill_cts_mask = 0,
+	};
+
+	return iwl_send_cmd_pdu(priv, REPLY_BT_CONFIG,
+				sizeof(struct iwl_bt_cmd), &bt_cmd);
+}
+
+static int iwl_send_scan_abort(struct iwl_priv *priv)
+{
+	int rc = 0;
+	struct iwl_rx_packet *res;
+	struct iwl_host_cmd cmd = {
+		.id = REPLY_SCAN_ABORT_CMD,
+		.meta.flags = CMD_WANT_SKB,
+	};
+
+	/* If there isn't a scan actively going on in the hardware
+	 * then we are in between scan bands and not actually
+	 * actively scanning, so don't send the abort command */
+	if (!test_bit(STATUS_SCAN_HW, &priv->status)) {
+		clear_bit(STATUS_SCAN_ABORTING, &priv->status);
+		return 0;
+	}
+
+	rc = iwl_send_cmd_sync(priv, &cmd);
+	if (rc) {
+		clear_bit(STATUS_SCAN_ABORTING, &priv->status);
+		return rc;
+	}
+
+	res = (struct iwl_rx_packet *)cmd.meta.u.skb->data;
+	if (res->u.status != CAN_ABORT_STATUS) {
+		/* The scan abort will return 1 for success or
+		 * 2 for "failure".  A failure condition can be
+		 * due to simply not being in an active scan which
+		 * can occur if we send the scan abort before we
+		 * the microcode has notified us that a scan is
+		 * completed. */
+		IWL_DEBUG_INFO("SCAN_ABORT returned %d.\n", res->u.status);
+		clear_bit(STATUS_SCAN_ABORTING, &priv->status);
+		clear_bit(STATUS_SCAN_HW, &priv->status);
+	}
+
+	dev_kfree_skb_any(cmd.meta.u.skb);
+
+	return rc;
+}
+
+static int iwl_card_state_sync_callback(struct iwl_priv *priv,
+					struct iwl_cmd *cmd,
+					struct sk_buff *skb)
+{
+	return 1;
+}
+
+/*
+ * CARD_STATE_CMD
+ *
+ * Use: Sets the internal card state to enable, disable, or halt
+ *
+ * When in the 'enable' state the card operates as normal.
+ * When in the 'disable' state, the card enters into a low power mode.
+ * When in the 'halt' state, the card is shut down and must be fully
+ * restarted to come back on.
+ */
+static int iwl_send_card_state(struct iwl_priv *priv, u32 flags, u8 meta_flag)
+{
+	struct iwl_host_cmd cmd = {
+		.id = REPLY_CARD_STATE_CMD,
+		.len = sizeof(u32),
+		.data = &flags,
+		.meta.flags = meta_flag,
+	};
+
+	if (meta_flag & CMD_ASYNC)
+		cmd.meta.u.callback = iwl_card_state_sync_callback;
+
+	return iwl_send_cmd(priv, &cmd);
+}
+
+static int iwl_add_sta_sync_callback(struct iwl_priv *priv,
+				     struct iwl_cmd *cmd, struct sk_buff *skb)
+{
+	struct iwl_rx_packet *res = NULL;
+
+	if (!skb) {
+		IWL_ERROR("Error: Response NULL in REPLY_ADD_STA.\n");
+		return 1;
+	}
+
+	res = (struct iwl_rx_packet *)skb->data;
+	if (res->hdr.flags & IWL_CMD_FAILED_MSK) {
+		IWL_ERROR("Bad return from REPLY_ADD_STA (0x%08X)\n",
+			  res->hdr.flags);
+		return 1;
+	}
+
+	switch (res->u.add_sta.status) {
+	case ADD_STA_SUCCESS_MSK:
+		break;
+	default:
+		break;
+	}
+
+	/* We didn't cache the SKB; let the caller free it */
+	return 1;
+}
+
+int iwl_send_add_station(struct iwl_priv *priv,
+			 struct iwl_addsta_cmd *sta, u8 flags)
+{
+	struct iwl_rx_packet *res = NULL;
+	int rc = 0;
+	struct iwl_host_cmd cmd = {
+		.id = REPLY_ADD_STA,
+		.len = sizeof(struct iwl_addsta_cmd),
+		.meta.flags = flags,
+		.data = sta,
+	};
+
+	if (flags & CMD_ASYNC)
+		cmd.meta.u.callback = iwl_add_sta_sync_callback;
+	else
+		cmd.meta.flags |= CMD_WANT_SKB;
+
+	rc = iwl_send_cmd(priv, &cmd);
+
+	if (rc || (flags & CMD_ASYNC))
+		return rc;
+
+	res = (struct iwl_rx_packet *)cmd.meta.u.skb->data;
+	if (res->hdr.flags & IWL_CMD_FAILED_MSK) {
+		IWL_ERROR("Bad return from REPLY_ADD_STA (0x%08X)\n",
+			  res->hdr.flags);
+		rc = -EIO;
+	}
+
+	if (rc == 0) {
+		switch (res->u.add_sta.status) {
+		case ADD_STA_SUCCESS_MSK:
+			IWL_DEBUG_INFO("REPLY_ADD_STA PASSED\n");
+			break;
+		default:
+			rc = -EIO;
+			IWL_WARNING("REPLY_ADD_STA failed\n");
+			break;
+		}
+	}
+
+	priv->alloc_rxb_skb--;
+	dev_kfree_skb_any(cmd.meta.u.skb);
+
+	return rc;
+}
+
+static int iwl_update_sta_key_info(struct iwl_priv *priv,
+				   struct ieee80211_key_conf *keyconf,
+				   u8 sta_id)
+{
+	unsigned long flags;
+	__le16 key_flags = 0;
+
+	switch (keyconf->alg) {
+	case ALG_CCMP:
+		key_flags |= STA_KEY_FLG_CCMP;
+		key_flags |= cpu_to_le16(
+				keyconf->keyidx << STA_KEY_FLG_KEYID_POS);
+		key_flags &= ~STA_KEY_FLG_INVALID;
+		break;
+	case ALG_TKIP:
+	case ALG_WEP:
+		return -EINVAL;
+	default:
+		return -EINVAL;
+	}
+	spin_lock_irqsave(&priv->sta_lock, flags);
+	priv->stations[sta_id].keyinfo.alg = keyconf->alg;
+	priv->stations[sta_id].keyinfo.keylen = keyconf->keylen;
+	memcpy(priv->stations[sta_id].keyinfo.key, keyconf->key,
+	       keyconf->keylen);
+
+	memcpy(priv->stations[sta_id].sta.key.key, keyconf->key,
+	       keyconf->keylen);
+	priv->stations[sta_id].sta.key.key_flags = key_flags;
+	priv->stations[sta_id].sta.sta.modify_mask = STA_MODIFY_KEY_MASK;
+	priv->stations[sta_id].sta.mode = STA_CONTROL_MODIFY_MSK;
+
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+
+	IWL_DEBUG_INFO("hwcrypto: modify ucode station key info\n");
+	iwl_send_add_station(priv, &priv->stations[sta_id].sta, 0);
+	return 0;
+}
+
+static int iwl_clear_sta_key_info(struct iwl_priv *priv, u8 sta_id)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->sta_lock, flags);
+	memset(&priv->stations[sta_id].keyinfo, 0, sizeof(struct iwl_hw_key));
+	memset(&priv->stations[sta_id].sta.key, 0, sizeof(struct iwl_keyinfo));
+	priv->stations[sta_id].sta.key.key_flags = STA_KEY_FLG_NO_ENC;
+	priv->stations[sta_id].sta.sta.modify_mask = STA_MODIFY_KEY_MASK;
+	priv->stations[sta_id].sta.mode = STA_CONTROL_MODIFY_MSK;
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+
+	IWL_DEBUG_INFO("hwcrypto: clear ucode station key info\n");
+	iwl_send_add_station(priv, &priv->stations[sta_id].sta, 0);
+	return 0;
+}
+
+static void iwl_clear_free_frames(struct iwl_priv *priv)
+{
+	struct list_head *element;
+
+	IWL_DEBUG_INFO("%d frames on pre-allocated heap on clear.\n",
+		       priv->frames_count);
+
+	while (!list_empty(&priv->free_frames)) {
+		element = priv->free_frames.next;
+		list_del(element);
+		kfree(list_entry(element, struct iwl_frame, list));
+		priv->frames_count--;
+	}
+
+	if (priv->frames_count) {
+		IWL_WARNING("%d frames still in use.  Did we lose one?\n",
+			    priv->frames_count);
+		priv->frames_count = 0;
+	}
+}
+
+static struct iwl_frame *iwl_get_free_frame(struct iwl_priv *priv)
+{
+	struct iwl_frame *frame;
+	struct list_head *element;
+	if (list_empty(&priv->free_frames)) {
+		frame = kzalloc(sizeof(*frame), GFP_KERNEL);
+		if (!frame) {
+			IWL_ERROR("Could not allocate frame!\n");
+			return NULL;
+		}
+
+		priv->frames_count++;
+		return frame;
+	}
+
+	element = priv->free_frames.next;
+	list_del(element);
+	return list_entry(element, struct iwl_frame, list);
+}
+
+static void iwl_free_frame(struct iwl_priv *priv, struct iwl_frame *frame)
+{
+	memset(frame, 0, sizeof(*frame));
+	list_add(&frame->list, &priv->free_frames);
+}
+
+unsigned int iwl_fill_beacon_frame(struct iwl_priv *priv,
+				struct ieee80211_hdr *hdr,
+				const u8 *dest, int left)
+{
+
+	if (!iwl_is_associated(priv) || !priv->ibss_beacon ||
+	    ((priv->iw_mode != IEEE80211_IF_TYPE_IBSS) &&
+	     (priv->iw_mode != IEEE80211_IF_TYPE_AP)))
+		return 0;
+
+	if (priv->ibss_beacon->len > left)
+		return 0;
+
+	memcpy(hdr, priv->ibss_beacon->data, priv->ibss_beacon->len);
+
+	return priv->ibss_beacon->len;
+}
+
+static int iwl_rate_index_from_plcp(int plcp)
+{
+	int i = 0;
+
+	for (i = 0; i < IWL_RATE_COUNT; i++)
+		if (iwl_rates[i].plcp == plcp)
+			return i;
+	return -1;
+}
+
+static u8 iwl_rate_get_lowest_plcp(int rate_mask)
+{
+	u8 i;
+
+	for (i = IWL_RATE_1M_INDEX; i != IWL_RATE_INVALID;
+	     i = iwl_rates[i].next_ieee) {
+		if (rate_mask & (1 << i))
+			return iwl_rates[i].plcp;
+	}
+
+	return IWL_RATE_INVALID;
+}
+
+static int iwl_send_beacon_cmd(struct iwl_priv *priv)
+{
+	struct iwl_frame *frame;
+	unsigned int frame_size;
+	int rc;
+	u8 rate;
+
+	frame = iwl_get_free_frame(priv);
+
+	if (!frame) {
+		IWL_ERROR("Could not obtain free frame buffer for beacon "
+			  "command.\n");
+		return -ENOMEM;
+	}
+
+	if (!(priv->staging_rxon.flags & RXON_FLG_BAND_24G_MSK)) {
+		rate = iwl_rate_get_lowest_plcp(priv->active_rate_basic &
+						0xFF0);
+		if (rate == IWL_INVALID_RATE)
+			rate = IWL_RATE_6M_PLCP;
+	} else {
+		rate = iwl_rate_get_lowest_plcp(priv->active_rate_basic & 0xF);
+		if (rate == IWL_INVALID_RATE)
+			rate = IWL_RATE_1M_PLCP;
+	}
+
+	frame_size = iwl_hw_get_beacon_cmd(priv, frame, rate);
+
+	rc = iwl_send_cmd_pdu(priv, REPLY_TX_BEACON, frame_size,
+			      &frame->u.cmd[0]);
+
+	iwl_free_frame(priv, frame);
+
+	return rc;
+}
+
+/******************************************************************************
+ *
+ * EEPROM related functions
+ *
+ ******************************************************************************/
+
+static void get_eeprom_mac(struct iwl_priv *priv, u8 *mac)
+{
+	memcpy(mac, priv->eeprom.mac_address, 6);
+}
+
+/**
+ * iwl_eeprom_init - read EEPROM contents
+ *
+ * Load the EEPROM from adapter into priv->eeprom
+ *
+ * NOTE:  This routine uses the non-debug IO access functions.
+ */
+int iwl_eeprom_init(struct iwl_priv *priv)
+{
+	u16 *e = (u16 *)&priv->eeprom;
+	u32 gp = iwl_read32(priv, CSR_EEPROM_GP);
+	u32 r;
+	int sz = sizeof(priv->eeprom);
+	int rc;
+	int i;
+	u16 addr;
+
+	/* The EEPROM structure has several padding buffers within it
+	 * and when adding new EEPROM maps is subject to programmer errors
+	 * which may be very difficult to identify without explicitly
+	 * checking the resulting size of the eeprom map. */
+	BUILD_BUG_ON(sizeof(priv->eeprom) != IWL_EEPROM_IMAGE_SIZE);
+
+	if ((gp & CSR_EEPROM_GP_VALID_MSK) == CSR_EEPROM_GP_BAD_SIGNATURE) {
+		IWL_ERROR("EEPROM not found, EEPROM_GP=0x%08x", gp);
+		return -ENOENT;
+	}
+
+	rc = iwl_eeprom_aqcuire_semaphore(priv);
+	if (rc < 0) {
+		IWL_ERROR("Failed to aqcuire EEPROM semaphore.\n");
+		return -ENOENT;
+	}
+
+	/* eeprom is an array of 16bit values */
+	for (addr = 0; addr < sz; addr += sizeof(u16)) {
+		_iwl_write32(priv, CSR_EEPROM_REG, addr << 1);
+		_iwl_clear_bit(priv, CSR_EEPROM_REG, CSR_EEPROM_REG_BIT_CMD);
+
+		for (i = 0; i < IWL_EEPROM_ACCESS_TIMEOUT;
+					i += IWL_EEPROM_ACCESS_DELAY) {
+			r = _iwl_read_restricted(priv, CSR_EEPROM_REG);
+			if (r & CSR_EEPROM_REG_READ_VALID_MSK)
+				break;
+			udelay(IWL_EEPROM_ACCESS_DELAY);
+		}
+
+		if (!(r & CSR_EEPROM_REG_READ_VALID_MSK)) {
+			IWL_ERROR("Time out reading EEPROM[%d]", addr);
+			return -ETIMEDOUT;
+		}
+		e[addr / 2] = le16_to_cpu(r >> 16);
+	}
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ * Misc. internal state and helper functions
+ *
+ ******************************************************************************/
+#ifdef CONFIG_IWLWIFI_DEBUG
+
+/**
+ * iwl_report_frame - dump frame to syslog during debug sessions
+ *
+ * hack this function to show different aspects of received frames,
+ * including selective frame dumps.
+ * group100 parameter selects whether to show 1 out of 100 good frames.
+ *
+ * TODO:  ieee80211_hdr stuff is common to 3945 and 4965, so frame type
+ *        info output is okay, but some of this stuff (e.g. iwl_rx_frame_stats)
+ *        is 3945-specific and gives bad output for 4965.  Need to split the
+ *        functionality, keep common stuff here.
+ */
+void iwl_report_frame(struct iwl_priv *priv,
+		      struct iwl_rx_packet *pkt,
+		      struct ieee80211_hdr *header, int group100)
+{
+	u32 to_us;
+	u32 print_summary = 0;
+	u32 print_dump = 0;	/* set to 1 to dump all frames' contents */
+	u32 hundred = 0;
+	u32 dataframe = 0;
+	u16 fc;
+	u16 seq_ctl;
+	u16 channel;
+	u16 phy_flags;
+	int rate_sym;
+	u16 length;
+	u16 status;
+	u16 bcn_tmr;
+	u32 tsf_low;
+	u64 tsf;
+	u8 rssi;
+	u8 agc;
+	u16 sig_avg;
+	u16 noise_diff;
+	struct iwl_rx_frame_stats *rx_stats = IWL_RX_STATS(pkt);
+	struct iwl_rx_frame_hdr *rx_hdr = IWL_RX_HDR(pkt);
+	struct iwl_rx_frame_end *rx_end = IWL_RX_END(pkt);
+	u8 *data = IWL_RX_DATA(pkt);
+
+	/* MAC header */
+	fc = le16_to_cpu(header->frame_control);
+	seq_ctl = le16_to_cpu(header->seq_ctrl);
+
+	/* metadata */
+	channel = le16_to_cpu(rx_hdr->channel);
+	phy_flags = le16_to_cpu(rx_hdr->phy_flags);
+	rate_sym = rx_hdr->rate;
+	length = le16_to_cpu(rx_hdr->len);
+
+	/* end-of-frame status and timestamp */
+	status = le32_to_cpu(rx_end->status);
+	bcn_tmr = le32_to_cpu(rx_end->beacon_timestamp);
+	tsf_low = le64_to_cpu(rx_end->timestamp) & 0x0ffffffff;
+	tsf = le64_to_cpu(rx_end->timestamp);
+
+	/* signal statistics */
+	rssi = rx_stats->rssi;
+	agc = rx_stats->agc;
+	sig_avg = le16_to_cpu(rx_stats->sig_avg);
+	noise_diff = le16_to_cpu(rx_stats->noise_diff);
+
+	to_us = !compare_ether_addr(header->addr1, priv->mac_addr);
+
+	/* if data frame is to us and all is good,
+	 *   (optionally) print summary for only 1 out of every 100 */
+	if (to_us && (fc & ~IEEE80211_FCTL_PROTECTED) ==
+	    (IEEE80211_FCTL_FROMDS | IEEE80211_FTYPE_DATA)) {
+		dataframe = 1;
+		if (!group100)
+			print_summary = 1;	/* print each frame */
+		else if (priv->framecnt_to_us < 100) {
+			priv->framecnt_to_us++;
+			print_summary = 0;
+		} else {
+			priv->framecnt_to_us = 0;
+			print_summary = 1;
+			hundred = 1;
+		}
+	} else {
+		/* print summary for all other frames */
+		print_summary = 1;
+	}
+
+	if (print_summary) {
+		char *title;
+		u32 rate;
+
+		if (hundred)
+			title = "100Frames";
+		else if (fc & IEEE80211_FCTL_RETRY)
+			title = "Retry";
+		else if (ieee80211_is_assoc_response(fc))
+			title = "AscRsp";
+		else if (ieee80211_is_reassoc_response(fc))
+			title = "RasRsp";
+		else if (ieee80211_is_probe_response(fc)) {
+			title = "PrbRsp";
+			print_dump = 1;	/* dump frame contents */
+		} else if (ieee80211_is_beacon(fc)) {
+			title = "Beacon";
+			print_dump = 1;	/* dump frame contents */
+		} else if (ieee80211_is_atim(fc))
+			title = "ATIM";
+		else if (ieee80211_is_auth(fc))
+			title = "Auth";
+		else if (ieee80211_is_deauth(fc))
+			title = "DeAuth";
+		else if (ieee80211_is_disassoc(fc))
+			title = "DisAssoc";
+		else
+			title = "Frame";
+
+		rate = iwl_rate_index_from_plcp(rate_sym);
+		if (rate == -1)
+			rate = 0;
+		else
+			rate = iwl_rates[rate].ieee / 2;
+
+		/* print frame summary.
+		 * MAC addresses show just the last byte (for brevity),
+		 *    but you can hack it to show more, if you'd like to. */
+		if (dataframe)
+			IWL_DEBUG_RX("%s: mhd=0x%04x, dst=0x%02x, "
+				     "len=%u, rssi=%d, chnl=%d, rate=%u, \n",
+				     title, fc, header->addr1[5],
+				     length, rssi, channel, rate);
+		else {
+			/* src/dst addresses assume managed mode */
+			IWL_DEBUG_RX("%s: 0x%04x, dst=0x%02x, "
+				     "src=0x%02x, rssi=%u, tim=%lu usec, "
+				     "phy=0x%02x, chnl=%d\n",
+				     title, fc, header->addr1[5],
+				     header->addr3[5], rssi,
+				     tsf_low - priv->scan_start_tsf,
+				     phy_flags, channel);
+		}
+	}
+	if (print_dump)
+		iwl_print_hex_dump(IWL_DL_RX, data, length);
+}
+#endif
+
+static void iwl_unset_hw_setting(struct iwl_priv *priv)
+{
+	if (priv->hw_setting.shared_virt)
+		pci_free_consistent(priv->pci_dev,
+				    sizeof(struct iwl_shared),
+				    priv->hw_setting.shared_virt,
+				    priv->hw_setting.shared_phys);
+}
+
+/**
+ * iwl_supported_rate_to_ie - fill in the supported rate in IE field
+ *
+ * return : set the bit for each supported rate insert in ie
+ */
+static u16 iwl_supported_rate_to_ie(u8 *ie, u16 supported_rate,
+				    u16 basic_rate, int max_count)
+{
+	u16 ret_rates = 0, bit;
+	int i;
+	u8 *rates;
+
+	rates = &(ie[1]);
+
+	for (bit = 1, i = 0; i < IWL_RATE_COUNT; i++, bit <<= 1) {
+		if (bit & supported_rate) {
+			ret_rates |= bit;
+			rates[*ie] = iwl_rates[i].ieee |
+			    ((bit & basic_rate) ? 0x80 : 0x00);
+			*ie = *ie + 1;
+			if (*ie >= max_count)
+				break;
+		}
+	}
+
+	return ret_rates;
+}
+
+/**
+ * iwl_fill_probe_req - fill in all required fields and IE for probe request
+ */
+static u16 iwl_fill_probe_req(struct iwl_priv *priv,
+			      struct ieee80211_mgmt *frame,
+			      int left, int is_direct)
+{
+	int len = 0;
+	u8 *pos = NULL;
+	u16 ret_rates;
+
+	/* Make sure there is enough space for the probe request,
+	 * two mandatory IEs and the data */
+	left -= 24;
+	if (left < 0)
+		return 0;
+	len += 24;
+
+	frame->frame_control = cpu_to_le16(IEEE80211_STYPE_PROBE_REQ);
+	memcpy(frame->da, BROADCAST_ADDR, ETH_ALEN);
+	memcpy(frame->sa, priv->mac_addr, ETH_ALEN);
+	memcpy(frame->bssid, BROADCAST_ADDR, ETH_ALEN);
+	frame->seq_ctrl = 0;
+
+	/* fill in our indirect SSID IE */
+	/* ...next IE... */
+
+	left -= 2;
+	if (left < 0)
+		return 0;
+	len += 2;
+	pos = &(frame->u.probe_req.variable[0]);
+	*pos++ = WLAN_EID_SSID;
+	*pos++ = 0;
+
+	/* fill in our direct SSID IE... */
+	if (is_direct) {
+		/* ...next IE... */
+		left -= 2 + priv->essid_len;
+		if (left < 0)
+			return 0;
+		/* ... fill it in... */
+		*pos++ = WLAN_EID_SSID;
+		*pos++ = priv->essid_len;
+		memcpy(pos, priv->essid, priv->essid_len);
+		pos += priv->essid_len;
+		len += 2 + priv->essid_len;
+	}
+
+	/* fill in supported rate */
+	/* ...next IE... */
+	left -= 2;
+	if (left < 0)
+		return 0;
+	/* ... fill it in... */
+	*pos++ = WLAN_EID_SUPP_RATES;
+	*pos = 0;
+	ret_rates = priv->active_rate = priv->rates_mask;
+	priv->active_rate_basic = priv->rates_mask & IWL_BASIC_RATES_MASK;
+
+	iwl_supported_rate_to_ie(pos, priv->active_rate,
+				 priv->active_rate_basic, left);
+	len += 2 + *pos;
+	pos += (*pos) + 1;
+	ret_rates = ~ret_rates & priv->active_rate;
+
+	if (ret_rates == 0)
+		goto fill_end;
+
+	/* fill in supported extended rate */
+	/* ...next IE... */
+	left -= 2;
+	if (left < 0)
+		return 0;
+	/* ... fill it in... */
+	*pos++ = WLAN_EID_EXT_SUPP_RATES;
+	*pos = 0;
+	iwl_supported_rate_to_ie(pos, ret_rates, priv->active_rate_basic, left);
+	if (*pos > 0)
+		len += 2 + *pos;
+
+ fill_end:
+	return (u16)len;
+}
+
+/*
+ * QoS  support
+*/
+#ifdef CONFIG_IWLWIFI_QOS
+static int iwl_send_qos_params_command(struct iwl_priv *priv,
+				       struct iwl_qosparam_cmd *qos)
+{
+
+	return iwl_send_cmd_pdu(priv, REPLY_QOS_PARAM,
+				sizeof(struct iwl_qosparam_cmd), qos);
+}
+
+static void iwl_reset_qos(struct iwl_priv *priv)
+{
+	u16 cw_min = 15;
+	u16 cw_max = 1023;
+	u8 aifs = 2;
+	u8 is_legacy = 0;
+	unsigned long flags;
+	int i;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	priv->qos_data.qos_active = 0;
+
+	if (priv->iw_mode == IEEE80211_IF_TYPE_IBSS) {
+		if (priv->qos_data.qos_enable)
+			priv->qos_data.qos_active = 1;
+		if (!(priv->active_rate & 0xfff0)) {
+			cw_min = 31;
+			is_legacy = 1;
+		}
+	} else if (priv->iw_mode == IEEE80211_IF_TYPE_AP) {
+		if (priv->qos_data.qos_enable)
+			priv->qos_data.qos_active = 1;
+	} else if (!(priv->staging_rxon.flags & RXON_FLG_SHORT_SLOT_MSK)) {
+		cw_min = 31;
+		is_legacy = 1;
+	}
+
+	if (priv->qos_data.qos_active)
+		aifs = 3;
+
+	priv->qos_data.def_qos_parm.ac[0].cw_min = cpu_to_le16(cw_min);
+	priv->qos_data.def_qos_parm.ac[0].cw_max = cpu_to_le16(cw_max);
+	priv->qos_data.def_qos_parm.ac[0].aifsn = aifs;
+	priv->qos_data.def_qos_parm.ac[0].edca_txop = 0;
+	priv->qos_data.def_qos_parm.ac[0].reserved1 = 0;
+
+	if (priv->qos_data.qos_active) {
+		i = 1;
+		priv->qos_data.def_qos_parm.ac[i].cw_min = cpu_to_le16(cw_min);
+		priv->qos_data.def_qos_parm.ac[i].cw_max = cpu_to_le16(cw_max);
+		priv->qos_data.def_qos_parm.ac[i].aifsn = 7;
+		priv->qos_data.def_qos_parm.ac[i].edca_txop = 0;
+		priv->qos_data.def_qos_parm.ac[i].reserved1 = 0;
+
+		i = 2;
+		priv->qos_data.def_qos_parm.ac[i].cw_min =
+			cpu_to_le16((cw_min + 1) / 2 - 1);
+		priv->qos_data.def_qos_parm.ac[i].cw_max =
+			cpu_to_le16(cw_max);
+		priv->qos_data.def_qos_parm.ac[i].aifsn = 2;
+		if (is_legacy)
+			priv->qos_data.def_qos_parm.ac[i].edca_txop =
+				cpu_to_le16(6016);
+		else
+			priv->qos_data.def_qos_parm.ac[i].edca_txop =
+				cpu_to_le16(3008);
+		priv->qos_data.def_qos_parm.ac[i].reserved1 = 0;
+
+		i = 3;
+		priv->qos_data.def_qos_parm.ac[i].cw_min =
+			cpu_to_le16((cw_min + 1) / 4 - 1);
+		priv->qos_data.def_qos_parm.ac[i].cw_max =
+			cpu_to_le16((cw_max + 1) / 2 - 1);
+		priv->qos_data.def_qos_parm.ac[i].aifsn = 2;
+		priv->qos_data.def_qos_parm.ac[i].reserved1 = 0;
+		if (is_legacy)
+			priv->qos_data.def_qos_parm.ac[i].edca_txop =
+				cpu_to_le16(3264);
+		else
+			priv->qos_data.def_qos_parm.ac[i].edca_txop =
+				cpu_to_le16(1504);
+	} else {
+		for (i = 1; i < 4; i++) {
+			priv->qos_data.def_qos_parm.ac[i].cw_min =
+				cpu_to_le16(cw_min);
+			priv->qos_data.def_qos_parm.ac[i].cw_max =
+				cpu_to_le16(cw_max);
+			priv->qos_data.def_qos_parm.ac[i].aifsn = aifs;
+			priv->qos_data.def_qos_parm.ac[i].edca_txop = 0;
+			priv->qos_data.def_qos_parm.ac[i].reserved1 = 0;
+		}
+	}
+	IWL_DEBUG_QOS("set QoS to default \n");
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+}
+
+static void iwl_activate_qos(struct iwl_priv *priv, u8 force)
+{
+	unsigned long flags;
+
+	if (priv == NULL)
+		return;
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+		return;
+
+	if (!priv->qos_data.qos_enable)
+		return;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	priv->qos_data.def_qos_parm.qos_flags = 0;
+
+	if (priv->qos_data.qos_cap.q_AP.queue_request &&
+	    !priv->qos_data.qos_cap.q_AP.txop_request)
+		priv->qos_data.def_qos_parm.qos_flags |=
+			QOS_PARAM_FLG_TXOP_TYPE_MSK;
+
+	if (priv->qos_data.qos_active)
+		priv->qos_data.def_qos_parm.qos_flags |=
+			QOS_PARAM_FLG_UPDATE_EDCA_MSK;
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	if (force || iwl_is_associated(priv)) {
+		IWL_DEBUG_QOS("send QoS cmd with Qos active %d \n",
+			      priv->qos_data.qos_active);
+
+		iwl_send_qos_params_command(priv,
+				&(priv->qos_data.def_qos_parm));
+	}
+}
+
+#endif /* CONFIG_IWLWIFI_QOS */
+/*
+ * Power management (not Tx power!) functions
+ */
+#define MSEC_TO_USEC 1024
+
+#define NOSLP __constant_cpu_to_le32(0)
+#define SLP IWL_POWER_DRIVER_ALLOW_SLEEP_MSK
+#define SLP_TIMEOUT(T) __constant_cpu_to_le32((T) * MSEC_TO_USEC)
+#define SLP_VEC(X0, X1, X2, X3, X4) {__constant_cpu_to_le32(X0), \
+				     __constant_cpu_to_le32(X1), \
+				     __constant_cpu_to_le32(X2), \
+				     __constant_cpu_to_le32(X3), \
+				     __constant_cpu_to_le32(X4)}
+
+
+/* default power management (not Tx power) table values */
+/* for tim  0-10 */
+static struct iwl_power_vec_entry range_0[IWL_POWER_AC] = {
+	{{NOSLP, SLP_TIMEOUT(0), SLP_TIMEOUT(0), SLP_VEC(0, 0, 0, 0, 0)}, 0},
+	{{SLP, SLP_TIMEOUT(200), SLP_TIMEOUT(500), SLP_VEC(1, 2, 3, 4, 4)}, 0},
+	{{SLP, SLP_TIMEOUT(200), SLP_TIMEOUT(300), SLP_VEC(2, 4, 6, 7, 7)}, 0},
+	{{SLP, SLP_TIMEOUT(50), SLP_TIMEOUT(100), SLP_VEC(2, 6, 9, 9, 10)}, 0},
+	{{SLP, SLP_TIMEOUT(50), SLP_TIMEOUT(25), SLP_VEC(2, 7, 9, 9, 10)}, 1},
+	{{SLP, SLP_TIMEOUT(25), SLP_TIMEOUT(25), SLP_VEC(4, 7, 10, 10, 10)}, 1}
+};
+
+/* for tim > 10 */
+static struct iwl_power_vec_entry range_1[IWL_POWER_AC] = {
+	{{NOSLP, SLP_TIMEOUT(0), SLP_TIMEOUT(0), SLP_VEC(0, 0, 0, 0, 0)}, 0},
+	{{SLP, SLP_TIMEOUT(200), SLP_TIMEOUT(500),
+		 SLP_VEC(1, 2, 3, 4, 0xFF)}, 0},
+	{{SLP, SLP_TIMEOUT(200), SLP_TIMEOUT(300),
+		 SLP_VEC(2, 4, 6, 7, 0xFF)}, 0},
+	{{SLP, SLP_TIMEOUT(50), SLP_TIMEOUT(100),
+		 SLP_VEC(2, 6, 9, 9, 0xFF)}, 0},
+	{{SLP, SLP_TIMEOUT(50), SLP_TIMEOUT(25), SLP_VEC(2, 7, 9, 9, 0xFF)}, 0},
+	{{SLP, SLP_TIMEOUT(25), SLP_TIMEOUT(25),
+		 SLP_VEC(4, 7, 10, 10, 0xFF)}, 0}
+};
+
+int iwl_power_init_handle(struct iwl_priv *priv)
+{
+	int rc = 0, i;
+	struct iwl_power_mgr *pow_data;
+	int size = sizeof(struct iwl_power_vec_entry) * IWL_POWER_AC;
+	u16 pci_pm;
+
+	IWL_DEBUG_POWER("Initialize power \n");
+
+	pow_data = &(priv->power_data);
+
+	memset(pow_data, 0, sizeof(*pow_data));
+
+	pow_data->active_index = IWL_POWER_RANGE_0;
+	pow_data->dtim_val = 0xffff;
+
+	memcpy(&pow_data->pwr_range_0[0], &range_0[0], size);
+	memcpy(&pow_data->pwr_range_1[0], &range_1[0], size);
+
+	rc = pci_read_config_word(priv->pci_dev, PCI_LINK_CTRL, &pci_pm);
+	if (rc != 0)
+		return 0;
+	else {
+		struct iwl_powertable_cmd *cmd;
+
+		IWL_DEBUG_POWER("adjust power command flags\n");
+
+		for (i = 0; i < IWL_POWER_AC; i++) {
+			cmd = &pow_data->pwr_range_0[i].cmd;
+
+			if (pci_pm & 0x1)
+				cmd->flags &= ~IWL_POWER_PCI_PM_MSK;
+			else
+				cmd->flags |= IWL_POWER_PCI_PM_MSK;
+		}
+	}
+	return rc;
+}
+
+static int iwl_update_power_cmd(struct iwl_priv *priv,
+				struct iwl_powertable_cmd *cmd, u32 mode)
+{
+	int rc = 0, i;
+	u8 skip;
+	u32 max_sleep = 0;
+	struct iwl_power_vec_entry *range;
+	u8 period = 0;
+	struct iwl_power_mgr *pow_data;
+
+	if (mode > IWL_POWER_INDEX_5) {
+		IWL_DEBUG_POWER("Error invalid power mode \n");
+		return -1;
+	}
+	pow_data = &(priv->power_data);
+
+	if (pow_data->active_index == IWL_POWER_RANGE_0)
+		range = &pow_data->pwr_range_0[0];
+	else
+		range = &pow_data->pwr_range_1[1];
+
+	memcpy(cmd, &range[mode].cmd, sizeof(struct iwl_powertable_cmd));
+
+#ifdef IWL_MAC80211_DISABLE
+	if (priv->assoc_network != NULL) {
+		unsigned long flags;
+
+		period = priv->assoc_network->tim.tim_period;
+	}
+#endif	/*IWL_MAC80211_DISABLE */
+	skip = range[mode].no_dtim;
+
+	if (period == 0) {
+		period = 1;
+		skip = 0;
+	}
+
+	if (skip == 0) {
+		max_sleep = period;
+		cmd->flags &= ~IWL_POWER_SLEEP_OVER_DTIM_MSK;
+	} else {
+		__le32 slp_itrvl = cmd->sleep_interval[IWL_POWER_VEC_SIZE - 1];
+		max_sleep = (le32_to_cpu(slp_itrvl) / period) * period;
+		cmd->flags |= IWL_POWER_SLEEP_OVER_DTIM_MSK;
+	}
+
+	for (i = 0; i < IWL_POWER_VEC_SIZE; i++) {
+		if (le32_to_cpu(cmd->sleep_interval[i]) > max_sleep)
+			cmd->sleep_interval[i] = cpu_to_le32(max_sleep);
+	}
+
+	IWL_DEBUG_POWER("Flags value = 0x%08X\n", cmd->flags);
+	IWL_DEBUG_POWER("Tx timeout = %u\n", le32_to_cpu(cmd->tx_data_timeout));
+	IWL_DEBUG_POWER("Rx timeout = %u\n", le32_to_cpu(cmd->rx_data_timeout));
+	IWL_DEBUG_POWER("Sleep interval vector = { %d , %d , %d , %d , %d }\n",
+			le32_to_cpu(cmd->sleep_interval[0]),
+			le32_to_cpu(cmd->sleep_interval[1]),
+			le32_to_cpu(cmd->sleep_interval[2]),
+			le32_to_cpu(cmd->sleep_interval[3]),
+			le32_to_cpu(cmd->sleep_interval[4]));
+
+	return rc;
+}
+
+static int iwl_send_power_mode(struct iwl_priv *priv, u32 mode)
+{
+	u32 final_mode = mode;
+	int rc;
+	struct iwl_powertable_cmd cmd;
+
+	/* If on battery, set to 3,
+	 * if plugged into AC power, set to CAM ("continuosly aware mode"),
+	 * else user level */
+	switch (mode) {
+	case IWL_POWER_BATTERY:
+		final_mode = IWL_POWER_INDEX_3;
+		break;
+	case IWL_POWER_AC:
+		final_mode = IWL_POWER_MODE_CAM;
+		break;
+	default:
+		final_mode = mode;
+		break;
+	}
+
+	iwl_update_power_cmd(priv, &cmd, final_mode);
+
+	rc = iwl_send_cmd_pdu(priv, POWER_TABLE_CMD, sizeof(cmd), &cmd);
+
+	if (final_mode == IWL_POWER_MODE_CAM)
+		clear_bit(STATUS_POWER_PMI, &priv->status);
+	else
+		set_bit(STATUS_POWER_PMI, &priv->status);
+
+	return rc;
+}
+
+int iwl_is_network_packet(struct iwl_priv *priv, struct ieee80211_hdr *header)
+{
+	/* Filter incoming packets to determine if they are targeted toward
+	 * this network, discarding packets coming from ourselves */
+	switch (priv->iw_mode) {
+	case IEEE80211_IF_TYPE_IBSS: /* Header: Dest. | Source    | BSSID */
+		/* packets from our adapter are dropped (echo) */
+		if (!compare_ether_addr(header->addr2, priv->mac_addr))
+			return 0;
+		/* {broad,multi}cast packets to our IBSS go through */
+		if (is_multicast_ether_addr(header->addr1))
+			return !compare_ether_addr(header->addr3, priv->bssid);
+		/* packets to our adapter go through */
+		return !compare_ether_addr(header->addr1, priv->mac_addr);
+	case IEEE80211_IF_TYPE_STA: /* Header: Dest. | AP{BSSID} | Source */
+		/* packets from our adapter are dropped (echo) */
+		if (!compare_ether_addr(header->addr3, priv->mac_addr))
+			return 0;
+		/* {broad,multi}cast packets to our BSS go through */
+		if (is_multicast_ether_addr(header->addr1))
+			return !compare_ether_addr(header->addr2, priv->bssid);
+		/* packets to our adapter go through */
+		return !compare_ether_addr(header->addr1, priv->mac_addr);
+	}
+
+	return 1;
+}
+
+#define TX_STATUS_ENTRY(x) case TX_STATUS_FAIL_ ## x: return #x
+
+const char *iwl_get_tx_fail_reason(u32 status)
+{
+	switch (status & TX_STATUS_MSK) {
+	case TX_STATUS_SUCCESS:
+		return "SUCCESS";
+		TX_STATUS_ENTRY(SHORT_LIMIT);
+		TX_STATUS_ENTRY(LONG_LIMIT);
+		TX_STATUS_ENTRY(FIFO_UNDERRUN);
+		TX_STATUS_ENTRY(MGMNT_ABORT);
+		TX_STATUS_ENTRY(NEXT_FRAG);
+		TX_STATUS_ENTRY(LIFE_EXPIRE);
+		TX_STATUS_ENTRY(DEST_PS);
+		TX_STATUS_ENTRY(ABORTED);
+		TX_STATUS_ENTRY(BT_RETRY);
+		TX_STATUS_ENTRY(STA_INVALID);
+		TX_STATUS_ENTRY(FRAG_DROPPED);
+		TX_STATUS_ENTRY(TID_DISABLE);
+		TX_STATUS_ENTRY(FRAME_FLUSHED);
+		TX_STATUS_ENTRY(INSUFFICIENT_CF_POLL);
+		TX_STATUS_ENTRY(TX_LOCKED);
+		TX_STATUS_ENTRY(NO_BEACON_ON_RADAR);
+	}
+
+	return "UNKNOWN";
+}
+
+/**
+ * iwl_scan_cancel - Cancel any currently executing HW scan
+ *
+ * NOTE: priv->mutex is not required before calling this function
+ */
+static int iwl_scan_cancel(struct iwl_priv *priv)
+{
+	if (!test_bit(STATUS_SCAN_HW, &priv->status)) {
+		clear_bit(STATUS_SCANNING, &priv->status);
+		return 0;
+	}
+
+	if (test_bit(STATUS_SCANNING, &priv->status)) {
+		if (!test_bit(STATUS_SCAN_ABORTING, &priv->status)) {
+			IWL_DEBUG_SCAN("Queuing scan abort.\n");
+			set_bit(STATUS_SCAN_ABORTING, &priv->status);
+			queue_work(priv->workqueue, &priv->abort_scan);
+
+		} else
+			IWL_DEBUG_SCAN("Scan abort already in progress.\n");
+
+		return test_bit(STATUS_SCANNING, &priv->status);
+	}
+
+	return 0;
+}
+
+/**
+ * iwl_scan_cancel_timeout - Cancel any currently executing HW scan
+ * @ms: amount of time to wait (in milliseconds) for scan to abort
+ *
+ * NOTE: priv->mutex must be held before calling this function
+ */
+static int iwl_scan_cancel_timeout(struct iwl_priv *priv, unsigned long ms)
+{
+	unsigned long now = jiffies;
+	int ret;
+
+	ret = iwl_scan_cancel(priv);
+	if (ret && ms) {
+		mutex_unlock(&priv->mutex);
+		while (!time_after(jiffies, now + msecs_to_jiffies(ms)) &&
+				test_bit(STATUS_SCANNING, &priv->status))
+			msleep(1);
+		mutex_lock(&priv->mutex);
+
+		return test_bit(STATUS_SCANNING, &priv->status);
+	}
+
+	return ret;
+}
+
+static void iwl_sequence_reset(struct iwl_priv *priv)
+{
+	/* Reset ieee stats */
+
+	/* We don't reset the net_device_stats (ieee->stats) on
+	 * re-association */
+
+	priv->last_seq_num = -1;
+	priv->last_frag_num = -1;
+	priv->last_packet_time = 0;
+
+	iwl_scan_cancel(priv);
+}
+
+#define MAX_UCODE_BEACON_INTERVAL	1024
+#define INTEL_CONN_LISTEN_INTERVAL	__constant_cpu_to_le16(0xA)
+
+static __le16 iwl_adjust_beacon_interval(u16 beacon_val)
+{
+	u16 new_val = 0;
+	u16 beacon_factor = 0;
+
+	beacon_factor =
+	    (beacon_val + MAX_UCODE_BEACON_INTERVAL)
+		/ MAX_UCODE_BEACON_INTERVAL;
+	new_val = beacon_val / beacon_factor;
+
+	return cpu_to_le16(new_val);
+}
+
+static void iwl_setup_rxon_timing(struct iwl_priv *priv)
+{
+	u64 interval_tm_unit;
+	u64 tsf, result;
+	unsigned long flags;
+	struct ieee80211_conf *conf = NULL;
+	u16 beacon_int = 0;
+
+	conf = ieee80211_get_hw_conf(priv->hw);
+
+	spin_lock_irqsave(&priv->lock, flags);
+	priv->rxon_timing.timestamp.dw[1] = cpu_to_le32(priv->timestamp1);
+	priv->rxon_timing.timestamp.dw[0] = cpu_to_le32(priv->timestamp0);
+
+	priv->rxon_timing.listen_interval = INTEL_CONN_LISTEN_INTERVAL;
+
+	tsf = priv->timestamp1;
+	tsf = ((tsf << 32) | priv->timestamp0);
+
+	beacon_int = priv->beacon_int;
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	if (priv->iw_mode == IEEE80211_IF_TYPE_STA) {
+		if (beacon_int == 0) {
+			priv->rxon_timing.beacon_interval = cpu_to_le16(100);
+			priv->rxon_timing.beacon_init_val = cpu_to_le32(102400);
+		} else {
+			priv->rxon_timing.beacon_interval =
+				cpu_to_le16(beacon_int);
+			priv->rxon_timing.beacon_interval =
+			    iwl_adjust_beacon_interval(
+				le16_to_cpu(priv->rxon_timing.beacon_interval));
+		}
+
+		priv->rxon_timing.atim_window = 0;
+	} else {
+		priv->rxon_timing.beacon_interval =
+			iwl_adjust_beacon_interval(conf->beacon_int);
+		/* TODO: we need to get atim_window from upper stack
+		 * for now we set to 0 */
+		priv->rxon_timing.atim_window = 0;
+	}
+
+	interval_tm_unit =
+		(le16_to_cpu(priv->rxon_timing.beacon_interval) * 1024);
+	result = do_div(tsf, interval_tm_unit);
+	priv->rxon_timing.beacon_init_val =
+	    cpu_to_le32((u32) ((u64) interval_tm_unit - result));
+
+	IWL_DEBUG_ASSOC
+	    ("beacon interval %d beacon timer %d beacon tim %d\n",
+		le16_to_cpu(priv->rxon_timing.beacon_interval),
+		le32_to_cpu(priv->rxon_timing.beacon_init_val),
+		le16_to_cpu(priv->rxon_timing.atim_window));
+}
+
+static int iwl_scan_initiate(struct iwl_priv *priv)
+{
+	if (priv->iw_mode == IEEE80211_IF_TYPE_AP) {
+		IWL_ERROR("APs don't scan.\n");
+		return 0;
+	}
+
+	if (!iwl_is_ready_rf(priv)) {
+		IWL_DEBUG_SCAN("Aborting scan due to not ready.\n");
+		return -EIO;
+	}
+
+	if (test_bit(STATUS_SCANNING, &priv->status)) {
+		IWL_DEBUG_SCAN("Scan already in progress.\n");
+		return -EAGAIN;
+	}
+
+	if (test_bit(STATUS_SCAN_ABORTING, &priv->status)) {
+		IWL_DEBUG_SCAN("Scan request while abort pending.  "
+			       "Queuing.\n");
+		return -EAGAIN;
+	}
+
+	IWL_DEBUG_INFO("Starting scan...\n");
+	priv->scan_bands = 2;
+	set_bit(STATUS_SCANNING, &priv->status);
+	priv->scan_start = jiffies;
+	priv->scan_pass_start = priv->scan_start;
+
+	queue_work(priv->workqueue, &priv->request_scan);
+
+	return 0;
+}
+
+static int iwl_set_rxon_hwcrypto(struct iwl_priv *priv, int hw_decrypt)
+{
+	struct iwl_rxon_cmd *rxon = &priv->staging_rxon;
+
+	if (hw_decrypt)
+		rxon->filter_flags &= ~RXON_FILTER_DIS_DECRYPT_MSK;
+	else
+		rxon->filter_flags |= RXON_FILTER_DIS_DECRYPT_MSK;
+
+	return 0;
+}
+
+static void iwl_set_flags_for_phymode(struct iwl_priv *priv, u8 phymode)
+{
+	if (phymode == MODE_IEEE80211A) {
+		priv->staging_rxon.flags &=
+		    ~(RXON_FLG_BAND_24G_MSK | RXON_FLG_AUTO_DETECT_MSK
+		      | RXON_FLG_CCK_MSK);
+		priv->staging_rxon.flags |= RXON_FLG_SHORT_SLOT_MSK;
+	} else {
+		/* Copied from iwl_bg_post_associate() */
+		if (priv->assoc_capability & WLAN_CAPABILITY_SHORT_SLOT_TIME)
+			priv->staging_rxon.flags |= RXON_FLG_SHORT_SLOT_MSK;
+		else
+			priv->staging_rxon.flags &= ~RXON_FLG_SHORT_SLOT_MSK;
+
+		if (priv->iw_mode == IEEE80211_IF_TYPE_IBSS)
+			priv->staging_rxon.flags &= ~RXON_FLG_SHORT_SLOT_MSK;
+
+		priv->staging_rxon.flags |= RXON_FLG_BAND_24G_MSK;
+		priv->staging_rxon.flags |= RXON_FLG_AUTO_DETECT_MSK;
+		priv->staging_rxon.flags &= ~RXON_FLG_CCK_MSK;
+	}
+}
+
+/*
+ * initilize rxon structure with default values fromm eeprom
+ */
+static void iwl_connection_init_rx_config(struct iwl_priv *priv)
+{
+	const struct iwl_channel_info *ch_info;
+
+	memset(&priv->staging_rxon, 0, sizeof(priv->staging_rxon));
+
+	switch (priv->iw_mode) {
+	case IEEE80211_IF_TYPE_AP:
+		priv->staging_rxon.dev_type = RXON_DEV_TYPE_AP;
+		break;
+
+	case IEEE80211_IF_TYPE_STA:
+		priv->staging_rxon.dev_type = RXON_DEV_TYPE_ESS;
+		priv->staging_rxon.filter_flags = RXON_FILTER_ACCEPT_GRP_MSK;
+		break;
+
+	case IEEE80211_IF_TYPE_IBSS:
+		priv->staging_rxon.dev_type = RXON_DEV_TYPE_IBSS;
+		priv->staging_rxon.flags = RXON_FLG_SHORT_PREAMBLE_MSK;
+		priv->staging_rxon.filter_flags = RXON_FILTER_BCON_AWARE_MSK |
+						  RXON_FILTER_ACCEPT_GRP_MSK;
+		break;
+
+	case IEEE80211_IF_TYPE_MNTR:
+		priv->staging_rxon.dev_type = RXON_DEV_TYPE_SNIFFER;
+		priv->staging_rxon.filter_flags = RXON_FILTER_PROMISC_MSK |
+		    RXON_FILTER_CTL2HOST_MSK | RXON_FILTER_ACCEPT_GRP_MSK;
+		break;
+	}
+
+#if 0
+	/* TODO:  Figure out when short_preamble would be set and cache from
+	 * that */
+	if (!hw_to_local(priv->hw)->short_preamble)
+		priv->staging_rxon.flags &= ~RXON_FLG_SHORT_PREAMBLE_MSK;
+	else
+		priv->staging_rxon.flags |= RXON_FLG_SHORT_PREAMBLE_MSK;
+#endif
+
+	ch_info = iwl_get_channel_info(priv, priv->phymode,
+				       le16_to_cpu(priv->staging_rxon.channel));
+
+	if (!ch_info)
+		ch_info = &priv->channel_info[0];
+
+	/*
+	 * in some case A channels are all non IBSS
+	 * in this case force B/G channel
+	 */
+	if ((priv->iw_mode == IEEE80211_IF_TYPE_IBSS) &&
+	    !(is_channel_ibss(ch_info)))
+		ch_info = &priv->channel_info[0];
+
+	priv->staging_rxon.channel = cpu_to_le16(ch_info->channel);
+	if (is_channel_a_band(ch_info))
+		priv->phymode = MODE_IEEE80211A;
+	else
+		priv->phymode = MODE_IEEE80211G;
+
+	iwl_set_flags_for_phymode(priv, priv->phymode);
+
+	priv->staging_rxon.ofdm_basic_rates =
+	    (IWL_OFDM_RATES_MASK >> IWL_FIRST_OFDM_RATE) & 0xFF;
+	priv->staging_rxon.cck_basic_rates =
+	    (IWL_CCK_RATES_MASK >> IWL_FIRST_CCK_RATE) & 0xF;
+}
+
+static int iwl_set_mode(struct iwl_priv *priv, int mode)
+{
+	if (!iwl_is_ready_rf(priv))
+		return -EAGAIN;
+
+	if (mode == IEEE80211_IF_TYPE_IBSS) {
+		const struct iwl_channel_info *ch_info;
+
+		ch_info = iwl_get_channel_info(priv,
+			priv->phymode,
+			le16_to_cpu(priv->staging_rxon.channel));
+
+		if (!ch_info || !is_channel_ibss(ch_info)) {
+			IWL_ERROR("channel %d not IBSS channel\n",
+				  le16_to_cpu(priv->staging_rxon.channel));
+			return -EINVAL;
+		}
+	}
+
+	cancel_delayed_work(&priv->scan_check);
+	if (iwl_scan_cancel_timeout(priv, 100)) {
+		IWL_WARNING("Aborted scan still in progress after 100ms\n");
+		IWL_DEBUG_MAC80211("leaving - scan abort failed.\n");
+		return -EAGAIN;
+	}
+
+	priv->iw_mode = mode;
+
+	iwl_connection_init_rx_config(priv);
+	memcpy(priv->staging_rxon.node_addr, priv->mac_addr, ETH_ALEN);
+
+	iwl_clear_stations_table(priv);
+
+	iwl_commit_rxon(priv);
+
+	return 0;
+}
+
+static void iwl_build_tx_cmd_hwcrypto(struct iwl_priv *priv,
+				      struct ieee80211_tx_control *ctl,
+				      struct iwl_cmd *cmd,
+				      struct sk_buff *skb_frag,
+				      int last_frag)
+{
+	struct iwl_hw_key *keyinfo = &priv->stations[ctl->key_idx].keyinfo;
+
+	switch (keyinfo->alg) {
+	case ALG_CCMP:
+		cmd->cmd.tx.sec_ctl = TX_CMD_SEC_CCM;
+		memcpy(cmd->cmd.tx.key, keyinfo->key, keyinfo->keylen);
+		IWL_DEBUG_TX("tx_cmd with aes hwcrypto\n");
+		break;
+
+	case ALG_TKIP:
+#if 0
+		cmd->cmd.tx.sec_ctl = TX_CMD_SEC_TKIP;
+
+		if (last_frag)
+			memcpy(cmd->cmd.tx.tkip_mic.byte, skb_frag->tail - 8,
+			       8);
+		else
+			memset(cmd->cmd.tx.tkip_mic.byte, 0, 8);
+#endif
+		break;
+
+	case ALG_WEP:
+		cmd->cmd.tx.sec_ctl = TX_CMD_SEC_WEP |
+		    (ctl->key_idx & TX_CMD_SEC_MSK) << TX_CMD_SEC_SHIFT;
+
+		if (keyinfo->keylen == 13)
+			cmd->cmd.tx.sec_ctl |= TX_CMD_SEC_KEY128;
+
+		memcpy(&cmd->cmd.tx.key[3], keyinfo->key, keyinfo->keylen);
+
+		IWL_DEBUG_TX("Configuring packet for WEP encryption "
+			     "with key %d\n", ctl->key_idx);
+		break;
+
+	default:
+		printk(KERN_ERR "Unknown encode alg %d\n", keyinfo->alg);
+		break;
+	}
+}
+
+/*
+ * handle build REPLY_TX command notification.
+ */
+static void iwl_build_tx_cmd_basic(struct iwl_priv *priv,
+				  struct iwl_cmd *cmd,
+				  struct ieee80211_tx_control *ctrl,
+				  struct ieee80211_hdr *hdr,
+				  int is_unicast, u8 std_id)
+{
+	__le16 *qc;
+	u16 fc = le16_to_cpu(hdr->frame_control);
+	__le32 tx_flags = cmd->cmd.tx.tx_flags;
+
+	cmd->cmd.tx.stop_time.life_time = TX_CMD_LIFE_TIME_INFINITE;
+	if (!(ctrl->flags & IEEE80211_TXCTL_NO_ACK)) {
+		tx_flags |= TX_CMD_FLG_ACK_MSK;
+		if ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT)
+			tx_flags |= TX_CMD_FLG_SEQ_CTL_MSK;
+		if (ieee80211_is_probe_response(fc) &&
+		    !(le16_to_cpu(hdr->seq_ctrl) & 0xf))
+			tx_flags |= TX_CMD_FLG_TSF_MSK;
+	} else {
+		tx_flags &= (~TX_CMD_FLG_ACK_MSK);
+		tx_flags |= TX_CMD_FLG_SEQ_CTL_MSK;
+	}
+
+	cmd->cmd.tx.sta_id = std_id;
+	if (ieee80211_get_morefrag(hdr))
+		tx_flags |= TX_CMD_FLG_MORE_FRAG_MSK;
+
+	qc = ieee80211_get_qos_ctrl(hdr);
+	if (qc) {
+		cmd->cmd.tx.tid_tspec = (u8) (le16_to_cpu(*qc) & 0xf);
+		tx_flags &= ~TX_CMD_FLG_SEQ_CTL_MSK;
+	} else
+		tx_flags |= TX_CMD_FLG_SEQ_CTL_MSK;
+
+	if (ctrl->flags & IEEE80211_TXCTL_USE_RTS_CTS) {
+		tx_flags |= TX_CMD_FLG_RTS_MSK;
+		tx_flags &= ~TX_CMD_FLG_CTS_MSK;
+	} else if (ctrl->flags & IEEE80211_TXCTL_USE_CTS_PROTECT) {
+		tx_flags &= ~TX_CMD_FLG_RTS_MSK;
+		tx_flags |= TX_CMD_FLG_CTS_MSK;
+	}
+
+	if ((tx_flags & TX_CMD_FLG_RTS_MSK) || (tx_flags & TX_CMD_FLG_CTS_MSK))
+		tx_flags |= TX_CMD_FLG_FULL_TXOP_PROT_MSK;
+
+	tx_flags &= ~(TX_CMD_FLG_ANT_SEL_MSK);
+	if ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT) {
+		if ((fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_ASSOC_REQ ||
+		    (fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_REASSOC_REQ)
+			cmd->cmd.tx.timeout.pm_frame_timeout =
+				cpu_to_le16(3);
+		else
+			cmd->cmd.tx.timeout.pm_frame_timeout =
+				cpu_to_le16(2);
+	} else
+		cmd->cmd.tx.timeout.pm_frame_timeout = 0;
+
+	cmd->cmd.tx.driver_txop = 0;
+	cmd->cmd.tx.tx_flags = tx_flags;
+	cmd->cmd.tx.next_frame_len = 0;
+}
+
+static int iwl_get_sta_id(struct iwl_priv *priv, struct ieee80211_hdr *hdr)
+{
+	int sta_id;
+	u16 fc = le16_to_cpu(hdr->frame_control);
+
+	/* If this frame is broadcast or not data then use the broadcast
+	 * station id */
+	if (((fc & IEEE80211_FCTL_FTYPE) != IEEE80211_FTYPE_DATA) ||
+	    is_multicast_ether_addr(hdr->addr1))
+		return priv->hw_setting.bcast_sta_id;
+
+	switch (priv->iw_mode) {
+
+	/* If this frame is part of a BSS network (we're a station), then
+	 * we use the AP's station id */
+	case IEEE80211_IF_TYPE_STA:
+		return IWL_AP_ID;
+
+	/* If we are an AP, then find the station, or use BCAST */
+	case IEEE80211_IF_TYPE_AP:
+		sta_id = iwl_hw_find_station(priv, hdr->addr1);
+		if (sta_id != IWL_INVALID_STATION)
+			return sta_id;
+		return priv->hw_setting.bcast_sta_id;
+
+	/* If this frame is part of a IBSS network, then we use the
+	 * target specific station id */
+	case IEEE80211_IF_TYPE_IBSS: {
+		DECLARE_MAC_BUF(mac);
+
+		sta_id = iwl_hw_find_station(priv, hdr->addr1);
+		if (sta_id != IWL_INVALID_STATION)
+			return sta_id;
+
+		sta_id = iwl_add_station(priv, hdr->addr1, 0, CMD_ASYNC);
+
+		if (sta_id != IWL_INVALID_STATION)
+			return sta_id;
+
+		IWL_DEBUG_DROP("Station %s not in station map. "
+			       "Defaulting to broadcast...\n",
+			       print_mac(mac, hdr->addr1));
+		iwl_print_hex_dump(IWL_DL_DROP, (u8 *) hdr, sizeof(*hdr));
+		return priv->hw_setting.bcast_sta_id;
+	}
+	default:
+		IWL_WARNING("Unkown mode of operation: %d", priv->iw_mode);
+		return priv->hw_setting.bcast_sta_id;
+	}
+}
+
+/*
+ * start REPLY_TX command process
+ */
+static int iwl_tx_skb(struct iwl_priv *priv,
+		      struct sk_buff *skb, struct ieee80211_tx_control *ctl)
+{
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
+	struct iwl_tfd_frame *tfd;
+	u32 *control_flags;
+	int txq_id = ctl->queue;
+	struct iwl_tx_queue *txq = NULL;
+	struct iwl_queue *q = NULL;
+	dma_addr_t phys_addr;
+	dma_addr_t txcmd_phys;
+	struct iwl_cmd *out_cmd = NULL;
+	u16 len, idx, len_org;
+	u8 id, hdr_len, unicast;
+	u8 sta_id;
+	u16 seq_number = 0;
+	u16 fc;
+	__le16 *qc;
+	u8 wait_write_ptr = 0;
+	unsigned long flags;
+	int rc;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	if (iwl_is_rfkill(priv)) {
+		IWL_DEBUG_DROP("Dropping - RF KILL\n");
+		goto drop_unlock;
+	}
+
+	if (!priv->interface_id) {
+		IWL_DEBUG_DROP("Dropping - !priv->interface_id\n");
+		goto drop_unlock;
+	}
+
+	if ((ctl->tx_rate & 0xFF) == IWL_INVALID_RATE) {
+		IWL_ERROR("ERROR: No TX rate available.\n");
+		goto drop_unlock;
+	}
+
+	unicast = !is_multicast_ether_addr(hdr->addr1);
+	id = 0;
+
+	fc = le16_to_cpu(hdr->frame_control);
+
+#ifdef CONFIG_IWLWIFI_DEBUG
+	if (ieee80211_is_auth(fc))
+		IWL_DEBUG_TX("Sending AUTH frame\n");
+	else if (ieee80211_is_assoc_request(fc))
+		IWL_DEBUG_TX("Sending ASSOC frame\n");
+	else if (ieee80211_is_reassoc_request(fc))
+		IWL_DEBUG_TX("Sending REASSOC frame\n");
+#endif
+
+	if (!iwl_is_associated(priv) &&
+	    ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_DATA)) {
+		IWL_DEBUG_DROP("Dropping - !iwl_is_associated\n");
+		goto drop_unlock;
+	}
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	hdr_len = ieee80211_get_hdrlen(fc);
+	sta_id = iwl_get_sta_id(priv, hdr);
+	if (sta_id == IWL_INVALID_STATION) {
+		DECLARE_MAC_BUF(mac);
+
+		IWL_DEBUG_DROP("Dropping - INVALID STATION: %s\n",
+			       print_mac(mac, hdr->addr1));
+		goto drop;
+	}
+
+	IWL_DEBUG_RATE("station Id %d\n", sta_id);
+
+	qc = ieee80211_get_qos_ctrl(hdr);
+	if (qc) {
+		u8 tid = (u8)(le16_to_cpu(*qc) & 0xf);
+		seq_number = priv->stations[sta_id].tid[tid].seq_number &
+				IEEE80211_SCTL_SEQ;
+		hdr->seq_ctrl = cpu_to_le16(seq_number) |
+			(hdr->seq_ctrl &
+				__constant_cpu_to_le16(IEEE80211_SCTL_FRAG));
+		seq_number += 0x10;
+	}
+	txq = &priv->txq[txq_id];
+	q = &txq->q;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	tfd = &txq->bd[q->first_empty];
+	memset(tfd, 0, sizeof(*tfd));
+	control_flags = (u32 *) tfd;
+	idx = get_cmd_index(q, q->first_empty, 0);
+
+	memset(&(txq->txb[q->first_empty]), 0, sizeof(struct iwl_tx_info));
+	txq->txb[q->first_empty].skb[0] = skb;
+	memcpy(&(txq->txb[q->first_empty].status.control),
+	       ctl, sizeof(struct ieee80211_tx_control));
+	out_cmd = &txq->cmd[idx];
+	memset(&out_cmd->hdr, 0, sizeof(out_cmd->hdr));
+	memset(&out_cmd->cmd.tx, 0, sizeof(out_cmd->cmd.tx));
+	out_cmd->hdr.cmd = REPLY_TX;
+	out_cmd->hdr.sequence = cpu_to_le16((u16)(QUEUE_TO_SEQ(txq_id) |
+				INDEX_TO_SEQ(q->first_empty)));
+	/* copy frags header */
+	memcpy(out_cmd->cmd.tx.hdr, hdr, hdr_len);
+
+	/* hdr = (struct ieee80211_hdr *)out_cmd->cmd.tx.hdr; */
+	len = priv->hw_setting.tx_cmd_len +
+		sizeof(struct iwl_cmd_header) + hdr_len;
+
+	len_org = len;
+	len = (len + 3) & ~3;
+
+	if (len_org != len)
+		len_org = 1;
+	else
+		len_org = 0;
+
+	txcmd_phys = txq->dma_addr_cmd + sizeof(struct iwl_cmd) * idx +
+		     offsetof(struct iwl_cmd, hdr);
+
+	iwl_hw_txq_attach_buf_to_tfd(priv, tfd, txcmd_phys, len);
+
+	if (!(ctl->flags & IEEE80211_TXCTL_DO_NOT_ENCRYPT))
+		iwl_build_tx_cmd_hwcrypto(priv, ctl, out_cmd, skb, 0);
+
+	/* 802.11 null functions have no payload... */
+	len = skb->len - hdr_len;
+	if (len) {
+		phys_addr = pci_map_single(priv->pci_dev, skb->data + hdr_len,
+					   len, PCI_DMA_TODEVICE);
+		iwl_hw_txq_attach_buf_to_tfd(priv, tfd, phys_addr, len);
+	}
+
+	/* If there is no payload, then only one TFD is used */
+	if (!len)
+		*control_flags = TFD_CTL_COUNT_SET(1);
+	else
+		*control_flags = TFD_CTL_COUNT_SET(2) |
+			TFD_CTL_PAD_SET(U32_PAD(len));
+
+	len = (u16)skb->len;
+	out_cmd->cmd.tx.len = cpu_to_le16(len);
+
+	/* TODO need this for burst mode later on */
+	iwl_build_tx_cmd_basic(priv, out_cmd, ctl, hdr, unicast, sta_id);
+
+	/* set is_hcca to 0; it probably will never be implemented */
+	iwl_hw_build_tx_cmd_rate(priv, out_cmd, ctl, hdr, sta_id, 0);
+
+	out_cmd->cmd.tx.tx_flags &= ~TX_CMD_FLG_ANT_A_MSK;
+	out_cmd->cmd.tx.tx_flags &= ~TX_CMD_FLG_ANT_B_MSK;
+
+	if (!ieee80211_get_morefrag(hdr)) {
+		txq->need_update = 1;
+		if (qc) {
+			u8 tid = (u8)(le16_to_cpu(*qc) & 0xf);
+			priv->stations[sta_id].tid[tid].seq_number = seq_number;
+		}
+	} else {
+		wait_write_ptr = 1;
+		txq->need_update = 0;
+	}
+
+	iwl_print_hex_dump(IWL_DL_TX, out_cmd->cmd.payload,
+			   sizeof(out_cmd->cmd.tx));
+
+	iwl_print_hex_dump(IWL_DL_TX, (u8 *)out_cmd->cmd.tx.hdr,
+			   ieee80211_get_hdrlen(fc));
+
+	q->first_empty = iwl_queue_inc_wrap(q->first_empty, q->n_bd);
+	rc = iwl_tx_queue_update_write_ptr(priv, txq);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	if (rc)
+		return rc;
+
+	if ((iwl_queue_space(q) < q->high_mark)
+	    && priv->mac80211_registered) {
+		if (wait_write_ptr) {
+			spin_lock_irqsave(&priv->lock, flags);
+			txq->need_update = 1;
+			iwl_tx_queue_update_write_ptr(priv, txq);
+			spin_unlock_irqrestore(&priv->lock, flags);
+		}
+
+		ieee80211_stop_queue(priv->hw, ctl->queue);
+	}
+
+	return 0;
+
+drop_unlock:
+	spin_unlock_irqrestore(&priv->lock, flags);
+drop:
+	return -1;
+}
+
+static void iwl_set_rate(struct iwl_priv *priv)
+{
+	const struct ieee80211_hw_mode *hw = NULL;
+	struct ieee80211_rate *rate;
+	int i;
+
+	hw = iwl_get_hw_mode(priv, priv->phymode);
+
+	priv->active_rate = 0;
+	priv->active_rate_basic = 0;
+
+	IWL_DEBUG_RATE("Setting rates for 802.11%c\n",
+		       hw->mode == MODE_IEEE80211A ?
+		       'a' : ((hw->mode == MODE_IEEE80211B) ? 'b' : 'g'));
+
+	for (i = 0; i < hw->num_rates; i++) {
+		rate = &(hw->rates[i]);
+		if ((rate->val < IWL_RATE_COUNT) &&
+		    (rate->flags & IEEE80211_RATE_SUPPORTED)) {
+			IWL_DEBUG_RATE("Adding rate index %d (plcp %d)%s\n",
+				       rate->val, iwl_rates[rate->val].plcp,
+				       (rate->flags & IEEE80211_RATE_BASIC) ?
+				       "*" : "");
+			priv->active_rate |= (1 << rate->val);
+			if (rate->flags & IEEE80211_RATE_BASIC)
+				priv->active_rate_basic |= (1 << rate->val);
+		} else
+			IWL_DEBUG_RATE("Not adding rate %d (plcp %d)\n",
+				       rate->val, iwl_rates[rate->val].plcp);
+	}
+
+	IWL_DEBUG_RATE("Set active_rate = %0x, active_rate_basic = %0x\n",
+		       priv->active_rate, priv->active_rate_basic);
+
+	/*
+	 * If a basic rate is configured, then use it (adding IWL_RATE_1M_MASK)
+	 * otherwise set it to the default of all CCK rates and 6, 12, 24 for
+	 * OFDM
+	 */
+	if (priv->active_rate_basic & IWL_CCK_BASIC_RATES_MASK)
+		priv->staging_rxon.cck_basic_rates =
+		    ((priv->active_rate_basic &
+		      IWL_CCK_RATES_MASK) >> IWL_FIRST_CCK_RATE) & 0xF;
+	else
+		priv->staging_rxon.cck_basic_rates =
+		    (IWL_CCK_BASIC_RATES_MASK >> IWL_FIRST_CCK_RATE) & 0xF;
+
+	if (priv->active_rate_basic & IWL_OFDM_BASIC_RATES_MASK)
+		priv->staging_rxon.ofdm_basic_rates =
+		    ((priv->active_rate_basic &
+		      (IWL_OFDM_BASIC_RATES_MASK | IWL_RATE_6M_MASK)) >>
+		      IWL_FIRST_OFDM_RATE) & 0xFF;
+	else
+		priv->staging_rxon.ofdm_basic_rates =
+		   (IWL_OFDM_BASIC_RATES_MASK >> IWL_FIRST_OFDM_RATE) & 0xFF;
+}
+
+static void iwl_radio_kill_sw(struct iwl_priv *priv, int disable_radio)
+{
+	unsigned long flags;
+
+	if (!!disable_radio == test_bit(STATUS_RF_KILL_SW, &priv->status))
+		return;
+
+	IWL_DEBUG_RF_KILL("Manual SW RF KILL set to: RADIO %s\n",
+			  disable_radio ? "OFF" : "ON");
+
+	if (disable_radio) {
+		iwl_scan_cancel(priv);
+		/* FIXME: This is a workaround for AP */
+		if (priv->iw_mode != IEEE80211_IF_TYPE_AP) {
+			spin_lock_irqsave(&priv->lock, flags);
+			iwl_write32(priv, CSR_UCODE_DRV_GP1_SET,
+				    CSR_UCODE_SW_BIT_RFKILL);
+			spin_unlock_irqrestore(&priv->lock, flags);
+			iwl_send_card_state(priv, CARD_STATE_CMD_DISABLE, 0);
+			set_bit(STATUS_RF_KILL_SW, &priv->status);
+		}
+		return;
+	}
+
+	spin_lock_irqsave(&priv->lock, flags);
+	iwl_write32(priv, CSR_UCODE_DRV_GP1_CLR, CSR_UCODE_SW_BIT_RFKILL);
+
+	clear_bit(STATUS_RF_KILL_SW, &priv->status);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	/* wake up ucode */
+	msleep(10);
+
+	spin_lock_irqsave(&priv->lock, flags);
+	iwl_read32(priv, CSR_UCODE_DRV_GP1);
+	if (!iwl_grab_restricted_access(priv))
+		iwl_release_restricted_access(priv);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	if (test_bit(STATUS_RF_KILL_HW, &priv->status)) {
+		IWL_DEBUG_RF_KILL("Can not turn radio back on - "
+				  "disabled by HW switch\n");
+		return;
+	}
+
+	queue_work(priv->workqueue, &priv->restart);
+	return;
+}
+
+void iwl_set_decrypted_flag(struct iwl_priv *priv, struct sk_buff *skb,
+			    u32 decrypt_res, struct ieee80211_rx_status *stats)
+{
+	u16 fc =
+	    le16_to_cpu(((struct ieee80211_hdr *)skb->data)->frame_control);
+
+	if (priv->active_rxon.filter_flags & RXON_FILTER_DIS_DECRYPT_MSK)
+		return;
+
+	if (!(fc & IEEE80211_FCTL_PROTECTED))
+		return;
+
+	IWL_DEBUG_RX("decrypt_res:0x%x\n", decrypt_res);
+	switch (decrypt_res & RX_RES_STATUS_SEC_TYPE_MSK) {
+	case RX_RES_STATUS_SEC_TYPE_TKIP:
+		if ((decrypt_res & RX_RES_STATUS_DECRYPT_TYPE_MSK) ==
+		    RX_RES_STATUS_BAD_ICV_MIC)
+			stats->flag |= RX_FLAG_MMIC_ERROR;
+	case RX_RES_STATUS_SEC_TYPE_WEP:
+	case RX_RES_STATUS_SEC_TYPE_CCMP:
+		if ((decrypt_res & RX_RES_STATUS_DECRYPT_TYPE_MSK) ==
+		    RX_RES_STATUS_DECRYPT_OK) {
+			IWL_DEBUG_RX("hw decrypt successfully!!!\n");
+			stats->flag |= RX_FLAG_DECRYPTED;
+		}
+		break;
+
+	default:
+		break;
+	}
+}
+
+void iwl_handle_data_packet_monitor(struct iwl_priv *priv,
+				    struct iwl_rx_mem_buffer *rxb,
+				    void *data, short len,
+				    struct ieee80211_rx_status *stats,
+				    u16 phy_flags)
+{
+	struct iwl_rt_rx_hdr *iwl_rt;
+
+	/* First cache any information we need before we overwrite
+	 * the information provided in the skb from the hardware */
+	s8 signal = stats->ssi;
+	s8 noise = 0;
+	int rate = stats->rate;
+	u64 tsf = stats->mactime;
+	__le16 phy_flags_hw = cpu_to_le16(phy_flags);
+
+	/* We received data from the HW, so stop the watchdog */
+	if (len > IWL_RX_BUF_SIZE - sizeof(*iwl_rt)) {
+		IWL_DEBUG_DROP("Dropping too large packet in monitor\n");
+		return;
+	}
+
+	/* copy the frame data to write after where the radiotap header goes */
+	iwl_rt = (void *)rxb->skb->data;
+	memmove(iwl_rt->payload, data, len);
+
+	iwl_rt->rt_hdr.it_version = PKTHDR_RADIOTAP_VERSION;
+	iwl_rt->rt_hdr.it_pad = 0; /* always good to zero */
+
+	/* total header + data */
+	iwl_rt->rt_hdr.it_len = cpu_to_le16(sizeof(*iwl_rt));
+
+	/* Set the size of the skb to the size of the frame */
+	skb_put(rxb->skb, sizeof(*iwl_rt) + len);
+
+	/* Big bitfield of all the fields we provide in radiotap */
+	iwl_rt->rt_hdr.it_present =
+	    cpu_to_le32((1 << IEEE80211_RADIOTAP_TSFT) |
+			(1 << IEEE80211_RADIOTAP_FLAGS) |
+			(1 << IEEE80211_RADIOTAP_RATE) |
+			(1 << IEEE80211_RADIOTAP_CHANNEL) |
+			(1 << IEEE80211_RADIOTAP_DBM_ANTSIGNAL) |
+			(1 << IEEE80211_RADIOTAP_DBM_ANTNOISE) |
+			(1 << IEEE80211_RADIOTAP_ANTENNA));
+
+	/* Zero the flags, we'll add to them as we go */
+	iwl_rt->rt_flags = 0;
+
+	iwl_rt->rt_tsf = cpu_to_le64(tsf);
+
+	/* Convert to dBm */
+	iwl_rt->rt_dbmsignal = signal;
+	iwl_rt->rt_dbmnoise = noise;
+
+	/* Convert the channel frequency and set the flags */
+	iwl_rt->rt_channelMHz = cpu_to_le16(stats->freq);
+	if (!(phy_flags_hw & RX_RES_PHY_FLAGS_BAND_24_MSK))
+		iwl_rt->rt_chbitmask =
+		    cpu_to_le16((IEEE80211_CHAN_OFDM | IEEE80211_CHAN_5GHZ));
+	else if (phy_flags_hw & RX_RES_PHY_FLAGS_MOD_CCK_MSK)
+		iwl_rt->rt_chbitmask =
+		    cpu_to_le16((IEEE80211_CHAN_CCK | IEEE80211_CHAN_2GHZ));
+	else	/* 802.11g */
+		iwl_rt->rt_chbitmask =
+		    cpu_to_le16((IEEE80211_CHAN_OFDM | IEEE80211_CHAN_2GHZ));
+
+	rate = iwl_rate_index_from_plcp(rate);
+	if (rate == -1)
+		iwl_rt->rt_rate = 0;
+	else
+		iwl_rt->rt_rate = iwl_rates[rate].ieee;
+
+	/* antenna number */
+	iwl_rt->rt_antenna =
+		le16_to_cpu(phy_flags_hw & RX_RES_PHY_FLAGS_ANTENNA_MSK) >> 4;
+
+	/* set the preamble flag if we have it */
+	if (phy_flags_hw & RX_RES_PHY_FLAGS_SHORT_PREAMBLE_MSK)
+		iwl_rt->rt_flags |= IEEE80211_RADIOTAP_F_SHORTPRE;
+
+	IWL_DEBUG_RX("Rx packet of %d bytes.\n", rxb->skb->len);
+
+	stats->flag |= RX_FLAG_RADIOTAP;
+	ieee80211_rx_irqsafe(priv->hw, rxb->skb, stats);
+	rxb->skb = NULL;
+}
+
+
+#define IWL_PACKET_RETRY_TIME HZ
+
+int is_duplicate_packet(struct iwl_priv *priv, struct ieee80211_hdr *header)
+{
+	u16 sc = le16_to_cpu(header->seq_ctrl);
+	u16 seq = (sc & IEEE80211_SCTL_SEQ) >> 4;
+	u16 frag = sc & IEEE80211_SCTL_FRAG;
+	u16 *last_seq, *last_frag;
+	unsigned long *last_time;
+
+	switch (priv->iw_mode) {
+	case IEEE80211_IF_TYPE_IBSS:{
+		struct list_head *p;
+		struct iwl_ibss_seq *entry = NULL;
+		u8 *mac = header->addr2;
+		int index = mac[5] & (IWL_IBSS_MAC_HASH_SIZE - 1);
+
+		__list_for_each(p, &priv->ibss_mac_hash[index]) {
+			entry =
+				list_entry(p, struct iwl_ibss_seq, list);
+			if (!compare_ether_addr(entry->mac, mac))
+				break;
+		}
+		if (p == &priv->ibss_mac_hash[index]) {
+			entry = kzalloc(sizeof(*entry), GFP_ATOMIC);
+			if (!entry) {
+				IWL_ERROR
+					("Cannot malloc new mac entry\n");
+				return 0;
+			}
+			memcpy(entry->mac, mac, ETH_ALEN);
+			entry->seq_num = seq;
+			entry->frag_num = frag;
+			entry->packet_time = jiffies;
+			list_add(&entry->list,
+				 &priv->ibss_mac_hash[index]);
+			return 0;
+		}
+		last_seq = &entry->seq_num;
+		last_frag = &entry->frag_num;
+		last_time = &entry->packet_time;
+		break;
+	}
+	case IEEE80211_IF_TYPE_STA:
+		last_seq = &priv->last_seq_num;
+		last_frag = &priv->last_frag_num;
+		last_time = &priv->last_packet_time;
+		break;
+	default:
+		return 0;
+	}
+	if ((*last_seq == seq) &&
+	    time_after(*last_time + IWL_PACKET_RETRY_TIME, jiffies)) {
+		if (*last_frag == frag)
+			goto drop;
+		if (*last_frag + 1 != frag)
+			/* out-of-order fragment */
+			goto drop;
+	} else
+		*last_seq = seq;
+
+	*last_frag = frag;
+	*last_time = jiffies;
+	return 0;
+
+ drop:
+	return 1;
+}
+
+#ifdef CONFIG_IWLWIFI_SPECTRUM_MEASUREMENT
+
+#include "iwl-spectrum.h"
+
+#define BEACON_TIME_MASK_LOW	0x00FFFFFF
+#define BEACON_TIME_MASK_HIGH	0xFF000000
+#define TIME_UNIT		1024
+
+/*
+ * extended beacon time format
+ * time in usec will be changed into a 32-bit value in 8:24 format
+ * the high 1 byte is the beacon counts
+ * the lower 3 bytes is the time in usec within one beacon interval
+ */
+
+static u32 iwl_usecs_to_beacons(u32 usec, u32 beacon_interval)
+{
+	u32 quot;
+	u32 rem;
+	u32 interval = beacon_interval * 1024;
+
+	if (!interval || !usec)
+		return 0;
+
+	quot = (usec / interval) & (BEACON_TIME_MASK_HIGH >> 24);
+	rem = (usec % interval) & BEACON_TIME_MASK_LOW;
+
+	return (quot << 24) + rem;
+}
+
+/* base is usually what we get from ucode with each received frame,
+ * the same as HW timer counter counting down
+ */
+
+static __le32 iwl_add_beacon_time(u32 base, u32 addon, u32 beacon_interval)
+{
+	u32 base_low = base & BEACON_TIME_MASK_LOW;
+	u32 addon_low = addon & BEACON_TIME_MASK_LOW;
+	u32 interval = beacon_interval * TIME_UNIT;
+	u32 res = (base & BEACON_TIME_MASK_HIGH) +
+	    (addon & BEACON_TIME_MASK_HIGH);
+
+	if (base_low > addon_low)
+		res += base_low - addon_low;
+	else if (base_low < addon_low) {
+		res += interval + base_low - addon_low;
+		res += (1 << 24);
+	} else
+		res += (1 << 24);
+
+	return cpu_to_le32(res);
+}
+
+static int iwl_get_measurement(struct iwl_priv *priv,
+			       struct ieee80211_measurement_params *params,
+			       u8 type)
+{
+	struct iwl_spectrum_cmd spectrum;
+	struct iwl_rx_packet *res;
+	struct iwl_host_cmd cmd = {
+		.id = REPLY_SPECTRUM_MEASUREMENT_CMD,
+		.data = (void *)&spectrum,
+		.meta.flags = CMD_WANT_SKB,
+	};
+	u32 add_time = le64_to_cpu(params->start_time);
+	int rc;
+	int spectrum_resp_status;
+	int duration = le16_to_cpu(params->duration);
+
+	if (iwl_is_associated(priv))
+		add_time =
+		    iwl_usecs_to_beacons(
+			le64_to_cpu(params->start_time) - priv->last_tsf,
+			le16_to_cpu(priv->rxon_timing.beacon_interval));
+
+	memset(&spectrum, 0, sizeof(spectrum));
+
+	spectrum.channel_count = cpu_to_le16(1);
+	spectrum.flags =
+	    RXON_FLG_TSF2HOST_MSK | RXON_FLG_ANT_A_MSK | RXON_FLG_DIS_DIV_MSK;
+	spectrum.filter_flags = MEASUREMENT_FILTER_FLAG;
+	cmd.len = sizeof(spectrum);
+	spectrum.len = cpu_to_le16(cmd.len - sizeof(spectrum.len));
+
+	if (iwl_is_associated(priv))
+		spectrum.start_time =
+		    iwl_add_beacon_time(priv->last_beacon_time,
+				add_time,
+				le16_to_cpu(priv->rxon_timing.beacon_interval));
+	else
+		spectrum.start_time = 0;
+
+	spectrum.channels[0].duration = cpu_to_le32(duration * TIME_UNIT);
+	spectrum.channels[0].channel = params->channel;
+	spectrum.channels[0].type = type;
+	if (priv->active_rxon.flags & RXON_FLG_BAND_24G_MSK)
+		spectrum.flags |= RXON_FLG_BAND_24G_MSK |
+		    RXON_FLG_AUTO_DETECT_MSK | RXON_FLG_TGG_PROTECT_MSK;
+
+	rc = iwl_send_cmd_sync(priv, &cmd);
+	if (rc)
+		return rc;
+
+	res = (struct iwl_rx_packet *)cmd.meta.u.skb->data;
+	if (res->hdr.flags & IWL_CMD_FAILED_MSK) {
+		IWL_ERROR("Bad return from REPLY_RX_ON_ASSOC command\n");
+		rc = -EIO;
+	}
+
+	spectrum_resp_status = le16_to_cpu(res->u.spectrum.status);
+	switch (spectrum_resp_status) {
+	case 0:		/* Command will be handled */
+		if (res->u.spectrum.id != 0xff) {
+			IWL_DEBUG_INFO
+			    ("Replaced existing measurement: %d\n",
+			     res->u.spectrum.id);
+			priv->measurement_status &= ~MEASUREMENT_READY;
+		}
+		priv->measurement_status |= MEASUREMENT_ACTIVE;
+		rc = 0;
+		break;
+
+	case 1:		/* Command will not be handled */
+		rc = -EAGAIN;
+		break;
+	}
+
+	dev_kfree_skb_any(cmd.meta.u.skb);
+
+	return rc;
+}
+#endif
+
+static void iwl_txstatus_to_ieee(struct iwl_priv *priv,
+				 struct iwl_tx_info *tx_sta)
+{
+
+	tx_sta->status.ack_signal = 0;
+	tx_sta->status.excessive_retries = 0;
+	tx_sta->status.queue_length = 0;
+	tx_sta->status.queue_number = 0;
+
+	if (in_interrupt())
+		ieee80211_tx_status_irqsafe(priv->hw,
+					    tx_sta->skb[0], &(tx_sta->status));
+	else
+		ieee80211_tx_status(priv->hw,
+				    tx_sta->skb[0], &(tx_sta->status));
+
+	tx_sta->skb[0] = NULL;
+}
+
+/**
+ * iwl_tx_queue_reclaim - Reclaim Tx queue entries no more used by NIC.
+ *
+ * When FW advances 'R' index, all entries between old and
+ * new 'R' index need to be reclaimed. As result, some free space
+ * forms. If there is enough free space (> low mark), wake Tx queue.
+ */
+int iwl_tx_queue_reclaim(struct iwl_priv *priv, int txq_id, int index)
+{
+	struct iwl_tx_queue *txq = &priv->txq[txq_id];
+	struct iwl_queue *q = &txq->q;
+	int nfreed = 0;
+
+	if ((index >= q->n_bd) || (x2_queue_used(q, index) == 0)) {
+		IWL_ERROR("Read index for DMA queue txq id (%d), index %d, "
+			  "is out of range [0-%d] %d %d.\n", txq_id,
+			  index, q->n_bd, q->first_empty, q->last_used);
+		return 0;
+	}
+
+	for (index = iwl_queue_inc_wrap(index, q->n_bd);
+		q->last_used != index;
+		q->last_used = iwl_queue_inc_wrap(q->last_used, q->n_bd)) {
+		if (txq_id != IWL_CMD_QUEUE_NUM) {
+			iwl_txstatus_to_ieee(priv,
+					&(txq->txb[txq->q.last_used]));
+			iwl_hw_txq_free_tfd(priv, txq);
+		} else if (nfreed > 1) {
+			IWL_ERROR("HCMD skipped: index (%d) %d %d\n", index,
+					q->first_empty, q->last_used);
+			queue_work(priv->workqueue, &priv->restart);
+		}
+		nfreed++;
+	}
+
+	if (iwl_queue_space(q) > q->low_mark && (txq_id >= 0) &&
+			(txq_id != IWL_CMD_QUEUE_NUM) &&
+			priv->mac80211_registered)
+		ieee80211_wake_queue(priv->hw, txq_id);
+
+
+	return nfreed;
+}
+
+static int iwl_is_tx_success(u32 status)
+{
+	return (status & 0xFF) == 0x1;
+}
+
+/******************************************************************************
+ *
+ * Generic RX handler implementations
+ *
+ ******************************************************************************/
+static void iwl_rx_reply_tx(struct iwl_priv *priv,
+			    struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = (void *)rxb->skb->data;
+	u16 sequence = le16_to_cpu(pkt->hdr.sequence);
+	int txq_id = SEQ_TO_QUEUE(sequence);
+	int index = SEQ_TO_INDEX(sequence);
+	struct iwl_tx_queue *txq = &priv->txq[txq_id];
+	struct ieee80211_tx_status *tx_status;
+	struct iwl_tx_resp *tx_resp = (void *)&pkt->u.raw[0];
+	u32  status = le32_to_cpu(tx_resp->status);
+
+	if ((index >= txq->q.n_bd) || (x2_queue_used(&txq->q, index) == 0)) {
+		IWL_ERROR("Read index for DMA queue txq_id (%d) index %d "
+			  "is out of range [0-%d] %d %d\n", txq_id,
+			  index, txq->q.n_bd, txq->q.first_empty,
+			  txq->q.last_used);
+		return;
+	}
+
+	tx_status = &(txq->txb[txq->q.last_used].status);
+
+	tx_status->retry_count = tx_resp->failure_frame;
+	tx_status->queue_number = status;
+	tx_status->queue_length = tx_resp->bt_kill_count;
+	tx_status->queue_length |= tx_resp->failure_rts;
+
+	tx_status->flags =
+	    iwl_is_tx_success(status) ? IEEE80211_TX_STATUS_ACK : 0;
+
+	tx_status->control.tx_rate = iwl_rate_index_from_plcp(tx_resp->rate);
+
+	IWL_DEBUG_TX("Tx queue %d Status %s (0x%08x) plcp rate %d retries %d\n",
+			txq_id, iwl_get_tx_fail_reason(status), status,
+			tx_resp->rate, tx_resp->failure_frame);
+
+	IWL_DEBUG_TX_REPLY("Tx queue reclaim %d\n", index);
+	if (index != -1)
+		iwl_tx_queue_reclaim(priv, txq_id, index);
+
+	if (iwl_check_bits(status, TX_ABORT_REQUIRED_MSK))
+		IWL_ERROR("TODO:  Implement Tx ABORT REQUIRED!!!\n");
+}
+
+
+static void iwl_rx_reply_alive(struct iwl_priv *priv,
+			       struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = (void *)rxb->skb->data;
+	struct iwl_alive_resp *palive;
+	struct delayed_work *pwork;
+
+	palive = &pkt->u.alive_frame;
+
+	IWL_DEBUG_INFO("Alive ucode status 0x%08X revision "
+		       "0x%01X 0x%01X\n",
+		       palive->is_valid, palive->ver_type,
+		       palive->ver_subtype);
+
+	if (palive->ver_subtype == INITIALIZE_SUBTYPE) {
+		IWL_DEBUG_INFO("Initialization Alive received.\n");
+		memcpy(&priv->card_alive_init,
+		       &pkt->u.alive_frame,
+		       sizeof(struct iwl_init_alive_resp));
+		pwork = &priv->init_alive_start;
+	} else {
+		IWL_DEBUG_INFO("Runtime Alive received.\n");
+		memcpy(&priv->card_alive, &pkt->u.alive_frame,
+		       sizeof(struct iwl_alive_resp));
+		pwork = &priv->alive_start;
+		iwl_disable_events(priv);
+	}
+
+	/* We delay the ALIVE response by 5ms to
+	 * give the HW RF Kill time to activate... */
+	if (palive->is_valid == UCODE_VALID_OK)
+		queue_delayed_work(priv->workqueue, pwork,
+				   msecs_to_jiffies(5));
+	else
+		IWL_WARNING("uCode did not respond OK.\n");
+}
+
+static void iwl_rx_reply_add_sta(struct iwl_priv *priv,
+				 struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = (void *)rxb->skb->data;
+
+	IWL_DEBUG_RX("Received REPLY_ADD_STA: 0x%02X\n", pkt->u.status);
+	return;
+}
+
+static void iwl_rx_reply_error(struct iwl_priv *priv,
+			       struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = (void *)rxb->skb->data;
+
+	IWL_ERROR("Error Reply type 0x%08X cmd %s (0x%02X) "
+		"seq 0x%04X ser 0x%08X\n",
+		le32_to_cpu(pkt->u.err_resp.error_type),
+		get_cmd_string(pkt->u.err_resp.cmd_id),
+		pkt->u.err_resp.cmd_id,
+		le16_to_cpu(pkt->u.err_resp.bad_cmd_seq_num),
+		le32_to_cpu(pkt->u.err_resp.error_info));
+}
+
+#define TX_STATUS_ENTRY(x) case TX_STATUS_FAIL_ ## x: return #x
+
+static void iwl_rx_csa(struct iwl_priv *priv, struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = (void *)rxb->skb->data;
+	struct iwl_rxon_cmd *rxon = (void *)&priv->active_rxon;
+	struct iwl_csa_notification *csa = &(pkt->u.csa_notif);
+	IWL_DEBUG_11H("CSA notif: channel %d, status %d\n",
+		      le16_to_cpu(csa->channel), le32_to_cpu(csa->status));
+	rxon->channel = csa->channel;
+	priv->staging_rxon.channel = csa->channel;
+}
+
+static void iwl_rx_spectrum_measure_notif(struct iwl_priv *priv,
+					  struct iwl_rx_mem_buffer *rxb)
+{
+#ifdef CONFIG_IWLWIFI_SPECTRUM_MEASUREMENT
+	struct iwl_rx_packet *pkt = (void *)rxb->skb->data;
+	struct iwl_spectrum_notification *report = &(pkt->u.spectrum_notif);
+
+	if (!report->state) {
+		IWL_DEBUG(IWL_DL_11H | IWL_DL_INFO,
+			  "Spectrum Measure Notification: Start\n");
+		return;
+	}
+
+	memcpy(&priv->measure_report, report, sizeof(*report));
+	priv->measurement_status |= MEASUREMENT_READY;
+#endif
+}
+
+static void iwl_rx_pm_sleep_notif(struct iwl_priv *priv,
+				  struct iwl_rx_mem_buffer *rxb)
+{
+#ifdef CONFIG_IWLWIFI_DEBUG
+	struct iwl_rx_packet *pkt = (void *)rxb->skb->data;
+	struct iwl_sleep_notification *sleep = &(pkt->u.sleep_notif);
+	IWL_DEBUG_RX("sleep mode: %d, src: %d\n",
+		     sleep->pm_sleep_mode, sleep->pm_wakeup_src);
+#endif
+}
+
+static void iwl_rx_pm_debug_statistics_notif(struct iwl_priv *priv,
+					     struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = (void *)rxb->skb->data;
+	IWL_DEBUG_RADIO("Dumping %d bytes of unhandled "
+			"notification for %s:\n",
+			le32_to_cpu(pkt->len), get_cmd_string(pkt->hdr.cmd));
+	iwl_print_hex_dump(IWL_DL_RADIO, pkt->u.raw, le32_to_cpu(pkt->len));
+}
+
+static void iwl_bg_beacon_update(struct work_struct *work)
+{
+	struct iwl_priv *priv =
+		container_of(work, struct iwl_priv, beacon_update);
+	struct sk_buff *beacon;
+
+	/* Pull updated AP beacon from mac80211. will fail if not in AP mode */
+	beacon = ieee80211_beacon_get(priv->hw, priv->interface_id, NULL);
+
+	if (!beacon) {
+		IWL_ERROR("update beacon failed\n");
+		return;
+	}
+
+	mutex_lock(&priv->mutex);
+	/* new beacon skb is allocated every time; dispose previous.*/
+	if (priv->ibss_beacon)
+		dev_kfree_skb(priv->ibss_beacon);
+
+	priv->ibss_beacon = beacon;
+	mutex_unlock(&priv->mutex);
+
+	iwl_send_beacon_cmd(priv);
+}
+
+static void iwl_rx_beacon_notif(struct iwl_priv *priv,
+				struct iwl_rx_mem_buffer *rxb)
+{
+#ifdef CONFIG_IWLWIFI_DEBUG
+	struct iwl_rx_packet *pkt = (void *)rxb->skb->data;
+	struct iwl_beacon_notif *beacon = &(pkt->u.beacon_status);
+	u8 rate = beacon->beacon_notify_hdr.rate;
+
+	IWL_DEBUG_RX("beacon status %x retries %d iss %d "
+		"tsf %d %d rate %d\n",
+		le32_to_cpu(beacon->beacon_notify_hdr.status) & TX_STATUS_MSK,
+		beacon->beacon_notify_hdr.failure_frame,
+		le32_to_cpu(beacon->ibss_mgr_status),
+		le32_to_cpu(beacon->high_tsf),
+		le32_to_cpu(beacon->low_tsf), rate);
+#endif
+
+	if ((priv->iw_mode == IEEE80211_IF_TYPE_AP) &&
+	    (!test_bit(STATUS_EXIT_PENDING, &priv->status)))
+		queue_work(priv->workqueue, &priv->beacon_update);
+}
+
+/* Service response to REPLY_SCAN_CMD (0x80) */
+static void iwl_rx_reply_scan(struct iwl_priv *priv,
+			      struct iwl_rx_mem_buffer *rxb)
+{
+#ifdef CONFIG_IWLWIFI_DEBUG
+	struct iwl_rx_packet *pkt = (void *)rxb->skb->data;
+	struct iwl_scanreq_notification *notif =
+	    (struct iwl_scanreq_notification *)pkt->u.raw;
+
+	IWL_DEBUG_RX("Scan request status = 0x%x\n", notif->status);
+#endif
+}
+
+/* Service SCAN_START_NOTIFICATION (0x82) */
+static void iwl_rx_scan_start_notif(struct iwl_priv *priv,
+				    struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = (void *)rxb->skb->data;
+	struct iwl_scanstart_notification *notif =
+	    (struct iwl_scanstart_notification *)pkt->u.raw;
+	priv->scan_start_tsf = le32_to_cpu(notif->tsf_low);
+	IWL_DEBUG_SCAN("Scan start: "
+		       "%d [802.11%s] "
+		       "(TSF: 0x%08X:%08X) - %d (beacon timer %u)\n",
+		       notif->channel,
+		       notif->band ? "bg" : "a",
+		       notif->tsf_high,
+		       notif->tsf_low, notif->status, notif->beacon_timer);
+}
+
+/* Service SCAN_RESULTS_NOTIFICATION (0x83) */
+static void iwl_rx_scan_results_notif(struct iwl_priv *priv,
+				      struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = (void *)rxb->skb->data;
+	struct iwl_scanresults_notification *notif =
+	    (struct iwl_scanresults_notification *)pkt->u.raw;
+
+	IWL_DEBUG_SCAN("Scan ch.res: "
+		       "%d [802.11%s] "
+		       "(TSF: 0x%08X:%08X) - %d "
+		       "elapsed=%lu usec (%dms since last)\n",
+		       notif->channel,
+		       notif->band ? "bg" : "a",
+		       le32_to_cpu(notif->tsf_high),
+		       le32_to_cpu(notif->tsf_low),
+		       le32_to_cpu(notif->statistics[0]),
+		       le32_to_cpu(notif->tsf_low) - priv->scan_start_tsf,
+		       jiffies_to_msecs(elapsed_jiffies
+					(priv->last_scan_jiffies, jiffies)));
+
+	priv->last_scan_jiffies = jiffies;
+}
+
+/* Service SCAN_COMPLETE_NOTIFICATION (0x84) */
+static void iwl_rx_scan_complete_notif(struct iwl_priv *priv,
+				       struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = (void *)rxb->skb->data;
+	struct iwl_scancomplete_notification *scan_notif = (void *)pkt->u.raw;
+
+	IWL_DEBUG_SCAN("Scan complete: %d channels (TSF 0x%08X:%08X) - %d\n",
+		       scan_notif->scanned_channels,
+		       scan_notif->tsf_low,
+		       scan_notif->tsf_high, scan_notif->status);
+
+	/* The HW is no longer scanning */
+	clear_bit(STATUS_SCAN_HW, &priv->status);
+
+	/* The scan completion notification came in, so kill that timer... */
+	cancel_delayed_work(&priv->scan_check);
+
+	IWL_DEBUG_INFO("Scan pass on %sGHz took %dms\n",
+		       (priv->scan_bands == 2) ? "2.4" : "5.2",
+		       jiffies_to_msecs(elapsed_jiffies
+					(priv->scan_pass_start, jiffies)));
+
+	/* Remove this scanned band from the list
+	 * of pending bands to scan */
+	priv->scan_bands--;
+
+	/* If a request to abort was given, or the scan did not succeed
+	 * then we reset the scan state machine and terminate,
+	 * re-queuing another scan if one has been requested */
+	if (test_bit(STATUS_SCAN_ABORTING, &priv->status)) {
+		IWL_DEBUG_INFO("Aborted scan completed.\n");
+		clear_bit(STATUS_SCAN_ABORTING, &priv->status);
+	} else {
+		/* If there are more bands on this scan pass reschedule */
+		if (priv->scan_bands > 0)
+			goto reschedule;
+	}
+
+	priv->last_scan_jiffies = jiffies;
+	IWL_DEBUG_INFO("Setting scan to off\n");
+
+	clear_bit(STATUS_SCANNING, &priv->status);
+
+	IWL_DEBUG_INFO("Scan took %dms\n",
+		jiffies_to_msecs(elapsed_jiffies(priv->scan_start, jiffies)));
+
+	queue_work(priv->workqueue, &priv->scan_completed);
+
+	return;
+
+reschedule:
+	priv->scan_pass_start = jiffies;
+	queue_work(priv->workqueue, &priv->request_scan);
+}
+
+/* Handle notification from uCode that card's power state is changing
+ * due to software, hardware, or critical temperature RFKILL */
+static void iwl_rx_card_state_notif(struct iwl_priv *priv,
+				    struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = (void *)rxb->skb->data;
+	u32 flags = le32_to_cpu(pkt->u.card_state_notif.flags);
+	unsigned long status = priv->status;
+
+	IWL_DEBUG_RF_KILL("Card state received: HW:%s SW:%s\n",
+			  (flags & HW_CARD_DISABLED) ? "Kill" : "On",
+			  (flags & SW_CARD_DISABLED) ? "Kill" : "On");
+
+	iwl_write32(priv, CSR_UCODE_DRV_GP1_SET,
+		    CSR_UCODE_DRV_GP1_BIT_CMD_BLOCKED);
+
+	if (flags & HW_CARD_DISABLED)
+		set_bit(STATUS_RF_KILL_HW, &priv->status);
+	else
+		clear_bit(STATUS_RF_KILL_HW, &priv->status);
+
+
+	if (flags & SW_CARD_DISABLED)
+		set_bit(STATUS_RF_KILL_SW, &priv->status);
+	else
+		clear_bit(STATUS_RF_KILL_SW, &priv->status);
+
+	iwl_scan_cancel(priv);
+
+	if ((test_bit(STATUS_RF_KILL_HW, &status) !=
+	     test_bit(STATUS_RF_KILL_HW, &priv->status)) ||
+	    (test_bit(STATUS_RF_KILL_SW, &status) !=
+	     test_bit(STATUS_RF_KILL_SW, &priv->status)))
+		queue_work(priv->workqueue, &priv->rf_kill);
+	else
+		wake_up_interruptible(&priv->wait_command_queue);
+}
+
+/**
+ * iwl_setup_rx_handlers - Initialize Rx handler callbacks
+ *
+ * Setup the RX handlers for each of the reply types sent from the uCode
+ * to the host.
+ *
+ * This function chains into the hardware specific files for them to setup
+ * any hardware specific handlers as well.
+ */
+static void iwl_setup_rx_handlers(struct iwl_priv *priv)
+{
+	priv->rx_handlers[REPLY_ALIVE] = iwl_rx_reply_alive;
+	priv->rx_handlers[REPLY_ADD_STA] = iwl_rx_reply_add_sta;
+	priv->rx_handlers[REPLY_ERROR] = iwl_rx_reply_error;
+	priv->rx_handlers[CHANNEL_SWITCH_NOTIFICATION] = iwl_rx_csa;
+	priv->rx_handlers[SPECTRUM_MEASURE_NOTIFICATION] =
+	    iwl_rx_spectrum_measure_notif;
+	priv->rx_handlers[PM_SLEEP_NOTIFICATION] = iwl_rx_pm_sleep_notif;
+	priv->rx_handlers[PM_DEBUG_STATISTIC_NOTIFIC] =
+	    iwl_rx_pm_debug_statistics_notif;
+	priv->rx_handlers[BEACON_NOTIFICATION] = iwl_rx_beacon_notif;
+
+	/* NOTE:  iwl_rx_statistics is different based on whether
+	 * the build is for the 3945 or the 4965.  See the
+	 * corresponding implementation in iwl-XXXX.c
+	 *
+	 * The same handler is used for both the REPLY to a
+	 * discrete statistics request from the host as well as
+	 * for the periodic statistics notification from the uCode
+	 */
+	priv->rx_handlers[REPLY_STATISTICS_CMD] = iwl_hw_rx_statistics;
+	priv->rx_handlers[STATISTICS_NOTIFICATION] = iwl_hw_rx_statistics;
+
+	priv->rx_handlers[REPLY_SCAN_CMD] = iwl_rx_reply_scan;
+	priv->rx_handlers[SCAN_START_NOTIFICATION] = iwl_rx_scan_start_notif;
+	priv->rx_handlers[SCAN_RESULTS_NOTIFICATION] =
+	    iwl_rx_scan_results_notif;
+	priv->rx_handlers[SCAN_COMPLETE_NOTIFICATION] =
+	    iwl_rx_scan_complete_notif;
+	priv->rx_handlers[CARD_STATE_NOTIFICATION] = iwl_rx_card_state_notif;
+	priv->rx_handlers[REPLY_TX] = iwl_rx_reply_tx;
+
+	/* Setup hardware specific Rx handlers */
+	iwl_hw_rx_handler_setup(priv);
+}
+
+/**
+ * iwl_tx_cmd_complete - Pull unused buffers off the queue and reclaim them
+ * @rxb: Rx buffer to reclaim
+ *
+ * If an Rx buffer has an async callback associated with it the callback
+ * will be executed.  The attached skb (if present) will only be freed
+ * if the callback returns 1
+ */
+static void iwl_tx_cmd_complete(struct iwl_priv *priv,
+				struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = (struct iwl_rx_packet *)rxb->skb->data;
+	u16 sequence = le16_to_cpu(pkt->hdr.sequence);
+	int txq_id = SEQ_TO_QUEUE(sequence);
+	int index = SEQ_TO_INDEX(sequence);
+	int huge = sequence & SEQ_HUGE_FRAME;
+	int cmd_index;
+	struct iwl_cmd *cmd;
+
+	/* If a Tx command is being handled and it isn't in the actual
+	 * command queue then there a command routing bug has been introduced
+	 * in the queue management code. */
+	if (txq_id != IWL_CMD_QUEUE_NUM)
+		IWL_ERROR("Error wrong command queue %d command id 0x%X\n",
+			  txq_id, pkt->hdr.cmd);
+	BUG_ON(txq_id != IWL_CMD_QUEUE_NUM);
+
+	cmd_index = get_cmd_index(&priv->txq[IWL_CMD_QUEUE_NUM].q, index, huge);
+	cmd = &priv->txq[IWL_CMD_QUEUE_NUM].cmd[cmd_index];
+
+	/* Input error checking is done when commands are added to queue. */
+	if (cmd->meta.flags & CMD_WANT_SKB) {
+		cmd->meta.source->u.skb = rxb->skb;
+		rxb->skb = NULL;
+	} else if (cmd->meta.u.callback &&
+		   !cmd->meta.u.callback(priv, cmd, rxb->skb))
+		rxb->skb = NULL;
+
+	iwl_tx_queue_reclaim(priv, txq_id, index);
+
+	if (!(cmd->meta.flags & CMD_ASYNC)) {
+		clear_bit(STATUS_HCMD_ACTIVE, &priv->status);
+		wake_up_interruptible(&priv->wait_command_queue);
+	}
+}
+
+/************************** RX-FUNCTIONS ****************************/
+/*
+ * Rx theory of operation
+ *
+ * The host allocates 32 DMA target addresses and passes the host address
+ * to the firmware at register IWL_RFDS_TABLE_LOWER + N * RFD_SIZE where N is
+ * 0 to 31
+ *
+ * Rx Queue Indexes
+ * The host/firmware share two index registers for managing the Rx buffers.
+ *
+ * The READ index maps to the first position that the firmware may be writing
+ * to -- the driver can read up to (but not including) this position and get
+ * good data.
+ * The READ index is managed by the firmware once the card is enabled.
+ *
+ * The WRITE index maps to the last position the driver has read from -- the
+ * position preceding WRITE is the last slot the firmware can place a packet.
+ *
+ * The queue is empty (no good data) if WRITE = READ - 1, and is full if
+ * WRITE = READ.
+ *
+ * During initialization the host sets up the READ queue position to the first
+ * INDEX position, and WRITE to the last (READ - 1 wrapped)
+ *
+ * When the firmware places a packet in a buffer it will advance the READ index
+ * and fire the RX interrupt.  The driver can then query the READ index and
+ * process as many packets as possible, moving the WRITE index forward as it
+ * resets the Rx queue buffers with new memory.
+ *
+ * The management in the driver is as follows:
+ * + A list of pre-allocated SKBs is stored in iwl->rxq->rx_free.  When
+ *   iwl->rxq->free_count drops to or below RX_LOW_WATERMARK, work is scheduled
+ *   to replensish the iwl->rxq->rx_free.
+ * + In iwl_rx_replenish (scheduled) if 'processed' != 'read' then the
+ *   iwl->rxq is replenished and the READ INDEX is updated (updating the
+ *   'processed' and 'read' driver indexes as well)
+ * + A received packet is processed and handed to the kernel network stack,
+ *   detached from the iwl->rxq.  The driver 'processed' index is updated.
+ * + The Host/Firmware iwl->rxq is replenished at tasklet time from the rx_free
+ *   list. If there are no allocated buffers in iwl->rxq->rx_free, the READ
+ *   INDEX is not incremented and iwl->status(RX_STALLED) is set.  If there
+ *   were enough free buffers and RX_STALLED is set it is cleared.
+ *
+ *
+ * Driver sequence:
+ *
+ * iwl_rx_queue_alloc()       Allocates rx_free
+ * iwl_rx_replenish()         Replenishes rx_free list from rx_used, and calls
+ *                            iwl_rx_queue_restock
+ * iwl_rx_queue_restock()     Moves available buffers from rx_free into Rx
+ *                            queue, updates firmware pointers, and updates
+ *                            the WRITE index.  If insufficient rx_free buffers
+ *                            are available, schedules iwl_rx_replenish
+ *
+ * -- enable interrupts --
+ * ISR - iwl_rx()             Detach iwl_rx_mem_buffers from pool up to the
+ *                            READ INDEX, detaching the SKB from the pool.
+ *                            Moves the packet buffer from queue to rx_used.
+ *                            Calls iwl_rx_queue_restock to refill any empty
+ *                            slots.
+ * ...
+ *
+ */
+
+/**
+ * iwl_rx_queue_space - Return number of free slots available in queue.
+ */
+static int iwl_rx_queue_space(const struct iwl_rx_queue *q)
+{
+	int s = q->read - q->write;
+	if (s <= 0)
+		s += RX_QUEUE_SIZE;
+	/* keep some buffer to not confuse full and empty queue */
+	s -= 2;
+	if (s < 0)
+		s = 0;
+	return s;
+}
+
+/**
+ * iwl_rx_queue_update_write_ptr - Update the write pointer for the RX queue
+ *
+ * NOTE: This function has 3945 and 4965 specific code sections
+ * but is declared in base due to the majority of the
+ * implementation being the same (only a numeric constant is
+ * different)
+ *
+ */
+int iwl_rx_queue_update_write_ptr(struct iwl_priv *priv, struct iwl_rx_queue *q)
+{
+	u32 reg = 0;
+	int rc = 0;
+	unsigned long flags;
+
+	spin_lock_irqsave(&q->lock, flags);
+
+	if (q->need_update == 0)
+		goto exit_unlock;
+
+	if (test_bit(STATUS_POWER_PMI, &priv->status)) {
+		reg = iwl_read32(priv, CSR_UCODE_DRV_GP1);
+
+		if (reg & CSR_UCODE_DRV_GP1_BIT_MAC_SLEEP) {
+			iwl_set_bit(priv, CSR_GP_CNTRL,
+				    CSR_GP_CNTRL_REG_FLAG_MAC_ACCESS_REQ);
+			goto exit_unlock;
+		}
+
+		rc = iwl_grab_restricted_access(priv);
+		if (rc)
+			goto exit_unlock;
+
+		iwl_write_restricted(priv, FH_RSCSR_CHNL0_WPTR,
+				     q->write & ~0x7);
+		iwl_release_restricted_access(priv);
+	} else
+		iwl_write32(priv, FH_RSCSR_CHNL0_WPTR, q->write & ~0x7);
+
+
+	q->need_update = 0;
+
+ exit_unlock:
+	spin_unlock_irqrestore(&q->lock, flags);
+	return rc;
+}
+
+/**
+ * iwl_dma_addr2rbd_ptr - convert a DMA address to a uCode read buffer pointer.
+ *
+ * NOTE: This function has 3945 and 4965 specific code paths in it.
+ */
+static inline __le32 iwl_dma_addr2rbd_ptr(struct iwl_priv *priv,
+					  dma_addr_t dma_addr)
+{
+	return cpu_to_le32((u32)dma_addr);
+}
+
+/**
+ * iwl_rx_queue_restock - refill RX queue from pre-allocated pool
+ *
+ * If there are slots in the RX queue that  need to be restocked,
+ * and we have free pre-allocated buffers, fill the ranks as much
+ * as we can pulling from rx_free.
+ *
+ * This moves the 'write' index forward to catch up with 'processed', and
+ * also updates the memory address in the firmware to reference the new
+ * target buffer.
+ */
+int iwl_rx_queue_restock(struct iwl_priv *priv)
+{
+	struct iwl_rx_queue *rxq = &priv->rxq;
+	struct list_head *element;
+	struct iwl_rx_mem_buffer *rxb;
+	unsigned long flags;
+	int write, rc;
+
+	spin_lock_irqsave(&rxq->lock, flags);
+	write = rxq->write & ~0x7;
+	while ((iwl_rx_queue_space(rxq) > 0) && (rxq->free_count)) {
+		element = rxq->rx_free.next;
+		rxb = list_entry(element, struct iwl_rx_mem_buffer, list);
+		list_del(element);
+		rxq->bd[rxq->write] = iwl_dma_addr2rbd_ptr(priv, rxb->dma_addr);
+		rxq->queue[rxq->write] = rxb;
+		rxq->write = (rxq->write + 1) & RX_QUEUE_MASK;
+		rxq->free_count--;
+	}
+	spin_unlock_irqrestore(&rxq->lock, flags);
+	/* If the pre-allocated buffer pool is dropping low, schedule to
+	 * refill it */
+	if (rxq->free_count <= RX_LOW_WATERMARK)
+		queue_work(priv->workqueue, &priv->rx_replenish);
+
+
+	/* If we've added more space for the firmware to place data, tell it */
+	if ((write != (rxq->write & ~0x7))
+	    || (abs(rxq->write - rxq->read) > 7)) {
+		spin_lock_irqsave(&rxq->lock, flags);
+		rxq->need_update = 1;
+		spin_unlock_irqrestore(&rxq->lock, flags);
+		rc = iwl_rx_queue_update_write_ptr(priv, rxq);
+		if (rc)
+			return rc;
+	}
+
+	return 0;
+}
+
+/**
+ * iwl_rx_replensih - Move all used packet from rx_used to rx_free
+ *
+ * When moving to rx_free an SKB is allocated for the slot.
+ *
+ * Also restock the Rx queue via iwl_rx_queue_restock.
+ * This is called as a scheduled work item (except for during intialization)
+ */
+void iwl_rx_replenish(void *data)
+{
+	struct iwl_priv *priv = data;
+	struct iwl_rx_queue *rxq = &priv->rxq;
+	struct list_head *element;
+	struct iwl_rx_mem_buffer *rxb;
+	unsigned long flags;
+	spin_lock_irqsave(&rxq->lock, flags);
+	while (!list_empty(&rxq->rx_used)) {
+		element = rxq->rx_used.next;
+		rxb = list_entry(element, struct iwl_rx_mem_buffer, list);
+		rxb->skb =
+		    alloc_skb(IWL_RX_BUF_SIZE, __GFP_NOWARN | GFP_ATOMIC);
+		if (!rxb->skb) {
+			if (net_ratelimit())
+				printk(KERN_CRIT DRV_NAME
+				       ": Can not allocate SKB buffers\n");
+			/* We don't reschedule replenish work here -- we will
+			 * call the restock method and if it still needs
+			 * more buffers it will schedule replenish */
+			break;
+		}
+		priv->alloc_rxb_skb++;
+		list_del(element);
+		rxb->dma_addr =
+		    pci_map_single(priv->pci_dev, rxb->skb->data,
+				   IWL_RX_BUF_SIZE, PCI_DMA_FROMDEVICE);
+		list_add_tail(&rxb->list, &rxq->rx_free);
+		rxq->free_count++;
+	}
+	spin_unlock_irqrestore(&rxq->lock, flags);
+
+	spin_lock_irqsave(&priv->lock, flags);
+	iwl_rx_queue_restock(priv);
+	spin_unlock_irqrestore(&priv->lock, flags);
+}
+
+/* Assumes that the skb field of the buffers in 'pool' is kept accurate.
+ * If an SKB has been detached, the POOL needs to have it's SKB set to NULL
+ * This free routine walks the list of POOL entries and if SKB is set to
+ * non NULL it is unmapped and freed
+ */
+void iwl_rx_queue_free(struct iwl_priv *priv, struct iwl_rx_queue *rxq)
+{
+	int i;
+	for (i = 0; i < RX_QUEUE_SIZE + RX_FREE_BUFFERS; i++) {
+		if (rxq->pool[i].skb != NULL) {
+			pci_unmap_single(priv->pci_dev,
+					 rxq->pool[i].dma_addr,
+					 IWL_RX_BUF_SIZE, PCI_DMA_FROMDEVICE);
+			dev_kfree_skb(rxq->pool[i].skb);
+		}
+	}
+
+	pci_free_consistent(priv->pci_dev, 4 * RX_QUEUE_SIZE, rxq->bd,
+			    rxq->dma_addr);
+	rxq->bd = NULL;
+}
+
+int iwl_rx_queue_alloc(struct iwl_priv *priv)
+{
+	struct iwl_rx_queue *rxq = &priv->rxq;
+	struct pci_dev *dev = priv->pci_dev;
+	int i;
+
+	spin_lock_init(&rxq->lock);
+	INIT_LIST_HEAD(&rxq->rx_free);
+	INIT_LIST_HEAD(&rxq->rx_used);
+	rxq->bd = pci_alloc_consistent(dev, 4 * RX_QUEUE_SIZE, &rxq->dma_addr);
+	if (!rxq->bd)
+		return -ENOMEM;
+	/* Fill the rx_used queue with _all_ of the Rx buffers */
+	for (i = 0; i < RX_FREE_BUFFERS + RX_QUEUE_SIZE; i++)
+		list_add_tail(&rxq->pool[i].list, &rxq->rx_used);
+	/* Set us so that we have processed and used all buffers, but have
+	 * not restocked the Rx queue with fresh buffers */
+	rxq->read = rxq->write = 0;
+	rxq->free_count = 0;
+	rxq->need_update = 0;
+	return 0;
+}
+
+void iwl_rx_queue_reset(struct iwl_priv *priv, struct iwl_rx_queue *rxq)
+{
+	unsigned long flags;
+	int i;
+	spin_lock_irqsave(&rxq->lock, flags);
+	INIT_LIST_HEAD(&rxq->rx_free);
+	INIT_LIST_HEAD(&rxq->rx_used);
+	/* Fill the rx_used queue with _all_ of the Rx buffers */
+	for (i = 0; i < RX_FREE_BUFFERS + RX_QUEUE_SIZE; i++) {
+		/* In the reset function, these buffers may have been allocated
+		 * to an SKB, so we need to unmap and free potential storage */
+		if (rxq->pool[i].skb != NULL) {
+			pci_unmap_single(priv->pci_dev,
+					 rxq->pool[i].dma_addr,
+					 IWL_RX_BUF_SIZE, PCI_DMA_FROMDEVICE);
+			priv->alloc_rxb_skb--;
+			dev_kfree_skb(rxq->pool[i].skb);
+			rxq->pool[i].skb = NULL;
+		}
+		list_add_tail(&rxq->pool[i].list, &rxq->rx_used);
+	}
+
+	/* Set us so that we have processed and used all buffers, but have
+	 * not restocked the Rx queue with fresh buffers */
+	rxq->read = rxq->write = 0;
+	rxq->free_count = 0;
+	spin_unlock_irqrestore(&rxq->lock, flags);
+}
+
+/* Convert linear signal-to-noise ratio into dB */
+static u8 ratio2dB[100] = {
+/*	 0   1   2   3   4   5   6   7   8   9 */
+	 0,  0,  6, 10, 12, 14, 16, 17, 18, 19, /* 00 - 09 */
+	20, 21, 22, 22, 23, 23, 24, 25, 26, 26, /* 10 - 19 */
+	26, 26, 26, 27, 27, 28, 28, 28, 29, 29, /* 20 - 29 */
+	29, 30, 30, 30, 31, 31, 31, 31, 32, 32, /* 30 - 39 */
+	32, 32, 32, 33, 33, 33, 33, 33, 34, 34, /* 40 - 49 */
+	34, 34, 34, 34, 35, 35, 35, 35, 35, 35, /* 50 - 59 */
+	36, 36, 36, 36, 36, 36, 36, 37, 37, 37, /* 60 - 69 */
+	37, 37, 37, 37, 37, 38, 38, 38, 38, 38, /* 70 - 79 */
+	38, 38, 38, 38, 38, 39, 39, 39, 39, 39, /* 80 - 89 */
+	39, 39, 39, 39, 39, 40, 40, 40, 40, 40  /* 90 - 99 */
+};
+
+/* Calculates a relative dB value from a ratio of linear
+ *   (i.e. not dB) signal levels.
+ * Conversion assumes that levels are voltages (20*log), not powers (10*log). */
+int iwl_calc_db_from_ratio(int sig_ratio)
+{
+	/* Anything above 1000:1 just report as 60 dB */
+	if (sig_ratio > 1000)
+		return 60;
+
+	/* Above 100:1, divide by 10 and use table,
+	 *   add 20 dB to make up for divide by 10 */
+	if (sig_ratio > 100)
+		return (20 + (int)ratio2dB[sig_ratio/10]);
+
+	/* We shouldn't see this */
+	if (sig_ratio < 1)
+		return 0;
+
+	/* Use table for ratios 1:1 - 99:1 */
+	return (int)ratio2dB[sig_ratio];
+}
+
+#define PERFECT_RSSI (-20) /* dBm */
+#define WORST_RSSI (-95)   /* dBm */
+#define RSSI_RANGE (PERFECT_RSSI - WORST_RSSI)
+
+/* Calculate an indication of rx signal quality (a percentage, not dBm!).
+ * See http://www.ces.clemson.edu/linux/signal_quality.shtml for info
+ *   about formulas used below. */
+int iwl_calc_sig_qual(int rssi_dbm, int noise_dbm)
+{
+	int sig_qual;
+	int degradation = PERFECT_RSSI - rssi_dbm;
+
+	/* If we get a noise measurement, use signal-to-noise ratio (SNR)
+	 * as indicator; formula is (signal dbm - noise dbm).
+	 * SNR at or above 40 is a great signal (100%).
+	 * Below that, scale to fit SNR of 0 - 40 dB within 0 - 100% indicator.
+	 * Weakest usable signal is usually 10 - 15 dB SNR. */
+	if (noise_dbm) {
+		if (rssi_dbm - noise_dbm >= 40)
+			return 100;
+		else if (rssi_dbm < noise_dbm)
+			return 0;
+		sig_qual = ((rssi_dbm - noise_dbm) * 5) / 2;
+
+	/* Else use just the signal level.
+	 * This formula is a least squares fit of data points collected and
+	 *   compared with a reference system that had a percentage (%) display
+	 *   for signal quality. */
+	} else
+		sig_qual = (100 * (RSSI_RANGE * RSSI_RANGE) - degradation *
+			    (15 * RSSI_RANGE + 62 * degradation)) /
+			   (RSSI_RANGE * RSSI_RANGE);
+
+	if (sig_qual > 100)
+		sig_qual = 100;
+	else if (sig_qual < 1)
+		sig_qual = 0;
+
+	return sig_qual;
+}
+
+/**
+ * iwl_rx_handle - Main entry function for receiving responses from the uCode
+ *
+ * Uses the priv->rx_handlers callback function array to invoke
+ * the appropriate handlers, including command responses,
+ * frame-received notifications, and other notifications.
+ */
+static void iwl_rx_handle(struct iwl_priv *priv)
+{
+	struct iwl_rx_mem_buffer *rxb;
+	struct iwl_rx_packet *pkt;
+	struct iwl_rx_queue *rxq = &priv->rxq;
+	u32 r, i;
+	int reclaim;
+	unsigned long flags;
+
+	r = iwl_hw_get_rx_read(priv);
+	i = rxq->read;
+
+	/* Rx interrupt, but nothing sent from uCode */
+	if (i == r)
+		IWL_DEBUG(IWL_DL_RX | IWL_DL_ISR, "r = %d, i = %d\n", r, i);
+
+	while (i != r) {
+		rxb = rxq->queue[i];
+
+		/* If an RXB doesn't have a queue slot associated with it
+		 * then a bug has been introduced in the queue refilling
+		 * routines -- catch it here */
+		BUG_ON(rxb == NULL);
+
+		rxq->queue[i] = NULL;
+
+		pci_dma_sync_single_for_cpu(priv->pci_dev, rxb->dma_addr,
+					    IWL_RX_BUF_SIZE,
+					    PCI_DMA_FROMDEVICE);
+		pkt = (struct iwl_rx_packet *)rxb->skb->data;
+
+		/* Reclaim a command buffer only if this packet is a response
+		 *   to a (driver-originated) command.
+		 * If the packet (e.g. Rx frame) originated from uCode,
+		 *   there is no command buffer to reclaim.
+		 * Ucode should set SEQ_RX_FRAME bit if ucode-originated,
+		 *   but apparently a few don't get set; catch them here. */
+		reclaim = !(pkt->hdr.sequence & SEQ_RX_FRAME) &&
+			(pkt->hdr.cmd != STATISTICS_NOTIFICATION) &&
+			(pkt->hdr.cmd != REPLY_TX);
+
+		/* Based on type of command response or notification,
+		 *   handle those that need handling via function in
+		 *   rx_handlers table.  See iwl_setup_rx_handlers() */
+		if (priv->rx_handlers[pkt->hdr.cmd]) {
+			IWL_DEBUG(IWL_DL_HOST_COMMAND | IWL_DL_RX | IWL_DL_ISR,
+				"r = %d, i = %d, %s, 0x%02x\n", r, i,
+				get_cmd_string(pkt->hdr.cmd), pkt->hdr.cmd);
+			priv->rx_handlers[pkt->hdr.cmd] (priv, rxb);
+		} else {
+			/* No handling needed */
+			IWL_DEBUG(IWL_DL_HOST_COMMAND | IWL_DL_RX | IWL_DL_ISR,
+				"r %d i %d No handler needed for %s, 0x%02x\n",
+				r, i, get_cmd_string(pkt->hdr.cmd),
+				pkt->hdr.cmd);
+		}
+
+		if (reclaim) {
+			/* Invoke any callbacks, transfer the skb to caller,
+			 * and fire off the (possibly) blocking iwl_send_cmd()
+			 * as we reclaim the driver command queue */
+			if (rxb && rxb->skb)
+				iwl_tx_cmd_complete(priv, rxb);
+			else
+				IWL_WARNING("Claim null rxb?\n");
+		}
+
+		/* For now we just don't re-use anything.  We can tweak this
+		 * later to try and re-use notification packets and SKBs that
+		 * fail to Rx correctly */
+		if (rxb->skb != NULL) {
+			priv->alloc_rxb_skb--;
+			dev_kfree_skb_any(rxb->skb);
+			rxb->skb = NULL;
+		}
+
+		pci_unmap_single(priv->pci_dev, rxb->dma_addr,
+				 IWL_RX_BUF_SIZE, PCI_DMA_FROMDEVICE);
+		spin_lock_irqsave(&rxq->lock, flags);
+		list_add_tail(&rxb->list, &priv->rxq.rx_used);
+		spin_unlock_irqrestore(&rxq->lock, flags);
+		i = (i + 1) & RX_QUEUE_MASK;
+	}
+
+	/* Backtrack one entry */
+	priv->rxq.read = i;
+	iwl_rx_queue_restock(priv);
+}
+
+int iwl_tx_queue_update_write_ptr(struct iwl_priv *priv,
+				  struct iwl_tx_queue *txq)
+{
+	u32 reg = 0;
+	int rc = 0;
+	int txq_id = txq->q.id;
+
+	if (txq->need_update == 0)
+		return rc;
+
+	/* if we're trying to save power */
+	if (test_bit(STATUS_POWER_PMI, &priv->status)) {
+		/* wake up nic if it's powered down ...
+		 * uCode will wake up, and interrupt us again, so next
+		 * time we'll skip this part. */
+		reg = iwl_read32(priv, CSR_UCODE_DRV_GP1);
+
+		if (reg & CSR_UCODE_DRV_GP1_BIT_MAC_SLEEP) {
+			IWL_DEBUG_INFO("Requesting wakeup, GP1 = 0x%x\n", reg);
+			iwl_set_bit(priv, CSR_GP_CNTRL,
+				    CSR_GP_CNTRL_REG_FLAG_MAC_ACCESS_REQ);
+			return rc;
+		}
+
+		/* restore this queue's parameters in nic hardware. */
+		rc = iwl_grab_restricted_access(priv);
+		if (rc)
+			return rc;
+		iwl_write_restricted(priv, HBUS_TARG_WRPTR,
+				     txq->q.first_empty | (txq_id << 8));
+		iwl_release_restricted_access(priv);
+
+	/* else not in power-save mode, uCode will never sleep when we're
+	 * trying to tx (during RFKILL, we're not trying to tx). */
+	} else
+		iwl_write32(priv, HBUS_TARG_WRPTR,
+			    txq->q.first_empty | (txq_id << 8));
+
+	txq->need_update = 0;
+
+	return rc;
+}
+
+#ifdef CONFIG_IWLWIFI_DEBUG
+static void iwl_print_rx_config_cmd(struct iwl_rxon_cmd *rxon)
+{
+	DECLARE_MAC_BUF(mac);
+
+	IWL_DEBUG_RADIO("RX CONFIG:\n");
+	iwl_print_hex_dump(IWL_DL_RADIO, (u8 *) rxon, sizeof(*rxon));
+	IWL_DEBUG_RADIO("u16 channel: 0x%x\n", le16_to_cpu(rxon->channel));
+	IWL_DEBUG_RADIO("u32 flags: 0x%08X\n", le32_to_cpu(rxon->flags));
+	IWL_DEBUG_RADIO("u32 filter_flags: 0x%08x\n",
+			le32_to_cpu(rxon->filter_flags));
+	IWL_DEBUG_RADIO("u8 dev_type: 0x%x\n", rxon->dev_type);
+	IWL_DEBUG_RADIO("u8 ofdm_basic_rates: 0x%02x\n",
+			rxon->ofdm_basic_rates);
+	IWL_DEBUG_RADIO("u8 cck_basic_rates: 0x%02x\n", rxon->cck_basic_rates);
+	IWL_DEBUG_RADIO("u8[6] node_addr: %s\n",
+			print_mac(mac, rxon->node_addr));
+	IWL_DEBUG_RADIO("u8[6] bssid_addr: %s\n",
+			print_mac(mac, rxon->bssid_addr));
+	IWL_DEBUG_RADIO("u16 assoc_id: 0x%x\n", le16_to_cpu(rxon->assoc_id));
+}
+#endif
+
+static void iwl_enable_interrupts(struct iwl_priv *priv)
+{
+	IWL_DEBUG_ISR("Enabling interrupts\n");
+	set_bit(STATUS_INT_ENABLED, &priv->status);
+	iwl_write32(priv, CSR_INT_MASK, CSR_INI_SET_MASK);
+}
+
+static inline void iwl_disable_interrupts(struct iwl_priv *priv)
+{
+	clear_bit(STATUS_INT_ENABLED, &priv->status);
+
+	/* disable interrupts from uCode/NIC to host */
+	iwl_write32(priv, CSR_INT_MASK, 0x00000000);
+
+	/* acknowledge/clear/reset any interrupts still pending
+	 * from uCode or flow handler (Rx/Tx DMA) */
+	iwl_write32(priv, CSR_INT, 0xffffffff);
+	iwl_write32(priv, CSR_FH_INT_STATUS, 0xffffffff);
+	IWL_DEBUG_ISR("Disabled interrupts\n");
+}
+
+static const char *desc_lookup(int i)
+{
+	switch (i) {
+	case 1:
+		return "FAIL";
+	case 2:
+		return "BAD_PARAM";
+	case 3:
+		return "BAD_CHECKSUM";
+	case 4:
+		return "NMI_INTERRUPT";
+	case 5:
+		return "SYSASSERT";
+	case 6:
+		return "FATAL_ERROR";
+	}
+
+	return "UNKNOWN";
+}
+
+#define ERROR_START_OFFSET  (1 * sizeof(u32))
+#define ERROR_ELEM_SIZE     (7 * sizeof(u32))
+
+static void iwl_dump_nic_error_log(struct iwl_priv *priv)
+{
+	u32 i;
+	u32 desc, time, count, base, data1;
+	u32 blink1, blink2, ilink1, ilink2;
+	int rc;
+
+	base = le32_to_cpu(priv->card_alive.error_event_table_ptr);
+
+	if (!iwl_hw_valid_rtc_data_addr(base)) {
+		IWL_ERROR("Not valid error log pointer 0x%08X\n", base);
+		return;
+	}
+
+	rc = iwl_grab_restricted_access(priv);
+	if (rc) {
+		IWL_WARNING("Can not read from adapter at this time.\n");
+		return;
+	}
+
+	count = iwl_read_restricted_mem(priv, base);
+
+	if (ERROR_START_OFFSET <= count * ERROR_ELEM_SIZE) {
+		IWL_ERROR("Start IWL Error Log Dump:\n");
+		IWL_ERROR("Status: 0x%08lX, Config: %08X count: %d\n",
+			  priv->status, priv->config, count);
+	}
+
+	IWL_ERROR("Desc       Time       asrtPC  blink2 "
+		  "ilink1  nmiPC   Line\n");
+	for (i = ERROR_START_OFFSET;
+	     i < (count * ERROR_ELEM_SIZE) + ERROR_START_OFFSET;
+	     i += ERROR_ELEM_SIZE) {
+		desc = iwl_read_restricted_mem(priv, base + i);
+		time =
+		    iwl_read_restricted_mem(priv, base + i + 1 * sizeof(u32));
+		blink1 =
+		    iwl_read_restricted_mem(priv, base + i + 2 * sizeof(u32));
+		blink2 =
+		    iwl_read_restricted_mem(priv, base + i + 3 * sizeof(u32));
+		ilink1 =
+		    iwl_read_restricted_mem(priv, base + i + 4 * sizeof(u32));
+		ilink2 =
+		    iwl_read_restricted_mem(priv, base + i + 5 * sizeof(u32));
+		data1 =
+		    iwl_read_restricted_mem(priv, base + i + 6 * sizeof(u32));
+
+		IWL_ERROR
+		    ("%-13s (#%d) %010u 0x%05X 0x%05X 0x%05X 0x%05X %u\n\n",
+		     desc_lookup(desc), desc, time, blink1, blink2,
+		     ilink1, ilink2, data1);
+	}
+
+	iwl_release_restricted_access(priv);
+
+}
+
+#define EVENT_START_OFFSET  (4 * sizeof(u32))
+
+/**
+ * iwl_print_event_log - Dump error event log to syslog
+ *
+ * NOTE: Must be called with iwl_grab_restricted_access() already obtained!
+ */
+static void iwl_print_event_log(struct iwl_priv *priv, u32 start_idx,
+				u32 num_events, u32 mode)
+{
+	u32 i;
+	u32 base;       /* SRAM byte address of event log header */
+	u32 event_size;	/* 2 u32s, or 3 u32s if timestamp recorded */
+	u32 ptr;        /* SRAM byte address of log data */
+	u32 ev, time, data; /* event log data */
+
+	if (num_events == 0)
+		return;
+
+	base = le32_to_cpu(priv->card_alive.log_event_table_ptr);
+
+	if (mode == 0)
+		event_size = 2 * sizeof(u32);
+	else
+		event_size = 3 * sizeof(u32);
+
+	ptr = base + EVENT_START_OFFSET + (start_idx * event_size);
+
+	/* "time" is actually "data" for mode 0 (no timestamp).
+	 * place event id # at far right for easier visual parsing. */
+	for (i = 0; i < num_events; i++) {
+		ev = iwl_read_restricted_mem(priv, ptr);
+		ptr += sizeof(u32);
+		time = iwl_read_restricted_mem(priv, ptr);
+		ptr += sizeof(u32);
+		if (mode == 0)
+			IWL_ERROR("0x%08x\t%04u\n", time, ev); /* data, ev */
+		else {
+			data = iwl_read_restricted_mem(priv, ptr);
+			ptr += sizeof(u32);
+			IWL_ERROR("%010u\t0x%08x\t%04u\n", time, data, ev);
+		}
+	}
+}
+
+static void iwl_dump_nic_event_log(struct iwl_priv *priv)
+{
+	int rc;
+	u32 base;       /* SRAM byte address of event log header */
+	u32 capacity;   /* event log capacity in # entries */
+	u32 mode;       /* 0 - no timestamp, 1 - timestamp recorded */
+	u32 num_wraps;  /* # times uCode wrapped to top of log */
+	u32 next_entry; /* index of next entry to be written by uCode */
+	u32 size;       /* # entries that we'll print */
+
+	base = le32_to_cpu(priv->card_alive.log_event_table_ptr);
+	if (!iwl_hw_valid_rtc_data_addr(base)) {
+		IWL_ERROR("Invalid event log pointer 0x%08X\n", base);
+		return;
+	}
+
+	rc = iwl_grab_restricted_access(priv);
+	if (rc) {
+		IWL_WARNING("Can not read from adapter at this time.\n");
+		return;
+	}
+
+	/* event log header */
+	capacity = iwl_read_restricted_mem(priv, base);
+	mode = iwl_read_restricted_mem(priv, base + (1 * sizeof(u32)));
+	num_wraps = iwl_read_restricted_mem(priv, base + (2 * sizeof(u32)));
+	next_entry = iwl_read_restricted_mem(priv, base + (3 * sizeof(u32)));
+
+	size = num_wraps ? capacity : next_entry;
+
+	/* bail out if nothing in log */
+	if (size == 0) {
+		IWL_ERROR("Start IWL Event Log Dump: nothing in log\n");
+		iwl_release_restricted_access(priv);
+		return;
+	}
+
+	IWL_ERROR("Start IWL Event Log Dump: display count %d, wraps %d\n",
+		  size, num_wraps);
+
+	/* if uCode has wrapped back to top of log, start at the oldest entry,
+	 * i.e the next one that uCode would fill. */
+	if (num_wraps)
+		iwl_print_event_log(priv, next_entry,
+				    capacity - next_entry, mode);
+
+	/* (then/else) start at top of log */
+	iwl_print_event_log(priv, 0, next_entry, mode);
+
+	iwl_release_restricted_access(priv);
+}
+
+/**
+ * iwl_irq_handle_error - called for HW or SW error interrupt from card
+ */
+static void iwl_irq_handle_error(struct iwl_priv *priv)
+{
+	/* Set the FW error flag -- cleared on iwl_down */
+	set_bit(STATUS_FW_ERROR, &priv->status);
+
+	/* Cancel currently queued command. */
+	clear_bit(STATUS_HCMD_ACTIVE, &priv->status);
+
+#ifdef CONFIG_IWLWIFI_DEBUG
+	if (iwl_debug_level & IWL_DL_FW_ERRORS) {
+		iwl_dump_nic_error_log(priv);
+		iwl_dump_nic_event_log(priv);
+		iwl_print_rx_config_cmd(&priv->staging_rxon);
+	}
+#endif
+
+	wake_up_interruptible(&priv->wait_command_queue);
+
+	/* Keep the restart process from trying to send host
+	 * commands by clearing the INIT status bit */
+	clear_bit(STATUS_READY, &priv->status);
+
+	if (!test_bit(STATUS_EXIT_PENDING, &priv->status)) {
+		IWL_DEBUG(IWL_DL_INFO | IWL_DL_FW_ERRORS,
+			  "Restarting adapter due to uCode error.\n");
+
+		if (iwl_is_associated(priv)) {
+			memcpy(&priv->recovery_rxon, &priv->active_rxon,
+			       sizeof(priv->recovery_rxon));
+			priv->error_recovering = 1;
+		}
+		queue_work(priv->workqueue, &priv->restart);
+	}
+}
+
+static void iwl_error_recovery(struct iwl_priv *priv)
+{
+	unsigned long flags;
+
+	memcpy(&priv->staging_rxon, &priv->recovery_rxon,
+	       sizeof(priv->staging_rxon));
+	priv->staging_rxon.filter_flags &= ~RXON_FILTER_ASSOC_MSK;
+	iwl_commit_rxon(priv);
+
+	iwl_add_station(priv, priv->bssid, 1, 0);
+
+	spin_lock_irqsave(&priv->lock, flags);
+	priv->assoc_id = le16_to_cpu(priv->staging_rxon.assoc_id);
+	priv->error_recovering = 0;
+	spin_unlock_irqrestore(&priv->lock, flags);
+}
+
+static void iwl_irq_tasklet(struct iwl_priv *priv)
+{
+	u32 inta, handled = 0;
+	u32 inta_fh;
+	unsigned long flags;
+#ifdef CONFIG_IWLWIFI_DEBUG
+	u32 inta_mask;
+#endif
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	/* Ack/clear/reset pending uCode interrupts.
+	 * Note:  Some bits in CSR_INT are "OR" of bits in CSR_FH_INT_STATUS,
+	 *  and will clear only when CSR_FH_INT_STATUS gets cleared. */
+	inta = iwl_read32(priv, CSR_INT);
+	iwl_write32(priv, CSR_INT, inta);
+
+	/* Ack/clear/reset pending flow-handler (DMA) interrupts.
+	 * Any new interrupts that happen after this, either while we're
+	 * in this tasklet, or later, will show up in next ISR/tasklet. */
+	inta_fh = iwl_read32(priv, CSR_FH_INT_STATUS);
+	iwl_write32(priv, CSR_FH_INT_STATUS, inta_fh);
+
+#ifdef CONFIG_IWLWIFI_DEBUG
+	if (iwl_debug_level & IWL_DL_ISR) {
+		inta_mask = iwl_read32(priv, CSR_INT_MASK); /* just for debug */
+		IWL_DEBUG_ISR("inta 0x%08x, enabled 0x%08x, fh 0x%08x\n",
+			      inta, inta_mask, inta_fh);
+	}
+#endif
+
+	/* Since CSR_INT and CSR_FH_INT_STATUS reads and clears are not
+	 * atomic, make sure that inta covers all the interrupts that
+	 * we've discovered, even if FH interrupt came in just after
+	 * reading CSR_INT. */
+	if (inta_fh & CSR_FH_INT_RX_MASK)
+		inta |= CSR_INT_BIT_FH_RX;
+	if (inta_fh & CSR_FH_INT_TX_MASK)
+		inta |= CSR_INT_BIT_FH_TX;
+
+	/* Now service all interrupt bits discovered above. */
+	if (inta & CSR_INT_BIT_HW_ERR) {
+		IWL_ERROR("Microcode HW error detected.  Restarting.\n");
+
+		/* Tell the device to stop sending interrupts */
+		iwl_disable_interrupts(priv);
+
+		iwl_irq_handle_error(priv);
+
+		handled |= CSR_INT_BIT_HW_ERR;
+
+		spin_unlock_irqrestore(&priv->lock, flags);
+
+		return;
+	}
+
+#ifdef CONFIG_IWLWIFI_DEBUG
+	if (iwl_debug_level & (IWL_DL_ISR)) {
+		/* NIC fires this, but we don't use it, redundant with WAKEUP */
+		if (inta & CSR_INT_BIT_MAC_CLK_ACTV)
+			IWL_DEBUG_ISR("Microcode started or stopped.\n");
+
+		/* Alive notification via Rx interrupt will do the real work */
+		if (inta & CSR_INT_BIT_ALIVE)
+			IWL_DEBUG_ISR("Alive interrupt\n");
+	}
+#endif
+	/* Safely ignore these bits for debug checks below */
+	inta &= ~(CSR_INT_BIT_MAC_CLK_ACTV | CSR_INT_BIT_ALIVE);
+
+	/* HW RF KILL switch toggled (4965 only) */
+	if (inta & CSR_INT_BIT_RF_KILL) {
+		int hw_rf_kill = 0;
+		if (!(iwl_read32(priv, CSR_GP_CNTRL) &
+				CSR_GP_CNTRL_REG_FLAG_HW_RF_KILL_SW))
+			hw_rf_kill = 1;
+
+		IWL_DEBUG(IWL_DL_INFO | IWL_DL_RF_KILL | IWL_DL_ISR,
+				"RF_KILL bit toggled to %s.\n",
+				hw_rf_kill ? "disable radio":"enable radio");
+
+		/* Queue restart only if RF_KILL switch was set to "kill"
+		 *   when we loaded driver, and is now set to "enable".
+		 * After we're Alive, RF_KILL gets handled by
+		 *   iwl_rx_card_state_notif() */
+		if (!hw_rf_kill && !test_bit(STATUS_ALIVE, &priv->status))
+			queue_work(priv->workqueue, &priv->restart);
+
+		handled |= CSR_INT_BIT_RF_KILL;
+	}
+
+	/* Chip got too hot and stopped itself (4965 only) */
+	if (inta & CSR_INT_BIT_CT_KILL) {
+		IWL_ERROR("Microcode CT kill error detected.\n");
+		handled |= CSR_INT_BIT_CT_KILL;
+	}
+
+	/* Error detected by uCode */
+	if (inta & CSR_INT_BIT_SW_ERR) {
+		IWL_ERROR("Microcode SW error detected.  Restarting 0x%X.\n",
+			  inta);
+		iwl_irq_handle_error(priv);
+		handled |= CSR_INT_BIT_SW_ERR;
+	}
+
+	/* uCode wakes up after power-down sleep */
+	if (inta & CSR_INT_BIT_WAKEUP) {
+		IWL_DEBUG_ISR("Wakeup interrupt\n");
+		iwl_rx_queue_update_write_ptr(priv, &priv->rxq);
+		iwl_tx_queue_update_write_ptr(priv, &priv->txq[0]);
+		iwl_tx_queue_update_write_ptr(priv, &priv->txq[1]);
+		iwl_tx_queue_update_write_ptr(priv, &priv->txq[2]);
+		iwl_tx_queue_update_write_ptr(priv, &priv->txq[3]);
+		iwl_tx_queue_update_write_ptr(priv, &priv->txq[4]);
+		iwl_tx_queue_update_write_ptr(priv, &priv->txq[5]);
+
+		handled |= CSR_INT_BIT_WAKEUP;
+	}
+
+	/* All uCode command responses, including Tx command responses,
+	 * Rx "responses" (frame-received notification), and other
+	 * notifications from uCode come through here*/
+	if (inta & (CSR_INT_BIT_FH_RX | CSR_INT_BIT_SW_RX)) {
+		iwl_rx_handle(priv);
+		handled |= (CSR_INT_BIT_FH_RX | CSR_INT_BIT_SW_RX);
+	}
+
+	if (inta & CSR_INT_BIT_FH_TX) {
+		IWL_DEBUG_ISR("Tx interrupt\n");
+
+		iwl_write32(priv, CSR_FH_INT_STATUS, (1 << 6));
+		if (!iwl_grab_restricted_access(priv)) {
+			iwl_write_restricted(priv,
+					     FH_TCSR_CREDIT
+					     (ALM_FH_SRVC_CHNL), 0x0);
+			iwl_release_restricted_access(priv);
+		}
+		handled |= CSR_INT_BIT_FH_TX;
+	}
+
+	if (inta & ~handled)
+		IWL_ERROR("Unhandled INTA bits 0x%08x\n", inta & ~handled);
+
+	if (inta & ~CSR_INI_SET_MASK) {
+		IWL_WARNING("Disabled INTA bits 0x%08x were pending\n",
+			 inta & ~CSR_INI_SET_MASK);
+		IWL_WARNING("   with FH_INT = 0x%08x\n", inta_fh);
+	}
+
+	/* Re-enable all interrupts */
+	iwl_enable_interrupts(priv);
+
+#ifdef CONFIG_IWLWIFI_DEBUG
+	if (iwl_debug_level & (IWL_DL_ISR)) {
+		inta = iwl_read32(priv, CSR_INT);
+		inta_mask = iwl_read32(priv, CSR_INT_MASK);
+		inta_fh = iwl_read32(priv, CSR_FH_INT_STATUS);
+		IWL_DEBUG_ISR("End inta 0x%08x, enabled 0x%08x, fh 0x%08x, "
+			"flags 0x%08lx\n", inta, inta_mask, inta_fh, flags);
+	}
+#endif
+	spin_unlock_irqrestore(&priv->lock, flags);
+}
+
+static irqreturn_t iwl_isr(int irq, void *data)
+{
+	struct iwl_priv *priv = data;
+	u32 inta, inta_mask;
+	u32 inta_fh;
+	if (!priv)
+		return IRQ_NONE;
+
+	spin_lock(&priv->lock);
+
+	/* Disable (but don't clear!) interrupts here to avoid
+	 *    back-to-back ISRs and sporadic interrupts from our NIC.
+	 * If we have something to service, the tasklet will re-enable ints.
+	 * If we *don't* have something, we'll re-enable before leaving here. */
+	inta_mask = iwl_read32(priv, CSR_INT_MASK);  /* just for debug */
+	iwl_write32(priv, CSR_INT_MASK, 0x00000000);
+
+	/* Discover which interrupts are active/pending */
+	inta = iwl_read32(priv, CSR_INT);
+	inta_fh = iwl_read32(priv, CSR_FH_INT_STATUS);
+
+	/* Ignore interrupt if there's nothing in NIC to service.
+	 * This may be due to IRQ shared with another device,
+	 * or due to sporadic interrupts thrown from our NIC. */
+	if (!inta && !inta_fh) {
+		IWL_DEBUG_ISR("Ignore interrupt, inta == 0, inta_fh == 0\n");
+		goto none;
+	}
+
+	if ((inta == 0xFFFFFFFF) || ((inta & 0xFFFFFFF0) == 0xa5a5a5a0)) {
+		/* Hardware disappeared */
+		IWL_WARNING("HARDWARE GONE?? INTA == 0x%080x\n", inta);
+		goto none;
+	}
+
+	IWL_DEBUG_ISR("ISR inta 0x%08x, enabled 0x%08x, fh 0x%08x\n",
+		      inta, inta_mask, inta_fh);
+
+	/* iwl_irq_tasklet() will service interrupts and re-enable them */
+	tasklet_schedule(&priv->irq_tasklet);
+	spin_unlock(&priv->lock);
+
+	return IRQ_HANDLED;
+
+ none:
+	/* re-enable interrupts here since we don't have anything to service. */
+	iwl_enable_interrupts(priv);
+	spin_unlock(&priv->lock);
+	return IRQ_NONE;
+}
+
+/************************** EEPROM BANDS ****************************
+ *
+ * The iwl_eeprom_band definitions below provide the mapping from the
+ * EEPROM contents to the specific channel number supported for each
+ * band.
+ *
+ * For example, iwl_priv->eeprom.band_3_channels[4] from the band_3
+ * definition below maps to physical channel 42 in the 5.2GHz spectrum.
+ * The specific geography and calibration information for that channel
+ * is contained in the eeprom map itself.
+ *
+ * During init, we copy the eeprom information and channel map
+ * information into priv->channel_info_24/52 and priv->channel_map_24/52
+ *
+ * channel_map_24/52 provides the index in the channel_info array for a
+ * given channel.  We have to have two separate maps as there is channel
+ * overlap with the 2.4GHz and 5.2GHz spectrum as seen in band_1 and
+ * band_2
+ *
+ * A value of 0xff stored in the channel_map indicates that the channel
+ * is not supported by the hardware at all.
+ *
+ * A value of 0xfe in the channel_map indicates that the channel is not
+ * valid for Tx with the current hardware.  This means that
+ * while the system can tune and receive on a given channel, it may not
+ * be able to associate or transmit any frames on that
+ * channel.  There is no corresponding channel information for that
+ * entry.
+ *
+ *********************************************************************/
+
+/* 2.4 GHz */
+static const u8 iwl_eeprom_band_1[14] = {
+	1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14
+};
+
+/* 5.2 GHz bands */
+static const u8 iwl_eeprom_band_2[] = {
+	183, 184, 185, 187, 188, 189, 192, 196, 7, 8, 11, 12, 16
+};
+
+static const u8 iwl_eeprom_band_3[] = {	/* 5205-5320MHz */
+	34, 36, 38, 40, 42, 44, 46, 48, 52, 56, 60, 64
+};
+
+static const u8 iwl_eeprom_band_4[] = {	/* 5500-5700MHz */
+	100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140
+};
+
+static const u8 iwl_eeprom_band_5[] = {	/* 5725-5825MHz */
+	145, 149, 153, 157, 161, 165
+};
+
+static void iwl_init_band_reference(const struct iwl_priv *priv, int band,
+				    int *eeprom_ch_count,
+				    const struct iwl_eeprom_channel
+				    **eeprom_ch_info,
+				    const u8 **eeprom_ch_index)
+{
+	switch (band) {
+	case 1:		/* 2.4GHz band */
+		*eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_1);
+		*eeprom_ch_info = priv->eeprom.band_1_channels;
+		*eeprom_ch_index = iwl_eeprom_band_1;
+		break;
+	case 2:		/* 5.2GHz band */
+		*eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_2);
+		*eeprom_ch_info = priv->eeprom.band_2_channels;
+		*eeprom_ch_index = iwl_eeprom_band_2;
+		break;
+	case 3:		/* 5.2GHz band */
+		*eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_3);
+		*eeprom_ch_info = priv->eeprom.band_3_channels;
+		*eeprom_ch_index = iwl_eeprom_band_3;
+		break;
+	case 4:		/* 5.2GHz band */
+		*eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_4);
+		*eeprom_ch_info = priv->eeprom.band_4_channels;
+		*eeprom_ch_index = iwl_eeprom_band_4;
+		break;
+	case 5:		/* 5.2GHz band */
+		*eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_5);
+		*eeprom_ch_info = priv->eeprom.band_5_channels;
+		*eeprom_ch_index = iwl_eeprom_band_5;
+		break;
+	default:
+		BUG();
+		return;
+	}
+}
+
+const struct iwl_channel_info *iwl_get_channel_info(const struct iwl_priv *priv,
+						    int phymode, u16 channel)
+{
+	int i;
+
+	switch (phymode) {
+	case MODE_IEEE80211A:
+		for (i = 14; i < priv->channel_count; i++) {
+			if (priv->channel_info[i].channel == channel)
+				return &priv->channel_info[i];
+		}
+		break;
+
+	case MODE_IEEE80211B:
+	case MODE_IEEE80211G:
+		if (channel >= 1 && channel <= 14)
+			return &priv->channel_info[channel - 1];
+		break;
+
+	}
+
+	return NULL;
+}
+
+#define CHECK_AND_PRINT(x) ((eeprom_ch_info[ch].flags & EEPROM_CHANNEL_##x) \
+			    ? # x " " : "")
+
+static int iwl_init_channel_map(struct iwl_priv *priv)
+{
+	int eeprom_ch_count = 0;
+	const u8 *eeprom_ch_index = NULL;
+	const struct iwl_eeprom_channel *eeprom_ch_info = NULL;
+	int band, ch;
+	struct iwl_channel_info *ch_info;
+
+	if (priv->channel_count) {
+		IWL_DEBUG_INFO("Channel map already initialized.\n");
+		return 0;
+	}
+
+	if (priv->eeprom.version < 0x2f) {
+		IWL_WARNING("Unsupported EEPROM version: 0x%04X\n",
+			    priv->eeprom.version);
+		return -EINVAL;
+	}
+
+	IWL_DEBUG_INFO("Initializing regulatory info from EEPROM\n");
+
+	priv->channel_count =
+	    ARRAY_SIZE(iwl_eeprom_band_1) +
+	    ARRAY_SIZE(iwl_eeprom_band_2) +
+	    ARRAY_SIZE(iwl_eeprom_band_3) +
+	    ARRAY_SIZE(iwl_eeprom_band_4) +
+	    ARRAY_SIZE(iwl_eeprom_band_5);
+
+	IWL_DEBUG_INFO("Parsing data for %d channels.\n", priv->channel_count);
+
+	priv->channel_info = kzalloc(sizeof(struct iwl_channel_info) *
+				     priv->channel_count, GFP_KERNEL);
+	if (!priv->channel_info) {
+		IWL_ERROR("Could not allocate channel_info\n");
+		priv->channel_count = 0;
+		return -ENOMEM;
+	}
+
+	ch_info = priv->channel_info;
+
+	/* Loop through the 5 EEPROM bands adding them in order to the
+	 * channel map we maintain (that contains additional information than
+	 * what just in the EEPROM) */
+	for (band = 1; band <= 5; band++) {
+
+		iwl_init_band_reference(priv, band, &eeprom_ch_count,
+					&eeprom_ch_info, &eeprom_ch_index);
+
+		/* Loop through each band adding each of the channels */
+		for (ch = 0; ch < eeprom_ch_count; ch++) {
+			ch_info->channel = eeprom_ch_index[ch];
+			ch_info->phymode = (band == 1) ? MODE_IEEE80211B :
+			    MODE_IEEE80211A;
+
+			/* permanently store EEPROM's channel regulatory flags
+			 *   and max power in channel info database. */
+			ch_info->eeprom = eeprom_ch_info[ch];
+
+			/* Copy the run-time flags so they are there even on
+			 * invalid channels */
+			ch_info->flags = eeprom_ch_info[ch].flags;
+
+			if (!(is_channel_valid(ch_info))) {
+				IWL_DEBUG_INFO("Ch. %d Flags %x [%sGHz] - "
+					       "No traffic\n",
+					       ch_info->channel,
+					       ch_info->flags,
+					       is_channel_a_band(ch_info) ?
+					       "5.2" : "2.4");
+				ch_info++;
+				continue;
+			}
+
+			/* Initialize regulatory-based run-time data */
+			ch_info->max_power_avg = ch_info->curr_txpow =
+			    eeprom_ch_info[ch].max_power_avg;
+			ch_info->scan_power = eeprom_ch_info[ch].max_power_avg;
+			ch_info->min_power = 0;
+
+			IWL_DEBUG_INFO("Ch. %d [%sGHz] %s%s%s%s%s%s(0x%02x"
+				       " %ddBm): Ad-Hoc %ssupported\n",
+				       ch_info->channel,
+				       is_channel_a_band(ch_info) ?
+				       "5.2" : "2.4",
+				       CHECK_AND_PRINT(IBSS),
+				       CHECK_AND_PRINT(ACTIVE),
+				       CHECK_AND_PRINT(RADAR),
+				       CHECK_AND_PRINT(WIDE),
+				       CHECK_AND_PRINT(NARROW),
+				       CHECK_AND_PRINT(DFS),
+				       eeprom_ch_info[ch].flags,
+				       eeprom_ch_info[ch].max_power_avg,
+				       ((eeprom_ch_info[ch].
+					 flags & EEPROM_CHANNEL_IBSS)
+					&& !(eeprom_ch_info[ch].
+					     flags & EEPROM_CHANNEL_RADAR))
+				       ? "" : "not ");
+
+			/* Set the user_txpower_limit to the highest power
+			 * supported by any channel */
+			if (eeprom_ch_info[ch].max_power_avg >
+			    priv->user_txpower_limit)
+				priv->user_txpower_limit =
+				    eeprom_ch_info[ch].max_power_avg;
+
+			ch_info++;
+		}
+	}
+
+	if (iwl3945_txpower_set_from_eeprom(priv))
+		return -EIO;
+
+	return 0;
+}
+
+/* For active scan, listen ACTIVE_DWELL_TIME (msec) on each channel after
+ * sending probe req.  This should be set long enough to hear probe responses
+ * from more than one AP.  */
+#define IWL_ACTIVE_DWELL_TIME_24    (20)	/* all times in msec */
+#define IWL_ACTIVE_DWELL_TIME_52    (10)
+
+/* For faster active scanning, scan will move to the next channel if fewer than
+ * PLCP_QUIET_THRESH packets are heard on this channel within
+ * ACTIVE_QUIET_TIME after sending probe request.  This shortens the dwell
+ * time if it's a quiet channel (nothing responded to our probe, and there's
+ * no other traffic).
+ * Disable "quiet" feature by setting PLCP_QUIET_THRESH to 0. */
+#define IWL_PLCP_QUIET_THRESH       __constant_cpu_to_le16(1)	/* packets */
+#define IWL_ACTIVE_QUIET_TIME       __constant_cpu_to_le16(5)	/* msec */
+
+/* For passive scan, listen PASSIVE_DWELL_TIME (msec) on each channel.
+ * Must be set longer than active dwell time.
+ * For the most reliable scan, set > AP beacon interval (typically 100msec). */
+#define IWL_PASSIVE_DWELL_TIME_24   (20)	/* all times in msec */
+#define IWL_PASSIVE_DWELL_TIME_52   (10)
+#define IWL_PASSIVE_DWELL_BASE      (100)
+#define IWL_CHANNEL_TUNE_TIME       5
+
+static inline u16 iwl_get_active_dwell_time(struct iwl_priv *priv, int phymode)
+{
+	if (phymode == MODE_IEEE80211A)
+		return IWL_ACTIVE_DWELL_TIME_52;
+	else
+		return IWL_ACTIVE_DWELL_TIME_24;
+}
+
+static u16 iwl_get_passive_dwell_time(struct iwl_priv *priv, int phymode)
+{
+	u16 active = iwl_get_active_dwell_time(priv, phymode);
+	u16 passive = (phymode != MODE_IEEE80211A) ?
+	    IWL_PASSIVE_DWELL_BASE + IWL_PASSIVE_DWELL_TIME_24 :
+	    IWL_PASSIVE_DWELL_BASE + IWL_PASSIVE_DWELL_TIME_52;
+
+	if (iwl_is_associated(priv)) {
+		/* If we're associated, we clamp the maximum passive
+		 * dwell time to be 98% of the beacon interval (minus
+		 * 2 * channel tune time) */
+		passive = priv->beacon_int;
+		if ((passive > IWL_PASSIVE_DWELL_BASE) || !passive)
+			passive = IWL_PASSIVE_DWELL_BASE;
+		passive = (passive * 98) / 100 - IWL_CHANNEL_TUNE_TIME * 2;
+	}
+
+	if (passive <= active)
+		passive = active + 1;
+
+	return passive;
+}
+
+static int iwl_get_channels_for_scan(struct iwl_priv *priv, int phymode,
+				     u8 is_active, u8 direct_mask,
+				     struct iwl_scan_channel *scan_ch)
+{
+	const struct ieee80211_channel *channels = NULL;
+	const struct ieee80211_hw_mode *hw_mode;
+	const struct iwl_channel_info *ch_info;
+	u16 passive_dwell = 0;
+	u16 active_dwell = 0;
+	int added, i;
+
+	hw_mode = iwl_get_hw_mode(priv, phymode);
+	if (!hw_mode)
+		return 0;
+
+	channels = hw_mode->channels;
+
+	active_dwell = iwl_get_active_dwell_time(priv, phymode);
+	passive_dwell = iwl_get_passive_dwell_time(priv, phymode);
+
+	for (i = 0, added = 0; i < hw_mode->num_channels; i++) {
+		if (channels[i].chan ==
+		    le16_to_cpu(priv->active_rxon.channel)) {
+			if (iwl_is_associated(priv)) {
+				IWL_DEBUG_SCAN
+				    ("Skipping current channel %d\n",
+				     le16_to_cpu(priv->active_rxon.channel));
+				continue;
+			}
+		} else if (priv->only_active_channel)
+			continue;
+
+		scan_ch->channel = channels[i].chan;
+
+		ch_info = iwl_get_channel_info(priv, phymode, scan_ch->channel);
+		if (!is_channel_valid(ch_info)) {
+			IWL_DEBUG_SCAN("Channel %d is INVALID for this SKU.\n",
+				       scan_ch->channel);
+			continue;
+		}
+
+		if (!is_active || is_channel_passive(ch_info) ||
+		    !(channels[i].flag & IEEE80211_CHAN_W_ACTIVE_SCAN))
+			scan_ch->type = 0;	/* passive */
+		else
+			scan_ch->type = 1;	/* active */
+
+		if (scan_ch->type & 1)
+			scan_ch->type |= (direct_mask << 1);
+
+		if (is_channel_narrow(ch_info))
+			scan_ch->type |= (1 << 7);
+
+		scan_ch->active_dwell = cpu_to_le16(active_dwell);
+		scan_ch->passive_dwell = cpu_to_le16(passive_dwell);
+
+		/* Set power levels to defaults */
+		scan_ch->tpc.dsp_atten = 110;
+		/* scan_pwr_info->tpc.dsp_atten; */
+
+		/*scan_pwr_info->tpc.tx_gain; */
+		if (phymode == MODE_IEEE80211A)
+			scan_ch->tpc.tx_gain = ((1 << 5) | (3 << 3)) | 3;
+		else {
+			scan_ch->tpc.tx_gain = ((1 << 5) | (5 << 3));
+			/* NOTE: if we were doing 6Mb OFDM for scans we'd use
+			 * power level
+			 scan_ch->tpc.tx_gain = ((1<<5) | (2 << 3)) | 3;
+			 */
+		}
+
+		IWL_DEBUG_SCAN("Scanning %d [%s %d]\n",
+			       scan_ch->channel,
+			       (scan_ch->type & 1) ? "ACTIVE" : "PASSIVE",
+			       (scan_ch->type & 1) ?
+			       active_dwell : passive_dwell);
+
+		scan_ch++;
+		added++;
+	}
+
+	IWL_DEBUG_SCAN("total channels to scan %d \n", added);
+	return added;
+}
+
+static void iwl_reset_channel_flag(struct iwl_priv *priv)
+{
+	int i, j;
+	for (i = 0; i < 3; i++) {
+		struct ieee80211_hw_mode *hw_mode = (void *)&priv->modes[i];
+		for (j = 0; j < hw_mode->num_channels; j++)
+			hw_mode->channels[j].flag = hw_mode->channels[j].val;
+	}
+}
+
+static void iwl_init_hw_rates(struct iwl_priv *priv,
+			      struct ieee80211_rate *rates)
+{
+	int i;
+
+	for (i = 0; i < IWL_RATE_COUNT; i++) {
+		rates[i].rate = iwl_rates[i].ieee * 5;
+		rates[i].val = i; /* Rate scaling will work on indexes */
+		rates[i].val2 = i;
+		rates[i].flags = IEEE80211_RATE_SUPPORTED;
+		/* Only OFDM have the bits-per-symbol set */
+		if ((i <= IWL_LAST_OFDM_RATE) && (i >= IWL_FIRST_OFDM_RATE))
+			rates[i].flags |= IEEE80211_RATE_OFDM;
+		else {
+			/*
+			 * If CCK 1M then set rate flag to CCK else CCK_2
+			 * which is CCK | PREAMBLE2
+			 */
+			rates[i].flags |= (iwl_rates[i].plcp == 10) ?
+				IEEE80211_RATE_CCK : IEEE80211_RATE_CCK_2;
+		}
+
+		/* Set up which ones are basic rates... */
+		if (IWL_BASIC_RATES_MASK & (1 << i))
+			rates[i].flags |= IEEE80211_RATE_BASIC;
+	}
+}
+
+/**
+ * iwl_init_geos - Initialize mac80211's geo/channel info based from eeprom
+ */
+static int iwl_init_geos(struct iwl_priv *priv)
+{
+	struct iwl_channel_info *ch;
+	struct ieee80211_hw_mode *modes;
+	struct ieee80211_channel *channels;
+	struct ieee80211_channel *geo_ch;
+	struct ieee80211_rate *rates;
+	int i = 0;
+	enum {
+		A = 0,
+		B = 1,
+		G = 2,
+	};
+	int mode_count = 3;
+
+	if (priv->modes) {
+		IWL_DEBUG_INFO("Geography modes already initialized.\n");
+		set_bit(STATUS_GEO_CONFIGURED, &priv->status);
+		return 0;
+	}
+
+	modes = kzalloc(sizeof(struct ieee80211_hw_mode) * mode_count,
+			GFP_KERNEL);
+	if (!modes)
+		return -ENOMEM;
+
+	channels = kzalloc(sizeof(struct ieee80211_channel) *
+			   priv->channel_count, GFP_KERNEL);
+	if (!channels) {
+		kfree(modes);
+		return -ENOMEM;
+	}
+
+	rates = kzalloc((sizeof(struct ieee80211_rate) * (IWL_MAX_RATES + 1)),
+			GFP_KERNEL);
+	if (!rates) {
+		kfree(modes);
+		kfree(channels);
+		return -ENOMEM;
+	}
+
+	/* 0 = 802.11a
+	 * 1 = 802.11b
+	 * 2 = 802.11g
+	 */
+
+	/* 5.2GHz channels start after the 2.4GHz channels */
+	modes[A].mode = MODE_IEEE80211A;
+	modes[A].channels = &channels[ARRAY_SIZE(iwl_eeprom_band_1)];
+	modes[A].rates = rates;
+	modes[A].num_rates = 8;	/* just OFDM */
+	modes[A].num_channels = 0;
+
+	modes[B].mode = MODE_IEEE80211B;
+	modes[B].channels = channels;
+	modes[B].rates = &rates[8];
+	modes[B].num_rates = 4;	/* just CCK */
+	modes[B].num_channels = 0;
+
+	modes[G].mode = MODE_IEEE80211G;
+	modes[G].channels = channels;
+	modes[G].rates = rates;
+	modes[G].num_rates = 12;	/* OFDM & CCK */
+	modes[G].num_channels = 0;
+
+	priv->ieee_channels = channels;
+	priv->ieee_rates = rates;
+
+	iwl_init_hw_rates(priv, rates);
+
+	for (i = 0, geo_ch = channels; i < priv->channel_count; i++) {
+		ch = &priv->channel_info[i];
+
+		if (!is_channel_valid(ch)) {
+			IWL_DEBUG_INFO("Channel %d [%sGHz] is restricted -- "
+				    "skipping.\n",
+				    ch->channel, is_channel_a_band(ch) ?
+				    "5.2" : "2.4");
+			continue;
+		}
+
+		if (is_channel_a_band(ch))
+			geo_ch = &modes[A].channels[modes[A].num_channels++];
+		else {
+			geo_ch = &modes[B].channels[modes[B].num_channels++];
+			modes[G].num_channels++;
+		}
+
+		geo_ch->freq = ieee80211chan2mhz(ch->channel);
+		geo_ch->chan = ch->channel;
+		geo_ch->power_level = ch->max_power_avg;
+		geo_ch->antenna_max = 0xff;
+
+		if (is_channel_valid(ch)) {
+			geo_ch->flag = IEEE80211_CHAN_W_SCAN;
+			if (ch->flags & EEPROM_CHANNEL_IBSS)
+				geo_ch->flag |= IEEE80211_CHAN_W_IBSS;
+
+			if (ch->flags & EEPROM_CHANNEL_ACTIVE)
+				geo_ch->flag |= IEEE80211_CHAN_W_ACTIVE_SCAN;
+
+			if (ch->flags & EEPROM_CHANNEL_RADAR)
+				geo_ch->flag |= IEEE80211_CHAN_W_RADAR_DETECT;
+
+			if (ch->max_power_avg > priv->max_channel_txpower_limit)
+				priv->max_channel_txpower_limit =
+				    ch->max_power_avg;
+		}
+
+		geo_ch->val = geo_ch->flag;
+	}
+
+	if ((modes[A].num_channels == 0) && priv->is_abg) {
+		printk(KERN_INFO DRV_NAME
+		       ": Incorrectly detected BG card as ABG.  Please send "
+		       "your PCI ID 0x%04X:0x%04X to maintainer.\n",
+		       priv->pci_dev->device, priv->pci_dev->subsystem_device);
+		priv->is_abg = 0;
+	}
+
+	printk(KERN_INFO DRV_NAME
+	       ": Tunable channels: %d 802.11bg, %d 802.11a channels\n",
+	       modes[G].num_channels, modes[A].num_channels);
+
+	/*
+	 * NOTE:  We register these in preference of order -- the
+	 * stack doesn't currently (as of 7.0.6 / Apr 24 '07) pick
+	 * a phymode based on rates or AP capabilities but seems to
+	 * configure it purely on if the channel being configured
+	 * is supported by a mode -- and the first match is taken
+	 */
+
+	if (modes[G].num_channels)
+		ieee80211_register_hwmode(priv->hw, &modes[G]);
+	if (modes[B].num_channels)
+		ieee80211_register_hwmode(priv->hw, &modes[B]);
+	if (modes[A].num_channels)
+		ieee80211_register_hwmode(priv->hw, &modes[A]);
+
+	priv->modes = modes;
+	set_bit(STATUS_GEO_CONFIGURED, &priv->status);
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ * uCode download functions
+ *
+ ******************************************************************************/
+
+static void iwl_dealloc_ucode_pci(struct iwl_priv *priv)
+{
+	if (priv->ucode_code.v_addr != NULL) {
+		pci_free_consistent(priv->pci_dev,
+				    priv->ucode_code.len,
+				    priv->ucode_code.v_addr,
+				    priv->ucode_code.p_addr);
+		priv->ucode_code.v_addr = NULL;
+	}
+	if (priv->ucode_data.v_addr != NULL) {
+		pci_free_consistent(priv->pci_dev,
+				    priv->ucode_data.len,
+				    priv->ucode_data.v_addr,
+				    priv->ucode_data.p_addr);
+		priv->ucode_data.v_addr = NULL;
+	}
+	if (priv->ucode_data_backup.v_addr != NULL) {
+		pci_free_consistent(priv->pci_dev,
+				    priv->ucode_data_backup.len,
+				    priv->ucode_data_backup.v_addr,
+				    priv->ucode_data_backup.p_addr);
+		priv->ucode_data_backup.v_addr = NULL;
+	}
+	if (priv->ucode_init.v_addr != NULL) {
+		pci_free_consistent(priv->pci_dev,
+				    priv->ucode_init.len,
+				    priv->ucode_init.v_addr,
+				    priv->ucode_init.p_addr);
+		priv->ucode_init.v_addr = NULL;
+	}
+	if (priv->ucode_init_data.v_addr != NULL) {
+		pci_free_consistent(priv->pci_dev,
+				    priv->ucode_init_data.len,
+				    priv->ucode_init_data.v_addr,
+				    priv->ucode_init_data.p_addr);
+		priv->ucode_init_data.v_addr = NULL;
+	}
+	if (priv->ucode_boot.v_addr != NULL) {
+		pci_free_consistent(priv->pci_dev,
+				    priv->ucode_boot.len,
+				    priv->ucode_boot.v_addr,
+				    priv->ucode_boot.p_addr);
+		priv->ucode_boot.v_addr = NULL;
+	}
+}
+
+/**
+ * iwl_verify_inst_full - verify runtime uCode image in card vs. host,
+ *     looking at all data.
+ */
+static int iwl_verify_inst_full(struct iwl_priv *priv, __le32 * image, u32 len)
+{
+	u32 val;
+	u32 save_len = len;
+	int rc = 0;
+	u32 errcnt;
+
+	IWL_DEBUG_INFO("ucode inst image size is %u\n", len);
+
+	rc = iwl_grab_restricted_access(priv);
+	if (rc)
+		return rc;
+
+	iwl_write_restricted(priv, HBUS_TARG_MEM_RADDR, RTC_INST_LOWER_BOUND);
+
+	errcnt = 0;
+	for (; len > 0; len -= sizeof(u32), image++) {
+		/* read data comes through single port, auto-incr addr */
+		/* NOTE: Use the debugless read so we don't flood kernel log
+		 * if IWL_DL_IO is set */
+		val = _iwl_read_restricted(priv, HBUS_TARG_MEM_RDAT);
+		if (val != le32_to_cpu(*image)) {
+			IWL_ERROR("uCode INST section is invalid at "
+				  "offset 0x%x, is 0x%x, s/b 0x%x\n",
+				  save_len - len, val, le32_to_cpu(*image));
+			rc = -EIO;
+			errcnt++;
+			if (errcnt >= 20)
+				break;
+		}
+	}
+
+	iwl_release_restricted_access(priv);
+
+	if (!errcnt)
+		IWL_DEBUG_INFO
+		    ("ucode image in INSTRUCTION memory is good\n");
+
+	return rc;
+}
+
+
+/**
+ * iwl_verify_inst_sparse - verify runtime uCode image in card vs. host,
+ *   using sample data 100 bytes apart.  If these sample points are good,
+ *   it's a pretty good bet that everything between them is good, too.
+ */
+static int iwl_verify_inst_sparse(struct iwl_priv *priv, __le32 *image, u32 len)
+{
+	u32 val;
+	int rc = 0;
+	u32 errcnt = 0;
+	u32 i;
+
+	IWL_DEBUG_INFO("ucode inst image size is %u\n", len);
+
+	rc = iwl_grab_restricted_access(priv);
+	if (rc)
+		return rc;
+
+	for (i = 0; i < len; i += 100, image += 100/sizeof(u32)) {
+		/* read data comes through single port, auto-incr addr */
+		/* NOTE: Use the debugless read so we don't flood kernel log
+		 * if IWL_DL_IO is set */
+		iwl_write_restricted(priv, HBUS_TARG_MEM_RADDR,
+			i + RTC_INST_LOWER_BOUND);
+		val = _iwl_read_restricted(priv, HBUS_TARG_MEM_RDAT);
+		if (val != le32_to_cpu(*image)) {
+#if 0 /* Enable this if you want to see details */
+			IWL_ERROR("uCode INST section is invalid at "
+				  "offset 0x%x, is 0x%x, s/b 0x%x\n",
+				  i, val, *image);
+#endif
+			rc = -EIO;
+			errcnt++;
+			if (errcnt >= 3)
+				break;
+		}
+	}
+
+	iwl_release_restricted_access(priv);
+
+	return rc;
+}
+
+
+/**
+ * iwl_verify_ucode - determine which instruction image is in SRAM,
+ *    and verify its contents
+ */
+static int iwl_verify_ucode(struct iwl_priv *priv)
+{
+	__le32 *image;
+	u32 len;
+	int rc = 0;
+
+	/* Try bootstrap */
+	image = (__le32 *)priv->ucode_boot.v_addr;
+	len = priv->ucode_boot.len;
+	rc = iwl_verify_inst_sparse(priv, image, len);
+	if (rc == 0) {
+		IWL_DEBUG_INFO("Bootstrap uCode is good in inst SRAM\n");
+		return 0;
+	}
+
+	/* Try initialize */
+	image = (__le32 *)priv->ucode_init.v_addr;
+	len = priv->ucode_init.len;
+	rc = iwl_verify_inst_sparse(priv, image, len);
+	if (rc == 0) {
+		IWL_DEBUG_INFO("Initialize uCode is good in inst SRAM\n");
+		return 0;
+	}
+
+	/* Try runtime/protocol */
+	image = (__le32 *)priv->ucode_code.v_addr;
+	len = priv->ucode_code.len;
+	rc = iwl_verify_inst_sparse(priv, image, len);
+	if (rc == 0) {
+		IWL_DEBUG_INFO("Runtime uCode is good in inst SRAM\n");
+		return 0;
+	}
+
+	IWL_ERROR("NO VALID UCODE IMAGE IN INSTRUCTION SRAM!!\n");
+
+	/* Show first several data entries in instruction SRAM.
+	 * Selection of bootstrap image is arbitrary. */
+	image = (__le32 *)priv->ucode_boot.v_addr;
+	len = priv->ucode_boot.len;
+	rc = iwl_verify_inst_full(priv, image, len);
+
+	return rc;
+}
+
+
+/* check contents of special bootstrap uCode SRAM */
+static int iwl_verify_bsm(struct iwl_priv *priv)
+{
+	__le32 *image = priv->ucode_boot.v_addr;
+	u32 len = priv->ucode_boot.len;
+	u32 reg;
+	u32 val;
+
+	IWL_DEBUG_INFO("Begin verify bsm\n");
+
+	/* verify BSM SRAM contents */
+	val = iwl_read_restricted_reg(priv, BSM_WR_DWCOUNT_REG);
+	for (reg = BSM_SRAM_LOWER_BOUND;
+	     reg < BSM_SRAM_LOWER_BOUND + len;
+	     reg += sizeof(u32), image ++) {
+		val = iwl_read_restricted_reg(priv, reg);
+		if (val != le32_to_cpu(*image)) {
+			IWL_ERROR("BSM uCode verification failed at "
+				  "addr 0x%08X+%u (of %u), is 0x%x, s/b 0x%x\n",
+				  BSM_SRAM_LOWER_BOUND,
+				  reg - BSM_SRAM_LOWER_BOUND, len,
+				  val, le32_to_cpu(*image));
+			return -EIO;
+		}
+	}
+
+	IWL_DEBUG_INFO("BSM bootstrap uCode image OK\n");
+
+	return 0;
+}
+
+/**
+ * iwl_load_bsm - Load bootstrap instructions
+ *
+ * BSM operation:
+ *
+ * The Bootstrap State Machine (BSM) stores a short bootstrap uCode program
+ * in special SRAM that does not power down during RFKILL.  When powering back
+ * up after power-saving sleeps (or during initial uCode load), the BSM loads
+ * the bootstrap program into the on-board processor, and starts it.
+ *
+ * The bootstrap program loads (via DMA) instructions and data for a new
+ * program from host DRAM locations indicated by the host driver in the
+ * BSM_DRAM_* registers.  Once the new program is loaded, it starts
+ * automatically.
+ *
+ * When initializing the NIC, the host driver points the BSM to the
+ * "initialize" uCode image.  This uCode sets up some internal data, then
+ * notifies host via "initialize alive" that it is complete.
+ *
+ * The host then replaces the BSM_DRAM_* pointer values to point to the
+ * normal runtime uCode instructions and a backup uCode data cache buffer
+ * (filled initially with starting data values for the on-board processor),
+ * then triggers the "initialize" uCode to load and launch the runtime uCode,
+ * which begins normal operation.
+ *
+ * When doing a power-save shutdown, runtime uCode saves data SRAM into
+ * the backup data cache in DRAM before SRAM is powered down.
+ *
+ * When powering back up, the BSM loads the bootstrap program.  This reloads
+ * the runtime uCode instructions and the backup data cache into SRAM,
+ * and re-launches the runtime uCode from where it left off.
+ */
+static int iwl_load_bsm(struct iwl_priv *priv)
+{
+	__le32 *image = priv->ucode_boot.v_addr;
+	u32 len = priv->ucode_boot.len;
+	dma_addr_t pinst;
+	dma_addr_t pdata;
+	u32 inst_len;
+	u32 data_len;
+	int rc;
+	int i;
+	u32 done;
+	u32 reg_offset;
+
+	IWL_DEBUG_INFO("Begin load bsm\n");
+
+	/* make sure bootstrap program is no larger than BSM's SRAM size */
+	if (len > IWL_MAX_BSM_SIZE)
+		return -EINVAL;
+
+	/* Tell bootstrap uCode where to find the "Initialize" uCode
+	 *   in host DRAM ... bits 31:0 for 3945, bits 35:4 for 4965.
+	 * NOTE:  iwl_initialize_alive_start() will replace these values,
+	 *        after the "initialize" uCode has run, to point to
+	 *        runtime/protocol instructions and backup data cache. */
+	pinst = priv->ucode_init.p_addr;
+	pdata = priv->ucode_init_data.p_addr;
+	inst_len = priv->ucode_init.len;
+	data_len = priv->ucode_init_data.len;
+
+	rc = iwl_grab_restricted_access(priv);
+	if (rc)
+		return rc;
+
+	iwl_write_restricted_reg(priv, BSM_DRAM_INST_PTR_REG, pinst);
+	iwl_write_restricted_reg(priv, BSM_DRAM_DATA_PTR_REG, pdata);
+	iwl_write_restricted_reg(priv, BSM_DRAM_INST_BYTECOUNT_REG, inst_len);
+	iwl_write_restricted_reg(priv, BSM_DRAM_DATA_BYTECOUNT_REG, data_len);
+
+	/* Fill BSM memory with bootstrap instructions */
+	for (reg_offset = BSM_SRAM_LOWER_BOUND;
+	     reg_offset < BSM_SRAM_LOWER_BOUND + len;
+	     reg_offset += sizeof(u32), image++)
+		_iwl_write_restricted_reg(priv, reg_offset,
+					  le32_to_cpu(*image));
+
+	rc = iwl_verify_bsm(priv);
+	if (rc) {
+		iwl_release_restricted_access(priv);
+		return rc;
+	}
+
+	/* Tell BSM to copy from BSM SRAM into instruction SRAM, when asked */
+	iwl_write_restricted_reg(priv, BSM_WR_MEM_SRC_REG, 0x0);
+	iwl_write_restricted_reg(priv, BSM_WR_MEM_DST_REG,
+				 RTC_INST_LOWER_BOUND);
+	iwl_write_restricted_reg(priv, BSM_WR_DWCOUNT_REG, len / sizeof(u32));
+
+	/* Load bootstrap code into instruction SRAM now,
+	 *   to prepare to load "initialize" uCode */
+	iwl_write_restricted_reg(priv, BSM_WR_CTRL_REG,
+		BSM_WR_CTRL_REG_BIT_START);
+
+	/* Wait for load of bootstrap uCode to finish */
+	for (i = 0; i < 100; i++) {
+		done = iwl_read_restricted_reg(priv, BSM_WR_CTRL_REG);
+		if (!(done & BSM_WR_CTRL_REG_BIT_START))
+			break;
+		udelay(10);
+	}
+	if (i < 100)
+		IWL_DEBUG_INFO("BSM write complete, poll %d iterations\n", i);
+	else {
+		IWL_ERROR("BSM write did not complete!\n");
+		return -EIO;
+	}
+
+	/* Enable future boot loads whenever power management unit triggers it
+	 *   (e.g. when powering back up after power-save shutdown) */
+	iwl_write_restricted_reg(priv, BSM_WR_CTRL_REG,
+		BSM_WR_CTRL_REG_BIT_START_EN);
+
+	iwl_release_restricted_access(priv);
+
+	return 0;
+}
+
+static void iwl_nic_start(struct iwl_priv *priv)
+{
+	/* Remove all resets to allow NIC to operate */
+	iwl_write32(priv, CSR_RESET, 0);
+}
+
+/**
+ * iwl_read_ucode - Read uCode images from disk file.
+ *
+ * Copy into buffers for card to fetch via bus-mastering
+ */
+static int iwl_read_ucode(struct iwl_priv *priv)
+{
+	struct iwl_ucode *ucode;
+	int rc = 0;
+	const struct firmware *ucode_raw;
+	/* firmware file name contains uCode/driver compatibility version */
+	const char *name = "iwlwifi-3945" IWL3945_UCODE_API ".ucode";
+	u8 *src;
+	size_t len;
+	u32 ver, inst_size, data_size, init_size, init_data_size, boot_size;
+
+	/* Ask kernel firmware_class module to get the boot firmware off disk.
+	 * request_firmware() is synchronous, file is in memory on return. */
+	rc = request_firmware(&ucode_raw, name, &priv->pci_dev->dev);
+	if (rc < 0) {
+		IWL_ERROR("%s firmware file req failed: Reason %d\n", name, rc);
+		goto error;
+	}
+
+	IWL_DEBUG_INFO("Got firmware '%s' file (%zd bytes) from disk\n",
+		       name, ucode_raw->size);
+
+	/* Make sure that we got at least our header! */
+	if (ucode_raw->size < sizeof(*ucode)) {
+		IWL_ERROR("File size way too small!\n");
+		rc = -EINVAL;
+		goto err_release;
+	}
+
+	/* Data from ucode file:  header followed by uCode images */
+	ucode = (void *)ucode_raw->data;
+
+	ver = le32_to_cpu(ucode->ver);
+	inst_size = le32_to_cpu(ucode->inst_size);
+	data_size = le32_to_cpu(ucode->data_size);
+	init_size = le32_to_cpu(ucode->init_size);
+	init_data_size = le32_to_cpu(ucode->init_data_size);
+	boot_size = le32_to_cpu(ucode->boot_size);
+
+	IWL_DEBUG_INFO("f/w package hdr ucode version = 0x%x\n", ver);
+	IWL_DEBUG_INFO("f/w package hdr runtime inst size = %u\n",
+		       inst_size);
+	IWL_DEBUG_INFO("f/w package hdr runtime data size = %u\n",
+		       data_size);
+	IWL_DEBUG_INFO("f/w package hdr init inst size = %u\n",
+		       init_size);
+	IWL_DEBUG_INFO("f/w package hdr init data size = %u\n",
+		       init_data_size);
+	IWL_DEBUG_INFO("f/w package hdr boot inst size = %u\n",
+		       boot_size);
+
+	/* Verify size of file vs. image size info in file's header */
+	if (ucode_raw->size < sizeof(*ucode) +
+		inst_size + data_size + init_size +
+		init_data_size + boot_size) {
+
+		IWL_DEBUG_INFO("uCode file size %d too small\n",
+			       (int)ucode_raw->size);
+		rc = -EINVAL;
+		goto err_release;
+	}
+
+	/* Verify that uCode images will fit in card's SRAM */
+	if (inst_size > IWL_MAX_INST_SIZE) {
+		IWL_DEBUG_INFO("uCode instr len %d too large to fit in card\n",
+			       (int)inst_size);
+		rc = -EINVAL;
+		goto err_release;
+	}
+
+	if (data_size > IWL_MAX_DATA_SIZE) {
+		IWL_DEBUG_INFO("uCode data len %d too large to fit in card\n",
+			       (int)data_size);
+		rc = -EINVAL;
+		goto err_release;
+	}
+	if (init_size > IWL_MAX_INST_SIZE) {
+		IWL_DEBUG_INFO
+		    ("uCode init instr len %d too large to fit in card\n",
+		     (int)init_size);
+		rc = -EINVAL;
+		goto err_release;
+	}
+	if (init_data_size > IWL_MAX_DATA_SIZE) {
+		IWL_DEBUG_INFO
+		    ("uCode init data len %d too large to fit in card\n",
+		     (int)init_data_size);
+		rc = -EINVAL;
+		goto err_release;
+	}
+	if (boot_size > IWL_MAX_BSM_SIZE) {
+		IWL_DEBUG_INFO
+		    ("uCode boot instr len %d too large to fit in bsm\n",
+		     (int)boot_size);
+		rc = -EINVAL;
+		goto err_release;
+	}
+
+	/* Allocate ucode buffers for card's bus-master loading ... */
+
+	/* Runtime instructions and 2 copies of data:
+	 * 1) unmodified from disk
+	 * 2) backup cache for save/restore during power-downs */
+	priv->ucode_code.len = inst_size;
+	priv->ucode_code.v_addr =
+	    pci_alloc_consistent(priv->pci_dev,
+				 priv->ucode_code.len,
+				 &(priv->ucode_code.p_addr));
+
+	priv->ucode_data.len = data_size;
+	priv->ucode_data.v_addr =
+	    pci_alloc_consistent(priv->pci_dev,
+				 priv->ucode_data.len,
+				 &(priv->ucode_data.p_addr));
+
+	priv->ucode_data_backup.len = data_size;
+	priv->ucode_data_backup.v_addr =
+	    pci_alloc_consistent(priv->pci_dev,
+				 priv->ucode_data_backup.len,
+				 &(priv->ucode_data_backup.p_addr));
+
+
+	/* Initialization instructions and data */
+	priv->ucode_init.len = init_size;
+	priv->ucode_init.v_addr =
+	    pci_alloc_consistent(priv->pci_dev,
+				 priv->ucode_init.len,
+				 &(priv->ucode_init.p_addr));
+
+	priv->ucode_init_data.len = init_data_size;
+	priv->ucode_init_data.v_addr =
+	    pci_alloc_consistent(priv->pci_dev,
+				 priv->ucode_init_data.len,
+				 &(priv->ucode_init_data.p_addr));
+
+	/* Bootstrap (instructions only, no data) */
+	priv->ucode_boot.len = boot_size;
+	priv->ucode_boot.v_addr =
+	    pci_alloc_consistent(priv->pci_dev,
+				 priv->ucode_boot.len,
+				 &(priv->ucode_boot.p_addr));
+
+	if (!priv->ucode_code.v_addr || !priv->ucode_data.v_addr ||
+	    !priv->ucode_init.v_addr || !priv->ucode_init_data.v_addr ||
+	    !priv->ucode_boot.v_addr || !priv->ucode_data_backup.v_addr)
+		goto err_pci_alloc;
+
+	/* Copy images into buffers for card's bus-master reads ... */
+
+	/* Runtime instructions (first block of data in file) */
+	src = &ucode->data[0];
+	len = priv->ucode_code.len;
+	IWL_DEBUG_INFO("Copying (but not loading) uCode instr len %d\n",
+		       (int)len);
+	memcpy(priv->ucode_code.v_addr, src, len);
+	IWL_DEBUG_INFO("uCode instr buf vaddr = 0x%p, paddr = 0x%08x\n",
+		priv->ucode_code.v_addr, (u32)priv->ucode_code.p_addr);
+
+	/* Runtime data (2nd block)
+	 * NOTE:  Copy into backup buffer will be done in iwl_up()  */
+	src = &ucode->data[inst_size];
+	len = priv->ucode_data.len;
+	IWL_DEBUG_INFO("Copying (but not loading) uCode data len %d\n",
+		       (int)len);
+	memcpy(priv->ucode_data.v_addr, src, len);
+	memcpy(priv->ucode_data_backup.v_addr, src, len);
+
+	/* Initialization instructions (3rd block) */
+	if (init_size) {
+		src = &ucode->data[inst_size + data_size];
+		len = priv->ucode_init.len;
+		IWL_DEBUG_INFO("Copying (but not loading) init instr len %d\n",
+			       (int)len);
+		memcpy(priv->ucode_init.v_addr, src, len);
+	}
+
+	/* Initialization data (4th block) */
+	if (init_data_size) {
+		src = &ucode->data[inst_size + data_size + init_size];
+		len = priv->ucode_init_data.len;
+		IWL_DEBUG_INFO("Copying (but not loading) init data len %d\n",
+			       (int)len);
+		memcpy(priv->ucode_init_data.v_addr, src, len);
+	}
+
+	/* Bootstrap instructions (5th block) */
+	src = &ucode->data[inst_size + data_size + init_size + init_data_size];
+	len = priv->ucode_boot.len;
+	IWL_DEBUG_INFO("Copying (but not loading) boot instr len %d\n",
+		       (int)len);
+	memcpy(priv->ucode_boot.v_addr, src, len);
+
+	/* We have our copies now, allow OS release its copies */
+	release_firmware(ucode_raw);
+	return 0;
+
+ err_pci_alloc:
+	IWL_ERROR("failed to allocate pci memory\n");
+	rc = -ENOMEM;
+	iwl_dealloc_ucode_pci(priv);
+
+ err_release:
+	release_firmware(ucode_raw);
+
+ error:
+	return rc;
+}
+
+
+/**
+ * iwl_set_ucode_ptrs - Set uCode address location
+ *
+ * Tell initialization uCode where to find runtime uCode.
+ *
+ * BSM registers initially contain pointers to initialization uCode.
+ * We need to replace them to load runtime uCode inst and data,
+ * and to save runtime data when powering down.
+ */
+static int iwl_set_ucode_ptrs(struct iwl_priv *priv)
+{
+	dma_addr_t pinst;
+	dma_addr_t pdata;
+	int rc = 0;
+	unsigned long flags;
+
+	/* bits 31:0 for 3945 */
+	pinst = priv->ucode_code.p_addr;
+	pdata = priv->ucode_data_backup.p_addr;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	rc = iwl_grab_restricted_access(priv);
+	if (rc) {
+		spin_unlock_irqrestore(&priv->lock, flags);
+		return rc;
+	}
+
+	/* Tell bootstrap uCode where to find image to load */
+	iwl_write_restricted_reg(priv, BSM_DRAM_INST_PTR_REG, pinst);
+	iwl_write_restricted_reg(priv, BSM_DRAM_DATA_PTR_REG, pdata);
+	iwl_write_restricted_reg(priv, BSM_DRAM_DATA_BYTECOUNT_REG,
+				 priv->ucode_data.len);
+
+	/* Inst bytecount must be last to set up, bit 31 signals uCode
+	 *   that all new ptr/size info is in place */
+	iwl_write_restricted_reg(priv, BSM_DRAM_INST_BYTECOUNT_REG,
+				 priv->ucode_code.len | BSM_DRAM_INST_LOAD);
+
+	iwl_release_restricted_access(priv);
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	IWL_DEBUG_INFO("Runtime uCode pointers are set.\n");
+
+	return rc;
+}
+
+/**
+ * iwl_init_alive_start - Called after REPLY_ALIVE notification receieved
+ *
+ * Called after REPLY_ALIVE notification received from "initialize" uCode.
+ *
+ * The 4965 "initialize" ALIVE reply contains calibration data for:
+ *   Voltage, temperature, and MIMO tx gain correction, now stored in priv
+ *   (3945 does not contain this data).
+ *
+ * Tell "initialize" uCode to go ahead and load the runtime uCode.
+*/
+static void iwl_init_alive_start(struct iwl_priv *priv)
+{
+	/* Check alive response for "valid" sign from uCode */
+	if (priv->card_alive_init.is_valid != UCODE_VALID_OK) {
+		/* We had an error bringing up the hardware, so take it
+		 * all the way back down so we can try again */
+		IWL_DEBUG_INFO("Initialize Alive failed.\n");
+		goto restart;
+	}
+
+	/* Bootstrap uCode has loaded initialize uCode ... verify inst image.
+	 * This is a paranoid check, because we would not have gotten the
+	 * "initialize" alive if code weren't properly loaded.  */
+	if (iwl_verify_ucode(priv)) {
+		/* Runtime instruction load was bad;
+		 * take it all the way back down so we can try again */
+		IWL_DEBUG_INFO("Bad \"initialize\" uCode load.\n");
+		goto restart;
+	}
+
+	/* Send pointers to protocol/runtime uCode image ... init code will
+	 * load and launch runtime uCode, which will send us another "Alive"
+	 * notification. */
+	IWL_DEBUG_INFO("Initialization Alive received.\n");
+	if (iwl_set_ucode_ptrs(priv)) {
+		/* Runtime instruction load won't happen;
+		 * take it all the way back down so we can try again */
+		IWL_DEBUG_INFO("Couldn't set up uCode pointers.\n");
+		goto restart;
+	}
+	return;
+
+ restart:
+	queue_work(priv->workqueue, &priv->restart);
+}
+
+
+/**
+ * iwl_alive_start - called after REPLY_ALIVE notification received
+ *                   from protocol/runtime uCode (initialization uCode's
+ *                   Alive gets handled by iwl_init_alive_start()).
+ */
+static void iwl_alive_start(struct iwl_priv *priv)
+{
+	int rc = 0;
+	int thermal_spin = 0;
+	u32 rfkill;
+
+	IWL_DEBUG_INFO("Runtime Alive received.\n");
+
+	if (priv->card_alive.is_valid != UCODE_VALID_OK) {
+		/* We had an error bringing up the hardware, so take it
+		 * all the way back down so we can try again */
+		IWL_DEBUG_INFO("Alive failed.\n");
+		goto restart;
+	}
+
+	/* Initialize uCode has loaded Runtime uCode ... verify inst image.
+	 * This is a paranoid check, because we would not have gotten the
+	 * "runtime" alive if code weren't properly loaded.  */
+	if (iwl_verify_ucode(priv)) {
+		/* Runtime instruction load was bad;
+		 * take it all the way back down so we can try again */
+		IWL_DEBUG_INFO("Bad runtime uCode load.\n");
+		goto restart;
+	}
+
+	iwl_clear_stations_table(priv);
+
+	rc = iwl_grab_restricted_access(priv);
+	if (rc) {
+		IWL_WARNING("Can not read rfkill status from adapter\n");
+		return;
+	}
+
+	rfkill = iwl_read_restricted_reg(priv, APMG_RFKILL_REG);
+	IWL_DEBUG_INFO("RFKILL status: 0x%x\n", rfkill);
+	iwl_release_restricted_access(priv);
+
+	if (rfkill & 0x1) {
+		clear_bit(STATUS_RF_KILL_HW, &priv->status);
+		/* if rfkill is not on, then wait for thermal
+		 * sensor in adapter to kick in */
+		while (iwl_hw_get_temperature(priv) == 0) {
+			thermal_spin++;
+			udelay(10);
+		}
+
+		if (thermal_spin)
+			IWL_DEBUG_INFO("Thermal calibration took %dus\n",
+				       thermal_spin * 10);
+	} else
+		set_bit(STATUS_RF_KILL_HW, &priv->status);
+
+	/* After the ALIVE response, we can process host commands */
+	set_bit(STATUS_ALIVE, &priv->status);
+
+	/* Clear out the uCode error bit if it is set */
+	clear_bit(STATUS_FW_ERROR, &priv->status);
+
+	rc = iwl_init_channel_map(priv);
+	if (rc) {
+		IWL_ERROR("initializing regulatory failed: %d\n", rc);
+		return;
+	}
+
+	iwl_init_geos(priv);
+
+	if (iwl_is_rfkill(priv))
+		return;
+
+	if (!priv->mac80211_registered) {
+		/* Unlock so any user space entry points can call back into
+		 * the driver without a deadlock... */
+		mutex_unlock(&priv->mutex);
+		iwl_rate_control_register(priv->hw);
+		rc = ieee80211_register_hw(priv->hw);
+		priv->hw->conf.beacon_int = 100;
+		mutex_lock(&priv->mutex);
+
+		if (rc) {
+			IWL_ERROR("Failed to register network "
+				  "device (error %d)\n", rc);
+			return;
+		}
+
+		priv->mac80211_registered = 1;
+
+		iwl_reset_channel_flag(priv);
+	} else
+		ieee80211_start_queues(priv->hw);
+
+	priv->active_rate = priv->rates_mask;
+	priv->active_rate_basic = priv->rates_mask & IWL_BASIC_RATES_MASK;
+
+	iwl_send_power_mode(priv, IWL_POWER_LEVEL(priv->power_mode));
+
+	if (iwl_is_associated(priv)) {
+		struct iwl_rxon_cmd *active_rxon =
+				(struct iwl_rxon_cmd *)(&priv->active_rxon);
+
+		memcpy(&priv->staging_rxon, &priv->active_rxon,
+		       sizeof(priv->staging_rxon));
+		active_rxon->filter_flags &= ~RXON_FILTER_ASSOC_MSK;
+	} else {
+		/* Initialize our rx_config data */
+		iwl_connection_init_rx_config(priv);
+		memcpy(priv->staging_rxon.node_addr, priv->mac_addr, ETH_ALEN);
+	}
+
+	/* Configure BT coexistence */
+	iwl_send_bt_config(priv);
+
+	/* Configure the adapter for unassociated operation */
+	iwl_commit_rxon(priv);
+
+	/* At this point, the NIC is initialized and operational */
+	priv->notif_missed_beacons = 0;
+	set_bit(STATUS_READY, &priv->status);
+
+	iwl3945_reg_txpower_periodic(priv);
+
+	IWL_DEBUG_INFO("ALIVE processing complete.\n");
+
+	if (priv->error_recovering)
+		iwl_error_recovery(priv);
+
+	return;
+
+ restart:
+	queue_work(priv->workqueue, &priv->restart);
+}
+
+static void iwl_cancel_deferred_work(struct iwl_priv *priv);
+
+static void __iwl_down(struct iwl_priv *priv)
+{
+	unsigned long flags;
+	int exit_pending = test_bit(STATUS_EXIT_PENDING, &priv->status);
+	struct ieee80211_conf *conf = NULL;
+
+	IWL_DEBUG_INFO(DRV_NAME " is going down\n");
+
+	conf = ieee80211_get_hw_conf(priv->hw);
+
+	if (!exit_pending)
+		set_bit(STATUS_EXIT_PENDING, &priv->status);
+
+	iwl_clear_stations_table(priv);
+
+	/* Unblock any waiting calls */
+	wake_up_interruptible_all(&priv->wait_command_queue);
+
+	iwl_cancel_deferred_work(priv);
+
+	/* Wipe out the EXIT_PENDING status bit if we are not actually
+	 * exiting the module */
+	if (!exit_pending)
+		clear_bit(STATUS_EXIT_PENDING, &priv->status);
+
+	/* stop and reset the on-board processor */
+	iwl_write32(priv, CSR_RESET, CSR_RESET_REG_FLAG_NEVO_RESET);
+
+	/* tell the device to stop sending interrupts */
+	iwl_disable_interrupts(priv);
+
+	if (priv->mac80211_registered)
+		ieee80211_stop_queues(priv->hw);
+
+	/* If we have not previously called iwl_init() then
+	 * clear all bits but the RF Kill and SUSPEND bits and return */
+	if (!iwl_is_init(priv)) {
+		priv->status = test_bit(STATUS_RF_KILL_HW, &priv->status) <<
+					STATUS_RF_KILL_HW |
+			       test_bit(STATUS_RF_KILL_SW, &priv->status) <<
+					STATUS_RF_KILL_SW |
+			       test_bit(STATUS_IN_SUSPEND, &priv->status) <<
+					STATUS_IN_SUSPEND;
+		goto exit;
+	}
+
+	/* ...otherwise clear out all the status bits but the RF Kill and
+	 * SUSPEND bits and continue taking the NIC down. */
+	priv->status &= test_bit(STATUS_RF_KILL_HW, &priv->status) <<
+				STATUS_RF_KILL_HW |
+			test_bit(STATUS_RF_KILL_SW, &priv->status) <<
+				STATUS_RF_KILL_SW |
+			test_bit(STATUS_IN_SUSPEND, &priv->status) <<
+				STATUS_IN_SUSPEND |
+			test_bit(STATUS_FW_ERROR, &priv->status) <<
+				STATUS_FW_ERROR;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	iwl_clear_bit(priv, CSR_GP_CNTRL, CSR_GP_CNTRL_REG_FLAG_MAC_ACCESS_REQ);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	iwl_hw_txq_ctx_stop(priv);
+	iwl_hw_rxq_stop(priv);
+
+	spin_lock_irqsave(&priv->lock, flags);
+	if (!iwl_grab_restricted_access(priv)) {
+		iwl_write_restricted_reg(priv, APMG_CLK_DIS_REG,
+					 APMG_CLK_VAL_DMA_CLK_RQT);
+		iwl_release_restricted_access(priv);
+	}
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	udelay(5);
+
+	iwl_hw_nic_stop_master(priv);
+	iwl_set_bit(priv, CSR_RESET, CSR_RESET_REG_FLAG_SW_RESET);
+	iwl_hw_nic_reset(priv);
+
+ exit:
+	memset(&priv->card_alive, 0, sizeof(struct iwl_alive_resp));
+
+	if (priv->ibss_beacon)
+		dev_kfree_skb(priv->ibss_beacon);
+	priv->ibss_beacon = NULL;
+
+	/* clear out any free frames */
+	iwl_clear_free_frames(priv);
+}
+
+static void iwl_down(struct iwl_priv *priv)
+{
+	mutex_lock(&priv->mutex);
+	__iwl_down(priv);
+	mutex_unlock(&priv->mutex);
+}
+
+#define MAX_HW_RESTARTS 5
+
+static int __iwl_up(struct iwl_priv *priv)
+{
+	DECLARE_MAC_BUF(mac);
+	int rc, i;
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status)) {
+		IWL_WARNING("Exit pending; will not bring the NIC up\n");
+		return -EIO;
+	}
+
+	if (test_bit(STATUS_RF_KILL_SW, &priv->status)) {
+		IWL_WARNING("Radio disabled by SW RF kill (module "
+			    "parameter)\n");
+		return 0;
+	}
+
+	iwl_write32(priv, CSR_INT, 0xFFFFFFFF);
+
+	rc = iwl_hw_nic_init(priv);
+	if (rc) {
+		IWL_ERROR("Unable to int nic\n");
+		return rc;
+	}
+
+	/* make sure rfkill handshake bits are cleared */
+	iwl_write32(priv, CSR_UCODE_DRV_GP1_CLR, CSR_UCODE_SW_BIT_RFKILL);
+	iwl_write32(priv, CSR_UCODE_DRV_GP1_CLR,
+		    CSR_UCODE_DRV_GP1_BIT_CMD_BLOCKED);
+
+	/* clear (again), then enable host interrupts */
+	iwl_write32(priv, CSR_INT, 0xFFFFFFFF);
+	iwl_enable_interrupts(priv);
+
+	/* really make sure rfkill handshake bits are cleared */
+	iwl_write32(priv, CSR_UCODE_DRV_GP1_CLR, CSR_UCODE_SW_BIT_RFKILL);
+	iwl_write32(priv, CSR_UCODE_DRV_GP1_CLR, CSR_UCODE_SW_BIT_RFKILL);
+
+	/* Copy original ucode data image from disk into backup cache.
+	 * This will be used to initialize the on-board processor's
+	 * data SRAM for a clean start when the runtime program first loads. */
+	memcpy(priv->ucode_data_backup.v_addr, priv->ucode_data.v_addr,
+			priv->ucode_data.len);
+
+	for (i = 0; i < MAX_HW_RESTARTS; i++) {
+
+		iwl_clear_stations_table(priv);
+
+		/* load bootstrap state machine,
+		 * load bootstrap program into processor's memory,
+		 * prepare to load the "initialize" uCode */
+		rc = iwl_load_bsm(priv);
+
+		if (rc) {
+			IWL_ERROR("Unable to set up bootstrap uCode: %d\n", rc);
+			continue;
+		}
+
+		/* start card; "initialize" will load runtime ucode */
+		iwl_nic_start(priv);
+
+		/* MAC Address location in EEPROM same for 3945/4965 */
+		get_eeprom_mac(priv, priv->mac_addr);
+		IWL_DEBUG_INFO("MAC address: %s\n",
+			       print_mac(mac, priv->mac_addr));
+
+		SET_IEEE80211_PERM_ADDR(priv->hw, priv->mac_addr);
+
+		IWL_DEBUG_INFO(DRV_NAME " is coming up\n");
+
+		return 0;
+	}
+
+	set_bit(STATUS_EXIT_PENDING, &priv->status);
+	__iwl_down(priv);
+
+	/* tried to restart and config the device for as long as our
+	 * patience could withstand */
+	IWL_ERROR("Unable to initialize device after %d attempts.\n", i);
+	return -EIO;
+}
+
+
+/*****************************************************************************
+ *
+ * Workqueue callbacks
+ *
+ *****************************************************************************/
+
+static void iwl_bg_init_alive_start(struct work_struct *data)
+{
+	struct iwl_priv *priv =
+	    container_of(data, struct iwl_priv, init_alive_start.work);
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+		return;
+
+	mutex_lock(&priv->mutex);
+	iwl_init_alive_start(priv);
+	mutex_unlock(&priv->mutex);
+}
+
+static void iwl_bg_alive_start(struct work_struct *data)
+{
+	struct iwl_priv *priv =
+	    container_of(data, struct iwl_priv, alive_start.work);
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+		return;
+
+	mutex_lock(&priv->mutex);
+	iwl_alive_start(priv);
+	mutex_unlock(&priv->mutex);
+}
+
+static void iwl_bg_rf_kill(struct work_struct *work)
+{
+	struct iwl_priv *priv = container_of(work, struct iwl_priv, rf_kill);
+
+	wake_up_interruptible(&priv->wait_command_queue);
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+		return;
+
+	mutex_lock(&priv->mutex);
+
+	if (!iwl_is_rfkill(priv)) {
+		IWL_DEBUG(IWL_DL_INFO | IWL_DL_RF_KILL,
+			  "HW and/or SW RF Kill no longer active, restarting "
+			  "device\n");
+		if (!test_bit(STATUS_EXIT_PENDING, &priv->status))
+			queue_work(priv->workqueue, &priv->restart);
+	} else {
+
+		if (!test_bit(STATUS_RF_KILL_HW, &priv->status))
+			IWL_DEBUG_RF_KILL("Can not turn radio back on - "
+					  "disabled by SW switch\n");
+		else
+			IWL_WARNING("Radio Frequency Kill Switch is On:\n"
+				    "Kill switch must be turned off for "
+				    "wireless networking to work.\n");
+	}
+	mutex_unlock(&priv->mutex);
+}
+
+#define IWL_SCAN_CHECK_WATCHDOG (7 * HZ)
+
+static void iwl_bg_scan_check(struct work_struct *data)
+{
+	struct iwl_priv *priv =
+	    container_of(data, struct iwl_priv, scan_check.work);
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+		return;
+
+	mutex_lock(&priv->mutex);
+	if (test_bit(STATUS_SCANNING, &priv->status) ||
+	    test_bit(STATUS_SCAN_ABORTING, &priv->status)) {
+		IWL_DEBUG(IWL_DL_INFO | IWL_DL_SCAN,
+			  "Scan completion watchdog resetting adapter (%dms)\n",
+			  jiffies_to_msecs(IWL_SCAN_CHECK_WATCHDOG));
+		if (!test_bit(STATUS_EXIT_PENDING, &priv->status))
+			queue_work(priv->workqueue, &priv->restart);
+	}
+	mutex_unlock(&priv->mutex);
+}
+
+static void iwl_bg_request_scan(struct work_struct *data)
+{
+	struct iwl_priv *priv =
+	    container_of(data, struct iwl_priv, request_scan);
+	struct iwl_host_cmd cmd = {
+		.id = REPLY_SCAN_CMD,
+		.len = sizeof(struct iwl_scan_cmd),
+		.meta.flags = CMD_SIZE_HUGE,
+	};
+	int rc = 0;
+	struct iwl_scan_cmd *scan;
+	struct ieee80211_conf *conf = NULL;
+	u8 direct_mask;
+	int phymode;
+
+	conf = ieee80211_get_hw_conf(priv->hw);
+
+	mutex_lock(&priv->mutex);
+
+	if (!iwl_is_ready(priv)) {
+		IWL_WARNING("request scan called when driver not ready.\n");
+		goto done;
+	}
+
+	/* Make sure the scan wasn't cancelled before this queued work
+	 * was given the chance to run... */
+	if (!test_bit(STATUS_SCANNING, &priv->status))
+		goto done;
+
+	/* This should never be called or scheduled if there is currently
+	 * a scan active in the hardware. */
+	if (test_bit(STATUS_SCAN_HW, &priv->status)) {
+		IWL_DEBUG_INFO("Multiple concurrent scan requests in parallel. "
+			       "Ignoring second request.\n");
+		rc = -EIO;
+		goto done;
+	}
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status)) {
+		IWL_DEBUG_SCAN("Aborting scan due to device shutdown\n");
+		goto done;
+	}
+
+	if (test_bit(STATUS_SCAN_ABORTING, &priv->status)) {
+		IWL_DEBUG_HC("Scan request while abort pending.  Queuing.\n");
+		goto done;
+	}
+
+	if (iwl_is_rfkill(priv)) {
+		IWL_DEBUG_HC("Aborting scan due to RF Kill activation\n");
+		goto done;
+	}
+
+	if (!test_bit(STATUS_READY, &priv->status)) {
+		IWL_DEBUG_HC("Scan request while uninitialized.  Queuing.\n");
+		goto done;
+	}
+
+	if (!priv->scan_bands) {
+		IWL_DEBUG_HC("Aborting scan due to no requested bands\n");
+		goto done;
+	}
+
+	if (!priv->scan) {
+		priv->scan = kmalloc(sizeof(struct iwl_scan_cmd) +
+				     IWL_MAX_SCAN_SIZE, GFP_KERNEL);
+		if (!priv->scan) {
+			rc = -ENOMEM;
+			goto done;
+		}
+	}
+	scan = priv->scan;
+	memset(scan, 0, sizeof(struct iwl_scan_cmd) + IWL_MAX_SCAN_SIZE);
+
+	scan->quiet_plcp_th = IWL_PLCP_QUIET_THRESH;
+	scan->quiet_time = IWL_ACTIVE_QUIET_TIME;
+
+	if (iwl_is_associated(priv)) {
+		u16 interval = 0;
+		u32 extra;
+		u32 suspend_time = 100;
+		u32 scan_suspend_time = 100;
+		unsigned long flags;
+
+		IWL_DEBUG_INFO("Scanning while associated...\n");
+
+		spin_lock_irqsave(&priv->lock, flags);
+		interval = priv->beacon_int;
+		spin_unlock_irqrestore(&priv->lock, flags);
+
+		scan->suspend_time = 0;
+		scan->max_out_time = cpu_to_le32(600 * 1024);
+		if (!interval)
+			interval = suspend_time;
+		/*
+		 * suspend time format:
+		 *  0-19: beacon interval in usec (time before exec.)
+		 * 20-23: 0
+		 * 24-31: number of beacons (suspend between channels)
+		 */
+
+		extra = (suspend_time / interval) << 24;
+		scan_suspend_time = 0xFF0FFFFF &
+		    (extra | ((suspend_time % interval) * 1024));
+
+		scan->suspend_time = cpu_to_le32(scan_suspend_time);
+		IWL_DEBUG_SCAN("suspend_time 0x%X beacon interval %d\n",
+			       scan_suspend_time, interval);
+	}
+
+	/* We should add the ability for user to lock to PASSIVE ONLY */
+	if (priv->one_direct_scan) {
+		IWL_DEBUG_SCAN
+		    ("Kicking off one direct scan for '%s'\n",
+		     iwl_escape_essid(priv->direct_ssid,
+				      priv->direct_ssid_len));
+		scan->direct_scan[0].id = WLAN_EID_SSID;
+		scan->direct_scan[0].len = priv->direct_ssid_len;
+		memcpy(scan->direct_scan[0].ssid,
+		       priv->direct_ssid, priv->direct_ssid_len);
+		direct_mask = 1;
+	} else if (!iwl_is_associated(priv)) {
+		scan->direct_scan[0].id = WLAN_EID_SSID;
+		scan->direct_scan[0].len = priv->essid_len;
+		memcpy(scan->direct_scan[0].ssid, priv->essid, priv->essid_len);
+		direct_mask = 1;
+	} else
+		direct_mask = 0;
+
+	/* We don't build a direct scan probe request; the uCode will do
+	 * that based on the direct_mask added to each channel entry */
+	scan->tx_cmd.len = cpu_to_le16(
+		iwl_fill_probe_req(priv, (struct ieee80211_mgmt *)scan->data,
+			IWL_MAX_SCAN_SIZE - sizeof(scan), 0));
+	scan->tx_cmd.tx_flags = TX_CMD_FLG_SEQ_CTL_MSK;
+	scan->tx_cmd.sta_id = priv->hw_setting.bcast_sta_id;
+	scan->tx_cmd.stop_time.life_time = TX_CMD_LIFE_TIME_INFINITE;
+
+	/* flags + rate selection */
+
+	switch (priv->scan_bands) {
+	case 2:
+		scan->flags = RXON_FLG_BAND_24G_MSK | RXON_FLG_AUTO_DETECT_MSK;
+		scan->tx_cmd.rate = IWL_RATE_1M_PLCP;
+		scan->good_CRC_th = 0;
+		phymode = MODE_IEEE80211G;
+		break;
+
+	case 1:
+		scan->tx_cmd.rate = IWL_RATE_6M_PLCP;
+		scan->good_CRC_th = IWL_GOOD_CRC_TH;
+		phymode = MODE_IEEE80211A;
+		break;
+
+	default:
+		IWL_WARNING("Invalid scan band count\n");
+		goto done;
+	}
+
+	/* select Rx antennas */
+	scan->flags |= iwl3945_get_antenna_flags(priv);
+
+	if (priv->iw_mode == IEEE80211_IF_TYPE_MNTR)
+		scan->filter_flags = RXON_FILTER_PROMISC_MSK;
+
+	if (direct_mask)
+		IWL_DEBUG_SCAN
+		    ("Initiating direct scan for %s.\n",
+		     iwl_escape_essid(priv->essid, priv->essid_len));
+	else
+		IWL_DEBUG_SCAN("Initiating indirect scan.\n");
+
+	scan->channel_count =
+		iwl_get_channels_for_scan(
+			priv, phymode, 1, /* active */
+			direct_mask,
+			(void *)&scan->data[le16_to_cpu(scan->tx_cmd.len)]);
+
+	cmd.len += le16_to_cpu(scan->tx_cmd.len) +
+	    scan->channel_count * sizeof(struct iwl_scan_channel);
+	cmd.data = scan;
+	scan->len = cpu_to_le16(cmd.len);
+
+	set_bit(STATUS_SCAN_HW, &priv->status);
+	rc = iwl_send_cmd_sync(priv, &cmd);
+	if (rc)
+		goto done;
+
+	queue_delayed_work(priv->workqueue, &priv->scan_check,
+			   IWL_SCAN_CHECK_WATCHDOG);
+
+	mutex_unlock(&priv->mutex);
+	return;
+
+ done:
+	/* inform mac80211 sacn aborted */
+	queue_work(priv->workqueue, &priv->scan_completed);
+	mutex_unlock(&priv->mutex);
+}
+
+static void iwl_bg_up(struct work_struct *data)
+{
+	struct iwl_priv *priv = container_of(data, struct iwl_priv, up);
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+		return;
+
+	mutex_lock(&priv->mutex);
+	__iwl_up(priv);
+	mutex_unlock(&priv->mutex);
+}
+
+static void iwl_bg_restart(struct work_struct *data)
+{
+	struct iwl_priv *priv = container_of(data, struct iwl_priv, restart);
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+		return;
+
+	iwl_down(priv);
+	queue_work(priv->workqueue, &priv->up);
+}
+
+static void iwl_bg_rx_replenish(struct work_struct *data)
+{
+	struct iwl_priv *priv =
+	    container_of(data, struct iwl_priv, rx_replenish);
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+		return;
+
+	mutex_lock(&priv->mutex);
+	iwl_rx_replenish(priv);
+	mutex_unlock(&priv->mutex);
+}
+
+static void iwl_bg_post_associate(struct work_struct *data)
+{
+	struct iwl_priv *priv = container_of(data, struct iwl_priv,
+					     post_associate.work);
+
+	int rc = 0;
+	struct ieee80211_conf *conf = NULL;
+	DECLARE_MAC_BUF(mac);
+
+	if (priv->iw_mode == IEEE80211_IF_TYPE_AP) {
+		IWL_ERROR("%s Should not be called in AP mode\n", __FUNCTION__);
+		return;
+	}
+
+
+	IWL_DEBUG_ASSOC("Associated as %d to: %s\n",
+			priv->assoc_id,
+			print_mac(mac, priv->active_rxon.bssid_addr));
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+		return;
+
+	mutex_lock(&priv->mutex);
+
+	conf = ieee80211_get_hw_conf(priv->hw);
+
+	priv->staging_rxon.filter_flags &= ~RXON_FILTER_ASSOC_MSK;
+	iwl_commit_rxon(priv);
+
+	memset(&priv->rxon_timing, 0, sizeof(struct iwl_rxon_time_cmd));
+	iwl_setup_rxon_timing(priv);
+	rc = iwl_send_cmd_pdu(priv, REPLY_RXON_TIMING,
+			      sizeof(priv->rxon_timing), &priv->rxon_timing);
+	if (rc)
+		IWL_WARNING("REPLY_RXON_TIMING failed - "
+			    "Attempting to continue.\n");
+
+	priv->staging_rxon.filter_flags |= RXON_FILTER_ASSOC_MSK;
+
+	priv->staging_rxon.assoc_id = cpu_to_le16(priv->assoc_id);
+
+	IWL_DEBUG_ASSOC("assoc id %d beacon interval %d\n",
+			priv->assoc_id, priv->beacon_int);
+
+	if (priv->assoc_capability & WLAN_CAPABILITY_SHORT_PREAMBLE)
+		priv->staging_rxon.flags |= RXON_FLG_SHORT_PREAMBLE_MSK;
+	else
+		priv->staging_rxon.flags &= ~RXON_FLG_SHORT_PREAMBLE_MSK;
+
+	if (priv->staging_rxon.flags & RXON_FLG_BAND_24G_MSK) {
+		if (priv->assoc_capability & WLAN_CAPABILITY_SHORT_SLOT_TIME)
+			priv->staging_rxon.flags |= RXON_FLG_SHORT_SLOT_MSK;
+		else
+			priv->staging_rxon.flags &= ~RXON_FLG_SHORT_SLOT_MSK;
+
+		if (priv->iw_mode == IEEE80211_IF_TYPE_IBSS)
+			priv->staging_rxon.flags &= ~RXON_FLG_SHORT_SLOT_MSK;
+
+	}
+
+	iwl_commit_rxon(priv);
+
+	switch (priv->iw_mode) {
+	case IEEE80211_IF_TYPE_STA:
+		iwl_rate_scale_init(priv->hw, IWL_AP_ID);
+		break;
+
+	case IEEE80211_IF_TYPE_IBSS:
+
+		/* clear out the station table */
+		iwl_clear_stations_table(priv);
+
+		iwl_add_station(priv, BROADCAST_ADDR, 0, 0);
+		iwl_add_station(priv, priv->bssid, 0, 0);
+		iwl3945_sync_sta(priv, IWL_STA_ID,
+				 (priv->phymode == MODE_IEEE80211A)?
+				 IWL_RATE_6M_PLCP : IWL_RATE_1M_PLCP,
+				 CMD_ASYNC);
+		iwl_rate_scale_init(priv->hw, IWL_STA_ID);
+		iwl_send_beacon_cmd(priv);
+
+		break;
+
+	default:
+		 IWL_ERROR("%s Should not be called in %d mode\n",
+				__FUNCTION__, priv->iw_mode);
+		break;
+	}
+
+	iwl_sequence_reset(priv);
+
+#ifdef CONFIG_IWLWIFI_QOS
+	iwl_activate_qos(priv, 0);
+#endif /* CONFIG_IWLWIFI_QOS */
+	mutex_unlock(&priv->mutex);
+}
+
+static void iwl_bg_abort_scan(struct work_struct *work)
+{
+	struct iwl_priv *priv = container_of(work, struct iwl_priv,
+					     abort_scan);
+
+	if (!iwl_is_ready(priv))
+		return;
+
+	mutex_lock(&priv->mutex);
+
+	set_bit(STATUS_SCAN_ABORTING, &priv->status);
+	iwl_send_scan_abort(priv);
+
+	mutex_unlock(&priv->mutex);
+}
+
+static void iwl_bg_scan_completed(struct work_struct *work)
+{
+	struct iwl_priv *priv =
+	    container_of(work, struct iwl_priv, scan_completed);
+
+	IWL_DEBUG(IWL_DL_INFO | IWL_DL_SCAN, "SCAN complete scan\n");
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+		return;
+
+	ieee80211_scan_completed(priv->hw);
+
+	/* Since setting the TXPOWER may have been deferred while
+	 * performing the scan, fire one off */
+	mutex_lock(&priv->mutex);
+	iwl_hw_reg_send_txpower(priv);
+	mutex_unlock(&priv->mutex);
+}
+
+/*****************************************************************************
+ *
+ * mac80211 entry point functions
+ *
+ *****************************************************************************/
+
+static int iwl_mac_start(struct ieee80211_hw *hw)
+{
+	struct iwl_priv *priv = hw->priv;
+
+	IWL_DEBUG_MAC80211("enter\n");
+
+	/* we should be verifying the device is ready to be opened */
+	mutex_lock(&priv->mutex);
+
+	priv->is_open = 1;
+
+	if (!iwl_is_rfkill(priv))
+		ieee80211_start_queues(priv->hw);
+
+	mutex_unlock(&priv->mutex);
+	IWL_DEBUG_MAC80211("leave\n");
+	return 0;
+}
+
+static void iwl_mac_stop(struct ieee80211_hw *hw)
+{
+	struct iwl_priv *priv = hw->priv;
+
+	IWL_DEBUG_MAC80211("enter\n");
+	priv->is_open = 0;
+	/*netif_stop_queue(dev); */
+	flush_workqueue(priv->workqueue);
+	IWL_DEBUG_MAC80211("leave\n");
+}
+
+static int iwl_mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb,
+		      struct ieee80211_tx_control *ctl)
+{
+	struct iwl_priv *priv = hw->priv;
+
+	IWL_DEBUG_MAC80211("enter\n");
+
+	if (priv->iw_mode == IEEE80211_IF_TYPE_MNTR) {
+		IWL_DEBUG_MAC80211("leave - monitor\n");
+		return -1;
+	}
+
+	IWL_DEBUG_TX("dev->xmit(%d bytes) at rate 0x%02x\n", skb->len,
+		     ctl->tx_rate);
+
+	if (iwl_tx_skb(priv, skb, ctl))
+		dev_kfree_skb_any(skb);
+
+	IWL_DEBUG_MAC80211("leave\n");
+	return 0;
+}
+
+static int iwl_mac_add_interface(struct ieee80211_hw *hw,
+				 struct ieee80211_if_init_conf *conf)
+{
+	struct iwl_priv *priv = hw->priv;
+	unsigned long flags;
+	DECLARE_MAC_BUF(mac);
+
+	IWL_DEBUG_MAC80211("enter: id %d, type %d\n", conf->if_id, conf->type);
+	if (conf->mac_addr)
+		IWL_DEBUG_MAC80211("enter: MAC %s\n",
+				   print_mac(mac, conf->mac_addr));
+
+	if (priv->interface_id) {
+		IWL_DEBUG_MAC80211("leave - interface_id != 0\n");
+		return 0;
+	}
+
+	spin_lock_irqsave(&priv->lock, flags);
+	priv->interface_id = conf->if_id;
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	mutex_lock(&priv->mutex);
+	iwl_set_mode(priv, conf->type);
+
+	IWL_DEBUG_MAC80211("leave\n");
+	mutex_unlock(&priv->mutex);
+
+	return 0;
+}
+
+/**
+ * iwl_mac_config - mac80211 config callback
+ *
+ * We ignore conf->flags & IEEE80211_CONF_SHORT_SLOT_TIME since it seems to
+ * be set inappropriately and the driver currently sets the hardware up to
+ * use it whenever needed.
+ */
+static int iwl_mac_config(struct ieee80211_hw *hw, struct ieee80211_conf *conf)
+{
+	struct iwl_priv *priv = hw->priv;
+	const struct iwl_channel_info *ch_info;
+	unsigned long flags;
+
+	mutex_lock(&priv->mutex);
+	IWL_DEBUG_MAC80211("enter to channel %d\n", conf->channel);
+
+	if (!iwl_is_ready(priv)) {
+		IWL_DEBUG_MAC80211("leave - not ready\n");
+		mutex_unlock(&priv->mutex);
+		return -EIO;
+	}
+
+	/* TODO: Figure out how to get ieee80211_local->sta_scanning w/ only
+	 * what is exposed through include/ declrations */
+	if (unlikely(!iwl_param_disable_hw_scan &&
+		     test_bit(STATUS_SCANNING, &priv->status))) {
+		IWL_DEBUG_MAC80211("leave - scanning\n");
+		mutex_unlock(&priv->mutex);
+		return 0;
+	}
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	ch_info = iwl_get_channel_info(priv, conf->phymode, conf->channel);
+	if (!is_channel_valid(ch_info)) {
+		IWL_DEBUG_SCAN("Channel %d [%d] is INVALID for this SKU.\n",
+			       conf->channel, conf->phymode);
+		IWL_DEBUG_MAC80211("leave - invalid channel\n");
+		spin_unlock_irqrestore(&priv->lock, flags);
+		mutex_unlock(&priv->mutex);
+		return -EINVAL;
+	}
+
+	iwl_set_rxon_channel(priv, conf->phymode, conf->channel);
+
+	iwl_set_flags_for_phymode(priv, conf->phymode);
+
+	/* The list of supported rates and rate mask can be different
+	 * for each phymode; since the phymode may have changed, reset
+	 * the rate mask to what mac80211 lists */
+	iwl_set_rate(priv);
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+#ifdef IEEE80211_CONF_CHANNEL_SWITCH
+	if (conf->flags & IEEE80211_CONF_CHANNEL_SWITCH) {
+		iwl_hw_channel_switch(priv, conf->channel);
+		mutex_unlock(&priv->mutex);
+		return 0;
+	}
+#endif
+
+	iwl_radio_kill_sw(priv, !conf->radio_enabled);
+
+	if (!conf->radio_enabled) {
+		IWL_DEBUG_MAC80211("leave - radio disabled\n");
+		mutex_unlock(&priv->mutex);
+		return 0;
+	}
+
+	if (iwl_is_rfkill(priv)) {
+		IWL_DEBUG_MAC80211("leave - RF kill\n");
+		mutex_unlock(&priv->mutex);
+		return -EIO;
+	}
+
+	iwl_set_rate(priv);
+
+	if (memcmp(&priv->active_rxon,
+		   &priv->staging_rxon, sizeof(priv->staging_rxon)))
+		iwl_commit_rxon(priv);
+	else
+		IWL_DEBUG_INFO("No re-sending same RXON configuration.\n");
+
+	IWL_DEBUG_MAC80211("leave\n");
+
+	mutex_unlock(&priv->mutex);
+
+	return 0;
+}
+
+static void iwl_config_ap(struct iwl_priv *priv)
+{
+	int rc = 0;
+
+	if (priv->status & STATUS_EXIT_PENDING)
+		return;
+
+	/* The following should be done only at AP bring up */
+	if ((priv->active_rxon.filter_flags & RXON_FILTER_ASSOC_MSK) == 0) {
+
+		/* RXON - unassoc (to set timing command) */
+		priv->staging_rxon.filter_flags &= ~RXON_FILTER_ASSOC_MSK;
+		iwl_commit_rxon(priv);
+
+		/* RXON Timing */
+		memset(&priv->rxon_timing, 0, sizeof(struct iwl_rxon_time_cmd));
+		iwl_setup_rxon_timing(priv);
+		rc = iwl_send_cmd_pdu(priv, REPLY_RXON_TIMING,
+				sizeof(priv->rxon_timing), &priv->rxon_timing);
+		if (rc)
+			IWL_WARNING("REPLY_RXON_TIMING failed - "
+					"Attempting to continue.\n");
+
+		/* FIXME: what should be the assoc_id for AP? */
+		priv->staging_rxon.assoc_id = cpu_to_le16(priv->assoc_id);
+		if (priv->assoc_capability & WLAN_CAPABILITY_SHORT_PREAMBLE)
+			priv->staging_rxon.flags |=
+				RXON_FLG_SHORT_PREAMBLE_MSK;
+		else
+			priv->staging_rxon.flags &=
+				~RXON_FLG_SHORT_PREAMBLE_MSK;
+
+		if (priv->staging_rxon.flags & RXON_FLG_BAND_24G_MSK) {
+			if (priv->assoc_capability &
+				WLAN_CAPABILITY_SHORT_SLOT_TIME)
+				priv->staging_rxon.flags |=
+					RXON_FLG_SHORT_SLOT_MSK;
+			else
+				priv->staging_rxon.flags &=
+					~RXON_FLG_SHORT_SLOT_MSK;
+
+			if (priv->iw_mode == IEEE80211_IF_TYPE_IBSS)
+				priv->staging_rxon.flags &=
+					~RXON_FLG_SHORT_SLOT_MSK;
+		}
+		/* restore RXON assoc */
+		priv->staging_rxon.filter_flags |= RXON_FILTER_ASSOC_MSK;
+		iwl_commit_rxon(priv);
+		iwl_add_station(priv, BROADCAST_ADDR, 0, 0);
+	}
+	iwl_send_beacon_cmd(priv);
+
+	/* FIXME - we need to add code here to detect a totally new
+	 * configuration, reset the AP, unassoc, rxon timing, assoc,
+	 * clear sta table, add BCAST sta... */
+}
+
+static int iwl_mac_config_interface(struct ieee80211_hw *hw, int if_id,
+				    struct ieee80211_if_conf *conf)
+{
+	struct iwl_priv *priv = hw->priv;
+	DECLARE_MAC_BUF(mac);
+	unsigned long flags;
+	int rc;
+
+	if (conf == NULL)
+		return -EIO;
+
+	/* XXX: this MUST use conf->mac_addr */
+
+	if ((priv->iw_mode == IEEE80211_IF_TYPE_AP) &&
+	    (!conf->beacon || !conf->ssid_len)) {
+		IWL_DEBUG_MAC80211
+		    ("Leaving in AP mode because HostAPD is not ready.\n");
+		return 0;
+	}
+
+	mutex_lock(&priv->mutex);
+
+	IWL_DEBUG_MAC80211("enter: interface id %d\n", if_id);
+	if (conf->bssid)
+		IWL_DEBUG_MAC80211("bssid: %s\n",
+				   print_mac(mac, conf->bssid));
+
+/*
+ * very dubious code was here; the probe filtering flag is never set:
+ *
+	if (unlikely(test_bit(STATUS_SCANNING, &priv->status)) &&
+	    !(priv->hw->flags & IEEE80211_HW_NO_PROBE_FILTERING)) {
+ */
+	if (unlikely(test_bit(STATUS_SCANNING, &priv->status))) {
+		IWL_DEBUG_MAC80211("leave - scanning\n");
+		mutex_unlock(&priv->mutex);
+		return 0;
+	}
+
+	if (priv->interface_id != if_id) {
+		IWL_DEBUG_MAC80211("leave - interface_id != if_id\n");
+		mutex_unlock(&priv->mutex);
+		return 0;
+	}
+
+	if (priv->iw_mode == IEEE80211_IF_TYPE_AP) {
+		if (!conf->bssid) {
+			conf->bssid = priv->mac_addr;
+			memcpy(priv->bssid, priv->mac_addr, ETH_ALEN);
+			IWL_DEBUG_MAC80211("bssid was set to: %s\n",
+					   print_mac(mac, conf->bssid));
+		}
+		if (priv->ibss_beacon)
+			dev_kfree_skb(priv->ibss_beacon);
+
+		priv->ibss_beacon = conf->beacon;
+	}
+
+	if (conf->bssid && !is_zero_ether_addr(conf->bssid) &&
+	    !is_multicast_ether_addr(conf->bssid)) {
+		/* If there is currently a HW scan going on in the background
+		 * then we need to cancel it else the RXON below will fail. */
+		if (iwl_scan_cancel_timeout(priv, 100)) {
+			IWL_WARNING("Aborted scan still in progress "
+				    "after 100ms\n");
+			IWL_DEBUG_MAC80211("leaving - scan abort failed.\n");
+			mutex_unlock(&priv->mutex);
+			return -EAGAIN;
+		}
+		memcpy(priv->staging_rxon.bssid_addr, conf->bssid, ETH_ALEN);
+
+		/* TODO: Audit driver for usage of these members and see
+		 * if mac80211 deprecates them (priv->bssid looks like it
+		 * shouldn't be there, but I haven't scanned the IBSS code
+		 * to verify) - jpk */
+		memcpy(priv->bssid, conf->bssid, ETH_ALEN);
+
+		if (priv->iw_mode == IEEE80211_IF_TYPE_AP)
+			iwl_config_ap(priv);
+		else {
+			priv->staging_rxon.filter_flags |=
+						RXON_FILTER_ASSOC_MSK;
+			rc = iwl_commit_rxon(priv);
+			if ((priv->iw_mode == IEEE80211_IF_TYPE_STA) && rc)
+				iwl_add_station(priv,
+					priv->active_rxon.bssid_addr, 1, 0);
+		}
+
+	} else {
+		priv->staging_rxon.filter_flags &= ~RXON_FILTER_ASSOC_MSK;
+		iwl_commit_rxon(priv);
+	}
+
+	spin_lock_irqsave(&priv->lock, flags);
+	if (!conf->ssid_len)
+		memset(priv->essid, 0, IW_ESSID_MAX_SIZE);
+	else
+		memcpy(priv->essid, conf->ssid, conf->ssid_len);
+
+	priv->essid_len = conf->ssid_len;
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	IWL_DEBUG_MAC80211("leave\n");
+	mutex_unlock(&priv->mutex);
+
+	return 0;
+}
+
+static void iwl_configure_filter(struct ieee80211_hw *hw,
+				 unsigned int changed_flags,
+				 unsigned int *total_flags,
+				 int mc_count, struct dev_addr_list *mc_list)
+{
+	/*
+	 * XXX: dummy
+	 * see also iwl_connection_init_rx_config
+	 */
+	*total_flags = 0;
+}
+
+static void iwl_mac_remove_interface(struct ieee80211_hw *hw,
+				     struct ieee80211_if_init_conf *conf)
+{
+	struct iwl_priv *priv = hw->priv;
+
+	IWL_DEBUG_MAC80211("enter\n");
+
+	mutex_lock(&priv->mutex);
+	if (priv->interface_id == conf->if_id) {
+		priv->interface_id = 0;
+		memset(priv->bssid, 0, ETH_ALEN);
+		memset(priv->essid, 0, IW_ESSID_MAX_SIZE);
+		priv->essid_len = 0;
+	}
+	mutex_unlock(&priv->mutex);
+
+	IWL_DEBUG_MAC80211("leave\n");
+
+}
+
+#define IWL_DELAY_NEXT_SCAN (HZ*2)
+static int iwl_mac_hw_scan(struct ieee80211_hw *hw, u8 *ssid, size_t len)
+{
+	int rc = 0;
+	unsigned long flags;
+	struct iwl_priv *priv = hw->priv;
+
+	IWL_DEBUG_MAC80211("enter\n");
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	if (!iwl_is_ready_rf(priv)) {
+		rc = -EIO;
+		IWL_DEBUG_MAC80211("leave - not ready or exit pending\n");
+		goto out_unlock;
+	}
+
+	if (priv->iw_mode == IEEE80211_IF_TYPE_AP) {	/* APs don't scan */
+		rc = -EIO;
+		IWL_ERROR("ERROR: APs don't scan\n");
+		goto out_unlock;
+	}
+
+	/* if we just finished scan ask for delay */
+	if (priv->last_scan_jiffies &&
+	    time_after(priv->last_scan_jiffies + IWL_DELAY_NEXT_SCAN,
+		       jiffies)) {
+		rc = -EAGAIN;
+		goto out_unlock;
+	}
+	if (len) {
+		IWL_DEBUG_SCAN("direct scan for  "
+			       "%s [%d]\n ",
+			       iwl_escape_essid(ssid, len), (int)len);
+
+		priv->one_direct_scan = 1;
+		priv->direct_ssid_len = (u8)
+		    min((u8) len, (u8) IW_ESSID_MAX_SIZE);
+		memcpy(priv->direct_ssid, ssid, priv->direct_ssid_len);
+	}
+
+	rc = iwl_scan_initiate(priv);
+
+	IWL_DEBUG_MAC80211("leave\n");
+
+out_unlock:
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return rc;
+}
+
+static int iwl_mac_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
+			   const u8 *local_addr, const u8 *addr,
+			   struct ieee80211_key_conf *key)
+{
+	struct iwl_priv *priv = hw->priv;
+	int rc = 0;
+	u8 sta_id;
+
+	IWL_DEBUG_MAC80211("enter\n");
+
+	if (!iwl_param_hwcrypto) {
+		IWL_DEBUG_MAC80211("leave - hwcrypto disabled\n");
+		return -EOPNOTSUPP;
+	}
+
+	if (is_zero_ether_addr(addr))
+		/* only support pairwise keys */
+		return -EOPNOTSUPP;
+
+	sta_id = iwl_hw_find_station(priv, addr);
+	if (sta_id == IWL_INVALID_STATION) {
+		DECLARE_MAC_BUF(mac);
+
+		IWL_DEBUG_MAC80211("leave - %s not in station map.\n",
+				   print_mac(mac, addr));
+		return -EINVAL;
+	}
+
+	mutex_lock(&priv->mutex);
+
+	switch (cmd) {
+	case  SET_KEY:
+		rc = iwl_update_sta_key_info(priv, key, sta_id);
+		if (!rc) {
+			iwl_set_rxon_hwcrypto(priv, 1);
+			iwl_commit_rxon(priv);
+			key->hw_key_idx = sta_id;
+			IWL_DEBUG_MAC80211("set_key success, using hwcrypto\n");
+			key->flags |= IEEE80211_KEY_FLAG_GENERATE_IV;
+		}
+		break;
+	case DISABLE_KEY:
+		rc = iwl_clear_sta_key_info(priv, sta_id);
+		if (!rc) {
+			iwl_set_rxon_hwcrypto(priv, 0);
+			iwl_commit_rxon(priv);
+			IWL_DEBUG_MAC80211("disable hwcrypto key\n");
+		}
+		break;
+	default:
+		rc = -EINVAL;
+	}
+
+	IWL_DEBUG_MAC80211("leave\n");
+	mutex_unlock(&priv->mutex);
+
+	return rc;
+}
+
+static int iwl_mac_conf_tx(struct ieee80211_hw *hw, int queue,
+			   const struct ieee80211_tx_queue_params *params)
+{
+	struct iwl_priv *priv = hw->priv;
+#ifdef CONFIG_IWLWIFI_QOS
+	unsigned long flags;
+	int q;
+#endif /* CONFIG_IWL_QOS */
+
+	IWL_DEBUG_MAC80211("enter\n");
+
+	if (!iwl_is_ready_rf(priv)) {
+		IWL_DEBUG_MAC80211("leave - RF not ready\n");
+		return -EIO;
+	}
+
+	if (queue >= AC_NUM) {
+		IWL_DEBUG_MAC80211("leave - queue >= AC_NUM %d\n", queue);
+		return 0;
+	}
+
+#ifdef CONFIG_IWLWIFI_QOS
+	if (!priv->qos_data.qos_enable) {
+		priv->qos_data.qos_active = 0;
+		IWL_DEBUG_MAC80211("leave - qos not enabled\n");
+		return 0;
+	}
+	q = AC_NUM - 1 - queue;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	priv->qos_data.def_qos_parm.ac[q].cw_min = cpu_to_le16(params->cw_min);
+	priv->qos_data.def_qos_parm.ac[q].cw_max = cpu_to_le16(params->cw_max);
+	priv->qos_data.def_qos_parm.ac[q].aifsn = params->aifs;
+	priv->qos_data.def_qos_parm.ac[q].edca_txop =
+			cpu_to_le16((params->burst_time * 100));
+
+	priv->qos_data.def_qos_parm.ac[q].reserved1 = 0;
+	priv->qos_data.qos_active = 1;
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	mutex_lock(&priv->mutex);
+	if (priv->iw_mode == IEEE80211_IF_TYPE_AP)
+		iwl_activate_qos(priv, 1);
+	else if (priv->assoc_id && iwl_is_associated(priv))
+		iwl_activate_qos(priv, 0);
+
+	mutex_unlock(&priv->mutex);
+
+#endif /*CONFIG_IWLWIFI_QOS */
+
+	IWL_DEBUG_MAC80211("leave\n");
+	return 0;
+}
+
+static int iwl_mac_get_tx_stats(struct ieee80211_hw *hw,
+				struct ieee80211_tx_queue_stats *stats)
+{
+	struct iwl_priv *priv = hw->priv;
+	int i, avail;
+	struct iwl_tx_queue *txq;
+	struct iwl_queue *q;
+	unsigned long flags;
+
+	IWL_DEBUG_MAC80211("enter\n");
+
+	if (!iwl_is_ready_rf(priv)) {
+		IWL_DEBUG_MAC80211("leave - RF not ready\n");
+		return -EIO;
+	}
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	for (i = 0; i < AC_NUM; i++) {
+		txq = &priv->txq[i];
+		q = &txq->q;
+		avail = iwl_queue_space(q);
+
+		stats->data[i].len = q->n_window - avail;
+		stats->data[i].limit = q->n_window - q->high_mark;
+		stats->data[i].count = q->n_window;
+
+	}
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	IWL_DEBUG_MAC80211("leave\n");
+
+	return 0;
+}
+
+static int iwl_mac_get_stats(struct ieee80211_hw *hw,
+			     struct ieee80211_low_level_stats *stats)
+{
+	IWL_DEBUG_MAC80211("enter\n");
+	IWL_DEBUG_MAC80211("leave\n");
+
+	return 0;
+}
+
+static u64 iwl_mac_get_tsf(struct ieee80211_hw *hw)
+{
+	IWL_DEBUG_MAC80211("enter\n");
+	IWL_DEBUG_MAC80211("leave\n");
+
+	return 0;
+}
+
+static void iwl_mac_reset_tsf(struct ieee80211_hw *hw)
+{
+	struct iwl_priv *priv = hw->priv;
+	unsigned long flags;
+
+	mutex_lock(&priv->mutex);
+	IWL_DEBUG_MAC80211("enter\n");
+
+#ifdef CONFIG_IWLWIFI_QOS
+	iwl_reset_qos(priv);
+#endif
+	cancel_delayed_work(&priv->post_associate);
+
+	spin_lock_irqsave(&priv->lock, flags);
+	priv->assoc_id = 0;
+	priv->assoc_capability = 0;
+	priv->call_post_assoc_from_beacon = 0;
+
+	/* new association get rid of ibss beacon skb */
+	if (priv->ibss_beacon)
+		dev_kfree_skb(priv->ibss_beacon);
+
+	priv->ibss_beacon = NULL;
+
+	priv->beacon_int = priv->hw->conf.beacon_int;
+	priv->timestamp1 = 0;
+	priv->timestamp0 = 0;
+	if ((priv->iw_mode == IEEE80211_IF_TYPE_STA))
+		priv->beacon_int = 0;
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	/* Per mac80211.h: This is only used in IBSS mode... */
+	if (priv->iw_mode != IEEE80211_IF_TYPE_IBSS) {
+		IWL_DEBUG_MAC80211("leave - not in IBSS\n");
+		mutex_unlock(&priv->mutex);
+		return;
+	}
+
+	if (!iwl_is_ready_rf(priv)) {
+		IWL_DEBUG_MAC80211("leave - not ready\n");
+		mutex_unlock(&priv->mutex);
+		return;
+	}
+
+	priv->only_active_channel = 0;
+
+	iwl_set_rate(priv);
+
+	mutex_unlock(&priv->mutex);
+
+	IWL_DEBUG_MAC80211("leave\n");
+
+}
+
+static int iwl_mac_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb,
+				 struct ieee80211_tx_control *control)
+{
+	struct iwl_priv *priv = hw->priv;
+	unsigned long flags;
+
+	mutex_lock(&priv->mutex);
+	IWL_DEBUG_MAC80211("enter\n");
+
+	if (!iwl_is_ready_rf(priv)) {
+		IWL_DEBUG_MAC80211("leave - RF not ready\n");
+		mutex_unlock(&priv->mutex);
+		return -EIO;
+	}
+
+	if (priv->iw_mode != IEEE80211_IF_TYPE_IBSS) {
+		IWL_DEBUG_MAC80211("leave - not IBSS\n");
+		mutex_unlock(&priv->mutex);
+		return -EIO;
+	}
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	if (priv->ibss_beacon)
+		dev_kfree_skb(priv->ibss_beacon);
+
+	priv->ibss_beacon = skb;
+
+	priv->assoc_id = 0;
+
+	IWL_DEBUG_MAC80211("leave\n");
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+#ifdef CONFIG_IWLWIFI_QOS
+	iwl_reset_qos(priv);
+#endif
+
+	queue_work(priv->workqueue, &priv->post_associate.work);
+
+	mutex_unlock(&priv->mutex);
+
+	return 0;
+}
+
+/*****************************************************************************
+ *
+ * sysfs attributes
+ *
+ *****************************************************************************/
+
+#ifdef CONFIG_IWLWIFI_DEBUG
+
+/*
+ * The following adds a new attribute to the sysfs representation
+ * of this device driver (i.e. a new file in /sys/bus/pci/drivers/iwl/)
+ * used for controlling the debug level.
+ *
+ * See the level definitions in iwl for details.
+ */
+
+static ssize_t show_debug_level(struct device_driver *d, char *buf)
+{
+	return sprintf(buf, "0x%08X\n", iwl_debug_level);
+}
+static ssize_t store_debug_level(struct device_driver *d,
+				 const char *buf, size_t count)
+{
+	char *p = (char *)buf;
+	u32 val;
+
+	val = simple_strtoul(p, &p, 0);
+	if (p == buf)
+		printk(KERN_INFO DRV_NAME
+		       ": %s is not in hex or decimal form.\n", buf);
+	else
+		iwl_debug_level = val;
+
+	return strnlen(buf, count);
+}
+
+static DRIVER_ATTR(debug_level, S_IWUSR | S_IRUGO,
+		   show_debug_level, store_debug_level);
+
+#endif /* CONFIG_IWLWIFI_DEBUG */
+
+static ssize_t show_rf_kill(struct device *d,
+			    struct device_attribute *attr, char *buf)
+{
+	/*
+	 * 0 - RF kill not enabled
+	 * 1 - SW based RF kill active (sysfs)
+	 * 2 - HW based RF kill active
+	 * 3 - Both HW and SW based RF kill active
+	 */
+	struct iwl_priv *priv = (struct iwl_priv *)d->driver_data;
+	int val = (test_bit(STATUS_RF_KILL_SW, &priv->status) ? 0x1 : 0x0) |
+		  (test_bit(STATUS_RF_KILL_HW, &priv->status) ? 0x2 : 0x0);
+
+	return sprintf(buf, "%i\n", val);
+}
+
+static ssize_t store_rf_kill(struct device *d,
+			     struct device_attribute *attr,
+			     const char *buf, size_t count)
+{
+	struct iwl_priv *priv = (struct iwl_priv *)d->driver_data;
+
+	mutex_lock(&priv->mutex);
+	iwl_radio_kill_sw(priv, buf[0] == '1');
+	mutex_unlock(&priv->mutex);
+
+	return count;
+}
+
+static DEVICE_ATTR(rf_kill, S_IWUSR | S_IRUGO, show_rf_kill, store_rf_kill);
+
+static ssize_t show_temperature(struct device *d,
+				struct device_attribute *attr, char *buf)
+{
+	struct iwl_priv *priv = (struct iwl_priv *)d->driver_data;
+
+	if (!iwl_is_alive(priv))
+		return -EAGAIN;
+
+	return sprintf(buf, "%d\n", iwl_hw_get_temperature(priv));
+}
+
+static DEVICE_ATTR(temperature, S_IRUGO, show_temperature, NULL);
+
+static ssize_t show_rs_window(struct device *d,
+			      struct device_attribute *attr,
+			      char *buf)
+{
+	struct iwl_priv *priv = d->driver_data;
+	return iwl_fill_rs_info(priv->hw, buf, IWL_AP_ID);
+}
+static DEVICE_ATTR(rs_window, S_IRUGO, show_rs_window, NULL);
+
+static ssize_t show_tx_power(struct device *d,
+			     struct device_attribute *attr, char *buf)
+{
+	struct iwl_priv *priv = (struct iwl_priv *)d->driver_data;
+	return sprintf(buf, "%d\n", priv->user_txpower_limit);
+}
+
+static ssize_t store_tx_power(struct device *d,
+			      struct device_attribute *attr,
+			      const char *buf, size_t count)
+{
+	struct iwl_priv *priv = (struct iwl_priv *)d->driver_data;
+	char *p = (char *)buf;
+	u32 val;
+
+	val = simple_strtoul(p, &p, 10);
+	if (p == buf)
+		printk(KERN_INFO DRV_NAME
+		       ": %s is not in decimal form.\n", buf);
+	else
+		iwl_hw_reg_set_txpower(priv, val);
+
+	return count;
+}
+
+static DEVICE_ATTR(tx_power, S_IWUSR | S_IRUGO, show_tx_power, store_tx_power);
+
+static ssize_t show_flags(struct device *d,
+			  struct device_attribute *attr, char *buf)
+{
+	struct iwl_priv *priv = (struct iwl_priv *)d->driver_data;
+
+	return sprintf(buf, "0x%04X\n", priv->active_rxon.flags);
+}
+
+static ssize_t store_flags(struct device *d,
+			   struct device_attribute *attr,
+			   const char *buf, size_t count)
+{
+	struct iwl_priv *priv = (struct iwl_priv *)d->driver_data;
+	u32 flags = simple_strtoul(buf, NULL, 0);
+
+	mutex_lock(&priv->mutex);
+	if (le32_to_cpu(priv->staging_rxon.flags) != flags) {
+		/* Cancel any currently running scans... */
+		if (iwl_scan_cancel_timeout(priv, 100))
+			IWL_WARNING("Could not cancel scan.\n");
+		else {
+			IWL_DEBUG_INFO("Committing rxon.flags = 0x%04X\n",
+				       flags);
+			priv->staging_rxon.flags = cpu_to_le32(flags);
+			iwl_commit_rxon(priv);
+		}
+	}
+	mutex_unlock(&priv->mutex);
+
+	return count;
+}
+
+static DEVICE_ATTR(flags, S_IWUSR | S_IRUGO, show_flags, store_flags);
+
+static ssize_t show_filter_flags(struct device *d,
+				 struct device_attribute *attr, char *buf)
+{
+	struct iwl_priv *priv = (struct iwl_priv *)d->driver_data;
+
+	return sprintf(buf, "0x%04X\n",
+		le32_to_cpu(priv->active_rxon.filter_flags));
+}
+
+static ssize_t store_filter_flags(struct device *d,
+				  struct device_attribute *attr,
+				  const char *buf, size_t count)
+{
+	struct iwl_priv *priv = (struct iwl_priv *)d->driver_data;
+	u32 filter_flags = simple_strtoul(buf, NULL, 0);
+
+	mutex_lock(&priv->mutex);
+	if (le32_to_cpu(priv->staging_rxon.filter_flags) != filter_flags) {
+		/* Cancel any currently running scans... */
+		if (iwl_scan_cancel_timeout(priv, 100))
+			IWL_WARNING("Could not cancel scan.\n");
+		else {
+			IWL_DEBUG_INFO("Committing rxon.filter_flags = "
+				       "0x%04X\n", filter_flags);
+			priv->staging_rxon.filter_flags =
+				cpu_to_le32(filter_flags);
+			iwl_commit_rxon(priv);
+		}
+	}
+	mutex_unlock(&priv->mutex);
+
+	return count;
+}
+
+static DEVICE_ATTR(filter_flags, S_IWUSR | S_IRUGO, show_filter_flags,
+		   store_filter_flags);
+
+static ssize_t show_tune(struct device *d,
+			 struct device_attribute *attr, char *buf)
+{
+	struct iwl_priv *priv = (struct iwl_priv *)d->driver_data;
+
+	return sprintf(buf, "0x%04X\n",
+		       (priv->phymode << 8) |
+			le16_to_cpu(priv->active_rxon.channel));
+}
+
+static void iwl_set_flags_for_phymode(struct iwl_priv *priv, u8 phymode);
+
+static ssize_t store_tune(struct device *d,
+			  struct device_attribute *attr,
+			  const char *buf, size_t count)
+{
+	struct iwl_priv *priv = (struct iwl_priv *)d->driver_data;
+	char *p = (char *)buf;
+	u16 tune = simple_strtoul(p, &p, 0);
+	u8 phymode = (tune >> 8) & 0xff;
+	u16 channel = tune & 0xff;
+
+	IWL_DEBUG_INFO("Tune request to:%d channel:%d\n", phymode, channel);
+
+	mutex_lock(&priv->mutex);
+	if ((le16_to_cpu(priv->staging_rxon.channel) != channel) ||
+	    (priv->phymode != phymode)) {
+		const struct iwl_channel_info *ch_info;
+
+		ch_info = iwl_get_channel_info(priv, phymode, channel);
+		if (!ch_info) {
+			IWL_WARNING("Requested invalid phymode/channel "
+				    "combination: %d %d\n", phymode, channel);
+			mutex_unlock(&priv->mutex);
+			return -EINVAL;
+		}
+
+		/* Cancel any currently running scans... */
+		if (iwl_scan_cancel_timeout(priv, 100))
+			IWL_WARNING("Could not cancel scan.\n");
+		else {
+			IWL_DEBUG_INFO("Committing phymode and "
+				       "rxon.channel = %d %d\n",
+				       phymode, channel);
+
+			iwl_set_rxon_channel(priv, phymode, channel);
+			iwl_set_flags_for_phymode(priv, phymode);
+
+			iwl_set_rate(priv);
+			iwl_commit_rxon(priv);
+		}
+	}
+	mutex_unlock(&priv->mutex);
+
+	return count;
+}
+
+static DEVICE_ATTR(tune, S_IWUSR | S_IRUGO, show_tune, store_tune);
+
+#ifdef CONFIG_IWLWIFI_SPECTRUM_MEASUREMENT
+
+static ssize_t show_measurement(struct device *d,
+				struct device_attribute *attr, char *buf)
+{
+	struct iwl_priv *priv = dev_get_drvdata(d);
+	struct iwl_spectrum_notification measure_report;
+	u32 size = sizeof(measure_report), len = 0, ofs = 0;
+	u8 *data = (u8 *) & measure_report;
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	if (!(priv->measurement_status & MEASUREMENT_READY)) {
+		spin_unlock_irqrestore(&priv->lock, flags);
+		return 0;
+	}
+	memcpy(&measure_report, &priv->measure_report, size);
+	priv->measurement_status = 0;
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	while (size && (PAGE_SIZE - len)) {
+		hex_dump_to_buffer(data + ofs, size, 16, 1, buf + len,
+				   PAGE_SIZE - len, 1);
+		len = strlen(buf);
+		if (PAGE_SIZE - len)
+			buf[len++] = '\n';
+
+		ofs += 16;
+		size -= min(size, 16U);
+	}
+
+	return len;
+}
+
+static ssize_t store_measurement(struct device *d,
+				 struct device_attribute *attr,
+				 const char *buf, size_t count)
+{
+	struct iwl_priv *priv = dev_get_drvdata(d);
+	struct ieee80211_measurement_params params = {
+		.channel = le16_to_cpu(priv->active_rxon.channel),
+		.start_time = cpu_to_le64(priv->last_tsf),
+		.duration = cpu_to_le16(1),
+	};
+	u8 type = IWL_MEASURE_BASIC;
+	u8 buffer[32];
+	u8 channel;
+
+	if (count) {
+		char *p = buffer;
+		strncpy(buffer, buf, min(sizeof(buffer), count));
+		channel = simple_strtoul(p, NULL, 0);
+		if (channel)
+			params.channel = channel;
+
+		p = buffer;
+		while (*p && *p != ' ')
+			p++;
+		if (*p)
+			type = simple_strtoul(p + 1, NULL, 0);
+	}
+
+	IWL_DEBUG_INFO("Invoking measurement of type %d on "
+		       "channel %d (for '%s')\n", type, params.channel, buf);
+	iwl_get_measurement(priv, &params, type);
+
+	return count;
+}
+
+static DEVICE_ATTR(measurement, S_IRUSR | S_IWUSR,
+		   show_measurement, store_measurement);
+#endif /* CONFIG_IWLWIFI_SPECTRUM_MEASUREMENT */
+
+static ssize_t show_rate(struct device *d,
+			 struct device_attribute *attr, char *buf)
+{
+	struct iwl_priv *priv = dev_get_drvdata(d);
+	unsigned long flags;
+	int i;
+
+	spin_lock_irqsave(&priv->sta_lock, flags);
+	if (priv->iw_mode == IEEE80211_IF_TYPE_STA)
+		i = priv->stations[IWL_AP_ID].current_rate.s.rate;
+	else
+		i = priv->stations[IWL_STA_ID].current_rate.s.rate;
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+
+	i = iwl_rate_index_from_plcp(i);
+	if (i == -1)
+		return sprintf(buf, "0\n");
+
+	return sprintf(buf, "%d%s\n",
+		       (iwl_rates[i].ieee >> 1),
+		       (iwl_rates[i].ieee & 0x1) ? ".5" : "");
+}
+
+static DEVICE_ATTR(rate, S_IRUSR, show_rate, NULL);
+
+static ssize_t store_retry_rate(struct device *d,
+				struct device_attribute *attr,
+				const char *buf, size_t count)
+{
+	struct iwl_priv *priv = dev_get_drvdata(d);
+
+	priv->retry_rate = simple_strtoul(buf, NULL, 0);
+	if (priv->retry_rate <= 0)
+		priv->retry_rate = 1;
+
+	return count;
+}
+
+static ssize_t show_retry_rate(struct device *d,
+			       struct device_attribute *attr, char *buf)
+{
+	struct iwl_priv *priv = dev_get_drvdata(d);
+	return sprintf(buf, "%d", priv->retry_rate);
+}
+
+static DEVICE_ATTR(retry_rate, S_IWUSR | S_IRUSR, show_retry_rate,
+		   store_retry_rate);
+
+static ssize_t store_power_level(struct device *d,
+				 struct device_attribute *attr,
+				 const char *buf, size_t count)
+{
+	struct iwl_priv *priv = dev_get_drvdata(d);
+	int rc;
+	int mode;
+
+	mode = simple_strtoul(buf, NULL, 0);
+	mutex_lock(&priv->mutex);
+
+	if (!iwl_is_ready(priv)) {
+		rc = -EAGAIN;
+		goto out;
+	}
+
+	if ((mode < 1) || (mode > IWL_POWER_LIMIT) || (mode == IWL_POWER_AC))
+		mode = IWL_POWER_AC;
+	else
+		mode |= IWL_POWER_ENABLED;
+
+	if (mode != priv->power_mode) {
+		rc = iwl_send_power_mode(priv, IWL_POWER_LEVEL(mode));
+		if (rc) {
+			IWL_DEBUG_MAC80211("failed setting power mode.\n");
+			goto out;
+		}
+		priv->power_mode = mode;
+	}
+
+	rc = count;
+
+ out:
+	mutex_unlock(&priv->mutex);
+	return rc;
+}
+
+#define MAX_WX_STRING 80
+
+/* Values are in microsecond */
+static const s32 timeout_duration[] = {
+	350000,
+	250000,
+	75000,
+	37000,
+	25000,
+};
+static const s32 period_duration[] = {
+	400000,
+	700000,
+	1000000,
+	1000000,
+	1000000
+};
+
+static ssize_t show_power_level(struct device *d,
+				struct device_attribute *attr, char *buf)
+{
+	struct iwl_priv *priv = dev_get_drvdata(d);
+	int level = IWL_POWER_LEVEL(priv->power_mode);
+	char *p = buf;
+
+	p += sprintf(p, "%d ", level);
+	switch (level) {
+	case IWL_POWER_MODE_CAM:
+	case IWL_POWER_AC:
+		p += sprintf(p, "(AC)");
+		break;
+	case IWL_POWER_BATTERY:
+		p += sprintf(p, "(BATTERY)");
+		break;
+	default:
+		p += sprintf(p,
+			     "(Timeout %dms, Period %dms)",
+			     timeout_duration[level - 1] / 1000,
+			     period_duration[level - 1] / 1000);
+	}
+
+	if (!(priv->power_mode & IWL_POWER_ENABLED))
+		p += sprintf(p, " OFF\n");
+	else
+		p += sprintf(p, " \n");
+
+	return (p - buf + 1);
+
+}
+
+static DEVICE_ATTR(power_level, S_IWUSR | S_IRUSR, show_power_level,
+		   store_power_level);
+
+static ssize_t show_channels(struct device *d,
+			     struct device_attribute *attr, char *buf)
+{
+	struct iwl_priv *priv = dev_get_drvdata(d);
+	int len = 0, i;
+	struct ieee80211_channel *channels = NULL;
+	const struct ieee80211_hw_mode *hw_mode = NULL;
+	int count = 0;
+
+	if (!iwl_is_ready(priv))
+		return -EAGAIN;
+
+	hw_mode = iwl_get_hw_mode(priv, MODE_IEEE80211G);
+	if (!hw_mode)
+		hw_mode = iwl_get_hw_mode(priv, MODE_IEEE80211B);
+	if (hw_mode) {
+		channels = hw_mode->channels;
+		count = hw_mode->num_channels;
+	}
+
+	len +=
+	    sprintf(&buf[len],
+		    "Displaying %d channels in 2.4GHz band "
+		    "(802.11bg):\n", count);
+
+	for (i = 0; i < count; i++)
+		len += sprintf(&buf[len], "%d: %ddBm: BSS%s%s, %s.\n",
+			       channels[i].chan,
+			       channels[i].power_level,
+			       channels[i].
+			       flag & IEEE80211_CHAN_W_RADAR_DETECT ?
+			       " (IEEE 802.11h required)" : "",
+			       (!(channels[i].flag & IEEE80211_CHAN_W_IBSS)
+				|| (channels[i].
+				    flag &
+				    IEEE80211_CHAN_W_RADAR_DETECT)) ? "" :
+			       ", IBSS",
+			       channels[i].
+			       flag & IEEE80211_CHAN_W_ACTIVE_SCAN ?
+			       "active/passive" : "passive only");
+
+	hw_mode = iwl_get_hw_mode(priv, MODE_IEEE80211A);
+	if (hw_mode) {
+		channels = hw_mode->channels;
+		count = hw_mode->num_channels;
+	} else {
+		channels = NULL;
+		count = 0;
+	}
+
+	len += sprintf(&buf[len], "Displaying %d channels in 5.2GHz band "
+		       "(802.11a):\n", count);
+
+	for (i = 0; i < count; i++)
+		len += sprintf(&buf[len], "%d: %ddBm: BSS%s%s, %s.\n",
+			       channels[i].chan,
+			       channels[i].power_level,
+			       channels[i].
+			       flag & IEEE80211_CHAN_W_RADAR_DETECT ?
+			       " (IEEE 802.11h required)" : "",
+			       (!(channels[i].flag & IEEE80211_CHAN_W_IBSS)
+				|| (channels[i].
+				    flag &
+				    IEEE80211_CHAN_W_RADAR_DETECT)) ? "" :
+			       ", IBSS",
+			       channels[i].
+			       flag & IEEE80211_CHAN_W_ACTIVE_SCAN ?
+			       "active/passive" : "passive only");
+
+	return len;
+}
+
+static DEVICE_ATTR(channels, S_IRUSR, show_channels, NULL);
+
+static ssize_t show_statistics(struct device *d,
+			       struct device_attribute *attr, char *buf)
+{
+	struct iwl_priv *priv = dev_get_drvdata(d);
+	u32 size = sizeof(struct iwl_notif_statistics);
+	u32 len = 0, ofs = 0;
+	u8 *data = (u8 *) & priv->statistics;
+	int rc = 0;
+
+	if (!iwl_is_alive(priv))
+		return -EAGAIN;
+
+	mutex_lock(&priv->mutex);
+	rc = iwl_send_statistics_request(priv);
+	mutex_unlock(&priv->mutex);
+
+	if (rc) {
+		len = sprintf(buf,
+			      "Error sending statistics request: 0x%08X\n", rc);
+		return len;
+	}
+
+	while (size && (PAGE_SIZE - len)) {
+		hex_dump_to_buffer(data + ofs, size, 16, 1, buf + len,
+				   PAGE_SIZE - len, 1);
+		len = strlen(buf);
+		if (PAGE_SIZE - len)
+			buf[len++] = '\n';
+
+		ofs += 16;
+		size -= min(size, 16U);
+	}
+
+	return len;
+}
+
+static DEVICE_ATTR(statistics, S_IRUGO, show_statistics, NULL);
+
+static ssize_t show_antenna(struct device *d,
+			    struct device_attribute *attr, char *buf)
+{
+	struct iwl_priv *priv = dev_get_drvdata(d);
+
+	if (!iwl_is_alive(priv))
+		return -EAGAIN;
+
+	return sprintf(buf, "%d\n", priv->antenna);
+}
+
+static ssize_t store_antenna(struct device *d,
+			     struct device_attribute *attr,
+			     const char *buf, size_t count)
+{
+	int ant;
+	struct iwl_priv *priv = dev_get_drvdata(d);
+
+	if (count == 0)
+		return 0;
+
+	if (sscanf(buf, "%1i", &ant) != 1) {
+		IWL_DEBUG_INFO("not in hex or decimal form.\n");
+		return count;
+	}
+
+	if ((ant >= 0) && (ant <= 2)) {
+		IWL_DEBUG_INFO("Setting antenna select to %d.\n", ant);
+		priv->antenna = (enum iwl_antenna)ant;
+	} else
+		IWL_DEBUG_INFO("Bad antenna select value %d.\n", ant);
+
+
+	return count;
+}
+
+static DEVICE_ATTR(antenna, S_IWUSR | S_IRUGO, show_antenna, store_antenna);
+
+static ssize_t show_status(struct device *d,
+			   struct device_attribute *attr, char *buf)
+{
+	struct iwl_priv *priv = (struct iwl_priv *)d->driver_data;
+	if (!iwl_is_alive(priv))
+		return -EAGAIN;
+	return sprintf(buf, "0x%08x\n", (int)priv->status);
+}
+
+static DEVICE_ATTR(status, S_IRUGO, show_status, NULL);
+
+static ssize_t dump_error_log(struct device *d,
+			      struct device_attribute *attr,
+			      const char *buf, size_t count)
+{
+	char *p = (char *)buf;
+
+	if (p[0] == '1')
+		iwl_dump_nic_error_log((struct iwl_priv *)d->driver_data);
+
+	return strnlen(buf, count);
+}
+
+static DEVICE_ATTR(dump_errors, S_IWUSR, NULL, dump_error_log);
+
+static ssize_t dump_event_log(struct device *d,
+			      struct device_attribute *attr,
+			      const char *buf, size_t count)
+{
+	char *p = (char *)buf;
+
+	if (p[0] == '1')
+		iwl_dump_nic_event_log((struct iwl_priv *)d->driver_data);
+
+	return strnlen(buf, count);
+}
+
+static DEVICE_ATTR(dump_events, S_IWUSR, NULL, dump_event_log);
+
+/*****************************************************************************
+ *
+ * driver setup and teardown
+ *
+ *****************************************************************************/
+
+static void iwl_setup_deferred_work(struct iwl_priv *priv)
+{
+	priv->workqueue = create_workqueue(DRV_NAME);
+
+	init_waitqueue_head(&priv->wait_command_queue);
+
+	INIT_WORK(&priv->up, iwl_bg_up);
+	INIT_WORK(&priv->restart, iwl_bg_restart);
+	INIT_WORK(&priv->rx_replenish, iwl_bg_rx_replenish);
+	INIT_WORK(&priv->scan_completed, iwl_bg_scan_completed);
+	INIT_WORK(&priv->request_scan, iwl_bg_request_scan);
+	INIT_WORK(&priv->abort_scan, iwl_bg_abort_scan);
+	INIT_WORK(&priv->rf_kill, iwl_bg_rf_kill);
+	INIT_WORK(&priv->beacon_update, iwl_bg_beacon_update);
+	INIT_DELAYED_WORK(&priv->post_associate, iwl_bg_post_associate);
+	INIT_DELAYED_WORK(&priv->init_alive_start, iwl_bg_init_alive_start);
+	INIT_DELAYED_WORK(&priv->alive_start, iwl_bg_alive_start);
+	INIT_DELAYED_WORK(&priv->scan_check, iwl_bg_scan_check);
+
+	iwl_hw_setup_deferred_work(priv);
+
+	tasklet_init(&priv->irq_tasklet, (void (*)(unsigned long))
+		     iwl_irq_tasklet, (unsigned long)priv);
+}
+
+static void iwl_cancel_deferred_work(struct iwl_priv *priv)
+{
+	iwl_hw_cancel_deferred_work(priv);
+
+	cancel_delayed_work(&priv->scan_check);
+	cancel_delayed_work(&priv->alive_start);
+	cancel_delayed_work(&priv->post_associate);
+	cancel_work_sync(&priv->beacon_update);
+}
+
+static struct attribute *iwl_sysfs_entries[] = {
+	&dev_attr_antenna.attr,
+	&dev_attr_channels.attr,
+	&dev_attr_dump_errors.attr,
+	&dev_attr_dump_events.attr,
+	&dev_attr_flags.attr,
+	&dev_attr_filter_flags.attr,
+#ifdef CONFIG_IWLWIFI_SPECTRUM_MEASUREMENT
+	&dev_attr_measurement.attr,
+#endif
+	&dev_attr_power_level.attr,
+	&dev_attr_rate.attr,
+	&dev_attr_retry_rate.attr,
+	&dev_attr_rf_kill.attr,
+	&dev_attr_rs_window.attr,
+	&dev_attr_statistics.attr,
+	&dev_attr_status.attr,
+	&dev_attr_temperature.attr,
+	&dev_attr_tune.attr,
+	&dev_attr_tx_power.attr,
+
+	NULL
+};
+
+static struct attribute_group iwl_attribute_group = {
+	.name = NULL,		/* put in device directory */
+	.attrs = iwl_sysfs_entries,
+};
+
+static struct ieee80211_ops iwl_hw_ops = {
+	.tx = iwl_mac_tx,
+	.start = iwl_mac_start,
+	.stop = iwl_mac_stop,
+	.add_interface = iwl_mac_add_interface,
+	.remove_interface = iwl_mac_remove_interface,
+	.config = iwl_mac_config,
+	.config_interface = iwl_mac_config_interface,
+	.configure_filter = iwl_configure_filter,
+	.set_key = iwl_mac_set_key,
+	.get_stats = iwl_mac_get_stats,
+	.get_tx_stats = iwl_mac_get_tx_stats,
+	.conf_tx = iwl_mac_conf_tx,
+	.get_tsf = iwl_mac_get_tsf,
+	.reset_tsf = iwl_mac_reset_tsf,
+	.beacon_update = iwl_mac_beacon_update,
+	.hw_scan = iwl_mac_hw_scan
+};
+
+static int iwl_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
+{
+	int err = 0;
+	u32 pci_id;
+	struct iwl_priv *priv;
+	struct ieee80211_hw *hw;
+	int i;
+
+	if (iwl_param_disable_hw_scan) {
+		IWL_DEBUG_INFO("Disabling hw_scan\n");
+		iwl_hw_ops.hw_scan = NULL;
+	}
+
+	if ((iwl_param_queues_num > IWL_MAX_NUM_QUEUES) ||
+	    (iwl_param_queues_num < IWL_MIN_NUM_QUEUES)) {
+		IWL_ERROR("invalid queues_num, should be between %d and %d\n",
+			  IWL_MIN_NUM_QUEUES, IWL_MAX_NUM_QUEUES);
+		err = -EINVAL;
+		goto out;
+	}
+
+	/* mac80211 allocates memory for this device instance, including
+	 *   space for this driver's private structure */
+	hw = ieee80211_alloc_hw(sizeof(struct iwl_priv), &iwl_hw_ops);
+	if (hw == NULL) {
+		IWL_ERROR("Can not allocate network device\n");
+		err = -ENOMEM;
+		goto out;
+	}
+	SET_IEEE80211_DEV(hw, &pdev->dev);
+
+	IWL_DEBUG_INFO("*** LOAD DRIVER ***\n");
+	priv = hw->priv;
+	priv->hw = hw;
+
+	priv->pci_dev = pdev;
+	priv->antenna = (enum iwl_antenna)iwl_param_antenna;
+#ifdef CONFIG_IWLWIFI_DEBUG
+	iwl_debug_level = iwl_param_debug;
+	atomic_set(&priv->restrict_refcnt, 0);
+#endif
+	priv->retry_rate = 1;
+
+	priv->ibss_beacon = NULL;
+
+	/* Tell mac80211 and its clients (e.g. Wireless Extensions)
+	 *   the range of signal quality values that we'll provide.
+	 * Negative values for level/noise indicate that we'll provide dBm.
+	 * For WE, at least, non-0 values here *enable* display of values
+	 *   in app (iwconfig). */
+	hw->max_rssi = -20;	/* signal level, negative indicates dBm */
+	hw->max_noise = -20;	/* noise level, negative indicates dBm */
+	hw->max_signal = 100;	/* link quality indication (%) */
+
+	/* Tell mac80211 our Tx characteristics */
+	hw->flags = IEEE80211_HW_HOST_GEN_BEACON_TEMPLATE;
+
+	hw->queues = 4;
+
+	spin_lock_init(&priv->lock);
+	spin_lock_init(&priv->power_data.lock);
+	spin_lock_init(&priv->sta_lock);
+	spin_lock_init(&priv->hcmd_lock);
+
+	for (i = 0; i < IWL_IBSS_MAC_HASH_SIZE; i++)
+		INIT_LIST_HEAD(&priv->ibss_mac_hash[i]);
+
+	INIT_LIST_HEAD(&priv->free_frames);
+
+	mutex_init(&priv->mutex);
+	if (pci_enable_device(pdev)) {
+		err = -ENODEV;
+		goto out_ieee80211_free_hw;
+	}
+
+	pci_set_master(pdev);
+
+	iwl_clear_stations_table(priv);
+
+	priv->data_retry_limit = -1;
+	priv->ieee_channels = NULL;
+	priv->ieee_rates = NULL;
+	priv->phymode = -1;
+
+	err = pci_set_dma_mask(pdev, DMA_32BIT_MASK);
+	if (!err)
+		err = pci_set_consistent_dma_mask(pdev, DMA_32BIT_MASK);
+	if (err) {
+		printk(KERN_WARNING DRV_NAME ": No suitable DMA available.\n");
+		goto out_pci_disable_device;
+	}
+
+	pci_set_drvdata(pdev, priv);
+	err = pci_request_regions(pdev, DRV_NAME);
+	if (err)
+		goto out_pci_disable_device;
+	/* We disable the RETRY_TIMEOUT register (0x41) to keep
+	 * PCI Tx retries from interfering with C3 CPU state */
+	pci_write_config_byte(pdev, 0x41, 0x00);
+	priv->hw_base = pci_iomap(pdev, 0, 0);
+	if (!priv->hw_base) {
+		err = -ENODEV;
+		goto out_pci_release_regions;
+	}
+
+	IWL_DEBUG_INFO("pci_resource_len = 0x%08llx\n",
+			(unsigned long long) pci_resource_len(pdev, 0));
+	IWL_DEBUG_INFO("pci_resource_base = %p\n", priv->hw_base);
+
+	/* Initialize module parameter values here */
+
+	if (iwl_param_disable) {
+		set_bit(STATUS_RF_KILL_SW, &priv->status);
+		IWL_DEBUG_INFO("Radio disabled.\n");
+	}
+
+	priv->iw_mode = IEEE80211_IF_TYPE_STA;
+
+	pci_id =
+	    (priv->pci_dev->device << 16) | priv->pci_dev->subsystem_device;
+
+	switch (pci_id) {
+	case 0x42221005:	/* 0x4222 0x8086 0x1005 is BG SKU */
+	case 0x42221034:	/* 0x4222 0x8086 0x1034 is BG SKU */
+	case 0x42271014:	/* 0x4227 0x8086 0x1014 is BG SKU */
+	case 0x42221044:	/* 0x4222 0x8086 0x1044 is BG SKU */
+		priv->is_abg = 0;
+		break;
+
+	/*
+	 * Rest are assumed ABG SKU -- if this is not the
+	 * case then the card will get the wrong 'Detected'
+	 * line in the kernel log however the code that
+	 * initializes the GEO table will detect no A-band
+	 * channels and remove the is_abg mask.
+	 */
+	default:
+		priv->is_abg = 1;
+		break;
+	}
+
+	printk(KERN_INFO DRV_NAME
+	       ": Detected Intel PRO/Wireless 3945%sBG Network Connection\n",
+	       priv->is_abg ? "A" : "");
+
+	/* Device-specific setup */
+	if (iwl_hw_set_hw_setting(priv)) {
+		IWL_ERROR("failed to set hw settings\n");
+		mutex_unlock(&priv->mutex);
+		goto out_iounmap;
+	}
+
+#ifdef CONFIG_IWLWIFI_QOS
+	if (iwl_param_qos_enable)
+		priv->qos_data.qos_enable = 1;
+
+	iwl_reset_qos(priv);
+
+	priv->qos_data.qos_active = 0;
+	priv->qos_data.qos_cap.val = 0;
+#endif /* CONFIG_IWLWIFI_QOS */
+
+	iwl_set_rxon_channel(priv, MODE_IEEE80211G, 6);
+	iwl_setup_deferred_work(priv);
+	iwl_setup_rx_handlers(priv);
+
+	priv->rates_mask = IWL_RATES_MASK;
+	/* If power management is turned on, default to AC mode */
+	priv->power_mode = IWL_POWER_AC;
+	priv->user_txpower_limit = IWL_DEFAULT_TX_POWER;
+
+	pci_enable_msi(pdev);
+
+	err = request_irq(pdev->irq, iwl_isr, IRQF_SHARED, DRV_NAME, priv);
+	if (err) {
+		IWL_ERROR("Error allocating IRQ %d\n", pdev->irq);
+		goto out_disable_msi;
+	}
+
+	mutex_lock(&priv->mutex);
+
+	err = sysfs_create_group(&pdev->dev.kobj, &iwl_attribute_group);
+	if (err) {
+		IWL_ERROR("failed to create sysfs device attributes\n");
+		mutex_unlock(&priv->mutex);
+		goto out_release_irq;
+	}
+
+	/* fetch ucode file from disk, alloc and copy to bus-master buffers ...
+	 * ucode filename and max sizes are card-specific. */
+	err = iwl_read_ucode(priv);
+	if (err) {
+		IWL_ERROR("Could not read microcode: %d\n", err);
+		mutex_unlock(&priv->mutex);
+		goto out_pci_alloc;
+	}
+
+	mutex_unlock(&priv->mutex);
+
+	IWL_DEBUG_INFO("Queing UP work.\n");
+
+	queue_work(priv->workqueue, &priv->up);
+
+	return 0;
+
+ out_pci_alloc:
+	iwl_dealloc_ucode_pci(priv);
+
+	sysfs_remove_group(&pdev->dev.kobj, &iwl_attribute_group);
+
+ out_release_irq:
+	free_irq(pdev->irq, priv);
+
+ out_disable_msi:
+	pci_disable_msi(pdev);
+	destroy_workqueue(priv->workqueue);
+	priv->workqueue = NULL;
+	iwl_unset_hw_setting(priv);
+
+ out_iounmap:
+	pci_iounmap(pdev, priv->hw_base);
+ out_pci_release_regions:
+	pci_release_regions(pdev);
+ out_pci_disable_device:
+	pci_disable_device(pdev);
+	pci_set_drvdata(pdev, NULL);
+ out_ieee80211_free_hw:
+	ieee80211_free_hw(priv->hw);
+ out:
+	return err;
+}
+
+static void iwl_pci_remove(struct pci_dev *pdev)
+{
+	struct iwl_priv *priv = pci_get_drvdata(pdev);
+	struct list_head *p, *q;
+	int i;
+
+	if (!priv)
+		return;
+
+	IWL_DEBUG_INFO("*** UNLOAD DRIVER ***\n");
+
+	mutex_lock(&priv->mutex);
+	set_bit(STATUS_EXIT_PENDING, &priv->status);
+	__iwl_down(priv);
+	mutex_unlock(&priv->mutex);
+
+	/* Free MAC hash list for ADHOC */
+	for (i = 0; i < IWL_IBSS_MAC_HASH_SIZE; i++) {
+		list_for_each_safe(p, q, &priv->ibss_mac_hash[i]) {
+			list_del(p);
+			kfree(list_entry(p, struct iwl_ibss_seq, list));
+		}
+	}
+
+	sysfs_remove_group(&pdev->dev.kobj, &iwl_attribute_group);
+
+	iwl_dealloc_ucode_pci(priv);
+
+	if (priv->rxq.bd)
+		iwl_rx_queue_free(priv, &priv->rxq);
+	iwl_hw_txq_ctx_free(priv);
+
+	iwl_unset_hw_setting(priv);
+	iwl_clear_stations_table(priv);
+
+	if (priv->mac80211_registered) {
+		ieee80211_unregister_hw(priv->hw);
+		iwl_rate_control_unregister(priv->hw);
+	}
+
+	/* ieee80211_unregister_hw calls iwl_mac_stop, which flushes
+	 * priv->workqueue... so we can't take down the workqueue
+	 * until now... */
+	destroy_workqueue(priv->workqueue);
+	priv->workqueue = NULL;
+
+	free_irq(pdev->irq, priv);
+	pci_disable_msi(pdev);
+	pci_iounmap(pdev, priv->hw_base);
+	pci_release_regions(pdev);
+	pci_disable_device(pdev);
+	pci_set_drvdata(pdev, NULL);
+
+	kfree(priv->channel_info);
+
+	kfree(priv->ieee_channels);
+	kfree(priv->ieee_rates);
+
+	if (priv->ibss_beacon)
+		dev_kfree_skb(priv->ibss_beacon);
+
+	ieee80211_free_hw(priv->hw);
+}
+
+#ifdef CONFIG_PM
+
+static int iwl_pci_suspend(struct pci_dev *pdev, pm_message_t state)
+{
+	struct iwl_priv *priv = pci_get_drvdata(pdev);
+
+	mutex_lock(&priv->mutex);
+
+	set_bit(STATUS_IN_SUSPEND, &priv->status);
+
+	/* Take down the device; powers it off, etc. */
+	__iwl_down(priv);
+
+	if (priv->mac80211_registered)
+		ieee80211_stop_queues(priv->hw);
+
+	pci_save_state(pdev);
+	pci_disable_device(pdev);
+	pci_set_power_state(pdev, PCI_D3hot);
+
+	mutex_unlock(&priv->mutex);
+
+	return 0;
+}
+
+static void iwl_resume(struct iwl_priv *priv)
+{
+	unsigned long flags;
+
+	/* The following it a temporary work around due to the
+	 * suspend / resume not fully initializing the NIC correctly.
+	 * Without all of the following, resume will not attempt to take
+	 * down the NIC (it shouldn't really need to) and will just try
+	 * and bring the NIC back up.  However that fails during the
+	 * ucode verification process.  This then causes iwl_down to be
+	 * called *after* iwl_hw_nic_init() has succeeded -- which
+	 * then lets the next init sequence succeed.  So, we've
+	 * replicated all of that NIC init code here... */
+
+	iwl_write32(priv, CSR_INT, 0xFFFFFFFF);
+
+	iwl_hw_nic_init(priv);
+
+	iwl_write32(priv, CSR_UCODE_DRV_GP1_CLR, CSR_UCODE_SW_BIT_RFKILL);
+	iwl_write32(priv, CSR_UCODE_DRV_GP1_CLR,
+		    CSR_UCODE_DRV_GP1_BIT_CMD_BLOCKED);
+	iwl_write32(priv, CSR_INT, 0xFFFFFFFF);
+	iwl_write32(priv, CSR_UCODE_DRV_GP1_CLR, CSR_UCODE_SW_BIT_RFKILL);
+	iwl_write32(priv, CSR_UCODE_DRV_GP1_CLR, CSR_UCODE_SW_BIT_RFKILL);
+
+	/* tell the device to stop sending interrupts */
+	iwl_disable_interrupts(priv);
+
+	spin_lock_irqsave(&priv->lock, flags);
+	iwl_clear_bit(priv, CSR_GP_CNTRL, CSR_GP_CNTRL_REG_FLAG_MAC_ACCESS_REQ);
+
+	if (!iwl_grab_restricted_access(priv)) {
+		iwl_write_restricted_reg(priv, APMG_CLK_DIS_REG,
+					 APMG_CLK_VAL_DMA_CLK_RQT);
+		iwl_release_restricted_access(priv);
+	}
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	udelay(5);
+
+	iwl_hw_nic_reset(priv);
+
+	/* Bring the device back up */
+	clear_bit(STATUS_IN_SUSPEND, &priv->status);
+	queue_work(priv->workqueue, &priv->up);
+}
+
+static int iwl_pci_resume(struct pci_dev *pdev)
+{
+	struct iwl_priv *priv = pci_get_drvdata(pdev);
+	int err;
+
+	printk(KERN_INFO "Coming out of suspend...\n");
+
+	mutex_lock(&priv->mutex);
+
+	pci_set_power_state(pdev, PCI_D0);
+	err = pci_enable_device(pdev);
+	pci_restore_state(pdev);
+
+	/*
+	 * Suspend/Resume resets the PCI configuration space, so we have to
+	 * re-disable the RETRY_TIMEOUT register (0x41) to keep PCI Tx retries
+	 * from interfering with C3 CPU state. pci_restore_state won't help
+	 * here since it only restores the first 64 bytes pci config header.
+	 */
+	pci_write_config_byte(pdev, 0x41, 0x00);
+
+	iwl_resume(priv);
+	mutex_unlock(&priv->mutex);
+
+	return 0;
+}
+
+#endif /* CONFIG_PM */
+
+/*****************************************************************************
+ *
+ * driver and module entry point
+ *
+ *****************************************************************************/
+
+static struct pci_driver iwl_driver = {
+	.name = DRV_NAME,
+	.id_table = iwl_hw_card_ids,
+	.probe = iwl_pci_probe,
+	.remove = __devexit_p(iwl_pci_remove),
+#ifdef CONFIG_PM
+	.suspend = iwl_pci_suspend,
+	.resume = iwl_pci_resume,
+#endif
+};
+
+static int __init iwl_init(void)
+{
+
+	int ret;
+	printk(KERN_INFO DRV_NAME ": " DRV_DESCRIPTION ", " DRV_VERSION "\n");
+	printk(KERN_INFO DRV_NAME ": " DRV_COPYRIGHT "\n");
+	ret = pci_register_driver(&iwl_driver);
+	if (ret) {
+		IWL_ERROR("Unable to initialize PCI module\n");
+		return ret;
+	}
+#ifdef CONFIG_IWLWIFI_DEBUG
+	ret = driver_create_file(&iwl_driver.driver, &driver_attr_debug_level);
+	if (ret) {
+		IWL_ERROR("Unable to create driver sysfs file\n");
+		pci_unregister_driver(&iwl_driver);
+		return ret;
+	}
+#endif
+
+	return ret;
+}
+
+static void __exit iwl_exit(void)
+{
+#ifdef CONFIG_IWLWIFI_DEBUG
+	driver_remove_file(&iwl_driver.driver, &driver_attr_debug_level);
+#endif
+	pci_unregister_driver(&iwl_driver);
+}
+
+module_param_named(antenna, iwl_param_antenna, int, 0444);
+MODULE_PARM_DESC(antenna, "select antenna (1=Main, 2=Aux, default 0 [both])");
+module_param_named(disable, iwl_param_disable, int, 0444);
+MODULE_PARM_DESC(disable, "manually disable the radio (default 0 [radio on])");
+module_param_named(hwcrypto, iwl_param_hwcrypto, int, 0444);
+MODULE_PARM_DESC(hwcrypto,
+		 "using hardware crypto engine (default 0 [software])\n");
+module_param_named(debug, iwl_param_debug, int, 0444);
+MODULE_PARM_DESC(debug, "debug output mask");
+module_param_named(disable_hw_scan, iwl_param_disable_hw_scan, int, 0444);
+MODULE_PARM_DESC(disable_hw_scan, "disable hardware scanning (default 0)");
+
+module_param_named(queues_num, iwl_param_queues_num, int, 0444);
+MODULE_PARM_DESC(queues_num, "number of hw queues.");
+
+/* QoS */
+module_param_named(qos_enable, iwl_param_qos_enable, int, 0444);
+MODULE_PARM_DESC(qos_enable, "enable all QoS functionality");
+
+module_exit(iwl_exit);
+module_init(iwl_init);
diff --git a/drivers/net/wireless/iwlwifi/iwl4965-base.c b/drivers/net/wireless/iwlwifi/iwl4965-base.c
new file mode 100644
index 000000000000..b1a6e39f7821
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl4965-base.c
@@ -0,0 +1,9340 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2003 - 2007 Intel Corporation. All rights reserved.
+ *
+ * Portions of this file are derived from the ipw3945 project, as well
+ * as portions of the ieee80211 subsystem header files.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * James P. Ketrenos <ipw2100-admin@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+
+/*
+ * NOTE:  This file (iwl-base.c) is used to build to multiple hardware targets
+ * by defining IWL to either 3945 or 4965.  The Makefile used when building
+ * the base targets will create base-3945.o and base-4965.o
+ *
+ * The eventual goal is to move as many of the #if IWL / #endif blocks out of
+ * this file and into the hardware specific implementation files (iwl-XXXX.c)
+ * and leave only the common (non #ifdef sprinkled) code in this file
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/version.h>
+#include <linux/init.h>
+#include <linux/pci.h>
+#include <linux/dma-mapping.h>
+#include <linux/delay.h>
+#include <linux/skbuff.h>
+#include <linux/netdevice.h>
+#include <linux/wireless.h>
+#include <linux/firmware.h>
+#include <linux/skbuff.h>
+#include <linux/netdevice.h>
+#include <linux/etherdevice.h>
+#include <linux/if_arp.h>
+
+#include <net/ieee80211_radiotap.h>
+#include <net/mac80211.h>
+
+#include <asm/div64.h>
+
+#define IWL 4965
+
+#include "iwlwifi.h"
+#include "iwl-4965.h"
+#include "iwl-helpers.h"
+
+#ifdef CONFIG_IWLWIFI_DEBUG
+u32 iwl_debug_level;
+#endif
+
+/******************************************************************************
+ *
+ * module boiler plate
+ *
+ ******************************************************************************/
+
+/* module parameters */
+int iwl_param_disable_hw_scan;
+int iwl_param_debug;
+int iwl_param_disable;      /* def: enable radio */
+int iwl_param_antenna;      /* def: 0 = both antennas (use diversity) */
+int iwl_param_hwcrypto;     /* def: using software encryption */
+int iwl_param_qos_enable = 1;
+int iwl_param_queues_num = IWL_MAX_NUM_QUEUES;
+
+/*
+ * module name, copyright, version, etc.
+ * NOTE: DRV_NAME is defined in iwlwifi.h for use by iwl-debug.h and printk
+ */
+
+#define DRV_DESCRIPTION	"Intel(R) Wireless WiFi Link 4965AGN driver for Linux"
+
+#ifdef CONFIG_IWLWIFI_DEBUG
+#define VD "d"
+#else
+#define VD
+#endif
+
+#ifdef CONFIG_IWLWIFI_SPECTRUM_MEASUREMENT
+#define VS "s"
+#else
+#define VS
+#endif
+
+#define IWLWIFI_VERSION "1.1.17k" VD VS
+#define DRV_COPYRIGHT	"Copyright(c) 2003-2007 Intel Corporation"
+#define DRV_VERSION     IWLWIFI_VERSION
+
+/* Change firmware file name, using "-" and incrementing number,
+ *   *only* when uCode interface or architecture changes so that it
+ *   is not compatible with earlier drivers.
+ * This number will also appear in << 8 position of 1st dword of uCode file */
+#define IWL4965_UCODE_API "-1"
+
+MODULE_DESCRIPTION(DRV_DESCRIPTION);
+MODULE_VERSION(DRV_VERSION);
+MODULE_AUTHOR(DRV_COPYRIGHT);
+MODULE_LICENSE("GPL");
+
+__le16 *ieee80211_get_qos_ctrl(struct ieee80211_hdr *hdr)
+{
+	u16 fc = le16_to_cpu(hdr->frame_control);
+	int hdr_len = ieee80211_get_hdrlen(fc);
+
+	if ((fc & 0x00cc) == (IEEE80211_STYPE_QOS_DATA | IEEE80211_FTYPE_DATA))
+		return (__le16 *) ((u8 *) hdr + hdr_len - QOS_CONTROL_LEN);
+	return NULL;
+}
+
+static const struct ieee80211_hw_mode *iwl_get_hw_mode(
+		struct iwl_priv *priv, int mode)
+{
+	int i;
+
+	for (i = 0; i < 3; i++)
+		if (priv->modes[i].mode == mode)
+			return &priv->modes[i];
+
+	return NULL;
+}
+
+static int iwl_is_empty_essid(const char *essid, int essid_len)
+{
+	/* Single white space is for Linksys APs */
+	if (essid_len == 1 && essid[0] == ' ')
+		return 1;
+
+	/* Otherwise, if the entire essid is 0, we assume it is hidden */
+	while (essid_len) {
+		essid_len--;
+		if (essid[essid_len] != '\0')
+			return 0;
+	}
+
+	return 1;
+}
+
+static const char *iwl_escape_essid(const char *essid, u8 essid_len)
+{
+	static char escaped[IW_ESSID_MAX_SIZE * 2 + 1];
+	const char *s = essid;
+	char *d = escaped;
+
+	if (iwl_is_empty_essid(essid, essid_len)) {
+		memcpy(escaped, "<hidden>", sizeof("<hidden>"));
+		return escaped;
+	}
+
+	essid_len = min(essid_len, (u8) IW_ESSID_MAX_SIZE);
+	while (essid_len--) {
+		if (*s == '\0') {
+			*d++ = '\\';
+			*d++ = '0';
+			s++;
+		} else
+			*d++ = *s++;
+	}
+	*d = '\0';
+	return escaped;
+}
+
+static void iwl_print_hex_dump(int level, void *p, u32 len)
+{
+#ifdef CONFIG_IWLWIFI_DEBUG
+	if (!(iwl_debug_level & level))
+		return;
+
+	print_hex_dump(KERN_DEBUG, "iwl data: ", DUMP_PREFIX_OFFSET, 16, 1,
+			p, len, 1);
+#endif
+}
+
+/*************** DMA-QUEUE-GENERAL-FUNCTIONS  *****
+ * DMA services
+ *
+ * Theory of operation
+ *
+ * A queue is a circular buffers with 'Read' and 'Write' pointers.
+ * 2 empty entries always kept in the buffer to protect from overflow.
+ *
+ * For Tx queue, there are low mark and high mark limits. If, after queuing
+ * the packet for Tx, free space become < low mark, Tx queue stopped. When
+ * reclaiming packets (on 'tx done IRQ), if free space become > high mark,
+ * Tx queue resumed.
+ *
+ * The IWL operates with six queues, one receive queue in the device's
+ * sram, one transmit queue for sending commands to the device firmware,
+ * and four transmit queues for data.
+ ***************************************************/
+
+static int iwl_queue_space(const struct iwl_queue *q)
+{
+	int s = q->last_used - q->first_empty;
+
+	if (q->last_used > q->first_empty)
+		s -= q->n_bd;
+
+	if (s <= 0)
+		s += q->n_window;
+	/* keep some reserve to not confuse empty and full situations */
+	s -= 2;
+	if (s < 0)
+		s = 0;
+	return s;
+}
+
+/* XXX: n_bd must be power-of-two size */
+static inline int iwl_queue_inc_wrap(int index, int n_bd)
+{
+	return ++index & (n_bd - 1);
+}
+
+/* XXX: n_bd must be power-of-two size */
+static inline int iwl_queue_dec_wrap(int index, int n_bd)
+{
+	return --index & (n_bd - 1);
+}
+
+static inline int x2_queue_used(const struct iwl_queue *q, int i)
+{
+	return q->first_empty > q->last_used ?
+		(i >= q->last_used && i < q->first_empty) :
+		!(i < q->last_used && i >= q->first_empty);
+}
+
+static inline u8 get_cmd_index(struct iwl_queue *q, u32 index, int is_huge)
+{
+	if (is_huge)
+		return q->n_window;
+
+	return index & (q->n_window - 1);
+}
+
+static int iwl_queue_init(struct iwl_priv *priv, struct iwl_queue *q,
+			  int count, int slots_num, u32 id)
+{
+	q->n_bd = count;
+	q->n_window = slots_num;
+	q->id = id;
+
+	/* count must be power-of-two size, otherwise iwl_queue_inc_wrap
+	 * and iwl_queue_dec_wrap are broken. */
+	BUG_ON(!is_power_of_2(count));
+
+	/* slots_num must be power-of-two size, otherwise
+	 * get_cmd_index is broken. */
+	BUG_ON(!is_power_of_2(slots_num));
+
+	q->low_mark = q->n_window / 4;
+	if (q->low_mark < 4)
+		q->low_mark = 4;
+
+	q->high_mark = q->n_window / 8;
+	if (q->high_mark < 2)
+		q->high_mark = 2;
+
+	q->first_empty = q->last_used = 0;
+
+	return 0;
+}
+
+static int iwl_tx_queue_alloc(struct iwl_priv *priv,
+			      struct iwl_tx_queue *txq, u32 id)
+{
+	struct pci_dev *dev = priv->pci_dev;
+
+	if (id != IWL_CMD_QUEUE_NUM) {
+		txq->txb = kmalloc(sizeof(txq->txb[0]) *
+				   TFD_QUEUE_SIZE_MAX, GFP_KERNEL);
+		if (!txq->txb) {
+			IWL_ERROR("kmalloc for auxilary BD "
+				  "structures failed\n");
+			goto error;
+		}
+	} else
+		txq->txb = NULL;
+
+	txq->bd = pci_alloc_consistent(dev,
+			sizeof(txq->bd[0]) * TFD_QUEUE_SIZE_MAX,
+			&txq->q.dma_addr);
+
+	if (!txq->bd) {
+		IWL_ERROR("pci_alloc_consistent(%zd) failed\n",
+			  sizeof(txq->bd[0]) * TFD_QUEUE_SIZE_MAX);
+		goto error;
+	}
+	txq->q.id = id;
+
+	return 0;
+
+ error:
+	if (txq->txb) {
+		kfree(txq->txb);
+		txq->txb = NULL;
+	}
+
+	return -ENOMEM;
+}
+
+int iwl_tx_queue_init(struct iwl_priv *priv,
+		      struct iwl_tx_queue *txq, int slots_num, u32 txq_id)
+{
+	struct pci_dev *dev = priv->pci_dev;
+	int len;
+	int rc = 0;
+
+	/* alocate command space + one big command for scan since scan
+	 * command is very huge the system will not have two scan at the
+	 * same time */
+	len = sizeof(struct iwl_cmd) * slots_num;
+	if (txq_id == IWL_CMD_QUEUE_NUM)
+		len +=  IWL_MAX_SCAN_SIZE;
+	txq->cmd = pci_alloc_consistent(dev, len, &txq->dma_addr_cmd);
+	if (!txq->cmd)
+		return -ENOMEM;
+
+	rc = iwl_tx_queue_alloc(priv, txq, txq_id);
+	if (rc) {
+		pci_free_consistent(dev, len, txq->cmd, txq->dma_addr_cmd);
+
+		return -ENOMEM;
+	}
+	txq->need_update = 0;
+
+	/* TFD_QUEUE_SIZE_MAX must be power-of-two size, otherwise
+	 * iwl_queue_inc_wrap and iwl_queue_dec_wrap are broken. */
+	BUILD_BUG_ON(TFD_QUEUE_SIZE_MAX & (TFD_QUEUE_SIZE_MAX - 1));
+	iwl_queue_init(priv, &txq->q, TFD_QUEUE_SIZE_MAX, slots_num, txq_id);
+
+	iwl_hw_tx_queue_init(priv, txq);
+
+	return 0;
+}
+
+/**
+ * iwl_tx_queue_free - Deallocate DMA queue.
+ * @txq: Transmit queue to deallocate.
+ *
+ * Empty queue by removing and destroying all BD's.
+ * Free all buffers.  txq itself is not freed.
+ *
+ */
+void iwl_tx_queue_free(struct iwl_priv *priv, struct iwl_tx_queue *txq)
+{
+	struct iwl_queue *q = &txq->q;
+	struct pci_dev *dev = priv->pci_dev;
+	int len;
+
+	if (q->n_bd == 0)
+		return;
+
+	/* first, empty all BD's */
+	for (; q->first_empty != q->last_used;
+	     q->last_used = iwl_queue_inc_wrap(q->last_used, q->n_bd))
+		iwl_hw_txq_free_tfd(priv, txq);
+
+	len = sizeof(struct iwl_cmd) * q->n_window;
+	if (q->id == IWL_CMD_QUEUE_NUM)
+		len += IWL_MAX_SCAN_SIZE;
+
+	pci_free_consistent(dev, len, txq->cmd, txq->dma_addr_cmd);
+
+	/* free buffers belonging to queue itself */
+	if (txq->q.n_bd)
+		pci_free_consistent(dev, sizeof(struct iwl_tfd_frame) *
+				    txq->q.n_bd, txq->bd, txq->q.dma_addr);
+
+	if (txq->txb) {
+		kfree(txq->txb);
+		txq->txb = NULL;
+	}
+
+	/* 0 fill whole structure */
+	memset(txq, 0, sizeof(*txq));
+}
+
+const u8 BROADCAST_ADDR[ETH_ALEN] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };
+
+/*************** STATION TABLE MANAGEMENT ****
+ *
+ * NOTE:  This needs to be overhauled to better synchronize between
+ * how the iwl-4965.c is using iwl_hw_find_station vs. iwl-3945.c
+ *
+ * mac80211 should also be examined to determine if sta_info is duplicating
+ * the functionality provided here
+ */
+
+/**************************************************************/
+
+#if 0 /* temparary disable till we add real remove station */
+static u8 iwl_remove_station(struct iwl_priv *priv, const u8 *addr, int is_ap)
+{
+	int index = IWL_INVALID_STATION;
+	int i;
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->sta_lock, flags);
+
+	if (is_ap)
+		index = IWL_AP_ID;
+	else if (is_broadcast_ether_addr(addr))
+		index = priv->hw_setting.bcast_sta_id;
+	else
+		for (i = IWL_STA_ID; i < priv->hw_setting.max_stations; i++)
+			if (priv->stations[i].used &&
+			    !compare_ether_addr(priv->stations[i].sta.sta.addr,
+						addr)) {
+				index = i;
+				break;
+			}
+
+	if (unlikely(index == IWL_INVALID_STATION))
+		goto out;
+
+	if (priv->stations[index].used) {
+		priv->stations[index].used = 0;
+		priv->num_stations--;
+	}
+
+	BUG_ON(priv->num_stations < 0);
+
+out:
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+	return 0;
+}
+#endif
+
+static void iwl_clear_stations_table(struct iwl_priv *priv)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->sta_lock, flags);
+
+	priv->num_stations = 0;
+	memset(priv->stations, 0, sizeof(priv->stations));
+
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+}
+
+u8 iwl_add_station(struct iwl_priv *priv, const u8 *addr, int is_ap, u8 flags)
+{
+	int i;
+	int index = IWL_INVALID_STATION;
+	struct iwl_station_entry *station;
+	unsigned long flags_spin;
+	DECLARE_MAC_BUF(mac);
+
+	spin_lock_irqsave(&priv->sta_lock, flags_spin);
+	if (is_ap)
+		index = IWL_AP_ID;
+	else if (is_broadcast_ether_addr(addr))
+		index = priv->hw_setting.bcast_sta_id;
+	else
+		for (i = IWL_STA_ID; i < priv->hw_setting.max_stations; i++) {
+			if (!compare_ether_addr(priv->stations[i].sta.sta.addr,
+						addr)) {
+				index = i;
+				break;
+			}
+
+			if (!priv->stations[i].used &&
+			    index == IWL_INVALID_STATION)
+				index = i;
+		}
+
+
+	/* These twh conditions has the same outcome but keep them separate
+	  since they have different meaning */
+	if (unlikely(index == IWL_INVALID_STATION)) {
+		spin_unlock_irqrestore(&priv->sta_lock, flags_spin);
+		return index;
+	}
+
+	if (priv->stations[index].used &&
+	    !compare_ether_addr(priv->stations[index].sta.sta.addr, addr)) {
+		spin_unlock_irqrestore(&priv->sta_lock, flags_spin);
+		return index;
+	}
+
+
+	IWL_DEBUG_ASSOC("Add STA ID %d: %s\n", index, print_mac(mac, addr));
+	station = &priv->stations[index];
+	station->used = 1;
+	priv->num_stations++;
+
+	memset(&station->sta, 0, sizeof(struct iwl_addsta_cmd));
+	memcpy(station->sta.sta.addr, addr, ETH_ALEN);
+	station->sta.mode = 0;
+	station->sta.sta.sta_id = index;
+	station->sta.station_flags = 0;
+
+#ifdef CONFIG_IWLWIFI_HT
+	/* BCAST station and IBSS stations do not work in HT mode */
+	if (index != priv->hw_setting.bcast_sta_id &&
+	    priv->iw_mode != IEEE80211_IF_TYPE_IBSS)
+		iwl4965_set_ht_add_station(priv, index);
+#endif /*CONFIG_IWLWIFI_HT*/
+
+	spin_unlock_irqrestore(&priv->sta_lock, flags_spin);
+	iwl_send_add_station(priv, &station->sta, flags);
+	return index;
+
+}
+
+/*************** DRIVER STATUS FUNCTIONS   *****/
+
+static inline int iwl_is_ready(struct iwl_priv *priv)
+{
+	/* The adapter is 'ready' if READY and GEO_CONFIGURED bits are
+	 * set but EXIT_PENDING is not */
+	return test_bit(STATUS_READY, &priv->status) &&
+	       test_bit(STATUS_GEO_CONFIGURED, &priv->status) &&
+	       !test_bit(STATUS_EXIT_PENDING, &priv->status);
+}
+
+static inline int iwl_is_alive(struct iwl_priv *priv)
+{
+	return test_bit(STATUS_ALIVE, &priv->status);
+}
+
+static inline int iwl_is_init(struct iwl_priv *priv)
+{
+	return test_bit(STATUS_INIT, &priv->status);
+}
+
+static inline int iwl_is_rfkill(struct iwl_priv *priv)
+{
+	return test_bit(STATUS_RF_KILL_HW, &priv->status) ||
+	       test_bit(STATUS_RF_KILL_SW, &priv->status);
+}
+
+static inline int iwl_is_ready_rf(struct iwl_priv *priv)
+{
+
+	if (iwl_is_rfkill(priv))
+		return 0;
+
+	return iwl_is_ready(priv);
+}
+
+/*************** HOST COMMAND QUEUE FUNCTIONS   *****/
+
+#define IWL_CMD(x) case x : return #x
+
+static const char *get_cmd_string(u8 cmd)
+{
+	switch (cmd) {
+		IWL_CMD(REPLY_ALIVE);
+		IWL_CMD(REPLY_ERROR);
+		IWL_CMD(REPLY_RXON);
+		IWL_CMD(REPLY_RXON_ASSOC);
+		IWL_CMD(REPLY_QOS_PARAM);
+		IWL_CMD(REPLY_RXON_TIMING);
+		IWL_CMD(REPLY_ADD_STA);
+		IWL_CMD(REPLY_REMOVE_STA);
+		IWL_CMD(REPLY_REMOVE_ALL_STA);
+		IWL_CMD(REPLY_TX);
+		IWL_CMD(REPLY_RATE_SCALE);
+		IWL_CMD(REPLY_LEDS_CMD);
+		IWL_CMD(REPLY_TX_LINK_QUALITY_CMD);
+		IWL_CMD(RADAR_NOTIFICATION);
+		IWL_CMD(REPLY_QUIET_CMD);
+		IWL_CMD(REPLY_CHANNEL_SWITCH);
+		IWL_CMD(CHANNEL_SWITCH_NOTIFICATION);
+		IWL_CMD(REPLY_SPECTRUM_MEASUREMENT_CMD);
+		IWL_CMD(SPECTRUM_MEASURE_NOTIFICATION);
+		IWL_CMD(POWER_TABLE_CMD);
+		IWL_CMD(PM_SLEEP_NOTIFICATION);
+		IWL_CMD(PM_DEBUG_STATISTIC_NOTIFIC);
+		IWL_CMD(REPLY_SCAN_CMD);
+		IWL_CMD(REPLY_SCAN_ABORT_CMD);
+		IWL_CMD(SCAN_START_NOTIFICATION);
+		IWL_CMD(SCAN_RESULTS_NOTIFICATION);
+		IWL_CMD(SCAN_COMPLETE_NOTIFICATION);
+		IWL_CMD(BEACON_NOTIFICATION);
+		IWL_CMD(REPLY_TX_BEACON);
+		IWL_CMD(WHO_IS_AWAKE_NOTIFICATION);
+		IWL_CMD(QUIET_NOTIFICATION);
+		IWL_CMD(REPLY_TX_PWR_TABLE_CMD);
+		IWL_CMD(MEASURE_ABORT_NOTIFICATION);
+		IWL_CMD(REPLY_BT_CONFIG);
+		IWL_CMD(REPLY_STATISTICS_CMD);
+		IWL_CMD(STATISTICS_NOTIFICATION);
+		IWL_CMD(REPLY_CARD_STATE_CMD);
+		IWL_CMD(CARD_STATE_NOTIFICATION);
+		IWL_CMD(MISSED_BEACONS_NOTIFICATION);
+		IWL_CMD(REPLY_CT_KILL_CONFIG_CMD);
+		IWL_CMD(SENSITIVITY_CMD);
+		IWL_CMD(REPLY_PHY_CALIBRATION_CMD);
+		IWL_CMD(REPLY_RX_PHY_CMD);
+		IWL_CMD(REPLY_RX_MPDU_CMD);
+		IWL_CMD(REPLY_4965_RX);
+		IWL_CMD(REPLY_COMPRESSED_BA);
+	default:
+		return "UNKNOWN";
+
+	}
+}
+
+#define HOST_COMPLETE_TIMEOUT (HZ / 2)
+
+/**
+ * iwl_enqueue_hcmd - enqueue a uCode command
+ * @priv: device private data point
+ * @cmd: a point to the ucode command structure
+ *
+ * The function returns < 0 values to indicate the operation is
+ * failed. On success, it turns the index (> 0) of command in the
+ * command queue.
+ */
+static int iwl_enqueue_hcmd(struct iwl_priv *priv, struct iwl_host_cmd *cmd)
+{
+	struct iwl_tx_queue *txq = &priv->txq[IWL_CMD_QUEUE_NUM];
+	struct iwl_queue *q = &txq->q;
+	struct iwl_tfd_frame *tfd;
+	u32 *control_flags;
+	struct iwl_cmd *out_cmd;
+	u32 idx;
+	u16 fix_size = (u16)(cmd->len + sizeof(out_cmd->hdr));
+	dma_addr_t phys_addr;
+	int ret;
+	unsigned long flags;
+
+	/* If any of the command structures end up being larger than
+	 * the TFD_MAX_PAYLOAD_SIZE, and it sent as a 'small' command then
+	 * we will need to increase the size of the TFD entries */
+	BUG_ON((fix_size > TFD_MAX_PAYLOAD_SIZE) &&
+	       !(cmd->meta.flags & CMD_SIZE_HUGE));
+
+	if (iwl_queue_space(q) < ((cmd->meta.flags & CMD_ASYNC) ? 2 : 1)) {
+		IWL_ERROR("No space for Tx\n");
+		return -ENOSPC;
+	}
+
+	spin_lock_irqsave(&priv->hcmd_lock, flags);
+
+	tfd = &txq->bd[q->first_empty];
+	memset(tfd, 0, sizeof(*tfd));
+
+	control_flags = (u32 *) tfd;
+
+	idx = get_cmd_index(q, q->first_empty, cmd->meta.flags & CMD_SIZE_HUGE);
+	out_cmd = &txq->cmd[idx];
+
+	out_cmd->hdr.cmd = cmd->id;
+	memcpy(&out_cmd->meta, &cmd->meta, sizeof(cmd->meta));
+	memcpy(&out_cmd->cmd.payload, cmd->data, cmd->len);
+
+	/* At this point, the out_cmd now has all of the incoming cmd
+	 * information */
+
+	out_cmd->hdr.flags = 0;
+	out_cmd->hdr.sequence = cpu_to_le16(QUEUE_TO_SEQ(IWL_CMD_QUEUE_NUM) |
+			INDEX_TO_SEQ(q->first_empty));
+	if (out_cmd->meta.flags & CMD_SIZE_HUGE)
+		out_cmd->hdr.sequence |= cpu_to_le16(SEQ_HUGE_FRAME);
+
+	phys_addr = txq->dma_addr_cmd + sizeof(txq->cmd[0]) * idx +
+			offsetof(struct iwl_cmd, hdr);
+	iwl_hw_txq_attach_buf_to_tfd(priv, tfd, phys_addr, fix_size);
+
+	IWL_DEBUG_HC("Sending command %s (#%x), seq: 0x%04X, "
+		     "%d bytes at %d[%d]:%d\n",
+		     get_cmd_string(out_cmd->hdr.cmd),
+		     out_cmd->hdr.cmd, le16_to_cpu(out_cmd->hdr.sequence),
+		     fix_size, q->first_empty, idx, IWL_CMD_QUEUE_NUM);
+
+	txq->need_update = 1;
+	ret = iwl4965_tx_queue_update_wr_ptr(priv, txq, 0);
+	q->first_empty = iwl_queue_inc_wrap(q->first_empty, q->n_bd);
+	iwl_tx_queue_update_write_ptr(priv, txq);
+
+	spin_unlock_irqrestore(&priv->hcmd_lock, flags);
+	return ret ? ret : idx;
+}
+
+int iwl_send_cmd_async(struct iwl_priv *priv, struct iwl_host_cmd *cmd)
+{
+	int ret;
+
+	BUG_ON(!(cmd->meta.flags & CMD_ASYNC));
+
+	/* An asynchronous command can not expect an SKB to be set. */
+	BUG_ON(cmd->meta.flags & CMD_WANT_SKB);
+
+	/* An asynchronous command MUST have a callback. */
+	BUG_ON(!cmd->meta.u.callback);
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+		return -EBUSY;
+
+	ret = iwl_enqueue_hcmd(priv, cmd);
+	if (ret < 0) {
+		IWL_ERROR("Error sending %s: iwl_enqueue_hcmd failed: %d\n",
+			  get_cmd_string(cmd->id), ret);
+		return ret;
+	}
+	return 0;
+}
+
+int iwl_send_cmd_sync(struct iwl_priv *priv, struct iwl_host_cmd *cmd)
+{
+	int cmd_idx;
+	int ret;
+	static atomic_t entry = ATOMIC_INIT(0); /* reentrance protection */
+
+	BUG_ON(cmd->meta.flags & CMD_ASYNC);
+
+	 /* A synchronous command can not have a callback set. */
+	BUG_ON(cmd->meta.u.callback != NULL);
+
+	if (atomic_xchg(&entry, 1)) {
+		IWL_ERROR("Error sending %s: Already sending a host command\n",
+			  get_cmd_string(cmd->id));
+		return -EBUSY;
+	}
+
+	set_bit(STATUS_HCMD_ACTIVE, &priv->status);
+
+	if (cmd->meta.flags & CMD_WANT_SKB)
+		cmd->meta.source = &cmd->meta;
+
+	cmd_idx = iwl_enqueue_hcmd(priv, cmd);
+	if (cmd_idx < 0) {
+		ret = cmd_idx;
+		IWL_ERROR("Error sending %s: iwl_enqueue_hcmd failed: %d\n",
+			  get_cmd_string(cmd->id), ret);
+		goto out;
+	}
+
+	ret = wait_event_interruptible_timeout(priv->wait_command_queue,
+			!test_bit(STATUS_HCMD_ACTIVE, &priv->status),
+			HOST_COMPLETE_TIMEOUT);
+	if (!ret) {
+		if (test_bit(STATUS_HCMD_ACTIVE, &priv->status)) {
+			IWL_ERROR("Error sending %s: time out after %dms.\n",
+				  get_cmd_string(cmd->id),
+				  jiffies_to_msecs(HOST_COMPLETE_TIMEOUT));
+
+			clear_bit(STATUS_HCMD_ACTIVE, &priv->status);
+			ret = -ETIMEDOUT;
+			goto cancel;
+		}
+	}
+
+	if (test_bit(STATUS_RF_KILL_HW, &priv->status)) {
+		IWL_DEBUG_INFO("Command %s aborted: RF KILL Switch\n",
+			       get_cmd_string(cmd->id));
+		ret = -ECANCELED;
+		goto fail;
+	}
+	if (test_bit(STATUS_FW_ERROR, &priv->status)) {
+		IWL_DEBUG_INFO("Command %s failed: FW Error\n",
+			       get_cmd_string(cmd->id));
+		ret = -EIO;
+		goto fail;
+	}
+	if ((cmd->meta.flags & CMD_WANT_SKB) && !cmd->meta.u.skb) {
+		IWL_ERROR("Error: Response NULL in '%s'\n",
+			  get_cmd_string(cmd->id));
+		ret = -EIO;
+		goto out;
+	}
+
+	ret = 0;
+	goto out;
+
+cancel:
+	if (cmd->meta.flags & CMD_WANT_SKB) {
+		struct iwl_cmd *qcmd;
+
+		/* Cancel the CMD_WANT_SKB flag for the cmd in the
+		 * TX cmd queue. Otherwise in case the cmd comes
+		 * in later, it will possibly set an invalid
+		 * address (cmd->meta.source). */
+		qcmd = &priv->txq[IWL_CMD_QUEUE_NUM].cmd[cmd_idx];
+		qcmd->meta.flags &= ~CMD_WANT_SKB;
+	}
+fail:
+	if (cmd->meta.u.skb) {
+		dev_kfree_skb_any(cmd->meta.u.skb);
+		cmd->meta.u.skb = NULL;
+	}
+out:
+	atomic_set(&entry, 0);
+	return ret;
+}
+
+int iwl_send_cmd(struct iwl_priv *priv, struct iwl_host_cmd *cmd)
+{
+	/* A command can not be asynchronous AND expect an SKB to be set. */
+	BUG_ON((cmd->meta.flags & CMD_ASYNC) &&
+	       (cmd->meta.flags & CMD_WANT_SKB));
+
+	if (cmd->meta.flags & CMD_ASYNC)
+		return iwl_send_cmd_async(priv, cmd);
+
+	return iwl_send_cmd_sync(priv, cmd);
+}
+
+int iwl_send_cmd_pdu(struct iwl_priv *priv, u8 id, u16 len, const void *data)
+{
+	struct iwl_host_cmd cmd = {
+		.id = id,
+		.len = len,
+		.data = data,
+	};
+
+	return iwl_send_cmd_sync(priv, &cmd);
+}
+
+static int __must_check iwl_send_cmd_u32(struct iwl_priv *priv, u8 id, u32 val)
+{
+	struct iwl_host_cmd cmd = {
+		.id = id,
+		.len = sizeof(val),
+		.data = &val,
+	};
+
+	return iwl_send_cmd_sync(priv, &cmd);
+}
+
+int iwl_send_statistics_request(struct iwl_priv *priv)
+{
+	return iwl_send_cmd_u32(priv, REPLY_STATISTICS_CMD, 0);
+}
+
+/**
+ * iwl_rxon_add_station - add station into station table.
+ *
+ * there is only one AP station with id= IWL_AP_ID
+ * NOTE: mutex must be held before calling the this fnction
+*/
+static int iwl_rxon_add_station(struct iwl_priv *priv,
+				const u8 *addr, int is_ap)
+{
+	u8 sta_id;
+
+	sta_id = iwl_add_station(priv, addr, is_ap, 0);
+	iwl4965_add_station(priv, addr, is_ap);
+
+	return sta_id;
+}
+
+/**
+ * iwl_set_rxon_channel - Set the phymode and channel values in staging RXON
+ * @phymode: MODE_IEEE80211A sets to 5.2GHz; all else set to 2.4GHz
+ * @channel: Any channel valid for the requested phymode
+
+ * In addition to setting the staging RXON, priv->phymode is also set.
+ *
+ * NOTE:  Does not commit to the hardware; it sets appropriate bit fields
+ * in the staging RXON flag structure based on the phymode
+ */
+static int iwl_set_rxon_channel(struct iwl_priv *priv, u8 phymode, u16 channel)
+{
+	if (!iwl_get_channel_info(priv, phymode, channel)) {
+		IWL_DEBUG_INFO("Could not set channel to %d [%d]\n",
+			       channel, phymode);
+		return -EINVAL;
+	}
+
+	if ((le16_to_cpu(priv->staging_rxon.channel) == channel) &&
+	    (priv->phymode == phymode))
+		return 0;
+
+	priv->staging_rxon.channel = cpu_to_le16(channel);
+	if (phymode == MODE_IEEE80211A)
+		priv->staging_rxon.flags &= ~RXON_FLG_BAND_24G_MSK;
+	else
+		priv->staging_rxon.flags |= RXON_FLG_BAND_24G_MSK;
+
+	priv->phymode = phymode;
+
+	IWL_DEBUG_INFO("Staging channel set to %d [%d]\n", channel, phymode);
+
+	return 0;
+}
+
+/**
+ * iwl_check_rxon_cmd - validate RXON structure is valid
+ *
+ * NOTE:  This is really only useful during development and can eventually
+ * be #ifdef'd out once the driver is stable and folks aren't actively
+ * making changes
+ */
+static int iwl_check_rxon_cmd(struct iwl_rxon_cmd *rxon)
+{
+	int error = 0;
+	int counter = 1;
+
+	if (rxon->flags & RXON_FLG_BAND_24G_MSK) {
+		error |= le32_to_cpu(rxon->flags &
+				(RXON_FLG_TGJ_NARROW_BAND_MSK |
+				 RXON_FLG_RADAR_DETECT_MSK));
+		if (error)
+			IWL_WARNING("check 24G fields %d | %d\n",
+				    counter++, error);
+	} else {
+		error |= (rxon->flags & RXON_FLG_SHORT_SLOT_MSK) ?
+				0 : le32_to_cpu(RXON_FLG_SHORT_SLOT_MSK);
+		if (error)
+			IWL_WARNING("check 52 fields %d | %d\n",
+				    counter++, error);
+		error |= le32_to_cpu(rxon->flags & RXON_FLG_CCK_MSK);
+		if (error)
+			IWL_WARNING("check 52 CCK %d | %d\n",
+				    counter++, error);
+	}
+	error |= (rxon->node_addr[0] | rxon->bssid_addr[0]) & 0x1;
+	if (error)
+		IWL_WARNING("check mac addr %d | %d\n", counter++, error);
+
+	/* make sure basic rates 6Mbps and 1Mbps are supported */
+	error |= (((rxon->ofdm_basic_rates & IWL_RATE_6M_MASK) == 0) &&
+		  ((rxon->cck_basic_rates & IWL_RATE_1M_MASK) == 0));
+	if (error)
+		IWL_WARNING("check basic rate %d | %d\n", counter++, error);
+
+	error |= (le16_to_cpu(rxon->assoc_id) > 2007);
+	if (error)
+		IWL_WARNING("check assoc id %d | %d\n", counter++, error);
+
+	error |= ((rxon->flags & (RXON_FLG_CCK_MSK | RXON_FLG_SHORT_SLOT_MSK))
+			== (RXON_FLG_CCK_MSK | RXON_FLG_SHORT_SLOT_MSK));
+	if (error)
+		IWL_WARNING("check CCK and short slot %d | %d\n",
+			    counter++, error);
+
+	error |= ((rxon->flags & (RXON_FLG_CCK_MSK | RXON_FLG_AUTO_DETECT_MSK))
+			== (RXON_FLG_CCK_MSK | RXON_FLG_AUTO_DETECT_MSK));
+	if (error)
+		IWL_WARNING("check CCK & auto detect %d | %d\n",
+			    counter++, error);
+
+	error |= ((rxon->flags & (RXON_FLG_AUTO_DETECT_MSK |
+			RXON_FLG_TGG_PROTECT_MSK)) == RXON_FLG_TGG_PROTECT_MSK);
+	if (error)
+		IWL_WARNING("check TGG and auto detect %d | %d\n",
+			    counter++, error);
+
+	if (error)
+		IWL_WARNING("Tuning to channel %d\n",
+			    le16_to_cpu(rxon->channel));
+
+	if (error) {
+		IWL_ERROR("Not a valid iwl_rxon_assoc_cmd field values\n");
+		return -1;
+	}
+	return 0;
+}
+
+/**
+ * iwl_full_rxon_required - determine if RXON_ASSOC can be used in RXON commit
+ * @priv: staging_rxon is comapred to active_rxon
+ *
+ * If the RXON structure is changing sufficient to require a new
+ * tune or to clear and reset the RXON_FILTER_ASSOC_MSK then return 1
+ * to indicate a new tune is required.
+ */
+static int iwl_full_rxon_required(struct iwl_priv *priv)
+{
+
+	/* These items are only settable from the full RXON command */
+	if (!(priv->active_rxon.filter_flags & RXON_FILTER_ASSOC_MSK) ||
+	    compare_ether_addr(priv->staging_rxon.bssid_addr,
+			       priv->active_rxon.bssid_addr) ||
+	    compare_ether_addr(priv->staging_rxon.node_addr,
+			       priv->active_rxon.node_addr) ||
+	    compare_ether_addr(priv->staging_rxon.wlap_bssid_addr,
+			       priv->active_rxon.wlap_bssid_addr) ||
+	    (priv->staging_rxon.dev_type != priv->active_rxon.dev_type) ||
+	    (priv->staging_rxon.channel != priv->active_rxon.channel) ||
+	    (priv->staging_rxon.air_propagation !=
+	     priv->active_rxon.air_propagation) ||
+	    (priv->staging_rxon.ofdm_ht_single_stream_basic_rates !=
+	     priv->active_rxon.ofdm_ht_single_stream_basic_rates) ||
+	    (priv->staging_rxon.ofdm_ht_dual_stream_basic_rates !=
+	     priv->active_rxon.ofdm_ht_dual_stream_basic_rates) ||
+	    (priv->staging_rxon.rx_chain != priv->active_rxon.rx_chain) ||
+	    (priv->staging_rxon.assoc_id != priv->active_rxon.assoc_id))
+		return 1;
+
+	/* flags, filter_flags, ofdm_basic_rates, and cck_basic_rates can
+	 * be updated with the RXON_ASSOC command -- however only some
+	 * flag transitions are allowed using RXON_ASSOC */
+
+	/* Check if we are not switching bands */
+	if ((priv->staging_rxon.flags & RXON_FLG_BAND_24G_MSK) !=
+	    (priv->active_rxon.flags & RXON_FLG_BAND_24G_MSK))
+		return 1;
+
+	/* Check if we are switching association toggle */
+	if ((priv->staging_rxon.filter_flags & RXON_FILTER_ASSOC_MSK) !=
+		(priv->active_rxon.filter_flags & RXON_FILTER_ASSOC_MSK))
+		return 1;
+
+	return 0;
+}
+
+static int iwl_send_rxon_assoc(struct iwl_priv *priv)
+{
+	int rc = 0;
+	struct iwl_rx_packet *res = NULL;
+	struct iwl_rxon_assoc_cmd rxon_assoc;
+	struct iwl_host_cmd cmd = {
+		.id = REPLY_RXON_ASSOC,
+		.len = sizeof(rxon_assoc),
+		.meta.flags = CMD_WANT_SKB,
+		.data = &rxon_assoc,
+	};
+	const struct iwl_rxon_cmd *rxon1 = &priv->staging_rxon;
+	const struct iwl_rxon_cmd *rxon2 = &priv->active_rxon;
+
+	if ((rxon1->flags == rxon2->flags) &&
+	    (rxon1->filter_flags == rxon2->filter_flags) &&
+	    (rxon1->cck_basic_rates == rxon2->cck_basic_rates) &&
+	    (rxon1->ofdm_ht_single_stream_basic_rates ==
+	     rxon2->ofdm_ht_single_stream_basic_rates) &&
+	    (rxon1->ofdm_ht_dual_stream_basic_rates ==
+	     rxon2->ofdm_ht_dual_stream_basic_rates) &&
+	    (rxon1->rx_chain == rxon2->rx_chain) &&
+	    (rxon1->ofdm_basic_rates == rxon2->ofdm_basic_rates)) {
+		IWL_DEBUG_INFO("Using current RXON_ASSOC.  Not resending.\n");
+		return 0;
+	}
+
+	rxon_assoc.flags = priv->staging_rxon.flags;
+	rxon_assoc.filter_flags = priv->staging_rxon.filter_flags;
+	rxon_assoc.ofdm_basic_rates = priv->staging_rxon.ofdm_basic_rates;
+	rxon_assoc.cck_basic_rates = priv->staging_rxon.cck_basic_rates;
+	rxon_assoc.reserved = 0;
+	rxon_assoc.ofdm_ht_single_stream_basic_rates =
+	    priv->staging_rxon.ofdm_ht_single_stream_basic_rates;
+	rxon_assoc.ofdm_ht_dual_stream_basic_rates =
+	    priv->staging_rxon.ofdm_ht_dual_stream_basic_rates;
+	rxon_assoc.rx_chain_select_flags = priv->staging_rxon.rx_chain;
+
+	rc = iwl_send_cmd_sync(priv, &cmd);
+	if (rc)
+		return rc;
+
+	res = (struct iwl_rx_packet *)cmd.meta.u.skb->data;
+	if (res->hdr.flags & IWL_CMD_FAILED_MSK) {
+		IWL_ERROR("Bad return from REPLY_RXON_ASSOC command\n");
+		rc = -EIO;
+	}
+
+	priv->alloc_rxb_skb--;
+	dev_kfree_skb_any(cmd.meta.u.skb);
+
+	return rc;
+}
+
+/**
+ * iwl_commit_rxon - commit staging_rxon to hardware
+ *
+ * The RXON command in staging_rxon is commited to the hardware and
+ * the active_rxon structure is updated with the new data.  This
+ * function correctly transitions out of the RXON_ASSOC_MSK state if
+ * a HW tune is required based on the RXON structure changes.
+ */
+static int iwl_commit_rxon(struct iwl_priv *priv)
+{
+	/* cast away the const for active_rxon in this function */
+	struct iwl_rxon_cmd *active_rxon = (void *)&priv->active_rxon;
+	DECLARE_MAC_BUF(mac);
+	int rc = 0;
+
+	if (!iwl_is_alive(priv))
+		return -1;
+
+	/* always get timestamp with Rx frame */
+	priv->staging_rxon.flags |= RXON_FLG_TSF2HOST_MSK;
+
+	rc = iwl_check_rxon_cmd(&priv->staging_rxon);
+	if (rc) {
+		IWL_ERROR("Invalid RXON configuration.  Not committing.\n");
+		return -EINVAL;
+	}
+
+	/* If we don't need to send a full RXON, we can use
+	 * iwl_rxon_assoc_cmd which is used to reconfigure filter
+	 * and other flags for the current radio configuration. */
+	if (!iwl_full_rxon_required(priv)) {
+		rc = iwl_send_rxon_assoc(priv);
+		if (rc) {
+			IWL_ERROR("Error setting RXON_ASSOC "
+				  "configuration (%d).\n", rc);
+			return rc;
+		}
+
+		memcpy(active_rxon, &priv->staging_rxon, sizeof(*active_rxon));
+
+		return 0;
+	}
+
+	/* station table will be cleared */
+	priv->assoc_station_added = 0;
+
+#ifdef CONFIG_IWLWIFI_SENSITIVITY
+	priv->sensitivity_data.state = IWL_SENS_CALIB_NEED_REINIT;
+	if (!priv->error_recovering)
+		priv->start_calib = 0;
+
+	iwl4965_init_sensitivity(priv, CMD_ASYNC, 1);
+#endif /* CONFIG_IWLWIFI_SENSITIVITY */
+
+	/* If we are currently associated and the new config requires
+	 * an RXON_ASSOC and the new config wants the associated mask enabled,
+	 * we must clear the associated from the active configuration
+	 * before we apply the new config */
+	if (iwl_is_associated(priv) &&
+	    (priv->staging_rxon.filter_flags & RXON_FILTER_ASSOC_MSK)) {
+		IWL_DEBUG_INFO("Toggling associated bit on current RXON\n");
+		active_rxon->filter_flags &= ~RXON_FILTER_ASSOC_MSK;
+
+		rc = iwl_send_cmd_pdu(priv, REPLY_RXON,
+				      sizeof(struct iwl_rxon_cmd),
+				      &priv->active_rxon);
+
+		/* If the mask clearing failed then we set
+		 * active_rxon back to what it was previously */
+		if (rc) {
+			active_rxon->filter_flags |= RXON_FILTER_ASSOC_MSK;
+			IWL_ERROR("Error clearing ASSOC_MSK on current "
+				  "configuration (%d).\n", rc);
+			return rc;
+		}
+	}
+
+	IWL_DEBUG_INFO("Sending RXON\n"
+		       "* with%s RXON_FILTER_ASSOC_MSK\n"
+		       "* channel = %d\n"
+		       "* bssid = %s\n",
+		       ((priv->staging_rxon.filter_flags &
+			 RXON_FILTER_ASSOC_MSK) ? "" : "out"),
+		       le16_to_cpu(priv->staging_rxon.channel),
+		       print_mac(mac, priv->staging_rxon.bssid_addr));
+
+	/* Apply the new configuration */
+	rc = iwl_send_cmd_pdu(priv, REPLY_RXON,
+			      sizeof(struct iwl_rxon_cmd), &priv->staging_rxon);
+	if (rc) {
+		IWL_ERROR("Error setting new configuration (%d).\n", rc);
+		return rc;
+	}
+
+	iwl_clear_stations_table(priv);
+
+#ifdef CONFIG_IWLWIFI_SENSITIVITY
+	if (!priv->error_recovering)
+		priv->start_calib = 0;
+
+	priv->sensitivity_data.state = IWL_SENS_CALIB_NEED_REINIT;
+	iwl4965_init_sensitivity(priv, CMD_ASYNC, 1);
+#endif /* CONFIG_IWLWIFI_SENSITIVITY */
+
+	memcpy(active_rxon, &priv->staging_rxon, sizeof(*active_rxon));
+
+	/* If we issue a new RXON command which required a tune then we must
+	 * send a new TXPOWER command or we won't be able to Tx any frames */
+	rc = iwl_hw_reg_send_txpower(priv);
+	if (rc) {
+		IWL_ERROR("Error setting Tx power (%d).\n", rc);
+		return rc;
+	}
+
+	/* Add the broadcast address so we can send broadcast frames */
+	if (iwl_rxon_add_station(priv, BROADCAST_ADDR, 0) ==
+	    IWL_INVALID_STATION) {
+		IWL_ERROR("Error adding BROADCAST address for transmit.\n");
+		return -EIO;
+	}
+
+	/* If we have set the ASSOC_MSK and we are in BSS mode then
+	 * add the IWL_AP_ID to the station rate table */
+	if (iwl_is_associated(priv) &&
+	    (priv->iw_mode == IEEE80211_IF_TYPE_STA)) {
+		if (iwl_rxon_add_station(priv, priv->active_rxon.bssid_addr, 1)
+		    == IWL_INVALID_STATION) {
+			IWL_ERROR("Error adding AP address for transmit.\n");
+			return -EIO;
+		}
+		priv->assoc_station_added = 1;
+	}
+
+	return 0;
+}
+
+static int iwl_send_bt_config(struct iwl_priv *priv)
+{
+	struct iwl_bt_cmd bt_cmd = {
+		.flags = 3,
+		.lead_time = 0xAA,
+		.max_kill = 1,
+		.kill_ack_mask = 0,
+		.kill_cts_mask = 0,
+	};
+
+	return iwl_send_cmd_pdu(priv, REPLY_BT_CONFIG,
+				sizeof(struct iwl_bt_cmd), &bt_cmd);
+}
+
+static int iwl_send_scan_abort(struct iwl_priv *priv)
+{
+	int rc = 0;
+	struct iwl_rx_packet *res;
+	struct iwl_host_cmd cmd = {
+		.id = REPLY_SCAN_ABORT_CMD,
+		.meta.flags = CMD_WANT_SKB,
+	};
+
+	/* If there isn't a scan actively going on in the hardware
+	 * then we are in between scan bands and not actually
+	 * actively scanning, so don't send the abort command */
+	if (!test_bit(STATUS_SCAN_HW, &priv->status)) {
+		clear_bit(STATUS_SCAN_ABORTING, &priv->status);
+		return 0;
+	}
+
+	rc = iwl_send_cmd_sync(priv, &cmd);
+	if (rc) {
+		clear_bit(STATUS_SCAN_ABORTING, &priv->status);
+		return rc;
+	}
+
+	res = (struct iwl_rx_packet *)cmd.meta.u.skb->data;
+	if (res->u.status != CAN_ABORT_STATUS) {
+		/* The scan abort will return 1 for success or
+		 * 2 for "failure".  A failure condition can be
+		 * due to simply not being in an active scan which
+		 * can occur if we send the scan abort before we
+		 * the microcode has notified us that a scan is
+		 * completed. */
+		IWL_DEBUG_INFO("SCAN_ABORT returned %d.\n", res->u.status);
+		clear_bit(STATUS_SCAN_ABORTING, &priv->status);
+		clear_bit(STATUS_SCAN_HW, &priv->status);
+	}
+
+	dev_kfree_skb_any(cmd.meta.u.skb);
+
+	return rc;
+}
+
+static int iwl_card_state_sync_callback(struct iwl_priv *priv,
+					struct iwl_cmd *cmd,
+					struct sk_buff *skb)
+{
+	return 1;
+}
+
+/*
+ * CARD_STATE_CMD
+ *
+ * Use: Sets the internal card state to enable, disable, or halt
+ *
+ * When in the 'enable' state the card operates as normal.
+ * When in the 'disable' state, the card enters into a low power mode.
+ * When in the 'halt' state, the card is shut down and must be fully
+ * restarted to come back on.
+ */
+static int iwl_send_card_state(struct iwl_priv *priv, u32 flags, u8 meta_flag)
+{
+	struct iwl_host_cmd cmd = {
+		.id = REPLY_CARD_STATE_CMD,
+		.len = sizeof(u32),
+		.data = &flags,
+		.meta.flags = meta_flag,
+	};
+
+	if (meta_flag & CMD_ASYNC)
+		cmd.meta.u.callback = iwl_card_state_sync_callback;
+
+	return iwl_send_cmd(priv, &cmd);
+}
+
+static int iwl_add_sta_sync_callback(struct iwl_priv *priv,
+				     struct iwl_cmd *cmd, struct sk_buff *skb)
+{
+	struct iwl_rx_packet *res = NULL;
+
+	if (!skb) {
+		IWL_ERROR("Error: Response NULL in REPLY_ADD_STA.\n");
+		return 1;
+	}
+
+	res = (struct iwl_rx_packet *)skb->data;
+	if (res->hdr.flags & IWL_CMD_FAILED_MSK) {
+		IWL_ERROR("Bad return from REPLY_ADD_STA (0x%08X)\n",
+			  res->hdr.flags);
+		return 1;
+	}
+
+	switch (res->u.add_sta.status) {
+	case ADD_STA_SUCCESS_MSK:
+		break;
+	default:
+		break;
+	}
+
+	/* We didn't cache the SKB; let the caller free it */
+	return 1;
+}
+
+int iwl_send_add_station(struct iwl_priv *priv,
+			 struct iwl_addsta_cmd *sta, u8 flags)
+{
+	struct iwl_rx_packet *res = NULL;
+	int rc = 0;
+	struct iwl_host_cmd cmd = {
+		.id = REPLY_ADD_STA,
+		.len = sizeof(struct iwl_addsta_cmd),
+		.meta.flags = flags,
+		.data = sta,
+	};
+
+	if (flags & CMD_ASYNC)
+		cmd.meta.u.callback = iwl_add_sta_sync_callback;
+	else
+		cmd.meta.flags |= CMD_WANT_SKB;
+
+	rc = iwl_send_cmd(priv, &cmd);
+
+	if (rc || (flags & CMD_ASYNC))
+		return rc;
+
+	res = (struct iwl_rx_packet *)cmd.meta.u.skb->data;
+	if (res->hdr.flags & IWL_CMD_FAILED_MSK) {
+		IWL_ERROR("Bad return from REPLY_ADD_STA (0x%08X)\n",
+			  res->hdr.flags);
+		rc = -EIO;
+	}
+
+	if (rc == 0) {
+		switch (res->u.add_sta.status) {
+		case ADD_STA_SUCCESS_MSK:
+			IWL_DEBUG_INFO("REPLY_ADD_STA PASSED\n");
+			break;
+		default:
+			rc = -EIO;
+			IWL_WARNING("REPLY_ADD_STA failed\n");
+			break;
+		}
+	}
+
+	priv->alloc_rxb_skb--;
+	dev_kfree_skb_any(cmd.meta.u.skb);
+
+	return rc;
+}
+
+static int iwl_update_sta_key_info(struct iwl_priv *priv,
+				   struct ieee80211_key_conf *keyconf,
+				   u8 sta_id)
+{
+	unsigned long flags;
+	__le16 key_flags = 0;
+
+	switch (keyconf->alg) {
+	case ALG_CCMP:
+		key_flags |= STA_KEY_FLG_CCMP;
+		key_flags |= cpu_to_le16(
+				keyconf->keyidx << STA_KEY_FLG_KEYID_POS);
+		key_flags &= ~STA_KEY_FLG_INVALID;
+		break;
+	case ALG_TKIP:
+	case ALG_WEP:
+		return -EINVAL;
+	default:
+		return -EINVAL;
+	}
+	spin_lock_irqsave(&priv->sta_lock, flags);
+	priv->stations[sta_id].keyinfo.alg = keyconf->alg;
+	priv->stations[sta_id].keyinfo.keylen = keyconf->keylen;
+	memcpy(priv->stations[sta_id].keyinfo.key, keyconf->key,
+	       keyconf->keylen);
+
+	memcpy(priv->stations[sta_id].sta.key.key, keyconf->key,
+	       keyconf->keylen);
+	priv->stations[sta_id].sta.key.key_flags = key_flags;
+	priv->stations[sta_id].sta.sta.modify_mask = STA_MODIFY_KEY_MASK;
+	priv->stations[sta_id].sta.mode = STA_CONTROL_MODIFY_MSK;
+
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+
+	IWL_DEBUG_INFO("hwcrypto: modify ucode station key info\n");
+	iwl_send_add_station(priv, &priv->stations[sta_id].sta, 0);
+	return 0;
+}
+
+static int iwl_clear_sta_key_info(struct iwl_priv *priv, u8 sta_id)
+{
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->sta_lock, flags);
+	memset(&priv->stations[sta_id].keyinfo, 0, sizeof(struct iwl_hw_key));
+	memset(&priv->stations[sta_id].sta.key, 0, sizeof(struct iwl_keyinfo));
+	priv->stations[sta_id].sta.key.key_flags = STA_KEY_FLG_NO_ENC;
+	priv->stations[sta_id].sta.sta.modify_mask = STA_MODIFY_KEY_MASK;
+	priv->stations[sta_id].sta.mode = STA_CONTROL_MODIFY_MSK;
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+
+	IWL_DEBUG_INFO("hwcrypto: clear ucode station key info\n");
+	iwl_send_add_station(priv, &priv->stations[sta_id].sta, 0);
+	return 0;
+}
+
+static void iwl_clear_free_frames(struct iwl_priv *priv)
+{
+	struct list_head *element;
+
+	IWL_DEBUG_INFO("%d frames on pre-allocated heap on clear.\n",
+		       priv->frames_count);
+
+	while (!list_empty(&priv->free_frames)) {
+		element = priv->free_frames.next;
+		list_del(element);
+		kfree(list_entry(element, struct iwl_frame, list));
+		priv->frames_count--;
+	}
+
+	if (priv->frames_count) {
+		IWL_WARNING("%d frames still in use.  Did we lose one?\n",
+			    priv->frames_count);
+		priv->frames_count = 0;
+	}
+}
+
+static struct iwl_frame *iwl_get_free_frame(struct iwl_priv *priv)
+{
+	struct iwl_frame *frame;
+	struct list_head *element;
+	if (list_empty(&priv->free_frames)) {
+		frame = kzalloc(sizeof(*frame), GFP_KERNEL);
+		if (!frame) {
+			IWL_ERROR("Could not allocate frame!\n");
+			return NULL;
+		}
+
+		priv->frames_count++;
+		return frame;
+	}
+
+	element = priv->free_frames.next;
+	list_del(element);
+	return list_entry(element, struct iwl_frame, list);
+}
+
+static void iwl_free_frame(struct iwl_priv *priv, struct iwl_frame *frame)
+{
+	memset(frame, 0, sizeof(*frame));
+	list_add(&frame->list, &priv->free_frames);
+}
+
+unsigned int iwl_fill_beacon_frame(struct iwl_priv *priv,
+				struct ieee80211_hdr *hdr,
+				const u8 *dest, int left)
+{
+
+	if (!iwl_is_associated(priv) || !priv->ibss_beacon ||
+	    ((priv->iw_mode != IEEE80211_IF_TYPE_IBSS) &&
+	     (priv->iw_mode != IEEE80211_IF_TYPE_AP)))
+		return 0;
+
+	if (priv->ibss_beacon->len > left)
+		return 0;
+
+	memcpy(hdr, priv->ibss_beacon->data, priv->ibss_beacon->len);
+
+	return priv->ibss_beacon->len;
+}
+
+int iwl_rate_index_from_plcp(int plcp)
+{
+	int i = 0;
+
+	if (plcp & RATE_MCS_HT_MSK) {
+		i = (plcp & 0xff);
+
+		if (i >= IWL_RATE_MIMO_6M_PLCP)
+			i = i - IWL_RATE_MIMO_6M_PLCP;
+
+		i += IWL_FIRST_OFDM_RATE;
+		/* skip 9M not supported in ht*/
+		if (i >= IWL_RATE_9M_INDEX)
+			i += 1;
+		if ((i >= IWL_FIRST_OFDM_RATE) &&
+		    (i <= IWL_LAST_OFDM_RATE))
+			return i;
+	} else {
+		for (i = 0; i < ARRAY_SIZE(iwl_rates); i++)
+			if (iwl_rates[i].plcp == (plcp &0xFF))
+				return i;
+	}
+	return -1;
+}
+
+static u8 iwl_rate_get_lowest_plcp(int rate_mask)
+{
+	u8 i;
+
+	for (i = IWL_RATE_1M_INDEX; i != IWL_RATE_INVALID;
+	     i = iwl_rates[i].next_ieee) {
+		if (rate_mask & (1 << i))
+			return iwl_rates[i].plcp;
+	}
+
+	return IWL_RATE_INVALID;
+}
+
+static int iwl_send_beacon_cmd(struct iwl_priv *priv)
+{
+	struct iwl_frame *frame;
+	unsigned int frame_size;
+	int rc;
+	u8 rate;
+
+	frame = iwl_get_free_frame(priv);
+
+	if (!frame) {
+		IWL_ERROR("Could not obtain free frame buffer for beacon "
+			  "command.\n");
+		return -ENOMEM;
+	}
+
+	if (!(priv->staging_rxon.flags & RXON_FLG_BAND_24G_MSK)) {
+		rate = iwl_rate_get_lowest_plcp(priv->active_rate_basic &
+						0xFF0);
+		if (rate == IWL_INVALID_RATE)
+			rate = IWL_RATE_6M_PLCP;
+	} else {
+		rate = iwl_rate_get_lowest_plcp(priv->active_rate_basic & 0xF);
+		if (rate == IWL_INVALID_RATE)
+			rate = IWL_RATE_1M_PLCP;
+	}
+
+	frame_size = iwl_hw_get_beacon_cmd(priv, frame, rate);
+
+	rc = iwl_send_cmd_pdu(priv, REPLY_TX_BEACON, frame_size,
+			      &frame->u.cmd[0]);
+
+	iwl_free_frame(priv, frame);
+
+	return rc;
+}
+
+/******************************************************************************
+ *
+ * EEPROM related functions
+ *
+ ******************************************************************************/
+
+static void get_eeprom_mac(struct iwl_priv *priv, u8 *mac)
+{
+	memcpy(mac, priv->eeprom.mac_address, 6);
+}
+
+/**
+ * iwl_eeprom_init - read EEPROM contents
+ *
+ * Load the EEPROM from adapter into priv->eeprom
+ *
+ * NOTE:  This routine uses the non-debug IO access functions.
+ */
+int iwl_eeprom_init(struct iwl_priv *priv)
+{
+	u16 *e = (u16 *)&priv->eeprom;
+	u32 gp = iwl_read32(priv, CSR_EEPROM_GP);
+	u32 r;
+	int sz = sizeof(priv->eeprom);
+	int rc;
+	int i;
+	u16 addr;
+
+	/* The EEPROM structure has several padding buffers within it
+	 * and when adding new EEPROM maps is subject to programmer errors
+	 * which may be very difficult to identify without explicitly
+	 * checking the resulting size of the eeprom map. */
+	BUILD_BUG_ON(sizeof(priv->eeprom) != IWL_EEPROM_IMAGE_SIZE);
+
+	if ((gp & CSR_EEPROM_GP_VALID_MSK) == CSR_EEPROM_GP_BAD_SIGNATURE) {
+		IWL_ERROR("EEPROM not found, EEPROM_GP=0x%08x", gp);
+		return -ENOENT;
+	}
+
+	rc = iwl_eeprom_aqcuire_semaphore(priv);
+	if (rc < 0) {
+		IWL_ERROR("Failed to aqcuire EEPROM semaphore.\n");
+		return -ENOENT;
+	}
+
+	/* eeprom is an array of 16bit values */
+	for (addr = 0; addr < sz; addr += sizeof(u16)) {
+		_iwl_write32(priv, CSR_EEPROM_REG, addr << 1);
+		_iwl_clear_bit(priv, CSR_EEPROM_REG, CSR_EEPROM_REG_BIT_CMD);
+
+		for (i = 0; i < IWL_EEPROM_ACCESS_TIMEOUT;
+					i += IWL_EEPROM_ACCESS_DELAY) {
+			r = _iwl_read_restricted(priv, CSR_EEPROM_REG);
+			if (r & CSR_EEPROM_REG_READ_VALID_MSK)
+				break;
+			udelay(IWL_EEPROM_ACCESS_DELAY);
+		}
+
+		if (!(r & CSR_EEPROM_REG_READ_VALID_MSK)) {
+			IWL_ERROR("Time out reading EEPROM[%d]", addr);
+			rc = -ETIMEDOUT;
+			goto done;
+		}
+		e[addr / 2] = le16_to_cpu(r >> 16);
+	}
+	rc = 0;
+
+done:
+	iwl_eeprom_release_semaphore(priv);
+	return rc;
+}
+
+/******************************************************************************
+ *
+ * Misc. internal state and helper functions
+ *
+ ******************************************************************************/
+#ifdef CONFIG_IWLWIFI_DEBUG
+
+/**
+ * iwl_report_frame - dump frame to syslog during debug sessions
+ *
+ * hack this function to show different aspects of received frames,
+ * including selective frame dumps.
+ * group100 parameter selects whether to show 1 out of 100 good frames.
+ *
+ * TODO:  ieee80211_hdr stuff is common to 3945 and 4965, so frame type
+ *        info output is okay, but some of this stuff (e.g. iwl_rx_frame_stats)
+ *        is 3945-specific and gives bad output for 4965.  Need to split the
+ *        functionality, keep common stuff here.
+ */
+void iwl_report_frame(struct iwl_priv *priv,
+		      struct iwl_rx_packet *pkt,
+		      struct ieee80211_hdr *header, int group100)
+{
+	u32 to_us;
+	u32 print_summary = 0;
+	u32 print_dump = 0;	/* set to 1 to dump all frames' contents */
+	u32 hundred = 0;
+	u32 dataframe = 0;
+	u16 fc;
+	u16 seq_ctl;
+	u16 channel;
+	u16 phy_flags;
+	int rate_sym;
+	u16 length;
+	u16 status;
+	u16 bcn_tmr;
+	u32 tsf_low;
+	u64 tsf;
+	u8 rssi;
+	u8 agc;
+	u16 sig_avg;
+	u16 noise_diff;
+	struct iwl_rx_frame_stats *rx_stats = IWL_RX_STATS(pkt);
+	struct iwl_rx_frame_hdr *rx_hdr = IWL_RX_HDR(pkt);
+	struct iwl_rx_frame_end *rx_end = IWL_RX_END(pkt);
+	u8 *data = IWL_RX_DATA(pkt);
+
+	/* MAC header */
+	fc = le16_to_cpu(header->frame_control);
+	seq_ctl = le16_to_cpu(header->seq_ctrl);
+
+	/* metadata */
+	channel = le16_to_cpu(rx_hdr->channel);
+	phy_flags = le16_to_cpu(rx_hdr->phy_flags);
+	rate_sym = rx_hdr->rate;
+	length = le16_to_cpu(rx_hdr->len);
+
+	/* end-of-frame status and timestamp */
+	status = le32_to_cpu(rx_end->status);
+	bcn_tmr = le32_to_cpu(rx_end->beacon_timestamp);
+	tsf_low = le64_to_cpu(rx_end->timestamp) & 0x0ffffffff;
+	tsf = le64_to_cpu(rx_end->timestamp);
+
+	/* signal statistics */
+	rssi = rx_stats->rssi;
+	agc = rx_stats->agc;
+	sig_avg = le16_to_cpu(rx_stats->sig_avg);
+	noise_diff = le16_to_cpu(rx_stats->noise_diff);
+
+	to_us = !compare_ether_addr(header->addr1, priv->mac_addr);
+
+	/* if data frame is to us and all is good,
+	 *   (optionally) print summary for only 1 out of every 100 */
+	if (to_us && (fc & ~IEEE80211_FCTL_PROTECTED) ==
+	    (IEEE80211_FCTL_FROMDS | IEEE80211_FTYPE_DATA)) {
+		dataframe = 1;
+		if (!group100)
+			print_summary = 1;	/* print each frame */
+		else if (priv->framecnt_to_us < 100) {
+			priv->framecnt_to_us++;
+			print_summary = 0;
+		} else {
+			priv->framecnt_to_us = 0;
+			print_summary = 1;
+			hundred = 1;
+		}
+	} else {
+		/* print summary for all other frames */
+		print_summary = 1;
+	}
+
+	if (print_summary) {
+		char *title;
+		u32 rate;
+
+		if (hundred)
+			title = "100Frames";
+		else if (fc & IEEE80211_FCTL_RETRY)
+			title = "Retry";
+		else if (ieee80211_is_assoc_response(fc))
+			title = "AscRsp";
+		else if (ieee80211_is_reassoc_response(fc))
+			title = "RasRsp";
+		else if (ieee80211_is_probe_response(fc)) {
+			title = "PrbRsp";
+			print_dump = 1;	/* dump frame contents */
+		} else if (ieee80211_is_beacon(fc)) {
+			title = "Beacon";
+			print_dump = 1;	/* dump frame contents */
+		} else if (ieee80211_is_atim(fc))
+			title = "ATIM";
+		else if (ieee80211_is_auth(fc))
+			title = "Auth";
+		else if (ieee80211_is_deauth(fc))
+			title = "DeAuth";
+		else if (ieee80211_is_disassoc(fc))
+			title = "DisAssoc";
+		else
+			title = "Frame";
+
+		rate = iwl_rate_index_from_plcp(rate_sym);
+		if (rate == -1)
+			rate = 0;
+		else
+			rate = iwl_rates[rate].ieee / 2;
+
+		/* print frame summary.
+		 * MAC addresses show just the last byte (for brevity),
+		 *    but you can hack it to show more, if you'd like to. */
+		if (dataframe)
+			IWL_DEBUG_RX("%s: mhd=0x%04x, dst=0x%02x, "
+				     "len=%u, rssi=%d, chnl=%d, rate=%u, \n",
+				     title, fc, header->addr1[5],
+				     length, rssi, channel, rate);
+		else {
+			/* src/dst addresses assume managed mode */
+			IWL_DEBUG_RX("%s: 0x%04x, dst=0x%02x, "
+				     "src=0x%02x, rssi=%u, tim=%lu usec, "
+				     "phy=0x%02x, chnl=%d\n",
+				     title, fc, header->addr1[5],
+				     header->addr3[5], rssi,
+				     tsf_low - priv->scan_start_tsf,
+				     phy_flags, channel);
+		}
+	}
+	if (print_dump)
+		iwl_print_hex_dump(IWL_DL_RX, data, length);
+}
+#endif
+
+static void iwl_unset_hw_setting(struct iwl_priv *priv)
+{
+	if (priv->hw_setting.shared_virt)
+		pci_free_consistent(priv->pci_dev,
+				    sizeof(struct iwl_shared),
+				    priv->hw_setting.shared_virt,
+				    priv->hw_setting.shared_phys);
+}
+
+/**
+ * iwl_supported_rate_to_ie - fill in the supported rate in IE field
+ *
+ * return : set the bit for each supported rate insert in ie
+ */
+static u16 iwl_supported_rate_to_ie(u8 *ie, u16 supported_rate,
+				    u16 basic_rate, int max_count)
+{
+	u16 ret_rates = 0, bit;
+	int i;
+	u8 *rates;
+
+	rates = &(ie[1]);
+
+	for (bit = 1, i = 0; i < IWL_RATE_COUNT; i++, bit <<= 1) {
+		if (bit & supported_rate) {
+			ret_rates |= bit;
+			rates[*ie] = iwl_rates[i].ieee |
+			    ((bit & basic_rate) ? 0x80 : 0x00);
+			*ie = *ie + 1;
+			if (*ie >= max_count)
+				break;
+		}
+	}
+
+	return ret_rates;
+}
+
+#ifdef CONFIG_IWLWIFI_HT
+void static iwl_set_ht_capab(struct ieee80211_hw *hw,
+			     struct ieee80211_ht_capability *ht_cap,
+			     u8 use_wide_chan);
+#endif
+
+/**
+ * iwl_fill_probe_req - fill in all required fields and IE for probe request
+ */
+static u16 iwl_fill_probe_req(struct iwl_priv *priv,
+			      struct ieee80211_mgmt *frame,
+			      int left, int is_direct)
+{
+	int len = 0;
+	u8 *pos = NULL;
+	u16 ret_rates;
+
+	/* Make sure there is enough space for the probe request,
+	 * two mandatory IEs and the data */
+	left -= 24;
+	if (left < 0)
+		return 0;
+	len += 24;
+
+	frame->frame_control = cpu_to_le16(IEEE80211_STYPE_PROBE_REQ);
+	memcpy(frame->da, BROADCAST_ADDR, ETH_ALEN);
+	memcpy(frame->sa, priv->mac_addr, ETH_ALEN);
+	memcpy(frame->bssid, BROADCAST_ADDR, ETH_ALEN);
+	frame->seq_ctrl = 0;
+
+	/* fill in our indirect SSID IE */
+	/* ...next IE... */
+
+	left -= 2;
+	if (left < 0)
+		return 0;
+	len += 2;
+	pos = &(frame->u.probe_req.variable[0]);
+	*pos++ = WLAN_EID_SSID;
+	*pos++ = 0;
+
+	/* fill in our direct SSID IE... */
+	if (is_direct) {
+		/* ...next IE... */
+		left -= 2 + priv->essid_len;
+		if (left < 0)
+			return 0;
+		/* ... fill it in... */
+		*pos++ = WLAN_EID_SSID;
+		*pos++ = priv->essid_len;
+		memcpy(pos, priv->essid, priv->essid_len);
+		pos += priv->essid_len;
+		len += 2 + priv->essid_len;
+	}
+
+	/* fill in supported rate */
+	/* ...next IE... */
+	left -= 2;
+	if (left < 0)
+		return 0;
+	/* ... fill it in... */
+	*pos++ = WLAN_EID_SUPP_RATES;
+	*pos = 0;
+	ret_rates = priv->active_rate = priv->rates_mask;
+	priv->active_rate_basic = priv->rates_mask & IWL_BASIC_RATES_MASK;
+
+	iwl_supported_rate_to_ie(pos, priv->active_rate,
+				 priv->active_rate_basic, left);
+	len += 2 + *pos;
+	pos += (*pos) + 1;
+	ret_rates = ~ret_rates & priv->active_rate;
+
+	if (ret_rates == 0)
+		goto fill_end;
+
+	/* fill in supported extended rate */
+	/* ...next IE... */
+	left -= 2;
+	if (left < 0)
+		return 0;
+	/* ... fill it in... */
+	*pos++ = WLAN_EID_EXT_SUPP_RATES;
+	*pos = 0;
+	iwl_supported_rate_to_ie(pos, ret_rates, priv->active_rate_basic, left);
+	if (*pos > 0)
+		len += 2 + *pos;
+
+#ifdef CONFIG_IWLWIFI_HT
+	if (is_direct && priv->is_ht_enabled) {
+		u8 use_wide_chan = 1;
+
+		if (priv->channel_width != IWL_CHANNEL_WIDTH_40MHZ)
+			use_wide_chan = 0;
+		pos += (*pos) + 1;
+		*pos++ = WLAN_EID_HT_CAPABILITY;
+		*pos++ = sizeof(struct ieee80211_ht_capability);
+		iwl_set_ht_capab(NULL, (struct ieee80211_ht_capability *)pos,
+				 use_wide_chan);
+		len += 2 + sizeof(struct ieee80211_ht_capability);
+	}
+#endif  /*CONFIG_IWLWIFI_HT */
+
+ fill_end:
+	return (u16)len;
+}
+
+/*
+ * QoS  support
+*/
+#ifdef CONFIG_IWLWIFI_QOS
+static int iwl_send_qos_params_command(struct iwl_priv *priv,
+				       struct iwl_qosparam_cmd *qos)
+{
+
+	return iwl_send_cmd_pdu(priv, REPLY_QOS_PARAM,
+				sizeof(struct iwl_qosparam_cmd), qos);
+}
+
+static void iwl_reset_qos(struct iwl_priv *priv)
+{
+	u16 cw_min = 15;
+	u16 cw_max = 1023;
+	u8 aifs = 2;
+	u8 is_legacy = 0;
+	unsigned long flags;
+	int i;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	priv->qos_data.qos_active = 0;
+
+	if (priv->iw_mode == IEEE80211_IF_TYPE_IBSS) {
+		if (priv->qos_data.qos_enable)
+			priv->qos_data.qos_active = 1;
+		if (!(priv->active_rate & 0xfff0)) {
+			cw_min = 31;
+			is_legacy = 1;
+		}
+	} else if (priv->iw_mode == IEEE80211_IF_TYPE_AP) {
+		if (priv->qos_data.qos_enable)
+			priv->qos_data.qos_active = 1;
+	} else if (!(priv->staging_rxon.flags & RXON_FLG_SHORT_SLOT_MSK)) {
+		cw_min = 31;
+		is_legacy = 1;
+	}
+
+	if (priv->qos_data.qos_active)
+		aifs = 3;
+
+	priv->qos_data.def_qos_parm.ac[0].cw_min = cpu_to_le16(cw_min);
+	priv->qos_data.def_qos_parm.ac[0].cw_max = cpu_to_le16(cw_max);
+	priv->qos_data.def_qos_parm.ac[0].aifsn = aifs;
+	priv->qos_data.def_qos_parm.ac[0].edca_txop = 0;
+	priv->qos_data.def_qos_parm.ac[0].reserved1 = 0;
+
+	if (priv->qos_data.qos_active) {
+		i = 1;
+		priv->qos_data.def_qos_parm.ac[i].cw_min = cpu_to_le16(cw_min);
+		priv->qos_data.def_qos_parm.ac[i].cw_max = cpu_to_le16(cw_max);
+		priv->qos_data.def_qos_parm.ac[i].aifsn = 7;
+		priv->qos_data.def_qos_parm.ac[i].edca_txop = 0;
+		priv->qos_data.def_qos_parm.ac[i].reserved1 = 0;
+
+		i = 2;
+		priv->qos_data.def_qos_parm.ac[i].cw_min =
+			cpu_to_le16((cw_min + 1) / 2 - 1);
+		priv->qos_data.def_qos_parm.ac[i].cw_max =
+			cpu_to_le16(cw_max);
+		priv->qos_data.def_qos_parm.ac[i].aifsn = 2;
+		if (is_legacy)
+			priv->qos_data.def_qos_parm.ac[i].edca_txop =
+				cpu_to_le16(6016);
+		else
+			priv->qos_data.def_qos_parm.ac[i].edca_txop =
+				cpu_to_le16(3008);
+		priv->qos_data.def_qos_parm.ac[i].reserved1 = 0;
+
+		i = 3;
+		priv->qos_data.def_qos_parm.ac[i].cw_min =
+			cpu_to_le16((cw_min + 1) / 4 - 1);
+		priv->qos_data.def_qos_parm.ac[i].cw_max =
+			cpu_to_le16((cw_max + 1) / 2 - 1);
+		priv->qos_data.def_qos_parm.ac[i].aifsn = 2;
+		priv->qos_data.def_qos_parm.ac[i].reserved1 = 0;
+		if (is_legacy)
+			priv->qos_data.def_qos_parm.ac[i].edca_txop =
+				cpu_to_le16(3264);
+		else
+			priv->qos_data.def_qos_parm.ac[i].edca_txop =
+				cpu_to_le16(1504);
+	} else {
+		for (i = 1; i < 4; i++) {
+			priv->qos_data.def_qos_parm.ac[i].cw_min =
+				cpu_to_le16(cw_min);
+			priv->qos_data.def_qos_parm.ac[i].cw_max =
+				cpu_to_le16(cw_max);
+			priv->qos_data.def_qos_parm.ac[i].aifsn = aifs;
+			priv->qos_data.def_qos_parm.ac[i].edca_txop = 0;
+			priv->qos_data.def_qos_parm.ac[i].reserved1 = 0;
+		}
+	}
+	IWL_DEBUG_QOS("set QoS to default \n");
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+}
+
+static void iwl_activate_qos(struct iwl_priv *priv, u8 force)
+{
+	unsigned long flags;
+
+	if (priv == NULL)
+		return;
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+		return;
+
+	if (!priv->qos_data.qos_enable)
+		return;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	priv->qos_data.def_qos_parm.qos_flags = 0;
+
+	if (priv->qos_data.qos_cap.q_AP.queue_request &&
+	    !priv->qos_data.qos_cap.q_AP.txop_request)
+		priv->qos_data.def_qos_parm.qos_flags |=
+			QOS_PARAM_FLG_TXOP_TYPE_MSK;
+
+	if (priv->qos_data.qos_active)
+		priv->qos_data.def_qos_parm.qos_flags |=
+			QOS_PARAM_FLG_UPDATE_EDCA_MSK;
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	if (force || iwl_is_associated(priv)) {
+		IWL_DEBUG_QOS("send QoS cmd with Qos active %d \n",
+			      priv->qos_data.qos_active);
+
+		iwl_send_qos_params_command(priv,
+				&(priv->qos_data.def_qos_parm));
+	}
+}
+
+#endif /* CONFIG_IWLWIFI_QOS */
+/*
+ * Power management (not Tx power!) functions
+ */
+#define MSEC_TO_USEC 1024
+
+#define NOSLP __constant_cpu_to_le16(0), 0, 0
+#define SLP IWL_POWER_DRIVER_ALLOW_SLEEP_MSK, 0, 0
+#define SLP_TIMEOUT(T) __constant_cpu_to_le32((T) * MSEC_TO_USEC)
+#define SLP_VEC(X0, X1, X2, X3, X4) {__constant_cpu_to_le32(X0), \
+				     __constant_cpu_to_le32(X1), \
+				     __constant_cpu_to_le32(X2), \
+				     __constant_cpu_to_le32(X3), \
+				     __constant_cpu_to_le32(X4)}
+
+
+/* default power management (not Tx power) table values */
+/* for tim  0-10 */
+static struct iwl_power_vec_entry range_0[IWL_POWER_AC] = {
+	{{NOSLP, SLP_TIMEOUT(0), SLP_TIMEOUT(0), SLP_VEC(0, 0, 0, 0, 0)}, 0},
+	{{SLP, SLP_TIMEOUT(200), SLP_TIMEOUT(500), SLP_VEC(1, 2, 3, 4, 4)}, 0},
+	{{SLP, SLP_TIMEOUT(200), SLP_TIMEOUT(300), SLP_VEC(2, 4, 6, 7, 7)}, 0},
+	{{SLP, SLP_TIMEOUT(50), SLP_TIMEOUT(100), SLP_VEC(2, 6, 9, 9, 10)}, 0},
+	{{SLP, SLP_TIMEOUT(50), SLP_TIMEOUT(25), SLP_VEC(2, 7, 9, 9, 10)}, 1},
+	{{SLP, SLP_TIMEOUT(25), SLP_TIMEOUT(25), SLP_VEC(4, 7, 10, 10, 10)}, 1}
+};
+
+/* for tim > 10 */
+static struct iwl_power_vec_entry range_1[IWL_POWER_AC] = {
+	{{NOSLP, SLP_TIMEOUT(0), SLP_TIMEOUT(0), SLP_VEC(0, 0, 0, 0, 0)}, 0},
+	{{SLP, SLP_TIMEOUT(200), SLP_TIMEOUT(500),
+		 SLP_VEC(1, 2, 3, 4, 0xFF)}, 0},
+	{{SLP, SLP_TIMEOUT(200), SLP_TIMEOUT(300),
+		 SLP_VEC(2, 4, 6, 7, 0xFF)}, 0},
+	{{SLP, SLP_TIMEOUT(50), SLP_TIMEOUT(100),
+		 SLP_VEC(2, 6, 9, 9, 0xFF)}, 0},
+	{{SLP, SLP_TIMEOUT(50), SLP_TIMEOUT(25), SLP_VEC(2, 7, 9, 9, 0xFF)}, 0},
+	{{SLP, SLP_TIMEOUT(25), SLP_TIMEOUT(25),
+		 SLP_VEC(4, 7, 10, 10, 0xFF)}, 0}
+};
+
+int iwl_power_init_handle(struct iwl_priv *priv)
+{
+	int rc = 0, i;
+	struct iwl_power_mgr *pow_data;
+	int size = sizeof(struct iwl_power_vec_entry) * IWL_POWER_AC;
+	u16 pci_pm;
+
+	IWL_DEBUG_POWER("Initialize power \n");
+
+	pow_data = &(priv->power_data);
+
+	memset(pow_data, 0, sizeof(*pow_data));
+
+	pow_data->active_index = IWL_POWER_RANGE_0;
+	pow_data->dtim_val = 0xffff;
+
+	memcpy(&pow_data->pwr_range_0[0], &range_0[0], size);
+	memcpy(&pow_data->pwr_range_1[0], &range_1[0], size);
+
+	rc = pci_read_config_word(priv->pci_dev, PCI_LINK_CTRL, &pci_pm);
+	if (rc != 0)
+		return 0;
+	else {
+		struct iwl_powertable_cmd *cmd;
+
+		IWL_DEBUG_POWER("adjust power command flags\n");
+
+		for (i = 0; i < IWL_POWER_AC; i++) {
+			cmd = &pow_data->pwr_range_0[i].cmd;
+
+			if (pci_pm & 0x1)
+				cmd->flags &= ~IWL_POWER_PCI_PM_MSK;
+			else
+				cmd->flags |= IWL_POWER_PCI_PM_MSK;
+		}
+	}
+	return rc;
+}
+
+static int iwl_update_power_cmd(struct iwl_priv *priv,
+				struct iwl_powertable_cmd *cmd, u32 mode)
+{
+	int rc = 0, i;
+	u8 skip;
+	u32 max_sleep = 0;
+	struct iwl_power_vec_entry *range;
+	u8 period = 0;
+	struct iwl_power_mgr *pow_data;
+
+	if (mode > IWL_POWER_INDEX_5) {
+		IWL_DEBUG_POWER("Error invalid power mode \n");
+		return -1;
+	}
+	pow_data = &(priv->power_data);
+
+	if (pow_data->active_index == IWL_POWER_RANGE_0)
+		range = &pow_data->pwr_range_0[0];
+	else
+		range = &pow_data->pwr_range_1[1];
+
+	memcpy(cmd, &range[mode].cmd, sizeof(struct iwl_powertable_cmd));
+
+#ifdef IWL_MAC80211_DISABLE
+	if (priv->assoc_network != NULL) {
+		unsigned long flags;
+
+		period = priv->assoc_network->tim.tim_period;
+	}
+#endif	/*IWL_MAC80211_DISABLE */
+	skip = range[mode].no_dtim;
+
+	if (period == 0) {
+		period = 1;
+		skip = 0;
+	}
+
+	if (skip == 0) {
+		max_sleep = period;
+		cmd->flags &= ~IWL_POWER_SLEEP_OVER_DTIM_MSK;
+	} else {
+		__le32 slp_itrvl = cmd->sleep_interval[IWL_POWER_VEC_SIZE - 1];
+		max_sleep = (le32_to_cpu(slp_itrvl) / period) * period;
+		cmd->flags |= IWL_POWER_SLEEP_OVER_DTIM_MSK;
+	}
+
+	for (i = 0; i < IWL_POWER_VEC_SIZE; i++) {
+		if (le32_to_cpu(cmd->sleep_interval[i]) > max_sleep)
+			cmd->sleep_interval[i] = cpu_to_le32(max_sleep);
+	}
+
+	IWL_DEBUG_POWER("Flags value = 0x%08X\n", cmd->flags);
+	IWL_DEBUG_POWER("Tx timeout = %u\n", le32_to_cpu(cmd->tx_data_timeout));
+	IWL_DEBUG_POWER("Rx timeout = %u\n", le32_to_cpu(cmd->rx_data_timeout));
+	IWL_DEBUG_POWER("Sleep interval vector = { %d , %d , %d , %d , %d }\n",
+			le32_to_cpu(cmd->sleep_interval[0]),
+			le32_to_cpu(cmd->sleep_interval[1]),
+			le32_to_cpu(cmd->sleep_interval[2]),
+			le32_to_cpu(cmd->sleep_interval[3]),
+			le32_to_cpu(cmd->sleep_interval[4]));
+
+	return rc;
+}
+
+static int iwl_send_power_mode(struct iwl_priv *priv, u32 mode)
+{
+	u32 final_mode = mode;
+	int rc;
+	struct iwl_powertable_cmd cmd;
+
+	/* If on battery, set to 3,
+	 * if plugged into AC power, set to CAM ("continuosly aware mode"),
+	 * else user level */
+	switch (mode) {
+	case IWL_POWER_BATTERY:
+		final_mode = IWL_POWER_INDEX_3;
+		break;
+	case IWL_POWER_AC:
+		final_mode = IWL_POWER_MODE_CAM;
+		break;
+	default:
+		final_mode = mode;
+		break;
+	}
+
+	cmd.keep_alive_beacons = 0;
+
+	iwl_update_power_cmd(priv, &cmd, final_mode);
+
+	rc = iwl_send_cmd_pdu(priv, POWER_TABLE_CMD, sizeof(cmd), &cmd);
+
+	if (final_mode == IWL_POWER_MODE_CAM)
+		clear_bit(STATUS_POWER_PMI, &priv->status);
+	else
+		set_bit(STATUS_POWER_PMI, &priv->status);
+
+	return rc;
+}
+
+int iwl_is_network_packet(struct iwl_priv *priv, struct ieee80211_hdr *header)
+{
+	/* Filter incoming packets to determine if they are targeted toward
+	 * this network, discarding packets coming from ourselves */
+	switch (priv->iw_mode) {
+	case IEEE80211_IF_TYPE_IBSS: /* Header: Dest. | Source    | BSSID */
+		/* packets from our adapter are dropped (echo) */
+		if (!compare_ether_addr(header->addr2, priv->mac_addr))
+			return 0;
+		/* {broad,multi}cast packets to our IBSS go through */
+		if (is_multicast_ether_addr(header->addr1))
+			return !compare_ether_addr(header->addr3, priv->bssid);
+		/* packets to our adapter go through */
+		return !compare_ether_addr(header->addr1, priv->mac_addr);
+	case IEEE80211_IF_TYPE_STA: /* Header: Dest. | AP{BSSID} | Source */
+		/* packets from our adapter are dropped (echo) */
+		if (!compare_ether_addr(header->addr3, priv->mac_addr))
+			return 0;
+		/* {broad,multi}cast packets to our BSS go through */
+		if (is_multicast_ether_addr(header->addr1))
+			return !compare_ether_addr(header->addr2, priv->bssid);
+		/* packets to our adapter go through */
+		return !compare_ether_addr(header->addr1, priv->mac_addr);
+	}
+
+	return 1;
+}
+
+#define TX_STATUS_ENTRY(x) case TX_STATUS_FAIL_ ## x: return #x
+
+const char *iwl_get_tx_fail_reason(u32 status)
+{
+	switch (status & TX_STATUS_MSK) {
+	case TX_STATUS_SUCCESS:
+		return "SUCCESS";
+		TX_STATUS_ENTRY(SHORT_LIMIT);
+		TX_STATUS_ENTRY(LONG_LIMIT);
+		TX_STATUS_ENTRY(FIFO_UNDERRUN);
+		TX_STATUS_ENTRY(MGMNT_ABORT);
+		TX_STATUS_ENTRY(NEXT_FRAG);
+		TX_STATUS_ENTRY(LIFE_EXPIRE);
+		TX_STATUS_ENTRY(DEST_PS);
+		TX_STATUS_ENTRY(ABORTED);
+		TX_STATUS_ENTRY(BT_RETRY);
+		TX_STATUS_ENTRY(STA_INVALID);
+		TX_STATUS_ENTRY(FRAG_DROPPED);
+		TX_STATUS_ENTRY(TID_DISABLE);
+		TX_STATUS_ENTRY(FRAME_FLUSHED);
+		TX_STATUS_ENTRY(INSUFFICIENT_CF_POLL);
+		TX_STATUS_ENTRY(TX_LOCKED);
+		TX_STATUS_ENTRY(NO_BEACON_ON_RADAR);
+	}
+
+	return "UNKNOWN";
+}
+
+/**
+ * iwl_scan_cancel - Cancel any currently executing HW scan
+ *
+ * NOTE: priv->mutex is not required before calling this function
+ */
+static int iwl_scan_cancel(struct iwl_priv *priv)
+{
+	if (!test_bit(STATUS_SCAN_HW, &priv->status)) {
+		clear_bit(STATUS_SCANNING, &priv->status);
+		return 0;
+	}
+
+	if (test_bit(STATUS_SCANNING, &priv->status)) {
+		if (!test_bit(STATUS_SCAN_ABORTING, &priv->status)) {
+			IWL_DEBUG_SCAN("Queuing scan abort.\n");
+			set_bit(STATUS_SCAN_ABORTING, &priv->status);
+			queue_work(priv->workqueue, &priv->abort_scan);
+
+		} else
+			IWL_DEBUG_SCAN("Scan abort already in progress.\n");
+
+		return test_bit(STATUS_SCANNING, &priv->status);
+	}
+
+	return 0;
+}
+
+/**
+ * iwl_scan_cancel_timeout - Cancel any currently executing HW scan
+ * @ms: amount of time to wait (in milliseconds) for scan to abort
+ *
+ * NOTE: priv->mutex must be held before calling this function
+ */
+static int iwl_scan_cancel_timeout(struct iwl_priv *priv, unsigned long ms)
+{
+	unsigned long now = jiffies;
+	int ret;
+
+	ret = iwl_scan_cancel(priv);
+	if (ret && ms) {
+		mutex_unlock(&priv->mutex);
+		while (!time_after(jiffies, now + msecs_to_jiffies(ms)) &&
+				test_bit(STATUS_SCANNING, &priv->status))
+			msleep(1);
+		mutex_lock(&priv->mutex);
+
+		return test_bit(STATUS_SCANNING, &priv->status);
+	}
+
+	return ret;
+}
+
+static void iwl_sequence_reset(struct iwl_priv *priv)
+{
+	/* Reset ieee stats */
+
+	/* We don't reset the net_device_stats (ieee->stats) on
+	 * re-association */
+
+	priv->last_seq_num = -1;
+	priv->last_frag_num = -1;
+	priv->last_packet_time = 0;
+
+	iwl_scan_cancel(priv);
+}
+
+#define MAX_UCODE_BEACON_INTERVAL	4096
+#define INTEL_CONN_LISTEN_INTERVAL	__constant_cpu_to_le16(0xA)
+
+static __le16 iwl_adjust_beacon_interval(u16 beacon_val)
+{
+	u16 new_val = 0;
+	u16 beacon_factor = 0;
+
+	beacon_factor =
+	    (beacon_val + MAX_UCODE_BEACON_INTERVAL)
+		/ MAX_UCODE_BEACON_INTERVAL;
+	new_val = beacon_val / beacon_factor;
+
+	return cpu_to_le16(new_val);
+}
+
+static void iwl_setup_rxon_timing(struct iwl_priv *priv)
+{
+	u64 interval_tm_unit;
+	u64 tsf, result;
+	unsigned long flags;
+	struct ieee80211_conf *conf = NULL;
+	u16 beacon_int = 0;
+
+	conf = ieee80211_get_hw_conf(priv->hw);
+
+	spin_lock_irqsave(&priv->lock, flags);
+	priv->rxon_timing.timestamp.dw[1] = cpu_to_le32(priv->timestamp1);
+	priv->rxon_timing.timestamp.dw[0] = cpu_to_le32(priv->timestamp0);
+
+	priv->rxon_timing.listen_interval = INTEL_CONN_LISTEN_INTERVAL;
+
+	tsf = priv->timestamp1;
+	tsf = ((tsf << 32) | priv->timestamp0);
+
+	beacon_int = priv->beacon_int;
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	if (priv->iw_mode == IEEE80211_IF_TYPE_STA) {
+		if (beacon_int == 0) {
+			priv->rxon_timing.beacon_interval = cpu_to_le16(100);
+			priv->rxon_timing.beacon_init_val = cpu_to_le32(102400);
+		} else {
+			priv->rxon_timing.beacon_interval =
+				cpu_to_le16(beacon_int);
+			priv->rxon_timing.beacon_interval =
+			    iwl_adjust_beacon_interval(
+				le16_to_cpu(priv->rxon_timing.beacon_interval));
+		}
+
+		priv->rxon_timing.atim_window = 0;
+	} else {
+		priv->rxon_timing.beacon_interval =
+			iwl_adjust_beacon_interval(conf->beacon_int);
+		/* TODO: we need to get atim_window from upper stack
+		 * for now we set to 0 */
+		priv->rxon_timing.atim_window = 0;
+	}
+
+	interval_tm_unit =
+		(le16_to_cpu(priv->rxon_timing.beacon_interval) * 1024);
+	result = do_div(tsf, interval_tm_unit);
+	priv->rxon_timing.beacon_init_val =
+	    cpu_to_le32((u32) ((u64) interval_tm_unit - result));
+
+	IWL_DEBUG_ASSOC
+	    ("beacon interval %d beacon timer %d beacon tim %d\n",
+		le16_to_cpu(priv->rxon_timing.beacon_interval),
+		le32_to_cpu(priv->rxon_timing.beacon_init_val),
+		le16_to_cpu(priv->rxon_timing.atim_window));
+}
+
+static int iwl_scan_initiate(struct iwl_priv *priv)
+{
+	if (priv->iw_mode == IEEE80211_IF_TYPE_AP) {
+		IWL_ERROR("APs don't scan.\n");
+		return 0;
+	}
+
+	if (!iwl_is_ready_rf(priv)) {
+		IWL_DEBUG_SCAN("Aborting scan due to not ready.\n");
+		return -EIO;
+	}
+
+	if (test_bit(STATUS_SCANNING, &priv->status)) {
+		IWL_DEBUG_SCAN("Scan already in progress.\n");
+		return -EAGAIN;
+	}
+
+	if (test_bit(STATUS_SCAN_ABORTING, &priv->status)) {
+		IWL_DEBUG_SCAN("Scan request while abort pending.  "
+			       "Queuing.\n");
+		return -EAGAIN;
+	}
+
+	IWL_DEBUG_INFO("Starting scan...\n");
+	priv->scan_bands = 2;
+	set_bit(STATUS_SCANNING, &priv->status);
+	priv->scan_start = jiffies;
+	priv->scan_pass_start = priv->scan_start;
+
+	queue_work(priv->workqueue, &priv->request_scan);
+
+	return 0;
+}
+
+static int iwl_set_rxon_hwcrypto(struct iwl_priv *priv, int hw_decrypt)
+{
+	struct iwl_rxon_cmd *rxon = &priv->staging_rxon;
+
+	if (hw_decrypt)
+		rxon->filter_flags &= ~RXON_FILTER_DIS_DECRYPT_MSK;
+	else
+		rxon->filter_flags |= RXON_FILTER_DIS_DECRYPT_MSK;
+
+	return 0;
+}
+
+static void iwl_set_flags_for_phymode(struct iwl_priv *priv, u8 phymode)
+{
+	if (phymode == MODE_IEEE80211A) {
+		priv->staging_rxon.flags &=
+		    ~(RXON_FLG_BAND_24G_MSK | RXON_FLG_AUTO_DETECT_MSK
+		      | RXON_FLG_CCK_MSK);
+		priv->staging_rxon.flags |= RXON_FLG_SHORT_SLOT_MSK;
+	} else {
+		/* Copied from iwl_bg_post_associate() */
+		if (priv->assoc_capability & WLAN_CAPABILITY_SHORT_SLOT_TIME)
+			priv->staging_rxon.flags |= RXON_FLG_SHORT_SLOT_MSK;
+		else
+			priv->staging_rxon.flags &= ~RXON_FLG_SHORT_SLOT_MSK;
+
+		if (priv->iw_mode == IEEE80211_IF_TYPE_IBSS)
+			priv->staging_rxon.flags &= ~RXON_FLG_SHORT_SLOT_MSK;
+
+		priv->staging_rxon.flags |= RXON_FLG_BAND_24G_MSK;
+		priv->staging_rxon.flags |= RXON_FLG_AUTO_DETECT_MSK;
+		priv->staging_rxon.flags &= ~RXON_FLG_CCK_MSK;
+	}
+}
+
+/*
+ * initilize rxon structure with default values fromm eeprom
+ */
+static void iwl_connection_init_rx_config(struct iwl_priv *priv)
+{
+	const struct iwl_channel_info *ch_info;
+
+	memset(&priv->staging_rxon, 0, sizeof(priv->staging_rxon));
+
+	switch (priv->iw_mode) {
+	case IEEE80211_IF_TYPE_AP:
+		priv->staging_rxon.dev_type = RXON_DEV_TYPE_AP;
+		break;
+
+	case IEEE80211_IF_TYPE_STA:
+		priv->staging_rxon.dev_type = RXON_DEV_TYPE_ESS;
+		priv->staging_rxon.filter_flags = RXON_FILTER_ACCEPT_GRP_MSK;
+		break;
+
+	case IEEE80211_IF_TYPE_IBSS:
+		priv->staging_rxon.dev_type = RXON_DEV_TYPE_IBSS;
+		priv->staging_rxon.flags = RXON_FLG_SHORT_PREAMBLE_MSK;
+		priv->staging_rxon.filter_flags = RXON_FILTER_BCON_AWARE_MSK |
+						  RXON_FILTER_ACCEPT_GRP_MSK;
+		break;
+
+	case IEEE80211_IF_TYPE_MNTR:
+		priv->staging_rxon.dev_type = RXON_DEV_TYPE_SNIFFER;
+		priv->staging_rxon.filter_flags = RXON_FILTER_PROMISC_MSK |
+		    RXON_FILTER_CTL2HOST_MSK | RXON_FILTER_ACCEPT_GRP_MSK;
+		break;
+	}
+
+#if 0
+	/* TODO:  Figure out when short_preamble would be set and cache from
+	 * that */
+	if (!hw_to_local(priv->hw)->short_preamble)
+		priv->staging_rxon.flags &= ~RXON_FLG_SHORT_PREAMBLE_MSK;
+	else
+		priv->staging_rxon.flags |= RXON_FLG_SHORT_PREAMBLE_MSK;
+#endif
+
+	ch_info = iwl_get_channel_info(priv, priv->phymode,
+				       le16_to_cpu(priv->staging_rxon.channel));
+
+	if (!ch_info)
+		ch_info = &priv->channel_info[0];
+
+	/*
+	 * in some case A channels are all non IBSS
+	 * in this case force B/G channel
+	 */
+	if ((priv->iw_mode == IEEE80211_IF_TYPE_IBSS) &&
+	    !(is_channel_ibss(ch_info)))
+		ch_info = &priv->channel_info[0];
+
+	priv->staging_rxon.channel = cpu_to_le16(ch_info->channel);
+	if (is_channel_a_band(ch_info))
+		priv->phymode = MODE_IEEE80211A;
+	else
+		priv->phymode = MODE_IEEE80211G;
+
+	iwl_set_flags_for_phymode(priv, priv->phymode);
+
+	priv->staging_rxon.ofdm_basic_rates =
+	    (IWL_OFDM_RATES_MASK >> IWL_FIRST_OFDM_RATE) & 0xFF;
+	priv->staging_rxon.cck_basic_rates =
+	    (IWL_CCK_RATES_MASK >> IWL_FIRST_CCK_RATE) & 0xF;
+
+	priv->staging_rxon.flags &= ~(RXON_FLG_CHANNEL_MODE_MIXED_MSK |
+					RXON_FLG_CHANNEL_MODE_PURE_40_MSK);
+	memcpy(priv->staging_rxon.node_addr, priv->mac_addr, ETH_ALEN);
+	memcpy(priv->staging_rxon.wlap_bssid_addr, priv->mac_addr, ETH_ALEN);
+	priv->staging_rxon.ofdm_ht_single_stream_basic_rates = 0xff;
+	priv->staging_rxon.ofdm_ht_dual_stream_basic_rates = 0xff;
+	iwl4965_set_rxon_chain(priv);
+}
+
+static int iwl_set_mode(struct iwl_priv *priv, int mode)
+{
+	if (!iwl_is_ready_rf(priv))
+		return -EAGAIN;
+
+	if (mode == IEEE80211_IF_TYPE_IBSS) {
+		const struct iwl_channel_info *ch_info;
+
+		ch_info = iwl_get_channel_info(priv,
+			priv->phymode,
+			le16_to_cpu(priv->staging_rxon.channel));
+
+		if (!ch_info || !is_channel_ibss(ch_info)) {
+			IWL_ERROR("channel %d not IBSS channel\n",
+				  le16_to_cpu(priv->staging_rxon.channel));
+			return -EINVAL;
+		}
+	}
+
+	cancel_delayed_work(&priv->scan_check);
+	if (iwl_scan_cancel_timeout(priv, 100)) {
+		IWL_WARNING("Aborted scan still in progress after 100ms\n");
+		IWL_DEBUG_MAC80211("leaving - scan abort failed.\n");
+		return -EAGAIN;
+	}
+
+	priv->iw_mode = mode;
+
+	iwl_connection_init_rx_config(priv);
+	memcpy(priv->staging_rxon.node_addr, priv->mac_addr, ETH_ALEN);
+
+	iwl_clear_stations_table(priv);
+
+	iwl_commit_rxon(priv);
+
+	return 0;
+}
+
+static void iwl_build_tx_cmd_hwcrypto(struct iwl_priv *priv,
+				      struct ieee80211_tx_control *ctl,
+				      struct iwl_cmd *cmd,
+				      struct sk_buff *skb_frag,
+				      int last_frag)
+{
+	struct iwl_hw_key *keyinfo = &priv->stations[ctl->key_idx].keyinfo;
+
+	switch (keyinfo->alg) {
+	case ALG_CCMP:
+		cmd->cmd.tx.sec_ctl = TX_CMD_SEC_CCM;
+		memcpy(cmd->cmd.tx.key, keyinfo->key, keyinfo->keylen);
+		IWL_DEBUG_TX("tx_cmd with aes hwcrypto\n");
+		break;
+
+	case ALG_TKIP:
+#if 0
+		cmd->cmd.tx.sec_ctl = TX_CMD_SEC_TKIP;
+
+		if (last_frag)
+			memcpy(cmd->cmd.tx.tkip_mic.byte, skb_frag->tail - 8,
+			       8);
+		else
+			memset(cmd->cmd.tx.tkip_mic.byte, 0, 8);
+#endif
+		break;
+
+	case ALG_WEP:
+		cmd->cmd.tx.sec_ctl = TX_CMD_SEC_WEP |
+			(ctl->key_idx & TX_CMD_SEC_MSK) << TX_CMD_SEC_SHIFT;
+
+		if (keyinfo->keylen == 13)
+			cmd->cmd.tx.sec_ctl |= TX_CMD_SEC_KEY128;
+
+		memcpy(&cmd->cmd.tx.key[3], keyinfo->key, keyinfo->keylen);
+
+		IWL_DEBUG_TX("Configuring packet for WEP encryption "
+			     "with key %d\n", ctl->key_idx);
+		break;
+
+	default:
+		printk(KERN_ERR "Unknown encode alg %d\n", keyinfo->alg);
+		break;
+	}
+}
+
+/*
+ * handle build REPLY_TX command notification.
+ */
+static void iwl_build_tx_cmd_basic(struct iwl_priv *priv,
+				  struct iwl_cmd *cmd,
+				  struct ieee80211_tx_control *ctrl,
+				  struct ieee80211_hdr *hdr,
+				  int is_unicast, u8 std_id)
+{
+	__le16 *qc;
+	u16 fc = le16_to_cpu(hdr->frame_control);
+	__le32 tx_flags = cmd->cmd.tx.tx_flags;
+
+	cmd->cmd.tx.stop_time.life_time = TX_CMD_LIFE_TIME_INFINITE;
+	if (!(ctrl->flags & IEEE80211_TXCTL_NO_ACK)) {
+		tx_flags |= TX_CMD_FLG_ACK_MSK;
+		if ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT)
+			tx_flags |= TX_CMD_FLG_SEQ_CTL_MSK;
+		if (ieee80211_is_probe_response(fc) &&
+		    !(le16_to_cpu(hdr->seq_ctrl) & 0xf))
+			tx_flags |= TX_CMD_FLG_TSF_MSK;
+	} else {
+		tx_flags &= (~TX_CMD_FLG_ACK_MSK);
+		tx_flags |= TX_CMD_FLG_SEQ_CTL_MSK;
+	}
+
+	cmd->cmd.tx.sta_id = std_id;
+	if (ieee80211_get_morefrag(hdr))
+		tx_flags |= TX_CMD_FLG_MORE_FRAG_MSK;
+
+	qc = ieee80211_get_qos_ctrl(hdr);
+	if (qc) {
+		cmd->cmd.tx.tid_tspec = (u8) (le16_to_cpu(*qc) & 0xf);
+		tx_flags &= ~TX_CMD_FLG_SEQ_CTL_MSK;
+	} else
+		tx_flags |= TX_CMD_FLG_SEQ_CTL_MSK;
+
+	if (ctrl->flags & IEEE80211_TXCTL_USE_RTS_CTS) {
+		tx_flags |= TX_CMD_FLG_RTS_MSK;
+		tx_flags &= ~TX_CMD_FLG_CTS_MSK;
+	} else if (ctrl->flags & IEEE80211_TXCTL_USE_CTS_PROTECT) {
+		tx_flags &= ~TX_CMD_FLG_RTS_MSK;
+		tx_flags |= TX_CMD_FLG_CTS_MSK;
+	}
+
+	if ((tx_flags & TX_CMD_FLG_RTS_MSK) || (tx_flags & TX_CMD_FLG_CTS_MSK))
+		tx_flags |= TX_CMD_FLG_FULL_TXOP_PROT_MSK;
+
+	tx_flags &= ~(TX_CMD_FLG_ANT_SEL_MSK);
+	if ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT) {
+		if ((fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_ASSOC_REQ ||
+		    (fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_REASSOC_REQ)
+			cmd->cmd.tx.timeout.pm_frame_timeout =
+				cpu_to_le16(3);
+		else
+			cmd->cmd.tx.timeout.pm_frame_timeout =
+				cpu_to_le16(2);
+	} else
+		cmd->cmd.tx.timeout.pm_frame_timeout = 0;
+
+	cmd->cmd.tx.driver_txop = 0;
+	cmd->cmd.tx.tx_flags = tx_flags;
+	cmd->cmd.tx.next_frame_len = 0;
+}
+
+static int iwl_get_sta_id(struct iwl_priv *priv, struct ieee80211_hdr *hdr)
+{
+	int sta_id;
+	u16 fc = le16_to_cpu(hdr->frame_control);
+	DECLARE_MAC_BUF(mac);
+
+	/* If this frame is broadcast or not data then use the broadcast
+	 * station id */
+	if (((fc & IEEE80211_FCTL_FTYPE) != IEEE80211_FTYPE_DATA) ||
+	    is_multicast_ether_addr(hdr->addr1))
+		return priv->hw_setting.bcast_sta_id;
+
+	switch (priv->iw_mode) {
+
+	/* If this frame is part of a BSS network (we're a station), then
+	 * we use the AP's station id */
+	case IEEE80211_IF_TYPE_STA:
+		return IWL_AP_ID;
+
+	/* If we are an AP, then find the station, or use BCAST */
+	case IEEE80211_IF_TYPE_AP:
+		sta_id = iwl_hw_find_station(priv, hdr->addr1);
+		if (sta_id != IWL_INVALID_STATION)
+			return sta_id;
+		return priv->hw_setting.bcast_sta_id;
+
+	/* If this frame is part of a IBSS network, then we use the
+	 * target specific station id */
+	case IEEE80211_IF_TYPE_IBSS:
+		sta_id = iwl_hw_find_station(priv, hdr->addr1);
+		if (sta_id != IWL_INVALID_STATION)
+			return sta_id;
+
+		sta_id = iwl_add_station(priv, hdr->addr1, 0, CMD_ASYNC);
+
+		if (sta_id != IWL_INVALID_STATION)
+			return sta_id;
+
+		IWL_DEBUG_DROP("Station %s not in station map. "
+			       "Defaulting to broadcast...\n",
+			       print_mac(mac, hdr->addr1));
+		iwl_print_hex_dump(IWL_DL_DROP, (u8 *) hdr, sizeof(*hdr));
+		return priv->hw_setting.bcast_sta_id;
+
+	default:
+		IWL_WARNING("Unkown mode of operation: %d", priv->iw_mode);
+		return priv->hw_setting.bcast_sta_id;
+	}
+}
+
+/*
+ * start REPLY_TX command process
+ */
+static int iwl_tx_skb(struct iwl_priv *priv,
+		      struct sk_buff *skb, struct ieee80211_tx_control *ctl)
+{
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
+	struct iwl_tfd_frame *tfd;
+	u32 *control_flags;
+	int txq_id = ctl->queue;
+	struct iwl_tx_queue *txq = NULL;
+	struct iwl_queue *q = NULL;
+	dma_addr_t phys_addr;
+	dma_addr_t txcmd_phys;
+	struct iwl_cmd *out_cmd = NULL;
+	u16 len, idx, len_org;
+	u8 id, hdr_len, unicast;
+	u8 sta_id;
+	u16 seq_number = 0;
+	u16 fc;
+	__le16 *qc;
+	u8 wait_write_ptr = 0;
+	unsigned long flags;
+	int rc;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	if (iwl_is_rfkill(priv)) {
+		IWL_DEBUG_DROP("Dropping - RF KILL\n");
+		goto drop_unlock;
+	}
+
+	if (!priv->interface_id) {
+		IWL_DEBUG_DROP("Dropping - !priv->interface_id\n");
+		goto drop_unlock;
+	}
+
+	if ((ctl->tx_rate & 0xFF) == IWL_INVALID_RATE) {
+		IWL_ERROR("ERROR: No TX rate available.\n");
+		goto drop_unlock;
+	}
+
+	unicast = !is_multicast_ether_addr(hdr->addr1);
+	id = 0;
+
+	fc = le16_to_cpu(hdr->frame_control);
+
+#ifdef CONFIG_IWLWIFI_DEBUG
+	if (ieee80211_is_auth(fc))
+		IWL_DEBUG_TX("Sending AUTH frame\n");
+	else if (ieee80211_is_assoc_request(fc))
+		IWL_DEBUG_TX("Sending ASSOC frame\n");
+	else if (ieee80211_is_reassoc_request(fc))
+		IWL_DEBUG_TX("Sending REASSOC frame\n");
+#endif
+
+	if (!iwl_is_associated(priv) &&
+	    ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_DATA)) {
+		IWL_DEBUG_DROP("Dropping - !iwl_is_associated\n");
+		goto drop_unlock;
+	}
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	hdr_len = ieee80211_get_hdrlen(fc);
+	sta_id = iwl_get_sta_id(priv, hdr);
+	if (sta_id == IWL_INVALID_STATION) {
+		DECLARE_MAC_BUF(mac);
+
+		IWL_DEBUG_DROP("Dropping - INVALID STATION: %s\n",
+			       print_mac(mac, hdr->addr1));
+		goto drop;
+	}
+
+	IWL_DEBUG_RATE("station Id %d\n", sta_id);
+
+	qc = ieee80211_get_qos_ctrl(hdr);
+	if (qc) {
+		u8 tid = (u8)(le16_to_cpu(*qc) & 0xf);
+		seq_number = priv->stations[sta_id].tid[tid].seq_number &
+				IEEE80211_SCTL_SEQ;
+		hdr->seq_ctrl = cpu_to_le16(seq_number) |
+			(hdr->seq_ctrl &
+				__constant_cpu_to_le16(IEEE80211_SCTL_FRAG));
+		seq_number += 0x10;
+#ifdef CONFIG_IWLWIFI_HT
+#ifdef CONFIG_IWLWIFI_HT_AGG
+		/* aggregation is on for this <sta,tid> */
+		if (ctl->flags & IEEE80211_TXCTL_HT_MPDU_AGG)
+			txq_id = priv->stations[sta_id].tid[tid].agg.txq_id;
+#endif /* CONFIG_IWLWIFI_HT_AGG */
+#endif /* CONFIG_IWLWIFI_HT */
+	}
+	txq = &priv->txq[txq_id];
+	q = &txq->q;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	tfd = &txq->bd[q->first_empty];
+	memset(tfd, 0, sizeof(*tfd));
+	control_flags = (u32 *) tfd;
+	idx = get_cmd_index(q, q->first_empty, 0);
+
+	memset(&(txq->txb[q->first_empty]), 0, sizeof(struct iwl_tx_info));
+	txq->txb[q->first_empty].skb[0] = skb;
+	memcpy(&(txq->txb[q->first_empty].status.control),
+	       ctl, sizeof(struct ieee80211_tx_control));
+	out_cmd = &txq->cmd[idx];
+	memset(&out_cmd->hdr, 0, sizeof(out_cmd->hdr));
+	memset(&out_cmd->cmd.tx, 0, sizeof(out_cmd->cmd.tx));
+	out_cmd->hdr.cmd = REPLY_TX;
+	out_cmd->hdr.sequence = cpu_to_le16((u16)(QUEUE_TO_SEQ(txq_id) |
+				INDEX_TO_SEQ(q->first_empty)));
+	/* copy frags header */
+	memcpy(out_cmd->cmd.tx.hdr, hdr, hdr_len);
+
+	/* hdr = (struct ieee80211_hdr *)out_cmd->cmd.tx.hdr; */
+	len = priv->hw_setting.tx_cmd_len +
+		sizeof(struct iwl_cmd_header) + hdr_len;
+
+	len_org = len;
+	len = (len + 3) & ~3;
+
+	if (len_org != len)
+		len_org = 1;
+	else
+		len_org = 0;
+
+	txcmd_phys = txq->dma_addr_cmd + sizeof(struct iwl_cmd) * idx +
+		     offsetof(struct iwl_cmd, hdr);
+
+	iwl_hw_txq_attach_buf_to_tfd(priv, tfd, txcmd_phys, len);
+
+	if (!(ctl->flags & IEEE80211_TXCTL_DO_NOT_ENCRYPT))
+		iwl_build_tx_cmd_hwcrypto(priv, ctl, out_cmd, skb, 0);
+
+	/* 802.11 null functions have no payload... */
+	len = skb->len - hdr_len;
+	if (len) {
+		phys_addr = pci_map_single(priv->pci_dev, skb->data + hdr_len,
+					   len, PCI_DMA_TODEVICE);
+		iwl_hw_txq_attach_buf_to_tfd(priv, tfd, phys_addr, len);
+	}
+
+	if (len_org)
+		out_cmd->cmd.tx.tx_flags |= TX_CMD_FLG_MH_PAD_MSK;
+
+	len = (u16)skb->len;
+	out_cmd->cmd.tx.len = cpu_to_le16(len);
+
+	/* TODO need this for burst mode later on */
+	iwl_build_tx_cmd_basic(priv, out_cmd, ctl, hdr, unicast, sta_id);
+
+	/* set is_hcca to 0; it probably will never be implemented */
+	iwl_hw_build_tx_cmd_rate(priv, out_cmd, ctl, hdr, sta_id, 0);
+
+	iwl4965_tx_cmd(priv, out_cmd, sta_id, txcmd_phys,
+		       hdr, hdr_len, ctl, NULL);
+
+	if (!ieee80211_get_morefrag(hdr)) {
+		txq->need_update = 1;
+		if (qc) {
+			u8 tid = (u8)(le16_to_cpu(*qc) & 0xf);
+			priv->stations[sta_id].tid[tid].seq_number = seq_number;
+		}
+	} else {
+		wait_write_ptr = 1;
+		txq->need_update = 0;
+	}
+
+	iwl_print_hex_dump(IWL_DL_TX, out_cmd->cmd.payload,
+			   sizeof(out_cmd->cmd.tx));
+
+	iwl_print_hex_dump(IWL_DL_TX, (u8 *)out_cmd->cmd.tx.hdr,
+			   ieee80211_get_hdrlen(fc));
+
+	iwl4965_tx_queue_update_wr_ptr(priv, txq, len);
+
+	q->first_empty = iwl_queue_inc_wrap(q->first_empty, q->n_bd);
+	rc = iwl_tx_queue_update_write_ptr(priv, txq);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	if (rc)
+		return rc;
+
+	if ((iwl_queue_space(q) < q->high_mark)
+	    && priv->mac80211_registered) {
+		if (wait_write_ptr) {
+			spin_lock_irqsave(&priv->lock, flags);
+			txq->need_update = 1;
+			iwl_tx_queue_update_write_ptr(priv, txq);
+			spin_unlock_irqrestore(&priv->lock, flags);
+		}
+
+		ieee80211_stop_queue(priv->hw, ctl->queue);
+	}
+
+	return 0;
+
+drop_unlock:
+	spin_unlock_irqrestore(&priv->lock, flags);
+drop:
+	return -1;
+}
+
+static void iwl_set_rate(struct iwl_priv *priv)
+{
+	const struct ieee80211_hw_mode *hw = NULL;
+	struct ieee80211_rate *rate;
+	int i;
+
+	hw = iwl_get_hw_mode(priv, priv->phymode);
+
+	priv->active_rate = 0;
+	priv->active_rate_basic = 0;
+
+	IWL_DEBUG_RATE("Setting rates for 802.11%c\n",
+		       hw->mode == MODE_IEEE80211A ?
+		       'a' : ((hw->mode == MODE_IEEE80211B) ? 'b' : 'g'));
+
+	for (i = 0; i < hw->num_rates; i++) {
+		rate = &(hw->rates[i]);
+		if ((rate->val < IWL_RATE_COUNT) &&
+		    (rate->flags & IEEE80211_RATE_SUPPORTED)) {
+			IWL_DEBUG_RATE("Adding rate index %d (plcp %d)%s\n",
+				       rate->val, iwl_rates[rate->val].plcp,
+				       (rate->flags & IEEE80211_RATE_BASIC) ?
+				       "*" : "");
+			priv->active_rate |= (1 << rate->val);
+			if (rate->flags & IEEE80211_RATE_BASIC)
+				priv->active_rate_basic |= (1 << rate->val);
+		} else
+			IWL_DEBUG_RATE("Not adding rate %d (plcp %d)\n",
+				       rate->val, iwl_rates[rate->val].plcp);
+	}
+
+	IWL_DEBUG_RATE("Set active_rate = %0x, active_rate_basic = %0x\n",
+		       priv->active_rate, priv->active_rate_basic);
+
+	/*
+	 * If a basic rate is configured, then use it (adding IWL_RATE_1M_MASK)
+	 * otherwise set it to the default of all CCK rates and 6, 12, 24 for
+	 * OFDM
+	 */
+	if (priv->active_rate_basic & IWL_CCK_BASIC_RATES_MASK)
+		priv->staging_rxon.cck_basic_rates =
+		    ((priv->active_rate_basic &
+		      IWL_CCK_RATES_MASK) >> IWL_FIRST_CCK_RATE) & 0xF;
+	else
+		priv->staging_rxon.cck_basic_rates =
+		    (IWL_CCK_BASIC_RATES_MASK >> IWL_FIRST_CCK_RATE) & 0xF;
+
+	if (priv->active_rate_basic & IWL_OFDM_BASIC_RATES_MASK)
+		priv->staging_rxon.ofdm_basic_rates =
+		    ((priv->active_rate_basic &
+		      (IWL_OFDM_BASIC_RATES_MASK | IWL_RATE_6M_MASK)) >>
+		      IWL_FIRST_OFDM_RATE) & 0xFF;
+	else
+		priv->staging_rxon.ofdm_basic_rates =
+		   (IWL_OFDM_BASIC_RATES_MASK >> IWL_FIRST_OFDM_RATE) & 0xFF;
+}
+
+static void iwl_radio_kill_sw(struct iwl_priv *priv, int disable_radio)
+{
+	unsigned long flags;
+
+	if (!!disable_radio == test_bit(STATUS_RF_KILL_SW, &priv->status))
+		return;
+
+	IWL_DEBUG_RF_KILL("Manual SW RF KILL set to: RADIO %s\n",
+			  disable_radio ? "OFF" : "ON");
+
+	if (disable_radio) {
+		iwl_scan_cancel(priv);
+		/* FIXME: This is a workaround for AP */
+		if (priv->iw_mode != IEEE80211_IF_TYPE_AP) {
+			spin_lock_irqsave(&priv->lock, flags);
+			iwl_write32(priv, CSR_UCODE_DRV_GP1_SET,
+				    CSR_UCODE_SW_BIT_RFKILL);
+			spin_unlock_irqrestore(&priv->lock, flags);
+			iwl_send_card_state(priv, CARD_STATE_CMD_DISABLE, 0);
+			set_bit(STATUS_RF_KILL_SW, &priv->status);
+		}
+		return;
+	}
+
+	spin_lock_irqsave(&priv->lock, flags);
+	iwl_write32(priv, CSR_UCODE_DRV_GP1_CLR, CSR_UCODE_SW_BIT_RFKILL);
+
+	clear_bit(STATUS_RF_KILL_SW, &priv->status);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	/* wake up ucode */
+	msleep(10);
+
+	spin_lock_irqsave(&priv->lock, flags);
+	iwl_read32(priv, CSR_UCODE_DRV_GP1);
+	if (!iwl_grab_restricted_access(priv))
+		iwl_release_restricted_access(priv);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	if (test_bit(STATUS_RF_KILL_HW, &priv->status)) {
+		IWL_DEBUG_RF_KILL("Can not turn radio back on - "
+				  "disabled by HW switch\n");
+		return;
+	}
+
+	queue_work(priv->workqueue, &priv->restart);
+	return;
+}
+
+void iwl_set_decrypted_flag(struct iwl_priv *priv, struct sk_buff *skb,
+			    u32 decrypt_res, struct ieee80211_rx_status *stats)
+{
+	u16 fc =
+	    le16_to_cpu(((struct ieee80211_hdr *)skb->data)->frame_control);
+
+	if (priv->active_rxon.filter_flags & RXON_FILTER_DIS_DECRYPT_MSK)
+		return;
+
+	if (!(fc & IEEE80211_FCTL_PROTECTED))
+		return;
+
+	IWL_DEBUG_RX("decrypt_res:0x%x\n", decrypt_res);
+	switch (decrypt_res & RX_RES_STATUS_SEC_TYPE_MSK) {
+	case RX_RES_STATUS_SEC_TYPE_TKIP:
+		if ((decrypt_res & RX_RES_STATUS_DECRYPT_TYPE_MSK) ==
+		    RX_RES_STATUS_BAD_ICV_MIC)
+			stats->flag |= RX_FLAG_MMIC_ERROR;
+	case RX_RES_STATUS_SEC_TYPE_WEP:
+	case RX_RES_STATUS_SEC_TYPE_CCMP:
+		if ((decrypt_res & RX_RES_STATUS_DECRYPT_TYPE_MSK) ==
+		    RX_RES_STATUS_DECRYPT_OK) {
+			IWL_DEBUG_RX("hw decrypt successfully!!!\n");
+			stats->flag |= RX_FLAG_DECRYPTED;
+		}
+		break;
+
+	default:
+		break;
+	}
+}
+
+void iwl_handle_data_packet_monitor(struct iwl_priv *priv,
+				    struct iwl_rx_mem_buffer *rxb,
+				    void *data, short len,
+				    struct ieee80211_rx_status *stats,
+				    u16 phy_flags)
+{
+	struct iwl_rt_rx_hdr *iwl_rt;
+
+	/* First cache any information we need before we overwrite
+	 * the information provided in the skb from the hardware */
+	s8 signal = stats->ssi;
+	s8 noise = 0;
+	int rate = stats->rate;
+	u64 tsf = stats->mactime;
+	__le16 phy_flags_hw = cpu_to_le16(phy_flags);
+
+	/* We received data from the HW, so stop the watchdog */
+	if (len > IWL_RX_BUF_SIZE - sizeof(*iwl_rt)) {
+		IWL_DEBUG_DROP("Dropping too large packet in monitor\n");
+		return;
+	}
+
+	/* copy the frame data to write after where the radiotap header goes */
+	iwl_rt = (void *)rxb->skb->data;
+	memmove(iwl_rt->payload, data, len);
+
+	iwl_rt->rt_hdr.it_version = PKTHDR_RADIOTAP_VERSION;
+	iwl_rt->rt_hdr.it_pad = 0; /* always good to zero */
+
+	/* total header + data */
+	iwl_rt->rt_hdr.it_len = cpu_to_le16(sizeof(*iwl_rt));
+
+	/* Set the size of the skb to the size of the frame */
+	skb_put(rxb->skb, sizeof(*iwl_rt) + len);
+
+	/* Big bitfield of all the fields we provide in radiotap */
+	iwl_rt->rt_hdr.it_present =
+	    cpu_to_le32((1 << IEEE80211_RADIOTAP_TSFT) |
+			(1 << IEEE80211_RADIOTAP_FLAGS) |
+			(1 << IEEE80211_RADIOTAP_RATE) |
+			(1 << IEEE80211_RADIOTAP_CHANNEL) |
+			(1 << IEEE80211_RADIOTAP_DBM_ANTSIGNAL) |
+			(1 << IEEE80211_RADIOTAP_DBM_ANTNOISE) |
+			(1 << IEEE80211_RADIOTAP_ANTENNA));
+
+	/* Zero the flags, we'll add to them as we go */
+	iwl_rt->rt_flags = 0;
+
+	iwl_rt->rt_tsf = cpu_to_le64(tsf);
+
+	/* Convert to dBm */
+	iwl_rt->rt_dbmsignal = signal;
+	iwl_rt->rt_dbmnoise = noise;
+
+	/* Convert the channel frequency and set the flags */
+	iwl_rt->rt_channelMHz = cpu_to_le16(stats->freq);
+	if (!(phy_flags_hw & RX_RES_PHY_FLAGS_BAND_24_MSK))
+		iwl_rt->rt_chbitmask =
+		    cpu_to_le16((IEEE80211_CHAN_OFDM | IEEE80211_CHAN_5GHZ));
+	else if (phy_flags_hw & RX_RES_PHY_FLAGS_MOD_CCK_MSK)
+		iwl_rt->rt_chbitmask =
+		    cpu_to_le16((IEEE80211_CHAN_CCK | IEEE80211_CHAN_2GHZ));
+	else	/* 802.11g */
+		iwl_rt->rt_chbitmask =
+		    cpu_to_le16((IEEE80211_CHAN_OFDM | IEEE80211_CHAN_2GHZ));
+
+	rate = iwl_rate_index_from_plcp(rate);
+	if (rate == -1)
+		iwl_rt->rt_rate = 0;
+	else
+		iwl_rt->rt_rate = iwl_rates[rate].ieee;
+
+	/* antenna number */
+	iwl_rt->rt_antenna =
+		le16_to_cpu(phy_flags_hw & RX_RES_PHY_FLAGS_ANTENNA_MSK) >> 4;
+
+	/* set the preamble flag if we have it */
+	if (phy_flags_hw & RX_RES_PHY_FLAGS_SHORT_PREAMBLE_MSK)
+		iwl_rt->rt_flags |= IEEE80211_RADIOTAP_F_SHORTPRE;
+
+	IWL_DEBUG_RX("Rx packet of %d bytes.\n", rxb->skb->len);
+
+	stats->flag |= RX_FLAG_RADIOTAP;
+	ieee80211_rx_irqsafe(priv->hw, rxb->skb, stats);
+	rxb->skb = NULL;
+}
+
+
+#define IWL_PACKET_RETRY_TIME HZ
+
+int is_duplicate_packet(struct iwl_priv *priv, struct ieee80211_hdr *header)
+{
+	u16 sc = le16_to_cpu(header->seq_ctrl);
+	u16 seq = (sc & IEEE80211_SCTL_SEQ) >> 4;
+	u16 frag = sc & IEEE80211_SCTL_FRAG;
+	u16 *last_seq, *last_frag;
+	unsigned long *last_time;
+
+	switch (priv->iw_mode) {
+	case IEEE80211_IF_TYPE_IBSS:{
+		struct list_head *p;
+		struct iwl_ibss_seq *entry = NULL;
+		u8 *mac = header->addr2;
+		int index = mac[5] & (IWL_IBSS_MAC_HASH_SIZE - 1);
+
+		__list_for_each(p, &priv->ibss_mac_hash[index]) {
+			entry =
+				list_entry(p, struct iwl_ibss_seq, list);
+			if (!compare_ether_addr(entry->mac, mac))
+				break;
+		}
+		if (p == &priv->ibss_mac_hash[index]) {
+			entry = kzalloc(sizeof(*entry), GFP_ATOMIC);
+			if (!entry) {
+				IWL_ERROR
+					("Cannot malloc new mac entry\n");
+				return 0;
+			}
+			memcpy(entry->mac, mac, ETH_ALEN);
+			entry->seq_num = seq;
+			entry->frag_num = frag;
+			entry->packet_time = jiffies;
+			list_add(&entry->list,
+				 &priv->ibss_mac_hash[index]);
+			return 0;
+		}
+		last_seq = &entry->seq_num;
+		last_frag = &entry->frag_num;
+		last_time = &entry->packet_time;
+		break;
+	}
+	case IEEE80211_IF_TYPE_STA:
+		last_seq = &priv->last_seq_num;
+		last_frag = &priv->last_frag_num;
+		last_time = &priv->last_packet_time;
+		break;
+	default:
+		return 0;
+	}
+	if ((*last_seq == seq) &&
+	    time_after(*last_time + IWL_PACKET_RETRY_TIME, jiffies)) {
+		if (*last_frag == frag)
+			goto drop;
+		if (*last_frag + 1 != frag)
+			/* out-of-order fragment */
+			goto drop;
+	} else
+		*last_seq = seq;
+
+	*last_frag = frag;
+	*last_time = jiffies;
+	return 0;
+
+ drop:
+	return 1;
+}
+
+#ifdef CONFIG_IWLWIFI_SPECTRUM_MEASUREMENT
+
+#include "iwl-spectrum.h"
+
+#define BEACON_TIME_MASK_LOW	0x00FFFFFF
+#define BEACON_TIME_MASK_HIGH	0xFF000000
+#define TIME_UNIT		1024
+
+/*
+ * extended beacon time format
+ * time in usec will be changed into a 32-bit value in 8:24 format
+ * the high 1 byte is the beacon counts
+ * the lower 3 bytes is the time in usec within one beacon interval
+ */
+
+static u32 iwl_usecs_to_beacons(u32 usec, u32 beacon_interval)
+{
+	u32 quot;
+	u32 rem;
+	u32 interval = beacon_interval * 1024;
+
+	if (!interval || !usec)
+		return 0;
+
+	quot = (usec / interval) & (BEACON_TIME_MASK_HIGH >> 24);
+	rem = (usec % interval) & BEACON_TIME_MASK_LOW;
+
+	return (quot << 24) + rem;
+}
+
+/* base is usually what we get from ucode with each received frame,
+ * the same as HW timer counter counting down
+ */
+
+static __le32 iwl_add_beacon_time(u32 base, u32 addon, u32 beacon_interval)
+{
+	u32 base_low = base & BEACON_TIME_MASK_LOW;
+	u32 addon_low = addon & BEACON_TIME_MASK_LOW;
+	u32 interval = beacon_interval * TIME_UNIT;
+	u32 res = (base & BEACON_TIME_MASK_HIGH) +
+	    (addon & BEACON_TIME_MASK_HIGH);
+
+	if (base_low > addon_low)
+		res += base_low - addon_low;
+	else if (base_low < addon_low) {
+		res += interval + base_low - addon_low;
+		res += (1 << 24);
+	} else
+		res += (1 << 24);
+
+	return cpu_to_le32(res);
+}
+
+static int iwl_get_measurement(struct iwl_priv *priv,
+			       struct ieee80211_measurement_params *params,
+			       u8 type)
+{
+	struct iwl_spectrum_cmd spectrum;
+	struct iwl_rx_packet *res;
+	struct iwl_host_cmd cmd = {
+		.id = REPLY_SPECTRUM_MEASUREMENT_CMD,
+		.data = (void *)&spectrum,
+		.meta.flags = CMD_WANT_SKB,
+	};
+	u32 add_time = le64_to_cpu(params->start_time);
+	int rc;
+	int spectrum_resp_status;
+	int duration = le16_to_cpu(params->duration);
+
+	if (iwl_is_associated(priv))
+		add_time =
+		    iwl_usecs_to_beacons(
+			le64_to_cpu(params->start_time) - priv->last_tsf,
+			le16_to_cpu(priv->rxon_timing.beacon_interval));
+
+	memset(&spectrum, 0, sizeof(spectrum));
+
+	spectrum.channel_count = cpu_to_le16(1);
+	spectrum.flags =
+	    RXON_FLG_TSF2HOST_MSK | RXON_FLG_ANT_A_MSK | RXON_FLG_DIS_DIV_MSK;
+	spectrum.filter_flags = MEASUREMENT_FILTER_FLAG;
+	cmd.len = sizeof(spectrum);
+	spectrum.len = cpu_to_le16(cmd.len - sizeof(spectrum.len));
+
+	if (iwl_is_associated(priv))
+		spectrum.start_time =
+		    iwl_add_beacon_time(priv->last_beacon_time,
+				add_time,
+				le16_to_cpu(priv->rxon_timing.beacon_interval));
+	else
+		spectrum.start_time = 0;
+
+	spectrum.channels[0].duration = cpu_to_le32(duration * TIME_UNIT);
+	spectrum.channels[0].channel = params->channel;
+	spectrum.channels[0].type = type;
+	if (priv->active_rxon.flags & RXON_FLG_BAND_24G_MSK)
+		spectrum.flags |= RXON_FLG_BAND_24G_MSK |
+		    RXON_FLG_AUTO_DETECT_MSK | RXON_FLG_TGG_PROTECT_MSK;
+
+	rc = iwl_send_cmd_sync(priv, &cmd);
+	if (rc)
+		return rc;
+
+	res = (struct iwl_rx_packet *)cmd.meta.u.skb->data;
+	if (res->hdr.flags & IWL_CMD_FAILED_MSK) {
+		IWL_ERROR("Bad return from REPLY_RX_ON_ASSOC command\n");
+		rc = -EIO;
+	}
+
+	spectrum_resp_status = le16_to_cpu(res->u.spectrum.status);
+	switch (spectrum_resp_status) {
+	case 0:		/* Command will be handled */
+		if (res->u.spectrum.id != 0xff) {
+			IWL_DEBUG_INFO
+			    ("Replaced existing measurement: %d\n",
+			     res->u.spectrum.id);
+			priv->measurement_status &= ~MEASUREMENT_READY;
+		}
+		priv->measurement_status |= MEASUREMENT_ACTIVE;
+		rc = 0;
+		break;
+
+	case 1:		/* Command will not be handled */
+		rc = -EAGAIN;
+		break;
+	}
+
+	dev_kfree_skb_any(cmd.meta.u.skb);
+
+	return rc;
+}
+#endif
+
+static void iwl_txstatus_to_ieee(struct iwl_priv *priv,
+				 struct iwl_tx_info *tx_sta)
+{
+
+	tx_sta->status.ack_signal = 0;
+	tx_sta->status.excessive_retries = 0;
+	tx_sta->status.queue_length = 0;
+	tx_sta->status.queue_number = 0;
+
+	if (in_interrupt())
+		ieee80211_tx_status_irqsafe(priv->hw,
+					    tx_sta->skb[0], &(tx_sta->status));
+	else
+		ieee80211_tx_status(priv->hw,
+				    tx_sta->skb[0], &(tx_sta->status));
+
+	tx_sta->skb[0] = NULL;
+}
+
+/**
+ * iwl_tx_queue_reclaim - Reclaim Tx queue entries no more used by NIC.
+ *
+ * When FW advances 'R' index, all entries between old and
+ * new 'R' index need to be reclaimed. As result, some free space
+ * forms. If there is enough free space (> low mark), wake Tx queue.
+ */
+int iwl_tx_queue_reclaim(struct iwl_priv *priv, int txq_id, int index)
+{
+	struct iwl_tx_queue *txq = &priv->txq[txq_id];
+	struct iwl_queue *q = &txq->q;
+	int nfreed = 0;
+
+	if ((index >= q->n_bd) || (x2_queue_used(q, index) == 0)) {
+		IWL_ERROR("Read index for DMA queue txq id (%d), index %d, "
+			  "is out of range [0-%d] %d %d.\n", txq_id,
+			  index, q->n_bd, q->first_empty, q->last_used);
+		return 0;
+	}
+
+	for (index = iwl_queue_inc_wrap(index, q->n_bd);
+		q->last_used != index;
+		q->last_used = iwl_queue_inc_wrap(q->last_used, q->n_bd)) {
+		if (txq_id != IWL_CMD_QUEUE_NUM) {
+			iwl_txstatus_to_ieee(priv,
+					&(txq->txb[txq->q.last_used]));
+			iwl_hw_txq_free_tfd(priv, txq);
+		} else if (nfreed > 1) {
+			IWL_ERROR("HCMD skipped: index (%d) %d %d\n", index,
+					q->first_empty, q->last_used);
+			queue_work(priv->workqueue, &priv->restart);
+		}
+		nfreed++;
+	}
+
+	if (iwl_queue_space(q) > q->low_mark && (txq_id >= 0) &&
+			(txq_id != IWL_CMD_QUEUE_NUM) &&
+			priv->mac80211_registered)
+		ieee80211_wake_queue(priv->hw, txq_id);
+
+
+	return nfreed;
+}
+
+static int iwl_is_tx_success(u32 status)
+{
+	status &= TX_STATUS_MSK;
+	return (status == TX_STATUS_SUCCESS)
+	    || (status == TX_STATUS_DIRECT_DONE);
+}
+
+/******************************************************************************
+ *
+ * Generic RX handler implementations
+ *
+ ******************************************************************************/
+#ifdef CONFIG_IWLWIFI_HT
+#ifdef CONFIG_IWLWIFI_HT_AGG
+
+static inline int iwl_get_ra_sta_id(struct iwl_priv *priv,
+				    struct ieee80211_hdr *hdr)
+{
+	if (priv->iw_mode == IEEE80211_IF_TYPE_STA)
+		return IWL_AP_ID;
+	else {
+		u8 *da = ieee80211_get_DA(hdr);
+		return iwl_hw_find_station(priv, da);
+	}
+}
+
+static struct ieee80211_hdr *iwl_tx_queue_get_hdr(
+	struct iwl_priv *priv, int txq_id, int idx)
+{
+	if (priv->txq[txq_id].txb[idx].skb[0])
+		return (struct ieee80211_hdr *)priv->txq[txq_id].
+				txb[idx].skb[0]->data;
+	return NULL;
+}
+
+static inline u32 iwl_get_scd_ssn(struct iwl_tx_resp *tx_resp)
+{
+	__le32 *scd_ssn = (__le32 *)((u32 *)&tx_resp->status +
+				tx_resp->frame_count);
+	return le32_to_cpu(*scd_ssn) & MAX_SN;
+
+}
+static int iwl4965_tx_status_reply_tx(struct iwl_priv *priv,
+				      struct iwl_ht_agg *agg,
+				      struct iwl_tx_resp *tx_resp,
+				      u16 start_idx)
+{
+	u32 status;
+	__le32 *frame_status = &tx_resp->status;
+	struct ieee80211_tx_status *tx_status = NULL;
+	struct ieee80211_hdr *hdr = NULL;
+	int i, sh;
+	int txq_id, idx;
+	u16 seq;
+
+	if (agg->wait_for_ba)
+		IWL_DEBUG_TX_REPLY("got tx repsons w/o back\n");
+
+	agg->frame_count = tx_resp->frame_count;
+	agg->start_idx = start_idx;
+	agg->rate_n_flags = le32_to_cpu(tx_resp->rate_n_flags);
+	agg->bitmap0 = agg->bitmap1 = 0;
+
+	if (agg->frame_count == 1) {
+		struct iwl_tx_queue *txq ;
+		status = le32_to_cpu(frame_status[0]);
+
+		txq_id = agg->txq_id;
+		txq = &priv->txq[txq_id];
+		/* FIXME: code repetition */
+		IWL_DEBUG_TX_REPLY("FrameCnt = %d, StartIdx=%d \n",
+				   agg->frame_count, agg->start_idx);
+
+		tx_status = &(priv->txq[txq_id].txb[txq->q.last_used].status);
+		tx_status->retry_count = tx_resp->failure_frame;
+		tx_status->queue_number = status & 0xff;
+		tx_status->queue_length = tx_resp->bt_kill_count;
+		tx_status->queue_length |= tx_resp->failure_rts;
+
+		tx_status->flags = iwl_is_tx_success(status)?
+			IEEE80211_TX_STATUS_ACK : 0;
+		tx_status->control.tx_rate =
+				iwl_hw_get_rate_n_flags(tx_resp->rate_n_flags);
+		/* FIXME: code repetition end */
+
+		IWL_DEBUG_TX_REPLY("1 Frame 0x%x failure :%d\n",
+				    status & 0xff, tx_resp->failure_frame);
+		IWL_DEBUG_TX_REPLY("Rate Info rate_n_flags=%x\n",
+				iwl_hw_get_rate_n_flags(tx_resp->rate_n_flags));
+
+		agg->wait_for_ba = 0;
+	} else {
+		u64 bitmap = 0;
+		int start = agg->start_idx;
+
+		for (i = 0; i < agg->frame_count; i++) {
+			u16 sc;
+			status = le32_to_cpu(frame_status[i]);
+			seq  = status >> 16;
+			idx = SEQ_TO_INDEX(seq);
+			txq_id = SEQ_TO_QUEUE(seq);
+
+			if (status & (AGG_TX_STATE_FEW_BYTES_MSK |
+				      AGG_TX_STATE_ABORT_MSK))
+				continue;
+
+			IWL_DEBUG_TX_REPLY("FrameCnt = %d, txq_id=%d idx=%d\n",
+					   agg->frame_count, txq_id, idx);
+
+			hdr = iwl_tx_queue_get_hdr(priv, txq_id, idx);
+
+			sc = le16_to_cpu(hdr->seq_ctrl);
+			if (idx != (SEQ_TO_SN(sc) & 0xff)) {
+				IWL_ERROR("BUG_ON idx doesn't match seq control"
+					  " idx=%d, seq_idx=%d, seq=%d\n",
+					  idx, SEQ_TO_SN(sc),
+					  hdr->seq_ctrl);
+				return -1;
+			}
+
+			IWL_DEBUG_TX_REPLY("AGG Frame i=%d idx %d seq=%d\n",
+					   i, idx, SEQ_TO_SN(sc));
+
+			sh = idx - start;
+			if (sh > 64) {
+				sh = (start - idx) + 0xff;
+				bitmap = bitmap << sh;
+				sh = 0;
+				start = idx;
+			} else if (sh < -64)
+				sh  = 0xff - (start - idx);
+			else if (sh < 0) {
+				sh = start - idx;
+				start = idx;
+				bitmap = bitmap << sh;
+				sh = 0;
+			}
+			bitmap |= (1 << sh);
+			IWL_DEBUG_TX_REPLY("start=%d bitmap=0x%x\n",
+					   start, (u32)(bitmap & 0xFFFFFFFF));
+		}
+
+		agg->bitmap0 = bitmap & 0xFFFFFFFF;
+		agg->bitmap1 = bitmap >> 32;
+		agg->start_idx = start;
+		agg->rate_n_flags = le32_to_cpu(tx_resp->rate_n_flags);
+		IWL_DEBUG_TX_REPLY("Frames %d start_idx=%d bitmap=0x%x\n",
+				   agg->frame_count, agg->start_idx,
+				   agg->bitmap0);
+
+		if (bitmap)
+			agg->wait_for_ba = 1;
+	}
+	return 0;
+}
+#endif
+#endif
+
+static void iwl_rx_reply_tx(struct iwl_priv *priv,
+			    struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = (void *)rxb->skb->data;
+	u16 sequence = le16_to_cpu(pkt->hdr.sequence);
+	int txq_id = SEQ_TO_QUEUE(sequence);
+	int index = SEQ_TO_INDEX(sequence);
+	struct iwl_tx_queue *txq = &priv->txq[txq_id];
+	struct ieee80211_tx_status *tx_status;
+	struct iwl_tx_resp *tx_resp = (void *)&pkt->u.raw[0];
+	u32  status = le32_to_cpu(tx_resp->status);
+#ifdef CONFIG_IWLWIFI_HT
+#ifdef CONFIG_IWLWIFI_HT_AGG
+	int tid, sta_id;
+#endif
+#endif
+
+	if ((index >= txq->q.n_bd) || (x2_queue_used(&txq->q, index) == 0)) {
+		IWL_ERROR("Read index for DMA queue txq_id (%d) index %d "
+			  "is out of range [0-%d] %d %d\n", txq_id,
+			  index, txq->q.n_bd, txq->q.first_empty,
+			  txq->q.last_used);
+		return;
+	}
+
+#ifdef CONFIG_IWLWIFI_HT
+#ifdef CONFIG_IWLWIFI_HT_AGG
+	if (txq->sched_retry) {
+		const u32 scd_ssn = iwl_get_scd_ssn(tx_resp);
+		struct ieee80211_hdr *hdr =
+			iwl_tx_queue_get_hdr(priv, txq_id, index);
+		struct iwl_ht_agg *agg = NULL;
+		__le16 *qc = ieee80211_get_qos_ctrl(hdr);
+
+		if (qc == NULL) {
+			IWL_ERROR("BUG_ON qc is null!!!!\n");
+			return;
+		}
+
+		tid = le16_to_cpu(*qc) & 0xf;
+
+		sta_id = iwl_get_ra_sta_id(priv, hdr);
+		if (unlikely(sta_id == IWL_INVALID_STATION)) {
+			IWL_ERROR("Station not known for\n");
+			return;
+		}
+
+		agg = &priv->stations[sta_id].tid[tid].agg;
+
+		iwl4965_tx_status_reply_tx(priv, agg, tx_resp, index);
+
+		if ((tx_resp->frame_count == 1) &&
+		    !iwl_is_tx_success(status)) {
+			/* TODO: send BAR */
+		}
+
+		if ((txq->q.last_used != (scd_ssn & 0xff))) {
+			index = iwl_queue_dec_wrap(scd_ssn & 0xff, txq->q.n_bd);
+			IWL_DEBUG_TX_REPLY("Retry scheduler reclaim scd_ssn "
+					   "%d index %d\n", scd_ssn , index);
+			iwl_tx_queue_reclaim(priv, txq_id, index);
+		}
+	} else {
+#endif /* CONFIG_IWLWIFI_HT_AGG */
+#endif /* CONFIG_IWLWIFI_HT */
+	tx_status = &(txq->txb[txq->q.last_used].status);
+
+	tx_status->retry_count = tx_resp->failure_frame;
+	tx_status->queue_number = status;
+	tx_status->queue_length = tx_resp->bt_kill_count;
+	tx_status->queue_length |= tx_resp->failure_rts;
+
+	tx_status->flags =
+	    iwl_is_tx_success(status) ? IEEE80211_TX_STATUS_ACK : 0;
+
+	tx_status->control.tx_rate =
+		iwl_hw_get_rate_n_flags(tx_resp->rate_n_flags);
+
+	IWL_DEBUG_TX("Tx queue %d Status %s (0x%08x) rate_n_flags 0x%x "
+		     "retries %d\n", txq_id, iwl_get_tx_fail_reason(status),
+		     status, le32_to_cpu(tx_resp->rate_n_flags),
+		     tx_resp->failure_frame);
+
+	IWL_DEBUG_TX_REPLY("Tx queue reclaim %d\n", index);
+	if (index != -1)
+		iwl_tx_queue_reclaim(priv, txq_id, index);
+#ifdef CONFIG_IWLWIFI_HT
+#ifdef CONFIG_IWLWIFI_HT_AGG
+	}
+#endif /* CONFIG_IWLWIFI_HT_AGG */
+#endif /* CONFIG_IWLWIFI_HT */
+
+	if (iwl_check_bits(status, TX_ABORT_REQUIRED_MSK))
+		IWL_ERROR("TODO:  Implement Tx ABORT REQUIRED!!!\n");
+}
+
+
+static void iwl_rx_reply_alive(struct iwl_priv *priv,
+			       struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = (void *)rxb->skb->data;
+	struct iwl_alive_resp *palive;
+	struct delayed_work *pwork;
+
+	palive = &pkt->u.alive_frame;
+
+	IWL_DEBUG_INFO("Alive ucode status 0x%08X revision "
+		       "0x%01X 0x%01X\n",
+		       palive->is_valid, palive->ver_type,
+		       palive->ver_subtype);
+
+	if (palive->ver_subtype == INITIALIZE_SUBTYPE) {
+		IWL_DEBUG_INFO("Initialization Alive received.\n");
+		memcpy(&priv->card_alive_init,
+		       &pkt->u.alive_frame,
+		       sizeof(struct iwl_init_alive_resp));
+		pwork = &priv->init_alive_start;
+	} else {
+		IWL_DEBUG_INFO("Runtime Alive received.\n");
+		memcpy(&priv->card_alive, &pkt->u.alive_frame,
+		       sizeof(struct iwl_alive_resp));
+		pwork = &priv->alive_start;
+	}
+
+	/* We delay the ALIVE response by 5ms to
+	 * give the HW RF Kill time to activate... */
+	if (palive->is_valid == UCODE_VALID_OK)
+		queue_delayed_work(priv->workqueue, pwork,
+				   msecs_to_jiffies(5));
+	else
+		IWL_WARNING("uCode did not respond OK.\n");
+}
+
+static void iwl_rx_reply_add_sta(struct iwl_priv *priv,
+				 struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = (void *)rxb->skb->data;
+
+	IWL_DEBUG_RX("Received REPLY_ADD_STA: 0x%02X\n", pkt->u.status);
+	return;
+}
+
+static void iwl_rx_reply_error(struct iwl_priv *priv,
+			       struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = (void *)rxb->skb->data;
+
+	IWL_ERROR("Error Reply type 0x%08X cmd %s (0x%02X) "
+		"seq 0x%04X ser 0x%08X\n",
+		le32_to_cpu(pkt->u.err_resp.error_type),
+		get_cmd_string(pkt->u.err_resp.cmd_id),
+		pkt->u.err_resp.cmd_id,
+		le16_to_cpu(pkt->u.err_resp.bad_cmd_seq_num),
+		le32_to_cpu(pkt->u.err_resp.error_info));
+}
+
+#define TX_STATUS_ENTRY(x) case TX_STATUS_FAIL_ ## x: return #x
+
+static void iwl_rx_csa(struct iwl_priv *priv, struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = (void *)rxb->skb->data;
+	struct iwl_rxon_cmd *rxon = (void *)&priv->active_rxon;
+	struct iwl_csa_notification *csa = &(pkt->u.csa_notif);
+	IWL_DEBUG_11H("CSA notif: channel %d, status %d\n",
+		      le16_to_cpu(csa->channel), le32_to_cpu(csa->status));
+	rxon->channel = csa->channel;
+	priv->staging_rxon.channel = csa->channel;
+}
+
+static void iwl_rx_spectrum_measure_notif(struct iwl_priv *priv,
+					  struct iwl_rx_mem_buffer *rxb)
+{
+#ifdef CONFIG_IWLWIFI_SPECTRUM_MEASUREMENT
+	struct iwl_rx_packet *pkt = (void *)rxb->skb->data;
+	struct iwl_spectrum_notification *report = &(pkt->u.spectrum_notif);
+
+	if (!report->state) {
+		IWL_DEBUG(IWL_DL_11H | IWL_DL_INFO,
+			  "Spectrum Measure Notification: Start\n");
+		return;
+	}
+
+	memcpy(&priv->measure_report, report, sizeof(*report));
+	priv->measurement_status |= MEASUREMENT_READY;
+#endif
+}
+
+static void iwl_rx_pm_sleep_notif(struct iwl_priv *priv,
+				  struct iwl_rx_mem_buffer *rxb)
+{
+#ifdef CONFIG_IWLWIFI_DEBUG
+	struct iwl_rx_packet *pkt = (void *)rxb->skb->data;
+	struct iwl_sleep_notification *sleep = &(pkt->u.sleep_notif);
+	IWL_DEBUG_RX("sleep mode: %d, src: %d\n",
+		     sleep->pm_sleep_mode, sleep->pm_wakeup_src);
+#endif
+}
+
+static void iwl_rx_pm_debug_statistics_notif(struct iwl_priv *priv,
+					     struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = (void *)rxb->skb->data;
+	IWL_DEBUG_RADIO("Dumping %d bytes of unhandled "
+			"notification for %s:\n",
+			le32_to_cpu(pkt->len), get_cmd_string(pkt->hdr.cmd));
+	iwl_print_hex_dump(IWL_DL_RADIO, pkt->u.raw, le32_to_cpu(pkt->len));
+}
+
+static void iwl_bg_beacon_update(struct work_struct *work)
+{
+	struct iwl_priv *priv =
+		container_of(work, struct iwl_priv, beacon_update);
+	struct sk_buff *beacon;
+
+	/* Pull updated AP beacon from mac80211. will fail if not in AP mode */
+	beacon = ieee80211_beacon_get(priv->hw, priv->interface_id, NULL);
+
+	if (!beacon) {
+		IWL_ERROR("update beacon failed\n");
+		return;
+	}
+
+	mutex_lock(&priv->mutex);
+	/* new beacon skb is allocated every time; dispose previous.*/
+	if (priv->ibss_beacon)
+		dev_kfree_skb(priv->ibss_beacon);
+
+	priv->ibss_beacon = beacon;
+	mutex_unlock(&priv->mutex);
+
+	iwl_send_beacon_cmd(priv);
+}
+
+static void iwl_rx_beacon_notif(struct iwl_priv *priv,
+				struct iwl_rx_mem_buffer *rxb)
+{
+#ifdef CONFIG_IWLWIFI_DEBUG
+	struct iwl_rx_packet *pkt = (void *)rxb->skb->data;
+	struct iwl_beacon_notif *beacon = &(pkt->u.beacon_status);
+	u8 rate = iwl_hw_get_rate(beacon->beacon_notify_hdr.rate_n_flags);
+
+	IWL_DEBUG_RX("beacon status %x retries %d iss %d "
+		"tsf %d %d rate %d\n",
+		le32_to_cpu(beacon->beacon_notify_hdr.status) & TX_STATUS_MSK,
+		beacon->beacon_notify_hdr.failure_frame,
+		le32_to_cpu(beacon->ibss_mgr_status),
+		le32_to_cpu(beacon->high_tsf),
+		le32_to_cpu(beacon->low_tsf), rate);
+#endif
+
+	if ((priv->iw_mode == IEEE80211_IF_TYPE_AP) &&
+	    (!test_bit(STATUS_EXIT_PENDING, &priv->status)))
+		queue_work(priv->workqueue, &priv->beacon_update);
+}
+
+/* Service response to REPLY_SCAN_CMD (0x80) */
+static void iwl_rx_reply_scan(struct iwl_priv *priv,
+			      struct iwl_rx_mem_buffer *rxb)
+{
+#ifdef CONFIG_IWLWIFI_DEBUG
+	struct iwl_rx_packet *pkt = (void *)rxb->skb->data;
+	struct iwl_scanreq_notification *notif =
+	    (struct iwl_scanreq_notification *)pkt->u.raw;
+
+	IWL_DEBUG_RX("Scan request status = 0x%x\n", notif->status);
+#endif
+}
+
+/* Service SCAN_START_NOTIFICATION (0x82) */
+static void iwl_rx_scan_start_notif(struct iwl_priv *priv,
+				    struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = (void *)rxb->skb->data;
+	struct iwl_scanstart_notification *notif =
+	    (struct iwl_scanstart_notification *)pkt->u.raw;
+	priv->scan_start_tsf = le32_to_cpu(notif->tsf_low);
+	IWL_DEBUG_SCAN("Scan start: "
+		       "%d [802.11%s] "
+		       "(TSF: 0x%08X:%08X) - %d (beacon timer %u)\n",
+		       notif->channel,
+		       notif->band ? "bg" : "a",
+		       notif->tsf_high,
+		       notif->tsf_low, notif->status, notif->beacon_timer);
+}
+
+/* Service SCAN_RESULTS_NOTIFICATION (0x83) */
+static void iwl_rx_scan_results_notif(struct iwl_priv *priv,
+				      struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = (void *)rxb->skb->data;
+	struct iwl_scanresults_notification *notif =
+	    (struct iwl_scanresults_notification *)pkt->u.raw;
+
+	IWL_DEBUG_SCAN("Scan ch.res: "
+		       "%d [802.11%s] "
+		       "(TSF: 0x%08X:%08X) - %d "
+		       "elapsed=%lu usec (%dms since last)\n",
+		       notif->channel,
+		       notif->band ? "bg" : "a",
+		       le32_to_cpu(notif->tsf_high),
+		       le32_to_cpu(notif->tsf_low),
+		       le32_to_cpu(notif->statistics[0]),
+		       le32_to_cpu(notif->tsf_low) - priv->scan_start_tsf,
+		       jiffies_to_msecs(elapsed_jiffies
+					(priv->last_scan_jiffies, jiffies)));
+
+	priv->last_scan_jiffies = jiffies;
+}
+
+/* Service SCAN_COMPLETE_NOTIFICATION (0x84) */
+static void iwl_rx_scan_complete_notif(struct iwl_priv *priv,
+				       struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = (void *)rxb->skb->data;
+	struct iwl_scancomplete_notification *scan_notif = (void *)pkt->u.raw;
+
+	IWL_DEBUG_SCAN("Scan complete: %d channels (TSF 0x%08X:%08X) - %d\n",
+		       scan_notif->scanned_channels,
+		       scan_notif->tsf_low,
+		       scan_notif->tsf_high, scan_notif->status);
+
+	/* The HW is no longer scanning */
+	clear_bit(STATUS_SCAN_HW, &priv->status);
+
+	/* The scan completion notification came in, so kill that timer... */
+	cancel_delayed_work(&priv->scan_check);
+
+	IWL_DEBUG_INFO("Scan pass on %sGHz took %dms\n",
+		       (priv->scan_bands == 2) ? "2.4" : "5.2",
+		       jiffies_to_msecs(elapsed_jiffies
+					(priv->scan_pass_start, jiffies)));
+
+	/* Remove this scanned band from the list
+	 * of pending bands to scan */
+	priv->scan_bands--;
+
+	/* If a request to abort was given, or the scan did not succeed
+	 * then we reset the scan state machine and terminate,
+	 * re-queuing another scan if one has been requested */
+	if (test_bit(STATUS_SCAN_ABORTING, &priv->status)) {
+		IWL_DEBUG_INFO("Aborted scan completed.\n");
+		clear_bit(STATUS_SCAN_ABORTING, &priv->status);
+	} else {
+		/* If there are more bands on this scan pass reschedule */
+		if (priv->scan_bands > 0)
+			goto reschedule;
+	}
+
+	priv->last_scan_jiffies = jiffies;
+	IWL_DEBUG_INFO("Setting scan to off\n");
+
+	clear_bit(STATUS_SCANNING, &priv->status);
+
+	IWL_DEBUG_INFO("Scan took %dms\n",
+		jiffies_to_msecs(elapsed_jiffies(priv->scan_start, jiffies)));
+
+	queue_work(priv->workqueue, &priv->scan_completed);
+
+	return;
+
+reschedule:
+	priv->scan_pass_start = jiffies;
+	queue_work(priv->workqueue, &priv->request_scan);
+}
+
+/* Handle notification from uCode that card's power state is changing
+ * due to software, hardware, or critical temperature RFKILL */
+static void iwl_rx_card_state_notif(struct iwl_priv *priv,
+				    struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = (void *)rxb->skb->data;
+	u32 flags = le32_to_cpu(pkt->u.card_state_notif.flags);
+	unsigned long status = priv->status;
+
+	IWL_DEBUG_RF_KILL("Card state received: HW:%s SW:%s\n",
+			  (flags & HW_CARD_DISABLED) ? "Kill" : "On",
+			  (flags & SW_CARD_DISABLED) ? "Kill" : "On");
+
+	if (flags & (SW_CARD_DISABLED | HW_CARD_DISABLED |
+		     RF_CARD_DISABLED)) {
+
+		iwl_write32(priv, CSR_UCODE_DRV_GP1_SET,
+			    CSR_UCODE_DRV_GP1_BIT_CMD_BLOCKED);
+
+		if (!iwl_grab_restricted_access(priv)) {
+			iwl_write_restricted(
+				priv, HBUS_TARG_MBX_C,
+				HBUS_TARG_MBX_C_REG_BIT_CMD_BLOCKED);
+
+			iwl_release_restricted_access(priv);
+		}
+
+		if (!(flags & RXON_CARD_DISABLED)) {
+			iwl_write32(priv, CSR_UCODE_DRV_GP1_CLR,
+				    CSR_UCODE_DRV_GP1_BIT_CMD_BLOCKED);
+			if (!iwl_grab_restricted_access(priv)) {
+				iwl_write_restricted(
+					priv, HBUS_TARG_MBX_C,
+					HBUS_TARG_MBX_C_REG_BIT_CMD_BLOCKED);
+
+				iwl_release_restricted_access(priv);
+			}
+		}
+
+		if (flags & RF_CARD_DISABLED) {
+			iwl_write32(priv, CSR_UCODE_DRV_GP1_SET,
+				    CSR_UCODE_DRV_GP1_REG_BIT_CT_KILL_EXIT);
+			iwl_read32(priv, CSR_UCODE_DRV_GP1);
+			if (!iwl_grab_restricted_access(priv))
+				iwl_release_restricted_access(priv);
+		}
+	}
+
+	if (flags & HW_CARD_DISABLED)
+		set_bit(STATUS_RF_KILL_HW, &priv->status);
+	else
+		clear_bit(STATUS_RF_KILL_HW, &priv->status);
+
+
+	if (flags & SW_CARD_DISABLED)
+		set_bit(STATUS_RF_KILL_SW, &priv->status);
+	else
+		clear_bit(STATUS_RF_KILL_SW, &priv->status);
+
+	if (!(flags & RXON_CARD_DISABLED))
+		iwl_scan_cancel(priv);
+
+	if ((test_bit(STATUS_RF_KILL_HW, &status) !=
+	     test_bit(STATUS_RF_KILL_HW, &priv->status)) ||
+	    (test_bit(STATUS_RF_KILL_SW, &status) !=
+	     test_bit(STATUS_RF_KILL_SW, &priv->status)))
+		queue_work(priv->workqueue, &priv->rf_kill);
+	else
+		wake_up_interruptible(&priv->wait_command_queue);
+}
+
+/**
+ * iwl_setup_rx_handlers - Initialize Rx handler callbacks
+ *
+ * Setup the RX handlers for each of the reply types sent from the uCode
+ * to the host.
+ *
+ * This function chains into the hardware specific files for them to setup
+ * any hardware specific handlers as well.
+ */
+static void iwl_setup_rx_handlers(struct iwl_priv *priv)
+{
+	priv->rx_handlers[REPLY_ALIVE] = iwl_rx_reply_alive;
+	priv->rx_handlers[REPLY_ADD_STA] = iwl_rx_reply_add_sta;
+	priv->rx_handlers[REPLY_ERROR] = iwl_rx_reply_error;
+	priv->rx_handlers[CHANNEL_SWITCH_NOTIFICATION] = iwl_rx_csa;
+	priv->rx_handlers[SPECTRUM_MEASURE_NOTIFICATION] =
+	    iwl_rx_spectrum_measure_notif;
+	priv->rx_handlers[PM_SLEEP_NOTIFICATION] = iwl_rx_pm_sleep_notif;
+	priv->rx_handlers[PM_DEBUG_STATISTIC_NOTIFIC] =
+	    iwl_rx_pm_debug_statistics_notif;
+	priv->rx_handlers[BEACON_NOTIFICATION] = iwl_rx_beacon_notif;
+
+	/* NOTE:  iwl_rx_statistics is different based on whether
+	 * the build is for the 3945 or the 4965.  See the
+	 * corresponding implementation in iwl-XXXX.c
+	 *
+	 * The same handler is used for both the REPLY to a
+	 * discrete statistics request from the host as well as
+	 * for the periodic statistics notification from the uCode
+	 */
+	priv->rx_handlers[REPLY_STATISTICS_CMD] = iwl_hw_rx_statistics;
+	priv->rx_handlers[STATISTICS_NOTIFICATION] = iwl_hw_rx_statistics;
+
+	priv->rx_handlers[REPLY_SCAN_CMD] = iwl_rx_reply_scan;
+	priv->rx_handlers[SCAN_START_NOTIFICATION] = iwl_rx_scan_start_notif;
+	priv->rx_handlers[SCAN_RESULTS_NOTIFICATION] =
+	    iwl_rx_scan_results_notif;
+	priv->rx_handlers[SCAN_COMPLETE_NOTIFICATION] =
+	    iwl_rx_scan_complete_notif;
+	priv->rx_handlers[CARD_STATE_NOTIFICATION] = iwl_rx_card_state_notif;
+	priv->rx_handlers[REPLY_TX] = iwl_rx_reply_tx;
+
+	/* Setup hardware specific Rx handlers */
+	iwl_hw_rx_handler_setup(priv);
+}
+
+/**
+ * iwl_tx_cmd_complete - Pull unused buffers off the queue and reclaim them
+ * @rxb: Rx buffer to reclaim
+ *
+ * If an Rx buffer has an async callback associated with it the callback
+ * will be executed.  The attached skb (if present) will only be freed
+ * if the callback returns 1
+ */
+static void iwl_tx_cmd_complete(struct iwl_priv *priv,
+				struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = (struct iwl_rx_packet *)rxb->skb->data;
+	u16 sequence = le16_to_cpu(pkt->hdr.sequence);
+	int txq_id = SEQ_TO_QUEUE(sequence);
+	int index = SEQ_TO_INDEX(sequence);
+	int huge = sequence & SEQ_HUGE_FRAME;
+	int cmd_index;
+	struct iwl_cmd *cmd;
+
+	/* If a Tx command is being handled and it isn't in the actual
+	 * command queue then there a command routing bug has been introduced
+	 * in the queue management code. */
+	if (txq_id != IWL_CMD_QUEUE_NUM)
+		IWL_ERROR("Error wrong command queue %d command id 0x%X\n",
+			  txq_id, pkt->hdr.cmd);
+	BUG_ON(txq_id != IWL_CMD_QUEUE_NUM);
+
+	cmd_index = get_cmd_index(&priv->txq[IWL_CMD_QUEUE_NUM].q, index, huge);
+	cmd = &priv->txq[IWL_CMD_QUEUE_NUM].cmd[cmd_index];
+
+	/* Input error checking is done when commands are added to queue. */
+	if (cmd->meta.flags & CMD_WANT_SKB) {
+		cmd->meta.source->u.skb = rxb->skb;
+		rxb->skb = NULL;
+	} else if (cmd->meta.u.callback &&
+		   !cmd->meta.u.callback(priv, cmd, rxb->skb))
+		rxb->skb = NULL;
+
+	iwl_tx_queue_reclaim(priv, txq_id, index);
+
+	if (!(cmd->meta.flags & CMD_ASYNC)) {
+		clear_bit(STATUS_HCMD_ACTIVE, &priv->status);
+		wake_up_interruptible(&priv->wait_command_queue);
+	}
+}
+
+/************************** RX-FUNCTIONS ****************************/
+/*
+ * Rx theory of operation
+ *
+ * The host allocates 32 DMA target addresses and passes the host address
+ * to the firmware at register IWL_RFDS_TABLE_LOWER + N * RFD_SIZE where N is
+ * 0 to 31
+ *
+ * Rx Queue Indexes
+ * The host/firmware share two index registers for managing the Rx buffers.
+ *
+ * The READ index maps to the first position that the firmware may be writing
+ * to -- the driver can read up to (but not including) this position and get
+ * good data.
+ * The READ index is managed by the firmware once the card is enabled.
+ *
+ * The WRITE index maps to the last position the driver has read from -- the
+ * position preceding WRITE is the last slot the firmware can place a packet.
+ *
+ * The queue is empty (no good data) if WRITE = READ - 1, and is full if
+ * WRITE = READ.
+ *
+ * During initialization the host sets up the READ queue position to the first
+ * INDEX position, and WRITE to the last (READ - 1 wrapped)
+ *
+ * When the firmware places a packet in a buffer it will advance the READ index
+ * and fire the RX interrupt.  The driver can then query the READ index and
+ * process as many packets as possible, moving the WRITE index forward as it
+ * resets the Rx queue buffers with new memory.
+ *
+ * The management in the driver is as follows:
+ * + A list of pre-allocated SKBs is stored in iwl->rxq->rx_free.  When
+ *   iwl->rxq->free_count drops to or below RX_LOW_WATERMARK, work is scheduled
+ *   to replensish the iwl->rxq->rx_free.
+ * + In iwl_rx_replenish (scheduled) if 'processed' != 'read' then the
+ *   iwl->rxq is replenished and the READ INDEX is updated (updating the
+ *   'processed' and 'read' driver indexes as well)
+ * + A received packet is processed and handed to the kernel network stack,
+ *   detached from the iwl->rxq.  The driver 'processed' index is updated.
+ * + The Host/Firmware iwl->rxq is replenished at tasklet time from the rx_free
+ *   list. If there are no allocated buffers in iwl->rxq->rx_free, the READ
+ *   INDEX is not incremented and iwl->status(RX_STALLED) is set.  If there
+ *   were enough free buffers and RX_STALLED is set it is cleared.
+ *
+ *
+ * Driver sequence:
+ *
+ * iwl_rx_queue_alloc()       Allocates rx_free
+ * iwl_rx_replenish()         Replenishes rx_free list from rx_used, and calls
+ *                            iwl_rx_queue_restock
+ * iwl_rx_queue_restock()     Moves available buffers from rx_free into Rx
+ *                            queue, updates firmware pointers, and updates
+ *                            the WRITE index.  If insufficient rx_free buffers
+ *                            are available, schedules iwl_rx_replenish
+ *
+ * -- enable interrupts --
+ * ISR - iwl_rx()             Detach iwl_rx_mem_buffers from pool up to the
+ *                            READ INDEX, detaching the SKB from the pool.
+ *                            Moves the packet buffer from queue to rx_used.
+ *                            Calls iwl_rx_queue_restock to refill any empty
+ *                            slots.
+ * ...
+ *
+ */
+
+/**
+ * iwl_rx_queue_space - Return number of free slots available in queue.
+ */
+static int iwl_rx_queue_space(const struct iwl_rx_queue *q)
+{
+	int s = q->read - q->write;
+	if (s <= 0)
+		s += RX_QUEUE_SIZE;
+	/* keep some buffer to not confuse full and empty queue */
+	s -= 2;
+	if (s < 0)
+		s = 0;
+	return s;
+}
+
+/**
+ * iwl_rx_queue_update_write_ptr - Update the write pointer for the RX queue
+ *
+ * NOTE: This function has 3945 and 4965 specific code sections
+ * but is declared in base due to the majority of the
+ * implementation being the same (only a numeric constant is
+ * different)
+ *
+ */
+int iwl_rx_queue_update_write_ptr(struct iwl_priv *priv, struct iwl_rx_queue *q)
+{
+	u32 reg = 0;
+	int rc = 0;
+	unsigned long flags;
+
+	spin_lock_irqsave(&q->lock, flags);
+
+	if (q->need_update == 0)
+		goto exit_unlock;
+
+	if (test_bit(STATUS_POWER_PMI, &priv->status)) {
+		reg = iwl_read32(priv, CSR_UCODE_DRV_GP1);
+
+		if (reg & CSR_UCODE_DRV_GP1_BIT_MAC_SLEEP) {
+			iwl_set_bit(priv, CSR_GP_CNTRL,
+				    CSR_GP_CNTRL_REG_FLAG_MAC_ACCESS_REQ);
+			goto exit_unlock;
+		}
+
+		rc = iwl_grab_restricted_access(priv);
+		if (rc)
+			goto exit_unlock;
+
+		iwl_write_restricted(priv, FH_RSCSR_CHNL0_WPTR,
+				     q->write & ~0x7);
+		iwl_release_restricted_access(priv);
+	} else
+		iwl_write32(priv, FH_RSCSR_CHNL0_WPTR, q->write & ~0x7);
+
+
+	q->need_update = 0;
+
+ exit_unlock:
+	spin_unlock_irqrestore(&q->lock, flags);
+	return rc;
+}
+
+/**
+ * iwl_dma_addr2rbd_ptr - convert a DMA address to a uCode read buffer pointer.
+ *
+ * NOTE: This function has 3945 and 4965 specific code paths in it.
+ */
+static inline __le32 iwl_dma_addr2rbd_ptr(struct iwl_priv *priv,
+					  dma_addr_t dma_addr)
+{
+	return cpu_to_le32((u32)(dma_addr >> 8));
+}
+
+
+/**
+ * iwl_rx_queue_restock - refill RX queue from pre-allocated pool
+ *
+ * If there are slots in the RX queue that  need to be restocked,
+ * and we have free pre-allocated buffers, fill the ranks as much
+ * as we can pulling from rx_free.
+ *
+ * This moves the 'write' index forward to catch up with 'processed', and
+ * also updates the memory address in the firmware to reference the new
+ * target buffer.
+ */
+int iwl_rx_queue_restock(struct iwl_priv *priv)
+{
+	struct iwl_rx_queue *rxq = &priv->rxq;
+	struct list_head *element;
+	struct iwl_rx_mem_buffer *rxb;
+	unsigned long flags;
+	int write, rc;
+
+	spin_lock_irqsave(&rxq->lock, flags);
+	write = rxq->write & ~0x7;
+	while ((iwl_rx_queue_space(rxq) > 0) && (rxq->free_count)) {
+		element = rxq->rx_free.next;
+		rxb = list_entry(element, struct iwl_rx_mem_buffer, list);
+		list_del(element);
+		rxq->bd[rxq->write] = iwl_dma_addr2rbd_ptr(priv, rxb->dma_addr);
+		rxq->queue[rxq->write] = rxb;
+		rxq->write = (rxq->write + 1) & RX_QUEUE_MASK;
+		rxq->free_count--;
+	}
+	spin_unlock_irqrestore(&rxq->lock, flags);
+	/* If the pre-allocated buffer pool is dropping low, schedule to
+	 * refill it */
+	if (rxq->free_count <= RX_LOW_WATERMARK)
+		queue_work(priv->workqueue, &priv->rx_replenish);
+
+
+	/* If we've added more space for the firmware to place data, tell it */
+	if ((write != (rxq->write & ~0x7))
+	    || (abs(rxq->write - rxq->read) > 7)) {
+		spin_lock_irqsave(&rxq->lock, flags);
+		rxq->need_update = 1;
+		spin_unlock_irqrestore(&rxq->lock, flags);
+		rc = iwl_rx_queue_update_write_ptr(priv, rxq);
+		if (rc)
+			return rc;
+	}
+
+	return 0;
+}
+
+/**
+ * iwl_rx_replensih - Move all used packet from rx_used to rx_free
+ *
+ * When moving to rx_free an SKB is allocated for the slot.
+ *
+ * Also restock the Rx queue via iwl_rx_queue_restock.
+ * This is called as a scheduled work item (except for during intialization)
+ */
+void iwl_rx_replenish(void *data)
+{
+	struct iwl_priv *priv = data;
+	struct iwl_rx_queue *rxq = &priv->rxq;
+	struct list_head *element;
+	struct iwl_rx_mem_buffer *rxb;
+	unsigned long flags;
+	spin_lock_irqsave(&rxq->lock, flags);
+	while (!list_empty(&rxq->rx_used)) {
+		element = rxq->rx_used.next;
+		rxb = list_entry(element, struct iwl_rx_mem_buffer, list);
+		rxb->skb =
+		    alloc_skb(IWL_RX_BUF_SIZE, __GFP_NOWARN | GFP_ATOMIC);
+		if (!rxb->skb) {
+			if (net_ratelimit())
+				printk(KERN_CRIT DRV_NAME
+				       ": Can not allocate SKB buffers\n");
+			/* We don't reschedule replenish work here -- we will
+			 * call the restock method and if it still needs
+			 * more buffers it will schedule replenish */
+			break;
+		}
+		priv->alloc_rxb_skb++;
+		list_del(element);
+		rxb->dma_addr =
+		    pci_map_single(priv->pci_dev, rxb->skb->data,
+				   IWL_RX_BUF_SIZE, PCI_DMA_FROMDEVICE);
+		list_add_tail(&rxb->list, &rxq->rx_free);
+		rxq->free_count++;
+	}
+	spin_unlock_irqrestore(&rxq->lock, flags);
+
+	spin_lock_irqsave(&priv->lock, flags);
+	iwl_rx_queue_restock(priv);
+	spin_unlock_irqrestore(&priv->lock, flags);
+}
+
+/* Assumes that the skb field of the buffers in 'pool' is kept accurate.
+ * If an SKB has been detached, the POOL needs to have it's SKB set to NULL
+ * This free routine walks the list of POOL entries and if SKB is set to
+ * non NULL it is unmapped and freed
+ */
+void iwl_rx_queue_free(struct iwl_priv *priv, struct iwl_rx_queue *rxq)
+{
+	int i;
+	for (i = 0; i < RX_QUEUE_SIZE + RX_FREE_BUFFERS; i++) {
+		if (rxq->pool[i].skb != NULL) {
+			pci_unmap_single(priv->pci_dev,
+					 rxq->pool[i].dma_addr,
+					 IWL_RX_BUF_SIZE, PCI_DMA_FROMDEVICE);
+			dev_kfree_skb(rxq->pool[i].skb);
+		}
+	}
+
+	pci_free_consistent(priv->pci_dev, 4 * RX_QUEUE_SIZE, rxq->bd,
+			    rxq->dma_addr);
+	rxq->bd = NULL;
+}
+
+int iwl_rx_queue_alloc(struct iwl_priv *priv)
+{
+	struct iwl_rx_queue *rxq = &priv->rxq;
+	struct pci_dev *dev = priv->pci_dev;
+	int i;
+
+	spin_lock_init(&rxq->lock);
+	INIT_LIST_HEAD(&rxq->rx_free);
+	INIT_LIST_HEAD(&rxq->rx_used);
+	rxq->bd = pci_alloc_consistent(dev, 4 * RX_QUEUE_SIZE, &rxq->dma_addr);
+	if (!rxq->bd)
+		return -ENOMEM;
+	/* Fill the rx_used queue with _all_ of the Rx buffers */
+	for (i = 0; i < RX_FREE_BUFFERS + RX_QUEUE_SIZE; i++)
+		list_add_tail(&rxq->pool[i].list, &rxq->rx_used);
+	/* Set us so that we have processed and used all buffers, but have
+	 * not restocked the Rx queue with fresh buffers */
+	rxq->read = rxq->write = 0;
+	rxq->free_count = 0;
+	rxq->need_update = 0;
+	return 0;
+}
+
+void iwl_rx_queue_reset(struct iwl_priv *priv, struct iwl_rx_queue *rxq)
+{
+	unsigned long flags;
+	int i;
+	spin_lock_irqsave(&rxq->lock, flags);
+	INIT_LIST_HEAD(&rxq->rx_free);
+	INIT_LIST_HEAD(&rxq->rx_used);
+	/* Fill the rx_used queue with _all_ of the Rx buffers */
+	for (i = 0; i < RX_FREE_BUFFERS + RX_QUEUE_SIZE; i++) {
+		/* In the reset function, these buffers may have been allocated
+		 * to an SKB, so we need to unmap and free potential storage */
+		if (rxq->pool[i].skb != NULL) {
+			pci_unmap_single(priv->pci_dev,
+					 rxq->pool[i].dma_addr,
+					 IWL_RX_BUF_SIZE, PCI_DMA_FROMDEVICE);
+			priv->alloc_rxb_skb--;
+			dev_kfree_skb(rxq->pool[i].skb);
+			rxq->pool[i].skb = NULL;
+		}
+		list_add_tail(&rxq->pool[i].list, &rxq->rx_used);
+	}
+
+	/* Set us so that we have processed and used all buffers, but have
+	 * not restocked the Rx queue with fresh buffers */
+	rxq->read = rxq->write = 0;
+	rxq->free_count = 0;
+	spin_unlock_irqrestore(&rxq->lock, flags);
+}
+
+/* Convert linear signal-to-noise ratio into dB */
+static u8 ratio2dB[100] = {
+/*	 0   1   2   3   4   5   6   7   8   9 */
+	 0,  0,  6, 10, 12, 14, 16, 17, 18, 19, /* 00 - 09 */
+	20, 21, 22, 22, 23, 23, 24, 25, 26, 26, /* 10 - 19 */
+	26, 26, 26, 27, 27, 28, 28, 28, 29, 29, /* 20 - 29 */
+	29, 30, 30, 30, 31, 31, 31, 31, 32, 32, /* 30 - 39 */
+	32, 32, 32, 33, 33, 33, 33, 33, 34, 34, /* 40 - 49 */
+	34, 34, 34, 34, 35, 35, 35, 35, 35, 35, /* 50 - 59 */
+	36, 36, 36, 36, 36, 36, 36, 37, 37, 37, /* 60 - 69 */
+	37, 37, 37, 37, 37, 38, 38, 38, 38, 38, /* 70 - 79 */
+	38, 38, 38, 38, 38, 39, 39, 39, 39, 39, /* 80 - 89 */
+	39, 39, 39, 39, 39, 40, 40, 40, 40, 40  /* 90 - 99 */
+};
+
+/* Calculates a relative dB value from a ratio of linear
+ *   (i.e. not dB) signal levels.
+ * Conversion assumes that levels are voltages (20*log), not powers (10*log). */
+int iwl_calc_db_from_ratio(int sig_ratio)
+{
+	/* Anything above 1000:1 just report as 60 dB */
+	if (sig_ratio > 1000)
+		return 60;
+
+	/* Above 100:1, divide by 10 and use table,
+	 *   add 20 dB to make up for divide by 10 */
+	if (sig_ratio > 100)
+		return (20 + (int)ratio2dB[sig_ratio/10]);
+
+	/* We shouldn't see this */
+	if (sig_ratio < 1)
+		return 0;
+
+	/* Use table for ratios 1:1 - 99:1 */
+	return (int)ratio2dB[sig_ratio];
+}
+
+#define PERFECT_RSSI (-20) /* dBm */
+#define WORST_RSSI (-95)   /* dBm */
+#define RSSI_RANGE (PERFECT_RSSI - WORST_RSSI)
+
+/* Calculate an indication of rx signal quality (a percentage, not dBm!).
+ * See http://www.ces.clemson.edu/linux/signal_quality.shtml for info
+ *   about formulas used below. */
+int iwl_calc_sig_qual(int rssi_dbm, int noise_dbm)
+{
+	int sig_qual;
+	int degradation = PERFECT_RSSI - rssi_dbm;
+
+	/* If we get a noise measurement, use signal-to-noise ratio (SNR)
+	 * as indicator; formula is (signal dbm - noise dbm).
+	 * SNR at or above 40 is a great signal (100%).
+	 * Below that, scale to fit SNR of 0 - 40 dB within 0 - 100% indicator.
+	 * Weakest usable signal is usually 10 - 15 dB SNR. */
+	if (noise_dbm) {
+		if (rssi_dbm - noise_dbm >= 40)
+			return 100;
+		else if (rssi_dbm < noise_dbm)
+			return 0;
+		sig_qual = ((rssi_dbm - noise_dbm) * 5) / 2;
+
+	/* Else use just the signal level.
+	 * This formula is a least squares fit of data points collected and
+	 *   compared with a reference system that had a percentage (%) display
+	 *   for signal quality. */
+	} else
+		sig_qual = (100 * (RSSI_RANGE * RSSI_RANGE) - degradation *
+			    (15 * RSSI_RANGE + 62 * degradation)) /
+			   (RSSI_RANGE * RSSI_RANGE);
+
+	if (sig_qual > 100)
+		sig_qual = 100;
+	else if (sig_qual < 1)
+		sig_qual = 0;
+
+	return sig_qual;
+}
+
+/**
+ * iwl_rx_handle - Main entry function for receiving responses from the uCode
+ *
+ * Uses the priv->rx_handlers callback function array to invoke
+ * the appropriate handlers, including command responses,
+ * frame-received notifications, and other notifications.
+ */
+static void iwl_rx_handle(struct iwl_priv *priv)
+{
+	struct iwl_rx_mem_buffer *rxb;
+	struct iwl_rx_packet *pkt;
+	struct iwl_rx_queue *rxq = &priv->rxq;
+	u32 r, i;
+	int reclaim;
+	unsigned long flags;
+
+	r = iwl_hw_get_rx_read(priv);
+	i = rxq->read;
+
+	/* Rx interrupt, but nothing sent from uCode */
+	if (i == r)
+		IWL_DEBUG(IWL_DL_RX | IWL_DL_ISR, "r = %d, i = %d\n", r, i);
+
+	while (i != r) {
+		rxb = rxq->queue[i];
+
+		/* If an RXB doesn't have a queue slot associated with it
+		 * then a bug has been introduced in the queue refilling
+		 * routines -- catch it here */
+		BUG_ON(rxb == NULL);
+
+		rxq->queue[i] = NULL;
+
+		pci_dma_sync_single_for_cpu(priv->pci_dev, rxb->dma_addr,
+					    IWL_RX_BUF_SIZE,
+					    PCI_DMA_FROMDEVICE);
+		pkt = (struct iwl_rx_packet *)rxb->skb->data;
+
+		/* Reclaim a command buffer only if this packet is a response
+		 *   to a (driver-originated) command.
+		 * If the packet (e.g. Rx frame) originated from uCode,
+		 *   there is no command buffer to reclaim.
+		 * Ucode should set SEQ_RX_FRAME bit if ucode-originated,
+		 *   but apparently a few don't get set; catch them here. */
+		reclaim = !(pkt->hdr.sequence & SEQ_RX_FRAME) &&
+			(pkt->hdr.cmd != REPLY_RX_PHY_CMD) &&
+			(pkt->hdr.cmd != REPLY_4965_RX) &&
+			(pkt->hdr.cmd != REPLY_COMPRESSED_BA) &&
+			(pkt->hdr.cmd != STATISTICS_NOTIFICATION) &&
+			(pkt->hdr.cmd != REPLY_TX);
+
+		/* Based on type of command response or notification,
+		 *   handle those that need handling via function in
+		 *   rx_handlers table.  See iwl_setup_rx_handlers() */
+		if (priv->rx_handlers[pkt->hdr.cmd]) {
+			IWL_DEBUG(IWL_DL_HOST_COMMAND | IWL_DL_RX | IWL_DL_ISR,
+				"r = %d, i = %d, %s, 0x%02x\n", r, i,
+				get_cmd_string(pkt->hdr.cmd), pkt->hdr.cmd);
+			priv->rx_handlers[pkt->hdr.cmd] (priv, rxb);
+		} else {
+			/* No handling needed */
+			IWL_DEBUG(IWL_DL_HOST_COMMAND | IWL_DL_RX | IWL_DL_ISR,
+				"r %d i %d No handler needed for %s, 0x%02x\n",
+				r, i, get_cmd_string(pkt->hdr.cmd),
+				pkt->hdr.cmd);
+		}
+
+		if (reclaim) {
+			/* Invoke any callbacks, transfer the skb to caller,
+			 * and fire off the (possibly) blocking iwl_send_cmd()
+			 * as we reclaim the driver command queue */
+			if (rxb && rxb->skb)
+				iwl_tx_cmd_complete(priv, rxb);
+			else
+				IWL_WARNING("Claim null rxb?\n");
+		}
+
+		/* For now we just don't re-use anything.  We can tweak this
+		 * later to try and re-use notification packets and SKBs that
+		 * fail to Rx correctly */
+		if (rxb->skb != NULL) {
+			priv->alloc_rxb_skb--;
+			dev_kfree_skb_any(rxb->skb);
+			rxb->skb = NULL;
+		}
+
+		pci_unmap_single(priv->pci_dev, rxb->dma_addr,
+				 IWL_RX_BUF_SIZE, PCI_DMA_FROMDEVICE);
+		spin_lock_irqsave(&rxq->lock, flags);
+		list_add_tail(&rxb->list, &priv->rxq.rx_used);
+		spin_unlock_irqrestore(&rxq->lock, flags);
+		i = (i + 1) & RX_QUEUE_MASK;
+	}
+
+	/* Backtrack one entry */
+	priv->rxq.read = i;
+	iwl_rx_queue_restock(priv);
+}
+
+int iwl_tx_queue_update_write_ptr(struct iwl_priv *priv,
+				  struct iwl_tx_queue *txq)
+{
+	u32 reg = 0;
+	int rc = 0;
+	int txq_id = txq->q.id;
+
+	if (txq->need_update == 0)
+		return rc;
+
+	/* if we're trying to save power */
+	if (test_bit(STATUS_POWER_PMI, &priv->status)) {
+		/* wake up nic if it's powered down ...
+		 * uCode will wake up, and interrupt us again, so next
+		 * time we'll skip this part. */
+		reg = iwl_read32(priv, CSR_UCODE_DRV_GP1);
+
+		if (reg & CSR_UCODE_DRV_GP1_BIT_MAC_SLEEP) {
+			IWL_DEBUG_INFO("Requesting wakeup, GP1 = 0x%x\n", reg);
+			iwl_set_bit(priv, CSR_GP_CNTRL,
+				    CSR_GP_CNTRL_REG_FLAG_MAC_ACCESS_REQ);
+			return rc;
+		}
+
+		/* restore this queue's parameters in nic hardware. */
+		rc = iwl_grab_restricted_access(priv);
+		if (rc)
+			return rc;
+		iwl_write_restricted(priv, HBUS_TARG_WRPTR,
+				     txq->q.first_empty | (txq_id << 8));
+		iwl_release_restricted_access(priv);
+
+	/* else not in power-save mode, uCode will never sleep when we're
+	 * trying to tx (during RFKILL, we're not trying to tx). */
+	} else
+		iwl_write32(priv, HBUS_TARG_WRPTR,
+			    txq->q.first_empty | (txq_id << 8));
+
+	txq->need_update = 0;
+
+	return rc;
+}
+
+#ifdef CONFIG_IWLWIFI_DEBUG
+static void iwl_print_rx_config_cmd(struct iwl_rxon_cmd *rxon)
+{
+	DECLARE_MAC_BUF(mac);
+
+	IWL_DEBUG_RADIO("RX CONFIG:\n");
+	iwl_print_hex_dump(IWL_DL_RADIO, (u8 *) rxon, sizeof(*rxon));
+	IWL_DEBUG_RADIO("u16 channel: 0x%x\n", le16_to_cpu(rxon->channel));
+	IWL_DEBUG_RADIO("u32 flags: 0x%08X\n", le32_to_cpu(rxon->flags));
+	IWL_DEBUG_RADIO("u32 filter_flags: 0x%08x\n",
+			le32_to_cpu(rxon->filter_flags));
+	IWL_DEBUG_RADIO("u8 dev_type: 0x%x\n", rxon->dev_type);
+	IWL_DEBUG_RADIO("u8 ofdm_basic_rates: 0x%02x\n",
+			rxon->ofdm_basic_rates);
+	IWL_DEBUG_RADIO("u8 cck_basic_rates: 0x%02x\n", rxon->cck_basic_rates);
+	IWL_DEBUG_RADIO("u8[6] node_addr: %s\n",
+			print_mac(mac, rxon->node_addr));
+	IWL_DEBUG_RADIO("u8[6] bssid_addr: %s\n",
+			print_mac(mac, rxon->bssid_addr));
+	IWL_DEBUG_RADIO("u16 assoc_id: 0x%x\n", le16_to_cpu(rxon->assoc_id));
+}
+#endif
+
+static void iwl_enable_interrupts(struct iwl_priv *priv)
+{
+	IWL_DEBUG_ISR("Enabling interrupts\n");
+	set_bit(STATUS_INT_ENABLED, &priv->status);
+	iwl_write32(priv, CSR_INT_MASK, CSR_INI_SET_MASK);
+}
+
+static inline void iwl_disable_interrupts(struct iwl_priv *priv)
+{
+	clear_bit(STATUS_INT_ENABLED, &priv->status);
+
+	/* disable interrupts from uCode/NIC to host */
+	iwl_write32(priv, CSR_INT_MASK, 0x00000000);
+
+	/* acknowledge/clear/reset any interrupts still pending
+	 * from uCode or flow handler (Rx/Tx DMA) */
+	iwl_write32(priv, CSR_INT, 0xffffffff);
+	iwl_write32(priv, CSR_FH_INT_STATUS, 0xffffffff);
+	IWL_DEBUG_ISR("Disabled interrupts\n");
+}
+
+static const char *desc_lookup(int i)
+{
+	switch (i) {
+	case 1:
+		return "FAIL";
+	case 2:
+		return "BAD_PARAM";
+	case 3:
+		return "BAD_CHECKSUM";
+	case 4:
+		return "NMI_INTERRUPT";
+	case 5:
+		return "SYSASSERT";
+	case 6:
+		return "FATAL_ERROR";
+	}
+
+	return "UNKNOWN";
+}
+
+#define ERROR_START_OFFSET  (1 * sizeof(u32))
+#define ERROR_ELEM_SIZE     (7 * sizeof(u32))
+
+static void iwl_dump_nic_error_log(struct iwl_priv *priv)
+{
+	u32 data2, line;
+	u32 desc, time, count, base, data1;
+	u32 blink1, blink2, ilink1, ilink2;
+	int rc;
+
+	base = le32_to_cpu(priv->card_alive.error_event_table_ptr);
+
+	if (!iwl_hw_valid_rtc_data_addr(base)) {
+		IWL_ERROR("Not valid error log pointer 0x%08X\n", base);
+		return;
+	}
+
+	rc = iwl_grab_restricted_access(priv);
+	if (rc) {
+		IWL_WARNING("Can not read from adapter at this time.\n");
+		return;
+	}
+
+	count = iwl_read_restricted_mem(priv, base);
+
+	if (ERROR_START_OFFSET <= count * ERROR_ELEM_SIZE) {
+		IWL_ERROR("Start IWL Error Log Dump:\n");
+		IWL_ERROR("Status: 0x%08lX, Config: %08X count: %d\n",
+			  priv->status, priv->config, count);
+	}
+
+	desc = iwl_read_restricted_mem(priv, base + 1 * sizeof(u32));
+	blink1 = iwl_read_restricted_mem(priv, base + 3 * sizeof(u32));
+	blink2 = iwl_read_restricted_mem(priv, base + 4 * sizeof(u32));
+	ilink1 = iwl_read_restricted_mem(priv, base + 5 * sizeof(u32));
+	ilink2 = iwl_read_restricted_mem(priv, base + 6 * sizeof(u32));
+	data1 = iwl_read_restricted_mem(priv, base + 7 * sizeof(u32));
+	data2 = iwl_read_restricted_mem(priv, base + 8 * sizeof(u32));
+	line = iwl_read_restricted_mem(priv, base + 9 * sizeof(u32));
+	time = iwl_read_restricted_mem(priv, base + 11 * sizeof(u32));
+
+	IWL_ERROR("Desc               Time       "
+		  "data1      data2      line\n");
+	IWL_ERROR("%-13s (#%d) %010u 0x%08X 0x%08X %u\n",
+		  desc_lookup(desc), desc, time, data1, data2, line);
+	IWL_ERROR("blink1  blink2  ilink1  ilink2\n");
+	IWL_ERROR("0x%05X 0x%05X 0x%05X 0x%05X\n", blink1, blink2,
+		  ilink1, ilink2);
+
+	iwl_release_restricted_access(priv);
+}
+
+#define EVENT_START_OFFSET  (4 * sizeof(u32))
+
+/**
+ * iwl_print_event_log - Dump error event log to syslog
+ *
+ * NOTE: Must be called with iwl_grab_restricted_access() already obtained!
+ */
+static void iwl_print_event_log(struct iwl_priv *priv, u32 start_idx,
+				u32 num_events, u32 mode)
+{
+	u32 i;
+	u32 base;       /* SRAM byte address of event log header */
+	u32 event_size;	/* 2 u32s, or 3 u32s if timestamp recorded */
+	u32 ptr;        /* SRAM byte address of log data */
+	u32 ev, time, data; /* event log data */
+
+	if (num_events == 0)
+		return;
+
+	base = le32_to_cpu(priv->card_alive.log_event_table_ptr);
+
+	if (mode == 0)
+		event_size = 2 * sizeof(u32);
+	else
+		event_size = 3 * sizeof(u32);
+
+	ptr = base + EVENT_START_OFFSET + (start_idx * event_size);
+
+	/* "time" is actually "data" for mode 0 (no timestamp).
+	 * place event id # at far right for easier visual parsing. */
+	for (i = 0; i < num_events; i++) {
+		ev = iwl_read_restricted_mem(priv, ptr);
+		ptr += sizeof(u32);
+		time = iwl_read_restricted_mem(priv, ptr);
+		ptr += sizeof(u32);
+		if (mode == 0)
+			IWL_ERROR("0x%08x\t%04u\n", time, ev); /* data, ev */
+		else {
+			data = iwl_read_restricted_mem(priv, ptr);
+			ptr += sizeof(u32);
+			IWL_ERROR("%010u\t0x%08x\t%04u\n", time, data, ev);
+		}
+	}
+}
+
+static void iwl_dump_nic_event_log(struct iwl_priv *priv)
+{
+	int rc;
+	u32 base;       /* SRAM byte address of event log header */
+	u32 capacity;   /* event log capacity in # entries */
+	u32 mode;       /* 0 - no timestamp, 1 - timestamp recorded */
+	u32 num_wraps;  /* # times uCode wrapped to top of log */
+	u32 next_entry; /* index of next entry to be written by uCode */
+	u32 size;       /* # entries that we'll print */
+
+	base = le32_to_cpu(priv->card_alive.log_event_table_ptr);
+	if (!iwl_hw_valid_rtc_data_addr(base)) {
+		IWL_ERROR("Invalid event log pointer 0x%08X\n", base);
+		return;
+	}
+
+	rc = iwl_grab_restricted_access(priv);
+	if (rc) {
+		IWL_WARNING("Can not read from adapter at this time.\n");
+		return;
+	}
+
+	/* event log header */
+	capacity = iwl_read_restricted_mem(priv, base);
+	mode = iwl_read_restricted_mem(priv, base + (1 * sizeof(u32)));
+	num_wraps = iwl_read_restricted_mem(priv, base + (2 * sizeof(u32)));
+	next_entry = iwl_read_restricted_mem(priv, base + (3 * sizeof(u32)));
+
+	size = num_wraps ? capacity : next_entry;
+
+	/* bail out if nothing in log */
+	if (size == 0) {
+		IWL_ERROR("Start IWL Event Log Dump: nothing in log\n");
+		iwl_release_restricted_access(priv);
+		return;
+	}
+
+	IWL_ERROR("Start IWL Event Log Dump: display count %d, wraps %d\n",
+		  size, num_wraps);
+
+	/* if uCode has wrapped back to top of log, start at the oldest entry,
+	 * i.e the next one that uCode would fill. */
+	if (num_wraps)
+		iwl_print_event_log(priv, next_entry,
+				    capacity - next_entry, mode);
+
+	/* (then/else) start at top of log */
+	iwl_print_event_log(priv, 0, next_entry, mode);
+
+	iwl_release_restricted_access(priv);
+}
+
+/**
+ * iwl_irq_handle_error - called for HW or SW error interrupt from card
+ */
+static void iwl_irq_handle_error(struct iwl_priv *priv)
+{
+	/* Set the FW error flag -- cleared on iwl_down */
+	set_bit(STATUS_FW_ERROR, &priv->status);
+
+	/* Cancel currently queued command. */
+	clear_bit(STATUS_HCMD_ACTIVE, &priv->status);
+
+#ifdef CONFIG_IWLWIFI_DEBUG
+	if (iwl_debug_level & IWL_DL_FW_ERRORS) {
+		iwl_dump_nic_error_log(priv);
+		iwl_dump_nic_event_log(priv);
+		iwl_print_rx_config_cmd(&priv->staging_rxon);
+	}
+#endif
+
+	wake_up_interruptible(&priv->wait_command_queue);
+
+	/* Keep the restart process from trying to send host
+	 * commands by clearing the INIT status bit */
+	clear_bit(STATUS_READY, &priv->status);
+
+	if (!test_bit(STATUS_EXIT_PENDING, &priv->status)) {
+		IWL_DEBUG(IWL_DL_INFO | IWL_DL_FW_ERRORS,
+			  "Restarting adapter due to uCode error.\n");
+
+		if (iwl_is_associated(priv)) {
+			memcpy(&priv->recovery_rxon, &priv->active_rxon,
+			       sizeof(priv->recovery_rxon));
+			priv->error_recovering = 1;
+		}
+		queue_work(priv->workqueue, &priv->restart);
+	}
+}
+
+static void iwl_error_recovery(struct iwl_priv *priv)
+{
+	unsigned long flags;
+
+	memcpy(&priv->staging_rxon, &priv->recovery_rxon,
+	       sizeof(priv->staging_rxon));
+	priv->staging_rxon.filter_flags &= ~RXON_FILTER_ASSOC_MSK;
+	iwl_commit_rxon(priv);
+
+	iwl_rxon_add_station(priv, priv->bssid, 1);
+
+	spin_lock_irqsave(&priv->lock, flags);
+	priv->assoc_id = le16_to_cpu(priv->staging_rxon.assoc_id);
+	priv->error_recovering = 0;
+	spin_unlock_irqrestore(&priv->lock, flags);
+}
+
+static void iwl_irq_tasklet(struct iwl_priv *priv)
+{
+	u32 inta, handled = 0;
+	u32 inta_fh;
+	unsigned long flags;
+#ifdef CONFIG_IWLWIFI_DEBUG
+	u32 inta_mask;
+#endif
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	/* Ack/clear/reset pending uCode interrupts.
+	 * Note:  Some bits in CSR_INT are "OR" of bits in CSR_FH_INT_STATUS,
+	 *  and will clear only when CSR_FH_INT_STATUS gets cleared. */
+	inta = iwl_read32(priv, CSR_INT);
+	iwl_write32(priv, CSR_INT, inta);
+
+	/* Ack/clear/reset pending flow-handler (DMA) interrupts.
+	 * Any new interrupts that happen after this, either while we're
+	 * in this tasklet, or later, will show up in next ISR/tasklet. */
+	inta_fh = iwl_read32(priv, CSR_FH_INT_STATUS);
+	iwl_write32(priv, CSR_FH_INT_STATUS, inta_fh);
+
+#ifdef CONFIG_IWLWIFI_DEBUG
+	if (iwl_debug_level & IWL_DL_ISR) {
+		inta_mask = iwl_read32(priv, CSR_INT_MASK); /* just for debug */
+		IWL_DEBUG_ISR("inta 0x%08x, enabled 0x%08x, fh 0x%08x\n",
+			      inta, inta_mask, inta_fh);
+	}
+#endif
+
+	/* Since CSR_INT and CSR_FH_INT_STATUS reads and clears are not
+	 * atomic, make sure that inta covers all the interrupts that
+	 * we've discovered, even if FH interrupt came in just after
+	 * reading CSR_INT. */
+	if (inta_fh & CSR_FH_INT_RX_MASK)
+		inta |= CSR_INT_BIT_FH_RX;
+	if (inta_fh & CSR_FH_INT_TX_MASK)
+		inta |= CSR_INT_BIT_FH_TX;
+
+	/* Now service all interrupt bits discovered above. */
+	if (inta & CSR_INT_BIT_HW_ERR) {
+		IWL_ERROR("Microcode HW error detected.  Restarting.\n");
+
+		/* Tell the device to stop sending interrupts */
+		iwl_disable_interrupts(priv);
+
+		iwl_irq_handle_error(priv);
+
+		handled |= CSR_INT_BIT_HW_ERR;
+
+		spin_unlock_irqrestore(&priv->lock, flags);
+
+		return;
+	}
+
+#ifdef CONFIG_IWLWIFI_DEBUG
+	if (iwl_debug_level & (IWL_DL_ISR)) {
+		/* NIC fires this, but we don't use it, redundant with WAKEUP */
+		if (inta & CSR_INT_BIT_MAC_CLK_ACTV)
+			IWL_DEBUG_ISR("Microcode started or stopped.\n");
+
+		/* Alive notification via Rx interrupt will do the real work */
+		if (inta & CSR_INT_BIT_ALIVE)
+			IWL_DEBUG_ISR("Alive interrupt\n");
+	}
+#endif
+	/* Safely ignore these bits for debug checks below */
+	inta &= ~(CSR_INT_BIT_MAC_CLK_ACTV | CSR_INT_BIT_ALIVE);
+
+	/* HW RF KILL switch toggled (4965 only) */
+	if (inta & CSR_INT_BIT_RF_KILL) {
+		int hw_rf_kill = 0;
+		if (!(iwl_read32(priv, CSR_GP_CNTRL) &
+				CSR_GP_CNTRL_REG_FLAG_HW_RF_KILL_SW))
+			hw_rf_kill = 1;
+
+		IWL_DEBUG(IWL_DL_INFO | IWL_DL_RF_KILL | IWL_DL_ISR,
+				"RF_KILL bit toggled to %s.\n",
+				hw_rf_kill ? "disable radio":"enable radio");
+
+		/* Queue restart only if RF_KILL switch was set to "kill"
+		 *   when we loaded driver, and is now set to "enable".
+		 * After we're Alive, RF_KILL gets handled by
+		 *   iwl_rx_card_state_notif() */
+		if (!hw_rf_kill && !test_bit(STATUS_ALIVE, &priv->status))
+			queue_work(priv->workqueue, &priv->restart);
+
+		handled |= CSR_INT_BIT_RF_KILL;
+	}
+
+	/* Chip got too hot and stopped itself (4965 only) */
+	if (inta & CSR_INT_BIT_CT_KILL) {
+		IWL_ERROR("Microcode CT kill error detected.\n");
+		handled |= CSR_INT_BIT_CT_KILL;
+	}
+
+	/* Error detected by uCode */
+	if (inta & CSR_INT_BIT_SW_ERR) {
+		IWL_ERROR("Microcode SW error detected.  Restarting 0x%X.\n",
+			  inta);
+		iwl_irq_handle_error(priv);
+		handled |= CSR_INT_BIT_SW_ERR;
+	}
+
+	/* uCode wakes up after power-down sleep */
+	if (inta & CSR_INT_BIT_WAKEUP) {
+		IWL_DEBUG_ISR("Wakeup interrupt\n");
+		iwl_rx_queue_update_write_ptr(priv, &priv->rxq);
+		iwl_tx_queue_update_write_ptr(priv, &priv->txq[0]);
+		iwl_tx_queue_update_write_ptr(priv, &priv->txq[1]);
+		iwl_tx_queue_update_write_ptr(priv, &priv->txq[2]);
+		iwl_tx_queue_update_write_ptr(priv, &priv->txq[3]);
+		iwl_tx_queue_update_write_ptr(priv, &priv->txq[4]);
+		iwl_tx_queue_update_write_ptr(priv, &priv->txq[5]);
+
+		handled |= CSR_INT_BIT_WAKEUP;
+	}
+
+	/* All uCode command responses, including Tx command responses,
+	 * Rx "responses" (frame-received notification), and other
+	 * notifications from uCode come through here*/
+	if (inta & (CSR_INT_BIT_FH_RX | CSR_INT_BIT_SW_RX)) {
+		iwl_rx_handle(priv);
+		handled |= (CSR_INT_BIT_FH_RX | CSR_INT_BIT_SW_RX);
+	}
+
+	if (inta & CSR_INT_BIT_FH_TX) {
+		IWL_DEBUG_ISR("Tx interrupt\n");
+		handled |= CSR_INT_BIT_FH_TX;
+	}
+
+	if (inta & ~handled)
+		IWL_ERROR("Unhandled INTA bits 0x%08x\n", inta & ~handled);
+
+	if (inta & ~CSR_INI_SET_MASK) {
+		IWL_WARNING("Disabled INTA bits 0x%08x were pending\n",
+			 inta & ~CSR_INI_SET_MASK);
+		IWL_WARNING("   with FH_INT = 0x%08x\n", inta_fh);
+	}
+
+	/* Re-enable all interrupts */
+	iwl_enable_interrupts(priv);
+
+#ifdef CONFIG_IWLWIFI_DEBUG
+	if (iwl_debug_level & (IWL_DL_ISR)) {
+		inta = iwl_read32(priv, CSR_INT);
+		inta_mask = iwl_read32(priv, CSR_INT_MASK);
+		inta_fh = iwl_read32(priv, CSR_FH_INT_STATUS);
+		IWL_DEBUG_ISR("End inta 0x%08x, enabled 0x%08x, fh 0x%08x, "
+			"flags 0x%08lx\n", inta, inta_mask, inta_fh, flags);
+	}
+#endif
+	spin_unlock_irqrestore(&priv->lock, flags);
+}
+
+static irqreturn_t iwl_isr(int irq, void *data)
+{
+	struct iwl_priv *priv = data;
+	u32 inta, inta_mask;
+	u32 inta_fh;
+	if (!priv)
+		return IRQ_NONE;
+
+	spin_lock(&priv->lock);
+
+	/* Disable (but don't clear!) interrupts here to avoid
+	 *    back-to-back ISRs and sporadic interrupts from our NIC.
+	 * If we have something to service, the tasklet will re-enable ints.
+	 * If we *don't* have something, we'll re-enable before leaving here. */
+	inta_mask = iwl_read32(priv, CSR_INT_MASK);  /* just for debug */
+	iwl_write32(priv, CSR_INT_MASK, 0x00000000);
+
+	/* Discover which interrupts are active/pending */
+	inta = iwl_read32(priv, CSR_INT);
+	inta_fh = iwl_read32(priv, CSR_FH_INT_STATUS);
+
+	/* Ignore interrupt if there's nothing in NIC to service.
+	 * This may be due to IRQ shared with another device,
+	 * or due to sporadic interrupts thrown from our NIC. */
+	if (!inta && !inta_fh) {
+		IWL_DEBUG_ISR("Ignore interrupt, inta == 0, inta_fh == 0\n");
+		goto none;
+	}
+
+	if ((inta == 0xFFFFFFFF) || ((inta & 0xFFFFFFF0) == 0xa5a5a5a0)) {
+		/* Hardware disappeared */
+		IWL_WARNING("HARDWARE GONE?? INTA == 0x%080x\n", inta);
+		goto none;
+	}
+
+	IWL_DEBUG_ISR("ISR inta 0x%08x, enabled 0x%08x, fh 0x%08x\n",
+		      inta, inta_mask, inta_fh);
+
+	/* iwl_irq_tasklet() will service interrupts and re-enable them */
+	tasklet_schedule(&priv->irq_tasklet);
+	spin_unlock(&priv->lock);
+
+	return IRQ_HANDLED;
+
+ none:
+	/* re-enable interrupts here since we don't have anything to service. */
+	iwl_enable_interrupts(priv);
+	spin_unlock(&priv->lock);
+	return IRQ_NONE;
+}
+
+/************************** EEPROM BANDS ****************************
+ *
+ * The iwl_eeprom_band definitions below provide the mapping from the
+ * EEPROM contents to the specific channel number supported for each
+ * band.
+ *
+ * For example, iwl_priv->eeprom.band_3_channels[4] from the band_3
+ * definition below maps to physical channel 42 in the 5.2GHz spectrum.
+ * The specific geography and calibration information for that channel
+ * is contained in the eeprom map itself.
+ *
+ * During init, we copy the eeprom information and channel map
+ * information into priv->channel_info_24/52 and priv->channel_map_24/52
+ *
+ * channel_map_24/52 provides the index in the channel_info array for a
+ * given channel.  We have to have two separate maps as there is channel
+ * overlap with the 2.4GHz and 5.2GHz spectrum as seen in band_1 and
+ * band_2
+ *
+ * A value of 0xff stored in the channel_map indicates that the channel
+ * is not supported by the hardware at all.
+ *
+ * A value of 0xfe in the channel_map indicates that the channel is not
+ * valid for Tx with the current hardware.  This means that
+ * while the system can tune and receive on a given channel, it may not
+ * be able to associate or transmit any frames on that
+ * channel.  There is no corresponding channel information for that
+ * entry.
+ *
+ *********************************************************************/
+
+/* 2.4 GHz */
+static const u8 iwl_eeprom_band_1[14] = {
+	1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14
+};
+
+/* 5.2 GHz bands */
+static const u8 iwl_eeprom_band_2[] = {
+	183, 184, 185, 187, 188, 189, 192, 196, 7, 8, 11, 12, 16
+};
+
+static const u8 iwl_eeprom_band_3[] = {	/* 5205-5320MHz */
+	34, 36, 38, 40, 42, 44, 46, 48, 52, 56, 60, 64
+};
+
+static const u8 iwl_eeprom_band_4[] = {	/* 5500-5700MHz */
+	100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140
+};
+
+static const u8 iwl_eeprom_band_5[] = {	/* 5725-5825MHz */
+	145, 149, 153, 157, 161, 165
+};
+
+static u8 iwl_eeprom_band_6[] = {       /* 2.4 FAT channel */
+	1, 2, 3, 4, 5, 6, 7
+};
+
+static u8 iwl_eeprom_band_7[] = {       /* 5.2 FAT channel */
+	36, 44, 52, 60, 100, 108, 116, 124, 132, 149, 157
+};
+
+static void iwl_init_band_reference(const struct iwl_priv *priv, int band,
+				    int *eeprom_ch_count,
+				    const struct iwl_eeprom_channel
+				    **eeprom_ch_info,
+				    const u8 **eeprom_ch_index)
+{
+	switch (band) {
+	case 1:		/* 2.4GHz band */
+		*eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_1);
+		*eeprom_ch_info = priv->eeprom.band_1_channels;
+		*eeprom_ch_index = iwl_eeprom_band_1;
+		break;
+	case 2:		/* 5.2GHz band */
+		*eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_2);
+		*eeprom_ch_info = priv->eeprom.band_2_channels;
+		*eeprom_ch_index = iwl_eeprom_band_2;
+		break;
+	case 3:		/* 5.2GHz band */
+		*eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_3);
+		*eeprom_ch_info = priv->eeprom.band_3_channels;
+		*eeprom_ch_index = iwl_eeprom_band_3;
+		break;
+	case 4:		/* 5.2GHz band */
+		*eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_4);
+		*eeprom_ch_info = priv->eeprom.band_4_channels;
+		*eeprom_ch_index = iwl_eeprom_band_4;
+		break;
+	case 5:		/* 5.2GHz band */
+		*eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_5);
+		*eeprom_ch_info = priv->eeprom.band_5_channels;
+		*eeprom_ch_index = iwl_eeprom_band_5;
+		break;
+	case 6:
+		*eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_6);
+		*eeprom_ch_info = priv->eeprom.band_24_channels;
+		*eeprom_ch_index = iwl_eeprom_band_6;
+		break;
+	case 7:
+		*eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_7);
+		*eeprom_ch_info = priv->eeprom.band_52_channels;
+		*eeprom_ch_index = iwl_eeprom_band_7;
+		break;
+	default:
+		BUG();
+		return;
+	}
+}
+
+const struct iwl_channel_info *iwl_get_channel_info(const struct iwl_priv *priv,
+						    int phymode, u16 channel)
+{
+	int i;
+
+	switch (phymode) {
+	case MODE_IEEE80211A:
+		for (i = 14; i < priv->channel_count; i++) {
+			if (priv->channel_info[i].channel == channel)
+				return &priv->channel_info[i];
+		}
+		break;
+
+	case MODE_IEEE80211B:
+	case MODE_IEEE80211G:
+		if (channel >= 1 && channel <= 14)
+			return &priv->channel_info[channel - 1];
+		break;
+
+	}
+
+	return NULL;
+}
+
+#define CHECK_AND_PRINT(x) ((eeprom_ch_info[ch].flags & EEPROM_CHANNEL_##x) \
+			    ? # x " " : "")
+
+static int iwl_init_channel_map(struct iwl_priv *priv)
+{
+	int eeprom_ch_count = 0;
+	const u8 *eeprom_ch_index = NULL;
+	const struct iwl_eeprom_channel *eeprom_ch_info = NULL;
+	int band, ch;
+	struct iwl_channel_info *ch_info;
+
+	if (priv->channel_count) {
+		IWL_DEBUG_INFO("Channel map already initialized.\n");
+		return 0;
+	}
+
+	if (priv->eeprom.version < 0x2f) {
+		IWL_WARNING("Unsupported EEPROM version: 0x%04X\n",
+			    priv->eeprom.version);
+		return -EINVAL;
+	}
+
+	IWL_DEBUG_INFO("Initializing regulatory info from EEPROM\n");
+
+	priv->channel_count =
+	    ARRAY_SIZE(iwl_eeprom_band_1) +
+	    ARRAY_SIZE(iwl_eeprom_band_2) +
+	    ARRAY_SIZE(iwl_eeprom_band_3) +
+	    ARRAY_SIZE(iwl_eeprom_band_4) +
+	    ARRAY_SIZE(iwl_eeprom_band_5);
+
+	IWL_DEBUG_INFO("Parsing data for %d channels.\n", priv->channel_count);
+
+	priv->channel_info = kzalloc(sizeof(struct iwl_channel_info) *
+				     priv->channel_count, GFP_KERNEL);
+	if (!priv->channel_info) {
+		IWL_ERROR("Could not allocate channel_info\n");
+		priv->channel_count = 0;
+		return -ENOMEM;
+	}
+
+	ch_info = priv->channel_info;
+
+	/* Loop through the 5 EEPROM bands adding them in order to the
+	 * channel map we maintain (that contains additional information than
+	 * what just in the EEPROM) */
+	for (band = 1; band <= 5; band++) {
+
+		iwl_init_band_reference(priv, band, &eeprom_ch_count,
+					&eeprom_ch_info, &eeprom_ch_index);
+
+		/* Loop through each band adding each of the channels */
+		for (ch = 0; ch < eeprom_ch_count; ch++) {
+			ch_info->channel = eeprom_ch_index[ch];
+			ch_info->phymode = (band == 1) ? MODE_IEEE80211B :
+			    MODE_IEEE80211A;
+
+			/* permanently store EEPROM's channel regulatory flags
+			 *   and max power in channel info database. */
+			ch_info->eeprom = eeprom_ch_info[ch];
+
+			/* Copy the run-time flags so they are there even on
+			 * invalid channels */
+			ch_info->flags = eeprom_ch_info[ch].flags;
+
+			if (!(is_channel_valid(ch_info))) {
+				IWL_DEBUG_INFO("Ch. %d Flags %x [%sGHz] - "
+					       "No traffic\n",
+					       ch_info->channel,
+					       ch_info->flags,
+					       is_channel_a_band(ch_info) ?
+					       "5.2" : "2.4");
+				ch_info++;
+				continue;
+			}
+
+			/* Initialize regulatory-based run-time data */
+			ch_info->max_power_avg = ch_info->curr_txpow =
+			    eeprom_ch_info[ch].max_power_avg;
+			ch_info->scan_power = eeprom_ch_info[ch].max_power_avg;
+			ch_info->min_power = 0;
+
+			IWL_DEBUG_INFO("Ch. %d [%sGHz] %s%s%s%s%s%s(0x%02x"
+				       " %ddBm): Ad-Hoc %ssupported\n",
+				       ch_info->channel,
+				       is_channel_a_band(ch_info) ?
+				       "5.2" : "2.4",
+				       CHECK_AND_PRINT(IBSS),
+				       CHECK_AND_PRINT(ACTIVE),
+				       CHECK_AND_PRINT(RADAR),
+				       CHECK_AND_PRINT(WIDE),
+				       CHECK_AND_PRINT(NARROW),
+				       CHECK_AND_PRINT(DFS),
+				       eeprom_ch_info[ch].flags,
+				       eeprom_ch_info[ch].max_power_avg,
+				       ((eeprom_ch_info[ch].
+					 flags & EEPROM_CHANNEL_IBSS)
+					&& !(eeprom_ch_info[ch].
+					     flags & EEPROM_CHANNEL_RADAR))
+				       ? "" : "not ");
+
+			/* Set the user_txpower_limit to the highest power
+			 * supported by any channel */
+			if (eeprom_ch_info[ch].max_power_avg >
+			    priv->user_txpower_limit)
+				priv->user_txpower_limit =
+				    eeprom_ch_info[ch].max_power_avg;
+
+			ch_info++;
+		}
+	}
+
+	for (band = 6; band <= 7; band++) {
+		int phymode;
+		u8 fat_extension_chan;
+
+		iwl_init_band_reference(priv, band, &eeprom_ch_count,
+					&eeprom_ch_info, &eeprom_ch_index);
+
+		phymode = (band == 6) ? MODE_IEEE80211B : MODE_IEEE80211A;
+		/* Loop through each band adding each of the channels */
+		for (ch = 0; ch < eeprom_ch_count; ch++) {
+
+			if ((band == 6) &&
+			    ((eeprom_ch_index[ch] == 5) ||
+			    (eeprom_ch_index[ch] == 6) ||
+			    (eeprom_ch_index[ch] == 7)))
+			       fat_extension_chan = HT_IE_EXT_CHANNEL_MAX;
+			else
+				fat_extension_chan = HT_IE_EXT_CHANNEL_ABOVE;
+
+			iwl4965_set_fat_chan_info(priv, phymode,
+						  eeprom_ch_index[ch],
+						  &(eeprom_ch_info[ch]),
+						  fat_extension_chan);
+
+			iwl4965_set_fat_chan_info(priv, phymode,
+						  (eeprom_ch_index[ch] + 4),
+						  &(eeprom_ch_info[ch]),
+						  HT_IE_EXT_CHANNEL_BELOW);
+		}
+	}
+
+	return 0;
+}
+
+/* For active scan, listen ACTIVE_DWELL_TIME (msec) on each channel after
+ * sending probe req.  This should be set long enough to hear probe responses
+ * from more than one AP.  */
+#define IWL_ACTIVE_DWELL_TIME_24    (20)	/* all times in msec */
+#define IWL_ACTIVE_DWELL_TIME_52    (10)
+
+/* For faster active scanning, scan will move to the next channel if fewer than
+ * PLCP_QUIET_THRESH packets are heard on this channel within
+ * ACTIVE_QUIET_TIME after sending probe request.  This shortens the dwell
+ * time if it's a quiet channel (nothing responded to our probe, and there's
+ * no other traffic).
+ * Disable "quiet" feature by setting PLCP_QUIET_THRESH to 0. */
+#define IWL_PLCP_QUIET_THRESH       __constant_cpu_to_le16(1)	/* packets */
+#define IWL_ACTIVE_QUIET_TIME       __constant_cpu_to_le16(5)	/* msec */
+
+/* For passive scan, listen PASSIVE_DWELL_TIME (msec) on each channel.
+ * Must be set longer than active dwell time.
+ * For the most reliable scan, set > AP beacon interval (typically 100msec). */
+#define IWL_PASSIVE_DWELL_TIME_24   (20)	/* all times in msec */
+#define IWL_PASSIVE_DWELL_TIME_52   (10)
+#define IWL_PASSIVE_DWELL_BASE      (100)
+#define IWL_CHANNEL_TUNE_TIME       5
+
+static inline u16 iwl_get_active_dwell_time(struct iwl_priv *priv, int phymode)
+{
+	if (phymode == MODE_IEEE80211A)
+		return IWL_ACTIVE_DWELL_TIME_52;
+	else
+		return IWL_ACTIVE_DWELL_TIME_24;
+}
+
+static u16 iwl_get_passive_dwell_time(struct iwl_priv *priv, int phymode)
+{
+	u16 active = iwl_get_active_dwell_time(priv, phymode);
+	u16 passive = (phymode != MODE_IEEE80211A) ?
+	    IWL_PASSIVE_DWELL_BASE + IWL_PASSIVE_DWELL_TIME_24 :
+	    IWL_PASSIVE_DWELL_BASE + IWL_PASSIVE_DWELL_TIME_52;
+
+	if (iwl_is_associated(priv)) {
+		/* If we're associated, we clamp the maximum passive
+		 * dwell time to be 98% of the beacon interval (minus
+		 * 2 * channel tune time) */
+		passive = priv->beacon_int;
+		if ((passive > IWL_PASSIVE_DWELL_BASE) || !passive)
+			passive = IWL_PASSIVE_DWELL_BASE;
+		passive = (passive * 98) / 100 - IWL_CHANNEL_TUNE_TIME * 2;
+	}
+
+	if (passive <= active)
+		passive = active + 1;
+
+	return passive;
+}
+
+static int iwl_get_channels_for_scan(struct iwl_priv *priv, int phymode,
+				     u8 is_active, u8 direct_mask,
+				     struct iwl_scan_channel *scan_ch)
+{
+	const struct ieee80211_channel *channels = NULL;
+	const struct ieee80211_hw_mode *hw_mode;
+	const struct iwl_channel_info *ch_info;
+	u16 passive_dwell = 0;
+	u16 active_dwell = 0;
+	int added, i;
+
+	hw_mode = iwl_get_hw_mode(priv, phymode);
+	if (!hw_mode)
+		return 0;
+
+	channels = hw_mode->channels;
+
+	active_dwell = iwl_get_active_dwell_time(priv, phymode);
+	passive_dwell = iwl_get_passive_dwell_time(priv, phymode);
+
+	for (i = 0, added = 0; i < hw_mode->num_channels; i++) {
+		if (channels[i].chan ==
+		    le16_to_cpu(priv->active_rxon.channel)) {
+			if (iwl_is_associated(priv)) {
+				IWL_DEBUG_SCAN
+				    ("Skipping current channel %d\n",
+				     le16_to_cpu(priv->active_rxon.channel));
+				continue;
+			}
+		} else if (priv->only_active_channel)
+			continue;
+
+		scan_ch->channel = channels[i].chan;
+
+		ch_info = iwl_get_channel_info(priv, phymode, scan_ch->channel);
+		if (!is_channel_valid(ch_info)) {
+			IWL_DEBUG_SCAN("Channel %d is INVALID for this SKU.\n",
+				       scan_ch->channel);
+			continue;
+		}
+
+		if (!is_active || is_channel_passive(ch_info) ||
+		    !(channels[i].flag & IEEE80211_CHAN_W_ACTIVE_SCAN))
+			scan_ch->type = 0;	/* passive */
+		else
+			scan_ch->type = 1;	/* active */
+
+		if (scan_ch->type & 1)
+			scan_ch->type |= (direct_mask << 1);
+
+		if (is_channel_narrow(ch_info))
+			scan_ch->type |= (1 << 7);
+
+		scan_ch->active_dwell = cpu_to_le16(active_dwell);
+		scan_ch->passive_dwell = cpu_to_le16(passive_dwell);
+
+		/* Set power levels to defaults */
+		scan_ch->tpc.dsp_atten = 110;
+		/* scan_pwr_info->tpc.dsp_atten; */
+
+		/*scan_pwr_info->tpc.tx_gain; */
+		if (phymode == MODE_IEEE80211A)
+			scan_ch->tpc.tx_gain = ((1 << 5) | (3 << 3)) | 3;
+		else {
+			scan_ch->tpc.tx_gain = ((1 << 5) | (5 << 3));
+			/* NOTE: if we were doing 6Mb OFDM for scans we'd use
+			 * power level
+			 scan_ch->tpc.tx_gain = ((1<<5) | (2 << 3)) | 3;
+			 */
+		}
+
+		IWL_DEBUG_SCAN("Scanning %d [%s %d]\n",
+			       scan_ch->channel,
+			       (scan_ch->type & 1) ? "ACTIVE" : "PASSIVE",
+			       (scan_ch->type & 1) ?
+			       active_dwell : passive_dwell);
+
+		scan_ch++;
+		added++;
+	}
+
+	IWL_DEBUG_SCAN("total channels to scan %d \n", added);
+	return added;
+}
+
+static void iwl_reset_channel_flag(struct iwl_priv *priv)
+{
+	int i, j;
+	for (i = 0; i < 3; i++) {
+		struct ieee80211_hw_mode *hw_mode = (void *)&priv->modes[i];
+		for (j = 0; j < hw_mode->num_channels; j++)
+			hw_mode->channels[j].flag = hw_mode->channels[j].val;
+	}
+}
+
+static void iwl_init_hw_rates(struct iwl_priv *priv,
+			      struct ieee80211_rate *rates)
+{
+	int i;
+
+	for (i = 0; i < IWL_RATE_COUNT; i++) {
+		rates[i].rate = iwl_rates[i].ieee * 5;
+		rates[i].val = i; /* Rate scaling will work on indexes */
+		rates[i].val2 = i;
+		rates[i].flags = IEEE80211_RATE_SUPPORTED;
+		/* Only OFDM have the bits-per-symbol set */
+		if ((i <= IWL_LAST_OFDM_RATE) && (i >= IWL_FIRST_OFDM_RATE))
+			rates[i].flags |= IEEE80211_RATE_OFDM;
+		else {
+			/*
+			 * If CCK 1M then set rate flag to CCK else CCK_2
+			 * which is CCK | PREAMBLE2
+			 */
+			rates[i].flags |= (iwl_rates[i].plcp == 10) ?
+				IEEE80211_RATE_CCK : IEEE80211_RATE_CCK_2;
+		}
+
+		/* Set up which ones are basic rates... */
+		if (IWL_BASIC_RATES_MASK & (1 << i))
+			rates[i].flags |= IEEE80211_RATE_BASIC;
+	}
+
+	iwl4965_init_hw_rates(priv, rates);
+}
+
+/**
+ * iwl_init_geos - Initialize mac80211's geo/channel info based from eeprom
+ */
+static int iwl_init_geos(struct iwl_priv *priv)
+{
+	struct iwl_channel_info *ch;
+	struct ieee80211_hw_mode *modes;
+	struct ieee80211_channel *channels;
+	struct ieee80211_channel *geo_ch;
+	struct ieee80211_rate *rates;
+	int i = 0;
+	enum {
+		A = 0,
+		B = 1,
+		G = 2,
+		A_11N = 3,
+		G_11N = 4,
+	};
+	int mode_count = 5;
+
+	if (priv->modes) {
+		IWL_DEBUG_INFO("Geography modes already initialized.\n");
+		set_bit(STATUS_GEO_CONFIGURED, &priv->status);
+		return 0;
+	}
+
+	modes = kzalloc(sizeof(struct ieee80211_hw_mode) * mode_count,
+			GFP_KERNEL);
+	if (!modes)
+		return -ENOMEM;
+
+	channels = kzalloc(sizeof(struct ieee80211_channel) *
+			   priv->channel_count, GFP_KERNEL);
+	if (!channels) {
+		kfree(modes);
+		return -ENOMEM;
+	}
+
+	rates = kzalloc((sizeof(struct ieee80211_rate) * (IWL_MAX_RATES + 1)),
+			GFP_KERNEL);
+	if (!rates) {
+		kfree(modes);
+		kfree(channels);
+		return -ENOMEM;
+	}
+
+	/* 0 = 802.11a
+	 * 1 = 802.11b
+	 * 2 = 802.11g
+	 */
+
+	/* 5.2GHz channels start after the 2.4GHz channels */
+	modes[A].mode = MODE_IEEE80211A;
+	modes[A].channels = &channels[ARRAY_SIZE(iwl_eeprom_band_1)];
+	modes[A].rates = rates;
+	modes[A].num_rates = 8;	/* just OFDM */
+	modes[A].rates = &rates[4];
+	modes[A].num_channels = 0;
+
+	modes[B].mode = MODE_IEEE80211B;
+	modes[B].channels = channels;
+	modes[B].rates = rates;
+	modes[B].num_rates = 4;	/* just CCK */
+	modes[B].num_channels = 0;
+
+	modes[G].mode = MODE_IEEE80211G;
+	modes[G].channels = channels;
+	modes[G].rates = rates;
+	modes[G].num_rates = 12;	/* OFDM & CCK */
+	modes[G].num_channels = 0;
+
+	modes[G_11N].mode = MODE_IEEE80211G;
+	modes[G_11N].channels = channels;
+	modes[G_11N].num_rates = 13;        /* OFDM & CCK */
+	modes[G_11N].rates = rates;
+	modes[G_11N].num_channels = 0;
+
+	modes[A_11N].mode = MODE_IEEE80211A;
+	modes[A_11N].channels = &channels[ARRAY_SIZE(iwl_eeprom_band_1)];
+	modes[A_11N].rates = &rates[4];
+	modes[A_11N].num_rates = 9; /* just OFDM */
+	modes[A_11N].num_channels = 0;
+
+	priv->ieee_channels = channels;
+	priv->ieee_rates = rates;
+
+	iwl_init_hw_rates(priv, rates);
+
+	for (i = 0, geo_ch = channels; i < priv->channel_count; i++) {
+		ch = &priv->channel_info[i];
+
+		if (!is_channel_valid(ch)) {
+			IWL_DEBUG_INFO("Channel %d [%sGHz] is restricted -- "
+				    "skipping.\n",
+				    ch->channel, is_channel_a_band(ch) ?
+				    "5.2" : "2.4");
+			continue;
+		}
+
+		if (is_channel_a_band(ch)) {
+			geo_ch = &modes[A].channels[modes[A].num_channels++];
+			modes[A_11N].num_channels++;
+		} else {
+			geo_ch = &modes[B].channels[modes[B].num_channels++];
+			modes[G].num_channels++;
+			modes[G_11N].num_channels++;
+		}
+
+		geo_ch->freq = ieee80211chan2mhz(ch->channel);
+		geo_ch->chan = ch->channel;
+		geo_ch->power_level = ch->max_power_avg;
+		geo_ch->antenna_max = 0xff;
+
+		if (is_channel_valid(ch)) {
+			geo_ch->flag = IEEE80211_CHAN_W_SCAN;
+			if (ch->flags & EEPROM_CHANNEL_IBSS)
+				geo_ch->flag |= IEEE80211_CHAN_W_IBSS;
+
+			if (ch->flags & EEPROM_CHANNEL_ACTIVE)
+				geo_ch->flag |= IEEE80211_CHAN_W_ACTIVE_SCAN;
+
+			if (ch->flags & EEPROM_CHANNEL_RADAR)
+				geo_ch->flag |= IEEE80211_CHAN_W_RADAR_DETECT;
+
+			if (ch->max_power_avg > priv->max_channel_txpower_limit)
+				priv->max_channel_txpower_limit =
+				    ch->max_power_avg;
+		}
+
+		geo_ch->val = geo_ch->flag;
+	}
+
+	if ((modes[A].num_channels == 0) && priv->is_abg) {
+		printk(KERN_INFO DRV_NAME
+		       ": Incorrectly detected BG card as ABG.  Please send "
+		       "your PCI ID 0x%04X:0x%04X to maintainer.\n",
+		       priv->pci_dev->device, priv->pci_dev->subsystem_device);
+		priv->is_abg = 0;
+	}
+
+	printk(KERN_INFO DRV_NAME
+	       ": Tunable channels: %d 802.11bg, %d 802.11a channels\n",
+	       modes[G].num_channels, modes[A].num_channels);
+
+	/*
+	 * NOTE:  We register these in preference of order -- the
+	 * stack doesn't currently (as of 7.0.6 / Apr 24 '07) pick
+	 * a phymode based on rates or AP capabilities but seems to
+	 * configure it purely on if the channel being configured
+	 * is supported by a mode -- and the first match is taken
+	 */
+
+	if (modes[G].num_channels)
+		ieee80211_register_hwmode(priv->hw, &modes[G]);
+	if (modes[B].num_channels)
+		ieee80211_register_hwmode(priv->hw, &modes[B]);
+	if (modes[A].num_channels)
+		ieee80211_register_hwmode(priv->hw, &modes[A]);
+
+	priv->modes = modes;
+	set_bit(STATUS_GEO_CONFIGURED, &priv->status);
+
+	return 0;
+}
+
+/******************************************************************************
+ *
+ * uCode download functions
+ *
+ ******************************************************************************/
+
+static void iwl_dealloc_ucode_pci(struct iwl_priv *priv)
+{
+	if (priv->ucode_code.v_addr != NULL) {
+		pci_free_consistent(priv->pci_dev,
+				    priv->ucode_code.len,
+				    priv->ucode_code.v_addr,
+				    priv->ucode_code.p_addr);
+		priv->ucode_code.v_addr = NULL;
+	}
+	if (priv->ucode_data.v_addr != NULL) {
+		pci_free_consistent(priv->pci_dev,
+				    priv->ucode_data.len,
+				    priv->ucode_data.v_addr,
+				    priv->ucode_data.p_addr);
+		priv->ucode_data.v_addr = NULL;
+	}
+	if (priv->ucode_data_backup.v_addr != NULL) {
+		pci_free_consistent(priv->pci_dev,
+				    priv->ucode_data_backup.len,
+				    priv->ucode_data_backup.v_addr,
+				    priv->ucode_data_backup.p_addr);
+		priv->ucode_data_backup.v_addr = NULL;
+	}
+	if (priv->ucode_init.v_addr != NULL) {
+		pci_free_consistent(priv->pci_dev,
+				    priv->ucode_init.len,
+				    priv->ucode_init.v_addr,
+				    priv->ucode_init.p_addr);
+		priv->ucode_init.v_addr = NULL;
+	}
+	if (priv->ucode_init_data.v_addr != NULL) {
+		pci_free_consistent(priv->pci_dev,
+				    priv->ucode_init_data.len,
+				    priv->ucode_init_data.v_addr,
+				    priv->ucode_init_data.p_addr);
+		priv->ucode_init_data.v_addr = NULL;
+	}
+	if (priv->ucode_boot.v_addr != NULL) {
+		pci_free_consistent(priv->pci_dev,
+				    priv->ucode_boot.len,
+				    priv->ucode_boot.v_addr,
+				    priv->ucode_boot.p_addr);
+		priv->ucode_boot.v_addr = NULL;
+	}
+}
+
+/**
+ * iwl_verify_inst_full - verify runtime uCode image in card vs. host,
+ *     looking at all data.
+ */
+static int iwl_verify_inst_full(struct iwl_priv *priv, __le32 * image, u32 len)
+{
+	u32 val;
+	u32 save_len = len;
+	int rc = 0;
+	u32 errcnt;
+
+	IWL_DEBUG_INFO("ucode inst image size is %u\n", len);
+
+	rc = iwl_grab_restricted_access(priv);
+	if (rc)
+		return rc;
+
+	iwl_write_restricted(priv, HBUS_TARG_MEM_RADDR, RTC_INST_LOWER_BOUND);
+
+	errcnt = 0;
+	for (; len > 0; len -= sizeof(u32), image++) {
+		/* read data comes through single port, auto-incr addr */
+		/* NOTE: Use the debugless read so we don't flood kernel log
+		 * if IWL_DL_IO is set */
+		val = _iwl_read_restricted(priv, HBUS_TARG_MEM_RDAT);
+		if (val != le32_to_cpu(*image)) {
+			IWL_ERROR("uCode INST section is invalid at "
+				  "offset 0x%x, is 0x%x, s/b 0x%x\n",
+				  save_len - len, val, le32_to_cpu(*image));
+			rc = -EIO;
+			errcnt++;
+			if (errcnt >= 20)
+				break;
+		}
+	}
+
+	iwl_release_restricted_access(priv);
+
+	if (!errcnt)
+		IWL_DEBUG_INFO
+		    ("ucode image in INSTRUCTION memory is good\n");
+
+	return rc;
+}
+
+
+/**
+ * iwl_verify_inst_sparse - verify runtime uCode image in card vs. host,
+ *   using sample data 100 bytes apart.  If these sample points are good,
+ *   it's a pretty good bet that everything between them is good, too.
+ */
+static int iwl_verify_inst_sparse(struct iwl_priv *priv, __le32 *image, u32 len)
+{
+	u32 val;
+	int rc = 0;
+	u32 errcnt = 0;
+	u32 i;
+
+	IWL_DEBUG_INFO("ucode inst image size is %u\n", len);
+
+	rc = iwl_grab_restricted_access(priv);
+	if (rc)
+		return rc;
+
+	for (i = 0; i < len; i += 100, image += 100/sizeof(u32)) {
+		/* read data comes through single port, auto-incr addr */
+		/* NOTE: Use the debugless read so we don't flood kernel log
+		 * if IWL_DL_IO is set */
+		iwl_write_restricted(priv, HBUS_TARG_MEM_RADDR,
+			i + RTC_INST_LOWER_BOUND);
+		val = _iwl_read_restricted(priv, HBUS_TARG_MEM_RDAT);
+		if (val != le32_to_cpu(*image)) {
+#if 0 /* Enable this if you want to see details */
+			IWL_ERROR("uCode INST section is invalid at "
+				  "offset 0x%x, is 0x%x, s/b 0x%x\n",
+				  i, val, *image);
+#endif
+			rc = -EIO;
+			errcnt++;
+			if (errcnt >= 3)
+				break;
+		}
+	}
+
+	iwl_release_restricted_access(priv);
+
+	return rc;
+}
+
+
+/**
+ * iwl_verify_ucode - determine which instruction image is in SRAM,
+ *    and verify its contents
+ */
+static int iwl_verify_ucode(struct iwl_priv *priv)
+{
+	__le32 *image;
+	u32 len;
+	int rc = 0;
+
+	/* Try bootstrap */
+	image = (__le32 *)priv->ucode_boot.v_addr;
+	len = priv->ucode_boot.len;
+	rc = iwl_verify_inst_sparse(priv, image, len);
+	if (rc == 0) {
+		IWL_DEBUG_INFO("Bootstrap uCode is good in inst SRAM\n");
+		return 0;
+	}
+
+	/* Try initialize */
+	image = (__le32 *)priv->ucode_init.v_addr;
+	len = priv->ucode_init.len;
+	rc = iwl_verify_inst_sparse(priv, image, len);
+	if (rc == 0) {
+		IWL_DEBUG_INFO("Initialize uCode is good in inst SRAM\n");
+		return 0;
+	}
+
+	/* Try runtime/protocol */
+	image = (__le32 *)priv->ucode_code.v_addr;
+	len = priv->ucode_code.len;
+	rc = iwl_verify_inst_sparse(priv, image, len);
+	if (rc == 0) {
+		IWL_DEBUG_INFO("Runtime uCode is good in inst SRAM\n");
+		return 0;
+	}
+
+	IWL_ERROR("NO VALID UCODE IMAGE IN INSTRUCTION SRAM!!\n");
+
+	/* Show first several data entries in instruction SRAM.
+	 * Selection of bootstrap image is arbitrary. */
+	image = (__le32 *)priv->ucode_boot.v_addr;
+	len = priv->ucode_boot.len;
+	rc = iwl_verify_inst_full(priv, image, len);
+
+	return rc;
+}
+
+
+/* check contents of special bootstrap uCode SRAM */
+static int iwl_verify_bsm(struct iwl_priv *priv)
+{
+	__le32 *image = priv->ucode_boot.v_addr;
+	u32 len = priv->ucode_boot.len;
+	u32 reg;
+	u32 val;
+
+	IWL_DEBUG_INFO("Begin verify bsm\n");
+
+	/* verify BSM SRAM contents */
+	val = iwl_read_restricted_reg(priv, BSM_WR_DWCOUNT_REG);
+	for (reg = BSM_SRAM_LOWER_BOUND;
+	     reg < BSM_SRAM_LOWER_BOUND + len;
+	     reg += sizeof(u32), image ++) {
+		val = iwl_read_restricted_reg(priv, reg);
+		if (val != le32_to_cpu(*image)) {
+			IWL_ERROR("BSM uCode verification failed at "
+				  "addr 0x%08X+%u (of %u), is 0x%x, s/b 0x%x\n",
+				  BSM_SRAM_LOWER_BOUND,
+				  reg - BSM_SRAM_LOWER_BOUND, len,
+				  val, le32_to_cpu(*image));
+			return -EIO;
+		}
+	}
+
+	IWL_DEBUG_INFO("BSM bootstrap uCode image OK\n");
+
+	return 0;
+}
+
+/**
+ * iwl_load_bsm - Load bootstrap instructions
+ *
+ * BSM operation:
+ *
+ * The Bootstrap State Machine (BSM) stores a short bootstrap uCode program
+ * in special SRAM that does not power down during RFKILL.  When powering back
+ * up after power-saving sleeps (or during initial uCode load), the BSM loads
+ * the bootstrap program into the on-board processor, and starts it.
+ *
+ * The bootstrap program loads (via DMA) instructions and data for a new
+ * program from host DRAM locations indicated by the host driver in the
+ * BSM_DRAM_* registers.  Once the new program is loaded, it starts
+ * automatically.
+ *
+ * When initializing the NIC, the host driver points the BSM to the
+ * "initialize" uCode image.  This uCode sets up some internal data, then
+ * notifies host via "initialize alive" that it is complete.
+ *
+ * The host then replaces the BSM_DRAM_* pointer values to point to the
+ * normal runtime uCode instructions and a backup uCode data cache buffer
+ * (filled initially with starting data values for the on-board processor),
+ * then triggers the "initialize" uCode to load and launch the runtime uCode,
+ * which begins normal operation.
+ *
+ * When doing a power-save shutdown, runtime uCode saves data SRAM into
+ * the backup data cache in DRAM before SRAM is powered down.
+ *
+ * When powering back up, the BSM loads the bootstrap program.  This reloads
+ * the runtime uCode instructions and the backup data cache into SRAM,
+ * and re-launches the runtime uCode from where it left off.
+ */
+static int iwl_load_bsm(struct iwl_priv *priv)
+{
+	__le32 *image = priv->ucode_boot.v_addr;
+	u32 len = priv->ucode_boot.len;
+	dma_addr_t pinst;
+	dma_addr_t pdata;
+	u32 inst_len;
+	u32 data_len;
+	int rc;
+	int i;
+	u32 done;
+	u32 reg_offset;
+
+	IWL_DEBUG_INFO("Begin load bsm\n");
+
+	/* make sure bootstrap program is no larger than BSM's SRAM size */
+	if (len > IWL_MAX_BSM_SIZE)
+		return -EINVAL;
+
+	/* Tell bootstrap uCode where to find the "Initialize" uCode
+	 *   in host DRAM ... bits 31:0 for 3945, bits 35:4 for 4965.
+	 * NOTE:  iwl_initialize_alive_start() will replace these values,
+	 *        after the "initialize" uCode has run, to point to
+	 *        runtime/protocol instructions and backup data cache. */
+	pinst = priv->ucode_init.p_addr >> 4;
+	pdata = priv->ucode_init_data.p_addr >> 4;
+	inst_len = priv->ucode_init.len;
+	data_len = priv->ucode_init_data.len;
+
+	rc = iwl_grab_restricted_access(priv);
+	if (rc)
+		return rc;
+
+	iwl_write_restricted_reg(priv, BSM_DRAM_INST_PTR_REG, pinst);
+	iwl_write_restricted_reg(priv, BSM_DRAM_DATA_PTR_REG, pdata);
+	iwl_write_restricted_reg(priv, BSM_DRAM_INST_BYTECOUNT_REG, inst_len);
+	iwl_write_restricted_reg(priv, BSM_DRAM_DATA_BYTECOUNT_REG, data_len);
+
+	/* Fill BSM memory with bootstrap instructions */
+	for (reg_offset = BSM_SRAM_LOWER_BOUND;
+	     reg_offset < BSM_SRAM_LOWER_BOUND + len;
+	     reg_offset += sizeof(u32), image++)
+		_iwl_write_restricted_reg(priv, reg_offset,
+					  le32_to_cpu(*image));
+
+	rc = iwl_verify_bsm(priv);
+	if (rc) {
+		iwl_release_restricted_access(priv);
+		return rc;
+	}
+
+	/* Tell BSM to copy from BSM SRAM into instruction SRAM, when asked */
+	iwl_write_restricted_reg(priv, BSM_WR_MEM_SRC_REG, 0x0);
+	iwl_write_restricted_reg(priv, BSM_WR_MEM_DST_REG,
+				 RTC_INST_LOWER_BOUND);
+	iwl_write_restricted_reg(priv, BSM_WR_DWCOUNT_REG, len / sizeof(u32));
+
+	/* Load bootstrap code into instruction SRAM now,
+	 *   to prepare to load "initialize" uCode */
+	iwl_write_restricted_reg(priv, BSM_WR_CTRL_REG,
+		BSM_WR_CTRL_REG_BIT_START);
+
+	/* Wait for load of bootstrap uCode to finish */
+	for (i = 0; i < 100; i++) {
+		done = iwl_read_restricted_reg(priv, BSM_WR_CTRL_REG);
+		if (!(done & BSM_WR_CTRL_REG_BIT_START))
+			break;
+		udelay(10);
+	}
+	if (i < 100)
+		IWL_DEBUG_INFO("BSM write complete, poll %d iterations\n", i);
+	else {
+		IWL_ERROR("BSM write did not complete!\n");
+		return -EIO;
+	}
+
+	/* Enable future boot loads whenever power management unit triggers it
+	 *   (e.g. when powering back up after power-save shutdown) */
+	iwl_write_restricted_reg(priv, BSM_WR_CTRL_REG,
+		BSM_WR_CTRL_REG_BIT_START_EN);
+
+	iwl_release_restricted_access(priv);
+
+	return 0;
+}
+
+static void iwl_nic_start(struct iwl_priv *priv)
+{
+	/* Remove all resets to allow NIC to operate */
+	iwl_write32(priv, CSR_RESET, 0);
+}
+
+/**
+ * iwl_read_ucode - Read uCode images from disk file.
+ *
+ * Copy into buffers for card to fetch via bus-mastering
+ */
+static int iwl_read_ucode(struct iwl_priv *priv)
+{
+	struct iwl_ucode *ucode;
+	int rc = 0;
+	const struct firmware *ucode_raw;
+	const char *name = "iwlwifi-4965" IWL4965_UCODE_API ".ucode";
+	u8 *src;
+	size_t len;
+	u32 ver, inst_size, data_size, init_size, init_data_size, boot_size;
+
+	/* Ask kernel firmware_class module to get the boot firmware off disk.
+	 * request_firmware() is synchronous, file is in memory on return. */
+	rc = request_firmware(&ucode_raw, name, &priv->pci_dev->dev);
+	if (rc < 0) {
+		IWL_ERROR("%s firmware file req failed: Reason %d\n", name, rc);
+		goto error;
+	}
+
+	IWL_DEBUG_INFO("Got firmware '%s' file (%zd bytes) from disk\n",
+		       name, ucode_raw->size);
+
+	/* Make sure that we got at least our header! */
+	if (ucode_raw->size < sizeof(*ucode)) {
+		IWL_ERROR("File size way too small!\n");
+		rc = -EINVAL;
+		goto err_release;
+	}
+
+	/* Data from ucode file:  header followed by uCode images */
+	ucode = (void *)ucode_raw->data;
+
+	ver = le32_to_cpu(ucode->ver);
+	inst_size = le32_to_cpu(ucode->inst_size);
+	data_size = le32_to_cpu(ucode->data_size);
+	init_size = le32_to_cpu(ucode->init_size);
+	init_data_size = le32_to_cpu(ucode->init_data_size);
+	boot_size = le32_to_cpu(ucode->boot_size);
+
+	IWL_DEBUG_INFO("f/w package hdr ucode version = 0x%x\n", ver);
+	IWL_DEBUG_INFO("f/w package hdr runtime inst size = %u\n",
+		       inst_size);
+	IWL_DEBUG_INFO("f/w package hdr runtime data size = %u\n",
+		       data_size);
+	IWL_DEBUG_INFO("f/w package hdr init inst size = %u\n",
+		       init_size);
+	IWL_DEBUG_INFO("f/w package hdr init data size = %u\n",
+		       init_data_size);
+	IWL_DEBUG_INFO("f/w package hdr boot inst size = %u\n",
+		       boot_size);
+
+	/* Verify size of file vs. image size info in file's header */
+	if (ucode_raw->size < sizeof(*ucode) +
+		inst_size + data_size + init_size +
+		init_data_size + boot_size) {
+
+		IWL_DEBUG_INFO("uCode file size %d too small\n",
+			       (int)ucode_raw->size);
+		rc = -EINVAL;
+		goto err_release;
+	}
+
+	/* Verify that uCode images will fit in card's SRAM */
+	if (inst_size > IWL_MAX_INST_SIZE) {
+		IWL_DEBUG_INFO("uCode instr len %d too large to fit in card\n",
+			       (int)inst_size);
+		rc = -EINVAL;
+		goto err_release;
+	}
+
+	if (data_size > IWL_MAX_DATA_SIZE) {
+		IWL_DEBUG_INFO("uCode data len %d too large to fit in card\n",
+			       (int)data_size);
+		rc = -EINVAL;
+		goto err_release;
+	}
+	if (init_size > IWL_MAX_INST_SIZE) {
+		IWL_DEBUG_INFO
+		    ("uCode init instr len %d too large to fit in card\n",
+		     (int)init_size);
+		rc = -EINVAL;
+		goto err_release;
+	}
+	if (init_data_size > IWL_MAX_DATA_SIZE) {
+		IWL_DEBUG_INFO
+		    ("uCode init data len %d too large to fit in card\n",
+		     (int)init_data_size);
+		rc = -EINVAL;
+		goto err_release;
+	}
+	if (boot_size > IWL_MAX_BSM_SIZE) {
+		IWL_DEBUG_INFO
+		    ("uCode boot instr len %d too large to fit in bsm\n",
+		     (int)boot_size);
+		rc = -EINVAL;
+		goto err_release;
+	}
+
+	/* Allocate ucode buffers for card's bus-master loading ... */
+
+	/* Runtime instructions and 2 copies of data:
+	 * 1) unmodified from disk
+	 * 2) backup cache for save/restore during power-downs */
+	priv->ucode_code.len = inst_size;
+	priv->ucode_code.v_addr =
+	    pci_alloc_consistent(priv->pci_dev,
+				 priv->ucode_code.len,
+				 &(priv->ucode_code.p_addr));
+
+	priv->ucode_data.len = data_size;
+	priv->ucode_data.v_addr =
+	    pci_alloc_consistent(priv->pci_dev,
+				 priv->ucode_data.len,
+				 &(priv->ucode_data.p_addr));
+
+	priv->ucode_data_backup.len = data_size;
+	priv->ucode_data_backup.v_addr =
+	    pci_alloc_consistent(priv->pci_dev,
+				 priv->ucode_data_backup.len,
+				 &(priv->ucode_data_backup.p_addr));
+
+
+	/* Initialization instructions and data */
+	priv->ucode_init.len = init_size;
+	priv->ucode_init.v_addr =
+	    pci_alloc_consistent(priv->pci_dev,
+				 priv->ucode_init.len,
+				 &(priv->ucode_init.p_addr));
+
+	priv->ucode_init_data.len = init_data_size;
+	priv->ucode_init_data.v_addr =
+	    pci_alloc_consistent(priv->pci_dev,
+				 priv->ucode_init_data.len,
+				 &(priv->ucode_init_data.p_addr));
+
+	/* Bootstrap (instructions only, no data) */
+	priv->ucode_boot.len = boot_size;
+	priv->ucode_boot.v_addr =
+	    pci_alloc_consistent(priv->pci_dev,
+				 priv->ucode_boot.len,
+				 &(priv->ucode_boot.p_addr));
+
+	if (!priv->ucode_code.v_addr || !priv->ucode_data.v_addr ||
+	    !priv->ucode_init.v_addr || !priv->ucode_init_data.v_addr ||
+	    !priv->ucode_boot.v_addr || !priv->ucode_data_backup.v_addr)
+		goto err_pci_alloc;
+
+	/* Copy images into buffers for card's bus-master reads ... */
+
+	/* Runtime instructions (first block of data in file) */
+	src = &ucode->data[0];
+	len = priv->ucode_code.len;
+	IWL_DEBUG_INFO("Copying (but not loading) uCode instr len %d\n",
+		       (int)len);
+	memcpy(priv->ucode_code.v_addr, src, len);
+	IWL_DEBUG_INFO("uCode instr buf vaddr = 0x%p, paddr = 0x%08x\n",
+		priv->ucode_code.v_addr, (u32)priv->ucode_code.p_addr);
+
+	/* Runtime data (2nd block)
+	 * NOTE:  Copy into backup buffer will be done in iwl_up()  */
+	src = &ucode->data[inst_size];
+	len = priv->ucode_data.len;
+	IWL_DEBUG_INFO("Copying (but not loading) uCode data len %d\n",
+		       (int)len);
+	memcpy(priv->ucode_data.v_addr, src, len);
+	memcpy(priv->ucode_data_backup.v_addr, src, len);
+
+	/* Initialization instructions (3rd block) */
+	if (init_size) {
+		src = &ucode->data[inst_size + data_size];
+		len = priv->ucode_init.len;
+		IWL_DEBUG_INFO("Copying (but not loading) init instr len %d\n",
+			       (int)len);
+		memcpy(priv->ucode_init.v_addr, src, len);
+	}
+
+	/* Initialization data (4th block) */
+	if (init_data_size) {
+		src = &ucode->data[inst_size + data_size + init_size];
+		len = priv->ucode_init_data.len;
+		IWL_DEBUG_INFO("Copying (but not loading) init data len %d\n",
+			       (int)len);
+		memcpy(priv->ucode_init_data.v_addr, src, len);
+	}
+
+	/* Bootstrap instructions (5th block) */
+	src = &ucode->data[inst_size + data_size + init_size + init_data_size];
+	len = priv->ucode_boot.len;
+	IWL_DEBUG_INFO("Copying (but not loading) boot instr len %d\n",
+		       (int)len);
+	memcpy(priv->ucode_boot.v_addr, src, len);
+
+	/* We have our copies now, allow OS release its copies */
+	release_firmware(ucode_raw);
+	return 0;
+
+ err_pci_alloc:
+	IWL_ERROR("failed to allocate pci memory\n");
+	rc = -ENOMEM;
+	iwl_dealloc_ucode_pci(priv);
+
+ err_release:
+	release_firmware(ucode_raw);
+
+ error:
+	return rc;
+}
+
+
+/**
+ * iwl_set_ucode_ptrs - Set uCode address location
+ *
+ * Tell initialization uCode where to find runtime uCode.
+ *
+ * BSM registers initially contain pointers to initialization uCode.
+ * We need to replace them to load runtime uCode inst and data,
+ * and to save runtime data when powering down.
+ */
+static int iwl_set_ucode_ptrs(struct iwl_priv *priv)
+{
+	dma_addr_t pinst;
+	dma_addr_t pdata;
+	int rc = 0;
+	unsigned long flags;
+
+	/* bits 35:4 for 4965 */
+	pinst = priv->ucode_code.p_addr >> 4;
+	pdata = priv->ucode_data_backup.p_addr >> 4;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	rc = iwl_grab_restricted_access(priv);
+	if (rc) {
+		spin_unlock_irqrestore(&priv->lock, flags);
+		return rc;
+	}
+
+	/* Tell bootstrap uCode where to find image to load */
+	iwl_write_restricted_reg(priv, BSM_DRAM_INST_PTR_REG, pinst);
+	iwl_write_restricted_reg(priv, BSM_DRAM_DATA_PTR_REG, pdata);
+	iwl_write_restricted_reg(priv, BSM_DRAM_DATA_BYTECOUNT_REG,
+				 priv->ucode_data.len);
+
+	/* Inst bytecount must be last to set up, bit 31 signals uCode
+	 *   that all new ptr/size info is in place */
+	iwl_write_restricted_reg(priv, BSM_DRAM_INST_BYTECOUNT_REG,
+				 priv->ucode_code.len | BSM_DRAM_INST_LOAD);
+
+	iwl_release_restricted_access(priv);
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	IWL_DEBUG_INFO("Runtime uCode pointers are set.\n");
+
+	return rc;
+}
+
+/**
+ * iwl_init_alive_start - Called after REPLY_ALIVE notification receieved
+ *
+ * Called after REPLY_ALIVE notification received from "initialize" uCode.
+ *
+ * The 4965 "initialize" ALIVE reply contains calibration data for:
+ *   Voltage, temperature, and MIMO tx gain correction, now stored in priv
+ *   (3945 does not contain this data).
+ *
+ * Tell "initialize" uCode to go ahead and load the runtime uCode.
+*/
+static void iwl_init_alive_start(struct iwl_priv *priv)
+{
+	/* Check alive response for "valid" sign from uCode */
+	if (priv->card_alive_init.is_valid != UCODE_VALID_OK) {
+		/* We had an error bringing up the hardware, so take it
+		 * all the way back down so we can try again */
+		IWL_DEBUG_INFO("Initialize Alive failed.\n");
+		goto restart;
+	}
+
+	/* Bootstrap uCode has loaded initialize uCode ... verify inst image.
+	 * This is a paranoid check, because we would not have gotten the
+	 * "initialize" alive if code weren't properly loaded.  */
+	if (iwl_verify_ucode(priv)) {
+		/* Runtime instruction load was bad;
+		 * take it all the way back down so we can try again */
+		IWL_DEBUG_INFO("Bad \"initialize\" uCode load.\n");
+		goto restart;
+	}
+
+	/* Calculate temperature */
+	priv->temperature = iwl4965_get_temperature(priv);
+
+	/* Send pointers to protocol/runtime uCode image ... init code will
+	 * load and launch runtime uCode, which will send us another "Alive"
+	 * notification. */
+	IWL_DEBUG_INFO("Initialization Alive received.\n");
+	if (iwl_set_ucode_ptrs(priv)) {
+		/* Runtime instruction load won't happen;
+		 * take it all the way back down so we can try again */
+		IWL_DEBUG_INFO("Couldn't set up uCode pointers.\n");
+		goto restart;
+	}
+	return;
+
+ restart:
+	queue_work(priv->workqueue, &priv->restart);
+}
+
+
+/**
+ * iwl_alive_start - called after REPLY_ALIVE notification received
+ *                   from protocol/runtime uCode (initialization uCode's
+ *                   Alive gets handled by iwl_init_alive_start()).
+ */
+static void iwl_alive_start(struct iwl_priv *priv)
+{
+	int rc = 0;
+
+	IWL_DEBUG_INFO("Runtime Alive received.\n");
+
+	if (priv->card_alive.is_valid != UCODE_VALID_OK) {
+		/* We had an error bringing up the hardware, so take it
+		 * all the way back down so we can try again */
+		IWL_DEBUG_INFO("Alive failed.\n");
+		goto restart;
+	}
+
+	/* Initialize uCode has loaded Runtime uCode ... verify inst image.
+	 * This is a paranoid check, because we would not have gotten the
+	 * "runtime" alive if code weren't properly loaded.  */
+	if (iwl_verify_ucode(priv)) {
+		/* Runtime instruction load was bad;
+		 * take it all the way back down so we can try again */
+		IWL_DEBUG_INFO("Bad runtime uCode load.\n");
+		goto restart;
+	}
+
+	iwl_clear_stations_table(priv);
+
+	rc = iwl4965_alive_notify(priv);
+	if (rc) {
+		IWL_WARNING("Could not complete ALIVE transition [ntf]: %d\n",
+			    rc);
+		goto restart;
+	}
+
+	/* After the ALIVE response, we can process host commands */
+	set_bit(STATUS_ALIVE, &priv->status);
+
+	/* Clear out the uCode error bit if it is set */
+	clear_bit(STATUS_FW_ERROR, &priv->status);
+
+	rc = iwl_init_channel_map(priv);
+	if (rc) {
+		IWL_ERROR("initializing regulatory failed: %d\n", rc);
+		return;
+	}
+
+	iwl_init_geos(priv);
+
+	if (iwl_is_rfkill(priv))
+		return;
+
+	if (!priv->mac80211_registered) {
+		/* Unlock so any user space entry points can call back into
+		 * the driver without a deadlock... */
+		mutex_unlock(&priv->mutex);
+		iwl_rate_control_register(priv->hw);
+		rc = ieee80211_register_hw(priv->hw);
+		priv->hw->conf.beacon_int = 100;
+		mutex_lock(&priv->mutex);
+
+		if (rc) {
+			IWL_ERROR("Failed to register network "
+				  "device (error %d)\n", rc);
+			return;
+		}
+
+		priv->mac80211_registered = 1;
+
+		iwl_reset_channel_flag(priv);
+	} else
+		ieee80211_start_queues(priv->hw);
+
+	priv->active_rate = priv->rates_mask;
+	priv->active_rate_basic = priv->rates_mask & IWL_BASIC_RATES_MASK;
+
+	iwl_send_power_mode(priv, IWL_POWER_LEVEL(priv->power_mode));
+
+	if (iwl_is_associated(priv)) {
+		struct iwl_rxon_cmd *active_rxon =
+				(struct iwl_rxon_cmd *)(&priv->active_rxon);
+
+		memcpy(&priv->staging_rxon, &priv->active_rxon,
+		       sizeof(priv->staging_rxon));
+		active_rxon->filter_flags &= ~RXON_FILTER_ASSOC_MSK;
+	} else {
+		/* Initialize our rx_config data */
+		iwl_connection_init_rx_config(priv);
+		memcpy(priv->staging_rxon.node_addr, priv->mac_addr, ETH_ALEN);
+	}
+
+	/* Configure BT coexistence */
+	iwl_send_bt_config(priv);
+
+	/* Configure the adapter for unassociated operation */
+	iwl_commit_rxon(priv);
+
+	/* At this point, the NIC is initialized and operational */
+	priv->notif_missed_beacons = 0;
+	set_bit(STATUS_READY, &priv->status);
+
+	iwl4965_rf_kill_ct_config(priv);
+	IWL_DEBUG_INFO("ALIVE processing complete.\n");
+
+	if (priv->error_recovering)
+		iwl_error_recovery(priv);
+
+	return;
+
+ restart:
+	queue_work(priv->workqueue, &priv->restart);
+}
+
+static void iwl_cancel_deferred_work(struct iwl_priv *priv);
+
+static void __iwl_down(struct iwl_priv *priv)
+{
+	unsigned long flags;
+	int exit_pending = test_bit(STATUS_EXIT_PENDING, &priv->status);
+	struct ieee80211_conf *conf = NULL;
+
+	IWL_DEBUG_INFO(DRV_NAME " is going down\n");
+
+	conf = ieee80211_get_hw_conf(priv->hw);
+
+	if (!exit_pending)
+		set_bit(STATUS_EXIT_PENDING, &priv->status);
+
+	iwl_clear_stations_table(priv);
+
+	/* Unblock any waiting calls */
+	wake_up_interruptible_all(&priv->wait_command_queue);
+
+	iwl_cancel_deferred_work(priv);
+
+	/* Wipe out the EXIT_PENDING status bit if we are not actually
+	 * exiting the module */
+	if (!exit_pending)
+		clear_bit(STATUS_EXIT_PENDING, &priv->status);
+
+	/* stop and reset the on-board processor */
+	iwl_write32(priv, CSR_RESET, CSR_RESET_REG_FLAG_NEVO_RESET);
+
+	/* tell the device to stop sending interrupts */
+	iwl_disable_interrupts(priv);
+
+	if (priv->mac80211_registered)
+		ieee80211_stop_queues(priv->hw);
+
+	/* If we have not previously called iwl_init() then
+	 * clear all bits but the RF Kill and SUSPEND bits and return */
+	if (!iwl_is_init(priv)) {
+		priv->status = test_bit(STATUS_RF_KILL_HW, &priv->status) <<
+					STATUS_RF_KILL_HW |
+			       test_bit(STATUS_RF_KILL_SW, &priv->status) <<
+					STATUS_RF_KILL_SW |
+			       test_bit(STATUS_IN_SUSPEND, &priv->status) <<
+					STATUS_IN_SUSPEND;
+		goto exit;
+	}
+
+	/* ...otherwise clear out all the status bits but the RF Kill and
+	 * SUSPEND bits and continue taking the NIC down. */
+	priv->status &= test_bit(STATUS_RF_KILL_HW, &priv->status) <<
+				STATUS_RF_KILL_HW |
+			test_bit(STATUS_RF_KILL_SW, &priv->status) <<
+				STATUS_RF_KILL_SW |
+			test_bit(STATUS_IN_SUSPEND, &priv->status) <<
+				STATUS_IN_SUSPEND |
+			test_bit(STATUS_FW_ERROR, &priv->status) <<
+				STATUS_FW_ERROR;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	iwl_clear_bit(priv, CSR_GP_CNTRL, CSR_GP_CNTRL_REG_FLAG_MAC_ACCESS_REQ);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	iwl_hw_txq_ctx_stop(priv);
+	iwl_hw_rxq_stop(priv);
+
+	spin_lock_irqsave(&priv->lock, flags);
+	if (!iwl_grab_restricted_access(priv)) {
+		iwl_write_restricted_reg(priv, APMG_CLK_DIS_REG,
+					 APMG_CLK_VAL_DMA_CLK_RQT);
+		iwl_release_restricted_access(priv);
+	}
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	udelay(5);
+
+	iwl_hw_nic_stop_master(priv);
+	iwl_set_bit(priv, CSR_RESET, CSR_RESET_REG_FLAG_SW_RESET);
+	iwl_hw_nic_reset(priv);
+
+ exit:
+	memset(&priv->card_alive, 0, sizeof(struct iwl_alive_resp));
+
+	if (priv->ibss_beacon)
+		dev_kfree_skb(priv->ibss_beacon);
+	priv->ibss_beacon = NULL;
+
+	/* clear out any free frames */
+	iwl_clear_free_frames(priv);
+}
+
+static void iwl_down(struct iwl_priv *priv)
+{
+	mutex_lock(&priv->mutex);
+	__iwl_down(priv);
+	mutex_unlock(&priv->mutex);
+}
+
+#define MAX_HW_RESTARTS 5
+
+static int __iwl_up(struct iwl_priv *priv)
+{
+	DECLARE_MAC_BUF(mac);
+	int rc, i;
+	u32 hw_rf_kill = 0;
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status)) {
+		IWL_WARNING("Exit pending; will not bring the NIC up\n");
+		return -EIO;
+	}
+
+	if (test_bit(STATUS_RF_KILL_SW, &priv->status)) {
+		IWL_WARNING("Radio disabled by SW RF kill (module "
+			    "parameter)\n");
+		return 0;
+	}
+
+	iwl_write32(priv, CSR_INT, 0xFFFFFFFF);
+
+	rc = iwl_hw_nic_init(priv);
+	if (rc) {
+		IWL_ERROR("Unable to int nic\n");
+		return rc;
+	}
+
+	/* make sure rfkill handshake bits are cleared */
+	iwl_write32(priv, CSR_UCODE_DRV_GP1_CLR, CSR_UCODE_SW_BIT_RFKILL);
+	iwl_write32(priv, CSR_UCODE_DRV_GP1_CLR,
+		    CSR_UCODE_DRV_GP1_BIT_CMD_BLOCKED);
+
+	/* clear (again), then enable host interrupts */
+	iwl_write32(priv, CSR_INT, 0xFFFFFFFF);
+	iwl_enable_interrupts(priv);
+
+	/* really make sure rfkill handshake bits are cleared */
+	iwl_write32(priv, CSR_UCODE_DRV_GP1_CLR, CSR_UCODE_SW_BIT_RFKILL);
+	iwl_write32(priv, CSR_UCODE_DRV_GP1_CLR, CSR_UCODE_SW_BIT_RFKILL);
+
+	/* Copy original ucode data image from disk into backup cache.
+	 * This will be used to initialize the on-board processor's
+	 * data SRAM for a clean start when the runtime program first loads. */
+	memcpy(priv->ucode_data_backup.v_addr, priv->ucode_data.v_addr,
+			priv->ucode_data.len);
+
+	/* If platform's RF_KILL switch is set to KILL,
+	 * wait for BIT_INT_RF_KILL interrupt before loading uCode
+	 * and getting things started */
+	if (!(iwl_read32(priv, CSR_GP_CNTRL) &
+				CSR_GP_CNTRL_REG_FLAG_HW_RF_KILL_SW))
+		hw_rf_kill = 1;
+
+	if (test_bit(STATUS_RF_KILL_HW, &priv->status) || hw_rf_kill) {
+		IWL_WARNING("Radio disabled by HW RF Kill switch\n");
+		return 0;
+	}
+
+	for (i = 0; i < MAX_HW_RESTARTS; i++) {
+
+		iwl_clear_stations_table(priv);
+
+		/* load bootstrap state machine,
+		 * load bootstrap program into processor's memory,
+		 * prepare to load the "initialize" uCode */
+		rc = iwl_load_bsm(priv);
+
+		if (rc) {
+			IWL_ERROR("Unable to set up bootstrap uCode: %d\n", rc);
+			continue;
+		}
+
+		/* start card; "initialize" will load runtime ucode */
+		iwl_nic_start(priv);
+
+		/* MAC Address location in EEPROM same for 3945/4965 */
+		get_eeprom_mac(priv, priv->mac_addr);
+		IWL_DEBUG_INFO("MAC address: %s\n",
+			       print_mac(mac, priv->mac_addr));
+
+		SET_IEEE80211_PERM_ADDR(priv->hw, priv->mac_addr);
+
+		IWL_DEBUG_INFO(DRV_NAME " is coming up\n");
+
+		return 0;
+	}
+
+	set_bit(STATUS_EXIT_PENDING, &priv->status);
+	__iwl_down(priv);
+
+	/* tried to restart and config the device for as long as our
+	 * patience could withstand */
+	IWL_ERROR("Unable to initialize device after %d attempts.\n", i);
+	return -EIO;
+}
+
+
+/*****************************************************************************
+ *
+ * Workqueue callbacks
+ *
+ *****************************************************************************/
+
+static void iwl_bg_init_alive_start(struct work_struct *data)
+{
+	struct iwl_priv *priv =
+	    container_of(data, struct iwl_priv, init_alive_start.work);
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+		return;
+
+	mutex_lock(&priv->mutex);
+	iwl_init_alive_start(priv);
+	mutex_unlock(&priv->mutex);
+}
+
+static void iwl_bg_alive_start(struct work_struct *data)
+{
+	struct iwl_priv *priv =
+	    container_of(data, struct iwl_priv, alive_start.work);
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+		return;
+
+	mutex_lock(&priv->mutex);
+	iwl_alive_start(priv);
+	mutex_unlock(&priv->mutex);
+}
+
+static void iwl_bg_rf_kill(struct work_struct *work)
+{
+	struct iwl_priv *priv = container_of(work, struct iwl_priv, rf_kill);
+
+	wake_up_interruptible(&priv->wait_command_queue);
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+		return;
+
+	mutex_lock(&priv->mutex);
+
+	if (!iwl_is_rfkill(priv)) {
+		IWL_DEBUG(IWL_DL_INFO | IWL_DL_RF_KILL,
+			  "HW and/or SW RF Kill no longer active, restarting "
+			  "device\n");
+		if (!test_bit(STATUS_EXIT_PENDING, &priv->status))
+			queue_work(priv->workqueue, &priv->restart);
+	} else {
+
+		if (!test_bit(STATUS_RF_KILL_HW, &priv->status))
+			IWL_DEBUG_RF_KILL("Can not turn radio back on - "
+					  "disabled by SW switch\n");
+		else
+			IWL_WARNING("Radio Frequency Kill Switch is On:\n"
+				    "Kill switch must be turned off for "
+				    "wireless networking to work.\n");
+	}
+	mutex_unlock(&priv->mutex);
+}
+
+#define IWL_SCAN_CHECK_WATCHDOG (7 * HZ)
+
+static void iwl_bg_scan_check(struct work_struct *data)
+{
+	struct iwl_priv *priv =
+	    container_of(data, struct iwl_priv, scan_check.work);
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+		return;
+
+	mutex_lock(&priv->mutex);
+	if (test_bit(STATUS_SCANNING, &priv->status) ||
+	    test_bit(STATUS_SCAN_ABORTING, &priv->status)) {
+		IWL_DEBUG(IWL_DL_INFO | IWL_DL_SCAN,
+			  "Scan completion watchdog resetting adapter (%dms)\n",
+			  jiffies_to_msecs(IWL_SCAN_CHECK_WATCHDOG));
+		if (!test_bit(STATUS_EXIT_PENDING, &priv->status))
+			queue_work(priv->workqueue, &priv->restart);
+	}
+	mutex_unlock(&priv->mutex);
+}
+
+static void iwl_bg_request_scan(struct work_struct *data)
+{
+	struct iwl_priv *priv =
+	    container_of(data, struct iwl_priv, request_scan);
+	struct iwl_host_cmd cmd = {
+		.id = REPLY_SCAN_CMD,
+		.len = sizeof(struct iwl_scan_cmd),
+		.meta.flags = CMD_SIZE_HUGE,
+	};
+	int rc = 0;
+	struct iwl_scan_cmd *scan;
+	struct ieee80211_conf *conf = NULL;
+	u8 direct_mask;
+	int phymode;
+
+	conf = ieee80211_get_hw_conf(priv->hw);
+
+	mutex_lock(&priv->mutex);
+
+	if (!iwl_is_ready(priv)) {
+		IWL_WARNING("request scan called when driver not ready.\n");
+		goto done;
+	}
+
+	/* Make sure the scan wasn't cancelled before this queued work
+	 * was given the chance to run... */
+	if (!test_bit(STATUS_SCANNING, &priv->status))
+		goto done;
+
+	/* This should never be called or scheduled if there is currently
+	 * a scan active in the hardware. */
+	if (test_bit(STATUS_SCAN_HW, &priv->status)) {
+		IWL_DEBUG_INFO("Multiple concurrent scan requests in parallel. "
+			       "Ignoring second request.\n");
+		rc = -EIO;
+		goto done;
+	}
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status)) {
+		IWL_DEBUG_SCAN("Aborting scan due to device shutdown\n");
+		goto done;
+	}
+
+	if (test_bit(STATUS_SCAN_ABORTING, &priv->status)) {
+		IWL_DEBUG_HC("Scan request while abort pending.  Queuing.\n");
+		goto done;
+	}
+
+	if (iwl_is_rfkill(priv)) {
+		IWL_DEBUG_HC("Aborting scan due to RF Kill activation\n");
+		goto done;
+	}
+
+	if (!test_bit(STATUS_READY, &priv->status)) {
+		IWL_DEBUG_HC("Scan request while uninitialized.  Queuing.\n");
+		goto done;
+	}
+
+	if (!priv->scan_bands) {
+		IWL_DEBUG_HC("Aborting scan due to no requested bands\n");
+		goto done;
+	}
+
+	if (!priv->scan) {
+		priv->scan = kmalloc(sizeof(struct iwl_scan_cmd) +
+				     IWL_MAX_SCAN_SIZE, GFP_KERNEL);
+		if (!priv->scan) {
+			rc = -ENOMEM;
+			goto done;
+		}
+	}
+	scan = priv->scan;
+	memset(scan, 0, sizeof(struct iwl_scan_cmd) + IWL_MAX_SCAN_SIZE);
+
+	scan->quiet_plcp_th = IWL_PLCP_QUIET_THRESH;
+	scan->quiet_time = IWL_ACTIVE_QUIET_TIME;
+
+	if (iwl_is_associated(priv)) {
+		u16 interval = 0;
+		u32 extra;
+		u32 suspend_time = 100;
+		u32 scan_suspend_time = 100;
+		unsigned long flags;
+
+		IWL_DEBUG_INFO("Scanning while associated...\n");
+
+		spin_lock_irqsave(&priv->lock, flags);
+		interval = priv->beacon_int;
+		spin_unlock_irqrestore(&priv->lock, flags);
+
+		scan->suspend_time = 0;
+		scan->max_out_time = cpu_to_le32(600 * 1024);
+		if (!interval)
+			interval = suspend_time;
+
+		extra = (suspend_time / interval) << 22;
+		scan_suspend_time = (extra |
+		    ((suspend_time % interval) * 1024));
+		scan->suspend_time = cpu_to_le32(scan_suspend_time);
+		IWL_DEBUG_SCAN("suspend_time 0x%X beacon interval %d\n",
+			       scan_suspend_time, interval);
+	}
+
+	/* We should add the ability for user to lock to PASSIVE ONLY */
+	if (priv->one_direct_scan) {
+		IWL_DEBUG_SCAN
+		    ("Kicking off one direct scan for '%s'\n",
+		     iwl_escape_essid(priv->direct_ssid,
+				      priv->direct_ssid_len));
+		scan->direct_scan[0].id = WLAN_EID_SSID;
+		scan->direct_scan[0].len = priv->direct_ssid_len;
+		memcpy(scan->direct_scan[0].ssid,
+		       priv->direct_ssid, priv->direct_ssid_len);
+		direct_mask = 1;
+	} else if (!iwl_is_associated(priv)) {
+		scan->direct_scan[0].id = WLAN_EID_SSID;
+		scan->direct_scan[0].len = priv->essid_len;
+		memcpy(scan->direct_scan[0].ssid, priv->essid, priv->essid_len);
+		direct_mask = 1;
+	} else
+		direct_mask = 0;
+
+	/* We don't build a direct scan probe request; the uCode will do
+	 * that based on the direct_mask added to each channel entry */
+	scan->tx_cmd.len = cpu_to_le16(
+		iwl_fill_probe_req(priv, (struct ieee80211_mgmt *)scan->data,
+			IWL_MAX_SCAN_SIZE - sizeof(scan), 0));
+	scan->tx_cmd.tx_flags = TX_CMD_FLG_SEQ_CTL_MSK;
+	scan->tx_cmd.sta_id = priv->hw_setting.bcast_sta_id;
+	scan->tx_cmd.stop_time.life_time = TX_CMD_LIFE_TIME_INFINITE;
+
+	/* flags + rate selection */
+
+	scan->tx_cmd.tx_flags |= cpu_to_le32(0x200);
+
+	switch (priv->scan_bands) {
+	case 2:
+		scan->flags = RXON_FLG_BAND_24G_MSK | RXON_FLG_AUTO_DETECT_MSK;
+		scan->tx_cmd.rate_n_flags =
+				iwl_hw_set_rate_n_flags(IWL_RATE_1M_PLCP,
+				RATE_MCS_ANT_B_MSK|RATE_MCS_CCK_MSK);
+
+		scan->good_CRC_th = 0;
+		phymode = MODE_IEEE80211G;
+		break;
+
+	case 1:
+		scan->tx_cmd.rate_n_flags =
+				iwl_hw_set_rate_n_flags(IWL_RATE_6M_PLCP,
+				RATE_MCS_ANT_B_MSK);
+		scan->good_CRC_th = IWL_GOOD_CRC_TH;
+		phymode = MODE_IEEE80211A;
+		break;
+
+	default:
+		IWL_WARNING("Invalid scan band count\n");
+		goto done;
+	}
+
+	/* select Rx chains */
+
+	/* Force use of chains B and C (0x6) for scan Rx.
+	 * Avoid A (0x1) because of its off-channel reception on A-band.
+	 * MIMO is not used here, but value is required to make uCode happy. */
+	scan->rx_chain = RXON_RX_CHAIN_DRIVER_FORCE_MSK |
+			cpu_to_le16((0x7 << RXON_RX_CHAIN_VALID_POS) |
+			(0x6 << RXON_RX_CHAIN_FORCE_SEL_POS) |
+			(0x7 << RXON_RX_CHAIN_FORCE_MIMO_SEL_POS));
+
+	if (priv->iw_mode == IEEE80211_IF_TYPE_MNTR)
+		scan->filter_flags = RXON_FILTER_PROMISC_MSK;
+
+	if (direct_mask)
+		IWL_DEBUG_SCAN
+		    ("Initiating direct scan for %s.\n",
+		     iwl_escape_essid(priv->essid, priv->essid_len));
+	else
+		IWL_DEBUG_SCAN("Initiating indirect scan.\n");
+
+	scan->channel_count =
+		iwl_get_channels_for_scan(
+			priv, phymode, 1, /* active */
+			direct_mask,
+			(void *)&scan->data[le16_to_cpu(scan->tx_cmd.len)]);
+
+	cmd.len += le16_to_cpu(scan->tx_cmd.len) +
+	    scan->channel_count * sizeof(struct iwl_scan_channel);
+	cmd.data = scan;
+	scan->len = cpu_to_le16(cmd.len);
+
+	set_bit(STATUS_SCAN_HW, &priv->status);
+	rc = iwl_send_cmd_sync(priv, &cmd);
+	if (rc)
+		goto done;
+
+	queue_delayed_work(priv->workqueue, &priv->scan_check,
+			   IWL_SCAN_CHECK_WATCHDOG);
+
+	mutex_unlock(&priv->mutex);
+	return;
+
+ done:
+	/* inform mac80211 sacn aborted */
+	queue_work(priv->workqueue, &priv->scan_completed);
+	mutex_unlock(&priv->mutex);
+}
+
+static void iwl_bg_up(struct work_struct *data)
+{
+	struct iwl_priv *priv = container_of(data, struct iwl_priv, up);
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+		return;
+
+	mutex_lock(&priv->mutex);
+	__iwl_up(priv);
+	mutex_unlock(&priv->mutex);
+}
+
+static void iwl_bg_restart(struct work_struct *data)
+{
+	struct iwl_priv *priv = container_of(data, struct iwl_priv, restart);
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+		return;
+
+	iwl_down(priv);
+	queue_work(priv->workqueue, &priv->up);
+}
+
+static void iwl_bg_rx_replenish(struct work_struct *data)
+{
+	struct iwl_priv *priv =
+	    container_of(data, struct iwl_priv, rx_replenish);
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+		return;
+
+	mutex_lock(&priv->mutex);
+	iwl_rx_replenish(priv);
+	mutex_unlock(&priv->mutex);
+}
+
+static void iwl_bg_post_associate(struct work_struct *data)
+{
+	struct iwl_priv *priv = container_of(data, struct iwl_priv,
+					     post_associate.work);
+
+	int rc = 0;
+	struct ieee80211_conf *conf = NULL;
+	DECLARE_MAC_BUF(mac);
+
+	if (priv->iw_mode == IEEE80211_IF_TYPE_AP) {
+		IWL_ERROR("%s Should not be called in AP mode\n", __FUNCTION__);
+		return;
+	}
+
+	IWL_DEBUG_ASSOC("Associated as %d to: %s\n",
+			priv->assoc_id,
+			print_mac(mac, priv->active_rxon.bssid_addr));
+
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+		return;
+
+	mutex_lock(&priv->mutex);
+
+	conf = ieee80211_get_hw_conf(priv->hw);
+
+	priv->staging_rxon.filter_flags &= ~RXON_FILTER_ASSOC_MSK;
+	iwl_commit_rxon(priv);
+
+	memset(&priv->rxon_timing, 0, sizeof(struct iwl_rxon_time_cmd));
+	iwl_setup_rxon_timing(priv);
+	rc = iwl_send_cmd_pdu(priv, REPLY_RXON_TIMING,
+			      sizeof(priv->rxon_timing), &priv->rxon_timing);
+	if (rc)
+		IWL_WARNING("REPLY_RXON_TIMING failed - "
+			    "Attempting to continue.\n");
+
+	priv->staging_rxon.filter_flags |= RXON_FILTER_ASSOC_MSK;
+
+#ifdef CONFIG_IWLWIFI_HT
+	if (priv->is_ht_enabled && priv->current_assoc_ht.is_ht)
+		iwl4965_set_rxon_ht(priv, &priv->current_assoc_ht);
+	else {
+		priv->active_rate_ht[0] = 0;
+		priv->active_rate_ht[1] = 0;
+		priv->current_channel_width = IWL_CHANNEL_WIDTH_20MHZ;
+	}
+#endif /* CONFIG_IWLWIFI_HT*/
+	iwl4965_set_rxon_chain(priv);
+	priv->staging_rxon.assoc_id = cpu_to_le16(priv->assoc_id);
+
+	IWL_DEBUG_ASSOC("assoc id %d beacon interval %d\n",
+			priv->assoc_id, priv->beacon_int);
+
+	if (priv->assoc_capability & WLAN_CAPABILITY_SHORT_PREAMBLE)
+		priv->staging_rxon.flags |= RXON_FLG_SHORT_PREAMBLE_MSK;
+	else
+		priv->staging_rxon.flags &= ~RXON_FLG_SHORT_PREAMBLE_MSK;
+
+	if (priv->staging_rxon.flags & RXON_FLG_BAND_24G_MSK) {
+		if (priv->assoc_capability & WLAN_CAPABILITY_SHORT_SLOT_TIME)
+			priv->staging_rxon.flags |= RXON_FLG_SHORT_SLOT_MSK;
+		else
+			priv->staging_rxon.flags &= ~RXON_FLG_SHORT_SLOT_MSK;
+
+		if (priv->iw_mode == IEEE80211_IF_TYPE_IBSS)
+			priv->staging_rxon.flags &= ~RXON_FLG_SHORT_SLOT_MSK;
+
+	}
+
+	iwl_commit_rxon(priv);
+
+	switch (priv->iw_mode) {
+	case IEEE80211_IF_TYPE_STA:
+		iwl_rate_scale_init(priv->hw, IWL_AP_ID);
+		break;
+
+	case IEEE80211_IF_TYPE_IBSS:
+
+		/* clear out the station table */
+		iwl_clear_stations_table(priv);
+
+		iwl_rxon_add_station(priv, BROADCAST_ADDR, 0);
+		iwl_rxon_add_station(priv, priv->bssid, 0);
+		iwl_rate_scale_init(priv->hw, IWL_STA_ID);
+		iwl_send_beacon_cmd(priv);
+
+		break;
+
+	default:
+		IWL_ERROR("%s Should not be called in %d mode\n",
+				__FUNCTION__, priv->iw_mode);
+		break;
+	}
+
+	iwl_sequence_reset(priv);
+
+#ifdef CONFIG_IWLWIFI_SENSITIVITY
+	/* Enable Rx differential gain and sensitivity calibrations */
+	iwl4965_chain_noise_reset(priv);
+	priv->start_calib = 1;
+#endif /* CONFIG_IWLWIFI_SENSITIVITY */
+
+	if (priv->iw_mode == IEEE80211_IF_TYPE_IBSS)
+		priv->assoc_station_added = 1;
+
+#ifdef CONFIG_IWLWIFI_QOS
+	iwl_activate_qos(priv, 0);
+#endif /* CONFIG_IWLWIFI_QOS */
+	mutex_unlock(&priv->mutex);
+}
+
+static void iwl_bg_abort_scan(struct work_struct *work)
+{
+	struct iwl_priv *priv = container_of(work, struct iwl_priv,
+					     abort_scan);
+
+	if (!iwl_is_ready(priv))
+		return;
+
+	mutex_lock(&priv->mutex);
+
+	set_bit(STATUS_SCAN_ABORTING, &priv->status);
+	iwl_send_scan_abort(priv);
+
+	mutex_unlock(&priv->mutex);
+}
+
+static void iwl_bg_scan_completed(struct work_struct *work)
+{
+	struct iwl_priv *priv =
+	    container_of(work, struct iwl_priv, scan_completed);
+
+	IWL_DEBUG(IWL_DL_INFO | IWL_DL_SCAN, "SCAN complete scan\n");
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+		return;
+
+	ieee80211_scan_completed(priv->hw);
+
+	/* Since setting the TXPOWER may have been deferred while
+	 * performing the scan, fire one off */
+	mutex_lock(&priv->mutex);
+	iwl_hw_reg_send_txpower(priv);
+	mutex_unlock(&priv->mutex);
+}
+
+/*****************************************************************************
+ *
+ * mac80211 entry point functions
+ *
+ *****************************************************************************/
+
+static int iwl_mac_start(struct ieee80211_hw *hw)
+{
+	struct iwl_priv *priv = hw->priv;
+
+	IWL_DEBUG_MAC80211("enter\n");
+
+	/* we should be verifying the device is ready to be opened */
+	mutex_lock(&priv->mutex);
+
+	priv->is_open = 1;
+
+	if (!iwl_is_rfkill(priv))
+		ieee80211_start_queues(priv->hw);
+
+	mutex_unlock(&priv->mutex);
+	IWL_DEBUG_MAC80211("leave\n");
+	return 0;
+}
+
+static void iwl_mac_stop(struct ieee80211_hw *hw)
+{
+	struct iwl_priv *priv = hw->priv;
+
+	IWL_DEBUG_MAC80211("enter\n");
+	priv->is_open = 0;
+	/*netif_stop_queue(dev); */
+	flush_workqueue(priv->workqueue);
+	IWL_DEBUG_MAC80211("leave\n");
+}
+
+static int iwl_mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb,
+		      struct ieee80211_tx_control *ctl)
+{
+	struct iwl_priv *priv = hw->priv;
+
+	IWL_DEBUG_MAC80211("enter\n");
+
+	if (priv->iw_mode == IEEE80211_IF_TYPE_MNTR) {
+		IWL_DEBUG_MAC80211("leave - monitor\n");
+		return -1;
+	}
+
+	IWL_DEBUG_TX("dev->xmit(%d bytes) at rate 0x%02x\n", skb->len,
+		     ctl->tx_rate);
+
+	if (iwl_tx_skb(priv, skb, ctl))
+		dev_kfree_skb_any(skb);
+
+	IWL_DEBUG_MAC80211("leave\n");
+	return 0;
+}
+
+static int iwl_mac_add_interface(struct ieee80211_hw *hw,
+				 struct ieee80211_if_init_conf *conf)
+{
+	struct iwl_priv *priv = hw->priv;
+	unsigned long flags;
+	DECLARE_MAC_BUF(mac);
+
+	IWL_DEBUG_MAC80211("enter: id %d, type %d\n", conf->if_id, conf->type);
+	if (conf->mac_addr)
+		IWL_DEBUG_MAC80211("enter: MAC %s\n",
+				   print_mac(mac, conf->mac_addr));
+
+	if (priv->interface_id) {
+		IWL_DEBUG_MAC80211("leave - interface_id != 0\n");
+		return 0;
+	}
+
+	spin_lock_irqsave(&priv->lock, flags);
+	priv->interface_id = conf->if_id;
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	mutex_lock(&priv->mutex);
+	iwl_set_mode(priv, conf->type);
+
+	IWL_DEBUG_MAC80211("leave\n");
+	mutex_unlock(&priv->mutex);
+
+	return 0;
+}
+
+/**
+ * iwl_mac_config - mac80211 config callback
+ *
+ * We ignore conf->flags & IEEE80211_CONF_SHORT_SLOT_TIME since it seems to
+ * be set inappropriately and the driver currently sets the hardware up to
+ * use it whenever needed.
+ */
+static int iwl_mac_config(struct ieee80211_hw *hw, struct ieee80211_conf *conf)
+{
+	struct iwl_priv *priv = hw->priv;
+	const struct iwl_channel_info *ch_info;
+	unsigned long flags;
+
+	mutex_lock(&priv->mutex);
+	IWL_DEBUG_MAC80211("enter to channel %d\n", conf->channel);
+
+	if (!iwl_is_ready(priv)) {
+		IWL_DEBUG_MAC80211("leave - not ready\n");
+		mutex_unlock(&priv->mutex);
+		return -EIO;
+	}
+
+	/* TODO: Figure out how to get ieee80211_local->sta_scanning w/ only
+	 * what is exposed through include/ declrations */
+	if (unlikely(!iwl_param_disable_hw_scan &&
+		     test_bit(STATUS_SCANNING, &priv->status))) {
+		IWL_DEBUG_MAC80211("leave - scanning\n");
+		mutex_unlock(&priv->mutex);
+		return 0;
+	}
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	ch_info = iwl_get_channel_info(priv, conf->phymode, conf->channel);
+	if (!is_channel_valid(ch_info)) {
+		IWL_DEBUG_SCAN("Channel %d [%d] is INVALID for this SKU.\n",
+			       conf->channel, conf->phymode);
+		IWL_DEBUG_MAC80211("leave - invalid channel\n");
+		spin_unlock_irqrestore(&priv->lock, flags);
+		mutex_unlock(&priv->mutex);
+		return -EINVAL;
+	}
+
+#ifdef CONFIG_IWLWIFI_HT
+	/* if we are switching fron ht to 2.4 clear flags
+	 * from any ht related info since 2.4 does not
+	 * support ht */
+	if ((le16_to_cpu(priv->staging_rxon.channel) != conf->channel)
+#ifdef IEEE80211_CONF_CHANNEL_SWITCH
+	    && !(conf->flags & IEEE80211_CONF_CHANNEL_SWITCH)
+#endif
+	)
+		priv->staging_rxon.flags = 0;
+#endif /* CONFIG_IWLWIFI_HT */
+
+	iwl_set_rxon_channel(priv, conf->phymode, conf->channel);
+
+	iwl_set_flags_for_phymode(priv, conf->phymode);
+
+	/* The list of supported rates and rate mask can be different
+	 * for each phymode; since the phymode may have changed, reset
+	 * the rate mask to what mac80211 lists */
+	iwl_set_rate(priv);
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+#ifdef IEEE80211_CONF_CHANNEL_SWITCH
+	if (conf->flags & IEEE80211_CONF_CHANNEL_SWITCH) {
+		iwl_hw_channel_switch(priv, conf->channel);
+		mutex_unlock(&priv->mutex);
+		return 0;
+	}
+#endif
+
+	iwl_radio_kill_sw(priv, !conf->radio_enabled);
+
+	if (!conf->radio_enabled) {
+		IWL_DEBUG_MAC80211("leave - radio disabled\n");
+		mutex_unlock(&priv->mutex);
+		return 0;
+	}
+
+	if (iwl_is_rfkill(priv)) {
+		IWL_DEBUG_MAC80211("leave - RF kill\n");
+		mutex_unlock(&priv->mutex);
+		return -EIO;
+	}
+
+	iwl_set_rate(priv);
+
+	if (memcmp(&priv->active_rxon,
+		   &priv->staging_rxon, sizeof(priv->staging_rxon)))
+		iwl_commit_rxon(priv);
+	else
+		IWL_DEBUG_INFO("No re-sending same RXON configuration.\n");
+
+	IWL_DEBUG_MAC80211("leave\n");
+
+	mutex_unlock(&priv->mutex);
+
+	return 0;
+}
+
+static void iwl_config_ap(struct iwl_priv *priv)
+{
+	int rc = 0;
+
+	if (priv->status & STATUS_EXIT_PENDING)
+		return;
+
+	/* The following should be done only at AP bring up */
+	if ((priv->active_rxon.filter_flags & RXON_FILTER_ASSOC_MSK) == 0) {
+
+		/* RXON - unassoc (to set timing command) */
+		priv->staging_rxon.filter_flags &= ~RXON_FILTER_ASSOC_MSK;
+		iwl_commit_rxon(priv);
+
+		/* RXON Timing */
+		memset(&priv->rxon_timing, 0, sizeof(struct iwl_rxon_time_cmd));
+		iwl_setup_rxon_timing(priv);
+		rc = iwl_send_cmd_pdu(priv, REPLY_RXON_TIMING,
+				sizeof(priv->rxon_timing), &priv->rxon_timing);
+		if (rc)
+			IWL_WARNING("REPLY_RXON_TIMING failed - "
+					"Attempting to continue.\n");
+
+		iwl4965_set_rxon_chain(priv);
+
+		/* FIXME: what should be the assoc_id for AP? */
+		priv->staging_rxon.assoc_id = cpu_to_le16(priv->assoc_id);
+		if (priv->assoc_capability & WLAN_CAPABILITY_SHORT_PREAMBLE)
+			priv->staging_rxon.flags |=
+				RXON_FLG_SHORT_PREAMBLE_MSK;
+		else
+			priv->staging_rxon.flags &=
+				~RXON_FLG_SHORT_PREAMBLE_MSK;
+
+		if (priv->staging_rxon.flags & RXON_FLG_BAND_24G_MSK) {
+			if (priv->assoc_capability &
+				WLAN_CAPABILITY_SHORT_SLOT_TIME)
+				priv->staging_rxon.flags |=
+					RXON_FLG_SHORT_SLOT_MSK;
+			else
+				priv->staging_rxon.flags &=
+					~RXON_FLG_SHORT_SLOT_MSK;
+
+			if (priv->iw_mode == IEEE80211_IF_TYPE_IBSS)
+				priv->staging_rxon.flags &=
+					~RXON_FLG_SHORT_SLOT_MSK;
+		}
+		/* restore RXON assoc */
+		priv->staging_rxon.filter_flags |= RXON_FILTER_ASSOC_MSK;
+		iwl_commit_rxon(priv);
+#ifdef CONFIG_IWLWIFI_QOS
+		iwl_activate_qos(priv, 1);
+#endif
+		iwl_rxon_add_station(priv, BROADCAST_ADDR, 0);
+	}
+	iwl_send_beacon_cmd(priv);
+
+	/* FIXME - we need to add code here to detect a totally new
+	 * configuration, reset the AP, unassoc, rxon timing, assoc,
+	 * clear sta table, add BCAST sta... */
+}
+
+static int iwl_mac_config_interface(struct ieee80211_hw *hw, int if_id,
+				    struct ieee80211_if_conf *conf)
+{
+	struct iwl_priv *priv = hw->priv;
+	DECLARE_MAC_BUF(mac);
+	unsigned long flags;
+	int rc;
+
+	if (conf == NULL)
+		return -EIO;
+
+	if ((priv->iw_mode == IEEE80211_IF_TYPE_AP) &&
+	    (!conf->beacon || !conf->ssid_len)) {
+		IWL_DEBUG_MAC80211
+		    ("Leaving in AP mode because HostAPD is not ready.\n");
+		return 0;
+	}
+
+	mutex_lock(&priv->mutex);
+
+	IWL_DEBUG_MAC80211("enter: interface id %d\n", if_id);
+	if (conf->bssid)
+		IWL_DEBUG_MAC80211("bssid: %s\n",
+				   print_mac(mac, conf->bssid));
+
+/*
+ * very dubious code was here; the probe filtering flag is never set:
+ *
+	if (unlikely(test_bit(STATUS_SCANNING, &priv->status)) &&
+	    !(priv->hw->flags & IEEE80211_HW_NO_PROBE_FILTERING)) {
+ */
+	if (unlikely(test_bit(STATUS_SCANNING, &priv->status))) {
+		IWL_DEBUG_MAC80211("leave - scanning\n");
+		mutex_unlock(&priv->mutex);
+		return 0;
+	}
+
+	if (priv->interface_id != if_id) {
+		IWL_DEBUG_MAC80211("leave - interface_id != if_id\n");
+		mutex_unlock(&priv->mutex);
+		return 0;
+	}
+
+	if (priv->iw_mode == IEEE80211_IF_TYPE_AP) {
+		if (!conf->bssid) {
+			conf->bssid = priv->mac_addr;
+			memcpy(priv->bssid, priv->mac_addr, ETH_ALEN);
+			IWL_DEBUG_MAC80211("bssid was set to: %s\n",
+					   print_mac(mac, conf->bssid));
+		}
+		if (priv->ibss_beacon)
+			dev_kfree_skb(priv->ibss_beacon);
+
+		priv->ibss_beacon = conf->beacon;
+	}
+
+	if (conf->bssid && !is_zero_ether_addr(conf->bssid) &&
+	    !is_multicast_ether_addr(conf->bssid)) {
+		/* If there is currently a HW scan going on in the background
+		 * then we need to cancel it else the RXON below will fail. */
+		if (iwl_scan_cancel_timeout(priv, 100)) {
+			IWL_WARNING("Aborted scan still in progress "
+				    "after 100ms\n");
+			IWL_DEBUG_MAC80211("leaving - scan abort failed.\n");
+			mutex_unlock(&priv->mutex);
+			return -EAGAIN;
+		}
+		memcpy(priv->staging_rxon.bssid_addr, conf->bssid, ETH_ALEN);
+
+		/* TODO: Audit driver for usage of these members and see
+		 * if mac80211 deprecates them (priv->bssid looks like it
+		 * shouldn't be there, but I haven't scanned the IBSS code
+		 * to verify) - jpk */
+		memcpy(priv->bssid, conf->bssid, ETH_ALEN);
+
+		if (priv->iw_mode == IEEE80211_IF_TYPE_AP)
+			iwl_config_ap(priv);
+		else {
+			priv->staging_rxon.filter_flags |=
+						RXON_FILTER_ASSOC_MSK;
+			rc = iwl_commit_rxon(priv);
+			if ((priv->iw_mode == IEEE80211_IF_TYPE_STA) && rc)
+				iwl_rxon_add_station(
+					priv, priv->active_rxon.bssid_addr, 1);
+		}
+
+	} else {
+		priv->staging_rxon.filter_flags &= ~RXON_FILTER_ASSOC_MSK;
+		iwl_commit_rxon(priv);
+	}
+
+	spin_lock_irqsave(&priv->lock, flags);
+	if (!conf->ssid_len)
+		memset(priv->essid, 0, IW_ESSID_MAX_SIZE);
+	else
+		memcpy(priv->essid, conf->ssid, conf->ssid_len);
+
+	priv->essid_len = conf->ssid_len;
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	IWL_DEBUG_MAC80211("leave\n");
+	mutex_unlock(&priv->mutex);
+
+	return 0;
+}
+
+static void iwl_configure_filter(struct ieee80211_hw *hw,
+				 unsigned int changed_flags,
+				 unsigned int *total_flags,
+				 int mc_count, struct dev_addr_list *mc_list)
+{
+	/*
+	 * XXX: dummy
+	 * see also iwl_connection_init_rx_config
+	 */
+	*total_flags = 0;
+}
+
+static void iwl_mac_remove_interface(struct ieee80211_hw *hw,
+				     struct ieee80211_if_init_conf *conf)
+{
+	struct iwl_priv *priv = hw->priv;
+
+	IWL_DEBUG_MAC80211("enter\n");
+
+	mutex_lock(&priv->mutex);
+	if (priv->interface_id == conf->if_id) {
+		priv->interface_id = 0;
+		memset(priv->bssid, 0, ETH_ALEN);
+		memset(priv->essid, 0, IW_ESSID_MAX_SIZE);
+		priv->essid_len = 0;
+	}
+	mutex_unlock(&priv->mutex);
+
+	IWL_DEBUG_MAC80211("leave\n");
+
+}
+
+#define IWL_DELAY_NEXT_SCAN (HZ*2)
+static int iwl_mac_hw_scan(struct ieee80211_hw *hw, u8 *ssid, size_t len)
+{
+	int rc = 0;
+	unsigned long flags;
+	struct iwl_priv *priv = hw->priv;
+
+	IWL_DEBUG_MAC80211("enter\n");
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	if (!iwl_is_ready_rf(priv)) {
+		rc = -EIO;
+		IWL_DEBUG_MAC80211("leave - not ready or exit pending\n");
+		goto out_unlock;
+	}
+
+	if (priv->iw_mode == IEEE80211_IF_TYPE_AP) {	/* APs don't scan */
+		rc = -EIO;
+		IWL_ERROR("ERROR: APs don't scan\n");
+		goto out_unlock;
+	}
+
+	/* if we just finished scan ask for delay */
+	if (priv->last_scan_jiffies &&
+	    time_after(priv->last_scan_jiffies + IWL_DELAY_NEXT_SCAN,
+		       jiffies)) {
+		rc = -EAGAIN;
+		goto out_unlock;
+	}
+	if (len) {
+		IWL_DEBUG_SCAN("direct scan for  "
+			       "%s [%d]\n ",
+			       iwl_escape_essid(ssid, len), (int)len);
+
+		priv->one_direct_scan = 1;
+		priv->direct_ssid_len = (u8)
+		    min((u8) len, (u8) IW_ESSID_MAX_SIZE);
+		memcpy(priv->direct_ssid, ssid, priv->direct_ssid_len);
+	}
+
+	rc = iwl_scan_initiate(priv);
+
+	IWL_DEBUG_MAC80211("leave\n");
+
+out_unlock:
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return rc;
+}
+
+static int iwl_mac_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
+			   const u8 *local_addr, const u8 *addr,
+			   struct ieee80211_key_conf *key)
+{
+	struct iwl_priv *priv = hw->priv;
+	DECLARE_MAC_BUF(mac);
+	int rc = 0;
+	u8 sta_id;
+
+	IWL_DEBUG_MAC80211("enter\n");
+
+	if (!iwl_param_hwcrypto) {
+		IWL_DEBUG_MAC80211("leave - hwcrypto disabled\n");
+		return -EOPNOTSUPP;
+	}
+
+	if (is_zero_ether_addr(addr))
+		/* only support pairwise keys */
+		return -EOPNOTSUPP;
+
+	sta_id = iwl_hw_find_station(priv, addr);
+	if (sta_id == IWL_INVALID_STATION) {
+		IWL_DEBUG_MAC80211("leave - %s not in station map.\n",
+				   print_mac(mac, addr));
+		return -EINVAL;
+	}
+
+	mutex_lock(&priv->mutex);
+
+	switch (cmd) {
+	case  SET_KEY:
+		rc = iwl_update_sta_key_info(priv, key, sta_id);
+		if (!rc) {
+			iwl_set_rxon_hwcrypto(priv, 1);
+			iwl_commit_rxon(priv);
+			key->hw_key_idx = sta_id;
+			IWL_DEBUG_MAC80211("set_key success, using hwcrypto\n");
+			key->flags |= IEEE80211_KEY_FLAG_GENERATE_IV;
+		}
+		break;
+	case DISABLE_KEY:
+		rc = iwl_clear_sta_key_info(priv, sta_id);
+		if (!rc) {
+			iwl_set_rxon_hwcrypto(priv, 0);
+			iwl_commit_rxon(priv);
+			IWL_DEBUG_MAC80211("disable hwcrypto key\n");
+		}
+		break;
+	default:
+		rc = -EINVAL;
+	}
+
+	IWL_DEBUG_MAC80211("leave\n");
+	mutex_unlock(&priv->mutex);
+
+	return rc;
+}
+
+static int iwl_mac_conf_tx(struct ieee80211_hw *hw, int queue,
+			   const struct ieee80211_tx_queue_params *params)
+{
+	struct iwl_priv *priv = hw->priv;
+#ifdef CONFIG_IWLWIFI_QOS
+	unsigned long flags;
+	int q;
+#endif /* CONFIG_IWL_QOS */
+
+	IWL_DEBUG_MAC80211("enter\n");
+
+	if (!iwl_is_ready_rf(priv)) {
+		IWL_DEBUG_MAC80211("leave - RF not ready\n");
+		return -EIO;
+	}
+
+	if (queue >= AC_NUM) {
+		IWL_DEBUG_MAC80211("leave - queue >= AC_NUM %d\n", queue);
+		return 0;
+	}
+
+#ifdef CONFIG_IWLWIFI_QOS
+	if (!priv->qos_data.qos_enable) {
+		priv->qos_data.qos_active = 0;
+		IWL_DEBUG_MAC80211("leave - qos not enabled\n");
+		return 0;
+	}
+	q = AC_NUM - 1 - queue;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	priv->qos_data.def_qos_parm.ac[q].cw_min = cpu_to_le16(params->cw_min);
+	priv->qos_data.def_qos_parm.ac[q].cw_max = cpu_to_le16(params->cw_max);
+	priv->qos_data.def_qos_parm.ac[q].aifsn = params->aifs;
+	priv->qos_data.def_qos_parm.ac[q].edca_txop =
+			cpu_to_le16((params->burst_time * 100));
+
+	priv->qos_data.def_qos_parm.ac[q].reserved1 = 0;
+	priv->qos_data.qos_active = 1;
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	mutex_lock(&priv->mutex);
+	if (priv->iw_mode == IEEE80211_IF_TYPE_AP)
+		iwl_activate_qos(priv, 1);
+	else if (priv->assoc_id && iwl_is_associated(priv))
+		iwl_activate_qos(priv, 0);
+
+	mutex_unlock(&priv->mutex);
+
+#endif /*CONFIG_IWLWIFI_QOS */
+
+	IWL_DEBUG_MAC80211("leave\n");
+	return 0;
+}
+
+static int iwl_mac_get_tx_stats(struct ieee80211_hw *hw,
+				struct ieee80211_tx_queue_stats *stats)
+{
+	struct iwl_priv *priv = hw->priv;
+	int i, avail;
+	struct iwl_tx_queue *txq;
+	struct iwl_queue *q;
+	unsigned long flags;
+
+	IWL_DEBUG_MAC80211("enter\n");
+
+	if (!iwl_is_ready_rf(priv)) {
+		IWL_DEBUG_MAC80211("leave - RF not ready\n");
+		return -EIO;
+	}
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	for (i = 0; i < AC_NUM; i++) {
+		txq = &priv->txq[i];
+		q = &txq->q;
+		avail = iwl_queue_space(q);
+
+		stats->data[i].len = q->n_window - avail;
+		stats->data[i].limit = q->n_window - q->high_mark;
+		stats->data[i].count = q->n_window;
+
+	}
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	IWL_DEBUG_MAC80211("leave\n");
+
+	return 0;
+}
+
+static int iwl_mac_get_stats(struct ieee80211_hw *hw,
+			     struct ieee80211_low_level_stats *stats)
+{
+	IWL_DEBUG_MAC80211("enter\n");
+	IWL_DEBUG_MAC80211("leave\n");
+
+	return 0;
+}
+
+static u64 iwl_mac_get_tsf(struct ieee80211_hw *hw)
+{
+	IWL_DEBUG_MAC80211("enter\n");
+	IWL_DEBUG_MAC80211("leave\n");
+
+	return 0;
+}
+
+static void iwl_mac_reset_tsf(struct ieee80211_hw *hw)
+{
+	struct iwl_priv *priv = hw->priv;
+	unsigned long flags;
+
+	mutex_lock(&priv->mutex);
+	IWL_DEBUG_MAC80211("enter\n");
+
+	priv->lq_mngr.lq_ready = 0;
+#ifdef CONFIG_IWLWIFI_HT
+	spin_lock_irqsave(&priv->lock, flags);
+	memset(&priv->current_assoc_ht, 0, sizeof(struct sta_ht_info));
+	spin_unlock_irqrestore(&priv->lock, flags);
+#ifdef CONFIG_IWLWIFI_HT_AGG
+/*	if (priv->lq_mngr.agg_ctrl.granted_ba)
+		iwl4965_turn_off_agg(priv, TID_ALL_SPECIFIED);*/
+
+	memset(&(priv->lq_mngr.agg_ctrl), 0, sizeof(struct iwl_agg_control));
+	priv->lq_mngr.agg_ctrl.tid_traffic_load_threshold = 10;
+	priv->lq_mngr.agg_ctrl.ba_timeout = 5000;
+	priv->lq_mngr.agg_ctrl.auto_agg = 1;
+
+	if (priv->lq_mngr.agg_ctrl.auto_agg)
+		priv->lq_mngr.agg_ctrl.requested_ba = TID_ALL_ENABLED;
+#endif /*CONFIG_IWLWIFI_HT_AGG */
+#endif /* CONFIG_IWLWIFI_HT */
+
+#ifdef CONFIG_IWLWIFI_QOS
+	iwl_reset_qos(priv);
+#endif
+
+	cancel_delayed_work(&priv->post_associate);
+
+	spin_lock_irqsave(&priv->lock, flags);
+	priv->assoc_id = 0;
+	priv->assoc_capability = 0;
+	priv->call_post_assoc_from_beacon = 0;
+	priv->assoc_station_added = 0;
+
+	/* new association get rid of ibss beacon skb */
+	if (priv->ibss_beacon)
+		dev_kfree_skb(priv->ibss_beacon);
+
+	priv->ibss_beacon = NULL;
+
+	priv->beacon_int = priv->hw->conf.beacon_int;
+	priv->timestamp1 = 0;
+	priv->timestamp0 = 0;
+	if ((priv->iw_mode == IEEE80211_IF_TYPE_STA))
+		priv->beacon_int = 0;
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	/* Per mac80211.h: This is only used in IBSS mode... */
+	if (priv->iw_mode != IEEE80211_IF_TYPE_IBSS) {
+		IWL_DEBUG_MAC80211("leave - not in IBSS\n");
+		mutex_unlock(&priv->mutex);
+		return;
+	}
+
+	if (!iwl_is_ready_rf(priv)) {
+		IWL_DEBUG_MAC80211("leave - not ready\n");
+		mutex_unlock(&priv->mutex);
+		return;
+	}
+
+	priv->only_active_channel = 0;
+
+	iwl_set_rate(priv);
+
+	mutex_unlock(&priv->mutex);
+
+	IWL_DEBUG_MAC80211("leave\n");
+
+}
+
+static int iwl_mac_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb,
+				 struct ieee80211_tx_control *control)
+{
+	struct iwl_priv *priv = hw->priv;
+	unsigned long flags;
+
+	mutex_lock(&priv->mutex);
+	IWL_DEBUG_MAC80211("enter\n");
+
+	if (!iwl_is_ready_rf(priv)) {
+		IWL_DEBUG_MAC80211("leave - RF not ready\n");
+		mutex_unlock(&priv->mutex);
+		return -EIO;
+	}
+
+	if (priv->iw_mode != IEEE80211_IF_TYPE_IBSS) {
+		IWL_DEBUG_MAC80211("leave - not IBSS\n");
+		mutex_unlock(&priv->mutex);
+		return -EIO;
+	}
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	if (priv->ibss_beacon)
+		dev_kfree_skb(priv->ibss_beacon);
+
+	priv->ibss_beacon = skb;
+
+	priv->assoc_id = 0;
+
+	IWL_DEBUG_MAC80211("leave\n");
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+#ifdef CONFIG_IWLWIFI_QOS
+	iwl_reset_qos(priv);
+#endif
+
+	queue_work(priv->workqueue, &priv->post_associate.work);
+
+	mutex_unlock(&priv->mutex);
+
+	return 0;
+}
+
+#ifdef CONFIG_IWLWIFI_HT
+union ht_cap_info {
+	struct {
+		u16 advanced_coding_cap		:1;
+		u16 supported_chan_width_set	:1;
+		u16 mimo_power_save_mode	:2;
+		u16 green_field			:1;
+		u16 short_GI20			:1;
+		u16 short_GI40			:1;
+		u16 tx_stbc			:1;
+		u16 rx_stbc			:1;
+		u16 beam_forming		:1;
+		u16 delayed_ba			:1;
+		u16 maximal_amsdu_size		:1;
+		u16 cck_mode_at_40MHz		:1;
+		u16 psmp_support		:1;
+		u16 stbc_ctrl_frame_support	:1;
+		u16 sig_txop_protection_support	:1;
+	};
+	u16 val;
+} __attribute__ ((packed));
+
+union ht_param_info{
+	struct {
+		u8 max_rx_ampdu_factor	:2;
+		u8 mpdu_density		:3;
+		u8 reserved		:3;
+	};
+	u8 val;
+} __attribute__ ((packed));
+
+union ht_exra_param_info {
+	struct {
+		u8 ext_chan_offset		:2;
+		u8 tx_chan_width		:1;
+		u8 rifs_mode			:1;
+		u8 controlled_access_only	:1;
+		u8 service_interval_granularity	:3;
+	};
+	u8 val;
+} __attribute__ ((packed));
+
+union ht_operation_mode{
+	struct {
+		u16 op_mode	:2;
+		u16 non_GF	:1;
+		u16 reserved	:13;
+	};
+	u16 val;
+} __attribute__ ((packed));
+
+
+static int sta_ht_info_init(struct ieee80211_ht_capability *ht_cap,
+			    struct ieee80211_ht_additional_info *ht_extra,
+			    struct sta_ht_info *ht_info_ap,
+			    struct sta_ht_info *ht_info)
+{
+	union ht_cap_info cap;
+	union ht_operation_mode op_mode;
+	union ht_param_info param_info;
+	union ht_exra_param_info extra_param_info;
+
+	IWL_DEBUG_MAC80211("enter: \n");
+
+	if (!ht_info) {
+		IWL_DEBUG_MAC80211("leave: ht_info is NULL\n");
+		return -1;
+	}
+
+	if (ht_cap) {
+		cap.val = (u16) le16_to_cpu(ht_cap->capabilities_info);
+		param_info.val = ht_cap->mac_ht_params_info;
+		ht_info->is_ht = 1;
+		if (cap.short_GI20)
+			ht_info->sgf |= 0x1;
+		if (cap.short_GI40)
+			ht_info->sgf |= 0x2;
+		ht_info->is_green_field = cap.green_field;
+		ht_info->max_amsdu_size = cap.maximal_amsdu_size;
+		ht_info->supported_chan_width = cap.supported_chan_width_set;
+		ht_info->tx_mimo_ps_mode = cap.mimo_power_save_mode;
+		memcpy(ht_info->supp_rates, ht_cap->supported_mcs_set, 16);
+
+		ht_info->ampdu_factor = param_info.max_rx_ampdu_factor;
+		ht_info->mpdu_density = param_info.mpdu_density;
+
+		IWL_DEBUG_MAC80211("SISO mask 0x%X MIMO mask 0x%X \n",
+				    ht_cap->supported_mcs_set[0],
+				    ht_cap->supported_mcs_set[1]);
+
+		if (ht_info_ap) {
+			ht_info->control_channel = ht_info_ap->control_channel;
+			ht_info->extension_chan_offset =
+				ht_info_ap->extension_chan_offset;
+			ht_info->tx_chan_width = ht_info_ap->tx_chan_width;
+			ht_info->operating_mode = ht_info_ap->operating_mode;
+		}
+
+		if (ht_extra) {
+			extra_param_info.val = ht_extra->ht_param;
+			ht_info->control_channel = ht_extra->control_chan;
+			ht_info->extension_chan_offset =
+			    extra_param_info.ext_chan_offset;
+			ht_info->tx_chan_width = extra_param_info.tx_chan_width;
+			op_mode.val = (u16)
+			    le16_to_cpu(ht_extra->operation_mode);
+			ht_info->operating_mode = op_mode.op_mode;
+			IWL_DEBUG_MAC80211("control channel %d\n",
+					    ht_extra->control_chan);
+		}
+	} else
+		ht_info->is_ht = 0;
+
+	IWL_DEBUG_MAC80211("leave\n");
+	return 0;
+}
+
+static int iwl_mac_conf_ht(struct ieee80211_hw *hw,
+			   struct ieee80211_ht_capability *ht_cap,
+			   struct ieee80211_ht_additional_info *ht_extra)
+{
+	struct iwl_priv *priv = hw->priv;
+	int rs;
+
+	IWL_DEBUG_MAC80211("enter: \n");
+
+	rs = sta_ht_info_init(ht_cap, ht_extra, NULL, &priv->current_assoc_ht);
+	iwl4965_set_rxon_chain(priv);
+
+	if (priv && priv->assoc_id &&
+	    (priv->iw_mode == IEEE80211_IF_TYPE_STA)) {
+		unsigned long flags;
+
+		spin_lock_irqsave(&priv->lock, flags);
+		if (priv->beacon_int)
+			queue_work(priv->workqueue, &priv->post_associate.work);
+		else
+			priv->call_post_assoc_from_beacon = 1;
+		spin_unlock_irqrestore(&priv->lock, flags);
+	}
+
+	IWL_DEBUG_MAC80211("leave: control channel %d\n",
+			ht_extra->control_chan);
+	return rs;
+
+}
+
+static void iwl_set_ht_capab(struct ieee80211_hw *hw,
+			     struct ieee80211_ht_capability *ht_cap,
+			     u8 use_wide_chan)
+{
+	union ht_cap_info cap;
+	union ht_param_info param_info;
+
+	memset(&cap, 0, sizeof(union ht_cap_info));
+	memset(&param_info, 0, sizeof(union ht_param_info));
+
+	cap.maximal_amsdu_size = HT_IE_MAX_AMSDU_SIZE_4K;
+	cap.green_field = 1;
+	cap.short_GI20 = 1;
+	cap.short_GI40 = 1;
+	cap.supported_chan_width_set = use_wide_chan;
+	cap.mimo_power_save_mode = 0x3;
+
+	param_info.max_rx_ampdu_factor = CFG_HT_RX_AMPDU_FACTOR_DEF;
+	param_info.mpdu_density = CFG_HT_MPDU_DENSITY_DEF;
+	ht_cap->capabilities_info = (__le16) cpu_to_le16(cap.val);
+	ht_cap->mac_ht_params_info = (u8) param_info.val;
+
+	ht_cap->supported_mcs_set[0] = 0xff;
+	ht_cap->supported_mcs_set[1] = 0xff;
+	ht_cap->supported_mcs_set[4] =
+	    (cap.supported_chan_width_set) ? 0x1: 0x0;
+}
+
+static void iwl_mac_get_ht_capab(struct ieee80211_hw *hw,
+				 struct ieee80211_ht_capability *ht_cap)
+{
+	u8 use_wide_channel = 1;
+	struct iwl_priv *priv = hw->priv;
+
+	IWL_DEBUG_MAC80211("enter: \n");
+	if (priv->channel_width != IWL_CHANNEL_WIDTH_40MHZ)
+		use_wide_channel = 0;
+
+	/* no fat tx allowed on 2.4GHZ */
+	if (priv->phymode != MODE_IEEE80211A)
+		use_wide_channel = 0;
+
+	iwl_set_ht_capab(hw, ht_cap, use_wide_channel);
+	IWL_DEBUG_MAC80211("leave: \n");
+}
+#endif /*CONFIG_IWLWIFI_HT*/
+
+/*****************************************************************************
+ *
+ * sysfs attributes
+ *
+ *****************************************************************************/
+
+#ifdef CONFIG_IWLWIFI_DEBUG
+
+/*
+ * The following adds a new attribute to the sysfs representation
+ * of this device driver (i.e. a new file in /sys/bus/pci/drivers/iwl/)
+ * used for controlling the debug level.
+ *
+ * See the level definitions in iwl for details.
+ */
+
+static ssize_t show_debug_level(struct device_driver *d, char *buf)
+{
+	return sprintf(buf, "0x%08X\n", iwl_debug_level);
+}
+static ssize_t store_debug_level(struct device_driver *d,
+				 const char *buf, size_t count)
+{
+	char *p = (char *)buf;
+	u32 val;
+
+	val = simple_strtoul(p, &p, 0);
+	if (p == buf)
+		printk(KERN_INFO DRV_NAME
+		       ": %s is not in hex or decimal form.\n", buf);
+	else
+		iwl_debug_level = val;
+
+	return strnlen(buf, count);
+}
+
+static DRIVER_ATTR(debug_level, S_IWUSR | S_IRUGO,
+		   show_debug_level, store_debug_level);
+
+#endif /* CONFIG_IWLWIFI_DEBUG */
+
+static ssize_t show_rf_kill(struct device *d,
+			    struct device_attribute *attr, char *buf)
+{
+	/*
+	 * 0 - RF kill not enabled
+	 * 1 - SW based RF kill active (sysfs)
+	 * 2 - HW based RF kill active
+	 * 3 - Both HW and SW based RF kill active
+	 */
+	struct iwl_priv *priv = (struct iwl_priv *)d->driver_data;
+	int val = (test_bit(STATUS_RF_KILL_SW, &priv->status) ? 0x1 : 0x0) |
+		  (test_bit(STATUS_RF_KILL_HW, &priv->status) ? 0x2 : 0x0);
+
+	return sprintf(buf, "%i\n", val);
+}
+
+static ssize_t store_rf_kill(struct device *d,
+			     struct device_attribute *attr,
+			     const char *buf, size_t count)
+{
+	struct iwl_priv *priv = (struct iwl_priv *)d->driver_data;
+
+	mutex_lock(&priv->mutex);
+	iwl_radio_kill_sw(priv, buf[0] == '1');
+	mutex_unlock(&priv->mutex);
+
+	return count;
+}
+
+static DEVICE_ATTR(rf_kill, S_IWUSR | S_IRUGO, show_rf_kill, store_rf_kill);
+
+static ssize_t show_temperature(struct device *d,
+				struct device_attribute *attr, char *buf)
+{
+	struct iwl_priv *priv = (struct iwl_priv *)d->driver_data;
+
+	if (!iwl_is_alive(priv))
+		return -EAGAIN;
+
+	return sprintf(buf, "%d\n", iwl_hw_get_temperature(priv));
+}
+
+static DEVICE_ATTR(temperature, S_IRUGO, show_temperature, NULL);
+
+static ssize_t show_rs_window(struct device *d,
+			      struct device_attribute *attr,
+			      char *buf)
+{
+	struct iwl_priv *priv = d->driver_data;
+	return iwl_fill_rs_info(priv->hw, buf, IWL_AP_ID);
+}
+static DEVICE_ATTR(rs_window, S_IRUGO, show_rs_window, NULL);
+
+static ssize_t show_tx_power(struct device *d,
+			     struct device_attribute *attr, char *buf)
+{
+	struct iwl_priv *priv = (struct iwl_priv *)d->driver_data;
+	return sprintf(buf, "%d\n", priv->user_txpower_limit);
+}
+
+static ssize_t store_tx_power(struct device *d,
+			      struct device_attribute *attr,
+			      const char *buf, size_t count)
+{
+	struct iwl_priv *priv = (struct iwl_priv *)d->driver_data;
+	char *p = (char *)buf;
+	u32 val;
+
+	val = simple_strtoul(p, &p, 10);
+	if (p == buf)
+		printk(KERN_INFO DRV_NAME
+		       ": %s is not in decimal form.\n", buf);
+	else
+		iwl_hw_reg_set_txpower(priv, val);
+
+	return count;
+}
+
+static DEVICE_ATTR(tx_power, S_IWUSR | S_IRUGO, show_tx_power, store_tx_power);
+
+static ssize_t show_flags(struct device *d,
+			  struct device_attribute *attr, char *buf)
+{
+	struct iwl_priv *priv = (struct iwl_priv *)d->driver_data;
+
+	return sprintf(buf, "0x%04X\n", priv->active_rxon.flags);
+}
+
+static ssize_t store_flags(struct device *d,
+			   struct device_attribute *attr,
+			   const char *buf, size_t count)
+{
+	struct iwl_priv *priv = (struct iwl_priv *)d->driver_data;
+	u32 flags = simple_strtoul(buf, NULL, 0);
+
+	mutex_lock(&priv->mutex);
+	if (le32_to_cpu(priv->staging_rxon.flags) != flags) {
+		/* Cancel any currently running scans... */
+		if (iwl_scan_cancel_timeout(priv, 100))
+			IWL_WARNING("Could not cancel scan.\n");
+		else {
+			IWL_DEBUG_INFO("Committing rxon.flags = 0x%04X\n",
+				       flags);
+			priv->staging_rxon.flags = cpu_to_le32(flags);
+			iwl_commit_rxon(priv);
+		}
+	}
+	mutex_unlock(&priv->mutex);
+
+	return count;
+}
+
+static DEVICE_ATTR(flags, S_IWUSR | S_IRUGO, show_flags, store_flags);
+
+static ssize_t show_filter_flags(struct device *d,
+				 struct device_attribute *attr, char *buf)
+{
+	struct iwl_priv *priv = (struct iwl_priv *)d->driver_data;
+
+	return sprintf(buf, "0x%04X\n",
+		le32_to_cpu(priv->active_rxon.filter_flags));
+}
+
+static ssize_t store_filter_flags(struct device *d,
+				  struct device_attribute *attr,
+				  const char *buf, size_t count)
+{
+	struct iwl_priv *priv = (struct iwl_priv *)d->driver_data;
+	u32 filter_flags = simple_strtoul(buf, NULL, 0);
+
+	mutex_lock(&priv->mutex);
+	if (le32_to_cpu(priv->staging_rxon.filter_flags) != filter_flags) {
+		/* Cancel any currently running scans... */
+		if (iwl_scan_cancel_timeout(priv, 100))
+			IWL_WARNING("Could not cancel scan.\n");
+		else {
+			IWL_DEBUG_INFO("Committing rxon.filter_flags = "
+				       "0x%04X\n", filter_flags);
+			priv->staging_rxon.filter_flags =
+				cpu_to_le32(filter_flags);
+			iwl_commit_rxon(priv);
+		}
+	}
+	mutex_unlock(&priv->mutex);
+
+	return count;
+}
+
+static DEVICE_ATTR(filter_flags, S_IWUSR | S_IRUGO, show_filter_flags,
+		   store_filter_flags);
+
+static ssize_t show_tune(struct device *d,
+			 struct device_attribute *attr, char *buf)
+{
+	struct iwl_priv *priv = (struct iwl_priv *)d->driver_data;
+
+	return sprintf(buf, "0x%04X\n",
+		       (priv->phymode << 8) |
+			le16_to_cpu(priv->active_rxon.channel));
+}
+
+static void iwl_set_flags_for_phymode(struct iwl_priv *priv, u8 phymode);
+
+static ssize_t store_tune(struct device *d,
+			  struct device_attribute *attr,
+			  const char *buf, size_t count)
+{
+	struct iwl_priv *priv = (struct iwl_priv *)d->driver_data;
+	char *p = (char *)buf;
+	u16 tune = simple_strtoul(p, &p, 0);
+	u8 phymode = (tune >> 8) & 0xff;
+	u16 channel = tune & 0xff;
+
+	IWL_DEBUG_INFO("Tune request to:%d channel:%d\n", phymode, channel);
+
+	mutex_lock(&priv->mutex);
+	if ((le16_to_cpu(priv->staging_rxon.channel) != channel) ||
+	    (priv->phymode != phymode)) {
+		const struct iwl_channel_info *ch_info;
+
+		ch_info = iwl_get_channel_info(priv, phymode, channel);
+		if (!ch_info) {
+			IWL_WARNING("Requested invalid phymode/channel "
+				    "combination: %d %d\n", phymode, channel);
+			mutex_unlock(&priv->mutex);
+			return -EINVAL;
+		}
+
+		/* Cancel any currently running scans... */
+		if (iwl_scan_cancel_timeout(priv, 100))
+			IWL_WARNING("Could not cancel scan.\n");
+		else {
+			IWL_DEBUG_INFO("Committing phymode and "
+				       "rxon.channel = %d %d\n",
+				       phymode, channel);
+
+			iwl_set_rxon_channel(priv, phymode, channel);
+			iwl_set_flags_for_phymode(priv, phymode);
+
+			iwl_set_rate(priv);
+			iwl_commit_rxon(priv);
+		}
+	}
+	mutex_unlock(&priv->mutex);
+
+	return count;
+}
+
+static DEVICE_ATTR(tune, S_IWUSR | S_IRUGO, show_tune, store_tune);
+
+#ifdef CONFIG_IWLWIFI_SPECTRUM_MEASUREMENT
+
+static ssize_t show_measurement(struct device *d,
+				struct device_attribute *attr, char *buf)
+{
+	struct iwl_priv *priv = dev_get_drvdata(d);
+	struct iwl_spectrum_notification measure_report;
+	u32 size = sizeof(measure_report), len = 0, ofs = 0;
+	u8 *data = (u8 *) & measure_report;
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	if (!(priv->measurement_status & MEASUREMENT_READY)) {
+		spin_unlock_irqrestore(&priv->lock, flags);
+		return 0;
+	}
+	memcpy(&measure_report, &priv->measure_report, size);
+	priv->measurement_status = 0;
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	while (size && (PAGE_SIZE - len)) {
+		hex_dump_to_buffer(data + ofs, size, 16, 1, buf + len,
+				   PAGE_SIZE - len, 1);
+		len = strlen(buf);
+		if (PAGE_SIZE - len)
+			buf[len++] = '\n';
+
+		ofs += 16;
+		size -= min(size, 16U);
+	}
+
+	return len;
+}
+
+static ssize_t store_measurement(struct device *d,
+				 struct device_attribute *attr,
+				 const char *buf, size_t count)
+{
+	struct iwl_priv *priv = dev_get_drvdata(d);
+	struct ieee80211_measurement_params params = {
+		.channel = le16_to_cpu(priv->active_rxon.channel),
+		.start_time = cpu_to_le64(priv->last_tsf),
+		.duration = cpu_to_le16(1),
+	};
+	u8 type = IWL_MEASURE_BASIC;
+	u8 buffer[32];
+	u8 channel;
+
+	if (count) {
+		char *p = buffer;
+		strncpy(buffer, buf, min(sizeof(buffer), count));
+		channel = simple_strtoul(p, NULL, 0);
+		if (channel)
+			params.channel = channel;
+
+		p = buffer;
+		while (*p && *p != ' ')
+			p++;
+		if (*p)
+			type = simple_strtoul(p + 1, NULL, 0);
+	}
+
+	IWL_DEBUG_INFO("Invoking measurement of type %d on "
+		       "channel %d (for '%s')\n", type, params.channel, buf);
+	iwl_get_measurement(priv, &params, type);
+
+	return count;
+}
+
+static DEVICE_ATTR(measurement, S_IRUSR | S_IWUSR,
+		   show_measurement, store_measurement);
+#endif /* CONFIG_IWLWIFI_SPECTRUM_MEASUREMENT */
+
+static ssize_t store_retry_rate(struct device *d,
+				struct device_attribute *attr,
+				const char *buf, size_t count)
+{
+	struct iwl_priv *priv = dev_get_drvdata(d);
+
+	priv->retry_rate = simple_strtoul(buf, NULL, 0);
+	if (priv->retry_rate <= 0)
+		priv->retry_rate = 1;
+
+	return count;
+}
+
+static ssize_t show_retry_rate(struct device *d,
+			       struct device_attribute *attr, char *buf)
+{
+	struct iwl_priv *priv = dev_get_drvdata(d);
+	return sprintf(buf, "%d", priv->retry_rate);
+}
+
+static DEVICE_ATTR(retry_rate, S_IWUSR | S_IRUSR, show_retry_rate,
+		   store_retry_rate);
+
+static ssize_t store_power_level(struct device *d,
+				 struct device_attribute *attr,
+				 const char *buf, size_t count)
+{
+	struct iwl_priv *priv = dev_get_drvdata(d);
+	int rc;
+	int mode;
+
+	mode = simple_strtoul(buf, NULL, 0);
+	mutex_lock(&priv->mutex);
+
+	if (!iwl_is_ready(priv)) {
+		rc = -EAGAIN;
+		goto out;
+	}
+
+	if ((mode < 1) || (mode > IWL_POWER_LIMIT) || (mode == IWL_POWER_AC))
+		mode = IWL_POWER_AC;
+	else
+		mode |= IWL_POWER_ENABLED;
+
+	if (mode != priv->power_mode) {
+		rc = iwl_send_power_mode(priv, IWL_POWER_LEVEL(mode));
+		if (rc) {
+			IWL_DEBUG_MAC80211("failed setting power mode.\n");
+			goto out;
+		}
+		priv->power_mode = mode;
+	}
+
+	rc = count;
+
+ out:
+	mutex_unlock(&priv->mutex);
+	return rc;
+}
+
+#define MAX_WX_STRING 80
+
+/* Values are in microsecond */
+static const s32 timeout_duration[] = {
+	350000,
+	250000,
+	75000,
+	37000,
+	25000,
+};
+static const s32 period_duration[] = {
+	400000,
+	700000,
+	1000000,
+	1000000,
+	1000000
+};
+
+static ssize_t show_power_level(struct device *d,
+				struct device_attribute *attr, char *buf)
+{
+	struct iwl_priv *priv = dev_get_drvdata(d);
+	int level = IWL_POWER_LEVEL(priv->power_mode);
+	char *p = buf;
+
+	p += sprintf(p, "%d ", level);
+	switch (level) {
+	case IWL_POWER_MODE_CAM:
+	case IWL_POWER_AC:
+		p += sprintf(p, "(AC)");
+		break;
+	case IWL_POWER_BATTERY:
+		p += sprintf(p, "(BATTERY)");
+		break;
+	default:
+		p += sprintf(p,
+			     "(Timeout %dms, Period %dms)",
+			     timeout_duration[level - 1] / 1000,
+			     period_duration[level - 1] / 1000);
+	}
+
+	if (!(priv->power_mode & IWL_POWER_ENABLED))
+		p += sprintf(p, " OFF\n");
+	else
+		p += sprintf(p, " \n");
+
+	return (p - buf + 1);
+
+}
+
+static DEVICE_ATTR(power_level, S_IWUSR | S_IRUSR, show_power_level,
+		   store_power_level);
+
+static ssize_t show_channels(struct device *d,
+			     struct device_attribute *attr, char *buf)
+{
+	struct iwl_priv *priv = dev_get_drvdata(d);
+	int len = 0, i;
+	struct ieee80211_channel *channels = NULL;
+	const struct ieee80211_hw_mode *hw_mode = NULL;
+	int count = 0;
+
+	if (!iwl_is_ready(priv))
+		return -EAGAIN;
+
+	hw_mode = iwl_get_hw_mode(priv, MODE_IEEE80211G);
+	if (!hw_mode)
+		hw_mode = iwl_get_hw_mode(priv, MODE_IEEE80211B);
+	if (hw_mode) {
+		channels = hw_mode->channels;
+		count = hw_mode->num_channels;
+	}
+
+	len +=
+	    sprintf(&buf[len],
+		    "Displaying %d channels in 2.4GHz band "
+		    "(802.11bg):\n", count);
+
+	for (i = 0; i < count; i++)
+		len += sprintf(&buf[len], "%d: %ddBm: BSS%s%s, %s.\n",
+			       channels[i].chan,
+			       channels[i].power_level,
+			       channels[i].
+			       flag & IEEE80211_CHAN_W_RADAR_DETECT ?
+			       " (IEEE 802.11h required)" : "",
+			       (!(channels[i].flag & IEEE80211_CHAN_W_IBSS)
+				|| (channels[i].
+				    flag &
+				    IEEE80211_CHAN_W_RADAR_DETECT)) ? "" :
+			       ", IBSS",
+			       channels[i].
+			       flag & IEEE80211_CHAN_W_ACTIVE_SCAN ?
+			       "active/passive" : "passive only");
+
+	hw_mode = iwl_get_hw_mode(priv, MODE_IEEE80211A);
+	if (hw_mode) {
+		channels = hw_mode->channels;
+		count = hw_mode->num_channels;
+	} else {
+		channels = NULL;
+		count = 0;
+	}
+
+	len += sprintf(&buf[len], "Displaying %d channels in 5.2GHz band "
+		       "(802.11a):\n", count);
+
+	for (i = 0; i < count; i++)
+		len += sprintf(&buf[len], "%d: %ddBm: BSS%s%s, %s.\n",
+			       channels[i].chan,
+			       channels[i].power_level,
+			       channels[i].
+			       flag & IEEE80211_CHAN_W_RADAR_DETECT ?
+			       " (IEEE 802.11h required)" : "",
+			       (!(channels[i].flag & IEEE80211_CHAN_W_IBSS)
+				|| (channels[i].
+				    flag &
+				    IEEE80211_CHAN_W_RADAR_DETECT)) ? "" :
+			       ", IBSS",
+			       channels[i].
+			       flag & IEEE80211_CHAN_W_ACTIVE_SCAN ?
+			       "active/passive" : "passive only");
+
+	return len;
+}
+
+static DEVICE_ATTR(channels, S_IRUSR, show_channels, NULL);
+
+static ssize_t show_statistics(struct device *d,
+			       struct device_attribute *attr, char *buf)
+{
+	struct iwl_priv *priv = dev_get_drvdata(d);
+	u32 size = sizeof(struct iwl_notif_statistics);
+	u32 len = 0, ofs = 0;
+	u8 *data = (u8 *) & priv->statistics;
+	int rc = 0;
+
+	if (!iwl_is_alive(priv))
+		return -EAGAIN;
+
+	mutex_lock(&priv->mutex);
+	rc = iwl_send_statistics_request(priv);
+	mutex_unlock(&priv->mutex);
+
+	if (rc) {
+		len = sprintf(buf,
+			      "Error sending statistics request: 0x%08X\n", rc);
+		return len;
+	}
+
+	while (size && (PAGE_SIZE - len)) {
+		hex_dump_to_buffer(data + ofs, size, 16, 1, buf + len,
+				   PAGE_SIZE - len, 1);
+		len = strlen(buf);
+		if (PAGE_SIZE - len)
+			buf[len++] = '\n';
+
+		ofs += 16;
+		size -= min(size, 16U);
+	}
+
+	return len;
+}
+
+static DEVICE_ATTR(statistics, S_IRUGO, show_statistics, NULL);
+
+static ssize_t show_antenna(struct device *d,
+			    struct device_attribute *attr, char *buf)
+{
+	struct iwl_priv *priv = dev_get_drvdata(d);
+
+	if (!iwl_is_alive(priv))
+		return -EAGAIN;
+
+	return sprintf(buf, "%d\n", priv->antenna);
+}
+
+static ssize_t store_antenna(struct device *d,
+			     struct device_attribute *attr,
+			     const char *buf, size_t count)
+{
+	int ant;
+	struct iwl_priv *priv = dev_get_drvdata(d);
+
+	if (count == 0)
+		return 0;
+
+	if (sscanf(buf, "%1i", &ant) != 1) {
+		IWL_DEBUG_INFO("not in hex or decimal form.\n");
+		return count;
+	}
+
+	if ((ant >= 0) && (ant <= 2)) {
+		IWL_DEBUG_INFO("Setting antenna select to %d.\n", ant);
+		priv->antenna = (enum iwl_antenna)ant;
+	} else
+		IWL_DEBUG_INFO("Bad antenna select value %d.\n", ant);
+
+
+	return count;
+}
+
+static DEVICE_ATTR(antenna, S_IWUSR | S_IRUGO, show_antenna, store_antenna);
+
+static ssize_t show_status(struct device *d,
+			   struct device_attribute *attr, char *buf)
+{
+	struct iwl_priv *priv = (struct iwl_priv *)d->driver_data;
+	if (!iwl_is_alive(priv))
+		return -EAGAIN;
+	return sprintf(buf, "0x%08x\n", (int)priv->status);
+}
+
+static DEVICE_ATTR(status, S_IRUGO, show_status, NULL);
+
+static ssize_t dump_error_log(struct device *d,
+			      struct device_attribute *attr,
+			      const char *buf, size_t count)
+{
+	char *p = (char *)buf;
+
+	if (p[0] == '1')
+		iwl_dump_nic_error_log((struct iwl_priv *)d->driver_data);
+
+	return strnlen(buf, count);
+}
+
+static DEVICE_ATTR(dump_errors, S_IWUSR, NULL, dump_error_log);
+
+static ssize_t dump_event_log(struct device *d,
+			      struct device_attribute *attr,
+			      const char *buf, size_t count)
+{
+	char *p = (char *)buf;
+
+	if (p[0] == '1')
+		iwl_dump_nic_event_log((struct iwl_priv *)d->driver_data);
+
+	return strnlen(buf, count);
+}
+
+static DEVICE_ATTR(dump_events, S_IWUSR, NULL, dump_event_log);
+
+/*****************************************************************************
+ *
+ * driver setup and teardown
+ *
+ *****************************************************************************/
+
+static void iwl_setup_deferred_work(struct iwl_priv *priv)
+{
+	priv->workqueue = create_workqueue(DRV_NAME);
+
+	init_waitqueue_head(&priv->wait_command_queue);
+
+	INIT_WORK(&priv->up, iwl_bg_up);
+	INIT_WORK(&priv->restart, iwl_bg_restart);
+	INIT_WORK(&priv->rx_replenish, iwl_bg_rx_replenish);
+	INIT_WORK(&priv->scan_completed, iwl_bg_scan_completed);
+	INIT_WORK(&priv->request_scan, iwl_bg_request_scan);
+	INIT_WORK(&priv->abort_scan, iwl_bg_abort_scan);
+	INIT_WORK(&priv->rf_kill, iwl_bg_rf_kill);
+	INIT_WORK(&priv->beacon_update, iwl_bg_beacon_update);
+	INIT_DELAYED_WORK(&priv->post_associate, iwl_bg_post_associate);
+	INIT_DELAYED_WORK(&priv->init_alive_start, iwl_bg_init_alive_start);
+	INIT_DELAYED_WORK(&priv->alive_start, iwl_bg_alive_start);
+	INIT_DELAYED_WORK(&priv->scan_check, iwl_bg_scan_check);
+
+	iwl_hw_setup_deferred_work(priv);
+
+	tasklet_init(&priv->irq_tasklet, (void (*)(unsigned long))
+		     iwl_irq_tasklet, (unsigned long)priv);
+}
+
+static void iwl_cancel_deferred_work(struct iwl_priv *priv)
+{
+	iwl_hw_cancel_deferred_work(priv);
+
+	cancel_delayed_work(&priv->scan_check);
+	cancel_delayed_work(&priv->alive_start);
+	cancel_delayed_work(&priv->post_associate);
+	cancel_work_sync(&priv->beacon_update);
+}
+
+static struct attribute *iwl_sysfs_entries[] = {
+	&dev_attr_antenna.attr,
+	&dev_attr_channels.attr,
+	&dev_attr_dump_errors.attr,
+	&dev_attr_dump_events.attr,
+	&dev_attr_flags.attr,
+	&dev_attr_filter_flags.attr,
+#ifdef CONFIG_IWLWIFI_SPECTRUM_MEASUREMENT
+	&dev_attr_measurement.attr,
+#endif
+	&dev_attr_power_level.attr,
+	&dev_attr_retry_rate.attr,
+	&dev_attr_rf_kill.attr,
+	&dev_attr_rs_window.attr,
+	&dev_attr_statistics.attr,
+	&dev_attr_status.attr,
+	&dev_attr_temperature.attr,
+	&dev_attr_tune.attr,
+	&dev_attr_tx_power.attr,
+
+	NULL
+};
+
+static struct attribute_group iwl_attribute_group = {
+	.name = NULL,		/* put in device directory */
+	.attrs = iwl_sysfs_entries,
+};
+
+static struct ieee80211_ops iwl_hw_ops = {
+	.tx = iwl_mac_tx,
+	.start = iwl_mac_start,
+	.stop = iwl_mac_stop,
+	.add_interface = iwl_mac_add_interface,
+	.remove_interface = iwl_mac_remove_interface,
+	.config = iwl_mac_config,
+	.config_interface = iwl_mac_config_interface,
+	.configure_filter = iwl_configure_filter,
+	.set_key = iwl_mac_set_key,
+	.get_stats = iwl_mac_get_stats,
+	.get_tx_stats = iwl_mac_get_tx_stats,
+	.conf_tx = iwl_mac_conf_tx,
+	.get_tsf = iwl_mac_get_tsf,
+	.reset_tsf = iwl_mac_reset_tsf,
+	.beacon_update = iwl_mac_beacon_update,
+#ifdef CONFIG_IWLWIFI_HT
+	.conf_ht = iwl_mac_conf_ht,
+	.get_ht_capab = iwl_mac_get_ht_capab,
+#ifdef CONFIG_IWLWIFI_HT_AGG
+	.ht_tx_agg_start = iwl_mac_ht_tx_agg_start,
+	.ht_tx_agg_stop = iwl_mac_ht_tx_agg_stop,
+	.ht_rx_agg_start = iwl_mac_ht_rx_agg_start,
+	.ht_rx_agg_stop = iwl_mac_ht_rx_agg_stop,
+#endif  /* CONFIG_IWLWIFI_HT_AGG */
+#endif  /* CONFIG_IWLWIFI_HT */
+	.hw_scan = iwl_mac_hw_scan
+};
+
+static int iwl_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
+{
+	int err = 0;
+	struct iwl_priv *priv;
+	struct ieee80211_hw *hw;
+	int i;
+
+	if (iwl_param_disable_hw_scan) {
+		IWL_DEBUG_INFO("Disabling hw_scan\n");
+		iwl_hw_ops.hw_scan = NULL;
+	}
+
+	if ((iwl_param_queues_num > IWL_MAX_NUM_QUEUES) ||
+	    (iwl_param_queues_num < IWL_MIN_NUM_QUEUES)) {
+		IWL_ERROR("invalid queues_num, should be between %d and %d\n",
+			  IWL_MIN_NUM_QUEUES, IWL_MAX_NUM_QUEUES);
+		err = -EINVAL;
+		goto out;
+	}
+
+	/* mac80211 allocates memory for this device instance, including
+	 *   space for this driver's private structure */
+	hw = ieee80211_alloc_hw(sizeof(struct iwl_priv), &iwl_hw_ops);
+	if (hw == NULL) {
+		IWL_ERROR("Can not allocate network device\n");
+		err = -ENOMEM;
+		goto out;
+	}
+	SET_IEEE80211_DEV(hw, &pdev->dev);
+
+	IWL_DEBUG_INFO("*** LOAD DRIVER ***\n");
+	priv = hw->priv;
+	priv->hw = hw;
+
+	priv->pci_dev = pdev;
+	priv->antenna = (enum iwl_antenna)iwl_param_antenna;
+#ifdef CONFIG_IWLWIFI_DEBUG
+	iwl_debug_level = iwl_param_debug;
+	atomic_set(&priv->restrict_refcnt, 0);
+#endif
+	priv->retry_rate = 1;
+
+	priv->ibss_beacon = NULL;
+
+	/* Tell mac80211 and its clients (e.g. Wireless Extensions)
+	 *   the range of signal quality values that we'll provide.
+	 * Negative values for level/noise indicate that we'll provide dBm.
+	 * For WE, at least, non-0 values here *enable* display of values
+	 *   in app (iwconfig). */
+	hw->max_rssi = -20;	/* signal level, negative indicates dBm */
+	hw->max_noise = -20;	/* noise level, negative indicates dBm */
+	hw->max_signal = 100;	/* link quality indication (%) */
+
+	/* Tell mac80211 our Tx characteristics */
+	hw->flags = IEEE80211_HW_HOST_GEN_BEACON_TEMPLATE;
+
+	hw->queues = 4;
+#ifdef CONFIG_IWLWIFI_HT
+#ifdef CONFIG_IWLWIFI_HT_AGG
+	hw->queues = 16;
+#endif /* CONFIG_IWLWIFI_HT_AGG */
+#endif /* CONFIG_IWLWIFI_HT */
+
+	spin_lock_init(&priv->lock);
+	spin_lock_init(&priv->power_data.lock);
+	spin_lock_init(&priv->sta_lock);
+	spin_lock_init(&priv->hcmd_lock);
+	spin_lock_init(&priv->lq_mngr.lock);
+
+	for (i = 0; i < IWL_IBSS_MAC_HASH_SIZE; i++)
+		INIT_LIST_HEAD(&priv->ibss_mac_hash[i]);
+
+	INIT_LIST_HEAD(&priv->free_frames);
+
+	mutex_init(&priv->mutex);
+	if (pci_enable_device(pdev)) {
+		err = -ENODEV;
+		goto out_ieee80211_free_hw;
+	}
+
+	pci_set_master(pdev);
+
+	iwl_clear_stations_table(priv);
+
+	priv->data_retry_limit = -1;
+	priv->ieee_channels = NULL;
+	priv->ieee_rates = NULL;
+	priv->phymode = -1;
+
+	err = pci_set_dma_mask(pdev, DMA_32BIT_MASK);
+	if (!err)
+		err = pci_set_consistent_dma_mask(pdev, DMA_32BIT_MASK);
+	if (err) {
+		printk(KERN_WARNING DRV_NAME ": No suitable DMA available.\n");
+		goto out_pci_disable_device;
+	}
+
+	pci_set_drvdata(pdev, priv);
+	err = pci_request_regions(pdev, DRV_NAME);
+	if (err)
+		goto out_pci_disable_device;
+	/* We disable the RETRY_TIMEOUT register (0x41) to keep
+	 * PCI Tx retries from interfering with C3 CPU state */
+	pci_write_config_byte(pdev, 0x41, 0x00);
+	priv->hw_base = pci_iomap(pdev, 0, 0);
+	if (!priv->hw_base) {
+		err = -ENODEV;
+		goto out_pci_release_regions;
+	}
+
+	IWL_DEBUG_INFO("pci_resource_len = 0x%08llx\n",
+			(unsigned long long) pci_resource_len(pdev, 0));
+	IWL_DEBUG_INFO("pci_resource_base = %p\n", priv->hw_base);
+
+	/* Initialize module parameter values here */
+
+	if (iwl_param_disable) {
+		set_bit(STATUS_RF_KILL_SW, &priv->status);
+		IWL_DEBUG_INFO("Radio disabled.\n");
+	}
+
+	priv->iw_mode = IEEE80211_IF_TYPE_STA;
+
+	priv->ps_mode = 0;
+	priv->use_ant_b_for_management_frame = 1; /* start with ant B */
+	priv->is_ht_enabled = 1;
+	priv->channel_width = IWL_CHANNEL_WIDTH_40MHZ;
+	priv->valid_antenna = 0x7;	/* assume all 3 connected */
+	priv->ps_mode = IWL_MIMO_PS_NONE;
+	priv->cck_power_index_compensation = iwl_read32(
+		priv, CSR_HW_REV_WA_REG);
+
+	iwl4965_set_rxon_chain(priv);
+
+	printk(KERN_INFO DRV_NAME
+	       ": Detected Intel Wireless WiFi Link 4965AGN\n");
+
+	/* Device-specific setup */
+	if (iwl_hw_set_hw_setting(priv)) {
+		IWL_ERROR("failed to set hw settings\n");
+		mutex_unlock(&priv->mutex);
+		goto out_iounmap;
+	}
+
+#ifdef CONFIG_IWLWIFI_QOS
+	if (iwl_param_qos_enable)
+		priv->qos_data.qos_enable = 1;
+
+	iwl_reset_qos(priv);
+
+	priv->qos_data.qos_active = 0;
+	priv->qos_data.qos_cap.val = 0;
+#endif /* CONFIG_IWLWIFI_QOS */
+
+	iwl_set_rxon_channel(priv, MODE_IEEE80211G, 6);
+	iwl_setup_deferred_work(priv);
+	iwl_setup_rx_handlers(priv);
+
+	priv->rates_mask = IWL_RATES_MASK;
+	/* If power management is turned on, default to AC mode */
+	priv->power_mode = IWL_POWER_AC;
+	priv->user_txpower_limit = IWL_DEFAULT_TX_POWER;
+
+	pci_enable_msi(pdev);
+
+	err = request_irq(pdev->irq, iwl_isr, IRQF_SHARED, DRV_NAME, priv);
+	if (err) {
+		IWL_ERROR("Error allocating IRQ %d\n", pdev->irq);
+		goto out_disable_msi;
+	}
+
+	mutex_lock(&priv->mutex);
+
+	err = sysfs_create_group(&pdev->dev.kobj, &iwl_attribute_group);
+	if (err) {
+		IWL_ERROR("failed to create sysfs device attributes\n");
+		mutex_unlock(&priv->mutex);
+		goto out_release_irq;
+	}
+
+	/* fetch ucode file from disk, alloc and copy to bus-master buffers ...
+	 * ucode filename and max sizes are card-specific. */
+	err = iwl_read_ucode(priv);
+	if (err) {
+		IWL_ERROR("Could not read microcode: %d\n", err);
+		mutex_unlock(&priv->mutex);
+		goto out_pci_alloc;
+	}
+
+	mutex_unlock(&priv->mutex);
+
+	IWL_DEBUG_INFO("Queing UP work.\n");
+
+	queue_work(priv->workqueue, &priv->up);
+
+	return 0;
+
+ out_pci_alloc:
+	iwl_dealloc_ucode_pci(priv);
+
+	sysfs_remove_group(&pdev->dev.kobj, &iwl_attribute_group);
+
+ out_release_irq:
+	free_irq(pdev->irq, priv);
+
+ out_disable_msi:
+	pci_disable_msi(pdev);
+	destroy_workqueue(priv->workqueue);
+	priv->workqueue = NULL;
+	iwl_unset_hw_setting(priv);
+
+ out_iounmap:
+	pci_iounmap(pdev, priv->hw_base);
+ out_pci_release_regions:
+	pci_release_regions(pdev);
+ out_pci_disable_device:
+	pci_disable_device(pdev);
+	pci_set_drvdata(pdev, NULL);
+ out_ieee80211_free_hw:
+	ieee80211_free_hw(priv->hw);
+ out:
+	return err;
+}
+
+static void iwl_pci_remove(struct pci_dev *pdev)
+{
+	struct iwl_priv *priv = pci_get_drvdata(pdev);
+	struct list_head *p, *q;
+	int i;
+
+	if (!priv)
+		return;
+
+	IWL_DEBUG_INFO("*** UNLOAD DRIVER ***\n");
+
+	mutex_lock(&priv->mutex);
+	set_bit(STATUS_EXIT_PENDING, &priv->status);
+	__iwl_down(priv);
+	mutex_unlock(&priv->mutex);
+
+	/* Free MAC hash list for ADHOC */
+	for (i = 0; i < IWL_IBSS_MAC_HASH_SIZE; i++) {
+		list_for_each_safe(p, q, &priv->ibss_mac_hash[i]) {
+			list_del(p);
+			kfree(list_entry(p, struct iwl_ibss_seq, list));
+		}
+	}
+
+	sysfs_remove_group(&pdev->dev.kobj, &iwl_attribute_group);
+
+	iwl_dealloc_ucode_pci(priv);
+
+	if (priv->rxq.bd)
+		iwl_rx_queue_free(priv, &priv->rxq);
+	iwl_hw_txq_ctx_free(priv);
+
+	iwl_unset_hw_setting(priv);
+	iwl_clear_stations_table(priv);
+
+	if (priv->mac80211_registered) {
+		ieee80211_unregister_hw(priv->hw);
+		iwl_rate_control_unregister(priv->hw);
+	}
+
+	/* ieee80211_unregister_hw calls iwl_mac_stop, which flushes
+	 * priv->workqueue... so we can't take down the workqueue
+	 * until now... */
+	destroy_workqueue(priv->workqueue);
+	priv->workqueue = NULL;
+
+	free_irq(pdev->irq, priv);
+	pci_disable_msi(pdev);
+	pci_iounmap(pdev, priv->hw_base);
+	pci_release_regions(pdev);
+	pci_disable_device(pdev);
+	pci_set_drvdata(pdev, NULL);
+
+	kfree(priv->channel_info);
+
+	kfree(priv->ieee_channels);
+	kfree(priv->ieee_rates);
+
+	if (priv->ibss_beacon)
+		dev_kfree_skb(priv->ibss_beacon);
+
+	ieee80211_free_hw(priv->hw);
+}
+
+#ifdef CONFIG_PM
+
+static int iwl_pci_suspend(struct pci_dev *pdev, pm_message_t state)
+{
+	struct iwl_priv *priv = pci_get_drvdata(pdev);
+
+	mutex_lock(&priv->mutex);
+
+	set_bit(STATUS_IN_SUSPEND, &priv->status);
+
+	/* Take down the device; powers it off, etc. */
+	__iwl_down(priv);
+
+	if (priv->mac80211_registered)
+		ieee80211_stop_queues(priv->hw);
+
+	pci_save_state(pdev);
+	pci_disable_device(pdev);
+	pci_set_power_state(pdev, PCI_D3hot);
+
+	mutex_unlock(&priv->mutex);
+
+	return 0;
+}
+
+static void iwl_resume(struct iwl_priv *priv)
+{
+	unsigned long flags;
+
+	/* The following it a temporary work around due to the
+	 * suspend / resume not fully initializing the NIC correctly.
+	 * Without all of the following, resume will not attempt to take
+	 * down the NIC (it shouldn't really need to) and will just try
+	 * and bring the NIC back up.  However that fails during the
+	 * ucode verification process.  This then causes iwl_down to be
+	 * called *after* iwl_hw_nic_init() has succeeded -- which
+	 * then lets the next init sequence succeed.  So, we've
+	 * replicated all of that NIC init code here... */
+
+	iwl_write32(priv, CSR_INT, 0xFFFFFFFF);
+
+	iwl_hw_nic_init(priv);
+
+	iwl_write32(priv, CSR_UCODE_DRV_GP1_CLR, CSR_UCODE_SW_BIT_RFKILL);
+	iwl_write32(priv, CSR_UCODE_DRV_GP1_CLR,
+		    CSR_UCODE_DRV_GP1_BIT_CMD_BLOCKED);
+	iwl_write32(priv, CSR_INT, 0xFFFFFFFF);
+	iwl_write32(priv, CSR_UCODE_DRV_GP1_CLR, CSR_UCODE_SW_BIT_RFKILL);
+	iwl_write32(priv, CSR_UCODE_DRV_GP1_CLR, CSR_UCODE_SW_BIT_RFKILL);
+
+	/* tell the device to stop sending interrupts */
+	iwl_disable_interrupts(priv);
+
+	spin_lock_irqsave(&priv->lock, flags);
+	iwl_clear_bit(priv, CSR_GP_CNTRL, CSR_GP_CNTRL_REG_FLAG_MAC_ACCESS_REQ);
+
+	if (!iwl_grab_restricted_access(priv)) {
+		iwl_write_restricted_reg(priv, APMG_CLK_DIS_REG,
+					 APMG_CLK_VAL_DMA_CLK_RQT);
+		iwl_release_restricted_access(priv);
+	}
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	udelay(5);
+
+	iwl_hw_nic_reset(priv);
+
+	/* Bring the device back up */
+	clear_bit(STATUS_IN_SUSPEND, &priv->status);
+	queue_work(priv->workqueue, &priv->up);
+}
+
+static int iwl_pci_resume(struct pci_dev *pdev)
+{
+	struct iwl_priv *priv = pci_get_drvdata(pdev);
+	int err;
+
+	printk(KERN_INFO "Coming out of suspend...\n");
+
+	mutex_lock(&priv->mutex);
+
+	pci_set_power_state(pdev, PCI_D0);
+	err = pci_enable_device(pdev);
+	pci_restore_state(pdev);
+
+	/*
+	 * Suspend/Resume resets the PCI configuration space, so we have to
+	 * re-disable the RETRY_TIMEOUT register (0x41) to keep PCI Tx retries
+	 * from interfering with C3 CPU state. pci_restore_state won't help
+	 * here since it only restores the first 64 bytes pci config header.
+	 */
+	pci_write_config_byte(pdev, 0x41, 0x00);
+
+	iwl_resume(priv);
+	mutex_unlock(&priv->mutex);
+
+	return 0;
+}
+
+#endif /* CONFIG_PM */
+
+/*****************************************************************************
+ *
+ * driver and module entry point
+ *
+ *****************************************************************************/
+
+static struct pci_driver iwl_driver = {
+	.name = DRV_NAME,
+	.id_table = iwl_hw_card_ids,
+	.probe = iwl_pci_probe,
+	.remove = __devexit_p(iwl_pci_remove),
+#ifdef CONFIG_PM
+	.suspend = iwl_pci_suspend,
+	.resume = iwl_pci_resume,
+#endif
+};
+
+static int __init iwl_init(void)
+{
+
+	int ret;
+	printk(KERN_INFO DRV_NAME ": " DRV_DESCRIPTION ", " DRV_VERSION "\n");
+	printk(KERN_INFO DRV_NAME ": " DRV_COPYRIGHT "\n");
+	ret = pci_register_driver(&iwl_driver);
+	if (ret) {
+		IWL_ERROR("Unable to initialize PCI module\n");
+		return ret;
+	}
+#ifdef CONFIG_IWLWIFI_DEBUG
+	ret = driver_create_file(&iwl_driver.driver, &driver_attr_debug_level);
+	if (ret) {
+		IWL_ERROR("Unable to create driver sysfs file\n");
+		pci_unregister_driver(&iwl_driver);
+		return ret;
+	}
+#endif
+
+	return ret;
+}
+
+static void __exit iwl_exit(void)
+{
+#ifdef CONFIG_IWLWIFI_DEBUG
+	driver_remove_file(&iwl_driver.driver, &driver_attr_debug_level);
+#endif
+	pci_unregister_driver(&iwl_driver);
+}
+
+module_param_named(antenna, iwl_param_antenna, int, 0444);
+MODULE_PARM_DESC(antenna, "select antenna (1=Main, 2=Aux, default 0 [both])");
+module_param_named(disable, iwl_param_disable, int, 0444);
+MODULE_PARM_DESC(disable, "manually disable the radio (default 0 [radio on])");
+module_param_named(hwcrypto, iwl_param_hwcrypto, int, 0444);
+MODULE_PARM_DESC(hwcrypto,
+		 "using hardware crypto engine (default 0 [software])\n");
+module_param_named(debug, iwl_param_debug, int, 0444);
+MODULE_PARM_DESC(debug, "debug output mask");
+module_param_named(disable_hw_scan, iwl_param_disable_hw_scan, int, 0444);
+MODULE_PARM_DESC(disable_hw_scan, "disable hardware scanning (default 0)");
+
+module_param_named(queues_num, iwl_param_queues_num, int, 0444);
+MODULE_PARM_DESC(queues_num, "number of hw queues.");
+
+/* QoS */
+module_param_named(qos_enable, iwl_param_qos_enable, int, 0444);
+MODULE_PARM_DESC(qos_enable, "enable all QoS functionality");
+
+module_exit(iwl_exit);
+module_init(iwl_init);
diff --git a/drivers/net/wireless/iwlwifi/iwlwifi.h b/drivers/net/wireless/iwlwifi/iwlwifi.h
new file mode 100644
index 000000000000..e0b97c341215
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwlwifi.h
@@ -0,0 +1,713 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2003 - 2007 Intel Corporation. All rights reserved.
+ *
+ * Portions of this file are derived from the ipw3945 project, as well
+ * as portions of the ieee80211 subsystem header files.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of version 2 of the GNU General Public License as
+ * published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU General Public License along with
+ * this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
+ *
+ * The full GNU General Public License is included in this distribution in the
+ * file called LICENSE.
+ *
+ * Contact Information:
+ * James P. Ketrenos <ipw2100-admin@linux.intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ *****************************************************************************/
+
+#ifndef __iwlwifi_h__
+#define __iwlwifi_h__
+
+#include <linux/pci.h> /* for struct pci_device_id */
+#include <linux/kernel.h>
+#include <net/ieee80211_radiotap.h>
+
+struct iwl_priv;
+
+/* Hardware specific file defines the PCI IDs table for that hardware module */
+extern struct pci_device_id iwl_hw_card_ids[];
+
+#if IWL == 3945
+
+#define DRV_NAME	"iwl3945"
+#include "iwl-hw.h"
+#include "iwl-3945-hw.h"
+
+#elif IWL == 4965
+
+#define DRV_NAME        "iwl4965"
+#include "iwl-hw.h"
+#include "iwl-4965-hw.h"
+
+#endif
+
+#include "iwl-prph.h"
+
+/*
+ * Driver implementation data structures, constants, inline
+ * functions
+ *
+ * NOTE:  DO NOT PUT HARDWARE/UCODE SPECIFIC DECLRATIONS HERE
+ *
+ * Hardware specific declrations go into iwl-*hw.h
+ *
+ */
+
+#include "iwl-debug.h"
+
+/* Default noise level to report when noise measurement is not available.
+ *   This may be because we're:
+ *   1)  Not associated (4965, no beacon statistics being sent to driver)
+ *   2)  Scanning (noise measurement does not apply to associated channel)
+ *   3)  Receiving CCK (3945 delivers noise info only for OFDM frames)
+ * Use default noise value of -127 ... this is below the range of measurable
+ *   Rx dBm for either 3945 or 4965, so it can indicate "unmeasurable" to user.
+ *   Also, -127 works better than 0 when averaging frames with/without
+ *   noise info (e.g. averaging might be done in app); measured dBm values are
+ *   always negative ... using a negative value as the default keeps all
+ *   averages within an s8's (used in some apps) range of negative values. */
+#define IWL_NOISE_MEAS_NOT_AVAILABLE (-127)
+
+/* Module parameters accessible from iwl-*.c */
+extern int iwl_param_disable_hw_scan;
+extern int iwl_param_debug;
+extern int iwl_param_mode;
+extern int iwl_param_disable;
+extern int iwl_param_antenna;
+extern int iwl_param_hwcrypto;
+extern int iwl_param_qos_enable;
+extern int iwl_param_queues_num;
+
+enum iwl_antenna {
+	IWL_ANTENNA_DIVERSITY,
+	IWL_ANTENNA_MAIN,
+	IWL_ANTENNA_AUX
+};
+
+/*
+ * RTS threshold here is total size [2347] minus 4 FCS bytes
+ * Per spec:
+ *   a value of 0 means RTS on all data/management packets
+ *   a value > max MSDU size means no RTS
+ * else RTS for data/management frames where MPDU is larger
+ *   than RTS value.
+ */
+#define DEFAULT_RTS_THRESHOLD     2347U
+#define MIN_RTS_THRESHOLD         0U
+#define MAX_RTS_THRESHOLD         2347U
+#define MAX_MSDU_SIZE		  2304U
+#define MAX_MPDU_SIZE		  2346U
+#define DEFAULT_BEACON_INTERVAL   100U
+#define	DEFAULT_SHORT_RETRY_LIMIT 7U
+#define	DEFAULT_LONG_RETRY_LIMIT  4U
+
+struct iwl_rx_mem_buffer {
+	dma_addr_t dma_addr;
+	struct sk_buff *skb;
+	struct list_head list;
+};
+
+struct iwl_rt_rx_hdr {
+	struct ieee80211_radiotap_header rt_hdr;
+	__le64 rt_tsf;		/* TSF */
+	u8 rt_flags;		/* radiotap packet flags */
+	u8 rt_rate;		/* rate in 500kb/s */
+	__le16 rt_channelMHz;	/* channel in MHz */
+	__le16 rt_chbitmask;	/* channel bitfield */
+	s8 rt_dbmsignal;	/* signal in dBm, kluged to signed */
+	s8 rt_dbmnoise;
+	u8 rt_antenna;		/* antenna number */
+	u8 payload[0];		/* payload... */
+} __attribute__ ((packed));
+
+struct iwl_rt_tx_hdr {
+	struct ieee80211_radiotap_header rt_hdr;
+	u8 rt_rate;		/* rate in 500kb/s */
+	__le16 rt_channel;	/* channel in mHz */
+	__le16 rt_chbitmask;	/* channel bitfield */
+	s8 rt_dbmsignal;	/* signal in dBm, kluged to signed */
+	u8 rt_antenna;		/* antenna number */
+	u8 payload[0];		/* payload... */
+} __attribute__ ((packed));
+
+/*
+ * Generic queue structure
+ *
+ * Contains common data for Rx and Tx queues
+ */
+struct iwl_queue {
+	int n_bd;              /* number of BDs in this queue */
+	int first_empty;       /* 1-st empty entry (index) host_w*/
+	int last_used;         /* last used entry (index) host_r*/
+	dma_addr_t dma_addr;   /* physical addr for BD's */
+	int n_window;	       /* safe queue window */
+	u32 id;
+	int low_mark;	       /* low watermark, resume queue if free
+				* space more than this */
+	int high_mark;         /* high watermark, stop queue if free
+				* space less than this */
+} __attribute__ ((packed));
+
+#define MAX_NUM_OF_TBS          (20)
+
+struct iwl_tx_info {
+	struct ieee80211_tx_status status;
+	struct sk_buff *skb[MAX_NUM_OF_TBS];
+};
+
+/**
+ * struct iwl_tx_queue - Tx Queue for DMA
+ * @need_update: need to update read/write index
+ * @shed_retry: queue is HT AGG enabled
+ *
+ * Queue consists of circular buffer of BD's and required locking structures.
+ */
+struct iwl_tx_queue {
+	struct iwl_queue q;
+	struct iwl_tfd_frame *bd;
+	struct iwl_cmd *cmd;
+	dma_addr_t dma_addr_cmd;
+	struct iwl_tx_info *txb;
+	int need_update;
+	int sched_retry;
+	int active;
+};
+
+#include "iwl-channel.h"
+
+#if IWL == 3945
+#include "iwl-3945-rs.h"
+#else
+#include "iwl-4965-rs.h"
+#endif
+
+#define IWL_TX_FIFO_AC0	0
+#define IWL_TX_FIFO_AC1	1
+#define IWL_TX_FIFO_AC2	2
+#define IWL_TX_FIFO_AC3	3
+#define IWL_TX_FIFO_HCCA_1	5
+#define IWL_TX_FIFO_HCCA_2	6
+#define IWL_TX_FIFO_NONE	7
+
+/* Minimum number of queues. MAX_NUM is defined in hw specific files */
+#define IWL_MIN_NUM_QUEUES	4
+
+/* Power management (not Tx power) structures */
+
+struct iwl_power_vec_entry {
+	struct iwl_powertable_cmd cmd;
+	u8 no_dtim;
+};
+#define IWL_POWER_RANGE_0  (0)
+#define IWL_POWER_RANGE_1  (1)
+
+#define IWL_POWER_MODE_CAM	0x00	/* Continuously Aware Mode, always on */
+#define IWL_POWER_INDEX_3	0x03
+#define IWL_POWER_INDEX_5	0x05
+#define IWL_POWER_AC		0x06
+#define IWL_POWER_BATTERY	0x07
+#define IWL_POWER_LIMIT		0x07
+#define IWL_POWER_MASK		0x0F
+#define IWL_POWER_ENABLED	0x10
+#define IWL_POWER_LEVEL(x)	((x) & IWL_POWER_MASK)
+
+struct iwl_power_mgr {
+	spinlock_t lock;
+	struct iwl_power_vec_entry pwr_range_0[IWL_POWER_AC];
+	struct iwl_power_vec_entry pwr_range_1[IWL_POWER_AC];
+	u8 active_index;
+	u32 dtim_val;
+};
+
+#define IEEE80211_DATA_LEN              2304
+#define IEEE80211_4ADDR_LEN             30
+#define IEEE80211_HLEN                  (IEEE80211_4ADDR_LEN)
+#define IEEE80211_FRAME_LEN             (IEEE80211_DATA_LEN + IEEE80211_HLEN)
+
+struct iwl_frame {
+	union {
+		struct ieee80211_hdr frame;
+		struct iwl_tx_beacon_cmd beacon;
+		u8 raw[IEEE80211_FRAME_LEN];
+		u8 cmd[360];
+	} u;
+	struct list_head list;
+};
+
+#define SEQ_TO_QUEUE(x)  ((x >> 8) & 0xbf)
+#define QUEUE_TO_SEQ(x)  ((x & 0xbf) << 8)
+#define SEQ_TO_INDEX(x) (x & 0xff)
+#define INDEX_TO_SEQ(x) (x & 0xff)
+#define SEQ_HUGE_FRAME  (0x4000)
+#define SEQ_RX_FRAME    __constant_cpu_to_le16(0x8000)
+#define SEQ_TO_SN(seq) (((seq) & IEEE80211_SCTL_SEQ) >> 4)
+#define SN_TO_SEQ(ssn) (((ssn) << 4) & IEEE80211_SCTL_SEQ)
+#define MAX_SN ((IEEE80211_SCTL_SEQ) >> 4)
+
+enum {
+	/* CMD_SIZE_NORMAL = 0, */
+	CMD_SIZE_HUGE = (1 << 0),
+	/* CMD_SYNC = 0, */
+	CMD_ASYNC = (1 << 1),
+	/* CMD_NO_SKB = 0, */
+	CMD_WANT_SKB = (1 << 2),
+};
+
+struct iwl_cmd;
+struct iwl_priv;
+
+struct iwl_cmd_meta {
+	struct iwl_cmd_meta *source;
+	union {
+		struct sk_buff *skb;
+		int (*callback)(struct iwl_priv *priv,
+				struct iwl_cmd *cmd, struct sk_buff *skb);
+	} __attribute__ ((packed)) u;
+
+	/* The CMD_SIZE_HUGE flag bit indicates that the command
+	 * structure is stored at the end of the shared queue memory. */
+	u32 flags;
+
+} __attribute__ ((packed));
+
+struct iwl_cmd {
+	struct iwl_cmd_meta meta;
+	struct iwl_cmd_header hdr;
+	union {
+		struct iwl_addsta_cmd addsta;
+		struct iwl_led_cmd led;
+		u32 flags;
+		u8 val8;
+		u16 val16;
+		u32 val32;
+		struct iwl_bt_cmd bt;
+		struct iwl_rxon_time_cmd rxon_time;
+		struct iwl_powertable_cmd powertable;
+		struct iwl_qosparam_cmd qosparam;
+		struct iwl_tx_cmd tx;
+		struct iwl_tx_beacon_cmd tx_beacon;
+		struct iwl_rxon_assoc_cmd rxon_assoc;
+		u8 *indirect;
+		u8 payload[360];
+	} __attribute__ ((packed)) cmd;
+} __attribute__ ((packed));
+
+struct iwl_host_cmd {
+	u8 id;
+	u16 len;
+	struct iwl_cmd_meta meta;
+	const void *data;
+};
+
+#define TFD_MAX_PAYLOAD_SIZE (sizeof(struct iwl_cmd) - \
+			      sizeof(struct iwl_cmd_meta))
+
+/*
+ * RX related structures and functions
+ */
+#define RX_FREE_BUFFERS 64
+#define RX_LOW_WATERMARK 8
+
+#define SUP_RATE_11A_MAX_NUM_CHANNELS  8
+#define SUP_RATE_11B_MAX_NUM_CHANNELS  4
+#define SUP_RATE_11G_MAX_NUM_CHANNELS  12
+
+/**
+ * struct iwl_rx_queue - Rx queue
+ * @processed: Internal index to last handled Rx packet
+ * @read: Shared index to newest available Rx buffer
+ * @write: Shared index to oldest written Rx packet
+ * @free_count: Number of pre-allocated buffers in rx_free
+ * @rx_free: list of free SKBs for use
+ * @rx_used: List of Rx buffers with no SKB
+ * @need_update: flag to indicate we need to update read/write index
+ *
+ * NOTE:  rx_free and rx_used are used as a FIFO for iwl_rx_mem_buffers
+ */
+struct iwl_rx_queue {
+	__le32 *bd;
+	dma_addr_t dma_addr;
+	struct iwl_rx_mem_buffer pool[RX_QUEUE_SIZE + RX_FREE_BUFFERS];
+	struct iwl_rx_mem_buffer *queue[RX_QUEUE_SIZE];
+	u32 processed;
+	u32 read;
+	u32 write;
+	u32 free_count;
+	struct list_head rx_free;
+	struct list_head rx_used;
+	int need_update;
+	spinlock_t lock;
+};
+
+#define IWL_SUPPORTED_RATES_IE_LEN         8
+
+#define SCAN_INTERVAL 100
+
+#define MAX_A_CHANNELS  252
+#define MIN_A_CHANNELS  7
+
+#define MAX_B_CHANNELS  14
+#define MIN_B_CHANNELS  1
+
+#define STATUS_HCMD_ACTIVE	0	/* host command in progress */
+#define STATUS_INT_ENABLED	1
+#define STATUS_RF_KILL_HW	2
+#define STATUS_RF_KILL_SW	3
+#define STATUS_INIT		4
+#define STATUS_ALIVE		5
+#define STATUS_READY		6
+#define STATUS_TEMPERATURE	7
+#define STATUS_GEO_CONFIGURED	8
+#define STATUS_EXIT_PENDING	9
+#define STATUS_IN_SUSPEND	10
+#define STATUS_STATISTICS	11
+#define STATUS_SCANNING		12
+#define STATUS_SCAN_ABORTING	13
+#define STATUS_SCAN_HW		14
+#define STATUS_POWER_PMI	15
+#define STATUS_FW_ERROR		16
+
+#define MAX_TID_COUNT        9
+
+#define IWL_INVALID_RATE     0xFF
+#define IWL_INVALID_VALUE    -1
+
+#if IWL == 4965
+#ifdef CONFIG_IWLWIFI_HT
+#ifdef CONFIG_IWLWIFI_HT_AGG
+struct iwl_ht_agg {
+	u16 txq_id;
+	u16 frame_count;
+	u16 wait_for_ba;
+	u16 start_idx;
+	u32 bitmap0;
+	u32 bitmap1;
+	u32 rate_n_flags;
+};
+#endif /* CONFIG_IWLWIFI_HT_AGG */
+#endif /* CONFIG_IWLWIFI_HT */
+#endif
+
+struct iwl_tid_data {
+	u16 seq_number;
+#if IWL == 4965
+#ifdef CONFIG_IWLWIFI_HT
+#ifdef CONFIG_IWLWIFI_HT_AGG
+	struct iwl_ht_agg agg;
+#endif	/* CONFIG_IWLWIFI_HT_AGG */
+#endif /* CONFIG_IWLWIFI_HT */
+#endif
+};
+
+struct iwl_hw_key {
+	enum ieee80211_key_alg alg;
+	int keylen;
+	u8 key[32];
+};
+
+union iwl_ht_rate_supp {
+	u16 rates;
+	struct {
+		u8 siso_rate;
+		u8 mimo_rate;
+	};
+};
+
+#ifdef CONFIG_IWLWIFI_HT
+#define CFG_HT_RX_AMPDU_FACTOR_DEF  (0x3)
+#define HT_IE_MAX_AMSDU_SIZE_4K     (0)
+#define CFG_HT_MPDU_DENSITY_2USEC   (0x5)
+#define CFG_HT_MPDU_DENSITY_DEF CFG_HT_MPDU_DENSITY_2USEC
+
+struct sta_ht_info {
+	u8 is_ht;
+	u16 rx_mimo_ps_mode;
+	u16 tx_mimo_ps_mode;
+	u16 control_channel;
+	u8 max_amsdu_size;
+	u8 ampdu_factor;
+	u8 mpdu_density;
+	u8 operating_mode;
+	u8 supported_chan_width;
+	u8 extension_chan_offset;
+	u8 is_green_field;
+	u8 sgf;
+	u8 supp_rates[16];
+	u8 tx_chan_width;
+	u8 chan_width_cap;
+};
+#endif				/*CONFIG_IWLWIFI_HT */
+
+#ifdef CONFIG_IWLWIFI_QOS
+
+union iwl_qos_capabity {
+	struct {
+		u8 edca_count:4;	/* bit 0-3 */
+		u8 q_ack:1;		/* bit 4 */
+		u8 queue_request:1;	/* bit 5 */
+		u8 txop_request:1;	/* bit 6 */
+		u8 reserved:1;		/* bit 7 */
+	} q_AP;
+	struct {
+		u8 acvo_APSD:1;		/* bit 0 */
+		u8 acvi_APSD:1;		/* bit 1 */
+		u8 ac_bk_APSD:1;	/* bit 2 */
+		u8 ac_be_APSD:1;	/* bit 3 */
+		u8 q_ack:1;		/* bit 4 */
+		u8 max_len:2;		/* bit 5-6 */
+		u8 more_data_ack:1;	/* bit 7 */
+	} q_STA;
+	u8 val;
+};
+
+/* QoS sturctures */
+struct iwl_qos_info {
+	int qos_enable;
+	int qos_active;
+	union iwl_qos_capabity qos_cap;
+	struct iwl_qosparam_cmd def_qos_parm;
+};
+#endif /*CONFIG_IWLWIFI_QOS */
+
+#define STA_PS_STATUS_WAKE             0
+#define STA_PS_STATUS_SLEEP            1
+
+struct iwl_station_entry {
+	struct iwl_addsta_cmd sta;
+	struct iwl_tid_data tid[MAX_TID_COUNT];
+#if IWL == 3945
+	union {
+		struct {
+			u8 rate;
+			u8 flags;
+		} s;
+		u16 rate_n_flags;
+	} current_rate;
+#endif
+	u8 used;
+	u8 ps_status;
+	struct iwl_hw_key keyinfo;
+};
+
+/* one for each uCode image (inst/data, boot/init/runtime) */
+struct fw_image_desc {
+	void *v_addr;		/* access by driver */
+	dma_addr_t p_addr;	/* access by card's busmaster DMA */
+	u32 len;		/* bytes */
+};
+
+/* uCode file layout */
+struct iwl_ucode {
+	__le32 ver;		/* major/minor/subminor */
+	__le32 inst_size;	/* bytes of runtime instructions */
+	__le32 data_size;	/* bytes of runtime data */
+	__le32 init_size;	/* bytes of initialization instructions */
+	__le32 init_data_size;	/* bytes of initialization data */
+	__le32 boot_size;	/* bytes of bootstrap instructions */
+	u8 data[0];		/* data in same order as "size" elements */
+};
+
+#define IWL_IBSS_MAC_HASH_SIZE 32
+
+struct iwl_ibss_seq {
+	u8 mac[ETH_ALEN];
+	u16 seq_num;
+	u16 frag_num;
+	unsigned long packet_time;
+	struct list_head list;
+};
+
+struct iwl_driver_hw_info {
+	u16 max_txq_num;
+	u16 ac_queue_count;
+	u32 rx_buffer_size;
+	u16 tx_cmd_len;
+	u16 max_rxq_size;
+	u16 max_rxq_log;
+	u32 cck_flag;
+	u8  max_stations;
+	u8  bcast_sta_id;
+	void *shared_virt;
+	dma_addr_t shared_phys;
+};
+
+
+#define STA_FLG_RTS_MIMO_PROT_MSK	__constant_cpu_to_le32(1 << 17)
+#define STA_FLG_AGG_MPDU_8US_MSK	__constant_cpu_to_le32(1 << 18)
+#define STA_FLG_MAX_AGG_SIZE_POS	(19)
+#define STA_FLG_MAX_AGG_SIZE_MSK	__constant_cpu_to_le32(3 << 19)
+#define STA_FLG_FAT_EN_MSK		__constant_cpu_to_le32(1 << 21)
+#define STA_FLG_MIMO_DIS_MSK		__constant_cpu_to_le32(1 << 22)
+#define STA_FLG_AGG_MPDU_DENSITY_POS	(23)
+#define STA_FLG_AGG_MPDU_DENSITY_MSK	__constant_cpu_to_le32(7 << 23)
+#define HT_SHORT_GI_20MHZ_ONLY          (1 << 0)
+#define HT_SHORT_GI_40MHZ_ONLY          (1 << 1)
+
+
+#include "iwl-priv.h"
+
+/* Requires full declaration of iwl_priv before including */
+#include "iwl-io.h"
+
+#define IWL_RX_HDR(x) ((struct iwl_rx_frame_hdr *)(\
+		       x->u.rx_frame.stats.payload + \
+		       x->u.rx_frame.stats.phy_count))
+#define IWL_RX_END(x) ((struct iwl_rx_frame_end *)(\
+		       IWL_RX_HDR(x)->payload + \
+		       le16_to_cpu(IWL_RX_HDR(x)->len)))
+#define IWL_RX_STATS(x) (&x->u.rx_frame.stats)
+#define IWL_RX_DATA(x) (IWL_RX_HDR(x)->payload)
+
+
+/******************************************************************************
+ *
+ * Functions implemented in iwl-base.c which are forward declared here
+ * for use by iwl-*.c
+ *
+ *****************************************************************************/
+struct iwl_addsta_cmd;
+extern int iwl_send_add_station(struct iwl_priv *priv,
+				struct iwl_addsta_cmd *sta, u8 flags);
+extern const char *iwl_get_tx_fail_reason(u32 status);
+extern u8 iwl_add_station(struct iwl_priv *priv, const u8 *bssid,
+			  int is_ap, u8 flags);
+extern int iwl_is_network_packet(struct iwl_priv *priv,
+				 struct ieee80211_hdr *header);
+extern int iwl_power_init_handle(struct iwl_priv *priv);
+extern int iwl_eeprom_init(struct iwl_priv *priv);
+#ifdef CONFIG_IWLWIFI_DEBUG
+extern void iwl_report_frame(struct iwl_priv *priv,
+			     struct iwl_rx_packet *pkt,
+			     struct ieee80211_hdr *header, int group100);
+#else
+static inline void iwl_report_frame(struct iwl_priv *priv,
+				    struct iwl_rx_packet *pkt,
+				    struct ieee80211_hdr *header,
+				    int group100) {}
+#endif
+extern int iwl_tx_queue_update_write_ptr(struct iwl_priv *priv,
+					 struct iwl_tx_queue *txq);
+extern void iwl_handle_data_packet_monitor(struct iwl_priv *priv,
+					   struct iwl_rx_mem_buffer *rxb,
+					   void *data, short len,
+					   struct ieee80211_rx_status *stats,
+					   u16 phy_flags);
+extern int is_duplicate_packet(struct iwl_priv *priv, struct ieee80211_hdr
+			       *header);
+extern void iwl_rx_queue_free(struct iwl_priv *priv, struct iwl_rx_queue *rxq);
+extern int iwl_rx_queue_alloc(struct iwl_priv *priv);
+extern void iwl_rx_queue_reset(struct iwl_priv *priv,
+			       struct iwl_rx_queue *rxq);
+extern int iwl_calc_db_from_ratio(int sig_ratio);
+extern int iwl_calc_sig_qual(int rssi_dbm, int noise_dbm);
+extern int iwl_tx_queue_init(struct iwl_priv *priv,
+			     struct iwl_tx_queue *txq, int count, u32 id);
+extern int iwl_rx_queue_restock(struct iwl_priv *priv);
+extern void iwl_rx_replenish(void *data);
+extern void iwl_tx_queue_free(struct iwl_priv *priv, struct iwl_tx_queue *txq);
+extern int iwl_send_cmd_pdu(struct iwl_priv *priv, u8 id, u16 len,
+			    const void *data);
+extern int __must_check iwl_send_cmd_async(struct iwl_priv *priv,
+		struct iwl_host_cmd *cmd);
+extern int __must_check iwl_send_cmd_sync(struct iwl_priv *priv,
+		struct iwl_host_cmd *cmd);
+extern int __must_check iwl_send_cmd(struct iwl_priv *priv,
+		struct iwl_host_cmd *cmd);
+extern unsigned int iwl_fill_beacon_frame(struct iwl_priv *priv,
+					struct ieee80211_hdr *hdr,
+					const u8 *dest, int left);
+extern int iwl_rx_queue_update_write_ptr(struct iwl_priv *priv,
+					 struct iwl_rx_queue *q);
+extern int iwl_send_statistics_request(struct iwl_priv *priv);
+extern void iwl_set_decrypted_flag(struct iwl_priv *priv, struct sk_buff *skb,
+				   u32 decrypt_res,
+				   struct ieee80211_rx_status *stats);
+extern __le16 *ieee80211_get_qos_ctrl(struct ieee80211_hdr *hdr);
+
+extern const u8 BROADCAST_ADDR[ETH_ALEN];
+
+/*
+ * Currently used by iwl-3945-rs... look at restructuring so that it doesn't
+ * call this... todo... fix that.
+*/
+extern u8 iwl_sync_station(struct iwl_priv *priv, int sta_id,
+			   u16 tx_rate, u8 flags);
+
+static inline int iwl_is_associated(struct iwl_priv *priv)
+{
+	return (priv->active_rxon.filter_flags & RXON_FILTER_ASSOC_MSK) ? 1 : 0;
+}
+
+/******************************************************************************
+ *
+ * Functions implemented in iwl-[34]*.c which are forward declared here
+ * for use by iwl-base.c
+ *
+ * NOTE:  The implementation of these functions are hardware specific
+ * which is why they are in the hardware specific files (vs. iwl-base.c)
+ *
+ * Naming convention --
+ * iwl_         <-- Its part of iwlwifi (should be changed to iwl_)
+ * iwl_hw_      <-- Hardware specific (implemented in iwl-XXXX.c by all HW)
+ * iwlXXXX_     <-- Hardware specific (implemented in iwl-XXXX.c for XXXX)
+ * iwl_bg_      <-- Called from work queue context
+ * iwl_mac_     <-- mac80211 callback
+ *
+ ****************************************************************************/
+extern void iwl_hw_rx_handler_setup(struct iwl_priv *priv);
+extern void iwl_hw_setup_deferred_work(struct iwl_priv *priv);
+extern void iwl_hw_cancel_deferred_work(struct iwl_priv *priv);
+extern int iwl_hw_rxq_stop(struct iwl_priv *priv);
+extern int iwl_hw_set_hw_setting(struct iwl_priv *priv);
+extern int iwl_hw_nic_init(struct iwl_priv *priv);
+extern void iwl_hw_card_show_info(struct iwl_priv *priv);
+extern int iwl_hw_nic_stop_master(struct iwl_priv *priv);
+extern void iwl_hw_txq_ctx_free(struct iwl_priv *priv);
+extern void iwl_hw_txq_ctx_stop(struct iwl_priv *priv);
+extern int iwl_hw_nic_reset(struct iwl_priv *priv);
+extern int iwl_hw_txq_attach_buf_to_tfd(struct iwl_priv *priv, void *tfd,
+					dma_addr_t addr, u16 len);
+extern int iwl_hw_txq_free_tfd(struct iwl_priv *priv, struct iwl_tx_queue *txq);
+extern int iwl_hw_get_temperature(struct iwl_priv *priv);
+extern int iwl_hw_tx_queue_init(struct iwl_priv *priv,
+				struct iwl_tx_queue *txq);
+extern unsigned int iwl_hw_get_beacon_cmd(struct iwl_priv *priv,
+				 struct iwl_frame *frame, u8 rate);
+extern int iwl_hw_get_rx_read(struct iwl_priv *priv);
+extern void iwl_hw_build_tx_cmd_rate(struct iwl_priv *priv,
+				     struct iwl_cmd *cmd,
+				     struct ieee80211_tx_control *ctrl,
+				     struct ieee80211_hdr *hdr,
+				     int sta_id, int tx_id);
+extern int iwl_hw_reg_send_txpower(struct iwl_priv *priv);
+extern int iwl_hw_reg_set_txpower(struct iwl_priv *priv, s8 power);
+extern void iwl_hw_rx_statistics(struct iwl_priv *priv,
+				 struct iwl_rx_mem_buffer *rxb);
+extern void iwl_disable_events(struct iwl_priv *priv);
+extern int iwl4965_get_temperature(const struct iwl_priv *priv);
+
+/**
+ * iwl_hw_find_station - Find station id for a given BSSID
+ * @bssid: MAC address of station ID to find
+ *
+ * NOTE:  This should not be hardware specific but the code has
+ * not yet been merged into a single common layer for managing the
+ * station tables.
+ */
+extern u8 iwl_hw_find_station(struct iwl_priv *priv, const u8 *bssid);
+
+extern int iwl_hw_channel_switch(struct iwl_priv *priv, u16 channel);
+extern int iwl_tx_queue_reclaim(struct iwl_priv *priv, int txq_id, int index);
+#endif
diff --git a/drivers/net/wireless/libertas/11d.c b/drivers/net/wireless/libertas/11d.c
index 4cf0ff7b833d..9cf0211de67f 100644
--- a/drivers/net/wireless/libertas/11d.c
+++ b/drivers/net/wireless/libertas/11d.c
@@ -124,17 +124,17 @@ static u8 wlan_channel_known_11d(u8 chan,
 	u8 nr_chan = parsed_region_chan->nr_chan;
 	u8 i = 0;
 
-	lbs_dbg_hex("11D:parsed_region_chan:", (char *)chanpwr,
+	lbs_deb_hex(LBS_DEB_11D, "parsed_region_chan", (char *)chanpwr,
 		sizeof(struct chan_power_11d) * nr_chan);
 
 	for (i = 0; i < nr_chan; i++) {
 		if (chan == chanpwr[i].chan) {
-			lbs_deb_11d("11D: Found Chan:%d\n", chan);
+			lbs_deb_11d("found chan %d\n", chan);
 			return 1;
 		}
 	}
 
-	lbs_deb_11d("11D: Not Find Chan:%d\n", chan);
+	lbs_deb_11d("chan %d not found\n", chan);
 	return 0;
 }
 
@@ -174,8 +174,8 @@ static int generate_domain_info_11d(struct parsed_region_chan_11d
 	memcpy(domaininfo->countrycode, parsed_region_chan->countrycode,
 	       COUNTRY_CODE_LEN);
 
-	lbs_deb_11d("11D:nrchan=%d\n", nr_chan);
-	lbs_dbg_hex("11D:parsed_region_chan:", (char *)parsed_region_chan,
+	lbs_deb_11d("nrchan %d\n", nr_chan);
+	lbs_deb_hex(LBS_DEB_11D, "parsed_region_chan", (char *)parsed_region_chan,
 		sizeof(struct parsed_region_chan_11d));
 
 	for (i = 0; i < nr_chan; i++) {
@@ -213,7 +213,7 @@ static int generate_domain_info_11d(struct parsed_region_chan_11d
 	domaininfo->nr_subband = nr_subband;
 
 	lbs_deb_11d("nr_subband=%x\n", domaininfo->nr_subband);
-	lbs_dbg_hex("11D:domaininfo:", (char *)domaininfo,
+	lbs_deb_hex(LBS_DEB_11D, "domaininfo", (char *)domaininfo,
 		COUNTRY_CODE_LEN + 1 +
 		sizeof(struct ieeetypes_subbandset) * nr_subband);
 	return 0;
@@ -233,13 +233,13 @@ static void wlan_generate_parsed_region_chan_11d(struct region_channel * region_
 	struct chan_freq_power *cfp;
 
 	if (region_chan == NULL) {
-		lbs_deb_11d("11D: region_chan is NULL\n");
+		lbs_deb_11d("region_chan is NULL\n");
 		return;
 	}
 
 	cfp = region_chan->CFP;
 	if (cfp == NULL) {
-		lbs_deb_11d("11D: cfp equal NULL \n");
+		lbs_deb_11d("cfp is NULL \n");
 		return;
 	}
 
@@ -248,19 +248,19 @@ static void wlan_generate_parsed_region_chan_11d(struct region_channel * region_
 	memcpy(parsed_region_chan->countrycode,
 	       wlan_code_2_region(region_chan->region), COUNTRY_CODE_LEN);
 
-	lbs_deb_11d("11D: region[0x%x] band[%d]\n", parsed_region_chan->region,
+	lbs_deb_11d("region 0x%x, band %d\n", parsed_region_chan->region,
 	       parsed_region_chan->band);
 
 	for (i = 0; i < region_chan->nrcfp; i++, cfp++) {
 		parsed_region_chan->chanpwr[i].chan = cfp->channel;
 		parsed_region_chan->chanpwr[i].pwr = cfp->maxtxpower;
-		lbs_deb_11d("11D: Chan[%d] Pwr[%d]\n",
+		lbs_deb_11d("chan %d, pwr %d\n",
 		       parsed_region_chan->chanpwr[i].chan,
 		       parsed_region_chan->chanpwr[i].pwr);
 	}
 	parsed_region_chan->nr_chan = region_chan->nrcfp;
 
-	lbs_deb_11d("11D: nrchan[%d]\n", parsed_region_chan->nr_chan);
+	lbs_deb_11d("nrchan %d\n", parsed_region_chan->nr_chan);
 
 	return;
 }
@@ -336,7 +336,7 @@ static int parse_domain_info_11d(struct ieeetypes_countryinfofullset*
 	   6. Others
 	 */
 
-	lbs_dbg_hex("CountryInfo:", (u8 *) countryinfo, 30);
+	lbs_deb_hex(LBS_DEB_11D, "countryinfo", (u8 *) countryinfo, 30);
 
 	if ((*(countryinfo->countrycode)) == 0
 	    || (countryinfo->len <= COUNTRY_CODE_LEN)) {
@@ -349,7 +349,7 @@ static int parse_domain_info_11d(struct ieeetypes_countryinfofullset*
 	    wlan_region_2_code(countryinfo->countrycode);
 
 	lbs_deb_11d("regioncode=%x\n", (u8) parsed_region_chan->region);
-	lbs_dbg_hex("CountryCode:", (char *)countryinfo->countrycode,
+	lbs_deb_hex(LBS_DEB_11D, "countrycode", (char *)countryinfo->countrycode,
 		COUNTRY_CODE_LEN);
 
 	parsed_region_chan->band = band;
@@ -364,7 +364,7 @@ static int parse_domain_info_11d(struct ieeetypes_countryinfofullset*
 
 		if (countryinfo->subband[j].firstchan <= lastchan) {
 			/*Step2&3. Check First Chan Num increment and no overlap */
-			lbs_deb_11d("11D: Chan[%d>%d] Overlap\n",
+			lbs_deb_11d("chan %d>%d, overlap\n",
 			       countryinfo->subband[j].firstchan, lastchan);
 			continue;
 		}
@@ -393,7 +393,7 @@ static int parse_domain_info_11d(struct ieeetypes_countryinfofullset*
 			} else {
 				/*not supported and ignore the chan */
 				lbs_deb_11d(
-				       "11D:i[%d] chan[%d] unsupported in region[%x] band[%d]\n",
+				       "i %d, chan %d unsupported in region %x, band %d\n",
 				       i, curchan, region, band);
 			}
 		}
@@ -405,7 +405,7 @@ static int parse_domain_info_11d(struct ieeetypes_countryinfofullset*
 	parsed_region_chan->nr_chan = idx;
 
 	lbs_deb_11d("nrchan=%x\n", parsed_region_chan->nr_chan);
-	lbs_dbg_hex("11D:parsed_region_chan:", (u8 *) parsed_region_chan,
+	lbs_deb_hex(LBS_DEB_11D, "parsed_region_chan", (u8 *) parsed_region_chan,
 		2 + COUNTRY_CODE_LEN + sizeof(struct parsed_region_chan_11d) * idx);
 
 done:
@@ -422,15 +422,15 @@ done:
 u8 libertas_get_scan_type_11d(u8 chan,
 			  struct parsed_region_chan_11d * parsed_region_chan)
 {
-	u8 scan_type = cmd_scan_type_passive;
+	u8 scan_type = CMD_SCAN_TYPE_PASSIVE;
 
 	lbs_deb_enter(LBS_DEB_11D);
 
 	if (wlan_channel_known_11d(chan, parsed_region_chan)) {
-		lbs_deb_11d("11D: Found and do Active Scan\n");
-		scan_type = cmd_scan_type_active;
+		lbs_deb_11d("found, do active scan\n");
+		scan_type = CMD_SCAN_TYPE_ACTIVE;
 	} else {
-		lbs_deb_11d("11D: Not Find and do Passive Scan\n");
+		lbs_deb_11d("not found, do passive scan\n");
 	}
 
 	lbs_deb_leave_args(LBS_DEB_11D, "ret scan_type %d", scan_type);
@@ -446,25 +446,6 @@ void libertas_init_11d(wlan_private * priv)
 	return;
 }
 
-static int wlan_enable_11d(wlan_private * priv, u8 flag)
-{
-	int ret;
-
-	priv->adapter->enable11d = flag;
-
-	/* send cmd to FW to enable/disable 11D function in FW */
-	ret = libertas_prepare_and_send_command(priv,
-				    cmd_802_11_snmp_mib,
-				    cmd_act_set,
-				    cmd_option_waitforrsp,
-				    OID_802_11D_ENABLE,
-				    &priv->adapter->enable11d);
-	if (ret)
-		lbs_deb_11d("11D: Fail to enable 11D \n");
-
-	return 0;
-}
-
 /**
  *  @brief This function sets DOMAIN INFO to FW
  *  @param priv       pointer to wlan_private
@@ -475,15 +456,15 @@ static int set_domain_info_11d(wlan_private * priv)
 	int ret;
 
 	if (!priv->adapter->enable11d) {
-		lbs_deb_11d("11D: dnld domain Info with 11d disabled\n");
+		lbs_deb_11d("dnld domain Info with 11d disabled\n");
 		return 0;
 	}
 
-	ret = libertas_prepare_and_send_command(priv, cmd_802_11d_domain_info,
-				    cmd_act_set,
-				    cmd_option_waitforrsp, 0, NULL);
+	ret = libertas_prepare_and_send_command(priv, CMD_802_11D_DOMAIN_INFO,
+				    CMD_ACT_SET,
+				    CMD_OPTION_WAITFORRSP, 0, NULL);
 	if (ret)
-		lbs_deb_11d("11D: Fail to dnld domain Info\n");
+		lbs_deb_11d("fail to dnld domain info\n");
 
 	return ret;
 }
@@ -505,7 +486,7 @@ int libertas_set_universaltable(wlan_private * priv, u8 band)
 
 	adapter->universal_channel[i].nrcfp =
 	    sizeof(channel_freq_power_UN_BG) / size;
-	lbs_deb_11d("11D: BG-band nrcfp=%d\n",
+	lbs_deb_11d("BG-band nrcfp %d\n",
 	       adapter->universal_channel[i].nrcfp);
 
 	adapter->universal_channel[i].CFP = channel_freq_power_UN_BG;
@@ -541,10 +522,10 @@ int libertas_cmd_802_11d_domain_info(wlan_private * priv,
 
 	cmd->command = cpu_to_le16(cmdno);
 	pdomaininfo->action = cpu_to_le16(cmdoption);
-	if (cmdoption == cmd_act_get) {
+	if (cmdoption == CMD_ACT_GET) {
 		cmd->size =
 		    cpu_to_le16(sizeof(pdomaininfo->action) + S_DS_GEN);
-		lbs_dbg_hex("11D: 802_11D_DOMAIN_INFO:", (u8 *) cmd,
+		lbs_deb_hex(LBS_DEB_11D, "802_11D_DOMAIN_INFO", (u8 *) cmd,
 			(int)(cmd->size));
 		goto done;
 	}
@@ -562,7 +543,7 @@ int libertas_cmd_802_11d_domain_info(wlan_private * priv,
 		       nr_subband * sizeof(struct ieeetypes_subbandset));
 
 		cmd->size = cpu_to_le16(sizeof(pdomaininfo->action) +
-					     domain->header.len +
+					     le16_to_cpu(domain->header.len) +
 					     sizeof(struct mrvlietypesheader) +
 					     S_DS_GEN);
 	} else {
@@ -570,7 +551,7 @@ int libertas_cmd_802_11d_domain_info(wlan_private * priv,
 		    cpu_to_le16(sizeof(pdomaininfo->action) + S_DS_GEN);
 	}
 
-	lbs_dbg_hex("11D:802_11D_DOMAIN_INFO:", (u8 *) cmd, le16_to_cpu(cmd->size));
+	lbs_deb_hex(LBS_DEB_11D, "802_11D_DOMAIN_INFO", (u8 *) cmd, le16_to_cpu(cmd->size));
 
 done:
 	lbs_deb_enter(LBS_DEB_11D);
@@ -578,31 +559,6 @@ done:
 }
 
 /**
- *  @brief This function implements private cmd: enable/disable 11D
- *  @param priv    pointer to wlan_private
- *  @param wrq     pointer to user data
- *  @return 	   0 or -1
- */
-int libertas_cmd_enable_11d(wlan_private * priv, struct iwreq *wrq)
-{
-	int data = 0;
-	int *val;
-
-	lbs_deb_enter(LBS_DEB_11D);
-	data = SUBCMD_DATA(wrq);
-
-	lbs_deb_11d("enable 11D: %s\n",
-	       (data == 1) ? "enable" : "Disable");
-
-	wlan_enable_11d(priv, data);
-	val = (int *)wrq->u.name;
-	*val = priv->adapter->enable11d;
-
-	lbs_deb_enter(LBS_DEB_11D);
-	return 0;
-}
-
-/**
  *  @brief This function parses countryinfo from AP and download country info to FW
  *  @param priv    pointer to wlan_private
  *  @param resp    pointer to command response buffer
@@ -619,13 +575,13 @@ int libertas_ret_802_11d_domain_info(wlan_private * priv,
 
 	lbs_deb_enter(LBS_DEB_11D);
 
-	lbs_dbg_hex("11D DOMAIN Info Rsp Data:", (u8 *) resp,
+	lbs_deb_hex(LBS_DEB_11D, "domain info resp", (u8 *) resp,
 		(int)le16_to_cpu(resp->size));
 
 	nr_subband = (le16_to_cpu(domain->header.len) - COUNTRY_CODE_LEN) /
 		      sizeof(struct ieeetypes_subbandset);
 
-	lbs_deb_11d("11D Domain Info Resp: nr_subband=%d\n", nr_subband);
+	lbs_deb_11d("domain info resp: nr_subband %d\n", nr_subband);
 
 	if (nr_subband > MRVDRV_MAX_SUBBAND_802_11D) {
 		lbs_deb_11d("Invalid Numrer of Subband returned!!\n");
@@ -633,10 +589,10 @@ int libertas_ret_802_11d_domain_info(wlan_private * priv,
 	}
 
 	switch (action) {
-	case cmd_act_set:	/*Proc Set action */
+	case CMD_ACT_SET:	/*Proc Set action */
 		break;
 
-	case cmd_act_get:
+	case CMD_ACT_GET:
 		break;
 	default:
 		lbs_deb_11d("Invalid action:%d\n", domaininfo->action);
@@ -667,7 +623,7 @@ int libertas_parse_dnld_countryinfo_11d(wlan_private * priv,
 					       &adapter->parsed_region_chan);
 
 		if (ret == -1) {
-			lbs_deb_11d("11D: Err Parse domain_info from AP..\n");
+			lbs_deb_11d("error parsing domain_info from AP\n");
 			goto done;
 		}
 
@@ -679,7 +635,7 @@ int libertas_parse_dnld_countryinfo_11d(wlan_private * priv,
 		ret = set_domain_info_11d(priv);
 
 		if (ret) {
-			lbs_deb_11d("11D: Err set domainInfo to FW\n");
+			lbs_deb_11d("error setting domain info\n");
 			goto done;
 		}
 	}
@@ -703,7 +659,7 @@ int libertas_create_dnld_countryinfo_11d(wlan_private * priv)
 	u8 j;
 
 	lbs_deb_enter(LBS_DEB_11D);
-	lbs_deb_11d("11D:curbssparams.band[%d]\n", adapter->curbssparams.band);
+	lbs_deb_11d("curbssparams.band %d\n", adapter->curbssparams.band);
 
 	if (priv->adapter->enable11d) {
 		/* update parsed_region_chan_11; dnld domaininf to FW */
@@ -712,7 +668,7 @@ int libertas_create_dnld_countryinfo_11d(wlan_private * priv)
 		     sizeof(adapter->region_channel[0]); j++) {
 			region_chan = &adapter->region_channel[j];
 
-			lbs_deb_11d("11D:[%d] region_chan->band[%d]\n", j,
+			lbs_deb_11d("%d region_chan->band %d\n", j,
 			       region_chan->band);
 
 			if (!region_chan || !region_chan->valid
@@ -725,7 +681,7 @@ int libertas_create_dnld_countryinfo_11d(wlan_private * priv)
 
 		if (j >= sizeof(adapter->region_channel) /
 		    sizeof(adapter->region_channel[0])) {
-			lbs_deb_11d("11D:region_chan not found. band[%d]\n",
+			lbs_deb_11d("region_chan not found, band %d\n",
 			       adapter->curbssparams.band);
 			ret = -1;
 			goto done;
@@ -745,7 +701,7 @@ int libertas_create_dnld_countryinfo_11d(wlan_private * priv)
 		ret = set_domain_info_11d(priv);
 
 		if (ret) {
-			lbs_deb_11d("11D: Err set domainInfo to FW\n");
+			lbs_deb_11d("error setting domain info\n");
 			goto done;
 		}
 
diff --git a/drivers/net/wireless/libertas/11d.h b/drivers/net/wireless/libertas/11d.h
index 73e42e712911..3a6d1f8db78f 100644
--- a/drivers/net/wireless/libertas/11d.h
+++ b/drivers/net/wireless/libertas/11d.h
@@ -83,8 +83,6 @@ u8 libertas_get_scan_type_11d(u8 chan,
 
 u32 libertas_chan_2_freq(u8 chan, u8 band);
 
-enum state_11d libertas_get_state_11d(wlan_private * priv);
-
 void libertas_init_11d(wlan_private * priv);
 
 int libertas_set_universaltable(wlan_private * priv, u8 band);
@@ -93,8 +91,6 @@ int libertas_cmd_802_11d_domain_info(wlan_private * priv,
 				 struct cmd_ds_command *cmd, u16 cmdno,
 				 u16 cmdOption);
 
-int libertas_cmd_enable_11d(wlan_private * priv, struct iwreq *wrq);
-
 int libertas_ret_802_11d_domain_info(wlan_private * priv,
 				 struct cmd_ds_command *resp);
 
diff --git a/drivers/net/wireless/libertas/Makefile b/drivers/net/wireless/libertas/Makefile
index 32ed4136b0d4..c469d569f090 100644
--- a/drivers/net/wireless/libertas/Makefile
+++ b/drivers/net/wireless/libertas/Makefile
@@ -1,12 +1,13 @@
-libertas-objs := main.o fw.o wext.o \
+libertas-objs := main.o wext.o \
 		rx.o tx.o cmd.o 	  \
 		cmdresp.o scan.o	  \
 		join.o 11d.o 		  \
 		debugfs.o	  \
 		ethtool.o assoc.o
 
-usb8xxx-objs += if_bootcmd.o
 usb8xxx-objs += if_usb.o
+libertas_cs-objs += if_cs.o
 
 obj-$(CONFIG_LIBERTAS)     += libertas.o
 obj-$(CONFIG_LIBERTAS_USB) += usb8xxx.o
+obj-$(CONFIG_LIBERTAS_CS)  += libertas_cs.o
diff --git a/drivers/net/wireless/libertas/assoc.c b/drivers/net/wireless/libertas/assoc.c
index afd5617dd92b..b61b176e9d07 100644
--- a/drivers/net/wireless/libertas/assoc.c
+++ b/drivers/net/wireless/libertas/assoc.c
@@ -16,6 +16,7 @@ static const u8 bssid_off[ETH_ALEN] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
 
 static void print_assoc_req(const char * extra, struct assoc_request * assoc_req)
 {
+	DECLARE_MAC_BUF(mac);
 	lbs_deb_assoc(
 	       "#### Association Request: %s\n"
 	       "       flags:      0x%08lX\n"
@@ -23,13 +24,13 @@ static void print_assoc_req(const char * extra, struct assoc_request * assoc_req
 	       "       channel:    %d\n"
 	       "       band:       %d\n"
 	       "       mode:       %d\n"
-	       "       BSSID:      " MAC_FMT "\n"
+	       "       BSSID:      %s\n"
 	       "       Encryption:%s%s%s\n"
 	       "       auth:       %d\n",
 	       extra, assoc_req->flags,
 	       escape_essid(assoc_req->ssid, assoc_req->ssid_len),
 	       assoc_req->channel, assoc_req->band, assoc_req->mode,
-	       MAC_ARG(assoc_req->bssid),
+	       print_mac(mac, assoc_req->bssid),
 	       assoc_req->secinfo.WPAenabled ? " WPA" : "",
 	       assoc_req->secinfo.WPA2enabled ? " WPA2" : "",
 	       assoc_req->secinfo.wep_enabled ? " WEP" : "",
@@ -57,10 +58,8 @@ static int assoc_helper_essid(wlan_private *priv,
 	lbs_deb_assoc("New SSID requested: '%s'\n",
 	              escape_essid(assoc_req->ssid, assoc_req->ssid_len));
 	if (assoc_req->mode == IW_MODE_INFRA) {
-		if (adapter->prescan) {
-			libertas_send_specific_ssid_scan(priv, assoc_req->ssid,
-				assoc_req->ssid_len, 0);
-		}
+		libertas_send_specific_ssid_scan(priv, assoc_req->ssid,
+			assoc_req->ssid_len, 0);
 
 		bss = libertas_find_ssid_in_list(adapter, assoc_req->ssid,
 				assoc_req->ssid_len, NULL, IW_MODE_INFRA, channel);
@@ -106,16 +105,17 @@ static int assoc_helper_bssid(wlan_private *priv,
 	wlan_adapter *adapter = priv->adapter;
 	int ret = 0;
 	struct bss_descriptor * bss;
+	DECLARE_MAC_BUF(mac);
 
-	lbs_deb_enter_args(LBS_DEB_ASSOC, "BSSID " MAC_FMT,
-		MAC_ARG(assoc_req->bssid));
+	lbs_deb_enter_args(LBS_DEB_ASSOC, "BSSID %s",
+		print_mac(mac, assoc_req->bssid));
 
 	/* Search for index position in list for requested MAC */
 	bss = libertas_find_bssid_in_list(adapter, assoc_req->bssid,
 			    assoc_req->mode);
 	if (bss == NULL) {
-		lbs_deb_assoc("ASSOC: WAP: BSSID " MAC_FMT " not found, "
-			"cannot associate.\n", MAC_ARG(assoc_req->bssid));
+		lbs_deb_assoc("ASSOC: WAP: BSSID %s not found, "
+			"cannot associate.\n", print_mac(mac, assoc_req->bssid));
 		goto out;
 	}
 
@@ -175,14 +175,14 @@ static int assoc_helper_mode(wlan_private *priv,
 
 	if (assoc_req->mode == IW_MODE_INFRA) {
 		if (adapter->psstate != PS_STATE_FULL_POWER)
-			libertas_ps_wakeup(priv, cmd_option_waitforrsp);
-		adapter->psmode = wlan802_11powermodecam;
+			libertas_ps_wakeup(priv, CMD_OPTION_WAITFORRSP);
+		adapter->psmode = WLAN802_11POWERMODECAM;
 	}
 
 	adapter->mode = assoc_req->mode;
 	ret = libertas_prepare_and_send_command(priv,
-				    cmd_802_11_snmp_mib,
-				    0, cmd_option_waitforrsp,
+				    CMD_802_11_SNMP_MIB,
+				    0, CMD_OPTION_WAITFORRSP,
 				    OID_802_11_INFRASTRUCTURE_MODE,
 		/* Shoot me now */  (void *) (size_t) assoc_req->mode);
 
@@ -195,9 +195,9 @@ done:
 static int update_channel(wlan_private * priv)
 {
 	/* the channel in f/w could be out of sync, get the current channel */
-	return libertas_prepare_and_send_command(priv, cmd_802_11_rf_channel,
-				    cmd_opt_802_11_rf_channel_get,
-				    cmd_option_waitforrsp, 0, NULL);
+	return libertas_prepare_and_send_command(priv, CMD_802_11_RF_CHANNEL,
+				    CMD_OPT_802_11_RF_CHANNEL_GET,
+				    CMD_OPTION_WAITFORRSP, 0, NULL);
 }
 
 void libertas_sync_channel(struct work_struct *work)
@@ -227,9 +227,9 @@ static int assoc_helper_channel(wlan_private *priv,
 	lbs_deb_assoc("ASSOC: channel: %d -> %d\n",
 	       adapter->curbssparams.channel, assoc_req->channel);
 
-	ret = libertas_prepare_and_send_command(priv, cmd_802_11_rf_channel,
-				cmd_opt_802_11_rf_channel_set,
-				cmd_option_waitforrsp, 0, &assoc_req->channel);
+	ret = libertas_prepare_and_send_command(priv, CMD_802_11_RF_CHANNEL,
+				CMD_OPT_802_11_RF_CHANNEL_SET,
+				CMD_OPTION_WAITFORRSP, 0, &assoc_req->channel);
 	if (ret < 0) {
 		lbs_deb_assoc("ASSOC: channel: error setting channel.");
 	}
@@ -278,15 +278,15 @@ static int assoc_helper_wep_keys(wlan_private *priv,
 	    || assoc_req->wep_keys[2].len
 	    || assoc_req->wep_keys[3].len) {
 		ret = libertas_prepare_and_send_command(priv,
-					    cmd_802_11_set_wep,
-					    cmd_act_add,
-					    cmd_option_waitforrsp,
+					    CMD_802_11_SET_WEP,
+					    CMD_ACT_ADD,
+					    CMD_OPTION_WAITFORRSP,
 					    0, assoc_req);
 	} else {
 		ret = libertas_prepare_and_send_command(priv,
-					    cmd_802_11_set_wep,
-					    cmd_act_remove,
-					    cmd_option_waitforrsp,
+					    CMD_802_11_SET_WEP,
+					    CMD_ACT_REMOVE,
+					    CMD_OPTION_WAITFORRSP,
 					    0, NULL);
 	}
 
@@ -295,9 +295,9 @@ static int assoc_helper_wep_keys(wlan_private *priv,
 
 	/* enable/disable the MAC's WEP packet filter */
 	if (assoc_req->secinfo.wep_enabled)
-		adapter->currentpacketfilter |= cmd_act_mac_wep_enable;
+		adapter->currentpacketfilter |= CMD_ACT_MAC_WEP_ENABLE;
 	else
-		adapter->currentpacketfilter &= ~cmd_act_mac_wep_enable;
+		adapter->currentpacketfilter &= ~CMD_ACT_MAC_WEP_ENABLE;
 	ret = libertas_set_mac_packet_filter(priv);
 	if (ret)
 		goto out;
@@ -307,7 +307,7 @@ static int assoc_helper_wep_keys(wlan_private *priv,
 	/* Copy WEP keys into adapter wep key fields */
 	for (i = 0; i < 4; i++) {
 		memcpy(&adapter->wep_keys[i], &assoc_req->wep_keys[i],
-			sizeof(struct WLAN_802_11_KEY));
+			sizeof(struct enc_key));
 	}
 	adapter->wep_tx_keyidx = assoc_req->wep_tx_keyidx;
 
@@ -342,9 +342,9 @@ static int assoc_helper_secinfo(wlan_private *priv,
 
 	/* Get RSN enabled/disabled */
 	ret = libertas_prepare_and_send_command(priv,
-				    cmd_802_11_enable_rsn,
-				    cmd_act_set,
-				    cmd_option_waitforrsp,
+				    CMD_802_11_ENABLE_RSN,
+				    CMD_ACT_GET,
+				    CMD_OPTION_WAITFORRSP,
 				    0, &rsn);
 	if (ret) {
 		lbs_deb_assoc("Failed to get RSN status: %d", ret);
@@ -359,9 +359,9 @@ static int assoc_helper_secinfo(wlan_private *priv,
 	/* Set RSN enabled/disabled */
 	rsn = do_wpa;
 	ret = libertas_prepare_and_send_command(priv,
-				    cmd_802_11_enable_rsn,
-				    cmd_act_set,
-				    cmd_option_waitforrsp,
+				    CMD_802_11_ENABLE_RSN,
+				    CMD_ACT_SET,
+				    CMD_OPTION_WAITFORRSP,
 				    0, &rsn);
 
 out:
@@ -374,15 +374,40 @@ static int assoc_helper_wpa_keys(wlan_private *priv,
                                  struct assoc_request * assoc_req)
 {
 	int ret = 0;
+	unsigned int flags = assoc_req->flags;
 
 	lbs_deb_enter(LBS_DEB_ASSOC);
 
-	ret = libertas_prepare_and_send_command(priv,
-				    cmd_802_11_key_material,
-				    cmd_act_set,
-				    cmd_option_waitforrsp,
-				    0, assoc_req);
+	/* Work around older firmware bug where WPA unicast and multicast
+	 * keys must be set independently.  Seen in SDIO parts with firmware
+	 * version 5.0.11p0.
+	 */
+
+	if (test_bit(ASSOC_FLAG_WPA_UCAST_KEY, &assoc_req->flags)) {
+		clear_bit(ASSOC_FLAG_WPA_MCAST_KEY, &assoc_req->flags);
+		ret = libertas_prepare_and_send_command(priv,
+					CMD_802_11_KEY_MATERIAL,
+					CMD_ACT_SET,
+					CMD_OPTION_WAITFORRSP,
+					0, assoc_req);
+		assoc_req->flags = flags;
+	}
+
+	if (ret)
+		goto out;
 
+	if (test_bit(ASSOC_FLAG_WPA_MCAST_KEY, &assoc_req->flags)) {
+		clear_bit(ASSOC_FLAG_WPA_UCAST_KEY, &assoc_req->flags);
+
+		ret = libertas_prepare_and_send_command(priv,
+					CMD_802_11_KEY_MATERIAL,
+					CMD_ACT_SET,
+					CMD_OPTION_WAITFORRSP,
+					0, assoc_req);
+		assoc_req->flags = flags;
+	}
+
+out:
 	lbs_deb_leave_args(LBS_DEB_ASSOC, "ret %d", ret);
 	return ret;
 }
@@ -412,7 +437,7 @@ static int assoc_helper_wpa_ie(wlan_private *priv,
 static int should_deauth_infrastructure(wlan_adapter *adapter,
                                         struct assoc_request * assoc_req)
 {
-	if (adapter->connect_status != libertas_connected)
+	if (adapter->connect_status != LIBERTAS_CONNECTED)
 		return 0;
 
 	if (test_bit(ASSOC_FLAG_SSID, &assoc_req->flags)) {
@@ -453,7 +478,7 @@ static int should_deauth_infrastructure(wlan_adapter *adapter,
 static int should_stop_adhoc(wlan_adapter *adapter,
                              struct assoc_request * assoc_req)
 {
-	if (adapter->connect_status != libertas_connected)
+	if (adapter->connect_status != LIBERTAS_CONNECTED)
 		return 0;
 
 	if (libertas_ssid_cmp(adapter->curbssparams.ssid,
@@ -483,6 +508,7 @@ void libertas_association_worker(struct work_struct *work)
 	struct assoc_request * assoc_req = NULL;
 	int ret = 0;
 	int find_any_ssid = 0;
+	DECLARE_MAC_BUF(mac);
 
 	lbs_deb_enter(LBS_DEB_ASSOC);
 
@@ -556,7 +582,8 @@ void libertas_association_worker(struct work_struct *work)
 	if (test_bit(ASSOC_FLAG_MODE, &assoc_req->flags)) {
 		ret = assoc_helper_mode(priv, assoc_req);
 		if (ret) {
-lbs_deb_assoc("ASSOC(:%d) mode: ret = %d\n", __LINE__, ret);
+			lbs_deb_assoc("ASSOC(:%d) mode: ret = %d\n",
+			              __LINE__, ret);
 			goto out;
 		}
 	}
@@ -574,7 +601,8 @@ lbs_deb_assoc("ASSOC(:%d) mode: ret = %d\n", __LINE__, ret);
 	    || test_bit(ASSOC_FLAG_WEP_TX_KEYIDX, &assoc_req->flags)) {
 		ret = assoc_helper_wep_keys(priv, assoc_req);
 		if (ret) {
-lbs_deb_assoc("ASSOC(:%d) wep_keys: ret = %d\n", __LINE__, ret);
+			lbs_deb_assoc("ASSOC(:%d) wep_keys: ret = %d\n",
+			              __LINE__, ret);
 			goto out;
 		}
 	}
@@ -582,7 +610,8 @@ lbs_deb_assoc("ASSOC(:%d) wep_keys: ret = %d\n", __LINE__, ret);
 	if (test_bit(ASSOC_FLAG_SECINFO, &assoc_req->flags)) {
 		ret = assoc_helper_secinfo(priv, assoc_req);
 		if (ret) {
-lbs_deb_assoc("ASSOC(:%d) secinfo: ret = %d\n", __LINE__, ret);
+			lbs_deb_assoc("ASSOC(:%d) secinfo: ret = %d\n",
+			              __LINE__, ret);
 			goto out;
 		}
 	}
@@ -590,7 +619,8 @@ lbs_deb_assoc("ASSOC(:%d) secinfo: ret = %d\n", __LINE__, ret);
 	if (test_bit(ASSOC_FLAG_WPA_IE, &assoc_req->flags)) {
 		ret = assoc_helper_wpa_ie(priv, assoc_req);
 		if (ret) {
-lbs_deb_assoc("ASSOC(:%d) wpa_ie: ret = %d\n", __LINE__, ret);
+			lbs_deb_assoc("ASSOC(:%d) wpa_ie: ret = %d\n",
+			              __LINE__, ret);
 			goto out;
 		}
 	}
@@ -599,7 +629,8 @@ lbs_deb_assoc("ASSOC(:%d) wpa_ie: ret = %d\n", __LINE__, ret);
 	    || test_bit(ASSOC_FLAG_WPA_UCAST_KEY, &assoc_req->flags)) {
 		ret = assoc_helper_wpa_keys(priv, assoc_req);
 		if (ret) {
-lbs_deb_assoc("ASSOC(:%d) wpa_keys: ret = %d\n", __LINE__, ret);
+			lbs_deb_assoc("ASSOC(:%d) wpa_keys: ret = %d\n",
+			              __LINE__, ret);
 			goto out;
 		}
 	}
@@ -618,25 +649,25 @@ lbs_deb_assoc("ASSOC(:%d) wpa_keys: ret = %d\n", __LINE__, ret);
 			success = 0;
 		}
 
-		if (adapter->connect_status != libertas_connected) {
-			lbs_deb_assoc("ASSOC: assoication attempt unsuccessful, "
+		if (adapter->connect_status != LIBERTAS_CONNECTED) {
+			lbs_deb_assoc("ASSOC: association attempt unsuccessful, "
 				"not connected.\n");
 			success = 0;
 		}
 
 		if (success) {
 			lbs_deb_assoc("ASSOC: association attempt successful. "
-				"Associated to '%s' (" MAC_FMT ")\n",
+				"Associated to '%s' (%s)\n",
 				escape_essid(adapter->curbssparams.ssid,
 				             adapter->curbssparams.ssid_len),
-				MAC_ARG(adapter->curbssparams.bssid));
+				print_mac(mac, adapter->curbssparams.bssid));
 			libertas_prepare_and_send_command(priv,
-				cmd_802_11_rssi,
-				0, cmd_option_waitforrsp, 0, NULL);
+				CMD_802_11_RSSI,
+				0, CMD_OPTION_WAITFORRSP, 0, NULL);
 
 			libertas_prepare_and_send_command(priv,
-				cmd_802_11_get_log,
-				0, cmd_option_waitforrsp, 0, NULL);
+				CMD_802_11_GET_LOG,
+				0, CMD_OPTION_WAITFORRSP, 0, NULL);
 		} else {
 			ret = -1;
 		}
@@ -703,7 +734,7 @@ struct assoc_request * wlan_get_association_request(wlan_adapter *adapter)
 		int i;
 		for (i = 0; i < 4; i++) {
 			memcpy(&assoc_req->wep_keys[i], &adapter->wep_keys[i],
-				sizeof(struct WLAN_802_11_KEY));
+				sizeof(struct enc_key));
 		}
 	}
 
@@ -712,12 +743,12 @@ struct assoc_request * wlan_get_association_request(wlan_adapter *adapter)
 
 	if (!test_bit(ASSOC_FLAG_WPA_MCAST_KEY, &assoc_req->flags)) {
 		memcpy(&assoc_req->wpa_mcast_key, &adapter->wpa_mcast_key,
-			sizeof(struct WLAN_802_11_KEY));
+			sizeof(struct enc_key));
 	}
 
 	if (!test_bit(ASSOC_FLAG_WPA_UCAST_KEY, &assoc_req->flags)) {
 		memcpy(&assoc_req->wpa_unicast_key, &adapter->wpa_unicast_key,
-			sizeof(struct WLAN_802_11_KEY));
+			sizeof(struct enc_key));
 	}
 
 	if (!test_bit(ASSOC_FLAG_SECINFO, &assoc_req->flags)) {
diff --git a/drivers/net/wireless/libertas/assoc.h b/drivers/net/wireless/libertas/assoc.h
index 5e9c31f0932b..e09b7490abbd 100644
--- a/drivers/net/wireless/libertas/assoc.h
+++ b/drivers/net/wireless/libertas/assoc.h
@@ -17,7 +17,7 @@ static inline void wlan_postpone_association_work(wlan_private *priv)
 	if (priv->adapter->surpriseremoved)
 		return;
 	cancel_delayed_work(&priv->assoc_work);
-	queue_delayed_work(priv->assoc_thread, &priv->assoc_work, ASSOC_DELAY);
+	queue_delayed_work(priv->work_thread, &priv->assoc_work, ASSOC_DELAY);
 }
 
 static inline void wlan_cancel_association_work(wlan_private *priv)
diff --git a/drivers/net/wireless/libertas/cmd.c b/drivers/net/wireless/libertas/cmd.c
index 4a8f5dc70239..1cbbd96fdbde 100644
--- a/drivers/net/wireless/libertas/cmd.c
+++ b/drivers/net/wireless/libertas/cmd.c
@@ -15,7 +15,7 @@
 static void cleanup_cmdnode(struct cmd_ctrl_node *ptempnode);
 
 static u16 commands_allowed_in_ps[] = {
-	cmd_802_11_rssi,
+	CMD_802_11_RSSI,
 };
 
 /**
@@ -43,7 +43,7 @@ static int wlan_cmd_hw_spec(wlan_private * priv, struct cmd_ds_command *cmd)
 
 	lbs_deb_enter(LBS_DEB_CMD);
 
-	cmd->command = cpu_to_le16(cmd_get_hw_spec);
+	cmd->command = cpu_to_le16(CMD_GET_HW_SPEC);
 	cmd->size = cpu_to_le16(sizeof(struct cmd_ds_get_hw_spec) + S_DS_GEN);
 	memcpy(hwspec->permanentaddr, priv->adapter->current_addr, ETH_ALEN);
 
@@ -56,34 +56,29 @@ static int wlan_cmd_802_11_ps_mode(wlan_private * priv,
 				   u16 cmd_action)
 {
 	struct cmd_ds_802_11_ps_mode *psm = &cmd->params.psmode;
-	wlan_adapter *adapter = priv->adapter;
 
 	lbs_deb_enter(LBS_DEB_CMD);
 
-	cmd->command = cpu_to_le16(cmd_802_11_ps_mode);
+	cmd->command = cpu_to_le16(CMD_802_11_PS_MODE);
 	cmd->size = cpu_to_le16(sizeof(struct cmd_ds_802_11_ps_mode) +
 				S_DS_GEN);
 	psm->action = cpu_to_le16(cmd_action);
 	psm->multipledtim = 0;
 	switch (cmd_action) {
-	case cmd_subcmd_enter_ps:
+	case CMD_SUBCMD_ENTER_PS:
 		lbs_deb_cmd("PS command:" "SubCode- Enter PS\n");
-		lbs_deb_cmd("locallisteninterval = %d\n",
-		       adapter->locallisteninterval);
 
-		psm->locallisteninterval =
-		    cpu_to_le16(adapter->locallisteninterval);
-		psm->nullpktinterval =
-		    cpu_to_le16(adapter->nullpktinterval);
+		psm->locallisteninterval = 0;
+		psm->nullpktinterval = 0;
 		psm->multipledtim =
-		    cpu_to_le16(priv->adapter->multipledtim);
+		    cpu_to_le16(MRVDRV_DEFAULT_MULTIPLE_DTIM);
 		break;
 
-	case cmd_subcmd_exit_ps:
+	case CMD_SUBCMD_EXIT_PS:
 		lbs_deb_cmd("PS command:" "SubCode- Exit PS\n");
 		break;
 
-	case cmd_subcmd_sleep_confirmed:
+	case CMD_SUBCMD_SLEEP_CONFIRMED:
 		lbs_deb_cmd("PS command: SubCode- sleep confirm\n");
 		break;
 
@@ -101,7 +96,9 @@ static int wlan_cmd_802_11_inactivity_timeout(wlan_private * priv,
 {
 	u16 *timeout = pdata_buf;
 
-	cmd->command = cpu_to_le16(cmd_802_11_inactivity_timeout);
+	lbs_deb_enter(LBS_DEB_CMD);
+
+	cmd->command = cpu_to_le16(CMD_802_11_INACTIVITY_TIMEOUT);
 	cmd->size =
 	    cpu_to_le16(sizeof(struct cmd_ds_802_11_inactivity_timeout)
 			     + S_DS_GEN);
@@ -113,6 +110,7 @@ static int wlan_cmd_802_11_inactivity_timeout(wlan_private * priv,
 	else
 		cmd->params.inactivity_timeout.timeout = 0;
 
+	lbs_deb_leave(LBS_DEB_CMD);
 	return 0;
 }
 
@@ -127,13 +125,13 @@ static int wlan_cmd_802_11_sleep_params(wlan_private * priv,
 
 	cmd->size = cpu_to_le16((sizeof(struct cmd_ds_802_11_sleep_params)) +
 				S_DS_GEN);
-	cmd->command = cpu_to_le16(cmd_802_11_sleep_params);
+	cmd->command = cpu_to_le16(CMD_802_11_SLEEP_PARAMS);
 
-	if (cmd_action == cmd_act_get) {
+	if (cmd_action == CMD_ACT_GET) {
 		memset(&adapter->sp, 0, sizeof(struct sleep_params));
 		memset(sp, 0, sizeof(struct cmd_ds_802_11_sleep_params));
 		sp->action = cpu_to_le16(cmd_action);
-	} else if (cmd_action == cmd_act_set) {
+	} else if (cmd_action == CMD_ACT_SET) {
 		sp->action = cpu_to_le16(cmd_action);
 		sp->error = cpu_to_le16(adapter->sp.sp_error);
 		sp->offset = cpu_to_le16(adapter->sp.sp_offset);
@@ -159,10 +157,10 @@ static int wlan_cmd_802_11_set_wep(wlan_private * priv,
 
 	lbs_deb_enter(LBS_DEB_CMD);
 
-	cmd->command = cpu_to_le16(cmd_802_11_set_wep);
+	cmd->command = cpu_to_le16(CMD_802_11_SET_WEP);
 	cmd->size = cpu_to_le16(sizeof(*wep) + S_DS_GEN);
 
-	if (cmd_act == cmd_act_add) {
+	if (cmd_act == CMD_ACT_ADD) {
 		int i;
 
 		if (!assoc_req) {
@@ -171,48 +169,47 @@ static int wlan_cmd_802_11_set_wep(wlan_private * priv,
 			goto done;
 		}
 
-		wep->action = cpu_to_le16(cmd_act_add);
+		wep->action = cpu_to_le16(CMD_ACT_ADD);
 
 		/* default tx key index */
 		wep->keyindex = cpu_to_le16((u16)(assoc_req->wep_tx_keyidx &
-						  (u32)cmd_WEP_KEY_INDEX_MASK));
-
-		lbs_deb_cmd("Tx key Index: %u\n", le16_to_cpu(wep->keyindex));
+						  (u32)CMD_WEP_KEY_INDEX_MASK));
 
 		/* Copy key types and material to host command structure */
 		for (i = 0; i < 4; i++) {
-			struct WLAN_802_11_KEY * pkey = &assoc_req->wep_keys[i];
+			struct enc_key * pkey = &assoc_req->wep_keys[i];
 
 			switch (pkey->len) {
 			case KEY_LEN_WEP_40:
-				wep->keytype[i] =
-					cpu_to_le16(cmd_type_wep_40_bit);
+				wep->keytype[i] = CMD_TYPE_WEP_40_BIT;
 				memmove(&wep->keymaterial[i], pkey->key,
 				        pkey->len);
+				lbs_deb_cmd("SET_WEP: add key %d (40 bit)\n", i);
 				break;
 			case KEY_LEN_WEP_104:
-				wep->keytype[i] =
-					cpu_to_le16(cmd_type_wep_104_bit);
+				wep->keytype[i] = CMD_TYPE_WEP_104_BIT;
 				memmove(&wep->keymaterial[i], pkey->key,
 				        pkey->len);
+				lbs_deb_cmd("SET_WEP: add key %d (104 bit)\n", i);
 				break;
 			case 0:
 				break;
 			default:
-				lbs_deb_cmd("Invalid WEP key %d length of %d\n",
+				lbs_deb_cmd("SET_WEP: invalid key %d, length %d\n",
 				       i, pkey->len);
 				ret = -1;
 				goto done;
 				break;
 			}
 		}
-	} else if (cmd_act == cmd_act_remove) {
+	} else if (cmd_act == CMD_ACT_REMOVE) {
 		/* ACT_REMOVE clears _all_ WEP keys */
-		wep->action = cpu_to_le16(cmd_act_remove);
+		wep->action = cpu_to_le16(CMD_ACT_REMOVE);
 
 		/* default tx key index */
 		wep->keyindex = cpu_to_le16((u16)(adapter->wep_tx_keyidx &
-						  (u32)cmd_WEP_KEY_INDEX_MASK));
+						  (u32)CMD_WEP_KEY_INDEX_MASK));
+		lbs_deb_cmd("SET_WEP: remove key %d\n", adapter->wep_tx_keyidx);
 	}
 
 	ret = 0;
@@ -232,15 +229,16 @@ static int wlan_cmd_802_11_enable_rsn(wlan_private * priv,
 
 	lbs_deb_enter(LBS_DEB_CMD);
 
-	cmd->command = cpu_to_le16(cmd_802_11_enable_rsn);
+	cmd->command = cpu_to_le16(CMD_802_11_ENABLE_RSN);
 	cmd->size = cpu_to_le16(sizeof(*penableRSN) + S_DS_GEN);
 	penableRSN->action = cpu_to_le16(cmd_action);
 
-	if (cmd_action == cmd_act_set) {
+	if (cmd_action == CMD_ACT_SET) {
 		if (*enable)
-			penableRSN->enable = cpu_to_le16(cmd_enable_rsn);
+			penableRSN->enable = cpu_to_le16(CMD_ENABLE_RSN);
 		else
-			penableRSN->enable = cpu_to_le16(cmd_disable_rsn);
+			penableRSN->enable = cpu_to_le16(CMD_DISABLE_RSN);
+		lbs_deb_cmd("ENABLE_RSN: %d\n", *enable);
 	}
 
 	lbs_deb_leave(LBS_DEB_CMD);
@@ -249,9 +247,9 @@ static int wlan_cmd_802_11_enable_rsn(wlan_private * priv,
 
 
 static void set_one_wpa_key(struct MrvlIEtype_keyParamSet * pkeyparamset,
-                            struct WLAN_802_11_KEY * pkey)
+                            struct enc_key * pkey)
 {
-	pkeyparamset->keytypeid = cpu_to_le16(pkey->type);
+	lbs_deb_enter(LBS_DEB_CMD);
 
 	if (pkey->flags & KEY_INFO_WPA_ENABLED) {
 		pkeyparamset->keyinfo |= cpu_to_le16(KEY_INFO_WPA_ENABLED);
@@ -264,12 +262,14 @@ static void set_one_wpa_key(struct MrvlIEtype_keyParamSet * pkeyparamset,
 	}
 
 	pkeyparamset->type = cpu_to_le16(TLV_TYPE_KEY_MATERIAL);
+	pkeyparamset->keytypeid = cpu_to_le16(pkey->type);
 	pkeyparamset->keylen = cpu_to_le16(pkey->len);
 	memcpy(pkeyparamset->key, pkey->key, pkey->len);
 	pkeyparamset->length = cpu_to_le16(  sizeof(pkeyparamset->keytypeid)
 	                                        + sizeof(pkeyparamset->keyinfo)
 	                                        + sizeof(pkeyparamset->keylen)
 	                                        + sizeof(pkeyparamset->key));
+	lbs_deb_leave(LBS_DEB_CMD);
 }
 
 static int wlan_cmd_802_11_key_material(wlan_private * priv,
@@ -285,10 +285,10 @@ static int wlan_cmd_802_11_key_material(wlan_private * priv,
 
 	lbs_deb_enter(LBS_DEB_CMD);
 
-	cmd->command = cpu_to_le16(cmd_802_11_key_material);
+	cmd->command = cpu_to_le16(CMD_802_11_KEY_MATERIAL);
 	pkeymaterial->action = cpu_to_le16(cmd_action);
 
-	if (cmd_action == cmd_act_get) {
+	if (cmd_action == CMD_ACT_GET) {
 		cmd->size = cpu_to_le16(S_DS_GEN + sizeof (pkeymaterial->action));
 		ret = 0;
 		goto done;
@@ -324,30 +324,37 @@ static int wlan_cmd_802_11_reset(wlan_private * priv,
 {
 	struct cmd_ds_802_11_reset *reset = &cmd->params.reset;
 
-	cmd->command = cpu_to_le16(cmd_802_11_reset);
+	lbs_deb_enter(LBS_DEB_CMD);
+
+	cmd->command = cpu_to_le16(CMD_802_11_RESET);
 	cmd->size = cpu_to_le16(sizeof(struct cmd_ds_802_11_reset) + S_DS_GEN);
 	reset->action = cpu_to_le16(cmd_action);
 
+	lbs_deb_leave(LBS_DEB_CMD);
 	return 0;
 }
 
 static int wlan_cmd_802_11_get_log(wlan_private * priv,
 				   struct cmd_ds_command *cmd)
 {
-	cmd->command = cpu_to_le16(cmd_802_11_get_log);
+	lbs_deb_enter(LBS_DEB_CMD);
+	cmd->command = cpu_to_le16(CMD_802_11_GET_LOG);
 	cmd->size =
 		cpu_to_le16(sizeof(struct cmd_ds_802_11_get_log) + S_DS_GEN);
 
+	lbs_deb_leave(LBS_DEB_CMD);
 	return 0;
 }
 
 static int wlan_cmd_802_11_get_stat(wlan_private * priv,
 				    struct cmd_ds_command *cmd)
 {
-	cmd->command = cpu_to_le16(cmd_802_11_get_stat);
+	lbs_deb_enter(LBS_DEB_CMD);
+	cmd->command = cpu_to_le16(CMD_802_11_GET_STAT);
 	cmd->size =
 	    cpu_to_le16(sizeof(struct cmd_ds_802_11_get_stat) + S_DS_GEN);
 
+	lbs_deb_leave(LBS_DEB_CMD);
 	return 0;
 }
 
@@ -364,15 +371,15 @@ static int wlan_cmd_802_11_snmp_mib(wlan_private * priv,
 
 	lbs_deb_cmd("SNMP_CMD: cmd_oid = 0x%x\n", cmd_oid);
 
-	cmd->command = cpu_to_le16(cmd_802_11_snmp_mib);
+	cmd->command = cpu_to_le16(CMD_802_11_SNMP_MIB);
 	cmd->size = cpu_to_le16(sizeof(*pSNMPMIB) + S_DS_GEN);
 
 	switch (cmd_oid) {
 	case OID_802_11_INFRASTRUCTURE_MODE:
 	{
 		u8 mode = (u8) (size_t) pdata_buf;
-		pSNMPMIB->querytype = cpu_to_le16(cmd_act_set);
-		pSNMPMIB->oid = cpu_to_le16((u16) desired_bsstype_i);
+		pSNMPMIB->querytype = cpu_to_le16(CMD_ACT_SET);
+		pSNMPMIB->oid = cpu_to_le16((u16) DESIRED_BSSTYPE_I);
 		pSNMPMIB->bufsize = sizeof(u8);
 		if (mode == IW_MODE_ADHOC) {
 			ucTemp = SNMP_MIB_VALUE_ADHOC;
@@ -390,10 +397,10 @@ static int wlan_cmd_802_11_snmp_mib(wlan_private * priv,
 		{
 			u32 ulTemp;
 
-			pSNMPMIB->oid = cpu_to_le16((u16) dot11d_i);
+			pSNMPMIB->oid = cpu_to_le16((u16) DOT11D_I);
 
-			if (cmd_action == cmd_act_set) {
-				pSNMPMIB->querytype = cmd_act_set;
+			if (cmd_action == CMD_ACT_SET) {
+				pSNMPMIB->querytype = CMD_ACT_SET;
 				pSNMPMIB->bufsize = sizeof(u16);
 				ulTemp = *(u32 *)pdata_buf;
 				*((__le16 *)(pSNMPMIB->value)) =
@@ -406,12 +413,12 @@ static int wlan_cmd_802_11_snmp_mib(wlan_private * priv,
 		{
 			u32 ulTemp;
 
-			pSNMPMIB->oid = cpu_to_le16((u16) fragthresh_i);
+			pSNMPMIB->oid = cpu_to_le16((u16) FRAGTHRESH_I);
 
-			if (cmd_action == cmd_act_get) {
-				pSNMPMIB->querytype = cpu_to_le16(cmd_act_get);
-			} else if (cmd_action == cmd_act_set) {
-				pSNMPMIB->querytype = cpu_to_le16(cmd_act_set);
+			if (cmd_action == CMD_ACT_GET) {
+				pSNMPMIB->querytype = cpu_to_le16(CMD_ACT_GET);
+			} else if (cmd_action == CMD_ACT_SET) {
+				pSNMPMIB->querytype = cpu_to_le16(CMD_ACT_SET);
 				pSNMPMIB->bufsize = cpu_to_le16(sizeof(u16));
 				ulTemp = *((u32 *) pdata_buf);
 				*((__le16 *)(pSNMPMIB->value)) =
@@ -426,12 +433,12 @@ static int wlan_cmd_802_11_snmp_mib(wlan_private * priv,
 		{
 
 			u32 ulTemp;
-			pSNMPMIB->oid = le16_to_cpu((u16) rtsthresh_i);
+			pSNMPMIB->oid = le16_to_cpu((u16) RTSTHRESH_I);
 
-			if (cmd_action == cmd_act_get) {
-				pSNMPMIB->querytype = cpu_to_le16(cmd_act_get);
-			} else if (cmd_action == cmd_act_set) {
-				pSNMPMIB->querytype = cpu_to_le16(cmd_act_set);
+			if (cmd_action == CMD_ACT_GET) {
+				pSNMPMIB->querytype = cpu_to_le16(CMD_ACT_GET);
+			} else if (cmd_action == CMD_ACT_SET) {
+				pSNMPMIB->querytype = cpu_to_le16(CMD_ACT_SET);
 				pSNMPMIB->bufsize = cpu_to_le16(sizeof(u16));
 				ulTemp = *((u32 *)pdata_buf);
 				*(__le16 *)(pSNMPMIB->value) =
@@ -441,12 +448,12 @@ static int wlan_cmd_802_11_snmp_mib(wlan_private * priv,
 			break;
 		}
 	case OID_802_11_TX_RETRYCOUNT:
-		pSNMPMIB->oid = cpu_to_le16((u16) short_retrylim_i);
+		pSNMPMIB->oid = cpu_to_le16((u16) SHORT_RETRYLIM_I);
 
-		if (cmd_action == cmd_act_get) {
-			pSNMPMIB->querytype = cpu_to_le16(cmd_act_get);
-		} else if (cmd_action == cmd_act_set) {
-			pSNMPMIB->querytype = cpu_to_le16(cmd_act_set);
+		if (cmd_action == CMD_ACT_GET) {
+			pSNMPMIB->querytype = cpu_to_le16(CMD_ACT_GET);
+		} else if (cmd_action == CMD_ACT_SET) {
+			pSNMPMIB->querytype = cpu_to_le16(CMD_ACT_SET);
 			pSNMPMIB->bufsize = cpu_to_le16(sizeof(u16));
 			*((__le16 *)(pSNMPMIB->value)) =
 			    cpu_to_le16((u16) adapter->txretrycount);
@@ -463,7 +470,7 @@ static int wlan_cmd_802_11_snmp_mib(wlan_private * priv,
 	       le16_to_cpu(cmd->seqnum), le16_to_cpu(cmd->result));
 
 	lbs_deb_cmd(
-	       "SNMP_CMD: action=0x%x, oid=0x%x, oidsize=0x%x, value=0x%x\n",
+	       "SNMP_CMD: action 0x%x, oid 0x%x, oidsize 0x%x, value 0x%x\n",
 	       le16_to_cpu(pSNMPMIB->querytype), le16_to_cpu(pSNMPMIB->oid),
 	       le16_to_cpu(pSNMPMIB->bufsize),
 	       le16_to_cpu(*(__le16 *) pSNMPMIB->value));
@@ -484,20 +491,20 @@ static int wlan_cmd_802_11_radio_control(wlan_private * priv,
 	cmd->size =
 	    cpu_to_le16((sizeof(struct cmd_ds_802_11_radio_control)) +
 			     S_DS_GEN);
-	cmd->command = cpu_to_le16(cmd_802_11_radio_control);
+	cmd->command = cpu_to_le16(CMD_802_11_RADIO_CONTROL);
 
 	pradiocontrol->action = cpu_to_le16(cmd_action);
 
 	switch (adapter->preamble) {
-	case cmd_type_short_preamble:
+	case CMD_TYPE_SHORT_PREAMBLE:
 		pradiocontrol->control = cpu_to_le16(SET_SHORT_PREAMBLE);
 		break;
 
-	case cmd_type_long_preamble:
+	case CMD_TYPE_LONG_PREAMBLE:
 		pradiocontrol->control = cpu_to_le16(SET_LONG_PREAMBLE);
 		break;
 
-	case cmd_type_auto_preamble:
+	case CMD_TYPE_AUTO_PREAMBLE:
 	default:
 		pradiocontrol->control = cpu_to_le16(SET_AUTO_PREAMBLE);
 		break;
@@ -523,7 +530,7 @@ static int wlan_cmd_802_11_rf_tx_power(wlan_private * priv,
 
 	cmd->size =
 	    cpu_to_le16((sizeof(struct cmd_ds_802_11_rf_tx_power)) + S_DS_GEN);
-	cmd->command = cpu_to_le16(cmd_802_11_rf_tx_power);
+	cmd->command = cpu_to_le16(CMD_802_11_RF_TX_POWER);
 	prtp->action = cpu_to_le16(cmd_action);
 
 	lbs_deb_cmd("RF_TX_POWER_CMD: size:%d cmd:0x%x Act:%d\n",
@@ -531,23 +538,23 @@ static int wlan_cmd_802_11_rf_tx_power(wlan_private * priv,
 		    le16_to_cpu(prtp->action));
 
 	switch (cmd_action) {
-	case cmd_act_tx_power_opt_get:
-		prtp->action = cpu_to_le16(cmd_act_get);
+	case CMD_ACT_TX_POWER_OPT_GET:
+		prtp->action = cpu_to_le16(CMD_ACT_GET);
 		prtp->currentlevel = 0;
 		break;
 
-	case cmd_act_tx_power_opt_set_high:
-		prtp->action = cpu_to_le16(cmd_act_set);
-		prtp->currentlevel = cpu_to_le16(cmd_act_tx_power_index_high);
+	case CMD_ACT_TX_POWER_OPT_SET_HIGH:
+		prtp->action = cpu_to_le16(CMD_ACT_SET);
+		prtp->currentlevel = cpu_to_le16(CMD_ACT_TX_POWER_INDEX_HIGH);
 		break;
 
-	case cmd_act_tx_power_opt_set_mid:
-		prtp->action = cpu_to_le16(cmd_act_set);
-		prtp->currentlevel = cpu_to_le16(cmd_act_tx_power_index_mid);
+	case CMD_ACT_TX_POWER_OPT_SET_MID:
+		prtp->action = cpu_to_le16(CMD_ACT_SET);
+		prtp->currentlevel = cpu_to_le16(CMD_ACT_TX_POWER_INDEX_MID);
 		break;
 
-	case cmd_act_tx_power_opt_set_low:
-		prtp->action = cpu_to_le16(cmd_act_set);
+	case CMD_ACT_TX_POWER_OPT_SET_LOW:
+		prtp->action = cpu_to_le16(CMD_ACT_SET);
 		prtp->currentlevel = cpu_to_le16(*((u16 *) pdata_buf));
 		break;
 	}
@@ -556,19 +563,21 @@ static int wlan_cmd_802_11_rf_tx_power(wlan_private * priv,
 	return 0;
 }
 
-static int wlan_cmd_802_11_rf_antenna(wlan_private * priv,
+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_rf_antenna *rant = &cmd->params.rant;
+	struct cmd_ds_802_11_monitor_mode *monitor = &cmd->params.monitor;
 
-	cmd->command = cpu_to_le16(cmd_802_11_rf_antenna);
-	cmd->size = cpu_to_le16(sizeof(struct cmd_ds_802_11_rf_antenna) +
-				S_DS_GEN);
+	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);
 
-	rant->action = cpu_to_le16(cmd_action);
-	if ((cmd_action == cmd_act_set_rx) || (cmd_action == cmd_act_set_tx)) {
-		rant->antennamode = cpu_to_le16((u16) (*(u32 *) pdata_buf));
+	monitor->action = cpu_to_le16(cmd_action);
+	if (cmd_action == CMD_ACT_SET) {
+		monitor->mode =
+		    cpu_to_le16((u16) (*(u32 *) pdata_buf));
 	}
 
 	return 0;
@@ -582,12 +591,11 @@ static int wlan_cmd_802_11_rate_adapt_rateset(wlan_private * priv,
 	*rateadapt = &cmd->params.rateset;
 	wlan_adapter *adapter = priv->adapter;
 
+	lbs_deb_enter(LBS_DEB_CMD);
 	cmd->size =
 	    cpu_to_le16(sizeof(struct cmd_ds_802_11_rate_adapt_rateset)
 			     + S_DS_GEN);
-	cmd->command = cpu_to_le16(cmd_802_11_rate_adapt_rateset);
-
-	lbs_deb_enter(LBS_DEB_CMD);
+	cmd->command = cpu_to_le16(CMD_802_11_RATE_ADAPT_RATESET);
 
 	rateadapt->action = cpu_to_le16(cmd_action);
 	rateadapt->enablehwauto = cpu_to_le16(adapter->enablehwauto);
@@ -608,19 +616,16 @@ static int wlan_cmd_802_11_data_rate(wlan_private * priv,
 
 	cmd->size = cpu_to_le16(sizeof(struct cmd_ds_802_11_data_rate) +
 			     S_DS_GEN);
-
-	cmd->command = cpu_to_le16(cmd_802_11_data_rate);
-
+	cmd->command = cpu_to_le16(CMD_802_11_DATA_RATE);
 	memset(pdatarate, 0, sizeof(struct cmd_ds_802_11_data_rate));
-
 	pdatarate->action = cpu_to_le16(cmd_action);
 
-	if (cmd_action == cmd_act_set_tx_fix_rate) {
-		pdatarate->datarate[0] = libertas_data_rate_to_index(adapter->datarate);
-		lbs_deb_cmd("Setting FW for fixed rate 0x%02X\n",
-		       adapter->datarate);
-	} else if (cmd_action == cmd_act_set_tx_auto) {
-		lbs_deb_cmd("Setting FW for AUTO rate\n");
+	if (cmd_action == CMD_ACT_SET_TX_FIX_RATE) {
+		pdatarate->rates[0] = libertas_data_rate_to_fw_index(adapter->cur_rate);
+		lbs_deb_cmd("DATA_RATE: set fixed 0x%02X\n",
+		       adapter->cur_rate);
+	} else if (cmd_action == CMD_ACT_SET_TX_AUTO) {
+		lbs_deb_cmd("DATA_RATE: setting auto\n");
 	}
 
 	lbs_deb_leave(LBS_DEB_CMD);
@@ -634,16 +639,19 @@ static int wlan_cmd_mac_multicast_adr(wlan_private * priv,
 	struct cmd_ds_mac_multicast_adr *pMCastAdr = &cmd->params.madr;
 	wlan_adapter *adapter = priv->adapter;
 
+	lbs_deb_enter(LBS_DEB_CMD);
 	cmd->size = cpu_to_le16(sizeof(struct cmd_ds_mac_multicast_adr) +
 			     S_DS_GEN);
-	cmd->command = cpu_to_le16(cmd_mac_multicast_adr);
+	cmd->command = cpu_to_le16(CMD_MAC_MULTICAST_ADR);
 
+	lbs_deb_cmd("MULTICAST_ADR: setting %d addresses\n", pMCastAdr->nr_of_adrs);
 	pMCastAdr->action = cpu_to_le16(cmd_action);
 	pMCastAdr->nr_of_adrs =
 	    cpu_to_le16((u16) adapter->nr_of_multicastmacaddr);
 	memcpy(pMCastAdr->maclist, adapter->multicastlist,
 	       adapter->nr_of_multicastmacaddr * ETH_ALEN);
 
+	lbs_deb_leave(LBS_DEB_CMD);
 	return 0;
 }
 
@@ -653,16 +661,18 @@ static int wlan_cmd_802_11_rf_channel(wlan_private * priv,
 {
 	struct cmd_ds_802_11_rf_channel *rfchan = &cmd->params.rfchannel;
 
-	cmd->command = cpu_to_le16(cmd_802_11_rf_channel);
+	lbs_deb_enter(LBS_DEB_CMD);
+	cmd->command = cpu_to_le16(CMD_802_11_RF_CHANNEL);
 	cmd->size = cpu_to_le16(sizeof(struct cmd_ds_802_11_rf_channel) +
 				S_DS_GEN);
 
-	if (option == cmd_opt_802_11_rf_channel_set) {
+	if (option == CMD_OPT_802_11_RF_CHANNEL_SET) {
 		rfchan->currentchannel = cpu_to_le16(*((u16 *) pdata_buf));
 	}
 
 	rfchan->action = cpu_to_le16(option);
 
+	lbs_deb_leave(LBS_DEB_CMD);
 	return 0;
 }
 
@@ -671,9 +681,10 @@ static int wlan_cmd_802_11_rssi(wlan_private * priv,
 {
 	wlan_adapter *adapter = priv->adapter;
 
-	cmd->command = cpu_to_le16(cmd_802_11_rssi);
+	lbs_deb_enter(LBS_DEB_CMD);
+	cmd->command = cpu_to_le16(CMD_802_11_RSSI);
 	cmd->size = cpu_to_le16(sizeof(struct cmd_ds_802_11_rssi) + S_DS_GEN);
-	cmd->params.rssi.N = cpu_to_le16(priv->adapter->bcn_avg_factor);
+	cmd->params.rssi.N = cpu_to_le16(DEFAULT_BCN_AVG_FACTOR);
 
 	/* reset Beacon SNR/NF/RSSI values */
 	adapter->SNR[TYPE_BEACON][TYPE_NOAVG] = 0;
@@ -683,6 +694,7 @@ static int wlan_cmd_802_11_rssi(wlan_private * priv,
 	adapter->RSSI[TYPE_BEACON][TYPE_NOAVG] = 0;
 	adapter->RSSI[TYPE_BEACON][TYPE_AVG] = 0;
 
+	lbs_deb_leave(LBS_DEB_CMD);
 	return 0;
 }
 
@@ -697,7 +709,7 @@ static int wlan_cmd_reg_access(wlan_private * priv,
 	offval = (struct wlan_offset_value *)pdata_buf;
 
 	switch (cmdptr->command) {
-	case cmd_mac_reg_access:
+	case CMD_MAC_REG_ACCESS:
 		{
 			struct cmd_ds_mac_reg_access *macreg;
 
@@ -715,7 +727,7 @@ static int wlan_cmd_reg_access(wlan_private * priv,
 			break;
 		}
 
-	case cmd_bbp_reg_access:
+	case CMD_BBP_REG_ACCESS:
 		{
 			struct cmd_ds_bbp_reg_access *bbpreg;
 
@@ -734,7 +746,7 @@ static int wlan_cmd_reg_access(wlan_private * priv,
 			break;
 		}
 
-	case cmd_rf_reg_access:
+	case CMD_RF_REG_ACCESS:
 		{
 			struct cmd_ds_rf_reg_access *rfreg;
 
@@ -767,19 +779,21 @@ static int wlan_cmd_802_11_mac_address(wlan_private * priv,
 {
 	wlan_adapter *adapter = priv->adapter;
 
-	cmd->command = cpu_to_le16(cmd_802_11_mac_address);
+	lbs_deb_enter(LBS_DEB_CMD);
+	cmd->command = cpu_to_le16(CMD_802_11_MAC_ADDRESS);
 	cmd->size = cpu_to_le16(sizeof(struct cmd_ds_802_11_mac_address) +
 			     S_DS_GEN);
 	cmd->result = 0;
 
 	cmd->params.macadd.action = cpu_to_le16(cmd_action);
 
-	if (cmd_action == cmd_act_set) {
+	if (cmd_action == CMD_ACT_SET) {
 		memcpy(cmd->params.macadd.macadd,
 		       adapter->current_addr, ETH_ALEN);
-		lbs_dbg_hex("SET_CMD: MAC ADDRESS-", adapter->current_addr, 6);
+		lbs_deb_hex(LBS_DEB_CMD, "SET_CMD: MAC addr", adapter->current_addr, 6);
 	}
 
+	lbs_deb_leave(LBS_DEB_CMD);
 	return 0;
 }
 
@@ -791,7 +805,7 @@ static int wlan_cmd_802_11_eeprom_access(wlan_private * priv,
 
 	lbs_deb_enter(LBS_DEB_CMD);
 
-	cmd->command = cpu_to_le16(cmd_802_11_eeprom_access);
+	cmd->command = cpu_to_le16(CMD_802_11_EEPROM_ACCESS);
 	cmd->size = cpu_to_le16(sizeof(struct cmd_ds_802_11_eeprom_access) +
 				S_DS_GEN);
 	cmd->result = 0;
@@ -801,6 +815,7 @@ static int wlan_cmd_802_11_eeprom_access(wlan_private * priv,
 	cmd->params.rdeeprom.bytecount = cpu_to_le16(ea->NOB);
 	cmd->params.rdeeprom.value = 0;
 
+	lbs_deb_leave(LBS_DEB_CMD);
 	return 0;
 }
 
@@ -809,35 +824,36 @@ static int wlan_cmd_bt_access(wlan_private * priv,
 			       u16 cmd_action, void *pdata_buf)
 {
 	struct cmd_ds_bt_access *bt_access = &cmd->params.bt;
-	lbs_deb_cmd("BT CMD(%d)\n", cmd_action);
+	lbs_deb_enter_args(LBS_DEB_CMD, "action %d", cmd_action);
 
-	cmd->command = cpu_to_le16(cmd_bt_access);
+	cmd->command = cpu_to_le16(CMD_BT_ACCESS);
 	cmd->size = cpu_to_le16(sizeof(struct cmd_ds_bt_access) + S_DS_GEN);
 	cmd->result = 0;
 	bt_access->action = cpu_to_le16(cmd_action);
 
 	switch (cmd_action) {
-	case cmd_act_bt_access_add:
+	case CMD_ACT_BT_ACCESS_ADD:
 		memcpy(bt_access->addr1, pdata_buf, 2 * ETH_ALEN);
-		lbs_dbg_hex("BT_ADD: blinded mac address-", bt_access->addr1, 6);
+		lbs_deb_hex(LBS_DEB_MESH, "BT_ADD: blinded MAC addr", bt_access->addr1, 6);
 		break;
-	case cmd_act_bt_access_del:
+	case CMD_ACT_BT_ACCESS_DEL:
 		memcpy(bt_access->addr1, pdata_buf, 1 * ETH_ALEN);
-		lbs_dbg_hex("BT_DEL: blinded mac address-", bt_access->addr1, 6);
+		lbs_deb_hex(LBS_DEB_MESH, "BT_DEL: blinded MAC addr", bt_access->addr1, 6);
 		break;
-	case cmd_act_bt_access_list:
+	case CMD_ACT_BT_ACCESS_LIST:
 		bt_access->id = cpu_to_le32(*(u32 *) pdata_buf);
 		break;
-	case cmd_act_bt_access_reset:
+	case CMD_ACT_BT_ACCESS_RESET:
 		break;
-	case cmd_act_bt_access_set_invert:
+	case CMD_ACT_BT_ACCESS_SET_INVERT:
 		bt_access->id = cpu_to_le32(*(u32 *) pdata_buf);
 		break;
-	case cmd_act_bt_access_get_invert:
+	case CMD_ACT_BT_ACCESS_GET_INVERT:
 		break;
 	default:
 		break;
 	}
+	lbs_deb_leave(LBS_DEB_CMD);
 	return 0;
 }
 
@@ -846,9 +862,9 @@ static int wlan_cmd_fwt_access(wlan_private * priv,
 			       u16 cmd_action, void *pdata_buf)
 {
 	struct cmd_ds_fwt_access *fwt_access = &cmd->params.fwt;
-	lbs_deb_cmd("FWT CMD(%d)\n", cmd_action);
+	lbs_deb_enter_args(LBS_DEB_CMD, "action %d", cmd_action);
 
-	cmd->command = cpu_to_le16(cmd_fwt_access);
+	cmd->command = cpu_to_le16(CMD_FWT_ACCESS);
 	cmd->size = cpu_to_le16(sizeof(struct cmd_ds_fwt_access) + S_DS_GEN);
 	cmd->result = 0;
 
@@ -859,6 +875,7 @@ static int wlan_cmd_fwt_access(wlan_private * priv,
 
 	fwt_access->action = cpu_to_le16(cmd_action);
 
+	lbs_deb_leave(LBS_DEB_CMD);
 	return 0;
 }
 
@@ -867,9 +884,9 @@ static int wlan_cmd_mesh_access(wlan_private * priv,
 				u16 cmd_action, void *pdata_buf)
 {
 	struct cmd_ds_mesh_access *mesh_access = &cmd->params.mesh;
-	lbs_deb_cmd("FWT CMD(%d)\n", cmd_action);
+	lbs_deb_enter_args(LBS_DEB_CMD, "action %d", cmd_action);
 
-	cmd->command = cpu_to_le16(cmd_mesh_access);
+	cmd->command = cpu_to_le16(CMD_MESH_ACCESS);
 	cmd->size = cpu_to_le16(sizeof(struct cmd_ds_mesh_access) + S_DS_GEN);
 	cmd->result = 0;
 
@@ -880,6 +897,18 @@ static int wlan_cmd_mesh_access(wlan_private * priv,
 
 	mesh_access->action = cpu_to_le16(cmd_action);
 
+	lbs_deb_leave(LBS_DEB_CMD);
+	return 0;
+}
+
+static int wlan_cmd_set_boot2_ver(wlan_private * priv,
+				struct cmd_ds_command *cmd,
+				u16 cmd_action, void *pdata_buf)
+{
+	struct cmd_ds_set_boot2_ver *boot2_ver = &cmd->params.boot2_ver;
+	cmd->command = cpu_to_le16(CMD_SET_BOOT2_VER);
+	cmd->size = cpu_to_le16(sizeof(struct cmd_ds_set_boot2_ver) + S_DS_GEN);
+	boot2_ver->version = priv->boot2_version;
 	return 0;
 }
 
@@ -888,23 +917,23 @@ void libertas_queue_cmd(wlan_adapter * adapter, struct cmd_ctrl_node *cmdnode, u
 	unsigned long flags;
 	struct cmd_ds_command *cmdptr;
 
-	lbs_deb_enter(LBS_DEB_CMD);
+	lbs_deb_enter(LBS_DEB_HOST);
 
 	if (!cmdnode) {
-		lbs_deb_cmd("QUEUE_CMD: cmdnode is NULL\n");
+		lbs_deb_host("QUEUE_CMD: cmdnode is NULL\n");
 		goto done;
 	}
 
 	cmdptr = (struct cmd_ds_command *)cmdnode->bufvirtualaddr;
 	if (!cmdptr) {
-		lbs_deb_cmd("QUEUE_CMD: cmdptr is NULL\n");
+		lbs_deb_host("QUEUE_CMD: cmdptr is NULL\n");
 		goto done;
 	}
 
 	/* Exit_PS command needs to be queued in the header always. */
-	if (cmdptr->command == cmd_802_11_ps_mode) {
+	if (cmdptr->command == CMD_802_11_PS_MODE) {
 		struct cmd_ds_802_11_ps_mode *psm = &cmdptr->params.psmode;
-		if (psm->action == cpu_to_le16(cmd_subcmd_exit_ps)) {
+		if (psm->action == cpu_to_le16(CMD_SUBCMD_EXIT_PS)) {
 			if (adapter->psstate != PS_STATE_FULL_POWER)
 				addtail = 0;
 		}
@@ -920,17 +949,16 @@ void libertas_queue_cmd(wlan_adapter * adapter, struct cmd_ctrl_node *cmdnode, u
 
 	spin_unlock_irqrestore(&adapter->driver_lock, flags);
 
-	lbs_deb_cmd("QUEUE_CMD: Inserted node=%p, cmd=0x%x in cmdpendingq\n",
-	       cmdnode,
+	lbs_deb_host("QUEUE_CMD: inserted command 0x%04x into cmdpendingq\n",
 	       le16_to_cpu(((struct cmd_ds_gen*)cmdnode->bufvirtualaddr)->command));
 
 done:
-	lbs_deb_leave(LBS_DEB_CMD);
+	lbs_deb_leave(LBS_DEB_HOST);
 }
 
 /*
  * TODO: Fix the issue when DownloadcommandToStation is being called the
- * second time when the command timesout. All the cmdptr->xxx are in little
+ * second time when the command times out. All the cmdptr->xxx are in little
  * endian and therefore all the comparissions will fail.
  * For now - we are not performing the endian conversion the second time - but
  * for PS and DEEP_SLEEP we need to worry
@@ -941,68 +969,59 @@ static int DownloadcommandToStation(wlan_private * priv,
 	unsigned long flags;
 	struct cmd_ds_command *cmdptr;
 	wlan_adapter *adapter = priv->adapter;
-	int ret = 0;
+	int ret = -1;
 	u16 cmdsize;
 	u16 command;
 
-	lbs_deb_enter(LBS_DEB_CMD);
+	lbs_deb_enter(LBS_DEB_HOST);
 
 	if (!adapter || !cmdnode) {
-		lbs_deb_cmd("DNLD_CMD: adapter = %p, cmdnode = %p\n",
-		       adapter, cmdnode);
-		if (cmdnode) {
-			spin_lock_irqsave(&adapter->driver_lock, flags);
-			__libertas_cleanup_and_insert_cmd(priv, cmdnode);
-			spin_unlock_irqrestore(&adapter->driver_lock, flags);
-		}
-		ret = -1;
+		lbs_deb_host("DNLD_CMD: adapter or cmdmode is NULL\n");
 		goto done;
 	}
 
 	cmdptr = (struct cmd_ds_command *)cmdnode->bufvirtualaddr;
 
-
 	spin_lock_irqsave(&adapter->driver_lock, flags);
 	if (!cmdptr || !cmdptr->size) {
-		lbs_deb_cmd("DNLD_CMD: cmdptr is Null or cmd size is Zero, "
-		       "Not sending\n");
+		lbs_deb_host("DNLD_CMD: cmdptr is NULL or zero\n");
 		__libertas_cleanup_and_insert_cmd(priv, cmdnode);
 		spin_unlock_irqrestore(&adapter->driver_lock, flags);
-		ret = -1;
 		goto done;
 	}
 
 	adapter->cur_cmd = cmdnode;
 	adapter->cur_cmd_retcode = 0;
 	spin_unlock_irqrestore(&adapter->driver_lock, flags);
-	lbs_deb_cmd("DNLD_CMD:: Before download, size of cmd = %d\n",
-		    le16_to_cpu(cmdptr->size));
 
 	cmdsize = cmdptr->size;
-
 	command = cpu_to_le16(cmdptr->command);
 
+	lbs_deb_host("DNLD_CMD: command 0x%04x, size %d, jiffies %lu\n",
+		    command, le16_to_cpu(cmdptr->size), jiffies);
+	lbs_deb_hex(LBS_DEB_HOST, "DNLD_CMD", cmdnode->bufvirtualaddr, cmdsize);
+
 	cmdnode->cmdwaitqwoken = 0;
 	cmdsize = cpu_to_le16(cmdsize);
 
 	ret = priv->hw_host_to_card(priv, MVMS_CMD, (u8 *) cmdptr, cmdsize);
 
 	if (ret != 0) {
-		lbs_deb_cmd("DNLD_CMD: Host to Card failed\n");
+		lbs_deb_host("DNLD_CMD: hw_host_to_card failed\n");
 		spin_lock_irqsave(&adapter->driver_lock, flags);
+		adapter->cur_cmd_retcode = ret;
 		__libertas_cleanup_and_insert_cmd(priv, adapter->cur_cmd);
+		adapter->nr_cmd_pending--;
 		adapter->cur_cmd = NULL;
 		spin_unlock_irqrestore(&adapter->driver_lock, flags);
-		ret = -1;
 		goto done;
 	}
 
-	lbs_deb_cmd("DNLD_CMD: Sent command 0x%x @ %lu\n", command, jiffies);
-	lbs_dbg_hex("DNLD_CMD: command", cmdnode->bufvirtualaddr, cmdsize);
+	lbs_deb_cmd("DNLD_CMD: sent command 0x%04x, jiffies %lu\n", command, jiffies);
 
 	/* Setup the timer after transmit command */
-	if (command == cmd_802_11_scan || command == cmd_802_11_authenticate
-	    || command == cmd_802_11_associate)
+	if (command == CMD_802_11_SCAN || command == CMD_802_11_AUTHENTICATE
+	    || command == CMD_802_11_ASSOCIATE)
 		mod_timer(&adapter->command_timer, jiffies + (10*HZ));
 	else
 		mod_timer(&adapter->command_timer, jiffies + (5*HZ));
@@ -1010,7 +1029,7 @@ static int DownloadcommandToStation(wlan_private * priv,
 	ret = 0;
 
 done:
-	lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
+	lbs_deb_leave_args(LBS_DEB_HOST, "ret %d", ret);
 	return ret;
 }
 
@@ -1021,11 +1040,11 @@ static int wlan_cmd_mac_control(wlan_private * priv,
 
 	lbs_deb_enter(LBS_DEB_CMD);
 
-	cmd->command = cpu_to_le16(cmd_mac_control);
+	cmd->command = cpu_to_le16(CMD_MAC_CONTROL);
 	cmd->size = cpu_to_le16(sizeof(struct cmd_ds_mac_control) + S_DS_GEN);
 	mac->action = cpu_to_le16(priv->adapter->currentpacketfilter);
 
-	lbs_deb_cmd("wlan_cmd_mac_control(): action=0x%X size=%d\n",
+	lbs_deb_cmd("MAC_CONTROL: action 0x%x, size %d\n",
 		    le16_to_cpu(mac->action), le16_to_cpu(cmd->size));
 
 	lbs_deb_leave(LBS_DEB_CMD);
@@ -1041,15 +1060,13 @@ void __libertas_cleanup_and_insert_cmd(wlan_private * priv, struct cmd_ctrl_node
 	wlan_adapter *adapter = priv->adapter;
 
 	if (!ptempcmd)
-		goto done;
+		return;
 
 	cleanup_cmdnode(ptempcmd);
 	list_add_tail((struct list_head *)ptempcmd, &adapter->cmdfreeq);
-done:
-	return;
 }
 
-void libertas_cleanup_and_insert_cmd(wlan_private * priv, struct cmd_ctrl_node *ptempcmd)
+static void libertas_cleanup_and_insert_cmd(wlan_private * priv, struct cmd_ctrl_node *ptempcmd)
 {
 	unsigned long flags;
 
@@ -1065,11 +1082,11 @@ int libertas_set_radio_control(wlan_private * priv)
 	lbs_deb_enter(LBS_DEB_CMD);
 
 	ret = libertas_prepare_and_send_command(priv,
-				    cmd_802_11_radio_control,
-				    cmd_act_set,
-				    cmd_option_waitforrsp, 0, NULL);
+				    CMD_802_11_RADIO_CONTROL,
+				    CMD_ACT_SET,
+				    CMD_OPTION_WAITFORRSP, 0, NULL);
 
-	lbs_deb_cmd("RADIO_SET: on or off: 0x%X, preamble = 0x%X\n",
+	lbs_deb_cmd("RADIO_SET: radio %d, preamble %d\n",
 	       priv->adapter->radioon, priv->adapter->preamble);
 
 	lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
@@ -1082,12 +1099,9 @@ int libertas_set_mac_packet_filter(wlan_private * priv)
 
 	lbs_deb_enter(LBS_DEB_CMD);
 
-	lbs_deb_cmd("libertas_set_mac_packet_filter value = %x\n",
-	       priv->adapter->currentpacketfilter);
-
 	/* Send MAC control command to station */
 	ret = libertas_prepare_and_send_command(priv,
-				    cmd_mac_control, 0, 0, 0, NULL);
+				    CMD_MAC_CONTROL, 0, 0, 0, NULL);
 
 	lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
 	return ret;
@@ -1115,16 +1129,16 @@ int libertas_prepare_and_send_command(wlan_private * priv,
 	struct cmd_ds_command *cmdptr;
 	unsigned long flags;
 
-	lbs_deb_enter(LBS_DEB_CMD);
+	lbs_deb_enter(LBS_DEB_HOST);
 
 	if (!adapter) {
-		lbs_deb_cmd("PREP_CMD: adapter is Null\n");
+		lbs_deb_host("PREP_CMD: adapter is NULL\n");
 		ret = -1;
 		goto done;
 	}
 
 	if (adapter->surpriseremoved) {
-		lbs_deb_cmd("PREP_CMD: Card is Removed\n");
+		lbs_deb_host("PREP_CMD: card removed\n");
 		ret = -1;
 		goto done;
 	}
@@ -1132,10 +1146,10 @@ int libertas_prepare_and_send_command(wlan_private * priv,
 	cmdnode = libertas_get_free_cmd_ctrl_node(priv);
 
 	if (cmdnode == NULL) {
-		lbs_deb_cmd("PREP_CMD: No free cmdnode\n");
+		lbs_deb_host("PREP_CMD: cmdnode is NULL\n");
 
 		/* Wake up main thread to execute next command */
-		wake_up_interruptible(&priv->mainthread.waitq);
+		wake_up_interruptible(&priv->waitq);
 		ret = -1;
 		goto done;
 	}
@@ -1144,11 +1158,10 @@ int libertas_prepare_and_send_command(wlan_private * priv,
 
 	cmdptr = (struct cmd_ds_command *)cmdnode->bufvirtualaddr;
 
-	lbs_deb_cmd("PREP_CMD: Val of cmd ptr=%p, command=0x%X\n",
-	       cmdptr, cmd_no);
+	lbs_deb_host("PREP_CMD: command 0x%04x\n", cmd_no);
 
 	if (!cmdptr) {
-		lbs_deb_cmd("PREP_CMD: bufvirtualaddr of cmdnode is NULL\n");
+		lbs_deb_host("PREP_CMD: cmdptr is NULL\n");
 		libertas_cleanup_and_insert_cmd(priv, cmdnode);
 		ret = -1;
 		goto done;
@@ -1162,136 +1175,136 @@ int libertas_prepare_and_send_command(wlan_private * priv,
 	cmdptr->result = 0;
 
 	switch (cmd_no) {
-	case cmd_get_hw_spec:
+	case CMD_GET_HW_SPEC:
 		ret = wlan_cmd_hw_spec(priv, cmdptr);
 		break;
-	case cmd_802_11_ps_mode:
+	case CMD_802_11_PS_MODE:
 		ret = wlan_cmd_802_11_ps_mode(priv, cmdptr, cmd_action);
 		break;
 
-	case cmd_802_11_scan:
+	case CMD_802_11_SCAN:
 		ret = libertas_cmd_80211_scan(priv, cmdptr, pdata_buf);
 		break;
 
-	case cmd_mac_control:
+	case CMD_MAC_CONTROL:
 		ret = wlan_cmd_mac_control(priv, cmdptr);
 		break;
 
-	case cmd_802_11_associate:
-	case cmd_802_11_reassociate:
+	case CMD_802_11_ASSOCIATE:
+	case CMD_802_11_REASSOCIATE:
 		ret = libertas_cmd_80211_associate(priv, cmdptr, pdata_buf);
 		break;
 
-	case cmd_802_11_deauthenticate:
+	case CMD_802_11_DEAUTHENTICATE:
 		ret = libertas_cmd_80211_deauthenticate(priv, cmdptr);
 		break;
 
-	case cmd_802_11_set_wep:
+	case CMD_802_11_SET_WEP:
 		ret = wlan_cmd_802_11_set_wep(priv, cmdptr, cmd_action, pdata_buf);
 		break;
 
-	case cmd_802_11_ad_hoc_start:
+	case CMD_802_11_AD_HOC_START:
 		ret = libertas_cmd_80211_ad_hoc_start(priv, cmdptr, pdata_buf);
 		break;
-	case cmd_code_dnld:
+	case CMD_CODE_DNLD:
 		break;
 
-	case cmd_802_11_reset:
+	case CMD_802_11_RESET:
 		ret = wlan_cmd_802_11_reset(priv, cmdptr, cmd_action);
 		break;
 
-	case cmd_802_11_get_log:
+	case CMD_802_11_GET_LOG:
 		ret = wlan_cmd_802_11_get_log(priv, cmdptr);
 		break;
 
-	case cmd_802_11_authenticate:
+	case CMD_802_11_AUTHENTICATE:
 		ret = libertas_cmd_80211_authenticate(priv, cmdptr, pdata_buf);
 		break;
 
-	case cmd_802_11_get_stat:
+	case CMD_802_11_GET_STAT:
 		ret = wlan_cmd_802_11_get_stat(priv, cmdptr);
 		break;
 
-	case cmd_802_11_snmp_mib:
+	case CMD_802_11_SNMP_MIB:
 		ret = wlan_cmd_802_11_snmp_mib(priv, cmdptr,
 					       cmd_action, cmd_oid, pdata_buf);
 		break;
 
-	case cmd_mac_reg_access:
-	case cmd_bbp_reg_access:
-	case cmd_rf_reg_access:
+	case CMD_MAC_REG_ACCESS:
+	case CMD_BBP_REG_ACCESS:
+	case CMD_RF_REG_ACCESS:
 		ret = wlan_cmd_reg_access(priv, cmdptr, cmd_action, pdata_buf);
 		break;
 
-	case cmd_802_11_rf_channel:
+	case CMD_802_11_RF_CHANNEL:
 		ret = wlan_cmd_802_11_rf_channel(priv, cmdptr,
 						 cmd_action, pdata_buf);
 		break;
 
-	case cmd_802_11_rf_tx_power:
+	case CMD_802_11_RF_TX_POWER:
 		ret = wlan_cmd_802_11_rf_tx_power(priv, cmdptr,
 						  cmd_action, pdata_buf);
 		break;
 
-	case cmd_802_11_radio_control:
+	case CMD_802_11_RADIO_CONTROL:
 		ret = wlan_cmd_802_11_radio_control(priv, cmdptr, cmd_action);
 		break;
 
-	case cmd_802_11_rf_antenna:
-		ret = wlan_cmd_802_11_rf_antenna(priv, cmdptr,
-						 cmd_action, pdata_buf);
-		break;
-
-	case cmd_802_11_data_rate:
+	case CMD_802_11_DATA_RATE:
 		ret = wlan_cmd_802_11_data_rate(priv, cmdptr, cmd_action);
 		break;
-	case cmd_802_11_rate_adapt_rateset:
+	case CMD_802_11_RATE_ADAPT_RATESET:
 		ret = wlan_cmd_802_11_rate_adapt_rateset(priv,
 							 cmdptr, cmd_action);
 		break;
 
-	case cmd_mac_multicast_adr:
+	case CMD_MAC_MULTICAST_ADR:
 		ret = wlan_cmd_mac_multicast_adr(priv, cmdptr, cmd_action);
 		break;
 
-	case cmd_802_11_ad_hoc_join:
+	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;
 
-	case cmd_802_11_rssi:
+	case CMD_802_11_RSSI:
 		ret = wlan_cmd_802_11_rssi(priv, cmdptr);
 		break;
 
-	case cmd_802_11_ad_hoc_stop:
+	case CMD_802_11_AD_HOC_STOP:
 		ret = libertas_cmd_80211_ad_hoc_stop(priv, cmdptr);
 		break;
 
-	case cmd_802_11_enable_rsn:
+	case CMD_802_11_ENABLE_RSN:
 		ret = wlan_cmd_802_11_enable_rsn(priv, cmdptr, cmd_action,
 				pdata_buf);
 		break;
 
-	case cmd_802_11_key_material:
+	case CMD_802_11_KEY_MATERIAL:
 		ret = wlan_cmd_802_11_key_material(priv, cmdptr, cmd_action,
 				cmd_oid, pdata_buf);
 		break;
 
-	case cmd_802_11_pairwise_tsc:
+	case CMD_802_11_PAIRWISE_TSC:
 		break;
-	case cmd_802_11_group_tsc:
+	case CMD_802_11_GROUP_TSC:
 		break;
 
-	case cmd_802_11_mac_address:
+	case CMD_802_11_MAC_ADDRESS:
 		ret = wlan_cmd_802_11_mac_address(priv, cmdptr, cmd_action);
 		break;
 
-	case cmd_802_11_eeprom_access:
+	case CMD_802_11_EEPROM_ACCESS:
 		ret = wlan_cmd_802_11_eeprom_access(priv, cmdptr,
 						    cmd_action, pdata_buf);
 		break;
 
-	case cmd_802_11_set_afc:
-	case cmd_802_11_get_afc:
+	case CMD_802_11_SET_AFC:
+	case CMD_802_11_GET_AFC:
 
 		cmdptr->command = cpu_to_le16(cmd_no);
 		cmdptr->size = cpu_to_le16(sizeof(struct cmd_ds_802_11_afc) +
@@ -1303,22 +1316,22 @@ int libertas_prepare_and_send_command(wlan_private * priv,
 		ret = 0;
 		goto done;
 
-	case cmd_802_11d_domain_info:
+	case CMD_802_11D_DOMAIN_INFO:
 		ret = libertas_cmd_802_11d_domain_info(priv, cmdptr,
 						   cmd_no, cmd_action);
 		break;
 
-	case cmd_802_11_sleep_params:
+	case CMD_802_11_SLEEP_PARAMS:
 		ret = wlan_cmd_802_11_sleep_params(priv, cmdptr, cmd_action);
 		break;
-	case cmd_802_11_inactivity_timeout:
+	case CMD_802_11_INACTIVITY_TIMEOUT:
 		ret = wlan_cmd_802_11_inactivity_timeout(priv, cmdptr,
 							 cmd_action, pdata_buf);
 		libertas_set_cmd_ctrl_node(priv, cmdnode, 0, 0, pdata_buf);
 		break;
 
-	case cmd_802_11_tpc_cfg:
-		cmdptr->command = cpu_to_le16(cmd_802_11_tpc_cfg);
+	case CMD_802_11_TPC_CFG:
+		cmdptr->command = cpu_to_le16(CMD_802_11_TPC_CFG);
 		cmdptr->size =
 		    cpu_to_le16(sizeof(struct cmd_ds_802_11_tpc_cfg) +
 				     S_DS_GEN);
@@ -1328,7 +1341,7 @@ int libertas_prepare_and_send_command(wlan_private * priv,
 
 		ret = 0;
 		break;
-	case cmd_802_11_led_gpio_ctrl:
+	case CMD_802_11_LED_GPIO_CTRL:
 		{
 			struct mrvlietypes_ledgpio *gpio =
 			    (struct mrvlietypes_ledgpio*)
@@ -1339,7 +1352,7 @@ int libertas_prepare_and_send_command(wlan_private * priv,
 				sizeof(struct cmd_ds_802_11_led_ctrl));
 
 			cmdptr->command =
-			    cpu_to_le16(cmd_802_11_led_gpio_ctrl);
+			    cpu_to_le16(CMD_802_11_LED_GPIO_CTRL);
 
 #define ACTION_NUMLED_TLVTYPE_LEN_FIELDS_LEN 8
 			cmdptr->size =
@@ -1350,8 +1363,8 @@ int libertas_prepare_and_send_command(wlan_private * priv,
 			ret = 0;
 			break;
 		}
-	case cmd_802_11_pwr_cfg:
-		cmdptr->command = cpu_to_le16(cmd_802_11_pwr_cfg);
+	case CMD_802_11_PWR_CFG:
+		cmdptr->command = cpu_to_le16(CMD_802_11_PWR_CFG);
 		cmdptr->size =
 		    cpu_to_le16(sizeof(struct cmd_ds_802_11_pwr_cfg) +
 				     S_DS_GEN);
@@ -1360,40 +1373,37 @@ int libertas_prepare_and_send_command(wlan_private * priv,
 
 		ret = 0;
 		break;
-	case cmd_bt_access:
+	case CMD_BT_ACCESS:
 		ret = wlan_cmd_bt_access(priv, cmdptr, cmd_action, pdata_buf);
 		break;
 
-	case cmd_fwt_access:
+	case CMD_FWT_ACCESS:
 		ret = wlan_cmd_fwt_access(priv, cmdptr, cmd_action, pdata_buf);
 		break;
 
-	case cmd_mesh_access:
+	case CMD_MESH_ACCESS:
 		ret = wlan_cmd_mesh_access(priv, cmdptr, cmd_action, pdata_buf);
 		break;
 
-	case cmd_get_tsf:
-		cmdptr->command = cpu_to_le16(cmd_get_tsf);
-		cmdptr->size = cpu_to_le16(sizeof(struct cmd_ds_get_tsf) +
-					   S_DS_GEN);
-		ret = 0;
+	case CMD_SET_BOOT2_VER:
+		ret = wlan_cmd_set_boot2_ver(priv, cmdptr, cmd_action, pdata_buf);
 		break;
-	case cmd_802_11_tx_rate_query:
-		cmdptr->command = cpu_to_le16(cmd_802_11_tx_rate_query);
-		cmdptr->size = cpu_to_le16(sizeof(struct cmd_tx_rate_query) +
+
+	case CMD_GET_TSF:
+		cmdptr->command = cpu_to_le16(CMD_GET_TSF);
+		cmdptr->size = cpu_to_le16(sizeof(struct cmd_ds_get_tsf) +
 					   S_DS_GEN);
-		adapter->txrate = 0;
 		ret = 0;
 		break;
 	default:
-		lbs_deb_cmd("PREP_CMD: unknown command- %#x\n", cmd_no);
+		lbs_deb_host("PREP_CMD: unknown command 0x%04x\n", cmd_no);
 		ret = -1;
 		break;
 	}
 
 	/* return error, since the command preparation failed */
 	if (ret != 0) {
-		lbs_deb_cmd("PREP_CMD: command preparation failed\n");
+		lbs_deb_host("PREP_CMD: command preparation failed\n");
 		libertas_cleanup_and_insert_cmd(priv, cmdnode);
 		ret = -1;
 		goto done;
@@ -1403,10 +1413,10 @@ int libertas_prepare_and_send_command(wlan_private * priv,
 
 	libertas_queue_cmd(adapter, cmdnode, 1);
 	adapter->nr_cmd_pending++;
-	wake_up_interruptible(&priv->mainthread.waitq);
+	wake_up_interruptible(&priv->waitq);
 
-	if (wait_option & cmd_option_waitforrsp) {
-		lbs_deb_cmd("PREP_CMD: Wait for CMD response\n");
+	if (wait_option & CMD_OPTION_WAITFORRSP) {
+		lbs_deb_host("PREP_CMD: wait for response\n");
 		might_sleep();
 		wait_event_interruptible(cmdnode->cmdwait_q,
 					 cmdnode->cmdwaitqwoken);
@@ -1414,7 +1424,7 @@ int libertas_prepare_and_send_command(wlan_private * priv,
 
 	spin_lock_irqsave(&adapter->driver_lock, flags);
 	if (adapter->cur_cmd_retcode) {
-		lbs_deb_cmd("PREP_CMD: command failed with return code=%d\n",
+		lbs_deb_host("PREP_CMD: command failed with return code %d\n",
 		       adapter->cur_cmd_retcode);
 		adapter->cur_cmd_retcode = 0;
 		ret = -1;
@@ -1422,7 +1432,7 @@ int libertas_prepare_and_send_command(wlan_private * priv,
 	spin_unlock_irqrestore(&adapter->driver_lock, flags);
 
 done:
-	lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
+	lbs_deb_leave_args(LBS_DEB_HOST, "ret %d", ret);
 	return ret;
 }
 EXPORT_SYMBOL_GPL(libertas_prepare_and_send_command);
@@ -1443,14 +1453,13 @@ int libertas_allocate_cmd_buffer(wlan_private * priv)
 	u8 *ptempvirtualaddr;
 	wlan_adapter *adapter = priv->adapter;
 
-	lbs_deb_enter(LBS_DEB_CMD);
+	lbs_deb_enter(LBS_DEB_HOST);
 
 	/* Allocate and initialize cmdCtrlNode */
 	ulbufsize = sizeof(struct cmd_ctrl_node) * MRVDRV_NUM_OF_CMD_BUFFER;
 
 	if (!(tempcmd_array = kzalloc(ulbufsize, GFP_KERNEL))) {
-		lbs_deb_cmd(
-		       "ALLOC_CMD_BUF: failed to allocate tempcmd_array\n");
+		lbs_deb_host("ALLOC_CMD_BUF: tempcmd_array is NULL\n");
 		ret = -1;
 		goto done;
 	}
@@ -1460,8 +1469,7 @@ int libertas_allocate_cmd_buffer(wlan_private * priv)
 	ulbufsize = MRVDRV_SIZE_OF_CMD_BUFFER;
 	for (i = 0; i < MRVDRV_NUM_OF_CMD_BUFFER; i++) {
 		if (!(ptempvirtualaddr = kzalloc(ulbufsize, GFP_KERNEL))) {
-			lbs_deb_cmd(
-			       "ALLOC_CMD_BUF: ptempvirtualaddr: out of memory\n");
+			lbs_deb_host("ALLOC_CMD_BUF: ptempvirtualaddr is NULL\n");
 			ret = -1;
 			goto done;
 		}
@@ -1478,7 +1486,7 @@ int libertas_allocate_cmd_buffer(wlan_private * priv)
 	ret = 0;
 
 done:
-	lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
+	lbs_deb_leave_args(LBS_DEB_HOST, "ret %d", ret);
 	return ret;
 }
 
@@ -1495,11 +1503,11 @@ int libertas_free_cmd_buffer(wlan_private * priv)
 	struct cmd_ctrl_node *tempcmd_array;
 	wlan_adapter *adapter = priv->adapter;
 
-	lbs_deb_enter(LBS_DEB_CMD);
+	lbs_deb_enter(LBS_DEB_HOST);
 
 	/* need to check if cmd array is allocated or not */
 	if (adapter->cmd_array == NULL) {
-		lbs_deb_cmd("FREE_CMD_BUF: cmd_array is Null\n");
+		lbs_deb_host("FREE_CMD_BUF: cmd_array is NULL\n");
 		goto done;
 	}
 
@@ -1509,7 +1517,6 @@ int libertas_free_cmd_buffer(wlan_private * priv)
 	ulbufsize = MRVDRV_SIZE_OF_CMD_BUFFER;
 	for (i = 0; i < MRVDRV_NUM_OF_CMD_BUFFER; i++) {
 		if (tempcmd_array[i].bufvirtualaddr) {
-			lbs_deb_cmd("Free all the array\n");
 			kfree(tempcmd_array[i].bufvirtualaddr);
 			tempcmd_array[i].bufvirtualaddr = NULL;
 		}
@@ -1517,13 +1524,12 @@ int libertas_free_cmd_buffer(wlan_private * priv)
 
 	/* Release cmd_ctrl_node */
 	if (adapter->cmd_array) {
-		lbs_deb_cmd("Free cmd_array\n");
 		kfree(adapter->cmd_array);
 		adapter->cmd_array = NULL;
 	}
 
 done:
-	lbs_deb_leave(LBS_DEB_CMD);
+	lbs_deb_leave(LBS_DEB_HOST);
 	return 0;
 }
 
@@ -1540,6 +1546,8 @@ struct cmd_ctrl_node *libertas_get_free_cmd_ctrl_node(wlan_private * priv)
 	wlan_adapter *adapter = priv->adapter;
 	unsigned long flags;
 
+	lbs_deb_enter(LBS_DEB_HOST);
+
 	if (!adapter)
 		return NULL;
 
@@ -1549,21 +1557,16 @@ struct cmd_ctrl_node *libertas_get_free_cmd_ctrl_node(wlan_private * priv)
 		tempnode = (struct cmd_ctrl_node *)adapter->cmdfreeq.next;
 		list_del((struct list_head *)tempnode);
 	} else {
-		lbs_deb_cmd("GET_CMD_NODE: cmd_ctrl_node is not available\n");
+		lbs_deb_host("GET_CMD_NODE: cmd_ctrl_node is not available\n");
 		tempnode = NULL;
 	}
 
 	spin_unlock_irqrestore(&adapter->driver_lock, flags);
 
-	if (tempnode) {
-		/*
-		lbs_pr_debug(3, "GET_CMD_NODE: cmdCtrlNode available\n");
-		lbs_pr_debug(3, "GET_CMD_NODE: cmdCtrlNode Address = %p\n",
-		       tempnode);
-		*/
+	if (tempnode)
 		cleanup_cmdnode(tempnode);
-	}
 
+	lbs_deb_leave(LBS_DEB_HOST);
 	return tempnode;
 }
 
@@ -1575,6 +1578,8 @@ struct cmd_ctrl_node *libertas_get_free_cmd_ctrl_node(wlan_private * priv)
  */
 static void cleanup_cmdnode(struct cmd_ctrl_node *ptempnode)
 {
+	lbs_deb_enter(LBS_DEB_HOST);
+
 	if (!ptempnode)
 		return;
 	ptempnode->cmdwaitqwoken = 1;
@@ -1586,7 +1591,8 @@ static void cleanup_cmdnode(struct cmd_ctrl_node *ptempnode)
 
 	if (ptempnode->bufvirtualaddr != NULL)
 		memset(ptempnode->bufvirtualaddr, 0, MRVDRV_SIZE_OF_CMD_BUFFER);
-	return;
+
+	lbs_deb_leave(LBS_DEB_HOST);
 }
 
 /**
@@ -1603,7 +1609,7 @@ void libertas_set_cmd_ctrl_node(wlan_private * priv,
 		    struct cmd_ctrl_node *ptempnode,
 		    u32 cmd_oid, u16 wait_option, void *pdata_buf)
 {
-	lbs_deb_enter(LBS_DEB_CMD);
+	lbs_deb_enter(LBS_DEB_HOST);
 
 	if (!ptempnode)
 		return;
@@ -1612,7 +1618,7 @@ void libertas_set_cmd_ctrl_node(wlan_private * priv,
 	ptempnode->wait_option = wait_option;
 	ptempnode->pdata_buf = pdata_buf;
 
-	lbs_deb_leave(LBS_DEB_CMD);
+	lbs_deb_leave(LBS_DEB_HOST);
 }
 
 /**
@@ -1631,12 +1637,15 @@ int libertas_execute_next_command(wlan_private * priv)
 	unsigned long flags;
 	int ret = 0;
 
-	lbs_deb_enter(LBS_DEB_CMD);
+	// Debug group is LBS_DEB_THREAD and not LBS_DEB_HOST, because the
+	// only caller to us is libertas_thread() and we get even when a
+	// data packet is received
+	lbs_deb_enter(LBS_DEB_THREAD);
 
 	spin_lock_irqsave(&adapter->driver_lock, flags);
 
 	if (adapter->cur_cmd) {
-		lbs_pr_alert( "EXEC_NEXT_CMD: there is command in processing!\n");
+		lbs_pr_alert( "EXEC_NEXT_CMD: already processing command!\n");
 		spin_unlock_irqrestore(&adapter->driver_lock, flags);
 		ret = -1;
 		goto done;
@@ -1650,22 +1659,20 @@ int libertas_execute_next_command(wlan_private * priv)
 	spin_unlock_irqrestore(&adapter->driver_lock, flags);
 
 	if (cmdnode) {
-		lbs_deb_cmd(
-		       "EXEC_NEXT_CMD: Got next command from cmdpendingq\n");
 		cmdptr = (struct cmd_ds_command *)cmdnode->bufvirtualaddr;
 
 		if (is_command_allowed_in_ps(cmdptr->command)) {
 			if ((adapter->psstate == PS_STATE_SLEEP) ||
 			    (adapter->psstate == PS_STATE_PRE_SLEEP)) {
-				lbs_deb_cmd(
-				       "EXEC_NEXT_CMD: Cannot send cmd 0x%x in psstate %d\n",
+				lbs_deb_host(
+				       "EXEC_NEXT_CMD: cannot send cmd 0x%04x in psstate %d\n",
 				       le16_to_cpu(cmdptr->command),
 				       adapter->psstate);
 				ret = -1;
 				goto done;
 			}
-			lbs_deb_cmd("EXEC_NEXT_CMD: OK to send command "
-			       "0x%x in psstate %d\n",
+			lbs_deb_host("EXEC_NEXT_CMD: OK to send command "
+			       "0x%04x in psstate %d\n",
 				    le16_to_cpu(cmdptr->command),
 				    adapter->psstate);
 		} else if (adapter->psstate != PS_STATE_FULL_POWER) {
@@ -1681,7 +1688,7 @@ int libertas_execute_next_command(wlan_private * priv)
 			 * immediately.
 			 */
 			if (cmdptr->command !=
-			    cpu_to_le16(cmd_802_11_ps_mode)) {
+			    cpu_to_le16(CMD_802_11_PS_MODE)) {
 				/*  Prepare to send Exit PS,
 				 *  this non PS command will be sent later */
 				if ((adapter->psstate == PS_STATE_SLEEP)
@@ -1703,13 +1710,13 @@ int libertas_execute_next_command(wlan_private * priv)
 				struct cmd_ds_802_11_ps_mode *psm =
 				    &cmdptr->params.psmode;
 
-				lbs_deb_cmd(
-				       "EXEC_NEXT_CMD: PS cmd- action=0x%x\n",
+				lbs_deb_host(
+				       "EXEC_NEXT_CMD: PS cmd, action 0x%02x\n",
 				       psm->action);
 				if (psm->action !=
-				    cpu_to_le16(cmd_subcmd_exit_ps)) {
-					lbs_deb_cmd(
-					       "EXEC_NEXT_CMD: Ignore Enter PS cmd\n");
+				    cpu_to_le16(CMD_SUBCMD_EXIT_PS)) {
+					lbs_deb_host(
+					       "EXEC_NEXT_CMD: ignore ENTER_PS cmd\n");
 					list_del((struct list_head *)cmdnode);
 					libertas_cleanup_and_insert_cmd(priv, cmdnode);
 
@@ -1719,8 +1726,8 @@ int libertas_execute_next_command(wlan_private * priv)
 
 				if ((adapter->psstate == PS_STATE_SLEEP) ||
 				    (adapter->psstate == PS_STATE_PRE_SLEEP)) {
-					lbs_deb_cmd(
-					       "EXEC_NEXT_CMD: Ignore ExitPS cmd in sleep\n");
+					lbs_deb_host(
+					       "EXEC_NEXT_CMD: ignore EXIT_PS cmd in sleep\n");
 					list_del((struct list_head *)cmdnode);
 					libertas_cleanup_and_insert_cmd(priv, cmdnode);
 					adapter->needtowakeup = 1;
@@ -1729,12 +1736,12 @@ int libertas_execute_next_command(wlan_private * priv)
 					goto done;
 				}
 
-				lbs_deb_cmd(
-				       "EXEC_NEXT_CMD: Sending Exit_PS down...\n");
+				lbs_deb_host(
+				       "EXEC_NEXT_CMD: sending EXIT_PS\n");
 			}
 		}
 		list_del((struct list_head *)cmdnode);
-		lbs_deb_cmd("EXEC_NEXT_CMD: Sending 0x%04X command\n",
+		lbs_deb_host("EXEC_NEXT_CMD: sending command 0x%04x\n",
 			    le16_to_cpu(cmdptr->command));
 		DownloadcommandToStation(priv, cmdnode);
 	} else {
@@ -1742,23 +1749,23 @@ int libertas_execute_next_command(wlan_private * priv)
 		 * check if in power save mode, if yes, put the device back
 		 * to PS mode
 		 */
-		if ((adapter->psmode != wlan802_11powermodecam) &&
+		if ((adapter->psmode != WLAN802_11POWERMODECAM) &&
 		    (adapter->psstate == PS_STATE_FULL_POWER) &&
-		    (adapter->connect_status == libertas_connected)) {
+		    (adapter->connect_status == LIBERTAS_CONNECTED)) {
 			if (adapter->secinfo.WPAenabled ||
 			    adapter->secinfo.WPA2enabled) {
 				/* check for valid WPA group keys */
 				if (adapter->wpa_mcast_key.len ||
 				    adapter->wpa_unicast_key.len) {
-					lbs_deb_cmd(
+					lbs_deb_host(
 					       "EXEC_NEXT_CMD: WPA enabled and GTK_SET"
 					       " go back to PS_SLEEP");
 					libertas_ps_sleep(priv, 0);
 				}
 			} else {
-				lbs_deb_cmd(
-				       "EXEC_NEXT_CMD: command PendQ is empty,"
-				       " go back to PS_SLEEP");
+				lbs_deb_host(
+				       "EXEC_NEXT_CMD: cmdpendingq empty, "
+				       "go back to PS_SLEEP");
 				libertas_ps_sleep(priv, 0);
 			}
 		}
@@ -1766,7 +1773,7 @@ int libertas_execute_next_command(wlan_private * priv)
 
 	ret = 0;
 done:
-	lbs_deb_leave(LBS_DEB_CMD);
+	lbs_deb_leave(LBS_DEB_THREAD);
 	return ret;
 }
 
@@ -1775,7 +1782,7 @@ void libertas_send_iwevcustom_event(wlan_private * priv, s8 * str)
 	union iwreq_data iwrq;
 	u8 buf[50];
 
-	lbs_deb_enter(LBS_DEB_CMD);
+	lbs_deb_enter(LBS_DEB_WEXT);
 
 	memset(&iwrq, 0, sizeof(union iwreq_data));
 	memset(buf, 0, sizeof(buf));
@@ -1785,13 +1792,13 @@ void libertas_send_iwevcustom_event(wlan_private * priv, s8 * str)
 	iwrq.data.length = strlen(buf) + 1 + IW_EV_LCP_LEN;
 
 	/* Send Event to upper layer */
-	lbs_deb_cmd("Event Indication string = %s\n", (char *)buf);
-	lbs_deb_cmd("Event Indication String length = %d\n", iwrq.data.length);
+	lbs_deb_wext("event indication string %s\n", (char *)buf);
+	lbs_deb_wext("event indication length %d\n", iwrq.data.length);
+	lbs_deb_wext("sending wireless event IWEVCUSTOM for %s\n", str);
 
-	lbs_deb_cmd("Sending wireless event IWEVCUSTOM for %s\n", str);
 	wireless_send_event(priv->dev, IWEVCUSTOM, &iwrq, buf);
 
-	lbs_deb_leave(LBS_DEB_CMD);
+	lbs_deb_leave(LBS_DEB_WEXT);
 }
 
 static int sendconfirmsleep(wlan_private * priv, u8 * cmdptr, u16 size)
@@ -1800,19 +1807,19 @@ static int sendconfirmsleep(wlan_private * priv, u8 * cmdptr, u16 size)
 	wlan_adapter *adapter = priv->adapter;
 	int ret = 0;
 
-	lbs_deb_enter(LBS_DEB_CMD);
+	lbs_deb_enter(LBS_DEB_HOST);
 
-	lbs_deb_cmd("SEND_SLEEPC_CMD: Before download, size of cmd = %d\n",
+	lbs_deb_host("SEND_SLEEPC_CMD: before download, cmd size %d\n",
 	       size);
 
-	lbs_dbg_hex("SEND_SLEEPC_CMD: Sleep confirm command", cmdptr, size);
+	lbs_deb_hex(LBS_DEB_HOST, "sleep confirm command", cmdptr, size);
 
 	ret = priv->hw_host_to_card(priv, MVMS_CMD, cmdptr, size);
 	priv->dnld_sent = DNLD_RES_RECEIVED;
 
 	spin_lock_irqsave(&adapter->driver_lock, flags);
 	if (adapter->intcounter || adapter->currenttxskb)
-		lbs_deb_cmd("SEND_SLEEPC_CMD: intcounter=%d currenttxskb=%p\n",
+		lbs_deb_host("SEND_SLEEPC_CMD: intcounter %d, currenttxskb %p\n",
 		       adapter->intcounter, adapter->currenttxskb);
 	spin_unlock_irqrestore(&adapter->driver_lock, flags);
 
@@ -1824,36 +1831,35 @@ static int sendconfirmsleep(wlan_private * priv, u8 * cmdptr, u16 size)
 		if (!adapter->intcounter) {
 			adapter->psstate = PS_STATE_SLEEP;
 		} else {
-			lbs_deb_cmd("SEND_SLEEPC_CMD: After sent,IntC=%d\n",
+			lbs_deb_host("SEND_SLEEPC_CMD: after sent, intcounter %d\n",
 			       adapter->intcounter);
 		}
 		spin_unlock_irqrestore(&adapter->driver_lock, flags);
 
-		lbs_deb_cmd("SEND_SLEEPC_CMD: Sent Confirm Sleep command\n");
-		lbs_deb_cmd("+");
+		lbs_deb_host("SEND_SLEEPC_CMD: sent confirm sleep\n");
 	}
 
-	lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
+	lbs_deb_leave_args(LBS_DEB_HOST, "ret %d", ret);
 	return ret;
 }
 
 void libertas_ps_sleep(wlan_private * priv, int wait_option)
 {
-	lbs_deb_enter(LBS_DEB_CMD);
+	lbs_deb_enter(LBS_DEB_HOST);
 
 	/*
 	 * PS is currently supported only in Infrastructure mode
 	 * Remove this check if it is to be supported in IBSS mode also
 	 */
 
-	libertas_prepare_and_send_command(priv, cmd_802_11_ps_mode,
-			      cmd_subcmd_enter_ps, wait_option, 0, NULL);
+	libertas_prepare_and_send_command(priv, CMD_802_11_PS_MODE,
+			      CMD_SUBCMD_ENTER_PS, wait_option, 0, NULL);
 
-	lbs_deb_leave(LBS_DEB_CMD);
+	lbs_deb_leave(LBS_DEB_HOST);
 }
 
 /**
- *  @brief This function sends Eixt_PS command to firmware.
+ *  @brief This function sends Exit_PS command to firmware.
  *
  *  @param priv    	A pointer to wlan_private structure
  *  @param wait_option	wait response or not
@@ -1863,17 +1869,15 @@ void libertas_ps_wakeup(wlan_private * priv, int wait_option)
 {
 	__le32 Localpsmode;
 
-	lbs_deb_enter(LBS_DEB_CMD);
-
-	Localpsmode = cpu_to_le32(wlan802_11powermodecam);
+	lbs_deb_enter(LBS_DEB_HOST);
 
-	lbs_deb_cmd("Exit_PS: Localpsmode = %d\n", wlan802_11powermodecam);
+	Localpsmode = cpu_to_le32(WLAN802_11POWERMODECAM);
 
-	libertas_prepare_and_send_command(priv, cmd_802_11_ps_mode,
-			      cmd_subcmd_exit_ps,
+	libertas_prepare_and_send_command(priv, CMD_802_11_PS_MODE,
+			      CMD_SUBCMD_EXIT_PS,
 			      wait_option, 0, &Localpsmode);
 
-	lbs_deb_leave(LBS_DEB_CMD);
+	lbs_deb_leave(LBS_DEB_HOST);
 }
 
 /**
@@ -1890,31 +1894,31 @@ void libertas_ps_confirm_sleep(wlan_private * priv, u16 psmode)
 	wlan_adapter *adapter = priv->adapter;
 	u8 allowed = 1;
 
-	lbs_deb_enter(LBS_DEB_CMD);
+	lbs_deb_enter(LBS_DEB_HOST);
 
 	if (priv->dnld_sent) {
 		allowed = 0;
-		lbs_deb_cmd("D");
+		lbs_deb_host("dnld_sent was set");
 	}
 
 	spin_lock_irqsave(&adapter->driver_lock, flags);
 	if (adapter->cur_cmd) {
 		allowed = 0;
-		lbs_deb_cmd("C");
+		lbs_deb_host("cur_cmd was set");
 	}
 	if (adapter->intcounter > 0) {
 		allowed = 0;
-		lbs_deb_cmd("I%d", adapter->intcounter);
+		lbs_deb_host("intcounter %d", adapter->intcounter);
 	}
 	spin_unlock_irqrestore(&adapter->driver_lock, flags);
 
 	if (allowed) {
-		lbs_deb_cmd("Sending libertas_ps_confirm_sleep\n");
+		lbs_deb_host("sending libertas_ps_confirm_sleep\n");
 		sendconfirmsleep(priv, (u8 *) & adapter->libertas_ps_confirm_sleep,
 				 sizeof(struct PS_CMD_ConfirmSleep));
 	} else {
-		lbs_deb_cmd("Sleep Confirm has been delayed\n");
+		lbs_deb_host("sleep confirm has been delayed\n");
 	}
 
-	lbs_deb_leave(LBS_DEB_CMD);
+	lbs_deb_leave(LBS_DEB_HOST);
 }
diff --git a/drivers/net/wireless/libertas/cmdresp.c b/drivers/net/wireless/libertas/cmdresp.c
index 6ac0d4752fa4..8f90892ea22e 100644
--- a/drivers/net/wireless/libertas/cmdresp.c
+++ b/drivers/net/wireless/libertas/cmdresp.c
@@ -28,10 +28,10 @@ void libertas_mac_event_disconnected(wlan_private * priv)
 	wlan_adapter *adapter = priv->adapter;
 	union iwreq_data wrqu;
 
-	if (adapter->connect_status != libertas_connected)
+	if (adapter->connect_status != LIBERTAS_CONNECTED)
 		return;
 
-	lbs_deb_cmd("Handles disconnect event.\n");
+	lbs_deb_enter(LBS_DEB_CMD);
 
 	memset(wrqu.ap_addr.sa_data, 0x00, ETH_ALEN);
 	wrqu.ap_addr.sa_family = ARPHRD_ETHER;
@@ -60,22 +60,12 @@ void libertas_mac_event_disconnected(wlan_private * priv)
 	memset(adapter->rawNF, 0x00, sizeof(adapter->rawNF));
 	adapter->nextSNRNF = 0;
 	adapter->numSNRNF = 0;
-	adapter->rxpd_rate = 0;
-	lbs_deb_cmd("Current SSID='%s', ssid length=%u\n",
+	lbs_deb_cmd("current SSID '%s', length %u\n",
 	            escape_essid(adapter->curbssparams.ssid,
 	                         adapter->curbssparams.ssid_len),
 	            adapter->curbssparams.ssid_len);
-	lbs_deb_cmd("Previous SSID='%s', ssid length=%u\n",
-	            escape_essid(adapter->prev_ssid, adapter->prev_ssid_len),
-	            adapter->prev_ssid_len);
-
-	adapter->connect_status = libertas_disconnected;
 
-	/* Save previous SSID and BSSID for possible reassociation */
-	memcpy(&adapter->prev_ssid, &adapter->curbssparams.ssid,
-	       IW_ESSID_MAX_SIZE);
-	adapter->prev_ssid_len = adapter->curbssparams.ssid_len;
-	memcpy(adapter->prev_bssid, adapter->curbssparams.bssid, ETH_ALEN);
+	adapter->connect_status = LIBERTAS_DISCONNECTED;
 
 	/* Clear out associated SSID and BSSID since connection is
 	 * no longer valid.
@@ -86,9 +76,10 @@ void libertas_mac_event_disconnected(wlan_private * priv)
 
 	if (adapter->psstate != PS_STATE_FULL_POWER) {
 		/* make firmware to exit PS mode */
-		lbs_deb_cmd("Disconnected, so exit PS mode.\n");
+		lbs_deb_cmd("disconnected, so exit PS mode\n");
 		libertas_ps_wakeup(priv, 0);
 	}
+	lbs_deb_leave(LBS_DEB_CMD);
 }
 
 /**
@@ -102,6 +93,7 @@ static void handle_mic_failureevent(wlan_private * priv, u32 event)
 {
 	char buf[50];
 
+	lbs_deb_enter(LBS_DEB_CMD);
 	memset(buf, 0, sizeof(buf));
 
 	sprintf(buf, "%s", "MLME-MICHAELMICFAILURE.indication ");
@@ -113,6 +105,7 @@ static void handle_mic_failureevent(wlan_private * priv, u32 event)
 	}
 
 	libertas_send_iwevcustom_event(priv, buf);
+	lbs_deb_leave(LBS_DEB_CMD);
 }
 
 static int wlan_ret_reg_access(wlan_private * priv,
@@ -124,7 +117,7 @@ static int wlan_ret_reg_access(wlan_private * priv,
 	lbs_deb_enter(LBS_DEB_CMD);
 
 	switch (type) {
-	case cmd_ret_mac_reg_access:
+	case CMD_RET(CMD_MAC_REG_ACCESS):
 		{
 			struct cmd_ds_mac_reg_access *reg = &resp->params.macreg;
 
@@ -133,7 +126,7 @@ static int wlan_ret_reg_access(wlan_private * priv,
 			break;
 		}
 
-	case cmd_ret_bbp_reg_access:
+	case CMD_RET(CMD_BBP_REG_ACCESS):
 		{
 			struct cmd_ds_bbp_reg_access *reg = &resp->params.bbpreg;
 
@@ -142,7 +135,7 @@ static int wlan_ret_reg_access(wlan_private * priv,
 			break;
 		}
 
-	case cmd_ret_rf_reg_access:
+	case CMD_RET(CMD_RF_REG_ACCESS):
 		{
 			struct cmd_ds_rf_reg_access *reg = &resp->params.rfreg;
 
@@ -155,7 +148,7 @@ static int wlan_ret_reg_access(wlan_private * priv,
 		ret = -1;
 	}
 
-	lbs_deb_enter_args(LBS_DEB_CMD, "ret %d", ret);
+	lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
 	return ret;
 }
 
@@ -166,6 +159,7 @@ static int wlan_ret_get_hw_spec(wlan_private * priv,
 	struct cmd_ds_get_hw_spec *hwspec = &resp->params.hwspec;
 	wlan_adapter *adapter = priv->adapter;
 	int ret = 0;
+	DECLARE_MAC_BUF(mac);
 
 	lbs_deb_enter(LBS_DEB_CMD);
 
@@ -173,22 +167,23 @@ static int wlan_ret_get_hw_spec(wlan_private * priv,
 
 	memcpy(adapter->fwreleasenumber, hwspec->fwreleasenumber, 4);
 
-	lbs_deb_cmd("GET_HW_SPEC: FWReleaseVersion- %u.%u.%u.p%u\n",
+	lbs_deb_cmd("GET_HW_SPEC: firmware release %u.%u.%up%u\n",
 		    adapter->fwreleasenumber[2], adapter->fwreleasenumber[1],
 		    adapter->fwreleasenumber[0], adapter->fwreleasenumber[3]);
-	lbs_deb_cmd("GET_HW_SPEC: Permanent addr- %2x:%2x:%2x:%2x:%2x:%2x\n",
-	       hwspec->permanentaddr[0], hwspec->permanentaddr[1],
-	       hwspec->permanentaddr[2], hwspec->permanentaddr[3],
-	       hwspec->permanentaddr[4], hwspec->permanentaddr[5]);
-	lbs_deb_cmd("GET_HW_SPEC: hwifversion=0x%X  version=0x%X\n",
+	lbs_deb_cmd("GET_HW_SPEC: MAC addr %s\n",
+		    print_mac(mac, hwspec->permanentaddr));
+	lbs_deb_cmd("GET_HW_SPEC: hardware interface 0x%x, hardware spec 0x%04x\n",
 	       hwspec->hwifversion, hwspec->version);
 
-	adapter->regioncode = le16_to_cpu(hwspec->regioncode);
+	/* Clamp region code to 8-bit since FW spec indicates that it should
+	 * only ever be 8-bit, even though the field size is 16-bit.  Some firmware
+	 * returns non-zero high 8 bits here.
+	 */
+	adapter->regioncode = le16_to_cpu(hwspec->regioncode) & 0xFF;
 
 	for (i = 0; i < MRVDRV_MAX_REGION_CODE; i++) {
 		/* use the region code to search for the index */
 		if (adapter->regioncode == libertas_region_code_to_index[i]) {
-			adapter->regiontableindex = (u16) i;
 			break;
 		}
 	}
@@ -196,7 +191,6 @@ static int wlan_ret_get_hw_spec(wlan_private * priv,
 	/* if it's unidentified region code, use the default (USA) */
 	if (i >= MRVDRV_MAX_REGION_CODE) {
 		adapter->regioncode = 0x10;
-		adapter->regiontableindex = 0;
 		lbs_pr_info("unidentified region code; using the default (USA)\n");
 	}
 
@@ -230,8 +224,8 @@ static int wlan_ret_802_11_sleep_params(wlan_private * priv,
 
 	lbs_deb_enter(LBS_DEB_CMD);
 
-	lbs_deb_cmd("error=%x offset=%x stabletime=%x calcontrol=%x\n"
-		    " extsleepclk=%x\n", le16_to_cpu(sp->error),
+	lbs_deb_cmd("error 0x%x, offset 0x%x, stabletime 0x%x, calcontrol 0x%x "
+		    "extsleepclk 0x%x\n", le16_to_cpu(sp->error),
 		    le16_to_cpu(sp->offset), le16_to_cpu(sp->stabletime),
 		    sp->calcontrol, sp->externalsleepclk);
 
@@ -249,6 +243,7 @@ static int wlan_ret_802_11_sleep_params(wlan_private * priv,
 static int wlan_ret_802_11_stat(wlan_private * priv,
 				struct cmd_ds_command *resp)
 {
+	lbs_deb_enter(LBS_DEB_CMD);
 /*	currently adapter->wlan802_11Stat is unused
 
 	struct cmd_ds_802_11_get_stat *p11Stat = &resp->params.gstat;
@@ -258,6 +253,7 @@ static int wlan_ret_802_11_stat(wlan_private * priv,
 	memcpy(&adapter->wlan802_11Stat,
 	       p11Stat, sizeof(struct cmd_ds_802_11_get_stat));
 */
+	lbs_deb_leave(LBS_DEB_CMD);
 	return 0;
 }
 
@@ -270,28 +266,28 @@ static int wlan_ret_802_11_snmp_mib(wlan_private * priv,
 
 	lbs_deb_enter(LBS_DEB_CMD);
 
-	lbs_deb_cmd("SNMP_RESP: value of the oid = %x, querytype=%x\n", oid,
+	lbs_deb_cmd("SNMP_RESP: oid 0x%x, querytype 0x%x\n", oid,
 	       querytype);
-	lbs_deb_cmd("SNMP_RESP: Buf size  = %x\n", le16_to_cpu(smib->bufsize));
+	lbs_deb_cmd("SNMP_RESP: Buf size %d\n", le16_to_cpu(smib->bufsize));
 
-	if (querytype == cmd_act_get) {
+	if (querytype == CMD_ACT_GET) {
 		switch (oid) {
-		case fragthresh_i:
+		case FRAGTHRESH_I:
 			priv->adapter->fragthsd =
 				le16_to_cpu(*((__le16 *)(smib->value)));
-			lbs_deb_cmd("SNMP_RESP: fragthsd =%u\n",
+			lbs_deb_cmd("SNMP_RESP: frag threshold %u\n",
 				    priv->adapter->fragthsd);
 			break;
-		case rtsthresh_i:
+		case RTSTHRESH_I:
 			priv->adapter->rtsthsd =
 				le16_to_cpu(*((__le16 *)(smib->value)));
-			lbs_deb_cmd("SNMP_RESP: rtsthsd =%u\n",
+			lbs_deb_cmd("SNMP_RESP: rts threshold %u\n",
 				    priv->adapter->rtsthsd);
 			break;
-		case short_retrylim_i:
+		case SHORT_RETRYLIM_I:
 			priv->adapter->txretrycount =
 				le16_to_cpu(*((__le16 *)(smib->value)));
-			lbs_deb_cmd("SNMP_RESP: txretrycount =%u\n",
+			lbs_deb_cmd("SNMP_RESP: tx retry count %u\n",
 				    priv->adapter->rtsthsd);
 			break;
 		default:
@@ -314,18 +310,19 @@ static int wlan_ret_802_11_key_material(wlan_private * priv,
 	lbs_deb_enter(LBS_DEB_CMD);
 
 	/* Copy the returned key to driver private data */
-	if (action == cmd_act_get) {
+	if (action == CMD_ACT_GET) {
 		u8 * buf_ptr = (u8 *) &pkeymaterial->keyParamSet;
 		u8 * resp_end = (u8 *) (resp + le16_to_cpu(resp->size));
 
 		while (buf_ptr < resp_end) {
 			struct MrvlIEtype_keyParamSet * pkeyparamset =
 			    (struct MrvlIEtype_keyParamSet *) buf_ptr;
-			struct WLAN_802_11_KEY * pkey;
-			u16 key_info = le16_to_cpu(pkeyparamset->keyinfo);
+			struct enc_key * pkey;
 			u16 param_set_len = le16_to_cpu(pkeyparamset->length);
-			u8 * end;
 			u16 key_len = le16_to_cpu(pkeyparamset->keylen);
+			u16 key_flags = le16_to_cpu(pkeyparamset->keyinfo);
+			u16 key_type = le16_to_cpu(pkeyparamset->keytypeid);
+			u8 * end;
 
 			end = (u8 *) pkeyparamset + sizeof (pkeyparamset->type)
 			                          + sizeof (pkeyparamset->length)
@@ -334,20 +331,20 @@ static int wlan_ret_802_11_key_material(wlan_private * priv,
 			if (end > resp_end)
 				break;
 
-			if (key_info & KEY_INFO_WPA_UNICAST)
+			if (key_flags & KEY_INFO_WPA_UNICAST)
 				pkey = &adapter->wpa_unicast_key;
-			else if (key_info & KEY_INFO_WPA_MCAST)
+			else if (key_flags & KEY_INFO_WPA_MCAST)
 				pkey = &adapter->wpa_mcast_key;
 			else
 				break;
 
 			/* Copy returned key into driver */
-			memset(pkey, 0, sizeof(struct WLAN_802_11_KEY));
+			memset(pkey, 0, sizeof(struct enc_key));
 			if (key_len > sizeof(pkey->key))
 				break;
-			pkey->type = le16_to_cpu(pkeyparamset->keytypeid);
-			pkey->flags = le16_to_cpu(pkeyparamset->keyinfo);
-			pkey->len = le16_to_cpu(pkeyparamset->keylen);
+			pkey->type = key_type;
+			pkey->flags = key_flags;
+			pkey->len = key_len;
 			memcpy(pkey->key, pkeyparamset->key, pkey->len);
 
 			buf_ptr = end + 1;
@@ -382,28 +379,9 @@ static int wlan_ret_802_11_rf_tx_power(wlan_private * priv,
 
 	adapter->txpowerlevel = le16_to_cpu(rtp->currentlevel);
 
-	lbs_deb_cmd("Current TxPower Level = %d\n", adapter->txpowerlevel);
-
-	lbs_deb_enter(LBS_DEB_CMD);
-	return 0;
-}
-
-static int wlan_ret_802_11_rf_antenna(wlan_private * priv,
-				      struct cmd_ds_command *resp)
-{
-	struct cmd_ds_802_11_rf_antenna *pAntenna = &resp->params.rant;
-	wlan_adapter *adapter = priv->adapter;
-	u16 action = le16_to_cpu(pAntenna->action);
-
-	if (action == cmd_act_get_rx)
-		adapter->rxantennamode = le16_to_cpu(pAntenna->antennamode);
-
-	if (action == cmd_act_get_tx)
-		adapter->txantennamode = le16_to_cpu(pAntenna->antennamode);
-
-	lbs_deb_cmd("RF_ANT_RESP: action = 0x%x, mode = 0x%04x\n",
-	       action, le16_to_cpu(pAntenna->antennamode));
+	lbs_deb_cmd("TX power currently %d\n", adapter->txpowerlevel);
 
+	lbs_deb_leave(LBS_DEB_CMD);
 	return 0;
 }
 
@@ -415,12 +393,12 @@ static int wlan_ret_802_11_rate_adapt_rateset(wlan_private * priv,
 
 	lbs_deb_enter(LBS_DEB_CMD);
 
-	if (rates->action == cmd_act_get) {
+	if (rates->action == CMD_ACT_GET) {
 		adapter->enablehwauto = le16_to_cpu(rates->enablehwauto);
 		adapter->ratebitmap = le16_to_cpu(rates->bitmap);
 	}
 
-	lbs_deb_enter(LBS_DEB_CMD);
+	lbs_deb_leave(LBS_DEB_CMD);
 	return 0;
 }
 
@@ -429,21 +407,19 @@ static int wlan_ret_802_11_data_rate(wlan_private * priv,
 {
 	struct cmd_ds_802_11_data_rate *pdatarate = &resp->params.drate;
 	wlan_adapter *adapter = priv->adapter;
-	u8 dot11datarate;
 
 	lbs_deb_enter(LBS_DEB_CMD);
 
-	lbs_dbg_hex("DATA_RATE_RESP: data_rate- ",
-		(u8 *) pdatarate, sizeof(struct cmd_ds_802_11_data_rate));
+	lbs_deb_hex(LBS_DEB_CMD, "DATA_RATE_RESP", (u8 *) pdatarate,
+		sizeof(struct cmd_ds_802_11_data_rate));
 
-	dot11datarate = pdatarate->datarate[0];
-	if (pdatarate->action == cpu_to_le16(cmd_act_get_tx_rate)) {
-		memcpy(adapter->libertas_supported_rates, pdatarate->datarate,
-		       sizeof(adapter->libertas_supported_rates));
-	}
-	adapter->datarate = libertas_index_to_data_rate(dot11datarate);
+	/* FIXME: get actual rates FW can do if this command actually returns
+	 * all data rates supported.
+	 */
+	adapter->cur_rate = libertas_fw_index_to_data_rate(pdatarate->rates[0]);
+	lbs_deb_cmd("DATA_RATE: current rate 0x%02x\n", adapter->cur_rate);
 
-	lbs_deb_enter(LBS_DEB_CMD);
+	lbs_deb_leave(LBS_DEB_CMD);
 	return 0;
 }
 
@@ -457,9 +433,9 @@ static int wlan_ret_802_11_rf_channel(wlan_private * priv,
 
 	lbs_deb_enter(LBS_DEB_CMD);
 
-	if (action == cmd_opt_802_11_rf_channel_get
+	if (action == CMD_OPT_802_11_RF_CHANNEL_GET
 	    && adapter->curbssparams.channel != newchannel) {
-		lbs_deb_cmd("channel Switch: %d to %d\n",
+		lbs_deb_cmd("channel switch from %d to %d\n",
 		       adapter->curbssparams.channel, newchannel);
 
 		/* Update the channel again */
@@ -476,6 +452,8 @@ static int wlan_ret_802_11_rssi(wlan_private * priv,
 	struct cmd_ds_802_11_rssi_rsp *rssirsp = &resp->params.rssirsp;
 	wlan_adapter *adapter = priv->adapter;
 
+	lbs_deb_enter(LBS_DEB_CMD);
+
 	/* store the non average value */
 	adapter->SNR[TYPE_BEACON][TYPE_NOAVG] = le16_to_cpu(rssirsp->SNR);
 	adapter->NF[TYPE_BEACON][TYPE_NOAVG] = le16_to_cpu(rssirsp->noisefloor);
@@ -491,9 +469,11 @@ static int wlan_ret_802_11_rssi(wlan_private * priv,
 	    CAL_RSSI(adapter->SNR[TYPE_BEACON][TYPE_AVG] / AVG_SCALE,
 		     adapter->NF[TYPE_BEACON][TYPE_AVG] / AVG_SCALE);
 
-	lbs_deb_cmd("Beacon RSSI value = 0x%x\n",
+	lbs_deb_cmd("RSSI: beacon %d, avg %d\n",
+	       adapter->RSSI[TYPE_BEACON][TYPE_NOAVG],
 	       adapter->RSSI[TYPE_BEACON][TYPE_AVG]);
 
+	lbs_deb_leave(LBS_DEB_CMD);
 	return 0;
 }
 
@@ -504,11 +484,11 @@ static int wlan_ret_802_11_eeprom_access(wlan_private * priv,
 	struct wlan_ioctl_regrdwr *pbuf;
 	pbuf = (struct wlan_ioctl_regrdwr *) adapter->prdeeprom;
 
-	lbs_deb_cmd("eeprom read len=%x\n",
+	lbs_deb_enter_args(LBS_DEB_CMD, "len %d",
 	       le16_to_cpu(resp->params.rdeeprom.bytecount));
 	if (pbuf->NOB < le16_to_cpu(resp->params.rdeeprom.bytecount)) {
 		pbuf->NOB = 0;
-		lbs_deb_cmd("eeprom read return length is too big\n");
+		lbs_deb_cmd("EEPROM read length too big\n");
 		return -1;
 	}
 	pbuf->NOB = le16_to_cpu(resp->params.rdeeprom.bytecount);
@@ -516,9 +496,10 @@ static int wlan_ret_802_11_eeprom_access(wlan_private * priv,
 
 		memcpy(&pbuf->value, (u8 *) & resp->params.rdeeprom.value,
 		       le16_to_cpu(resp->params.rdeeprom.bytecount));
-		lbs_dbg_hex("adapter", (char *)&pbuf->value,
+		lbs_deb_hex(LBS_DEB_CMD, "EEPROM", (char *)&pbuf->value,
 			le16_to_cpu(resp->params.rdeeprom.bytecount));
 	}
+	lbs_deb_leave(LBS_DEB_CMD);
 	return 0;
 }
 
@@ -533,7 +514,7 @@ static int wlan_ret_get_log(wlan_private * priv,
 	/* Stored little-endian */
 	memcpy(&adapter->logmsg, logmessage, sizeof(struct cmd_ds_802_11_get_log));
 
-	lbs_deb_enter(LBS_DEB_CMD);
+	lbs_deb_leave(LBS_DEB_CMD);
 	return 0;
 }
 
@@ -546,12 +527,12 @@ static int libertas_ret_802_11_enable_rsn(wlan_private * priv,
 
 	lbs_deb_enter(LBS_DEB_CMD);
 
-	if (enable_rsn->action == cpu_to_le16(cmd_act_get)) {
+	if (enable_rsn->action == cpu_to_le16(CMD_ACT_GET)) {
 		if (pdata_buf)
 			*pdata_buf = (u32) le16_to_cpu(enable_rsn->enable);
 	}
 
-	lbs_deb_enter(LBS_DEB_CMD);
+	lbs_deb_leave(LBS_DEB_CMD);
 	return 0;
 }
 
@@ -563,135 +544,134 @@ static inline int handle_cmd_response(u16 respcmd,
 	unsigned long flags;
 	wlan_adapter *adapter = priv->adapter;
 
+	lbs_deb_enter(LBS_DEB_HOST);
+
 	switch (respcmd) {
-	case cmd_ret_mac_reg_access:
-	case cmd_ret_bbp_reg_access:
-	case cmd_ret_rf_reg_access:
+	case CMD_RET(CMD_MAC_REG_ACCESS):
+	case CMD_RET(CMD_BBP_REG_ACCESS):
+	case CMD_RET(CMD_RF_REG_ACCESS):
 		ret = wlan_ret_reg_access(priv, respcmd, resp);
 		break;
 
-	case cmd_ret_hw_spec_info:
+	case CMD_RET(CMD_GET_HW_SPEC):
 		ret = wlan_ret_get_hw_spec(priv, resp);
 		break;
 
-	case cmd_ret_802_11_scan:
+	case CMD_RET(CMD_802_11_SCAN):
 		ret = libertas_ret_80211_scan(priv, resp);
 		break;
 
-	case cmd_ret_802_11_get_log:
+	case CMD_RET(CMD_802_11_GET_LOG):
 		ret = wlan_ret_get_log(priv, resp);
 		break;
 
-	case cmd_ret_802_11_associate:
-	case cmd_ret_802_11_reassociate:
+	case CMD_RET_802_11_ASSOCIATE:
+	case CMD_RET(CMD_802_11_ASSOCIATE):
+	case CMD_RET(CMD_802_11_REASSOCIATE):
 		ret = libertas_ret_80211_associate(priv, resp);
 		break;
 
-	case cmd_ret_802_11_disassociate:
-	case cmd_ret_802_11_deauthenticate:
+	case CMD_RET(CMD_802_11_DISASSOCIATE):
+	case CMD_RET(CMD_802_11_DEAUTHENTICATE):
 		ret = libertas_ret_80211_disassociate(priv, resp);
 		break;
 
-	case cmd_ret_802_11_ad_hoc_start:
-	case cmd_ret_802_11_ad_hoc_join:
+	case CMD_RET(CMD_802_11_AD_HOC_START):
+	case CMD_RET(CMD_802_11_AD_HOC_JOIN):
 		ret = libertas_ret_80211_ad_hoc_start(priv, resp);
 		break;
 
-	case cmd_ret_802_11_stat:
+	case CMD_RET(CMD_802_11_GET_STAT):
 		ret = wlan_ret_802_11_stat(priv, resp);
 		break;
 
-	case cmd_ret_802_11_snmp_mib:
+	case CMD_RET(CMD_802_11_SNMP_MIB):
 		ret = wlan_ret_802_11_snmp_mib(priv, resp);
 		break;
 
-	case cmd_ret_802_11_rf_tx_power:
+	case CMD_RET(CMD_802_11_RF_TX_POWER):
 		ret = wlan_ret_802_11_rf_tx_power(priv, resp);
 		break;
 
-	case cmd_ret_802_11_set_afc:
-	case cmd_ret_802_11_get_afc:
+	case CMD_RET(CMD_802_11_SET_AFC):
+	case CMD_RET(CMD_802_11_GET_AFC):
 		spin_lock_irqsave(&adapter->driver_lock, flags);
 		memmove(adapter->cur_cmd->pdata_buf, &resp->params.afc,
 			sizeof(struct cmd_ds_802_11_afc));
 		spin_unlock_irqrestore(&adapter->driver_lock, flags);
 
 		break;
-	case cmd_ret_802_11_rf_antenna:
-		ret = wlan_ret_802_11_rf_antenna(priv, resp);
-		break;
 
-	case cmd_ret_mac_multicast_adr:
-	case cmd_ret_mac_control:
-	case cmd_ret_802_11_set_wep:
-	case cmd_ret_802_11_reset:
-	case cmd_ret_802_11_authenticate:
-	case cmd_ret_802_11_radio_control:
-	case cmd_ret_802_11_beacon_stop:
+	case CMD_RET(CMD_MAC_MULTICAST_ADR):
+	case CMD_RET(CMD_MAC_CONTROL):
+	case CMD_RET(CMD_802_11_SET_WEP):
+	case CMD_RET(CMD_802_11_RESET):
+	case CMD_RET(CMD_802_11_AUTHENTICATE):
+	case CMD_RET(CMD_802_11_RADIO_CONTROL):
+	case CMD_RET(CMD_802_11_BEACON_STOP):
 		break;
 
-	case cmd_ret_802_11_enable_rsn:
+	case CMD_RET(CMD_802_11_ENABLE_RSN):
 		ret = libertas_ret_802_11_enable_rsn(priv, resp);
 		break;
 
-	case cmd_ret_802_11_data_rate:
+	case CMD_RET(CMD_802_11_DATA_RATE):
 		ret = wlan_ret_802_11_data_rate(priv, resp);
 		break;
-	case cmd_ret_802_11_rate_adapt_rateset:
+	case CMD_RET(CMD_802_11_RATE_ADAPT_RATESET):
 		ret = wlan_ret_802_11_rate_adapt_rateset(priv, resp);
 		break;
-	case cmd_ret_802_11_rf_channel:
+	case CMD_RET(CMD_802_11_RF_CHANNEL):
 		ret = wlan_ret_802_11_rf_channel(priv, resp);
 		break;
 
-	case cmd_ret_802_11_rssi:
+	case CMD_RET(CMD_802_11_RSSI):
 		ret = wlan_ret_802_11_rssi(priv, resp);
 		break;
 
-	case cmd_ret_802_11_mac_address:
+	case CMD_RET(CMD_802_11_MAC_ADDRESS):
 		ret = wlan_ret_802_11_mac_address(priv, resp);
 		break;
 
-	case cmd_ret_802_11_ad_hoc_stop:
+	case CMD_RET(CMD_802_11_AD_HOC_STOP):
 		ret = libertas_ret_80211_ad_hoc_stop(priv, resp);
 		break;
 
-	case cmd_ret_802_11_key_material:
-		lbs_deb_cmd("CMD_RESP: KEY_MATERIAL command response\n");
+	case CMD_RET(CMD_802_11_KEY_MATERIAL):
 		ret = wlan_ret_802_11_key_material(priv, resp);
 		break;
 
-	case cmd_ret_802_11_eeprom_access:
+	case CMD_RET(CMD_802_11_EEPROM_ACCESS):
 		ret = wlan_ret_802_11_eeprom_access(priv, resp);
 		break;
 
-	case cmd_ret_802_11d_domain_info:
+	case CMD_RET(CMD_802_11D_DOMAIN_INFO):
 		ret = libertas_ret_802_11d_domain_info(priv, resp);
 		break;
 
-	case cmd_ret_802_11_sleep_params:
+	case CMD_RET(CMD_802_11_SLEEP_PARAMS):
 		ret = wlan_ret_802_11_sleep_params(priv, resp);
 		break;
-	case cmd_ret_802_11_inactivity_timeout:
+	case CMD_RET(CMD_802_11_INACTIVITY_TIMEOUT):
 		spin_lock_irqsave(&adapter->driver_lock, flags);
 		*((u16 *) adapter->cur_cmd->pdata_buf) =
 		    le16_to_cpu(resp->params.inactivity_timeout.timeout);
 		spin_unlock_irqrestore(&adapter->driver_lock, flags);
 		break;
 
-	case cmd_ret_802_11_tpc_cfg:
+	case CMD_RET(CMD_802_11_TPC_CFG):
 		spin_lock_irqsave(&adapter->driver_lock, flags);
 		memmove(adapter->cur_cmd->pdata_buf, &resp->params.tpccfg,
 			sizeof(struct cmd_ds_802_11_tpc_cfg));
 		spin_unlock_irqrestore(&adapter->driver_lock, flags);
 		break;
-	case cmd_ret_802_11_led_gpio_ctrl:
+	case CMD_RET(CMD_802_11_LED_GPIO_CTRL):
 		spin_lock_irqsave(&adapter->driver_lock, flags);
 		memmove(adapter->cur_cmd->pdata_buf, &resp->params.ledgpio,
 			sizeof(struct cmd_ds_802_11_led_ctrl));
 		spin_unlock_irqrestore(&adapter->driver_lock, flags);
 		break;
-	case cmd_ret_802_11_pwr_cfg:
+	case CMD_RET(CMD_802_11_PWR_CFG):
 		spin_lock_irqsave(&adapter->driver_lock, flags);
 		memmove(adapter->cur_cmd->pdata_buf, &resp->params.pwrcfg,
 			sizeof(struct cmd_ds_802_11_pwr_cfg));
@@ -699,39 +679,37 @@ static inline int handle_cmd_response(u16 respcmd,
 
 		break;
 
-	case cmd_ret_get_tsf:
+	case CMD_RET(CMD_GET_TSF):
 		spin_lock_irqsave(&adapter->driver_lock, flags);
 		memcpy(priv->adapter->cur_cmd->pdata_buf,
 		       &resp->params.gettsf.tsfvalue, sizeof(u64));
 		spin_unlock_irqrestore(&adapter->driver_lock, flags);
 		break;
-	case cmd_ret_bt_access:
+	case CMD_RET(CMD_BT_ACCESS):
 		spin_lock_irqsave(&adapter->driver_lock, flags);
 		if (adapter->cur_cmd->pdata_buf)
 			memcpy(adapter->cur_cmd->pdata_buf,
 			       &resp->params.bt.addr1, 2 * ETH_ALEN);
 		spin_unlock_irqrestore(&adapter->driver_lock, flags);
 		break;
-	case cmd_ret_fwt_access:
+	case CMD_RET(CMD_FWT_ACCESS):
 		spin_lock_irqsave(&adapter->driver_lock, flags);
 		if (adapter->cur_cmd->pdata_buf)
 			memcpy(adapter->cur_cmd->pdata_buf, &resp->params.fwt,
 			       sizeof(resp->params.fwt));
 		spin_unlock_irqrestore(&adapter->driver_lock, flags);
 		break;
-	case cmd_ret_mesh_access:
+	case CMD_RET(CMD_MESH_ACCESS):
 		if (adapter->cur_cmd->pdata_buf)
 			memcpy(adapter->cur_cmd->pdata_buf, &resp->params.mesh,
 			       sizeof(resp->params.mesh));
 		break;
-	case cmd_rte_802_11_tx_rate_query:
-		priv->adapter->txrate = resp->params.txrate.txrate;
-		break;
 	default:
-		lbs_deb_cmd("CMD_RESP: Unknown command response %#x\n",
+		lbs_deb_host("CMD_RESP: unknown cmd response 0x%04x\n",
 			    resp->command);
 		break;
 	}
+	lbs_deb_leave(LBS_DEB_HOST);
 	return ret;
 }
 
@@ -744,9 +722,7 @@ int libertas_process_rx_command(wlan_private * priv)
 	ulong flags;
 	u16 result;
 
-	lbs_deb_enter(LBS_DEB_CMD);
-
-	lbs_deb_cmd("CMD_RESP: @ %lu\n", jiffies);
+	lbs_deb_enter(LBS_DEB_HOST);
 
 	/* Now we got response from FW, cancel the command timer */
 	del_timer(&adapter->command_timer);
@@ -755,25 +731,23 @@ int libertas_process_rx_command(wlan_private * priv)
 	spin_lock_irqsave(&adapter->driver_lock, flags);
 
 	if (!adapter->cur_cmd) {
-		lbs_deb_cmd("CMD_RESP: NULL cur_cmd=%p\n", adapter->cur_cmd);
+		lbs_deb_host("CMD_RESP: cur_cmd is NULL\n");
 		ret = -1;
 		spin_unlock_irqrestore(&adapter->driver_lock, flags);
 		goto done;
 	}
 	resp = (struct cmd_ds_command *)(adapter->cur_cmd->bufvirtualaddr);
 
-	lbs_dbg_hex("CMD_RESP:", adapter->cur_cmd->bufvirtualaddr,
-		    priv->upld_len);
-
 	respcmd = le16_to_cpu(resp->command);
-
 	result = le16_to_cpu(resp->result);
 
-	lbs_deb_cmd("CMD_RESP: %x result: %d length: %d\n", respcmd,
-		    result, priv->upld_len);
+	lbs_deb_host("CMD_RESP: response 0x%04x, size %d, jiffies %lu\n",
+		respcmd, priv->upld_len, jiffies);
+	lbs_deb_hex(LBS_DEB_HOST, "CMD_RESP", adapter->cur_cmd->bufvirtualaddr,
+		    priv->upld_len);
 
 	if (!(respcmd & 0x8000)) {
-		lbs_deb_cmd("Invalid response to command!");
+		lbs_deb_host("invalid response!\n");
 		adapter->cur_cmd_retcode = -1;
 		__libertas_cleanup_and_insert_cmd(priv, adapter->cur_cmd);
 		adapter->nr_cmd_pending--;
@@ -786,16 +760,16 @@ int libertas_process_rx_command(wlan_private * priv)
 	/* Store the response code to cur_cmd_retcode. */
 	adapter->cur_cmd_retcode = result;;
 
-	if (respcmd == cmd_ret_802_11_ps_mode) {
+	if (respcmd == CMD_RET(CMD_802_11_PS_MODE)) {
 		struct cmd_ds_802_11_ps_mode *psmode = &resp->params.psmode;
 		u16 action = le16_to_cpu(psmode->action);
 
-		lbs_deb_cmd(
-		       "CMD_RESP: PS_MODE cmd reply result=%#x action=0x%X\n",
+		lbs_deb_host(
+		       "CMD_RESP: PS_MODE cmd reply result 0x%x, action 0x%x\n",
 		       result, action);
 
 		if (result) {
-			lbs_deb_cmd("CMD_RESP: PS command failed- %#x \n",
+			lbs_deb_host("CMD_RESP: PS command failed with 0x%x\n",
 				    result);
 			/*
 			 * We should not re-try enter-ps command in
@@ -803,20 +777,20 @@ int libertas_process_rx_command(wlan_private * priv)
 			 * libertas_execute_next_command().
 			 */
 			if (adapter->mode == IW_MODE_ADHOC &&
-			    action == cmd_subcmd_enter_ps)
-				adapter->psmode = wlan802_11powermodecam;
-		} else if (action == cmd_subcmd_enter_ps) {
+			    action == CMD_SUBCMD_ENTER_PS)
+				adapter->psmode = WLAN802_11POWERMODECAM;
+		} else if (action == CMD_SUBCMD_ENTER_PS) {
 			adapter->needtowakeup = 0;
 			adapter->psstate = PS_STATE_AWAKE;
 
-			lbs_deb_cmd("CMD_RESP: Enter_PS command response\n");
-			if (adapter->connect_status != libertas_connected) {
+			lbs_deb_host("CMD_RESP: ENTER_PS command response\n");
+			if (adapter->connect_status != LIBERTAS_CONNECTED) {
 				/*
 				 * When Deauth Event received before Enter_PS command
 				 * response, We need to wake up the firmware.
 				 */
-				lbs_deb_cmd(
-				       "Disconnected, Going to invoke libertas_ps_wakeup\n");
+				lbs_deb_host(
+				       "disconnected, invoking libertas_ps_wakeup\n");
 
 				spin_unlock_irqrestore(&adapter->driver_lock, flags);
 				mutex_unlock(&adapter->lock);
@@ -824,12 +798,12 @@ int libertas_process_rx_command(wlan_private * priv)
 				mutex_lock(&adapter->lock);
 				spin_lock_irqsave(&adapter->driver_lock, flags);
 			}
-		} else if (action == cmd_subcmd_exit_ps) {
+		} else if (action == CMD_SUBCMD_EXIT_PS) {
 			adapter->needtowakeup = 0;
 			adapter->psstate = PS_STATE_FULL_POWER;
-			lbs_deb_cmd("CMD_RESP: Exit_PS command response\n");
+			lbs_deb_host("CMD_RESP: EXIT_PS command response\n");
 		} else {
-			lbs_deb_cmd("CMD_RESP: PS- action=0x%X\n", action);
+			lbs_deb_host("CMD_RESP: PS action 0x%X\n", action);
 		}
 
 		__libertas_cleanup_and_insert_cmd(priv, adapter->cur_cmd);
@@ -843,22 +817,22 @@ int libertas_process_rx_command(wlan_private * priv)
 
 	if (adapter->cur_cmd->cmdflags & CMD_F_HOSTCMD) {
 		/* Copy the response back to response buffer */
-		memcpy(adapter->cur_cmd->pdata_buf, resp, resp->size);
-
+		memcpy(adapter->cur_cmd->pdata_buf, resp,
+		       le16_to_cpu(resp->size));
 		adapter->cur_cmd->cmdflags &= ~CMD_F_HOSTCMD;
 	}
 
 	/* If the command is not successful, cleanup and return failure */
 	if ((result != 0 || !(respcmd & 0x8000))) {
-		lbs_deb_cmd("CMD_RESP: command reply %#x result=%#x\n",
-		       respcmd, result);
+		lbs_deb_host("CMD_RESP: error 0x%04x in command reply 0x%04x\n",
+		       result, respcmd);
 		/*
 		 * Handling errors here
 		 */
 		switch (respcmd) {
-		case cmd_ret_hw_spec_info:
-		case cmd_ret_802_11_reset:
-			lbs_deb_cmd("CMD_RESP: Reset command failed\n");
+		case CMD_RET(CMD_GET_HW_SPEC):
+		case CMD_RET(CMD_802_11_RESET):
+			lbs_deb_host("CMD_RESP: reset failed\n");
 			break;
 
 		}
@@ -888,7 +862,7 @@ int libertas_process_rx_command(wlan_private * priv)
 
 done:
 	mutex_unlock(&adapter->lock);
-	lbs_deb_enter_args(LBS_DEB_CMD, "ret %d", ret);
+	lbs_deb_leave_args(LBS_DEB_HOST, "ret %d", ret);
 	return ret;
 }
 
@@ -898,13 +872,13 @@ int libertas_process_event(wlan_private * priv)
 	wlan_adapter *adapter = priv->adapter;
 	u32 eventcause;
 
+	lbs_deb_enter(LBS_DEB_CMD);
+
 	spin_lock_irq(&adapter->driver_lock);
 	eventcause = adapter->eventcause;
 	spin_unlock_irq(&adapter->driver_lock);
 
-	lbs_deb_enter(LBS_DEB_CMD);
-
-	lbs_deb_cmd("EVENT Cause %x\n", eventcause);
+	lbs_deb_cmd("event cause 0x%x\n", eventcause);
 
 	switch (eventcause >> SBI_EVENT_CAUSE_SHIFT) {
 	case MACREG_INT_CODE_LINK_SENSED:
@@ -912,28 +886,27 @@ int libertas_process_event(wlan_private * priv)
 		break;
 
 	case MACREG_INT_CODE_DEAUTHENTICATED:
-		lbs_deb_cmd("EVENT: Deauthenticated\n");
+		lbs_deb_cmd("EVENT: deauthenticated\n");
 		libertas_mac_event_disconnected(priv);
 		break;
 
 	case MACREG_INT_CODE_DISASSOCIATED:
-		lbs_deb_cmd("EVENT: Disassociated\n");
+		lbs_deb_cmd("EVENT: disassociated\n");
 		libertas_mac_event_disconnected(priv);
 		break;
 
 	case MACREG_INT_CODE_LINK_LOSE_NO_SCAN:
-		lbs_deb_cmd("EVENT: Link lost\n");
+		lbs_deb_cmd("EVENT: link lost\n");
 		libertas_mac_event_disconnected(priv);
 		break;
 
 	case MACREG_INT_CODE_PS_SLEEP:
-		lbs_deb_cmd("EVENT: SLEEP\n");
-		lbs_deb_cmd("_");
+		lbs_deb_cmd("EVENT: sleep\n");
 
 		/* handle unexpected PS SLEEP event */
 		if (adapter->psstate == PS_STATE_FULL_POWER) {
 			lbs_deb_cmd(
-			       "EVENT: In FULL POWER mode - ignore PS SLEEP\n");
+			       "EVENT: in FULL POWER mode, ignoreing PS_SLEEP\n");
 			break;
 		}
 		adapter->psstate = PS_STATE_PRE_SLEEP;
@@ -943,8 +916,7 @@ int libertas_process_event(wlan_private * priv)
 		break;
 
 	case MACREG_INT_CODE_PS_AWAKE:
-		lbs_deb_cmd("EVENT: AWAKE \n");
-		lbs_deb_cmd("|");
+		lbs_deb_cmd("EVENT: awake\n");
 
 		/* handle unexpected PS AWAKE event */
 		if (adapter->psstate == PS_STATE_FULL_POWER) {
@@ -962,7 +934,7 @@ int libertas_process_event(wlan_private * priv)
 			 * adapter->needtowakeup will be set to FALSE
 			 * in libertas_ps_wakeup()
 			 */
-			lbs_deb_cmd("Waking up...\n");
+			lbs_deb_cmd("waking up ...\n");
 			libertas_ps_wakeup(priv, 0);
 		}
 		break;
@@ -981,38 +953,43 @@ int libertas_process_event(wlan_private * priv)
 		break;
 
 	case MACREG_INT_CODE_ADHOC_BCN_LOST:
-		lbs_deb_cmd("EVENT: HWAC - ADHOC BCN LOST\n");
+		lbs_deb_cmd("EVENT: ADHOC beacon lost\n");
 		break;
 
 	case MACREG_INT_CODE_RSSI_LOW:
-		lbs_pr_alert( "EVENT: RSSI_LOW\n");
+		lbs_pr_alert("EVENT: rssi low\n");
 		break;
 	case MACREG_INT_CODE_SNR_LOW:
-		lbs_pr_alert( "EVENT: SNR_LOW\n");
+		lbs_pr_alert("EVENT: snr low\n");
 		break;
 	case MACREG_INT_CODE_MAX_FAIL:
-		lbs_pr_alert( "EVENT: MAX_FAIL\n");
+		lbs_pr_alert("EVENT: max fail\n");
 		break;
 	case MACREG_INT_CODE_RSSI_HIGH:
-		lbs_pr_alert( "EVENT: RSSI_HIGH\n");
+		lbs_pr_alert("EVENT: rssi high\n");
 		break;
 	case MACREG_INT_CODE_SNR_HIGH:
-		lbs_pr_alert( "EVENT: SNR_HIGH\n");
+		lbs_pr_alert("EVENT: snr high\n");
 		break;
 
 	case MACREG_INT_CODE_MESH_AUTO_STARTED:
-		lbs_pr_alert( "EVENT: MESH_AUTO_STARTED\n");
-		adapter->connect_status = libertas_connected ;
+		/* Ignore spurious autostart events if autostart is disabled */
+		if (!priv->mesh_autostart_enabled) {
+			lbs_pr_info("EVENT: MESH_AUTO_STARTED (ignoring)\n");
+			break;
+		}
+		lbs_pr_info("EVENT: MESH_AUTO_STARTED\n");
+		adapter->connect_status = LIBERTAS_CONNECTED;
 		if (priv->mesh_open == 1) {
-			netif_wake_queue(priv->mesh_dev) ;
-			netif_carrier_on(priv->mesh_dev) ;
+			netif_wake_queue(priv->mesh_dev);
+			netif_carrier_on(priv->mesh_dev);
 		}
-		adapter->mode = IW_MODE_ADHOC ;
+		adapter->mode = IW_MODE_ADHOC;
 		schedule_work(&priv->sync_channel);
 		break;
 
 	default:
-		lbs_pr_alert( "EVENT: unknown event id: %#x\n",
+		lbs_pr_alert("EVENT: unknown event id 0x%04x\n",
 		       eventcause >> SBI_EVENT_CAUSE_SHIFT);
 		break;
 	}
@@ -1021,6 +998,6 @@ int libertas_process_event(wlan_private * priv)
 	adapter->eventcause = 0;
 	spin_unlock_irq(&adapter->driver_lock);
 
-	lbs_deb_enter_args(LBS_DEB_CMD, "ret %d", ret);
+	lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
 	return ret;
 }
diff --git a/drivers/net/wireless/libertas/debugfs.c b/drivers/net/wireless/libertas/debugfs.c
index 715cbdaa1d4b..0bda0b511910 100644
--- a/drivers/net/wireless/libertas/debugfs.c
+++ b/drivers/net/wireless/libertas/debugfs.c
@@ -3,6 +3,7 @@
 #include <linux/debugfs.h>
 #include <linux/delay.h>
 #include <linux/mm.h>
+#include <linux/string.h>
 #include <net/iw_handler.h>
 
 #include "dev.h"
@@ -63,27 +64,27 @@ static ssize_t libertas_getscantable(struct file *file, char __user *userbuf,
 	int numscansdone = 0, res;
 	unsigned long addr = get_zeroed_page(GFP_KERNEL);
 	char *buf = (char *)addr;
+	DECLARE_MAC_BUF(mac);
 	struct bss_descriptor * iter_bss;
 
 	pos += snprintf(buf+pos, len-pos,
-		"# | ch  | ss  |       bssid       |   cap    |    TSF   | Qual | SSID \n");
+		"# | ch  | rssi |       bssid       |   cap    | Qual | SSID \n");
 
 	mutex_lock(&priv->adapter->lock);
 	list_for_each_entry (iter_bss, &priv->adapter->network_list, list) {
-		u16 cap;
+		u16 ibss = (iter_bss->capability & WLAN_CAPABILITY_IBSS);
+		u16 privacy = (iter_bss->capability & WLAN_CAPABILITY_PRIVACY);
+		u16 spectrum_mgmt = (iter_bss->capability & WLAN_CAPABILITY_SPECTRUM_MGMT);
 
-		memcpy(&cap, &iter_bss->cap, sizeof(cap));
 		pos += snprintf(buf+pos, len-pos,
-			"%02u| %03d | %03ld | " MAC_FMT " |",
+			"%02u| %03d | %04ld | %s |",
 			numscansdone, iter_bss->channel, iter_bss->rssi,
-			MAC_ARG(iter_bss->bssid));
-		pos += snprintf(buf+pos, len-pos, " %04x-", cap);
+			print_mac(mac, iter_bss->bssid));
+		pos += snprintf(buf+pos, len-pos, " %04x-", iter_bss->capability);
 		pos += snprintf(buf+pos, len-pos, "%c%c%c |",
-				iter_bss->cap.ibss ? 'A' : 'I',
-				iter_bss->cap.privacy ? 'P' : ' ',
-				iter_bss->cap.spectrummgmt ? 'S' : ' ');
-		pos += snprintf(buf+pos, len-pos, " %08llx |", iter_bss->networktsf);
-		pos += snprintf(buf+pos, len-pos, " %d |", SCAN_RSSI(iter_bss->rssi));
+				ibss ? 'A' : 'I', privacy ? 'P' : ' ',
+				spectrum_mgmt ? 'S' : ' ');
+		pos += snprintf(buf+pos, len-pos, " %04d |", SCAN_RSSI(iter_bss->rssi));
 		pos += snprintf(buf+pos, len-pos, " %s\n",
 		                escape_essid(iter_bss->ssid, iter_bss->ssid_len));
 
@@ -125,9 +126,9 @@ static ssize_t libertas_sleepparams_write(struct file *file,
 	priv->adapter->sp.sp_reserved = p6;
 
         res = libertas_prepare_and_send_command(priv,
-				cmd_802_11_sleep_params,
-				cmd_act_set,
-				cmd_option_waitforrsp, 0, NULL);
+				CMD_802_11_SLEEP_PARAMS,
+				CMD_ACT_SET,
+				CMD_OPTION_WAITFORRSP, 0, NULL);
 
 	if (!res)
 		res = count;
@@ -150,9 +151,9 @@ static ssize_t libertas_sleepparams_read(struct file *file, char __user *userbuf
 	char *buf = (char *)addr;
 
         res = libertas_prepare_and_send_command(priv,
-				cmd_802_11_sleep_params,
-				cmd_act_get,
-				cmd_option_waitforrsp, 0, NULL);
+				CMD_802_11_SLEEP_PARAMS,
+				CMD_ACT_GET,
+				CMD_OPTION_WAITFORRSP, 0, NULL);
 	if (res) {
 		res = -EFAULT;
 		goto out_unlock;
@@ -205,7 +206,7 @@ static int libertas_parse_chan(char *buf, size_t count,
 	if (!start)
 		return -EINVAL;
 	start += 5;
-	end = strstr(start, " ");
+	end = strchr(start, ' ');
 	if (!end)
 		end = buf + count;
 	hold = kzalloc((end - start)+1, GFP_KERNEL);
@@ -256,7 +257,7 @@ static void libertas_parse_ssid(char *buf, size_t count,
 	if (!hold)
 		return;
 	hold += 5;
-	end = strstr(hold, " ");
+	end = strchr(hold, ' ');
 	if (!end)
 		end = buf + count - 1;
 
@@ -386,7 +387,7 @@ static int libertas_event_initcmd(wlan_private *priv, void **response_buf,
 			struct cmd_ctrl_node **cmdnode,
 			struct cmd_ds_command **cmd)
 {
-	u16 wait_option = cmd_option_waitforrsp;
+	u16 wait_option = CMD_OPTION_WAITFORRSP;
 
 	if (!(*cmdnode = libertas_get_free_cmd_ctrl_node(priv))) {
 		lbs_deb_debugfs("failed libertas_get_free_cmd_ctrl_node\n");
@@ -402,7 +403,7 @@ static int libertas_event_initcmd(wlan_private *priv, void **response_buf,
 	(*cmdnode)->cmdflags |= CMD_F_HOSTCMD;
 	(*cmdnode)->cmdwaitqwoken = 0;
 	*cmd = (struct cmd_ds_command *)(*cmdnode)->bufvirtualaddr;
-	(*cmd)->command = cpu_to_le16(cmd_802_11_subscribe_event);
+	(*cmd)->command = cpu_to_le16(CMD_802_11_SUBSCRIBE_EVENT);
 	(*cmd)->seqnum = cpu_to_le16(++priv->adapter->seqnum);
 	(*cmd)->result = 0;
 	return 0;
@@ -429,10 +430,10 @@ static ssize_t libertas_lowrssi_read(struct file *file, char __user *userbuf,
 	}
 
 	event = &pcmdptr->params.subscribe_event;
-	event->action = cpu_to_le16(cmd_act_get);
+	event->action = cpu_to_le16(CMD_ACT_GET);
 	pcmdptr->size = cpu_to_le16(sizeof(*event) + S_DS_GEN);
 	libertas_queue_cmd(adapter, pcmdnode, 1);
-	wake_up_interruptible(&priv->mainthread.waitq);
+	wake_up_interruptible(&priv->waitq);
 
 	/* Sleep until response is generated by FW */
 	wait_event_interruptible(pcmdnode->cmdwait_q,
@@ -447,7 +448,7 @@ static ssize_t libertas_lowrssi_read(struct file *file, char __user *userbuf,
 		return 0;
 	}
 
-	if (pcmdptr->command != cpu_to_le16(cmd_ret_802_11_subscribe_event)) {
+	if (pcmdptr->command != cpu_to_le16(CMD_RET(CMD_802_11_SUBSCRIBE_EVENT))) {
 		lbs_pr_err("command response incorrect!\n");
 		kfree(response_buf);
 		free_page(addr);
@@ -493,10 +494,10 @@ static u16 libertas_get_events_bitmap(wlan_private *priv)
 		return res;
 
 	event = &pcmdptr->params.subscribe_event;
-	event->action = cpu_to_le16(cmd_act_get);
+	event->action = cpu_to_le16(CMD_ACT_GET);
 	pcmdptr->size = cpu_to_le16(sizeof(*event) + S_DS_GEN);
 	libertas_queue_cmd(adapter, pcmdnode, 1);
-	wake_up_interruptible(&priv->mainthread.waitq);
+	wake_up_interruptible(&priv->waitq);
 
 	/* Sleep until response is generated by FW */
 	wait_event_interruptible(pcmdnode->cmdwait_q,
@@ -511,7 +512,7 @@ static u16 libertas_get_events_bitmap(wlan_private *priv)
 		return 0;
 	}
 
-	if (pcmdptr->command != cmd_ret_802_11_subscribe_event) {
+	if (le16_to_cpu(pcmdptr->command) != CMD_RET(CMD_802_11_SUBSCRIBE_EVENT)) {
 		lbs_pr_err("command response incorrect!\n");
 		kfree(response_buf);
 		return 0;
@@ -559,7 +560,7 @@ static ssize_t libertas_lowrssi_write(struct file *file,
 		goto out_unlock;
 
 	event = &pcmdptr->params.subscribe_event;
-	event->action = cpu_to_le16(cmd_act_set);
+	event->action = cpu_to_le16(CMD_ACT_SET);
 	pcmdptr->size = cpu_to_le16(S_DS_GEN +
 		sizeof(struct cmd_ds_802_11_subscribe_event) +
 		sizeof(struct mrvlietypes_rssithreshold));
@@ -575,7 +576,7 @@ static ssize_t libertas_lowrssi_write(struct file *file,
 	event->events = cpu_to_le16(event_bitmap);
 
 	libertas_queue_cmd(adapter, pcmdnode, 1);
-	wake_up_interruptible(&priv->mainthread.waitq);
+	wake_up_interruptible(&priv->waitq);
 
 	/* Sleep until response is generated by FW */
 	wait_event_interruptible(pcmdnode->cmdwait_q,
@@ -591,7 +592,7 @@ static ssize_t libertas_lowrssi_write(struct file *file,
 		return 0;
 	}
 
-	if (pcmdptr->command != cpu_to_le16(cmd_ret_802_11_subscribe_event)) {
+	if (pcmdptr->command != cpu_to_le16(CMD_RET(CMD_802_11_SUBSCRIBE_EVENT))) {
 		lbs_pr_err("command response incorrect!\n");
 		kfree(response_buf);
 		free_page(addr);
@@ -625,10 +626,10 @@ static ssize_t libertas_lowsnr_read(struct file *file, char __user *userbuf,
 	}
 
 	event = &pcmdptr->params.subscribe_event;
-	event->action = cpu_to_le16(cmd_act_get);
+	event->action = cpu_to_le16(CMD_ACT_GET);
 	pcmdptr->size = cpu_to_le16(sizeof(*event) + S_DS_GEN);
 	libertas_queue_cmd(adapter, pcmdnode, 1);
-	wake_up_interruptible(&priv->mainthread.waitq);
+	wake_up_interruptible(&priv->waitq);
 
 	/* Sleep until response is generated by FW */
 	wait_event_interruptible(pcmdnode->cmdwait_q,
@@ -644,7 +645,7 @@ static ssize_t libertas_lowsnr_read(struct file *file, char __user *userbuf,
 		return 0;
 	}
 
-	if (pcmdptr->command != cpu_to_le16(cmd_ret_802_11_subscribe_event)) {
+	if (pcmdptr->command != cpu_to_le16(CMD_RET(CMD_802_11_SUBSCRIBE_EVENT))) {
 		lbs_pr_err("command response incorrect!\n");
 		kfree(response_buf);
 		free_page(addr);
@@ -712,7 +713,7 @@ static ssize_t libertas_lowsnr_write(struct file *file,
 		goto out_unlock;
 
 	event = &pcmdptr->params.subscribe_event;
-	event->action = cpu_to_le16(cmd_act_set);
+	event->action = cpu_to_le16(CMD_ACT_SET);
 	pcmdptr->size = cpu_to_le16(S_DS_GEN +
 		sizeof(struct cmd_ds_802_11_subscribe_event) +
 		sizeof(struct mrvlietypes_snrthreshold));
@@ -727,7 +728,7 @@ static ssize_t libertas_lowsnr_write(struct file *file,
 	event->events = cpu_to_le16(event_bitmap);
 
 	libertas_queue_cmd(adapter, pcmdnode, 1);
-	wake_up_interruptible(&priv->mainthread.waitq);
+	wake_up_interruptible(&priv->waitq);
 
 	/* Sleep until response is generated by FW */
 	wait_event_interruptible(pcmdnode->cmdwait_q,
@@ -743,7 +744,7 @@ static ssize_t libertas_lowsnr_write(struct file *file,
 		return 0;
 	}
 
-	if (pcmdptr->command != cpu_to_le16(cmd_ret_802_11_subscribe_event)) {
+	if (pcmdptr->command != cpu_to_le16(CMD_RET(CMD_802_11_SUBSCRIBE_EVENT))) {
 		lbs_pr_err("command response incorrect!\n");
 		kfree(response_buf);
 		free_page(addr);
@@ -778,10 +779,10 @@ static ssize_t libertas_failcount_read(struct file *file, char __user *userbuf,
 	}
 
 	event = &pcmdptr->params.subscribe_event;
-	event->action = cpu_to_le16(cmd_act_get);
+	event->action = cpu_to_le16(CMD_ACT_GET);
 	pcmdptr->size =	cpu_to_le16(sizeof(*event) + S_DS_GEN);
 	libertas_queue_cmd(adapter, pcmdnode, 1);
-	wake_up_interruptible(&priv->mainthread.waitq);
+	wake_up_interruptible(&priv->waitq);
 
 	/* Sleep until response is generated by FW */
 	wait_event_interruptible(pcmdnode->cmdwait_q,
@@ -797,7 +798,7 @@ static ssize_t libertas_failcount_read(struct file *file, char __user *userbuf,
 		return 0;
 	}
 
-	if (pcmdptr->command != cpu_to_le16(cmd_ret_802_11_subscribe_event)) {
+	if (pcmdptr->command != cpu_to_le16(CMD_RET(CMD_802_11_SUBSCRIBE_EVENT))) {
 		lbs_pr_err("command response incorrect!\n");
 		kfree(response_buf);
 		free_page(addr);
@@ -864,7 +865,7 @@ static ssize_t libertas_failcount_write(struct file *file,
 		goto out_unlock;
 
 	event = &pcmdptr->params.subscribe_event;
-	event->action = cpu_to_le16(cmd_act_set);
+	event->action = cpu_to_le16(CMD_ACT_SET);
 	pcmdptr->size = cpu_to_le16(S_DS_GEN +
 		sizeof(struct cmd_ds_802_11_subscribe_event) +
 		sizeof(struct mrvlietypes_failurecount));
@@ -879,7 +880,7 @@ static ssize_t libertas_failcount_write(struct file *file,
 	event->events = cpu_to_le16(event_bitmap);
 
 	libertas_queue_cmd(adapter, pcmdnode, 1);
-	wake_up_interruptible(&priv->mainthread.waitq);
+	wake_up_interruptible(&priv->waitq);
 
 	/* Sleep until response is generated by FW */
 	wait_event_interruptible(pcmdnode->cmdwait_q,
@@ -895,7 +896,7 @@ static ssize_t libertas_failcount_write(struct file *file,
 		return 0;
 	}
 
-	if (pcmdptr->command != cpu_to_le16(cmd_ret_802_11_subscribe_event)) {
+	if (pcmdptr->command != cpu_to_le16(CMD_RET(CMD_802_11_SUBSCRIBE_EVENT))) {
 		lbs_pr_err("command response incorrect!\n");
 		kfree(response_buf);
 		free_page(addr);
@@ -929,10 +930,10 @@ static ssize_t libertas_bcnmiss_read(struct file *file, char __user *userbuf,
 	}
 
 	event = &pcmdptr->params.subscribe_event;
-	event->action = cpu_to_le16(cmd_act_get);
+	event->action = cpu_to_le16(CMD_ACT_GET);
 	pcmdptr->size = cpu_to_le16(sizeof(*event) + S_DS_GEN);
 	libertas_queue_cmd(adapter, pcmdnode, 1);
-	wake_up_interruptible(&priv->mainthread.waitq);
+	wake_up_interruptible(&priv->waitq);
 
 	/* Sleep until response is generated by FW */
 	wait_event_interruptible(pcmdnode->cmdwait_q,
@@ -948,7 +949,7 @@ static ssize_t libertas_bcnmiss_read(struct file *file, char __user *userbuf,
 		return 0;
 	}
 
-	if (pcmdptr->command != cpu_to_le16(cmd_ret_802_11_subscribe_event)) {
+	if (pcmdptr->command != cpu_to_le16(CMD_RET(CMD_802_11_SUBSCRIBE_EVENT))) {
 		lbs_pr_err("command response incorrect!\n");
 		free_page(addr);
 		kfree(response_buf);
@@ -1015,7 +1016,7 @@ static ssize_t libertas_bcnmiss_write(struct file *file,
 		goto out_unlock;
 
 	event = &pcmdptr->params.subscribe_event;
-	event->action = cpu_to_le16(cmd_act_set);
+	event->action = cpu_to_le16(CMD_ACT_SET);
 	pcmdptr->size = cpu_to_le16(S_DS_GEN +
 		sizeof(struct cmd_ds_802_11_subscribe_event) +
 		sizeof(struct mrvlietypes_beaconsmissed));
@@ -1029,7 +1030,7 @@ static ssize_t libertas_bcnmiss_write(struct file *file,
 	event->events = cpu_to_le16(event_bitmap);
 
 	libertas_queue_cmd(adapter, pcmdnode, 1);
-	wake_up_interruptible(&priv->mainthread.waitq);
+	wake_up_interruptible(&priv->waitq);
 
 	/* Sleep until response is generated by FW */
 	wait_event_interruptible(pcmdnode->cmdwait_q,
@@ -1045,7 +1046,7 @@ static ssize_t libertas_bcnmiss_write(struct file *file,
 		return 0;
 	}
 
-	if (pcmdptr->command != cpu_to_le16(cmd_ret_802_11_subscribe_event)) {
+	if (pcmdptr->command != cpu_to_le16(CMD_RET(CMD_802_11_SUBSCRIBE_EVENT))) {
 		lbs_pr_err("command response incorrect!\n");
 		free_page(addr);
 		kfree(response_buf);
@@ -1079,10 +1080,10 @@ static ssize_t libertas_highrssi_read(struct file *file, char __user *userbuf,
 	}
 
 	event = &pcmdptr->params.subscribe_event;
-	event->action = cpu_to_le16(cmd_act_get);
+	event->action = cpu_to_le16(CMD_ACT_GET);
 	pcmdptr->size = cpu_to_le16(sizeof(*event) + S_DS_GEN);
 	libertas_queue_cmd(adapter, pcmdnode, 1);
-	wake_up_interruptible(&priv->mainthread.waitq);
+	wake_up_interruptible(&priv->waitq);
 
 	/* Sleep until response is generated by FW */
 	wait_event_interruptible(pcmdnode->cmdwait_q,
@@ -1098,7 +1099,7 @@ static ssize_t libertas_highrssi_read(struct file *file, char __user *userbuf,
 		return 0;
 	}
 
-	if (pcmdptr->command != cpu_to_le16(cmd_ret_802_11_subscribe_event)) {
+	if (pcmdptr->command != cpu_to_le16(CMD_RET(CMD_802_11_SUBSCRIBE_EVENT))) {
 		lbs_pr_err("command response incorrect!\n");
 		kfree(response_buf);
 		free_page(addr);
@@ -1166,7 +1167,7 @@ static ssize_t libertas_highrssi_write(struct file *file,
 		goto out_unlock;
 
 	event = &pcmdptr->params.subscribe_event;
-	event->action = cpu_to_le16(cmd_act_set);
+	event->action = cpu_to_le16(CMD_ACT_SET);
 	pcmdptr->size = cpu_to_le16(S_DS_GEN +
 		sizeof(struct cmd_ds_802_11_subscribe_event) +
 		sizeof(struct mrvlietypes_rssithreshold));
@@ -1181,7 +1182,7 @@ static ssize_t libertas_highrssi_write(struct file *file,
 	event->events = cpu_to_le16(event_bitmap);
 
 	libertas_queue_cmd(adapter, pcmdnode, 1);
-	wake_up_interruptible(&priv->mainthread.waitq);
+	wake_up_interruptible(&priv->waitq);
 
 	/* Sleep until response is generated by FW */
 	wait_event_interruptible(pcmdnode->cmdwait_q,
@@ -1196,7 +1197,7 @@ static ssize_t libertas_highrssi_write(struct file *file,
 		return 0;
 	}
 
-	if (pcmdptr->command != cpu_to_le16(cmd_ret_802_11_subscribe_event)) {
+	if (pcmdptr->command != cpu_to_le16(CMD_RET(CMD_802_11_SUBSCRIBE_EVENT))) {
 		lbs_pr_err("command response incorrect!\n");
 		kfree(response_buf);
 		return 0;
@@ -1229,10 +1230,10 @@ static ssize_t libertas_highsnr_read(struct file *file, char __user *userbuf,
 	}
 
 	event = &pcmdptr->params.subscribe_event;
-	event->action = cpu_to_le16(cmd_act_get);
+	event->action = cpu_to_le16(CMD_ACT_GET);
 	pcmdptr->size = cpu_to_le16(sizeof(*event) + S_DS_GEN);
 	libertas_queue_cmd(adapter, pcmdnode, 1);
-	wake_up_interruptible(&priv->mainthread.waitq);
+	wake_up_interruptible(&priv->waitq);
 
 	/* Sleep until response is generated by FW */
 	wait_event_interruptible(pcmdnode->cmdwait_q,
@@ -1248,7 +1249,7 @@ static ssize_t libertas_highsnr_read(struct file *file, char __user *userbuf,
 		return 0;
 	}
 
-	if (pcmdptr->command != cpu_to_le16(cmd_ret_802_11_subscribe_event)) {
+	if (pcmdptr->command != cpu_to_le16(CMD_RET(CMD_802_11_SUBSCRIBE_EVENT))) {
 		lbs_pr_err("command response incorrect!\n");
 		kfree(response_buf);
 		free_page(addr);
@@ -1316,7 +1317,7 @@ static ssize_t libertas_highsnr_write(struct file *file,
 		goto out_unlock;
 
 	event = &pcmdptr->params.subscribe_event;
-	event->action = cpu_to_le16(cmd_act_set);
+	event->action = cpu_to_le16(CMD_ACT_SET);
 	pcmdptr->size = cpu_to_le16(S_DS_GEN +
 		sizeof(struct cmd_ds_802_11_subscribe_event) +
 		sizeof(struct mrvlietypes_snrthreshold));
@@ -1331,7 +1332,7 @@ static ssize_t libertas_highsnr_write(struct file *file,
 	event->events = cpu_to_le16(event_bitmap);
 
 	libertas_queue_cmd(adapter, pcmdnode, 1);
-	wake_up_interruptible(&priv->mainthread.waitq);
+	wake_up_interruptible(&priv->waitq);
 
 	/* Sleep until response is generated by FW */
 	wait_event_interruptible(pcmdnode->cmdwait_q,
@@ -1347,7 +1348,7 @@ static ssize_t libertas_highsnr_write(struct file *file,
 		return 0;
 	}
 
-	if (pcmdptr->command != cpu_to_le16(cmd_ret_802_11_subscribe_event)) {
+	if (pcmdptr->command != cpu_to_le16(CMD_RET(CMD_802_11_SUBSCRIBE_EVENT))) {
 		lbs_pr_err("command response incorrect!\n");
 		kfree(response_buf);
 		free_page(addr);
@@ -1375,8 +1376,8 @@ static ssize_t libertas_rdmac_read(struct file *file, char __user *userbuf,
 	offval.value = 0;
 
 	ret = libertas_prepare_and_send_command(priv,
-				cmd_mac_reg_access, 0,
-				cmd_option_waitforrsp, 0, &offval);
+				CMD_MAC_REG_ACCESS, 0,
+				CMD_OPTION_WAITFORRSP, 0, &offval);
 	mdelay(10);
 	pos += snprintf(buf+pos, len-pos, "MAC[0x%x] = 0x%08x\n",
 				priv->mac_offset, adapter->offsetvalue.value);
@@ -1433,8 +1434,8 @@ static ssize_t libertas_wrmac_write(struct file *file,
 	offval.offset = offset;
 	offval.value = value;
 	res = libertas_prepare_and_send_command(priv,
-				cmd_mac_reg_access, 1,
-				cmd_option_waitforrsp, 0, &offval);
+				CMD_MAC_REG_ACCESS, 1,
+				CMD_OPTION_WAITFORRSP, 0, &offval);
 	mdelay(10);
 
 	res = count;
@@ -1458,8 +1459,8 @@ static ssize_t libertas_rdbbp_read(struct file *file, char __user *userbuf,
 	offval.value = 0;
 
 	ret = libertas_prepare_and_send_command(priv,
-				cmd_bbp_reg_access, 0,
-				cmd_option_waitforrsp, 0, &offval);
+				CMD_BBP_REG_ACCESS, 0,
+				CMD_OPTION_WAITFORRSP, 0, &offval);
 	mdelay(10);
 	pos += snprintf(buf+pos, len-pos, "BBP[0x%x] = 0x%08x\n",
 				priv->bbp_offset, adapter->offsetvalue.value);
@@ -1517,8 +1518,8 @@ static ssize_t libertas_wrbbp_write(struct file *file,
 	offval.offset = offset;
 	offval.value = value;
 	res = libertas_prepare_and_send_command(priv,
-				cmd_bbp_reg_access, 1,
-				cmd_option_waitforrsp, 0, &offval);
+				CMD_BBP_REG_ACCESS, 1,
+				CMD_OPTION_WAITFORRSP, 0, &offval);
 	mdelay(10);
 
 	res = count;
@@ -1542,8 +1543,8 @@ static ssize_t libertas_rdrf_read(struct file *file, char __user *userbuf,
 	offval.value = 0;
 
 	ret = libertas_prepare_and_send_command(priv,
-				cmd_rf_reg_access, 0,
-				cmd_option_waitforrsp, 0, &offval);
+				CMD_RF_REG_ACCESS, 0,
+				CMD_OPTION_WAITFORRSP, 0, &offval);
 	mdelay(10);
 	pos += snprintf(buf+pos, len-pos, "RF[0x%x] = 0x%08x\n",
 				priv->rf_offset, adapter->offsetvalue.value);
@@ -1601,8 +1602,8 @@ static ssize_t libertas_wrrf_write(struct file *file,
 	offval.offset = offset;
 	offval.value = value;
 	res = libertas_prepare_and_send_command(priv,
-				cmd_rf_reg_access, 1,
-				cmd_option_waitforrsp, 0, &offval);
+				CMD_RF_REG_ACCESS, 1,
+				CMD_OPTION_WAITFORRSP, 0, &offval);
 	mdelay(10);
 
 	res = count;
@@ -1839,7 +1840,7 @@ static ssize_t wlan_debugfs_write(struct file *f, const char __user *buf,
 	char *p2;
 	struct debug_data *d = (struct debug_data *)f->private_data;
 
-	pdata = (char *)kmalloc(cnt, GFP_KERNEL);
+	pdata = kmalloc(cnt, GFP_KERNEL);
 	if (pdata == NULL)
 		return 0;
 
diff --git a/drivers/net/wireless/libertas/decl.h b/drivers/net/wireless/libertas/decl.h
index 40f56bb1eac8..87fea9d5b90a 100644
--- a/drivers/net/wireless/libertas/decl.h
+++ b/drivers/net/wireless/libertas/decl.h
@@ -15,14 +15,9 @@ struct wlan_private;
 struct sk_buff;
 struct net_device;
 
-extern char *libertas_fw_name;
-
-void libertas_free_adapter(wlan_private * priv);
 int libertas_set_mac_packet_filter(wlan_private * priv);
 
-int libertas_send_null_packet(wlan_private * priv, u8 pwr_mgmt);
 void libertas_send_tx_feedback(wlan_private * priv);
-u8 libertas_check_last_packet_indication(wlan_private * priv);
 
 int libertas_free_cmd_buffer(wlan_private * priv);
 struct cmd_ctrl_node;
@@ -44,8 +39,8 @@ int libertas_execute_next_command(wlan_private * priv);
 int libertas_process_event(wlan_private * priv);
 void libertas_interrupt(struct net_device *);
 int libertas_set_radio_control(wlan_private * priv);
-u32 libertas_index_to_data_rate(u8 index);
-u8 libertas_data_rate_to_index(u32 rate);
+u32 libertas_fw_index_to_data_rate(u8 index);
+u8 libertas_data_rate_to_fw_index(u32 rate);
 void libertas_get_fwversion(wlan_adapter * adapter, char *fwversion, int maxlen);
 
 void libertas_upload_rx_packet(wlan_private * priv, struct sk_buff *skb);
@@ -53,8 +48,6 @@ void libertas_upload_rx_packet(wlan_private * priv, struct sk_buff *skb);
 /** The proc fs interface */
 int libertas_process_rx_command(wlan_private * priv);
 int libertas_process_tx(wlan_private * priv, struct sk_buff *skb);
-void libertas_cleanup_and_insert_cmd(wlan_private * priv,
-					struct cmd_ctrl_node *ptempcmd);
 void __libertas_cleanup_and_insert_cmd(wlan_private * priv,
 					struct cmd_ctrl_node *ptempcmd);
 
@@ -75,17 +68,15 @@ void libertas_mac_event_disconnected(wlan_private * priv);
 
 void libertas_send_iwevcustom_event(wlan_private * priv, s8 * str);
 
-/* fw.c */
-int libertas_init_fw(wlan_private * priv, char *fw_name);
-
 /* main.c */
 struct chan_freq_power *libertas_get_region_cfp_table(u8 region, u8 band,
 						             int *cfp_no);
 wlan_private *libertas_add_card(void *card, struct device *dmdev);
-int libertas_activate_card(wlan_private *priv, char *fw_name);
 int libertas_remove_card(wlan_private *priv);
+int libertas_start_card(wlan_private *priv);
+int libertas_stop_card(wlan_private *priv);
 int libertas_add_mesh(wlan_private *priv, struct device *dev);
 void libertas_remove_mesh(wlan_private *priv);
-
+int libertas_reset_device(wlan_private *priv);
 
 #endif				/* _WLAN_DECL_H_ */
diff --git a/drivers/net/wireless/libertas/defs.h b/drivers/net/wireless/libertas/defs.h
index 4dd43e59bda0..7c5b7f7b45db 100644
--- a/drivers/net/wireless/libertas/defs.h
+++ b/drivers/net/wireless/libertas/defs.h
@@ -43,43 +43,43 @@
 extern unsigned int libertas_debug;
 
 #ifdef DEBUG
-#define LBS_DEB_LL(grp, fmt, args...) \
+#define LBS_DEB_LL(grp, grpnam, fmt, args...) \
 do { if ((libertas_debug & (grp)) == (grp)) \
-  printk(KERN_DEBUG DRV_NAME "%s: " fmt, \
+  printk(KERN_DEBUG DRV_NAME grpnam "%s: " fmt, \
          in_interrupt() ? " (INT)" : "", ## args); } while (0)
 #else
-#define LBS_DEB_LL(grp, fmt, args...) do {} while (0)
+#define LBS_DEB_LL(grp, grpnam, fmt, args...) do {} while (0)
 #endif
 
 #define lbs_deb_enter(grp) \
-  LBS_DEB_LL(grp | LBS_DEB_ENTER, "%s():%d enter\n", __FUNCTION__, __LINE__);
+  LBS_DEB_LL(grp | LBS_DEB_ENTER, " enter", "%s():%d\n", __FUNCTION__, __LINE__);
 #define lbs_deb_enter_args(grp, fmt, args...) \
-  LBS_DEB_LL(grp | LBS_DEB_ENTER, "%s(" fmt "):%d\n", __FUNCTION__, ## args, __LINE__);
+  LBS_DEB_LL(grp | LBS_DEB_ENTER, " enter", "%s(" fmt "):%d\n", __FUNCTION__, ## args, __LINE__);
 #define lbs_deb_leave(grp) \
-  LBS_DEB_LL(grp | LBS_DEB_LEAVE, "%s():%d leave\n", __FUNCTION__, __LINE__);
+  LBS_DEB_LL(grp | LBS_DEB_LEAVE, " leave", "%s():%d\n", __FUNCTION__, __LINE__);
 #define lbs_deb_leave_args(grp, fmt, args...) \
-  LBS_DEB_LL(grp | LBS_DEB_LEAVE, "%s():%d leave, " fmt "\n", \
+  LBS_DEB_LL(grp | LBS_DEB_LEAVE, " leave", "%s():%d, " fmt "\n", \
   __FUNCTION__, __LINE__, ##args);
-#define lbs_deb_main(fmt, args...)      LBS_DEB_LL(LBS_DEB_MAIN, fmt, ##args)
-#define lbs_deb_net(fmt, args...)       LBS_DEB_LL(LBS_DEB_NET, fmt, ##args)
-#define lbs_deb_mesh(fmt, args...)      LBS_DEB_LL(LBS_DEB_MESH, fmt, ##args)
-#define lbs_deb_wext(fmt, args...)      LBS_DEB_LL(LBS_DEB_WEXT, fmt, ##args)
-#define lbs_deb_ioctl(fmt, args...)     LBS_DEB_LL(LBS_DEB_IOCTL, fmt, ##args)
-#define lbs_deb_scan(fmt, args...)      LBS_DEB_LL(LBS_DEB_SCAN, fmt, ##args)
-#define lbs_deb_assoc(fmt, args...)     LBS_DEB_LL(LBS_DEB_ASSOC, fmt, ##args)
-#define lbs_deb_join(fmt, args...)      LBS_DEB_LL(LBS_DEB_JOIN, fmt, ##args)
-#define lbs_deb_11d(fmt, args...)       LBS_DEB_LL(LBS_DEB_11D, fmt, ##args)
-#define lbs_deb_debugfs(fmt, args...)   LBS_DEB_LL(LBS_DEB_DEBUGFS, fmt, ##args)
-#define lbs_deb_ethtool(fmt, args...)   LBS_DEB_LL(LBS_DEB_ETHTOOL, fmt, ##args)
-#define lbs_deb_host(fmt, args...)      LBS_DEB_LL(LBS_DEB_HOST, fmt, ##args)
-#define lbs_deb_cmd(fmt, args...)       LBS_DEB_LL(LBS_DEB_CMD, fmt, ##args)
-#define lbs_deb_rx(fmt, args...)        LBS_DEB_LL(LBS_DEB_RX, fmt, ##args)
-#define lbs_deb_tx(fmt, args...)        LBS_DEB_LL(LBS_DEB_TX, fmt, ##args)
-#define lbs_deb_fw(fmt, args...)        LBS_DEB_LL(LBS_DEB_FW, fmt, ##args)
-#define lbs_deb_usb(fmt, args...)       LBS_DEB_LL(LBS_DEB_USB, fmt, ##args)
-#define lbs_deb_usbd(dev, fmt, args...) LBS_DEB_LL(LBS_DEB_USB, "%s:" fmt, (dev)->bus_id, ##args)
-#define lbs_deb_cs(fmt, args...)        LBS_DEB_LL(LBS_DEB_CS, fmt, ##args)
-#define lbs_deb_thread(fmt, args...)    LBS_DEB_LL(LBS_DEB_THREAD, fmt, ##args)
+#define lbs_deb_main(fmt, args...)      LBS_DEB_LL(LBS_DEB_MAIN, " main", fmt, ##args)
+#define lbs_deb_net(fmt, args...)       LBS_DEB_LL(LBS_DEB_NET, " net", fmt, ##args)
+#define lbs_deb_mesh(fmt, args...)      LBS_DEB_LL(LBS_DEB_MESH, " mesh", fmt, ##args)
+#define lbs_deb_wext(fmt, args...)      LBS_DEB_LL(LBS_DEB_WEXT, " wext", fmt, ##args)
+#define lbs_deb_ioctl(fmt, args...)     LBS_DEB_LL(LBS_DEB_IOCTL, " ioctl", fmt, ##args)
+#define lbs_deb_scan(fmt, args...)      LBS_DEB_LL(LBS_DEB_SCAN, " scan", fmt, ##args)
+#define lbs_deb_assoc(fmt, args...)     LBS_DEB_LL(LBS_DEB_ASSOC, " assoc", fmt, ##args)
+#define lbs_deb_join(fmt, args...)      LBS_DEB_LL(LBS_DEB_JOIN, " join", fmt, ##args)
+#define lbs_deb_11d(fmt, args...)       LBS_DEB_LL(LBS_DEB_11D, " 11d", fmt, ##args)
+#define lbs_deb_debugfs(fmt, args...)   LBS_DEB_LL(LBS_DEB_DEBUGFS, " debugfs", fmt, ##args)
+#define lbs_deb_ethtool(fmt, args...)   LBS_DEB_LL(LBS_DEB_ETHTOOL, " ethtool", fmt, ##args)
+#define lbs_deb_host(fmt, args...)      LBS_DEB_LL(LBS_DEB_HOST, " host", fmt, ##args)
+#define lbs_deb_cmd(fmt, args...)       LBS_DEB_LL(LBS_DEB_CMD, " cmd", fmt, ##args)
+#define lbs_deb_rx(fmt, args...)        LBS_DEB_LL(LBS_DEB_RX, " rx", fmt, ##args)
+#define lbs_deb_tx(fmt, args...)        LBS_DEB_LL(LBS_DEB_TX, " tx", fmt, ##args)
+#define lbs_deb_fw(fmt, args...)        LBS_DEB_LL(LBS_DEB_FW, " fw", fmt, ##args)
+#define lbs_deb_usb(fmt, args...)       LBS_DEB_LL(LBS_DEB_USB, " usb", fmt, ##args)
+#define lbs_deb_usbd(dev, fmt, args...) LBS_DEB_LL(LBS_DEB_USB, " usbd", "%s:" fmt, (dev)->bus_id, ##args)
+#define lbs_deb_cs(fmt, args...)        LBS_DEB_LL(LBS_DEB_CS, " cs", fmt, ##args)
+#define lbs_deb_thread(fmt, args...)    LBS_DEB_LL(LBS_DEB_THREAD, " thread", fmt, ##args)
 
 #define lbs_pr_info(format, args...) \
 	printk(KERN_INFO DRV_NAME": " format, ## args)
@@ -89,22 +89,28 @@ do { if ((libertas_debug & (grp)) == (grp)) \
 	printk(KERN_ALERT DRV_NAME": " format, ## args)
 
 #ifdef DEBUG
-static inline void lbs_dbg_hex(char *prompt, u8 * buf, int len)
+static inline void lbs_deb_hex(unsigned int grp, const char *prompt, u8 *buf, int len)
 {
 	int i = 0;
 
-	if (!(libertas_debug & LBS_DEB_HEX))
-		return;
-
-	printk(KERN_DEBUG "%s: ", prompt);
-	for (i = 1; i <= len; i++) {
-		printk("%02x ", (u8) * buf);
-		buf++;
+	if (len &&
+	    (libertas_debug & LBS_DEB_HEX) &&
+	    (libertas_debug & grp))
+	{
+		for (i = 1; i <= len; i++) {
+			if ((i & 0xf) == 1) {
+				if (i != 1)
+					printk("\n");
+				printk(DRV_NAME " %s: ", prompt);
+			}
+			printk("%02x ", (u8) * buf);
+			buf++;
+		}
+		printk("\n");
 	}
-	printk("\n");
 }
 #else
-#define lbs_dbg_hex(x,y,z)				do {} while (0)
+#define lbs_deb_hex(grp,prompt,buf,len)	do {} while (0)
 #endif
 
 
@@ -149,17 +155,18 @@ static inline void lbs_dbg_hex(char *prompt, u8 * buf, int len)
 #define	MRVDRV_CHANNELS_PER_SCAN		4
 #define	MRVDRV_MAX_CHANNELS_PER_SCAN		14
 
-#define MRVDRV_DEBUG_RX_PATH		0x00000001
-#define MRVDRV_DEBUG_TX_PATH		0x00000002
-
 #define MRVDRV_MIN_BEACON_INTERVAL		20
 #define MRVDRV_MAX_BEACON_INTERVAL		1000
 #define MRVDRV_BEACON_INTERVAL			100
 
+#define MARVELL_MESH_IE_LENGTH		9
+
 /** INT status Bit Definition*/
-#define his_cmddnldrdy			0x01
-#define his_cardevent			0x02
-#define his_cmdupldrdy			0x04
+#define MRVDRV_TX_DNLD_RDY		0x0001
+#define MRVDRV_RX_UPLD_RDY		0x0002
+#define MRVDRV_CMD_DNLD_RDY		0x0004
+#define MRVDRV_CMD_UPLD_RDY		0x0008
+#define MRVDRV_CARDEVENT		0x0010
 
 #define SBI_EVENT_CAUSE_SHIFT		3
 
@@ -218,9 +225,6 @@ static inline void lbs_dbg_hex(char *prompt, u8 * buf, int len)
 #define	CMD_F_HOSTCMD		(1 << 0)
 #define FW_CAPINFO_WPA  	(1 << 0)
 
-/** WPA key LENGTH*/
-#define MRVL_MAX_KEY_WPA_KEY_LENGTH     32
-
 #define KEY_LEN_WPA_AES			16
 #define KEY_LEN_WPA_TKIP		32
 #define KEY_LEN_WEP_104			13
@@ -247,10 +251,7 @@ static inline void lbs_dbg_hex(char *prompt, u8 * buf, int len)
                         ((((int)(AVG) * (N -1)) + ((u16)(SNRNF) * \
                         AVG_SCALE))  / N))
 
-#define B_SUPPORTED_RATES		8
-#define G_SUPPORTED_RATES		14
-
-#define	WLAN_SUPPORTED_RATES		14
+#define MAX_RATES			14
 
 #define	MAX_LEDS			8
 
@@ -264,11 +265,7 @@ typedef struct _wlan_adapter wlan_adapter;
 extern const char libertas_driver_version[];
 extern u16 libertas_region_code_to_index[MRVDRV_MAX_REGION_CODE];
 
-extern u8 libertas_supported_rates[G_SUPPORTED_RATES];
-
-extern u8 libertas_adhoc_rates_g[G_SUPPORTED_RATES];
-
-extern u8 libertas_adhoc_rates_b[4];
+extern u8 libertas_bg_rates[MAX_RATES];
 
 /** ENUM definition*/
 /** SNRNF_TYPE */
@@ -287,11 +284,11 @@ enum SNRNF_DATA {
 
 /** WLAN_802_11_POWER_MODE */
 enum WLAN_802_11_POWER_MODE {
-	wlan802_11powermodecam,
-	wlan802_11powermodemax_psp,
-	wlan802_11Powermodefast_psp,
+	WLAN802_11POWERMODECAM,
+	WLAN802_11POWERMODEMAX_PSP,
+	WLAN802_11POWERMODEFAST_PSP,
 	/*not a real mode, defined as an upper bound */
-	wlan802_11powemodemax
+	WLAN802_11POWEMODEMAX
 };
 
 /** PS_STATE */
@@ -311,14 +308,14 @@ enum DNLD_STATE {
 
 /** WLAN_MEDIA_STATE */
 enum WLAN_MEDIA_STATE {
-	libertas_connected,
-	libertas_disconnected
+	LIBERTAS_CONNECTED,
+	LIBERTAS_DISCONNECTED
 };
 
 /** WLAN_802_11_PRIVACY_FILTER */
 enum WLAN_802_11_PRIVACY_FILTER {
-	wlan802_11privfilteracceptall,
-	wlan802_11privfilter8021xWEP
+	WLAN802_11PRIVFILTERACCEPTALL,
+	WLAN802_11PRIVFILTER8021XWEP
 };
 
 /** mv_ms_type */
@@ -331,23 +328,23 @@ enum mv_ms_type {
 
 /** SNMP_MIB_INDEX_e */
 enum SNMP_MIB_INDEX_e {
-	desired_bsstype_i = 0,
-	op_rateset_i,
-	bcnperiod_i,
-	dtimperiod_i,
-	assocrsp_timeout_i,
-	rtsthresh_i,
-	short_retrylim_i,
-	long_retrylim_i,
-	fragthresh_i,
-	dot11d_i,
-	dot11h_i,
-	manufid_i,
-	prodID_i,
-	manuf_oui_i,
-	manuf_name_i,
-	manuf_prodname_i,
-	manuf_prodver_i,
+	DESIRED_BSSTYPE_I = 0,
+	OP_RATESET_I,
+	BCNPERIOD_I,
+	DTIMPERIOD_I,
+	ASSOCRSP_TIMEOUT_I,
+	RTSTHRESH_I,
+	SHORT_RETRYLIM_I,
+	LONG_RETRYLIM_I,
+	FRAGTHRESH_I,
+	DOT11D_I,
+	DOT11H_I,
+	MANUFID_I,
+	PRODID_I,
+	MANUF_OUI_I,
+	MANUF_NAME_I,
+	MANUF_PRODNAME_I,
+	MANUF_PRODVER_I,
 };
 
 /** KEY_TYPE_ID */
diff --git a/drivers/net/wireless/libertas/dev.h b/drivers/net/wireless/libertas/dev.h
index 785192b884bc..1fb807aa91b9 100644
--- a/drivers/net/wireless/libertas/dev.h
+++ b/drivers/net/wireless/libertas/dev.h
@@ -14,7 +14,6 @@
 
 #include "defs.h"
 #include "scan.h"
-#include "thread.h"
 
 extern struct ethtool_ops libertas_ethtool_ops;
 
@@ -73,10 +72,8 @@ struct current_bss_params {
 	u8 band;
 	/** channel */
 	u8 channel;
-	/** number of rates supported */
-	int numofrates;
-	/** supported rates*/
-	u8 datarates[WLAN_SUPPORTED_RATES];
+	/** zero-terminated array of supported data rates */
+	u8 rates[MAX_RATES + 1];
 };
 
 /** sleep_params */
@@ -106,6 +103,8 @@ struct _wlan_private {
 	int open;
 	int mesh_open;
 	int infra_open;
+	int mesh_autostart_enabled;
+	__le16 boot2_version;
 
 	char name[DEV_NAME_LEN];
 
@@ -114,7 +113,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;
@@ -142,20 +143,18 @@ struct _wlan_private {
 	   all other bits reserved 0 */
 	u8 dnld_sent;
 
-	const struct firmware *firmware;
 	struct device *hotplug_device;
 
 	/** thread to service interrupts */
-	struct wlan_thread mainthread;
+	struct task_struct *main_thread;
+	wait_queue_head_t waitq;
+	struct workqueue_struct *work_thread;
 
+	struct delayed_work scan_work;
 	struct delayed_work assoc_work;
-	struct workqueue_struct *assoc_thread;
 	struct work_struct sync_channel;
 
 	/** Hardware access */
-	int (*hw_register_dev) (wlan_private * priv);
-	int (*hw_unregister_dev) (wlan_private *);
-	int (*hw_prog_firmware) (wlan_private *);
 	int (*hw_host_to_card) (wlan_private * priv, u8 type, u8 * payload, u16 nb);
 	int (*hw_get_int_status) (wlan_private * priv, u8 *);
 	int (*hw_read_event_cause) (wlan_private *);
@@ -188,12 +187,12 @@ struct assoc_request {
 	u8 bssid[ETH_ALEN];
 
 	/** WEP keys */
-	struct WLAN_802_11_KEY wep_keys[4];
+	struct enc_key wep_keys[4];
 	u16 wep_tx_keyidx;
 
 	/** WPA keys */
-	struct WLAN_802_11_KEY wpa_mcast_key;
-	struct WLAN_802_11_KEY wpa_unicast_key;
+	struct enc_key wpa_mcast_key;
+	struct enc_key wpa_unicast_key;
 
 	struct wlan_802_11_security secinfo;
 
@@ -259,23 +258,15 @@ struct _wlan_adapter {
 	/* IW_MODE_* */
 	u8 mode;
 
-	u8 prev_ssid[IW_ESSID_MAX_SIZE + 1];
-	u8 prev_ssid_len;
-	u8 prev_bssid[ETH_ALEN];
-
 	/* Scan results list */
 	struct list_head network_list;
 	struct list_head network_free_list;
 	struct bss_descriptor *networks;
 
-	u8 scantype;
-	u32 scanmode;
-
-	u16 beaconperiod;
 	u8 adhoccreate;
 
 	/** capability Info used in Association, start, join */
-	struct ieeetypes_capinfo capinfo;
+	u16 capability;
 
 	/** MAC address information */
 	u8 current_addr[ETH_ALEN];
@@ -287,20 +278,10 @@ struct _wlan_adapter {
 
 	u16 enablehwauto;
 	u16 ratebitmap;
-	/** control G rates */
-	u8 adhoc_grate_enabled;
-
-	u32 txantenna;
-	u32 rxantenna;
 
 	u32 fragthsd;
 	u32 rtsthsd;
 
-	u32 datarate;
-	u8 is_datarate_auto;
-
-	u16 listeninterval;
-	u16 prescan;
 	u8 txretrycount;
 
 	/** Tx-related variables (for single packet tx) */
@@ -311,22 +292,17 @@ struct _wlan_adapter {
 	u16 currentpacketfilter;
 	u32 connect_status;
 	u16 regioncode;
-	u16 regiontableindex;
 	u16 txpowerlevel;
 
 	/** POWER MANAGEMENT AND PnP SUPPORT */
 	u8 surpriseremoved;
-	u16 atimwindow;
 
 	u16 psmode;		/* Wlan802_11PowermodeCAM=disable
 				   Wlan802_11PowermodeMAX_PSP=enable */
-	u16 multipledtim;
 	u32 psstate;
 	u8 needtowakeup;
 
 	struct PS_CMD_ConfirmSleep libertas_ps_confirm_sleep;
-	u16 locallisteninterval;
-	u16 nullpktinterval;
 
 	struct assoc_request * pending_assoc_req;
 	struct assoc_request * in_progress_assoc_req;
@@ -335,23 +311,18 @@ struct _wlan_adapter {
 	struct wlan_802_11_security secinfo;
 
 	/** WEP keys */
-	struct WLAN_802_11_KEY wep_keys[4];
+	struct enc_key wep_keys[4];
 	u16 wep_tx_keyidx;
 
 	/** WPA keys */
-	struct WLAN_802_11_KEY wpa_mcast_key;
-	struct WLAN_802_11_KEY wpa_unicast_key;
+	struct enc_key wpa_mcast_key;
+	struct enc_key wpa_unicast_key;
 
 	/** WPA Information Elements*/
 	u8 wpa_ie[MAX_WPA_IE_LEN];
 	u8 wpa_ie_len;
 
-	u16 rxantennamode;
-	u16 txantennamode;
-
 	/** Requested Signal Strength*/
-	u16 bcn_avg_factor;
-	u16 data_avg_factor;
 	u16 SNR[MAX_TYPE_B][MAX_TYPE_AVG];
 	u16 NF[MAX_TYPE_B][MAX_TYPE_AVG];
 	u8 RSSI[MAX_TYPE_B][MAX_TYPE_AVG];
@@ -359,15 +330,13 @@ struct _wlan_adapter {
 	u8 rawNF[DEFAULT_DATA_AVG_FACTOR];
 	u16 nextSNRNF;
 	u16 numSNRNF;
-	u16 rxpd_rate;
 
 	u8 radioon;
 	u32 preamble;
 
-	/** Multi bands Parameter*/
-	u8 libertas_supported_rates[G_SUPPORTED_RATES];
-
-	/** Blue Tooth Co-existence Arbitration */
+	/** data rate stuff */
+	u8 cur_rate;
+	u8 auto_rate;
 
 	/** sleep_params */
 	struct sleep_params sp;
@@ -392,14 +361,8 @@ struct _wlan_adapter {
 	struct wlan_offset_value offsetvalue;
 
 	struct cmd_ds_802_11_get_log logmsg;
-	u16 scanprobes;
-
-	u32 pkttxctrl;
 
-	u16 txrate;
-	u32 linkmode;
-	u32 radiomode;
-	u32 debugmode;
+	u32 monitormode;
 	u8 fw_ready;
 
 	u8 last_scanned_channel;
diff --git a/drivers/net/wireless/libertas/ethtool.c b/drivers/net/wireless/libertas/ethtool.c
index 96f1974685d4..3dae15211b6a 100644
--- a/drivers/net/wireless/libertas/ethtool.c
+++ b/drivers/net/wireless/libertas/ethtool.c
@@ -60,8 +60,7 @@ static int libertas_ethtool_get_eeprom(struct net_device *dev,
 
 //      mutex_lock(&priv->mutex);
 
-	adapter->prdeeprom =
-		    (char *)kmalloc(eeprom->len+sizeof(regctrl), GFP_KERNEL);
+	adapter->prdeeprom = kmalloc(eeprom->len+sizeof(regctrl), GFP_KERNEL);
 	if (!adapter->prdeeprom)
 		return -ENOMEM;
 	memcpy(adapter->prdeeprom, &regctrl, sizeof(regctrl));
@@ -72,9 +71,9 @@ static int libertas_ethtool_get_eeprom(struct net_device *dev,
 	       regctrl.action, regctrl.offset, regctrl.NOB);
 
 	ret = libertas_prepare_and_send_command(priv,
-				    cmd_802_11_eeprom_access,
+				    CMD_802_11_EEPROM_ACCESS,
 				    regctrl.action,
-				    cmd_option_waitforrsp, 0,
+				    CMD_OPTION_WAITFORRSP, 0,
 				    &regctrl);
 
 	if (ret) {
@@ -110,56 +109,48 @@ static void libertas_ethtool_get_stats(struct net_device * dev,
 				struct ethtool_stats * stats, u64 * data)
 {
 	wlan_private *priv = dev->priv;
+	struct cmd_ds_mesh_access mesh_access;
+	int ret;
 
 	lbs_deb_enter(LBS_DEB_ETHTOOL);
 
-	stats->cmd = ETHTOOL_GSTATS;
-	BUG_ON(stats->n_stats != MESH_STATS_NUM);
-
-        data[0] = priv->mstats.fwd_drop_rbt;
-        data[1] = priv->mstats.fwd_drop_ttl;
-        data[2] = priv->mstats.fwd_drop_noroute;
-        data[3] = priv->mstats.fwd_drop_nobuf;
-        data[4] = priv->mstats.fwd_unicast_cnt;
-        data[5] = priv->mstats.fwd_bcast_cnt;
-        data[6] = priv->mstats.drop_blind;
-        data[7] = priv->mstats.tx_failed_cnt;
+	/* Get Mesh Statistics */
+	ret = libertas_prepare_and_send_command(priv,
+			CMD_MESH_ACCESS, CMD_ACT_MESH_GET_STATS,
+			CMD_OPTION_WAITFORRSP, 0, &mesh_access);
+
+	if (ret)
+		return;
+
+	priv->mstats.fwd_drop_rbt = le32_to_cpu(mesh_access.data[0]);
+	priv->mstats.fwd_drop_ttl = le32_to_cpu(mesh_access.data[1]);
+	priv->mstats.fwd_drop_noroute = le32_to_cpu(mesh_access.data[2]);
+	priv->mstats.fwd_drop_nobuf = le32_to_cpu(mesh_access.data[3]);
+	priv->mstats.fwd_unicast_cnt = le32_to_cpu(mesh_access.data[4]);
+	priv->mstats.fwd_bcast_cnt = le32_to_cpu(mesh_access.data[5]);
+	priv->mstats.drop_blind = le32_to_cpu(mesh_access.data[6]);
+	priv->mstats.tx_failed_cnt = le32_to_cpu(mesh_access.data[7]);
+
+	data[0] = priv->mstats.fwd_drop_rbt;
+	data[1] = priv->mstats.fwd_drop_ttl;
+	data[2] = priv->mstats.fwd_drop_noroute;
+	data[3] = priv->mstats.fwd_drop_nobuf;
+	data[4] = priv->mstats.fwd_unicast_cnt;
+	data[5] = priv->mstats.fwd_bcast_cnt;
+	data[6] = priv->mstats.drop_blind;
+	data[7] = priv->mstats.tx_failed_cnt;
 
 	lbs_deb_enter(LBS_DEB_ETHTOOL);
 }
 
-static int libertas_ethtool_get_stats_count(struct net_device * dev)
+static int libertas_ethtool_get_sset_count(struct net_device * dev, int sset)
 {
-	int ret;
-	wlan_private *priv = dev->priv;
-	struct cmd_ds_mesh_access mesh_access;
-
-	lbs_deb_enter(LBS_DEB_ETHTOOL);
-
-	/* Get Mesh Statistics */
-	ret = libertas_prepare_and_send_command(priv,
-			cmd_mesh_access, cmd_act_mesh_get_stats,
-			cmd_option_waitforrsp, 0, &mesh_access);
-
-	if (ret) {
-		ret = 0;
-		goto done;
+	switch (sset) {
+	case ETH_SS_STATS:
+		return MESH_STATS_NUM;
+	default:
+		return -EOPNOTSUPP;
 	}
-
-        priv->mstats.fwd_drop_rbt = le32_to_cpu(mesh_access.data[0]);
-        priv->mstats.fwd_drop_ttl = le32_to_cpu(mesh_access.data[1]);
-        priv->mstats.fwd_drop_noroute = le32_to_cpu(mesh_access.data[2]);
-        priv->mstats.fwd_drop_nobuf = le32_to_cpu(mesh_access.data[3]);
-        priv->mstats.fwd_unicast_cnt = le32_to_cpu(mesh_access.data[4]);
-        priv->mstats.fwd_bcast_cnt = le32_to_cpu(mesh_access.data[5]);
-        priv->mstats.drop_blind = le32_to_cpu(mesh_access.data[6]);
-        priv->mstats.tx_failed_cnt = le32_to_cpu(mesh_access.data[7]);
-
-	ret = MESH_STATS_NUM;
-
-done:
-	lbs_deb_enter_args(LBS_DEB_ETHTOOL, "ret %d", ret);
-	return ret;
 }
 
 static void libertas_ethtool_get_strings (struct net_device * dev,
@@ -186,7 +177,7 @@ struct ethtool_ops libertas_ethtool_ops = {
 	.get_drvinfo = libertas_ethtool_get_drvinfo,
 	.get_eeprom =  libertas_ethtool_get_eeprom,
 	.get_eeprom_len = libertas_ethtool_get_eeprom_len,
-	.get_stats_count = libertas_ethtool_get_stats_count,
+	.get_sset_count = libertas_ethtool_get_sset_count,
 	.get_ethtool_stats = libertas_ethtool_get_stats,
 	.get_strings = libertas_ethtool_get_strings,
 };
diff --git a/drivers/net/wireless/libertas/fw.c b/drivers/net/wireless/libertas/fw.c
deleted file mode 100644
index 2dc84ff7a54a..000000000000
--- a/drivers/net/wireless/libertas/fw.c
+++ /dev/null
@@ -1,349 +0,0 @@
-/**
-  * This file contains the initialization for FW and HW
-  */
-#include <linux/firmware.h>
-
-#include "host.h"
-#include "defs.h"
-#include "decl.h"
-#include "dev.h"
-#include "wext.h"
-#include "if_usb.h"
-
-/**
- *  @brief This function checks the validity of Boot2/FW image.
- *
- *  @param data              pointer to image
- *         len               image length
- *  @return     0 or -1
- */
-static int check_fwfile_format(u8 *data, u32 totlen)
-{
-	u32 bincmd, exit;
-	u32 blksize, offset, len;
-	int ret;
-
-	ret = 1;
-	exit = len = 0;
-
-	do {
-		struct fwheader *fwh = (void *)data;
-
-		bincmd = le32_to_cpu(fwh->dnldcmd);
-		blksize = le32_to_cpu(fwh->datalength);
-		switch (bincmd) {
-		case FW_HAS_DATA_TO_RECV:
-			offset = sizeof(struct fwheader) + blksize;
-			data += offset;
-			len += offset;
-			if (len >= totlen)
-				exit = 1;
-			break;
-		case FW_HAS_LAST_BLOCK:
-			exit = 1;
-			ret = 0;
-			break;
-		default:
-			exit = 1;
-			break;
-		}
-	} while (!exit);
-
-	if (ret)
-		lbs_pr_err("firmware file format check FAIL\n");
-	else
-		lbs_deb_fw("firmware file format check PASS\n");
-
-	return ret;
-}
-
-/**
- *  @brief This function downloads firmware image, gets
- *  HW spec from firmware and set basic parameters to
- *  firmware.
- *
- *  @param priv    A pointer to wlan_private structure
- *  @return 	   0 or -1
- */
-static int wlan_setup_station_hw(wlan_private * priv, char *fw_name)
-{
-	int ret = -1;
-	wlan_adapter *adapter = priv->adapter;
-
-	lbs_deb_enter(LBS_DEB_FW);
-
-	if ((ret = request_firmware(&priv->firmware, fw_name,
-				    priv->hotplug_device)) < 0) {
-		lbs_pr_err("request_firmware() failed with %#x\n", ret);
-		lbs_pr_err("firmware %s not found\n", fw_name);
-		goto done;
-	}
-
-	if (check_fwfile_format(priv->firmware->data, priv->firmware->size)) {
-		release_firmware(priv->firmware);
-		goto done;
-	}
-
-	ret = priv->hw_prog_firmware(priv);
-
-	release_firmware(priv->firmware);
-
-	if (ret) {
-		lbs_deb_fw("bootloader in invalid state\n");
-		ret = -1;
-		goto done;
-	}
-
-	/*
-	 * Read MAC address from HW
-	 */
-	memset(adapter->current_addr, 0xff, ETH_ALEN);
-
-	ret = libertas_prepare_and_send_command(priv, cmd_get_hw_spec,
-				    0, cmd_option_waitforrsp, 0, NULL);
-
-	if (ret) {
-		ret = -1;
-		goto done;
-	}
-
-	libertas_set_mac_packet_filter(priv);
-
-	/* Get the supported Data rates */
-	ret = libertas_prepare_and_send_command(priv, cmd_802_11_data_rate,
-				    cmd_act_get_tx_rate,
-				    cmd_option_waitforrsp, 0, NULL);
-
-	if (ret) {
-		ret = -1;
-		goto done;
-	}
-
-	ret = 0;
-done:
-	lbs_deb_leave_args(LBS_DEB_FW, "ret %d", ret);
-	return ret;
-}
-
-static int wlan_allocate_adapter(wlan_private * priv)
-{
-	size_t bufsize;
-	wlan_adapter *adapter = priv->adapter;
-
-	/* Allocate buffer to store the BSSID list */
-	bufsize = MAX_NETWORK_COUNT * sizeof(struct bss_descriptor);
-	adapter->networks = kzalloc(bufsize, GFP_KERNEL);
-	if (!adapter->networks) {
-		lbs_pr_err("Out of memory allocating beacons\n");
-		libertas_free_adapter(priv);
-		return -ENOMEM;
-	}
-
-	/* Allocate the command buffers */
-	libertas_allocate_cmd_buffer(priv);
-
-	memset(&adapter->libertas_ps_confirm_sleep, 0, sizeof(struct PS_CMD_ConfirmSleep));
-	adapter->libertas_ps_confirm_sleep.seqnum = cpu_to_le16(++adapter->seqnum);
-	adapter->libertas_ps_confirm_sleep.command =
-	    cpu_to_le16(cmd_802_11_ps_mode);
-	adapter->libertas_ps_confirm_sleep.size =
-	    cpu_to_le16(sizeof(struct PS_CMD_ConfirmSleep));
-	adapter->libertas_ps_confirm_sleep.result = 0;
-	adapter->libertas_ps_confirm_sleep.action =
-	    cpu_to_le16(cmd_subcmd_sleep_confirmed);
-
-	return 0;
-}
-
-static void wlan_init_adapter(wlan_private * priv)
-{
-	wlan_adapter *adapter = priv->adapter;
-	int i;
-
-	adapter->scanprobes = 0;
-
-	adapter->bcn_avg_factor = DEFAULT_BCN_AVG_FACTOR;
-	adapter->data_avg_factor = DEFAULT_DATA_AVG_FACTOR;
-
-	/* ATIM params */
-	adapter->atimwindow = 0;
-
-	adapter->connect_status = libertas_disconnected;
-	memset(adapter->current_addr, 0xff, ETH_ALEN);
-
-	/* scan type */
-	adapter->scantype = cmd_scan_type_active;
-
-	/* scan mode */
-	adapter->scanmode = cmd_bss_type_any;
-
-	/* 802.11 specific */
-	adapter->secinfo.wep_enabled = 0;
-	for (i = 0; i < sizeof(adapter->wep_keys) / sizeof(adapter->wep_keys[0]);
-	     i++)
-		memset(&adapter->wep_keys[i], 0, sizeof(struct WLAN_802_11_KEY));
-	adapter->wep_tx_keyidx = 0;
-	adapter->secinfo.auth_mode = IW_AUTH_ALG_OPEN_SYSTEM;
-	adapter->mode = IW_MODE_INFRA;
-
-	adapter->pending_assoc_req = NULL;
-	adapter->in_progress_assoc_req = NULL;
-
-	/* Initialize scan result lists */
-	INIT_LIST_HEAD(&adapter->network_free_list);
-	INIT_LIST_HEAD(&adapter->network_list);
-	for (i = 0; i < MAX_NETWORK_COUNT; i++) {
-		list_add_tail(&adapter->networks[i].list,
-			      &adapter->network_free_list);
-	}
-
-	mutex_init(&adapter->lock);
-
-	adapter->prescan = 1;
-
-	memset(&adapter->curbssparams, 0, sizeof(adapter->curbssparams));
-	adapter->curbssparams.channel = DEFAULT_AD_HOC_CHANNEL;
-
-	/* PnP and power profile */
-	adapter->surpriseremoved = 0;
-
-	adapter->currentpacketfilter =
-	    cmd_act_mac_rx_on | cmd_act_mac_tx_on;
-
-	adapter->radioon = RADIO_ON;
-	adapter->txantenna = RF_ANTENNA_2;
-	adapter->rxantenna = RF_ANTENNA_AUTO;
-
-	adapter->is_datarate_auto = 1;
-	adapter->beaconperiod = MRVDRV_BEACON_INTERVAL;
-
-	// set default value of capinfo.
-#define SHORT_PREAMBLE_ALLOWED		1
-	memset(&adapter->capinfo, 0, sizeof(adapter->capinfo));
-	adapter->capinfo.shortpreamble = SHORT_PREAMBLE_ALLOWED;
-
-	adapter->psmode = wlan802_11powermodecam;
-	adapter->multipledtim = MRVDRV_DEFAULT_MULTIPLE_DTIM;
-
-	adapter->listeninterval = MRVDRV_DEFAULT_LISTEN_INTERVAL;
-
-	adapter->psstate = PS_STATE_FULL_POWER;
-	adapter->needtowakeup = 0;
-	adapter->locallisteninterval = 0;	/* default value in firmware will be used */
-
-	adapter->datarate = 0;	// Initially indicate the rate as auto
-
-	adapter->adhoc_grate_enabled = 0;
-
-	adapter->intcounter = 0;
-
-	adapter->currenttxskb = NULL;
-	adapter->pkttxctrl = 0;
-
-	memset(&adapter->tx_queue_ps, 0, NR_TX_QUEUE*sizeof(struct sk_buff*));
-	adapter->tx_queue_idx = 0;
-	spin_lock_init(&adapter->txqueue_lock);
-
-	return;
-}
-
-static void command_timer_fn(unsigned long data);
-
-int libertas_init_fw(wlan_private * priv, char *fw_name)
-{
-	int ret = -1;
-	wlan_adapter *adapter = priv->adapter;
-
-	lbs_deb_enter(LBS_DEB_FW);
-
-	/* Allocate adapter structure */
-	if ((ret = wlan_allocate_adapter(priv)) != 0)
-		goto done;
-
-	/* init adapter structure */
-	wlan_init_adapter(priv);
-
-	/* init timer etc. */
-	setup_timer(&adapter->command_timer, command_timer_fn,
-			(unsigned long)priv);
-
-	/* download fimrware etc. */
-	if ((ret = wlan_setup_station_hw(priv, fw_name)) != 0) {
-		del_timer_sync(&adapter->command_timer);
-		goto done;
-	}
-
-	/* init 802.11d */
-	libertas_init_11d(priv);
-
-	ret = 0;
-done:
-	lbs_deb_leave_args(LBS_DEB_FW, "ret %d", ret);
-	return ret;
-}
-
-void libertas_free_adapter(wlan_private * priv)
-{
-	wlan_adapter *adapter = priv->adapter;
-
-	if (!adapter) {
-		lbs_deb_fw("why double free adapter?\n");
-		return;
-	}
-
-	lbs_deb_fw("free command buffer\n");
-	libertas_free_cmd_buffer(priv);
-
-	lbs_deb_fw("free command_timer\n");
-	del_timer(&adapter->command_timer);
-
-	lbs_deb_fw("free scan results table\n");
-	kfree(adapter->networks);
-	adapter->networks = NULL;
-
-	/* Free the adapter object itself */
-	lbs_deb_fw("free adapter\n");
-	kfree(adapter);
-	priv->adapter = NULL;
-}
-
-/**
- *  This function handles the timeout of command sending.
- *  It will re-send the same command again.
- */
-static void command_timer_fn(unsigned long data)
-{
-	wlan_private *priv = (wlan_private *)data;
-	wlan_adapter *adapter = priv->adapter;
-	struct cmd_ctrl_node *ptempnode;
-	struct cmd_ds_command *cmd;
-	unsigned long flags;
-
-	ptempnode = adapter->cur_cmd;
-	if (ptempnode == NULL) {
-		lbs_deb_fw("ptempnode empty\n");
-		return;
-	}
-
-	cmd = (struct cmd_ds_command *)ptempnode->bufvirtualaddr;
-	if (!cmd) {
-		lbs_deb_fw("cmd is NULL\n");
-		return;
-	}
-
-	lbs_deb_fw("command_timer_fn fired, cmd %x\n", cmd->command);
-
-	if (!adapter->fw_ready)
-		return;
-
-	spin_lock_irqsave(&adapter->driver_lock, flags);
-	adapter->cur_cmd = NULL;
-	spin_unlock_irqrestore(&adapter->driver_lock, flags);
-
-	lbs_deb_fw("re-sending same command because of timeout\n");
-	libertas_queue_cmd(adapter, ptempnode, 0);
-
-	wake_up_interruptible(&priv->mainthread.waitq);
-
-	return;
-}
diff --git a/drivers/net/wireless/libertas/host.h b/drivers/net/wireless/libertas/host.h
index 7509cc10af3c..b37ddbca969f 100644
--- a/drivers/net/wireless/libertas/host.h
+++ b/drivers/net/wireless/libertas/host.h
@@ -20,224 +20,163 @@
 #define OID_802_11_TX_RETRYCOUNT              0x0000801D
 #define OID_802_11D_ENABLE                    0x00008020
 
-#define cmd_option_waitforrsp             0x0002
+#define CMD_OPTION_WAITFORRSP             0x0002
 
-/** Host command ID */
-#define cmd_code_dnld                 0x0002
-#define cmd_get_hw_spec               0x0003
-#define cmd_eeprom_update             0x0004
-#define cmd_802_11_reset              0x0005
-#define cmd_802_11_scan               0x0006
-#define cmd_802_11_get_log            0x000b
-#define cmd_mac_multicast_adr         0x0010
-#define cmd_802_11_authenticate       0x0011
-#define cmd_802_11_eeprom_access      0x0059
-#define cmd_802_11_associate          0x0050
-#define cmd_802_11_set_wep            0x0013
-#define cmd_802_11_get_stat           0x0014
-#define cmd_802_3_get_stat            0x0015
-#define cmd_802_11_snmp_mib           0x0016
-#define cmd_mac_reg_map               0x0017
-#define cmd_bbp_reg_map               0x0018
-#define cmd_mac_reg_access            0x0019
-#define cmd_bbp_reg_access            0x001a
-#define cmd_rf_reg_access             0x001b
-#define cmd_802_11_radio_control      0x001c
-#define cmd_802_11_rf_channel         0x001d
-#define cmd_802_11_rf_tx_power        0x001e
-#define cmd_802_11_rssi               0x001f
-#define cmd_802_11_rf_antenna         0x0020
+/** Host command IDs */
 
-#define cmd_802_11_ps_mode	      0x0021
+/* Return command are almost always the same as the host command, but with
+ * bit 15 set high.  There are a few exceptions, though...
+ */
+#define CMD_RET(cmd)			(0x8000 | cmd)
 
-#define cmd_802_11_data_rate          0x0022
-#define cmd_rf_reg_map                0x0023
-#define cmd_802_11_deauthenticate     0x0024
-#define cmd_802_11_reassociate        0x0025
-#define cmd_802_11_disassociate       0x0026
-#define cmd_mac_control               0x0028
-#define cmd_802_11_ad_hoc_start       0x002b
-#define cmd_802_11_ad_hoc_join        0x002c
+/* Return command convention exceptions: */
+#define CMD_RET_802_11_ASSOCIATE      0x8012
 
-#define cmd_802_11_query_tkip_reply_cntrs  0x002e
-#define cmd_802_11_enable_rsn              0x002f
-#define cmd_802_11_pairwise_tsc       0x0036
-#define cmd_802_11_group_tsc          0x0037
-#define cmd_802_11_key_material       0x005e
+/* Command codes */
+#define CMD_CODE_DNLD                 0x0002
+#define CMD_GET_HW_SPEC               0x0003
+#define CMD_EEPROM_UPDATE             0x0004
+#define CMD_802_11_RESET              0x0005
+#define CMD_802_11_SCAN               0x0006
+#define CMD_802_11_GET_LOG            0x000b
+#define CMD_MAC_MULTICAST_ADR         0x0010
+#define CMD_802_11_AUTHENTICATE       0x0011
+#define CMD_802_11_EEPROM_ACCESS      0x0059
+#define CMD_802_11_ASSOCIATE          0x0050
+#define CMD_802_11_SET_WEP            0x0013
+#define CMD_802_11_GET_STAT           0x0014
+#define CMD_802_3_GET_STAT            0x0015
+#define CMD_802_11_SNMP_MIB           0x0016
+#define CMD_MAC_REG_MAP               0x0017
+#define CMD_BBP_REG_MAP               0x0018
+#define CMD_MAC_REG_ACCESS            0x0019
+#define CMD_BBP_REG_ACCESS            0x001a
+#define CMD_RF_REG_ACCESS             0x001b
+#define CMD_802_11_RADIO_CONTROL      0x001c
+#define CMD_802_11_RF_CHANNEL         0x001d
+#define CMD_802_11_RF_TX_POWER        0x001e
+#define CMD_802_11_RSSI               0x001f
+#define CMD_802_11_RF_ANTENNA         0x0020
 
-#define cmd_802_11_set_afc            0x003c
-#define cmd_802_11_get_afc            0x003d
+#define CMD_802_11_PS_MODE	      0x0021
 
-#define cmd_802_11_ad_hoc_stop        0x0040
+#define CMD_802_11_DATA_RATE          0x0022
+#define CMD_RF_REG_MAP                0x0023
+#define CMD_802_11_DEAUTHENTICATE     0x0024
+#define CMD_802_11_REASSOCIATE        0x0025
+#define CMD_802_11_DISASSOCIATE       0x0026
+#define CMD_MAC_CONTROL               0x0028
+#define CMD_802_11_AD_HOC_START       0x002b
+#define CMD_802_11_AD_HOC_JOIN        0x002c
 
-#define cmd_802_11_beacon_stop        0x0049
+#define CMD_802_11_QUERY_TKIP_REPLY_CNTRS  0x002e
+#define CMD_802_11_ENABLE_RSN              0x002f
+#define CMD_802_11_PAIRWISE_TSC       0x0036
+#define CMD_802_11_GROUP_TSC          0x0037
+#define CMD_802_11_KEY_MATERIAL       0x005e
 
-#define cmd_802_11_mac_address        0x004D
-#define cmd_802_11_eeprom_access      0x0059
+#define CMD_802_11_SET_AFC            0x003c
+#define CMD_802_11_GET_AFC            0x003d
 
-#define cmd_802_11_band_config        0x0058
+#define CMD_802_11_AD_HOC_STOP        0x0040
 
-#define cmd_802_11d_domain_info       0x005b
+#define CMD_802_11_BEACON_STOP        0x0049
 
-#define cmd_802_11_sleep_params          0x0066
+#define CMD_802_11_MAC_ADDRESS        0x004D
+#define CMD_802_11_EEPROM_ACCESS      0x0059
 
-#define cmd_802_11_inactivity_timeout    0x0067
+#define CMD_802_11_BAND_CONFIG        0x0058
 
-#define cmd_802_11_tpc_cfg               0x0072
-#define cmd_802_11_pwr_cfg               0x0073
+#define CMD_802_11D_DOMAIN_INFO       0x005b
 
-#define cmd_802_11_led_gpio_ctrl         0x004e
+#define CMD_802_11_SLEEP_PARAMS          0x0066
 
-#define cmd_802_11_subscribe_event       0x0075
+#define CMD_802_11_INACTIVITY_TIMEOUT    0x0067
 
-#define cmd_802_11_rate_adapt_rateset    0x0076
+#define CMD_802_11_TPC_CFG               0x0072
+#define CMD_802_11_PWR_CFG               0x0073
 
-#define cmd_802_11_tx_rate_query	0x007f
+#define CMD_802_11_LED_GPIO_CTRL         0x004e
 
-#define cmd_get_tsf                      0x0080
+#define CMD_802_11_SUBSCRIBE_EVENT       0x0075
 
-#define cmd_bt_access                 0x0087
-#define cmd_ret_bt_access                 0x8087
+#define CMD_802_11_RATE_ADAPT_RATESET    0x0076
 
-#define cmd_fwt_access                0x0095
-#define cmd_ret_fwt_access                0x8095
+#define CMD_802_11_TX_RATE_QUERY	0x007f
 
-#define cmd_mesh_access               0x009b
-#define cmd_ret_mesh_access               0x809b
+#define CMD_GET_TSF                      0x0080
+
+#define CMD_BT_ACCESS                 0x0087
+
+#define CMD_FWT_ACCESS                0x0095
+
+#define CMD_802_11_MONITOR_MODE       0x0098
+
+#define CMD_MESH_ACCESS               0x009b
+
+#define CMD_SET_BOOT2_VER                 0x00a5
 
 /* For the IEEE Power Save */
-#define cmd_subcmd_enter_ps               0x0030
-#define cmd_subcmd_exit_ps                0x0031
-#define cmd_subcmd_sleep_confirmed        0x0034
-#define cmd_subcmd_full_powerdown         0x0035
-#define cmd_subcmd_full_powerup           0x0036
+#define CMD_SUBCMD_ENTER_PS               0x0030
+#define CMD_SUBCMD_EXIT_PS                0x0031
+#define CMD_SUBCMD_SLEEP_CONFIRMED        0x0034
+#define CMD_SUBCMD_FULL_POWERDOWN         0x0035
+#define CMD_SUBCMD_FULL_POWERUP           0x0036
+
+#define CMD_ENABLE_RSN                    0x0001
+#define CMD_DISABLE_RSN                   0x0000
+
+#define CMD_ACT_SET                       0x0001
+#define CMD_ACT_GET                       0x0000
+
+#define CMD_ACT_GET_AES                   (CMD_ACT_GET + 2)
+#define CMD_ACT_SET_AES                   (CMD_ACT_SET + 2)
+#define CMD_ACT_REMOVE_AES                (CMD_ACT_SET + 3)
+
+/* Define action or option for CMD_802_11_SET_WEP */
+#define CMD_ACT_ADD                         0x0002
+#define CMD_ACT_REMOVE                      0x0004
+#define CMD_ACT_USE_DEFAULT                 0x0008
+
+#define CMD_TYPE_WEP_40_BIT                 0x01
+#define CMD_TYPE_WEP_104_BIT                0x02
+
+#define CMD_NUM_OF_WEP_KEYS                 4
+
+#define CMD_WEP_KEY_INDEX_MASK              0x3fff
 
-/* command RET code, MSB is set to 1 */
-#define cmd_ret_hw_spec_info              0x8003
-#define cmd_ret_eeprom_update             0x8004
-#define cmd_ret_802_11_reset              0x8005
-#define cmd_ret_802_11_scan               0x8006
-#define cmd_ret_802_11_get_log            0x800b
-#define cmd_ret_mac_control               0x8028
-#define cmd_ret_mac_multicast_adr         0x8010
-#define cmd_ret_802_11_authenticate       0x8011
-#define cmd_ret_802_11_deauthenticate     0x8024
-#define cmd_ret_802_11_associate          0x8012
-#define cmd_ret_802_11_reassociate        0x8025
-#define cmd_ret_802_11_disassociate       0x8026
-#define cmd_ret_802_11_set_wep            0x8013
-#define cmd_ret_802_11_stat               0x8014
-#define cmd_ret_802_3_stat                0x8015
-#define cmd_ret_802_11_snmp_mib           0x8016
-#define cmd_ret_mac_reg_map               0x8017
-#define cmd_ret_bbp_reg_map               0x8018
-#define cmd_ret_rf_reg_map                0x8023
-#define cmd_ret_mac_reg_access            0x8019
-#define cmd_ret_bbp_reg_access            0x801a
-#define cmd_ret_rf_reg_access             0x801b
-#define cmd_ret_802_11_radio_control      0x801c
-#define cmd_ret_802_11_rf_channel         0x801d
-#define cmd_ret_802_11_rssi               0x801f
-#define cmd_ret_802_11_rf_tx_power        0x801e
-#define cmd_ret_802_11_rf_antenna         0x8020
-#define cmd_ret_802_11_ps_mode            0x8021
-#define cmd_ret_802_11_data_rate          0x8022
-
-#define cmd_ret_802_11_ad_hoc_start       0x802B
-#define cmd_ret_802_11_ad_hoc_join        0x802C
-
-#define cmd_ret_802_11_query_tkip_reply_cntrs  0x802e
-#define cmd_ret_802_11_enable_rsn              0x802f
-#define cmd_ret_802_11_pairwise_tsc       0x8036
-#define cmd_ret_802_11_group_tsc          0x8037
-#define cmd_ret_802_11_key_material       0x805e
-
-#define cmd_enable_rsn                    0x0001
-#define cmd_disable_rsn                   0x0000
-
-#define cmd_act_set                       0x0001
-#define cmd_act_get                       0x0000
-
-#define cmd_act_get_AES                   (cmd_act_get + 2)
-#define cmd_act_set_AES                   (cmd_act_set + 2)
-#define cmd_act_remove_aes                (cmd_act_set + 3)
-
-#define cmd_ret_802_11_set_afc            0x803c
-#define cmd_ret_802_11_get_afc            0x803d
-
-#define cmd_ret_802_11_ad_hoc_stop        0x8040
-
-#define cmd_ret_802_11_beacon_stop        0x8049
-
-#define cmd_ret_802_11_mac_address        0x804D
-#define cmd_ret_802_11_eeprom_access      0x8059
-
-#define cmd_ret_802_11_band_config        0x8058
-
-#define cmd_ret_802_11_sleep_params          0x8066
-
-#define cmd_ret_802_11_inactivity_timeout    0x8067
-
-#define cmd_ret_802_11d_domain_info      (0x8000 |                  \
-                                              cmd_802_11d_domain_info)
-
-#define cmd_ret_802_11_tpc_cfg        (cmd_802_11_tpc_cfg | 0x8000)
-#define cmd_ret_802_11_pwr_cfg        (cmd_802_11_pwr_cfg | 0x8000)
-
-#define cmd_ret_802_11_led_gpio_ctrl     0x804e
-
-#define cmd_ret_802_11_subscribe_event	(cmd_802_11_subscribe_event | 0x8000)
-
-#define cmd_ret_802_11_rate_adapt_rateset	(cmd_802_11_rate_adapt_rateset | 0x8000)
-
-#define cmd_rte_802_11_tx_rate_query 	(cmd_802_11_tx_rate_query | 0x8000)
-
-#define cmd_ret_get_tsf             0x8080
-
-/* Define action or option for cmd_802_11_set_wep */
-#define cmd_act_add                         0x0002
-#define cmd_act_remove                      0x0004
-#define cmd_act_use_default                 0x0008
-
-#define cmd_type_wep_40_bit                 0x0001
-#define cmd_type_wep_104_bit                0x0002
-
-#define cmd_NUM_OF_WEP_KEYS                 4
-
-#define cmd_WEP_KEY_INDEX_MASK              0x3fff
-
-/* Define action or option for cmd_802_11_reset */
-#define cmd_act_halt                        0x0003
+/* Define action or option for CMD_802_11_RESET */
+#define CMD_ACT_HALT                        0x0003
 
-/* Define action or option for cmd_802_11_scan */
-#define cmd_bss_type_bss                    0x0001
-#define cmd_bss_type_ibss                   0x0002
-#define cmd_bss_type_any                    0x0003
+/* Define action or option for CMD_802_11_SCAN */
+#define CMD_BSS_TYPE_BSS                    0x0001
+#define CMD_BSS_TYPE_IBSS                   0x0002
+#define CMD_BSS_TYPE_ANY                    0x0003
 
-/* Define action or option for cmd_802_11_scan */
-#define cmd_scan_type_active                0x0000
-#define cmd_scan_type_passive               0x0001
+/* Define action or option for CMD_802_11_SCAN */
+#define CMD_SCAN_TYPE_ACTIVE                0x0000
+#define CMD_SCAN_TYPE_PASSIVE               0x0001
 
-#define cmd_scan_radio_type_bg		0
+#define CMD_SCAN_RADIO_TYPE_BG		0
 
-#define cmd_scan_probe_delay_time           0
+#define CMD_SCAN_PROBE_DELAY_TIME           0
 
-/* Define action or option for cmd_mac_control */
-#define cmd_act_mac_rx_on                   0x0001
-#define cmd_act_mac_tx_on                   0x0002
-#define cmd_act_mac_loopback_on             0x0004
-#define cmd_act_mac_wep_enable              0x0008
-#define cmd_act_mac_int_enable              0x0010
-#define cmd_act_mac_multicast_enable        0x0020
-#define cmd_act_mac_broadcast_enable        0x0040
-#define cmd_act_mac_promiscuous_enable      0x0080
-#define cmd_act_mac_all_multicast_enable    0x0100
-#define cmd_act_mac_strict_protection_enable  0x0400
+/* Define action or option for CMD_MAC_CONTROL */
+#define CMD_ACT_MAC_RX_ON                   0x0001
+#define CMD_ACT_MAC_TX_ON                   0x0002
+#define CMD_ACT_MAC_LOOPBACK_ON             0x0004
+#define CMD_ACT_MAC_WEP_ENABLE              0x0008
+#define CMD_ACT_MAC_INT_ENABLE              0x0010
+#define CMD_ACT_MAC_MULTICAST_ENABLE        0x0020
+#define CMD_ACT_MAC_BROADCAST_ENABLE        0x0040
+#define CMD_ACT_MAC_PROMISCUOUS_ENABLE      0x0080
+#define CMD_ACT_MAC_ALL_MULTICAST_ENABLE    0x0100
+#define CMD_ACT_MAC_STRICT_PROTECTION_ENABLE  0x0400
 
-/* Define action or option for cmd_802_11_radio_control */
-#define cmd_type_auto_preamble              0x0001
-#define cmd_type_short_preamble             0x0002
-#define cmd_type_long_preamble              0x0003
+/* Define action or option for CMD_802_11_RADIO_CONTROL */
+#define CMD_TYPE_AUTO_PREAMBLE              0x0001
+#define CMD_TYPE_SHORT_PREAMBLE             0x0002
+#define CMD_TYPE_LONG_PREAMBLE              0x0003
 
 #define TURN_ON_RF                              0x01
 #define RADIO_ON                                0x01
@@ -248,70 +187,80 @@
 #define SET_LONG_PREAMBLE                       0x01
 
 /* Define action or option for CMD_802_11_RF_CHANNEL */
-#define cmd_opt_802_11_rf_channel_get       0x00
-#define cmd_opt_802_11_rf_channel_set       0x01
-
-/* Define action or option for cmd_802_11_rf_tx_power */
-#define cmd_act_tx_power_opt_get            0x0000
-#define cmd_act_tx_power_opt_set_high       0x8007
-#define cmd_act_tx_power_opt_set_mid        0x8004
-#define cmd_act_tx_power_opt_set_low        0x8000
-
-#define cmd_act_tx_power_index_high         0x0007
-#define cmd_act_tx_power_index_mid          0x0004
-#define cmd_act_tx_power_index_low          0x0000
-
-/* Define action or option for cmd_802_11_data_rate */
-#define cmd_act_set_tx_auto                 0x0000
-#define cmd_act_set_tx_fix_rate             0x0001
-#define cmd_act_get_tx_rate                 0x0002
-
-#define cmd_act_set_rx                      0x0001
-#define cmd_act_set_tx                      0x0002
-#define cmd_act_set_both                    0x0003
-#define cmd_act_get_rx                      0x0004
-#define cmd_act_get_tx                      0x0008
-#define cmd_act_get_both                    0x000c
-
-/* Define action or option for cmd_802_11_ps_mode */
-#define cmd_type_cam                        0x0000
-#define cmd_type_max_psp                    0x0001
-#define cmd_type_fast_psp                   0x0002
-
-/* Define action or option for cmd_bt_access */
+#define CMD_OPT_802_11_RF_CHANNEL_GET       0x00
+#define CMD_OPT_802_11_RF_CHANNEL_SET       0x01
+
+/* Define action or option for CMD_802_11_RF_TX_POWER */
+#define CMD_ACT_TX_POWER_OPT_GET            0x0000
+#define CMD_ACT_TX_POWER_OPT_SET_HIGH       0x8007
+#define CMD_ACT_TX_POWER_OPT_SET_MID        0x8004
+#define CMD_ACT_TX_POWER_OPT_SET_LOW        0x8000
+
+#define CMD_ACT_TX_POWER_INDEX_HIGH         0x0007
+#define CMD_ACT_TX_POWER_INDEX_MID          0x0004
+#define CMD_ACT_TX_POWER_INDEX_LOW          0x0000
+
+/* Define action or option for CMD_802_11_DATA_RATE */
+#define CMD_ACT_SET_TX_AUTO                 0x0000
+#define CMD_ACT_SET_TX_FIX_RATE             0x0001
+#define CMD_ACT_GET_TX_RATE                 0x0002
+
+#define CMD_ACT_SET_RX                      0x0001
+#define CMD_ACT_SET_TX                      0x0002
+#define CMD_ACT_SET_BOTH                    0x0003
+#define CMD_ACT_GET_RX                      0x0004
+#define CMD_ACT_GET_TX                      0x0008
+#define CMD_ACT_GET_BOTH                    0x000c
+
+/* Define action or option for CMD_802_11_PS_MODE */
+#define CMD_TYPE_CAM                        0x0000
+#define CMD_TYPE_MAX_PSP                    0x0001
+#define CMD_TYPE_FAST_PSP                   0x0002
+
+/* Define action or option for CMD_BT_ACCESS */
 enum cmd_bt_access_opts {
 	/* The bt commands start at 5 instead of 1 because the old dft commands
 	 * are mapped to 1-4.  These old commands are no longer maintained and
 	 * should not be called.
 	 */
-	cmd_act_bt_access_add = 5,
-	cmd_act_bt_access_del,
-	cmd_act_bt_access_list,
-	cmd_act_bt_access_reset,
-	cmd_act_bt_access_set_invert,
-	cmd_act_bt_access_get_invert
+	CMD_ACT_BT_ACCESS_ADD = 5,
+	CMD_ACT_BT_ACCESS_DEL,
+	CMD_ACT_BT_ACCESS_LIST,
+	CMD_ACT_BT_ACCESS_RESET,
+	CMD_ACT_BT_ACCESS_SET_INVERT,
+	CMD_ACT_BT_ACCESS_GET_INVERT
 };
 
-/* Define action or option for cmd_fwt_access */
+/* Define action or option for CMD_FWT_ACCESS */
 enum cmd_fwt_access_opts {
-	cmd_act_fwt_access_add = 1,
-	cmd_act_fwt_access_del,
-	cmd_act_fwt_access_lookup,
-	cmd_act_fwt_access_list,
-	cmd_act_fwt_access_list_route,
-	cmd_act_fwt_access_list_neighbor,
-	cmd_act_fwt_access_reset,
-	cmd_act_fwt_access_cleanup,
-	cmd_act_fwt_access_time,
+	CMD_ACT_FWT_ACCESS_ADD = 1,
+	CMD_ACT_FWT_ACCESS_DEL,
+	CMD_ACT_FWT_ACCESS_LOOKUP,
+	CMD_ACT_FWT_ACCESS_LIST,
+	CMD_ACT_FWT_ACCESS_LIST_route,
+	CMD_ACT_FWT_ACCESS_LIST_neighbor,
+	CMD_ACT_FWT_ACCESS_RESET,
+	CMD_ACT_FWT_ACCESS_CLEANUP,
+	CMD_ACT_FWT_ACCESS_TIME,
 };
 
-/* Define action or option for cmd_mesh_access */
+/* Define action or option for CMD_MESH_ACCESS */
 enum cmd_mesh_access_opts {
-	cmd_act_mesh_get_ttl = 1,
-	cmd_act_mesh_set_ttl,
-	cmd_act_mesh_get_stats,
-	cmd_act_mesh_get_anycast,
-	cmd_act_mesh_set_anycast,
+	CMD_ACT_MESH_GET_TTL = 1,
+	CMD_ACT_MESH_SET_TTL,
+	CMD_ACT_MESH_GET_STATS,
+	CMD_ACT_MESH_GET_ANYCAST,
+	CMD_ACT_MESH_SET_ANYCAST,
+	CMD_ACT_MESH_SET_LINK_COSTS,
+	CMD_ACT_MESH_GET_LINK_COSTS,
+	CMD_ACT_MESH_SET_BCAST_RATE,
+	CMD_ACT_MESH_GET_BCAST_RATE,
+	CMD_ACT_MESH_SET_RREQ_DELAY,
+	CMD_ACT_MESH_GET_RREQ_DELAY,
+	CMD_ACT_MESH_SET_ROUTE_EXP,
+	CMD_ACT_MESH_GET_ROUTE_EXP,
+	CMD_ACT_MESH_SET_AUTOSTART_ENABLED,
+	CMD_ACT_MESH_GET_AUTOSTART_ENABLED,
 };
 
 /** Card Event definition */
diff --git a/drivers/net/wireless/libertas/hostcmd.h b/drivers/net/wireless/libertas/hostcmd.h
index 09b898f6719c..e1045dc02cce 100644
--- a/drivers/net/wireless/libertas/hostcmd.h
+++ b/drivers/net/wireless/libertas/hostcmd.h
@@ -83,23 +83,12 @@ struct cmd_ctrl_node {
 	wait_queue_head_t cmdwait_q;
 };
 
-/* WLAN_802_11_KEY
- *
- * Generic structure to hold all key types.  key type (WEP40, WEP104, TKIP, AES)
- * is determined from the keylength field.
- */
-struct WLAN_802_11_KEY {
-	__le32 len;
-	__le32 flags;  /* KEY_INFO_* from wlan_defs.h */
-	u8 key[MRVL_MAX_KEY_WPA_KEY_LENGTH];
-	__le16 type; /* KEY_TYPE_* from wlan_defs.h */
-};
-
-struct IE_WPA {
-	u8 elementid;
-	u8 len;
-	u8 oui[4];
-	__le16 version;
+/* Generic structure to hold all key types. */
+struct enc_key {
+	u16 len;
+	u16 flags;  /* KEY_INFO_* from wlan_defs.h */
+	u16 type; /* KEY_TYPE_* from wlan_defs.h */
+	u8 key[32];
 };
 
 /* wlan_offset_value */
@@ -108,18 +97,6 @@ struct wlan_offset_value {
 	u32 value;
 };
 
-struct WLAN_802_11_FIXED_IEs {
-	__le64 timestamp;
-	__le16 beaconinterval;
-	u16 capabilities; /* Actually struct ieeetypes_capinfo */
-};
-
-struct WLAN_802_11_VARIABLE_IEs {
-	u8 elementid;
-	u8 length;
-	u8 data[1];
-};
-
 /* Define general data structure */
 /* cmd_DS_GEN */
 struct cmd_ds_gen {
@@ -131,7 +108,7 @@ struct cmd_ds_gen {
 
 #define S_DS_GEN sizeof(struct cmd_ds_gen)
 /*
- * Define data structure for cmd_get_hw_spec
+ * Define data structure for CMD_GET_HW_SPEC
  * This structure defines the response for the GET_HW_SPEC command
  */
 struct cmd_ds_get_hw_spec {
@@ -178,11 +155,11 @@ struct cmd_ds_802_11_subscribe_event {
 
 /*
  * This scan handle Country Information IE(802.11d compliant)
- * Define data structure for cmd_802_11_scan
+ * Define data structure for CMD_802_11_SCAN
  */
 struct cmd_ds_802_11_scan {
 	u8 bsstype;
-	u8 BSSID[ETH_ALEN];
+	u8 bssid[ETH_ALEN];
 	u8 tlvbuffer[1];
 #if 0
 	mrvlietypes_ssidparamset_t ssidParamSet;
@@ -237,7 +214,7 @@ struct cmd_ds_802_11_deauthenticate {
 
 struct cmd_ds_802_11_associate {
 	u8 peerstaaddr[6];
-	struct ieeetypes_capinfo capinfo;
+	__le16 capability;
 	__le16 listeninterval;
 	__le16 bcnperiod;
 	u8 dtimperiod;
@@ -260,8 +237,8 @@ struct cmd_ds_802_11_associate_rsp {
 };
 
 struct cmd_ds_802_11_ad_hoc_result {
-	u8 PAD[3];
-	u8 BSSID[ETH_ALEN];
+	u8 pad[3];
+	u8 bssid[ETH_ALEN];
 };
 
 struct cmd_ds_802_11_set_wep {
@@ -428,6 +405,16 @@ struct cmd_ds_802_11_rf_antenna {
 
 };
 
+struct cmd_ds_802_11_monitor_mode {
+	u16 action;
+	u16 mode;
+};
+
+struct cmd_ds_set_boot2_ver {
+	u16 action;
+	u16 version;
+};
+
 struct cmd_ds_802_11_ps_mode {
 	__le16 action;
 	__le16 nullpktinterval;
@@ -451,8 +438,8 @@ struct PS_CMD_ConfirmSleep {
 
 struct cmd_ds_802_11_data_rate {
 	__le16 action;
-	__le16 reserverd;
-	u8 datarate[G_SUPPORTED_RATES];
+	__le16 reserved;
+	u8 rates[MAX_RATES];
 };
 
 struct cmd_ds_802_11_rate_adapt_rateset {
@@ -462,30 +449,30 @@ struct cmd_ds_802_11_rate_adapt_rateset {
 };
 
 struct cmd_ds_802_11_ad_hoc_start {
-	u8 SSID[IW_ESSID_MAX_SIZE];
+	u8 ssid[IW_ESSID_MAX_SIZE];
 	u8 bsstype;
 	__le16 beaconperiod;
 	u8 dtimperiod;
 	union IEEEtypes_ssparamset ssparamset;
 	union ieeetypes_phyparamset phyparamset;
 	__le16 probedelay;
-	struct ieeetypes_capinfo cap;
-	u8 datarate[G_SUPPORTED_RATES];
+	__le16 capability;
+	u8 rates[MAX_RATES];
 	u8 tlv_memory_size_pad[100];
 } __attribute__ ((packed));
 
 struct adhoc_bssdesc {
-	u8 BSSID[6];
-	u8 SSID[32];
-	u8 bsstype;
+	u8 bssid[6];
+	u8 ssid[32];
+	u8 type;
 	__le16 beaconperiod;
 	u8 dtimperiod;
 	__le64 timestamp;
 	__le64 localtime;
 	union ieeetypes_phyparamset phyparamset;
 	union IEEEtypes_ssparamset ssparamset;
-	struct ieeetypes_capinfo cap;
-	u8 datarates[G_SUPPORTED_RATES];
+	__le16 capability;
+	u8 rates[MAX_RATES];
 
 	/* DO NOT ADD ANY FIELDS TO THIS STRUCTURE. It is used below in the
 	 * Adhoc join command and will cause a binary layout mismatch with
@@ -494,7 +481,7 @@ struct adhoc_bssdesc {
 } __attribute__ ((packed));
 
 struct cmd_ds_802_11_ad_hoc_join {
-	struct adhoc_bssdesc bssdescriptor;
+	struct adhoc_bssdesc bss;
 	__le16 failtimeout;
 	__le16 probedelay;
 
@@ -646,6 +633,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;
@@ -677,6 +665,7 @@ struct cmd_ds_command {
 		struct cmd_ds_bt_access bt;
 		struct cmd_ds_fwt_access fwt;
 		struct cmd_ds_mesh_access mesh;
+		struct cmd_ds_set_boot2_ver boot2_ver;
 		struct cmd_ds_get_tsf gettsf;
 		struct cmd_ds_802_11_subscribe_event subscribe_event;
 	} params;
diff --git a/drivers/net/wireless/libertas/if_bootcmd.c b/drivers/net/wireless/libertas/if_bootcmd.c
deleted file mode 100644
index 8bca306ffad9..000000000000
--- a/drivers/net/wireless/libertas/if_bootcmd.c
+++ /dev/null
@@ -1,40 +0,0 @@
-/**
-  * This file contains functions used in USB Boot command
-  * and Boot2/FW update
-  */
-
-#include <linux/delay.h>
-#include <linux/firmware.h>
-#include <linux/netdevice.h>
-#include <linux/usb.h>
-
-#define DRV_NAME "usb8xxx"
-
-#include "defs.h"
-#include "dev.h"
-#include "if_usb.h"
-
-/**
- *  @brief This function issues Boot command to the Boot2 code
- *  @param ivalue   1:Boot from FW by USB-Download
- *                  2:Boot from FW in EEPROM
- *  @return 	   	0
- */
-int if_usb_issue_boot_command(wlan_private *priv, int ivalue)
-{
-	struct usb_card_rec	*cardp = priv->card;
-	struct bootcmdstr	sbootcmd;
-	int i;
-
-	/* Prepare command */
-	sbootcmd.u32magicnumber = cpu_to_le32(BOOT_CMD_MAGIC_NUMBER);
-	sbootcmd.u8cmd_tag = ivalue;
-	for (i=0; i<11; i++)
-		sbootcmd.au8dumy[i]=0x00;
-	memcpy(cardp->bulk_out_buffer, &sbootcmd, sizeof(struct bootcmdstr));
-
-	/* Issue command */
-	usb_tx_block(priv, cardp->bulk_out_buffer, sizeof(struct bootcmdstr));
-
-	return 0;
-}
diff --git a/drivers/net/wireless/libertas/if_cs.c b/drivers/net/wireless/libertas/if_cs.c
new file mode 100644
index 000000000000..0360cad363a8
--- /dev/null
+++ b/drivers/net/wireless/libertas/if_cs.c
@@ -0,0 +1,969 @@
+/*
+
+  Driver for the Marvell 8385 based compact flash WLAN cards.
+
+  (C) 2007 by Holger Schurig <hs4233@mail.mn-solutions.de>
+
+  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, or
+  (at your option) any later version.
+
+  This program is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+  GNU General Public License for more details.
+
+  You should have received a copy of the GNU General Public License
+  along with this program; see the file COPYING.  If not, write to
+  the Free Software Foundation, Inc., 51 Franklin Steet, Fifth Floor,
+  Boston, MA 02110-1301, USA.
+
+*/
+
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/moduleparam.h>
+#include <linux/firmware.h>
+#include <linux/netdevice.h>
+
+#include <pcmcia/cs_types.h>
+#include <pcmcia/cs.h>
+#include <pcmcia/cistpl.h>
+#include <pcmcia/ds.h>
+
+#include <linux/io.h>
+
+#define DRV_NAME "libertas_cs"
+
+#include "decl.h"
+#include "defs.h"
+#include "dev.h"
+
+
+/********************************************************************/
+/* Module stuff                                                     */
+/********************************************************************/
+
+MODULE_AUTHOR("Holger Schurig <hs4233@mail.mn-solutions.de>");
+MODULE_DESCRIPTION("Driver for Marvell 83xx compact flash WLAN cards");
+MODULE_LICENSE("GPL");
+
+
+
+/********************************************************************/
+/* Data structures                                                  */
+/********************************************************************/
+
+struct if_cs_card {
+	struct pcmcia_device *p_dev;
+	wlan_private *priv;
+	void __iomem *iobase;
+};
+
+
+
+/********************************************************************/
+/* Hardware access                                                  */
+/********************************************************************/
+
+/* This define enables wrapper functions which allow you
+   to dump all register accesses. You normally won't this,
+   except for development */
+/* #define DEBUG_IO */
+
+#ifdef DEBUG_IO
+static int debug_output = 0;
+#else
+/* This way the compiler optimizes the printk's away */
+#define debug_output 0
+#endif
+
+static inline unsigned int if_cs_read8(struct if_cs_card *card, uint reg)
+{
+	unsigned int val = ioread8(card->iobase + reg);
+	if (debug_output)
+		printk(KERN_INFO "##inb %08x<%02x\n", reg, val);
+	return val;
+}
+static inline unsigned int if_cs_read16(struct if_cs_card *card, uint reg)
+{
+	unsigned int val = ioread16(card->iobase + reg);
+	if (debug_output)
+		printk(KERN_INFO "##inw %08x<%04x\n", reg, val);
+	return val;
+}
+static inline void if_cs_read16_rep(
+	struct if_cs_card *card,
+	uint reg,
+	void *buf,
+	unsigned long count)
+{
+	if (debug_output)
+		printk(KERN_INFO "##insw %08x<(0x%lx words)\n",
+			reg, count);
+	ioread16_rep(card->iobase + reg, buf, count);
+}
+
+static inline void if_cs_write8(struct if_cs_card *card, uint reg, u8 val)
+{
+	if (debug_output)
+		printk(KERN_INFO "##outb %08x>%02x\n", reg, val);
+	iowrite8(val, card->iobase + reg);
+}
+
+static inline void if_cs_write16(struct if_cs_card *card, uint reg, u16 val)
+{
+	if (debug_output)
+		printk(KERN_INFO "##outw %08x>%04x\n", reg, val);
+	iowrite16(val, card->iobase + reg);
+}
+
+static inline void if_cs_write16_rep(
+	struct if_cs_card *card,
+	uint reg,
+	void *buf,
+	unsigned long count)
+{
+	if (debug_output)
+		printk(KERN_INFO "##outsw %08x>(0x%lx words)\n",
+			reg, count);
+	iowrite16_rep(card->iobase + reg, buf, count);
+}
+
+
+/*
+ * I know that polling/delaying is frowned upon. However, this procedure
+ * with polling is needed while downloading the firmware. At this stage,
+ * the hardware does unfortunately not create any interrupts.
+ *
+ * Fortunately, this function is never used once the firmware is in
+ * the card. :-)
+ *
+ * As a reference, see the "Firmware Specification v5.1", page 18
+ * and 19. I did not follow their suggested timing to the word,
+ * but this works nice & fast anyway.
+ */
+static int if_cs_poll_while_fw_download(struct if_cs_card *card, uint addr, u8 reg)
+{
+	int i;
+
+	for (i = 0; i < 500; i++) {
+		u8 val = if_cs_read8(card, addr);
+		if (val == reg)
+			return i;
+		udelay(100);
+	}
+	return -ETIME;
+}
+
+
+
+/* Host control registers and their bit definitions */
+
+#define IF_CS_H_STATUS			0x00000000
+#define IF_CS_H_STATUS_TX_OVER		0x0001
+#define IF_CS_H_STATUS_RX_OVER		0x0002
+#define IF_CS_H_STATUS_DNLD_OVER	0x0004
+
+#define IF_CS_H_INT_CAUSE		0x00000002
+#define IF_CS_H_IC_TX_OVER		0x0001
+#define IF_CS_H_IC_RX_OVER		0x0002
+#define IF_CS_H_IC_DNLD_OVER		0x0004
+#define IF_CS_H_IC_HOST_EVENT		0x0008
+#define IF_CS_H_IC_MASK			0x001f
+
+#define IF_CS_H_INT_MASK		0x00000004
+#define	IF_CS_H_IM_MASK			0x001f
+
+#define IF_CS_H_WRITE_LEN		0x00000014
+
+#define IF_CS_H_WRITE			0x00000016
+
+#define IF_CS_H_CMD_LEN			0x00000018
+
+#define IF_CS_H_CMD			0x0000001A
+
+#define IF_CS_C_READ_LEN		0x00000024
+
+#define IF_CS_H_READ			0x00000010
+
+/* Card control registers and their bit definitions */
+
+#define IF_CS_C_STATUS			0x00000020
+#define IF_CS_C_S_TX_DNLD_RDY		0x0001
+#define IF_CS_C_S_RX_UPLD_RDY		0x0002
+#define IF_CS_C_S_CMD_DNLD_RDY		0x0004
+#define IF_CS_C_S_CMD_UPLD_RDY		0x0008
+#define IF_CS_C_S_CARDEVENT		0x0010
+#define IF_CS_C_S_MASK			0x001f
+#define IF_CS_C_S_STATUS_MASK		0x7f00
+/* The following definitions should be the same as the MRVDRV_ ones */
+
+#if MRVDRV_CMD_DNLD_RDY != IF_CS_C_S_CMD_DNLD_RDY
+#error MRVDRV_CMD_DNLD_RDY and IF_CS_C_S_CMD_DNLD_RDY not in sync
+#endif
+#if MRVDRV_CMD_UPLD_RDY != IF_CS_C_S_CMD_UPLD_RDY
+#error MRVDRV_CMD_UPLD_RDY and IF_CS_C_S_CMD_UPLD_RDY not in sync
+#endif
+#if MRVDRV_CARDEVENT != IF_CS_C_S_CARDEVENT
+#error MRVDRV_CARDEVENT and IF_CS_C_S_CARDEVENT not in sync
+#endif
+
+#define IF_CS_C_INT_CAUSE		0x00000022
+#define	IF_CS_C_IC_MASK			0x001f
+
+#define IF_CS_C_SQ_READ_LOW		0x00000028
+#define IF_CS_C_SQ_HELPER_OK		0x10
+
+#define IF_CS_C_CMD_LEN			0x00000030
+
+#define IF_CS_C_CMD			0x00000012
+
+#define IF_CS_SCRATCH			0x0000003F
+
+
+
+/********************************************************************/
+/* Interrupts                                                       */
+/********************************************************************/
+
+static inline void if_cs_enable_ints(struct if_cs_card *card)
+{
+	lbs_deb_enter(LBS_DEB_CS);
+	if_cs_write16(card, IF_CS_H_INT_MASK, 0);
+}
+
+static inline void if_cs_disable_ints(struct if_cs_card *card)
+{
+	lbs_deb_enter(LBS_DEB_CS);
+	if_cs_write16(card, IF_CS_H_INT_MASK, IF_CS_H_IM_MASK);
+}
+
+static irqreturn_t if_cs_interrupt(int irq, void *data)
+{
+	struct if_cs_card *card = (struct if_cs_card *)data;
+	u16 int_cause;
+
+	lbs_deb_enter(LBS_DEB_CS);
+
+	int_cause = if_cs_read16(card, IF_CS_C_INT_CAUSE);
+	if(int_cause == 0x0) {
+		/* Not for us */
+		return IRQ_NONE;
+
+	} else if(int_cause == 0xffff) {
+		/* Read in junk, the card has probably been removed */
+		card->priv->adapter->surpriseremoved = 1;
+
+	} else {
+		if(int_cause & IF_CS_H_IC_TX_OVER) {
+			card->priv->dnld_sent = DNLD_RES_RECEIVED;
+			if (!card->priv->adapter->cur_cmd)
+				wake_up_interruptible(&card->priv->waitq);
+
+			if (card->priv->adapter->connect_status == LIBERTAS_CONNECTED)
+				netif_wake_queue(card->priv->dev);
+		}
+
+		/* clear interrupt */
+		if_cs_write16(card, IF_CS_C_INT_CAUSE, int_cause & IF_CS_C_IC_MASK);
+	}
+
+	libertas_interrupt(card->priv->dev);
+
+	return IRQ_HANDLED;
+}
+
+
+
+
+/********************************************************************/
+/* I/O                                                              */
+/********************************************************************/
+
+/*
+ * Called from if_cs_host_to_card to send a command to the hardware
+ */
+static int if_cs_send_cmd(wlan_private *priv, u8 *buf, u16 nb)
+{
+	struct if_cs_card *card = (struct if_cs_card *)priv->card;
+	int ret = -1;
+	int loops = 0;
+
+	lbs_deb_enter(LBS_DEB_CS);
+
+	/* Is hardware ready? */
+	while (1) {
+		u16 val = if_cs_read16(card, IF_CS_C_STATUS);
+		if (val & IF_CS_C_S_CMD_DNLD_RDY)
+			break;
+		if (++loops > 100) {
+			lbs_pr_err("card not ready for commands\n");
+			goto done;
+		}
+		mdelay(1);
+	}
+
+	if_cs_write16(card, IF_CS_H_CMD_LEN, nb);
+
+	if_cs_write16_rep(card, IF_CS_H_CMD, buf, nb / 2);
+	/* Are we supposed to transfer an odd amount of bytes? */
+	if (nb & 1)
+		if_cs_write8(card, IF_CS_H_CMD, buf[nb-1]);
+
+	/* "Assert the download over interrupt command in the Host
+	 * status register" */
+	if_cs_write16(card, IF_CS_H_STATUS, IF_CS_H_STATUS_DNLD_OVER);
+
+	/* "Assert the download over interrupt command in the Card
+	 * interrupt case register" */
+	if_cs_write16(card, IF_CS_H_INT_CAUSE, IF_CS_H_IC_DNLD_OVER);
+	ret = 0;
+
+done:
+	lbs_deb_leave_args(LBS_DEB_CS, "ret %d", ret);
+	return ret;
+}
+
+
+/*
+ * Called from if_cs_host_to_card to send a data to the hardware
+ */
+static void if_cs_send_data(wlan_private *priv, u8 *buf, u16 nb)
+{
+	struct if_cs_card *card = (struct if_cs_card *)priv->card;
+
+	lbs_deb_enter(LBS_DEB_CS);
+
+	if_cs_write16(card, IF_CS_H_WRITE_LEN, nb);
+
+	/* write even number of bytes, then odd byte if necessary */
+	if_cs_write16_rep(card, IF_CS_H_WRITE, buf, nb / 2);
+	if (nb & 1)
+		if_cs_write8(card, IF_CS_H_WRITE, buf[nb-1]);
+
+	if_cs_write16(card, IF_CS_H_STATUS, IF_CS_H_STATUS_TX_OVER);
+	if_cs_write16(card, IF_CS_H_INT_CAUSE, IF_CS_H_STATUS_TX_OVER);
+
+	lbs_deb_leave(LBS_DEB_CS);
+}
+
+
+/*
+ * Get the command result out of the card.
+ */
+static int if_cs_receive_cmdres(wlan_private *priv, u8* data, u32 *len)
+{
+	int ret = -1;
+	u16 val;
+
+	lbs_deb_enter(LBS_DEB_CS);
+
+	/* is hardware ready? */
+	val = if_cs_read16(priv->card, IF_CS_C_STATUS);
+	if ((val & IF_CS_C_S_CMD_UPLD_RDY) == 0) {
+		lbs_pr_err("card not ready for CMD\n");
+		goto out;
+	}
+
+	*len = if_cs_read16(priv->card, IF_CS_C_CMD_LEN);
+	if ((*len == 0) || (*len > MRVDRV_SIZE_OF_CMD_BUFFER)) {
+		lbs_pr_err("card cmd buffer has invalid # of bytes (%d)\n", *len);
+		goto out;
+	}
+
+	/* read even number of bytes, then odd byte if necessary */
+	if_cs_read16_rep(priv->card, IF_CS_C_CMD, data, *len/sizeof(u16));
+	if (*len & 1)
+		data[*len-1] = if_cs_read8(priv->card, IF_CS_C_CMD);
+
+	ret = 0;
+out:
+	lbs_deb_leave_args(LBS_DEB_CS, "ret %d, len %d", ret, *len);
+	return ret;
+}
+
+
+static struct sk_buff *if_cs_receive_data(wlan_private *priv)
+{
+	struct sk_buff *skb = NULL;
+	u16 len;
+	u8 *data;
+
+	lbs_deb_enter(LBS_DEB_CS);
+
+	len = if_cs_read16(priv->card, IF_CS_C_READ_LEN);
+	if (len == 0 || len > MRVDRV_ETH_RX_PACKET_BUFFER_SIZE) {
+		lbs_pr_err("card data buffer has invalid # of bytes (%d)\n", len);
+		priv->stats.rx_dropped++;
+		printk(KERN_INFO "##HS %s:%d TODO\n", __FUNCTION__, __LINE__);
+		goto dat_err;
+	}
+
+	//TODO: skb = dev_alloc_skb(len+ETH_FRAME_LEN+MRVDRV_SNAP_HEADER_LEN+EXTRA_LEN);
+	skb = dev_alloc_skb(MRVDRV_ETH_RX_PACKET_BUFFER_SIZE + 2);
+	if (!skb)
+		goto out;
+	skb_put(skb, len);
+	skb_reserve(skb, 2);/* 16 byte align */
+	data = skb->data;
+
+	/* read even number of bytes, then odd byte if necessary */
+	if_cs_read16_rep(priv->card, IF_CS_H_READ, data, len/sizeof(u16));
+	if (len & 1)
+		data[len-1] = if_cs_read8(priv->card, IF_CS_H_READ);
+
+dat_err:
+	if_cs_write16(priv->card, IF_CS_H_STATUS, IF_CS_H_STATUS_RX_OVER);
+	if_cs_write16(priv->card, IF_CS_H_INT_CAUSE, IF_CS_H_IC_RX_OVER);
+
+out:
+	lbs_deb_leave_args(LBS_DEB_CS, "ret %p", skb);
+	return skb;
+}
+
+
+
+/********************************************************************/
+/* Firmware                                                         */
+/********************************************************************/
+
+/*
+ * Tries to program the helper firmware.
+ *
+ * Return 0 on success
+ */
+static int if_cs_prog_helper(struct if_cs_card *card)
+{
+	int ret = 0;
+	int sent = 0;
+	u8  scratch;
+	const struct firmware *fw;
+
+	lbs_deb_enter(LBS_DEB_CS);
+
+	scratch = if_cs_read8(card, IF_CS_SCRATCH);
+
+	/* "If the value is 0x5a, the firmware is already
+	 * downloaded successfully"
+	 */
+	if (scratch == 0x5a)
+		goto done;
+
+	/* "If the value is != 00, it is invalid value of register */
+	if (scratch != 0x00) {
+		ret = -ENODEV;
+		goto done;
+	}
+
+	/* TODO: make firmware file configurable */
+	ret = request_firmware(&fw, "libertas_cs_helper.fw",
+		&handle_to_dev(card->p_dev));
+	if (ret) {
+		lbs_pr_err("can't load helper firmware\n");
+		ret = -ENODEV;
+		goto done;
+	}
+	lbs_deb_cs("helper size %td\n", fw->size);
+
+	/* "Set the 5 bytes of the helper image to 0" */
+	/* Not needed, this contains an ARM branch instruction */
+
+	for (;;) {
+		/* "the number of bytes to send is 256" */
+		int count = 256;
+		int remain = fw->size - sent;
+
+		if (remain < count)
+			count = remain;
+		/* printk(KERN_INFO "//HS %d loading %d of %d bytes\n",
+			__LINE__, sent, fw->size); */
+
+		/* "write the number of bytes to be sent to the I/O Command
+		 * write length register" */
+		if_cs_write16(card, IF_CS_H_CMD_LEN, count);
+
+		/* "write this to I/O Command port register as 16 bit writes */
+		if (count)
+			if_cs_write16_rep(card, IF_CS_H_CMD,
+				&fw->data[sent],
+				count >> 1);
+
+		/* "Assert the download over interrupt command in the Host
+		 * status register" */
+		if_cs_write8(card, IF_CS_H_STATUS, IF_CS_H_STATUS_DNLD_OVER);
+
+		/* "Assert the download over interrupt command in the Card
+		 * interrupt case register" */
+		if_cs_write16(card, IF_CS_H_INT_CAUSE, IF_CS_H_IC_DNLD_OVER);
+
+		/* "The host polls the Card Status register ... for 50 ms before
+		   declaring a failure */
+		ret = if_cs_poll_while_fw_download(card, IF_CS_C_STATUS,
+			IF_CS_C_S_CMD_DNLD_RDY);
+		if (ret < 0) {
+			lbs_pr_err("can't download helper at 0x%x, ret %d\n",
+				sent, ret);
+			goto done;
+		}
+
+		if (count == 0)
+			break;
+
+		sent += count;
+	}
+
+	release_firmware(fw);
+	ret = 0;
+
+done:
+	lbs_deb_leave_args(LBS_DEB_CS, "ret %d", ret);
+	return ret;
+}
+
+
+static int if_cs_prog_real(struct if_cs_card *card)
+{
+	const struct firmware *fw;
+	int ret = 0;
+	int retry = 0;
+	int len = 0;
+	int sent;
+
+	lbs_deb_enter(LBS_DEB_CS);
+
+	/* TODO: make firmware file configurable */
+	ret = request_firmware(&fw, "libertas_cs.fw",
+		&handle_to_dev(card->p_dev));
+	if (ret) {
+		lbs_pr_err("can't load firmware\n");
+		ret = -ENODEV;
+		goto done;
+	}
+	lbs_deb_cs("fw size %td\n", fw->size);
+
+	ret = if_cs_poll_while_fw_download(card, IF_CS_C_SQ_READ_LOW, IF_CS_C_SQ_HELPER_OK);
+	if (ret < 0) {
+		int i;
+		lbs_pr_err("helper firmware doesn't answer\n");
+		for (i = 0; i < 0x50; i += 2)
+			printk(KERN_INFO "## HS %02x: %04x\n",
+				i, if_cs_read16(card, i));
+		goto err_release;
+	}
+
+	for (sent = 0; sent < fw->size; sent += len) {
+		len = if_cs_read16(card, IF_CS_C_SQ_READ_LOW);
+		/* printk(KERN_INFO "//HS %d loading %d of %d bytes\n",
+			__LINE__, sent, fw->size); */
+		if (len & 1) {
+			retry++;
+			lbs_pr_info("odd, need to retry this firmware block\n");
+		} else {
+			retry = 0;
+		}
+
+		if (retry > 20) {
+			lbs_pr_err("could not download firmware\n");
+			ret = -ENODEV;
+			goto err_release;
+		}
+		if (retry) {
+			sent -= len;
+		}
+
+
+		if_cs_write16(card, IF_CS_H_CMD_LEN, len);
+
+		if_cs_write16_rep(card, IF_CS_H_CMD,
+			&fw->data[sent],
+			(len+1) >> 1);
+		if_cs_write8(card, IF_CS_H_STATUS, IF_CS_H_STATUS_DNLD_OVER);
+		if_cs_write16(card, IF_CS_H_INT_CAUSE, IF_CS_H_IC_DNLD_OVER);
+
+		ret = if_cs_poll_while_fw_download(card, IF_CS_C_STATUS,
+			IF_CS_C_S_CMD_DNLD_RDY);
+		if (ret < 0) {
+			lbs_pr_err("can't download firmware at 0x%x\n", sent);
+			goto err_release;
+		}
+	}
+
+	ret = if_cs_poll_while_fw_download(card, IF_CS_SCRATCH, 0x5a);
+	if (ret < 0) {
+		lbs_pr_err("firmware download failed\n");
+		goto err_release;
+	}
+
+	ret = 0;
+	goto done;
+
+
+err_release:
+	release_firmware(fw);
+
+done:
+	lbs_deb_leave_args(LBS_DEB_CS, "ret %d", ret);
+	return ret;
+}
+
+
+
+/********************************************************************/
+/* Callback functions for libertas.ko                               */
+/********************************************************************/
+
+/* Send commands or data packets to the card */
+static int if_cs_host_to_card(wlan_private *priv, u8 type, u8 *buf, u16 nb)
+{
+	int ret = -1;
+
+	lbs_deb_enter_args(LBS_DEB_CS, "type %d, bytes %d", type, nb);
+
+	switch (type) {
+	case MVMS_DAT:
+		priv->dnld_sent = DNLD_DATA_SENT;
+		if_cs_send_data(priv, buf, nb);
+		ret = 0;
+		break;
+	case MVMS_CMD:
+		priv->dnld_sent = DNLD_CMD_SENT;
+		ret = if_cs_send_cmd(priv, buf, nb);
+		break;
+	default:
+		lbs_pr_err("%s: unsupported type %d\n", __FUNCTION__, type);
+	}
+
+	lbs_deb_leave_args(LBS_DEB_CS, "ret %d", ret);
+	return ret;
+}
+
+
+static int if_cs_get_int_status(wlan_private *priv, u8 *ireg)
+{
+	struct if_cs_card *card = (struct if_cs_card *)priv->card;
+	//wlan_adapter *adapter = priv->adapter;
+	int ret = 0;
+	u16 int_cause;
+	u8 *cmdbuf;
+	*ireg = 0;
+
+	lbs_deb_enter(LBS_DEB_CS);
+
+	if (priv->adapter->surpriseremoved)
+		goto out;
+
+	int_cause = if_cs_read16(card, IF_CS_C_INT_CAUSE) & IF_CS_C_IC_MASK;
+	if_cs_write16(card, IF_CS_C_INT_CAUSE, int_cause);
+
+	*ireg = if_cs_read16(card, IF_CS_C_STATUS) & IF_CS_C_S_MASK;
+
+	if (!*ireg)
+		goto sbi_get_int_status_exit;
+
+sbi_get_int_status_exit:
+
+	/* is there a data packet for us? */
+	if (*ireg & IF_CS_C_S_RX_UPLD_RDY) {
+		struct sk_buff *skb = if_cs_receive_data(priv);
+		libertas_process_rxed_packet(priv, skb);
+		*ireg &= ~IF_CS_C_S_RX_UPLD_RDY;
+	}
+
+	if (*ireg & IF_CS_C_S_TX_DNLD_RDY) {
+		priv->dnld_sent = DNLD_RES_RECEIVED;
+	}
+
+	/* Card has a command result for us */
+	if (*ireg & IF_CS_C_S_CMD_UPLD_RDY) {
+		spin_lock(&priv->adapter->driver_lock);
+		if (!priv->adapter->cur_cmd) {
+			cmdbuf = priv->upld_buf;
+			priv->adapter->hisregcpy &= ~IF_CS_C_S_RX_UPLD_RDY;
+		} else {
+			cmdbuf = priv->adapter->cur_cmd->bufvirtualaddr;
+		}
+
+		ret = if_cs_receive_cmdres(priv, cmdbuf, &priv->upld_len);
+		spin_unlock(&priv->adapter->driver_lock);
+		if (ret < 0)
+			lbs_pr_err("could not receive cmd from card\n");
+	}
+
+out:
+	lbs_deb_leave_args(LBS_DEB_CS, "ret %d, ireg 0x%x, hisregcpy 0x%x", ret, *ireg, priv->adapter->hisregcpy);
+	return ret;
+}
+
+
+static int if_cs_read_event_cause(wlan_private *priv)
+{
+	lbs_deb_enter(LBS_DEB_CS);
+
+	priv->adapter->eventcause = (if_cs_read16(priv->card, IF_CS_C_STATUS) & IF_CS_C_S_STATUS_MASK) >> 5;
+	if_cs_write16(priv->card, IF_CS_H_INT_CAUSE, IF_CS_H_IC_HOST_EVENT);
+
+	return 0;
+}
+
+
+
+/********************************************************************/
+/* Card Services                                                    */
+/********************************************************************/
+
+/*
+ * After a card is removed, if_cs_release() will unregister the
+ * device, and release the PCMCIA configuration.  If the device is
+ * still open, this will be postponed until it is closed.
+ */
+static void if_cs_release(struct pcmcia_device *p_dev)
+{
+	struct if_cs_card *card = p_dev->priv;
+
+	lbs_deb_enter(LBS_DEB_CS);
+
+	pcmcia_disable_device(p_dev);
+	free_irq(p_dev->irq.AssignedIRQ, card);
+	if (card->iobase)
+		ioport_unmap(card->iobase);
+
+	lbs_deb_leave(LBS_DEB_CS);
+}
+
+
+/*
+ * This creates an "instance" of the driver, allocating local data
+ * structures for one device.  The device is registered with Card
+ * Services.
+ *
+ * The dev_link structure is initialized, but we don't actually
+ * configure the card at this point -- we wait until we receive a card
+ * insertion event.
+ */
+static int if_cs_probe(struct pcmcia_device *p_dev)
+{
+	int ret = -ENOMEM;
+	wlan_private *priv;
+	struct if_cs_card *card;
+	/* CIS parsing */
+	tuple_t tuple;
+	cisparse_t parse;
+	cistpl_cftable_entry_t *cfg = &parse.cftable_entry;
+	cistpl_io_t *io = &cfg->io;
+	u_char buf[64];
+
+	lbs_deb_enter(LBS_DEB_CS);
+
+	card = kzalloc(sizeof(struct if_cs_card), GFP_KERNEL);
+	if (!card) {
+		lbs_pr_err("error in kzalloc\n");
+		goto out;
+	}
+	card->p_dev = p_dev;
+	p_dev->priv = card;
+
+	p_dev->irq.Attributes = IRQ_TYPE_DYNAMIC_SHARING;
+	p_dev->irq.Handler = NULL;
+	p_dev->irq.IRQInfo1 = IRQ_INFO2_VALID | IRQ_LEVEL_ID;
+
+	p_dev->conf.Attributes = 0;
+	p_dev->conf.IntType = INT_MEMORY_AND_IO;
+
+	tuple.Attributes = 0;
+	tuple.TupleData = buf;
+	tuple.TupleDataMax = sizeof(buf);
+	tuple.TupleOffset = 0;
+
+	tuple.DesiredTuple = CISTPL_CFTABLE_ENTRY;
+	if ((ret = pcmcia_get_first_tuple(p_dev, &tuple)) != 0 ||
+	    (ret = pcmcia_get_tuple_data(p_dev, &tuple)) != 0 ||
+	    (ret = pcmcia_parse_tuple(p_dev, &tuple, &parse)) != 0)
+	{
+		lbs_pr_err("error in pcmcia_get_first_tuple etc\n");
+		goto out1;
+	}
+
+	p_dev->conf.ConfigIndex = cfg->index;
+
+	/* Do we need to allocate an interrupt? */
+	if (cfg->irq.IRQInfo1) {
+		p_dev->conf.Attributes |= CONF_ENABLE_IRQ;
+	}
+
+	/* IO window settings */
+	if (cfg->io.nwin != 1) {
+		lbs_pr_err("wrong CIS (check number of IO windows)\n");
+		ret = -ENODEV;
+		goto out1;
+	}
+	p_dev->io.Attributes1 = IO_DATA_PATH_WIDTH_AUTO;
+	p_dev->io.BasePort1 = io->win[0].base;
+	p_dev->io.NumPorts1 = io->win[0].len;
+
+	/* This reserves IO space but doesn't actually enable it */
+	ret = pcmcia_request_io(p_dev, &p_dev->io);
+	if (ret) {
+		lbs_pr_err("error in pcmcia_request_io\n");
+		goto out1;
+	}
+
+	/*
+	 * Allocate an interrupt line.  Note that this does not assign
+	 * a handler to the interrupt, unless the 'Handler' member of
+	 * the irq structure is initialized.
+	 */
+	if (p_dev->conf.Attributes & CONF_ENABLE_IRQ) {
+		ret = pcmcia_request_irq(p_dev, &p_dev->irq);
+		if (ret) {
+			lbs_pr_err("error in pcmcia_request_irq\n");
+			goto out1;
+		}
+	}
+
+	/* Initialize io access */
+	card->iobase = ioport_map(p_dev->io.BasePort1, p_dev->io.NumPorts1);
+	if (!card->iobase) {
+		lbs_pr_err("error in ioport_map\n");
+		ret = -EIO;
+		goto out1;
+	}
+
+	/*
+	 * This actually configures the PCMCIA socket -- setting up
+	 * the I/O windows and the interrupt mapping, and putting the
+	 * card and host interface into "Memory and IO" mode.
+	 */
+	ret = pcmcia_request_configuration(p_dev, &p_dev->conf);
+	if (ret) {
+		lbs_pr_err("error in pcmcia_request_configuration\n");
+		goto out2;
+	}
+
+	/* Finally, report what we've done */
+	lbs_deb_cs("irq %d, io 0x%04x-0x%04x\n",
+	       p_dev->irq.AssignedIRQ, p_dev->io.BasePort1,
+	       p_dev->io.BasePort1 + p_dev->io.NumPorts1 - 1);
+
+
+	/* Load the firmware early, before calling into libertas.ko */
+	ret = if_cs_prog_helper(card);
+	if (ret == 0)
+		ret = if_cs_prog_real(card);
+	if (ret)
+		goto out2;
+
+	/* Make this card known to the libertas driver */
+	priv = libertas_add_card(card, &p_dev->dev);
+	if (!priv) {
+		ret = -ENOMEM;
+		goto out2;
+	}
+
+	/* Store pointers to our call-back functions */
+	card->priv = priv;
+	priv->card = card;
+	priv->hw_host_to_card     = if_cs_host_to_card;
+	priv->hw_get_int_status   = if_cs_get_int_status;
+	priv->hw_read_event_cause = if_cs_read_event_cause;
+
+	priv->adapter->fw_ready = 1;
+
+	/* Now actually get the IRQ */
+	ret = request_irq(p_dev->irq.AssignedIRQ, if_cs_interrupt,
+		IRQF_SHARED, DRV_NAME, card);
+	if (ret) {
+		lbs_pr_err("error in request_irq\n");
+		goto out3;
+	}
+
+	if_cs_enable_ints(card);
+
+	/* And finally bring the card up */
+	if (libertas_start_card(priv) != 0) {
+		lbs_pr_err("could not activate card\n");
+		goto out3;
+	}
+
+	ret = 0;
+	goto out;
+
+out3:
+	libertas_remove_card(priv);
+out2:
+	ioport_unmap(card->iobase);
+out1:
+	pcmcia_disable_device(p_dev);
+out:
+	lbs_deb_leave_args(LBS_DEB_CS, "ret %d", ret);
+	return ret;
+}
+
+
+/*
+ * This deletes a driver "instance".  The device is de-registered with
+ * Card Services.  If it has been released, all local data structures
+ * are freed.  Otherwise, the structures will be freed when the device
+ * is released.
+ */
+static void if_cs_detach(struct pcmcia_device *p_dev)
+{
+	struct if_cs_card *card = p_dev->priv;
+
+	lbs_deb_enter(LBS_DEB_CS);
+
+	libertas_stop_card(card->priv);
+	libertas_remove_card(card->priv);
+	if_cs_disable_ints(card);
+	if_cs_release(p_dev);
+	kfree(card);
+
+	lbs_deb_leave(LBS_DEB_CS);
+}
+
+
+
+/********************************************************************/
+/* Module initialization                                            */
+/********************************************************************/
+
+static struct pcmcia_device_id if_cs_ids[] = {
+	PCMCIA_DEVICE_MANF_CARD(0x02df, 0x8103),
+	PCMCIA_DEVICE_NULL,
+};
+MODULE_DEVICE_TABLE(pcmcia, if_cs_ids);
+
+
+static struct pcmcia_driver libertas_driver = {
+	.owner		= THIS_MODULE,
+	.drv		= {
+		.name	= DRV_NAME,
+	},
+	.probe		= if_cs_probe,
+	.remove		= if_cs_detach,
+	.id_table       = if_cs_ids,
+};
+
+
+static int __init if_cs_init(void)
+{
+	int ret;
+
+	lbs_deb_enter(LBS_DEB_CS);
+	ret = pcmcia_register_driver(&libertas_driver);
+	lbs_deb_leave(LBS_DEB_CS);
+	return ret;
+}
+
+
+static void __exit if_cs_exit(void)
+{
+	lbs_deb_enter(LBS_DEB_CS);
+	pcmcia_unregister_driver(&libertas_driver);
+	lbs_deb_leave(LBS_DEB_CS);
+}
+
+
+module_init(if_cs_init);
+module_exit(if_cs_exit);
diff --git a/drivers/net/wireless/libertas/if_usb.c b/drivers/net/wireless/libertas/if_usb.c
index 998317571ec2..cb59f46ed126 100644
--- a/drivers/net/wireless/libertas/if_usb.c
+++ b/drivers/net/wireless/libertas/if_usb.c
@@ -21,7 +21,7 @@
 static const char usbdriver_name[] = "usb8xxx";
 static u8 *default_fw_name = "usb8388.bin";
 
-char *libertas_fw_name = NULL;
+static char *libertas_fw_name = NULL;
 module_param_named(fw_name, libertas_fw_name, charp, 0644);
 
 /*
@@ -44,13 +44,14 @@ MODULE_DEVICE_TABLE(usb, if_usb_table);
 
 static void if_usb_receive(struct urb *urb);
 static void if_usb_receive_fwload(struct urb *urb);
-static int if_usb_reset_device(wlan_private *priv);
-static int if_usb_register_dev(wlan_private * priv);
-static int if_usb_unregister_dev(wlan_private *);
-static int if_usb_prog_firmware(wlan_private *);
+static int if_usb_prog_firmware(struct usb_card_rec *cardp);
 static int if_usb_host_to_card(wlan_private * priv, u8 type, u8 * payload, u16 nb);
 static int if_usb_get_int_status(wlan_private * priv, u8 *);
 static int if_usb_read_event_cause(wlan_private *);
+static int usb_tx_block(struct usb_card_rec *cardp, u8 *payload, u16 nb);
+static void if_usb_free(struct usb_card_rec *cardp);
+static int if_usb_submit_rx_urb(struct usb_card_rec *cardp);
+static int if_usb_reset_device(struct usb_card_rec *cardp);
 
 /**
  *  @brief  call back function to handle the status of the URB
@@ -59,29 +60,40 @@ static int if_usb_read_event_cause(wlan_private *);
  */
 static void if_usb_write_bulk_callback(struct urb *urb)
 {
-	wlan_private *priv = (wlan_private *) (urb->context);
-	wlan_adapter *adapter = priv->adapter;
-	struct net_device *dev = priv->dev;
+	struct usb_card_rec *cardp = (struct usb_card_rec *) urb->context;
 
 	/* handle the transmission complete validations */
 
-	if (urb->status != 0) {
-		/* print the failure status number for debug */
-		lbs_pr_info("URB in failure status: %d\n", urb->status);
-	} else {
+	if (urb->status == 0) {
+		wlan_private *priv = cardp->priv;
+
 		/*
 		lbs_deb_usbd(&urb->dev->dev, "URB status is successfull\n");
 		lbs_deb_usbd(&urb->dev->dev, "Actual length transmitted %d\n",
 		       urb->actual_length);
 		*/
-		priv->dnld_sent = DNLD_RES_RECEIVED;
-		/* Wake main thread if commands are pending */
-		if (!adapter->cur_cmd)
-			wake_up_interruptible(&priv->mainthread.waitq);
-		if ((adapter->connect_status == libertas_connected)) {
-			netif_wake_queue(dev);
-			netif_wake_queue(priv->mesh_dev);
+
+		/* Used for both firmware TX and regular TX.  priv isn't
+		 * valid at firmware load time.
+		 */
+		if (priv) {
+			wlan_adapter *adapter = priv->adapter;
+			struct net_device *dev = priv->dev;
+
+			priv->dnld_sent = DNLD_RES_RECEIVED;
+
+			/* Wake main thread if commands are pending */
+			if (!adapter->cur_cmd)
+				wake_up_interruptible(&priv->waitq);
+
+			if ((adapter->connect_status == LIBERTAS_CONNECTED)) {
+				netif_wake_queue(dev);
+				netif_wake_queue(priv->mesh_dev);
+			}
 		}
+	} else {
+		/* print the failure status number for debug */
+		lbs_pr_info("URB in failure status: %d\n", urb->status);
 	}
 
 	return;
@@ -92,7 +104,7 @@ static void if_usb_write_bulk_callback(struct urb *urb)
  *  @param cardp	pointer usb_card_rec
  *  @return 	   	N/A
  */
-void if_usb_free(struct usb_card_rec *cardp)
+static void if_usb_free(struct usb_card_rec *cardp)
 {
 	lbs_deb_enter(LBS_DEB_USB);
 
@@ -203,21 +215,35 @@ static int if_usb_probe(struct usb_interface *intf,
 		}
 	}
 
+	/* Upload firmware */
+	cardp->rinfo.cardp = cardp;
+	if (if_usb_prog_firmware(cardp)) {
+		lbs_deb_usbd(&udev->dev, "FW upload failed");
+		goto err_prog_firmware;
+	}
+
 	if (!(priv = libertas_add_card(cardp, &udev->dev)))
-		goto dealloc;
+		goto err_prog_firmware;
+
+	cardp->priv = priv;
 
 	if (libertas_add_mesh(priv, &udev->dev))
 		goto err_add_mesh;
 
-	priv->hw_register_dev = if_usb_register_dev;
-	priv->hw_unregister_dev = if_usb_unregister_dev;
-	priv->hw_prog_firmware = if_usb_prog_firmware;
+	cardp->eth_dev = priv->dev;
+
 	priv->hw_host_to_card = if_usb_host_to_card;
 	priv->hw_get_int_status = if_usb_get_int_status;
 	priv->hw_read_event_cause = if_usb_read_event_cause;
+	priv->boot2_version = udev->descriptor.bcdDevice;
 
-	if (libertas_activate_card(priv, libertas_fw_name))
-		goto err_activate_card;
+	/* Delay 200 ms to waiting for the FW ready */
+	if_usb_submit_rx_urb(cardp);
+	msleep_interruptible(200);
+	priv->adapter->fw_ready = 1;
+
+	if (libertas_start_card(priv))
+		goto err_start_card;
 
 	list_add_tail(&cardp->list, &usb_devices);
 
@@ -226,11 +252,12 @@ static int if_usb_probe(struct usb_interface *intf,
 
 	return 0;
 
-err_activate_card:
+err_start_card:
 	libertas_remove_mesh(priv);
 err_add_mesh:
-	free_netdev(priv->dev);
-	kfree(priv->adapter);
+	libertas_remove_card(priv);
+err_prog_firmware:
+	if_usb_reset_device(cardp);
 dealloc:
 	if_usb_free(cardp);
 
@@ -247,21 +274,22 @@ static void if_usb_disconnect(struct usb_interface *intf)
 {
 	struct usb_card_rec *cardp = usb_get_intfdata(intf);
 	wlan_private *priv = (wlan_private *) cardp->priv;
-	wlan_adapter *adapter = NULL;
 
-	adapter = priv->adapter;
+	lbs_deb_enter(LBS_DEB_MAIN);
 
-	/*
-	 * Update Surprise removed to TRUE
-	 */
-	adapter->surpriseremoved = 1;
+	/* Update Surprise removed to TRUE */
+	cardp->surprise_removed = 1;
 
 	list_del(&cardp->list);
 
-	/* card is removed and we can call wlan_remove_card */
-	lbs_deb_usbd(&cardp->udev->dev, "call remove card\n");
-	libertas_remove_mesh(priv);
-	libertas_remove_card(priv);
+	if (priv) {
+		wlan_adapter *adapter = priv->adapter;
+
+		adapter->surpriseremoved = 1;
+		libertas_stop_card(priv);
+		libertas_remove_mesh(priv);
+		libertas_remove_card(priv);
+	}
 
 	/* Unlink and free urb */
 	if_usb_free(cardp);
@@ -269,7 +297,7 @@ static void if_usb_disconnect(struct usb_interface *intf)
 	usb_set_intfdata(intf, NULL);
 	usb_put_dev(interface_to_usbdev(intf));
 
-	return;
+	lbs_deb_leave(LBS_DEB_MAIN);
 }
 
 /**
@@ -277,12 +305,11 @@ static void if_usb_disconnect(struct usb_interface *intf)
  *  @param priv		pointer to wlan_private
  *  @return 	   	0
  */
-static int if_prog_firmware(wlan_private * priv)
+static int if_prog_firmware(struct usb_card_rec *cardp)
 {
-	struct usb_card_rec *cardp = priv->card;
 	struct FWData *fwdata;
 	struct fwheader *fwheader;
-	u8 *firmware = priv->firmware->data;
+	u8 *firmware = cardp->fw->data;
 
 	fwdata = kmalloc(sizeof(struct FWData), GFP_ATOMIC);
 
@@ -330,7 +357,7 @@ static int if_prog_firmware(wlan_private * priv)
 			    cardp->totalbytes);
 		*/
 		memcpy(cardp->bulk_out_buffer, fwheader, FW_DATA_XMIT_SIZE);
-		usb_tx_block(priv, cardp->bulk_out_buffer, FW_DATA_XMIT_SIZE);
+		usb_tx_block(cardp, cardp->bulk_out_buffer, FW_DATA_XMIT_SIZE);
 
 	} else if (fwdata->fwheader.dnldcmd == cpu_to_le32(FW_HAS_LAST_BLOCK)) {
 		/*
@@ -340,7 +367,7 @@ static int if_prog_firmware(wlan_private * priv)
 			    "Donwloading FW JUMP BLOCK\n");
 		*/
 		memcpy(cardp->bulk_out_buffer, fwheader, FW_DATA_XMIT_SIZE);
-		usb_tx_block(priv, cardp->bulk_out_buffer, FW_DATA_XMIT_SIZE);
+		usb_tx_block(cardp, cardp->bulk_out_buffer, FW_DATA_XMIT_SIZE);
 		cardp->fwfinalblk = 1;
 	}
 
@@ -355,17 +382,20 @@ static int if_prog_firmware(wlan_private * priv)
 	return 0;
 }
 
-static int libertas_do_reset(wlan_private *priv)
+static int if_usb_reset_device(struct usb_card_rec *cardp)
 {
 	int ret;
-	struct usb_card_rec *cardp = priv->card;
+	wlan_private * priv = cardp->priv;
 
 	lbs_deb_enter(LBS_DEB_USB);
 
+	/* Try a USB port reset first, if that fails send the reset
+	 * command to the firmware.
+	 */
 	ret = usb_reset_device(cardp->udev);
-	if (!ret) {
+	if (!ret && priv) {
 		msleep(10);
-		if_usb_reset_device(priv);
+		ret = libertas_reset_device(priv);
 		msleep(10);
 	}
 
@@ -381,14 +411,12 @@ static int libertas_do_reset(wlan_private *priv)
  *  @param nb		data length
  *  @return 	   	0 or -1
  */
-int usb_tx_block(wlan_private * priv, u8 * payload, u16 nb)
+static int usb_tx_block(struct usb_card_rec *cardp, u8 * payload, u16 nb)
 {
-	/* pointer to card structure */
-	struct usb_card_rec *cardp = priv->card;
 	int ret = -1;
 
 	/* check if device is removed */
-	if (priv->adapter->surpriseremoved) {
+	if (cardp->surprise_removed) {
 		lbs_deb_usbd(&cardp->udev->dev, "Device removed\n");
 		goto tx_ret;
 	}
@@ -396,7 +424,7 @@ int usb_tx_block(wlan_private * priv, u8 * payload, u16 nb)
 	usb_fill_bulk_urb(cardp->tx_urb, cardp->udev,
 			  usb_sndbulkpipe(cardp->udev,
 					  cardp->bulk_out_endpointAddr),
-			  payload, nb, if_usb_write_bulk_callback, priv);
+			  payload, nb, if_usb_write_bulk_callback, cardp);
 
 	cardp->tx_urb->transfer_flags |= URB_ZERO_PACKET;
 
@@ -413,11 +441,9 @@ tx_ret:
 	return ret;
 }
 
-static int __if_usb_submit_rx_urb(wlan_private * priv,
-				  void (*callbackfn)
-				  (struct urb *urb))
+static int __if_usb_submit_rx_urb(struct usb_card_rec *cardp,
+				  void (*callbackfn)(struct urb *urb))
 {
-	struct usb_card_rec *cardp = priv->card;
 	struct sk_buff *skb;
 	struct read_cb_info *rinfo = &cardp->rinfo;
 	int ret = -1;
@@ -453,22 +479,21 @@ rx_ret:
 	return ret;
 }
 
-static inline int if_usb_submit_rx_urb_fwload(wlan_private * priv)
+static int if_usb_submit_rx_urb_fwload(struct usb_card_rec *cardp)
 {
-	return __if_usb_submit_rx_urb(priv, &if_usb_receive_fwload);
+	return __if_usb_submit_rx_urb(cardp, &if_usb_receive_fwload);
 }
 
-static inline int if_usb_submit_rx_urb(wlan_private * priv)
+static int if_usb_submit_rx_urb(struct usb_card_rec *cardp)
 {
-	return __if_usb_submit_rx_urb(priv, &if_usb_receive);
+	return __if_usb_submit_rx_urb(cardp, &if_usb_receive);
 }
 
 static void if_usb_receive_fwload(struct urb *urb)
 {
 	struct read_cb_info *rinfo = (struct read_cb_info *)urb->context;
-	wlan_private *priv = rinfo->priv;
 	struct sk_buff *skb = rinfo->skb;
-	struct usb_card_rec *cardp = (struct usb_card_rec *)priv->card;
+	struct usb_card_rec *cardp = (struct usb_card_rec *)rinfo->cardp;
 	struct fwsyncheader *syncfwheader;
 	struct bootcmdrespStr bootcmdresp;
 
@@ -484,7 +509,7 @@ static void if_usb_receive_fwload(struct urb *urb)
 			sizeof(bootcmdresp));
 		if (le16_to_cpu(cardp->udev->descriptor.bcdDevice) < 0x3106) {
 			kfree_skb(skb);
-			if_usb_submit_rx_urb_fwload(priv);
+			if_usb_submit_rx_urb_fwload(cardp);
 			cardp->bootcmdresp = 1;
 			lbs_deb_usbd(&cardp->udev->dev,
 				    "Received valid boot command response\n");
@@ -508,7 +533,7 @@ static void if_usb_receive_fwload(struct urb *urb)
 				    "Received valid boot command response\n");
 		}
 		kfree_skb(skb);
-		if_usb_submit_rx_urb_fwload(priv);
+		if_usb_submit_rx_urb_fwload(cardp);
 		return;
 	}
 
@@ -544,9 +569,9 @@ static void if_usb_receive_fwload(struct urb *urb)
 		goto exit;
 	}
 
-	if_prog_firmware(priv);
+	if_prog_firmware(cardp);
 
-	if_usb_submit_rx_urb_fwload(priv);
+	if_usb_submit_rx_urb_fwload(cardp);
 exit:
 	kfree(syncfwheader);
 
@@ -596,11 +621,11 @@ static inline void process_cmdrequest(int recvlength, u8 *recvbuff,
 	 * data to clear the interrupt */
 	if (!priv->adapter->cur_cmd) {
 		cmdbuf = priv->upld_buf;
-		priv->adapter->hisregcpy &= ~his_cmdupldrdy;
+		priv->adapter->hisregcpy &= ~MRVDRV_CMD_UPLD_RDY;
 	} else
 		cmdbuf = priv->adapter->cur_cmd->bufvirtualaddr;
 
-	cardp->usb_int_cause |= his_cmdupldrdy;
+	cardp->usb_int_cause |= MRVDRV_CMD_UPLD_RDY;
 	priv->upld_len = (recvlength - MESSAGE_HEADER_LEN);
 	memcpy(cmdbuf, recvbuff + MESSAGE_HEADER_LEN,
 	       priv->upld_len);
@@ -625,17 +650,19 @@ static inline void process_cmdrequest(int recvlength, u8 *recvbuff,
 static void if_usb_receive(struct urb *urb)
 {
 	struct read_cb_info *rinfo = (struct read_cb_info *)urb->context;
-	wlan_private *priv = rinfo->priv;
 	struct sk_buff *skb = rinfo->skb;
-	struct usb_card_rec *cardp = (struct usb_card_rec *)priv->card;
+	struct usb_card_rec *cardp = (struct usb_card_rec *) rinfo->cardp;
+	wlan_private * priv = cardp->priv;
 
 	int recvlength = urb->actual_length;
 	u8 *recvbuff = NULL;
-	u32 recvtype;
+	u32 recvtype = 0;
 
 	lbs_deb_enter(LBS_DEB_USB);
 
 	if (recvlength) {
+		__le32 tmp;
+
 		if (urb->status) {
 			lbs_deb_usbd(&cardp->udev->dev,
 				    "URB status is failed\n");
@@ -644,18 +671,14 @@ static void if_usb_receive(struct urb *urb)
 		}
 
 		recvbuff = skb->data + IPFIELD_ALIGN_OFFSET;
-		memcpy(&recvtype, recvbuff, sizeof(u32));
-		lbs_deb_usbd(&cardp->udev->dev,
-			    "Recv length = 0x%x\n", recvlength);
-		lbs_deb_usbd(&cardp->udev->dev,
-			    "Receive type = 0x%X\n", recvtype);
-		recvtype = le32_to_cpu(recvtype);
+		memcpy(&tmp, recvbuff, sizeof(u32));
+		recvtype = le32_to_cpu(tmp);
 		lbs_deb_usbd(&cardp->udev->dev,
-			    "Receive type after = 0x%X\n", recvtype);
+			    "Recv length = 0x%x, Recv type = 0x%X\n",
+			    recvlength, recvtype);
 	} else if (urb->status)
 		goto rx_exit;
 
-
 	switch (recvtype) {
 	case CMD_TYPE_DATA:
 		process_cmdtypedata(recvlength, skb, cardp, priv);
@@ -677,18 +700,20 @@ static void if_usb_receive(struct urb *urb)
 			break;
 		}
 		cardp->usb_event_cause <<= 3;
-		cardp->usb_int_cause |= his_cardevent;
+		cardp->usb_int_cause |= MRVDRV_CARDEVENT;
 		kfree_skb(skb);
 		libertas_interrupt(priv->dev);
 		spin_unlock(&priv->adapter->driver_lock);
 		goto rx_exit;
 	default:
+		lbs_deb_usbd(&cardp->udev->dev, "Unknown command type 0x%X\n",
+		             recvtype);
 		kfree_skb(skb);
 		break;
 	}
 
 setup_for_next:
-	if_usb_submit_rx_urb(priv);
+	if_usb_submit_rx_urb(cardp);
 rx_exit:
 	lbs_deb_leave(LBS_DEB_USB);
 }
@@ -703,21 +728,19 @@ rx_exit:
  */
 static int if_usb_host_to_card(wlan_private * priv, u8 type, u8 * payload, u16 nb)
 {
-	int ret = -1;
-	u32 tmp;
 	struct usb_card_rec *cardp = (struct usb_card_rec *)priv->card;
 
 	lbs_deb_usbd(&cardp->udev->dev,"*** type = %u\n", type);
 	lbs_deb_usbd(&cardp->udev->dev,"size after = %d\n", nb);
 
 	if (type == MVMS_CMD) {
-		tmp = cpu_to_le32(CMD_TYPE_REQUEST);
+		__le32 tmp = cpu_to_le32(CMD_TYPE_REQUEST);
 		priv->dnld_sent = DNLD_CMD_SENT;
 		memcpy(cardp->bulk_out_buffer, (u8 *) & tmp,
 		       MESSAGE_HEADER_LEN);
 
 	} else {
-		tmp = cpu_to_le32(CMD_TYPE_DATA);
+		__le32 tmp = cpu_to_le32(CMD_TYPE_DATA);
 		priv->dnld_sent = DNLD_DATA_SENT;
 		memcpy(cardp->bulk_out_buffer, (u8 *) & tmp,
 		       MESSAGE_HEADER_LEN);
@@ -725,10 +748,8 @@ static int if_usb_host_to_card(wlan_private * priv, u8 type, u8 * payload, u16 n
 
 	memcpy((cardp->bulk_out_buffer + MESSAGE_HEADER_LEN), payload, nb);
 
-	ret =
-	    usb_tx_block(priv, cardp->bulk_out_buffer, nb + MESSAGE_HEADER_LEN);
-
-	return ret;
+	return usb_tx_block(cardp, cardp->bulk_out_buffer,
+	                    nb + MESSAGE_HEADER_LEN);
 }
 
 /* called with adapter->driver_lock held */
@@ -747,80 +768,109 @@ static int if_usb_get_int_status(wlan_private * priv, u8 * ireg)
 static int if_usb_read_event_cause(wlan_private * priv)
 {
 	struct usb_card_rec *cardp = priv->card;
+
 	priv->adapter->eventcause = cardp->usb_event_cause;
 	/* Re-submit rx urb here to avoid event lost issue */
-	if_usb_submit_rx_urb(priv);
+	if_usb_submit_rx_urb(cardp);
 	return 0;
 }
 
-static int if_usb_reset_device(wlan_private *priv)
+/**
+ *  @brief This function issues Boot command to the Boot2 code
+ *  @param ivalue   1:Boot from FW by USB-Download
+ *                  2:Boot from FW in EEPROM
+ *  @return 	   	0
+ */
+static int if_usb_issue_boot_command(struct usb_card_rec *cardp, int ivalue)
 {
-	int ret;
-
-	lbs_deb_enter(LBS_DEB_USB);
-	ret = libertas_prepare_and_send_command(priv, cmd_802_11_reset,
-				    cmd_act_halt, 0, 0, NULL);
-	msleep_interruptible(10);
-
-	lbs_deb_leave_args(LBS_DEB_USB, "ret %d", ret);
-	return ret;
-}
+	struct bootcmdstr sbootcmd;
+	int i;
 
-static int if_usb_unregister_dev(wlan_private * priv)
-{
-	int ret = 0;
+	/* Prepare command */
+	sbootcmd.u32magicnumber = cpu_to_le32(BOOT_CMD_MAGIC_NUMBER);
+	sbootcmd.u8cmd_tag = ivalue;
+	for (i=0; i<11; i++)
+		sbootcmd.au8dumy[i]=0x00;
+	memcpy(cardp->bulk_out_buffer, &sbootcmd, sizeof(struct bootcmdstr));
 
-	/* Need to send a Reset command to device before USB resources freed
-	 * and wlan_remove_card() called, then device can handle FW download
-	 * again.
-	 */
-	if (priv)
-		if_usb_reset_device(priv);
+	/* Issue command */
+	usb_tx_block(cardp, cardp->bulk_out_buffer, sizeof(struct bootcmdstr));
 
-	return ret;
+	return 0;
 }
 
 
 /**
- *  @brief  This function register usb device and initialize parameter
- *  @param		priv pointer to wlan_private
- *  @return		0 or -1
+ *  @brief This function checks the validity of Boot2/FW image.
+ *
+ *  @param data              pointer to image
+ *         len               image length
+ *  @return     0 or -1
  */
-static int if_usb_register_dev(wlan_private * priv)
+static int check_fwfile_format(u8 *data, u32 totlen)
 {
-	struct usb_card_rec *cardp = (struct usb_card_rec *)priv->card;
+	u32 bincmd, exit;
+	u32 blksize, offset, len;
+	int ret;
 
-	lbs_deb_enter(LBS_DEB_USB);
+	ret = 1;
+	exit = len = 0;
 
-	cardp->priv = priv;
-	cardp->eth_dev = priv->dev;
-	priv->hotplug_device = &(cardp->udev->dev);
+	do {
+		struct fwheader *fwh = (void *)data;
+
+		bincmd = le32_to_cpu(fwh->dnldcmd);
+		blksize = le32_to_cpu(fwh->datalength);
+		switch (bincmd) {
+		case FW_HAS_DATA_TO_RECV:
+			offset = sizeof(struct fwheader) + blksize;
+			data += offset;
+			len += offset;
+			if (len >= totlen)
+				exit = 1;
+			break;
+		case FW_HAS_LAST_BLOCK:
+			exit = 1;
+			ret = 0;
+			break;
+		default:
+			exit = 1;
+			break;
+		}
+	} while (!exit);
 
-	lbs_deb_usbd(&cardp->udev->dev, "udev pointer is at %p\n",
-		    cardp->udev);
+	if (ret)
+		lbs_pr_err("firmware file format check FAIL\n");
+	else
+		lbs_deb_fw("firmware file format check PASS\n");
 
-	lbs_deb_leave(LBS_DEB_USB);
-	return 0;
+	return ret;
 }
 
 
-
-static int if_usb_prog_firmware(wlan_private * priv)
+static int if_usb_prog_firmware(struct usb_card_rec *cardp)
 {
-	struct usb_card_rec *cardp = priv->card;
 	int i = 0;
 	static int reset_count = 10;
 	int ret = 0;
 
 	lbs_deb_enter(LBS_DEB_USB);
 
-	cardp->rinfo.priv = priv;
+	if ((ret = request_firmware(&cardp->fw, libertas_fw_name,
+				    &cardp->udev->dev)) < 0) {
+		lbs_pr_err("request_firmware() failed with %#x\n", ret);
+		lbs_pr_err("firmware %s not found\n", libertas_fw_name);
+		goto done;
+	}
+
+	if (check_fwfile_format(cardp->fw->data, cardp->fw->size))
+		goto release_fw;
 
 restart:
-	if (if_usb_submit_rx_urb_fwload(priv) < 0) {
+	if (if_usb_submit_rx_urb_fwload(cardp) < 0) {
 		lbs_deb_usbd(&cardp->udev->dev, "URB submission is failed\n");
 		ret = -1;
-		goto done;
+		goto release_fw;
 	}
 
 	cardp->bootcmdresp = 0;
@@ -828,7 +878,7 @@ restart:
 		int j = 0;
 		i++;
 		/* Issue Boot command = 1, Boot from Download-FW */
-		if_usb_issue_boot_command(priv, BOOT_CMD_FW_BY_USB);
+		if_usb_issue_boot_command(cardp, BOOT_CMD_FW_BY_USB);
 		/* wait for command response */
 		do {
 			j++;
@@ -838,14 +888,13 @@ restart:
 
 	if (cardp->bootcmdresp == 0) {
 		if (--reset_count >= 0) {
-			libertas_do_reset(priv);
+			if_usb_reset_device(cardp);
 			goto restart;
 		}
 		return -1;
 	}
 
 	i = 0;
-	priv->adapter->fw_ready = 0;
 
 	cardp->totalbytes = 0;
 	cardp->fwlastblksent = 0;
@@ -855,40 +904,38 @@ restart:
 	cardp->totalbytes = 0;
 	cardp->fwfinalblk = 0;
 
-	if_prog_firmware(priv);
+	if_prog_firmware(cardp);
 
 	do {
 		lbs_deb_usbd(&cardp->udev->dev,"Wlan sched timeout\n");
 		i++;
 		msleep_interruptible(100);
-		if (priv->adapter->surpriseremoved || i >= 20)
+		if (cardp->surprise_removed || i >= 20)
 			break;
 	} while (!cardp->fwdnldover);
 
 	if (!cardp->fwdnldover) {
 		lbs_pr_info("failed to load fw, resetting device!\n");
 		if (--reset_count >= 0) {
-			libertas_do_reset(priv);
+			if_usb_reset_device(cardp);
 			goto restart;
 		}
 
 		lbs_pr_info("FW download failure, time = %d ms\n", i * 100);
 		ret = -1;
-		goto done;
+		goto release_fw;
 	}
 
-	if_usb_submit_rx_urb(priv);
-
-	/* Delay 200 ms to waiting for the FW ready */
-	msleep_interruptible(200);
-
-	priv->adapter->fw_ready = 1;
+release_fw:
+	release_firmware(cardp->fw);
+	cardp->fw = NULL;
 
 done:
 	lbs_deb_leave_args(LBS_DEB_USB, "ret %d", ret);
 	return ret;
 }
 
+
 #ifdef CONFIG_PM
 static int if_usb_suspend(struct usb_interface *intf, pm_message_t message)
 {
@@ -900,6 +947,19 @@ static int if_usb_suspend(struct usb_interface *intf, pm_message_t message)
 	if (priv->adapter->psstate != PS_STATE_FULL_POWER)
 		return -1;
 
+	if (priv->mesh_dev && !priv->mesh_autostart_enabled) {
+		/* Mesh autostart must be activated while sleeping
+		 * On resume it will go back to the current state
+		 */
+		struct cmd_ds_mesh_access mesh_access;
+		memset(&mesh_access, 0, sizeof(mesh_access));
+		mesh_access.data[0] = cpu_to_le32(1);
+		libertas_prepare_and_send_command(priv,
+				CMD_MESH_ACCESS,
+				CMD_ACT_MESH_SET_AUTOSTART_ENABLED,
+				CMD_OPTION_WAITFORRSP, 0, (void *)&mesh_access);
+	}
+
 	netif_device_detach(cardp->eth_dev);
 	netif_device_detach(priv->mesh_dev);
 
@@ -927,6 +987,19 @@ static int if_usb_resume(struct usb_interface *intf)
 	netif_device_attach(cardp->eth_dev);
 	netif_device_attach(priv->mesh_dev);
 
+	if (priv->mesh_dev && !priv->mesh_autostart_enabled) {
+		/* Mesh autostart was activated while sleeping
+		 * Disable it if appropriate
+		 */
+		struct cmd_ds_mesh_access mesh_access;
+		memset(&mesh_access, 0, sizeof(mesh_access));
+		mesh_access.data[0] = cpu_to_le32(0);
+		libertas_prepare_and_send_command(priv,
+				CMD_MESH_ACCESS,
+				CMD_ACT_MESH_SET_AUTOSTART_ENABLED,
+				CMD_OPTION_WAITFORRSP, 0, (void *)&mesh_access);
+	}
+
 	lbs_deb_leave(LBS_DEB_USB);
 	return 0;
 }
@@ -970,8 +1043,10 @@ static void if_usb_exit_module(void)
 
 	lbs_deb_enter(LBS_DEB_MAIN);
 
-	list_for_each_entry_safe(cardp, cardp_temp, &usb_devices, list)
-		if_usb_reset_device((wlan_private *) cardp->priv);
+	list_for_each_entry_safe(cardp, cardp_temp, &usb_devices, list) {
+		libertas_prepare_and_send_command(cardp->priv, CMD_802_11_RESET,
+		                                  CMD_ACT_HALT, 0, 0, NULL);
+	}
 
 	/* API unregisters the driver from USB subsystem */
 	usb_deregister(&if_usb_driver);
diff --git a/drivers/net/wireless/libertas/if_usb.h b/drivers/net/wireless/libertas/if_usb.h
index 156bb485e1a6..e07a10ed28b5 100644
--- a/drivers/net/wireless/libertas/if_usb.h
+++ b/drivers/net/wireless/libertas/if_usb.h
@@ -38,7 +38,7 @@ struct bootcmdrespStr
 
 /* read callback private data */
 struct read_cb_info {
-        wlan_private *priv;
+        struct usb_card_rec *cardp;
         struct sk_buff *skb;
 };
 
@@ -58,6 +58,7 @@ struct usb_card_rec {
 	int bulk_out_size;
 	u8 bulk_out_endpointAddr;
 
+	const struct firmware *fw;
 	u8 CRC_OK;
 	u32 fwseqnum;
 	u32 lastseqnum;
@@ -65,6 +66,7 @@ struct usb_card_rec {
 	u32 fwlastblksent;
 	u8 fwdnldover;
 	u8 fwfinalblk;
+	u8 surprise_removed;
 
 	u32 usb_event_cause;
 	u8 usb_int_cause;
@@ -102,8 +104,4 @@ struct fwsyncheader {
 #define FW_DATA_XMIT_SIZE \
 	sizeof(struct fwheader) + le32_to_cpu(fwdata->fwheader.datalength) + sizeof(u32)
 
-int usb_tx_block(wlan_private *priv, u8 *payload, u16 nb);
-void if_usb_free(struct usb_card_rec *cardp);
-int if_usb_issue_boot_command(wlan_private *priv, int ivalue);
-
 #endif
diff --git a/drivers/net/wireless/libertas/join.c b/drivers/net/wireless/libertas/join.c
index 78ac3064a0bd..dc24a05c9447 100644
--- a/drivers/net/wireless/libertas/join.c
+++ b/drivers/net/wireless/libertas/join.c
@@ -17,10 +17,13 @@
 #include "dev.h"
 #include "assoc.h"
 
-#define AD_HOC_CAP_PRIVACY_ON 1
+/* The firmware needs certain bits masked out of the beacon-derviced capability
+ * field when associating/joining to BSSs.
+ */
+#define CAPINFO_MASK	(~(0xda00))
 
 /**
- *  @brief This function finds out the common rates between rate1 and rate2.
+ *  @brief This function finds common rates between rate1 and card rates.
  *
  * It will fill common rates in rate1 as output if found.
  *
@@ -29,75 +32,87 @@
  *
  *  @param adapter     A pointer to wlan_adapter structure
  *  @param rate1       the buffer which keeps input and output
- *  @param rate1_size  the size of rate1 buffer
- *  @param rate2       the buffer which keeps rate2
- *  @param rate2_size  the size of rate2 buffer.
+ *  @param rate1_size  the size of rate1 buffer; new size of buffer on return
  *
  *  @return            0 or -1
  */
-static int get_common_rates(wlan_adapter * adapter, u8 * rate1,
-			    int rate1_size, u8 * rate2, int rate2_size)
+static int get_common_rates(wlan_adapter * adapter, u8 * rates, u16 *rates_size)
 {
-	u8 *ptr = rate1;
-	int ret = 0;
+	u8 *card_rates = libertas_bg_rates;
+	size_t num_card_rates = sizeof(libertas_bg_rates);
+	int ret = 0, i, j;
 	u8 tmp[30];
-	int i;
+	size_t tmp_size = 0;
 
-	memset(&tmp, 0, sizeof(tmp));
-	memcpy(&tmp, rate1, min_t(size_t, rate1_size, sizeof(tmp)));
-	memset(rate1, 0, rate1_size);
-
-	/* Mask the top bit of the original values */
-	for (i = 0; tmp[i] && i < sizeof(tmp); i++)
-		tmp[i] &= 0x7F;
-
-	for (i = 0; rate2[i] && i < rate2_size; i++) {
-		/* Check for Card Rate in tmp, excluding the top bit */
-		if (strchr(tmp, rate2[i] & 0x7F)) {
-			/* values match, so copy the Card Rate to rate1 */
-			*rate1++ = rate2[i];
+	/* For each rate in card_rates that exists in rate1, copy to tmp */
+	for (i = 0; card_rates[i] && (i < num_card_rates); i++) {
+		for (j = 0; rates[j] && (j < *rates_size); j++) {
+			if (rates[j] == card_rates[i])
+				tmp[tmp_size++] = card_rates[i];
 		}
 	}
 
-	lbs_dbg_hex("rate1 (AP) rates:", tmp, sizeof(tmp));
-	lbs_dbg_hex("rate2 (Card) rates:", rate2, rate2_size);
-	lbs_dbg_hex("Common rates:", ptr, rate1_size);
-	lbs_deb_join("Tx datarate is set to 0x%X\n", adapter->datarate);
+	lbs_deb_hex(LBS_DEB_JOIN, "AP rates    ", rates, *rates_size);
+	lbs_deb_hex(LBS_DEB_JOIN, "card rates  ", card_rates, num_card_rates);
+	lbs_deb_hex(LBS_DEB_JOIN, "common rates", tmp, tmp_size);
+	lbs_deb_join("Tx datarate is currently 0x%X\n", adapter->cur_rate);
 
-	if (!adapter->is_datarate_auto) {
-		while (*ptr) {
-			if ((*ptr & 0x7f) == adapter->datarate) {
-				ret = 0;
+	if (!adapter->auto_rate) {
+		for (i = 0; i < tmp_size; i++) {
+			if (tmp[i] == adapter->cur_rate)
 				goto done;
-			}
-			ptr++;
 		}
-		lbs_pr_alert( "Previously set fixed data rate %#x isn't "
-		       "compatible with the network.\n", adapter->datarate);
-
+		lbs_pr_alert("Previously set fixed data rate %#x isn't "
+		       "compatible with the network.\n", adapter->cur_rate);
 		ret = -1;
 		goto done;
 	}
-
 	ret = 0;
+
 done:
+	memset(rates, 0, *rates_size);
+	*rates_size = min_t(int, tmp_size, *rates_size);
+	memcpy(rates, tmp, *rates_size);
 	return ret;
 }
 
-int libertas_send_deauth(wlan_private * priv)
+
+/**
+ *  @brief Sets the MSB on basic rates as the firmware requires
+ *
+ * Scan through an array and set the MSB for basic data rates.
+ *
+ *  @param rates     buffer of data rates
+ *  @param len       size of buffer
+ */
+static void libertas_set_basic_rate_flags(u8 * rates, size_t len)
 {
-	wlan_adapter *adapter = priv->adapter;
-	int ret = 0;
+	int i;
 
-	if (adapter->mode == IW_MODE_INFRA &&
-	    adapter->connect_status == libertas_connected)
-		ret = libertas_send_deauthentication(priv);
-	else
-		ret = -ENOTSUPP;
+	for (i = 0; i < len; i++) {
+		if (rates[i] == 0x02 || rates[i] == 0x04 ||
+		    rates[i] == 0x0b || rates[i] == 0x16)
+			rates[i] |= 0x80;
+	}
+}
 
-	return ret;
+/**
+ *  @brief Unsets the MSB on basic rates
+ *
+ * Scan through an array and unset the MSB for basic data rates.
+ *
+ *  @param rates     buffer of data rates
+ *  @param len       size of buffer
+ */
+void libertas_unset_basic_rate_flags(u8 * rates, size_t len)
+{
+	int i;
+
+	for (i = 0; i < len; i++)
+		rates[i] &= 0x7f;
 }
 
+
 /**
  *  @brief Associate to a specific BSS discovered in a scan
  *
@@ -113,23 +128,24 @@ int wlan_associate(wlan_private * priv, struct assoc_request * assoc_req)
 
 	lbs_deb_enter(LBS_DEB_JOIN);
 
-	ret = libertas_prepare_and_send_command(priv, cmd_802_11_authenticate,
-				    0, cmd_option_waitforrsp,
+	ret = libertas_prepare_and_send_command(priv, CMD_802_11_AUTHENTICATE,
+				    0, CMD_OPTION_WAITFORRSP,
 				    0, assoc_req->bss.bssid);
 
 	if (ret)
 		goto done;
 
 	/* set preamble to firmware */
-	if (adapter->capinfo.shortpreamble && assoc_req->bss.cap.shortpreamble)
-		adapter->preamble = cmd_type_short_preamble;
+	if (   (adapter->capability & WLAN_CAPABILITY_SHORT_PREAMBLE)
+	    && (assoc_req->bss.capability & WLAN_CAPABILITY_SHORT_PREAMBLE))
+		adapter->preamble = CMD_TYPE_SHORT_PREAMBLE;
 	else
-		adapter->preamble = cmd_type_long_preamble;
+		adapter->preamble = CMD_TYPE_LONG_PREAMBLE;
 
 	libertas_set_radio_control(priv);
 
-	ret = libertas_prepare_and_send_command(priv, cmd_802_11_associate,
-				    0, cmd_option_waitforrsp, 0, assoc_req);
+	ret = libertas_prepare_and_send_command(priv, CMD_802_11_ASSOCIATE,
+				    0, CMD_OPTION_WAITFORRSP, 0, assoc_req);
 
 done:
 	lbs_deb_leave_args(LBS_DEB_JOIN, "ret %d", ret);
@@ -150,12 +166,12 @@ int libertas_start_adhoc_network(wlan_private * priv, struct assoc_request * ass
 
 	adapter->adhoccreate = 1;
 
-	if (!adapter->capinfo.shortpreamble) {
-		lbs_deb_join("AdhocStart: Long preamble\n");
-		adapter->preamble = cmd_type_long_preamble;
-	} else {
+	if (adapter->capability & WLAN_CAPABILITY_SHORT_PREAMBLE) {
 		lbs_deb_join("AdhocStart: Short preamble\n");
-		adapter->preamble = cmd_type_short_preamble;
+		adapter->preamble = CMD_TYPE_SHORT_PREAMBLE;
+	} else {
+		lbs_deb_join("AdhocStart: Long preamble\n");
+		adapter->preamble = CMD_TYPE_LONG_PREAMBLE;
 	}
 
 	libertas_set_radio_control(priv);
@@ -163,8 +179,8 @@ int libertas_start_adhoc_network(wlan_private * priv, struct assoc_request * ass
 	lbs_deb_join("AdhocStart: channel = %d\n", assoc_req->channel);
 	lbs_deb_join("AdhocStart: band = %d\n", assoc_req->band);
 
-	ret = libertas_prepare_and_send_command(priv, cmd_802_11_ad_hoc_start,
-				    0, cmd_option_waitforrsp, 0, assoc_req);
+	ret = libertas_prepare_and_send_command(priv, CMD_802_11_AD_HOC_START,
+				    0, CMD_OPTION_WAITFORRSP, 0, assoc_req);
 
 	return ret;
 }
@@ -194,25 +210,37 @@ int libertas_join_adhoc_network(wlan_private * priv, struct assoc_request * asso
 	             bss->ssid_len);
 
 	/* check if the requested SSID is already joined */
-	if (adapter->curbssparams.ssid_len
+	if (   adapter->curbssparams.ssid_len
 	    && !libertas_ssid_cmp(adapter->curbssparams.ssid,
 	                          adapter->curbssparams.ssid_len,
 	                          bss->ssid, bss->ssid_len)
-	    && (adapter->mode == IW_MODE_ADHOC)) {
-		lbs_deb_join(
-		       "ADHOC_J_CMD: New ad-hoc SSID is the same as current, "
-		       "not attempting to re-join");
-		return -1;
+	    && (adapter->mode == IW_MODE_ADHOC)
+	    && (adapter->connect_status == LIBERTAS_CONNECTED)) {
+		union iwreq_data wrqu;
+
+		lbs_deb_join("ADHOC_J_CMD: New ad-hoc SSID is the same as "
+		             "current, not attempting to re-join");
+
+		/* Send the re-association event though, because the association
+		 * request really was successful, even if just a null-op.
+		 */
+		memset(&wrqu, 0, sizeof(wrqu));
+		memcpy(wrqu.ap_addr.sa_data, adapter->curbssparams.bssid,
+		       ETH_ALEN);
+		wrqu.ap_addr.sa_family = ARPHRD_ETHER;
+		wireless_send_event(priv->dev, SIOCGIWAP, &wrqu, NULL);
+		goto out;
 	}
 
-	/*Use shortpreamble only when both creator and card supports
+	/* Use shortpreamble only when both creator and card supports
 	   short preamble */
-	if (!bss->cap.shortpreamble || !adapter->capinfo.shortpreamble) {
+	if (   !(bss->capability & WLAN_CAPABILITY_SHORT_PREAMBLE)
+	    || !(adapter->capability & WLAN_CAPABILITY_SHORT_PREAMBLE)) {
 		lbs_deb_join("AdhocJoin: Long preamble\n");
-		adapter->preamble = cmd_type_long_preamble;
+		adapter->preamble = CMD_TYPE_LONG_PREAMBLE;
 	} else {
 		lbs_deb_join("AdhocJoin: Short preamble\n");
-		adapter->preamble = cmd_type_short_preamble;
+		adapter->preamble = CMD_TYPE_SHORT_PREAMBLE;
 	}
 
 	libertas_set_radio_control(priv);
@@ -222,17 +250,18 @@ int libertas_join_adhoc_network(wlan_private * priv, struct assoc_request * asso
 
 	adapter->adhoccreate = 0;
 
-	ret = libertas_prepare_and_send_command(priv, cmd_802_11_ad_hoc_join,
-				    0, cmd_option_waitforrsp,
+	ret = libertas_prepare_and_send_command(priv, CMD_802_11_AD_HOC_JOIN,
+				    0, CMD_OPTION_WAITFORRSP,
 				    OID_802_11_SSID, assoc_req);
 
+out:
 	return ret;
 }
 
 int libertas_stop_adhoc_network(wlan_private * priv)
 {
-	return libertas_prepare_and_send_command(priv, cmd_802_11_ad_hoc_stop,
-				     0, cmd_option_waitforrsp, 0, NULL);
+	return libertas_prepare_and_send_command(priv, CMD_802_11_AD_HOC_STOP,
+				     0, CMD_OPTION_WAITFORRSP, 0, NULL);
 }
 
 /**
@@ -243,8 +272,8 @@ int libertas_stop_adhoc_network(wlan_private * priv)
  */
 int libertas_send_deauthentication(wlan_private * priv)
 {
-	return libertas_prepare_and_send_command(priv, cmd_802_11_deauthenticate,
-				     0, cmd_option_waitforrsp, 0, NULL);
+	return libertas_prepare_and_send_command(priv, CMD_802_11_DEAUTHENTICATE,
+				     0, CMD_OPTION_WAITFORRSP, 0, NULL);
 }
 
 /**
@@ -264,10 +293,11 @@ int libertas_cmd_80211_authenticate(wlan_private * priv,
 	struct cmd_ds_802_11_authenticate *pauthenticate = &cmd->params.auth;
 	int ret = -1;
 	u8 *bssid = pdata_buf;
+	DECLARE_MAC_BUF(mac);
 
 	lbs_deb_enter(LBS_DEB_JOIN);
 
-	cmd->command = cpu_to_le16(cmd_802_11_authenticate);
+	cmd->command = cpu_to_le16(CMD_802_11_AUTHENTICATE);
 	cmd->size = cpu_to_le16(sizeof(struct cmd_ds_802_11_authenticate)
 	                        + S_DS_GEN);
 
@@ -290,8 +320,8 @@ int libertas_cmd_80211_authenticate(wlan_private * priv,
 
 	memcpy(pauthenticate->macaddr, bssid, ETH_ALEN);
 
-	lbs_deb_join("AUTH_CMD: BSSID is : " MAC_FMT " auth=0x%X\n",
-	             MAC_ARG(bssid), pauthenticate->authtype);
+	lbs_deb_join("AUTH_CMD: BSSID is : %s auth=0x%X\n",
+	             print_mac(mac, bssid), pauthenticate->authtype);
 	ret = 0;
 
 out:
@@ -307,7 +337,7 @@ int libertas_cmd_80211_deauthenticate(wlan_private * priv,
 
 	lbs_deb_enter(LBS_DEB_JOIN);
 
-	cmd->command = cpu_to_le16(cmd_802_11_deauthenticate);
+	cmd->command = cpu_to_le16(CMD_802_11_DEAUTHENTICATE);
 	cmd->size = cpu_to_le16(sizeof(struct cmd_ds_802_11_deauthenticate) +
 			     S_DS_GEN);
 
@@ -330,9 +360,7 @@ int libertas_cmd_80211_associate(wlan_private * priv,
 	int ret = 0;
 	struct assoc_request * assoc_req = pdata_buf;
 	struct bss_descriptor * bss = &assoc_req->bss;
-	u8 *card_rates;
 	u8 *pos;
-	int card_rates_size;
 	u16 tmpcap, tmplen;
 	struct mrvlietypes_ssidparamset *ssid;
 	struct mrvlietypes_phyparamset *phy;
@@ -349,15 +377,15 @@ int libertas_cmd_80211_associate(wlan_private * priv,
 		goto done;
 	}
 
-	cmd->command = cpu_to_le16(cmd_802_11_associate);
+	cmd->command = cpu_to_le16(CMD_802_11_ASSOCIATE);
 
 	memcpy(passo->peerstaaddr, bss->bssid, sizeof(passo->peerstaaddr));
 	pos += sizeof(passo->peerstaaddr);
 
 	/* set the listen interval */
-	passo->listeninterval = cpu_to_le16(adapter->listeninterval);
+	passo->listeninterval = cpu_to_le16(MRVDRV_DEFAULT_LISTEN_INTERVAL);
 
-	pos += sizeof(passo->capinfo);
+	pos += sizeof(passo->capability);
 	pos += sizeof(passo->listeninterval);
 	pos += sizeof(passo->bcnperiod);
 	pos += sizeof(passo->dtimperiod);
@@ -386,23 +414,24 @@ int libertas_cmd_80211_associate(wlan_private * priv,
 
 	rates = (struct mrvlietypes_ratesparamset *) pos;
 	rates->header.type = cpu_to_le16(TLV_TYPE_RATES);
-
-	memcpy(&rates->rates, &bss->libertas_supported_rates, WLAN_SUPPORTED_RATES);
-
-	card_rates = libertas_supported_rates;
-	card_rates_size = sizeof(libertas_supported_rates);
-
-	if (get_common_rates(adapter, rates->rates, WLAN_SUPPORTED_RATES,
-			     card_rates, card_rates_size)) {
+	memcpy(&rates->rates, &bss->rates, MAX_RATES);
+	tmplen = MAX_RATES;
+	if (get_common_rates(adapter, rates->rates, &tmplen)) {
 		ret = -1;
 		goto done;
 	}
-
-	tmplen = min_t(size_t, strlen(rates->rates), WLAN_SUPPORTED_RATES);
-	adapter->curbssparams.numofrates = tmplen;
-
 	pos += sizeof(rates->header) + tmplen;
 	rates->header.len = cpu_to_le16(tmplen);
+	lbs_deb_join("ASSOC_CMD: num rates = %u\n", tmplen);
+
+	/* Copy the infra. association rates into Current BSS state structure */
+	memset(&adapter->curbssparams.rates, 0, sizeof(adapter->curbssparams.rates));
+	memcpy(&adapter->curbssparams.rates, &rates->rates, tmplen);
+
+	/* Set MSB on basic rates as the firmware requires, but _after_
+	 * copying to current bss rates.
+	 */
+	libertas_set_basic_rate_flags(rates->rates, tmplen);
 
 	if (assoc_req->secinfo.WPAenabled || assoc_req->secinfo.WPA2enabled) {
 		rsn = (struct mrvlietypes_rsnparamset *) pos;
@@ -411,7 +440,7 @@ int libertas_cmd_80211_associate(wlan_private * priv,
 		tmplen = (u16) assoc_req->wpa_ie[1];
 		rsn->header.len = cpu_to_le16(tmplen);
 		memcpy(rsn->rsnie, &assoc_req->wpa_ie[2], tmplen);
-		lbs_dbg_hex("ASSOC_CMD: RSN IE", (u8 *) rsn,
+		lbs_deb_hex(LBS_DEB_JOIN, "ASSOC_CMD: RSN IE", (u8 *) rsn,
 			sizeof(rsn->header) + tmplen);
 		pos += sizeof(rsn->header) + tmplen;
 	}
@@ -419,20 +448,6 @@ int libertas_cmd_80211_associate(wlan_private * priv,
 	/* update curbssparams */
 	adapter->curbssparams.channel = bss->phyparamset.dsparamset.currentchan;
 
-	/* Copy the infra. association rates into Current BSS state structure */
-	memcpy(&adapter->curbssparams.datarates, &rates->rates,
-	       min_t(size_t, sizeof(adapter->curbssparams.datarates),
-		     cpu_to_le16(rates->header.len)));
-
-	lbs_deb_join("ASSOC_CMD: rates->header.len = %d\n",
-		     cpu_to_le16(rates->header.len));
-
-	/* set IBSS field */
-	if (bss->mode == IW_MODE_INFRA) {
-#define CAPINFO_ESS_MODE 1
-		passo->capinfo.ess = CAPINFO_ESS_MODE;
-	}
-
 	if (libertas_parse_dnld_countryinfo_11d(priv, bss)) {
 		ret = -1;
 		goto done;
@@ -440,12 +455,13 @@ int libertas_cmd_80211_associate(wlan_private * priv,
 
 	cmd->size = cpu_to_le16((u16) (pos - (u8 *) passo) + S_DS_GEN);
 
-	/* set the capability info at last */
-	memcpy(&tmpcap, &bss->cap, sizeof(passo->capinfo));
-	tmpcap &= CAPINFO_MASK;
-	lbs_deb_join("ASSOC_CMD: tmpcap=%4X CAPINFO_MASK=%4X\n",
+	/* set the capability info */
+	tmpcap = (bss->capability & CAPINFO_MASK);
+	if (bss->mode == IW_MODE_INFRA)
+		tmpcap |= WLAN_CAPABILITY_ESS;
+	passo->capability = cpu_to_le16(tmpcap);
+	lbs_deb_join("ASSOC_CMD: capability=%4X CAPINFO_MASK=%4X\n",
 		     tmpcap, CAPINFO_MASK);
-	memcpy(&passo->capinfo, &tmpcap, sizeof(passo->capinfo));
 
 done:
 	lbs_deb_leave_args(LBS_DEB_JOIN, "ret %d", ret);
@@ -459,8 +475,9 @@ int libertas_cmd_80211_ad_hoc_start(wlan_private * priv,
 	struct cmd_ds_802_11_ad_hoc_start *adhs = &cmd->params.ads;
 	int ret = 0;
 	int cmdappendsize = 0;
-	int i;
 	struct assoc_request * assoc_req = pdata_buf;
+	u16 tmpcap = 0;
+	size_t ratesize = 0;
 
 	lbs_deb_enter(LBS_DEB_JOIN);
 
@@ -469,7 +486,7 @@ int libertas_cmd_80211_ad_hoc_start(wlan_private * priv,
 		goto done;
 	}
 
-	cmd->command = cpu_to_le16(cmd_802_11_ad_hoc_start);
+	cmd->command = cpu_to_le16(CMD_802_11_AD_HOC_START);
 
 	/*
 	 * Fill in the parameters for 2 data structures:
@@ -483,17 +500,17 @@ int libertas_cmd_80211_ad_hoc_start(wlan_private * priv,
 	 *   and operational rates.
 	 */
 
-	memset(adhs->SSID, 0, IW_ESSID_MAX_SIZE);
-	memcpy(adhs->SSID, assoc_req->ssid, assoc_req->ssid_len);
+	memset(adhs->ssid, 0, IW_ESSID_MAX_SIZE);
+	memcpy(adhs->ssid, assoc_req->ssid, assoc_req->ssid_len);
 
 	lbs_deb_join("ADHOC_S_CMD: SSID '%s', ssid length %u\n",
 	             escape_essid(assoc_req->ssid, assoc_req->ssid_len),
 	             assoc_req->ssid_len);
 
 	/* set the BSS type */
-	adhs->bsstype = cmd_bss_type_ibss;
+	adhs->bsstype = CMD_BSS_TYPE_IBSS;
 	adapter->mode = IW_MODE_ADHOC;
-	adhs->beaconperiod = cpu_to_le16(adapter->beaconperiod);
+	adhs->beaconperiod = cpu_to_le16(MRVDRV_BEACON_INTERVAL);
 
 	/* set Physical param set */
 #define DS_PARA_IE_ID   3
@@ -515,45 +532,36 @@ int libertas_cmd_80211_ad_hoc_start(wlan_private * priv,
 
 	adhs->ssparamset.ibssparamset.elementid = IBSS_PARA_IE_ID;
 	adhs->ssparamset.ibssparamset.len = IBSS_PARA_IE_LEN;
-	adhs->ssparamset.ibssparamset.atimwindow = cpu_to_le16(adapter->atimwindow);
+	adhs->ssparamset.ibssparamset.atimwindow = 0;
 
 	/* set capability info */
-	adhs->cap.ess = 0;
-	adhs->cap.ibss = 1;
-
-	/* probedelay */
-	adhs->probedelay = cpu_to_le16(cmd_scan_probe_delay_time);
-
-	/* set up privacy in adapter->scantable[i] */
+	tmpcap = WLAN_CAPABILITY_IBSS;
 	if (assoc_req->secinfo.wep_enabled) {
 		lbs_deb_join("ADHOC_S_CMD: WEP enabled, setting privacy on\n");
-		adhs->cap.privacy = AD_HOC_CAP_PRIVACY_ON;
+		tmpcap |= WLAN_CAPABILITY_PRIVACY;
 	} else {
 		lbs_deb_join("ADHOC_S_CMD: WEP disabled, setting privacy off\n");
 	}
+	adhs->capability = cpu_to_le16(tmpcap);
 
-	memset(adhs->datarate, 0, sizeof(adhs->datarate));
-
-	if (adapter->adhoc_grate_enabled) {
-		memcpy(adhs->datarate, libertas_adhoc_rates_g,
-		       min(sizeof(adhs->datarate), sizeof(libertas_adhoc_rates_g)));
-	} else {
-		memcpy(adhs->datarate, libertas_adhoc_rates_b,
-		       min(sizeof(adhs->datarate), sizeof(libertas_adhoc_rates_b)));
-	}
-
-	/* Find the last non zero */
-	for (i = 0; i < sizeof(adhs->datarate) && adhs->datarate[i]; i++) ;
+	/* probedelay */
+	adhs->probedelay = cpu_to_le16(CMD_SCAN_PROBE_DELAY_TIME);
 
-	adapter->curbssparams.numofrates = i;
+	memset(adhs->rates, 0, sizeof(adhs->rates));
+	ratesize = min(sizeof(adhs->rates), sizeof(libertas_bg_rates));
+	memcpy(adhs->rates, libertas_bg_rates, ratesize);
 
 	/* Copy the ad-hoc creating rates into Current BSS state structure */
-	memcpy(&adapter->curbssparams.datarates,
-	       &adhs->datarate, adapter->curbssparams.numofrates);
+	memset(&adapter->curbssparams.rates, 0, sizeof(adapter->curbssparams.rates));
+	memcpy(&adapter->curbssparams.rates, &adhs->rates, ratesize);
+
+	/* Set MSB on basic rates as the firmware requires, but _after_
+	 * copying to current bss rates.
+	 */
+	libertas_set_basic_rate_flags(adhs->rates, ratesize);
 
 	lbs_deb_join("ADHOC_S_CMD: rates=%02x %02x %02x %02x \n",
-	       adhs->datarate[0], adhs->datarate[1],
-	       adhs->datarate[2], adhs->datarate[3]);
+	       adhs->rates[0], adhs->rates[1], adhs->rates[2], adhs->rates[3]);
 
 	lbs_deb_join("ADHOC_S_CMD: AD HOC Start command is ready\n");
 
@@ -575,7 +583,7 @@ done:
 int libertas_cmd_80211_ad_hoc_stop(wlan_private * priv,
 				struct cmd_ds_command *cmd)
 {
-	cmd->command = cpu_to_le16(cmd_802_11_ad_hoc_stop);
+	cmd->command = cpu_to_le16(CMD_802_11_AD_HOC_STOP);
 	cmd->size = cpu_to_le16(S_DS_GEN);
 
 	return 0;
@@ -585,101 +593,84 @@ int libertas_cmd_80211_ad_hoc_join(wlan_private * priv,
 				struct cmd_ds_command *cmd, void *pdata_buf)
 {
 	wlan_adapter *adapter = priv->adapter;
-	struct cmd_ds_802_11_ad_hoc_join *padhocjoin = &cmd->params.adj;
+	struct cmd_ds_802_11_ad_hoc_join *join_cmd = &cmd->params.adj;
 	struct assoc_request * assoc_req = pdata_buf;
 	struct bss_descriptor *bss = &assoc_req->bss;
 	int cmdappendsize = 0;
 	int ret = 0;
-	u8 *card_rates;
-	int card_rates_size;
-	u16 tmpcap;
-	int i;
+	u16 ratesize = 0;
+	DECLARE_MAC_BUF(mac);
 
 	lbs_deb_enter(LBS_DEB_JOIN);
 
-	cmd->command = cpu_to_le16(cmd_802_11_ad_hoc_join);
+	cmd->command = cpu_to_le16(CMD_802_11_AD_HOC_JOIN);
 
-	padhocjoin->bssdescriptor.bsstype = cmd_bss_type_ibss;
+	join_cmd->bss.type = CMD_BSS_TYPE_IBSS;
+	join_cmd->bss.beaconperiod = cpu_to_le16(bss->beaconperiod);
 
-	padhocjoin->bssdescriptor.beaconperiod = cpu_to_le16(bss->beaconperiod);
+	memcpy(&join_cmd->bss.bssid, &bss->bssid, ETH_ALEN);
+	memcpy(&join_cmd->bss.ssid, &bss->ssid, bss->ssid_len);
 
-	memcpy(&padhocjoin->bssdescriptor.BSSID, &bss->bssid, ETH_ALEN);
-	memcpy(&padhocjoin->bssdescriptor.SSID, &bss->ssid, bss->ssid_len);
+	memcpy(&join_cmd->bss.phyparamset, &bss->phyparamset,
+	       sizeof(union ieeetypes_phyparamset));
 
-	memcpy(&padhocjoin->bssdescriptor.phyparamset,
-	       &bss->phyparamset, sizeof(union ieeetypes_phyparamset));
-
-	memcpy(&padhocjoin->bssdescriptor.ssparamset,
-	       &bss->ssparamset, sizeof(union IEEEtypes_ssparamset));
-
-	memcpy(&tmpcap, &bss->cap, sizeof(struct ieeetypes_capinfo));
-	tmpcap &= CAPINFO_MASK;
+	memcpy(&join_cmd->bss.ssparamset, &bss->ssparamset,
+	       sizeof(union IEEEtypes_ssparamset));
 
+	join_cmd->bss.capability = cpu_to_le16(bss->capability & CAPINFO_MASK);
 	lbs_deb_join("ADHOC_J_CMD: tmpcap=%4X CAPINFO_MASK=%4X\n",
-	       tmpcap, CAPINFO_MASK);
-	memcpy(&padhocjoin->bssdescriptor.cap, &tmpcap,
-	       sizeof(struct ieeetypes_capinfo));
+	       bss->capability, CAPINFO_MASK);
 
 	/* information on BSSID descriptor passed to FW */
 	lbs_deb_join(
-	       "ADHOC_J_CMD: BSSID = " MAC_FMT ", SSID = '%s'\n",
-	       MAC_ARG(padhocjoin->bssdescriptor.BSSID),
-	       padhocjoin->bssdescriptor.SSID);
+	       "ADHOC_J_CMD: BSSID = %s, SSID = '%s'\n",
+	       print_mac(mac, join_cmd->bss.bssid),
+	       join_cmd->bss.ssid);
 
 	/* failtimeout */
-	padhocjoin->failtimeout = cpu_to_le16(MRVDRV_ASSOCIATION_TIME_OUT);
+	join_cmd->failtimeout = cpu_to_le16(MRVDRV_ASSOCIATION_TIME_OUT);
 
 	/* probedelay */
-	padhocjoin->probedelay = cpu_to_le16(cmd_scan_probe_delay_time);
-
-	/* Copy Data rates from the rates recorded in scan response */
-	memset(padhocjoin->bssdescriptor.datarates, 0,
-	       sizeof(padhocjoin->bssdescriptor.datarates));
-	memcpy(padhocjoin->bssdescriptor.datarates, bss->datarates,
-	       min(sizeof(padhocjoin->bssdescriptor.datarates),
-		   sizeof(bss->datarates)));
-
-	card_rates = libertas_supported_rates;
-	card_rates_size = sizeof(libertas_supported_rates);
+	join_cmd->probedelay = cpu_to_le16(CMD_SCAN_PROBE_DELAY_TIME);
 
 	adapter->curbssparams.channel = bss->channel;
 
-	if (get_common_rates(adapter, padhocjoin->bssdescriptor.datarates,
-			     sizeof(padhocjoin->bssdescriptor.datarates),
-			     card_rates, card_rates_size)) {
+	/* Copy Data rates from the rates recorded in scan response */
+	memset(join_cmd->bss.rates, 0, sizeof(join_cmd->bss.rates));
+	ratesize = min_t(u16, sizeof(join_cmd->bss.rates), MAX_RATES);
+	memcpy(join_cmd->bss.rates, bss->rates, ratesize);
+	if (get_common_rates(adapter, join_cmd->bss.rates, &ratesize)) {
 		lbs_deb_join("ADHOC_J_CMD: get_common_rates returns error.\n");
 		ret = -1;
 		goto done;
 	}
 
-	/* Find the last non zero */
-	for (i = 0; i < sizeof(padhocjoin->bssdescriptor.datarates)
-	     && padhocjoin->bssdescriptor.datarates[i]; i++) ;
-
-	adapter->curbssparams.numofrates = i;
+	/* Copy the ad-hoc creating rates into Current BSS state structure */
+	memset(&adapter->curbssparams.rates, 0, sizeof(adapter->curbssparams.rates));
+	memcpy(&adapter->curbssparams.rates, join_cmd->bss.rates, ratesize);
 
-	/*
-	 * Copy the adhoc joining rates to Current BSS State structure
+	/* Set MSB on basic rates as the firmware requires, but _after_
+	 * copying to current bss rates.
 	 */
-	memcpy(adapter->curbssparams.datarates,
-	       padhocjoin->bssdescriptor.datarates,
-	       adapter->curbssparams.numofrates);
+	libertas_set_basic_rate_flags(join_cmd->bss.rates, ratesize);
 
-	padhocjoin->bssdescriptor.ssparamset.ibssparamset.atimwindow =
+	join_cmd->bss.ssparamset.ibssparamset.atimwindow =
 	    cpu_to_le16(bss->atimwindow);
 
 	if (assoc_req->secinfo.wep_enabled) {
-		padhocjoin->bssdescriptor.cap.privacy = AD_HOC_CAP_PRIVACY_ON;
+		u16 tmp = le16_to_cpu(join_cmd->bss.capability);
+		tmp |= WLAN_CAPABILITY_PRIVACY;
+		join_cmd->bss.capability = cpu_to_le16(tmp);
 	}
 
-	if (adapter->psmode == wlan802_11powermodemax_psp) {
+	if (adapter->psmode == WLAN802_11POWERMODEMAX_PSP) {
 		/* wake up first */
 		__le32 Localpsmode;
 
-		Localpsmode = cpu_to_le32(wlan802_11powermodecam);
+		Localpsmode = cpu_to_le32(WLAN802_11POWERMODECAM);
 		ret = libertas_prepare_and_send_command(priv,
-					    cmd_802_11_ps_mode,
-					    cmd_act_set,
+					    CMD_802_11_PS_MODE,
+					    CMD_ACT_SET,
 					    0, 0, &Localpsmode);
 
 		if (ret) {
@@ -709,6 +700,7 @@ int libertas_ret_80211_associate(wlan_private * priv,
 	union iwreq_data wrqu;
 	struct ieeetypes_assocrsp *passocrsp;
 	struct bss_descriptor * bss;
+	u16 status_code;
 
 	lbs_deb_enter(LBS_DEB_JOIN);
 
@@ -721,21 +713,65 @@ int libertas_ret_80211_associate(wlan_private * priv,
 
 	passocrsp = (struct ieeetypes_assocrsp *) & resp->params;
 
-	if (le16_to_cpu(passocrsp->statuscode)) {
-		libertas_mac_event_disconnected(priv);
+	/*
+	 * Older FW versions map the IEEE 802.11 Status Code in the association
+	 * response to the following values returned in passocrsp->statuscode:
+	 *
+	 *    IEEE Status Code                Marvell Status Code
+	 *    0                       ->      0x0000 ASSOC_RESULT_SUCCESS
+	 *    13                      ->      0x0004 ASSOC_RESULT_AUTH_REFUSED
+	 *    14                      ->      0x0004 ASSOC_RESULT_AUTH_REFUSED
+	 *    15                      ->      0x0004 ASSOC_RESULT_AUTH_REFUSED
+	 *    16                      ->      0x0004 ASSOC_RESULT_AUTH_REFUSED
+	 *    others                  ->      0x0003 ASSOC_RESULT_REFUSED
+	 *
+	 * Other response codes:
+	 *    0x0001 -> ASSOC_RESULT_INVALID_PARAMETERS (unused)
+	 *    0x0002 -> ASSOC_RESULT_TIMEOUT (internal timer expired waiting for
+	 *                                    association response from the AP)
+	 */
 
-		lbs_deb_join("ASSOC_RESP: Association failed, status code = %d\n",
-			     le16_to_cpu(passocrsp->statuscode));
+	status_code = le16_to_cpu(passocrsp->statuscode);
+	switch (status_code) {
+	case 0x00:
+		lbs_deb_join("ASSOC_RESP: Association succeeded\n");
+		break;
+	case 0x01:
+		lbs_deb_join("ASSOC_RESP: Association failed; invalid "
+		             "parameters (status code %d)\n", status_code);
+		break;
+	case 0x02:
+		lbs_deb_join("ASSOC_RESP: Association failed; internal timer "
+		             "expired while waiting for the AP (status code %d)"
+		             "\n", status_code);
+		break;
+	case 0x03:
+		lbs_deb_join("ASSOC_RESP: Association failed; association "
+		             "was refused by the AP (status code %d)\n",
+		             status_code);
+		break;
+	case 0x04:
+		lbs_deb_join("ASSOC_RESP: Association failed; authentication "
+		             "was refused by the AP (status code %d)\n",
+		             status_code);
+		break;
+	default:
+		lbs_deb_join("ASSOC_RESP: Association failed; reason unknown "
+		             "(status code %d)\n", status_code);
+		break;
+	}
 
+	if (status_code) {
+		libertas_mac_event_disconnected(priv);
 		ret = -1;
 		goto done;
 	}
 
-	lbs_dbg_hex("ASSOC_RESP:", (void *)&resp->params,
+	lbs_deb_hex(LBS_DEB_JOIN, "ASSOC_RESP", (void *)&resp->params,
 		le16_to_cpu(resp->size) - S_DS_GEN);
 
 	/* Send a Media Connected event, according to the Spec */
-	adapter->connect_status = libertas_connected;
+	adapter->connect_status = LIBERTAS_CONNECTED;
 
 	lbs_deb_join("ASSOC_RESP: assocated to '%s'\n",
 	             escape_essid(bss->ssid, bss->ssid_len));
@@ -759,10 +795,10 @@ int libertas_ret_80211_associate(wlan_private * priv,
 	netif_carrier_on(priv->dev);
 	netif_wake_queue(priv->dev);
 
-	netif_carrier_on(priv->mesh_dev);
-	netif_wake_queue(priv->mesh_dev);
-
-	lbs_deb_join("ASSOC_RESP: Associated \n");
+	if (priv->mesh_dev) {
+		netif_carrier_on(priv->mesh_dev);
+		netif_wake_queue(priv->mesh_dev);
+	}
 
 	memcpy(wrqu.ap_addr.sa_data, adapter->curbssparams.bssid, ETH_ALEN);
 	wrqu.ap_addr.sa_family = ARPHRD_ETHER;
@@ -794,6 +830,7 @@ int libertas_ret_80211_ad_hoc_start(wlan_private * priv,
 	struct cmd_ds_802_11_ad_hoc_result *padhocresult;
 	union iwreq_data wrqu;
 	struct bss_descriptor *bss;
+	DECLARE_MAC_BUF(mac);
 
 	lbs_deb_enter(LBS_DEB_JOIN);
 
@@ -815,7 +852,7 @@ int libertas_ret_80211_ad_hoc_start(wlan_private * priv,
 	 */
 	if (result) {
 		lbs_deb_join("ADHOC_RESP: failed\n");
-		if (adapter->connect_status == libertas_connected) {
+		if (adapter->connect_status == LIBERTAS_CONNECTED) {
 			libertas_mac_event_disconnected(priv);
 		}
 		ret = -1;
@@ -830,11 +867,11 @@ int libertas_ret_80211_ad_hoc_start(wlan_private * priv,
 	             escape_essid(bss->ssid, bss->ssid_len));
 
 	/* Send a Media Connected event, according to the Spec */
-	adapter->connect_status = libertas_connected;
+	adapter->connect_status = LIBERTAS_CONNECTED;
 
-	if (command == cmd_ret_802_11_ad_hoc_start) {
+	if (command == CMD_RET(CMD_802_11_AD_HOC_START)) {
 		/* Update the created network descriptor with the new BSSID */
-		memcpy(bss->bssid, padhocresult->BSSID, ETH_ALEN);
+		memcpy(bss->bssid, padhocresult->bssid, ETH_ALEN);
 	}
 
 	/* Set the BSSID from the joined/started descriptor */
@@ -847,8 +884,10 @@ int libertas_ret_80211_ad_hoc_start(wlan_private * priv,
 	netif_carrier_on(priv->dev);
 	netif_wake_queue(priv->dev);
 
-	netif_carrier_on(priv->mesh_dev);
-	netif_wake_queue(priv->mesh_dev);
+	if (priv->mesh_dev) {
+		netif_carrier_on(priv->mesh_dev);
+		netif_wake_queue(priv->mesh_dev);
+	}
 
 	memset(&wrqu, 0, sizeof(wrqu));
 	memcpy(wrqu.ap_addr.sa_data, adapter->curbssparams.bssid, ETH_ALEN);
@@ -857,8 +896,8 @@ int libertas_ret_80211_ad_hoc_start(wlan_private * priv,
 
 	lbs_deb_join("ADHOC_RESP: - Joined/Started Ad Hoc\n");
 	lbs_deb_join("ADHOC_RESP: channel = %d\n", adapter->curbssparams.channel);
-	lbs_deb_join("ADHOC_RESP: BSSID = " MAC_FMT "\n",
-	       MAC_ARG(padhocresult->BSSID));
+	lbs_deb_join("ADHOC_RESP: BSSID = %s\n",
+		     print_mac(mac, padhocresult->bssid));
 
 done:
 	lbs_deb_leave_args(LBS_DEB_JOIN, "ret %d", ret);
diff --git a/drivers/net/wireless/libertas/join.h b/drivers/net/wireless/libertas/join.h
index d522630ff8cf..894a072b9f8d 100644
--- a/drivers/net/wireless/libertas/join.h
+++ b/drivers/net/wireless/libertas/join.h
@@ -12,45 +12,42 @@
 #include "dev.h"
 
 struct cmd_ds_command;
-extern int libertas_cmd_80211_authenticate(wlan_private * priv,
+int libertas_cmd_80211_authenticate(wlan_private * priv,
 					struct cmd_ds_command *cmd,
 					void *pdata_buf);
-extern int libertas_cmd_80211_ad_hoc_join(wlan_private * priv,
+int libertas_cmd_80211_ad_hoc_join(wlan_private * priv,
 				       struct cmd_ds_command *cmd,
 				       void *pdata_buf);
-extern int libertas_cmd_80211_ad_hoc_stop(wlan_private * priv,
+int libertas_cmd_80211_ad_hoc_stop(wlan_private * priv,
 				       struct cmd_ds_command *cmd);
-extern int libertas_cmd_80211_ad_hoc_start(wlan_private * priv,
+int libertas_cmd_80211_ad_hoc_start(wlan_private * priv,
 					struct cmd_ds_command *cmd,
 					void *pdata_buf);
-extern int libertas_cmd_80211_deauthenticate(wlan_private * priv,
+int libertas_cmd_80211_deauthenticate(wlan_private * priv,
 					  struct cmd_ds_command *cmd);
-extern int libertas_cmd_80211_associate(wlan_private * priv,
+int libertas_cmd_80211_associate(wlan_private * priv,
 				     struct cmd_ds_command *cmd,
 				     void *pdata_buf);
 
-extern int libertas_ret_80211_ad_hoc_start(wlan_private * priv,
+int libertas_ret_80211_ad_hoc_start(wlan_private * priv,
 					struct cmd_ds_command *resp);
-extern int libertas_ret_80211_ad_hoc_stop(wlan_private * priv,
+int libertas_ret_80211_ad_hoc_stop(wlan_private * priv,
 				       struct cmd_ds_command *resp);
-extern int libertas_ret_80211_disassociate(wlan_private * priv,
+int libertas_ret_80211_disassociate(wlan_private * priv,
 					struct cmd_ds_command *resp);
-extern int libertas_ret_80211_associate(wlan_private * priv,
+int libertas_ret_80211_associate(wlan_private * priv,
 				     struct cmd_ds_command *resp);
 
-extern int libertas_reassociation_thread(void *data);
-
-extern int libertas_start_adhoc_network(wlan_private * priv,
+int libertas_start_adhoc_network(wlan_private * priv,
 			     struct assoc_request * assoc_req);
-extern int libertas_join_adhoc_network(wlan_private * priv,
+int libertas_join_adhoc_network(wlan_private * priv,
 				struct assoc_request * assoc_req);
-extern int libertas_stop_adhoc_network(wlan_private * priv);
-
-extern int libertas_send_deauthentication(wlan_private * priv);
-extern int libertas_send_deauth(wlan_private * priv);
+int libertas_stop_adhoc_network(wlan_private * priv);
 
-extern int libertas_do_adhocstop_ioctl(wlan_private * priv);
+int libertas_send_deauthentication(wlan_private * priv);
 
 int wlan_associate(wlan_private * priv, struct assoc_request * assoc_req);
 
+void libertas_unset_basic_rate_flags(u8 * rates, size_t len);
+
 #endif
diff --git a/drivers/net/wireless/libertas/main.c b/drivers/net/wireless/libertas/main.c
index 9f366242c392..5ead08312e1e 100644
--- a/drivers/net/wireless/libertas/main.c
+++ b/drivers/net/wireless/libertas/main.c
@@ -10,6 +10,7 @@
 #include <linux/etherdevice.h>
 #include <linux/netdevice.h>
 #include <linux/if_arp.h>
+#include <linux/kthread.h>
 
 #include <net/iw_handler.h>
 #include <net/ieee80211.h>
@@ -20,8 +21,9 @@
 #include "wext.h"
 #include "debugfs.h"
 #include "assoc.h"
+#include "join.h"
 
-#define DRIVER_RELEASE_VERSION "322.p0"
+#define DRIVER_RELEASE_VERSION "323.p0"
 const char libertas_driver_version[] = "COMM-USB8388-" DRIVER_RELEASE_VERSION
 #ifdef  DEBUG
     "-dbg"
@@ -121,57 +123,88 @@ struct region_cfp_table {
 static struct region_cfp_table region_cfp_table[] = {
 	{0x10,			/*US FCC */
 	 channel_freq_power_US_BG,
-	 sizeof(channel_freq_power_US_BG) / sizeof(struct chan_freq_power),
+	 ARRAY_SIZE(channel_freq_power_US_BG),
 	 }
 	,
 	{0x20,			/*CANADA IC */
 	 channel_freq_power_US_BG,
-	 sizeof(channel_freq_power_US_BG) / sizeof(struct chan_freq_power),
+	 ARRAY_SIZE(channel_freq_power_US_BG),
 	 }
 	,
 	{0x30, /*EU*/ channel_freq_power_EU_BG,
-	 sizeof(channel_freq_power_EU_BG) / sizeof(struct chan_freq_power),
+	 ARRAY_SIZE(channel_freq_power_EU_BG),
 	 }
 	,
 	{0x31, /*SPAIN*/ channel_freq_power_SPN_BG,
-	 sizeof(channel_freq_power_SPN_BG) / sizeof(struct chan_freq_power),
+	 ARRAY_SIZE(channel_freq_power_SPN_BG),
 	 }
 	,
 	{0x32, /*FRANCE*/ channel_freq_power_FR_BG,
-	 sizeof(channel_freq_power_FR_BG) / sizeof(struct chan_freq_power),
+	 ARRAY_SIZE(channel_freq_power_FR_BG),
 	 }
 	,
 	{0x40, /*JAPAN*/ channel_freq_power_JPN_BG,
-	 sizeof(channel_freq_power_JPN_BG) / sizeof(struct chan_freq_power),
+	 ARRAY_SIZE(channel_freq_power_JPN_BG),
 	 }
 	,
 /*Add new region here */
 };
 
 /**
- * the rates supported
+ * the table to keep region code
  */
-u8 libertas_supported_rates[G_SUPPORTED_RATES] =
-    { 0x82, 0x84, 0x8b, 0x96, 0x0c, 0x12, 0x18, 0x24, 0x30, 0x48, 0x60, 0x6c,
-0 };
+u16 libertas_region_code_to_index[MRVDRV_MAX_REGION_CODE] =
+    { 0x10, 0x20, 0x30, 0x31, 0x32, 0x40 };
 
 /**
- * the rates supported for ad-hoc G mode
+ * 802.11b/g supported bitrates (in 500Kb/s units)
  */
-u8 libertas_adhoc_rates_g[G_SUPPORTED_RATES] =
-    { 0x82, 0x84, 0x8b, 0x96, 0x0c, 0x12, 0x18, 0x24, 0x30, 0x48, 0x60, 0x6c,
-0 };
+u8 libertas_bg_rates[MAX_RATES] =
+    { 0x02, 0x04, 0x0b, 0x16, 0x0c, 0x12, 0x18, 0x24, 0x30, 0x48, 0x60, 0x6c,
+0x00, 0x00 };
 
 /**
- * the rates supported for ad-hoc B mode
+ * FW rate table.  FW refers to rates by their index in this table, not by the
+ * rate value itself.  Values of 0x00 are
+ * reserved positions.
  */
-u8 libertas_adhoc_rates_b[4] = { 0x82, 0x84, 0x8b, 0x96 };
+static u8 fw_data_rates[MAX_RATES] =
+    { 0x02, 0x04, 0x0B, 0x16, 0x00, 0x0C, 0x12,
+      0x18, 0x24, 0x30, 0x48, 0x60, 0x6C, 0x00
+};
 
 /**
- * the table to keep region code
+ *  @brief use index to get the data rate
+ *
+ *  @param idx                The index of data rate
+ *  @return 	   		data rate or 0
  */
-u16 libertas_region_code_to_index[MRVDRV_MAX_REGION_CODE] =
-    { 0x10, 0x20, 0x30, 0x31, 0x32, 0x40 };
+u32 libertas_fw_index_to_data_rate(u8 idx)
+{
+	if (idx >= sizeof(fw_data_rates))
+		idx = 0;
+	return fw_data_rates[idx];
+}
+
+/**
+ *  @brief use rate to get the index
+ *
+ *  @param rate                 data rate
+ *  @return 	   		index or 0
+ */
+u8 libertas_data_rate_to_fw_index(u32 rate)
+{
+	u8 i;
+
+	if (!rate)
+		return 0;
+
+	for (i = 0; i < sizeof(fw_data_rates); i++) {
+		if (rate == fw_data_rates[i])
+			return i;
+	}
+	return 0;
+}
 
 /**
  * Attributes exported through sysfs
@@ -187,9 +220,9 @@ static ssize_t libertas_anycast_get(struct device * dev,
 
 	memset(&mesh_access, 0, sizeof(mesh_access));
 	libertas_prepare_and_send_command(to_net_dev(dev)->priv,
-			cmd_mesh_access,
-			cmd_act_mesh_get_anycast,
-			cmd_option_waitforrsp, 0, (void *)&mesh_access);
+			CMD_MESH_ACCESS,
+			CMD_ACT_MESH_GET_ANYCAST,
+			CMD_OPTION_WAITFORRSP, 0, (void *)&mesh_access);
 
 	return snprintf(buf, 12, "0x%X\n", le32_to_cpu(mesh_access.data[0]));
 }
@@ -208,18 +241,127 @@ static ssize_t libertas_anycast_set(struct device * dev,
 	mesh_access.data[0] = cpu_to_le32(datum);
 
 	libertas_prepare_and_send_command((to_net_dev(dev))->priv,
-			cmd_mesh_access,
-			cmd_act_mesh_set_anycast,
-			cmd_option_waitforrsp, 0, (void *)&mesh_access);
+			CMD_MESH_ACCESS,
+			CMD_ACT_MESH_SET_ANYCAST,
+			CMD_OPTION_WAITFORRSP, 0, (void *)&mesh_access);
+	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 *) (to_net_dev(dev))->priv;
+	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 *) (to_net_dev(dev))->priv;
+	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)
  */
 static DEVICE_ATTR(anycast_mask, 0644, libertas_anycast_get, libertas_anycast_set);
 
+static ssize_t libertas_autostart_enabled_get(struct device * dev,
+		struct device_attribute *attr, char * buf)
+{
+	struct cmd_ds_mesh_access mesh_access;
+
+	memset(&mesh_access, 0, sizeof(mesh_access));
+	libertas_prepare_and_send_command(to_net_dev(dev)->priv,
+			CMD_MESH_ACCESS,
+			CMD_ACT_MESH_GET_AUTOSTART_ENABLED,
+			CMD_OPTION_WAITFORRSP, 0, (void *)&mesh_access);
+
+	return sprintf(buf, "%d\n", le32_to_cpu(mesh_access.data[0]));
+}
+
+static ssize_t libertas_autostart_enabled_set(struct device * dev,
+		struct device_attribute *attr, const char * buf, size_t count)
+{
+	struct cmd_ds_mesh_access mesh_access;
+	uint32_t datum;
+	wlan_private * priv = (to_net_dev(dev))->priv;
+	int ret;
+
+	memset(&mesh_access, 0, sizeof(mesh_access));
+	sscanf(buf, "%d", &datum);
+	mesh_access.data[0] = cpu_to_le32(datum);
+
+	ret = libertas_prepare_and_send_command(priv,
+			CMD_MESH_ACCESS,
+			CMD_ACT_MESH_SET_AUTOSTART_ENABLED,
+			CMD_OPTION_WAITFORRSP, 0, (void *)&mesh_access);
+	if (ret == 0)
+		priv->mesh_autostart_enabled = datum ? 1 : 0;
+
+	return strlen(buf);
+}
+
+static DEVICE_ATTR(autostart_enabled, 0644,
+		libertas_autostart_enabled_get, libertas_autostart_enabled_set);
+
+static struct attribute *libertas_mesh_sysfs_entries[] = {
+	&dev_attr_anycast_mask.attr,
+	&dev_attr_autostart_enabled.attr,
+	NULL,
+};
+
+static struct attribute_group libertas_mesh_attr_group = {
+	.attrs = libertas_mesh_sysfs_entries,
+};
+
 /**
  *  @brief Check if the device can be open and wait if necessary.
  *
@@ -255,7 +397,7 @@ static int pre_open_check(struct net_device *dev)
  *  @param dev     A pointer to net_device structure
  *  @return 	   0
  */
-static int wlan_dev_open(struct net_device *dev)
+static int libertas_dev_open(struct net_device *dev)
 {
 	wlan_private *priv = (wlan_private *) dev->priv;
 	wlan_adapter *adapter = priv->adapter;
@@ -264,12 +406,14 @@ static int wlan_dev_open(struct net_device *dev)
 
 	priv->open = 1;
 
-	if (adapter->connect_status == libertas_connected) {
+	if (adapter->connect_status == LIBERTAS_CONNECTED) {
 		netif_carrier_on(priv->dev);
-		netif_carrier_on(priv->mesh_dev);
+		if (priv->mesh_dev)
+			netif_carrier_on(priv->mesh_dev);
 	} else {
 		netif_carrier_off(priv->dev);
-		netif_carrier_off(priv->mesh_dev);
+		if (priv->mesh_dev)
+			netif_carrier_off(priv->mesh_dev);
 	}
 
 	lbs_deb_leave(LBS_DEB_NET);
@@ -281,7 +425,7 @@ static int wlan_dev_open(struct net_device *dev)
  *  @param dev     A pointer to net_device structure
  *  @return 	   0
  */
-static int mesh_open(struct net_device *dev)
+static int libertas_mesh_open(struct net_device *dev)
 {
 	wlan_private *priv = (wlan_private *) dev->priv ;
 
@@ -290,7 +434,7 @@ static int mesh_open(struct net_device *dev)
 	priv->mesh_open = 1 ;
 	netif_wake_queue(priv->mesh_dev);
 	if (priv->infra_open == 0)
-		return wlan_dev_open(priv->dev) ;
+		return libertas_dev_open(priv->dev) ;
 	return 0;
 }
 
@@ -300,7 +444,7 @@ static int mesh_open(struct net_device *dev)
  *  @param dev     A pointer to net_device structure
  *  @return 	   0
  */
-static int wlan_open(struct net_device *dev)
+static int libertas_open(struct net_device *dev)
 {
 	wlan_private *priv = (wlan_private *) dev->priv ;
 
@@ -309,11 +453,11 @@ static int wlan_open(struct net_device *dev)
 	priv->infra_open = 1 ;
 	netif_wake_queue(priv->dev);
 	if (priv->open == 0)
-		return wlan_dev_open(priv->dev) ;
+		return libertas_dev_open(priv->dev) ;
 	return 0;
 }
 
-static int wlan_dev_close(struct net_device *dev)
+static int libertas_dev_close(struct net_device *dev)
 {
 	wlan_private *priv = dev->priv;
 
@@ -332,14 +476,14 @@ static int wlan_dev_close(struct net_device *dev)
  *  @param dev     A pointer to net_device structure
  *  @return 	   0
  */
-static int mesh_close(struct net_device *dev)
+static int libertas_mesh_close(struct net_device *dev)
 {
 	wlan_private *priv = (wlan_private *) (dev->priv);
 
 	priv->mesh_open = 0;
 	netif_stop_queue(priv->mesh_dev);
 	if (priv->infra_open == 0)
-		return wlan_dev_close(dev);
+		return libertas_dev_close(dev);
 	else
 		return 0;
 }
@@ -350,20 +494,20 @@ static int mesh_close(struct net_device *dev)
  *  @param dev     A pointer to net_device structure
  *  @return 	   0
  */
-static int wlan_close(struct net_device *dev)
+static int libertas_close(struct net_device *dev)
 {
 	wlan_private *priv = (wlan_private *) dev->priv;
 
 	netif_stop_queue(dev);
 	priv->infra_open = 0;
 	if (priv->mesh_open == 0)
-		return wlan_dev_close(dev);
+		return libertas_dev_close(dev);
 	else
 		return 0;
 }
 
 
-static int wlan_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
+static int libertas_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
 {
 	int ret = 0;
 	wlan_private *priv = dev->priv;
@@ -376,7 +520,8 @@ static int wlan_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
 	}
 
 	netif_stop_queue(priv->dev);
-	netif_stop_queue(priv->mesh_dev);
+	if (priv->mesh_dev)
+		netif_stop_queue(priv->mesh_dev);
 
 	if (libertas_process_tx(priv, skb) == 0)
 		dev->trans_start = jiffies;
@@ -386,41 +531,52 @@ done:
 }
 
 /**
- * @brief Mark mesh packets and handover them to wlan_hard_start_xmit
+ * @brief Mark mesh packets and handover them to libertas_hard_start_xmit
  *
  */
-static int mesh_pre_start_xmit(struct sk_buff *skb, struct net_device *dev)
+static int libertas_mesh_pre_start_xmit(struct sk_buff *skb,
+		struct net_device *dev)
 {
 	wlan_private *priv = dev->priv;
 	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);
 
-	ret = wlan_hard_start_xmit(skb, priv->dev);
+	ret = libertas_hard_start_xmit(skb, priv->dev);
 	lbs_deb_leave_args(LBS_DEB_MESH, "ret %d", ret);
 	return ret;
 }
 
 /**
- * @brief Mark non-mesh packets and handover them to wlan_hard_start_xmit
+ * @brief Mark non-mesh packets and handover them to libertas_hard_start_xmit
  *
  */
-static int wlan_pre_start_xmit(struct sk_buff *skb, struct net_device *dev)
+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 = wlan_hard_start_xmit(skb, dev);
+	ret = libertas_hard_start_xmit(skb, dev);
 	lbs_deb_leave_args(LBS_DEB_NET, "ret %d", ret);
 	return ret;
 }
 
-static void wlan_tx_timeout(struct net_device *dev)
+static void libertas_tx_timeout(struct net_device *dev)
 {
 	wlan_private *priv = (wlan_private *) dev->priv;
 
@@ -432,16 +588,17 @@ static void wlan_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;
 			libertas_send_tx_feedback(priv);
 		} else
-			wake_up_interruptible(&priv->mainthread.waitq);
-	} else if (priv->adapter->connect_status == libertas_connected) {
+			wake_up_interruptible(&priv->waitq);
+	} else if (priv->adapter->connect_status == LIBERTAS_CONNECTED) {
 		netif_wake_queue(priv->dev);
-		netif_wake_queue(priv->mesh_dev);
+		if (priv->mesh_dev)
+			netif_wake_queue(priv->mesh_dev);
 	}
 
 	lbs_deb_leave(LBS_DEB_TX);
@@ -453,14 +610,14 @@ static void wlan_tx_timeout(struct net_device *dev)
  *  @param dev     A pointer to wlan_private structure
  *  @return 	   A pointer to net_device_stats structure
  */
-static struct net_device_stats *wlan_get_stats(struct net_device *dev)
+static struct net_device_stats *libertas_get_stats(struct net_device *dev)
 {
 	wlan_private *priv = (wlan_private *) dev->priv;
 
 	return &priv->stats;
 }
 
-static int wlan_set_mac_address(struct net_device *dev, void *addr)
+static int libertas_set_mac_address(struct net_device *dev, void *addr)
 {
 	int ret = 0;
 	wlan_private *priv = (wlan_private *) dev->priv;
@@ -475,14 +632,14 @@ static int wlan_set_mac_address(struct net_device *dev, void *addr)
 	memset(adapter->current_addr, 0, ETH_ALEN);
 
 	/* dev->dev_addr is 8 bytes */
-	lbs_dbg_hex("dev->dev_addr:", dev->dev_addr, ETH_ALEN);
+	lbs_deb_hex(LBS_DEB_NET, "dev->dev_addr", dev->dev_addr, ETH_ALEN);
 
-	lbs_dbg_hex("addr:", phwaddr->sa_data, ETH_ALEN);
+	lbs_deb_hex(LBS_DEB_NET, "addr", phwaddr->sa_data, ETH_ALEN);
 	memcpy(adapter->current_addr, phwaddr->sa_data, ETH_ALEN);
 
-	ret = libertas_prepare_and_send_command(priv, cmd_802_11_mac_address,
-				    cmd_act_set,
-				    cmd_option_waitforrsp, 0, NULL);
+	ret = libertas_prepare_and_send_command(priv, CMD_802_11_MAC_ADDRESS,
+				    CMD_ACT_SET,
+				    CMD_OPTION_WAITFORRSP, 0, NULL);
 
 	if (ret) {
 		lbs_deb_net("set MAC address failed\n");
@@ -490,7 +647,7 @@ static int wlan_set_mac_address(struct net_device *dev, void *addr)
 		goto done;
 	}
 
-	lbs_dbg_hex("adapter->macaddr:", adapter->current_addr, ETH_ALEN);
+	lbs_deb_hex(LBS_DEB_NET, "adapter->macaddr", adapter->current_addr, ETH_ALEN);
 	memcpy(dev->dev_addr, adapter->current_addr, ETH_ALEN);
 	if (priv->mesh_dev)
 		memcpy(priv->mesh_dev->dev_addr, adapter->current_addr, ETH_ALEN);
@@ -500,7 +657,7 @@ done:
 	return ret;
 }
 
-static int wlan_copy_multicast_address(wlan_adapter * adapter,
+static int libertas_copy_multicast_address(wlan_adapter * adapter,
 				     struct net_device *dev)
 {
 	int i = 0;
@@ -515,11 +672,12 @@ static int wlan_copy_multicast_address(wlan_adapter * adapter,
 
 }
 
-static void wlan_set_multicast_list(struct net_device *dev)
+static void libertas_set_multicast_list(struct net_device *dev)
 {
 	wlan_private *priv = dev->priv;
 	wlan_adapter *adapter = priv->adapter;
 	int oldpacketfilter;
+	DECLARE_MAC_BUF(mac);
 
 	lbs_deb_enter(LBS_DEB_NET);
 
@@ -528,57 +686,52 @@ static void wlan_set_multicast_list(struct net_device *dev)
 	if (dev->flags & IFF_PROMISC) {
 		lbs_deb_net("enable promiscuous mode\n");
 		adapter->currentpacketfilter |=
-		    cmd_act_mac_promiscuous_enable;
+		    CMD_ACT_MAC_PROMISCUOUS_ENABLE;
 		adapter->currentpacketfilter &=
-		    ~(cmd_act_mac_all_multicast_enable |
-		      cmd_act_mac_multicast_enable);
+		    ~(CMD_ACT_MAC_ALL_MULTICAST_ENABLE |
+		      CMD_ACT_MAC_MULTICAST_ENABLE);
 	} else {
 		/* Multicast */
 		adapter->currentpacketfilter &=
-		    ~cmd_act_mac_promiscuous_enable;
+		    ~CMD_ACT_MAC_PROMISCUOUS_ENABLE;
 
 		if (dev->flags & IFF_ALLMULTI || dev->mc_count >
 		    MRVDRV_MAX_MULTICAST_LIST_SIZE) {
 			lbs_deb_net( "enabling all multicast\n");
 			adapter->currentpacketfilter |=
-			    cmd_act_mac_all_multicast_enable;
+			    CMD_ACT_MAC_ALL_MULTICAST_ENABLE;
 			adapter->currentpacketfilter &=
-			    ~cmd_act_mac_multicast_enable;
+			    ~CMD_ACT_MAC_MULTICAST_ENABLE;
 		} else {
 			adapter->currentpacketfilter &=
-			    ~cmd_act_mac_all_multicast_enable;
+			    ~CMD_ACT_MAC_ALL_MULTICAST_ENABLE;
 
 			if (!dev->mc_count) {
 				lbs_deb_net("no multicast addresses, "
 				       "disabling multicast\n");
 				adapter->currentpacketfilter &=
-				    ~cmd_act_mac_multicast_enable;
+				    ~CMD_ACT_MAC_MULTICAST_ENABLE;
 			} else {
 				int i;
 
 				adapter->currentpacketfilter |=
-				    cmd_act_mac_multicast_enable;
+				    CMD_ACT_MAC_MULTICAST_ENABLE;
 
 				adapter->nr_of_multicastmacaddr =
-				    wlan_copy_multicast_address(adapter, dev);
+				    libertas_copy_multicast_address(adapter, dev);
 
 				lbs_deb_net("multicast addresses: %d\n",
 				       dev->mc_count);
 
 				for (i = 0; i < dev->mc_count; i++) {
-					lbs_deb_net("Multicast address %d:"
-					       MAC_FMT "\n", i,
-					       adapter->multicastlist[i][0],
-					       adapter->multicastlist[i][1],
-					       adapter->multicastlist[i][2],
-					       adapter->multicastlist[i][3],
-					       adapter->multicastlist[i][4],
-					       adapter->multicastlist[i][5]);
+					lbs_deb_net("Multicast address %d:%s\n",
+					       i, print_mac(mac,
+					       adapter->multicastlist[i]));
 				}
 				/* send multicast addresses to firmware */
 				libertas_prepare_and_send_command(priv,
-						      cmd_mac_multicast_adr,
-						      cmd_act_set, 0, 0,
+						      CMD_MAC_MULTICAST_ADR,
+						      CMD_ACT_SET, 0, 0,
 						      NULL);
 			}
 		}
@@ -599,18 +752,16 @@ static void wlan_set_multicast_list(struct net_device *dev)
  *  @param data    A pointer to wlan_thread structure
  *  @return 	   0
  */
-static int wlan_service_main_thread(void *data)
+static int libertas_thread(void *data)
 {
-	struct wlan_thread *thread = data;
-	wlan_private *priv = thread->priv;
+	struct net_device *dev = data;
+	wlan_private *priv = dev->priv;
 	wlan_adapter *adapter = priv->adapter;
 	wait_queue_t wait;
 	u8 ireg = 0;
 
 	lbs_deb_enter(LBS_DEB_THREAD);
 
-	wlan_activate_thread(thread);
-
 	init_waitqueue_entry(&wait, current);
 
 	set_freezable();
@@ -620,7 +771,7 @@ static int wlan_service_main_thread(void *data)
 		       adapter->intcounter,
 		       adapter->currenttxskb, priv->dnld_sent);
 
-		add_wait_queue(&thread->waitq, &wait);
+		add_wait_queue(&priv->waitq, &wait);
 		set_current_state(TASK_INTERRUPTIBLE);
 		spin_lock_irq(&adapter->driver_lock);
 		if ((adapter->psstate == PS_STATE_SLEEP) ||
@@ -636,14 +787,13 @@ static int wlan_service_main_thread(void *data)
 		} else
 			spin_unlock_irq(&adapter->driver_lock);
 
-
 		lbs_deb_thread(
 		       "main-thread 222 (waking up): intcounter=%d currenttxskb=%p "
 		       "dnld_sent=%d\n", adapter->intcounter,
 		       adapter->currenttxskb, priv->dnld_sent);
 
 		set_current_state(TASK_RUNNING);
-		remove_wait_queue(&thread->waitq, &wait);
+		remove_wait_queue(&priv->waitq, &wait);
 		try_to_freeze();
 
 		lbs_deb_thread("main-thread 333: intcounter=%d currenttxskb=%p "
@@ -681,20 +831,20 @@ static int wlan_service_main_thread(void *data)
 		       adapter->currenttxskb, priv->dnld_sent);
 
 		/* command response? */
-		if (adapter->hisregcpy & his_cmdupldrdy) {
+		if (adapter->hisregcpy & MRVDRV_CMD_UPLD_RDY) {
 			lbs_deb_thread("main-thread: cmd response ready\n");
 
-			adapter->hisregcpy &= ~his_cmdupldrdy;
+			adapter->hisregcpy &= ~MRVDRV_CMD_UPLD_RDY;
 			spin_unlock_irq(&adapter->driver_lock);
 			libertas_process_rx_command(priv);
 			spin_lock_irq(&adapter->driver_lock);
 		}
 
 		/* Any Card Event */
-		if (adapter->hisregcpy & his_cardevent) {
+		if (adapter->hisregcpy & MRVDRV_CARDEVENT) {
 			lbs_deb_thread("main-thread: Card Event Activity\n");
 
-			adapter->hisregcpy &= ~his_cardevent;
+			adapter->hisregcpy &= ~MRVDRV_CARDEVENT;
 
 			if (priv->hw_read_event_cause(priv)) {
 				lbs_pr_alert(
@@ -711,7 +861,7 @@ static int wlan_service_main_thread(void *data)
 		if (adapter->psstate == PS_STATE_PRE_SLEEP) {
 			if (!priv->dnld_sent && !adapter->cur_cmd) {
 				if (adapter->connect_status ==
-				    libertas_connected) {
+				    LIBERTAS_CONNECTED) {
 					lbs_deb_thread(
 					       "main_thread: PRE_SLEEP--intcounter=%d currenttxskb=%p "
 					       "dnld_sent=%d cur_cmd=%p, confirm now\n",
@@ -758,13 +908,214 @@ static int wlan_service_main_thread(void *data)
 	del_timer(&adapter->command_timer);
 	adapter->nr_cmd_pending = 0;
 	wake_up_all(&adapter->cmd_pending);
-	wlan_deactivate_thread(thread);
 
 	lbs_deb_leave(LBS_DEB_THREAD);
 	return 0;
 }
 
 /**
+ *  @brief This function downloads firmware image, gets
+ *  HW spec from firmware and set basic parameters to
+ *  firmware.
+ *
+ *  @param priv    A pointer to wlan_private structure
+ *  @return 	   0 or -1
+ */
+static int wlan_setup_firmware(wlan_private * priv)
+{
+	int ret = -1;
+	wlan_adapter *adapter = priv->adapter;
+	struct cmd_ds_mesh_access mesh_access;
+
+	lbs_deb_enter(LBS_DEB_FW);
+
+	/*
+	 * Read MAC address from HW
+	 */
+	memset(adapter->current_addr, 0xff, ETH_ALEN);
+
+	ret = libertas_prepare_and_send_command(priv, CMD_GET_HW_SPEC,
+				    0, CMD_OPTION_WAITFORRSP, 0, NULL);
+
+	if (ret) {
+		ret = -1;
+		goto done;
+	}
+
+	libertas_set_mac_packet_filter(priv);
+
+	/* Get the supported Data rates */
+	ret = libertas_prepare_and_send_command(priv, CMD_802_11_DATA_RATE,
+				    CMD_ACT_GET_TX_RATE,
+				    CMD_OPTION_WAITFORRSP, 0, NULL);
+
+	if (ret) {
+		ret = -1;
+		goto done;
+	}
+
+	/* Disable mesh autostart */
+	if (priv->mesh_dev) {
+		memset(&mesh_access, 0, sizeof(mesh_access));
+		mesh_access.data[0] = cpu_to_le32(0);
+		ret = libertas_prepare_and_send_command(priv,
+				CMD_MESH_ACCESS,
+				CMD_ACT_MESH_SET_AUTOSTART_ENABLED,
+				CMD_OPTION_WAITFORRSP, 0, (void *)&mesh_access);
+		if (ret) {
+			ret = -1;
+			goto done;
+		}
+		priv->mesh_autostart_enabled = 0;
+	}
+
+       /* Set the boot2 version in firmware */
+       ret = libertas_prepare_and_send_command(priv, CMD_SET_BOOT2_VER,
+                                   0, CMD_OPTION_WAITFORRSP, 0, NULL);
+
+	ret = 0;
+done:
+	lbs_deb_leave_args(LBS_DEB_FW, "ret %d", ret);
+	return ret;
+}
+
+/**
+ *  This function handles the timeout of command sending.
+ *  It will re-send the same command again.
+ */
+static void command_timer_fn(unsigned long data)
+{
+	wlan_private *priv = (wlan_private *)data;
+	wlan_adapter *adapter = priv->adapter;
+	struct cmd_ctrl_node *ptempnode;
+	struct cmd_ds_command *cmd;
+	unsigned long flags;
+
+	ptempnode = adapter->cur_cmd;
+	if (ptempnode == NULL) {
+		lbs_deb_fw("ptempnode empty\n");
+		return;
+	}
+
+	cmd = (struct cmd_ds_command *)ptempnode->bufvirtualaddr;
+	if (!cmd) {
+		lbs_deb_fw("cmd is NULL\n");
+		return;
+	}
+
+	lbs_deb_fw("command_timer_fn fired, cmd %x\n", cmd->command);
+
+	if (!adapter->fw_ready)
+		return;
+
+	spin_lock_irqsave(&adapter->driver_lock, flags);
+	adapter->cur_cmd = NULL;
+	spin_unlock_irqrestore(&adapter->driver_lock, flags);
+
+	lbs_deb_fw("re-sending same command because of timeout\n");
+	libertas_queue_cmd(adapter, ptempnode, 0);
+
+	wake_up_interruptible(&priv->waitq);
+
+	return;
+}
+
+static int libertas_init_adapter(wlan_private * priv)
+{
+	wlan_adapter *adapter = priv->adapter;
+	size_t bufsize;
+	int i, ret = 0;
+
+	/* Allocate buffer to store the BSSID list */
+	bufsize = MAX_NETWORK_COUNT * sizeof(struct bss_descriptor);
+	adapter->networks = kzalloc(bufsize, GFP_KERNEL);
+	if (!adapter->networks) {
+		lbs_pr_err("Out of memory allocating beacons\n");
+		ret = -1;
+		goto out;
+	}
+
+	/* Initialize scan result lists */
+	INIT_LIST_HEAD(&adapter->network_free_list);
+	INIT_LIST_HEAD(&adapter->network_list);
+	for (i = 0; i < MAX_NETWORK_COUNT; i++) {
+		list_add_tail(&adapter->networks[i].list,
+			      &adapter->network_free_list);
+	}
+
+	adapter->libertas_ps_confirm_sleep.seqnum = cpu_to_le16(++adapter->seqnum);
+	adapter->libertas_ps_confirm_sleep.command =
+	    cpu_to_le16(CMD_802_11_PS_MODE);
+	adapter->libertas_ps_confirm_sleep.size =
+	    cpu_to_le16(sizeof(struct PS_CMD_ConfirmSleep));
+	adapter->libertas_ps_confirm_sleep.action =
+	    cpu_to_le16(CMD_SUBCMD_SLEEP_CONFIRMED);
+
+	memset(adapter->current_addr, 0xff, ETH_ALEN);
+
+	adapter->connect_status = LIBERTAS_DISCONNECTED;
+	adapter->secinfo.auth_mode = IW_AUTH_ALG_OPEN_SYSTEM;
+	adapter->mode = IW_MODE_INFRA;
+	adapter->curbssparams.channel = DEFAULT_AD_HOC_CHANNEL;
+	adapter->currentpacketfilter = CMD_ACT_MAC_RX_ON | CMD_ACT_MAC_TX_ON;
+	adapter->radioon = RADIO_ON;
+	adapter->auto_rate = 1;
+	adapter->capability = WLAN_CAPABILITY_SHORT_PREAMBLE;
+	adapter->psmode = WLAN802_11POWERMODECAM;
+	adapter->psstate = PS_STATE_FULL_POWER;
+
+	mutex_init(&adapter->lock);
+
+	memset(&adapter->tx_queue_ps, 0, NR_TX_QUEUE*sizeof(struct sk_buff*));
+	adapter->tx_queue_idx = 0;
+	spin_lock_init(&adapter->txqueue_lock);
+
+	setup_timer(&adapter->command_timer, command_timer_fn,
+	            (unsigned long)priv);
+
+	INIT_LIST_HEAD(&adapter->cmdfreeq);
+	INIT_LIST_HEAD(&adapter->cmdpendingq);
+
+	spin_lock_init(&adapter->driver_lock);
+	init_waitqueue_head(&adapter->cmd_pending);
+	adapter->nr_cmd_pending = 0;
+
+	/* Allocate the command buffers */
+	if (libertas_allocate_cmd_buffer(priv)) {
+		lbs_pr_err("Out of memory allocating command buffers\n");
+		ret = -1;
+	}
+
+out:
+	return ret;
+}
+
+static void libertas_free_adapter(wlan_private * priv)
+{
+	wlan_adapter *adapter = priv->adapter;
+
+	if (!adapter) {
+		lbs_deb_fw("why double free adapter?\n");
+		return;
+	}
+
+	lbs_deb_fw("free command buffer\n");
+	libertas_free_cmd_buffer(priv);
+
+	lbs_deb_fw("free command_timer\n");
+	del_timer(&adapter->command_timer);
+
+	lbs_deb_fw("free scan results table\n");
+	kfree(adapter->networks);
+	adapter->networks = NULL;
+
+	/* Free the adapter object itself */
+	lbs_deb_fw("free adapter\n");
+	kfree(adapter);
+	priv->adapter = NULL;
+}
+
+/**
  * @brief This function adds the card. it will probe the
  * card, allocate the wlan_priv and initialize the device.
  *
@@ -781,7 +1132,7 @@ wlan_private *libertas_add_card(void *card, struct device *dmdev)
 	/* Allocate an Ethernet device and register it */
 	if (!(dev = alloc_etherdev(sizeof(wlan_private)))) {
 		lbs_pr_err("init ethX device failed\n");
-		return NULL;
+		goto done;
 	}
 	priv = dev->priv;
 
@@ -791,20 +1142,24 @@ wlan_private *libertas_add_card(void *card, struct device *dmdev)
 		goto err_kzalloc;
 	}
 
+	if (libertas_init_adapter(priv)) {
+		lbs_pr_err("failed to initialize adapter structure.\n");
+		goto err_init_adapter;
+	}
+
 	priv->dev = dev;
 	priv->card = card;
 	priv->mesh_open = 0;
 	priv->infra_open = 0;
-
-	SET_MODULE_OWNER(dev);
+	priv->hotplug_device = dmdev;
 
 	/* Setup the OS Interface to our functions */
-	dev->open = wlan_open;
-	dev->hard_start_xmit = wlan_pre_start_xmit;
-	dev->stop = wlan_close;
-	dev->set_mac_address = wlan_set_mac_address;
-	dev->tx_timeout = wlan_tx_timeout;
-	dev->get_stats = wlan_get_stats;
+	dev->open = libertas_open;
+	dev->hard_start_xmit = libertas_pre_start_xmit;
+	dev->stop = libertas_close;
+	dev->set_mac_address = libertas_set_mac_address;
+	dev->tx_timeout = libertas_tx_timeout;
+	dev->get_stats = libertas_get_stats;
 	dev->watchdog_timeo = 5 * HZ;
 	dev->ethtool_ops = &libertas_ethtool_ops;
 #ifdef	WIRELESS_EXT
@@ -813,84 +1168,148 @@ wlan_private *libertas_add_card(void *card, struct device *dmdev)
 #define NETIF_F_DYNALLOC 16
 	dev->features |= NETIF_F_DYNALLOC;
 	dev->flags |= IFF_BROADCAST | IFF_MULTICAST;
-	dev->set_multicast_list = wlan_set_multicast_list;
+	dev->set_multicast_list = libertas_set_multicast_list;
 
 	SET_NETDEV_DEV(dev, dmdev);
 
-	INIT_LIST_HEAD(&priv->adapter->cmdfreeq);
-	INIT_LIST_HEAD(&priv->adapter->cmdpendingq);
+	priv->rtap_net_dev = NULL;
+	if (device_create_file(dmdev, &dev_attr_libertas_rtap))
+		goto err_init_adapter;
+
+	lbs_deb_thread("Starting main thread...\n");
+	init_waitqueue_head(&priv->waitq);
+	priv->main_thread = kthread_run(libertas_thread, dev, "libertas_main");
+	if (IS_ERR(priv->main_thread)) {
+		lbs_deb_thread("Error creating main thread.\n");
+		goto err_kthread_run;
+	}
+
+	priv->work_thread = create_singlethread_workqueue("libertas_worker");
+	INIT_DELAYED_WORK(&priv->assoc_work, libertas_association_worker);
+	INIT_DELAYED_WORK(&priv->scan_work, libertas_scan_worker);
+	INIT_WORK(&priv->sync_channel, libertas_sync_channel);
 
-	spin_lock_init(&priv->adapter->driver_lock);
-	init_waitqueue_head(&priv->adapter->cmd_pending);
-	priv->adapter->nr_cmd_pending = 0;
 	goto done;
 
+err_kthread_run:
+	device_remove_file(dmdev, &dev_attr_libertas_rtap);
+
+err_init_adapter:
+	libertas_free_adapter(priv);
+
 err_kzalloc:
 	free_netdev(dev);
 	priv = NULL;
+
 done:
 	lbs_deb_leave_args(LBS_DEB_NET, "priv %p", priv);
 	return priv;
 }
 EXPORT_SYMBOL_GPL(libertas_add_card);
 
-int libertas_activate_card(wlan_private *priv, char *fw_name)
+
+int libertas_remove_card(wlan_private *priv)
 {
+	wlan_adapter *adapter = priv->adapter;
 	struct net_device *dev = priv->dev;
-	int ret = -1;
+	union iwreq_data wrqu;
 
 	lbs_deb_enter(LBS_DEB_MAIN);
 
-	lbs_deb_thread("Starting kthread...\n");
-	priv->mainthread.priv = priv;
-	wlan_create_thread(wlan_service_main_thread,
-			   &priv->mainthread, "wlan_main_service");
+	libertas_remove_rtap(priv);
 
-	priv->assoc_thread =
-		create_singlethread_workqueue("libertas_assoc");
-	INIT_DELAYED_WORK(&priv->assoc_work, libertas_association_worker);
-	INIT_WORK(&priv->sync_channel, libertas_sync_channel);
+	dev = priv->dev;
+	device_remove_file(priv->hotplug_device, &dev_attr_libertas_rtap);
 
-	/*
-	 * Register the device. Fillup the private data structure with
-	 * relevant information from the card and request for the required
-	 * IRQ.
-	 */
-	if (priv->hw_register_dev(priv) < 0) {
-		lbs_pr_err("failed to register WLAN device\n");
-		goto err_registerdev;
-	}
+	cancel_delayed_work(&priv->scan_work);
+	cancel_delayed_work(&priv->assoc_work);
+	destroy_workqueue(priv->work_thread);
 
-	/* init FW and HW */
-	if (fw_name && libertas_init_fw(priv, fw_name)) {
-		lbs_pr_err("firmware init failed\n");
-		goto err_registerdev;
+	if (adapter->psmode == WLAN802_11POWERMODEMAX_PSP) {
+		adapter->psmode = WLAN802_11POWERMODECAM;
+		libertas_ps_wakeup(priv, CMD_OPTION_WAITFORRSP);
 	}
 
+	memset(wrqu.ap_addr.sa_data, 0xaa, ETH_ALEN);
+	wrqu.ap_addr.sa_family = ARPHRD_ETHER;
+	wireless_send_event(priv->dev, SIOCGIWAP, &wrqu, NULL);
+
+	/* Stop the thread servicing the interrupts */
+	adapter->surpriseremoved = 1;
+	kthread_stop(priv->main_thread);
+
+	libertas_free_adapter(priv);
+
+	priv->dev = NULL;
+	free_netdev(dev);
+
+	lbs_deb_leave(LBS_DEB_MAIN);
+	return 0;
+}
+EXPORT_SYMBOL_GPL(libertas_remove_card);
+
+
+int libertas_start_card(wlan_private *priv)
+{
+	struct net_device *dev = priv->dev;
+	int ret = -1;
+
+	lbs_deb_enter(LBS_DEB_MAIN);
+
+	/* poke the firmware */
+	ret = wlan_setup_firmware(priv);
+	if (ret)
+		goto done;
+
+	/* init 802.11d */
+	libertas_init_11d(priv);
+
 	if (register_netdev(dev)) {
 		lbs_pr_err("cannot register ethX device\n");
-		goto err_init_fw;
+		goto done;
 	}
 
-	lbs_pr_info("%s: Marvell WLAN 802.11 adapter\n", dev->name);
-
 	libertas_debugfs_init_one(priv, dev);
 
+	lbs_pr_info("%s: Marvell WLAN 802.11 adapter\n", dev->name);
+
 	ret = 0;
-	goto done;
 
-err_init_fw:
-	priv->hw_unregister_dev(priv);
-err_registerdev:
-	destroy_workqueue(priv->assoc_thread);
-	/* Stop the thread servicing the interrupts */
-	wake_up_interruptible(&priv->mainthread.waitq);
-	wlan_terminate_thread(&priv->mainthread);
 done:
-	lbs_deb_leave_args(LBS_DEB_NET, "ret %d", ret);
+	lbs_deb_leave_args(LBS_DEB_MAIN, "ret %d", ret);
 	return ret;
 }
-EXPORT_SYMBOL_GPL(libertas_activate_card);
+EXPORT_SYMBOL_GPL(libertas_start_card);
+
+
+int libertas_stop_card(wlan_private *priv)
+{
+	struct net_device *dev = priv->dev;
+	int ret = -1;
+	struct cmd_ctrl_node *cmdnode;
+	unsigned long flags;
+
+	lbs_deb_enter(LBS_DEB_MAIN);
+
+	netif_stop_queue(priv->dev);
+	netif_carrier_off(priv->dev);
+
+	libertas_debugfs_remove_one(priv);
+
+	/* Flush pending command nodes */
+	spin_lock_irqsave(&priv->adapter->driver_lock, flags);
+	list_for_each_entry(cmdnode, &priv->adapter->cmdpendingq, list) {
+		cmdnode->cmdwaitqwoken = 1;
+		wake_up_interruptible(&cmdnode->cmdwait_q);
+	}
+	spin_unlock_irqrestore(&priv->adapter->driver_lock, flags);
+
+	unregister_netdev(dev);
+
+	lbs_deb_leave_args(LBS_DEB_MAIN, "ret %d", ret);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(libertas_stop_card);
 
 
 /**
@@ -915,13 +1334,11 @@ int libertas_add_mesh(wlan_private *priv, struct device *dev)
 	mesh_dev->priv = priv;
 	priv->mesh_dev = mesh_dev;
 
-	SET_MODULE_OWNER(mesh_dev);
-
-	mesh_dev->open = mesh_open;
-	mesh_dev->hard_start_xmit = mesh_pre_start_xmit;
-	mesh_dev->stop = mesh_close;
-	mesh_dev->get_stats = wlan_get_stats;
-	mesh_dev->set_mac_address = wlan_set_mac_address;
+	mesh_dev->open = libertas_mesh_open;
+	mesh_dev->hard_start_xmit = libertas_mesh_pre_start_xmit;
+	mesh_dev->stop = libertas_mesh_close;
+	mesh_dev->get_stats = libertas_get_stats;
+	mesh_dev->set_mac_address = libertas_set_mac_address;
 	mesh_dev->ethtool_ops = &libertas_ethtool_ops;
 	memcpy(mesh_dev->dev_addr, priv->dev->dev_addr,
 			sizeof(priv->dev->dev_addr));
@@ -940,7 +1357,7 @@ int libertas_add_mesh(wlan_private *priv, struct device *dev)
 		goto err_free;
 	}
 
-	ret = device_create_file(&(mesh_dev->dev), &dev_attr_anycast_mask);
+	ret = sysfs_create_group(&(mesh_dev->dev.kobj), &libertas_mesh_attr_group);
 	if (ret)
 		goto err_unregister;
 
@@ -948,7 +1365,6 @@ int libertas_add_mesh(wlan_private *priv, struct device *dev)
 	ret = 0;
 	goto done;
 
-
 err_unregister:
 	unregister_netdev(mesh_dev);
 
@@ -961,86 +1377,12 @@ done:
 }
 EXPORT_SYMBOL_GPL(libertas_add_mesh);
 
-static void wake_pending_cmdnodes(wlan_private *priv)
-{
-	struct cmd_ctrl_node *cmdnode;
-	unsigned long flags;
-
-	lbs_deb_enter(LBS_DEB_CMD);
-
-	spin_lock_irqsave(&priv->adapter->driver_lock, flags);
-	list_for_each_entry(cmdnode, &priv->adapter->cmdpendingq, list) {
-		cmdnode->cmdwaitqwoken = 1;
-		wake_up_interruptible(&cmdnode->cmdwait_q);
-	}
-	spin_unlock_irqrestore(&priv->adapter->driver_lock, flags);
-}
-
-
-int libertas_remove_card(wlan_private *priv)
-{
-	wlan_adapter *adapter;
-	struct net_device *dev;
-	union iwreq_data wrqu;
-
-	lbs_deb_enter(LBS_DEB_NET);
-
-	if (!priv)
-		goto out;
-
-	adapter = priv->adapter;
-
-	if (!adapter)
-		goto out;
-
-	dev = priv->dev;
-
-	netif_stop_queue(priv->dev);
-	netif_carrier_off(priv->dev);
-
-	wake_pending_cmdnodes(priv);
-
-	unregister_netdev(dev);
-
-	cancel_delayed_work(&priv->assoc_work);
-	destroy_workqueue(priv->assoc_thread);
-
-	if (adapter->psmode == wlan802_11powermodemax_psp) {
-		adapter->psmode = wlan802_11powermodecam;
-		libertas_ps_wakeup(priv, cmd_option_waitforrsp);
-	}
-
-	memset(wrqu.ap_addr.sa_data, 0xaa, ETH_ALEN);
-	wrqu.ap_addr.sa_family = ARPHRD_ETHER;
-	wireless_send_event(priv->dev, SIOCGIWAP, &wrqu, NULL);
-
-	adapter->surpriseremoved = 1;
-
-	/* Stop the thread servicing the interrupts */
-	wlan_terminate_thread(&priv->mainthread);
-
-	libertas_debugfs_remove_one(priv);
-
-	lbs_deb_net("free adapter\n");
-	libertas_free_adapter(priv);
-
-	lbs_deb_net("unregister finish\n");
-
-	priv->dev = NULL;
-	free_netdev(dev);
-
-out:
-	lbs_deb_leave(LBS_DEB_NET);
-	return 0;
-}
-EXPORT_SYMBOL_GPL(libertas_remove_card);
-
 
 void libertas_remove_mesh(wlan_private *priv)
 {
 	struct net_device *mesh_dev;
 
-	lbs_deb_enter(LBS_DEB_NET);
+	lbs_deb_enter(LBS_DEB_MAIN);
 
 	if (!priv)
 		goto out;
@@ -1050,14 +1392,14 @@ void libertas_remove_mesh(wlan_private *priv)
 	netif_stop_queue(mesh_dev);
 	netif_carrier_off(priv->mesh_dev);
 
-	device_remove_file(&(mesh_dev->dev), &dev_attr_anycast_mask);
+	sysfs_remove_group(&(mesh_dev->dev.kobj), &libertas_mesh_attr_group);
 	unregister_netdev(mesh_dev);
 
 	priv->mesh_dev = NULL ;
 	free_netdev(mesh_dev);
 
 out:
-	lbs_deb_leave(LBS_DEB_NET);
+	lbs_deb_leave(LBS_DEB_MAIN);
 }
 EXPORT_SYMBOL_GPL(libertas_remove_mesh);
 
@@ -1076,7 +1418,7 @@ struct chan_freq_power *libertas_get_region_cfp_table(u8 region, u8 band, int *c
 
 	lbs_deb_enter(LBS_DEB_MAIN);
 
-	end = sizeof(region_cfp_table)/sizeof(struct region_cfp_table);
+	end = ARRAY_SIZE(region_cfp_table);
 
 	for (i = 0; i < end ; i++) {
 		lbs_deb_main("region_cfp_table[i].region=%d\n",
@@ -1148,15 +1490,30 @@ void libertas_interrupt(struct net_device *dev)
 	if (priv->adapter->psstate == PS_STATE_SLEEP) {
 		priv->adapter->psstate = PS_STATE_AWAKE;
 		netif_wake_queue(dev);
-		netif_wake_queue(priv->mesh_dev);
+		if (priv->mesh_dev)
+			netif_wake_queue(priv->mesh_dev);
 	}
 
-	wake_up_interruptible(&priv->mainthread.waitq);
+	wake_up_interruptible(&priv->waitq);
 
 	lbs_deb_leave(LBS_DEB_THREAD);
 }
 EXPORT_SYMBOL_GPL(libertas_interrupt);
 
+int libertas_reset_device(wlan_private *priv)
+{
+	int ret;
+
+	lbs_deb_enter(LBS_DEB_MAIN);
+	ret = libertas_prepare_and_send_command(priv, CMD_802_11_RESET,
+				    CMD_ACT_HALT, 0, 0, NULL);
+	msleep_interruptible(10);
+
+	lbs_deb_leave_args(LBS_DEB_MAIN, "ret %d", ret);
+	return ret;
+}
+EXPORT_SYMBOL_GPL(libertas_reset_device);
+
 static int libertas_init_module(void)
 {
 	lbs_deb_enter(LBS_DEB_MAIN);
@@ -1174,6 +1531,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 769c86fb9509..0420e5b9ff9b 100644
--- a/drivers/net/wireless/libertas/rx.c
+++ b/drivers/net/wireless/libertas/rx.c
@@ -85,12 +85,12 @@ static u8 wlan_getavgnf(wlan_private * priv)
 static void wlan_save_rawSNRNF(wlan_private * priv, struct rxpd *p_rx_pd)
 {
 	wlan_adapter *adapter = priv->adapter;
-	if (adapter->numSNRNF < adapter->data_avg_factor)
+	if (adapter->numSNRNF < DEFAULT_DATA_AVG_FACTOR)
 		adapter->numSNRNF++;
 	adapter->rawSNR[adapter->nextSNRNF] = p_rx_pd->snr;
 	adapter->rawNF[adapter->nextSNRNF] = p_rx_pd->nf;
 	adapter->nextSNRNF++;
-	if (adapter->nextSNRNF >= adapter->data_avg_factor)
+	if (adapter->nextSNRNF >= DEFAULT_DATA_AVG_FACTOR)
 		adapter->nextSNRNF = 0;
 	return;
 }
@@ -117,8 +117,6 @@ static void wlan_compute_rssi(wlan_private * priv, struct rxpd *p_rx_pd)
 	adapter->NF[TYPE_RXPD][TYPE_NOAVG] = p_rx_pd->nf;
 	wlan_save_rawSNRNF(priv, p_rx_pd);
 
-	adapter->rxpd_rate = p_rx_pd->rx_rate;
-
 	adapter->SNR[TYPE_RXPD][TYPE_AVG] = wlan_getavgsnr(priv) * AVG_SCALE;
 	adapter->NF[TYPE_RXPD][TYPE_AVG] = wlan_getavgnf(priv) * AVG_SCALE;
 	lbs_deb_rx("after computing SNR: SNR-avg = %d, NF-avg = %d\n",
@@ -140,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);
 }
 
@@ -172,11 +173,7 @@ int libertas_process_rxed_packet(wlan_private * priv, struct sk_buff *skb)
 
 	lbs_deb_enter(LBS_DEB_RX);
 
-	if (priv->adapter->debugmode & MRVDRV_DEBUG_RX_PATH)
-		lbs_dbg_hex("RX packet: ", skb->data,
-			 min_t(unsigned int, skb->len, 100));
-
-	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;
@@ -186,7 +183,7 @@ int libertas_process_rxed_packet(wlan_private * priv, struct sk_buff *skb)
 	else
 		UNSET_MESH_FRAME(skb);
 
-	lbs_dbg_hex("RX Data: Before chop rxpd", skb->data,
+	lbs_deb_hex(LBS_DEB_RX, "RX Data: Before chop rxpd", skb->data,
 		 min_t(unsigned int, skb->len, 100));
 
 	if (skb->len < (ETH_HLEN + 8 + sizeof(struct rxpd))) {
@@ -210,9 +207,9 @@ int libertas_process_rxed_packet(wlan_private * priv, struct sk_buff *skb)
 	lbs_deb_rx("rx data: skb->len-sizeof(RxPd) = %d-%zd = %zd\n",
 	       skb->len, sizeof(struct rxpd), skb->len - sizeof(struct rxpd));
 
-	lbs_dbg_hex("RX Data: Dest", p_rx_pkt->eth803_hdr.dest_addr,
+	lbs_deb_hex(LBS_DEB_RX, "RX Data: Dest", p_rx_pkt->eth803_hdr.dest_addr,
 		sizeof(p_rx_pkt->eth803_hdr.dest_addr));
-	lbs_dbg_hex("RX Data: Src", p_rx_pkt->eth803_hdr.src_addr,
+	lbs_deb_hex(LBS_DEB_RX, "RX Data: Src", p_rx_pkt->eth803_hdr.src_addr,
 		sizeof(p_rx_pkt->eth803_hdr.src_addr));
 
 	if (memcmp(&p_rx_pkt->rfc1042_hdr,
@@ -244,7 +241,7 @@ int libertas_process_rxed_packet(wlan_private * priv, struct sk_buff *skb)
 		 */
 		hdrchop = (u8 *) p_ethhdr - (u8 *) p_rx_pkt;
 	} else {
-		lbs_dbg_hex("RX Data: LLC/SNAP",
+		lbs_deb_hex(LBS_DEB_RX, "RX Data: LLC/SNAP",
 			(u8 *) & p_rx_pkt->rfc1042_hdr,
 			sizeof(p_rx_pkt->rfc1042_hdr));
 
@@ -260,8 +257,8 @@ int libertas_process_rxed_packet(wlan_private * priv, struct sk_buff *skb)
 	/* Take the data rate from the rxpd structure
 	 * only if the rate is auto
 	 */
-	if (adapter->is_datarate_auto)
-		adapter->datarate = libertas_index_to_data_rate(p_rx_pd->rx_rate);
+	if (adapter->auto_rate)
+		adapter->cur_rate = libertas_fw_index_to_data_rate(p_rx_pd->rx_rate);
 
 	wlan_compute_rssi(priv, p_rx_pd);
 
@@ -296,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);
@@ -340,7 +338,7 @@ static int process_rxed_802_11_packet(wlan_private * priv, struct sk_buff *skb)
 	p_rx_pkt = (struct rx80211packethdr *) skb->data;
 	prxpd = &p_rx_pkt->rx_pd;
 
-	// lbs_dbg_hex("RX Data: Before chop rxpd", skb->data, min(skb->len, 100));
+	// lbs_deb_hex(LBS_DEB_RX, "RX Data: Before chop rxpd", skb->data, min(skb->len, 100));
 
 	if (skb->len < (ETH_HLEN + 8 + sizeof(struct rxpd))) {
 		lbs_deb_rx("rx err: frame received wit bad length\n");
@@ -361,20 +359,19 @@ 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 */
 		radiotap_hdr.hdr.it_pad = 0;
-		radiotap_hdr.hdr.it_len = sizeof(struct rx_radiotap_hdr);
-		radiotap_hdr.hdr.it_present = RX_RADIOTAP_PRESENT;
+		radiotap_hdr.hdr.it_len = cpu_to_le16 (sizeof(struct rx_radiotap_hdr));
+		radiotap_hdr.hdr.it_present = cpu_to_le32 (RX_RADIOTAP_PRESENT);
 		/* unknown values */
 		radiotap_hdr.flags = 0;
 		radiotap_hdr.chan_freq = 0;
@@ -389,8 +386,6 @@ static int process_rxed_802_11_packet(wlan_private * priv, struct sk_buff *skb)
 			radiotap_hdr.rx_flags |= IEEE80211_RADIOTAP_F_RX_BADFCS;
 		//memset(radiotap_hdr.pad, 0x11, IEEE80211_RADIOTAP_HDRLEN - 18);
 
-		// lbs_dbg_hex1("RX radiomode packet BEF: ", skb->data, min(skb->len, 100));
-
 		/* chop the rxpd */
 		skb_pull(skb, sizeof(struct rxpd));
 
@@ -408,25 +403,13 @@ 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));
-		//lbs_dbg_hex1("RX radiomode packet AFT: ", skb->data, min(skb->len, 100));
-		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
 	 * only if the rate is auto
 	 */
-	if (adapter->is_datarate_auto) {
-		adapter->datarate = libertas_index_to_data_rate(prxpd->rx_rate);
-	}
+	if (adapter->auto_rate)
+		adapter->cur_rate = libertas_fw_index_to_data_rate(prxpd->rx_rate);
 
 	wlan_compute_rssi(priv, prxpd);
 
diff --git a/drivers/net/wireless/libertas/scan.c b/drivers/net/wireless/libertas/scan.c
index c3043dcb541e..ad1e67d984ce 100644
--- a/drivers/net/wireless/libertas/scan.c
+++ b/drivers/net/wireless/libertas/scan.c
@@ -13,10 +13,13 @@
 #include <net/ieee80211.h>
 #include <net/iw_handler.h>
 
+#include <asm/unaligned.h>
+
 #include "host.h"
 #include "decl.h"
 #include "dev.h"
 #include "scan.h"
+#include "join.h"
 
 //! Approximate amount of data needed to pass a scan result back to iwlist
 #define MAX_SCAN_CELL_SIZE  (IW_EV_ADDR_LEN             \
@@ -62,6 +65,15 @@
 static const u8 zeromac[ETH_ALEN] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
 static const u8 bcastmac[ETH_ALEN] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };
 
+
+
+
+/*********************************************************************/
+/*                                                                   */
+/*  Misc helper functions                                            */
+/*                                                                   */
+/*********************************************************************/
+
 static inline void clear_bss_descriptor (struct bss_descriptor * bss)
 {
 	/* Don't blow away ->list, just BSS data */
@@ -74,9 +86,9 @@ static inline int match_bss_no_security(struct wlan_802_11_security * secinfo,
 	if (   !secinfo->wep_enabled
 	    && !secinfo->WPAenabled
 	    && !secinfo->WPA2enabled
-	    && match_bss->wpa_ie[0] != WPA_IE
-	    && match_bss->rsn_ie[0] != WPA2_IE
-	    && !match_bss->privacy) {
+	    && match_bss->wpa_ie[0] != MFIE_TYPE_GENERIC
+	    && match_bss->rsn_ie[0] != MFIE_TYPE_RSN
+	    && !(match_bss->capability & WLAN_CAPABILITY_PRIVACY)) {
 		return 1;
 	}
 	return 0;
@@ -88,7 +100,7 @@ static inline int match_bss_static_wep(struct wlan_802_11_security * secinfo,
 	if ( secinfo->wep_enabled
 	   && !secinfo->WPAenabled
 	   && !secinfo->WPA2enabled
-	   && match_bss->privacy) {
+	   && (match_bss->capability & WLAN_CAPABILITY_PRIVACY)) {
 		return 1;
 	}
 	return 0;
@@ -99,9 +111,10 @@ static inline int match_bss_wpa(struct wlan_802_11_security * secinfo,
 {
 	if (  !secinfo->wep_enabled
 	   && secinfo->WPAenabled
-	   && (match_bss->wpa_ie[0] == WPA_IE)
+	   && (match_bss->wpa_ie[0] == MFIE_TYPE_GENERIC)
 	   /* privacy bit may NOT be set in some APs like LinkSys WRT54G
-	      && bss->privacy */
+	      && (match_bss->capability & WLAN_CAPABILITY_PRIVACY)) {
+	    */
 	   ) {
 		return 1;
 	}
@@ -113,9 +126,10 @@ static inline int match_bss_wpa2(struct wlan_802_11_security * secinfo,
 {
 	if (  !secinfo->wep_enabled
 	   && secinfo->WPA2enabled
-	   && (match_bss->rsn_ie[0] == WPA2_IE)
+	   && (match_bss->rsn_ie[0] == MFIE_TYPE_RSN)
 	   /* privacy bit may NOT be set in some APs like LinkSys WRT54G
-	      && bss->privacy */
+	      && (match_bss->capability & WLAN_CAPABILITY_PRIVACY)) {
+	    */
 	   ) {
 		return 1;
 	}
@@ -128,9 +142,9 @@ static inline int match_bss_dynamic_wep(struct wlan_802_11_security * secinfo,
 	if (  !secinfo->wep_enabled
 	   && !secinfo->WPAenabled
 	   && !secinfo->WPA2enabled
-	   && (match_bss->wpa_ie[0] != WPA_IE)
-	   && (match_bss->rsn_ie[0] != WPA2_IE)
-	   && match_bss->privacy) {
+	   && (match_bss->wpa_ie[0] != MFIE_TYPE_GENERIC)
+	   && (match_bss->rsn_ie[0] != MFIE_TYPE_RSN)
+	   && (match_bss->capability & WLAN_CAPABILITY_PRIVACY)) {
 		return 1;
 	}
 	return 0;
@@ -160,7 +174,7 @@ static int is_network_compatible(wlan_adapter * adapter,
 {
 	int matched = 0;
 
-	lbs_deb_enter(LBS_DEB_ASSOC);
+	lbs_deb_enter(LBS_DEB_SCAN);
 
 	if (bss->mode != mode)
 		goto done;
@@ -177,7 +191,7 @@ static int is_network_compatible(wlan_adapter * adapter,
 		       adapter->secinfo.wep_enabled ? "e" : "d",
 		       adapter->secinfo.WPAenabled ? "e" : "d",
 		       adapter->secinfo.WPA2enabled ? "e" : "d",
-		       bss->privacy);
+		       (bss->capability & WLAN_CAPABILITY_PRIVACY));
 		goto done;
 	} else if ((matched = match_bss_wpa2(&adapter->secinfo, bss))) {
 		lbs_deb_scan(
@@ -187,15 +201,14 @@ static int is_network_compatible(wlan_adapter * adapter,
 		       adapter->secinfo.wep_enabled ? "e" : "d",
 		       adapter->secinfo.WPAenabled ? "e" : "d",
 		       adapter->secinfo.WPA2enabled ? "e" : "d",
-		       bss->privacy);
+		       (bss->capability & WLAN_CAPABILITY_PRIVACY));
 		goto done;
 	} else if ((matched = match_bss_dynamic_wep(&adapter->secinfo, bss))) {
 		lbs_deb_scan(
 		       "is_network_compatible() dynamic WEP: "
 		       "wpa_ie=%#x wpa2_ie=%#x privacy=%#x\n",
-		       bss->wpa_ie[0],
-		       bss->rsn_ie[0],
-		       bss->privacy);
+		       bss->wpa_ie[0], bss->rsn_ie[0],
+		       (bss->capability & WLAN_CAPABILITY_PRIVACY));
 		goto done;
 	}
 
@@ -207,16 +220,44 @@ static int is_network_compatible(wlan_adapter * adapter,
 	       adapter->secinfo.wep_enabled ? "e" : "d",
 	       adapter->secinfo.WPAenabled ? "e" : "d",
 	       adapter->secinfo.WPA2enabled ? "e" : "d",
-	       bss->privacy);
+	       (bss->capability & WLAN_CAPABILITY_PRIVACY));
 
 done:
-	lbs_deb_leave(LBS_DEB_SCAN);
+	lbs_deb_leave_args(LBS_DEB_SCAN, "matched: %d", matched);
 	return matched;
 }
 
 /**
+ *  @brief Compare two SSIDs
+ *
+ *  @param ssid1    A pointer to ssid to compare
+ *  @param ssid2    A pointer to ssid to compare
+ *
+ *  @return         0--ssid is same, otherwise is different
+ */
+int libertas_ssid_cmp(u8 *ssid1, u8 ssid1_len, u8 *ssid2, u8 ssid2_len)
+{
+	if (ssid1_len != ssid2_len)
+		return -1;
+
+	return memcmp(ssid1, ssid2, ssid1_len);
+}
+
+
+
+
+/*********************************************************************/
+/*                                                                   */
+/*  Main scanning support                                            */
+/*                                                                   */
+/*********************************************************************/
+
+
+/**
  *  @brief Create a channel list for the driver to scan based on region info
  *
+ *  Only used from wlan_scan_setup_scan_config()
+ *
  *  Use the driver region/band information to construct a comprehensive list
  *    of channels to scan.  This routine is used for any scan that is not
  *    provided a specific channel list to scan.
@@ -244,17 +285,19 @@ static void wlan_scan_create_channel_list(wlan_private * priv,
 	int nextchan;
 	u8 scantype;
 
+	lbs_deb_enter_args(LBS_DEB_SCAN, "filteredscan %d", filteredscan);
+
 	chanidx = 0;
 
 	/* Set the default scan type to the user specified type, will later
 	 *   be changed to passive on a per channel basis if restricted by
 	 *   regulatory requirements (11d or 11h)
 	 */
-	scantype = adapter->scantype;
+	scantype = CMD_SCAN_TYPE_ACTIVE;
 
 	for (rgnidx = 0; rgnidx < ARRAY_SIZE(adapter->region_channel); rgnidx++) {
 		if (priv->adapter->enable11d &&
-		    adapter->connect_status != libertas_connected) {
+		    adapter->connect_status != LIBERTAS_CONNECTED) {
 			/* Scan all the supported chan for the first scan */
 			if (!adapter->universal_channel[rgnidx].valid)
 				continue;
@@ -286,11 +329,11 @@ static void wlan_scan_create_channel_list(wlan_private * priv,
 			case BAND_G:
 			default:
 				scanchanlist[chanidx].radiotype =
-				    cmd_scan_radio_type_bg;
+				    CMD_SCAN_RADIO_TYPE_BG;
 				break;
 			}
 
-			if (scantype == cmd_scan_type_passive) {
+			if (scantype == CMD_SCAN_TYPE_PASSIVE) {
 				scanchanlist[chanidx].maxscantime =
 				    cpu_to_le16(MRVDRV_PASSIVE_SCAN_CHAN_TIME);
 				scanchanlist[chanidx].chanscanmode.passivescan =
@@ -312,6 +355,16 @@ static void wlan_scan_create_channel_list(wlan_private * priv,
 	}
 }
 
+
+/* Delayed partial scan worker */
+void libertas_scan_worker(struct work_struct *work)
+{
+	wlan_private *priv = container_of(work, wlan_private, scan_work.work);
+
+	wlan_scan_networks(priv, NULL, 0);
+}
+
+
 /**
  *  @brief Construct a wlan_scan_cmd_config structure to use in issue scan cmds
  *
@@ -359,7 +412,6 @@ wlan_scan_setup_scan_config(wlan_private * priv,
 			    u8 * pfilteredscan,
 			    u8 * pscancurrentonly)
 {
-	wlan_adapter *adapter = priv->adapter;
 	struct mrvlietypes_numprobes *pnumprobestlv;
 	struct mrvlietypes_ssidparamset *pssidtlv;
 	struct wlan_scan_cmd_config * pscancfgout = NULL;
@@ -371,6 +423,8 @@ wlan_scan_setup_scan_config(wlan_private * priv,
 	int channel;
 	int radiotype;
 
+	lbs_deb_enter(LBS_DEB_SCAN);
+
 	pscancfgout = kzalloc(MAX_SCAN_CFG_ALLOC, GFP_KERNEL);
 	if (pscancfgout == NULL)
 		goto out;
@@ -407,15 +461,12 @@ wlan_scan_setup_scan_config(wlan_private * priv,
 	*pscancurrentonly = 0;
 
 	if (puserscanin) {
-
 		/* Set the bss type scan filter, use adapter setting if unset */
 		pscancfgout->bsstype =
-		    (puserscanin->bsstype ? puserscanin->bsstype : adapter->
-		     scanmode);
+		    puserscanin->bsstype ? puserscanin->bsstype : CMD_BSS_TYPE_ANY;
 
 		/* Set the number of probes to send, use adapter setting if unset */
-		numprobes = (puserscanin->numprobes ? puserscanin->numprobes :
-			     adapter->scanprobes);
+		numprobes = puserscanin->numprobes ? puserscanin->numprobes : 0;
 
 		/*
 		 * Set the BSSID filter to the incoming configuration,
@@ -447,8 +498,8 @@ wlan_scan_setup_scan_config(wlan_private * priv,
 			*pfilteredscan = 1;
 		}
 	} else {
-		pscancfgout->bsstype = adapter->scanmode;
-		numprobes = adapter->scanprobes;
+		pscancfgout->bsstype = CMD_BSS_TYPE_ANY;
+		numprobes = 0;
 	}
 
 	/* If the input config or adapter has the number of Probes set, add tlv */
@@ -469,59 +520,56 @@ wlan_scan_setup_scan_config(wlan_private * priv,
 	 */
 	*ppchantlvout = (struct mrvlietypes_chanlistparamset *) ptlvpos;
 
-	if (puserscanin && puserscanin->chanlist[0].channumber) {
-
-		lbs_deb_scan("Scan: Using supplied channel list\n");
+	if (!puserscanin || !puserscanin->chanlist[0].channumber) {
+		/* Create a default channel scan list */
+		lbs_deb_scan("creating full region channel list\n");
+		wlan_scan_create_channel_list(priv, pscanchanlist,
+					      *pfilteredscan);
+		goto out;
+	}
 
-		for (chanidx = 0;
-		     chanidx < WLAN_IOCTL_USER_SCAN_CHAN_MAX
-		     && puserscanin->chanlist[chanidx].channumber; chanidx++) {
+	for (chanidx = 0;
+	     chanidx < WLAN_IOCTL_USER_SCAN_CHAN_MAX
+	     && puserscanin->chanlist[chanidx].channumber; chanidx++) {
 
-			channel = puserscanin->chanlist[chanidx].channumber;
-			(pscanchanlist + chanidx)->channumber = channel;
+		channel = puserscanin->chanlist[chanidx].channumber;
+		(pscanchanlist + chanidx)->channumber = channel;
 
-			radiotype = puserscanin->chanlist[chanidx].radiotype;
-			(pscanchanlist + chanidx)->radiotype = radiotype;
+		radiotype = puserscanin->chanlist[chanidx].radiotype;
+		(pscanchanlist + chanidx)->radiotype = radiotype;
 
-			scantype = puserscanin->chanlist[chanidx].scantype;
+		scantype = puserscanin->chanlist[chanidx].scantype;
 
-			if (scantype == cmd_scan_type_passive) {
-				(pscanchanlist +
-				 chanidx)->chanscanmode.passivescan = 1;
-			} else {
-				(pscanchanlist +
-				 chanidx)->chanscanmode.passivescan = 0;
-			}
+		if (scantype == CMD_SCAN_TYPE_PASSIVE) {
+			(pscanchanlist +
+			 chanidx)->chanscanmode.passivescan = 1;
+		} else {
+			(pscanchanlist +
+			 chanidx)->chanscanmode.passivescan = 0;
+		}
 
-			if (puserscanin->chanlist[chanidx].scantime) {
-				scandur =
-				    puserscanin->chanlist[chanidx].scantime;
+		if (puserscanin->chanlist[chanidx].scantime) {
+			scandur = puserscanin->chanlist[chanidx].scantime;
+		} else {
+			if (scantype == CMD_SCAN_TYPE_PASSIVE) {
+				scandur = MRVDRV_PASSIVE_SCAN_CHAN_TIME;
 			} else {
-				if (scantype == cmd_scan_type_passive) {
-					scandur = MRVDRV_PASSIVE_SCAN_CHAN_TIME;
-				} else {
-					scandur = MRVDRV_ACTIVE_SCAN_CHAN_TIME;
-				}
+				scandur = MRVDRV_ACTIVE_SCAN_CHAN_TIME;
 			}
-
-			(pscanchanlist + chanidx)->minscantime =
-			    cpu_to_le16(scandur);
-			(pscanchanlist + chanidx)->maxscantime =
-			    cpu_to_le16(scandur);
 		}
 
-		/* Check if we are only scanning the current channel */
-		if ((chanidx == 1) && (puserscanin->chanlist[0].channumber
-				       ==
-				       priv->adapter->curbssparams.channel)) {
-			*pscancurrentonly = 1;
-			lbs_deb_scan("Scan: Scanning current channel only");
-		}
+		(pscanchanlist + chanidx)->minscantime =
+		    cpu_to_le16(scandur);
+		(pscanchanlist + chanidx)->maxscantime =
+		    cpu_to_le16(scandur);
+	}
 
-	} else {
-		lbs_deb_scan("Scan: Creating full region channel list\n");
-		wlan_scan_create_channel_list(priv, pscanchanlist,
-					      *pfilteredscan);
+	/* Check if we are only scanning the current channel */
+	if ((chanidx == 1) &&
+	    (puserscanin->chanlist[0].channumber ==
+			       priv->adapter->curbssparams.channel)) {
+		*pscancurrentonly = 1;
+		lbs_deb_scan("scanning current channel only");
 	}
 
 out:
@@ -531,6 +579,8 @@ out:
 /**
  *  @brief Construct and send multiple scan config commands to the firmware
  *
+ *  Only used from wlan_scan_networks()
+ *
  *  Previous routines have created a wlan_scan_cmd_config with any requested
  *   TLVs.  This function splits the channel TLV into maxchanperscan lists
  *   and sends the portion of the channel TLV along with the other TLVs
@@ -568,12 +618,14 @@ static int wlan_scan_channel_list(wlan_private * priv,
 	int scanned = 0;
 	union iwreq_data wrqu;
 
-	lbs_deb_enter(LBS_DEB_ASSOC);
+	lbs_deb_enter_args(LBS_DEB_SCAN, "maxchanperscan %d, filteredscan %d, "
+		"full_scan %d", maxchanperscan, filteredscan, full_scan);
 
-	if (pscancfgout == 0 || pchantlvout == 0 || pscanchanlist == 0) {
-		lbs_deb_scan("Scan: Null detect: %p, %p, %p\n",
-		       pscancfgout, pchantlvout, pscanchanlist);
-		return -1;
+	if (!pscancfgout || !pchantlvout || !pscanchanlist) {
+		lbs_deb_scan("pscancfgout, pchantlvout or "
+			"pscanchanlist is NULL\n");
+		ret = -1;
+		goto out;
 	}
 
 	pchantlvout->header.type = cpu_to_le16(TLV_TYPE_CHANLIST);
@@ -605,12 +657,13 @@ static int wlan_scan_channel_list(wlan_private * priv,
 		while (tlvidx < maxchanperscan && ptmpchan->channumber
 		       && !doneearly && scanned < 2) {
 
-            lbs_deb_scan(
-                    "Scan: Chan(%3d), Radio(%d), mode(%d,%d), Dur(%d)\n",
-                ptmpchan->channumber, ptmpchan->radiotype,
-                ptmpchan->chanscanmode.passivescan,
-                ptmpchan->chanscanmode.disablechanfilt,
-                ptmpchan->maxscantime);
+			lbs_deb_scan("channel %d, radio %d, passive %d, "
+				"dischanflt %d, maxscantime %d\n",
+				ptmpchan->channumber,
+				ptmpchan->radiotype,
+			             ptmpchan->chanscanmode.passivescan,
+			             ptmpchan->chanscanmode.disablechanfilt,
+			             ptmpchan->maxscantime);
 
 			/* Copy the current channel TLV to the command being prepared */
 			memcpy(pchantlvout->chanscanparam + tlvidx,
@@ -667,7 +720,7 @@ static int wlan_scan_channel_list(wlan_private * priv,
 		}
 
 		/* Send the scan command to the firmware with the specified cfg */
-		ret = libertas_prepare_and_send_command(priv, cmd_802_11_scan, 0,
+		ret = libertas_prepare_and_send_command(priv, CMD_802_11_SCAN, 0,
 					    0, 0, pscancfgout);
 		if (scanned >= 2 && !full_scan) {
 			ret = 0;
@@ -679,24 +732,38 @@ static int wlan_scan_channel_list(wlan_private * priv,
 done:
 	priv->adapter->last_scanned_channel = ptmpchan->channumber;
 
-	/* Tell userspace the scan table has been updated */
-	memset(&wrqu, 0, sizeof(union iwreq_data));
-	wireless_send_event(priv->dev, SIOCGIWSCAN, &wrqu, NULL);
+	if (priv->adapter->last_scanned_channel) {
+		/* Schedule the next part of the partial scan */
+		if (!full_scan && !priv->adapter->surpriseremoved) {
+			cancel_delayed_work(&priv->scan_work);
+			queue_delayed_work(priv->work_thread, &priv->scan_work,
+			                   msecs_to_jiffies(300));
+		}
+	} else {
+		/* All done, tell userspace the scan table has been updated */
+		memset(&wrqu, 0, sizeof(union iwreq_data));
+		wireless_send_event(priv->dev, SIOCGIWSCAN, &wrqu, NULL);
+	}
 
+out:
 	lbs_deb_leave_args(LBS_DEB_SCAN, "ret %d", ret);
 	return ret;
 }
 
-static void
-clear_selected_scan_list_entries(wlan_adapter * adapter,
-                                 const struct wlan_ioctl_user_scan_cfg * scan_cfg)
+/*
+ * Only used from wlan_scan_networks()
+*/
+static void clear_selected_scan_list_entries(wlan_adapter *adapter,
+	const struct wlan_ioctl_user_scan_cfg *scan_cfg)
 {
-	struct bss_descriptor * bss;
-	struct bss_descriptor * safe;
+	struct bss_descriptor *bss;
+	struct bss_descriptor *safe;
 	u32 clear_ssid_flag = 0, clear_bssid_flag = 0;
 
+	lbs_deb_enter(LBS_DEB_SCAN);
+
 	if (!scan_cfg)
-		return;
+		goto out;
 
 	if (scan_cfg->clear_ssid && scan_cfg->ssid_len)
 		clear_ssid_flag = 1;
@@ -708,7 +775,7 @@ clear_selected_scan_list_entries(wlan_adapter * adapter,
 	}
 
 	if (!clear_ssid_flag && !clear_bssid_flag)
-		return;
+		goto out;
 
 	mutex_lock(&adapter->lock);
 	list_for_each_entry_safe (bss, safe, &adapter->network_list, list) {
@@ -731,12 +798,16 @@ clear_selected_scan_list_entries(wlan_adapter * adapter,
 		}
 	}
 	mutex_unlock(&adapter->lock);
+out:
+	lbs_deb_leave(LBS_DEB_SCAN);
 }
 
 
 /**
  *  @brief Internal function used to start a scan based on an input config
  *
+ *  Also used from debugfs
+ *
  *  Use the input user scan configuration information when provided in
  *    order to send the appropriate scan commands to firmware to populate or
  *    update the internal driver scan table
@@ -744,12 +815,13 @@ clear_selected_scan_list_entries(wlan_adapter * adapter,
  *  @param priv          A pointer to wlan_private structure
  *  @param puserscanin   Pointer to the input configuration for the requested
  *                       scan.
+ *  @param full_scan     ???
  *
  *  @return              0 or < 0 if error
  */
 int wlan_scan_networks(wlan_private * priv,
-			      const struct wlan_ioctl_user_scan_cfg * puserscanin,
-			      int full_scan)
+                       const struct wlan_ioctl_user_scan_cfg * puserscanin,
+                       int full_scan)
 {
 	wlan_adapter * adapter = priv->adapter;
 	struct mrvlietypes_chanlistparamset *pchantlvout;
@@ -762,9 +834,16 @@ int wlan_scan_networks(wlan_private * priv,
 #ifdef CONFIG_LIBERTAS_DEBUG
 	struct bss_descriptor * iter_bss;
 	int i = 0;
+	DECLARE_MAC_BUF(mac);
 #endif
 
-	lbs_deb_enter(LBS_DEB_ASSOC);
+	lbs_deb_enter_args(LBS_DEB_SCAN, "full_scan %d", full_scan);
+
+	/* Cancel any partial outstanding partial scans if this scan
+	 * is a full scan.
+	 */
+	if (full_scan && delayed_work_pending(&priv->scan_work))
+		cancel_delayed_work(&priv->scan_work);
 
 	scan_chan_list = kzalloc(sizeof(struct chanscanparamset) *
 				WLAN_IOCTL_USER_SCAN_CHAN_MAX, GFP_KERNEL);
@@ -791,8 +870,10 @@ int wlan_scan_networks(wlan_private * priv,
 	if (!scancurrentchanonly) {
 		netif_stop_queue(priv->dev);
 		netif_carrier_off(priv->dev);
-		netif_stop_queue(priv->mesh_dev);
-		netif_carrier_off(priv->mesh_dev);
+		if (priv->mesh_dev) {
+			netif_stop_queue(priv->mesh_dev);
+			netif_carrier_off(priv->mesh_dev);
+		}
 	}
 
 	ret = wlan_scan_channel_list(priv,
@@ -807,19 +888,22 @@ int wlan_scan_networks(wlan_private * priv,
 #ifdef CONFIG_LIBERTAS_DEBUG
 	/* Dump the scan table */
 	mutex_lock(&adapter->lock);
+	lbs_deb_scan("The scan table contains:\n");
 	list_for_each_entry (iter_bss, &adapter->network_list, list) {
-		lbs_deb_scan("Scan:(%02d) " MAC_FMT ", RSSI[%03d], SSID[%s]\n",
-		       i++, MAC_ARG(iter_bss->bssid), (s32) iter_bss->rssi,
+		lbs_deb_scan("scan %02d, %s, RSSI, %d, SSID '%s'\n",
+		       i++, print_mac(mac, iter_bss->bssid), (s32) iter_bss->rssi,
 		       escape_essid(iter_bss->ssid, iter_bss->ssid_len));
 	}
 	mutex_unlock(&adapter->lock);
 #endif
 
-	if (priv->adapter->connect_status == libertas_connected) {
+	if (priv->adapter->connect_status == LIBERTAS_CONNECTED) {
 		netif_carrier_on(priv->dev);
 		netif_wake_queue(priv->dev);
-		netif_carrier_on(priv->mesh_dev);
-		netif_wake_queue(priv->mesh_dev);
+		if (priv->mesh_dev) {
+			netif_carrier_on(priv->mesh_dev);
+			netif_wake_queue(priv->mesh_dev);
+		}
 	}
 
 out:
@@ -834,58 +918,6 @@ out:
 }
 
 /**
- *  @brief Inspect the scan response buffer for pointers to expected TLVs
- *
- *  TLVs can be included at the end of the scan response BSS information.
- *    Parse the data in the buffer for pointers to TLVs that can potentially
- *    be passed back in the response
- *
- *  @param ptlv        Pointer to the start of the TLV buffer to parse
- *  @param tlvbufsize  size of the TLV buffer
- *  @param ptsftlv     Output parameter: Pointer to the TSF TLV if found
- *
- *  @return            void
- */
-static
-void wlan_ret_802_11_scan_get_tlv_ptrs(struct mrvlietypes_data * ptlv,
-				       int tlvbufsize,
-				       struct mrvlietypes_tsftimestamp ** ptsftlv)
-{
-	struct mrvlietypes_data *pcurrenttlv;
-	int tlvbufleft;
-	u16 tlvtype;
-	u16 tlvlen;
-
-	pcurrenttlv = ptlv;
-	tlvbufleft = tlvbufsize;
-	*ptsftlv = NULL;
-
-	lbs_deb_scan("SCAN_RESP: tlvbufsize = %d\n", tlvbufsize);
-	lbs_dbg_hex("SCAN_RESP: TLV Buf", (u8 *) ptlv, tlvbufsize);
-
-	while (tlvbufleft >= sizeof(struct mrvlietypesheader)) {
-		tlvtype = le16_to_cpu(pcurrenttlv->header.type);
-		tlvlen = le16_to_cpu(pcurrenttlv->header.len);
-
-		switch (tlvtype) {
-		case TLV_TYPE_TSFTIMESTAMP:
-			*ptsftlv = (struct mrvlietypes_tsftimestamp *) pcurrenttlv;
-			break;
-
-		default:
-			lbs_deb_scan("SCAN_RESP: Unhandled TLV = %d\n",
-			       tlvtype);
-			/* Give up, this seems corrupted */
-			return;
-		}		/* switch */
-
-		tlvbufleft -= (sizeof(ptlv->header) + tlvlen);
-		pcurrenttlv =
-		    (struct mrvlietypes_data *) (pcurrenttlv->Data + tlvlen);
-	}			/* while */
-}
-
-/**
  *  @brief Interpret a BSS scan response returned from the firmware
  *
  *  Parse the various fixed fields and IEs passed back for a a BSS probe
@@ -899,67 +931,49 @@ void wlan_ret_802_11_scan_get_tlv_ptrs(struct mrvlietypes_data * ptlv,
 static int libertas_process_bss(struct bss_descriptor * bss,
 				u8 ** pbeaconinfo, int *bytesleft)
 {
-	enum ieeetypes_elementid elemID;
 	struct ieeetypes_fhparamset *pFH;
 	struct ieeetypes_dsparamset *pDS;
 	struct ieeetypes_cfparamset *pCF;
 	struct ieeetypes_ibssparamset *pibss;
-	struct ieeetypes_capinfo *pcap;
-	struct WLAN_802_11_FIXED_IEs fixedie;
-	u8 *pcurrentptr;
-	u8 *pRate;
-	u8 elemlen;
-	u8 bytestocopy;
-	u8 ratesize;
-	u16 beaconsize;
-	u8 founddatarateie;
-	int bytesleftforcurrentbeacon;
-	int ret;
-
-	struct IE_WPA *pIe;
-	const u8 oui01[4] = { 0x00, 0x50, 0xf2, 0x01 };
-
+	DECLARE_MAC_BUF(mac);
 	struct ieeetypes_countryinfoset *pcountryinfo;
+	u8 *pos, *end, *p;
+	u8 n_ex_rates = 0, got_basic_rates = 0, n_basic_rates = 0;
+	u16 beaconsize = 0;
+	int ret;
 
-	lbs_deb_enter(LBS_DEB_ASSOC);
-
-	founddatarateie = 0;
-	ratesize = 0;
-	beaconsize = 0;
+	lbs_deb_enter(LBS_DEB_SCAN);
 
 	if (*bytesleft >= sizeof(beaconsize)) {
 		/* Extract & convert beacon size from the command buffer */
-		beaconsize = le16_to_cpup((void *)*pbeaconinfo);
+		beaconsize = le16_to_cpu(get_unaligned((u16 *)*pbeaconinfo));
 		*bytesleft -= sizeof(beaconsize);
 		*pbeaconinfo += sizeof(beaconsize);
 	}
 
 	if (beaconsize == 0 || beaconsize > *bytesleft) {
-
 		*pbeaconinfo += *bytesleft;
 		*bytesleft = 0;
-
-		return -1;
+		ret = -1;
+		goto done;
 	}
 
 	/* Initialize the current working beacon pointer for this BSS iteration */
-	pcurrentptr = *pbeaconinfo;
+	pos = *pbeaconinfo;
+	end = pos + beaconsize;
 
 	/* Advance the return beacon pointer past the current beacon */
 	*pbeaconinfo += beaconsize;
 	*bytesleft -= beaconsize;
 
-	bytesleftforcurrentbeacon = beaconsize;
-
-	memcpy(bss->bssid, pcurrentptr, ETH_ALEN);
-	lbs_deb_scan("process_bss: AP BSSID " MAC_FMT "\n", MAC_ARG(bss->bssid));
+	memcpy(bss->bssid, pos, ETH_ALEN);
+	lbs_deb_scan("process_bss: AP BSSID %s\n", print_mac(mac, bss->bssid));
+	pos += ETH_ALEN;
 
-	pcurrentptr += ETH_ALEN;
-	bytesleftforcurrentbeacon -= ETH_ALEN;
-
-	if (bytesleftforcurrentbeacon < 12) {
+	if ((end - pos) < 12) {
 		lbs_deb_scan("process_bss: Not enough bytes left\n");
-		return -1;
+		ret = -1;
+		goto done;
 	}
 
 	/*
@@ -968,85 +982,61 @@ static int libertas_process_bss(struct bss_descriptor * bss,
 	 */
 
 	/* RSSI is 1 byte long */
-	bss->rssi = *pcurrentptr;
-	lbs_deb_scan("process_bss: RSSI=%02X\n", *pcurrentptr);
-	pcurrentptr += 1;
-	bytesleftforcurrentbeacon -= 1;
+	bss->rssi = *pos;
+	lbs_deb_scan("process_bss: RSSI=%02X\n", *pos);
+	pos++;
 
 	/* time stamp is 8 bytes long */
-	fixedie.timestamp = bss->timestamp = le64_to_cpup((void *)pcurrentptr);
-	pcurrentptr += 8;
-	bytesleftforcurrentbeacon -= 8;
+	pos += 8;
 
 	/* beacon interval is 2 bytes long */
-	fixedie.beaconinterval = bss->beaconperiod = le16_to_cpup((void *)pcurrentptr);
-	pcurrentptr += 2;
-	bytesleftforcurrentbeacon -= 2;
+	bss->beaconperiod = le16_to_cpup((void *) pos);
+	pos += 2;
 
 	/* capability information is 2 bytes long */
-        memcpy(&fixedie.capabilities, pcurrentptr, 2);
-	lbs_deb_scan("process_bss: fixedie.capabilities=0x%X\n",
-	       fixedie.capabilities);
-	pcap = (struct ieeetypes_capinfo *) & fixedie.capabilities;
-	memcpy(&bss->cap, pcap, sizeof(struct ieeetypes_capinfo));
-	pcurrentptr += 2;
-	bytesleftforcurrentbeacon -= 2;
-
-	/* rest of the current buffer are IE's */
-	lbs_deb_scan("process_bss: IE length for this AP = %d\n",
-	       bytesleftforcurrentbeacon);
+	bss->capability = le16_to_cpup((void *) pos);
+	lbs_deb_scan("process_bss: capabilities = 0x%4X\n", bss->capability);
+	pos += 2;
 
-	lbs_dbg_hex("process_bss: IE info", (u8 *) pcurrentptr,
-		bytesleftforcurrentbeacon);
-
-	if (pcap->privacy) {
+	if (bss->capability & WLAN_CAPABILITY_PRIVACY)
 		lbs_deb_scan("process_bss: AP WEP enabled\n");
-		bss->privacy = wlan802_11privfilter8021xWEP;
-	} else {
-		bss->privacy = wlan802_11privfilteracceptall;
-	}
-
-	if (pcap->ibss == 1) {
+	if (bss->capability & WLAN_CAPABILITY_IBSS)
 		bss->mode = IW_MODE_ADHOC;
-	} else {
+	else
 		bss->mode = IW_MODE_INFRA;
-	}
+
+	/* rest of the current buffer are IE's */
+	lbs_deb_scan("process_bss: IE length for this AP = %zd\n", end - pos);
+	lbs_deb_hex(LBS_DEB_SCAN, "process_bss: IE info", pos, end - pos);
 
 	/* process variable IE */
-	while (bytesleftforcurrentbeacon >= 2) {
-		elemID = (enum ieeetypes_elementid) (*((u8 *) pcurrentptr));
-		elemlen = *((u8 *) pcurrentptr + 1);
+	while (pos <= end - 2) {
+		struct ieee80211_info_element * elem =
+			(struct ieee80211_info_element *) pos;
 
-		if (bytesleftforcurrentbeacon < elemlen) {
+		if (pos + elem->len > end) {
 			lbs_deb_scan("process_bss: error in processing IE, "
 			       "bytes left < IE length\n");
-			bytesleftforcurrentbeacon = 0;
-			continue;
+			break;
 		}
 
-		switch (elemID) {
-		case SSID:
-			bss->ssid_len = elemlen;
-			memcpy(bss->ssid, (pcurrentptr + 2), elemlen);
+		switch (elem->id) {
+		case MFIE_TYPE_SSID:
+			bss->ssid_len = elem->len;
+			memcpy(bss->ssid, elem->data, elem->len);
 			lbs_deb_scan("ssid '%s', ssid length %u\n",
 			             escape_essid(bss->ssid, bss->ssid_len),
 			             bss->ssid_len);
 			break;
 
-		case SUPPORTED_RATES:
-			memcpy(bss->datarates, (pcurrentptr + 2), elemlen);
-			memmove(bss->libertas_supported_rates, (pcurrentptr + 2),
-				elemlen);
-			ratesize = elemlen;
-			founddatarateie = 1;
+		case MFIE_TYPE_RATES:
+			n_basic_rates = min_t(u8, MAX_RATES, elem->len);
+			memcpy(bss->rates, elem->data, n_basic_rates);
+			got_basic_rates = 1;
 			break;
 
-		case EXTRA_IE:
-			lbs_deb_scan("process_bss: EXTRA_IE Found!\n");
-			break;
-
-		case FH_PARAM_SET:
-			pFH = (struct ieeetypes_fhparamset *) pcurrentptr;
+		case MFIE_TYPE_FH_SET:
+			pFH = (struct ieeetypes_fhparamset *) pos;
 			memmove(&bss->phyparamset.fhparamset, pFH,
 				sizeof(struct ieeetypes_fhparamset));
 #if 0 /* I think we can store these LE */
@@ -1055,21 +1045,21 @@ static int libertas_process_bss(struct bss_descriptor * bss,
 #endif
 			break;
 
-		case DS_PARAM_SET:
-			pDS = (struct ieeetypes_dsparamset *) pcurrentptr;
+		case MFIE_TYPE_DS_SET:
+			pDS = (struct ieeetypes_dsparamset *) pos;
 			bss->channel = pDS->currentchan;
 			memcpy(&bss->phyparamset.dsparamset, pDS,
 			       sizeof(struct ieeetypes_dsparamset));
 			break;
 
-		case CF_PARAM_SET:
-			pCF = (struct ieeetypes_cfparamset *) pcurrentptr;
+		case MFIE_TYPE_CF_SET:
+			pCF = (struct ieeetypes_cfparamset *) pos;
 			memcpy(&bss->ssparamset.cfparamset, pCF,
 			       sizeof(struct ieeetypes_cfparamset));
 			break;
 
-		case IBSS_PARAM_SET:
-			pibss = (struct ieeetypes_ibssparamset *) pcurrentptr;
+		case MFIE_TYPE_IBSS_SET:
+			pibss = (struct ieeetypes_ibssparamset *) pos;
 			bss->atimwindow = le32_to_cpu(pibss->atimwindow);
 			memmove(&bss->ssparamset.ibssparamset, pibss,
 				sizeof(struct ieeetypes_ibssparamset));
@@ -1079,9 +1069,8 @@ static int libertas_process_bss(struct bss_descriptor * bss,
 #endif
 			break;
 
-			/* Handle Country Info IE */
-		case COUNTRY_INFO:
-			pcountryinfo = (struct ieeetypes_countryinfoset *) pcurrentptr;
+		case MFIE_TYPE_COUNTRY:
+			pcountryinfo = (struct ieeetypes_countryinfoset *) pos;
 			if (pcountryinfo->len < sizeof(pcountryinfo->countrycode)
 			    || pcountryinfo->len > 254) {
 				lbs_deb_scan("process_bss: 11D- Err "
@@ -1094,70 +1083,63 @@ static int libertas_process_bss(struct bss_descriptor * bss,
 
 			memcpy(&bss->countryinfo,
 			       pcountryinfo, pcountryinfo->len + 2);
-			lbs_dbg_hex("process_bss: 11D- CountryInfo:",
+			lbs_deb_hex(LBS_DEB_SCAN, "process_bss: 11d countryinfo",
 				(u8 *) pcountryinfo,
 				(u32) (pcountryinfo->len + 2));
 			break;
 
-		case EXTENDED_SUPPORTED_RATES:
-			/*
-			 * only process extended supported rate
-			 * if data rate is already found.
-			 * data rate IE should come before
+		case MFIE_TYPE_RATES_EX:
+			/* only process extended supported rate if data rate is
+			 * already found. Data rate IE should come before
 			 * extended supported rate IE
 			 */
-			if (founddatarateie) {
-				if ((elemlen + ratesize) > WLAN_SUPPORTED_RATES) {
-					bytestocopy =
-					    (WLAN_SUPPORTED_RATES - ratesize);
-				} else {
-					bytestocopy = elemlen;
-				}
-
-				pRate = (u8 *) bss->datarates;
-				pRate += ratesize;
-				memmove(pRate, (pcurrentptr + 2), bytestocopy);
-				pRate = (u8 *) bss->libertas_supported_rates;
-				pRate += ratesize;
-				memmove(pRate, (pcurrentptr + 2), bytestocopy);
-			}
-			break;
-
-		case VENDOR_SPECIFIC_221:
-#define IE_ID_LEN_FIELDS_BYTES 2
-			pIe = (struct IE_WPA *)pcurrentptr;
-
-			if (memcmp(pIe->oui, oui01, sizeof(oui01)))
+			if (!got_basic_rates)
 				break;
 
-			bss->wpa_ie_len = min(elemlen + IE_ID_LEN_FIELDS_BYTES,
-				MAX_WPA_IE_LEN);
-			memcpy(bss->wpa_ie, pcurrentptr, bss->wpa_ie_len);
-			lbs_dbg_hex("process_bss: WPA IE", bss->wpa_ie, elemlen);
+			n_ex_rates = elem->len;
+			if (n_basic_rates + n_ex_rates > MAX_RATES)
+				n_ex_rates = MAX_RATES - n_basic_rates;
+
+			p = bss->rates + n_basic_rates;
+			memcpy(p, elem->data, n_ex_rates);
 			break;
-		case WPA2_IE:
-			pIe = (struct IE_WPA *)pcurrentptr;
-			bss->rsn_ie_len = min(elemlen + IE_ID_LEN_FIELDS_BYTES,
-				MAX_WPA_IE_LEN);
-			memcpy(bss->rsn_ie, pcurrentptr, bss->rsn_ie_len);
-			lbs_dbg_hex("process_bss: RSN_IE", bss->rsn_ie, elemlen);
+
+		case MFIE_TYPE_GENERIC:
+			if (elem->len >= 4 &&
+			    elem->data[0] == 0x00 &&
+			    elem->data[1] == 0x50 &&
+			    elem->data[2] == 0xf2 &&
+			    elem->data[3] == 0x01) {
+				bss->wpa_ie_len = min(elem->len + 2,
+				                      MAX_WPA_IE_LEN);
+				memcpy(bss->wpa_ie, elem, bss->wpa_ie_len);
+				lbs_deb_hex(LBS_DEB_SCAN, "process_bss: WPA IE", bss->wpa_ie,
+				            elem->len);
+			} else if (elem->len >= MARVELL_MESH_IE_LENGTH &&
+			    elem->data[0] == 0x00 &&
+			    elem->data[1] == 0x50 &&
+			    elem->data[2] == 0x43 &&
+			    elem->data[3] == 0x04) {
+				bss->mesh = 1;
+			}
 			break;
-		case TIM:
+
+		case MFIE_TYPE_RSN:
+			bss->rsn_ie_len = min(elem->len + 2, MAX_WPA_IE_LEN);
+			memcpy(bss->rsn_ie, elem, bss->rsn_ie_len);
+			lbs_deb_hex(LBS_DEB_SCAN, "process_bss: RSN_IE", bss->rsn_ie, elem->len);
 			break;
 
-		case CHALLENGE_TEXT:
+		default:
 			break;
 		}
 
-		pcurrentptr += elemlen + 2;
-
-		/* need to account for IE ID and IE len */
-		bytesleftforcurrentbeacon -= (elemlen + 2);
-
-	}			/* while (bytesleftforcurrentbeacon > 2) */
+		pos += elem->len + 2;
+	}
 
 	/* Timestamp */
 	bss->last_scanned = jiffies;
+	libertas_unset_basic_rate_flags(bss->rates, sizeof(bss->rates));
 
 	ret = 0;
 
@@ -1167,40 +1149,28 @@ done:
 }
 
 /**
- *  @brief Compare two SSIDs
- *
- *  @param ssid1    A pointer to ssid to compare
- *  @param ssid2    A pointer to ssid to compare
- *
- *  @return         0--ssid is same, otherwise is different
- */
-int libertas_ssid_cmp(u8 *ssid1, u8 ssid1_len, u8 *ssid2, u8 ssid2_len)
-{
-	if (ssid1_len != ssid2_len)
-		return -1;
-
-	return memcmp(ssid1, ssid2, ssid1_len);
-}
-
-/**
  *  @brief This function finds a specific compatible BSSID in the scan list
  *
+ *  Used in association code
+ *
  *  @param adapter  A pointer to wlan_adapter
  *  @param bssid    BSSID to find in the scan list
  *  @param mode     Network mode: Infrastructure or IBSS
  *
  *  @return         index in BSSID list, or error return code (< 0)
  */
-struct bss_descriptor * libertas_find_bssid_in_list(wlan_adapter * adapter,
+struct bss_descriptor *libertas_find_bssid_in_list(wlan_adapter * adapter,
 		u8 * bssid, u8 mode)
 {
 	struct bss_descriptor * iter_bss;
 	struct bss_descriptor * found_bss = NULL;
 
+	lbs_deb_enter(LBS_DEB_SCAN);
+
 	if (!bssid)
-		return NULL;
+		goto out;
 
-	lbs_dbg_hex("libertas_find_BSSID_in_list: looking for ",
+	lbs_deb_hex(LBS_DEB_SCAN, "looking for",
 		bssid, ETH_ALEN);
 
 	/* Look through the scan table for a compatible match.  The loop will
@@ -1225,12 +1195,16 @@ struct bss_descriptor * libertas_find_bssid_in_list(wlan_adapter * adapter,
 	}
 	mutex_unlock(&adapter->lock);
 
+out:
+	lbs_deb_leave_args(LBS_DEB_SCAN, "found_bss %p", found_bss);
 	return found_bss;
 }
 
 /**
  *  @brief This function finds ssid in ssid list.
  *
+ *  Used in association code
+ *
  *  @param adapter  A pointer to wlan_adapter
  *  @param ssid     SSID to find in the list
  *  @param bssid    BSSID to qualify the SSID selection (if provided)
@@ -1247,6 +1221,8 @@ struct bss_descriptor * libertas_find_ssid_in_list(wlan_adapter * adapter,
 	struct bss_descriptor * found_bss = NULL;
 	struct bss_descriptor * tmp_oldest = NULL;
 
+	lbs_deb_enter(LBS_DEB_SCAN);
+
 	mutex_lock(&adapter->lock);
 
 	list_for_each_entry (iter_bss, &adapter->network_list, list) {
@@ -1291,6 +1267,7 @@ struct bss_descriptor * libertas_find_ssid_in_list(wlan_adapter * adapter,
 
 out:
 	mutex_unlock(&adapter->lock);
+	lbs_deb_leave_args(LBS_DEB_SCAN, "found_bss %p", found_bss);
 	return found_bss;
 }
 
@@ -1304,13 +1281,15 @@ out:
  *
  *  @return         index in BSSID list
  */
-struct bss_descriptor * libertas_find_best_ssid_in_list(wlan_adapter * adapter,
+static struct bss_descriptor * libertas_find_best_ssid_in_list(wlan_adapter * adapter,
 		u8 mode)
 {
 	u8 bestrssi = 0;
 	struct bss_descriptor * iter_bss;
 	struct bss_descriptor * best_bss = NULL;
 
+	lbs_deb_enter(LBS_DEB_SCAN);
+
 	mutex_lock(&adapter->lock);
 
 	list_for_each_entry (iter_bss, &adapter->network_list, list) {
@@ -1335,12 +1314,15 @@ struct bss_descriptor * libertas_find_best_ssid_in_list(wlan_adapter * adapter,
 	}
 
 	mutex_unlock(&adapter->lock);
+	lbs_deb_leave_args(LBS_DEB_SCAN, "best_bss %p", best_bss);
 	return best_bss;
 }
 
 /**
  *  @brief Find the AP with specific ssid in the scan list
  *
+ *  Used from association worker.
+ *
  *  @param priv         A pointer to wlan_private structure
  *  @param pSSID        A pointer to AP's ssid
  *
@@ -1353,11 +1335,11 @@ int libertas_find_best_network_ssid(wlan_private * priv,
 	int ret = -1;
 	struct bss_descriptor * found;
 
-	lbs_deb_enter(LBS_DEB_ASSOC);
+	lbs_deb_enter(LBS_DEB_SCAN);
 
 	wlan_scan_networks(priv, NULL, 1);
 	if (adapter->surpriseremoved)
-		return -1;
+		goto out;
 
 	wait_event_interruptible(adapter->cmd_pending, !adapter->nr_cmd_pending);
 
@@ -1369,6 +1351,7 @@ int libertas_find_best_network_ssid(wlan_private * priv,
 		ret = 0;
 	}
 
+out:
 	lbs_deb_leave_args(LBS_DEB_SCAN, "ret %d", ret);
 	return ret;
 }
@@ -1391,7 +1374,10 @@ int libertas_set_scan(struct net_device *dev, struct iw_request_info *info,
 
 	lbs_deb_enter(LBS_DEB_SCAN);
 
-	wlan_scan_networks(priv, NULL, 0);
+	if (!delayed_work_pending(&priv->scan_work)) {
+		queue_delayed_work(priv->work_thread, &priv->scan_work,
+		                   msecs_to_jiffies(50));
+	}
 
 	if (adapter->surpriseremoved)
 		return -1;
@@ -1400,10 +1386,17 @@ int libertas_set_scan(struct net_device *dev, struct iw_request_info *info,
 	return 0;
 }
 
+
 /**
  *  @brief Send a scan command for all available channels filtered on a spec
  *
+ *  Used in association code and from debugfs
+ *
  *  @param priv             A pointer to wlan_private structure
+ *  @param ssid             A pointer to the SSID to scan for
+ *  @param ssid_len         Length of the SSID
+ *  @param clear_ssid       Should existing scan results with this SSID
+ *                          be cleared?
  *  @param prequestedssid   A pointer to AP's ssid
  *  @param keeppreviousscan Flag used to save/clear scan table before scan
  *
@@ -1416,7 +1409,8 @@ int libertas_send_specific_ssid_scan(wlan_private * priv,
 	struct wlan_ioctl_user_scan_cfg scancfg;
 	int ret = 0;
 
-	lbs_deb_enter(LBS_DEB_ASSOC);
+	lbs_deb_enter_args(LBS_DEB_SCAN, "SSID '%s', clear %d",
+		escape_essid(ssid, ssid_len), clear_ssid);
 
 	if (!ssid_len)
 		goto out;
@@ -1427,47 +1421,27 @@ int libertas_send_specific_ssid_scan(wlan_private * priv,
 	scancfg.clear_ssid = clear_ssid;
 
 	wlan_scan_networks(priv, &scancfg, 1);
-	if (adapter->surpriseremoved)
-		return -1;
+	if (adapter->surpriseremoved) {
+		ret = -1;
+		goto out;
+	}
 	wait_event_interruptible(adapter->cmd_pending, !adapter->nr_cmd_pending);
 
 out:
-	lbs_deb_leave(LBS_DEB_ASSOC);
+	lbs_deb_leave_args(LBS_DEB_SCAN, "ret %d", ret);
 	return ret;
 }
 
-/**
- *  @brief scan an AP with specific BSSID
- *
- *  @param priv             A pointer to wlan_private structure
- *  @param bssid            A pointer to AP's bssid
- *  @param keeppreviousscan Flag used to save/clear scan table before scan
- *
- *  @return          0-success, otherwise fail
- */
-int libertas_send_specific_bssid_scan(wlan_private * priv, u8 * bssid, u8 clear_bssid)
-{
-	struct wlan_ioctl_user_scan_cfg scancfg;
-
-	lbs_deb_enter(LBS_DEB_ASSOC);
 
-	if (bssid == NULL)
-		goto out;
 
-	memset(&scancfg, 0x00, sizeof(scancfg));
-	memcpy(scancfg.bssid, bssid, ETH_ALEN);
-	scancfg.clear_bssid = clear_bssid;
 
-	wlan_scan_networks(priv, &scancfg, 1);
-	if (priv->adapter->surpriseremoved)
-		return -1;
-	wait_event_interruptible(priv->adapter->cmd_pending,
-		!priv->adapter->nr_cmd_pending);
+/*********************************************************************/
+/*                                                                   */
+/*  Support for Wireless Extensions                                  */
+/*                                                                   */
+/*********************************************************************/
 
-out:
-	lbs_deb_leave(LBS_DEB_ASSOC);
-	return 0;
-}
+#define MAX_CUSTOM_LEN 64
 
 static inline char *libertas_translate_scan(wlan_private *priv,
 					char *start, char *stop,
@@ -1483,10 +1457,13 @@ static inline char *libertas_translate_scan(wlan_private *priv,
 #define RSSI_DIFF    ((u8)(PERFECT_RSSI - WORST_RSSI))
 	u8 rssi;
 
+	lbs_deb_enter(LBS_DEB_SCAN);
+
 	cfp = libertas_find_cfp_by_band_and_channel(adapter, 0, bss->channel);
 	if (!cfp) {
 		lbs_deb_scan("Invalid channel number %d\n", bss->channel);
-		return NULL;
+		start = NULL;
+		goto out;
 	}
 
 	/* First entry *MUST* be the AP BSSID */
@@ -1550,7 +1527,7 @@ static inline char *libertas_translate_scan(wlan_private *priv,
 
 	/* Add encryption capability */
 	iwe.cmd = SIOCGIWENCODE;
-	if (bss->privacy) {
+	if (bss->capability & WLAN_CAPABILITY_PRIVACY) {
 		iwe.u.data.flags = IW_ENCODE_ENABLED | IW_ENCODE_NOKEY;
 	} else {
 		iwe.u.data.flags = IW_ENCODE_DISABLED;
@@ -1565,12 +1542,9 @@ static inline char *libertas_translate_scan(wlan_private *priv,
 	iwe.u.bitrate.disabled = 0;
 	iwe.u.bitrate.value = 0;
 
-	for (j = 0; j < sizeof(bss->libertas_supported_rates); j++) {
-		u8 rate = bss->libertas_supported_rates[j];
-		if (rate == 0)
-			break; /* no more rates */
-		/* Bit rate given in 500 kb/s units (+ 0x80) */
-		iwe.u.bitrate.value = (rate & 0x7f) * 500000;
+	for (j = 0; bss->rates[j] && (j < sizeof(bss->rates)); j++) {
+		/* Bit rate given in 500 kb/s units */
+		iwe.u.bitrate.value = bss->rates[j] * 500000;
 		current_val = iwe_stream_add_value(start, current_val,
 					 stop, &iwe, IW_EV_PARAM_LEN);
 	}
@@ -1605,11 +1579,25 @@ static inline char *libertas_translate_scan(wlan_private *priv,
 		start = iwe_stream_add_point(start, stop, &iwe, buf);
 	}
 
+	if (bss->mesh) {
+		char custom[MAX_CUSTOM_LEN];
+		char *p = custom;
+
+		iwe.cmd = IWEVCUSTOM;
+		p += snprintf(p, MAX_CUSTOM_LEN - (p - custom),
+		              "mesh-type: olpc");
+		iwe.u.data.length = p - custom;
+		if (iwe.u.data.length)
+			start = iwe_stream_add_point(start, stop, &iwe, custom);
+	}
+
+out:
+	lbs_deb_leave_args(LBS_DEB_SCAN, "start %p", start);
 	return start;
 }
 
 /**
- *  @brief  Retrieve the scan table entries via wireless tools IOCTL call
+ *  @brief  Handle Retrieve scan table ioctl
  *
  *  @param dev          A pointer to net_device structure
  *  @param info         A pointer to iw_request_info structure
@@ -1630,16 +1618,12 @@ int libertas_get_scan(struct net_device *dev, struct iw_request_info *info,
 	struct bss_descriptor * iter_bss;
 	struct bss_descriptor * safe;
 
-	lbs_deb_enter(LBS_DEB_ASSOC);
-
-	/* If we've got an uncompleted scan, schedule the next part */
-	if (!adapter->nr_cmd_pending && adapter->last_scanned_channel)
-		wlan_scan_networks(priv, NULL, 0);
+	lbs_deb_enter(LBS_DEB_SCAN);
 
 	/* Update RSSI if current BSS is a locally created ad-hoc BSS */
 	if ((adapter->mode == IW_MODE_ADHOC) && adapter->adhoccreate) {
-		libertas_prepare_and_send_command(priv, cmd_802_11_rssi, 0,
-					cmd_option_waitforrsp, 0, NULL);
+		libertas_prepare_and_send_command(priv, CMD_802_11_RSSI, 0,
+					CMD_OPTION_WAITFORRSP, 0, NULL);
 	}
 
 	mutex_lock(&adapter->lock);
@@ -1652,6 +1636,10 @@ int libertas_get_scan(struct net_device *dev, struct iw_request_info *info,
 			break;
 		}
 
+		/* For mesh device, list only mesh networks */
+		if (dev == priv->mesh_dev && !iter_bss->mesh)
+			continue;
+
 		/* Prune old an old scan result */
 		stale_time = iter_bss->last_scanned + DEFAULT_MAX_SCAN_AGE;
 		if (time_after(jiffies, stale_time)) {
@@ -1672,19 +1660,27 @@ int libertas_get_scan(struct net_device *dev, struct iw_request_info *info,
 	dwrq->length = (ev - extra);
 	dwrq->flags = 0;
 
-	lbs_deb_leave(LBS_DEB_ASSOC);
+	lbs_deb_leave_args(LBS_DEB_SCAN, "ret %d", err);
 	return err;
 }
 
+
+
+
+/*********************************************************************/
+/*                                                                   */
+/*  Command execution                                                */
+/*                                                                   */
+/*********************************************************************/
+
+
 /**
  *  @brief Prepare a scan command to be sent to the firmware
  *
- *  Use the wlan_scan_cmd_config sent to the command processing module in
- *   the libertas_prepare_and_send_command to configure a cmd_ds_802_11_scan command
- *   struct to send to firmware.
+ *  Called from libertas_prepare_and_send_command() in cmd.c
  *
- *  The fixed fields specifying the BSS type and BSSID filters as well as a
- *   variable number/length of TLVs are sent in the command to firmware.
+ *  Sends a fixed lenght data part (specifying the BSS type and BSSID filters)
+ *  as well as a variable number/length of TLVs to the firmware.
  *
  *  @param priv       A pointer to wlan_private structure
  *  @param cmd        A pointer to cmd_ds_command structure to be sent to
@@ -1693,36 +1689,31 @@ int libertas_get_scan(struct net_device *dev, struct iw_request_info *info,
  *                    to set the fields/TLVs for the command sent to firmware
  *
  *  @return           0 or -1
- *
- *  @sa wlan_scan_create_channel_list
  */
 int libertas_cmd_80211_scan(wlan_private * priv,
 			 struct cmd_ds_command *cmd, void *pdata_buf)
 {
 	struct cmd_ds_802_11_scan *pscan = &cmd->params.scan;
-	struct wlan_scan_cmd_config *pscancfg;
+	struct wlan_scan_cmd_config *pscancfg = pdata_buf;
 
-	lbs_deb_enter(LBS_DEB_ASSOC);
-
-	pscancfg = pdata_buf;
+	lbs_deb_enter(LBS_DEB_SCAN);
 
 	/* Set fixed field variables in scan command */
 	pscan->bsstype = pscancfg->bsstype;
-	memcpy(pscan->BSSID, pscancfg->bssid, sizeof(pscan->BSSID));
+	memcpy(pscan->bssid, pscancfg->bssid, ETH_ALEN);
 	memcpy(pscan->tlvbuffer, pscancfg->tlvbuffer, pscancfg->tlvbufferlen);
 
-	cmd->command = cpu_to_le16(cmd_802_11_scan);
+	cmd->command = cpu_to_le16(CMD_802_11_SCAN);
 
 	/* size is equal to the sizeof(fixed portions) + the TLV len + header */
-	cmd->size = cpu_to_le16(sizeof(pscan->bsstype)
-				     + sizeof(pscan->BSSID)
-				     + pscancfg->tlvbufferlen + S_DS_GEN);
+	cmd->size = cpu_to_le16(sizeof(pscan->bsstype) + ETH_ALEN
+				+ pscancfg->tlvbufferlen + S_DS_GEN);
 
-	lbs_deb_scan("SCAN_CMD: command=%x, size=%x, seqnum=%x\n",
+	lbs_deb_scan("SCAN_CMD: command 0x%04x, size %d, seqnum %d\n",
 		     le16_to_cpu(cmd->command), le16_to_cpu(cmd->size),
 		     le16_to_cpu(cmd->seqnum));
 
-	lbs_deb_leave(LBS_DEB_ASSOC);
+	lbs_deb_leave(LBS_DEB_SCAN);
 	return 0;
 }
 
@@ -1741,6 +1732,8 @@ static inline int is_same_network(struct bss_descriptor *src,
 /**
  *  @brief This function handles the command response of scan
  *
+ *  Called from handle_cmd_response() in cmdrespc.
+ *
  *   The response buffer for the scan command has the following
  *      memory layout:
  *
@@ -1766,8 +1759,6 @@ int libertas_ret_80211_scan(wlan_private * priv, struct cmd_ds_command *resp)
 {
 	wlan_adapter *adapter = priv->adapter;
 	struct cmd_ds_802_11_scan_rsp *pscan;
-	struct mrvlietypes_data *ptlv;
-	struct mrvlietypes_tsftimestamp *ptsftlv;
 	struct bss_descriptor * iter_bss;
 	struct bss_descriptor * safe;
 	u8 *pbssinfo;
@@ -1777,7 +1768,7 @@ int libertas_ret_80211_scan(wlan_private * priv, struct cmd_ds_command *resp)
 	int tlvbufsize;
 	int ret;
 
-	lbs_deb_enter(LBS_DEB_ASSOC);
+	lbs_deb_enter(LBS_DEB_SCAN);
 
 	/* Prune old entries from scan table */
 	list_for_each_entry_safe (iter_bss, safe, &adapter->network_list, list) {
@@ -1798,10 +1789,10 @@ int libertas_ret_80211_scan(wlan_private * priv, struct cmd_ds_command *resp)
 		goto done;
 	}
 
-	bytesleft = le16_to_cpu(pscan->bssdescriptsize);
+	bytesleft = le16_to_cpu(get_unaligned((u16*)&pscan->bssdescriptsize));
 	lbs_deb_scan("SCAN_RESP: bssdescriptsize %d\n", bytesleft);
 
-	scanrespsize = le16_to_cpu(resp->size);
+	scanrespsize = le16_to_cpu(get_unaligned((u16*)&resp->size));
 	lbs_deb_scan("SCAN_RESP: returned %d AP before parsing\n",
 	       pscan->nr_sets);
 
@@ -1816,11 +1807,6 @@ int libertas_ret_80211_scan(wlan_private * priv, struct cmd_ds_command *resp)
 				     + sizeof(pscan->nr_sets)
 				     + S_DS_GEN);
 
-	ptlv = (struct mrvlietypes_data *) (pscan->bssdesc_and_tlvbuffer + bytesleft);
-
-	/* Search the TLV buffer space in the scan response for any valid TLVs */
-	wlan_ret_802_11_scan_get_tlv_ptrs(ptlv, tlvbufsize, &ptsftlv);
-
 	/*
 	 *  Process each scan response returned (pscan->nr_sets).  Save
 	 *    the information in the newbssentry and then insert into the
@@ -1831,6 +1817,7 @@ int libertas_ret_80211_scan(wlan_private * priv, struct cmd_ds_command *resp)
 		struct bss_descriptor new;
 		struct bss_descriptor * found = NULL;
 		struct bss_descriptor * oldest = NULL;
+		DECLARE_MAC_BUF(mac);
 
 		/* Process the data fields and IEs returned for this BSS */
 		memset(&new, 0, sizeof (struct bss_descriptor));
@@ -1869,19 +1856,8 @@ int libertas_ret_80211_scan(wlan_private * priv, struct cmd_ds_command *resp)
 			continue;
 		}
 
-		lbs_deb_scan("SCAN_RESP: BSSID = " MAC_FMT "\n",
-		       new.bssid[0], new.bssid[1], new.bssid[2],
-		       new.bssid[3], new.bssid[4], new.bssid[5]);
-
-		/*
-		 * If the TSF TLV was appended to the scan results, save the
-		 *   this entries TSF value in the networktsf field.  The
-		 *   networktsf is the firmware's TSF value at the time the
-		 *   beacon or probe response was received.
-		 */
-		if (ptsftlv) {
-			new.networktsf = le64_to_cpup(&ptsftlv->tsftable[idx]);
-		}
+		lbs_deb_scan("SCAN_RESP: BSSID = %s\n",
+			     print_mac(mac, new.bssid));
 
 		/* Copy the locally created newbssentry to the scan table */
 		memcpy(found, &new, offsetof(struct bss_descriptor, list));
diff --git a/drivers/net/wireless/libertas/scan.h b/drivers/net/wireless/libertas/scan.h
index bd019e5ff1eb..c29c031bef8c 100644
--- a/drivers/net/wireless/libertas/scan.h
+++ b/drivers/net/wireless/libertas/scan.h
@@ -140,8 +140,7 @@ struct bss_descriptor {
 	u8 ssid[IW_ESSID_MAX_SIZE + 1];
 	u8 ssid_len;
 
-	/* WEP encryption requirement */
-	u32 privacy;
+	u16 capability;
 
 	/* receive signal strength in dBm */
 	long rssi;
@@ -152,18 +151,16 @@ struct bss_descriptor {
 
 	u32 atimwindow;
 
+	/* IW_MODE_AUTO, IW_MODE_ADHOC, IW_MODE_INFRA */
 	u8 mode;
-	u8 libertas_supported_rates[WLAN_SUPPORTED_RATES];
 
-	__le64 timestamp;	//!< TSF value included in the beacon/probe response
+	/* zero-terminated array of supported data rates */
+	u8 rates[MAX_RATES + 1];
+
 	unsigned long last_scanned;
 
 	union ieeetypes_phyparamset phyparamset;
 	union IEEEtypes_ssparamset ssparamset;
-	struct ieeetypes_capinfo cap;
-	u8 datarates[WLAN_SUPPORTED_RATES];
-
-	u64 networktsf;		//!< TSF timestamp from the current firmware TSF
 
 	struct ieeetypes_countryinfofullset countryinfo;
 
@@ -172,34 +169,31 @@ struct bss_descriptor {
 	u8 rsn_ie[MAX_WPA_IE_LEN];
 	size_t rsn_ie_len;
 
+	u8 mesh;
+
 	struct list_head list;
 };
 
-extern int libertas_ssid_cmp(u8 *ssid1, u8 ssid1_len, u8 *ssid2, u8 ssid2_len);
+int libertas_ssid_cmp(u8 *ssid1, u8 ssid1_len, u8 *ssid2, u8 ssid2_len);
 
 struct bss_descriptor * libertas_find_ssid_in_list(wlan_adapter * adapter,
 			u8 *ssid, u8 ssid_len, u8 * bssid, u8 mode,
 			int channel);
 
-struct bss_descriptor * libertas_find_best_ssid_in_list(wlan_adapter * adapter,
-			u8 mode);
-
-extern struct bss_descriptor * libertas_find_bssid_in_list(wlan_adapter * adapter,
+struct bss_descriptor * libertas_find_bssid_in_list(wlan_adapter * adapter,
 			u8 * bssid, u8 mode);
 
 int libertas_find_best_network_ssid(wlan_private * priv, u8 *out_ssid,
 			u8 *out_ssid_len, u8 preferred_mode, u8 *out_mode);
 
-extern int libertas_send_specific_ssid_scan(wlan_private * priv, u8 *ssid,
+int libertas_send_specific_ssid_scan(wlan_private * priv, u8 *ssid,
 				u8 ssid_len, u8 clear_ssid);
-extern int libertas_send_specific_bssid_scan(wlan_private * priv,
-				 u8 * bssid, u8 clear_bssid);
 
-extern int libertas_cmd_80211_scan(wlan_private * priv,
+int libertas_cmd_80211_scan(wlan_private * priv,
 				struct cmd_ds_command *cmd,
 				void *pdata_buf);
 
-extern int libertas_ret_80211_scan(wlan_private * priv,
+int libertas_ret_80211_scan(wlan_private * priv,
 				struct cmd_ds_command *resp);
 
 int wlan_scan_networks(wlan_private * priv,
@@ -211,9 +205,11 @@ struct ifreq;
 struct iw_point;
 struct iw_param;
 struct iw_request_info;
-extern int libertas_get_scan(struct net_device *dev, struct iw_request_info *info,
+int libertas_get_scan(struct net_device *dev, struct iw_request_info *info,
 			 struct iw_point *dwrq, char *extra);
-extern int libertas_set_scan(struct net_device *dev, struct iw_request_info *info,
+int libertas_set_scan(struct net_device *dev, struct iw_request_info *info,
 			 struct iw_param *vwrq, char *extra);
 
+void libertas_scan_worker(struct work_struct *work);
+
 #endif				/* _WLAN_SCAN_H */
diff --git a/drivers/net/wireless/libertas/thread.h b/drivers/net/wireless/libertas/thread.h
deleted file mode 100644
index b1f34d92ff3e..000000000000
--- a/drivers/net/wireless/libertas/thread.h
+++ /dev/null
@@ -1,52 +0,0 @@
-#ifndef	__WLAN_THREAD_H_
-#define	__WLAN_THREAD_H_
-
-#include	<linux/kthread.h>
-
-struct wlan_thread {
-	struct task_struct *task;
-	wait_queue_head_t waitq;
-	pid_t pid;
-	void *priv;
-};
-
-static inline void wlan_activate_thread(struct wlan_thread * thr)
-{
-	/** Record the thread pid */
-	thr->pid = current->pid;
-
-	/** Initialize the wait queue */
-	init_waitqueue_head(&thr->waitq);
-}
-
-static inline void wlan_deactivate_thread(struct wlan_thread * thr)
-{
-	lbs_deb_enter(LBS_DEB_THREAD);
-
-	thr->pid = 0;
-
-	lbs_deb_leave(LBS_DEB_THREAD);
-}
-
-static inline void wlan_create_thread(int (*wlanfunc) (void *),
-				      struct wlan_thread * thr, char *name)
-{
-	thr->task = kthread_run(wlanfunc, thr, "%s", name);
-}
-
-static inline int wlan_terminate_thread(struct wlan_thread * thr)
-{
-	lbs_deb_enter(LBS_DEB_THREAD);
-
-	/* Check if the thread is active or not */
-	if (!thr->pid) {
-		printk(KERN_ERR "Thread does not exist\n");
-		return -1;
-	}
-	kthread_stop(thr->task);
-
-	lbs_deb_leave(LBS_DEB_THREAD);
-	return 0;
-}
-
-#endif
diff --git a/drivers/net/wireless/libertas/tx.c b/drivers/net/wireless/libertas/tx.c
index 17c437635a00..fbec06c10dd7 100644
--- a/drivers/net/wireless/libertas/tx.c
+++ b/drivers/net/wireless/libertas/tx.c
@@ -58,7 +58,6 @@ static u32 convert_radiotap_rate_to_mv(u8 rate)
  */
 static int SendSinglePacket(wlan_private * priv, struct sk_buff *skb)
 {
-	wlan_adapter *adapter = priv->adapter;
 	int ret = 0;
 	struct txpd localtxpd;
 	struct txpd *plocaltxpd = &localtxpd;
@@ -72,10 +71,6 @@ static int SendSinglePacket(wlan_private * priv, struct sk_buff *skb)
 	if (priv->adapter->surpriseremoved)
 		return -1;
 
-	if ((priv->adapter->debugmode & MRVDRV_DEBUG_TX_PATH) != 0)
-		lbs_dbg_hex("TX packet: ", skb->data,
-			 min_t(unsigned int, skb->len, 100));
-
 	if (!skb->len || (skb->len > MRVDRV_ETH_TX_PACKET_BUFFER_SIZE)) {
 		lbs_deb_tx("tx err: skb length %d 0 or > %zd\n",
 		       skb->len, MRVDRV_ETH_TX_PACKET_BUFFER_SIZE);
@@ -90,11 +85,8 @@ static int SendSinglePacket(wlan_private * priv, struct sk_buff *skb)
 	/* offset of actual data */
 	plocaltxpd->tx_packet_location = cpu_to_le32(sizeof(struct txpd));
 
-	/* TxCtrl set by user or default */
-	plocaltxpd->tx_control = cpu_to_le32(adapter->pkttxctrl);
-
 	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;
@@ -103,7 +95,6 @@ static int SendSinglePacket(wlan_private * priv, struct sk_buff *skb)
 		new_rate = convert_radiotap_rate_to_mv(pradiotap_hdr->rate);
 		if (new_rate != 0) {
 			/* use new tx_control[4:0] */
-			new_rate |= (adapter->pkttxctrl & ~0x1f);
 			plocaltxpd->tx_control = cpu_to_le32(new_rate);
 		}
 
@@ -115,12 +106,12 @@ 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);
 
-	lbs_dbg_hex("txpd", (u8 *) plocaltxpd, sizeof(struct txpd));
+	lbs_deb_hex(LBS_DEB_TX, "txpd", (u8 *) plocaltxpd, sizeof(struct txpd));
 
 	if (IS_MESH_FRAME(skb)) {
 		plocaltxpd->tx_control |= cpu_to_le32(TxPD_MESH_FRAME);
@@ -130,7 +121,7 @@ static int SendSinglePacket(wlan_private * priv, struct sk_buff *skb)
 
 	ptr += sizeof(struct txpd);
 
-	lbs_dbg_hex("Tx Data", (u8 *) p802x_hdr, le16_to_cpu(plocaltxpd->tx_packet_length));
+	lbs_deb_hex(LBS_DEB_TX, "Tx Data", (u8 *) p802x_hdr, le16_to_cpu(plocaltxpd->tx_packet_length));
 	memcpy(ptr, p802x_hdr, le16_to_cpu(plocaltxpd->tx_packet_length));
 	ret = priv->hw_host_to_card(priv, MVMS_DAT,
 				    priv->adapter->tmptxbuf,
@@ -153,13 +144,14 @@ 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);
 		/* stop processing outgoing pkts */
 		netif_stop_queue(priv->dev);
-		netif_stop_queue(priv->mesh_dev);
+		if (priv->mesh_dev)
+			netif_stop_queue(priv->mesh_dev);
 		/* freeze any packets already in our queues */
 		priv->adapter->TxLockFlag = 1;
 	} else {
@@ -198,10 +190,12 @@ static void wlan_tx_queue(wlan_private *priv, struct sk_buff *skb)
 	adapter->tx_queue_ps[adapter->tx_queue_idx++] = skb;
 	if (adapter->tx_queue_idx == NR_TX_QUEUE) {
 		netif_stop_queue(priv->dev);
-		netif_stop_queue(priv->mesh_dev);
+		if (priv->mesh_dev)
+			netif_stop_queue(priv->mesh_dev);
 	} else {
 		netif_start_queue(priv->dev);
-		netif_start_queue(priv->mesh_dev);
+		if (priv->mesh_dev)
+			netif_start_queue(priv->mesh_dev);
 	}
 
 	spin_unlock(&adapter->txqueue_lock);
@@ -219,7 +213,7 @@ int libertas_process_tx(wlan_private * priv, struct sk_buff *skb)
 	int ret = -1;
 
 	lbs_deb_enter(LBS_DEB_TX);
-	lbs_dbg_hex("TX Data", skb->data, min_t(unsigned int, skb->len, 100));
+	lbs_deb_hex(LBS_DEB_TX, "TX Data", skb->data, min_t(unsigned int, skb->len, 100));
 
 	if (priv->dnld_sent) {
 		lbs_pr_alert( "TX error: dnld_sent = %d, not sending\n",
@@ -258,16 +252,12 @@ 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;
 
 	radiotap_hdr = (struct tx_radiotap_hdr *)adapter->currenttxskb->data;
 
-	if ((adapter->debugmode & MRVDRV_DEBUG_TX_PATH) != 0)
-		lbs_dbg_hex("TX feedback: ", (u8 *) radiotap_hdr,
-			min_t(unsigned int, adapter->currenttxskb->len, 100));
-
 	txfail = (status >> 24);
 
 #if 0
@@ -283,9 +273,10 @@ void libertas_send_tx_feedback(wlan_private * priv)
 	libertas_upload_rx_packet(priv, adapter->currenttxskb);
 	adapter->currenttxskb = NULL;
 	priv->adapter->TxLockFlag = 0;
-	if (priv->adapter->connect_status == libertas_connected) {
+	if (priv->adapter->connect_status == LIBERTAS_CONNECTED) {
 		netif_wake_queue(priv->dev);
-		netif_wake_queue(priv->mesh_dev);
+		if (priv->mesh_dev)
+			netif_wake_queue(priv->mesh_dev);
 	}
 }
 EXPORT_SYMBOL_GPL(libertas_send_tx_feedback);
diff --git a/drivers/net/wireless/libertas/types.h b/drivers/net/wireless/libertas/types.h
index 028e2f3b53d6..a43a5f63c879 100644
--- a/drivers/net/wireless/libertas/types.h
+++ b/drivers/net/wireless/libertas/types.h
@@ -7,71 +7,6 @@
 #include <linux/if_ether.h>
 #include <asm/byteorder.h>
 
-/** IEEE type definitions  */
-enum ieeetypes_elementid {
-	SSID = 0,
-	SUPPORTED_RATES,
-	FH_PARAM_SET,
-	DS_PARAM_SET,
-	CF_PARAM_SET,
-	TIM,
-	IBSS_PARAM_SET,
-	COUNTRY_INFO = 7,
-
-	CHALLENGE_TEXT = 16,
-
-	EXTENDED_SUPPORTED_RATES = 50,
-
-	VENDOR_SPECIFIC_221 = 221,
-
-	WPA_IE = 221,
-	WPA2_IE = 48,
-
-	EXTRA_IE = 133,
-} __attribute__ ((packed));
-
-#ifdef __BIG_ENDIAN
-#define CAPINFO_MASK	(~(0xda00))
-#else
-#define CAPINFO_MASK	(~(0x00da))
-#endif
-
-struct ieeetypes_capinfo {
-#ifdef __BIG_ENDIAN_BITFIELD
-	u8 chanagility:1;
-	u8 pbcc:1;
-	u8 shortpreamble:1;
-	u8 privacy:1;
-	u8 cfpollrqst:1;
-	u8 cfpollable:1;
-	u8 ibss:1;
-	u8 ess:1;
-	u8 rsrvd1:2;
-	u8 dsssofdm:1;
-	u8 rsvrd2:1;
-	u8 apsd:1;
-	u8 shortslottime:1;
-	u8 rsrvd3:1;
-	u8 spectrummgmt:1;
-#else
-	u8 ess:1;
-	u8 ibss:1;
-	u8 cfpollable:1;
-	u8 cfpollrqst:1;
-	u8 privacy:1;
-	u8 shortpreamble:1;
-	u8 pbcc:1;
-	u8 chanagility:1;
-	u8 spectrummgmt:1;
-	u8 rsrvd3:1;
-	u8 shortslottime:1;
-	u8 apsd:1;
-	u8 rsvrd2:1;
-	u8 dsssofdm:1;
-	u8 rsrvd1:2;
-#endif
-} __attribute__ ((packed));
-
 struct ieeetypes_cfparamset {
 	u8 elementid;
 	u8 len;
@@ -114,7 +49,7 @@ union ieeetypes_phyparamset {
 } __attribute__ ((packed));
 
 struct ieeetypes_assocrsp {
-	struct ieeetypes_capinfo capability;
+	__le16 capability;
 	__le16 statuscode;
 	__le16 aid;
 	u8 iebuffer[1];
diff --git a/drivers/net/wireless/libertas/wext.c b/drivers/net/wireless/libertas/wext.c
index 2fcc3bf21081..c6f5aa3cb465 100644
--- a/drivers/net/wireless/libertas/wext.c
+++ b/drivers/net/wireless/libertas/wext.c
@@ -22,60 +22,6 @@
 
 
 /**
- * the rates supported by the card
- */
-static u8 libertas_wlan_data_rates[WLAN_SUPPORTED_RATES] =
-    { 0x02, 0x04, 0x0B, 0x16, 0x00, 0x0C, 0x12,
-      0x18, 0x24, 0x30, 0x48, 0x60, 0x6C, 0x00
-};
-
-/**
- *  @brief Convert mw value to dbm value
- *
- *  @param mw	   the value of mw
- *  @return 	   the value of dbm
- */
-static int mw_to_dbm(int mw)
-{
-	if (mw < 2)
-		return 0;
-	else if (mw < 3)
-		return 3;
-	else if (mw < 4)
-		return 5;
-	else if (mw < 6)
-		return 7;
-	else if (mw < 7)
-		return 8;
-	else if (mw < 8)
-		return 9;
-	else if (mw < 10)
-		return 10;
-	else if (mw < 13)
-		return 11;
-	else if (mw < 16)
-		return 12;
-	else if (mw < 20)
-		return 13;
-	else if (mw < 25)
-		return 14;
-	else if (mw < 32)
-		return 15;
-	else if (mw < 40)
-		return 16;
-	else if (mw < 50)
-		return 17;
-	else if (mw < 63)
-		return 18;
-	else if (mw < 79)
-		return 19;
-	else if (mw < 100)
-		return 20;
-	else
-		return 21;
-}
-
-/**
  *  @brief Find the channel frequency power info with specific channel
  *
  *  @param adapter 	A pointer to wlan_adapter structure
@@ -165,7 +111,7 @@ static struct chan_freq_power *find_cfp_by_band_and_freq(wlan_adapter * adapter,
  *  @option 			Radio Option
  *  @return 	   		0 --success, otherwise fail
  */
-int wlan_radio_ioctl(wlan_private * priv, u8 option)
+static int wlan_radio_ioctl(wlan_private * priv, u8 option)
 {
 	int ret = 0;
 	wlan_adapter *adapter = priv->adapter;
@@ -177,9 +123,9 @@ int wlan_radio_ioctl(wlan_private * priv, u8 option)
 		adapter->radioon = option;
 
 		ret = libertas_prepare_and_send_command(priv,
-					    cmd_802_11_radio_control,
-					    cmd_act_set,
-					    cmd_option_waitforrsp, 0, NULL);
+					    CMD_802_11_RADIO_CONTROL,
+					    CMD_ACT_SET,
+					    CMD_OPTION_WAITFORRSP, 0, NULL);
 	}
 
 	lbs_deb_leave_args(LBS_DEB_WEXT, "ret %d", ret);
@@ -187,84 +133,31 @@ int wlan_radio_ioctl(wlan_private * priv, u8 option)
 }
 
 /**
- *  @brief Copy rates
- *
- *  @param dest                 A pointer to Dest Buf
- *  @param src		        A pointer to Src Buf
- *  @param len                  The len of Src Buf
- *  @return 	   	        Number of rates copyed
- */
-static inline int copyrates(u8 * dest, int pos, u8 * src, int len)
-{
-	int i;
-
-	for (i = 0; i < len && src[i]; i++, pos++) {
-		if (pos >= sizeof(u8) * WLAN_SUPPORTED_RATES)
-			break;
-		dest[pos] = src[i];
-	}
-
-	return pos;
-}
-
-/**
- *  @brief Get active data rates
+ *  @brief Copy active data rates based on adapter mode and status
  *
  *  @param adapter              A pointer to wlan_adapter structure
  *  @param rate		        The buf to return the active rates
- *  @return 	   	        The number of rates
  */
-static int get_active_data_rates(wlan_adapter * adapter,
-				 u8* rates)
+static void copy_active_data_rates(wlan_adapter * adapter, u8 * rates)
 {
-	int k = 0;
-
 	lbs_deb_enter(LBS_DEB_WEXT);
 
-	if (adapter->connect_status != libertas_connected) {
-		if (adapter->mode == IW_MODE_INFRA) {
-			lbs_deb_wext("infra\n");
-			k = copyrates(rates, k, libertas_supported_rates,
-				      sizeof(libertas_supported_rates));
-		} else {
-			lbs_deb_wext("Adhoc G\n");
-			k = copyrates(rates, k, libertas_adhoc_rates_g,
-				      sizeof(libertas_adhoc_rates_g));
-		}
-	} else {
-		k = copyrates(rates, 0, adapter->curbssparams.datarates,
-			      adapter->curbssparams.numofrates);
-	}
+	if (adapter->connect_status != LIBERTAS_CONNECTED)
+		memcpy(rates, libertas_bg_rates, MAX_RATES);
+	else
+		memcpy(rates, adapter->curbssparams.rates, MAX_RATES);
 
-	lbs_deb_leave_args(LBS_DEB_WEXT, "ret %d", k);
-	return k;
+	lbs_deb_leave(LBS_DEB_WEXT);
 }
 
 static int wlan_get_name(struct net_device *dev, struct iw_request_info *info,
 			 char *cwrq, char *extra)
 {
-	const char *cp;
-	char comm[6] = { "COMM-" };
-	char mrvl[6] = { "MRVL-" };
-	int cnt;
 
 	lbs_deb_enter(LBS_DEB_WEXT);
 
-	strcpy(cwrq, mrvl);
-
-	cp = strstr(libertas_driver_version, comm);
-	if (cp == libertas_driver_version)	//skip leading "COMM-"
-		cp = libertas_driver_version + strlen(comm);
-	else
-		cp = libertas_driver_version;
-
-	cnt = strlen(mrvl);
-	cwrq += cnt;
-	while (cnt < 16 && (*cp != '-')) {
-		*cwrq++ = toupper(*cp++);
-		cnt++;
-	}
-	*cwrq = '\0';
+	/* We could add support for 802.11n here as needed. Jean II */
+	snprintf(cwrq, IFNAMSIZ, "IEEE 802.11b/g");
 
 	lbs_deb_leave(LBS_DEB_WEXT);
 	return 0;
@@ -305,7 +198,7 @@ static int wlan_get_wap(struct net_device *dev, struct iw_request_info *info,
 
 	lbs_deb_enter(LBS_DEB_WEXT);
 
-	if (adapter->connect_status == libertas_connected) {
+	if (adapter->connect_status == LIBERTAS_CONNECTED) {
 		memcpy(awrq->sa_data, adapter->curbssparams.bssid, ETH_ALEN);
 	} else {
 		memset(awrq->sa_data, 0, ETH_ALEN);
@@ -349,24 +242,11 @@ static int wlan_get_nick(struct net_device *dev, struct iw_request_info *info,
 
 	lbs_deb_enter(LBS_DEB_WEXT);
 
-	/*
-	 * Get the Nick Name saved
-	 */
-
-	mutex_lock(&adapter->lock);
-	strncpy(extra, adapter->nodename, 16);
-	mutex_unlock(&adapter->lock);
-
-	extra[16] = '\0';
+	dwrq->length = strlen(adapter->nodename);
+	memcpy(extra, adapter->nodename, dwrq->length);
+	extra[dwrq->length] = '\0';
 
-	/*
-	 * If none, we may want to get the one that was set
-	 */
-
-	/*
-	 * Push it out !
-	 */
-	dwrq->length = strlen(extra) + 1;
+	dwrq->flags = 1;	/* active */
 
 	lbs_deb_leave(LBS_DEB_WEXT);
 	return 0;
@@ -382,20 +262,21 @@ static int mesh_get_nick(struct net_device *dev, struct iw_request_info *info,
 
 	/* Use nickname to indicate that mesh is on */
 
-	if (adapter->connect_status == libertas_connected) {
+	if (adapter->connect_status == LIBERTAS_CONNECTED) {
 		strncpy(extra, "Mesh", 12);
 		extra[12] = '\0';
-		dwrq->length = strlen(extra) + 1;
+		dwrq->length = strlen(extra);
 	}
 
 	else {
 		extra[0] = '\0';
-		dwrq->length = 1 ;
+		dwrq->length = 0;
 	}
 
 	lbs_deb_leave(LBS_DEB_WEXT);
 	return 0;
 }
+
 static int wlan_set_rts(struct net_device *dev, struct iw_request_info *info,
 			struct iw_param *vwrq, char *extra)
 {
@@ -414,8 +295,8 @@ static int wlan_set_rts(struct net_device *dev, struct iw_request_info *info,
 		adapter->rtsthsd = rthr;
 	}
 
-	ret = libertas_prepare_and_send_command(priv, cmd_802_11_snmp_mib,
-				    cmd_act_set, cmd_option_waitforrsp,
+	ret = libertas_prepare_and_send_command(priv, CMD_802_11_SNMP_MIB,
+				    CMD_ACT_SET, CMD_OPTION_WAITFORRSP,
 				    OID_802_11_RTS_THRESHOLD, &rthr);
 
 	lbs_deb_leave_args(LBS_DEB_WEXT, "ret %d", ret);
@@ -432,8 +313,8 @@ static int wlan_get_rts(struct net_device *dev, struct iw_request_info *info,
 	lbs_deb_enter(LBS_DEB_WEXT);
 
 	adapter->rtsthsd = 0;
-	ret = libertas_prepare_and_send_command(priv, cmd_802_11_snmp_mib,
-				    cmd_act_get, cmd_option_waitforrsp,
+	ret = libertas_prepare_and_send_command(priv, CMD_802_11_SNMP_MIB,
+				    CMD_ACT_GET, CMD_OPTION_WAITFORRSP,
 				    OID_802_11_RTS_THRESHOLD, NULL);
 	if (ret)
 		goto out;
@@ -467,8 +348,8 @@ static int wlan_set_frag(struct net_device *dev, struct iw_request_info *info,
 		adapter->fragthsd = fthr;
 	}
 
-	ret = libertas_prepare_and_send_command(priv, cmd_802_11_snmp_mib,
-				    cmd_act_set, cmd_option_waitforrsp,
+	ret = libertas_prepare_and_send_command(priv, CMD_802_11_SNMP_MIB,
+				    CMD_ACT_SET, CMD_OPTION_WAITFORRSP,
 				    OID_802_11_FRAGMENTATION_THRESHOLD, &fthr);
 
 	lbs_deb_leave_args(LBS_DEB_WEXT, "ret %d", ret);
@@ -486,8 +367,8 @@ static int wlan_get_frag(struct net_device *dev, struct iw_request_info *info,
 
 	adapter->fragthsd = 0;
 	ret = libertas_prepare_and_send_command(priv,
-				    cmd_802_11_snmp_mib,
-				    cmd_act_get, cmd_option_waitforrsp,
+				    CMD_802_11_SNMP_MIB,
+				    CMD_ACT_GET, CMD_OPTION_WAITFORRSP,
 				    OID_802_11_FRAGMENTATION_THRESHOLD, NULL);
 	if (ret)
 		goto out;
@@ -539,9 +420,9 @@ static int wlan_get_txpow(struct net_device *dev,
 	lbs_deb_enter(LBS_DEB_WEXT);
 
 	ret = libertas_prepare_and_send_command(priv,
-				    cmd_802_11_rf_tx_power,
-				    cmd_act_tx_power_opt_get,
-				    cmd_option_waitforrsp, 0, NULL);
+				    CMD_802_11_RF_TX_POWER,
+				    CMD_ACT_TX_POWER_OPT_GET,
+				    CMD_OPTION_WAITFORRSP, 0, NULL);
 
 	if (ret)
 		goto out;
@@ -581,9 +462,9 @@ static int wlan_set_retry(struct net_device *dev, struct iw_request_info *info,
 		/* Adding 1 to convert retry count to try count */
 		adapter->txretrycount = vwrq->value + 1;
 
-		ret = libertas_prepare_and_send_command(priv, cmd_802_11_snmp_mib,
-					    cmd_act_set,
-					    cmd_option_waitforrsp,
+		ret = libertas_prepare_and_send_command(priv, CMD_802_11_SNMP_MIB,
+					    CMD_ACT_SET,
+					    CMD_OPTION_WAITFORRSP,
 					    OID_802_11_TX_RETRYCOUNT, NULL);
 
 		if (ret)
@@ -608,8 +489,8 @@ static int wlan_get_retry(struct net_device *dev, struct iw_request_info *info,
 
 	adapter->txretrycount = 0;
 	ret = libertas_prepare_and_send_command(priv,
-				    cmd_802_11_snmp_mib,
-				    cmd_act_get, cmd_option_waitforrsp,
+				    CMD_802_11_SNMP_MIB,
+				    CMD_ACT_GET, CMD_OPTION_WAITFORRSP,
 				    OID_802_11_TX_RETRYCOUNT, NULL);
 	if (ret)
 		goto out;
@@ -673,7 +554,7 @@ static int wlan_get_range(struct net_device *dev, struct iw_request_info *info,
 	wlan_adapter *adapter = priv->adapter;
 	struct iw_range *range = (struct iw_range *)extra;
 	struct chan_freq_power *cfp;
-	u8 rates[WLAN_SUPPORTED_RATES];
+	u8 rates[MAX_RATES + 1];
 
 	u8 flag = 0;
 
@@ -686,19 +567,17 @@ static int wlan_get_range(struct net_device *dev, struct iw_request_info *info,
 	range->max_nwid = 0;
 
 	memset(rates, 0, sizeof(rates));
-	range->num_bitrates = get_active_data_rates(adapter, rates);
-
-	for (i = 0; i < min_t(__u8, range->num_bitrates, IW_MAX_BITRATES) && rates[i];
-	     i++) {
-		range->bitrate[i] = (rates[i] & 0x7f) * 500000;
-	}
+	copy_active_data_rates(adapter, rates);
+	range->num_bitrates = strnlen(rates, IW_MAX_BITRATES);
+	for (i = 0; i < range->num_bitrates; i++)
+		range->bitrate[i] = rates[i] * 500000;
 	range->num_bitrates = i;
 	lbs_deb_wext("IW_MAX_BITRATES %d, num_bitrates %d\n", IW_MAX_BITRATES,
 	       range->num_bitrates);
 
 	range->num_frequency = 0;
 	if (priv->adapter->enable11d &&
-	    adapter->connect_status == libertas_connected) {
+	    adapter->connect_status == LIBERTAS_CONNECTED) {
 		u8 chan_no;
 		u8 band;
 
@@ -858,9 +737,9 @@ static int wlan_set_power(struct net_device *dev, struct iw_request_info *info,
 	 */
 
 	if (vwrq->disabled) {
-		adapter->psmode = wlan802_11powermodecam;
+		adapter->psmode = WLAN802_11POWERMODECAM;
 		if (adapter->psstate != PS_STATE_FULL_POWER) {
-			libertas_ps_wakeup(priv, cmd_option_waitforrsp);
+			libertas_ps_wakeup(priv, CMD_OPTION_WAITFORRSP);
 		}
 
 		return 0;
@@ -875,14 +754,14 @@ static int wlan_set_power(struct net_device *dev, struct iw_request_info *info,
 		return -EINVAL;
 	}
 
-	if (adapter->psmode != wlan802_11powermodecam) {
+	if (adapter->psmode != WLAN802_11POWERMODECAM) {
 		return 0;
 	}
 
-	adapter->psmode = wlan802_11powermodemax_psp;
+	adapter->psmode = WLAN802_11POWERMODEMAX_PSP;
 
-	if (adapter->connect_status == libertas_connected) {
-		libertas_ps_sleep(priv, cmd_option_waitforrsp);
+	if (adapter->connect_status == LIBERTAS_CONNECTED) {
+		libertas_ps_sleep(priv, CMD_OPTION_WAITFORRSP);
 	}
 
 	lbs_deb_leave(LBS_DEB_WEXT);
@@ -900,8 +779,8 @@ static int wlan_get_power(struct net_device *dev, struct iw_request_info *info,
 
 	mode = adapter->psmode;
 
-	if ((vwrq->disabled = (mode == wlan802_11powermodecam))
-	    || adapter->connect_status == libertas_disconnected)
+	if ((vwrq->disabled = (mode == WLAN802_11POWERMODECAM))
+	    || adapter->connect_status == LIBERTAS_DISCONNECTED)
 	{
 		goto out;
 	}
@@ -937,7 +816,7 @@ static struct iw_statistics *wlan_get_wireless_stats(struct net_device *dev)
 	priv->wstats.status = adapter->mode;
 
 	/* If we're not associated, all quality values are meaningless */
-	if (adapter->connect_status != libertas_connected)
+	if (adapter->connect_status != LIBERTAS_CONNECTED)
 		goto out;
 
 	/* Quality by RSSI */
@@ -973,7 +852,7 @@ static struct iw_statistics *wlan_get_wireless_stats(struct net_device *dev)
 	/* Quality by TX errors */
 	priv->wstats.discard.retries = priv->stats.tx_errors;
 
-	tx_retries = le16_to_cpu(adapter->logmsg.retry);
+	tx_retries = le32_to_cpu(adapter->logmsg.retry);
 
 	if (tx_retries > 75)
 		tx_qual = (90 - tx_retries) * POOR / 15;
@@ -989,20 +868,20 @@ static struct iw_statistics *wlan_get_wireless_stats(struct net_device *dev)
 		    (PERFECT - VERY_GOOD) / 50 + VERY_GOOD;
 	quality = min(quality, tx_qual);
 
-	priv->wstats.discard.code = le16_to_cpu(adapter->logmsg.wepundecryptable);
-	priv->wstats.discard.fragment = le16_to_cpu(adapter->logmsg.rxfrag);
+	priv->wstats.discard.code = le32_to_cpu(adapter->logmsg.wepundecryptable);
+	priv->wstats.discard.fragment = le32_to_cpu(adapter->logmsg.rxfrag);
 	priv->wstats.discard.retries = tx_retries;
-	priv->wstats.discard.misc = le16_to_cpu(adapter->logmsg.ackfailure);
+	priv->wstats.discard.misc = le32_to_cpu(adapter->logmsg.ackfailure);
 
 	/* Calculate quality */
-	priv->wstats.qual.qual = max(quality, (u32)100);
+	priv->wstats.qual.qual = min_t(u8, quality, 100);
 	priv->wstats.qual.updated = IW_QUAL_ALL_UPDATED | IW_QUAL_DBM;
 	stats_valid = 1;
 
 	/* update stats asynchronously for future calls */
-	libertas_prepare_and_send_command(priv, cmd_802_11_rssi, 0,
+	libertas_prepare_and_send_command(priv, CMD_802_11_RSSI, 0,
 					0, 0, NULL);
-	libertas_prepare_and_send_command(priv, cmd_802_11_get_log, 0,
+	libertas_prepare_and_send_command(priv, CMD_802_11_GET_LOG, 0,
 					0, 0, NULL);
 out:
 	if (!stats_valid) {
@@ -1080,88 +959,46 @@ out:
 	return ret;
 }
 
-/**
- *  @brief use index to get the data rate
- *
- *  @param index                The index of data rate
- *  @return 	   		data rate or 0
- */
-u32 libertas_index_to_data_rate(u8 index)
-{
-	if (index >= sizeof(libertas_wlan_data_rates))
-		index = 0;
-
-	return libertas_wlan_data_rates[index];
-}
-
-/**
- *  @brief use rate to get the index
- *
- *  @param rate                 data rate
- *  @return 	   		index or 0
- */
-u8 libertas_data_rate_to_index(u32 rate)
-{
-	u8 *ptr;
-
-	if (rate)
-		if ((ptr = memchr(libertas_wlan_data_rates, (u8) rate,
-				  sizeof(libertas_wlan_data_rates))))
-			return (ptr - libertas_wlan_data_rates);
-
-	return 0;
-}
-
 static int wlan_set_rate(struct net_device *dev, struct iw_request_info *info,
 		  struct iw_param *vwrq, char *extra)
 {
 	wlan_private *priv = dev->priv;
 	wlan_adapter *adapter = priv->adapter;
-	u32 data_rate;
+	u32 new_rate;
 	u16 action;
-	int ret = 0;
-	u8 rates[WLAN_SUPPORTED_RATES];
-	u8 *rate;
+	int ret = -EINVAL;
+	u8 rates[MAX_RATES + 1];
 
 	lbs_deb_enter(LBS_DEB_WEXT);
-
 	lbs_deb_wext("vwrq->value %d\n", vwrq->value);
 
+	/* Auto rate? */
 	if (vwrq->value == -1) {
-		action = cmd_act_set_tx_auto;	// Auto
-		adapter->is_datarate_auto = 1;
-		adapter->datarate = 0;
+		action = CMD_ACT_SET_TX_AUTO;
+		adapter->auto_rate = 1;
+		adapter->cur_rate = 0;
 	} else {
-		if (vwrq->value % 100000) {
-			return -EINVAL;
-		}
-
-		data_rate = vwrq->value / 500000;
+		if (vwrq->value % 100000)
+			goto out;
 
 		memset(rates, 0, sizeof(rates));
-		get_active_data_rates(adapter, rates);
-		rate = rates;
-		while (*rate) {
-			lbs_deb_wext("rate=0x%X, wanted data_rate 0x%X\n", *rate,
-			       data_rate);
-			if ((*rate & 0x7f) == (data_rate & 0x7f))
-				break;
-			rate++;
-		}
-		if (!*rate) {
-			lbs_pr_alert("fixed data rate 0x%X out "
-			       "of range\n", data_rate);
-			return -EINVAL;
+		copy_active_data_rates(adapter, rates);
+		new_rate = vwrq->value / 500000;
+		if (!memchr(rates, new_rate, sizeof(rates))) {
+			lbs_pr_alert("fixed data rate 0x%X out of range\n",
+				new_rate);
+			goto out;
 		}
 
-		adapter->datarate = data_rate;
-		action = cmd_act_set_tx_fix_rate;
-		adapter->is_datarate_auto = 0;
+		adapter->cur_rate = new_rate;
+		action = CMD_ACT_SET_TX_FIX_RATE;
+		adapter->auto_rate = 0;
 	}
 
-	ret = libertas_prepare_and_send_command(priv, cmd_802_11_data_rate,
-				    action, cmd_option_waitforrsp, 0, NULL);
+	ret = libertas_prepare_and_send_command(priv, CMD_802_11_DATA_RATE,
+				    action, CMD_OPTION_WAITFORRSP, 0, NULL);
 
+out:
 	lbs_deb_leave_args(LBS_DEB_WEXT, "ret %d", ret);
 	return ret;
 }
@@ -1174,14 +1011,19 @@ static int wlan_get_rate(struct net_device *dev, struct iw_request_info *info,
 
 	lbs_deb_enter(LBS_DEB_WEXT);
 
-	if (adapter->is_datarate_auto) {
-		vwrq->fixed = 0;
+	if (adapter->connect_status == LIBERTAS_CONNECTED) {
+		vwrq->value = adapter->cur_rate * 500000;
+
+		if (adapter->auto_rate)
+			vwrq->fixed = 0;
+		else
+			vwrq->fixed = 1;
+
 	} else {
-		vwrq->fixed = 1;
+		vwrq->fixed = 0;
+		vwrq->value = 0;
 	}
 
-	vwrq->value = adapter->datarate * 500000;
-
 	lbs_deb_leave(LBS_DEB_WEXT);
 	return 0;
 }
@@ -1298,7 +1140,7 @@ static int wlan_get_encode(struct net_device *dev,
 
 	dwrq->flags |= IW_ENCODE_NOKEY;
 
-	lbs_deb_wext("key: " MAC_FMT ", keylen %d\n",
+	lbs_deb_wext("key: %02x:%02x:%02x:%02x:%02x:%02x, keylen %d\n",
 	       extra[0], extra[1], extra[2],
 	       extra[3], extra[4], extra[5], dwrq->length);
 
@@ -1325,7 +1167,7 @@ static int wlan_set_wep_key(struct assoc_request *assoc_req,
 			    int set_tx_key)
 {
 	int ret = 0;
-	struct WLAN_802_11_KEY *pkey;
+	struct enc_key *pkey;
 
 	lbs_deb_enter(LBS_DEB_WEXT);
 
@@ -1344,7 +1186,7 @@ static int wlan_set_wep_key(struct assoc_request *assoc_req,
 	pkey = &assoc_req->wep_keys[index];
 
 	if (key_length > 0) {
-		memset(pkey, 0, sizeof(struct WLAN_802_11_KEY));
+		memset(pkey, 0, sizeof(struct enc_key));
 		pkey->type = KEY_TYPE_ID_WEP;
 
 		/* Standardize the key length */
@@ -1412,11 +1254,11 @@ static void disable_wpa(struct assoc_request *assoc_req)
 {
 	lbs_deb_enter(LBS_DEB_WEXT);
 
-	memset(&assoc_req->wpa_mcast_key, 0, sizeof (struct WLAN_802_11_KEY));
+	memset(&assoc_req->wpa_mcast_key, 0, sizeof (struct enc_key));
 	assoc_req->wpa_mcast_key.flags = KEY_INFO_WPA_MCAST;
 	set_bit(ASSOC_FLAG_WPA_MCAST_KEY, &assoc_req->flags);
 
-	memset(&assoc_req->wpa_unicast_key, 0, sizeof (struct WLAN_802_11_KEY));
+	memset(&assoc_req->wpa_unicast_key, 0, sizeof (struct enc_key));
 	assoc_req->wpa_unicast_key.flags = KEY_INFO_WPA_UNICAST;
 	set_bit(ASSOC_FLAG_WPA_UCAST_KEY, &assoc_req->flags);
 
@@ -1567,7 +1409,7 @@ static int wlan_get_encodeext(struct net_device *dev,
 		           && (adapter->secinfo.WPAenabled ||
 		               adapter->secinfo.WPA2enabled)) {
 			/* WPA */
-			struct WLAN_802_11_KEY * pkey = NULL;
+			struct enc_key * pkey = NULL;
 
 			if (   adapter->wpa_mcast_key.len
 			    && (adapter->wpa_mcast_key.flags & KEY_INFO_WPA_ENABLED))
@@ -1679,7 +1521,7 @@ static int wlan_set_encodeext(struct net_device *dev,
 		if (set_tx_key)
 			set_bit(ASSOC_FLAG_WEP_TX_KEYIDX, &assoc_req->flags);
 	} else if ((alg == IW_ENCODE_ALG_TKIP) || (alg == IW_ENCODE_ALG_CCMP)) {
-		struct WLAN_802_11_KEY * pkey;
+		struct enc_key * pkey;
 
 		/* validate key length */
 		if (((alg == IW_ENCODE_ALG_TKIP)
@@ -1702,7 +1544,7 @@ static int wlan_set_encodeext(struct net_device *dev,
 			set_bit(ASSOC_FLAG_WPA_UCAST_KEY, &assoc_req->flags);
 		}
 
-		memset(pkey, 0, sizeof (struct WLAN_802_11_KEY));
+		memset(pkey, 0, sizeof (struct enc_key));
 		memcpy(pkey->key, ext->key, ext->key_len);
 		pkey->len = ext->key_len;
 		if (pkey->len)
@@ -1976,12 +1818,14 @@ static int wlan_set_txpow(struct net_device *dev, struct iw_request_info *info,
 		return 0;
 	}
 
-	adapter->preamble = cmd_type_auto_preamble;
+	adapter->preamble = CMD_TYPE_AUTO_PREAMBLE;
 
 	wlan_radio_ioctl(priv, RADIO_ON);
 
+	/* Userspace check in iwrange if it should use dBm or mW,
+	 * therefore this should never happen... Jean II */
 	if ((vwrq->flags & IW_TXPOW_TYPE) == IW_TXPOW_MWATT) {
-		dbm = (u16) mw_to_dbm(vwrq->value);
+		return -EOPNOTSUPP;
 	} else
 		dbm = (u16) vwrq->value;
 
@@ -1993,9 +1837,9 @@ static int wlan_set_txpow(struct net_device *dev, struct iw_request_info *info,
 	lbs_deb_wext("txpower set %d dbm\n", dbm);
 
 	ret = libertas_prepare_and_send_command(priv,
-				    cmd_802_11_rf_tx_power,
-				    cmd_act_tx_power_opt_set_low,
-				    cmd_option_waitforrsp, 0, (void *)&dbm);
+				    CMD_802_11_RF_TX_POWER,
+				    CMD_ACT_TX_POWER_OPT_SET_LOW,
+				    CMD_OPTION_WAITFORRSP, 0, (void *)&dbm);
 
 	lbs_deb_leave_args(LBS_DEB_WEXT, "ret %d", ret);
 	return ret;
@@ -2017,7 +1861,7 @@ static int wlan_get_essid(struct net_device *dev, struct iw_request_info *info,
 	/*
 	 * Get the current SSID
 	 */
-	if (adapter->connect_status == libertas_connected) {
+	if (adapter->connect_status == LIBERTAS_CONNECTED) {
 		memcpy(extra, adapter->curbssparams.ssid,
 		       adapter->curbssparams.ssid_len);
 		extra[adapter->curbssparams.ssid_len] = '\0';
@@ -2029,12 +1873,7 @@ static int wlan_get_essid(struct net_device *dev, struct iw_request_info *info,
 	 * If none, we may want to get the one that was set
 	 */
 
-	/* To make the driver backward compatible with WPA supplicant v0.2.4 */
-	if (dwrq->length == 32)	/* check with WPA supplicant buffer size */
-		dwrq->length = min_t(size_t, adapter->curbssparams.ssid_len,
-				   IW_ESSID_MAX_SIZE);
-	else
-		dwrq->length = adapter->curbssparams.ssid_len + 1;
+	dwrq->length = adapter->curbssparams.ssid_len;
 
 	dwrq->flags = 1;	/* active */
 
@@ -2055,14 +1894,6 @@ static int wlan_set_essid(struct net_device *dev, struct iw_request_info *info,
 
 	lbs_deb_enter(LBS_DEB_WEXT);
 
-	/*
-	 * WE-20 and earlier NULL pad the end of the SSID and increment
-	 * SSID length so it can be used like a string.  WE-21 and later don't,
-	 * but some userspace tools aren't able to cope with the change.
-	 */
-	if ((in_ssid_len > 0) && (extra[in_ssid_len - 1] == '\0'))
-		in_ssid_len--;
-
 	/* Check the size of the string */
 	if (in_ssid_len > IW_ESSID_MAX_SIZE) {
 		ret = -E2BIG;
@@ -2129,13 +1960,14 @@ static int wlan_set_wap(struct net_device *dev, struct iw_request_info *info,
 	wlan_adapter *adapter = priv->adapter;
 	struct assoc_request * assoc_req;
 	int ret = 0;
+	DECLARE_MAC_BUF(mac);
 
 	lbs_deb_enter(LBS_DEB_WEXT);
 
 	if (awrq->sa_family != ARPHRD_ETHER)
 		return -EINVAL;
 
-	lbs_deb_wext("ASSOC: WAP: sa_data " MAC_FMT "\n", MAC_ARG(awrq->sa_data));
+	lbs_deb_wext("ASSOC: WAP: sa_data %s\n", print_mac(mac, awrq->sa_data));
 
 	mutex_lock(&adapter->lock);
 
@@ -2298,13 +2130,13 @@ static const iw_handler mesh_wlan_handler[] = {
 	(iw_handler) NULL,		/* SIOCSIWPMKSA */
 };
 struct iw_handler_def libertas_handler_def = {
-	.num_standard	= sizeof(wlan_handler) / sizeof(iw_handler),
+	.num_standard	= ARRAY_SIZE(wlan_handler),
 	.standard	= (iw_handler *) wlan_handler,
 	.get_wireless_stats = wlan_get_wireless_stats,
 };
 
 struct iw_handler_def mesh_handler_def = {
-	.num_standard	= sizeof(mesh_wlan_handler) / sizeof(iw_handler),
+	.num_standard	= ARRAY_SIZE(mesh_wlan_handler),
 	.standard	= (iw_handler *) mesh_wlan_handler,
 	.get_wireless_stats = wlan_get_wireless_stats,
 };
diff --git a/drivers/net/wireless/libertas/wext.h b/drivers/net/wireless/libertas/wext.h
index 3d5196c9553a..6aa444c7de8d 100644
--- a/drivers/net/wireless/libertas/wext.h
+++ b/drivers/net/wireless/libertas/wext.h
@@ -4,9 +4,6 @@
 #ifndef	_WLAN_WEXT_H_
 #define	_WLAN_WEXT_H_
 
-#define SUBCMD_OFFSET			4
-#define SUBCMD_DATA(x)			*((int *)(x->u.name + SUBCMD_OFFSET))
-
 /** wlan_ioctl_regrdwr */
 struct wlan_ioctl_regrdwr {
 	/** Which register to access */
@@ -18,13 +15,9 @@ 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;
-int wlan_radio_ioctl(wlan_private * priv, u8 option);
 
 #endif				/* _WLAN_WEXT_H_ */
diff --git a/drivers/net/wireless/net2280.h b/drivers/net/wireless/net2280.h
new file mode 100644
index 000000000000..120eb831b287
--- /dev/null
+++ b/drivers/net/wireless/net2280.h
@@ -0,0 +1,452 @@
+#ifndef NET2280_H
+#define NET2280_H
+/*
+ * NetChip 2280 high/full speed USB device controller.
+ * Unlike many such controllers, this one talks PCI.
+ */
+
+/*
+ * Copyright (C) 2002 NetChip Technology, Inc. (http://www.netchip.com)
+ * Copyright (C) 2003 David Brownell
+ *
+ * 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, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+ */
+
+/*-------------------------------------------------------------------------*/
+
+/* NET2280 MEMORY MAPPED REGISTERS
+ *
+ * The register layout came from the chip documentation, and the bit
+ * number definitions were extracted from chip specification.
+ *
+ * Use the shift operator ('<<') to build bit masks, with readl/writel
+ * to access the registers through PCI.
+ */
+
+/* main registers, BAR0 + 0x0000 */
+struct net2280_regs {
+	// offset 0x0000
+	__le32                  devinit;
+#define     LOCAL_CLOCK_FREQUENCY                               8
+#define     FORCE_PCI_RESET                                     7
+#define     PCI_ID                                              6
+#define     PCI_ENABLE                                          5
+#define     FIFO_SOFT_RESET                                     4
+#define     CFG_SOFT_RESET                                      3
+#define     PCI_SOFT_RESET                                      2
+#define     USB_SOFT_RESET                                      1
+#define     M8051_RESET                                         0
+	__le32                  eectl;
+#define     EEPROM_ADDRESS_WIDTH                                23
+#define     EEPROM_CHIP_SELECT_ACTIVE                           22
+#define     EEPROM_PRESENT                                      21
+#define     EEPROM_VALID                                        20
+#define     EEPROM_BUSY                                         19
+#define     EEPROM_CHIP_SELECT_ENABLE                           18
+#define     EEPROM_BYTE_READ_START                              17
+#define     EEPROM_BYTE_WRITE_START                             16
+#define     EEPROM_READ_DATA                                    8
+#define     EEPROM_WRITE_DATA                                   0
+	__le32                  eeclkfreq;
+	u32                     _unused0;
+	// offset 0x0010
+
+	__le32                  pciirqenb0;	/* interrupt PCI master ... */
+#define     SETUP_PACKET_INTERRUPT_ENABLE                       7
+#define     ENDPOINT_F_INTERRUPT_ENABLE                         6
+#define     ENDPOINT_E_INTERRUPT_ENABLE                         5
+#define     ENDPOINT_D_INTERRUPT_ENABLE                         4
+#define     ENDPOINT_C_INTERRUPT_ENABLE                         3
+#define     ENDPOINT_B_INTERRUPT_ENABLE                         2
+#define     ENDPOINT_A_INTERRUPT_ENABLE                         1
+#define     ENDPOINT_0_INTERRUPT_ENABLE                         0
+	__le32                  pciirqenb1;
+#define     PCI_INTERRUPT_ENABLE                                31
+#define     POWER_STATE_CHANGE_INTERRUPT_ENABLE                 27
+#define     PCI_ARBITER_TIMEOUT_INTERRUPT_ENABLE                26
+#define     PCI_PARITY_ERROR_INTERRUPT_ENABLE                   25
+#define     PCI_MASTER_ABORT_RECEIVED_INTERRUPT_ENABLE          20
+#define     PCI_TARGET_ABORT_RECEIVED_INTERRUPT_ENABLE          19
+#define     PCI_TARGET_ABORT_ASSERTED_INTERRUPT_ENABLE          18
+#define     PCI_RETRY_ABORT_INTERRUPT_ENABLE                    17
+#define     PCI_MASTER_CYCLE_DONE_INTERRUPT_ENABLE              16
+#define     GPIO_INTERRUPT_ENABLE                               13
+#define     DMA_D_INTERRUPT_ENABLE                              12
+#define     DMA_C_INTERRUPT_ENABLE                              11
+#define     DMA_B_INTERRUPT_ENABLE                              10
+#define     DMA_A_INTERRUPT_ENABLE                              9
+#define     EEPROM_DONE_INTERRUPT_ENABLE                        8
+#define     VBUS_INTERRUPT_ENABLE                               7
+#define     CONTROL_STATUS_INTERRUPT_ENABLE                     6
+#define     ROOT_PORT_RESET_INTERRUPT_ENABLE                    4
+#define     SUSPEND_REQUEST_INTERRUPT_ENABLE                    3
+#define     SUSPEND_REQUEST_CHANGE_INTERRUPT_ENABLE             2
+#define     RESUME_INTERRUPT_ENABLE                             1
+#define     SOF_INTERRUPT_ENABLE                                0
+	__le32                  cpu_irqenb0;	/* ... or onboard 8051 */
+#define     SETUP_PACKET_INTERRUPT_ENABLE                       7
+#define     ENDPOINT_F_INTERRUPT_ENABLE                         6
+#define     ENDPOINT_E_INTERRUPT_ENABLE                         5
+#define     ENDPOINT_D_INTERRUPT_ENABLE                         4
+#define     ENDPOINT_C_INTERRUPT_ENABLE                         3
+#define     ENDPOINT_B_INTERRUPT_ENABLE                         2
+#define     ENDPOINT_A_INTERRUPT_ENABLE                         1
+#define     ENDPOINT_0_INTERRUPT_ENABLE                         0
+	__le32                  cpu_irqenb1;
+#define     CPU_INTERRUPT_ENABLE                                31
+#define     POWER_STATE_CHANGE_INTERRUPT_ENABLE                 27
+#define     PCI_ARBITER_TIMEOUT_INTERRUPT_ENABLE                26
+#define     PCI_PARITY_ERROR_INTERRUPT_ENABLE                   25
+#define     PCI_INTA_INTERRUPT_ENABLE                           24
+#define     PCI_PME_INTERRUPT_ENABLE                            23
+#define     PCI_SERR_INTERRUPT_ENABLE                           22
+#define     PCI_PERR_INTERRUPT_ENABLE                           21
+#define     PCI_MASTER_ABORT_RECEIVED_INTERRUPT_ENABLE          20
+#define     PCI_TARGET_ABORT_RECEIVED_INTERRUPT_ENABLE          19
+#define     PCI_RETRY_ABORT_INTERRUPT_ENABLE                    17
+#define     PCI_MASTER_CYCLE_DONE_INTERRUPT_ENABLE              16
+#define     GPIO_INTERRUPT_ENABLE                               13
+#define     DMA_D_INTERRUPT_ENABLE                              12
+#define     DMA_C_INTERRUPT_ENABLE                              11
+#define     DMA_B_INTERRUPT_ENABLE                              10
+#define     DMA_A_INTERRUPT_ENABLE                              9
+#define     EEPROM_DONE_INTERRUPT_ENABLE                        8
+#define     VBUS_INTERRUPT_ENABLE                               7
+#define     CONTROL_STATUS_INTERRUPT_ENABLE                     6
+#define     ROOT_PORT_RESET_INTERRUPT_ENABLE                    4
+#define     SUSPEND_REQUEST_INTERRUPT_ENABLE                    3
+#define     SUSPEND_REQUEST_CHANGE_INTERRUPT_ENABLE             2
+#define     RESUME_INTERRUPT_ENABLE                             1
+#define     SOF_INTERRUPT_ENABLE                                0
+
+	// offset 0x0020
+	u32                     _unused1;
+	__le32                  usbirqenb1;
+#define     USB_INTERRUPT_ENABLE                                31
+#define     POWER_STATE_CHANGE_INTERRUPT_ENABLE                 27
+#define     PCI_ARBITER_TIMEOUT_INTERRUPT_ENABLE                26
+#define     PCI_PARITY_ERROR_INTERRUPT_ENABLE                   25
+#define     PCI_INTA_INTERRUPT_ENABLE                           24
+#define     PCI_PME_INTERRUPT_ENABLE                            23
+#define     PCI_SERR_INTERRUPT_ENABLE                           22
+#define     PCI_PERR_INTERRUPT_ENABLE                           21
+#define     PCI_MASTER_ABORT_RECEIVED_INTERRUPT_ENABLE          20
+#define     PCI_TARGET_ABORT_RECEIVED_INTERRUPT_ENABLE          19
+#define     PCI_RETRY_ABORT_INTERRUPT_ENABLE                    17
+#define     PCI_MASTER_CYCLE_DONE_INTERRUPT_ENABLE              16
+#define     GPIO_INTERRUPT_ENABLE                               13
+#define     DMA_D_INTERRUPT_ENABLE                              12
+#define     DMA_C_INTERRUPT_ENABLE                              11
+#define     DMA_B_INTERRUPT_ENABLE                              10
+#define     DMA_A_INTERRUPT_ENABLE                              9
+#define     EEPROM_DONE_INTERRUPT_ENABLE                        8
+#define     VBUS_INTERRUPT_ENABLE                               7
+#define     CONTROL_STATUS_INTERRUPT_ENABLE                     6
+#define     ROOT_PORT_RESET_INTERRUPT_ENABLE                    4
+#define     SUSPEND_REQUEST_INTERRUPT_ENABLE                    3
+#define     SUSPEND_REQUEST_CHANGE_INTERRUPT_ENABLE             2
+#define     RESUME_INTERRUPT_ENABLE                             1
+#define     SOF_INTERRUPT_ENABLE                                0
+	__le32                  irqstat0;
+#define     INTA_ASSERTED                                       12
+#define     SETUP_PACKET_INTERRUPT                              7
+#define     ENDPOINT_F_INTERRUPT                                6
+#define     ENDPOINT_E_INTERRUPT                                5
+#define     ENDPOINT_D_INTERRUPT                                4
+#define     ENDPOINT_C_INTERRUPT                                3
+#define     ENDPOINT_B_INTERRUPT                                2
+#define     ENDPOINT_A_INTERRUPT                                1
+#define     ENDPOINT_0_INTERRUPT                                0
+	__le32                  irqstat1;
+#define     POWER_STATE_CHANGE_INTERRUPT                        27
+#define     PCI_ARBITER_TIMEOUT_INTERRUPT                       26
+#define     PCI_PARITY_ERROR_INTERRUPT                          25
+#define     PCI_INTA_INTERRUPT                                  24
+#define     PCI_PME_INTERRUPT                                   23
+#define     PCI_SERR_INTERRUPT                                  22
+#define     PCI_PERR_INTERRUPT                                  21
+#define     PCI_MASTER_ABORT_RECEIVED_INTERRUPT                 20
+#define     PCI_TARGET_ABORT_RECEIVED_INTERRUPT                 19
+#define     PCI_RETRY_ABORT_INTERRUPT                           17
+#define     PCI_MASTER_CYCLE_DONE_INTERRUPT                     16
+#define     GPIO_INTERRUPT                                      13
+#define     DMA_D_INTERRUPT                                     12
+#define     DMA_C_INTERRUPT                                     11
+#define     DMA_B_INTERRUPT                                     10
+#define     DMA_A_INTERRUPT                                     9
+#define     EEPROM_DONE_INTERRUPT                               8
+#define     VBUS_INTERRUPT                                      7
+#define     CONTROL_STATUS_INTERRUPT                            6
+#define     ROOT_PORT_RESET_INTERRUPT                           4
+#define     SUSPEND_REQUEST_INTERRUPT                           3
+#define     SUSPEND_REQUEST_CHANGE_INTERRUPT                    2
+#define     RESUME_INTERRUPT                                    1
+#define     SOF_INTERRUPT                                       0
+	// offset 0x0030
+	__le32                  idxaddr;
+	__le32                  idxdata;
+	__le32                  fifoctl;
+#define     PCI_BASE2_RANGE                                     16
+#define     IGNORE_FIFO_AVAILABILITY                            3
+#define     PCI_BASE2_SELECT                                    2
+#define     FIFO_CONFIGURATION_SELECT                           0
+	u32                     _unused2;
+	// offset 0x0040
+	__le32                  memaddr;
+#define     START                                               28
+#define     DIRECTION                                           27
+#define     FIFO_DIAGNOSTIC_SELECT                              24
+#define     MEMORY_ADDRESS                                      0
+	__le32                  memdata0;
+	__le32                  memdata1;
+	u32                     _unused3;
+	// offset 0x0050
+	__le32                  gpioctl;
+#define     GPIO3_LED_SELECT                                    12
+#define     GPIO3_INTERRUPT_ENABLE                              11
+#define     GPIO2_INTERRUPT_ENABLE                              10
+#define     GPIO1_INTERRUPT_ENABLE                              9
+#define     GPIO0_INTERRUPT_ENABLE                              8
+#define     GPIO3_OUTPUT_ENABLE                                 7
+#define     GPIO2_OUTPUT_ENABLE                                 6
+#define     GPIO1_OUTPUT_ENABLE                                 5
+#define     GPIO0_OUTPUT_ENABLE                                 4
+#define     GPIO3_DATA                                          3
+#define     GPIO2_DATA                                          2
+#define     GPIO1_DATA                                          1
+#define     GPIO0_DATA                                          0
+	__le32                  gpiostat;
+#define     GPIO3_INTERRUPT                                     3
+#define     GPIO2_INTERRUPT                                     2
+#define     GPIO1_INTERRUPT                                     1
+#define     GPIO0_INTERRUPT                                     0
+} __attribute__ ((packed));
+
+/* usb control, BAR0 + 0x0080 */
+struct net2280_usb_regs {
+	// offset 0x0080
+	__le32                  stdrsp;
+#define     STALL_UNSUPPORTED_REQUESTS                          31
+#define     SET_TEST_MODE                                       16
+#define     GET_OTHER_SPEED_CONFIGURATION                       15
+#define     GET_DEVICE_QUALIFIER                                14
+#define     SET_ADDRESS                                         13
+#define     ENDPOINT_SET_CLEAR_HALT                             12
+#define     DEVICE_SET_CLEAR_DEVICE_REMOTE_WAKEUP               11
+#define     GET_STRING_DESCRIPTOR_2                             10
+#define     GET_STRING_DESCRIPTOR_1                             9
+#define     GET_STRING_DESCRIPTOR_0                             8
+#define     GET_SET_INTERFACE                                   6
+#define     GET_SET_CONFIGURATION                               5
+#define     GET_CONFIGURATION_DESCRIPTOR                        4
+#define     GET_DEVICE_DESCRIPTOR                               3
+#define     GET_ENDPOINT_STATUS                                 2
+#define     GET_INTERFACE_STATUS                                1
+#define     GET_DEVICE_STATUS                                   0
+	__le32                  prodvendid;
+#define     PRODUCT_ID                                          16
+#define     VENDOR_ID                                           0
+	__le32                  relnum;
+	__le32                  usbctl;
+#define     SERIAL_NUMBER_INDEX                                 16
+#define     PRODUCT_ID_STRING_ENABLE                            13
+#define     VENDOR_ID_STRING_ENABLE                             12
+#define     USB_ROOT_PORT_WAKEUP_ENABLE                         11
+#define     VBUS_PIN                                            10
+#define     TIMED_DISCONNECT                                    9
+#define     SUSPEND_IMMEDIATELY                                 7
+#define     SELF_POWERED_USB_DEVICE                             6
+#define     REMOTE_WAKEUP_SUPPORT                               5
+#define     PME_POLARITY                                        4
+#define     USB_DETECT_ENABLE                                   3
+#define     PME_WAKEUP_ENABLE                                   2
+#define     DEVICE_REMOTE_WAKEUP_ENABLE                         1
+#define     SELF_POWERED_STATUS                                 0
+	// offset 0x0090
+	__le32                  usbstat;
+#define     HIGH_SPEED                                          7
+#define     FULL_SPEED                                          6
+#define     GENERATE_RESUME                                     5
+#define     GENERATE_DEVICE_REMOTE_WAKEUP                       4
+	__le32                  xcvrdiag;
+#define     FORCE_HIGH_SPEED_MODE                               31
+#define     FORCE_FULL_SPEED_MODE                               30
+#define     USB_TEST_MODE                                       24
+#define     LINE_STATE                                          16
+#define     TRANSCEIVER_OPERATION_MODE                          2
+#define     TRANSCEIVER_SELECT                                  1
+#define     TERMINATION_SELECT                                  0
+	__le32                  setup0123;
+	__le32                  setup4567;
+	// offset 0x0090
+	u32                     _unused0;
+	__le32                  ouraddr;
+#define     FORCE_IMMEDIATE                                     7
+#define     OUR_USB_ADDRESS                                     0
+	__le32                  ourconfig;
+} __attribute__ ((packed));
+
+/* pci control, BAR0 + 0x0100 */
+struct net2280_pci_regs {
+	// offset 0x0100
+	__le32                  pcimstctl;
+#define     PCI_ARBITER_PARK_SELECT                             13
+#define     PCI_MULTI LEVEL_ARBITER                             12
+#define     PCI_RETRY_ABORT_ENABLE                              11
+#define     DMA_MEMORY_WRITE_AND_INVALIDATE_ENABLE              10
+#define     DMA_READ_MULTIPLE_ENABLE                            9
+#define     DMA_READ_LINE_ENABLE                                8
+#define     PCI_MASTER_COMMAND_SELECT                           6
+#define         MEM_READ_OR_WRITE                                   0
+#define         IO_READ_OR_WRITE                                    1
+#define         CFG_READ_OR_WRITE                                   2
+#define     PCI_MASTER_START                                    5
+#define     PCI_MASTER_READ_WRITE                               4
+#define         PCI_MASTER_WRITE                                    0
+#define         PCI_MASTER_READ                                     1
+#define     PCI_MASTER_BYTE_WRITE_ENABLES                       0
+	__le32                  pcimstaddr;
+	__le32                  pcimstdata;
+	__le32                  pcimststat;
+#define     PCI_ARBITER_CLEAR                                   2
+#define     PCI_EXTERNAL_ARBITER                                1
+#define     PCI_HOST_MODE                                       0
+} __attribute__ ((packed));
+
+/* dma control, BAR0 + 0x0180 ... array of four structs like this,
+ * for channels 0..3.  see also struct net2280_dma:  descriptor
+ * that can be loaded into some of these registers.
+ */
+struct net2280_dma_regs {	/* [11.7] */
+	// offset 0x0180, 0x01a0, 0x01c0, 0x01e0,
+	__le32                  dmactl;
+#define     DMA_SCATTER_GATHER_DONE_INTERRUPT_ENABLE            25
+#define     DMA_CLEAR_COUNT_ENABLE                              21
+#define     DESCRIPTOR_POLLING_RATE                             19
+#define         POLL_CONTINUOUS                                     0
+#define         POLL_1_USEC                                         1
+#define         POLL_100_USEC                                       2
+#define         POLL_1_MSEC                                         3
+#define     DMA_VALID_BIT_POLLING_ENABLE                        18
+#define     DMA_VALID_BIT_ENABLE                                17
+#define     DMA_SCATTER_GATHER_ENABLE                           16
+#define     DMA_OUT_AUTO_START_ENABLE                           4
+#define     DMA_PREEMPT_ENABLE                                  3
+#define     DMA_FIFO_VALIDATE                                   2
+#define     DMA_ENABLE                                          1
+#define     DMA_ADDRESS_HOLD                                    0
+	__le32                  dmastat;
+#define     DMA_SCATTER_GATHER_DONE_INTERRUPT                   25
+#define     DMA_TRANSACTION_DONE_INTERRUPT                      24
+#define     DMA_ABORT                                           1
+#define     DMA_START                                           0
+	u32                     _unused0[2];
+	// offset 0x0190, 0x01b0, 0x01d0, 0x01f0,
+	__le32                  dmacount;
+#define     VALID_BIT                                           31
+#define     DMA_DIRECTION                                       30
+#define     DMA_DONE_INTERRUPT_ENABLE                           29
+#define     END_OF_CHAIN                                        28
+#define         DMA_BYTE_COUNT_MASK                                 ((1<<24)-1)
+#define     DMA_BYTE_COUNT                                      0
+	__le32                  dmaaddr;
+	__le32                  dmadesc;
+	u32                     _unused1;
+} __attribute__ ((packed));
+
+/* dedicated endpoint registers, BAR0 + 0x0200 */
+
+struct net2280_dep_regs {	/* [11.8] */
+	// offset 0x0200, 0x0210, 0x220, 0x230, 0x240
+	__le32                  dep_cfg;
+	// offset 0x0204, 0x0214, 0x224, 0x234, 0x244
+	__le32                  dep_rsp;
+	u32                     _unused[2];
+} __attribute__ ((packed));
+
+/* configurable endpoint registers, BAR0 + 0x0300 ... array of seven structs
+ * like this, for ep0 then the configurable endpoints A..F
+ * ep0 reserved for control; E and F have only 64 bytes of fifo
+ */
+struct net2280_ep_regs {	/* [11.9] */
+	// offset 0x0300, 0x0320, 0x0340, 0x0360, 0x0380, 0x03a0, 0x03c0
+	__le32                  ep_cfg;
+#define     ENDPOINT_BYTE_COUNT                                 16
+#define     ENDPOINT_ENABLE                                     10
+#define     ENDPOINT_TYPE                                       8
+#define     ENDPOINT_DIRECTION                                  7
+#define     ENDPOINT_NUMBER                                     0
+	__le32                  ep_rsp;
+#define     SET_NAK_OUT_PACKETS                                 15
+#define     SET_EP_HIDE_STATUS_PHASE                            14
+#define     SET_EP_FORCE_CRC_ERROR                              13
+#define     SET_INTERRUPT_MODE                                  12
+#define     SET_CONTROL_STATUS_PHASE_HANDSHAKE                  11
+#define     SET_NAK_OUT_PACKETS_MODE                            10
+#define     SET_ENDPOINT_TOGGLE                                 9
+#define     SET_ENDPOINT_HALT                                   8
+#define     CLEAR_NAK_OUT_PACKETS                               7
+#define     CLEAR_EP_HIDE_STATUS_PHASE                          6
+#define     CLEAR_EP_FORCE_CRC_ERROR                            5
+#define     CLEAR_INTERRUPT_MODE                                4
+#define     CLEAR_CONTROL_STATUS_PHASE_HANDSHAKE                3
+#define     CLEAR_NAK_OUT_PACKETS_MODE                          2
+#define     CLEAR_ENDPOINT_TOGGLE                               1
+#define     CLEAR_ENDPOINT_HALT                                 0
+	__le32                  ep_irqenb;
+#define     SHORT_PACKET_OUT_DONE_INTERRUPT_ENABLE              6
+#define     SHORT_PACKET_TRANSFERRED_INTERRUPT_ENABLE           5
+#define     DATA_PACKET_RECEIVED_INTERRUPT_ENABLE               3
+#define     DATA_PACKET_TRANSMITTED_INTERRUPT_ENABLE            2
+#define     DATA_OUT_PING_TOKEN_INTERRUPT_ENABLE                1
+#define     DATA_IN_TOKEN_INTERRUPT_ENABLE                      0
+	__le32                  ep_stat;
+#define     FIFO_VALID_COUNT                                    24
+#define     HIGH_BANDWIDTH_OUT_TRANSACTION_PID                  22
+#define     TIMEOUT                                             21
+#define     USB_STALL_SENT                                      20
+#define     USB_IN_NAK_SENT                                     19
+#define     USB_IN_ACK_RCVD                                     18
+#define     USB_OUT_PING_NAK_SENT                               17
+#define     USB_OUT_ACK_SENT                                    16
+#define     FIFO_OVERFLOW                                       13
+#define     FIFO_UNDERFLOW                                      12
+#define     FIFO_FULL                                           11
+#define     FIFO_EMPTY                                          10
+#define     FIFO_FLUSH                                          9
+#define     SHORT_PACKET_OUT_DONE_INTERRUPT                     6
+#define     SHORT_PACKET_TRANSFERRED_INTERRUPT                  5
+#define     NAK_OUT_PACKETS                                     4
+#define     DATA_PACKET_RECEIVED_INTERRUPT                      3
+#define     DATA_PACKET_TRANSMITTED_INTERRUPT                   2
+#define     DATA_OUT_PING_TOKEN_INTERRUPT                       1
+#define     DATA_IN_TOKEN_INTERRUPT                             0
+	// offset 0x0310, 0x0330, 0x0350, 0x0370, 0x0390, 0x03b0, 0x03d0
+	__le32                  ep_avail;
+	__le32                  ep_data;
+	u32                     _unused0[2];
+} __attribute__ ((packed));
+
+struct net2280_reg_write {
+	__le16 port;
+	__le32 addr;
+	__le32 val;
+} __attribute__ ((packed));
+
+struct net2280_reg_read {
+	__le16 port;
+	__le32 addr;
+} __attribute__ ((packed));
+#endif /* NET2280_H */
diff --git a/drivers/net/wireless/netwave_cs.c b/drivers/net/wireless/netwave_cs.c
index 45b00e13ab2b..c2d71afd57e5 100644
--- a/drivers/net/wireless/netwave_cs.c
+++ b/drivers/net/wireless/netwave_cs.c
@@ -412,7 +412,6 @@ static int netwave_probe(struct pcmcia_device *link)
     spin_lock_init(&priv->spinlock);
 
     /* Netwave specific entries in the device structure */
-    SET_MODULE_OWNER(dev);
     dev->hard_start_xmit = &netwave_start_xmit;
     dev->get_stats  = &netwave_get_stats;
     dev->set_multicast_list = &set_multicast_list;
@@ -710,9 +709,9 @@ static const iw_handler		netwave_private_handler[] =
 
 static const struct iw_handler_def	netwave_handler_def =
 {
-	.num_standard	= sizeof(netwave_handler)/sizeof(iw_handler),
-	.num_private	= sizeof(netwave_private_handler)/sizeof(iw_handler),
-	.num_private_args = sizeof(netwave_private_args)/sizeof(struct iw_priv_args),
+	.num_standard	= ARRAY_SIZE(netwave_handler),
+	.num_private	= ARRAY_SIZE(netwave_private_handler),
+	.num_private_args = ARRAY_SIZE(netwave_private_args),
 	.standard	= (iw_handler *) netwave_handler,
 	.private	= (iw_handler *) netwave_private_handler,
 	.private_args	= (struct iw_priv_args *) netwave_private_args,
@@ -738,6 +737,7 @@ static int netwave_pcmcia_config(struct pcmcia_device *link) {
     win_req_t req;
     memreq_t mem;
     u_char __iomem *ramBase = NULL;
+    DECLARE_MAC_BUF(mac);
 
     DEBUG(0, "netwave_pcmcia_config(0x%p)\n", link);
 
@@ -806,12 +806,13 @@ static int netwave_pcmcia_config(struct pcmcia_device *link) {
     for (i = 0; i < 6; i++) 
 	dev->dev_addr[i] = readb(ramBase + NETWAVE_EREG_PA + i);
 
-    printk(KERN_INFO "%s: Netwave: port %#3lx, irq %d, mem %lx id "
-	   "%c%c, hw_addr ", dev->name, dev->base_addr, dev->irq,
-	   (u_long) ramBase, (int) readb(ramBase+NETWAVE_EREG_NI),
-	   (int) readb(ramBase+NETWAVE_EREG_NI+1));
-    for (i = 0; i < 6; i++)
-	printk("%02X%s", dev->dev_addr[i], ((i<5) ? ":" : "\n"));
+    printk(KERN_INFO "%s: Netwave: port %#3lx, irq %d, mem %lx"
+	   "id %c%c, hw_addr %s\n",
+	   dev->name, dev->base_addr, dev->irq,
+	   (u_long) ramBase,
+	   (int) readb(ramBase+NETWAVE_EREG_NI),
+	   (int) readb(ramBase+NETWAVE_EREG_NI+1),
+	   print_mac(mac, dev->dev_addr));
 
     /* get revision words */
     printk(KERN_DEBUG "Netwave_reset: revision %04x %04x\n", 
diff --git a/drivers/net/wireless/orinoco.c b/drivers/net/wireless/orinoco.c
index 062286dc8e15..ca6c2da7bc5d 100644
--- a/drivers/net/wireless/orinoco.c
+++ b/drivers/net/wireless/orinoco.c
@@ -2232,6 +2232,7 @@ static int orinoco_init(struct net_device *dev)
 	struct hermes_idstring nickbuf;
 	u16 reclen;
 	int len;
+	DECLARE_MAC_BUF(mac);
 
 	/* No need to lock, the hw_unavailable flag is already set in
 	 * alloc_orinocodev() */
@@ -2274,10 +2275,8 @@ static int orinoco_init(struct net_device *dev)
 		goto out;
 	}
 
-	printk(KERN_DEBUG "%s: MAC address %02X:%02X:%02X:%02X:%02X:%02X\n",
-	       dev->name, dev->dev_addr[0], dev->dev_addr[1],
-	       dev->dev_addr[2], dev->dev_addr[3], dev->dev_addr[4],
-	       dev->dev_addr[5]);
+	printk(KERN_DEBUG "%s: MAC address %s\n",
+	       dev->name, print_mac(mac, dev->dev_addr));
 
 	/* Get the station name */
 	err = hermes_read_ltv(hw, USER_BAP, HERMES_RID_CNFOWNNAME,
diff --git a/drivers/net/wireless/orinoco_cs.c b/drivers/net/wireless/orinoco_cs.c
index d1e502236b2a..8b7f5768a103 100644
--- a/drivers/net/wireless/orinoco_cs.c
+++ b/drivers/net/wireless/orinoco_cs.c
@@ -313,7 +313,6 @@ orinoco_cs_config(struct pcmcia_device *link)
 	/* Ok, we have the configuration, prepare to register the netdev */
 	dev->base_addr = link->io.BasePort1;
 	dev->irq = link->irq.AssignedIRQ;
-	SET_MODULE_OWNER(dev);
 	card->node.major = card->node.minor = 0;
 
 	SET_NETDEV_DEV(dev, &handle_to_dev(link));
diff --git a/drivers/net/wireless/orinoco_nortel.c b/drivers/net/wireless/orinoco_nortel.c
index eaf3d13b851c..35ec5fcf81a6 100644
--- a/drivers/net/wireless/orinoco_nortel.c
+++ b/drivers/net/wireless/orinoco_nortel.c
@@ -193,7 +193,6 @@ static int orinoco_nortel_init_one(struct pci_dev *pdev,
 	card = priv->card;
 	card->bridge_io = bridge_io;
 	card->attr_io = attr_io;
-	SET_MODULE_OWNER(dev);
 	SET_NETDEV_DEV(dev, &pdev->dev);
 
 	hermes_struct_init(&priv->hw, hermes_io, HERMES_16BIT_REGSPACING);
diff --git a/drivers/net/wireless/orinoco_pci.c b/drivers/net/wireless/orinoco_pci.c
index 97a8b4ff32bd..2547d5dac0d3 100644
--- a/drivers/net/wireless/orinoco_pci.c
+++ b/drivers/net/wireless/orinoco_pci.c
@@ -148,7 +148,6 @@ static int orinoco_pci_init_one(struct pci_dev *pdev,
 
 	priv = netdev_priv(dev);
 	card = priv->card;
-	SET_MODULE_OWNER(dev);
 	SET_NETDEV_DEV(dev, &pdev->dev);
 
 	hermes_struct_init(&priv->hw, hermes_io, HERMES_32BIT_REGSPACING);
diff --git a/drivers/net/wireless/orinoco_plx.c b/drivers/net/wireless/orinoco_plx.c
index 31162ac25a92..98fe165337d1 100644
--- a/drivers/net/wireless/orinoco_plx.c
+++ b/drivers/net/wireless/orinoco_plx.c
@@ -232,7 +232,6 @@ static int orinoco_plx_init_one(struct pci_dev *pdev,
 	card = priv->card;
 	card->bridge_io = bridge_io;
 	card->attr_io = attr_io;
-	SET_MODULE_OWNER(dev);
 	SET_NETDEV_DEV(dev, &pdev->dev);
 
 	hermes_struct_init(&priv->hw, hermes_io, HERMES_16BIT_REGSPACING);
diff --git a/drivers/net/wireless/orinoco_tmd.c b/drivers/net/wireless/orinoco_tmd.c
index 7c7b960c91df..df493185a4af 100644
--- a/drivers/net/wireless/orinoco_tmd.c
+++ b/drivers/net/wireless/orinoco_tmd.c
@@ -134,7 +134,6 @@ static int orinoco_tmd_init_one(struct pci_dev *pdev,
 	priv = netdev_priv(dev);
 	card = priv->card;
 	card->bridge_io = bridge_io;
-	SET_MODULE_OWNER(dev);
 	SET_NETDEV_DEV(dev, &pdev->dev);
 
 	hermes_struct_init(&priv->hw, hermes_io, HERMES_16BIT_REGSPACING);
@@ -190,7 +189,7 @@ static int orinoco_tmd_init_one(struct pci_dev *pdev,
 static void __devexit orinoco_tmd_remove_one(struct pci_dev *pdev)
 {
 	struct net_device *dev = pci_get_drvdata(pdev);
-	struct orinoco_private *priv = dev->priv;
+	struct orinoco_private *priv = netdev_priv(dev);
 	struct orinoco_pci_card *card = priv->card;
 
 	unregister_netdev(dev);
diff --git a/drivers/net/wireless/p54.h b/drivers/net/wireless/p54.h
new file mode 100644
index 000000000000..744c866066c5
--- /dev/null
+++ b/drivers/net/wireless/p54.h
@@ -0,0 +1,81 @@
+#ifndef PRISM54_H
+#define PRISM54_H
+
+/*
+ * Shared defines for all mac80211 Prism54 code
+ *
+ * Copyright (c) 2006, Michael Wu <flamingice@sourmilk.net>
+ *
+ * Based on the islsm (softmac prism54) driver, which is:
+ * Copyright 2004-2006 Jean-Baptiste Note <jbnote@gmail.com>, et al.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+enum control_frame_types {
+	P54_CONTROL_TYPE_FILTER_SET = 0,
+	P54_CONTROL_TYPE_CHANNEL_CHANGE,
+	P54_CONTROL_TYPE_FREQDONE,
+	P54_CONTROL_TYPE_DCFINIT,
+	P54_CONTROL_TYPE_FREEQUEUE = 7,
+	P54_CONTROL_TYPE_TXDONE,
+	P54_CONTROL_TYPE_PING,
+	P54_CONTROL_TYPE_STAT_READBACK,
+	P54_CONTROL_TYPE_BBP,
+	P54_CONTROL_TYPE_EEPROM_READBACK,
+	P54_CONTROL_TYPE_LED
+};
+
+struct p54_control_hdr {
+	__le16 magic1;
+	__le16 len;
+	__le32 req_id;
+	__le16 type;	/* enum control_frame_types */
+	u8 retry1;
+	u8 retry2;
+	u8 data[0];
+} __attribute__ ((packed));
+
+#define EEPROM_READBACK_LEN (sizeof(struct p54_control_hdr) + 4 /* p54_eeprom_lm86 */)
+#define MAX_RX_SIZE (IEEE80211_MAX_RTS_THRESHOLD + sizeof(struct p54_control_hdr) + 20 /* length of struct p54_rx_hdr */ + 16 )
+
+#define ISL38XX_DEV_FIRMWARE_ADDR 0x20000
+
+struct p54_common {
+	u32 rx_start;
+	u32 rx_end;
+	struct sk_buff_head tx_queue;
+	void (*tx)(struct ieee80211_hw *dev, struct p54_control_hdr *data,
+		   size_t len, int free_on_tx);
+	int (*open)(struct ieee80211_hw *dev);
+	void (*stop)(struct ieee80211_hw *dev);
+	int mode;
+	u8 mac_addr[ETH_ALEN];
+	u8 bssid[ETH_ALEN];
+	struct pda_iq_autocal_entry *iq_autocal;
+	unsigned int iq_autocal_len;
+	struct pda_channel_output_limit *output_limit;
+	unsigned int output_limit_len;
+	struct pda_pa_curve_data *curve_data;
+	__le16 rxhw;
+	u8 version;
+	unsigned int tx_hdr_len;
+	void *cached_vdcf;
+	unsigned int fw_var;
+	/* FIXME: this channels/modes/rates stuff sucks */
+	struct ieee80211_channel channels[14];
+	struct ieee80211_rate rates[12];
+	struct ieee80211_hw_mode modes[2];
+	struct ieee80211_tx_queue_stats tx_stats;
+};
+
+int p54_rx(struct ieee80211_hw *dev, struct sk_buff *skb);
+void p54_parse_firmware(struct ieee80211_hw *dev, const struct firmware *fw);
+int p54_parse_eeprom(struct ieee80211_hw *dev, void *eeprom, int len);
+void p54_fill_eeprom_readback(struct p54_control_hdr *hdr);
+struct ieee80211_hw *p54_init_common(size_t priv_data_len);
+void p54_free_common(struct ieee80211_hw *dev);
+
+#endif /* PRISM54_H */
diff --git a/drivers/net/wireless/p54common.c b/drivers/net/wireless/p54common.c
new file mode 100644
index 000000000000..2c63cf0ad2cd
--- /dev/null
+++ b/drivers/net/wireless/p54common.c
@@ -0,0 +1,1019 @@
+
+/*
+ * Common code for mac80211 Prism54 drivers
+ *
+ * Copyright (c) 2006, Michael Wu <flamingice@sourmilk.net>
+ * Copyright (c) 2007, Christian Lamparter <chunkeey@web.de>
+ *
+ * Based on the islsm (softmac prism54) driver, which is:
+ * Copyright 2004-2006 Jean-Baptiste Note <jbnote@gmail.com>, et al.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/init.h>
+#include <linux/firmware.h>
+#include <linux/etherdevice.h>
+
+#include <net/mac80211.h>
+
+#include "p54.h"
+#include "p54common.h"
+
+MODULE_AUTHOR("Michael Wu <flamingice@sourmilk.net>");
+MODULE_DESCRIPTION("Softmac Prism54 common code");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("prism54common");
+
+void p54_parse_firmware(struct ieee80211_hw *dev, const struct firmware *fw)
+{
+	struct p54_common *priv = dev->priv;
+	struct bootrec_exp_if *exp_if;
+	struct bootrec *bootrec;
+	u32 *data = (u32 *)fw->data;
+	u32 *end_data = (u32 *)fw->data + (fw->size >> 2);
+	u8 *fw_version = NULL;
+	size_t len;
+	int i;
+
+	if (priv->rx_start)
+		return;
+
+	while (data < end_data && *data)
+		data++;
+
+	while (data < end_data && !*data)
+		data++;
+
+	bootrec = (struct bootrec *) data;
+
+	while (bootrec->data <= end_data &&
+	       (bootrec->data + (len = le32_to_cpu(bootrec->len))) <= end_data) {
+		u32 code = le32_to_cpu(bootrec->code);
+		switch (code) {
+		case BR_CODE_COMPONENT_ID:
+			switch (be32_to_cpu(*bootrec->data)) {
+			case FW_FMAC:
+				printk(KERN_INFO "p54: FreeMAC firmware\n");
+				break;
+			case FW_LM20:
+				printk(KERN_INFO "p54: LM20 firmware\n");
+				break;
+			case FW_LM86:
+				printk(KERN_INFO "p54: LM86 firmware\n");
+				break;
+			case FW_LM87:
+				printk(KERN_INFO "p54: LM87 firmware - not supported yet!\n");
+				break;
+			default:
+				printk(KERN_INFO "p54: unknown firmware\n");
+				break;
+			}
+			break;
+		case BR_CODE_COMPONENT_VERSION:
+			/* 24 bytes should be enough for all firmwares */
+			if (strnlen((unsigned char*)bootrec->data, 24) < 24)
+				fw_version = (unsigned char*)bootrec->data;
+			break;
+		case BR_CODE_DESCR:
+			priv->rx_start = le32_to_cpu(bootrec->data[1]);
+			/* FIXME add sanity checking */
+			priv->rx_end = le32_to_cpu(bootrec->data[2]) - 0x3500;
+			break;
+		case BR_CODE_EXPOSED_IF:
+			exp_if = (struct bootrec_exp_if *) bootrec->data;
+			for (i = 0; i < (len * sizeof(*exp_if) / 4); i++)
+				if (exp_if[i].if_id == 0x1a)
+					priv->fw_var = le16_to_cpu(exp_if[i].variant);
+			break;
+		case BR_CODE_DEPENDENT_IF:
+			break;
+		case BR_CODE_END_OF_BRA:
+		case LEGACY_BR_CODE_END_OF_BRA:
+			end_data = NULL;
+			break;
+		default:
+			break;
+		}
+		bootrec = (struct bootrec *)&bootrec->data[len];
+	}
+
+	if (fw_version)
+		printk(KERN_INFO "p54: FW rev %s - Softmac protocol %x.%x\n",
+			fw_version, priv->fw_var >> 8, priv->fw_var & 0xff);
+
+	if (priv->fw_var >= 0x300) {
+		/* Firmware supports QoS, use it! */
+		priv->tx_stats.data[0].limit = 3;
+		priv->tx_stats.data[1].limit = 4;
+		priv->tx_stats.data[2].limit = 3;
+		priv->tx_stats.data[3].limit = 1;
+		dev->queues = 4;
+	}
+}
+EXPORT_SYMBOL_GPL(p54_parse_firmware);
+
+static int p54_convert_rev0_to_rev1(struct ieee80211_hw *dev,
+				    struct pda_pa_curve_data *curve_data)
+{
+	struct p54_common *priv = dev->priv;
+	struct pda_pa_curve_data_sample_rev1 *rev1;
+	struct pda_pa_curve_data_sample_rev0 *rev0;
+	size_t cd_len = sizeof(*curve_data) +
+		(curve_data->points_per_channel*sizeof(*rev1) + 2) *
+		 curve_data->channels;
+	unsigned int i, j;
+	void *source, *target;
+
+	priv->curve_data = kmalloc(cd_len, GFP_KERNEL);
+	if (!priv->curve_data)
+		return -ENOMEM;
+
+	memcpy(priv->curve_data, curve_data, sizeof(*curve_data));
+	source = curve_data->data;
+	target = priv->curve_data->data;
+	for (i = 0; i < curve_data->channels; i++) {
+		__le16 *freq = source;
+		source += sizeof(__le16);
+		*((__le16 *)target) = *freq;
+		target += sizeof(__le16);
+		for (j = 0; j < curve_data->points_per_channel; j++) {
+			rev1 = target;
+			rev0 = source;
+
+			rev1->rf_power = rev0->rf_power;
+			rev1->pa_detector = rev0->pa_detector;
+			rev1->data_64qam = rev0->pcv;
+			/* "invent" the points for the other modulations */
+#define SUB(x,y) (u8)((x) - (y)) > (x) ? 0 : (x) - (y)
+			rev1->data_16qam = SUB(rev0->pcv, 12);
+			rev1->data_qpsk  = SUB(rev1->data_16qam, 12);
+			rev1->data_bpsk  = SUB(rev1->data_qpsk, 12);
+			rev1->data_barker= SUB(rev1->data_bpsk, 14);
+#undef SUB
+			target += sizeof(*rev1);
+			source += sizeof(*rev0);
+		}
+	}
+
+	return 0;
+}
+
+int p54_parse_eeprom(struct ieee80211_hw *dev, void *eeprom, int len)
+{
+	struct p54_common *priv = dev->priv;
+	struct eeprom_pda_wrap *wrap = NULL;
+	struct pda_entry *entry;
+	int i = 0;
+	unsigned int data_len, entry_len;
+	void *tmp;
+	int err;
+
+	wrap = (struct eeprom_pda_wrap *) eeprom;
+	entry = (void *)wrap->data + wrap->len;
+	i += 2;
+	i += le16_to_cpu(entry->len)*2;
+	while (i < len) {
+		entry_len = le16_to_cpu(entry->len);
+		data_len = ((entry_len - 1) << 1);
+		switch (le16_to_cpu(entry->code)) {
+		case PDR_MAC_ADDRESS:
+			SET_IEEE80211_PERM_ADDR(dev, entry->data);
+			break;
+		case PDR_PRISM_PA_CAL_OUTPUT_POWER_LIMITS:
+			if (data_len < 2) {
+				err = -EINVAL;
+				goto err;
+			}
+
+			if (2 + entry->data[1]*sizeof(*priv->output_limit) > data_len) {
+				err = -EINVAL;
+				goto err;
+			}
+
+			priv->output_limit = kmalloc(entry->data[1] *
+				sizeof(*priv->output_limit), GFP_KERNEL);
+
+			if (!priv->output_limit) {
+				err = -ENOMEM;
+				goto err;
+			}
+
+			memcpy(priv->output_limit, &entry->data[2],
+			       entry->data[1]*sizeof(*priv->output_limit));
+			priv->output_limit_len = entry->data[1];
+			break;
+		case PDR_PRISM_PA_CAL_CURVE_DATA:
+			if (data_len < sizeof(struct pda_pa_curve_data)) {
+				err = -EINVAL;
+				goto err;
+			}
+
+			if (((struct pda_pa_curve_data *)entry->data)->cal_method_rev) {
+				priv->curve_data = kmalloc(data_len, GFP_KERNEL);
+				if (!priv->curve_data) {
+					err = -ENOMEM;
+					goto err;
+				}
+
+				memcpy(priv->curve_data, entry->data, data_len);
+			} else {
+				err = p54_convert_rev0_to_rev1(dev, (struct pda_pa_curve_data *)entry->data);
+				if (err)
+					goto err;
+			}
+
+			break;
+		case PDR_PRISM_ZIF_TX_IQ_CALIBRATION:
+			priv->iq_autocal = kmalloc(data_len, GFP_KERNEL);
+			if (!priv->iq_autocal) {
+				err = -ENOMEM;
+				goto err;
+			}
+
+			memcpy(priv->iq_autocal, entry->data, data_len);
+			priv->iq_autocal_len = data_len / sizeof(struct pda_iq_autocal_entry);
+			break;
+		case PDR_INTERFACE_LIST:
+			tmp = entry->data;
+			while ((u8 *)tmp < entry->data + data_len) {
+				struct bootrec_exp_if *exp_if = tmp;
+				if (le16_to_cpu(exp_if->if_id) == 0xF)
+					priv->rxhw = exp_if->variant & cpu_to_le16(0x07);
+				tmp += sizeof(struct bootrec_exp_if);
+			}
+			break;
+		case PDR_HARDWARE_PLATFORM_COMPONENT_ID:
+			priv->version = *(u8 *)(entry->data + 1);
+			break;
+		case PDR_END:
+			i = len;
+			break;
+		}
+
+		entry = (void *)entry + (entry_len + 1)*2;
+		i += 2;
+		i += entry_len*2;
+	}
+
+	if (!priv->iq_autocal || !priv->output_limit || !priv->curve_data) {
+		printk(KERN_ERR "p54: not all required entries found in eeprom!\n");
+		err = -EINVAL;
+		goto err;
+	}
+
+	return 0;
+
+  err:
+	if (priv->iq_autocal) {
+		kfree(priv->iq_autocal);
+		priv->iq_autocal = NULL;
+	}
+
+	if (priv->output_limit) {
+		kfree(priv->output_limit);
+		priv->output_limit = NULL;
+	}
+
+	if (priv->curve_data) {
+		kfree(priv->curve_data);
+		priv->curve_data = NULL;
+	}
+
+	printk(KERN_ERR "p54: eeprom parse failed!\n");
+	return err;
+}
+EXPORT_SYMBOL_GPL(p54_parse_eeprom);
+
+void p54_fill_eeprom_readback(struct p54_control_hdr *hdr)
+{
+	struct p54_eeprom_lm86 *eeprom_hdr;
+
+	hdr->magic1 = cpu_to_le16(0x8000);
+	hdr->len = cpu_to_le16(sizeof(*eeprom_hdr) + 0x2000);
+	hdr->type = cpu_to_le16(P54_CONTROL_TYPE_EEPROM_READBACK);
+	hdr->retry1 = hdr->retry2 = 0;
+	eeprom_hdr = (struct p54_eeprom_lm86 *) hdr->data;
+	eeprom_hdr->offset = 0x0;
+	eeprom_hdr->len = cpu_to_le16(0x2000);
+}
+EXPORT_SYMBOL_GPL(p54_fill_eeprom_readback);
+
+static void p54_rx_data(struct ieee80211_hw *dev, struct sk_buff *skb)
+{
+	struct p54_rx_hdr *hdr = (struct p54_rx_hdr *) skb->data;
+	struct ieee80211_rx_status rx_status = {0};
+	u16 freq = le16_to_cpu(hdr->freq);
+
+	rx_status.ssi = hdr->rssi;
+	rx_status.rate = hdr->rate & 0x1f; /* report short preambles & CCK too */
+	rx_status.channel = freq == 2484 ? 14 : (freq - 2407)/5;
+	rx_status.freq = freq;
+	rx_status.phymode = MODE_IEEE80211G;
+	rx_status.antenna = hdr->antenna;
+	rx_status.mactime = le64_to_cpu(hdr->timestamp);
+
+	skb_pull(skb, sizeof(*hdr));
+	skb_trim(skb, le16_to_cpu(hdr->len));
+
+	ieee80211_rx_irqsafe(dev, skb, &rx_status);
+}
+
+static void inline p54_wake_free_queues(struct ieee80211_hw *dev)
+{
+	struct p54_common *priv = dev->priv;
+	int i;
+
+	/* ieee80211_start_queues is great if all queues are really empty.
+	 * But, what if some are full? */
+
+	for (i = 0; i < dev->queues; i++)
+		if (priv->tx_stats.data[i].len < priv->tx_stats.data[i].limit)
+			ieee80211_wake_queue(dev, i);
+}
+
+static void p54_rx_frame_sent(struct ieee80211_hw *dev, struct sk_buff *skb)
+{
+	struct p54_common *priv = dev->priv;
+	struct p54_control_hdr *hdr = (struct p54_control_hdr *) skb->data;
+	struct p54_frame_sent_hdr *payload = (struct p54_frame_sent_hdr *) hdr->data;
+	struct sk_buff *entry = (struct sk_buff *) priv->tx_queue.next;
+	u32 addr = le32_to_cpu(hdr->req_id) - 0x70;
+	struct memrecord *range = NULL;
+	u32 freed = 0;
+	u32 last_addr = priv->rx_start;
+
+	while (entry != (struct sk_buff *)&priv->tx_queue) {
+		range = (struct memrecord *)&entry->cb;
+		if (range->start_addr == addr) {
+			struct ieee80211_tx_status status = {{0}};
+			struct p54_control_hdr *entry_hdr;
+			struct p54_tx_control_allocdata *entry_data;
+			int pad = 0;
+
+			if (entry->next != (struct sk_buff *)&priv->tx_queue)
+				freed = ((struct memrecord *)&entry->next->cb)->start_addr - last_addr;
+			else
+				freed = priv->rx_end - last_addr;
+
+			last_addr = range->end_addr;
+			__skb_unlink(entry, &priv->tx_queue);
+			if (!range->control) {
+				kfree_skb(entry);
+				break;
+			}
+			memcpy(&status.control, range->control,
+			       sizeof(status.control));
+			kfree(range->control);
+			priv->tx_stats.data[status.control.queue].len--;
+
+			entry_hdr = (struct p54_control_hdr *) entry->data;
+			entry_data = (struct p54_tx_control_allocdata *) entry_hdr->data;
+			if ((entry_hdr->magic1 & cpu_to_le16(0x4000)) != 0)
+				pad = entry_data->align[0];
+
+			if (!status.control.flags & IEEE80211_TXCTL_NO_ACK) {
+				if (!(payload->status & 0x01))
+					status.flags |= IEEE80211_TX_STATUS_ACK;
+				else
+					status.excessive_retries = 1;
+			}
+			status.retry_count = payload->retries - 1;
+			status.ack_signal = le16_to_cpu(payload->ack_rssi);
+			skb_pull(entry, sizeof(*hdr) + pad + sizeof(*entry_data));
+			ieee80211_tx_status_irqsafe(dev, entry, &status);
+			break;
+		} else
+			last_addr = range->end_addr;
+		entry = entry->next;
+	}
+
+	if (freed >= IEEE80211_MAX_RTS_THRESHOLD + 0x170 +
+	    sizeof(struct p54_control_hdr))
+		p54_wake_free_queues(dev);
+}
+
+static void p54_rx_control(struct ieee80211_hw *dev, struct sk_buff *skb)
+{
+	struct p54_control_hdr *hdr = (struct p54_control_hdr *) skb->data;
+
+	switch (le16_to_cpu(hdr->type)) {
+	case P54_CONTROL_TYPE_TXDONE:
+		p54_rx_frame_sent(dev, skb);
+		break;
+	case P54_CONTROL_TYPE_BBP:
+		break;
+	default:
+		printk(KERN_DEBUG "%s: not handling 0x%02x type control frame\n",
+		       wiphy_name(dev->wiphy), le16_to_cpu(hdr->type));
+		break;
+	}
+}
+
+/* returns zero if skb can be reused */
+int p54_rx(struct ieee80211_hw *dev, struct sk_buff *skb)
+{
+	u8 type = le16_to_cpu(*((__le16 *)skb->data)) >> 8;
+	switch (type) {
+	case 0x00:
+	case 0x01:
+		p54_rx_data(dev, skb);
+		return -1;
+	case 0x4d:
+		/* TODO: do something better... but then again, I've never seen this happen */
+		printk(KERN_ERR "%s: Received fault. Probably need to restart hardware now..\n",
+		       wiphy_name(dev->wiphy));
+		break;
+	case 0x80:
+		p54_rx_control(dev, skb);
+		break;
+	default:
+		printk(KERN_ERR "%s: unknown frame RXed (0x%02x)\n",
+		       wiphy_name(dev->wiphy), type);
+		break;
+	}
+	return 0;
+}
+EXPORT_SYMBOL_GPL(p54_rx);
+
+/*
+ * So, the firmware is somewhat stupid and doesn't know what places in its
+ * memory incoming data should go to. By poking around in the firmware, we
+ * can find some unused memory to upload our packets to. However, data that we
+ * want the card to TX needs to stay intact until the card has told us that
+ * it is done with it. This function finds empty places we can upload to and
+ * marks allocated areas as reserved if necessary. p54_rx_frame_sent frees
+ * allocated areas.
+ */
+static void p54_assign_address(struct ieee80211_hw *dev, struct sk_buff *skb,
+			       struct p54_control_hdr *data, u32 len,
+			       struct ieee80211_tx_control *control)
+{
+	struct p54_common *priv = dev->priv;
+	struct sk_buff *entry = priv->tx_queue.next;
+	struct sk_buff *target_skb = NULL;
+	struct memrecord *range;
+	u32 last_addr = priv->rx_start;
+	u32 largest_hole = 0;
+	u32 target_addr = priv->rx_start;
+	unsigned long flags;
+	unsigned int left;
+	len = (len + 0x170 + 3) & ~0x3; /* 0x70 headroom, 0x100 tailroom */
+
+	spin_lock_irqsave(&priv->tx_queue.lock, flags);
+	left = skb_queue_len(&priv->tx_queue);
+	while (left--) {
+		u32 hole_size;
+		range = (struct memrecord *)&entry->cb;
+		hole_size = range->start_addr - last_addr;
+		if (!target_skb && hole_size >= len) {
+			target_skb = entry->prev;
+			hole_size -= len;
+			target_addr = last_addr;
+		}
+		largest_hole = max(largest_hole, hole_size);
+		last_addr = range->end_addr;
+		entry = entry->next;
+	}
+	if (!target_skb && priv->rx_end - last_addr >= len) {
+		target_skb = priv->tx_queue.prev;
+		largest_hole = max(largest_hole, priv->rx_end - last_addr - len);
+		if (!skb_queue_empty(&priv->tx_queue)) {
+			range = (struct memrecord *)&target_skb->cb;
+			target_addr = range->end_addr;
+		}
+	} else
+		largest_hole = max(largest_hole, priv->rx_end - last_addr);
+
+	if (skb) {
+		range = (struct memrecord *)&skb->cb;
+		range->start_addr = target_addr;
+		range->end_addr = target_addr + len;
+		range->control = control;
+		__skb_queue_after(&priv->tx_queue, target_skb, skb);
+		if (largest_hole < IEEE80211_MAX_RTS_THRESHOLD + 0x170 +
+				   sizeof(struct p54_control_hdr))
+			ieee80211_stop_queues(dev);
+	}
+	spin_unlock_irqrestore(&priv->tx_queue.lock, flags);
+
+	data->req_id = cpu_to_le32(target_addr + 0x70);
+}
+
+static int p54_tx(struct ieee80211_hw *dev, struct sk_buff *skb,
+		  struct ieee80211_tx_control *control)
+{
+	struct ieee80211_tx_queue_stats_data *current_queue;
+	struct p54_common *priv = dev->priv;
+	struct p54_control_hdr *hdr;
+	struct p54_tx_control_allocdata *txhdr;
+	struct ieee80211_tx_control *control_copy;
+	size_t padding, len;
+	u8 rate;
+
+	current_queue = &priv->tx_stats.data[control->queue];
+	if (unlikely(current_queue->len > current_queue->limit))
+		return NETDEV_TX_BUSY;
+	current_queue->len++;
+	current_queue->count++;
+	if (current_queue->len == current_queue->limit)
+		ieee80211_stop_queue(dev, control->queue);
+
+	padding = (unsigned long)(skb->data - (sizeof(*hdr) + sizeof(*txhdr))) & 3;
+	len = skb->len;
+
+	control_copy = kmalloc(sizeof(*control), GFP_ATOMIC);
+	if (control_copy)
+		memcpy(control_copy, control, sizeof(*control));
+
+	txhdr = (struct p54_tx_control_allocdata *)
+			skb_push(skb, sizeof(*txhdr) + padding);
+	hdr = (struct p54_control_hdr *) skb_push(skb, sizeof(*hdr));
+
+	if (padding)
+		hdr->magic1 = cpu_to_le16(0x4010);
+	else
+		hdr->magic1 = cpu_to_le16(0x0010);
+	hdr->len = cpu_to_le16(len);
+	hdr->type = (control->flags & IEEE80211_TXCTL_NO_ACK) ? 0 : cpu_to_le16(1);
+	hdr->retry1 = hdr->retry2 = control->retry_limit;
+	p54_assign_address(dev, skb, hdr, skb->len, control_copy);
+
+	memset(txhdr->wep_key, 0x0, 16);
+	txhdr->padding = 0;
+	txhdr->padding2 = 0;
+
+	/* TODO: add support for alternate retry TX rates */
+	rate = control->tx_rate;
+	if (control->flags & IEEE80211_TXCTL_USE_RTS_CTS)
+		rate |= 0x40;
+	else if (control->flags & IEEE80211_TXCTL_USE_CTS_PROTECT)
+		rate |= 0x20;
+	memset(txhdr->rateset, rate, 8);
+	txhdr->wep_key_present = 0;
+	txhdr->wep_key_len = 0;
+	txhdr->frame_type = cpu_to_le32(control->queue + 4);
+	txhdr->magic4 = 0;
+	txhdr->antenna = (control->antenna_sel_tx == 0) ?
+		2 : control->antenna_sel_tx - 1;
+	txhdr->output_power = 0x7f; // HW Maximum
+	txhdr->magic5 = (control->flags & IEEE80211_TXCTL_NO_ACK) ?
+		0 : ((rate > 0x3) ? cpu_to_le32(0x33) : cpu_to_le32(0x23));
+	if (padding)
+		txhdr->align[0] = padding;
+
+	priv->tx(dev, hdr, skb->len, 0);
+	return 0;
+}
+
+static int p54_set_filter(struct ieee80211_hw *dev, u16 filter_type,
+			  const u8 *dst, const u8 *src, u8 antenna,
+			  u32 magic3, u32 magic8, u32 magic9)
+{
+	struct p54_common *priv = dev->priv;
+	struct p54_control_hdr *hdr;
+	struct p54_tx_control_filter *filter;
+
+	hdr = kzalloc(sizeof(*hdr) + sizeof(*filter) +
+		      priv->tx_hdr_len, GFP_KERNEL);
+	if (!hdr)
+		return -ENOMEM;
+
+	hdr = (void *)hdr + priv->tx_hdr_len;
+
+	filter = (struct p54_tx_control_filter *) hdr->data;
+	hdr->magic1 = cpu_to_le16(0x8001);
+	hdr->len = cpu_to_le16(sizeof(*filter));
+	p54_assign_address(dev, NULL, hdr, sizeof(*hdr) + sizeof(*filter), NULL);
+	hdr->type = cpu_to_le16(P54_CONTROL_TYPE_FILTER_SET);
+
+	filter->filter_type = cpu_to_le16(filter_type);
+	memcpy(filter->dst, dst, ETH_ALEN);
+	if (!src)
+		memset(filter->src, ~0, ETH_ALEN);
+	else
+		memcpy(filter->src, src, ETH_ALEN);
+	filter->antenna = antenna;
+	filter->magic3 = cpu_to_le32(magic3);
+	filter->rx_addr = cpu_to_le32(priv->rx_end);
+	filter->max_rx = cpu_to_le16(0x0620);	/* FIXME: for usb ver 1.. maybe */
+	filter->rxhw = priv->rxhw;
+	filter->magic8 = cpu_to_le16(magic8);
+	filter->magic9 = cpu_to_le16(magic9);
+
+	priv->tx(dev, hdr, sizeof(*hdr) + sizeof(*filter), 1);
+	return 0;
+}
+
+static int p54_set_freq(struct ieee80211_hw *dev, __le16 freq)
+{
+	struct p54_common *priv = dev->priv;
+	struct p54_control_hdr *hdr;
+	struct p54_tx_control_channel *chan;
+	unsigned int i;
+	size_t payload_len = sizeof(*chan) + sizeof(u32)*2 +
+			     sizeof(*chan->curve_data) *
+			     priv->curve_data->points_per_channel;
+	void *entry;
+
+	hdr = kzalloc(sizeof(*hdr) + payload_len +
+		      priv->tx_hdr_len, GFP_KERNEL);
+	if (!hdr)
+		return -ENOMEM;
+
+	hdr = (void *)hdr + priv->tx_hdr_len;
+
+	chan = (struct p54_tx_control_channel *) hdr->data;
+
+	hdr->magic1 = cpu_to_le16(0x8001);
+	hdr->len = cpu_to_le16(sizeof(*chan));
+	hdr->type = cpu_to_le16(P54_CONTROL_TYPE_CHANNEL_CHANGE);
+	p54_assign_address(dev, NULL, hdr, sizeof(*hdr) + payload_len, NULL);
+
+	chan->magic1 = cpu_to_le16(0x1);
+	chan->magic2 = cpu_to_le16(0x0);
+
+	for (i = 0; i < priv->iq_autocal_len; i++) {
+		if (priv->iq_autocal[i].freq != freq)
+			continue;
+
+		memcpy(&chan->iq_autocal, &priv->iq_autocal[i],
+		       sizeof(*priv->iq_autocal));
+		break;
+	}
+	if (i == priv->iq_autocal_len)
+		goto err;
+
+	for (i = 0; i < priv->output_limit_len; i++) {
+		if (priv->output_limit[i].freq != freq)
+			continue;
+
+		chan->val_barker = 0x38;
+		chan->val_bpsk = priv->output_limit[i].val_bpsk;
+		chan->val_qpsk = priv->output_limit[i].val_qpsk;
+		chan->val_16qam = priv->output_limit[i].val_16qam;
+		chan->val_64qam = priv->output_limit[i].val_64qam;
+		break;
+	}
+	if (i == priv->output_limit_len)
+		goto err;
+
+	chan->pa_points_per_curve = priv->curve_data->points_per_channel;
+
+	entry = priv->curve_data->data;
+	for (i = 0; i < priv->curve_data->channels; i++) {
+		if (*((__le16 *)entry) != freq) {
+			entry += sizeof(__le16);
+			entry += sizeof(struct pda_pa_curve_data_sample_rev1) *
+				 chan->pa_points_per_curve;
+			continue;
+		}
+
+		entry += sizeof(__le16);
+		memcpy(chan->curve_data, entry, sizeof(*chan->curve_data) *
+		       chan->pa_points_per_curve);
+		break;
+	}
+
+	memcpy(hdr->data + payload_len - 4, &chan->val_bpsk, 4);
+
+	priv->tx(dev, hdr, sizeof(*hdr) + payload_len, 1);
+	return 0;
+
+ err:
+	printk(KERN_ERR "%s: frequency change failed\n", wiphy_name(dev->wiphy));
+	kfree(hdr);
+	return -EINVAL;
+}
+
+static int p54_set_leds(struct ieee80211_hw *dev, int mode, int link, int act)
+{
+	struct p54_common *priv = dev->priv;
+	struct p54_control_hdr *hdr;
+	struct p54_tx_control_led *led;
+
+	hdr = kzalloc(sizeof(*hdr) + sizeof(*led) +
+		      priv->tx_hdr_len, GFP_KERNEL);
+	if (!hdr)
+		return -ENOMEM;
+
+	hdr = (void *)hdr + priv->tx_hdr_len;
+	hdr->magic1 = cpu_to_le16(0x8001);
+	hdr->len = cpu_to_le16(sizeof(*led));
+	hdr->type = cpu_to_le16(P54_CONTROL_TYPE_LED);
+	p54_assign_address(dev, NULL, hdr, sizeof(*hdr) + sizeof(*led), NULL);
+
+	led = (struct p54_tx_control_led *) hdr->data;
+	led->mode = cpu_to_le16(mode);
+	led->led_permanent = cpu_to_le16(link);
+	led->led_temporary = cpu_to_le16(act);
+	led->duration = cpu_to_le16(1000);
+
+	priv->tx(dev, hdr, sizeof(*hdr) + sizeof(*led), 1);
+
+	return 0;
+}
+
+#define P54_SET_QUEUE(queue, ai_fs, cw_min, cw_max, burst)	\
+do {	 							\
+	queue.aifs = cpu_to_le16(ai_fs);			\
+	queue.cwmin = cpu_to_le16(cw_min);			\
+	queue.cwmax = cpu_to_le16(cw_max);			\
+	queue.txop = (burst == 0) ? 				\
+		0 : cpu_to_le16((burst * 100) / 32 + 1);	\
+} while(0)
+
+static void p54_init_vdcf(struct ieee80211_hw *dev)
+{
+	struct p54_common *priv = dev->priv;
+	struct p54_control_hdr *hdr;
+	struct p54_tx_control_vdcf *vdcf;
+
+	/* all USB V1 adapters need a extra headroom */
+	hdr = (void *)priv->cached_vdcf + priv->tx_hdr_len;
+	hdr->magic1 = cpu_to_le16(0x8001);
+	hdr->len = cpu_to_le16(sizeof(*vdcf));
+	hdr->type = cpu_to_le16(P54_CONTROL_TYPE_DCFINIT);
+	hdr->req_id = cpu_to_le32(priv->rx_start);
+
+	vdcf = (struct p54_tx_control_vdcf *) hdr->data;
+
+	P54_SET_QUEUE(vdcf->queue[0], 0x0002, 0x0003, 0x0007, 0x000f);
+	P54_SET_QUEUE(vdcf->queue[1], 0x0002, 0x0007, 0x000f, 0x001e);
+	P54_SET_QUEUE(vdcf->queue[2], 0x0002, 0x000f, 0x03ff, 0x0014);
+	P54_SET_QUEUE(vdcf->queue[3], 0x0007, 0x000f, 0x03ff, 0x0000);
+}
+
+static void p54_set_vdcf(struct ieee80211_hw *dev)
+{
+	struct p54_common *priv = dev->priv;
+	struct p54_control_hdr *hdr;
+	struct p54_tx_control_vdcf *vdcf;
+
+	hdr = (void *)priv->cached_vdcf + priv->tx_hdr_len;
+
+	p54_assign_address(dev, NULL, hdr, sizeof(*hdr) + sizeof(*vdcf), NULL);
+
+	vdcf = (struct p54_tx_control_vdcf *) hdr->data;
+
+	if (dev->conf.flags & IEEE80211_CONF_SHORT_SLOT_TIME) {
+		vdcf->slottime = 9;
+		vdcf->magic1 = 0x00;
+		vdcf->magic2 = 0x10;
+	} else {
+		vdcf->slottime = 20;
+		vdcf->magic1 = 0x0a;
+		vdcf->magic2 = 0x06;
+	}
+
+	/* (see prism54/isl_oid.h for further details) */
+	vdcf->frameburst = cpu_to_le16(0);
+
+	priv->tx(dev, hdr, sizeof(*hdr) + sizeof(*vdcf), 0);
+}
+
+static int p54_start(struct ieee80211_hw *dev)
+{
+	struct p54_common *priv = dev->priv;
+	int err;
+
+	err = priv->open(dev);
+	if (!err)
+		priv->mode = IEEE80211_IF_TYPE_MNTR;
+
+	return err;
+}
+
+static void p54_stop(struct ieee80211_hw *dev)
+{
+	struct p54_common *priv = dev->priv;
+	struct sk_buff *skb;
+	while ((skb = skb_dequeue(&priv->tx_queue))) {
+		struct memrecord *range = (struct memrecord *)&skb->cb;
+		if (range->control)
+			kfree(range->control);
+		kfree_skb(skb);
+	}
+	priv->stop(dev);
+	priv->mode = IEEE80211_IF_TYPE_INVALID;
+}
+
+static int p54_add_interface(struct ieee80211_hw *dev,
+			     struct ieee80211_if_init_conf *conf)
+{
+	struct p54_common *priv = dev->priv;
+
+	if (priv->mode != IEEE80211_IF_TYPE_MNTR)
+		return -EOPNOTSUPP;
+
+	switch (conf->type) {
+	case IEEE80211_IF_TYPE_STA:
+		priv->mode = conf->type;
+		break;
+	default:
+		return -EOPNOTSUPP;
+	}
+
+	memcpy(priv->mac_addr, conf->mac_addr, ETH_ALEN);
+
+	p54_set_filter(dev, 0, priv->mac_addr, NULL, 0, 1, 0, 0xF642);
+	p54_set_filter(dev, 0, priv->mac_addr, NULL, 1, 0, 0, 0xF642);
+
+	switch (conf->type) {
+	case IEEE80211_IF_TYPE_STA:
+		p54_set_filter(dev, 1, priv->mac_addr, NULL, 0, 0x15F, 0x1F4, 0);
+		break;
+	default:
+		BUG();	/* impossible */
+		break;
+	}
+
+	p54_set_leds(dev, 1, 0, 0);
+
+	return 0;
+}
+
+static void p54_remove_interface(struct ieee80211_hw *dev,
+				 struct ieee80211_if_init_conf *conf)
+{
+	struct p54_common *priv = dev->priv;
+	priv->mode = IEEE80211_IF_TYPE_MNTR;
+	memset(priv->mac_addr, 0, ETH_ALEN);
+	p54_set_filter(dev, 0, priv->mac_addr, NULL, 2, 0, 0, 0);
+}
+
+static int p54_config(struct ieee80211_hw *dev, struct ieee80211_conf *conf)
+{
+	int ret;
+
+	ret = p54_set_freq(dev, cpu_to_le16(conf->freq));
+	p54_set_vdcf(dev);
+	return ret;
+}
+
+static int p54_config_interface(struct ieee80211_hw *dev, int if_id,
+				struct ieee80211_if_conf *conf)
+{
+	struct p54_common *priv = dev->priv;
+
+	p54_set_filter(dev, 0, priv->mac_addr, conf->bssid, 0, 1, 0, 0xF642);
+	p54_set_filter(dev, 0, priv->mac_addr, conf->bssid, 2, 0, 0, 0);
+	p54_set_leds(dev, 1, !is_multicast_ether_addr(conf->bssid), 0);
+	memcpy(priv->bssid, conf->bssid, ETH_ALEN);
+	return 0;
+}
+
+static void p54_configure_filter(struct ieee80211_hw *dev,
+				 unsigned int changed_flags,
+				 unsigned int *total_flags,
+				 int mc_count, struct dev_mc_list *mclist)
+{
+	struct p54_common *priv = dev->priv;
+
+	*total_flags &= FIF_BCN_PRBRESP_PROMISC;
+
+	if (changed_flags & FIF_BCN_PRBRESP_PROMISC) {
+		if (*total_flags & FIF_BCN_PRBRESP_PROMISC)
+			p54_set_filter(dev, 0, priv->mac_addr,
+				       NULL, 2, 0, 0, 0);
+		else
+			p54_set_filter(dev, 0, priv->mac_addr,
+				       priv->bssid, 2, 0, 0, 0);
+	}
+}
+
+static int p54_conf_tx(struct ieee80211_hw *dev, int queue,
+		       const struct ieee80211_tx_queue_params *params)
+{
+	struct p54_common *priv = dev->priv;
+	struct p54_tx_control_vdcf *vdcf;
+
+	vdcf = (struct p54_tx_control_vdcf *)(((struct p54_control_hdr *)
+		((void *)priv->cached_vdcf + priv->tx_hdr_len))->data);
+
+	if ((params) && !((queue < 0) || (queue > 4))) {
+		P54_SET_QUEUE(vdcf->queue[queue], params->aifs,
+			params->cw_min, params->cw_max, params->burst_time);
+	} else
+		return -EINVAL;
+
+	p54_set_vdcf(dev);
+
+	return 0;
+}
+
+static int p54_get_stats(struct ieee80211_hw *dev,
+			 struct ieee80211_low_level_stats *stats)
+{
+	/* TODO */
+	return 0;
+}
+
+static int p54_get_tx_stats(struct ieee80211_hw *dev,
+			    struct ieee80211_tx_queue_stats *stats)
+{
+	struct p54_common *priv = dev->priv;
+	unsigned int i;
+
+	for (i = 0; i < dev->queues; i++)
+		memcpy(&stats->data[i], &priv->tx_stats.data[i],
+			sizeof(stats->data[i]));
+
+	return 0;
+}
+
+static const struct ieee80211_ops p54_ops = {
+	.tx			= p54_tx,
+	.start			= p54_start,
+	.stop			= p54_stop,
+	.add_interface		= p54_add_interface,
+	.remove_interface	= p54_remove_interface,
+	.config			= p54_config,
+	.config_interface	= p54_config_interface,
+	.configure_filter	= p54_configure_filter,
+	.conf_tx		= p54_conf_tx,
+	.get_stats		= p54_get_stats,
+	.get_tx_stats		= p54_get_tx_stats
+};
+
+struct ieee80211_hw *p54_init_common(size_t priv_data_len)
+{
+	struct ieee80211_hw *dev;
+	struct p54_common *priv;
+	int i;
+
+	dev = ieee80211_alloc_hw(priv_data_len, &p54_ops);
+	if (!dev)
+		return NULL;
+
+	priv = dev->priv;
+	priv->mode = IEEE80211_IF_TYPE_INVALID;
+	skb_queue_head_init(&priv->tx_queue);
+	memcpy(priv->channels, p54_channels, sizeof(p54_channels));
+	memcpy(priv->rates, p54_rates, sizeof(p54_rates));
+	priv->modes[1].mode = MODE_IEEE80211B;
+	priv->modes[1].num_rates = 4;
+	priv->modes[1].rates = priv->rates;
+	priv->modes[1].num_channels = ARRAY_SIZE(p54_channels);
+	priv->modes[1].channels = priv->channels;
+	priv->modes[0].mode = MODE_IEEE80211G;
+	priv->modes[0].num_rates = ARRAY_SIZE(p54_rates);
+	priv->modes[0].rates = priv->rates;
+	priv->modes[0].num_channels = ARRAY_SIZE(p54_channels);
+	priv->modes[0].channels = priv->channels;
+	dev->flags = IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING | /* not sure */
+		    IEEE80211_HW_RX_INCLUDES_FCS;
+	dev->channel_change_time = 1000;	/* TODO: find actual value */
+	dev->max_rssi = 127;
+
+	priv->tx_stats.data[0].limit = 5;
+	dev->queues = 1;
+
+	dev->extra_tx_headroom = sizeof(struct p54_control_hdr) + 4 +
+				 sizeof(struct p54_tx_control_allocdata);
+
+        priv->cached_vdcf = kzalloc(sizeof(struct p54_tx_control_vdcf) +
+              priv->tx_hdr_len + sizeof(struct p54_control_hdr), GFP_KERNEL);
+
+	if (!priv->cached_vdcf) {
+		ieee80211_free_hw(dev);
+		return NULL;
+	}
+
+	p54_init_vdcf(dev);
+
+	for (i = 0; i < 2; i++) {
+		if (ieee80211_register_hwmode(dev, &priv->modes[i])) {
+			kfree(priv->cached_vdcf);
+			ieee80211_free_hw(dev);
+			return NULL;
+		}
+	}
+
+	return dev;
+}
+EXPORT_SYMBOL_GPL(p54_init_common);
+
+void p54_free_common(struct ieee80211_hw *dev)
+{
+	struct p54_common *priv = dev->priv;
+	kfree(priv->iq_autocal);
+	kfree(priv->output_limit);
+	kfree(priv->curve_data);
+	kfree(priv->cached_vdcf);
+}
+EXPORT_SYMBOL_GPL(p54_free_common);
+
+static int __init p54_init(void)
+{
+	return 0;
+}
+
+static void __exit p54_exit(void)
+{
+}
+
+module_init(p54_init);
+module_exit(p54_exit);
diff --git a/drivers/net/wireless/p54common.h b/drivers/net/wireless/p54common.h
new file mode 100644
index 000000000000..a721334e20d9
--- /dev/null
+++ b/drivers/net/wireless/p54common.h
@@ -0,0 +1,329 @@
+#ifndef PRISM54COMMON_H
+#define PRISM54COMMON_H
+
+/*
+ * Common code specific definitions for mac80211 Prism54 drivers
+ *
+ * Copyright (c) 2006, Michael Wu <flamingice@sourmilk.net>
+ * Copyright (c) 2007, Christian Lamparter <chunkeey@web.de>
+ *
+ * Based on the islsm (softmac prism54) driver, which is:
+ * Copyright 2004-2006 Jean-Baptiste Note <jbnote@gmail.com>, et al.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+struct bootrec {
+	__le32 code;
+	__le32 len;
+	u32 data[0];
+} __attribute__((packed));
+
+struct bootrec_exp_if {
+	__le16 role;
+	__le16 if_id;
+	__le16 variant;
+	__le16 btm_compat;
+	__le16 top_compat;
+} __attribute__((packed));
+
+#define BR_CODE_MIN			0x80000000
+#define BR_CODE_COMPONENT_ID		0x80000001
+#define BR_CODE_COMPONENT_VERSION	0x80000002
+#define BR_CODE_DEPENDENT_IF		0x80000003
+#define BR_CODE_EXPOSED_IF		0x80000004
+#define BR_CODE_DESCR			0x80000101
+#define BR_CODE_MAX			0x8FFFFFFF
+#define BR_CODE_END_OF_BRA		0xFF0000FF
+#define LEGACY_BR_CODE_END_OF_BRA	0xFFFFFFFF
+
+#define FW_FMAC 0x464d4143
+#define FW_LM86 0x4c4d3836
+#define FW_LM87 0x4c4d3837
+#define FW_LM20 0x4c4d3230
+
+/* PDA defines are Copyright (C) 2005 Nokia Corporation (taken from islsm_pda.h) */
+
+struct pda_entry {
+	__le16 len;	/* includes both code and data */
+	__le16 code;
+	u8 data[0];
+} __attribute__ ((packed));
+
+struct eeprom_pda_wrap {
+	u32 magic;
+	u16 pad;
+	u16 len;
+	u32 arm_opcode;
+	u8 data[0];
+} __attribute__ ((packed));
+
+struct pda_iq_autocal_entry {
+        __le16 freq;
+        __le16 iq_param[4];
+} __attribute__ ((packed));
+
+struct pda_channel_output_limit {
+	__le16 freq;
+	u8 val_bpsk;
+	u8 val_qpsk;
+	u8 val_16qam;
+	u8 val_64qam;
+	u8 rate_set_mask;
+	u8 rate_set_size;
+} __attribute__ ((packed));
+
+struct pda_pa_curve_data_sample_rev0 {
+	u8 rf_power;
+	u8 pa_detector;
+	u8 pcv;
+} __attribute__ ((packed));
+
+struct pda_pa_curve_data_sample_rev1 {
+	u8 rf_power;
+	u8 pa_detector;
+	u8 data_barker;
+	u8 data_bpsk;
+	u8 data_qpsk;
+	u8 data_16qam;
+	u8 data_64qam;
+	u8 padding;
+} __attribute__ ((packed));
+
+struct pda_pa_curve_data {
+	u8 cal_method_rev;
+	u8 channels;
+	u8 points_per_channel;
+	u8 padding;
+	u8 data[0];
+} __attribute__ ((packed));
+
+/*
+ * this defines the PDR codes used to build PDAs as defined in document
+ * number 553155. The current implementation mirrors version 1.1 of the
+ * document and lists only PDRs supported by the ARM platform.
+ */
+
+/* common and choice range (0x0000 - 0x0fff) */
+#define PDR_END					0x0000
+#define PDR_MANUFACTURING_PART_NUMBER		0x0001
+#define PDR_PDA_VERSION				0x0002
+#define PDR_NIC_SERIAL_NUMBER			0x0003
+
+#define PDR_MAC_ADDRESS				0x0101
+#define PDR_REGULATORY_DOMAIN_LIST		0x0103
+#define PDR_TEMPERATURE_TYPE			0x0107
+
+#define PDR_PRISM_PCI_IDENTIFIER		0x0402
+
+/* ARM range (0x1000 - 0x1fff) */
+#define PDR_COUNTRY_INFORMATION			0x1000
+#define PDR_INTERFACE_LIST			0x1001
+#define PDR_HARDWARE_PLATFORM_COMPONENT_ID	0x1002
+#define PDR_OEM_NAME				0x1003
+#define PDR_PRODUCT_NAME			0x1004
+#define PDR_UTF8_OEM_NAME			0x1005
+#define PDR_UTF8_PRODUCT_NAME			0x1006
+#define PDR_COUNTRY_LIST			0x1007
+#define PDR_DEFAULT_COUNTRY			0x1008
+
+#define PDR_ANTENNA_GAIN			0x1100
+
+#define PDR_PRISM_INDIGO_PA_CALIBRATION_DATA	0x1901
+#define PDR_RSSI_LINEAR_APPROXIMATION		0x1902
+#define PDR_PRISM_PA_CAL_OUTPUT_POWER_LIMITS	0x1903
+#define PDR_PRISM_PA_CAL_CURVE_DATA		0x1904
+#define PDR_RSSI_LINEAR_APPROXIMATION_DUAL_BAND	0x1905
+#define PDR_PRISM_ZIF_TX_IQ_CALIBRATION		0x1906
+#define PDR_REGULATORY_POWER_LIMITS		0x1907
+#define PDR_RSSI_LINEAR_APPROXIMATION_EXTENDED	0x1908
+#define PDR_RADIATED_TRANSMISSION_CORRECTION	0x1909
+#define PDR_PRISM_TX_IQ_CALIBRATION		0x190a
+
+/* reserved range (0x2000 - 0x7fff) */
+
+/* customer range (0x8000 - 0xffff) */
+#define PDR_BASEBAND_REGISTERS			0x8000
+#define PDR_PER_CHANNEL_BASEBAND_REGISTERS	0x8001
+
+/* stored in skb->cb */
+struct memrecord {
+	u32 start_addr;
+	u32 end_addr;
+	struct ieee80211_tx_control *control;
+};
+
+struct p54_eeprom_lm86 {
+	__le16 offset;
+	__le16 len;
+	u8 data[0];
+} __attribute__ ((packed));
+
+struct p54_rx_hdr {
+	__le16 magic;
+	__le16 len;
+	__le16 freq;
+	u8 antenna;
+	u8 rate;
+	u8 rssi;
+	u8 quality;
+	u16 unknown2;
+	__le64 timestamp;
+	u8 data[0];
+} __attribute__ ((packed));
+
+struct p54_frame_sent_hdr {
+	u8 status;
+	u8 retries;
+	__le16 ack_rssi;
+	__le16 seq;
+	u16 rate;
+} __attribute__ ((packed));
+
+struct p54_tx_control_allocdata {
+	u8 rateset[8];
+	u16 padding;
+	u8 wep_key_present;
+	u8 wep_key_len;
+	u8 wep_key[16];
+	__le32 frame_type;
+	u32 padding2;
+	__le16 magic4;
+	u8 antenna;
+	u8 output_power;
+	__le32 magic5;
+	u8 align[0];
+} __attribute__ ((packed));
+
+struct p54_tx_control_filter {
+	__le16 filter_type;
+	u8 dst[ETH_ALEN];
+	u8 src[ETH_ALEN];
+	u8 antenna;
+	u8 debug;
+	__le32 magic3;
+	u8 rates[8];	// FIXME: what's this for?
+	__le32 rx_addr;
+	__le16 max_rx;
+	__le16 rxhw;
+	__le16 magic8;
+	__le16 magic9;
+} __attribute__ ((packed));
+
+struct p54_tx_control_channel {
+	__le16 magic1;
+	__le16 magic2;
+	u8 padding1[20];
+	struct pda_iq_autocal_entry iq_autocal;
+	u8 pa_points_per_curve;
+	u8 val_barker;
+	u8 val_bpsk;
+	u8 val_qpsk;
+	u8 val_16qam;
+	u8 val_64qam;
+	struct pda_pa_curve_data_sample_rev1 curve_data[0];
+	/* additional padding/data after curve_data */
+} __attribute__ ((packed));
+
+struct p54_tx_control_led {
+	__le16 mode;
+	__le16 led_temporary;
+	__le16 led_permanent;
+	__le16 duration;
+} __attribute__ ((packed));
+
+struct p54_tx_vdcf_queues {
+	__le16 aifs;
+	__le16 cwmin;
+	__le16 cwmax;
+	__le16 txop;
+} __attribute__ ((packed));
+
+struct p54_tx_control_vdcf {
+	u8 padding;
+	u8 slottime;
+	u8 magic1;
+	u8 magic2;
+	struct p54_tx_vdcf_queues queue[8];
+	u8 pad2[4];
+	__le16 frameburst;
+} __attribute__ ((packed));
+
+static const struct ieee80211_rate p54_rates[] = {
+	{ .rate = 10,
+	  .val = 0,
+	  .val2 = 0x10,
+	  .flags = IEEE80211_RATE_CCK_2 },
+	{ .rate = 20,
+	  .val = 1,
+	  .val2 = 0x11,
+	  .flags = IEEE80211_RATE_CCK_2 },
+	{ .rate = 55,
+	  .val = 2,
+	  .val2 = 0x12,
+	  .flags = IEEE80211_RATE_CCK_2 },
+	{ .rate = 110,
+	  .val = 3,
+	  .val2 = 0x13,
+	  .flags = IEEE80211_RATE_CCK_2 },
+	{ .rate = 60,
+	  .val = 4,
+	  .flags = IEEE80211_RATE_OFDM },
+	{ .rate = 90,
+	  .val = 5,
+	  .flags = IEEE80211_RATE_OFDM },
+	{ .rate = 120,
+	  .val = 6,
+	  .flags = IEEE80211_RATE_OFDM },
+	{ .rate = 180,
+	  .val = 7,
+	  .flags = IEEE80211_RATE_OFDM },
+	{ .rate = 240,
+	  .val = 8,
+	  .flags = IEEE80211_RATE_OFDM },
+	{ .rate = 360,
+	  .val = 9,
+	  .flags = IEEE80211_RATE_OFDM },
+	{ .rate = 480,
+	  .val = 10,
+	  .flags = IEEE80211_RATE_OFDM },
+	{ .rate = 540,
+	  .val = 11,
+	  .flags = IEEE80211_RATE_OFDM },
+};
+
+// TODO: just generate this..
+static const struct ieee80211_channel p54_channels[] = {
+	{ .chan = 1,
+	  .freq = 2412},
+	{ .chan = 2,
+	  .freq = 2417},
+	{ .chan = 3,
+	  .freq = 2422},
+	{ .chan = 4,
+	  .freq = 2427},
+	{ .chan = 5,
+	  .freq = 2432},
+	{ .chan = 6,
+	  .freq = 2437},
+	{ .chan = 7,
+	  .freq = 2442},
+	{ .chan = 8,
+	  .freq = 2447},
+	{ .chan = 9,
+	  .freq = 2452},
+	{ .chan = 10,
+	  .freq = 2457},
+	{ .chan = 11,
+	  .freq = 2462},
+	{ .chan = 12,
+	  .freq = 2467},
+	{ .chan = 13,
+	  .freq = 2472},
+	{ .chan = 14,
+	  .freq = 2484}
+};
+
+#endif /* PRISM54COMMON_H */
diff --git a/drivers/net/wireless/p54pci.c b/drivers/net/wireless/p54pci.c
new file mode 100644
index 000000000000..410b54387f23
--- /dev/null
+++ b/drivers/net/wireless/p54pci.c
@@ -0,0 +1,692 @@
+
+/*
+ * Linux device driver for PCI based Prism54
+ *
+ * Copyright (c) 2006, Michael Wu <flamingice@sourmilk.net>
+ *
+ * Based on the islsm (softmac prism54) driver, which is:
+ * Copyright 2004-2006 Jean-Baptiste Note <jean-baptiste.note@m4x.org>, et al.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/init.h>
+#include <linux/pci.h>
+#include <linux/firmware.h>
+#include <linux/etherdevice.h>
+#include <linux/delay.h>
+#include <linux/completion.h>
+#include <net/mac80211.h>
+
+#include "p54.h"
+#include "p54pci.h"
+
+MODULE_AUTHOR("Michael Wu <flamingice@sourmilk.net>");
+MODULE_DESCRIPTION("Prism54 PCI wireless driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("prism54pci");
+
+static struct pci_device_id p54p_table[] __devinitdata = {
+	/* Intersil PRISM Duette/Prism GT Wireless LAN adapter */
+	{ PCI_DEVICE(0x1260, 0x3890) },
+	/* 3COM 3CRWE154G72 Wireless LAN adapter */
+	{ PCI_DEVICE(0x10b7, 0x6001) },
+	/* Intersil PRISM Indigo Wireless LAN adapter */
+	{ PCI_DEVICE(0x1260, 0x3877) },
+	/* Intersil PRISM Javelin/Xbow Wireless LAN adapter */
+	{ PCI_DEVICE(0x1260, 0x3886) },
+	{ },
+};
+
+MODULE_DEVICE_TABLE(pci, p54p_table);
+
+static int p54p_upload_firmware(struct ieee80211_hw *dev)
+{
+	struct p54p_priv *priv = dev->priv;
+	const struct firmware *fw_entry = NULL;
+	__le32 reg;
+	int err;
+	u32 *data;
+	u32 remains, left, device_addr;
+
+	P54P_WRITE(int_enable, 0);
+	P54P_READ(int_enable);
+	udelay(10);
+
+	reg = P54P_READ(ctrl_stat);
+	reg &= cpu_to_le32(~ISL38XX_CTRL_STAT_RESET);
+	reg &= cpu_to_le32(~ISL38XX_CTRL_STAT_RAMBOOT);
+	P54P_WRITE(ctrl_stat, reg);
+	P54P_READ(ctrl_stat);
+	udelay(10);
+
+	reg |= cpu_to_le32(ISL38XX_CTRL_STAT_RESET);
+	P54P_WRITE(ctrl_stat, reg);
+	wmb();
+	udelay(10);
+
+	reg &= cpu_to_le32(~ISL38XX_CTRL_STAT_RESET);
+	P54P_WRITE(ctrl_stat, reg);
+	wmb();
+
+	mdelay(50);
+
+	err = request_firmware(&fw_entry, "isl3886", &priv->pdev->dev);
+	if (err) {
+		printk(KERN_ERR "%s (prism54pci): cannot find firmware "
+		       "(isl3886)\n", pci_name(priv->pdev));
+		return err;
+	}
+
+	p54_parse_firmware(dev, fw_entry);
+
+	data = (u32 *) fw_entry->data;
+	remains = fw_entry->size;
+	device_addr = ISL38XX_DEV_FIRMWARE_ADDR;
+	while (remains) {
+		u32 i = 0;
+		left = min((u32)0x1000, remains);
+		P54P_WRITE(direct_mem_base, cpu_to_le32(device_addr));
+		P54P_READ(int_enable);
+
+		device_addr += 0x1000;
+		while (i < left) {
+			P54P_WRITE(direct_mem_win[i], *data++);
+			i += sizeof(u32);
+		}
+
+		remains -= left;
+		P54P_READ(int_enable);
+	}
+
+	release_firmware(fw_entry);
+
+	reg = P54P_READ(ctrl_stat);
+	reg &= cpu_to_le32(~ISL38XX_CTRL_STAT_CLKRUN);
+	reg &= cpu_to_le32(~ISL38XX_CTRL_STAT_RESET);
+	reg |= cpu_to_le32(ISL38XX_CTRL_STAT_RAMBOOT);
+	P54P_WRITE(ctrl_stat, reg);
+	P54P_READ(ctrl_stat);
+	udelay(10);
+
+	reg |= cpu_to_le32(ISL38XX_CTRL_STAT_RESET);
+	P54P_WRITE(ctrl_stat, reg);
+	wmb();
+	udelay(10);
+
+	reg &= cpu_to_le32(~ISL38XX_CTRL_STAT_RESET);
+	P54P_WRITE(ctrl_stat, reg);
+	wmb();
+	udelay(10);
+
+	return 0;
+}
+
+static irqreturn_t p54p_simple_interrupt(int irq, void *dev_id)
+{
+	struct p54p_priv *priv = (struct p54p_priv *) dev_id;
+	__le32 reg;
+
+	reg = P54P_READ(int_ident);
+	P54P_WRITE(int_ack, reg);
+
+	if (reg & P54P_READ(int_enable))
+		complete(&priv->boot_comp);
+
+	return IRQ_HANDLED;
+}
+
+static int p54p_read_eeprom(struct ieee80211_hw *dev)
+{
+	struct p54p_priv *priv = dev->priv;
+	int err;
+	struct p54_control_hdr *hdr;
+	void *eeprom;
+	dma_addr_t rx_mapping, tx_mapping;
+	u16 alen;
+
+	init_completion(&priv->boot_comp);
+	err = request_irq(priv->pdev->irq, &p54p_simple_interrupt,
+			  IRQF_SHARED, "prism54pci", priv);
+	if (err) {
+		printk(KERN_ERR "%s (prism54pci): failed to register IRQ handler\n",
+		       pci_name(priv->pdev));
+		return err;
+	}
+
+	eeprom = kmalloc(0x2010 + EEPROM_READBACK_LEN, GFP_KERNEL);
+	if (!eeprom) {
+		printk(KERN_ERR "%s (prism54pci): no memory for eeprom!\n",
+		       pci_name(priv->pdev));
+		err = -ENOMEM;
+		goto out;
+	}
+
+	memset(priv->ring_control, 0, sizeof(*priv->ring_control));
+	P54P_WRITE(ring_control_base, priv->ring_control_dma);
+	P54P_READ(ring_control_base);
+	udelay(10);
+
+	P54P_WRITE(int_enable, cpu_to_le32(ISL38XX_INT_IDENT_INIT));
+	P54P_READ(int_enable);
+	udelay(10);
+
+	P54P_WRITE(dev_int, cpu_to_le32(ISL38XX_DEV_INT_RESET));
+
+	if (!wait_for_completion_interruptible_timeout(&priv->boot_comp, HZ)) {
+		printk(KERN_ERR "%s (prism54pci): Cannot boot firmware!\n",
+		       pci_name(priv->pdev));
+		err = -EINVAL;
+		goto out;
+	}
+
+	P54P_WRITE(int_enable, cpu_to_le32(ISL38XX_INT_IDENT_UPDATE));
+	P54P_READ(int_enable);
+
+	hdr = eeprom + 0x2010;
+	p54_fill_eeprom_readback(hdr);
+	hdr->req_id = cpu_to_le32(priv->common.rx_start);
+
+	rx_mapping = pci_map_single(priv->pdev, eeprom,
+				    0x2010, PCI_DMA_FROMDEVICE);
+	tx_mapping = pci_map_single(priv->pdev, (void *)hdr,
+				    EEPROM_READBACK_LEN, PCI_DMA_TODEVICE);
+
+	priv->ring_control->rx_mgmt[0].host_addr = cpu_to_le32(rx_mapping);
+	priv->ring_control->rx_mgmt[0].len = cpu_to_le16(0x2010);
+	priv->ring_control->tx_data[0].host_addr = cpu_to_le32(tx_mapping);
+	priv->ring_control->tx_data[0].device_addr = hdr->req_id;
+	priv->ring_control->tx_data[0].len = cpu_to_le16(EEPROM_READBACK_LEN);
+
+	priv->ring_control->host_idx[2] = cpu_to_le32(1);
+	priv->ring_control->host_idx[1] = cpu_to_le32(1);
+
+	wmb();
+	mdelay(100);
+	P54P_WRITE(dev_int, cpu_to_le32(ISL38XX_DEV_INT_UPDATE));
+
+	wait_for_completion_interruptible_timeout(&priv->boot_comp, HZ);
+	wait_for_completion_interruptible_timeout(&priv->boot_comp, HZ);
+
+	pci_unmap_single(priv->pdev, tx_mapping,
+			 EEPROM_READBACK_LEN, PCI_DMA_TODEVICE);
+	pci_unmap_single(priv->pdev, rx_mapping,
+			 0x2010, PCI_DMA_FROMDEVICE);
+
+	alen = le16_to_cpu(priv->ring_control->rx_mgmt[0].len);
+	if (le32_to_cpu(priv->ring_control->device_idx[2]) != 1 ||
+	    alen < 0x10) {
+		printk(KERN_ERR "%s (prism54pci): Cannot read eeprom!\n",
+		       pci_name(priv->pdev));
+		err = -EINVAL;
+		goto out;
+	}
+
+	p54_parse_eeprom(dev, (u8 *)eeprom + 0x10, alen - 0x10);
+
+ out:
+	kfree(eeprom);
+	P54P_WRITE(int_enable, 0);
+	P54P_READ(int_enable);
+	udelay(10);
+	free_irq(priv->pdev->irq, priv);
+	P54P_WRITE(dev_int, cpu_to_le32(ISL38XX_DEV_INT_RESET));
+	return err;
+}
+
+static void p54p_refill_rx_ring(struct ieee80211_hw *dev)
+{
+	struct p54p_priv *priv = dev->priv;
+	u32 limit, host_idx, idx;
+
+	host_idx = le32_to_cpu(priv->ring_control->host_idx[0]);
+	limit = host_idx;
+	limit -= le32_to_cpu(priv->ring_control->device_idx[0]);
+	limit = ARRAY_SIZE(priv->ring_control->rx_data) - limit;
+
+	idx = host_idx % ARRAY_SIZE(priv->ring_control->rx_data);
+	while (limit-- > 1) {
+		struct p54p_desc *desc = &priv->ring_control->rx_data[idx];
+
+		if (!desc->host_addr) {
+			struct sk_buff *skb;
+			dma_addr_t mapping;
+			skb = dev_alloc_skb(MAX_RX_SIZE);
+			if (!skb)
+				break;
+
+			mapping = pci_map_single(priv->pdev,
+						 skb_tail_pointer(skb),
+						 MAX_RX_SIZE,
+						 PCI_DMA_FROMDEVICE);
+			desc->host_addr = cpu_to_le32(mapping);
+			desc->device_addr = 0;	// FIXME: necessary?
+			desc->len = cpu_to_le16(MAX_RX_SIZE);
+			desc->flags = 0;
+			priv->rx_buf[idx] = skb;
+		}
+
+		idx++;
+		host_idx++;
+		idx %= ARRAY_SIZE(priv->ring_control->rx_data);
+	}
+
+	wmb();
+	priv->ring_control->host_idx[0] = cpu_to_le32(host_idx);
+}
+
+static irqreturn_t p54p_interrupt(int irq, void *dev_id)
+{
+	struct ieee80211_hw *dev = dev_id;
+	struct p54p_priv *priv = dev->priv;
+	__le32 reg;
+
+	spin_lock(&priv->lock);
+	reg = P54P_READ(int_ident);
+	if (unlikely(reg == 0xFFFFFFFF)) {
+		spin_unlock(&priv->lock);
+		return IRQ_HANDLED;
+	}
+
+	P54P_WRITE(int_ack, reg);
+
+	reg &= P54P_READ(int_enable);
+
+	if (reg & cpu_to_le32(ISL38XX_INT_IDENT_UPDATE)) {
+		struct p54p_desc *desc;
+		u32 idx, i;
+		i = priv->tx_idx;
+		i %= ARRAY_SIZE(priv->ring_control->tx_data);
+		priv->tx_idx = idx = le32_to_cpu(priv->ring_control->device_idx[1]);
+		idx %= ARRAY_SIZE(priv->ring_control->tx_data);
+
+		while (i != idx) {
+			desc = &priv->ring_control->tx_data[i];
+			if (priv->tx_buf[i]) {
+				kfree(priv->tx_buf[i]);
+				priv->tx_buf[i] = NULL;
+			}
+
+			pci_unmap_single(priv->pdev, le32_to_cpu(desc->host_addr),
+					 le16_to_cpu(desc->len), PCI_DMA_TODEVICE);
+
+			desc->host_addr = 0;
+			desc->device_addr = 0;
+			desc->len = 0;
+			desc->flags = 0;
+
+			i++;
+			i %= ARRAY_SIZE(priv->ring_control->tx_data);
+		}
+
+		i = priv->rx_idx;
+		i %= ARRAY_SIZE(priv->ring_control->rx_data);
+		priv->rx_idx = idx = le32_to_cpu(priv->ring_control->device_idx[0]);
+		idx %= ARRAY_SIZE(priv->ring_control->rx_data);
+		while (i != idx) {
+			u16 len;
+			struct sk_buff *skb;
+			desc = &priv->ring_control->rx_data[i];
+			len = le16_to_cpu(desc->len);
+			skb = priv->rx_buf[i];
+
+			skb_put(skb, len);
+
+			if (p54_rx(dev, skb)) {
+				pci_unmap_single(priv->pdev,
+						 le32_to_cpu(desc->host_addr),
+						 MAX_RX_SIZE, PCI_DMA_FROMDEVICE);
+
+				priv->rx_buf[i] = NULL;
+				desc->host_addr = 0;
+			} else {
+				skb_trim(skb, 0);
+				desc->len = cpu_to_le16(MAX_RX_SIZE);
+			}
+
+			i++;
+			i %= ARRAY_SIZE(priv->ring_control->rx_data);
+		}
+
+		p54p_refill_rx_ring(dev);
+
+		wmb();
+		P54P_WRITE(dev_int, cpu_to_le32(ISL38XX_DEV_INT_UPDATE));
+	} else if (reg & cpu_to_le32(ISL38XX_INT_IDENT_INIT))
+		complete(&priv->boot_comp);
+
+	spin_unlock(&priv->lock);
+
+	return reg ? IRQ_HANDLED : IRQ_NONE;
+}
+
+static void p54p_tx(struct ieee80211_hw *dev, struct p54_control_hdr *data,
+		    size_t len, int free_on_tx)
+{
+	struct p54p_priv *priv = dev->priv;
+	unsigned long flags;
+	struct p54p_desc *desc;
+	dma_addr_t mapping;
+	u32 device_idx, idx, i;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	device_idx = le32_to_cpu(priv->ring_control->device_idx[1]);
+	idx = le32_to_cpu(priv->ring_control->host_idx[1]);
+	i = idx % ARRAY_SIZE(priv->ring_control->tx_data);
+
+	mapping = pci_map_single(priv->pdev, data, len, PCI_DMA_TODEVICE);
+	desc = &priv->ring_control->tx_data[i];
+	desc->host_addr = cpu_to_le32(mapping);
+	desc->device_addr = data->req_id;
+	desc->len = cpu_to_le16(len);
+	desc->flags = 0;
+
+	wmb();
+	priv->ring_control->host_idx[1] = cpu_to_le32(idx + 1);
+
+	if (free_on_tx)
+		priv->tx_buf[i] = data;
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	P54P_WRITE(dev_int, cpu_to_le32(ISL38XX_DEV_INT_UPDATE));
+	P54P_READ(dev_int);
+
+	/* FIXME: unlikely to happen because the device usually runs out of
+	   memory before we fill the ring up, but we can make it impossible */
+	if (idx - device_idx > ARRAY_SIZE(priv->ring_control->tx_data) - 2)
+		printk(KERN_INFO "%s: tx overflow.\n", wiphy_name(dev->wiphy));
+}
+
+static int p54p_open(struct ieee80211_hw *dev)
+{
+	struct p54p_priv *priv = dev->priv;
+	int err;
+
+	init_completion(&priv->boot_comp);
+	err = request_irq(priv->pdev->irq, &p54p_interrupt,
+			  IRQF_SHARED, "prism54pci", dev);
+	if (err) {
+		printk(KERN_ERR "%s: failed to register IRQ handler\n",
+		       wiphy_name(dev->wiphy));
+		return err;
+	}
+
+	memset(priv->ring_control, 0, sizeof(*priv->ring_control));
+	priv->rx_idx = priv->tx_idx = 0;
+	p54p_refill_rx_ring(dev);
+
+	p54p_upload_firmware(dev);
+
+	P54P_WRITE(ring_control_base, priv->ring_control_dma);
+	P54P_READ(ring_control_base);
+	wmb();
+	udelay(10);
+
+	P54P_WRITE(int_enable, cpu_to_le32(ISL38XX_INT_IDENT_INIT));
+	P54P_READ(int_enable);
+	wmb();
+	udelay(10);
+
+	P54P_WRITE(dev_int, cpu_to_le32(ISL38XX_DEV_INT_RESET));
+	P54P_READ(dev_int);
+
+	if (!wait_for_completion_interruptible_timeout(&priv->boot_comp, HZ)) {
+		printk(KERN_ERR "%s: Cannot boot firmware!\n",
+		       wiphy_name(dev->wiphy));
+		free_irq(priv->pdev->irq, dev);
+		return -ETIMEDOUT;
+	}
+
+	P54P_WRITE(int_enable, cpu_to_le32(ISL38XX_INT_IDENT_UPDATE));
+	P54P_READ(int_enable);
+	wmb();
+	udelay(10);
+
+	P54P_WRITE(dev_int, cpu_to_le32(ISL38XX_DEV_INT_UPDATE));
+	P54P_READ(dev_int);
+	wmb();
+	udelay(10);
+
+	return 0;
+}
+
+static void p54p_stop(struct ieee80211_hw *dev)
+{
+	struct p54p_priv *priv = dev->priv;
+	unsigned int i;
+	struct p54p_desc *desc;
+
+	P54P_WRITE(int_enable, 0);
+	P54P_READ(int_enable);
+	udelay(10);
+
+	free_irq(priv->pdev->irq, dev);
+
+	P54P_WRITE(dev_int, cpu_to_le32(ISL38XX_DEV_INT_RESET));
+
+	for (i = 0; i < ARRAY_SIZE(priv->rx_buf); i++) {
+		desc = &priv->ring_control->rx_data[i];
+		if (desc->host_addr)
+			pci_unmap_single(priv->pdev, le32_to_cpu(desc->host_addr),
+					 MAX_RX_SIZE, PCI_DMA_FROMDEVICE);
+		kfree_skb(priv->rx_buf[i]);
+		priv->rx_buf[i] = NULL;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(priv->tx_buf); i++) {
+		desc = &priv->ring_control->tx_data[i];
+		if (desc->host_addr)
+			pci_unmap_single(priv->pdev, le32_to_cpu(desc->host_addr),
+					 le16_to_cpu(desc->len), PCI_DMA_TODEVICE);
+
+		kfree(priv->tx_buf[i]);
+		priv->tx_buf[i] = NULL;
+	}
+
+	memset(priv->ring_control, 0, sizeof(*priv->ring_control));
+}
+
+static int __devinit p54p_probe(struct pci_dev *pdev,
+				const struct pci_device_id *id)
+{
+	struct p54p_priv *priv;
+	struct ieee80211_hw *dev;
+	unsigned long mem_addr, mem_len;
+	int err;
+	DECLARE_MAC_BUF(mac);
+
+	err = pci_enable_device(pdev);
+	if (err) {
+		printk(KERN_ERR "%s (prism54pci): Cannot enable new PCI device\n",
+		       pci_name(pdev));
+		return err;
+	}
+
+	mem_addr = pci_resource_start(pdev, 0);
+	mem_len = pci_resource_len(pdev, 0);
+	if (mem_len < sizeof(struct p54p_csr)) {
+		printk(KERN_ERR "%s (prism54pci): Too short PCI resources\n",
+		       pci_name(pdev));
+		pci_disable_device(pdev);
+		return err;
+	}
+
+	err = pci_request_regions(pdev, "prism54pci");
+	if (err) {
+		printk(KERN_ERR "%s (prism54pci): Cannot obtain PCI resources\n",
+		       pci_name(pdev));
+		return err;
+	}
+
+	if (pci_set_dma_mask(pdev, DMA_32BIT_MASK) ||
+	    pci_set_consistent_dma_mask(pdev, DMA_32BIT_MASK)) {
+		printk(KERN_ERR "%s (prism54pci): No suitable DMA available\n",
+		       pci_name(pdev));
+		goto err_free_reg;
+	}
+
+	pci_set_master(pdev);
+	pci_try_set_mwi(pdev);
+
+	pci_write_config_byte(pdev, 0x40, 0);
+	pci_write_config_byte(pdev, 0x41, 0);
+
+	dev = p54_init_common(sizeof(*priv));
+	if (!dev) {
+		printk(KERN_ERR "%s (prism54pci): ieee80211 alloc failed\n",
+		       pci_name(pdev));
+		err = -ENOMEM;
+		goto err_free_reg;
+	}
+
+	priv = dev->priv;
+	priv->pdev = pdev;
+
+	SET_IEEE80211_DEV(dev, &pdev->dev);
+	pci_set_drvdata(pdev, dev);
+
+	priv->map = ioremap(mem_addr, mem_len);
+	if (!priv->map) {
+		printk(KERN_ERR "%s (prism54pci): Cannot map device memory\n",
+		       pci_name(pdev));
+		err = -EINVAL;	// TODO: use a better error code?
+		goto err_free_dev;
+	}
+
+	priv->ring_control = pci_alloc_consistent(pdev, sizeof(*priv->ring_control),
+						  &priv->ring_control_dma);
+	if (!priv->ring_control) {
+		printk(KERN_ERR "%s (prism54pci): Cannot allocate rings\n",
+		       pci_name(pdev));
+		err = -ENOMEM;
+		goto err_iounmap;
+	}
+	memset(priv->ring_control, 0, sizeof(*priv->ring_control));
+
+	err = p54p_upload_firmware(dev);
+	if (err)
+		goto err_free_desc;
+
+	err = p54p_read_eeprom(dev);
+	if (err)
+		goto err_free_desc;
+
+	priv->common.open = p54p_open;
+	priv->common.stop = p54p_stop;
+	priv->common.tx = p54p_tx;
+
+	spin_lock_init(&priv->lock);
+
+	err = ieee80211_register_hw(dev);
+	if (err) {
+		printk(KERN_ERR "%s (prism54pci): Cannot register netdevice\n",
+		       pci_name(pdev));
+		goto err_free_common;
+	}
+
+	printk(KERN_INFO "%s: hwaddr %s, isl38%02x\n",
+	       wiphy_name(dev->wiphy),
+	       print_mac(mac, dev->wiphy->perm_addr),
+	       priv->common.version);
+
+	return 0;
+
+ err_free_common:
+	p54_free_common(dev);
+
+ err_free_desc:
+	pci_free_consistent(pdev, sizeof(*priv->ring_control),
+			    priv->ring_control, priv->ring_control_dma);
+
+ err_iounmap:
+	iounmap(priv->map);
+
+ err_free_dev:
+	pci_set_drvdata(pdev, NULL);
+	ieee80211_free_hw(dev);
+
+ err_free_reg:
+	pci_release_regions(pdev);
+	pci_disable_device(pdev);
+	return err;
+}
+
+static void __devexit p54p_remove(struct pci_dev *pdev)
+{
+	struct ieee80211_hw *dev = pci_get_drvdata(pdev);
+	struct p54p_priv *priv;
+
+	if (!dev)
+		return;
+
+	ieee80211_unregister_hw(dev);
+	priv = dev->priv;
+	pci_free_consistent(pdev, sizeof(*priv->ring_control),
+			    priv->ring_control, priv->ring_control_dma);
+	p54_free_common(dev);
+	iounmap(priv->map);
+	pci_release_regions(pdev);
+	pci_disable_device(pdev);
+	ieee80211_free_hw(dev);
+}
+
+#ifdef CONFIG_PM
+static int p54p_suspend(struct pci_dev *pdev, pm_message_t state)
+{
+	struct ieee80211_hw *dev = pci_get_drvdata(pdev);
+	struct p54p_priv *priv = dev->priv;
+
+	if (priv->common.mode != IEEE80211_IF_TYPE_INVALID) {
+		ieee80211_stop_queues(dev);
+		p54p_stop(dev);
+	}
+
+	pci_save_state(pdev);
+	pci_set_power_state(pdev, pci_choose_state(pdev, state));
+	return 0;
+}
+
+static int p54p_resume(struct pci_dev *pdev)
+{
+	struct ieee80211_hw *dev = pci_get_drvdata(pdev);
+	struct p54p_priv *priv = dev->priv;
+
+	pci_set_power_state(pdev, PCI_D0);
+	pci_restore_state(pdev);
+
+	if (priv->common.mode != IEEE80211_IF_TYPE_INVALID) {
+		p54p_open(dev);
+		ieee80211_start_queues(dev);
+	}
+
+	return 0;
+}
+#endif /* CONFIG_PM */
+
+static struct pci_driver p54p_driver = {
+	.name		= "prism54pci",
+	.id_table	= p54p_table,
+	.probe		= p54p_probe,
+	.remove		= __devexit_p(p54p_remove),
+#ifdef CONFIG_PM
+	.suspend	= p54p_suspend,
+	.resume		= p54p_resume,
+#endif /* CONFIG_PM */
+};
+
+static int __init p54p_init(void)
+{
+	return pci_register_driver(&p54p_driver);
+}
+
+static void __exit p54p_exit(void)
+{
+	pci_unregister_driver(&p54p_driver);
+}
+
+module_init(p54p_init);
+module_exit(p54p_exit);
diff --git a/drivers/net/wireless/p54pci.h b/drivers/net/wireless/p54pci.h
new file mode 100644
index 000000000000..52feb597dc4a
--- /dev/null
+++ b/drivers/net/wireless/p54pci.h
@@ -0,0 +1,106 @@
+#ifndef PRISM54PCI_H
+#define PRISM54PCI_H
+
+/*
+ * Defines for PCI based mac80211 Prism54 driver
+ *
+ * Copyright (c) 2006, Michael Wu <flamingice@sourmilk.net>
+ *
+ * Based on the islsm (softmac prism54) driver, which is:
+ * Copyright 2004-2006 Jean-Baptiste Note <jbnote@gmail.com>, et al.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+/* Device Interrupt register bits */
+#define ISL38XX_DEV_INT_RESET                   0x0001
+#define ISL38XX_DEV_INT_UPDATE                  0x0002
+#define ISL38XX_DEV_INT_WAKEUP                  0x0008
+#define ISL38XX_DEV_INT_SLEEP                   0x0010
+#define ISL38XX_DEV_INT_ABORT                   0x0020
+/* these two only used in USB */
+#define ISL38XX_DEV_INT_DATA                    0x0040
+#define ISL38XX_DEV_INT_MGMT                    0x0080
+
+#define ISL38XX_DEV_INT_PCIUART_CTS             0x4000
+#define ISL38XX_DEV_INT_PCIUART_DR              0x8000
+
+/* Interrupt Identification/Acknowledge/Enable register bits */
+#define ISL38XX_INT_IDENT_UPDATE		0x0002
+#define ISL38XX_INT_IDENT_INIT			0x0004
+#define ISL38XX_INT_IDENT_WAKEUP		0x0008
+#define ISL38XX_INT_IDENT_SLEEP			0x0010
+#define ISL38XX_INT_IDENT_PCIUART_CTS		0x4000
+#define ISL38XX_INT_IDENT_PCIUART_DR		0x8000
+
+/* Control/Status register bits */
+#define ISL38XX_CTRL_STAT_SLEEPMODE		0x00000200
+#define ISL38XX_CTRL_STAT_CLKRUN		0x00800000
+#define ISL38XX_CTRL_STAT_RESET			0x10000000
+#define ISL38XX_CTRL_STAT_RAMBOOT		0x20000000
+#define ISL38XX_CTRL_STAT_STARTHALTED		0x40000000
+#define ISL38XX_CTRL_STAT_HOST_OVERRIDE		0x80000000
+
+struct p54p_csr {
+	__le32 dev_int;
+	u8 unused_1[12];
+	__le32 int_ident;
+	__le32 int_ack;
+	__le32 int_enable;
+	u8 unused_2[4];
+	union {
+		__le32 ring_control_base;
+		__le32 gen_purp_com[2];
+	};
+	u8 unused_3[8];
+	__le32 direct_mem_base;
+	u8 unused_4[44];
+	__le32 dma_addr;
+	__le32 dma_len;
+	__le32 dma_ctrl;
+	u8 unused_5[12];
+	__le32 ctrl_stat;
+	u8 unused_6[1924];
+	u8 cardbus_cis[0x800];
+	u8 direct_mem_win[0x1000];
+} __attribute__ ((packed));
+
+/* usb backend only needs the register defines above */
+#ifndef PRISM54USB_H
+struct p54p_desc {
+	__le32 host_addr;
+	__le32 device_addr;
+	__le16 len;
+	__le16 flags;
+} __attribute__ ((packed));
+
+struct p54p_ring_control {
+	__le32 host_idx[4];
+	__le32 device_idx[4];
+	struct p54p_desc rx_data[8];
+	struct p54p_desc tx_data[32];
+	struct p54p_desc rx_mgmt[4];
+	struct p54p_desc tx_mgmt[4];
+} __attribute__ ((packed));
+
+#define P54P_READ(r) __raw_readl(&priv->map->r)
+#define P54P_WRITE(r, val) __raw_writel((__force u32)(val), &priv->map->r)
+
+struct p54p_priv {
+	struct p54_common common;
+	struct pci_dev *pdev;
+	struct p54p_csr __iomem *map;
+
+	spinlock_t lock;
+	struct p54p_ring_control *ring_control;
+	dma_addr_t ring_control_dma;
+	u32 rx_idx, tx_idx;
+	struct sk_buff *rx_buf[8];
+	void *tx_buf[32];
+	struct completion boot_comp;
+};
+
+#endif /* PRISM54USB_H */
+#endif /* PRISM54PCI_H */
diff --git a/drivers/net/wireless/p54usb.c b/drivers/net/wireless/p54usb.c
new file mode 100644
index 000000000000..755482a5a938
--- /dev/null
+++ b/drivers/net/wireless/p54usb.c
@@ -0,0 +1,907 @@
+
+/*
+ * Linux device driver for USB based Prism54
+ *
+ * Copyright (c) 2006, Michael Wu <flamingice@sourmilk.net>
+ *
+ * Based on the islsm (softmac prism54) driver, which is:
+ * Copyright 2004-2006 Jean-Baptiste Note <jbnote@gmail.com>, et al.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <linux/init.h>
+#include <linux/usb.h>
+#include <linux/pci.h>
+#include <linux/firmware.h>
+#include <linux/etherdevice.h>
+#include <linux/delay.h>
+#include <linux/crc32.h>
+#include <net/mac80211.h>
+
+#include "p54.h"
+#include "p54usb.h"
+
+MODULE_AUTHOR("Michael Wu <flamingice@sourmilk.net>");
+MODULE_DESCRIPTION("Prism54 USB wireless driver");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("prism54usb");
+
+static struct usb_device_id p54u_table[] __devinitdata = {
+	/* Version 1 devices (pci chip + net2280) */
+	{USB_DEVICE(0x0506, 0x0a11)},	/* 3COM 3CRWE254G72 */
+	{USB_DEVICE(0x0707, 0xee06)},	/* SMC 2862W-G */
+	{USB_DEVICE(0x083a, 0x4501)},	/* Accton 802.11g WN4501 USB */
+	{USB_DEVICE(0x083a, 0x4502)},	/* Siemens Gigaset USB Adapter */
+	{USB_DEVICE(0x0846, 0x4200)},	/* Netgear WG121 */
+	{USB_DEVICE(0x0846, 0x4210)},	/* Netgear WG121 the second ? */
+	{USB_DEVICE(0x0846, 0x4220)},	/* Netgear WG111 */
+	{USB_DEVICE(0x0cde, 0x0006)},	/* Medion 40900, Roper Europe */
+	{USB_DEVICE(0x124a, 0x4023)},	/* Shuttle PN15, Airvast WM168g, IOGear GWU513 */
+	{USB_DEVICE(0x1915, 0x2234)},	/* Linksys WUSB54G OEM */
+	{USB_DEVICE(0x1915, 0x2235)},	/* Linksys WUSB54G Portable OEM */
+	{USB_DEVICE(0x2001, 0x3701)},	/* DLink DWL-G120 Spinnaker */
+	{USB_DEVICE(0x2001, 0x3703)},	/* DLink DWL-G122 */
+	{USB_DEVICE(0x5041, 0x2234)},	/* Linksys WUSB54G */
+	{USB_DEVICE(0x5041, 0x2235)},	/* Linksys WUSB54G Portable */
+
+	/* Version 2 devices (3887) */
+	{USB_DEVICE(0x050d, 0x7050)},	/* Belkin F5D7050 ver 1000 */
+	{USB_DEVICE(0x0572, 0x2000)},	/* Cohiba Proto board */
+	{USB_DEVICE(0x0572, 0x2002)},	/* Cohiba Proto board */
+	{USB_DEVICE(0x0707, 0xee13)},   /* SMC 2862W-G version 2 */
+	{USB_DEVICE(0x083a, 0x4521)},   /* Siemens Gigaset USB Adapter 54 version 2 */
+	{USB_DEVICE(0x0846, 0x4240)},	/* Netgear WG111 (v2) */
+	{USB_DEVICE(0x0915, 0x2000)},	/* Cohiba Proto board */
+	{USB_DEVICE(0x0915, 0x2002)},	/* Cohiba Proto board */
+	{USB_DEVICE(0x0baf, 0x0118)},   /* U.S. Robotics U5 802.11g Adapter*/
+	{USB_DEVICE(0x0bf8, 0x1009)},   /* FUJITSU E-5400 USB D1700*/
+	{USB_DEVICE(0x0cde, 0x0006)},   /* Medion MD40900 */
+	{USB_DEVICE(0x0cde, 0x0008)},	/* Sagem XG703A */
+	{USB_DEVICE(0x0d8e, 0x3762)},	/* DLink DWL-G120 Cohiba */
+	{USB_DEVICE(0x09aa, 0x1000)},	/* Spinnaker Proto board */
+	{USB_DEVICE(0x13B1, 0x000C)},	/* Linksys WUSB54AG */
+	{USB_DEVICE(0x1435, 0x0427)},	/* Inventel UR054G */
+	{USB_DEVICE(0x2001, 0x3704)},	/* DLink DWL-G122 rev A2 */
+	{USB_DEVICE(0x413c, 0x8102)},	/* Spinnaker DUT */
+	{USB_DEVICE(0x413c, 0x8104)},	/* Cohiba Proto board */
+	{}
+};
+
+MODULE_DEVICE_TABLE(usb, p54u_table);
+
+static void p54u_rx_cb(struct urb *urb)
+{
+	struct sk_buff *skb = (struct sk_buff *) urb->context;
+	struct p54u_rx_info *info = (struct p54u_rx_info *)skb->cb;
+	struct ieee80211_hw *dev = info->dev;
+	struct p54u_priv *priv = dev->priv;
+
+	if (unlikely(urb->status)) {
+		info->urb = NULL;
+		usb_free_urb(urb);
+		return;
+	}
+
+	skb_unlink(skb, &priv->rx_queue);
+	skb_put(skb, urb->actual_length);
+	if (!priv->hw_type)
+		skb_pull(skb, sizeof(struct net2280_tx_hdr));
+
+	if (p54_rx(dev, skb)) {
+		skb = dev_alloc_skb(MAX_RX_SIZE);
+		if (unlikely(!skb)) {
+			usb_free_urb(urb);
+			/* TODO check rx queue length and refill *somewhere* */
+			return;
+		}
+
+		info = (struct p54u_rx_info *) skb->cb;
+		info->urb = urb;
+		info->dev = dev;
+		urb->transfer_buffer = skb_tail_pointer(skb);
+		urb->context = skb;
+		skb_queue_tail(&priv->rx_queue, skb);
+	} else {
+		skb_trim(skb, 0);
+		skb_queue_tail(&priv->rx_queue, skb);
+	}
+
+	usb_submit_urb(urb, GFP_ATOMIC);
+}
+
+static void p54u_tx_cb(struct urb *urb)
+{
+	usb_free_urb(urb);
+}
+
+static void p54u_tx_free_cb(struct urb *urb)
+{
+	kfree(urb->transfer_buffer);
+	usb_free_urb(urb);
+}
+
+static int p54u_init_urbs(struct ieee80211_hw *dev)
+{
+	struct p54u_priv *priv = dev->priv;
+	struct urb *entry;
+	struct sk_buff *skb;
+	struct p54u_rx_info *info;
+
+	while (skb_queue_len(&priv->rx_queue) < 32) {
+		skb = __dev_alloc_skb(MAX_RX_SIZE, GFP_KERNEL);
+		if (!skb)
+			break;
+		entry = usb_alloc_urb(0, GFP_KERNEL);
+		if (!entry) {
+			kfree_skb(skb);
+			break;
+		}
+		usb_fill_bulk_urb(entry, priv->udev, usb_rcvbulkpipe(priv->udev, P54U_PIPE_DATA), skb_tail_pointer(skb), MAX_RX_SIZE, p54u_rx_cb, skb);
+		info = (struct p54u_rx_info *) skb->cb;
+		info->urb = entry;
+		info->dev = dev;
+		skb_queue_tail(&priv->rx_queue, skb);
+		usb_submit_urb(entry, GFP_KERNEL);
+	}
+
+	return 0;
+}
+
+static void p54u_free_urbs(struct ieee80211_hw *dev)
+{
+	struct p54u_priv *priv = dev->priv;
+	struct p54u_rx_info *info;
+	struct sk_buff *skb;
+
+	while ((skb = skb_dequeue(&priv->rx_queue))) {
+		info = (struct p54u_rx_info *) skb->cb;
+		if (!info->urb)
+			continue;
+
+		usb_kill_urb(info->urb);
+		kfree_skb(skb);
+	}
+}
+
+static void p54u_tx_3887(struct ieee80211_hw *dev, struct p54_control_hdr *data,
+			 size_t len, int free_on_tx)
+{
+	struct p54u_priv *priv = dev->priv;
+	struct urb *addr_urb, *data_urb;
+
+	addr_urb = usb_alloc_urb(0, GFP_ATOMIC);
+	if (!addr_urb)
+		return;
+
+	data_urb = usb_alloc_urb(0, GFP_ATOMIC);
+	if (!data_urb) {
+		usb_free_urb(addr_urb);
+		return;
+	}
+
+	usb_fill_bulk_urb(addr_urb, priv->udev,
+		usb_sndbulkpipe(priv->udev, P54U_PIPE_DATA), &data->req_id,
+		sizeof(data->req_id), p54u_tx_cb, dev);
+	usb_fill_bulk_urb(data_urb, priv->udev,
+		usb_sndbulkpipe(priv->udev, P54U_PIPE_DATA), data, len,
+		free_on_tx ? p54u_tx_free_cb : p54u_tx_cb, dev);
+
+	usb_submit_urb(addr_urb, GFP_ATOMIC);
+	usb_submit_urb(data_urb, GFP_ATOMIC);
+}
+
+static void p54u_tx_net2280(struct ieee80211_hw *dev, struct p54_control_hdr *data,
+			    size_t len, int free_on_tx)
+{
+	struct p54u_priv *priv = dev->priv;
+	struct urb *int_urb, *data_urb;
+	struct net2280_tx_hdr *hdr;
+	struct net2280_reg_write *reg;
+
+	reg = kmalloc(sizeof(*reg), GFP_ATOMIC);
+	if (!reg)
+		return;
+
+	int_urb = usb_alloc_urb(0, GFP_ATOMIC);
+	if (!int_urb) {
+		kfree(reg);
+		return;
+	}
+
+	data_urb = usb_alloc_urb(0, GFP_ATOMIC);
+	if (!data_urb) {
+		kfree(reg);
+		usb_free_urb(int_urb);
+		return;
+	}
+
+	reg->port = cpu_to_le16(NET2280_DEV_U32);
+	reg->addr = cpu_to_le32(P54U_DEV_BASE);
+	reg->val = cpu_to_le32(ISL38XX_DEV_INT_DATA);
+
+	len += sizeof(*data);
+	hdr = (void *)data - sizeof(*hdr);
+	memset(hdr, 0, sizeof(*hdr));
+	hdr->device_addr = data->req_id;
+	hdr->len = cpu_to_le16(len);
+
+	usb_fill_bulk_urb(int_urb, priv->udev,
+		usb_sndbulkpipe(priv->udev, P54U_PIPE_DEV), reg, sizeof(*reg),
+		p54u_tx_free_cb, dev);
+	usb_submit_urb(int_urb, GFP_ATOMIC);
+
+	usb_fill_bulk_urb(data_urb, priv->udev,
+		usb_sndbulkpipe(priv->udev, P54U_PIPE_DATA), hdr, len + sizeof(*hdr),
+		free_on_tx ? p54u_tx_free_cb : p54u_tx_cb, dev);
+	usb_submit_urb(data_urb, GFP_ATOMIC);
+}
+
+static int p54u_write(struct p54u_priv *priv,
+		      struct net2280_reg_write *buf,
+		      enum net2280_op_type type,
+		      __le32 addr, __le32 val)
+{
+	unsigned int ep;
+	int alen;
+
+	if (type & 0x0800)
+		ep = usb_sndbulkpipe(priv->udev, P54U_PIPE_DEV);
+	else
+		ep = usb_sndbulkpipe(priv->udev, P54U_PIPE_BRG);
+
+	buf->port = cpu_to_le16(type);
+	buf->addr = addr;
+	buf->val = val;
+
+	return usb_bulk_msg(priv->udev, ep, buf, sizeof(*buf), &alen, 1000);
+}
+
+static int p54u_read(struct p54u_priv *priv, void *buf,
+		     enum net2280_op_type type,
+		     __le32 addr, __le32 *val)
+{
+	struct net2280_reg_read *read = buf;
+	__le32 *reg = buf;
+	unsigned int ep;
+	int alen, err;
+
+	if (type & 0x0800)
+		ep = P54U_PIPE_DEV;
+	else
+		ep = P54U_PIPE_BRG;
+
+	read->port = cpu_to_le16(type);
+	read->addr = addr;
+
+	err = usb_bulk_msg(priv->udev, usb_sndbulkpipe(priv->udev, ep),
+			   read, sizeof(*read), &alen, 1000);
+	if (err)
+		return err;
+
+	err = usb_bulk_msg(priv->udev, usb_rcvbulkpipe(priv->udev, ep),
+			   reg, sizeof(*reg), &alen, 1000);
+	if (err)
+		return err;
+
+	*val = *reg;
+	return 0;
+}
+
+static int p54u_bulk_msg(struct p54u_priv *priv, unsigned int ep,
+			 void *data, size_t len)
+{
+	int alen;
+	return usb_bulk_msg(priv->udev, usb_sndbulkpipe(priv->udev, ep),
+			    data, len, &alen, 2000);
+}
+
+static int p54u_read_eeprom(struct ieee80211_hw *dev)
+{
+	struct p54u_priv *priv = dev->priv;
+	void *buf;
+	struct p54_control_hdr *hdr;
+	int err, alen;
+	size_t offset = priv->hw_type ? 0x10 : 0x20;
+
+	buf = kmalloc(0x2020, GFP_KERNEL);
+	if (!buf) {
+		printk(KERN_ERR "prism54usb: cannot allocate memory for"
+		       "eeprom readback!\n");
+		return -ENOMEM;
+	}
+
+	if (priv->hw_type) {
+		*((u32 *) buf) = priv->common.rx_start;
+		err = p54u_bulk_msg(priv, P54U_PIPE_DATA, buf, sizeof(u32));
+		if (err) {
+			printk(KERN_ERR "prism54usb: addr send failed\n");
+			goto fail;
+		}
+	} else {
+		struct net2280_reg_write *reg = buf;
+		reg->port = cpu_to_le16(NET2280_DEV_U32);
+		reg->addr = cpu_to_le32(P54U_DEV_BASE);
+		reg->val = cpu_to_le32(ISL38XX_DEV_INT_DATA);
+		err = p54u_bulk_msg(priv, P54U_PIPE_DEV, buf, sizeof(*reg));
+		if (err) {
+			printk(KERN_ERR "prism54usb: dev_int send failed\n");
+			goto fail;
+		}
+	}
+
+	hdr = buf + priv->common.tx_hdr_len;
+	p54_fill_eeprom_readback(hdr);
+	hdr->req_id = cpu_to_le32(priv->common.rx_start);
+	if (priv->common.tx_hdr_len) {
+		struct net2280_tx_hdr *tx_hdr = buf;
+		tx_hdr->device_addr = hdr->req_id;
+		tx_hdr->len = cpu_to_le16(EEPROM_READBACK_LEN);
+	}
+
+	/* we can just pretend to send 0x2000 bytes of nothing in the headers */
+	err = p54u_bulk_msg(priv, P54U_PIPE_DATA, buf,
+			    EEPROM_READBACK_LEN + priv->common.tx_hdr_len);
+	if (err) {
+		printk(KERN_ERR "prism54usb: eeprom req send failed\n");
+		goto fail;
+	}
+
+	err = usb_bulk_msg(priv->udev,
+			   usb_rcvbulkpipe(priv->udev, P54U_PIPE_DATA),
+			   buf, 0x2020, &alen, 1000);
+	if (!err && alen > offset) {
+		p54_parse_eeprom(dev, (u8 *)buf + offset, alen - offset);
+	} else {
+		printk(KERN_ERR "prism54usb: eeprom read failed!\n");
+		err = -EINVAL;
+		goto fail;
+	}
+
+ fail:
+	kfree(buf);
+	return err;
+}
+
+static int p54u_upload_firmware_3887(struct ieee80211_hw *dev)
+{
+	static char start_string[] = "~~~~<\r";
+	struct p54u_priv *priv = dev->priv;
+	const struct firmware *fw_entry = NULL;
+	int err, alen;
+	u8 carry = 0;
+	u8 *buf, *tmp, *data;
+	unsigned int left, remains, block_size;
+	struct x2_header *hdr;
+	unsigned long timeout;
+
+	tmp = buf = kmalloc(P54U_FW_BLOCK, GFP_KERNEL);
+	if (!buf) {
+		printk(KERN_ERR "p54usb: cannot allocate firmware upload buffer!\n");
+		err = -ENOMEM;
+		goto err_bufalloc;
+	}
+
+	memcpy(buf, start_string, 4);
+	err = p54u_bulk_msg(priv, P54U_PIPE_DATA, buf, 4);
+	if (err) {
+		printk(KERN_ERR "p54usb: reset failed! (%d)\n", err);
+		goto err_reset;
+	}
+
+	err = request_firmware(&fw_entry, "isl3887usb_bare", &priv->udev->dev);
+	if (err) {
+		printk(KERN_ERR "p54usb: cannot find firmware (isl3887usb_bare)!\n");
+		goto err_req_fw_failed;
+	}
+
+	p54_parse_firmware(dev, fw_entry);
+
+	left = block_size = min((size_t)P54U_FW_BLOCK, fw_entry->size);
+	strcpy(buf, start_string);
+	left -= strlen(start_string);
+	tmp += strlen(start_string);
+
+	data = fw_entry->data;
+	remains = fw_entry->size;
+
+	hdr = (struct x2_header *)(buf + strlen(start_string));
+	memcpy(hdr->signature, X2_SIGNATURE, X2_SIGNATURE_SIZE);
+	hdr->fw_load_addr = cpu_to_le32(ISL38XX_DEV_FIRMWARE_ADDR);
+	hdr->fw_length = cpu_to_le32(fw_entry->size);
+	hdr->crc = cpu_to_le32(~crc32_le(~0, (void *)&hdr->fw_load_addr,
+					 sizeof(u32)*2));
+	left -= sizeof(*hdr);
+	tmp += sizeof(*hdr);
+
+	while (remains) {
+		while (left--) {
+			if (carry) {
+				*tmp++ = carry;
+				carry = 0;
+				remains--;
+				continue;
+			}
+			switch (*data) {
+			case '~':
+				*tmp++ = '}';
+				carry = '^';
+				break;
+			case '}':
+				*tmp++ = '}';
+				carry = ']';
+				break;
+			default:
+				*tmp++ = *data;
+				remains--;
+				break;
+			}
+			data++;
+		}
+
+		err = p54u_bulk_msg(priv, P54U_PIPE_DATA, buf, block_size);
+		if (err) {
+			printk(KERN_ERR "prism54usb: firmware upload failed!\n");
+			goto err_upload_failed;
+		}
+
+		tmp = buf;
+		left = block_size = min((unsigned int)P54U_FW_BLOCK, remains);
+	}
+
+	*((__le32 *)buf) = cpu_to_le32(~crc32_le(~0, fw_entry->data, fw_entry->size));
+	err = p54u_bulk_msg(priv, P54U_PIPE_DATA, buf, sizeof(u32));
+	if (err) {
+		printk(KERN_ERR "prism54usb: firmware upload failed!\n");
+		goto err_upload_failed;
+	}
+
+	timeout = jiffies + msecs_to_jiffies(1000);
+	while (!(err = usb_bulk_msg(priv->udev,
+		usb_rcvbulkpipe(priv->udev, P54U_PIPE_DATA), buf, 128, &alen, 1000))) {
+		if (alen > 2 && !memcmp(buf, "OK", 2))
+			break;
+
+		if (alen > 5 && !memcmp(buf, "ERROR", 5)) {
+			printk(KERN_INFO "prism54usb: firmware upload failed!\n");
+			err = -EINVAL;
+			break;
+		}
+
+		if (time_after(jiffies, timeout)) {
+			printk(KERN_ERR "prism54usb: firmware boot timed out!\n");
+			err = -ETIMEDOUT;
+			break;
+		}
+	}
+	if (err)
+		goto err_upload_failed;
+
+	buf[0] = 'g';
+	buf[1] = '\r';
+	err = p54u_bulk_msg(priv, P54U_PIPE_DATA, buf, 2);
+	if (err) {
+		printk(KERN_ERR "prism54usb: firmware boot failed!\n");
+		goto err_upload_failed;
+	}
+
+	timeout = jiffies + msecs_to_jiffies(1000);
+	while (!(err = usb_bulk_msg(priv->udev,
+		usb_rcvbulkpipe(priv->udev, P54U_PIPE_DATA), buf, 128, &alen, 1000))) {
+		if (alen > 0 && buf[0] == 'g')
+			break;
+
+		if (time_after(jiffies, timeout)) {
+			err = -ETIMEDOUT;
+			break;
+		}
+	}
+	if (err)
+		goto err_upload_failed;
+
+  err_upload_failed:
+	release_firmware(fw_entry);
+  err_req_fw_failed:
+  err_reset:
+	kfree(buf);
+  err_bufalloc:
+	return err;
+}
+
+static int p54u_upload_firmware_net2280(struct ieee80211_hw *dev)
+{
+	struct p54u_priv *priv = dev->priv;
+	const struct firmware *fw_entry = NULL;
+	const struct p54p_csr *devreg = (const struct p54p_csr *) P54U_DEV_BASE;
+	int err, alen;
+	void *buf;
+	__le32 reg;
+	unsigned int remains, offset;
+	u8 *data;
+
+	buf = kmalloc(512, GFP_KERNEL);
+	if (!buf) {
+		printk(KERN_ERR "p54usb: firmware buffer alloc failed!\n");
+		return -ENOMEM;
+	}
+
+	err = request_firmware(&fw_entry, "isl3890usb", &priv->udev->dev);
+	if (err) {
+		printk(KERN_ERR "p54usb: cannot find firmware (isl3890usb)!\n");
+		kfree(buf);
+		return err;
+	}
+
+	p54_parse_firmware(dev, fw_entry);
+
+#define P54U_WRITE(type, addr, data) \
+	do {\
+		err = p54u_write(priv, buf, type,\
+				 cpu_to_le32((u32)(unsigned long)addr), data);\
+		if (err) \
+			goto fail;\
+	} while (0)
+
+#define P54U_READ(type, addr) \
+	do {\
+		err = p54u_read(priv, buf, type,\
+				cpu_to_le32((u32)(unsigned long)addr), &reg);\
+		if (err)\
+			goto fail;\
+	} while (0)
+
+	/* power down net2280 bridge */
+	P54U_READ(NET2280_BRG_U32, NET2280_GPIOCTL);
+	reg |= cpu_to_le32(P54U_BRG_POWER_DOWN);
+	reg &= cpu_to_le32(~P54U_BRG_POWER_UP);
+	P54U_WRITE(NET2280_BRG_U32, NET2280_GPIOCTL, reg);
+
+	mdelay(100);
+
+	/* power up bridge */
+	reg |= cpu_to_le32(P54U_BRG_POWER_UP);
+	reg &= cpu_to_le32(~P54U_BRG_POWER_DOWN);
+	P54U_WRITE(NET2280_BRG_U32, NET2280_GPIOCTL, reg);
+
+	mdelay(100);
+
+	P54U_WRITE(NET2280_BRG_U32, NET2280_DEVINIT,
+		   cpu_to_le32(NET2280_CLK_30Mhz |
+			       NET2280_PCI_ENABLE |
+			       NET2280_PCI_SOFT_RESET));
+
+	mdelay(20);
+
+	P54U_WRITE(NET2280_BRG_CFG_U16, PCI_COMMAND,
+		   cpu_to_le32(PCI_COMMAND_MEMORY |
+			       PCI_COMMAND_MASTER));
+
+	P54U_WRITE(NET2280_BRG_CFG_U32, PCI_BASE_ADDRESS_0,
+		   cpu_to_le32(NET2280_BASE));
+
+	P54U_READ(NET2280_BRG_CFG_U16, PCI_STATUS);
+	reg |= cpu_to_le32(PCI_STATUS_REC_MASTER_ABORT);
+	P54U_WRITE(NET2280_BRG_CFG_U16, PCI_STATUS, reg);
+
+	// TODO: we really need this?
+	P54U_READ(NET2280_BRG_U32, NET2280_RELNUM);
+
+	P54U_WRITE(NET2280_BRG_U32, NET2280_EPA_RSP,
+		   cpu_to_le32(NET2280_CLEAR_NAK_OUT_PACKETS_MODE));
+	P54U_WRITE(NET2280_BRG_U32, NET2280_EPC_RSP,
+		   cpu_to_le32(NET2280_CLEAR_NAK_OUT_PACKETS_MODE));
+
+	P54U_WRITE(NET2280_BRG_CFG_U32, PCI_BASE_ADDRESS_2,
+		   cpu_to_le32(NET2280_BASE2));
+
+	/* finally done setting up the bridge */
+
+	P54U_WRITE(NET2280_DEV_CFG_U16, 0x10000 | PCI_COMMAND,
+		   cpu_to_le32(PCI_COMMAND_MEMORY |
+			       PCI_COMMAND_MASTER));
+
+	P54U_WRITE(NET2280_DEV_CFG_U16, 0x10000 | 0x40 /* TRDY timeout */, 0);
+	P54U_WRITE(NET2280_DEV_CFG_U32, 0x10000 | PCI_BASE_ADDRESS_0,
+		   cpu_to_le32(P54U_DEV_BASE));
+
+	P54U_WRITE(NET2280_BRG_U32, NET2280_USBIRQENB1, 0);
+	P54U_WRITE(NET2280_BRG_U32, NET2280_IRQSTAT1,
+		   cpu_to_le32(NET2280_PCI_INTA_INTERRUPT));
+
+	/* do romboot */
+	P54U_WRITE(NET2280_DEV_U32, &devreg->int_enable, 0);
+
+	P54U_READ(NET2280_DEV_U32, &devreg->ctrl_stat);
+	reg &= cpu_to_le32(~ISL38XX_CTRL_STAT_RESET);
+	reg &= cpu_to_le32(~ISL38XX_CTRL_STAT_RAMBOOT);
+	reg &= cpu_to_le32(~ISL38XX_CTRL_STAT_CLKRUN);
+	P54U_WRITE(NET2280_DEV_U32, &devreg->ctrl_stat, reg);
+
+	mdelay(20);
+
+	reg |= cpu_to_le32(ISL38XX_CTRL_STAT_RESET);
+	P54U_WRITE(NET2280_DEV_U32, &devreg->ctrl_stat, reg);
+
+	mdelay(20);
+
+	reg &= cpu_to_le32(~ISL38XX_CTRL_STAT_RESET);
+	P54U_WRITE(NET2280_DEV_U32, &devreg->ctrl_stat, reg);
+
+	mdelay(100);
+
+	P54U_READ(NET2280_DEV_U32, &devreg->int_ident);
+	P54U_WRITE(NET2280_DEV_U32, &devreg->int_ack, reg);
+
+	/* finally, we can upload firmware now! */
+	remains = fw_entry->size;
+	data = fw_entry->data;
+	offset = ISL38XX_DEV_FIRMWARE_ADDR;
+
+	while (remains) {
+		unsigned int block_len = min(remains, (unsigned int)512);
+		memcpy(buf, data, block_len);
+
+		err = p54u_bulk_msg(priv, P54U_PIPE_DATA, buf, block_len);
+		if (err) {
+			printk(KERN_ERR "prism54usb: firmware block upload "
+			       "failed\n");
+			goto fail;
+		}
+
+		P54U_WRITE(NET2280_DEV_U32, &devreg->direct_mem_base,
+			   cpu_to_le32(0xc0000f00));
+
+		P54U_WRITE(NET2280_DEV_U32,
+			   0x0020 | (unsigned long)&devreg->direct_mem_win, 0);
+		P54U_WRITE(NET2280_DEV_U32,
+			   0x0020 | (unsigned long)&devreg->direct_mem_win,
+			   cpu_to_le32(1));
+
+		P54U_WRITE(NET2280_DEV_U32,
+			   0x0024 | (unsigned long)&devreg->direct_mem_win,
+			   cpu_to_le32(block_len));
+		P54U_WRITE(NET2280_DEV_U32,
+			   0x0028 | (unsigned long)&devreg->direct_mem_win,
+			   cpu_to_le32(offset));
+
+		P54U_WRITE(NET2280_DEV_U32, &devreg->dma_addr,
+			   cpu_to_le32(NET2280_EPA_FIFO_PCI_ADDR));
+		P54U_WRITE(NET2280_DEV_U32, &devreg->dma_len,
+			   cpu_to_le32(block_len >> 2));
+		P54U_WRITE(NET2280_DEV_U32, &devreg->dma_ctrl,
+			   cpu_to_le32(ISL38XX_DMA_MASTER_CONTROL_TRIGGER));
+
+		mdelay(10);
+
+		P54U_READ(NET2280_DEV_U32,
+			  0x002C | (unsigned long)&devreg->direct_mem_win);
+		if (!(reg & cpu_to_le32(ISL38XX_DMA_STATUS_DONE)) ||
+		    !(reg & cpu_to_le32(ISL38XX_DMA_STATUS_READY))) {
+			printk(KERN_ERR "prism54usb: firmware DMA transfer "
+			       "failed\n");
+			goto fail;
+		}
+
+		P54U_WRITE(NET2280_BRG_U32, NET2280_EPA_STAT,
+			   cpu_to_le32(NET2280_FIFO_FLUSH));
+
+		remains -= block_len;
+		data += block_len;
+		offset += block_len;
+	}
+
+	/* do ramboot */
+	P54U_READ(NET2280_DEV_U32, &devreg->ctrl_stat);
+	reg &= cpu_to_le32(~ISL38XX_CTRL_STAT_RESET);
+	reg &= cpu_to_le32(~ISL38XX_CTRL_STAT_CLKRUN);
+	reg |= cpu_to_le32(ISL38XX_CTRL_STAT_RAMBOOT);
+	P54U_WRITE(NET2280_DEV_U32, &devreg->ctrl_stat, reg);
+
+	mdelay(20);
+
+	reg |= cpu_to_le32(ISL38XX_CTRL_STAT_RESET);
+	P54U_WRITE(NET2280_DEV_U32, &devreg->ctrl_stat, reg);
+
+	reg &= cpu_to_le32(~ISL38XX_CTRL_STAT_RESET);
+	P54U_WRITE(NET2280_DEV_U32, &devreg->ctrl_stat, reg);
+
+	mdelay(100);
+
+	P54U_READ(NET2280_DEV_U32, &devreg->int_ident);
+	P54U_WRITE(NET2280_DEV_U32, &devreg->int_ack, reg);
+
+	/* start up the firmware */
+	P54U_WRITE(NET2280_DEV_U32, &devreg->int_enable,
+		   cpu_to_le32(ISL38XX_INT_IDENT_INIT));
+
+	P54U_WRITE(NET2280_BRG_U32, NET2280_IRQSTAT1,
+		   cpu_to_le32(NET2280_PCI_INTA_INTERRUPT));
+
+	P54U_WRITE(NET2280_BRG_U32, NET2280_USBIRQENB1,
+		   cpu_to_le32(NET2280_PCI_INTA_INTERRUPT_ENABLE |
+			       NET2280_USB_INTERRUPT_ENABLE));
+
+	P54U_WRITE(NET2280_DEV_U32, &devreg->dev_int,
+		   cpu_to_le32(ISL38XX_DEV_INT_RESET));
+
+	err = usb_interrupt_msg(priv->udev,
+				usb_rcvbulkpipe(priv->udev, P54U_PIPE_INT),
+				buf, sizeof(__le32), &alen, 1000);
+	if (err || alen != sizeof(__le32))
+		goto fail;
+
+	P54U_READ(NET2280_DEV_U32, &devreg->int_ident);
+	P54U_WRITE(NET2280_DEV_U32, &devreg->int_ack, reg);
+
+	if (!(reg & cpu_to_le32(ISL38XX_INT_IDENT_INIT)))
+		err = -EINVAL;
+
+	P54U_WRITE(NET2280_BRG_U32, NET2280_USBIRQENB1, 0);
+	P54U_WRITE(NET2280_BRG_U32, NET2280_IRQSTAT1,
+		   cpu_to_le32(NET2280_PCI_INTA_INTERRUPT));
+
+#undef P54U_WRITE
+#undef P54U_READ
+
+ fail:
+	release_firmware(fw_entry);
+	kfree(buf);
+	return err;
+}
+
+static int p54u_open(struct ieee80211_hw *dev)
+{
+	struct p54u_priv *priv = dev->priv;
+	int err;
+
+	err = p54u_init_urbs(dev);
+	if (err) {
+		return err;
+	}
+
+	priv->common.open = p54u_init_urbs;
+
+	return 0;
+}
+
+static void p54u_stop(struct ieee80211_hw *dev)
+{
+	/* TODO: figure out how to reliably stop the 3887 and net2280 so
+	   the hardware is still usable next time we want to start it.
+	   until then, we just stop listening to the hardware.. */
+	p54u_free_urbs(dev);
+	return;
+}
+
+static int __devinit p54u_probe(struct usb_interface *intf,
+				const struct usb_device_id *id)
+{
+	struct usb_device *udev = interface_to_usbdev(intf);
+	struct ieee80211_hw *dev;
+	struct p54u_priv *priv;
+	int err;
+	unsigned int i, recognized_pipes;
+	DECLARE_MAC_BUF(mac);
+
+	dev = p54_init_common(sizeof(*priv));
+	if (!dev) {
+		printk(KERN_ERR "prism54usb: ieee80211 alloc failed\n");
+		return -ENOMEM;
+	}
+
+	priv = dev->priv;
+
+	SET_IEEE80211_DEV(dev, &intf->dev);
+	usb_set_intfdata(intf, dev);
+	priv->udev = udev;
+
+	usb_get_dev(udev);
+
+	/* really lazy and simple way of figuring out if we're a 3887 */
+	/* TODO: should just stick the identification in the device table */
+	i = intf->altsetting->desc.bNumEndpoints;
+	recognized_pipes = 0;
+	while (i--) {
+		switch (intf->altsetting->endpoint[i].desc.bEndpointAddress) {
+		case P54U_PIPE_DATA:
+		case P54U_PIPE_MGMT:
+		case P54U_PIPE_BRG:
+		case P54U_PIPE_DEV:
+		case P54U_PIPE_DATA | USB_DIR_IN:
+		case P54U_PIPE_MGMT | USB_DIR_IN:
+		case P54U_PIPE_BRG | USB_DIR_IN:
+		case P54U_PIPE_DEV | USB_DIR_IN:
+		case P54U_PIPE_INT | USB_DIR_IN:
+			recognized_pipes++;
+		}
+	}
+	priv->common.open = p54u_open;
+
+	if (recognized_pipes < P54U_PIPE_NUMBER) {
+		priv->hw_type = P54U_3887;
+		priv->common.tx = p54u_tx_3887;
+	} else {
+		dev->extra_tx_headroom += sizeof(struct net2280_tx_hdr);
+		priv->common.tx_hdr_len = sizeof(struct net2280_tx_hdr);
+		priv->common.tx = p54u_tx_net2280;
+	}
+	priv->common.stop = p54u_stop;
+
+	if (priv->hw_type)
+		err = p54u_upload_firmware_3887(dev);
+	else
+		err = p54u_upload_firmware_net2280(dev);
+	if (err)
+		goto err_free_dev;
+
+	err = p54u_read_eeprom(dev);
+	if (err)
+		goto err_free_dev;
+
+	if (!is_valid_ether_addr(dev->wiphy->perm_addr)) {
+		u8 perm_addr[ETH_ALEN];
+
+		printk(KERN_WARNING "prism54usb: Invalid hwaddr! Using randomly generated MAC addr\n");
+		random_ether_addr(perm_addr);
+		SET_IEEE80211_PERM_ADDR(dev, perm_addr);
+	}
+
+	skb_queue_head_init(&priv->rx_queue);
+
+	err = ieee80211_register_hw(dev);
+	if (err) {
+		printk(KERN_ERR "prism54usb: Cannot register netdevice\n");
+		goto err_free_dev;
+	}
+
+	printk(KERN_INFO "%s: hwaddr %s, isl38%02x\n",
+	       wiphy_name(dev->wiphy),
+	       print_mac(mac, dev->wiphy->perm_addr),
+	       priv->common.version);
+
+	return 0;
+
+ err_free_dev:
+	ieee80211_free_hw(dev);
+	usb_set_intfdata(intf, NULL);
+	usb_put_dev(udev);
+	return err;
+}
+
+static void __devexit p54u_disconnect(struct usb_interface *intf)
+{
+	struct ieee80211_hw *dev = usb_get_intfdata(intf);
+	struct p54u_priv *priv;
+
+	if (!dev)
+		return;
+
+	ieee80211_unregister_hw(dev);
+
+	priv = dev->priv;
+	usb_put_dev(interface_to_usbdev(intf));
+	p54_free_common(dev);
+	ieee80211_free_hw(dev);
+}
+
+static struct usb_driver p54u_driver = {
+	.name	= "prism54usb",
+	.id_table = p54u_table,
+	.probe = p54u_probe,
+	.disconnect = p54u_disconnect,
+};
+
+static int __init p54u_init(void)
+{
+	return usb_register(&p54u_driver);
+}
+
+static void __exit p54u_exit(void)
+{
+	usb_deregister(&p54u_driver);
+}
+
+module_init(p54u_init);
+module_exit(p54u_exit);
diff --git a/drivers/net/wireless/p54usb.h b/drivers/net/wireless/p54usb.h
new file mode 100644
index 000000000000..d1896b396c1c
--- /dev/null
+++ b/drivers/net/wireless/p54usb.h
@@ -0,0 +1,133 @@
+#ifndef PRISM54USB_H
+#define PRISM54USB_H
+
+/*
+ * Defines for USB based mac80211 Prism54 driver
+ *
+ * Copyright (c) 2006, Michael Wu <flamingice@sourmilk.net>
+ *
+ * Based on the islsm (softmac prism54) driver, which is:
+ * Copyright 2004-2006 Jean-Baptiste Note <jbnote@gmail.com>, et al.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+/* for isl3886 register definitions used on ver 1 devices */
+#include "p54pci.h"
+#include "net2280.h"
+
+/* pci */
+#define NET2280_BASE		0x10000000
+#define NET2280_BASE2		0x20000000
+
+/* gpio */
+#define P54U_BRG_POWER_UP	(1 << GPIO0_DATA)
+#define P54U_BRG_POWER_DOWN	(1 << GPIO1_DATA)
+
+/* devinit */
+#define NET2280_CLK_4Mhz	(15 << LOCAL_CLOCK_FREQUENCY)
+#define NET2280_CLK_30Mhz	(2 << LOCAL_CLOCK_FREQUENCY)
+#define NET2280_CLK_60Mhz	(1 << LOCAL_CLOCK_FREQUENCY)
+#define NET2280_CLK_STOP	(0 << LOCAL_CLOCK_FREQUENCY)
+#define NET2280_PCI_ENABLE	(1 << PCI_ENABLE)
+#define NET2280_PCI_SOFT_RESET	(1 << PCI_SOFT_RESET)
+
+/* endpoints */
+#define NET2280_CLEAR_NAK_OUT_PACKETS_MODE	(1 << CLEAR_NAK_OUT_PACKETS_MODE)
+#define NET2280_FIFO_FLUSH			(1 << FIFO_FLUSH)
+
+/* irq */
+#define NET2280_USB_INTERRUPT_ENABLE		(1 << USB_INTERRUPT_ENABLE)
+#define NET2280_PCI_INTA_INTERRUPT		(1 << PCI_INTA_INTERRUPT)
+#define NET2280_PCI_INTA_INTERRUPT_ENABLE	(1 << PCI_INTA_INTERRUPT_ENABLE)
+
+/* registers */
+#define NET2280_DEVINIT		0x00
+#define NET2280_USBIRQENB1	0x24
+#define NET2280_IRQSTAT1	0x2c
+#define NET2280_FIFOCTL         0x38
+#define NET2280_GPIOCTL		0x50
+#define NET2280_RELNUM		0x88
+#define NET2280_EPA_RSP		0x324
+#define NET2280_EPA_STAT	0x32c
+#define NET2280_EPB_STAT	0x34c
+#define NET2280_EPC_RSP		0x364
+#define NET2280_EPC_STAT	0x36c
+#define NET2280_EPD_STAT	0x38c
+
+#define NET2280_EPA_CFG     0x320
+#define NET2280_EPB_CFG     0x340
+#define NET2280_EPC_CFG     0x360
+#define NET2280_EPD_CFG     0x380
+#define NET2280_EPE_CFG     0x3A0
+#define NET2280_EPF_CFG     0x3C0
+#define P54U_DEV_BASE 0x40000000
+
+struct net2280_tx_hdr {
+	__le32 device_addr;
+	__le16 len;
+	__le16 follower;	/* ? */
+	u8 padding[8];
+} __attribute__((packed));
+
+/* Some flags for the isl hardware registers controlling DMA inside the
+ * chip */
+#define ISL38XX_DMA_STATUS_DONE			0x00000001
+#define ISL38XX_DMA_STATUS_READY		0x00000002
+#define NET2280_EPA_FIFO_PCI_ADDR		0x20000000
+#define ISL38XX_DMA_MASTER_CONTROL_TRIGGER	0x00000004
+
+enum net2280_op_type {
+	NET2280_BRG_U32		= 0x001F,
+	NET2280_BRG_CFG_U32	= 0x000F,
+	NET2280_BRG_CFG_U16	= 0x0003,
+	NET2280_DEV_U32		= 0x080F,
+	NET2280_DEV_CFG_U32	= 0x088F,
+	NET2280_DEV_CFG_U16	= 0x0883
+};
+
+#define P54U_FW_BLOCK 2048
+
+#define X2_SIGNATURE "x2  "
+#define X2_SIGNATURE_SIZE 4
+
+struct x2_header {
+	u8 signature[X2_SIGNATURE_SIZE];
+	__le32 fw_load_addr;
+	__le32 fw_length;
+	__le32 crc;
+} __attribute__((packed));
+
+/* pipes 3 and 4 are not used by the driver */
+#define P54U_PIPE_NUMBER 9
+
+enum p54u_pipe_addr {
+        P54U_PIPE_DATA = 0x01,
+        P54U_PIPE_MGMT = 0x02,
+        P54U_PIPE_3 = 0x03,
+        P54U_PIPE_4 = 0x04,
+        P54U_PIPE_BRG = 0x0d,
+        P54U_PIPE_DEV = 0x0e,
+        P54U_PIPE_INT = 0x0f
+};
+
+struct p54u_rx_info {
+	struct urb *urb;
+	struct ieee80211_hw *dev;
+};
+
+struct p54u_priv {
+	struct p54_common common;
+	struct usb_device *udev;
+	enum {
+		P54U_NET2280 = 0,
+		P54U_3887
+	} hw_type;
+
+	spinlock_t lock;
+	struct sk_buff_head rx_queue;
+};
+
+#endif /* PRISM54USB_H */
diff --git a/drivers/net/wireless/prism54/isl_ioctl.c b/drivers/net/wireless/prism54/isl_ioctl.c
index 585f5996d292..6d80ca421cf0 100644
--- a/drivers/net/wireless/prism54/isl_ioctl.c
+++ b/drivers/net/wireless/prism54/isl_ioctl.c
@@ -1753,7 +1753,7 @@ prism54_get_oid(struct net_device *ndev, struct iw_request_info *info,
 	int rvalue;
 	enum oid_num_t n = dwrq->flags;
 
-	rvalue = mgt_get_request((islpci_private *) ndev->priv, n, 0, NULL, &r);
+	rvalue = mgt_get_request(netdev_priv(ndev), n, 0, NULL, &r);
 	dwrq->length = mgt_response_to_str(n, &r, extra);
 	if ((isl_oid[n].flags & OID_FLAG_TYPE) != OID_TYPE_U32)
 		kfree(r.ptr);
@@ -1766,7 +1766,7 @@ prism54_set_u32(struct net_device *ndev, struct iw_request_info *info,
 {
 	u32 oid = uwrq[0], u = uwrq[1];
 
-	return mgt_set_request((islpci_private *) ndev->priv, oid, 0, &u);
+	return mgt_set_request(netdev_priv(ndev), oid, 0, &u);
 }
 
 static int
@@ -1775,7 +1775,7 @@ prism54_set_raw(struct net_device *ndev, struct iw_request_info *info,
 {
 	u32 oid = dwrq->flags;
 
-	return mgt_set_request((islpci_private *) ndev->priv, oid, 0, extra);
+	return mgt_set_request(netdev_priv(ndev), oid, 0, extra);
 }
 
 void
@@ -2029,12 +2029,12 @@ static void
 format_event(islpci_private *priv, char *dest, const char *str,
 	     const struct obj_mlme *mlme, u16 *length, int error)
 {
-	const u8 *a = mlme->address;
+	DECLARE_MAC_BUF(mac);
 	int n = snprintf(dest, IW_CUSTOM_MAX,
-			 "%s %s %2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X %s (%2.2X)",
+			 "%s %s %s %s (%2.2X)",
 			 str,
 			 ((priv->iw_mode == IW_MODE_MASTER) ? "from" : "to"),
-			 a[0], a[1], a[2], a[3], a[4], a[5],
+			 print_mac(mac, mlme->address),
 			 (error ? (mlme->code ? " : REJECTED " : " : ACCEPTED ")
 			  : ""), mlme->code);
 	BUG_ON(n > IW_CUSTOM_MAX);
@@ -2105,15 +2105,13 @@ struct ieee80211_beacon_phdr {
 #define WLAN_EID_GENERIC 0xdd
 static u8 wpa_oid[4] = { 0x00, 0x50, 0xf2, 1 };
 
-#define MAC2STR(a) (a)[0], (a)[1], (a)[2], (a)[3], (a)[4], (a)[5]
-#define MACSTR "%02x:%02x:%02x:%02x:%02x:%02x"
-
 static void
 prism54_wpa_bss_ie_add(islpci_private *priv, u8 *bssid,
 		       u8 *wpa_ie, size_t wpa_ie_len)
 {
 	struct list_head *ptr;
 	struct islpci_bss_wpa_ie *bss = NULL;
+	DECLARE_MAC_BUF(mac);
 
 	if (wpa_ie_len > MAX_WPA_IE_LEN)
 		wpa_ie_len = MAX_WPA_IE_LEN;
@@ -2154,8 +2152,8 @@ prism54_wpa_bss_ie_add(islpci_private *priv, u8 *bssid,
 		bss->wpa_ie_len = wpa_ie_len;
 		bss->last_update = jiffies;
 	} else {
-		printk(KERN_DEBUG "Failed to add BSS WPA entry for " MACSTR
-		       "\n", MAC2STR(bssid));
+		printk(KERN_DEBUG "Failed to add BSS WPA entry for "
+		       "%s\n", print_mac(mac, bssid));
 	}
 
 	/* expire old entries from WPA list */
@@ -2221,6 +2219,7 @@ prism54_process_bss_data(islpci_private *priv, u32 oid, u8 *addr,
 {
 	struct ieee80211_beacon_phdr *hdr;
 	u8 *pos, *end;
+	DECLARE_MAC_BUF(mac);
 
 	if (!priv->wpa)
 		return;
@@ -2231,7 +2230,7 @@ prism54_process_bss_data(islpci_private *priv, u32 oid, u8 *addr,
 	while (pos < end) {
 		if (pos + 2 + pos[1] > end) {
 			printk(KERN_DEBUG "Parsing Beacon/ProbeResp failed "
-			       "for " MACSTR "\n", MAC2STR(addr));
+			       "for %s\n", print_mac(mac, addr));
 			return;
 		}
 		if (pos[0] == WLAN_EID_GENERIC && pos[1] >= 4 &&
@@ -2270,6 +2269,7 @@ prism54_process_trap_helper(islpci_private *priv, enum oid_num_t oid,
 	size_t len = 0; /* u16, better? */
 	u8 *payload = NULL, *pos = NULL;
 	int ret;
+	DECLARE_MAC_BUF(mac);
 
 	/* I think all trapable objects are listed here.
 	 * Some oids have a EX version. The difference is that they are emitted
@@ -2358,14 +2358,8 @@ prism54_process_trap_helper(islpci_private *priv, enum oid_num_t oid,
 			break;
 
 		memcpy(&confirm->address, mlmeex->address, ETH_ALEN);
-		printk(KERN_DEBUG "Authenticate from: address:\t%02x:%02x:%02x:%02x:%02x:%02x\n",
-				mlmeex->address[0],
-				mlmeex->address[1],
-				mlmeex->address[2],
-				mlmeex->address[3],
-				mlmeex->address[4],
-				mlmeex->address[5]
-				);
+		printk(KERN_DEBUG "Authenticate from: address:\t%s\n",
+		       print_mac(mac, mlmeex->address));
 		confirm->id = -1; /* or mlmeex->id ? */
 		confirm->state = 0; /* not used */
 		confirm->code = 0;
@@ -2410,15 +2404,8 @@ prism54_process_trap_helper(islpci_private *priv, enum oid_num_t oid,
 		wpa_ie_len = prism54_wpa_bss_ie_get(priv, mlmeex->address, wpa_ie);
 
 		if (!wpa_ie_len) {
-			printk(KERN_DEBUG "No WPA IE found from "
-					"address:\t%02x:%02x:%02x:%02x:%02x:%02x\n",
-				mlmeex->address[0],
-				mlmeex->address[1],
-				mlmeex->address[2],
-				mlmeex->address[3],
-				mlmeex->address[4],
-				mlmeex->address[5]
-				);
+			printk(KERN_DEBUG "No WPA IE found from address:\t%s\n",
+			       print_mac(mac, mlmeex->address));
 			kfree(confirm);
 			break;
 		}
@@ -2454,15 +2441,8 @@ prism54_process_trap_helper(islpci_private *priv, enum oid_num_t oid,
 		wpa_ie_len = prism54_wpa_bss_ie_get(priv, mlmeex->address, wpa_ie);
 
 		if (!wpa_ie_len) {
-			printk(KERN_DEBUG "No WPA IE found from "
-					"address:\t%02x:%02x:%02x:%02x:%02x:%02x\n",
-				mlmeex->address[0],
-				mlmeex->address[1],
-				mlmeex->address[2],
-				mlmeex->address[3],
-				mlmeex->address[4],
-				mlmeex->address[5]
-				);
+			printk(KERN_DEBUG "No WPA IE found from address:\t%s\n",
+			       print_mac(mac, mlmeex->address));
 			kfree(confirm);
 			break;
 		}
@@ -3239,10 +3219,9 @@ static const iw_handler prism54_private_handler[] = {
 };
 
 const struct iw_handler_def prism54_handler_def = {
-	.num_standard = sizeof (prism54_handler) / sizeof (iw_handler),
-	.num_private = sizeof (prism54_private_handler) / sizeof (iw_handler),
-	.num_private_args =
-	    sizeof (prism54_private_args) / sizeof (struct iw_priv_args),
+	.num_standard = ARRAY_SIZE(prism54_handler),
+	.num_private = ARRAY_SIZE(prism54_private_handler),
+	.num_private_args = ARRAY_SIZE(prism54_private_args),
 	.standard = (iw_handler *) prism54_handler,
 	.private = (iw_handler *) prism54_private_handler,
 	.private_args = (struct iw_priv_args *) prism54_private_args,
diff --git a/drivers/net/wireless/prism54/islpci_dev.c b/drivers/net/wireless/prism54/islpci_dev.c
index 084795355b74..219dd651dc41 100644
--- a/drivers/net/wireless/prism54/islpci_dev.c
+++ b/drivers/net/wireless/prism54/islpci_dev.c
@@ -808,7 +808,6 @@ islpci_setup(struct pci_dev *pdev)
 	if (!ndev)
 		return ndev;
 
-	SET_MODULE_OWNER(ndev);
 	pci_set_drvdata(pdev, ndev);
 #if defined(SET_NETDEV_DEV)
 	SET_NETDEV_DEV(ndev, &pdev->dev);
diff --git a/drivers/net/wireless/prism54/oid_mgt.c b/drivers/net/wireless/prism54/oid_mgt.c
index 42780320cd5c..57a4ac34bed6 100644
--- a/drivers/net/wireless/prism54/oid_mgt.c
+++ b/drivers/net/wireless/prism54/oid_mgt.c
@@ -244,13 +244,11 @@ mgt_init(islpci_private *priv)
 	/* Alloc the cache */
 	for (i = 0; i < OID_NUM_LAST; i++) {
 		if (isl_oid[i].flags & OID_FLAG_CACHED) {
-			priv->mib[i] = kmalloc(isl_oid[i].size *
+			priv->mib[i] = kzalloc(isl_oid[i].size *
 					       (isl_oid[i].range + 1),
 					       GFP_KERNEL);
 			if (!priv->mib[i])
 				return -ENOMEM;
-			memset(priv->mib[i], 0,
-			       isl_oid[i].size * (isl_oid[i].range + 1));
 		} else
 			priv->mib[i] = NULL;
 	}
diff --git a/drivers/net/wireless/ray_cs.c b/drivers/net/wireless/ray_cs.c
index 3be624295a1f..f87fe10059ae 100644
--- a/drivers/net/wireless/ray_cs.c
+++ b/drivers/net/wireless/ray_cs.c
@@ -314,7 +314,7 @@ static int ray_probe(struct pcmcia_device *p_dev)
     if (!dev)
 	    goto fail_alloc_dev;
 
-    local = dev->priv;
+    local = netdev_priv(dev);
     local->finder = p_dev;
 
     /* The io structure describes IO port mapping. None used here */
@@ -356,7 +356,6 @@ static int ray_probe(struct pcmcia_device *p_dev)
     dev->set_multicast_list = &set_multicast_list;
 
     DEBUG(2,"ray_cs ray_attach calling ether_setup.)\n");
-    SET_MODULE_OWNER(dev);
     dev->init = &ray_dev_init;
     dev->open = &ray_open;
     dev->stop = &ray_dev_close;
@@ -388,7 +387,7 @@ static void ray_detach(struct pcmcia_device *link)
 
     ray_release(link);
 
-    local = (ray_dev_t *)dev->priv;
+    local = netdev_priv(dev);
     del_timer(&local->timer);
 
     if (link->priv) {
@@ -412,7 +411,8 @@ static int ray_config(struct pcmcia_device *link)
     win_req_t req;
     memreq_t mem;
     struct net_device *dev = (struct net_device *)link->priv;
-    ray_dev_t *local = (ray_dev_t *)dev->priv;
+    ray_dev_t *local = netdev_priv(dev);
+    DECLARE_MAC_BUF(mac);
 
     DEBUG(1, "ray_config(0x%p)\n", link);
 
@@ -483,10 +483,8 @@ static int ray_config(struct pcmcia_device *link)
     strcpy(local->node.dev_name, dev->name);
     link->dev_node = &local->node;
 
-    printk(KERN_INFO "%s: RayLink, irq %d, hw_addr ",
-       dev->name, dev->irq);
-    for (i = 0; i < 6; i++)
-    printk("%02X%s", dev->dev_addr[i], ((i<5) ? ":" : "\n"));
+    printk(KERN_INFO "%s: RayLink, irq %d, hw_addr %s\n",
+       dev->name, dev->irq, print_mac(mac, dev->dev_addr));
 
     return 0;
 
@@ -520,7 +518,7 @@ static int ray_init(struct net_device *dev)
     int i;
     UCHAR *p;
     struct ccs __iomem *pccs;
-    ray_dev_t *local = (ray_dev_t *)dev->priv;
+    ray_dev_t *local = netdev_priv(dev);
     struct pcmcia_device *link = local->finder;
     DEBUG(1, "ray_init(0x%p)\n", dev);
     if (!(pcmcia_dev_present(link))) {
@@ -581,7 +579,7 @@ static int ray_init(struct net_device *dev)
 static int dl_startup_params(struct net_device *dev)
 {
     int ccsindex;
-    ray_dev_t *local = (ray_dev_t *)dev->priv;
+    ray_dev_t *local = netdev_priv(dev);
     struct ccs __iomem *pccs;
     struct pcmcia_device *link = local->finder;
 
@@ -786,7 +784,7 @@ static void join_net(u_long data)
 static void ray_release(struct pcmcia_device *link)
 {
     struct net_device *dev = link->priv; 
-    ray_dev_t *local = dev->priv;
+    ray_dev_t *local = netdev_priv(dev);
     int i;
     
     DEBUG(1, "ray_release(0x%p)\n", link);
@@ -834,7 +832,7 @@ int ray_dev_init(struct net_device *dev)
 #ifdef RAY_IMMEDIATE_INIT
     int i;
 #endif	/* RAY_IMMEDIATE_INIT */
-    ray_dev_t *local = dev->priv;
+    ray_dev_t *local = netdev_priv(dev);
     struct pcmcia_device *link = local->finder;
 
     DEBUG(1,"ray_dev_init(dev=%p)\n",dev);
@@ -868,7 +866,7 @@ int ray_dev_init(struct net_device *dev)
 /*===========================================================================*/
 static int ray_dev_config(struct net_device *dev, struct ifmap *map)
 {
-    ray_dev_t *local = dev->priv;
+    ray_dev_t *local = netdev_priv(dev);
     struct pcmcia_device *link = local->finder;
     /* Dummy routine to satisfy device structure */
     DEBUG(1,"ray_dev_config(dev=%p,ifmap=%p)\n",dev,map);
@@ -882,7 +880,7 @@ static int ray_dev_config(struct net_device *dev, struct ifmap *map)
 /*===========================================================================*/
 static int ray_dev_start_xmit(struct sk_buff *skb, struct net_device *dev)
 {
-    ray_dev_t *local = dev->priv;
+    ray_dev_t *local = netdev_priv(dev);
     struct pcmcia_device *link = local->finder;
     short length = skb->len;
 
@@ -925,7 +923,7 @@ static int ray_dev_start_xmit(struct sk_buff *skb, struct net_device *dev)
 static int ray_hw_xmit(unsigned char* data, int len, struct net_device* dev, 
                 UCHAR msg_type)
 {
-    ray_dev_t *local = (ray_dev_t *)dev->priv;
+    ray_dev_t *local = netdev_priv(dev);
     struct ccs __iomem *pccs;
     int ccsindex;
     int offset;
@@ -1099,7 +1097,7 @@ static int ray_set_freq(struct net_device *dev,
 			struct iw_freq *fwrq,
 			char *extra)
 {
-	ray_dev_t *local = (ray_dev_t *)dev->priv;
+	ray_dev_t *local = netdev_priv(dev);
 	int err = -EINPROGRESS;		/* Call commit handler */
 
 	/* Reject if card is already initialised */
@@ -1124,7 +1122,7 @@ static int ray_get_freq(struct net_device *dev,
 			struct iw_freq *fwrq,
 			char *extra)
 {
-	ray_dev_t *local = (ray_dev_t *)dev->priv;
+	ray_dev_t *local = netdev_priv(dev);
 
 	fwrq->m = local->sparm.b5.a_hop_pattern;
 	fwrq->e = 0;
@@ -1140,7 +1138,7 @@ static int ray_set_essid(struct net_device *dev,
 			 struct iw_point *dwrq,
 			 char *extra)
 {
-	ray_dev_t *local = (ray_dev_t *)dev->priv;
+	ray_dev_t *local = netdev_priv(dev);
 
 	/* Reject if card is already initialised */
 	if(local->card_status != CARD_AWAITING_PARAM)
@@ -1173,7 +1171,7 @@ static int ray_get_essid(struct net_device *dev,
 			 struct iw_point *dwrq,
 			 char *extra)
 {
-	ray_dev_t *local = (ray_dev_t *)dev->priv;
+	ray_dev_t *local = netdev_priv(dev);
 
 	/* Get the essid that was set */
 	memcpy(extra, local->sparm.b5.a_current_ess_id, IW_ESSID_MAX_SIZE);
@@ -1194,7 +1192,7 @@ static int ray_get_wap(struct net_device *dev,
 			struct sockaddr *awrq,
 			char *extra)
 {
-	ray_dev_t *local = (ray_dev_t *)dev->priv;
+	ray_dev_t *local = netdev_priv(dev);
 
 	memcpy(awrq->sa_data, local->bss_id, ETH_ALEN);
 	awrq->sa_family = ARPHRD_ETHER;
@@ -1211,7 +1209,7 @@ static int ray_set_rate(struct net_device *dev,
 			struct iw_param *vwrq,
 			char *extra)
 {
-	ray_dev_t *local = (ray_dev_t *)dev->priv;
+	ray_dev_t *local = netdev_priv(dev);
 
 	/* Reject if card is already initialised */
 	if(local->card_status != CARD_AWAITING_PARAM)
@@ -1240,7 +1238,7 @@ static int ray_get_rate(struct net_device *dev,
 			struct iw_param *vwrq,
 			char *extra)
 {
-	ray_dev_t *local = (ray_dev_t *)dev->priv;
+	ray_dev_t *local = netdev_priv(dev);
 
 	if(local->net_default_tx_rate == 3)
 		vwrq->value = 2000000;		/* Hum... */
@@ -1260,7 +1258,7 @@ static int ray_set_rts(struct net_device *dev,
 		       struct iw_param *vwrq,
 		       char *extra)
 {
-	ray_dev_t *local = (ray_dev_t *)dev->priv;
+	ray_dev_t *local = netdev_priv(dev);
 	int rthr = vwrq->value;
 
 	/* Reject if card is already initialised */
@@ -1290,7 +1288,7 @@ static int ray_get_rts(struct net_device *dev,
 		       struct iw_param *vwrq,
 		       char *extra)
 {
-	ray_dev_t *local = (ray_dev_t *)dev->priv;
+	ray_dev_t *local = netdev_priv(dev);
 
 	vwrq->value = (local->sparm.b5.a_rts_threshold[0] << 8)
 		+ local->sparm.b5.a_rts_threshold[1];
@@ -1309,7 +1307,7 @@ static int ray_set_frag(struct net_device *dev,
 			struct iw_param *vwrq,
 			char *extra)
 {
-	ray_dev_t *local = (ray_dev_t *)dev->priv;
+	ray_dev_t *local = netdev_priv(dev);
 	int fthr = vwrq->value;
 
 	/* Reject if card is already initialised */
@@ -1338,7 +1336,7 @@ static int ray_get_frag(struct net_device *dev,
 			struct iw_param *vwrq,
 			char *extra)
 {
-	ray_dev_t *local = (ray_dev_t *)dev->priv;
+	ray_dev_t *local = netdev_priv(dev);
 
 	vwrq->value = (local->sparm.b5.a_frag_threshold[0] << 8)
 		+ local->sparm.b5.a_frag_threshold[1];
@@ -1357,7 +1355,7 @@ static int ray_set_mode(struct net_device *dev,
 			__u32 *uwrq,
 			char *extra)
 {
-	ray_dev_t *local = (ray_dev_t *)dev->priv;
+	ray_dev_t *local = netdev_priv(dev);
 	int err = -EINPROGRESS;		/* Call commit handler */
 	char card_mode = 1;
 
@@ -1389,7 +1387,7 @@ static int ray_get_mode(struct net_device *dev,
 			__u32 *uwrq,
 			char *extra)
 {
-	ray_dev_t *local = (ray_dev_t *)dev->priv;
+	ray_dev_t *local = netdev_priv(dev);
 
 	if(local->sparm.b5.a_network_type)
 		*uwrq = IW_MODE_INFRA;
@@ -1492,7 +1490,7 @@ static int ray_commit(struct net_device *dev,
  */
 static iw_stats * ray_get_wireless_stats(struct net_device *	dev)
 {
-  ray_dev_t *	local = (ray_dev_t *) dev->priv;
+  ray_dev_t *	local = netdev_priv(dev);
   struct pcmcia_device *link = local->finder;
   struct status __iomem *p = local->sram + STATUS_BASE;
 
@@ -1568,9 +1566,9 @@ static const struct iw_priv_args	ray_private_args[] = {
 
 static const struct iw_handler_def	ray_handler_def =
 {
-	.num_standard	= sizeof(ray_handler)/sizeof(iw_handler),
-	.num_private	= sizeof(ray_private_handler)/sizeof(iw_handler),
-	.num_private_args = sizeof(ray_private_args)/sizeof(struct iw_priv_args),
+	.num_standard	= ARRAY_SIZE(ray_handler),
+	.num_private	= ARRAY_SIZE(ray_private_handler),
+	.num_private_args = ARRAY_SIZE(ray_private_args),
 	.standard	= ray_handler,
 	.private	= ray_private_handler,
 	.private_args	= ray_private_args,
@@ -1580,7 +1578,7 @@ static const struct iw_handler_def	ray_handler_def =
 /*===========================================================================*/
 static int ray_open(struct net_device *dev)
 {
-    ray_dev_t *local = (ray_dev_t *)dev->priv;
+    ray_dev_t *local = netdev_priv(dev);
     struct pcmcia_device *link;
     link = local->finder;
     
@@ -1614,7 +1612,7 @@ static int ray_open(struct net_device *dev)
 /*===========================================================================*/
 static int ray_dev_close(struct net_device *dev)
 {
-    ray_dev_t *local = (ray_dev_t *)dev->priv;
+    ray_dev_t *local = netdev_priv(dev);
     struct pcmcia_device *link;
     link = local->finder;
 
@@ -1773,7 +1771,7 @@ static int parse_addr(char *in_str, UCHAR *out)
 /*===========================================================================*/
 static struct net_device_stats *ray_get_stats(struct net_device *dev)
 {
-    ray_dev_t *local = (ray_dev_t *)dev->priv;
+    ray_dev_t *local = netdev_priv(dev);
     struct pcmcia_device *link = local->finder;
     struct status __iomem *p = local->sram + STATUS_BASE;
     if (!(pcmcia_dev_present(link))) {
@@ -1803,7 +1801,7 @@ static struct net_device_stats *ray_get_stats(struct net_device *dev)
 /*===========================================================================*/
 static void ray_update_parm(struct net_device *dev, UCHAR objid, UCHAR *value, int len)
 {
-    ray_dev_t *local = (ray_dev_t *)dev->priv;
+    ray_dev_t *local = netdev_priv(dev);
     struct pcmcia_device *link = local->finder;
     int ccsindex;
     int i;
@@ -1840,7 +1838,7 @@ static void ray_update_multi_list(struct net_device *dev, int all)
     int ccsindex;
     struct ccs __iomem *pccs;
     int i = 0;
-    ray_dev_t *local = (ray_dev_t *)dev->priv;
+    ray_dev_t *local = netdev_priv(dev);
     struct pcmcia_device *link = local->finder;
     void __iomem *p = local->sram + HOST_TO_ECF_BASE;
 
@@ -1884,7 +1882,7 @@ static void ray_update_multi_list(struct net_device *dev, int all)
 /*===========================================================================*/
 static void set_multicast_list(struct net_device *dev)
 {
-    ray_dev_t *local = (ray_dev_t *)dev->priv;
+    ray_dev_t *local = netdev_priv(dev);
     UCHAR promisc;
 
     DEBUG(2,"ray_cs set_multicast_list(%p)\n",dev);
@@ -1935,7 +1933,7 @@ static irqreturn_t ray_interrupt(int irq, void *dev_id)
 
     DEBUG(4,"ray_cs: interrupt for *dev=%p\n",dev);
 
-    local = (ray_dev_t *)dev->priv;
+    local = netdev_priv(dev);
     link = (struct pcmcia_device *)local->finder;
     if (!pcmcia_dev_present(link)) {
         DEBUG(2,"ray_cs interrupt from device not present or suspended.\n");
@@ -2165,7 +2163,7 @@ static void rx_data(struct net_device *dev, struct rcs __iomem *prcs, unsigned i
 {
     struct sk_buff *skb = NULL;
     struct rcs __iomem *prcslink = prcs;
-    ray_dev_t *local = dev->priv;
+    ray_dev_t *local = netdev_priv(dev);
     UCHAR *rx_ptr;
     int total_len;
     int tmp;
@@ -2611,6 +2609,7 @@ static int ray_cs_proc_read(char *buf, char **start, off_t offset, int len)
     UCHAR *p;
     struct freq_hop_element *pfh;
     UCHAR c[33];
+    DECLARE_MAC_BUF(mac);
 
     link = this_device;
     if (!link)
@@ -2618,7 +2617,7 @@ static int ray_cs_proc_read(char *buf, char **start, off_t offset, int len)
     dev = (struct net_device *)link->priv;
     if (!dev)
     	return 0;
-    local = (ray_dev_t *)dev->priv;
+    local = netdev_priv(dev);
     if (!local)
     	return 0;
 
@@ -2640,9 +2639,8 @@ static int ray_cs_proc_read(char *buf, char **start, off_t offset, int len)
                    nettype[local->sparm.b5.a_network_type], c);
 
     p = local->bss_id;
-    len += sprintf(buf + len, 
-                   "BSSID                = %02x:%02x:%02x:%02x:%02x:%02x\n",
-                   p[0],p[1],p[2],p[3],p[4],p[5]);
+    len += sprintf(buf + len, "BSSID                = %s\n",
+                   print_mac(mac, p));
 
     len += sprintf(buf + len, "Country code         = %d\n", 
                    local->sparm.b5.a_curr_country_code);
diff --git a/drivers/net/wireless/ray_cs.h b/drivers/net/wireless/ray_cs.h
index bd73ebf03340..1e23b7f4cca7 100644
--- a/drivers/net/wireless/ray_cs.h
+++ b/drivers/net/wireless/ray_cs.h
@@ -33,8 +33,8 @@ typedef struct ray_dev_t {
     void __iomem *rmem;            /* pointer to receive buffer window       */
     struct pcmcia_device *finder;            /* pointer back to struct pcmcia_device for card    */
     struct timer_list timer;
-    long tx_ccs_lock;
-    long ccs_lock;
+    unsigned long tx_ccs_lock;
+    unsigned long ccs_lock;
     int   dl_param_ccs;
     union {
         struct b4_startup_params b4;
diff --git a/drivers/net/wireless/rt2x00/Kconfig b/drivers/net/wireless/rt2x00/Kconfig
new file mode 100644
index 000000000000..da05b1faf60d
--- /dev/null
+++ b/drivers/net/wireless/rt2x00/Kconfig
@@ -0,0 +1,130 @@
+config RT2X00
+	tristate "Ralink driver support"
+	depends on MAC80211 && WLAN_80211 && EXPERIMENTAL
+	---help---
+	  This will enable the experimental support for the Ralink drivers,
+	  developed in the rt2x00 project <http://rt2x00.serialmonkey.com>.
+
+	  These drivers will make use of the Devicescape ieee80211 stack.
+
+	  When building one of the individual drivers, the rt2x00 library
+	  will also be created. That library (when the driver is built as
+	  a module) will be called "rt2x00lib.ko".
+
+config RT2X00_LIB
+	tristate
+	depends on RT2X00
+
+config RT2X00_LIB_PCI
+	tristate
+	depends on RT2X00
+	select RT2X00_LIB
+
+config RT2X00_LIB_USB
+	tristate
+	depends on RT2X00
+	select RT2X00_LIB
+
+config RT2X00_LIB_FIRMWARE
+	boolean
+	depends on RT2X00_LIB
+	select CRC_ITU_T
+	select FW_LOADER
+
+config RT2X00_LIB_RFKILL
+	boolean
+	depends on RT2X00_LIB
+	select RFKILL
+	select INPUT_POLLDEV
+
+config RT2400PCI
+	tristate "Ralink rt2400 pci/pcmcia support"
+	depends on RT2X00 && PCI
+	select RT2X00_LIB_PCI
+	select EEPROM_93CX6
+	---help---
+	  This is an experimental driver for the Ralink rt2400 wireless chip.
+
+	  When compiled as a module, this driver will be called "rt2400pci.ko".
+
+config RT2400PCI_RFKILL
+	bool "RT2400 rfkill support"
+	depends on RT2400PCI
+	select RT2X00_LIB_RFKILL
+	---help---
+	  This adds support for integrated rt2400 devices that feature a
+	  hardware button to control the radio state.
+	  This feature depends on the RF switch subsystem rfkill.
+
+config RT2500PCI
+	tristate "Ralink rt2500 pci/pcmcia support"
+	depends on RT2X00 && PCI
+	select RT2X00_LIB_PCI
+	select EEPROM_93CX6
+	---help---
+	  This is an experimental driver for the Ralink rt2500 wireless chip.
+
+	  When compiled as a module, this driver will be called "rt2500pci.ko".
+
+config RT2500PCI_RFKILL
+	bool "RT2500 rfkill support"
+	depends on RT2500PCI
+	select RT2X00_LIB_RFKILL
+	---help---
+	  This adds support for integrated rt2500 devices that feature a
+	  hardware button to control the radio state.
+	  This feature depends on the RF switch subsystem rfkill.
+
+config RT61PCI
+	tristate "Ralink rt61 pci/pcmcia support"
+	depends on RT2X00 && PCI
+	select RT2X00_LIB_PCI
+	select RT2X00_LIB_FIRMWARE
+	select EEPROM_93CX6
+	---help---
+	  This is an experimental driver for the Ralink rt61 wireless chip.
+
+	  When compiled as a module, this driver will be called "rt61pci.ko".
+
+config RT61PCI_RFKILL
+	bool "RT61 rfkill support"
+	depends on RT61PCI
+	select RT2X00_LIB_RFKILL
+	---help---
+	  This adds support for integrated rt61 devices that feature a
+	  hardware button to control the radio state.
+	  This feature depends on the RF switch subsystem rfkill.
+
+config RT2500USB
+	tristate "Ralink rt2500 usb support"
+	depends on RT2X00 && USB
+	select RT2X00_LIB_USB
+	---help---
+	  This is an experimental driver for the Ralink rt2500 wireless chip.
+
+	  When compiled as a module, this driver will be called "rt2500usb.ko".
+
+config RT73USB
+	tristate "Ralink rt73 usb support"
+	depends on RT2X00 && USB
+	select RT2X00_LIB_USB
+	select RT2X00_LIB_FIRMWARE
+	---help---
+	  This is an experimental driver for the Ralink rt73 wireless chip.
+
+	  When compiled as a module, this driver will be called "rt73usb.ko".
+
+config RT2X00_LIB_DEBUGFS
+	bool "Ralink debugfs support"
+	depends on RT2X00_LIB && MAC80211_DEBUGFS
+	---help---
+	  Enable creation of debugfs files for the rt2x00 drivers.
+	  These debugfs files support both reading and writing of the
+	  most important register types of the rt2x00 devices.
+
+config RT2X00_DEBUG
+	bool "Ralink debug output"
+	depends on RT2X00_LIB
+	---help---
+	  Enable debugging output for all rt2x00 modules
+
diff --git a/drivers/net/wireless/rt2x00/Makefile b/drivers/net/wireless/rt2x00/Makefile
new file mode 100644
index 000000000000..30d654a42eea
--- /dev/null
+++ b/drivers/net/wireless/rt2x00/Makefile
@@ -0,0 +1,22 @@
+rt2x00lib-objs := rt2x00dev.o rt2x00mac.o rt2x00config.o
+
+ifeq ($(CONFIG_RT2X00_LIB_DEBUGFS),y)
+	rt2x00lib-objs += rt2x00debug.o
+endif
+
+ifeq ($(CONFIG_RT2X00_LIB_RFKILL),y)
+	rt2x00lib-objs += rt2x00rfkill.o
+endif
+
+ifeq ($(CONFIG_RT2X00_LIB_FIRMWARE),y)
+	rt2x00lib-objs += rt2x00firmware.o
+endif
+
+obj-$(CONFIG_RT2X00_LIB)	+= rt2x00lib.o
+obj-$(CONFIG_RT2X00_LIB_PCI)	+= rt2x00pci.o
+obj-$(CONFIG_RT2X00_LIB_USB)	+= rt2x00usb.o
+obj-$(CONFIG_RT2400PCI)		+= rt2400pci.o
+obj-$(CONFIG_RT2500PCI)		+= rt2500pci.o
+obj-$(CONFIG_RT61PCI)		+= rt61pci.o
+obj-$(CONFIG_RT2500USB)		+= rt2500usb.o
+obj-$(CONFIG_RT73USB)		+= rt73usb.o
diff --git a/drivers/net/wireless/rt2x00/rt2400pci.c b/drivers/net/wireless/rt2x00/rt2400pci.c
new file mode 100644
index 000000000000..31c1dd271627
--- /dev/null
+++ b/drivers/net/wireless/rt2x00/rt2400pci.c
@@ -0,0 +1,1664 @@
+/*
+	Copyright (C) 2004 - 2007 rt2x00 SourceForge Project
+	<http://rt2x00.serialmonkey.com>
+
+	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, or
+	(at your option) any later version.
+
+	This program is distributed in the hope that it will be useful,
+	but WITHOUT ANY WARRANTY; without even the implied warranty of
+	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+	GNU General Public License for more details.
+
+	You should have received a copy of the GNU General Public License
+	along with this program; if not, write to the
+	Free Software Foundation, Inc.,
+	59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+/*
+	Module: rt2400pci
+	Abstract: rt2400pci device specific routines.
+	Supported chipsets: RT2460.
+ */
+
+/*
+ * Set enviroment defines for rt2x00.h
+ */
+#define DRV_NAME "rt2400pci"
+
+#include <linux/delay.h>
+#include <linux/etherdevice.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/pci.h>
+#include <linux/eeprom_93cx6.h>
+
+#include "rt2x00.h"
+#include "rt2x00pci.h"
+#include "rt2400pci.h"
+
+/*
+ * Register access.
+ * All access to the CSR registers will go through the methods
+ * rt2x00pci_register_read and rt2x00pci_register_write.
+ * BBP and RF register require indirect register access,
+ * and use the CSR registers BBPCSR and RFCSR to achieve this.
+ * These indirect registers work with busy bits,
+ * and we will try maximal REGISTER_BUSY_COUNT times to access
+ * the register while taking a REGISTER_BUSY_DELAY us delay
+ * between each attampt. When the busy bit is still set at that time,
+ * the access attempt is considered to have failed,
+ * and we will print an error.
+ */
+static u32 rt2400pci_bbp_check(const struct rt2x00_dev *rt2x00dev)
+{
+	u32 reg;
+	unsigned int i;
+
+	for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
+		rt2x00pci_register_read(rt2x00dev, BBPCSR, &reg);
+		if (!rt2x00_get_field32(reg, BBPCSR_BUSY))
+			break;
+		udelay(REGISTER_BUSY_DELAY);
+	}
+
+	return reg;
+}
+
+static void rt2400pci_bbp_write(const struct rt2x00_dev *rt2x00dev,
+				const unsigned int word, const u8 value)
+{
+	u32 reg;
+
+	/*
+	 * Wait until the BBP becomes ready.
+	 */
+	reg = rt2400pci_bbp_check(rt2x00dev);
+	if (rt2x00_get_field32(reg, BBPCSR_BUSY)) {
+		ERROR(rt2x00dev, "BBPCSR register busy. Write failed.\n");
+		return;
+	}
+
+	/*
+	 * Write the data into the BBP.
+	 */
+	reg = 0;
+	rt2x00_set_field32(&reg, BBPCSR_VALUE, value);
+	rt2x00_set_field32(&reg, BBPCSR_REGNUM, word);
+	rt2x00_set_field32(&reg, BBPCSR_BUSY, 1);
+	rt2x00_set_field32(&reg, BBPCSR_WRITE_CONTROL, 1);
+
+	rt2x00pci_register_write(rt2x00dev, BBPCSR, reg);
+}
+
+static void rt2400pci_bbp_read(const struct rt2x00_dev *rt2x00dev,
+			       const unsigned int word, u8 *value)
+{
+	u32 reg;
+
+	/*
+	 * Wait until the BBP becomes ready.
+	 */
+	reg = rt2400pci_bbp_check(rt2x00dev);
+	if (rt2x00_get_field32(reg, BBPCSR_BUSY)) {
+		ERROR(rt2x00dev, "BBPCSR register busy. Read failed.\n");
+		return;
+	}
+
+	/*
+	 * Write the request into the BBP.
+	 */
+	reg = 0;
+	rt2x00_set_field32(&reg, BBPCSR_REGNUM, word);
+	rt2x00_set_field32(&reg, BBPCSR_BUSY, 1);
+	rt2x00_set_field32(&reg, BBPCSR_WRITE_CONTROL, 0);
+
+	rt2x00pci_register_write(rt2x00dev, BBPCSR, reg);
+
+	/*
+	 * Wait until the BBP becomes ready.
+	 */
+	reg = rt2400pci_bbp_check(rt2x00dev);
+	if (rt2x00_get_field32(reg, BBPCSR_BUSY)) {
+		ERROR(rt2x00dev, "BBPCSR register busy. Read failed.\n");
+		*value = 0xff;
+		return;
+	}
+
+	*value = rt2x00_get_field32(reg, BBPCSR_VALUE);
+}
+
+static void rt2400pci_rf_write(const struct rt2x00_dev *rt2x00dev,
+			       const unsigned int word, const u32 value)
+{
+	u32 reg;
+	unsigned int i;
+
+	if (!word)
+		return;
+
+	for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
+		rt2x00pci_register_read(rt2x00dev, RFCSR, &reg);
+		if (!rt2x00_get_field32(reg, RFCSR_BUSY))
+			goto rf_write;
+		udelay(REGISTER_BUSY_DELAY);
+	}
+
+	ERROR(rt2x00dev, "RFCSR register busy. Write failed.\n");
+	return;
+
+rf_write:
+	reg = 0;
+	rt2x00_set_field32(&reg, RFCSR_VALUE, value);
+	rt2x00_set_field32(&reg, RFCSR_NUMBER_OF_BITS, 20);
+	rt2x00_set_field32(&reg, RFCSR_IF_SELECT, 0);
+	rt2x00_set_field32(&reg, RFCSR_BUSY, 1);
+
+	rt2x00pci_register_write(rt2x00dev, RFCSR, reg);
+	rt2x00_rf_write(rt2x00dev, word, value);
+}
+
+static void rt2400pci_eepromregister_read(struct eeprom_93cx6 *eeprom)
+{
+	struct rt2x00_dev *rt2x00dev = eeprom->data;
+	u32 reg;
+
+	rt2x00pci_register_read(rt2x00dev, CSR21, &reg);
+
+	eeprom->reg_data_in = !!rt2x00_get_field32(reg, CSR21_EEPROM_DATA_IN);
+	eeprom->reg_data_out = !!rt2x00_get_field32(reg, CSR21_EEPROM_DATA_OUT);
+	eeprom->reg_data_clock =
+	    !!rt2x00_get_field32(reg, CSR21_EEPROM_DATA_CLOCK);
+	eeprom->reg_chip_select =
+	    !!rt2x00_get_field32(reg, CSR21_EEPROM_CHIP_SELECT);
+}
+
+static void rt2400pci_eepromregister_write(struct eeprom_93cx6 *eeprom)
+{
+	struct rt2x00_dev *rt2x00dev = eeprom->data;
+	u32 reg = 0;
+
+	rt2x00_set_field32(&reg, CSR21_EEPROM_DATA_IN, !!eeprom->reg_data_in);
+	rt2x00_set_field32(&reg, CSR21_EEPROM_DATA_OUT, !!eeprom->reg_data_out);
+	rt2x00_set_field32(&reg, CSR21_EEPROM_DATA_CLOCK,
+			   !!eeprom->reg_data_clock);
+	rt2x00_set_field32(&reg, CSR21_EEPROM_CHIP_SELECT,
+			   !!eeprom->reg_chip_select);
+
+	rt2x00pci_register_write(rt2x00dev, CSR21, reg);
+}
+
+#ifdef CONFIG_RT2X00_LIB_DEBUGFS
+#define CSR_OFFSET(__word)	( CSR_REG_BASE + ((__word) * sizeof(u32)) )
+
+static void rt2400pci_read_csr(const struct rt2x00_dev *rt2x00dev,
+			       const unsigned int word, u32 *data)
+{
+	rt2x00pci_register_read(rt2x00dev, CSR_OFFSET(word), data);
+}
+
+static void rt2400pci_write_csr(const struct rt2x00_dev *rt2x00dev,
+				const unsigned int word, u32 data)
+{
+	rt2x00pci_register_write(rt2x00dev, CSR_OFFSET(word), data);
+}
+
+static const struct rt2x00debug rt2400pci_rt2x00debug = {
+	.owner	= THIS_MODULE,
+	.csr	= {
+		.read		= rt2400pci_read_csr,
+		.write		= rt2400pci_write_csr,
+		.word_size	= sizeof(u32),
+		.word_count	= CSR_REG_SIZE / sizeof(u32),
+	},
+	.eeprom	= {
+		.read		= rt2x00_eeprom_read,
+		.write		= rt2x00_eeprom_write,
+		.word_size	= sizeof(u16),
+		.word_count	= EEPROM_SIZE / sizeof(u16),
+	},
+	.bbp	= {
+		.read		= rt2400pci_bbp_read,
+		.write		= rt2400pci_bbp_write,
+		.word_size	= sizeof(u8),
+		.word_count	= BBP_SIZE / sizeof(u8),
+	},
+	.rf	= {
+		.read		= rt2x00_rf_read,
+		.write		= rt2400pci_rf_write,
+		.word_size	= sizeof(u32),
+		.word_count	= RF_SIZE / sizeof(u32),
+	},
+};
+#endif /* CONFIG_RT2X00_LIB_DEBUGFS */
+
+#ifdef CONFIG_RT2400PCI_RFKILL
+static int rt2400pci_rfkill_poll(struct rt2x00_dev *rt2x00dev)
+{
+	u32 reg;
+
+	rt2x00pci_register_read(rt2x00dev, GPIOCSR, &reg);
+	return rt2x00_get_field32(reg, GPIOCSR_BIT0);
+}
+#else
+#define rt2400pci_rfkill_poll	NULL
+#endif /* CONFIG_RT2400PCI_RFKILL */
+
+/*
+ * Configuration handlers.
+ */
+static void rt2400pci_config_mac_addr(struct rt2x00_dev *rt2x00dev,
+				      __le32 *mac)
+{
+	rt2x00pci_register_multiwrite(rt2x00dev, CSR3, mac,
+				      (2 * sizeof(__le32)));
+}
+
+static void rt2400pci_config_bssid(struct rt2x00_dev *rt2x00dev,
+				   __le32 *bssid)
+{
+	rt2x00pci_register_multiwrite(rt2x00dev, CSR5, bssid,
+				      (2 * sizeof(__le32)));
+}
+
+static void rt2400pci_config_type(struct rt2x00_dev *rt2x00dev, const int type,
+				  const int tsf_sync)
+{
+	u32 reg;
+
+	rt2x00pci_register_write(rt2x00dev, CSR14, 0);
+
+	/*
+	 * Enable beacon config
+	 */
+	rt2x00pci_register_read(rt2x00dev, BCNCSR1, &reg);
+	rt2x00_set_field32(&reg, BCNCSR1_PRELOAD,
+			   PREAMBLE + get_duration(IEEE80211_HEADER, 20));
+	rt2x00pci_register_write(rt2x00dev, BCNCSR1, reg);
+
+	/*
+	 * Enable synchronisation.
+	 */
+	rt2x00pci_register_read(rt2x00dev, CSR14, &reg);
+	rt2x00_set_field32(&reg, CSR14_TSF_COUNT, 1);
+	rt2x00_set_field32(&reg, CSR14_TBCN, 1);
+	rt2x00_set_field32(&reg, CSR14_BEACON_GEN, 0);
+	rt2x00_set_field32(&reg, CSR14_TSF_SYNC, tsf_sync);
+	rt2x00pci_register_write(rt2x00dev, CSR14, reg);
+}
+
+static void rt2400pci_config_preamble(struct rt2x00_dev *rt2x00dev,
+				      const int short_preamble,
+				      const int ack_timeout,
+				      const int ack_consume_time)
+{
+	int preamble_mask;
+	u32 reg;
+
+	/*
+	 * When short preamble is enabled, we should set bit 0x08
+	 */
+	preamble_mask = short_preamble << 3;
+
+	rt2x00pci_register_read(rt2x00dev, TXCSR1, &reg);
+	rt2x00_set_field32(&reg, TXCSR1_ACK_TIMEOUT, ack_timeout);
+	rt2x00_set_field32(&reg, TXCSR1_ACK_CONSUME_TIME, ack_consume_time);
+	rt2x00pci_register_write(rt2x00dev, TXCSR1, reg);
+
+	rt2x00pci_register_read(rt2x00dev, ARCSR2, &reg);
+	rt2x00_set_field32(&reg, ARCSR2_SIGNAL, 0x00 | preamble_mask);
+	rt2x00_set_field32(&reg, ARCSR2_SERVICE, 0x04);
+	rt2x00_set_field32(&reg, ARCSR2_LENGTH, get_duration(ACK_SIZE, 10));
+	rt2x00pci_register_write(rt2x00dev, ARCSR2, reg);
+
+	rt2x00pci_register_read(rt2x00dev, ARCSR3, &reg);
+	rt2x00_set_field32(&reg, ARCSR3_SIGNAL, 0x01 | preamble_mask);
+	rt2x00_set_field32(&reg, ARCSR3_SERVICE, 0x04);
+	rt2x00_set_field32(&reg, ARCSR2_LENGTH, get_duration(ACK_SIZE, 20));
+	rt2x00pci_register_write(rt2x00dev, ARCSR3, reg);
+
+	rt2x00pci_register_read(rt2x00dev, ARCSR4, &reg);
+	rt2x00_set_field32(&reg, ARCSR4_SIGNAL, 0x02 | preamble_mask);
+	rt2x00_set_field32(&reg, ARCSR4_SERVICE, 0x04);
+	rt2x00_set_field32(&reg, ARCSR2_LENGTH, get_duration(ACK_SIZE, 55));
+	rt2x00pci_register_write(rt2x00dev, ARCSR4, reg);
+
+	rt2x00pci_register_read(rt2x00dev, ARCSR5, &reg);
+	rt2x00_set_field32(&reg, ARCSR5_SIGNAL, 0x03 | preamble_mask);
+	rt2x00_set_field32(&reg, ARCSR5_SERVICE, 0x84);
+	rt2x00_set_field32(&reg, ARCSR2_LENGTH, get_duration(ACK_SIZE, 110));
+	rt2x00pci_register_write(rt2x00dev, ARCSR5, reg);
+}
+
+static void rt2400pci_config_phymode(struct rt2x00_dev *rt2x00dev,
+				     const int basic_rate_mask)
+{
+	rt2x00pci_register_write(rt2x00dev, ARCSR1, basic_rate_mask);
+}
+
+static void rt2400pci_config_channel(struct rt2x00_dev *rt2x00dev,
+				     struct rf_channel *rf)
+{
+	/*
+	 * Switch on tuning bits.
+	 */
+	rt2x00_set_field32(&rf->rf1, RF1_TUNER, 1);
+	rt2x00_set_field32(&rf->rf3, RF3_TUNER, 1);
+
+	rt2400pci_rf_write(rt2x00dev, 1, rf->rf1);
+	rt2400pci_rf_write(rt2x00dev, 2, rf->rf2);
+	rt2400pci_rf_write(rt2x00dev, 3, rf->rf3);
+
+	/*
+	 * RF2420 chipset don't need any additional actions.
+	 */
+	if (rt2x00_rf(&rt2x00dev->chip, RF2420))
+		return;
+
+	/*
+	 * For the RT2421 chipsets we need to write an invalid
+	 * reference clock rate to activate auto_tune.
+	 * After that we set the value back to the correct channel.
+	 */
+	rt2400pci_rf_write(rt2x00dev, 1, rf->rf1);
+	rt2400pci_rf_write(rt2x00dev, 2, 0x000c2a32);
+	rt2400pci_rf_write(rt2x00dev, 3, rf->rf3);
+
+	msleep(1);
+
+	rt2400pci_rf_write(rt2x00dev, 1, rf->rf1);
+	rt2400pci_rf_write(rt2x00dev, 2, rf->rf2);
+	rt2400pci_rf_write(rt2x00dev, 3, rf->rf3);
+
+	msleep(1);
+
+	/*
+	 * Switch off tuning bits.
+	 */
+	rt2x00_set_field32(&rf->rf1, RF1_TUNER, 0);
+	rt2x00_set_field32(&rf->rf3, RF3_TUNER, 0);
+
+	rt2400pci_rf_write(rt2x00dev, 1, rf->rf1);
+	rt2400pci_rf_write(rt2x00dev, 3, rf->rf3);
+
+	/*
+	 * Clear false CRC during channel switch.
+	 */
+	rt2x00pci_register_read(rt2x00dev, CNT0, &rf->rf1);
+}
+
+static void rt2400pci_config_txpower(struct rt2x00_dev *rt2x00dev, int txpower)
+{
+	rt2400pci_bbp_write(rt2x00dev, 3, TXPOWER_TO_DEV(txpower));
+}
+
+static void rt2400pci_config_antenna(struct rt2x00_dev *rt2x00dev,
+				     int antenna_tx, int antenna_rx)
+{
+	u8 r1;
+	u8 r4;
+
+	rt2400pci_bbp_read(rt2x00dev, 4, &r4);
+	rt2400pci_bbp_read(rt2x00dev, 1, &r1);
+
+	/*
+	 * Configure the TX antenna.
+	 */
+	switch (antenna_tx) {
+	case ANTENNA_SW_DIVERSITY:
+	case ANTENNA_HW_DIVERSITY:
+		rt2x00_set_field8(&r1, BBP_R1_TX_ANTENNA, 1);
+		break;
+	case ANTENNA_A:
+		rt2x00_set_field8(&r1, BBP_R1_TX_ANTENNA, 0);
+		break;
+	case ANTENNA_B:
+		rt2x00_set_field8(&r1, BBP_R1_TX_ANTENNA, 2);
+		break;
+	}
+
+	/*
+	 * Configure the RX antenna.
+	 */
+	switch (antenna_rx) {
+	case ANTENNA_SW_DIVERSITY:
+	case ANTENNA_HW_DIVERSITY:
+		rt2x00_set_field8(&r4, BBP_R4_RX_ANTENNA, 1);
+		break;
+	case ANTENNA_A:
+		rt2x00_set_field8(&r4, BBP_R4_RX_ANTENNA, 0);
+		break;
+	case ANTENNA_B:
+		rt2x00_set_field8(&r4, BBP_R4_RX_ANTENNA, 2);
+		break;
+	}
+
+	rt2400pci_bbp_write(rt2x00dev, 4, r4);
+	rt2400pci_bbp_write(rt2x00dev, 1, r1);
+}
+
+static void rt2400pci_config_duration(struct rt2x00_dev *rt2x00dev,
+				      struct rt2x00lib_conf *libconf)
+{
+	u32 reg;
+
+	rt2x00pci_register_read(rt2x00dev, CSR11, &reg);
+	rt2x00_set_field32(&reg, CSR11_SLOT_TIME, libconf->slot_time);
+	rt2x00pci_register_write(rt2x00dev, CSR11, reg);
+
+	rt2x00pci_register_read(rt2x00dev, CSR18, &reg);
+	rt2x00_set_field32(&reg, CSR18_SIFS, libconf->sifs);
+	rt2x00_set_field32(&reg, CSR18_PIFS, libconf->pifs);
+	rt2x00pci_register_write(rt2x00dev, CSR18, reg);
+
+	rt2x00pci_register_read(rt2x00dev, CSR19, &reg);
+	rt2x00_set_field32(&reg, CSR19_DIFS, libconf->difs);
+	rt2x00_set_field32(&reg, CSR19_EIFS, libconf->eifs);
+	rt2x00pci_register_write(rt2x00dev, CSR19, reg);
+
+	rt2x00pci_register_read(rt2x00dev, TXCSR1, &reg);
+	rt2x00_set_field32(&reg, TXCSR1_TSF_OFFSET, IEEE80211_HEADER);
+	rt2x00_set_field32(&reg, TXCSR1_AUTORESPONDER, 1);
+	rt2x00pci_register_write(rt2x00dev, TXCSR1, reg);
+
+	rt2x00pci_register_read(rt2x00dev, CSR12, &reg);
+	rt2x00_set_field32(&reg, CSR12_BEACON_INTERVAL,
+			   libconf->conf->beacon_int * 16);
+	rt2x00_set_field32(&reg, CSR12_CFP_MAX_DURATION,
+			   libconf->conf->beacon_int * 16);
+	rt2x00pci_register_write(rt2x00dev, CSR12, reg);
+}
+
+static void rt2400pci_config(struct rt2x00_dev *rt2x00dev,
+			     const unsigned int flags,
+			     struct rt2x00lib_conf *libconf)
+{
+	if (flags & CONFIG_UPDATE_PHYMODE)
+		rt2400pci_config_phymode(rt2x00dev, libconf->basic_rates);
+	if (flags & CONFIG_UPDATE_CHANNEL)
+		rt2400pci_config_channel(rt2x00dev, &libconf->rf);
+	if (flags & CONFIG_UPDATE_TXPOWER)
+		rt2400pci_config_txpower(rt2x00dev,
+					 libconf->conf->power_level);
+	if (flags & CONFIG_UPDATE_ANTENNA)
+		rt2400pci_config_antenna(rt2x00dev,
+					 libconf->conf->antenna_sel_tx,
+					 libconf->conf->antenna_sel_rx);
+	if (flags & (CONFIG_UPDATE_SLOT_TIME | CONFIG_UPDATE_BEACON_INT))
+		rt2400pci_config_duration(rt2x00dev, libconf);
+}
+
+static void rt2400pci_config_cw(struct rt2x00_dev *rt2x00dev,
+				struct ieee80211_tx_queue_params *params)
+{
+	u32 reg;
+
+	rt2x00pci_register_read(rt2x00dev, CSR11, &reg);
+	rt2x00_set_field32(&reg, CSR11_CWMIN, params->cw_min);
+	rt2x00_set_field32(&reg, CSR11_CWMAX, params->cw_max);
+	rt2x00pci_register_write(rt2x00dev, CSR11, reg);
+}
+
+/*
+ * LED functions.
+ */
+static void rt2400pci_enable_led(struct rt2x00_dev *rt2x00dev)
+{
+	u32 reg;
+
+	rt2x00pci_register_read(rt2x00dev, LEDCSR, &reg);
+
+	rt2x00_set_field32(&reg, LEDCSR_ON_PERIOD, 70);
+	rt2x00_set_field32(&reg, LEDCSR_OFF_PERIOD, 30);
+
+	if (rt2x00dev->led_mode == LED_MODE_TXRX_ACTIVITY) {
+		rt2x00_set_field32(&reg, LEDCSR_LINK, 1);
+		rt2x00_set_field32(&reg, LEDCSR_ACTIVITY, 0);
+	} else if (rt2x00dev->led_mode == LED_MODE_ASUS) {
+		rt2x00_set_field32(&reg, LEDCSR_LINK, 0);
+		rt2x00_set_field32(&reg, LEDCSR_ACTIVITY, 1);
+	} else {
+		rt2x00_set_field32(&reg, LEDCSR_LINK, 1);
+		rt2x00_set_field32(&reg, LEDCSR_ACTIVITY, 1);
+	}
+
+	rt2x00pci_register_write(rt2x00dev, LEDCSR, reg);
+}
+
+static void rt2400pci_disable_led(struct rt2x00_dev *rt2x00dev)
+{
+	u32 reg;
+
+	rt2x00pci_register_read(rt2x00dev, LEDCSR, &reg);
+	rt2x00_set_field32(&reg, LEDCSR_LINK, 0);
+	rt2x00_set_field32(&reg, LEDCSR_ACTIVITY, 0);
+	rt2x00pci_register_write(rt2x00dev, LEDCSR, reg);
+}
+
+/*
+ * Link tuning
+ */
+static void rt2400pci_link_stats(struct rt2x00_dev *rt2x00dev)
+{
+	u32 reg;
+	u8 bbp;
+
+	/*
+	 * Update FCS error count from register.
+	 */
+	rt2x00pci_register_read(rt2x00dev, CNT0, &reg);
+	rt2x00dev->link.rx_failed = rt2x00_get_field32(reg, CNT0_FCS_ERROR);
+
+	/*
+	 * Update False CCA count from register.
+	 */
+	rt2400pci_bbp_read(rt2x00dev, 39, &bbp);
+	rt2x00dev->link.false_cca = bbp;
+}
+
+static void rt2400pci_reset_tuner(struct rt2x00_dev *rt2x00dev)
+{
+	rt2400pci_bbp_write(rt2x00dev, 13, 0x08);
+	rt2x00dev->link.vgc_level = 0x08;
+}
+
+static void rt2400pci_link_tuner(struct rt2x00_dev *rt2x00dev)
+{
+	u8 reg;
+
+	/*
+	 * The link tuner should not run longer then 60 seconds,
+	 * and should run once every 2 seconds.
+	 */
+	if (rt2x00dev->link.count > 60 || !(rt2x00dev->link.count & 1))
+		return;
+
+	/*
+	 * Base r13 link tuning on the false cca count.
+	 */
+	rt2400pci_bbp_read(rt2x00dev, 13, &reg);
+
+	if (rt2x00dev->link.false_cca > 512 && reg < 0x20) {
+		rt2400pci_bbp_write(rt2x00dev, 13, ++reg);
+		rt2x00dev->link.vgc_level = reg;
+	} else if (rt2x00dev->link.false_cca < 100 && reg > 0x08) {
+		rt2400pci_bbp_write(rt2x00dev, 13, --reg);
+		rt2x00dev->link.vgc_level = reg;
+	}
+}
+
+/*
+ * Initialization functions.
+ */
+static void rt2400pci_init_rxring(struct rt2x00_dev *rt2x00dev)
+{
+	struct data_ring *ring = rt2x00dev->rx;
+	struct data_desc *rxd;
+	unsigned int i;
+	u32 word;
+
+	memset(ring->data_addr, 0x00, rt2x00_get_ring_size(ring));
+
+	for (i = 0; i < ring->stats.limit; i++) {
+		rxd = ring->entry[i].priv;
+
+		rt2x00_desc_read(rxd, 2, &word);
+		rt2x00_set_field32(&word, RXD_W2_BUFFER_LENGTH,
+				   ring->data_size);
+		rt2x00_desc_write(rxd, 2, word);
+
+		rt2x00_desc_read(rxd, 1, &word);
+		rt2x00_set_field32(&word, RXD_W1_BUFFER_ADDRESS,
+				   ring->entry[i].data_dma);
+		rt2x00_desc_write(rxd, 1, word);
+
+		rt2x00_desc_read(rxd, 0, &word);
+		rt2x00_set_field32(&word, RXD_W0_OWNER_NIC, 1);
+		rt2x00_desc_write(rxd, 0, word);
+	}
+
+	rt2x00_ring_index_clear(rt2x00dev->rx);
+}
+
+static void rt2400pci_init_txring(struct rt2x00_dev *rt2x00dev, const int queue)
+{
+	struct data_ring *ring = rt2x00lib_get_ring(rt2x00dev, queue);
+	struct data_desc *txd;
+	unsigned int i;
+	u32 word;
+
+	memset(ring->data_addr, 0x00, rt2x00_get_ring_size(ring));
+
+	for (i = 0; i < ring->stats.limit; i++) {
+		txd = ring->entry[i].priv;
+
+		rt2x00_desc_read(txd, 1, &word);
+		rt2x00_set_field32(&word, TXD_W1_BUFFER_ADDRESS,
+				   ring->entry[i].data_dma);
+		rt2x00_desc_write(txd, 1, word);
+
+		rt2x00_desc_read(txd, 2, &word);
+		rt2x00_set_field32(&word, TXD_W2_BUFFER_LENGTH,
+				   ring->data_size);
+		rt2x00_desc_write(txd, 2, word);
+
+		rt2x00_desc_read(txd, 0, &word);
+		rt2x00_set_field32(&word, TXD_W0_VALID, 0);
+		rt2x00_set_field32(&word, TXD_W0_OWNER_NIC, 0);
+		rt2x00_desc_write(txd, 0, word);
+	}
+
+	rt2x00_ring_index_clear(ring);
+}
+
+static int rt2400pci_init_rings(struct rt2x00_dev *rt2x00dev)
+{
+	u32 reg;
+
+	/*
+	 * Initialize rings.
+	 */
+	rt2400pci_init_rxring(rt2x00dev);
+	rt2400pci_init_txring(rt2x00dev, IEEE80211_TX_QUEUE_DATA0);
+	rt2400pci_init_txring(rt2x00dev, IEEE80211_TX_QUEUE_DATA1);
+	rt2400pci_init_txring(rt2x00dev, IEEE80211_TX_QUEUE_AFTER_BEACON);
+	rt2400pci_init_txring(rt2x00dev, IEEE80211_TX_QUEUE_BEACON);
+
+	/*
+	 * Initialize registers.
+	 */
+	rt2x00pci_register_read(rt2x00dev, TXCSR2, &reg);
+	rt2x00_set_field32(&reg, TXCSR2_TXD_SIZE,
+			   rt2x00dev->tx[IEEE80211_TX_QUEUE_DATA0].desc_size);
+	rt2x00_set_field32(&reg, TXCSR2_NUM_TXD,
+			   rt2x00dev->tx[IEEE80211_TX_QUEUE_DATA1].stats.limit);
+	rt2x00_set_field32(&reg, TXCSR2_NUM_ATIM,
+			   rt2x00dev->bcn[1].stats.limit);
+	rt2x00_set_field32(&reg, TXCSR2_NUM_PRIO,
+			   rt2x00dev->tx[IEEE80211_TX_QUEUE_DATA0].stats.limit);
+	rt2x00pci_register_write(rt2x00dev, TXCSR2, reg);
+
+	rt2x00pci_register_read(rt2x00dev, TXCSR3, &reg);
+	rt2x00_set_field32(&reg, TXCSR3_TX_RING_REGISTER,
+			   rt2x00dev->tx[IEEE80211_TX_QUEUE_DATA1].data_dma);
+	rt2x00pci_register_write(rt2x00dev, TXCSR3, reg);
+
+	rt2x00pci_register_read(rt2x00dev, TXCSR5, &reg);
+	rt2x00_set_field32(&reg, TXCSR5_PRIO_RING_REGISTER,
+			   rt2x00dev->tx[IEEE80211_TX_QUEUE_DATA0].data_dma);
+	rt2x00pci_register_write(rt2x00dev, TXCSR5, reg);
+
+	rt2x00pci_register_read(rt2x00dev, TXCSR4, &reg);
+	rt2x00_set_field32(&reg, TXCSR4_ATIM_RING_REGISTER,
+			   rt2x00dev->bcn[1].data_dma);
+	rt2x00pci_register_write(rt2x00dev, TXCSR4, reg);
+
+	rt2x00pci_register_read(rt2x00dev, TXCSR6, &reg);
+	rt2x00_set_field32(&reg, TXCSR6_BEACON_RING_REGISTER,
+			   rt2x00dev->bcn[0].data_dma);
+	rt2x00pci_register_write(rt2x00dev, TXCSR6, reg);
+
+	rt2x00pci_register_read(rt2x00dev, RXCSR1, &reg);
+	rt2x00_set_field32(&reg, RXCSR1_RXD_SIZE, rt2x00dev->rx->desc_size);
+	rt2x00_set_field32(&reg, RXCSR1_NUM_RXD, rt2x00dev->rx->stats.limit);
+	rt2x00pci_register_write(rt2x00dev, RXCSR1, reg);
+
+	rt2x00pci_register_read(rt2x00dev, RXCSR2, &reg);
+	rt2x00_set_field32(&reg, RXCSR2_RX_RING_REGISTER,
+			   rt2x00dev->rx->data_dma);
+	rt2x00pci_register_write(rt2x00dev, RXCSR2, reg);
+
+	return 0;
+}
+
+static int rt2400pci_init_registers(struct rt2x00_dev *rt2x00dev)
+{
+	u32 reg;
+
+	rt2x00pci_register_write(rt2x00dev, PSCSR0, 0x00020002);
+	rt2x00pci_register_write(rt2x00dev, PSCSR1, 0x00000002);
+	rt2x00pci_register_write(rt2x00dev, PSCSR2, 0x00023f20);
+	rt2x00pci_register_write(rt2x00dev, PSCSR3, 0x00000002);
+
+	rt2x00pci_register_read(rt2x00dev, TIMECSR, &reg);
+	rt2x00_set_field32(&reg, TIMECSR_US_COUNT, 33);
+	rt2x00_set_field32(&reg, TIMECSR_US_64_COUNT, 63);
+	rt2x00_set_field32(&reg, TIMECSR_BEACON_EXPECT, 0);
+	rt2x00pci_register_write(rt2x00dev, TIMECSR, reg);
+
+	rt2x00pci_register_read(rt2x00dev, CSR9, &reg);
+	rt2x00_set_field32(&reg, CSR9_MAX_FRAME_UNIT,
+			   (rt2x00dev->rx->data_size / 128));
+	rt2x00pci_register_write(rt2x00dev, CSR9, reg);
+
+	rt2x00pci_register_write(rt2x00dev, CNT3, 0x3f080000);
+
+	rt2x00pci_register_read(rt2x00dev, ARCSR0, &reg);
+	rt2x00_set_field32(&reg, ARCSR0_AR_BBP_DATA0, 133);
+	rt2x00_set_field32(&reg, ARCSR0_AR_BBP_ID0, 134);
+	rt2x00_set_field32(&reg, ARCSR0_AR_BBP_DATA1, 136);
+	rt2x00_set_field32(&reg, ARCSR0_AR_BBP_ID1, 135);
+	rt2x00pci_register_write(rt2x00dev, ARCSR0, reg);
+
+	rt2x00pci_register_read(rt2x00dev, RXCSR3, &reg);
+	rt2x00_set_field32(&reg, RXCSR3_BBP_ID0, 3); /* Tx power.*/
+	rt2x00_set_field32(&reg, RXCSR3_BBP_ID0_VALID, 1);
+	rt2x00_set_field32(&reg, RXCSR3_BBP_ID1, 32); /* Signal */
+	rt2x00_set_field32(&reg, RXCSR3_BBP_ID1_VALID, 1);
+	rt2x00_set_field32(&reg, RXCSR3_BBP_ID2, 36); /* Rssi */
+	rt2x00_set_field32(&reg, RXCSR3_BBP_ID2_VALID, 1);
+	rt2x00pci_register_write(rt2x00dev, RXCSR3, reg);
+
+	rt2x00pci_register_write(rt2x00dev, PWRCSR0, 0x3f3b3100);
+
+	if (rt2x00dev->ops->lib->set_device_state(rt2x00dev, STATE_AWAKE))
+		return -EBUSY;
+
+	rt2x00pci_register_write(rt2x00dev, MACCSR0, 0x00217223);
+	rt2x00pci_register_write(rt2x00dev, MACCSR1, 0x00235518);
+
+	rt2x00pci_register_read(rt2x00dev, MACCSR2, &reg);
+	rt2x00_set_field32(&reg, MACCSR2_DELAY, 64);
+	rt2x00pci_register_write(rt2x00dev, MACCSR2, reg);
+
+	rt2x00pci_register_read(rt2x00dev, RALINKCSR, &reg);
+	rt2x00_set_field32(&reg, RALINKCSR_AR_BBP_DATA0, 17);
+	rt2x00_set_field32(&reg, RALINKCSR_AR_BBP_ID0, 154);
+	rt2x00_set_field32(&reg, RALINKCSR_AR_BBP_DATA1, 0);
+	rt2x00_set_field32(&reg, RALINKCSR_AR_BBP_ID1, 154);
+	rt2x00pci_register_write(rt2x00dev, RALINKCSR, reg);
+
+	rt2x00pci_register_read(rt2x00dev, CSR1, &reg);
+	rt2x00_set_field32(&reg, CSR1_SOFT_RESET, 1);
+	rt2x00_set_field32(&reg, CSR1_BBP_RESET, 0);
+	rt2x00_set_field32(&reg, CSR1_HOST_READY, 0);
+	rt2x00pci_register_write(rt2x00dev, CSR1, reg);
+
+	rt2x00pci_register_read(rt2x00dev, CSR1, &reg);
+	rt2x00_set_field32(&reg, CSR1_SOFT_RESET, 0);
+	rt2x00_set_field32(&reg, CSR1_HOST_READY, 1);
+	rt2x00pci_register_write(rt2x00dev, CSR1, reg);
+
+	/*
+	 * We must clear the FCS and FIFO error count.
+	 * These registers are cleared on read,
+	 * so we may pass a useless variable to store the value.
+	 */
+	rt2x00pci_register_read(rt2x00dev, CNT0, &reg);
+	rt2x00pci_register_read(rt2x00dev, CNT4, &reg);
+
+	return 0;
+}
+
+static int rt2400pci_init_bbp(struct rt2x00_dev *rt2x00dev)
+{
+	unsigned int i;
+	u16 eeprom;
+	u8 reg_id;
+	u8 value;
+
+	for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
+		rt2400pci_bbp_read(rt2x00dev, 0, &value);
+		if ((value != 0xff) && (value != 0x00))
+			goto continue_csr_init;
+		NOTICE(rt2x00dev, "Waiting for BBP register.\n");
+		udelay(REGISTER_BUSY_DELAY);
+	}
+
+	ERROR(rt2x00dev, "BBP register access failed, aborting.\n");
+	return -EACCES;
+
+continue_csr_init:
+	rt2400pci_bbp_write(rt2x00dev, 1, 0x00);
+	rt2400pci_bbp_write(rt2x00dev, 3, 0x27);
+	rt2400pci_bbp_write(rt2x00dev, 4, 0x08);
+	rt2400pci_bbp_write(rt2x00dev, 10, 0x0f);
+	rt2400pci_bbp_write(rt2x00dev, 15, 0x72);
+	rt2400pci_bbp_write(rt2x00dev, 16, 0x74);
+	rt2400pci_bbp_write(rt2x00dev, 17, 0x20);
+	rt2400pci_bbp_write(rt2x00dev, 18, 0x72);
+	rt2400pci_bbp_write(rt2x00dev, 19, 0x0b);
+	rt2400pci_bbp_write(rt2x00dev, 20, 0x00);
+	rt2400pci_bbp_write(rt2x00dev, 28, 0x11);
+	rt2400pci_bbp_write(rt2x00dev, 29, 0x04);
+	rt2400pci_bbp_write(rt2x00dev, 30, 0x21);
+	rt2400pci_bbp_write(rt2x00dev, 31, 0x00);
+
+	DEBUG(rt2x00dev, "Start initialization from EEPROM...\n");
+	for (i = 0; i < EEPROM_BBP_SIZE; i++) {
+		rt2x00_eeprom_read(rt2x00dev, EEPROM_BBP_START + i, &eeprom);
+
+		if (eeprom != 0xffff && eeprom != 0x0000) {
+			reg_id = rt2x00_get_field16(eeprom, EEPROM_BBP_REG_ID);
+			value = rt2x00_get_field16(eeprom, EEPROM_BBP_VALUE);
+			DEBUG(rt2x00dev, "BBP: 0x%02x, value: 0x%02x.\n",
+			      reg_id, value);
+			rt2400pci_bbp_write(rt2x00dev, reg_id, value);
+		}
+	}
+	DEBUG(rt2x00dev, "...End initialization from EEPROM.\n");
+
+	return 0;
+}
+
+/*
+ * Device state switch handlers.
+ */
+static void rt2400pci_toggle_rx(struct rt2x00_dev *rt2x00dev,
+				enum dev_state state)
+{
+	u32 reg;
+
+	rt2x00pci_register_read(rt2x00dev, RXCSR0, &reg);
+	rt2x00_set_field32(&reg, RXCSR0_DISABLE_RX,
+			   state == STATE_RADIO_RX_OFF);
+	rt2x00pci_register_write(rt2x00dev, RXCSR0, reg);
+}
+
+static void rt2400pci_toggle_irq(struct rt2x00_dev *rt2x00dev,
+				 enum dev_state state)
+{
+	int mask = (state == STATE_RADIO_IRQ_OFF);
+	u32 reg;
+
+	/*
+	 * When interrupts are being enabled, the interrupt registers
+	 * should clear the register to assure a clean state.
+	 */
+	if (state == STATE_RADIO_IRQ_ON) {
+		rt2x00pci_register_read(rt2x00dev, CSR7, &reg);
+		rt2x00pci_register_write(rt2x00dev, CSR7, reg);
+	}
+
+	/*
+	 * Only toggle the interrupts bits we are going to use.
+	 * Non-checked interrupt bits are disabled by default.
+	 */
+	rt2x00pci_register_read(rt2x00dev, CSR8, &reg);
+	rt2x00_set_field32(&reg, CSR8_TBCN_EXPIRE, mask);
+	rt2x00_set_field32(&reg, CSR8_TXDONE_TXRING, mask);
+	rt2x00_set_field32(&reg, CSR8_TXDONE_ATIMRING, mask);
+	rt2x00_set_field32(&reg, CSR8_TXDONE_PRIORING, mask);
+	rt2x00_set_field32(&reg, CSR8_RXDONE, mask);
+	rt2x00pci_register_write(rt2x00dev, CSR8, reg);
+}
+
+static int rt2400pci_enable_radio(struct rt2x00_dev *rt2x00dev)
+{
+	/*
+	 * Initialize all registers.
+	 */
+	if (rt2400pci_init_rings(rt2x00dev) ||
+	    rt2400pci_init_registers(rt2x00dev) ||
+	    rt2400pci_init_bbp(rt2x00dev)) {
+		ERROR(rt2x00dev, "Register initialization failed.\n");
+		return -EIO;
+	}
+
+	/*
+	 * Enable interrupts.
+	 */
+	rt2400pci_toggle_irq(rt2x00dev, STATE_RADIO_IRQ_ON);
+
+	/*
+	 * Enable LED
+	 */
+	rt2400pci_enable_led(rt2x00dev);
+
+	return 0;
+}
+
+static void rt2400pci_disable_radio(struct rt2x00_dev *rt2x00dev)
+{
+	u32 reg;
+
+	/*
+	 * Disable LED
+	 */
+	rt2400pci_disable_led(rt2x00dev);
+
+	rt2x00pci_register_write(rt2x00dev, PWRCSR0, 0);
+
+	/*
+	 * Disable synchronisation.
+	 */
+	rt2x00pci_register_write(rt2x00dev, CSR14, 0);
+
+	/*
+	 * Cancel RX and TX.
+	 */
+	rt2x00pci_register_read(rt2x00dev, TXCSR0, &reg);
+	rt2x00_set_field32(&reg, TXCSR0_ABORT, 1);
+	rt2x00pci_register_write(rt2x00dev, TXCSR0, reg);
+
+	/*
+	 * Disable interrupts.
+	 */
+	rt2400pci_toggle_irq(rt2x00dev, STATE_RADIO_IRQ_OFF);
+}
+
+static int rt2400pci_set_state(struct rt2x00_dev *rt2x00dev,
+			       enum dev_state state)
+{
+	u32 reg;
+	unsigned int i;
+	char put_to_sleep;
+	char bbp_state;
+	char rf_state;
+
+	put_to_sleep = (state != STATE_AWAKE);
+
+	rt2x00pci_register_read(rt2x00dev, PWRCSR1, &reg);
+	rt2x00_set_field32(&reg, PWRCSR1_SET_STATE, 1);
+	rt2x00_set_field32(&reg, PWRCSR1_BBP_DESIRE_STATE, state);
+	rt2x00_set_field32(&reg, PWRCSR1_RF_DESIRE_STATE, state);
+	rt2x00_set_field32(&reg, PWRCSR1_PUT_TO_SLEEP, put_to_sleep);
+	rt2x00pci_register_write(rt2x00dev, PWRCSR1, reg);
+
+	/*
+	 * Device is not guaranteed to be in the requested state yet.
+	 * We must wait until the register indicates that the
+	 * device has entered the correct state.
+	 */
+	for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
+		rt2x00pci_register_read(rt2x00dev, PWRCSR1, &reg);
+		bbp_state = rt2x00_get_field32(reg, PWRCSR1_BBP_CURR_STATE);
+		rf_state = rt2x00_get_field32(reg, PWRCSR1_RF_CURR_STATE);
+		if (bbp_state == state && rf_state == state)
+			return 0;
+		msleep(10);
+	}
+
+	NOTICE(rt2x00dev, "Device failed to enter state %d, "
+	       "current device state: bbp %d and rf %d.\n",
+	       state, bbp_state, rf_state);
+
+	return -EBUSY;
+}
+
+static int rt2400pci_set_device_state(struct rt2x00_dev *rt2x00dev,
+				      enum dev_state state)
+{
+	int retval = 0;
+
+	switch (state) {
+	case STATE_RADIO_ON:
+		retval = rt2400pci_enable_radio(rt2x00dev);
+		break;
+	case STATE_RADIO_OFF:
+		rt2400pci_disable_radio(rt2x00dev);
+		break;
+	case STATE_RADIO_RX_ON:
+	case STATE_RADIO_RX_OFF:
+		rt2400pci_toggle_rx(rt2x00dev, state);
+		break;
+	case STATE_DEEP_SLEEP:
+	case STATE_SLEEP:
+	case STATE_STANDBY:
+	case STATE_AWAKE:
+		retval = rt2400pci_set_state(rt2x00dev, state);
+		break;
+	default:
+		retval = -ENOTSUPP;
+		break;
+	}
+
+	return retval;
+}
+
+/*
+ * TX descriptor initialization
+ */
+static void rt2400pci_write_tx_desc(struct rt2x00_dev *rt2x00dev,
+				    struct data_desc *txd,
+				    struct txdata_entry_desc *desc,
+				    struct ieee80211_hdr *ieee80211hdr,
+				    unsigned int length,
+				    struct ieee80211_tx_control *control)
+{
+	u32 word;
+	u32 signal = 0;
+	u32 service = 0;
+	u32 length_high = 0;
+	u32 length_low = 0;
+
+	/*
+	 * The PLCP values should be treated as if they
+	 * were BBP values.
+	 */
+	rt2x00_set_field32(&signal, BBPCSR_VALUE, desc->signal);
+	rt2x00_set_field32(&signal, BBPCSR_REGNUM, 5);
+	rt2x00_set_field32(&signal, BBPCSR_BUSY, 1);
+
+	rt2x00_set_field32(&service, BBPCSR_VALUE, desc->service);
+	rt2x00_set_field32(&service, BBPCSR_REGNUM, 6);
+	rt2x00_set_field32(&service, BBPCSR_BUSY, 1);
+
+	rt2x00_set_field32(&length_high, BBPCSR_VALUE, desc->length_high);
+	rt2x00_set_field32(&length_high, BBPCSR_REGNUM, 7);
+	rt2x00_set_field32(&length_high, BBPCSR_BUSY, 1);
+
+	rt2x00_set_field32(&length_low, BBPCSR_VALUE, desc->length_low);
+	rt2x00_set_field32(&length_low, BBPCSR_REGNUM, 8);
+	rt2x00_set_field32(&length_low, BBPCSR_BUSY, 1);
+
+	/*
+	 * Start writing the descriptor words.
+	 */
+	rt2x00_desc_read(txd, 2, &word);
+	rt2x00_set_field32(&word, TXD_W2_DATABYTE_COUNT, length);
+	rt2x00_desc_write(txd, 2, word);
+
+	rt2x00_desc_read(txd, 3, &word);
+	rt2x00_set_field32(&word, TXD_W3_PLCP_SIGNAL, signal);
+	rt2x00_set_field32(&word, TXD_W3_PLCP_SERVICE, service);
+	rt2x00_desc_write(txd, 3, word);
+
+	rt2x00_desc_read(txd, 4, &word);
+	rt2x00_set_field32(&word, TXD_W4_PLCP_LENGTH_LOW, length_low);
+	rt2x00_set_field32(&word, TXD_W4_PLCP_LENGTH_HIGH, length_high);
+	rt2x00_desc_write(txd, 4, word);
+
+	rt2x00_desc_read(txd, 0, &word);
+	rt2x00_set_field32(&word, TXD_W0_OWNER_NIC, 1);
+	rt2x00_set_field32(&word, TXD_W0_VALID, 1);
+	rt2x00_set_field32(&word, TXD_W0_MORE_FRAG,
+			   test_bit(ENTRY_TXD_MORE_FRAG, &desc->flags));
+	rt2x00_set_field32(&word, TXD_W0_ACK,
+			   !(control->flags & IEEE80211_TXCTL_NO_ACK));
+	rt2x00_set_field32(&word, TXD_W0_TIMESTAMP,
+			   test_bit(ENTRY_TXD_REQ_TIMESTAMP, &desc->flags));
+	rt2x00_set_field32(&word, TXD_W0_RTS,
+			   test_bit(ENTRY_TXD_RTS_FRAME, &desc->flags));
+	rt2x00_set_field32(&word, TXD_W0_IFS, desc->ifs);
+	rt2x00_set_field32(&word, TXD_W0_RETRY_MODE,
+			   !!(control->flags &
+			      IEEE80211_TXCTL_LONG_RETRY_LIMIT));
+	rt2x00_desc_write(txd, 0, word);
+}
+
+/*
+ * TX data initialization
+ */
+static void rt2400pci_kick_tx_queue(struct rt2x00_dev *rt2x00dev,
+				    unsigned int queue)
+{
+	u32 reg;
+
+	if (queue == IEEE80211_TX_QUEUE_BEACON) {
+		rt2x00pci_register_read(rt2x00dev, CSR14, &reg);
+		if (!rt2x00_get_field32(reg, CSR14_BEACON_GEN)) {
+			rt2x00_set_field32(&reg, CSR14_BEACON_GEN, 1);
+			rt2x00pci_register_write(rt2x00dev, CSR14, reg);
+		}
+		return;
+	}
+
+	rt2x00pci_register_read(rt2x00dev, TXCSR0, &reg);
+	if (queue == IEEE80211_TX_QUEUE_DATA0)
+		rt2x00_set_field32(&reg, TXCSR0_KICK_PRIO, 1);
+	else if (queue == IEEE80211_TX_QUEUE_DATA1)
+		rt2x00_set_field32(&reg, TXCSR0_KICK_TX, 1);
+	else if (queue == IEEE80211_TX_QUEUE_AFTER_BEACON)
+		rt2x00_set_field32(&reg, TXCSR0_KICK_ATIM, 1);
+	rt2x00pci_register_write(rt2x00dev, TXCSR0, reg);
+}
+
+/*
+ * RX control handlers
+ */
+static void rt2400pci_fill_rxdone(struct data_entry *entry,
+				  struct rxdata_entry_desc *desc)
+{
+	struct data_desc *rxd = entry->priv;
+	u32 word0;
+	u32 word2;
+
+	rt2x00_desc_read(rxd, 0, &word0);
+	rt2x00_desc_read(rxd, 2, &word2);
+
+	desc->flags = 0;
+	if (rt2x00_get_field32(word0, RXD_W0_CRC_ERROR))
+		desc->flags |= RX_FLAG_FAILED_FCS_CRC;
+	if (rt2x00_get_field32(word0, RXD_W0_PHYSICAL_ERROR))
+		desc->flags |= RX_FLAG_FAILED_PLCP_CRC;
+
+	/*
+	 * Obtain the status about this packet.
+	 */
+	desc->signal = rt2x00_get_field32(word2, RXD_W2_SIGNAL);
+	desc->rssi = rt2x00_get_field32(word2, RXD_W2_RSSI) -
+	    entry->ring->rt2x00dev->rssi_offset;
+	desc->ofdm = 0;
+	desc->size = rt2x00_get_field32(word0, RXD_W0_DATABYTE_COUNT);
+}
+
+/*
+ * Interrupt functions.
+ */
+static void rt2400pci_txdone(struct rt2x00_dev *rt2x00dev, const int queue)
+{
+	struct data_ring *ring = rt2x00lib_get_ring(rt2x00dev, queue);
+	struct data_entry *entry;
+	struct data_desc *txd;
+	u32 word;
+	int tx_status;
+	int retry;
+
+	while (!rt2x00_ring_empty(ring)) {
+		entry = rt2x00_get_data_entry_done(ring);
+		txd = entry->priv;
+		rt2x00_desc_read(txd, 0, &word);
+
+		if (rt2x00_get_field32(word, TXD_W0_OWNER_NIC) ||
+		    !rt2x00_get_field32(word, TXD_W0_VALID))
+			break;
+
+		/*
+		 * Obtain the status about this packet.
+		 */
+		tx_status = rt2x00_get_field32(word, TXD_W0_RESULT);
+		retry = rt2x00_get_field32(word, TXD_W0_RETRY_COUNT);
+
+		rt2x00lib_txdone(entry, tx_status, retry);
+
+		/*
+		 * Make this entry available for reuse.
+		 */
+		entry->flags = 0;
+		rt2x00_set_field32(&word, TXD_W0_VALID, 0);
+		rt2x00_desc_write(txd, 0, word);
+		rt2x00_ring_index_done_inc(ring);
+	}
+
+	/*
+	 * If the data ring was full before the txdone handler
+	 * we must make sure the packet queue in the mac80211 stack
+	 * is reenabled when the txdone handler has finished.
+	 */
+	entry = ring->entry;
+	if (!rt2x00_ring_full(ring))
+		ieee80211_wake_queue(rt2x00dev->hw,
+				     entry->tx_status.control.queue);
+}
+
+static irqreturn_t rt2400pci_interrupt(int irq, void *dev_instance)
+{
+	struct rt2x00_dev *rt2x00dev = dev_instance;
+	u32 reg;
+
+	/*
+	 * Get the interrupt sources & saved to local variable.
+	 * Write register value back to clear pending interrupts.
+	 */
+	rt2x00pci_register_read(rt2x00dev, CSR7, &reg);
+	rt2x00pci_register_write(rt2x00dev, CSR7, reg);
+
+	if (!reg)
+		return IRQ_NONE;
+
+	if (!test_bit(DEVICE_ENABLED_RADIO, &rt2x00dev->flags))
+		return IRQ_HANDLED;
+
+	/*
+	 * Handle interrupts, walk through all bits
+	 * and run the tasks, the bits are checked in order of
+	 * priority.
+	 */
+
+	/*
+	 * 1 - Beacon timer expired interrupt.
+	 */
+	if (rt2x00_get_field32(reg, CSR7_TBCN_EXPIRE))
+		rt2x00lib_beacondone(rt2x00dev);
+
+	/*
+	 * 2 - Rx ring done interrupt.
+	 */
+	if (rt2x00_get_field32(reg, CSR7_RXDONE))
+		rt2x00pci_rxdone(rt2x00dev);
+
+	/*
+	 * 3 - Atim ring transmit done interrupt.
+	 */
+	if (rt2x00_get_field32(reg, CSR7_TXDONE_ATIMRING))
+		rt2400pci_txdone(rt2x00dev, IEEE80211_TX_QUEUE_AFTER_BEACON);
+
+	/*
+	 * 4 - Priority ring transmit done interrupt.
+	 */
+	if (rt2x00_get_field32(reg, CSR7_TXDONE_PRIORING))
+		rt2400pci_txdone(rt2x00dev, IEEE80211_TX_QUEUE_DATA0);
+
+	/*
+	 * 5 - Tx ring transmit done interrupt.
+	 */
+	if (rt2x00_get_field32(reg, CSR7_TXDONE_TXRING))
+		rt2400pci_txdone(rt2x00dev, IEEE80211_TX_QUEUE_DATA1);
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * Device probe functions.
+ */
+static int rt2400pci_validate_eeprom(struct rt2x00_dev *rt2x00dev)
+{
+	struct eeprom_93cx6 eeprom;
+	u32 reg;
+	u16 word;
+	u8 *mac;
+
+	rt2x00pci_register_read(rt2x00dev, CSR21, &reg);
+
+	eeprom.data = rt2x00dev;
+	eeprom.register_read = rt2400pci_eepromregister_read;
+	eeprom.register_write = rt2400pci_eepromregister_write;
+	eeprom.width = rt2x00_get_field32(reg, CSR21_TYPE_93C46) ?
+	    PCI_EEPROM_WIDTH_93C46 : PCI_EEPROM_WIDTH_93C66;
+	eeprom.reg_data_in = 0;
+	eeprom.reg_data_out = 0;
+	eeprom.reg_data_clock = 0;
+	eeprom.reg_chip_select = 0;
+
+	eeprom_93cx6_multiread(&eeprom, EEPROM_BASE, rt2x00dev->eeprom,
+			       EEPROM_SIZE / sizeof(u16));
+
+	/*
+	 * Start validation of the data that has been read.
+	 */
+	mac = rt2x00_eeprom_addr(rt2x00dev, EEPROM_MAC_ADDR_0);
+	if (!is_valid_ether_addr(mac)) {
+		DECLARE_MAC_BUF(macbuf);
+
+		random_ether_addr(mac);
+		EEPROM(rt2x00dev, "MAC: %s\n", print_mac(macbuf, mac));
+	}
+
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_ANTENNA, &word);
+	if (word == 0xffff) {
+		ERROR(rt2x00dev, "Invalid EEPROM data detected.\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int rt2400pci_init_eeprom(struct rt2x00_dev *rt2x00dev)
+{
+	u32 reg;
+	u16 value;
+	u16 eeprom;
+
+	/*
+	 * Read EEPROM word for configuration.
+	 */
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_ANTENNA, &eeprom);
+
+	/*
+	 * Identify RF chipset.
+	 */
+	value = rt2x00_get_field16(eeprom, EEPROM_ANTENNA_RF_TYPE);
+	rt2x00pci_register_read(rt2x00dev, CSR0, &reg);
+	rt2x00_set_chip(rt2x00dev, RT2460, value, reg);
+
+	if (!rt2x00_rf(&rt2x00dev->chip, RF2420) &&
+	    !rt2x00_rf(&rt2x00dev->chip, RF2421)) {
+		ERROR(rt2x00dev, "Invalid RF chipset detected.\n");
+		return -ENODEV;
+	}
+
+	/*
+	 * Identify default antenna configuration.
+	 */
+	rt2x00dev->hw->conf.antenna_sel_tx =
+	    rt2x00_get_field16(eeprom, EEPROM_ANTENNA_TX_DEFAULT);
+	rt2x00dev->hw->conf.antenna_sel_rx =
+	    rt2x00_get_field16(eeprom, EEPROM_ANTENNA_RX_DEFAULT);
+
+	/*
+	 * Store led mode, for correct led behaviour.
+	 */
+	rt2x00dev->led_mode =
+	    rt2x00_get_field16(eeprom, EEPROM_ANTENNA_LED_MODE);
+
+	/*
+	 * Detect if this device has an hardware controlled radio.
+	 */
+#ifdef CONFIG_RT2400PCI_RFKILL
+	if (rt2x00_get_field16(eeprom, EEPROM_ANTENNA_HARDWARE_RADIO))
+		__set_bit(CONFIG_SUPPORT_HW_BUTTON, &rt2x00dev->flags);
+#endif /* CONFIG_RT2400PCI_RFKILL */
+
+	/*
+	 * Check if the BBP tuning should be enabled.
+	 */
+	if (!rt2x00_get_field16(eeprom, EEPROM_ANTENNA_RX_AGCVGC_TUNING))
+		__set_bit(CONFIG_DISABLE_LINK_TUNING, &rt2x00dev->flags);
+
+	return 0;
+}
+
+/*
+ * RF value list for RF2420 & RF2421
+ * Supports: 2.4 GHz
+ */
+static const struct rf_channel rf_vals_bg[] = {
+	{ 1,  0x00022058, 0x000c1fda, 0x00000101, 0 },
+	{ 2,  0x00022058, 0x000c1fee, 0x00000101, 0 },
+	{ 3,  0x00022058, 0x000c2002, 0x00000101, 0 },
+	{ 4,  0x00022058, 0x000c2016, 0x00000101, 0 },
+	{ 5,  0x00022058, 0x000c202a, 0x00000101, 0 },
+	{ 6,  0x00022058, 0x000c203e, 0x00000101, 0 },
+	{ 7,  0x00022058, 0x000c2052, 0x00000101, 0 },
+	{ 8,  0x00022058, 0x000c2066, 0x00000101, 0 },
+	{ 9,  0x00022058, 0x000c207a, 0x00000101, 0 },
+	{ 10, 0x00022058, 0x000c208e, 0x00000101, 0 },
+	{ 11, 0x00022058, 0x000c20a2, 0x00000101, 0 },
+	{ 12, 0x00022058, 0x000c20b6, 0x00000101, 0 },
+	{ 13, 0x00022058, 0x000c20ca, 0x00000101, 0 },
+	{ 14, 0x00022058, 0x000c20fa, 0x00000101, 0 },
+};
+
+static void rt2400pci_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
+{
+	struct hw_mode_spec *spec = &rt2x00dev->spec;
+	u8 *txpower;
+	unsigned int i;
+
+	/*
+	 * Initialize all hw fields.
+	 */
+	rt2x00dev->hw->flags = IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING;
+	rt2x00dev->hw->extra_tx_headroom = 0;
+	rt2x00dev->hw->max_signal = MAX_SIGNAL;
+	rt2x00dev->hw->max_rssi = MAX_RX_SSI;
+	rt2x00dev->hw->queues = 2;
+
+	SET_IEEE80211_DEV(rt2x00dev->hw, &rt2x00dev_pci(rt2x00dev)->dev);
+	SET_IEEE80211_PERM_ADDR(rt2x00dev->hw,
+				rt2x00_eeprom_addr(rt2x00dev,
+						   EEPROM_MAC_ADDR_0));
+
+	/*
+	 * Convert tx_power array in eeprom.
+	 */
+	txpower = rt2x00_eeprom_addr(rt2x00dev, EEPROM_TXPOWER_START);
+	for (i = 0; i < 14; i++)
+		txpower[i] = TXPOWER_FROM_DEV(txpower[i]);
+
+	/*
+	 * Initialize hw_mode information.
+	 */
+	spec->num_modes = 1;
+	spec->num_rates = 4;
+	spec->tx_power_a = NULL;
+	spec->tx_power_bg = txpower;
+	spec->tx_power_default = DEFAULT_TXPOWER;
+
+	spec->num_channels = ARRAY_SIZE(rf_vals_bg);
+	spec->channels = rf_vals_bg;
+}
+
+static int rt2400pci_probe_hw(struct rt2x00_dev *rt2x00dev)
+{
+	int retval;
+
+	/*
+	 * Allocate eeprom data.
+	 */
+	retval = rt2400pci_validate_eeprom(rt2x00dev);
+	if (retval)
+		return retval;
+
+	retval = rt2400pci_init_eeprom(rt2x00dev);
+	if (retval)
+		return retval;
+
+	/*
+	 * Initialize hw specifications.
+	 */
+	rt2400pci_probe_hw_mode(rt2x00dev);
+
+	/*
+	 * This device requires the beacon ring
+	 */
+	__set_bit(DRIVER_REQUIRE_BEACON_RING, &rt2x00dev->flags);
+
+	/*
+	 * Set the rssi offset.
+	 */
+	rt2x00dev->rssi_offset = DEFAULT_RSSI_OFFSET;
+
+	return 0;
+}
+
+/*
+ * IEEE80211 stack callback functions.
+ */
+static void rt2400pci_configure_filter(struct ieee80211_hw *hw,
+				       unsigned int changed_flags,
+				       unsigned int *total_flags,
+				       int mc_count,
+				       struct dev_addr_list *mc_list)
+{
+	struct rt2x00_dev *rt2x00dev = hw->priv;
+	struct interface *intf = &rt2x00dev->interface;
+	u32 reg;
+
+	/*
+	 * Mask off any flags we are going to ignore from
+	 * the total_flags field.
+	 */
+	*total_flags &=
+	    FIF_ALLMULTI |
+	    FIF_FCSFAIL |
+	    FIF_PLCPFAIL |
+	    FIF_CONTROL |
+	    FIF_OTHER_BSS |
+	    FIF_PROMISC_IN_BSS;
+
+	/*
+	 * Apply some rules to the filters:
+	 * - Some filters imply different filters to be set.
+	 * - Some things we can't filter out at all.
+	 * - Some filters are set based on interface type.
+	 */
+	*total_flags |= FIF_ALLMULTI;
+	if (*total_flags & FIF_OTHER_BSS ||
+	    *total_flags & FIF_PROMISC_IN_BSS)
+		*total_flags |= FIF_PROMISC_IN_BSS | FIF_OTHER_BSS;
+	if (is_interface_type(intf, IEEE80211_IF_TYPE_AP))
+		*total_flags |= FIF_PROMISC_IN_BSS;
+
+	/*
+	 * Check if there is any work left for us.
+	 */
+	if (intf->filter == *total_flags)
+		return;
+	intf->filter = *total_flags;
+
+	/*
+	 * Start configuration steps.
+	 * Note that the version error will always be dropped
+	 * since there is no filter for it at this time.
+	 */
+	rt2x00pci_register_read(rt2x00dev, RXCSR0, &reg);
+	rt2x00_set_field32(&reg, RXCSR0_DROP_CRC,
+			   !(*total_flags & FIF_FCSFAIL));
+	rt2x00_set_field32(&reg, RXCSR0_DROP_PHYSICAL,
+			   !(*total_flags & FIF_PLCPFAIL));
+	rt2x00_set_field32(&reg, RXCSR0_DROP_CONTROL,
+			   !(*total_flags & FIF_CONTROL));
+	rt2x00_set_field32(&reg, RXCSR0_DROP_NOT_TO_ME,
+			   !(*total_flags & FIF_PROMISC_IN_BSS));
+	rt2x00_set_field32(&reg, RXCSR0_DROP_TODS,
+			   !(*total_flags & FIF_PROMISC_IN_BSS));
+	rt2x00_set_field32(&reg, RXCSR0_DROP_VERSION_ERROR, 1);
+	rt2x00pci_register_write(rt2x00dev, RXCSR0, reg);
+}
+
+static int rt2400pci_set_retry_limit(struct ieee80211_hw *hw,
+				     u32 short_retry, u32 long_retry)
+{
+	struct rt2x00_dev *rt2x00dev = hw->priv;
+	u32 reg;
+
+	rt2x00pci_register_read(rt2x00dev, CSR11, &reg);
+	rt2x00_set_field32(&reg, CSR11_LONG_RETRY, long_retry);
+	rt2x00_set_field32(&reg, CSR11_SHORT_RETRY, short_retry);
+	rt2x00pci_register_write(rt2x00dev, CSR11, reg);
+
+	return 0;
+}
+
+static int rt2400pci_conf_tx(struct ieee80211_hw *hw,
+			     int queue,
+			     const struct ieee80211_tx_queue_params *params)
+{
+	struct rt2x00_dev *rt2x00dev = hw->priv;
+
+	/*
+	 * We don't support variating cw_min and cw_max variables
+	 * per queue. So by default we only configure the TX queue,
+	 * and ignore all other configurations.
+	 */
+	if (queue != IEEE80211_TX_QUEUE_DATA0)
+		return -EINVAL;
+
+	if (rt2x00mac_conf_tx(hw, queue, params))
+		return -EINVAL;
+
+	/*
+	 * Write configuration to register.
+	 */
+	rt2400pci_config_cw(rt2x00dev, &rt2x00dev->tx->tx_params);
+
+	return 0;
+}
+
+static u64 rt2400pci_get_tsf(struct ieee80211_hw *hw)
+{
+	struct rt2x00_dev *rt2x00dev = hw->priv;
+	u64 tsf;
+	u32 reg;
+
+	rt2x00pci_register_read(rt2x00dev, CSR17, &reg);
+	tsf = (u64) rt2x00_get_field32(reg, CSR17_HIGH_TSFTIMER) << 32;
+	rt2x00pci_register_read(rt2x00dev, CSR16, &reg);
+	tsf |= rt2x00_get_field32(reg, CSR16_LOW_TSFTIMER);
+
+	return tsf;
+}
+
+static void rt2400pci_reset_tsf(struct ieee80211_hw *hw)
+{
+	struct rt2x00_dev *rt2x00dev = hw->priv;
+
+	rt2x00pci_register_write(rt2x00dev, CSR16, 0);
+	rt2x00pci_register_write(rt2x00dev, CSR17, 0);
+}
+
+static int rt2400pci_tx_last_beacon(struct ieee80211_hw *hw)
+{
+	struct rt2x00_dev *rt2x00dev = hw->priv;
+	u32 reg;
+
+	rt2x00pci_register_read(rt2x00dev, CSR15, &reg);
+	return rt2x00_get_field32(reg, CSR15_BEACON_SENT);
+}
+
+static const struct ieee80211_ops rt2400pci_mac80211_ops = {
+	.tx			= rt2x00mac_tx,
+	.start			= rt2x00mac_start,
+	.stop			= rt2x00mac_stop,
+	.add_interface		= rt2x00mac_add_interface,
+	.remove_interface	= rt2x00mac_remove_interface,
+	.config			= rt2x00mac_config,
+	.config_interface	= rt2x00mac_config_interface,
+	.configure_filter	= rt2400pci_configure_filter,
+	.get_stats		= rt2x00mac_get_stats,
+	.set_retry_limit	= rt2400pci_set_retry_limit,
+	.erp_ie_changed		= rt2x00mac_erp_ie_changed,
+	.conf_tx		= rt2400pci_conf_tx,
+	.get_tx_stats		= rt2x00mac_get_tx_stats,
+	.get_tsf		= rt2400pci_get_tsf,
+	.reset_tsf		= rt2400pci_reset_tsf,
+	.beacon_update		= rt2x00pci_beacon_update,
+	.tx_last_beacon		= rt2400pci_tx_last_beacon,
+};
+
+static const struct rt2x00lib_ops rt2400pci_rt2x00_ops = {
+	.irq_handler		= rt2400pci_interrupt,
+	.probe_hw		= rt2400pci_probe_hw,
+	.initialize		= rt2x00pci_initialize,
+	.uninitialize		= rt2x00pci_uninitialize,
+	.set_device_state	= rt2400pci_set_device_state,
+	.rfkill_poll		= rt2400pci_rfkill_poll,
+	.link_stats		= rt2400pci_link_stats,
+	.reset_tuner		= rt2400pci_reset_tuner,
+	.link_tuner		= rt2400pci_link_tuner,
+	.write_tx_desc		= rt2400pci_write_tx_desc,
+	.write_tx_data		= rt2x00pci_write_tx_data,
+	.kick_tx_queue		= rt2400pci_kick_tx_queue,
+	.fill_rxdone		= rt2400pci_fill_rxdone,
+	.config_mac_addr	= rt2400pci_config_mac_addr,
+	.config_bssid		= rt2400pci_config_bssid,
+	.config_type		= rt2400pci_config_type,
+	.config_preamble	= rt2400pci_config_preamble,
+	.config			= rt2400pci_config,
+};
+
+static const struct rt2x00_ops rt2400pci_ops = {
+	.name		= DRV_NAME,
+	.rxd_size	= RXD_DESC_SIZE,
+	.txd_size	= TXD_DESC_SIZE,
+	.eeprom_size	= EEPROM_SIZE,
+	.rf_size	= RF_SIZE,
+	.lib		= &rt2400pci_rt2x00_ops,
+	.hw		= &rt2400pci_mac80211_ops,
+#ifdef CONFIG_RT2X00_LIB_DEBUGFS
+	.debugfs	= &rt2400pci_rt2x00debug,
+#endif /* CONFIG_RT2X00_LIB_DEBUGFS */
+};
+
+/*
+ * RT2400pci module information.
+ */
+static struct pci_device_id rt2400pci_device_table[] = {
+	{ PCI_DEVICE(0x1814, 0x0101), PCI_DEVICE_DATA(&rt2400pci_ops) },
+	{ 0, }
+};
+
+MODULE_AUTHOR(DRV_PROJECT);
+MODULE_VERSION(DRV_VERSION);
+MODULE_DESCRIPTION("Ralink RT2400 PCI & PCMCIA Wireless LAN driver.");
+MODULE_SUPPORTED_DEVICE("Ralink RT2460 PCI & PCMCIA chipset based cards");
+MODULE_DEVICE_TABLE(pci, rt2400pci_device_table);
+MODULE_LICENSE("GPL");
+
+static struct pci_driver rt2400pci_driver = {
+	.name		= DRV_NAME,
+	.id_table	= rt2400pci_device_table,
+	.probe		= rt2x00pci_probe,
+	.remove		= __devexit_p(rt2x00pci_remove),
+	.suspend	= rt2x00pci_suspend,
+	.resume		= rt2x00pci_resume,
+};
+
+static int __init rt2400pci_init(void)
+{
+	return pci_register_driver(&rt2400pci_driver);
+}
+
+static void __exit rt2400pci_exit(void)
+{
+	pci_unregister_driver(&rt2400pci_driver);
+}
+
+module_init(rt2400pci_init);
+module_exit(rt2400pci_exit);
diff --git a/drivers/net/wireless/rt2x00/rt2400pci.h b/drivers/net/wireless/rt2x00/rt2400pci.h
new file mode 100644
index 000000000000..ae22501f085d
--- /dev/null
+++ b/drivers/net/wireless/rt2x00/rt2400pci.h
@@ -0,0 +1,943 @@
+/*
+	Copyright (C) 2004 - 2007 rt2x00 SourceForge Project
+	<http://rt2x00.serialmonkey.com>
+
+	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, or
+	(at your option) any later version.
+
+	This program is distributed in the hope that it will be useful,
+	but WITHOUT ANY WARRANTY; without even the implied warranty of
+	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+	GNU General Public License for more details.
+
+	You should have received a copy of the GNU General Public License
+	along with this program; if not, write to the
+	Free Software Foundation, Inc.,
+	59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+/*
+	Module: rt2400pci
+	Abstract: Data structures and registers for the rt2400pci module.
+	Supported chipsets: RT2460.
+ */
+
+#ifndef RT2400PCI_H
+#define RT2400PCI_H
+
+/*
+ * RF chip defines.
+ */
+#define RF2420				0x0000
+#define RF2421				0x0001
+
+/*
+ * Signal information.
+ * Defaul offset is required for RSSI <-> dBm conversion.
+ */
+#define MAX_SIGNAL			100
+#define MAX_RX_SSI			-1
+#define DEFAULT_RSSI_OFFSET		100
+
+/*
+ * Register layout information.
+ */
+#define CSR_REG_BASE			0x0000
+#define CSR_REG_SIZE			0x014c
+#define EEPROM_BASE			0x0000
+#define EEPROM_SIZE			0x0100
+#define BBP_SIZE			0x0020
+#define RF_SIZE				0x0010
+
+/*
+ * Control/Status Registers(CSR).
+ * Some values are set in TU, whereas 1 TU == 1024 us.
+ */
+
+/*
+ * CSR0: ASIC revision number.
+ */
+#define CSR0				0x0000
+
+/*
+ * CSR1: System control register.
+ * SOFT_RESET: Software reset, 1: reset, 0: normal.
+ * BBP_RESET: Hardware reset, 1: reset, 0, release.
+ * HOST_READY: Host ready after initialization.
+ */
+#define CSR1				0x0004
+#define CSR1_SOFT_RESET			FIELD32(0x00000001)
+#define CSR1_BBP_RESET			FIELD32(0x00000002)
+#define CSR1_HOST_READY			FIELD32(0x00000004)
+
+/*
+ * CSR2: System admin status register (invalid).
+ */
+#define CSR2				0x0008
+
+/*
+ * CSR3: STA MAC address register 0.
+ */
+#define CSR3				0x000c
+#define CSR3_BYTE0			FIELD32(0x000000ff)
+#define CSR3_BYTE1			FIELD32(0x0000ff00)
+#define CSR3_BYTE2			FIELD32(0x00ff0000)
+#define CSR3_BYTE3			FIELD32(0xff000000)
+
+/*
+ * CSR4: STA MAC address register 1.
+ */
+#define CSR4				0x0010
+#define CSR4_BYTE4			FIELD32(0x000000ff)
+#define CSR4_BYTE5			FIELD32(0x0000ff00)
+
+/*
+ * CSR5: BSSID register 0.
+ */
+#define CSR5				0x0014
+#define CSR5_BYTE0			FIELD32(0x000000ff)
+#define CSR5_BYTE1			FIELD32(0x0000ff00)
+#define CSR5_BYTE2			FIELD32(0x00ff0000)
+#define CSR5_BYTE3			FIELD32(0xff000000)
+
+/*
+ * CSR6: BSSID register 1.
+ */
+#define CSR6				0x0018
+#define CSR6_BYTE4			FIELD32(0x000000ff)
+#define CSR6_BYTE5			FIELD32(0x0000ff00)
+
+/*
+ * CSR7: Interrupt source register.
+ * Write 1 to clear interrupt.
+ * TBCN_EXPIRE: Beacon timer expired interrupt.
+ * TWAKE_EXPIRE: Wakeup timer expired interrupt.
+ * TATIMW_EXPIRE: Timer of atim window expired interrupt.
+ * TXDONE_TXRING: Tx ring transmit done interrupt.
+ * TXDONE_ATIMRING: Atim ring transmit done interrupt.
+ * TXDONE_PRIORING: Priority ring transmit done interrupt.
+ * RXDONE: Receive done interrupt.
+ */
+#define CSR7				0x001c
+#define CSR7_TBCN_EXPIRE		FIELD32(0x00000001)
+#define CSR7_TWAKE_EXPIRE		FIELD32(0x00000002)
+#define CSR7_TATIMW_EXPIRE		FIELD32(0x00000004)
+#define CSR7_TXDONE_TXRING		FIELD32(0x00000008)
+#define CSR7_TXDONE_ATIMRING		FIELD32(0x00000010)
+#define CSR7_TXDONE_PRIORING		FIELD32(0x00000020)
+#define CSR7_RXDONE			FIELD32(0x00000040)
+
+/*
+ * CSR8: Interrupt mask register.
+ * Write 1 to mask interrupt.
+ * TBCN_EXPIRE: Beacon timer expired interrupt.
+ * TWAKE_EXPIRE: Wakeup timer expired interrupt.
+ * TATIMW_EXPIRE: Timer of atim window expired interrupt.
+ * TXDONE_TXRING: Tx ring transmit done interrupt.
+ * TXDONE_ATIMRING: Atim ring transmit done interrupt.
+ * TXDONE_PRIORING: Priority ring transmit done interrupt.
+ * RXDONE: Receive done interrupt.
+ */
+#define CSR8				0x0020
+#define CSR8_TBCN_EXPIRE		FIELD32(0x00000001)
+#define CSR8_TWAKE_EXPIRE		FIELD32(0x00000002)
+#define CSR8_TATIMW_EXPIRE		FIELD32(0x00000004)
+#define CSR8_TXDONE_TXRING		FIELD32(0x00000008)
+#define CSR8_TXDONE_ATIMRING		FIELD32(0x00000010)
+#define CSR8_TXDONE_PRIORING		FIELD32(0x00000020)
+#define CSR8_RXDONE			FIELD32(0x00000040)
+
+/*
+ * CSR9: Maximum frame length register.
+ * MAX_FRAME_UNIT: Maximum frame length in 128b unit, default: 12.
+ */
+#define CSR9				0x0024
+#define CSR9_MAX_FRAME_UNIT		FIELD32(0x00000f80)
+
+/*
+ * CSR11: Back-off control register.
+ * CWMIN: CWmin. Default cwmin is 31 (2^5 - 1).
+ * CWMAX: CWmax. Default cwmax is 1023 (2^10 - 1).
+ * SLOT_TIME: Slot time, default is 20us for 802.11b.
+ * LONG_RETRY: Long retry count.
+ * SHORT_RETRY: Short retry count.
+ */
+#define CSR11				0x002c
+#define CSR11_CWMIN			FIELD32(0x0000000f)
+#define CSR11_CWMAX			FIELD32(0x000000f0)
+#define CSR11_SLOT_TIME			FIELD32(0x00001f00)
+#define CSR11_LONG_RETRY		FIELD32(0x00ff0000)
+#define CSR11_SHORT_RETRY		FIELD32(0xff000000)
+
+/*
+ * CSR12: Synchronization configuration register 0.
+ * All units in 1/16 TU.
+ * BEACON_INTERVAL: Beacon interval, default is 100 TU.
+ * CFPMAX_DURATION: Cfp maximum duration, default is 100 TU.
+ */
+#define CSR12				0x0030
+#define CSR12_BEACON_INTERVAL		FIELD32(0x0000ffff)
+#define CSR12_CFP_MAX_DURATION		FIELD32(0xffff0000)
+
+/*
+ * CSR13: Synchronization configuration register 1.
+ * All units in 1/16 TU.
+ * ATIMW_DURATION: Atim window duration.
+ * CFP_PERIOD: Cfp period, default is 0 TU.
+ */
+#define CSR13				0x0034
+#define CSR13_ATIMW_DURATION		FIELD32(0x0000ffff)
+#define CSR13_CFP_PERIOD		FIELD32(0x00ff0000)
+
+/*
+ * CSR14: Synchronization control register.
+ * TSF_COUNT: Enable tsf auto counting.
+ * TSF_SYNC: Tsf sync, 0: disable, 1: infra, 2: ad-hoc/master mode.
+ * TBCN: Enable tbcn with reload value.
+ * TCFP: Enable tcfp & cfp / cp switching.
+ * TATIMW: Enable tatimw & atim window switching.
+ * BEACON_GEN: Enable beacon generator.
+ * CFP_COUNT_PRELOAD: Cfp count preload value.
+ * TBCM_PRELOAD: Tbcn preload value in units of 64us.
+ */
+#define CSR14				0x0038
+#define CSR14_TSF_COUNT			FIELD32(0x00000001)
+#define CSR14_TSF_SYNC			FIELD32(0x00000006)
+#define CSR14_TBCN			FIELD32(0x00000008)
+#define CSR14_TCFP			FIELD32(0x00000010)
+#define CSR14_TATIMW			FIELD32(0x00000020)
+#define CSR14_BEACON_GEN		FIELD32(0x00000040)
+#define CSR14_CFP_COUNT_PRELOAD		FIELD32(0x0000ff00)
+#define CSR14_TBCM_PRELOAD		FIELD32(0xffff0000)
+
+/*
+ * CSR15: Synchronization status register.
+ * CFP: ASIC is in contention-free period.
+ * ATIMW: ASIC is in ATIM window.
+ * BEACON_SENT: Beacon is send.
+ */
+#define CSR15				0x003c
+#define CSR15_CFP			FIELD32(0x00000001)
+#define CSR15_ATIMW			FIELD32(0x00000002)
+#define CSR15_BEACON_SENT		FIELD32(0x00000004)
+
+/*
+ * CSR16: TSF timer register 0.
+ */
+#define CSR16				0x0040
+#define CSR16_LOW_TSFTIMER		FIELD32(0xffffffff)
+
+/*
+ * CSR17: TSF timer register 1.
+ */
+#define CSR17				0x0044
+#define CSR17_HIGH_TSFTIMER		FIELD32(0xffffffff)
+
+/*
+ * CSR18: IFS timer register 0.
+ * SIFS: Sifs, default is 10 us.
+ * PIFS: Pifs, default is 30 us.
+ */
+#define CSR18				0x0048
+#define CSR18_SIFS			FIELD32(0x0000ffff)
+#define CSR18_PIFS			FIELD32(0xffff0000)
+
+/*
+ * CSR19: IFS timer register 1.
+ * DIFS: Difs, default is 50 us.
+ * EIFS: Eifs, default is 364 us.
+ */
+#define CSR19				0x004c
+#define CSR19_DIFS			FIELD32(0x0000ffff)
+#define CSR19_EIFS			FIELD32(0xffff0000)
+
+/*
+ * CSR20: Wakeup timer register.
+ * DELAY_AFTER_TBCN: Delay after tbcn expired in units of 1/16 TU.
+ * TBCN_BEFORE_WAKEUP: Number of beacon before wakeup.
+ * AUTOWAKE: Enable auto wakeup / sleep mechanism.
+ */
+#define CSR20				0x0050
+#define CSR20_DELAY_AFTER_TBCN		FIELD32(0x0000ffff)
+#define CSR20_TBCN_BEFORE_WAKEUP	FIELD32(0x00ff0000)
+#define CSR20_AUTOWAKE			FIELD32(0x01000000)
+
+/*
+ * CSR21: EEPROM control register.
+ * RELOAD: Write 1 to reload eeprom content.
+ * TYPE_93C46: 1: 93c46, 0:93c66.
+ */
+#define CSR21				0x0054
+#define CSR21_RELOAD			FIELD32(0x00000001)
+#define CSR21_EEPROM_DATA_CLOCK		FIELD32(0x00000002)
+#define CSR21_EEPROM_CHIP_SELECT	FIELD32(0x00000004)
+#define CSR21_EEPROM_DATA_IN		FIELD32(0x00000008)
+#define CSR21_EEPROM_DATA_OUT		FIELD32(0x00000010)
+#define CSR21_TYPE_93C46		FIELD32(0x00000020)
+
+/*
+ * CSR22: CFP control register.
+ * CFP_DURATION_REMAIN: Cfp duration remain, in units of TU.
+ * RELOAD_CFP_DURATION: Write 1 to reload cfp duration remain.
+ */
+#define CSR22				0x0058
+#define CSR22_CFP_DURATION_REMAIN	FIELD32(0x0000ffff)
+#define CSR22_RELOAD_CFP_DURATION	FIELD32(0x00010000)
+
+/*
+ * Transmit related CSRs.
+ * Some values are set in TU, whereas 1 TU == 1024 us.
+ */
+
+/*
+ * TXCSR0: TX Control Register.
+ * KICK_TX: Kick tx ring.
+ * KICK_ATIM: Kick atim ring.
+ * KICK_PRIO: Kick priority ring.
+ * ABORT: Abort all transmit related ring operation.
+ */
+#define TXCSR0				0x0060
+#define TXCSR0_KICK_TX			FIELD32(0x00000001)
+#define TXCSR0_KICK_ATIM		FIELD32(0x00000002)
+#define TXCSR0_KICK_PRIO		FIELD32(0x00000004)
+#define TXCSR0_ABORT			FIELD32(0x00000008)
+
+/*
+ * TXCSR1: TX Configuration Register.
+ * ACK_TIMEOUT: Ack timeout, default = sifs + 2*slottime + acktime @ 1mbps.
+ * ACK_CONSUME_TIME: Ack consume time, default = sifs + acktime @ 1mbps.
+ * TSF_OFFSET: Insert tsf offset.
+ * AUTORESPONDER: Enable auto responder which include ack & cts.
+ */
+#define TXCSR1				0x0064
+#define TXCSR1_ACK_TIMEOUT		FIELD32(0x000001ff)
+#define TXCSR1_ACK_CONSUME_TIME		FIELD32(0x0003fe00)
+#define TXCSR1_TSF_OFFSET		FIELD32(0x00fc0000)
+#define TXCSR1_AUTORESPONDER		FIELD32(0x01000000)
+
+/*
+ * TXCSR2: Tx descriptor configuration register.
+ * TXD_SIZE: Tx descriptor size, default is 48.
+ * NUM_TXD: Number of tx entries in ring.
+ * NUM_ATIM: Number of atim entries in ring.
+ * NUM_PRIO: Number of priority entries in ring.
+ */
+#define TXCSR2				0x0068
+#define TXCSR2_TXD_SIZE			FIELD32(0x000000ff)
+#define TXCSR2_NUM_TXD			FIELD32(0x0000ff00)
+#define TXCSR2_NUM_ATIM			FIELD32(0x00ff0000)
+#define TXCSR2_NUM_PRIO			FIELD32(0xff000000)
+
+/*
+ * TXCSR3: TX Ring Base address register.
+ */
+#define TXCSR3				0x006c
+#define TXCSR3_TX_RING_REGISTER		FIELD32(0xffffffff)
+
+/*
+ * TXCSR4: TX Atim Ring Base address register.
+ */
+#define TXCSR4				0x0070
+#define TXCSR4_ATIM_RING_REGISTER	FIELD32(0xffffffff)
+
+/*
+ * TXCSR5: TX Prio Ring Base address register.
+ */
+#define TXCSR5				0x0074
+#define TXCSR5_PRIO_RING_REGISTER	FIELD32(0xffffffff)
+
+/*
+ * TXCSR6: Beacon Base address register.
+ */
+#define TXCSR6				0x0078
+#define TXCSR6_BEACON_RING_REGISTER	FIELD32(0xffffffff)
+
+/*
+ * TXCSR7: Auto responder control register.
+ * AR_POWERMANAGEMENT: Auto responder power management bit.
+ */
+#define TXCSR7				0x007c
+#define TXCSR7_AR_POWERMANAGEMENT	FIELD32(0x00000001)
+
+/*
+ * Receive related CSRs.
+ * Some values are set in TU, whereas 1 TU == 1024 us.
+ */
+
+/*
+ * RXCSR0: RX Control Register.
+ * DISABLE_RX: Disable rx engine.
+ * DROP_CRC: Drop crc error.
+ * DROP_PHYSICAL: Drop physical error.
+ * DROP_CONTROL: Drop control frame.
+ * DROP_NOT_TO_ME: Drop not to me unicast frame.
+ * DROP_TODS: Drop frame tods bit is true.
+ * DROP_VERSION_ERROR: Drop version error frame.
+ * PASS_CRC: Pass all packets with crc attached.
+ */
+#define RXCSR0				0x0080
+#define RXCSR0_DISABLE_RX		FIELD32(0x00000001)
+#define RXCSR0_DROP_CRC			FIELD32(0x00000002)
+#define RXCSR0_DROP_PHYSICAL		FIELD32(0x00000004)
+#define RXCSR0_DROP_CONTROL		FIELD32(0x00000008)
+#define RXCSR0_DROP_NOT_TO_ME		FIELD32(0x00000010)
+#define RXCSR0_DROP_TODS		FIELD32(0x00000020)
+#define RXCSR0_DROP_VERSION_ERROR	FIELD32(0x00000040)
+#define RXCSR0_PASS_CRC			FIELD32(0x00000080)
+
+/*
+ * RXCSR1: RX descriptor configuration register.
+ * RXD_SIZE: Rx descriptor size, default is 32b.
+ * NUM_RXD: Number of rx entries in ring.
+ */
+#define RXCSR1				0x0084
+#define RXCSR1_RXD_SIZE			FIELD32(0x000000ff)
+#define RXCSR1_NUM_RXD			FIELD32(0x0000ff00)
+
+/*
+ * RXCSR2: RX Ring base address register.
+ */
+#define RXCSR2				0x0088
+#define RXCSR2_RX_RING_REGISTER		FIELD32(0xffffffff)
+
+/*
+ * RXCSR3: BBP ID register for Rx operation.
+ * BBP_ID#: BBP register # id.
+ * BBP_ID#_VALID: BBP register # id is valid or not.
+ */
+#define RXCSR3				0x0090
+#define RXCSR3_BBP_ID0			FIELD32(0x0000007f)
+#define RXCSR3_BBP_ID0_VALID		FIELD32(0x00000080)
+#define RXCSR3_BBP_ID1			FIELD32(0x00007f00)
+#define RXCSR3_BBP_ID1_VALID		FIELD32(0x00008000)
+#define RXCSR3_BBP_ID2			FIELD32(0x007f0000)
+#define RXCSR3_BBP_ID2_VALID		FIELD32(0x00800000)
+#define RXCSR3_BBP_ID3			FIELD32(0x7f000000)
+#define RXCSR3_BBP_ID3_VALID		FIELD32(0x80000000)
+
+/*
+ * RXCSR4: BBP ID register for Rx operation.
+ * BBP_ID#: BBP register # id.
+ * BBP_ID#_VALID: BBP register # id is valid or not.
+ */
+#define RXCSR4				0x0094
+#define RXCSR4_BBP_ID4			FIELD32(0x0000007f)
+#define RXCSR4_BBP_ID4_VALID		FIELD32(0x00000080)
+#define RXCSR4_BBP_ID5			FIELD32(0x00007f00)
+#define RXCSR4_BBP_ID5_VALID		FIELD32(0x00008000)
+
+/*
+ * ARCSR0: Auto Responder PLCP config register 0.
+ * ARCSR0_AR_BBP_DATA#: Auto responder BBP register # data.
+ * ARCSR0_AR_BBP_ID#: Auto responder BBP register # Id.
+ */
+#define ARCSR0				0x0098
+#define ARCSR0_AR_BBP_DATA0		FIELD32(0x000000ff)
+#define ARCSR0_AR_BBP_ID0		FIELD32(0x0000ff00)
+#define ARCSR0_AR_BBP_DATA1		FIELD32(0x00ff0000)
+#define ARCSR0_AR_BBP_ID1		FIELD32(0xff000000)
+
+/*
+ * ARCSR1: Auto Responder PLCP config register 1.
+ * ARCSR0_AR_BBP_DATA#: Auto responder BBP register # data.
+ * ARCSR0_AR_BBP_ID#: Auto responder BBP register # Id.
+ */
+#define ARCSR1				0x009c
+#define ARCSR1_AR_BBP_DATA2		FIELD32(0x000000ff)
+#define ARCSR1_AR_BBP_ID2		FIELD32(0x0000ff00)
+#define ARCSR1_AR_BBP_DATA3		FIELD32(0x00ff0000)
+#define ARCSR1_AR_BBP_ID3		FIELD32(0xff000000)
+
+/*
+ * Miscellaneous Registers.
+ * Some values are set in TU, whereas 1 TU == 1024 us.
+ */
+
+/*
+ * PCICSR: PCI control register.
+ * BIG_ENDIAN: 1: big endian, 0: little endian.
+ * RX_TRESHOLD: Rx threshold in dw to start pci access
+ * 0: 16dw (default), 1: 8dw, 2: 4dw, 3: 32dw.
+ * TX_TRESHOLD: Tx threshold in dw to start pci access
+ * 0: 0dw (default), 1: 1dw, 2: 4dw, 3: forward.
+ * BURST_LENTH: Pci burst length 0: 4dw (default, 1: 8dw, 2: 16dw, 3:32dw.
+ * ENABLE_CLK: Enable clk_run, pci clock can't going down to non-operational.
+ */
+#define PCICSR				0x008c
+#define PCICSR_BIG_ENDIAN		FIELD32(0x00000001)
+#define PCICSR_RX_TRESHOLD		FIELD32(0x00000006)
+#define PCICSR_TX_TRESHOLD		FIELD32(0x00000018)
+#define PCICSR_BURST_LENTH		FIELD32(0x00000060)
+#define PCICSR_ENABLE_CLK		FIELD32(0x00000080)
+
+/*
+ * CNT0: FCS error count.
+ * FCS_ERROR: FCS error count, cleared when read.
+ */
+#define CNT0				0x00a0
+#define CNT0_FCS_ERROR			FIELD32(0x0000ffff)
+
+/*
+ * Statistic Register.
+ * CNT1: PLCP error count.
+ * CNT2: Long error count.
+ * CNT3: CCA false alarm count.
+ * CNT4: Rx FIFO overflow count.
+ * CNT5: Tx FIFO underrun count.
+ */
+#define TIMECSR2			0x00a8
+#define CNT1				0x00ac
+#define CNT2				0x00b0
+#define TIMECSR3			0x00b4
+#define CNT3				0x00b8
+#define CNT4				0x00bc
+#define CNT5				0x00c0
+
+/*
+ * Baseband Control Register.
+ */
+
+/*
+ * PWRCSR0: Power mode configuration register.
+ */
+#define PWRCSR0				0x00c4
+
+/*
+ * Power state transition time registers.
+ */
+#define PSCSR0				0x00c8
+#define PSCSR1				0x00cc
+#define PSCSR2				0x00d0
+#define PSCSR3				0x00d4
+
+/*
+ * PWRCSR1: Manual power control / status register.
+ * Allowed state: 0 deep_sleep, 1: sleep, 2: standby, 3: awake.
+ * SET_STATE: Set state. Write 1 to trigger, self cleared.
+ * BBP_DESIRE_STATE: BBP desired state.
+ * RF_DESIRE_STATE: RF desired state.
+ * BBP_CURR_STATE: BBP current state.
+ * RF_CURR_STATE: RF current state.
+ * PUT_TO_SLEEP: Put to sleep. Write 1 to trigger, self cleared.
+ */
+#define PWRCSR1				0x00d8
+#define PWRCSR1_SET_STATE		FIELD32(0x00000001)
+#define PWRCSR1_BBP_DESIRE_STATE	FIELD32(0x00000006)
+#define PWRCSR1_RF_DESIRE_STATE		FIELD32(0x00000018)
+#define PWRCSR1_BBP_CURR_STATE		FIELD32(0x00000060)
+#define PWRCSR1_RF_CURR_STATE		FIELD32(0x00000180)
+#define PWRCSR1_PUT_TO_SLEEP		FIELD32(0x00000200)
+
+/*
+ * TIMECSR: Timer control register.
+ * US_COUNT: 1 us timer count in units of clock cycles.
+ * US_64_COUNT: 64 us timer count in units of 1 us timer.
+ * BEACON_EXPECT: Beacon expect window.
+ */
+#define TIMECSR				0x00dc
+#define TIMECSR_US_COUNT		FIELD32(0x000000ff)
+#define TIMECSR_US_64_COUNT		FIELD32(0x0000ff00)
+#define TIMECSR_BEACON_EXPECT		FIELD32(0x00070000)
+
+/*
+ * MACCSR0: MAC configuration register 0.
+ */
+#define MACCSR0				0x00e0
+
+/*
+ * MACCSR1: MAC configuration register 1.
+ * KICK_RX: Kick one-shot rx in one-shot rx mode.
+ * ONESHOT_RXMODE: Enable one-shot rx mode for debugging.
+ * BBPRX_RESET_MODE: Ralink bbp rx reset mode.
+ * AUTO_TXBBP: Auto tx logic access bbp control register.
+ * AUTO_RXBBP: Auto rx logic access bbp control register.
+ * LOOPBACK: Loopback mode. 0: normal, 1: internal, 2: external, 3:rsvd.
+ * INTERSIL_IF: Intersil if calibration pin.
+ */
+#define MACCSR1				0x00e4
+#define MACCSR1_KICK_RX			FIELD32(0x00000001)
+#define MACCSR1_ONESHOT_RXMODE		FIELD32(0x00000002)
+#define MACCSR1_BBPRX_RESET_MODE	FIELD32(0x00000004)
+#define MACCSR1_AUTO_TXBBP		FIELD32(0x00000008)
+#define MACCSR1_AUTO_RXBBP		FIELD32(0x00000010)
+#define MACCSR1_LOOPBACK		FIELD32(0x00000060)
+#define MACCSR1_INTERSIL_IF		FIELD32(0x00000080)
+
+/*
+ * RALINKCSR: Ralink Rx auto-reset BBCR.
+ * AR_BBP_DATA#: Auto reset BBP register # data.
+ * AR_BBP_ID#: Auto reset BBP register # id.
+ */
+#define RALINKCSR			0x00e8
+#define RALINKCSR_AR_BBP_DATA0		FIELD32(0x000000ff)
+#define RALINKCSR_AR_BBP_ID0		FIELD32(0x0000ff00)
+#define RALINKCSR_AR_BBP_DATA1		FIELD32(0x00ff0000)
+#define RALINKCSR_AR_BBP_ID1		FIELD32(0xff000000)
+
+/*
+ * BCNCSR: Beacon interval control register.
+ * CHANGE: Write one to change beacon interval.
+ * DELTATIME: The delta time value.
+ * NUM_BEACON: Number of beacon according to mode.
+ * MODE: Please refer to asic specs.
+ * PLUS: Plus or minus delta time value.
+ */
+#define BCNCSR				0x00ec
+#define BCNCSR_CHANGE			FIELD32(0x00000001)
+#define BCNCSR_DELTATIME		FIELD32(0x0000001e)
+#define BCNCSR_NUM_BEACON		FIELD32(0x00001fe0)
+#define BCNCSR_MODE			FIELD32(0x00006000)
+#define BCNCSR_PLUS			FIELD32(0x00008000)
+
+/*
+ * BBP / RF / IF Control Register.
+ */
+
+/*
+ * BBPCSR: BBP serial control register.
+ * VALUE: Register value to program into BBP.
+ * REGNUM: Selected BBP register.
+ * BUSY: 1: asic is busy execute BBP programming.
+ * WRITE_CONTROL: 1: write BBP, 0: read BBP.
+ */
+#define BBPCSR				0x00f0
+#define BBPCSR_VALUE			FIELD32(0x000000ff)
+#define BBPCSR_REGNUM			FIELD32(0x00007f00)
+#define BBPCSR_BUSY			FIELD32(0x00008000)
+#define BBPCSR_WRITE_CONTROL		FIELD32(0x00010000)
+
+/*
+ * RFCSR: RF serial control register.
+ * VALUE: Register value + id to program into rf/if.
+ * NUMBER_OF_BITS: Number of bits used in value (i:20, rfmd:22).
+ * IF_SELECT: Chip to program: 0: rf, 1: if.
+ * PLL_LD: Rf pll_ld status.
+ * BUSY: 1: asic is busy execute rf programming.
+ */
+#define RFCSR				0x00f4
+#define RFCSR_VALUE			FIELD32(0x00ffffff)
+#define RFCSR_NUMBER_OF_BITS		FIELD32(0x1f000000)
+#define RFCSR_IF_SELECT			FIELD32(0x20000000)
+#define RFCSR_PLL_LD			FIELD32(0x40000000)
+#define RFCSR_BUSY			FIELD32(0x80000000)
+
+/*
+ * LEDCSR: LED control register.
+ * ON_PERIOD: On period, default 70ms.
+ * OFF_PERIOD: Off period, default 30ms.
+ * LINK: 0: linkoff, 1: linkup.
+ * ACTIVITY: 0: idle, 1: active.
+ */
+#define LEDCSR				0x00f8
+#define LEDCSR_ON_PERIOD		FIELD32(0x000000ff)
+#define LEDCSR_OFF_PERIOD		FIELD32(0x0000ff00)
+#define LEDCSR_LINK			FIELD32(0x00010000)
+#define LEDCSR_ACTIVITY			FIELD32(0x00020000)
+
+/*
+ * ASIC pointer information.
+ * RXPTR: Current RX ring address.
+ * TXPTR: Current Tx ring address.
+ * PRIPTR: Current Priority ring address.
+ * ATIMPTR: Current ATIM ring address.
+ */
+#define RXPTR				0x0100
+#define TXPTR				0x0104
+#define PRIPTR				0x0108
+#define ATIMPTR				0x010c
+
+/*
+ * GPIO and others.
+ */
+
+/*
+ * GPIOCSR: GPIO control register.
+ */
+#define GPIOCSR				0x0120
+#define GPIOCSR_BIT0			FIELD32(0x00000001)
+#define GPIOCSR_BIT1			FIELD32(0x00000002)
+#define GPIOCSR_BIT2			FIELD32(0x00000004)
+#define GPIOCSR_BIT3			FIELD32(0x00000008)
+#define GPIOCSR_BIT4			FIELD32(0x00000010)
+#define GPIOCSR_BIT5			FIELD32(0x00000020)
+#define GPIOCSR_BIT6			FIELD32(0x00000040)
+#define GPIOCSR_BIT7			FIELD32(0x00000080)
+
+/*
+ * BBPPCSR: BBP Pin control register.
+ */
+#define BBPPCSR				0x0124
+
+/*
+ * BCNCSR1: Tx BEACON offset time control register.
+ * PRELOAD: Beacon timer offset in units of usec.
+ */
+#define BCNCSR1				0x0130
+#define BCNCSR1_PRELOAD			FIELD32(0x0000ffff)
+
+/*
+ * MACCSR2: TX_PE to RX_PE turn-around time control register
+ * DELAY: RX_PE low width, in units of pci clock cycle.
+ */
+#define MACCSR2				0x0134
+#define MACCSR2_DELAY			FIELD32(0x000000ff)
+
+/*
+ * ARCSR2: 1 Mbps ACK/CTS PLCP.
+ */
+#define ARCSR2				0x013c
+#define ARCSR2_SIGNAL			FIELD32(0x000000ff)
+#define ARCSR2_SERVICE			FIELD32(0x0000ff00)
+#define ARCSR2_LENGTH_LOW		FIELD32(0x00ff0000)
+#define ARCSR2_LENGTH			FIELD32(0xffff0000)
+
+/*
+ * ARCSR3: 2 Mbps ACK/CTS PLCP.
+ */
+#define ARCSR3				0x0140
+#define ARCSR3_SIGNAL			FIELD32(0x000000ff)
+#define ARCSR3_SERVICE			FIELD32(0x0000ff00)
+#define ARCSR3_LENGTH			FIELD32(0xffff0000)
+
+/*
+ * ARCSR4: 5.5 Mbps ACK/CTS PLCP.
+ */
+#define ARCSR4				0x0144
+#define ARCSR4_SIGNAL			FIELD32(0x000000ff)
+#define ARCSR4_SERVICE			FIELD32(0x0000ff00)
+#define ARCSR4_LENGTH			FIELD32(0xffff0000)
+
+/*
+ * ARCSR5: 11 Mbps ACK/CTS PLCP.
+ */
+#define ARCSR5				0x0148
+#define ARCSR5_SIGNAL			FIELD32(0x000000ff)
+#define ARCSR5_SERVICE			FIELD32(0x0000ff00)
+#define ARCSR5_LENGTH			FIELD32(0xffff0000)
+
+/*
+ * BBP registers.
+ * The wordsize of the BBP is 8 bits.
+ */
+
+/*
+ * R1: TX antenna control
+ */
+#define BBP_R1_TX_ANTENNA		FIELD8(0x03)
+
+/*
+ * R4: RX antenna control
+ */
+#define BBP_R4_RX_ANTENNA		FIELD8(0x06)
+
+/*
+ * RF registers
+ */
+
+/*
+ * RF 1
+ */
+#define RF1_TUNER			FIELD32(0x00020000)
+
+/*
+ * RF 3
+ */
+#define RF3_TUNER			FIELD32(0x00000100)
+#define RF3_TXPOWER			FIELD32(0x00003e00)
+
+/*
+ * EEPROM content.
+ * The wordsize of the EEPROM is 16 bits.
+ */
+
+/*
+ * HW MAC address.
+ */
+#define EEPROM_MAC_ADDR_0		0x0002
+#define EEPROM_MAC_ADDR_BYTE0		FIELD16(0x00ff)
+#define EEPROM_MAC_ADDR_BYTE1		FIELD16(0xff00)
+#define EEPROM_MAC_ADDR1		0x0003
+#define EEPROM_MAC_ADDR_BYTE2		FIELD16(0x00ff)
+#define EEPROM_MAC_ADDR_BYTE3		FIELD16(0xff00)
+#define EEPROM_MAC_ADDR_2		0x0004
+#define EEPROM_MAC_ADDR_BYTE4		FIELD16(0x00ff)
+#define EEPROM_MAC_ADDR_BYTE5		FIELD16(0xff00)
+
+/*
+ * EEPROM antenna.
+ * ANTENNA_NUM: Number of antenna's.
+ * TX_DEFAULT: Default antenna 0: diversity, 1: A, 2: B.
+ * RX_DEFAULT: Default antenna 0: diversity, 1: A, 2: B.
+ * RF_TYPE: Rf_type of this adapter.
+ * LED_MODE: 0: default, 1: TX/RX activity,2: Single (ignore link), 3: rsvd.
+ * RX_AGCVGC: 0: disable, 1:enable BBP R13 tuning.
+ * HARDWARE_RADIO: 1: Hardware controlled radio. Read GPIO0.
+ */
+#define EEPROM_ANTENNA			0x0b
+#define EEPROM_ANTENNA_NUM		FIELD16(0x0003)
+#define EEPROM_ANTENNA_TX_DEFAULT	FIELD16(0x000c)
+#define EEPROM_ANTENNA_RX_DEFAULT	FIELD16(0x0030)
+#define EEPROM_ANTENNA_RF_TYPE		FIELD16(0x0040)
+#define EEPROM_ANTENNA_LED_MODE		FIELD16(0x0180)
+#define EEPROM_ANTENNA_RX_AGCVGC_TUNING	FIELD16(0x0200)
+#define EEPROM_ANTENNA_HARDWARE_RADIO	FIELD16(0x0400)
+
+/*
+ * EEPROM BBP.
+ */
+#define EEPROM_BBP_START		0x0c
+#define EEPROM_BBP_SIZE			7
+#define EEPROM_BBP_VALUE		FIELD16(0x00ff)
+#define EEPROM_BBP_REG_ID		FIELD16(0xff00)
+
+/*
+ * EEPROM TXPOWER
+ */
+#define EEPROM_TXPOWER_START		0x13
+#define EEPROM_TXPOWER_SIZE		7
+#define EEPROM_TXPOWER_1		FIELD16(0x00ff)
+#define EEPROM_TXPOWER_2		FIELD16(0xff00)
+
+/*
+ * DMA descriptor defines.
+ */
+#define TXD_DESC_SIZE			( 8 * sizeof(struct data_desc) )
+#define RXD_DESC_SIZE			( 8 * sizeof(struct data_desc) )
+
+/*
+ * TX descriptor format for TX, PRIO, ATIM and Beacon Ring.
+ */
+
+/*
+ * Word0
+ */
+#define TXD_W0_OWNER_NIC		FIELD32(0x00000001)
+#define TXD_W0_VALID			FIELD32(0x00000002)
+#define TXD_W0_RESULT			FIELD32(0x0000001c)
+#define TXD_W0_RETRY_COUNT		FIELD32(0x000000e0)
+#define TXD_W0_MORE_FRAG		FIELD32(0x00000100)
+#define TXD_W0_ACK			FIELD32(0x00000200)
+#define TXD_W0_TIMESTAMP		FIELD32(0x00000400)
+#define TXD_W0_RTS			FIELD32(0x00000800)
+#define TXD_W0_IFS			FIELD32(0x00006000)
+#define TXD_W0_RETRY_MODE		FIELD32(0x00008000)
+#define TXD_W0_AGC			FIELD32(0x00ff0000)
+#define TXD_W0_R2			FIELD32(0xff000000)
+
+/*
+ * Word1
+ */
+#define TXD_W1_BUFFER_ADDRESS		FIELD32(0xffffffff)
+
+/*
+ * Word2
+ */
+#define TXD_W2_BUFFER_LENGTH		FIELD32(0x0000ffff)
+#define TXD_W2_DATABYTE_COUNT		FIELD32(0xffff0000)
+
+/*
+ * Word3 & 4: PLCP information
+ */
+#define TXD_W3_PLCP_SIGNAL		FIELD32(0x0000ffff)
+#define TXD_W3_PLCP_SERVICE		FIELD32(0xffff0000)
+#define TXD_W4_PLCP_LENGTH_LOW		FIELD32(0x0000ffff)
+#define TXD_W4_PLCP_LENGTH_HIGH		FIELD32(0xffff0000)
+
+/*
+ * Word5
+ */
+#define TXD_W5_BBCR4			FIELD32(0x0000ffff)
+#define TXD_W5_AGC_REG			FIELD32(0x007f0000)
+#define TXD_W5_AGC_REG_VALID		FIELD32(0x00800000)
+#define TXD_W5_XXX_REG			FIELD32(0x7f000000)
+#define TXD_W5_XXX_REG_VALID		FIELD32(0x80000000)
+
+/*
+ * Word6
+ */
+#define TXD_W6_SK_BUFF			FIELD32(0xffffffff)
+
+/*
+ * Word7
+ */
+#define TXD_W7_RESERVED			FIELD32(0xffffffff)
+
+/*
+ * RX descriptor format for RX Ring.
+ */
+
+/*
+ * Word0
+ */
+#define RXD_W0_OWNER_NIC		FIELD32(0x00000001)
+#define RXD_W0_UNICAST_TO_ME		FIELD32(0x00000002)
+#define RXD_W0_MULTICAST		FIELD32(0x00000004)
+#define RXD_W0_BROADCAST		FIELD32(0x00000008)
+#define RXD_W0_MY_BSS			FIELD32(0x00000010)
+#define RXD_W0_CRC_ERROR		FIELD32(0x00000020)
+#define RXD_W0_PHYSICAL_ERROR		FIELD32(0x00000080)
+#define RXD_W0_DATABYTE_COUNT		FIELD32(0xffff0000)
+
+/*
+ * Word1
+ */
+#define RXD_W1_BUFFER_ADDRESS		FIELD32(0xffffffff)
+
+/*
+ * Word2
+ */
+#define RXD_W2_BUFFER_LENGTH		FIELD32(0x0000ffff)
+#define RXD_W2_SIGNAL			FIELD32(0x00ff0000)
+#define RXD_W2_RSSI			FIELD32(0xff000000)
+
+/*
+ * Word3
+ */
+#define RXD_W3_BBR2			FIELD32(0x000000ff)
+#define RXD_W3_BBR3			FIELD32(0x0000ff00)
+#define RXD_W3_BBR4			FIELD32(0x00ff0000)
+#define RXD_W3_BBR5			FIELD32(0xff000000)
+
+/*
+ * Word4
+ */
+#define RXD_W4_RX_END_TIME		FIELD32(0xffffffff)
+
+/*
+ * Word5 & 6 & 7: Reserved
+ */
+#define RXD_W5_RESERVED			FIELD32(0xffffffff)
+#define RXD_W6_RESERVED			FIELD32(0xffffffff)
+#define RXD_W7_RESERVED			FIELD32(0xffffffff)
+
+/*
+ * Macro's for converting txpower from EEPROM to dscape value
+ * and from dscape value to register value.
+ * NOTE: Logics in rt2400pci for txpower are reversed
+ * compared to the other rt2x00 drivers. A higher txpower
+ * value means that the txpower must be lowered. This is
+ * important when converting the value coming from the
+ * dscape stack to the rt2400 acceptable value.
+ */
+#define MIN_TXPOWER	31
+#define MAX_TXPOWER	62
+#define DEFAULT_TXPOWER	39
+
+#define TXPOWER_FROM_DEV(__txpower)					\
+({									\
+	((__txpower) > MAX_TXPOWER) ? DEFAULT_TXPOWER - MIN_TXPOWER :	\
+	((__txpower) < MIN_TXPOWER) ? DEFAULT_TXPOWER - MIN_TXPOWER :	\
+	(((__txpower) - MAX_TXPOWER) + MIN_TXPOWER);			\
+})
+
+#define TXPOWER_TO_DEV(__txpower)			\
+({							\
+	(__txpower) += MIN_TXPOWER;			\
+	((__txpower) <= MIN_TXPOWER) ? MAX_TXPOWER :	\
+	(((__txpower) >= MAX_TXPOWER) ? MIN_TXPOWER :	\
+	(MAX_TXPOWER - ((__txpower) - MIN_TXPOWER)));	\
+})
+
+#endif /* RT2400PCI_H */
diff --git a/drivers/net/wireless/rt2x00/rt2500pci.c b/drivers/net/wireless/rt2x00/rt2500pci.c
new file mode 100644
index 000000000000..ff2d63267b19
--- /dev/null
+++ b/drivers/net/wireless/rt2x00/rt2500pci.c
@@ -0,0 +1,1971 @@
+/*
+	Copyright (C) 2004 - 2007 rt2x00 SourceForge Project
+	<http://rt2x00.serialmonkey.com>
+
+	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, or
+	(at your option) any later version.
+
+	This program is distributed in the hope that it will be useful,
+	but WITHOUT ANY WARRANTY; without even the implied warranty of
+	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+	GNU General Public License for more details.
+
+	You should have received a copy of the GNU General Public License
+	along with this program; if not, write to the
+	Free Software Foundation, Inc.,
+	59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+/*
+	Module: rt2500pci
+	Abstract: rt2500pci device specific routines.
+	Supported chipsets: RT2560.
+ */
+
+/*
+ * Set enviroment defines for rt2x00.h
+ */
+#define DRV_NAME "rt2500pci"
+
+#include <linux/delay.h>
+#include <linux/etherdevice.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/pci.h>
+#include <linux/eeprom_93cx6.h>
+
+#include "rt2x00.h"
+#include "rt2x00pci.h"
+#include "rt2500pci.h"
+
+/*
+ * Register access.
+ * All access to the CSR registers will go through the methods
+ * rt2x00pci_register_read and rt2x00pci_register_write.
+ * BBP and RF register require indirect register access,
+ * and use the CSR registers BBPCSR and RFCSR to achieve this.
+ * These indirect registers work with busy bits,
+ * and we will try maximal REGISTER_BUSY_COUNT times to access
+ * the register while taking a REGISTER_BUSY_DELAY us delay
+ * between each attampt. When the busy bit is still set at that time,
+ * the access attempt is considered to have failed,
+ * and we will print an error.
+ */
+static u32 rt2500pci_bbp_check(const struct rt2x00_dev *rt2x00dev)
+{
+	u32 reg;
+	unsigned int i;
+
+	for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
+		rt2x00pci_register_read(rt2x00dev, BBPCSR, &reg);
+		if (!rt2x00_get_field32(reg, BBPCSR_BUSY))
+			break;
+		udelay(REGISTER_BUSY_DELAY);
+	}
+
+	return reg;
+}
+
+static void rt2500pci_bbp_write(const struct rt2x00_dev *rt2x00dev,
+				const unsigned int word, const u8 value)
+{
+	u32 reg;
+
+	/*
+	 * Wait until the BBP becomes ready.
+	 */
+	reg = rt2500pci_bbp_check(rt2x00dev);
+	if (rt2x00_get_field32(reg, BBPCSR_BUSY)) {
+		ERROR(rt2x00dev, "BBPCSR register busy. Write failed.\n");
+		return;
+	}
+
+	/*
+	 * Write the data into the BBP.
+	 */
+	reg = 0;
+	rt2x00_set_field32(&reg, BBPCSR_VALUE, value);
+	rt2x00_set_field32(&reg, BBPCSR_REGNUM, word);
+	rt2x00_set_field32(&reg, BBPCSR_BUSY, 1);
+	rt2x00_set_field32(&reg, BBPCSR_WRITE_CONTROL, 1);
+
+	rt2x00pci_register_write(rt2x00dev, BBPCSR, reg);
+}
+
+static void rt2500pci_bbp_read(const struct rt2x00_dev *rt2x00dev,
+			       const unsigned int word, u8 *value)
+{
+	u32 reg;
+
+	/*
+	 * Wait until the BBP becomes ready.
+	 */
+	reg = rt2500pci_bbp_check(rt2x00dev);
+	if (rt2x00_get_field32(reg, BBPCSR_BUSY)) {
+		ERROR(rt2x00dev, "BBPCSR register busy. Read failed.\n");
+		return;
+	}
+
+	/*
+	 * Write the request into the BBP.
+	 */
+	reg = 0;
+	rt2x00_set_field32(&reg, BBPCSR_REGNUM, word);
+	rt2x00_set_field32(&reg, BBPCSR_BUSY, 1);
+	rt2x00_set_field32(&reg, BBPCSR_WRITE_CONTROL, 0);
+
+	rt2x00pci_register_write(rt2x00dev, BBPCSR, reg);
+
+	/*
+	 * Wait until the BBP becomes ready.
+	 */
+	reg = rt2500pci_bbp_check(rt2x00dev);
+	if (rt2x00_get_field32(reg, BBPCSR_BUSY)) {
+		ERROR(rt2x00dev, "BBPCSR register busy. Read failed.\n");
+		*value = 0xff;
+		return;
+	}
+
+	*value = rt2x00_get_field32(reg, BBPCSR_VALUE);
+}
+
+static void rt2500pci_rf_write(const struct rt2x00_dev *rt2x00dev,
+			       const unsigned int word, const u32 value)
+{
+	u32 reg;
+	unsigned int i;
+
+	if (!word)
+		return;
+
+	for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
+		rt2x00pci_register_read(rt2x00dev, RFCSR, &reg);
+		if (!rt2x00_get_field32(reg, RFCSR_BUSY))
+			goto rf_write;
+		udelay(REGISTER_BUSY_DELAY);
+	}
+
+	ERROR(rt2x00dev, "RFCSR register busy. Write failed.\n");
+	return;
+
+rf_write:
+	reg = 0;
+	rt2x00_set_field32(&reg, RFCSR_VALUE, value);
+	rt2x00_set_field32(&reg, RFCSR_NUMBER_OF_BITS, 20);
+	rt2x00_set_field32(&reg, RFCSR_IF_SELECT, 0);
+	rt2x00_set_field32(&reg, RFCSR_BUSY, 1);
+
+	rt2x00pci_register_write(rt2x00dev, RFCSR, reg);
+	rt2x00_rf_write(rt2x00dev, word, value);
+}
+
+static void rt2500pci_eepromregister_read(struct eeprom_93cx6 *eeprom)
+{
+	struct rt2x00_dev *rt2x00dev = eeprom->data;
+	u32 reg;
+
+	rt2x00pci_register_read(rt2x00dev, CSR21, &reg);
+
+	eeprom->reg_data_in = !!rt2x00_get_field32(reg, CSR21_EEPROM_DATA_IN);
+	eeprom->reg_data_out = !!rt2x00_get_field32(reg, CSR21_EEPROM_DATA_OUT);
+	eeprom->reg_data_clock =
+	    !!rt2x00_get_field32(reg, CSR21_EEPROM_DATA_CLOCK);
+	eeprom->reg_chip_select =
+	    !!rt2x00_get_field32(reg, CSR21_EEPROM_CHIP_SELECT);
+}
+
+static void rt2500pci_eepromregister_write(struct eeprom_93cx6 *eeprom)
+{
+	struct rt2x00_dev *rt2x00dev = eeprom->data;
+	u32 reg = 0;
+
+	rt2x00_set_field32(&reg, CSR21_EEPROM_DATA_IN, !!eeprom->reg_data_in);
+	rt2x00_set_field32(&reg, CSR21_EEPROM_DATA_OUT, !!eeprom->reg_data_out);
+	rt2x00_set_field32(&reg, CSR21_EEPROM_DATA_CLOCK,
+			   !!eeprom->reg_data_clock);
+	rt2x00_set_field32(&reg, CSR21_EEPROM_CHIP_SELECT,
+			   !!eeprom->reg_chip_select);
+
+	rt2x00pci_register_write(rt2x00dev, CSR21, reg);
+}
+
+#ifdef CONFIG_RT2X00_LIB_DEBUGFS
+#define CSR_OFFSET(__word)	( CSR_REG_BASE + ((__word) * sizeof(u32)) )
+
+static void rt2500pci_read_csr(const struct rt2x00_dev *rt2x00dev,
+			       const unsigned int word, u32 *data)
+{
+	rt2x00pci_register_read(rt2x00dev, CSR_OFFSET(word), data);
+}
+
+static void rt2500pci_write_csr(const struct rt2x00_dev *rt2x00dev,
+				const unsigned int word, u32 data)
+{
+	rt2x00pci_register_write(rt2x00dev, CSR_OFFSET(word), data);
+}
+
+static const struct rt2x00debug rt2500pci_rt2x00debug = {
+	.owner	= THIS_MODULE,
+	.csr	= {
+		.read		= rt2500pci_read_csr,
+		.write		= rt2500pci_write_csr,
+		.word_size	= sizeof(u32),
+		.word_count	= CSR_REG_SIZE / sizeof(u32),
+	},
+	.eeprom	= {
+		.read		= rt2x00_eeprom_read,
+		.write		= rt2x00_eeprom_write,
+		.word_size	= sizeof(u16),
+		.word_count	= EEPROM_SIZE / sizeof(u16),
+	},
+	.bbp	= {
+		.read		= rt2500pci_bbp_read,
+		.write		= rt2500pci_bbp_write,
+		.word_size	= sizeof(u8),
+		.word_count	= BBP_SIZE / sizeof(u8),
+	},
+	.rf	= {
+		.read		= rt2x00_rf_read,
+		.write		= rt2500pci_rf_write,
+		.word_size	= sizeof(u32),
+		.word_count	= RF_SIZE / sizeof(u32),
+	},
+};
+#endif /* CONFIG_RT2X00_LIB_DEBUGFS */
+
+#ifdef CONFIG_RT2500PCI_RFKILL
+static int rt2500pci_rfkill_poll(struct rt2x00_dev *rt2x00dev)
+{
+	u32 reg;
+
+	rt2x00pci_register_read(rt2x00dev, GPIOCSR, &reg);
+	return rt2x00_get_field32(reg, GPIOCSR_BIT0);
+}
+#else
+#define rt2500pci_rfkill_poll	NULL
+#endif /* CONFIG_RT2500PCI_RFKILL */
+
+/*
+ * Configuration handlers.
+ */
+static void rt2500pci_config_mac_addr(struct rt2x00_dev *rt2x00dev,
+				      __le32 *mac)
+{
+	rt2x00pci_register_multiwrite(rt2x00dev, CSR3, mac,
+				      (2 * sizeof(__le32)));
+}
+
+static void rt2500pci_config_bssid(struct rt2x00_dev *rt2x00dev,
+				   __le32 *bssid)
+{
+	rt2x00pci_register_multiwrite(rt2x00dev, CSR5, bssid,
+				      (2 * sizeof(__le32)));
+}
+
+static void rt2500pci_config_type(struct rt2x00_dev *rt2x00dev, const int type,
+				  const int tsf_sync)
+{
+	u32 reg;
+
+	rt2x00pci_register_write(rt2x00dev, CSR14, 0);
+
+	/*
+	 * Enable beacon config
+	 */
+	rt2x00pci_register_read(rt2x00dev, BCNCSR1, &reg);
+	rt2x00_set_field32(&reg, BCNCSR1_PRELOAD,
+			   PREAMBLE + get_duration(IEEE80211_HEADER, 20));
+	rt2x00_set_field32(&reg, BCNCSR1_BEACON_CWMIN,
+			   rt2x00lib_get_ring(rt2x00dev,
+					      IEEE80211_TX_QUEUE_BEACON)
+			   ->tx_params.cw_min);
+	rt2x00pci_register_write(rt2x00dev, BCNCSR1, reg);
+
+	/*
+	 * Enable synchronisation.
+	 */
+	rt2x00pci_register_read(rt2x00dev, CSR14, &reg);
+	rt2x00_set_field32(&reg, CSR14_TSF_COUNT, 1);
+	rt2x00_set_field32(&reg, CSR14_TBCN, 1);
+	rt2x00_set_field32(&reg, CSR14_BEACON_GEN, 0);
+	rt2x00_set_field32(&reg, CSR14_TSF_SYNC, tsf_sync);
+	rt2x00pci_register_write(rt2x00dev, CSR14, reg);
+}
+
+static void rt2500pci_config_preamble(struct rt2x00_dev *rt2x00dev,
+				      const int short_preamble,
+				      const int ack_timeout,
+				      const int ack_consume_time)
+{
+	int preamble_mask;
+	u32 reg;
+
+	/*
+	 * When short preamble is enabled, we should set bit 0x08
+	 */
+	preamble_mask = short_preamble << 3;
+
+	rt2x00pci_register_read(rt2x00dev, TXCSR1, &reg);
+	rt2x00_set_field32(&reg, TXCSR1_ACK_TIMEOUT, ack_timeout);
+	rt2x00_set_field32(&reg, TXCSR1_ACK_CONSUME_TIME, ack_consume_time);
+	rt2x00pci_register_write(rt2x00dev, TXCSR1, reg);
+
+	rt2x00pci_register_read(rt2x00dev, ARCSR2, &reg);
+	rt2x00_set_field32(&reg, ARCSR2_SIGNAL, 0x00 | preamble_mask);
+	rt2x00_set_field32(&reg, ARCSR2_SERVICE, 0x04);
+	rt2x00_set_field32(&reg, ARCSR2_LENGTH, get_duration(ACK_SIZE, 10));
+	rt2x00pci_register_write(rt2x00dev, ARCSR2, reg);
+
+	rt2x00pci_register_read(rt2x00dev, ARCSR3, &reg);
+	rt2x00_set_field32(&reg, ARCSR3_SIGNAL, 0x01 | preamble_mask);
+	rt2x00_set_field32(&reg, ARCSR3_SERVICE, 0x04);
+	rt2x00_set_field32(&reg, ARCSR2_LENGTH, get_duration(ACK_SIZE, 20));
+	rt2x00pci_register_write(rt2x00dev, ARCSR3, reg);
+
+	rt2x00pci_register_read(rt2x00dev, ARCSR4, &reg);
+	rt2x00_set_field32(&reg, ARCSR4_SIGNAL, 0x02 | preamble_mask);
+	rt2x00_set_field32(&reg, ARCSR4_SERVICE, 0x04);
+	rt2x00_set_field32(&reg, ARCSR2_LENGTH, get_duration(ACK_SIZE, 55));
+	rt2x00pci_register_write(rt2x00dev, ARCSR4, reg);
+
+	rt2x00pci_register_read(rt2x00dev, ARCSR5, &reg);
+	rt2x00_set_field32(&reg, ARCSR5_SIGNAL, 0x03 | preamble_mask);
+	rt2x00_set_field32(&reg, ARCSR5_SERVICE, 0x84);
+	rt2x00_set_field32(&reg, ARCSR2_LENGTH, get_duration(ACK_SIZE, 110));
+	rt2x00pci_register_write(rt2x00dev, ARCSR5, reg);
+}
+
+static void rt2500pci_config_phymode(struct rt2x00_dev *rt2x00dev,
+				     const int basic_rate_mask)
+{
+	rt2x00pci_register_write(rt2x00dev, ARCSR1, basic_rate_mask);
+}
+
+static void rt2500pci_config_channel(struct rt2x00_dev *rt2x00dev,
+				     struct rf_channel *rf, const int txpower)
+{
+	u8 r70;
+
+	/*
+	 * Set TXpower.
+	 */
+	rt2x00_set_field32(&rf->rf3, RF3_TXPOWER, TXPOWER_TO_DEV(txpower));
+
+	/*
+	 * Switch on tuning bits.
+	 * For RT2523 devices we do not need to update the R1 register.
+	 */
+	if (!rt2x00_rf(&rt2x00dev->chip, RF2523))
+		rt2x00_set_field32(&rf->rf1, RF1_TUNER, 1);
+	rt2x00_set_field32(&rf->rf3, RF3_TUNER, 1);
+
+	/*
+	 * For RT2525 we should first set the channel to half band higher.
+	 */
+	if (rt2x00_rf(&rt2x00dev->chip, RF2525)) {
+		static const u32 vals[] = {
+			0x00080cbe, 0x00080d02, 0x00080d06, 0x00080d0a,
+			0x00080d0e, 0x00080d12, 0x00080d16, 0x00080d1a,
+			0x00080d1e, 0x00080d22, 0x00080d26, 0x00080d2a,
+			0x00080d2e, 0x00080d3a
+		};
+
+		rt2500pci_rf_write(rt2x00dev, 1, rf->rf1);
+		rt2500pci_rf_write(rt2x00dev, 2, vals[rf->channel - 1]);
+		rt2500pci_rf_write(rt2x00dev, 3, rf->rf3);
+		if (rf->rf4)
+			rt2500pci_rf_write(rt2x00dev, 4, rf->rf4);
+	}
+
+	rt2500pci_rf_write(rt2x00dev, 1, rf->rf1);
+	rt2500pci_rf_write(rt2x00dev, 2, rf->rf2);
+	rt2500pci_rf_write(rt2x00dev, 3, rf->rf3);
+	if (rf->rf4)
+		rt2500pci_rf_write(rt2x00dev, 4, rf->rf4);
+
+	/*
+	 * Channel 14 requires the Japan filter bit to be set.
+	 */
+	r70 = 0x46;
+	rt2x00_set_field8(&r70, BBP_R70_JAPAN_FILTER, rf->channel == 14);
+	rt2500pci_bbp_write(rt2x00dev, 70, r70);
+
+	msleep(1);
+
+	/*
+	 * Switch off tuning bits.
+	 * For RT2523 devices we do not need to update the R1 register.
+	 */
+	if (!rt2x00_rf(&rt2x00dev->chip, RF2523)) {
+		rt2x00_set_field32(&rf->rf1, RF1_TUNER, 0);
+		rt2500pci_rf_write(rt2x00dev, 1, rf->rf1);
+	}
+
+	rt2x00_set_field32(&rf->rf3, RF3_TUNER, 0);
+	rt2500pci_rf_write(rt2x00dev, 3, rf->rf3);
+
+	/*
+	 * Clear false CRC during channel switch.
+	 */
+	rt2x00pci_register_read(rt2x00dev, CNT0, &rf->rf1);
+}
+
+static void rt2500pci_config_txpower(struct rt2x00_dev *rt2x00dev,
+				     const int txpower)
+{
+	u32 rf3;
+
+	rt2x00_rf_read(rt2x00dev, 3, &rf3);
+	rt2x00_set_field32(&rf3, RF3_TXPOWER, TXPOWER_TO_DEV(txpower));
+	rt2500pci_rf_write(rt2x00dev, 3, rf3);
+}
+
+static void rt2500pci_config_antenna(struct rt2x00_dev *rt2x00dev,
+				     const int antenna_tx, const int antenna_rx)
+{
+	u32 reg;
+	u8 r14;
+	u8 r2;
+
+	rt2x00pci_register_read(rt2x00dev, BBPCSR1, &reg);
+	rt2500pci_bbp_read(rt2x00dev, 14, &r14);
+	rt2500pci_bbp_read(rt2x00dev, 2, &r2);
+
+	/*
+	 * Configure the TX antenna.
+	 */
+	switch (antenna_tx) {
+	case ANTENNA_SW_DIVERSITY:
+	case ANTENNA_HW_DIVERSITY:
+		rt2x00_set_field8(&r2, BBP_R2_TX_ANTENNA, 2);
+		rt2x00_set_field32(&reg, BBPCSR1_CCK, 2);
+		rt2x00_set_field32(&reg, BBPCSR1_OFDM, 2);
+		break;
+	case ANTENNA_A:
+		rt2x00_set_field8(&r2, BBP_R2_TX_ANTENNA, 0);
+		rt2x00_set_field32(&reg, BBPCSR1_CCK, 0);
+		rt2x00_set_field32(&reg, BBPCSR1_OFDM, 0);
+		break;
+	case ANTENNA_B:
+		rt2x00_set_field8(&r2, BBP_R2_TX_ANTENNA, 2);
+		rt2x00_set_field32(&reg, BBPCSR1_CCK, 2);
+		rt2x00_set_field32(&reg, BBPCSR1_OFDM, 2);
+		break;
+	}
+
+	/*
+	 * Configure the RX antenna.
+	 */
+	switch (antenna_rx) {
+	case ANTENNA_SW_DIVERSITY:
+	case ANTENNA_HW_DIVERSITY:
+		rt2x00_set_field8(&r14, BBP_R14_RX_ANTENNA, 2);
+		break;
+	case ANTENNA_A:
+		rt2x00_set_field8(&r14, BBP_R14_RX_ANTENNA, 0);
+		break;
+	case ANTENNA_B:
+		rt2x00_set_field8(&r14, BBP_R14_RX_ANTENNA, 2);
+		break;
+	}
+
+	/*
+	 * RT2525E and RT5222 need to flip TX I/Q
+	 */
+	if (rt2x00_rf(&rt2x00dev->chip, RF2525E) ||
+	    rt2x00_rf(&rt2x00dev->chip, RF5222)) {
+		rt2x00_set_field8(&r2, BBP_R2_TX_IQ_FLIP, 1);
+		rt2x00_set_field32(&reg, BBPCSR1_CCK_FLIP, 1);
+		rt2x00_set_field32(&reg, BBPCSR1_OFDM_FLIP, 1);
+
+		/*
+		 * RT2525E does not need RX I/Q Flip.
+		 */
+		if (rt2x00_rf(&rt2x00dev->chip, RF2525E))
+			rt2x00_set_field8(&r14, BBP_R14_RX_IQ_FLIP, 0);
+	} else {
+		rt2x00_set_field32(&reg, BBPCSR1_CCK_FLIP, 0);
+		rt2x00_set_field32(&reg, BBPCSR1_OFDM_FLIP, 0);
+	}
+
+	rt2x00pci_register_write(rt2x00dev, BBPCSR1, reg);
+	rt2500pci_bbp_write(rt2x00dev, 14, r14);
+	rt2500pci_bbp_write(rt2x00dev, 2, r2);
+}
+
+static void rt2500pci_config_duration(struct rt2x00_dev *rt2x00dev,
+				      struct rt2x00lib_conf *libconf)
+{
+	u32 reg;
+
+	rt2x00pci_register_read(rt2x00dev, CSR11, &reg);
+	rt2x00_set_field32(&reg, CSR11_SLOT_TIME, libconf->slot_time);
+	rt2x00pci_register_write(rt2x00dev, CSR11, reg);
+
+	rt2x00pci_register_read(rt2x00dev, CSR18, &reg);
+	rt2x00_set_field32(&reg, CSR18_SIFS, libconf->sifs);
+	rt2x00_set_field32(&reg, CSR18_PIFS, libconf->pifs);
+	rt2x00pci_register_write(rt2x00dev, CSR18, reg);
+
+	rt2x00pci_register_read(rt2x00dev, CSR19, &reg);
+	rt2x00_set_field32(&reg, CSR19_DIFS, libconf->difs);
+	rt2x00_set_field32(&reg, CSR19_EIFS, libconf->eifs);
+	rt2x00pci_register_write(rt2x00dev, CSR19, reg);
+
+	rt2x00pci_register_read(rt2x00dev, TXCSR1, &reg);
+	rt2x00_set_field32(&reg, TXCSR1_TSF_OFFSET, IEEE80211_HEADER);
+	rt2x00_set_field32(&reg, TXCSR1_AUTORESPONDER, 1);
+	rt2x00pci_register_write(rt2x00dev, TXCSR1, reg);
+
+	rt2x00pci_register_read(rt2x00dev, CSR12, &reg);
+	rt2x00_set_field32(&reg, CSR12_BEACON_INTERVAL,
+			   libconf->conf->beacon_int * 16);
+	rt2x00_set_field32(&reg, CSR12_CFP_MAX_DURATION,
+			   libconf->conf->beacon_int * 16);
+	rt2x00pci_register_write(rt2x00dev, CSR12, reg);
+}
+
+static void rt2500pci_config(struct rt2x00_dev *rt2x00dev,
+			     const unsigned int flags,
+			     struct rt2x00lib_conf *libconf)
+{
+	if (flags & CONFIG_UPDATE_PHYMODE)
+		rt2500pci_config_phymode(rt2x00dev, libconf->basic_rates);
+	if (flags & CONFIG_UPDATE_CHANNEL)
+		rt2500pci_config_channel(rt2x00dev, &libconf->rf,
+					 libconf->conf->power_level);
+	if ((flags & CONFIG_UPDATE_TXPOWER) && !(flags & CONFIG_UPDATE_CHANNEL))
+		rt2500pci_config_txpower(rt2x00dev,
+					 libconf->conf->power_level);
+	if (flags & CONFIG_UPDATE_ANTENNA)
+		rt2500pci_config_antenna(rt2x00dev,
+					 libconf->conf->antenna_sel_tx,
+					 libconf->conf->antenna_sel_rx);
+	if (flags & (CONFIG_UPDATE_SLOT_TIME | CONFIG_UPDATE_BEACON_INT))
+		rt2500pci_config_duration(rt2x00dev, libconf);
+}
+
+/*
+ * LED functions.
+ */
+static void rt2500pci_enable_led(struct rt2x00_dev *rt2x00dev)
+{
+	u32 reg;
+
+	rt2x00pci_register_read(rt2x00dev, LEDCSR, &reg);
+
+	rt2x00_set_field32(&reg, LEDCSR_ON_PERIOD, 70);
+	rt2x00_set_field32(&reg, LEDCSR_OFF_PERIOD, 30);
+
+	if (rt2x00dev->led_mode == LED_MODE_TXRX_ACTIVITY) {
+		rt2x00_set_field32(&reg, LEDCSR_LINK, 1);
+		rt2x00_set_field32(&reg, LEDCSR_ACTIVITY, 0);
+	} else if (rt2x00dev->led_mode == LED_MODE_ASUS) {
+		rt2x00_set_field32(&reg, LEDCSR_LINK, 0);
+		rt2x00_set_field32(&reg, LEDCSR_ACTIVITY, 1);
+	} else {
+		rt2x00_set_field32(&reg, LEDCSR_LINK, 1);
+		rt2x00_set_field32(&reg, LEDCSR_ACTIVITY, 1);
+	}
+
+	rt2x00pci_register_write(rt2x00dev, LEDCSR, reg);
+}
+
+static void rt2500pci_disable_led(struct rt2x00_dev *rt2x00dev)
+{
+	u32 reg;
+
+	rt2x00pci_register_read(rt2x00dev, LEDCSR, &reg);
+	rt2x00_set_field32(&reg, LEDCSR_LINK, 0);
+	rt2x00_set_field32(&reg, LEDCSR_ACTIVITY, 0);
+	rt2x00pci_register_write(rt2x00dev, LEDCSR, reg);
+}
+
+/*
+ * Link tuning
+ */
+static void rt2500pci_link_stats(struct rt2x00_dev *rt2x00dev)
+{
+	u32 reg;
+
+	/*
+	 * Update FCS error count from register.
+	 */
+	rt2x00pci_register_read(rt2x00dev, CNT0, &reg);
+	rt2x00dev->link.rx_failed = rt2x00_get_field32(reg, CNT0_FCS_ERROR);
+
+	/*
+	 * Update False CCA count from register.
+	 */
+	rt2x00pci_register_read(rt2x00dev, CNT3, &reg);
+	rt2x00dev->link.false_cca = rt2x00_get_field32(reg, CNT3_FALSE_CCA);
+}
+
+static void rt2500pci_reset_tuner(struct rt2x00_dev *rt2x00dev)
+{
+	rt2500pci_bbp_write(rt2x00dev, 17, 0x48);
+	rt2x00dev->link.vgc_level = 0x48;
+}
+
+static void rt2500pci_link_tuner(struct rt2x00_dev *rt2x00dev)
+{
+	int rssi = rt2x00_get_link_rssi(&rt2x00dev->link);
+	u8 r17;
+
+	/*
+	 * To prevent collisions with MAC ASIC on chipsets
+	 * up to version C the link tuning should halt after 20
+	 * seconds.
+	 */
+	if (rt2x00_get_rev(&rt2x00dev->chip) < RT2560_VERSION_D &&
+	    rt2x00dev->link.count > 20)
+		return;
+
+	rt2500pci_bbp_read(rt2x00dev, 17, &r17);
+
+	/*
+	 * Chipset versions C and lower should directly continue
+	 * to the dynamic CCA tuning.
+	 */
+	if (rt2x00_get_rev(&rt2x00dev->chip) < RT2560_VERSION_D)
+		goto dynamic_cca_tune;
+
+	/*
+	 * A too low RSSI will cause too much false CCA which will
+	 * then corrupt the R17 tuning. To remidy this the tuning should
+	 * be stopped (While making sure the R17 value will not exceed limits)
+	 */
+	if (rssi < -80 && rt2x00dev->link.count > 20) {
+		if (r17 >= 0x41) {
+			r17 = rt2x00dev->link.vgc_level;
+			rt2500pci_bbp_write(rt2x00dev, 17, r17);
+		}
+		return;
+	}
+
+	/*
+	 * Special big-R17 for short distance
+	 */
+	if (rssi >= -58) {
+		if (r17 != 0x50)
+			rt2500pci_bbp_write(rt2x00dev, 17, 0x50);
+		return;
+	}
+
+	/*
+	 * Special mid-R17 for middle distance
+	 */
+	if (rssi >= -74) {
+		if (r17 != 0x41)
+			rt2500pci_bbp_write(rt2x00dev, 17, 0x41);
+		return;
+	}
+
+	/*
+	 * Leave short or middle distance condition, restore r17
+	 * to the dynamic tuning range.
+	 */
+	if (r17 >= 0x41) {
+		rt2500pci_bbp_write(rt2x00dev, 17, rt2x00dev->link.vgc_level);
+		return;
+	}
+
+dynamic_cca_tune:
+
+	/*
+	 * R17 is inside the dynamic tuning range,
+	 * start tuning the link based on the false cca counter.
+	 */
+	if (rt2x00dev->link.false_cca > 512 && r17 < 0x40) {
+		rt2500pci_bbp_write(rt2x00dev, 17, ++r17);
+		rt2x00dev->link.vgc_level = r17;
+	} else if (rt2x00dev->link.false_cca < 100 && r17 > 0x32) {
+		rt2500pci_bbp_write(rt2x00dev, 17, --r17);
+		rt2x00dev->link.vgc_level = r17;
+	}
+}
+
+/*
+ * Initialization functions.
+ */
+static void rt2500pci_init_rxring(struct rt2x00_dev *rt2x00dev)
+{
+	struct data_ring *ring = rt2x00dev->rx;
+	struct data_desc *rxd;
+	unsigned int i;
+	u32 word;
+
+	memset(ring->data_addr, 0x00, rt2x00_get_ring_size(ring));
+
+	for (i = 0; i < ring->stats.limit; i++) {
+		rxd = ring->entry[i].priv;
+
+		rt2x00_desc_read(rxd, 1, &word);
+		rt2x00_set_field32(&word, RXD_W1_BUFFER_ADDRESS,
+				   ring->entry[i].data_dma);
+		rt2x00_desc_write(rxd, 1, word);
+
+		rt2x00_desc_read(rxd, 0, &word);
+		rt2x00_set_field32(&word, RXD_W0_OWNER_NIC, 1);
+		rt2x00_desc_write(rxd, 0, word);
+	}
+
+	rt2x00_ring_index_clear(rt2x00dev->rx);
+}
+
+static void rt2500pci_init_txring(struct rt2x00_dev *rt2x00dev, const int queue)
+{
+	struct data_ring *ring = rt2x00lib_get_ring(rt2x00dev, queue);
+	struct data_desc *txd;
+	unsigned int i;
+	u32 word;
+
+	memset(ring->data_addr, 0x00, rt2x00_get_ring_size(ring));
+
+	for (i = 0; i < ring->stats.limit; i++) {
+		txd = ring->entry[i].priv;
+
+		rt2x00_desc_read(txd, 1, &word);
+		rt2x00_set_field32(&word, TXD_W1_BUFFER_ADDRESS,
+				   ring->entry[i].data_dma);
+		rt2x00_desc_write(txd, 1, word);
+
+		rt2x00_desc_read(txd, 0, &word);
+		rt2x00_set_field32(&word, TXD_W0_VALID, 0);
+		rt2x00_set_field32(&word, TXD_W0_OWNER_NIC, 0);
+		rt2x00_desc_write(txd, 0, word);
+	}
+
+	rt2x00_ring_index_clear(ring);
+}
+
+static int rt2500pci_init_rings(struct rt2x00_dev *rt2x00dev)
+{
+	u32 reg;
+
+	/*
+	 * Initialize rings.
+	 */
+	rt2500pci_init_rxring(rt2x00dev);
+	rt2500pci_init_txring(rt2x00dev, IEEE80211_TX_QUEUE_DATA0);
+	rt2500pci_init_txring(rt2x00dev, IEEE80211_TX_QUEUE_DATA1);
+	rt2500pci_init_txring(rt2x00dev, IEEE80211_TX_QUEUE_AFTER_BEACON);
+	rt2500pci_init_txring(rt2x00dev, IEEE80211_TX_QUEUE_BEACON);
+
+	/*
+	 * Initialize registers.
+	 */
+	rt2x00pci_register_read(rt2x00dev, TXCSR2, &reg);
+	rt2x00_set_field32(&reg, TXCSR2_TXD_SIZE,
+			   rt2x00dev->tx[IEEE80211_TX_QUEUE_DATA0].desc_size);
+	rt2x00_set_field32(&reg, TXCSR2_NUM_TXD,
+			   rt2x00dev->tx[IEEE80211_TX_QUEUE_DATA1].stats.limit);
+	rt2x00_set_field32(&reg, TXCSR2_NUM_ATIM,
+			   rt2x00dev->bcn[1].stats.limit);
+	rt2x00_set_field32(&reg, TXCSR2_NUM_PRIO,
+			   rt2x00dev->tx[IEEE80211_TX_QUEUE_DATA0].stats.limit);
+	rt2x00pci_register_write(rt2x00dev, TXCSR2, reg);
+
+	rt2x00pci_register_read(rt2x00dev, TXCSR3, &reg);
+	rt2x00_set_field32(&reg, TXCSR3_TX_RING_REGISTER,
+			   rt2x00dev->tx[IEEE80211_TX_QUEUE_DATA1].data_dma);
+	rt2x00pci_register_write(rt2x00dev, TXCSR3, reg);
+
+	rt2x00pci_register_read(rt2x00dev, TXCSR5, &reg);
+	rt2x00_set_field32(&reg, TXCSR5_PRIO_RING_REGISTER,
+			   rt2x00dev->tx[IEEE80211_TX_QUEUE_DATA0].data_dma);
+	rt2x00pci_register_write(rt2x00dev, TXCSR5, reg);
+
+	rt2x00pci_register_read(rt2x00dev, TXCSR4, &reg);
+	rt2x00_set_field32(&reg, TXCSR4_ATIM_RING_REGISTER,
+			   rt2x00dev->bcn[1].data_dma);
+	rt2x00pci_register_write(rt2x00dev, TXCSR4, reg);
+
+	rt2x00pci_register_read(rt2x00dev, TXCSR6, &reg);
+	rt2x00_set_field32(&reg, TXCSR6_BEACON_RING_REGISTER,
+			   rt2x00dev->bcn[0].data_dma);
+	rt2x00pci_register_write(rt2x00dev, TXCSR6, reg);
+
+	rt2x00pci_register_read(rt2x00dev, RXCSR1, &reg);
+	rt2x00_set_field32(&reg, RXCSR1_RXD_SIZE, rt2x00dev->rx->desc_size);
+	rt2x00_set_field32(&reg, RXCSR1_NUM_RXD, rt2x00dev->rx->stats.limit);
+	rt2x00pci_register_write(rt2x00dev, RXCSR1, reg);
+
+	rt2x00pci_register_read(rt2x00dev, RXCSR2, &reg);
+	rt2x00_set_field32(&reg, RXCSR2_RX_RING_REGISTER,
+			   rt2x00dev->rx->data_dma);
+	rt2x00pci_register_write(rt2x00dev, RXCSR2, reg);
+
+	return 0;
+}
+
+static int rt2500pci_init_registers(struct rt2x00_dev *rt2x00dev)
+{
+	u32 reg;
+
+	rt2x00pci_register_write(rt2x00dev, PSCSR0, 0x00020002);
+	rt2x00pci_register_write(rt2x00dev, PSCSR1, 0x00000002);
+	rt2x00pci_register_write(rt2x00dev, PSCSR2, 0x00020002);
+	rt2x00pci_register_write(rt2x00dev, PSCSR3, 0x00000002);
+
+	rt2x00pci_register_read(rt2x00dev, TIMECSR, &reg);
+	rt2x00_set_field32(&reg, TIMECSR_US_COUNT, 33);
+	rt2x00_set_field32(&reg, TIMECSR_US_64_COUNT, 63);
+	rt2x00_set_field32(&reg, TIMECSR_BEACON_EXPECT, 0);
+	rt2x00pci_register_write(rt2x00dev, TIMECSR, reg);
+
+	rt2x00pci_register_read(rt2x00dev, CSR9, &reg);
+	rt2x00_set_field32(&reg, CSR9_MAX_FRAME_UNIT,
+			   rt2x00dev->rx->data_size / 128);
+	rt2x00pci_register_write(rt2x00dev, CSR9, reg);
+
+	/*
+	 * Always use CWmin and CWmax set in descriptor.
+	 */
+	rt2x00pci_register_read(rt2x00dev, CSR11, &reg);
+	rt2x00_set_field32(&reg, CSR11_CW_SELECT, 0);
+	rt2x00pci_register_write(rt2x00dev, CSR11, reg);
+
+	rt2x00pci_register_write(rt2x00dev, CNT3, 0);
+
+	rt2x00pci_register_read(rt2x00dev, TXCSR8, &reg);
+	rt2x00_set_field32(&reg, TXCSR8_BBP_ID0, 10);
+	rt2x00_set_field32(&reg, TXCSR8_BBP_ID0_VALID, 1);
+	rt2x00_set_field32(&reg, TXCSR8_BBP_ID1, 11);
+	rt2x00_set_field32(&reg, TXCSR8_BBP_ID1_VALID, 1);
+	rt2x00_set_field32(&reg, TXCSR8_BBP_ID2, 13);
+	rt2x00_set_field32(&reg, TXCSR8_BBP_ID2_VALID, 1);
+	rt2x00_set_field32(&reg, TXCSR8_BBP_ID3, 12);
+	rt2x00_set_field32(&reg, TXCSR8_BBP_ID3_VALID, 1);
+	rt2x00pci_register_write(rt2x00dev, TXCSR8, reg);
+
+	rt2x00pci_register_read(rt2x00dev, ARTCSR0, &reg);
+	rt2x00_set_field32(&reg, ARTCSR0_ACK_CTS_1MBS, 112);
+	rt2x00_set_field32(&reg, ARTCSR0_ACK_CTS_2MBS, 56);
+	rt2x00_set_field32(&reg, ARTCSR0_ACK_CTS_5_5MBS, 20);
+	rt2x00_set_field32(&reg, ARTCSR0_ACK_CTS_11MBS, 10);
+	rt2x00pci_register_write(rt2x00dev, ARTCSR0, reg);
+
+	rt2x00pci_register_read(rt2x00dev, ARTCSR1, &reg);
+	rt2x00_set_field32(&reg, ARTCSR1_ACK_CTS_6MBS, 45);
+	rt2x00_set_field32(&reg, ARTCSR1_ACK_CTS_9MBS, 37);
+	rt2x00_set_field32(&reg, ARTCSR1_ACK_CTS_12MBS, 33);
+	rt2x00_set_field32(&reg, ARTCSR1_ACK_CTS_18MBS, 29);
+	rt2x00pci_register_write(rt2x00dev, ARTCSR1, reg);
+
+	rt2x00pci_register_read(rt2x00dev, ARTCSR2, &reg);
+	rt2x00_set_field32(&reg, ARTCSR2_ACK_CTS_24MBS, 29);
+	rt2x00_set_field32(&reg, ARTCSR2_ACK_CTS_36MBS, 25);
+	rt2x00_set_field32(&reg, ARTCSR2_ACK_CTS_48MBS, 25);
+	rt2x00_set_field32(&reg, ARTCSR2_ACK_CTS_54MBS, 25);
+	rt2x00pci_register_write(rt2x00dev, ARTCSR2, reg);
+
+	rt2x00pci_register_read(rt2x00dev, RXCSR3, &reg);
+	rt2x00_set_field32(&reg, RXCSR3_BBP_ID0, 47); /* CCK Signal */
+	rt2x00_set_field32(&reg, RXCSR3_BBP_ID0_VALID, 1);
+	rt2x00_set_field32(&reg, RXCSR3_BBP_ID1, 51); /* Rssi */
+	rt2x00_set_field32(&reg, RXCSR3_BBP_ID1_VALID, 1);
+	rt2x00_set_field32(&reg, RXCSR3_BBP_ID2, 42); /* OFDM Rate */
+	rt2x00_set_field32(&reg, RXCSR3_BBP_ID2_VALID, 1);
+	rt2x00_set_field32(&reg, RXCSR3_BBP_ID3, 51); /* RSSI */
+	rt2x00_set_field32(&reg, RXCSR3_BBP_ID3_VALID, 1);
+	rt2x00pci_register_write(rt2x00dev, RXCSR3, reg);
+
+	rt2x00pci_register_read(rt2x00dev, PCICSR, &reg);
+	rt2x00_set_field32(&reg, PCICSR_BIG_ENDIAN, 0);
+	rt2x00_set_field32(&reg, PCICSR_RX_TRESHOLD, 0);
+	rt2x00_set_field32(&reg, PCICSR_TX_TRESHOLD, 3);
+	rt2x00_set_field32(&reg, PCICSR_BURST_LENTH, 1);
+	rt2x00_set_field32(&reg, PCICSR_ENABLE_CLK, 1);
+	rt2x00_set_field32(&reg, PCICSR_READ_MULTIPLE, 1);
+	rt2x00_set_field32(&reg, PCICSR_WRITE_INVALID, 1);
+	rt2x00pci_register_write(rt2x00dev, PCICSR, reg);
+
+	rt2x00pci_register_write(rt2x00dev, PWRCSR0, 0x3f3b3100);
+
+	rt2x00pci_register_write(rt2x00dev, GPIOCSR, 0x0000ff00);
+	rt2x00pci_register_write(rt2x00dev, TESTCSR, 0x000000f0);
+
+	if (rt2x00dev->ops->lib->set_device_state(rt2x00dev, STATE_AWAKE))
+		return -EBUSY;
+
+	rt2x00pci_register_write(rt2x00dev, MACCSR0, 0x00213223);
+	rt2x00pci_register_write(rt2x00dev, MACCSR1, 0x00235518);
+
+	rt2x00pci_register_read(rt2x00dev, MACCSR2, &reg);
+	rt2x00_set_field32(&reg, MACCSR2_DELAY, 64);
+	rt2x00pci_register_write(rt2x00dev, MACCSR2, reg);
+
+	rt2x00pci_register_read(rt2x00dev, RALINKCSR, &reg);
+	rt2x00_set_field32(&reg, RALINKCSR_AR_BBP_DATA0, 17);
+	rt2x00_set_field32(&reg, RALINKCSR_AR_BBP_ID0, 26);
+	rt2x00_set_field32(&reg, RALINKCSR_AR_BBP_VALID0, 1);
+	rt2x00_set_field32(&reg, RALINKCSR_AR_BBP_DATA1, 0);
+	rt2x00_set_field32(&reg, RALINKCSR_AR_BBP_ID1, 26);
+	rt2x00_set_field32(&reg, RALINKCSR_AR_BBP_VALID1, 1);
+	rt2x00pci_register_write(rt2x00dev, RALINKCSR, reg);
+
+	rt2x00pci_register_write(rt2x00dev, BBPCSR1, 0x82188200);
+
+	rt2x00pci_register_write(rt2x00dev, TXACKCSR0, 0x00000020);
+
+	rt2x00pci_register_read(rt2x00dev, CSR1, &reg);
+	rt2x00_set_field32(&reg, CSR1_SOFT_RESET, 1);
+	rt2x00_set_field32(&reg, CSR1_BBP_RESET, 0);
+	rt2x00_set_field32(&reg, CSR1_HOST_READY, 0);
+	rt2x00pci_register_write(rt2x00dev, CSR1, reg);
+
+	rt2x00pci_register_read(rt2x00dev, CSR1, &reg);
+	rt2x00_set_field32(&reg, CSR1_SOFT_RESET, 0);
+	rt2x00_set_field32(&reg, CSR1_HOST_READY, 1);
+	rt2x00pci_register_write(rt2x00dev, CSR1, reg);
+
+	/*
+	 * We must clear the FCS and FIFO error count.
+	 * These registers are cleared on read,
+	 * so we may pass a useless variable to store the value.
+	 */
+	rt2x00pci_register_read(rt2x00dev, CNT0, &reg);
+	rt2x00pci_register_read(rt2x00dev, CNT4, &reg);
+
+	return 0;
+}
+
+static int rt2500pci_init_bbp(struct rt2x00_dev *rt2x00dev)
+{
+	unsigned int i;
+	u16 eeprom;
+	u8 reg_id;
+	u8 value;
+
+	for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
+		rt2500pci_bbp_read(rt2x00dev, 0, &value);
+		if ((value != 0xff) && (value != 0x00))
+			goto continue_csr_init;
+		NOTICE(rt2x00dev, "Waiting for BBP register.\n");
+		udelay(REGISTER_BUSY_DELAY);
+	}
+
+	ERROR(rt2x00dev, "BBP register access failed, aborting.\n");
+	return -EACCES;
+
+continue_csr_init:
+	rt2500pci_bbp_write(rt2x00dev, 3, 0x02);
+	rt2500pci_bbp_write(rt2x00dev, 4, 0x19);
+	rt2500pci_bbp_write(rt2x00dev, 14, 0x1c);
+	rt2500pci_bbp_write(rt2x00dev, 15, 0x30);
+	rt2500pci_bbp_write(rt2x00dev, 16, 0xac);
+	rt2500pci_bbp_write(rt2x00dev, 18, 0x18);
+	rt2500pci_bbp_write(rt2x00dev, 19, 0xff);
+	rt2500pci_bbp_write(rt2x00dev, 20, 0x1e);
+	rt2500pci_bbp_write(rt2x00dev, 21, 0x08);
+	rt2500pci_bbp_write(rt2x00dev, 22, 0x08);
+	rt2500pci_bbp_write(rt2x00dev, 23, 0x08);
+	rt2500pci_bbp_write(rt2x00dev, 24, 0x70);
+	rt2500pci_bbp_write(rt2x00dev, 25, 0x40);
+	rt2500pci_bbp_write(rt2x00dev, 26, 0x08);
+	rt2500pci_bbp_write(rt2x00dev, 27, 0x23);
+	rt2500pci_bbp_write(rt2x00dev, 30, 0x10);
+	rt2500pci_bbp_write(rt2x00dev, 31, 0x2b);
+	rt2500pci_bbp_write(rt2x00dev, 32, 0xb9);
+	rt2500pci_bbp_write(rt2x00dev, 34, 0x12);
+	rt2500pci_bbp_write(rt2x00dev, 35, 0x50);
+	rt2500pci_bbp_write(rt2x00dev, 39, 0xc4);
+	rt2500pci_bbp_write(rt2x00dev, 40, 0x02);
+	rt2500pci_bbp_write(rt2x00dev, 41, 0x60);
+	rt2500pci_bbp_write(rt2x00dev, 53, 0x10);
+	rt2500pci_bbp_write(rt2x00dev, 54, 0x18);
+	rt2500pci_bbp_write(rt2x00dev, 56, 0x08);
+	rt2500pci_bbp_write(rt2x00dev, 57, 0x10);
+	rt2500pci_bbp_write(rt2x00dev, 58, 0x08);
+	rt2500pci_bbp_write(rt2x00dev, 61, 0x6d);
+	rt2500pci_bbp_write(rt2x00dev, 62, 0x10);
+
+	DEBUG(rt2x00dev, "Start initialization from EEPROM...\n");
+	for (i = 0; i < EEPROM_BBP_SIZE; i++) {
+		rt2x00_eeprom_read(rt2x00dev, EEPROM_BBP_START + i, &eeprom);
+
+		if (eeprom != 0xffff && eeprom != 0x0000) {
+			reg_id = rt2x00_get_field16(eeprom, EEPROM_BBP_REG_ID);
+			value = rt2x00_get_field16(eeprom, EEPROM_BBP_VALUE);
+			DEBUG(rt2x00dev, "BBP: 0x%02x, value: 0x%02x.\n",
+			      reg_id, value);
+			rt2500pci_bbp_write(rt2x00dev, reg_id, value);
+		}
+	}
+	DEBUG(rt2x00dev, "...End initialization from EEPROM.\n");
+
+	return 0;
+}
+
+/*
+ * Device state switch handlers.
+ */
+static void rt2500pci_toggle_rx(struct rt2x00_dev *rt2x00dev,
+				enum dev_state state)
+{
+	u32 reg;
+
+	rt2x00pci_register_read(rt2x00dev, RXCSR0, &reg);
+	rt2x00_set_field32(&reg, RXCSR0_DISABLE_RX,
+			   state == STATE_RADIO_RX_OFF);
+	rt2x00pci_register_write(rt2x00dev, RXCSR0, reg);
+}
+
+static void rt2500pci_toggle_irq(struct rt2x00_dev *rt2x00dev,
+				 enum dev_state state)
+{
+	int mask = (state == STATE_RADIO_IRQ_OFF);
+	u32 reg;
+
+	/*
+	 * When interrupts are being enabled, the interrupt registers
+	 * should clear the register to assure a clean state.
+	 */
+	if (state == STATE_RADIO_IRQ_ON) {
+		rt2x00pci_register_read(rt2x00dev, CSR7, &reg);
+		rt2x00pci_register_write(rt2x00dev, CSR7, reg);
+	}
+
+	/*
+	 * Only toggle the interrupts bits we are going to use.
+	 * Non-checked interrupt bits are disabled by default.
+	 */
+	rt2x00pci_register_read(rt2x00dev, CSR8, &reg);
+	rt2x00_set_field32(&reg, CSR8_TBCN_EXPIRE, mask);
+	rt2x00_set_field32(&reg, CSR8_TXDONE_TXRING, mask);
+	rt2x00_set_field32(&reg, CSR8_TXDONE_ATIMRING, mask);
+	rt2x00_set_field32(&reg, CSR8_TXDONE_PRIORING, mask);
+	rt2x00_set_field32(&reg, CSR8_RXDONE, mask);
+	rt2x00pci_register_write(rt2x00dev, CSR8, reg);
+}
+
+static int rt2500pci_enable_radio(struct rt2x00_dev *rt2x00dev)
+{
+	/*
+	 * Initialize all registers.
+	 */
+	if (rt2500pci_init_rings(rt2x00dev) ||
+	    rt2500pci_init_registers(rt2x00dev) ||
+	    rt2500pci_init_bbp(rt2x00dev)) {
+		ERROR(rt2x00dev, "Register initialization failed.\n");
+		return -EIO;
+	}
+
+	/*
+	 * Enable interrupts.
+	 */
+	rt2500pci_toggle_irq(rt2x00dev, STATE_RADIO_IRQ_ON);
+
+	/*
+	 * Enable LED
+	 */
+	rt2500pci_enable_led(rt2x00dev);
+
+	return 0;
+}
+
+static void rt2500pci_disable_radio(struct rt2x00_dev *rt2x00dev)
+{
+	u32 reg;
+
+	/*
+	 * Disable LED
+	 */
+	rt2500pci_disable_led(rt2x00dev);
+
+	rt2x00pci_register_write(rt2x00dev, PWRCSR0, 0);
+
+	/*
+	 * Disable synchronisation.
+	 */
+	rt2x00pci_register_write(rt2x00dev, CSR14, 0);
+
+	/*
+	 * Cancel RX and TX.
+	 */
+	rt2x00pci_register_read(rt2x00dev, TXCSR0, &reg);
+	rt2x00_set_field32(&reg, TXCSR0_ABORT, 1);
+	rt2x00pci_register_write(rt2x00dev, TXCSR0, reg);
+
+	/*
+	 * Disable interrupts.
+	 */
+	rt2500pci_toggle_irq(rt2x00dev, STATE_RADIO_IRQ_OFF);
+}
+
+static int rt2500pci_set_state(struct rt2x00_dev *rt2x00dev,
+			       enum dev_state state)
+{
+	u32 reg;
+	unsigned int i;
+	char put_to_sleep;
+	char bbp_state;
+	char rf_state;
+
+	put_to_sleep = (state != STATE_AWAKE);
+
+	rt2x00pci_register_read(rt2x00dev, PWRCSR1, &reg);
+	rt2x00_set_field32(&reg, PWRCSR1_SET_STATE, 1);
+	rt2x00_set_field32(&reg, PWRCSR1_BBP_DESIRE_STATE, state);
+	rt2x00_set_field32(&reg, PWRCSR1_RF_DESIRE_STATE, state);
+	rt2x00_set_field32(&reg, PWRCSR1_PUT_TO_SLEEP, put_to_sleep);
+	rt2x00pci_register_write(rt2x00dev, PWRCSR1, reg);
+
+	/*
+	 * Device is not guaranteed to be in the requested state yet.
+	 * We must wait until the register indicates that the
+	 * device has entered the correct state.
+	 */
+	for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
+		rt2x00pci_register_read(rt2x00dev, PWRCSR1, &reg);
+		bbp_state = rt2x00_get_field32(reg, PWRCSR1_BBP_CURR_STATE);
+		rf_state = rt2x00_get_field32(reg, PWRCSR1_RF_CURR_STATE);
+		if (bbp_state == state && rf_state == state)
+			return 0;
+		msleep(10);
+	}
+
+	NOTICE(rt2x00dev, "Device failed to enter state %d, "
+	       "current device state: bbp %d and rf %d.\n",
+	       state, bbp_state, rf_state);
+
+	return -EBUSY;
+}
+
+static int rt2500pci_set_device_state(struct rt2x00_dev *rt2x00dev,
+				      enum dev_state state)
+{
+	int retval = 0;
+
+	switch (state) {
+	case STATE_RADIO_ON:
+		retval = rt2500pci_enable_radio(rt2x00dev);
+		break;
+	case STATE_RADIO_OFF:
+		rt2500pci_disable_radio(rt2x00dev);
+		break;
+	case STATE_RADIO_RX_ON:
+	case STATE_RADIO_RX_OFF:
+		rt2500pci_toggle_rx(rt2x00dev, state);
+		break;
+	case STATE_DEEP_SLEEP:
+	case STATE_SLEEP:
+	case STATE_STANDBY:
+	case STATE_AWAKE:
+		retval = rt2500pci_set_state(rt2x00dev, state);
+		break;
+	default:
+		retval = -ENOTSUPP;
+		break;
+	}
+
+	return retval;
+}
+
+/*
+ * TX descriptor initialization
+ */
+static void rt2500pci_write_tx_desc(struct rt2x00_dev *rt2x00dev,
+				    struct data_desc *txd,
+				    struct txdata_entry_desc *desc,
+				    struct ieee80211_hdr *ieee80211hdr,
+				    unsigned int length,
+				    struct ieee80211_tx_control *control)
+{
+	u32 word;
+
+	/*
+	 * Start writing the descriptor words.
+	 */
+	rt2x00_desc_read(txd, 2, &word);
+	rt2x00_set_field32(&word, TXD_W2_IV_OFFSET, IEEE80211_HEADER);
+	rt2x00_set_field32(&word, TXD_W2_AIFS, desc->aifs);
+	rt2x00_set_field32(&word, TXD_W2_CWMIN, desc->cw_min);
+	rt2x00_set_field32(&word, TXD_W2_CWMAX, desc->cw_max);
+	rt2x00_desc_write(txd, 2, word);
+
+	rt2x00_desc_read(txd, 3, &word);
+	rt2x00_set_field32(&word, TXD_W3_PLCP_SIGNAL, desc->signal);
+	rt2x00_set_field32(&word, TXD_W3_PLCP_SERVICE, desc->service);
+	rt2x00_set_field32(&word, TXD_W3_PLCP_LENGTH_LOW, desc->length_low);
+	rt2x00_set_field32(&word, TXD_W3_PLCP_LENGTH_HIGH, desc->length_high);
+	rt2x00_desc_write(txd, 3, word);
+
+	rt2x00_desc_read(txd, 10, &word);
+	rt2x00_set_field32(&word, TXD_W10_RTS,
+			   test_bit(ENTRY_TXD_RTS_FRAME, &desc->flags));
+	rt2x00_desc_write(txd, 10, word);
+
+	rt2x00_desc_read(txd, 0, &word);
+	rt2x00_set_field32(&word, TXD_W0_OWNER_NIC, 1);
+	rt2x00_set_field32(&word, TXD_W0_VALID, 1);
+	rt2x00_set_field32(&word, TXD_W0_MORE_FRAG,
+			   test_bit(ENTRY_TXD_MORE_FRAG, &desc->flags));
+	rt2x00_set_field32(&word, TXD_W0_ACK,
+			   !(control->flags & IEEE80211_TXCTL_NO_ACK));
+	rt2x00_set_field32(&word, TXD_W0_TIMESTAMP,
+			   test_bit(ENTRY_TXD_REQ_TIMESTAMP, &desc->flags));
+	rt2x00_set_field32(&word, TXD_W0_OFDM,
+			   test_bit(ENTRY_TXD_OFDM_RATE, &desc->flags));
+	rt2x00_set_field32(&word, TXD_W0_CIPHER_OWNER, 1);
+	rt2x00_set_field32(&word, TXD_W0_IFS, desc->ifs);
+	rt2x00_set_field32(&word, TXD_W0_RETRY_MODE,
+			   !!(control->flags &
+			      IEEE80211_TXCTL_LONG_RETRY_LIMIT));
+	rt2x00_set_field32(&word, TXD_W0_DATABYTE_COUNT, length);
+	rt2x00_set_field32(&word, TXD_W0_CIPHER_ALG, CIPHER_NONE);
+	rt2x00_desc_write(txd, 0, word);
+}
+
+/*
+ * TX data initialization
+ */
+static void rt2500pci_kick_tx_queue(struct rt2x00_dev *rt2x00dev,
+				    unsigned int queue)
+{
+	u32 reg;
+
+	if (queue == IEEE80211_TX_QUEUE_BEACON) {
+		rt2x00pci_register_read(rt2x00dev, CSR14, &reg);
+		if (!rt2x00_get_field32(reg, CSR14_BEACON_GEN)) {
+			rt2x00_set_field32(&reg, CSR14_BEACON_GEN, 1);
+			rt2x00pci_register_write(rt2x00dev, CSR14, reg);
+		}
+		return;
+	}
+
+	rt2x00pci_register_read(rt2x00dev, TXCSR0, &reg);
+	if (queue == IEEE80211_TX_QUEUE_DATA0)
+		rt2x00_set_field32(&reg, TXCSR0_KICK_PRIO, 1);
+	else if (queue == IEEE80211_TX_QUEUE_DATA1)
+		rt2x00_set_field32(&reg, TXCSR0_KICK_TX, 1);
+	else if (queue == IEEE80211_TX_QUEUE_AFTER_BEACON)
+		rt2x00_set_field32(&reg, TXCSR0_KICK_ATIM, 1);
+	rt2x00pci_register_write(rt2x00dev, TXCSR0, reg);
+}
+
+/*
+ * RX control handlers
+ */
+static void rt2500pci_fill_rxdone(struct data_entry *entry,
+				  struct rxdata_entry_desc *desc)
+{
+	struct data_desc *rxd = entry->priv;
+	u32 word0;
+	u32 word2;
+
+	rt2x00_desc_read(rxd, 0, &word0);
+	rt2x00_desc_read(rxd, 2, &word2);
+
+	desc->flags = 0;
+	if (rt2x00_get_field32(word0, RXD_W0_CRC_ERROR))
+		desc->flags |= RX_FLAG_FAILED_FCS_CRC;
+	if (rt2x00_get_field32(word0, RXD_W0_PHYSICAL_ERROR))
+		desc->flags |= RX_FLAG_FAILED_PLCP_CRC;
+
+	desc->signal = rt2x00_get_field32(word2, RXD_W2_SIGNAL);
+	desc->rssi = rt2x00_get_field32(word2, RXD_W2_RSSI) -
+	    entry->ring->rt2x00dev->rssi_offset;
+	desc->ofdm = rt2x00_get_field32(word0, RXD_W0_OFDM);
+	desc->size = rt2x00_get_field32(word0, RXD_W0_DATABYTE_COUNT);
+}
+
+/*
+ * Interrupt functions.
+ */
+static void rt2500pci_txdone(struct rt2x00_dev *rt2x00dev, const int queue)
+{
+	struct data_ring *ring = rt2x00lib_get_ring(rt2x00dev, queue);
+	struct data_entry *entry;
+	struct data_desc *txd;
+	u32 word;
+	int tx_status;
+	int retry;
+
+	while (!rt2x00_ring_empty(ring)) {
+		entry = rt2x00_get_data_entry_done(ring);
+		txd = entry->priv;
+		rt2x00_desc_read(txd, 0, &word);
+
+		if (rt2x00_get_field32(word, TXD_W0_OWNER_NIC) ||
+		    !rt2x00_get_field32(word, TXD_W0_VALID))
+			break;
+
+		/*
+		 * Obtain the status about this packet.
+		 */
+		tx_status = rt2x00_get_field32(word, TXD_W0_RESULT);
+		retry = rt2x00_get_field32(word, TXD_W0_RETRY_COUNT);
+
+		rt2x00lib_txdone(entry, tx_status, retry);
+
+		/*
+		 * Make this entry available for reuse.
+		 */
+		entry->flags = 0;
+		rt2x00_set_field32(&word, TXD_W0_VALID, 0);
+		rt2x00_desc_write(txd, 0, word);
+		rt2x00_ring_index_done_inc(ring);
+	}
+
+	/*
+	 * If the data ring was full before the txdone handler
+	 * we must make sure the packet queue in the mac80211 stack
+	 * is reenabled when the txdone handler has finished.
+	 */
+	entry = ring->entry;
+	if (!rt2x00_ring_full(ring))
+		ieee80211_wake_queue(rt2x00dev->hw,
+				     entry->tx_status.control.queue);
+}
+
+static irqreturn_t rt2500pci_interrupt(int irq, void *dev_instance)
+{
+	struct rt2x00_dev *rt2x00dev = dev_instance;
+	u32 reg;
+
+	/*
+	 * Get the interrupt sources & saved to local variable.
+	 * Write register value back to clear pending interrupts.
+	 */
+	rt2x00pci_register_read(rt2x00dev, CSR7, &reg);
+	rt2x00pci_register_write(rt2x00dev, CSR7, reg);
+
+	if (!reg)
+		return IRQ_NONE;
+
+	if (!test_bit(DEVICE_ENABLED_RADIO, &rt2x00dev->flags))
+		return IRQ_HANDLED;
+
+	/*
+	 * Handle interrupts, walk through all bits
+	 * and run the tasks, the bits are checked in order of
+	 * priority.
+	 */
+
+	/*
+	 * 1 - Beacon timer expired interrupt.
+	 */
+	if (rt2x00_get_field32(reg, CSR7_TBCN_EXPIRE))
+		rt2x00lib_beacondone(rt2x00dev);
+
+	/*
+	 * 2 - Rx ring done interrupt.
+	 */
+	if (rt2x00_get_field32(reg, CSR7_RXDONE))
+		rt2x00pci_rxdone(rt2x00dev);
+
+	/*
+	 * 3 - Atim ring transmit done interrupt.
+	 */
+	if (rt2x00_get_field32(reg, CSR7_TXDONE_ATIMRING))
+		rt2500pci_txdone(rt2x00dev, IEEE80211_TX_QUEUE_AFTER_BEACON);
+
+	/*
+	 * 4 - Priority ring transmit done interrupt.
+	 */
+	if (rt2x00_get_field32(reg, CSR7_TXDONE_PRIORING))
+		rt2500pci_txdone(rt2x00dev, IEEE80211_TX_QUEUE_DATA0);
+
+	/*
+	 * 5 - Tx ring transmit done interrupt.
+	 */
+	if (rt2x00_get_field32(reg, CSR7_TXDONE_TXRING))
+		rt2500pci_txdone(rt2x00dev, IEEE80211_TX_QUEUE_DATA1);
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * Device probe functions.
+ */
+static int rt2500pci_validate_eeprom(struct rt2x00_dev *rt2x00dev)
+{
+	struct eeprom_93cx6 eeprom;
+	u32 reg;
+	u16 word;
+	u8 *mac;
+
+	rt2x00pci_register_read(rt2x00dev, CSR21, &reg);
+
+	eeprom.data = rt2x00dev;
+	eeprom.register_read = rt2500pci_eepromregister_read;
+	eeprom.register_write = rt2500pci_eepromregister_write;
+	eeprom.width = rt2x00_get_field32(reg, CSR21_TYPE_93C46) ?
+	    PCI_EEPROM_WIDTH_93C46 : PCI_EEPROM_WIDTH_93C66;
+	eeprom.reg_data_in = 0;
+	eeprom.reg_data_out = 0;
+	eeprom.reg_data_clock = 0;
+	eeprom.reg_chip_select = 0;
+
+	eeprom_93cx6_multiread(&eeprom, EEPROM_BASE, rt2x00dev->eeprom,
+			       EEPROM_SIZE / sizeof(u16));
+
+	/*
+	 * Start validation of the data that has been read.
+	 */
+	mac = rt2x00_eeprom_addr(rt2x00dev, EEPROM_MAC_ADDR_0);
+	if (!is_valid_ether_addr(mac)) {
+		DECLARE_MAC_BUF(macbuf);
+
+		random_ether_addr(mac);
+		EEPROM(rt2x00dev, "MAC: %s\n",
+		       print_mac(macbuf, mac));
+	}
+
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_ANTENNA, &word);
+	if (word == 0xffff) {
+		rt2x00_set_field16(&word, EEPROM_ANTENNA_NUM, 2);
+		rt2x00_set_field16(&word, EEPROM_ANTENNA_TX_DEFAULT, 0);
+		rt2x00_set_field16(&word, EEPROM_ANTENNA_RX_DEFAULT, 0);
+		rt2x00_set_field16(&word, EEPROM_ANTENNA_LED_MODE, 0);
+		rt2x00_set_field16(&word, EEPROM_ANTENNA_DYN_TXAGC, 0);
+		rt2x00_set_field16(&word, EEPROM_ANTENNA_HARDWARE_RADIO, 0);
+		rt2x00_set_field16(&word, EEPROM_ANTENNA_RF_TYPE, RF2522);
+		rt2x00_eeprom_write(rt2x00dev, EEPROM_ANTENNA, word);
+		EEPROM(rt2x00dev, "Antenna: 0x%04x\n", word);
+	}
+
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC, &word);
+	if (word == 0xffff) {
+		rt2x00_set_field16(&word, EEPROM_NIC_CARDBUS_ACCEL, 0);
+		rt2x00_set_field16(&word, EEPROM_NIC_DYN_BBP_TUNE, 0);
+		rt2x00_set_field16(&word, EEPROM_NIC_CCK_TX_POWER, 0);
+		rt2x00_eeprom_write(rt2x00dev, EEPROM_NIC, word);
+		EEPROM(rt2x00dev, "NIC: 0x%04x\n", word);
+	}
+
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_CALIBRATE_OFFSET, &word);
+	if (word == 0xffff) {
+		rt2x00_set_field16(&word, EEPROM_CALIBRATE_OFFSET_RSSI,
+				   DEFAULT_RSSI_OFFSET);
+		rt2x00_eeprom_write(rt2x00dev, EEPROM_CALIBRATE_OFFSET, word);
+		EEPROM(rt2x00dev, "Calibrate offset: 0x%04x\n", word);
+	}
+
+	return 0;
+}
+
+static int rt2500pci_init_eeprom(struct rt2x00_dev *rt2x00dev)
+{
+	u32 reg;
+	u16 value;
+	u16 eeprom;
+
+	/*
+	 * Read EEPROM word for configuration.
+	 */
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_ANTENNA, &eeprom);
+
+	/*
+	 * Identify RF chipset.
+	 */
+	value = rt2x00_get_field16(eeprom, EEPROM_ANTENNA_RF_TYPE);
+	rt2x00pci_register_read(rt2x00dev, CSR0, &reg);
+	rt2x00_set_chip(rt2x00dev, RT2560, value, reg);
+
+	if (!rt2x00_rf(&rt2x00dev->chip, RF2522) &&
+	    !rt2x00_rf(&rt2x00dev->chip, RF2523) &&
+	    !rt2x00_rf(&rt2x00dev->chip, RF2524) &&
+	    !rt2x00_rf(&rt2x00dev->chip, RF2525) &&
+	    !rt2x00_rf(&rt2x00dev->chip, RF2525E) &&
+	    !rt2x00_rf(&rt2x00dev->chip, RF5222)) {
+		ERROR(rt2x00dev, "Invalid RF chipset detected.\n");
+		return -ENODEV;
+	}
+
+	/*
+	 * Identify default antenna configuration.
+	 */
+	rt2x00dev->hw->conf.antenna_sel_tx =
+	    rt2x00_get_field16(eeprom, EEPROM_ANTENNA_TX_DEFAULT);
+	rt2x00dev->hw->conf.antenna_sel_rx =
+	    rt2x00_get_field16(eeprom, EEPROM_ANTENNA_RX_DEFAULT);
+
+	/*
+	 * Store led mode, for correct led behaviour.
+	 */
+	rt2x00dev->led_mode =
+	    rt2x00_get_field16(eeprom, EEPROM_ANTENNA_LED_MODE);
+
+	/*
+	 * Detect if this device has an hardware controlled radio.
+	 */
+#ifdef CONFIG_RT2500PCI_RFKILL
+	if (rt2x00_get_field16(eeprom, EEPROM_ANTENNA_HARDWARE_RADIO))
+		__set_bit(CONFIG_SUPPORT_HW_BUTTON, &rt2x00dev->flags);
+#endif /* CONFIG_RT2500PCI_RFKILL */
+
+	/*
+	 * Check if the BBP tuning should be enabled.
+	 */
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC, &eeprom);
+
+	if (rt2x00_get_field16(eeprom, EEPROM_NIC_DYN_BBP_TUNE))
+		__set_bit(CONFIG_DISABLE_LINK_TUNING, &rt2x00dev->flags);
+
+	/*
+	 * Read the RSSI <-> dBm offset information.
+	 */
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_CALIBRATE_OFFSET, &eeprom);
+	rt2x00dev->rssi_offset =
+	    rt2x00_get_field16(eeprom, EEPROM_CALIBRATE_OFFSET_RSSI);
+
+	return 0;
+}
+
+/*
+ * RF value list for RF2522
+ * Supports: 2.4 GHz
+ */
+static const struct rf_channel rf_vals_bg_2522[] = {
+	{ 1,  0x00002050, 0x000c1fda, 0x00000101, 0 },
+	{ 2,  0x00002050, 0x000c1fee, 0x00000101, 0 },
+	{ 3,  0x00002050, 0x000c2002, 0x00000101, 0 },
+	{ 4,  0x00002050, 0x000c2016, 0x00000101, 0 },
+	{ 5,  0x00002050, 0x000c202a, 0x00000101, 0 },
+	{ 6,  0x00002050, 0x000c203e, 0x00000101, 0 },
+	{ 7,  0x00002050, 0x000c2052, 0x00000101, 0 },
+	{ 8,  0x00002050, 0x000c2066, 0x00000101, 0 },
+	{ 9,  0x00002050, 0x000c207a, 0x00000101, 0 },
+	{ 10, 0x00002050, 0x000c208e, 0x00000101, 0 },
+	{ 11, 0x00002050, 0x000c20a2, 0x00000101, 0 },
+	{ 12, 0x00002050, 0x000c20b6, 0x00000101, 0 },
+	{ 13, 0x00002050, 0x000c20ca, 0x00000101, 0 },
+	{ 14, 0x00002050, 0x000c20fa, 0x00000101, 0 },
+};
+
+/*
+ * RF value list for RF2523
+ * Supports: 2.4 GHz
+ */
+static const struct rf_channel rf_vals_bg_2523[] = {
+	{ 1,  0x00022010, 0x00000c9e, 0x000e0111, 0x00000a1b },
+	{ 2,  0x00022010, 0x00000ca2, 0x000e0111, 0x00000a1b },
+	{ 3,  0x00022010, 0x00000ca6, 0x000e0111, 0x00000a1b },
+	{ 4,  0x00022010, 0x00000caa, 0x000e0111, 0x00000a1b },
+	{ 5,  0x00022010, 0x00000cae, 0x000e0111, 0x00000a1b },
+	{ 6,  0x00022010, 0x00000cb2, 0x000e0111, 0x00000a1b },
+	{ 7,  0x00022010, 0x00000cb6, 0x000e0111, 0x00000a1b },
+	{ 8,  0x00022010, 0x00000cba, 0x000e0111, 0x00000a1b },
+	{ 9,  0x00022010, 0x00000cbe, 0x000e0111, 0x00000a1b },
+	{ 10, 0x00022010, 0x00000d02, 0x000e0111, 0x00000a1b },
+	{ 11, 0x00022010, 0x00000d06, 0x000e0111, 0x00000a1b },
+	{ 12, 0x00022010, 0x00000d0a, 0x000e0111, 0x00000a1b },
+	{ 13, 0x00022010, 0x00000d0e, 0x000e0111, 0x00000a1b },
+	{ 14, 0x00022010, 0x00000d1a, 0x000e0111, 0x00000a03 },
+};
+
+/*
+ * RF value list for RF2524
+ * Supports: 2.4 GHz
+ */
+static const struct rf_channel rf_vals_bg_2524[] = {
+	{ 1,  0x00032020, 0x00000c9e, 0x00000101, 0x00000a1b },
+	{ 2,  0x00032020, 0x00000ca2, 0x00000101, 0x00000a1b },
+	{ 3,  0x00032020, 0x00000ca6, 0x00000101, 0x00000a1b },
+	{ 4,  0x00032020, 0x00000caa, 0x00000101, 0x00000a1b },
+	{ 5,  0x00032020, 0x00000cae, 0x00000101, 0x00000a1b },
+	{ 6,  0x00032020, 0x00000cb2, 0x00000101, 0x00000a1b },
+	{ 7,  0x00032020, 0x00000cb6, 0x00000101, 0x00000a1b },
+	{ 8,  0x00032020, 0x00000cba, 0x00000101, 0x00000a1b },
+	{ 9,  0x00032020, 0x00000cbe, 0x00000101, 0x00000a1b },
+	{ 10, 0x00032020, 0x00000d02, 0x00000101, 0x00000a1b },
+	{ 11, 0x00032020, 0x00000d06, 0x00000101, 0x00000a1b },
+	{ 12, 0x00032020, 0x00000d0a, 0x00000101, 0x00000a1b },
+	{ 13, 0x00032020, 0x00000d0e, 0x00000101, 0x00000a1b },
+	{ 14, 0x00032020, 0x00000d1a, 0x00000101, 0x00000a03 },
+};
+
+/*
+ * RF value list for RF2525
+ * Supports: 2.4 GHz
+ */
+static const struct rf_channel rf_vals_bg_2525[] = {
+	{ 1,  0x00022020, 0x00080c9e, 0x00060111, 0x00000a1b },
+	{ 2,  0x00022020, 0x00080ca2, 0x00060111, 0x00000a1b },
+	{ 3,  0x00022020, 0x00080ca6, 0x00060111, 0x00000a1b },
+	{ 4,  0x00022020, 0x00080caa, 0x00060111, 0x00000a1b },
+	{ 5,  0x00022020, 0x00080cae, 0x00060111, 0x00000a1b },
+	{ 6,  0x00022020, 0x00080cb2, 0x00060111, 0x00000a1b },
+	{ 7,  0x00022020, 0x00080cb6, 0x00060111, 0x00000a1b },
+	{ 8,  0x00022020, 0x00080cba, 0x00060111, 0x00000a1b },
+	{ 9,  0x00022020, 0x00080cbe, 0x00060111, 0x00000a1b },
+	{ 10, 0x00022020, 0x00080d02, 0x00060111, 0x00000a1b },
+	{ 11, 0x00022020, 0x00080d06, 0x00060111, 0x00000a1b },
+	{ 12, 0x00022020, 0x00080d0a, 0x00060111, 0x00000a1b },
+	{ 13, 0x00022020, 0x00080d0e, 0x00060111, 0x00000a1b },
+	{ 14, 0x00022020, 0x00080d1a, 0x00060111, 0x00000a03 },
+};
+
+/*
+ * RF value list for RF2525e
+ * Supports: 2.4 GHz
+ */
+static const struct rf_channel rf_vals_bg_2525e[] = {
+	{ 1,  0x00022020, 0x00081136, 0x00060111, 0x00000a0b },
+	{ 2,  0x00022020, 0x0008113a, 0x00060111, 0x00000a0b },
+	{ 3,  0x00022020, 0x0008113e, 0x00060111, 0x00000a0b },
+	{ 4,  0x00022020, 0x00081182, 0x00060111, 0x00000a0b },
+	{ 5,  0x00022020, 0x00081186, 0x00060111, 0x00000a0b },
+	{ 6,  0x00022020, 0x0008118a, 0x00060111, 0x00000a0b },
+	{ 7,  0x00022020, 0x0008118e, 0x00060111, 0x00000a0b },
+	{ 8,  0x00022020, 0x00081192, 0x00060111, 0x00000a0b },
+	{ 9,  0x00022020, 0x00081196, 0x00060111, 0x00000a0b },
+	{ 10, 0x00022020, 0x0008119a, 0x00060111, 0x00000a0b },
+	{ 11, 0x00022020, 0x0008119e, 0x00060111, 0x00000a0b },
+	{ 12, 0x00022020, 0x000811a2, 0x00060111, 0x00000a0b },
+	{ 13, 0x00022020, 0x000811a6, 0x00060111, 0x00000a0b },
+	{ 14, 0x00022020, 0x000811ae, 0x00060111, 0x00000a1b },
+};
+
+/*
+ * RF value list for RF5222
+ * Supports: 2.4 GHz & 5.2 GHz
+ */
+static const struct rf_channel rf_vals_5222[] = {
+	{ 1,  0x00022020, 0x00001136, 0x00000101, 0x00000a0b },
+	{ 2,  0x00022020, 0x0000113a, 0x00000101, 0x00000a0b },
+	{ 3,  0x00022020, 0x0000113e, 0x00000101, 0x00000a0b },
+	{ 4,  0x00022020, 0x00001182, 0x00000101, 0x00000a0b },
+	{ 5,  0x00022020, 0x00001186, 0x00000101, 0x00000a0b },
+	{ 6,  0x00022020, 0x0000118a, 0x00000101, 0x00000a0b },
+	{ 7,  0x00022020, 0x0000118e, 0x00000101, 0x00000a0b },
+	{ 8,  0x00022020, 0x00001192, 0x00000101, 0x00000a0b },
+	{ 9,  0x00022020, 0x00001196, 0x00000101, 0x00000a0b },
+	{ 10, 0x00022020, 0x0000119a, 0x00000101, 0x00000a0b },
+	{ 11, 0x00022020, 0x0000119e, 0x00000101, 0x00000a0b },
+	{ 12, 0x00022020, 0x000011a2, 0x00000101, 0x00000a0b },
+	{ 13, 0x00022020, 0x000011a6, 0x00000101, 0x00000a0b },
+	{ 14, 0x00022020, 0x000011ae, 0x00000101, 0x00000a1b },
+
+	/* 802.11 UNI / HyperLan 2 */
+	{ 36, 0x00022010, 0x00018896, 0x00000101, 0x00000a1f },
+	{ 40, 0x00022010, 0x0001889a, 0x00000101, 0x00000a1f },
+	{ 44, 0x00022010, 0x0001889e, 0x00000101, 0x00000a1f },
+	{ 48, 0x00022010, 0x000188a2, 0x00000101, 0x00000a1f },
+	{ 52, 0x00022010, 0x000188a6, 0x00000101, 0x00000a1f },
+	{ 66, 0x00022010, 0x000188aa, 0x00000101, 0x00000a1f },
+	{ 60, 0x00022010, 0x000188ae, 0x00000101, 0x00000a1f },
+	{ 64, 0x00022010, 0x000188b2, 0x00000101, 0x00000a1f },
+
+	/* 802.11 HyperLan 2 */
+	{ 100, 0x00022010, 0x00008802, 0x00000101, 0x00000a0f },
+	{ 104, 0x00022010, 0x00008806, 0x00000101, 0x00000a0f },
+	{ 108, 0x00022010, 0x0000880a, 0x00000101, 0x00000a0f },
+	{ 112, 0x00022010, 0x0000880e, 0x00000101, 0x00000a0f },
+	{ 116, 0x00022010, 0x00008812, 0x00000101, 0x00000a0f },
+	{ 120, 0x00022010, 0x00008816, 0x00000101, 0x00000a0f },
+	{ 124, 0x00022010, 0x0000881a, 0x00000101, 0x00000a0f },
+	{ 128, 0x00022010, 0x0000881e, 0x00000101, 0x00000a0f },
+	{ 132, 0x00022010, 0x00008822, 0x00000101, 0x00000a0f },
+	{ 136, 0x00022010, 0x00008826, 0x00000101, 0x00000a0f },
+
+	/* 802.11 UNII */
+	{ 140, 0x00022010, 0x0000882a, 0x00000101, 0x00000a0f },
+	{ 149, 0x00022020, 0x000090a6, 0x00000101, 0x00000a07 },
+	{ 153, 0x00022020, 0x000090ae, 0x00000101, 0x00000a07 },
+	{ 157, 0x00022020, 0x000090b6, 0x00000101, 0x00000a07 },
+	{ 161, 0x00022020, 0x000090be, 0x00000101, 0x00000a07 },
+};
+
+static void rt2500pci_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
+{
+	struct hw_mode_spec *spec = &rt2x00dev->spec;
+	u8 *txpower;
+	unsigned int i;
+
+	/*
+	 * Initialize all hw fields.
+	 */
+	rt2x00dev->hw->flags = IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING;
+	rt2x00dev->hw->extra_tx_headroom = 0;
+	rt2x00dev->hw->max_signal = MAX_SIGNAL;
+	rt2x00dev->hw->max_rssi = MAX_RX_SSI;
+	rt2x00dev->hw->queues = 2;
+
+	SET_IEEE80211_DEV(rt2x00dev->hw, &rt2x00dev_pci(rt2x00dev)->dev);
+	SET_IEEE80211_PERM_ADDR(rt2x00dev->hw,
+				rt2x00_eeprom_addr(rt2x00dev,
+						   EEPROM_MAC_ADDR_0));
+
+	/*
+	 * Convert tx_power array in eeprom.
+	 */
+	txpower = rt2x00_eeprom_addr(rt2x00dev, EEPROM_TXPOWER_START);
+	for (i = 0; i < 14; i++)
+		txpower[i] = TXPOWER_FROM_DEV(txpower[i]);
+
+	/*
+	 * Initialize hw_mode information.
+	 */
+	spec->num_modes = 2;
+	spec->num_rates = 12;
+	spec->tx_power_a = NULL;
+	spec->tx_power_bg = txpower;
+	spec->tx_power_default = DEFAULT_TXPOWER;
+
+	if (rt2x00_rf(&rt2x00dev->chip, RF2522)) {
+		spec->num_channels = ARRAY_SIZE(rf_vals_bg_2522);
+		spec->channels = rf_vals_bg_2522;
+	} else if (rt2x00_rf(&rt2x00dev->chip, RF2523)) {
+		spec->num_channels = ARRAY_SIZE(rf_vals_bg_2523);
+		spec->channels = rf_vals_bg_2523;
+	} else if (rt2x00_rf(&rt2x00dev->chip, RF2524)) {
+		spec->num_channels = ARRAY_SIZE(rf_vals_bg_2524);
+		spec->channels = rf_vals_bg_2524;
+	} else if (rt2x00_rf(&rt2x00dev->chip, RF2525)) {
+		spec->num_channels = ARRAY_SIZE(rf_vals_bg_2525);
+		spec->channels = rf_vals_bg_2525;
+	} else if (rt2x00_rf(&rt2x00dev->chip, RF2525E)) {
+		spec->num_channels = ARRAY_SIZE(rf_vals_bg_2525e);
+		spec->channels = rf_vals_bg_2525e;
+	} else if (rt2x00_rf(&rt2x00dev->chip, RF5222)) {
+		spec->num_channels = ARRAY_SIZE(rf_vals_5222);
+		spec->channels = rf_vals_5222;
+		spec->num_modes = 3;
+	}
+}
+
+static int rt2500pci_probe_hw(struct rt2x00_dev *rt2x00dev)
+{
+	int retval;
+
+	/*
+	 * Allocate eeprom data.
+	 */
+	retval = rt2500pci_validate_eeprom(rt2x00dev);
+	if (retval)
+		return retval;
+
+	retval = rt2500pci_init_eeprom(rt2x00dev);
+	if (retval)
+		return retval;
+
+	/*
+	 * Initialize hw specifications.
+	 */
+	rt2500pci_probe_hw_mode(rt2x00dev);
+
+	/*
+	 * This device requires the beacon ring
+	 */
+	__set_bit(DRIVER_REQUIRE_BEACON_RING, &rt2x00dev->flags);
+
+	/*
+	 * Set the rssi offset.
+	 */
+	rt2x00dev->rssi_offset = DEFAULT_RSSI_OFFSET;
+
+	return 0;
+}
+
+/*
+ * IEEE80211 stack callback functions.
+ */
+static void rt2500pci_configure_filter(struct ieee80211_hw *hw,
+				       unsigned int changed_flags,
+				       unsigned int *total_flags,
+				       int mc_count,
+				       struct dev_addr_list *mc_list)
+{
+	struct rt2x00_dev *rt2x00dev = hw->priv;
+	struct interface *intf = &rt2x00dev->interface;
+	u32 reg;
+
+	/*
+	 * Mask off any flags we are going to ignore from
+	 * the total_flags field.
+	 */
+	*total_flags &=
+	    FIF_ALLMULTI |
+	    FIF_FCSFAIL |
+	    FIF_PLCPFAIL |
+	    FIF_CONTROL |
+	    FIF_OTHER_BSS |
+	    FIF_PROMISC_IN_BSS;
+
+	/*
+	 * Apply some rules to the filters:
+	 * - Some filters imply different filters to be set.
+	 * - Some things we can't filter out at all.
+	 * - Some filters are set based on interface type.
+	 */
+	if (mc_count)
+		*total_flags |= FIF_ALLMULTI;
+	if (*total_flags & FIF_OTHER_BSS ||
+	    *total_flags & FIF_PROMISC_IN_BSS)
+		*total_flags |= FIF_PROMISC_IN_BSS | FIF_OTHER_BSS;
+	if (is_interface_type(intf, IEEE80211_IF_TYPE_AP))
+		*total_flags |= FIF_PROMISC_IN_BSS;
+
+	/*
+	 * Check if there is any work left for us.
+	 */
+	if (intf->filter == *total_flags)
+		return;
+	intf->filter = *total_flags;
+
+	/*
+	 * Start configuration steps.
+	 * Note that the version error will always be dropped
+	 * and broadcast frames will always be accepted since
+	 * there is no filter for it at this time.
+	 */
+	rt2x00pci_register_read(rt2x00dev, RXCSR0, &reg);
+	rt2x00_set_field32(&reg, RXCSR0_DROP_CRC,
+			   !(*total_flags & FIF_FCSFAIL));
+	rt2x00_set_field32(&reg, RXCSR0_DROP_PHYSICAL,
+			   !(*total_flags & FIF_PLCPFAIL));
+	rt2x00_set_field32(&reg, RXCSR0_DROP_CONTROL,
+			   !(*total_flags & FIF_CONTROL));
+	rt2x00_set_field32(&reg, RXCSR0_DROP_NOT_TO_ME,
+			   !(*total_flags & FIF_PROMISC_IN_BSS));
+	rt2x00_set_field32(&reg, RXCSR0_DROP_TODS,
+			   !(*total_flags & FIF_PROMISC_IN_BSS));
+	rt2x00_set_field32(&reg, RXCSR0_DROP_VERSION_ERROR, 1);
+	rt2x00_set_field32(&reg, RXCSR0_DROP_MCAST,
+			   !(*total_flags & FIF_ALLMULTI));
+	rt2x00_set_field32(&reg, RXCSR0_DROP_BCAST, 0);
+	rt2x00pci_register_write(rt2x00dev, RXCSR0, reg);
+}
+
+static int rt2500pci_set_retry_limit(struct ieee80211_hw *hw,
+				     u32 short_retry, u32 long_retry)
+{
+	struct rt2x00_dev *rt2x00dev = hw->priv;
+	u32 reg;
+
+	rt2x00pci_register_read(rt2x00dev, CSR11, &reg);
+	rt2x00_set_field32(&reg, CSR11_LONG_RETRY, long_retry);
+	rt2x00_set_field32(&reg, CSR11_SHORT_RETRY, short_retry);
+	rt2x00pci_register_write(rt2x00dev, CSR11, reg);
+
+	return 0;
+}
+
+static u64 rt2500pci_get_tsf(struct ieee80211_hw *hw)
+{
+	struct rt2x00_dev *rt2x00dev = hw->priv;
+	u64 tsf;
+	u32 reg;
+
+	rt2x00pci_register_read(rt2x00dev, CSR17, &reg);
+	tsf = (u64) rt2x00_get_field32(reg, CSR17_HIGH_TSFTIMER) << 32;
+	rt2x00pci_register_read(rt2x00dev, CSR16, &reg);
+	tsf |= rt2x00_get_field32(reg, CSR16_LOW_TSFTIMER);
+
+	return tsf;
+}
+
+static void rt2500pci_reset_tsf(struct ieee80211_hw *hw)
+{
+	struct rt2x00_dev *rt2x00dev = hw->priv;
+
+	rt2x00pci_register_write(rt2x00dev, CSR16, 0);
+	rt2x00pci_register_write(rt2x00dev, CSR17, 0);
+}
+
+static int rt2500pci_tx_last_beacon(struct ieee80211_hw *hw)
+{
+	struct rt2x00_dev *rt2x00dev = hw->priv;
+	u32 reg;
+
+	rt2x00pci_register_read(rt2x00dev, CSR15, &reg);
+	return rt2x00_get_field32(reg, CSR15_BEACON_SENT);
+}
+
+static const struct ieee80211_ops rt2500pci_mac80211_ops = {
+	.tx			= rt2x00mac_tx,
+	.start			= rt2x00mac_start,
+	.stop			= rt2x00mac_stop,
+	.add_interface		= rt2x00mac_add_interface,
+	.remove_interface	= rt2x00mac_remove_interface,
+	.config			= rt2x00mac_config,
+	.config_interface	= rt2x00mac_config_interface,
+	.configure_filter	= rt2500pci_configure_filter,
+	.get_stats		= rt2x00mac_get_stats,
+	.set_retry_limit	= rt2500pci_set_retry_limit,
+	.erp_ie_changed		= rt2x00mac_erp_ie_changed,
+	.conf_tx		= rt2x00mac_conf_tx,
+	.get_tx_stats		= rt2x00mac_get_tx_stats,
+	.get_tsf		= rt2500pci_get_tsf,
+	.reset_tsf		= rt2500pci_reset_tsf,
+	.beacon_update		= rt2x00pci_beacon_update,
+	.tx_last_beacon		= rt2500pci_tx_last_beacon,
+};
+
+static const struct rt2x00lib_ops rt2500pci_rt2x00_ops = {
+	.irq_handler		= rt2500pci_interrupt,
+	.probe_hw		= rt2500pci_probe_hw,
+	.initialize		= rt2x00pci_initialize,
+	.uninitialize		= rt2x00pci_uninitialize,
+	.set_device_state	= rt2500pci_set_device_state,
+	.rfkill_poll		= rt2500pci_rfkill_poll,
+	.link_stats		= rt2500pci_link_stats,
+	.reset_tuner		= rt2500pci_reset_tuner,
+	.link_tuner		= rt2500pci_link_tuner,
+	.write_tx_desc		= rt2500pci_write_tx_desc,
+	.write_tx_data		= rt2x00pci_write_tx_data,
+	.kick_tx_queue		= rt2500pci_kick_tx_queue,
+	.fill_rxdone		= rt2500pci_fill_rxdone,
+	.config_mac_addr	= rt2500pci_config_mac_addr,
+	.config_bssid		= rt2500pci_config_bssid,
+	.config_type		= rt2500pci_config_type,
+	.config_preamble	= rt2500pci_config_preamble,
+	.config			= rt2500pci_config,
+};
+
+static const struct rt2x00_ops rt2500pci_ops = {
+	.name		= DRV_NAME,
+	.rxd_size	= RXD_DESC_SIZE,
+	.txd_size	= TXD_DESC_SIZE,
+	.eeprom_size	= EEPROM_SIZE,
+	.rf_size	= RF_SIZE,
+	.lib		= &rt2500pci_rt2x00_ops,
+	.hw		= &rt2500pci_mac80211_ops,
+#ifdef CONFIG_RT2X00_LIB_DEBUGFS
+	.debugfs	= &rt2500pci_rt2x00debug,
+#endif /* CONFIG_RT2X00_LIB_DEBUGFS */
+};
+
+/*
+ * RT2500pci module information.
+ */
+static struct pci_device_id rt2500pci_device_table[] = {
+	{ PCI_DEVICE(0x1814, 0x0201), PCI_DEVICE_DATA(&rt2500pci_ops) },
+	{ 0, }
+};
+
+MODULE_AUTHOR(DRV_PROJECT);
+MODULE_VERSION(DRV_VERSION);
+MODULE_DESCRIPTION("Ralink RT2500 PCI & PCMCIA Wireless LAN driver.");
+MODULE_SUPPORTED_DEVICE("Ralink RT2560 PCI & PCMCIA chipset based cards");
+MODULE_DEVICE_TABLE(pci, rt2500pci_device_table);
+MODULE_LICENSE("GPL");
+
+static struct pci_driver rt2500pci_driver = {
+	.name		= DRV_NAME,
+	.id_table	= rt2500pci_device_table,
+	.probe		= rt2x00pci_probe,
+	.remove		= __devexit_p(rt2x00pci_remove),
+	.suspend	= rt2x00pci_suspend,
+	.resume		= rt2x00pci_resume,
+};
+
+static int __init rt2500pci_init(void)
+{
+	return pci_register_driver(&rt2500pci_driver);
+}
+
+static void __exit rt2500pci_exit(void)
+{
+	pci_unregister_driver(&rt2500pci_driver);
+}
+
+module_init(rt2500pci_init);
+module_exit(rt2500pci_exit);
diff --git a/drivers/net/wireless/rt2x00/rt2500pci.h b/drivers/net/wireless/rt2x00/rt2500pci.h
new file mode 100644
index 000000000000..d92aa56b2f4b
--- /dev/null
+++ b/drivers/net/wireless/rt2x00/rt2500pci.h
@@ -0,0 +1,1236 @@
+/*
+	Copyright (C) 2004 - 2007 rt2x00 SourceForge Project
+	<http://rt2x00.serialmonkey.com>
+
+	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, or
+	(at your option) any later version.
+
+	This program is distributed in the hope that it will be useful,
+	but WITHOUT ANY WARRANTY; without even the implied warranty of
+	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+	GNU General Public License for more details.
+
+	You should have received a copy of the GNU General Public License
+	along with this program; if not, write to the
+	Free Software Foundation, Inc.,
+	59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+/*
+	Module: rt2500pci
+	Abstract: Data structures and registers for the rt2500pci module.
+	Supported chipsets: RT2560.
+ */
+
+#ifndef RT2500PCI_H
+#define RT2500PCI_H
+
+/*
+ * RF chip defines.
+ */
+#define RF2522				0x0000
+#define RF2523				0x0001
+#define RF2524				0x0002
+#define RF2525				0x0003
+#define RF2525E				0x0004
+#define RF5222				0x0010
+
+/*
+ * RT2560 version
+ */
+#define RT2560_VERSION_B		2
+#define RT2560_VERSION_C		3
+#define RT2560_VERSION_D		4
+
+/*
+ * Signal information.
+ * Defaul offset is required for RSSI <-> dBm conversion.
+ */
+#define MAX_SIGNAL			100
+#define MAX_RX_SSI			-1
+#define DEFAULT_RSSI_OFFSET		121
+
+/*
+ * Register layout information.
+ */
+#define CSR_REG_BASE			0x0000
+#define CSR_REG_SIZE			0x0174
+#define EEPROM_BASE			0x0000
+#define EEPROM_SIZE			0x0200
+#define BBP_SIZE			0x0040
+#define RF_SIZE				0x0014
+
+/*
+ * Control/Status Registers(CSR).
+ * Some values are set in TU, whereas 1 TU == 1024 us.
+ */
+
+/*
+ * CSR0: ASIC revision number.
+ */
+#define CSR0				0x0000
+
+/*
+ * CSR1: System control register.
+ * SOFT_RESET: Software reset, 1: reset, 0: normal.
+ * BBP_RESET: Hardware reset, 1: reset, 0, release.
+ * HOST_READY: Host ready after initialization.
+ */
+#define CSR1				0x0004
+#define CSR1_SOFT_RESET			FIELD32(0x00000001)
+#define CSR1_BBP_RESET			FIELD32(0x00000002)
+#define CSR1_HOST_READY			FIELD32(0x00000004)
+
+/*
+ * CSR2: System admin status register (invalid).
+ */
+#define CSR2				0x0008
+
+/*
+ * CSR3: STA MAC address register 0.
+ */
+#define CSR3				0x000c
+#define CSR3_BYTE0			FIELD32(0x000000ff)
+#define CSR3_BYTE1			FIELD32(0x0000ff00)
+#define CSR3_BYTE2			FIELD32(0x00ff0000)
+#define CSR3_BYTE3			FIELD32(0xff000000)
+
+/*
+ * CSR4: STA MAC address register 1.
+ */
+#define CSR4				0x0010
+#define CSR4_BYTE4			FIELD32(0x000000ff)
+#define CSR4_BYTE5			FIELD32(0x0000ff00)
+
+/*
+ * CSR5: BSSID register 0.
+ */
+#define CSR5				0x0014
+#define CSR5_BYTE0			FIELD32(0x000000ff)
+#define CSR5_BYTE1			FIELD32(0x0000ff00)
+#define CSR5_BYTE2			FIELD32(0x00ff0000)
+#define CSR5_BYTE3			FIELD32(0xff000000)
+
+/*
+ * CSR6: BSSID register 1.
+ */
+#define CSR6				0x0018
+#define CSR6_BYTE4			FIELD32(0x000000ff)
+#define CSR6_BYTE5			FIELD32(0x0000ff00)
+
+/*
+ * CSR7: Interrupt source register.
+ * Write 1 to clear.
+ * TBCN_EXPIRE: Beacon timer expired interrupt.
+ * TWAKE_EXPIRE: Wakeup timer expired interrupt.
+ * TATIMW_EXPIRE: Timer of atim window expired interrupt.
+ * TXDONE_TXRING: Tx ring transmit done interrupt.
+ * TXDONE_ATIMRING: Atim ring transmit done interrupt.
+ * TXDONE_PRIORING: Priority ring transmit done interrupt.
+ * RXDONE: Receive done interrupt.
+ * DECRYPTION_DONE: Decryption done interrupt.
+ * ENCRYPTION_DONE: Encryption done interrupt.
+ * UART1_TX_TRESHOLD: UART1 TX reaches threshold.
+ * UART1_RX_TRESHOLD: UART1 RX reaches threshold.
+ * UART1_IDLE_TRESHOLD: UART1 IDLE over threshold.
+ * UART1_TX_BUFF_ERROR: UART1 TX buffer error.
+ * UART1_RX_BUFF_ERROR: UART1 RX buffer error.
+ * UART2_TX_TRESHOLD: UART2 TX reaches threshold.
+ * UART2_RX_TRESHOLD: UART2 RX reaches threshold.
+ * UART2_IDLE_TRESHOLD: UART2 IDLE over threshold.
+ * UART2_TX_BUFF_ERROR: UART2 TX buffer error.
+ * UART2_RX_BUFF_ERROR: UART2 RX buffer error.
+ * TIMER_CSR3_EXPIRE: TIMECSR3 timer expired (802.1H quiet period).
+
+ */
+#define CSR7				0x001c
+#define CSR7_TBCN_EXPIRE		FIELD32(0x00000001)
+#define CSR7_TWAKE_EXPIRE		FIELD32(0x00000002)
+#define CSR7_TATIMW_EXPIRE		FIELD32(0x00000004)
+#define CSR7_TXDONE_TXRING		FIELD32(0x00000008)
+#define CSR7_TXDONE_ATIMRING		FIELD32(0x00000010)
+#define CSR7_TXDONE_PRIORING		FIELD32(0x00000020)
+#define CSR7_RXDONE			FIELD32(0x00000040)
+#define CSR7_DECRYPTION_DONE		FIELD32(0x00000080)
+#define CSR7_ENCRYPTION_DONE		FIELD32(0x00000100)
+#define CSR7_UART1_TX_TRESHOLD		FIELD32(0x00000200)
+#define CSR7_UART1_RX_TRESHOLD		FIELD32(0x00000400)
+#define CSR7_UART1_IDLE_TRESHOLD	FIELD32(0x00000800)
+#define CSR7_UART1_TX_BUFF_ERROR	FIELD32(0x00001000)
+#define CSR7_UART1_RX_BUFF_ERROR	FIELD32(0x00002000)
+#define CSR7_UART2_TX_TRESHOLD		FIELD32(0x00004000)
+#define CSR7_UART2_RX_TRESHOLD		FIELD32(0x00008000)
+#define CSR7_UART2_IDLE_TRESHOLD	FIELD32(0x00010000)
+#define CSR7_UART2_TX_BUFF_ERROR	FIELD32(0x00020000)
+#define CSR7_UART2_RX_BUFF_ERROR	FIELD32(0x00040000)
+#define CSR7_TIMER_CSR3_EXPIRE		FIELD32(0x00080000)
+
+/*
+ * CSR8: Interrupt mask register.
+ * Write 1 to mask interrupt.
+ * TBCN_EXPIRE: Beacon timer expired interrupt.
+ * TWAKE_EXPIRE: Wakeup timer expired interrupt.
+ * TATIMW_EXPIRE: Timer of atim window expired interrupt.
+ * TXDONE_TXRING: Tx ring transmit done interrupt.
+ * TXDONE_ATIMRING: Atim ring transmit done interrupt.
+ * TXDONE_PRIORING: Priority ring transmit done interrupt.
+ * RXDONE: Receive done interrupt.
+ * DECRYPTION_DONE: Decryption done interrupt.
+ * ENCRYPTION_DONE: Encryption done interrupt.
+ * UART1_TX_TRESHOLD: UART1 TX reaches threshold.
+ * UART1_RX_TRESHOLD: UART1 RX reaches threshold.
+ * UART1_IDLE_TRESHOLD: UART1 IDLE over threshold.
+ * UART1_TX_BUFF_ERROR: UART1 TX buffer error.
+ * UART1_RX_BUFF_ERROR: UART1 RX buffer error.
+ * UART2_TX_TRESHOLD: UART2 TX reaches threshold.
+ * UART2_RX_TRESHOLD: UART2 RX reaches threshold.
+ * UART2_IDLE_TRESHOLD: UART2 IDLE over threshold.
+ * UART2_TX_BUFF_ERROR: UART2 TX buffer error.
+ * UART2_RX_BUFF_ERROR: UART2 RX buffer error.
+ * TIMER_CSR3_EXPIRE: TIMECSR3 timer expired (802.1H quiet period).
+ */
+#define CSR8				0x0020
+#define CSR8_TBCN_EXPIRE		FIELD32(0x00000001)
+#define CSR8_TWAKE_EXPIRE		FIELD32(0x00000002)
+#define CSR8_TATIMW_EXPIRE		FIELD32(0x00000004)
+#define CSR8_TXDONE_TXRING		FIELD32(0x00000008)
+#define CSR8_TXDONE_ATIMRING		FIELD32(0x00000010)
+#define CSR8_TXDONE_PRIORING		FIELD32(0x00000020)
+#define CSR8_RXDONE			FIELD32(0x00000040)
+#define CSR8_DECRYPTION_DONE		FIELD32(0x00000080)
+#define CSR8_ENCRYPTION_DONE		FIELD32(0x00000100)
+#define CSR8_UART1_TX_TRESHOLD		FIELD32(0x00000200)
+#define CSR8_UART1_RX_TRESHOLD		FIELD32(0x00000400)
+#define CSR8_UART1_IDLE_TRESHOLD	FIELD32(0x00000800)
+#define CSR8_UART1_TX_BUFF_ERROR	FIELD32(0x00001000)
+#define CSR8_UART1_RX_BUFF_ERROR	FIELD32(0x00002000)
+#define CSR8_UART2_TX_TRESHOLD		FIELD32(0x00004000)
+#define CSR8_UART2_RX_TRESHOLD		FIELD32(0x00008000)
+#define CSR8_UART2_IDLE_TRESHOLD	FIELD32(0x00010000)
+#define CSR8_UART2_TX_BUFF_ERROR	FIELD32(0x00020000)
+#define CSR8_UART2_RX_BUFF_ERROR	FIELD32(0x00040000)
+#define CSR8_TIMER_CSR3_EXPIRE		FIELD32(0x00080000)
+
+/*
+ * CSR9: Maximum frame length register.
+ * MAX_FRAME_UNIT: Maximum frame length in 128b unit, default: 12.
+ */
+#define CSR9				0x0024
+#define CSR9_MAX_FRAME_UNIT		FIELD32(0x00000f80)
+
+/*
+ * SECCSR0: WEP control register.
+ * KICK_DECRYPT: Kick decryption engine, self-clear.
+ * ONE_SHOT: 0: ring mode, 1: One shot only mode.
+ * DESC_ADDRESS: Descriptor physical address of frame.
+ */
+#define SECCSR0				0x0028
+#define SECCSR0_KICK_DECRYPT		FIELD32(0x00000001)
+#define SECCSR0_ONE_SHOT		FIELD32(0x00000002)
+#define SECCSR0_DESC_ADDRESS		FIELD32(0xfffffffc)
+
+/*
+ * CSR11: Back-off control register.
+ * CWMIN: CWmin. Default cwmin is 31 (2^5 - 1).
+ * CWMAX: CWmax. Default cwmax is 1023 (2^10 - 1).
+ * SLOT_TIME: Slot time, default is 20us for 802.11b
+ * CW_SELECT: CWmin/CWmax selection, 1: Register, 0: TXD.
+ * LONG_RETRY: Long retry count.
+ * SHORT_RETRY: Short retry count.
+ */
+#define CSR11				0x002c
+#define CSR11_CWMIN			FIELD32(0x0000000f)
+#define CSR11_CWMAX			FIELD32(0x000000f0)
+#define CSR11_SLOT_TIME			FIELD32(0x00001f00)
+#define CSR11_CW_SELECT			FIELD32(0x00002000)
+#define CSR11_LONG_RETRY		FIELD32(0x00ff0000)
+#define CSR11_SHORT_RETRY		FIELD32(0xff000000)
+
+/*
+ * CSR12: Synchronization configuration register 0.
+ * All units in 1/16 TU.
+ * BEACON_INTERVAL: Beacon interval, default is 100 TU.
+ * CFP_MAX_DURATION: Cfp maximum duration, default is 100 TU.
+ */
+#define CSR12				0x0030
+#define CSR12_BEACON_INTERVAL		FIELD32(0x0000ffff)
+#define CSR12_CFP_MAX_DURATION		FIELD32(0xffff0000)
+
+/*
+ * CSR13: Synchronization configuration register 1.
+ * All units in 1/16 TU.
+ * ATIMW_DURATION: Atim window duration.
+ * CFP_PERIOD: Cfp period, default is 0 TU.
+ */
+#define CSR13				0x0034
+#define CSR13_ATIMW_DURATION		FIELD32(0x0000ffff)
+#define CSR13_CFP_PERIOD		FIELD32(0x00ff0000)
+
+/*
+ * CSR14: Synchronization control register.
+ * TSF_COUNT: Enable tsf auto counting.
+ * TSF_SYNC: Tsf sync, 0: disable, 1: infra, 2: ad-hoc/master mode.
+ * TBCN: Enable tbcn with reload value.
+ * TCFP: Enable tcfp & cfp / cp switching.
+ * TATIMW: Enable tatimw & atim window switching.
+ * BEACON_GEN: Enable beacon generator.
+ * CFP_COUNT_PRELOAD: Cfp count preload value.
+ * TBCM_PRELOAD: Tbcn preload value in units of 64us.
+ */
+#define CSR14				0x0038
+#define CSR14_TSF_COUNT			FIELD32(0x00000001)
+#define CSR14_TSF_SYNC			FIELD32(0x00000006)
+#define CSR14_TBCN			FIELD32(0x00000008)
+#define CSR14_TCFP			FIELD32(0x00000010)
+#define CSR14_TATIMW			FIELD32(0x00000020)
+#define CSR14_BEACON_GEN		FIELD32(0x00000040)
+#define CSR14_CFP_COUNT_PRELOAD		FIELD32(0x0000ff00)
+#define CSR14_TBCM_PRELOAD		FIELD32(0xffff0000)
+
+/*
+ * CSR15: Synchronization status register.
+ * CFP: ASIC is in contention-free period.
+ * ATIMW: ASIC is in ATIM window.
+ * BEACON_SENT: Beacon is send.
+ */
+#define CSR15				0x003c
+#define CSR15_CFP			FIELD32(0x00000001)
+#define CSR15_ATIMW			FIELD32(0x00000002)
+#define CSR15_BEACON_SENT		FIELD32(0x00000004)
+
+/*
+ * CSR16: TSF timer register 0.
+ */
+#define CSR16				0x0040
+#define CSR16_LOW_TSFTIMER		FIELD32(0xffffffff)
+
+/*
+ * CSR17: TSF timer register 1.
+ */
+#define CSR17				0x0044
+#define CSR17_HIGH_TSFTIMER		FIELD32(0xffffffff)
+
+/*
+ * CSR18: IFS timer register 0.
+ * SIFS: Sifs, default is 10 us.
+ * PIFS: Pifs, default is 30 us.
+ */
+#define CSR18				0x0048
+#define CSR18_SIFS			FIELD32(0x000001ff)
+#define CSR18_PIFS			FIELD32(0x001f0000)
+
+/*
+ * CSR19: IFS timer register 1.
+ * DIFS: Difs, default is 50 us.
+ * EIFS: Eifs, default is 364 us.
+ */
+#define CSR19				0x004c
+#define CSR19_DIFS			FIELD32(0x0000ffff)
+#define CSR19_EIFS			FIELD32(0xffff0000)
+
+/*
+ * CSR20: Wakeup timer register.
+ * DELAY_AFTER_TBCN: Delay after tbcn expired in units of 1/16 TU.
+ * TBCN_BEFORE_WAKEUP: Number of beacon before wakeup.
+ * AUTOWAKE: Enable auto wakeup / sleep mechanism.
+ */
+#define CSR20				0x0050
+#define CSR20_DELAY_AFTER_TBCN		FIELD32(0x0000ffff)
+#define CSR20_TBCN_BEFORE_WAKEUP	FIELD32(0x00ff0000)
+#define CSR20_AUTOWAKE			FIELD32(0x01000000)
+
+/*
+ * CSR21: EEPROM control register.
+ * RELOAD: Write 1 to reload eeprom content.
+ * TYPE_93C46: 1: 93c46, 0:93c66.
+ */
+#define CSR21				0x0054
+#define CSR21_RELOAD			FIELD32(0x00000001)
+#define CSR21_EEPROM_DATA_CLOCK		FIELD32(0x00000002)
+#define CSR21_EEPROM_CHIP_SELECT	FIELD32(0x00000004)
+#define CSR21_EEPROM_DATA_IN		FIELD32(0x00000008)
+#define CSR21_EEPROM_DATA_OUT		FIELD32(0x00000010)
+#define CSR21_TYPE_93C46		FIELD32(0x00000020)
+
+/*
+ * CSR22: CFP control register.
+ * CFP_DURATION_REMAIN: Cfp duration remain, in units of TU.
+ * RELOAD_CFP_DURATION: Write 1 to reload cfp duration remain.
+ */
+#define CSR22				0x0058
+#define CSR22_CFP_DURATION_REMAIN	FIELD32(0x0000ffff)
+#define CSR22_RELOAD_CFP_DURATION	FIELD32(0x00010000)
+
+/*
+ * Transmit related CSRs.
+ * Some values are set in TU, whereas 1 TU == 1024 us.
+ */
+
+/*
+ * TXCSR0: TX Control Register.
+ * KICK_TX: Kick tx ring.
+ * KICK_ATIM: Kick atim ring.
+ * KICK_PRIO: Kick priority ring.
+ * ABORT: Abort all transmit related ring operation.
+ */
+#define TXCSR0				0x0060
+#define TXCSR0_KICK_TX			FIELD32(0x00000001)
+#define TXCSR0_KICK_ATIM		FIELD32(0x00000002)
+#define TXCSR0_KICK_PRIO		FIELD32(0x00000004)
+#define TXCSR0_ABORT			FIELD32(0x00000008)
+
+/*
+ * TXCSR1: TX Configuration Register.
+ * ACK_TIMEOUT: Ack timeout, default = sifs + 2*slottime + acktime @ 1mbps.
+ * ACK_CONSUME_TIME: Ack consume time, default = sifs + acktime @ 1mbps.
+ * TSF_OFFSET: Insert tsf offset.
+ * AUTORESPONDER: Enable auto responder which include ack & cts.
+ */
+#define TXCSR1				0x0064
+#define TXCSR1_ACK_TIMEOUT		FIELD32(0x000001ff)
+#define TXCSR1_ACK_CONSUME_TIME		FIELD32(0x0003fe00)
+#define TXCSR1_TSF_OFFSET		FIELD32(0x00fc0000)
+#define TXCSR1_AUTORESPONDER		FIELD32(0x01000000)
+
+/*
+ * TXCSR2: Tx descriptor configuration register.
+ * TXD_SIZE: Tx descriptor size, default is 48.
+ * NUM_TXD: Number of tx entries in ring.
+ * NUM_ATIM: Number of atim entries in ring.
+ * NUM_PRIO: Number of priority entries in ring.
+ */
+#define TXCSR2				0x0068
+#define TXCSR2_TXD_SIZE			FIELD32(0x000000ff)
+#define TXCSR2_NUM_TXD			FIELD32(0x0000ff00)
+#define TXCSR2_NUM_ATIM			FIELD32(0x00ff0000)
+#define TXCSR2_NUM_PRIO			FIELD32(0xff000000)
+
+/*
+ * TXCSR3: TX Ring Base address register.
+ */
+#define TXCSR3				0x006c
+#define TXCSR3_TX_RING_REGISTER		FIELD32(0xffffffff)
+
+/*
+ * TXCSR4: TX Atim Ring Base address register.
+ */
+#define TXCSR4				0x0070
+#define TXCSR4_ATIM_RING_REGISTER	FIELD32(0xffffffff)
+
+/*
+ * TXCSR5: TX Prio Ring Base address register.
+ */
+#define TXCSR5				0x0074
+#define TXCSR5_PRIO_RING_REGISTER	FIELD32(0xffffffff)
+
+/*
+ * TXCSR6: Beacon Base address register.
+ */
+#define TXCSR6				0x0078
+#define TXCSR6_BEACON_RING_REGISTER	FIELD32(0xffffffff)
+
+/*
+ * TXCSR7: Auto responder control register.
+ * AR_POWERMANAGEMENT: Auto responder power management bit.
+ */
+#define TXCSR7				0x007c
+#define TXCSR7_AR_POWERMANAGEMENT	FIELD32(0x00000001)
+
+/*
+ * TXCSR8: CCK Tx BBP register.
+ */
+#define TXCSR8				0x0098
+#define TXCSR8_BBP_ID0			FIELD32(0x0000007f)
+#define TXCSR8_BBP_ID0_VALID		FIELD32(0x00000080)
+#define TXCSR8_BBP_ID1			FIELD32(0x00007f00)
+#define TXCSR8_BBP_ID1_VALID		FIELD32(0x00008000)
+#define TXCSR8_BBP_ID2			FIELD32(0x007f0000)
+#define TXCSR8_BBP_ID2_VALID		FIELD32(0x00800000)
+#define TXCSR8_BBP_ID3			FIELD32(0x7f000000)
+#define TXCSR8_BBP_ID3_VALID		FIELD32(0x80000000)
+
+/*
+ * TXCSR9: OFDM TX BBP registers
+ * OFDM_SIGNAL: BBP rate field address for OFDM.
+ * OFDM_SERVICE: BBP service field address for OFDM.
+ * OFDM_LENGTH_LOW: BBP length low byte address for OFDM.
+ * OFDM_LENGTH_HIGH: BBP length high byte address for OFDM.
+ */
+#define TXCSR9				0x0094
+#define TXCSR9_OFDM_RATE		FIELD32(0x000000ff)
+#define TXCSR9_OFDM_SERVICE		FIELD32(0x0000ff00)
+#define TXCSR9_OFDM_LENGTH_LOW		FIELD32(0x00ff0000)
+#define TXCSR9_OFDM_LENGTH_HIGH		FIELD32(0xff000000)
+
+/*
+ * Receive related CSRs.
+ * Some values are set in TU, whereas 1 TU == 1024 us.
+ */
+
+/*
+ * RXCSR0: RX Control Register.
+ * DISABLE_RX: Disable rx engine.
+ * DROP_CRC: Drop crc error.
+ * DROP_PHYSICAL: Drop physical error.
+ * DROP_CONTROL: Drop control frame.
+ * DROP_NOT_TO_ME: Drop not to me unicast frame.
+ * DROP_TODS: Drop frame tods bit is true.
+ * DROP_VERSION_ERROR: Drop version error frame.
+ * PASS_CRC: Pass all packets with crc attached.
+ * PASS_CRC: Pass all packets with crc attached.
+ * PASS_PLCP: Pass all packets with 4 bytes PLCP attached.
+ * DROP_MCAST: Drop multicast frames.
+ * DROP_BCAST: Drop broadcast frames.
+ * ENABLE_QOS: Accept QOS data frame and parse QOS field.
+ */
+#define RXCSR0				0x0080
+#define RXCSR0_DISABLE_RX		FIELD32(0x00000001)
+#define RXCSR0_DROP_CRC			FIELD32(0x00000002)
+#define RXCSR0_DROP_PHYSICAL		FIELD32(0x00000004)
+#define RXCSR0_DROP_CONTROL		FIELD32(0x00000008)
+#define RXCSR0_DROP_NOT_TO_ME		FIELD32(0x00000010)
+#define RXCSR0_DROP_TODS		FIELD32(0x00000020)
+#define RXCSR0_DROP_VERSION_ERROR	FIELD32(0x00000040)
+#define RXCSR0_PASS_CRC			FIELD32(0x00000080)
+#define RXCSR0_PASS_PLCP		FIELD32(0x00000100)
+#define RXCSR0_DROP_MCAST		FIELD32(0x00000200)
+#define RXCSR0_DROP_BCAST		FIELD32(0x00000400)
+#define RXCSR0_ENABLE_QOS		FIELD32(0x00000800)
+
+/*
+ * RXCSR1: RX descriptor configuration register.
+ * RXD_SIZE: Rx descriptor size, default is 32b.
+ * NUM_RXD: Number of rx entries in ring.
+ */
+#define RXCSR1				0x0084
+#define RXCSR1_RXD_SIZE			FIELD32(0x000000ff)
+#define RXCSR1_NUM_RXD			FIELD32(0x0000ff00)
+
+/*
+ * RXCSR2: RX Ring base address register.
+ */
+#define RXCSR2				0x0088
+#define RXCSR2_RX_RING_REGISTER		FIELD32(0xffffffff)
+
+/*
+ * RXCSR3: BBP ID register for Rx operation.
+ * BBP_ID#: BBP register # id.
+ * BBP_ID#_VALID: BBP register # id is valid or not.
+ */
+#define RXCSR3				0x0090
+#define RXCSR3_BBP_ID0			FIELD32(0x0000007f)
+#define RXCSR3_BBP_ID0_VALID		FIELD32(0x00000080)
+#define RXCSR3_BBP_ID1			FIELD32(0x00007f00)
+#define RXCSR3_BBP_ID1_VALID		FIELD32(0x00008000)
+#define RXCSR3_BBP_ID2			FIELD32(0x007f0000)
+#define RXCSR3_BBP_ID2_VALID		FIELD32(0x00800000)
+#define RXCSR3_BBP_ID3			FIELD32(0x7f000000)
+#define RXCSR3_BBP_ID3_VALID		FIELD32(0x80000000)
+
+/*
+ * ARCSR1: Auto Responder PLCP config register 1.
+ * AR_BBP_DATA#: Auto responder BBP register # data.
+ * AR_BBP_ID#: Auto responder BBP register # Id.
+ */
+#define ARCSR1				0x009c
+#define ARCSR1_AR_BBP_DATA2		FIELD32(0x000000ff)
+#define ARCSR1_AR_BBP_ID2		FIELD32(0x0000ff00)
+#define ARCSR1_AR_BBP_DATA3		FIELD32(0x00ff0000)
+#define ARCSR1_AR_BBP_ID3		FIELD32(0xff000000)
+
+/*
+ * Miscellaneous Registers.
+ * Some values are set in TU, whereas 1 TU == 1024 us.
+
+ */
+
+/*
+ * PCICSR: PCI control register.
+ * BIG_ENDIAN: 1: big endian, 0: little endian.
+ * RX_TRESHOLD: Rx threshold in dw to start pci access
+ * 0: 16dw (default), 1: 8dw, 2: 4dw, 3: 32dw.
+ * TX_TRESHOLD: Tx threshold in dw to start pci access
+ * 0: 0dw (default), 1: 1dw, 2: 4dw, 3: forward.
+ * BURST_LENTH: Pci burst length 0: 4dw (default, 1: 8dw, 2: 16dw, 3:32dw.
+ * ENABLE_CLK: Enable clk_run, pci clock can't going down to non-operational.
+ * READ_MULTIPLE: Enable memory read multiple.
+ * WRITE_INVALID: Enable memory write & invalid.
+ */
+#define PCICSR				0x008c
+#define PCICSR_BIG_ENDIAN		FIELD32(0x00000001)
+#define PCICSR_RX_TRESHOLD		FIELD32(0x00000006)
+#define PCICSR_TX_TRESHOLD		FIELD32(0x00000018)
+#define PCICSR_BURST_LENTH		FIELD32(0x00000060)
+#define PCICSR_ENABLE_CLK		FIELD32(0x00000080)
+#define PCICSR_READ_MULTIPLE		FIELD32(0x00000100)
+#define PCICSR_WRITE_INVALID		FIELD32(0x00000200)
+
+/*
+ * CNT0: FCS error count.
+ * FCS_ERROR: FCS error count, cleared when read.
+ */
+#define CNT0				0x00a0
+#define CNT0_FCS_ERROR			FIELD32(0x0000ffff)
+
+/*
+ * Statistic Register.
+ * CNT1: PLCP error count.
+ * CNT2: Long error count.
+ */
+#define TIMECSR2			0x00a8
+#define CNT1				0x00ac
+#define CNT2				0x00b0
+#define TIMECSR3			0x00b4
+
+/*
+ * CNT3: CCA false alarm count.
+ */
+#define CNT3				0x00b8
+#define CNT3_FALSE_CCA			FIELD32(0x0000ffff)
+
+/*
+ * Statistic Register.
+ * CNT4: Rx FIFO overflow count.
+ * CNT5: Tx FIFO underrun count.
+ */
+#define CNT4				0x00bc
+#define CNT5				0x00c0
+
+/*
+ * Baseband Control Register.
+ */
+
+/*
+ * PWRCSR0: Power mode configuration register.
+ */
+#define PWRCSR0				0x00c4
+
+/*
+ * Power state transition time registers.
+ */
+#define PSCSR0				0x00c8
+#define PSCSR1				0x00cc
+#define PSCSR2				0x00d0
+#define PSCSR3				0x00d4
+
+/*
+ * PWRCSR1: Manual power control / status register.
+ * Allowed state: 0 deep_sleep, 1: sleep, 2: standby, 3: awake.
+ * SET_STATE: Set state. Write 1 to trigger, self cleared.
+ * BBP_DESIRE_STATE: BBP desired state.
+ * RF_DESIRE_STATE: RF desired state.
+ * BBP_CURR_STATE: BBP current state.
+ * RF_CURR_STATE: RF current state.
+ * PUT_TO_SLEEP: Put to sleep. Write 1 to trigger, self cleared.
+ */
+#define PWRCSR1				0x00d8
+#define PWRCSR1_SET_STATE		FIELD32(0x00000001)
+#define PWRCSR1_BBP_DESIRE_STATE	FIELD32(0x00000006)
+#define PWRCSR1_RF_DESIRE_STATE		FIELD32(0x00000018)
+#define PWRCSR1_BBP_CURR_STATE		FIELD32(0x00000060)
+#define PWRCSR1_RF_CURR_STATE		FIELD32(0x00000180)
+#define PWRCSR1_PUT_TO_SLEEP		FIELD32(0x00000200)
+
+/*
+ * TIMECSR: Timer control register.
+ * US_COUNT: 1 us timer count in units of clock cycles.
+ * US_64_COUNT: 64 us timer count in units of 1 us timer.
+ * BEACON_EXPECT: Beacon expect window.
+ */
+#define TIMECSR				0x00dc
+#define TIMECSR_US_COUNT		FIELD32(0x000000ff)
+#define TIMECSR_US_64_COUNT		FIELD32(0x0000ff00)
+#define TIMECSR_BEACON_EXPECT		FIELD32(0x00070000)
+
+/*
+ * MACCSR0: MAC configuration register 0.
+ */
+#define MACCSR0				0x00e0
+
+/*
+ * MACCSR1: MAC configuration register 1.
+ * KICK_RX: Kick one-shot rx in one-shot rx mode.
+ * ONESHOT_RXMODE: Enable one-shot rx mode for debugging.
+ * BBPRX_RESET_MODE: Ralink bbp rx reset mode.
+ * AUTO_TXBBP: Auto tx logic access bbp control register.
+ * AUTO_RXBBP: Auto rx logic access bbp control register.
+ * LOOPBACK: Loopback mode. 0: normal, 1: internal, 2: external, 3:rsvd.
+ * INTERSIL_IF: Intersil if calibration pin.
+ */
+#define MACCSR1				0x00e4
+#define MACCSR1_KICK_RX			FIELD32(0x00000001)
+#define MACCSR1_ONESHOT_RXMODE		FIELD32(0x00000002)
+#define MACCSR1_BBPRX_RESET_MODE	FIELD32(0x00000004)
+#define MACCSR1_AUTO_TXBBP		FIELD32(0x00000008)
+#define MACCSR1_AUTO_RXBBP		FIELD32(0x00000010)
+#define MACCSR1_LOOPBACK		FIELD32(0x00000060)
+#define MACCSR1_INTERSIL_IF		FIELD32(0x00000080)
+
+/*
+ * RALINKCSR: Ralink Rx auto-reset BBCR.
+ * AR_BBP_DATA#: Auto reset BBP register # data.
+ * AR_BBP_ID#: Auto reset BBP register # id.
+ */
+#define RALINKCSR			0x00e8
+#define RALINKCSR_AR_BBP_DATA0		FIELD32(0x000000ff)
+#define RALINKCSR_AR_BBP_ID0		FIELD32(0x00007f00)
+#define RALINKCSR_AR_BBP_VALID0		FIELD32(0x00008000)
+#define RALINKCSR_AR_BBP_DATA1		FIELD32(0x00ff0000)
+#define RALINKCSR_AR_BBP_ID1		FIELD32(0x7f000000)
+#define RALINKCSR_AR_BBP_VALID1		FIELD32(0x80000000)
+
+/*
+ * BCNCSR: Beacon interval control register.
+ * CHANGE: Write one to change beacon interval.
+ * DELTATIME: The delta time value.
+ * NUM_BEACON: Number of beacon according to mode.
+ * MODE: Please refer to asic specs.
+ * PLUS: Plus or minus delta time value.
+ */
+#define BCNCSR				0x00ec
+#define BCNCSR_CHANGE			FIELD32(0x00000001)
+#define BCNCSR_DELTATIME		FIELD32(0x0000001e)
+#define BCNCSR_NUM_BEACON		FIELD32(0x00001fe0)
+#define BCNCSR_MODE			FIELD32(0x00006000)
+#define BCNCSR_PLUS			FIELD32(0x00008000)
+
+/*
+ * BBP / RF / IF Control Register.
+ */
+
+/*
+ * BBPCSR: BBP serial control register.
+ * VALUE: Register value to program into BBP.
+ * REGNUM: Selected BBP register.
+ * BUSY: 1: asic is busy execute BBP programming.
+ * WRITE_CONTROL: 1: write BBP, 0: read BBP.
+ */
+#define BBPCSR				0x00f0
+#define BBPCSR_VALUE			FIELD32(0x000000ff)
+#define BBPCSR_REGNUM			FIELD32(0x00007f00)
+#define BBPCSR_BUSY			FIELD32(0x00008000)
+#define BBPCSR_WRITE_CONTROL		FIELD32(0x00010000)
+
+/*
+ * RFCSR: RF serial control register.
+ * VALUE: Register value + id to program into rf/if.
+ * NUMBER_OF_BITS: Number of bits used in value (i:20, rfmd:22).
+ * IF_SELECT: Chip to program: 0: rf, 1: if.
+ * PLL_LD: Rf pll_ld status.
+ * BUSY: 1: asic is busy execute rf programming.
+ */
+#define RFCSR				0x00f4
+#define RFCSR_VALUE			FIELD32(0x00ffffff)
+#define RFCSR_NUMBER_OF_BITS		FIELD32(0x1f000000)
+#define RFCSR_IF_SELECT			FIELD32(0x20000000)
+#define RFCSR_PLL_LD			FIELD32(0x40000000)
+#define RFCSR_BUSY			FIELD32(0x80000000)
+
+/*
+ * LEDCSR: LED control register.
+ * ON_PERIOD: On period, default 70ms.
+ * OFF_PERIOD: Off period, default 30ms.
+ * LINK: 0: linkoff, 1: linkup.
+ * ACTIVITY: 0: idle, 1: active.
+ * LINK_POLARITY: 0: active low, 1: active high.
+ * ACTIVITY_POLARITY: 0: active low, 1: active high.
+ * LED_DEFAULT: LED state for "enable" 0: ON, 1: OFF.
+ */
+#define LEDCSR				0x00f8
+#define LEDCSR_ON_PERIOD		FIELD32(0x000000ff)
+#define LEDCSR_OFF_PERIOD		FIELD32(0x0000ff00)
+#define LEDCSR_LINK			FIELD32(0x00010000)
+#define LEDCSR_ACTIVITY			FIELD32(0x00020000)
+#define LEDCSR_LINK_POLARITY		FIELD32(0x00040000)
+#define LEDCSR_ACTIVITY_POLARITY	FIELD32(0x00080000)
+#define LEDCSR_LED_DEFAULT		FIELD32(0x00100000)
+
+/*
+ * AES control register.
+ */
+#define SECCSR3				0x00fc
+
+/*
+ * ASIC pointer information.
+ * RXPTR: Current RX ring address.
+ * TXPTR: Current Tx ring address.
+ * PRIPTR: Current Priority ring address.
+ * ATIMPTR: Current ATIM ring address.
+ */
+#define RXPTR				0x0100
+#define TXPTR				0x0104
+#define PRIPTR				0x0108
+#define ATIMPTR				0x010c
+
+/*
+ * TXACKCSR0: TX ACK timeout.
+ */
+#define TXACKCSR0			0x0110
+
+/*
+ * ACK timeout count registers.
+ * ACKCNT0: TX ACK timeout count.
+ * ACKCNT1: RX ACK timeout count.
+ */
+#define ACKCNT0				0x0114
+#define ACKCNT1				0x0118
+
+/*
+ * GPIO and others.
+ */
+
+/*
+ * GPIOCSR: GPIO control register.
+ */
+#define GPIOCSR				0x0120
+#define GPIOCSR_BIT0			FIELD32(0x00000001)
+#define GPIOCSR_BIT1			FIELD32(0x00000002)
+#define GPIOCSR_BIT2			FIELD32(0x00000004)
+#define GPIOCSR_BIT3			FIELD32(0x00000008)
+#define GPIOCSR_BIT4			FIELD32(0x00000010)
+#define GPIOCSR_BIT5			FIELD32(0x00000020)
+#define GPIOCSR_BIT6			FIELD32(0x00000040)
+#define GPIOCSR_BIT7			FIELD32(0x00000080)
+#define GPIOCSR_DIR0			FIELD32(0x00000100)
+#define GPIOCSR_DIR1			FIELD32(0x00000200)
+#define GPIOCSR_DIR2			FIELD32(0x00000400)
+#define GPIOCSR_DIR3			FIELD32(0x00000800)
+#define GPIOCSR_DIR4			FIELD32(0x00001000)
+#define GPIOCSR_DIR5			FIELD32(0x00002000)
+#define GPIOCSR_DIR6			FIELD32(0x00004000)
+#define GPIOCSR_DIR7			FIELD32(0x00008000)
+
+/*
+ * FIFO pointer registers.
+ * FIFOCSR0: TX FIFO pointer.
+ * FIFOCSR1: RX FIFO pointer.
+ */
+#define FIFOCSR0			0x0128
+#define FIFOCSR1			0x012c
+
+/*
+ * BCNCSR1: Tx BEACON offset time control register.
+ * PRELOAD: Beacon timer offset in units of usec.
+ * BEACON_CWMIN: 2^CwMin.
+ */
+#define BCNCSR1				0x0130
+#define BCNCSR1_PRELOAD			FIELD32(0x0000ffff)
+#define BCNCSR1_BEACON_CWMIN		FIELD32(0x000f0000)
+
+/*
+ * MACCSR2: TX_PE to RX_PE turn-around time control register
+ * DELAY: RX_PE low width, in units of pci clock cycle.
+ */
+#define MACCSR2				0x0134
+#define MACCSR2_DELAY			FIELD32(0x000000ff)
+
+/*
+ * TESTCSR: TEST mode selection register.
+ */
+#define TESTCSR				0x0138
+
+/*
+ * ARCSR2: 1 Mbps ACK/CTS PLCP.
+ */
+#define ARCSR2				0x013c
+#define ARCSR2_SIGNAL			FIELD32(0x000000ff)
+#define ARCSR2_SERVICE			FIELD32(0x0000ff00)
+#define ARCSR2_LENGTH			FIELD32(0xffff0000)
+
+/*
+ * ARCSR3: 2 Mbps ACK/CTS PLCP.
+ */
+#define ARCSR3				0x0140
+#define ARCSR3_SIGNAL			FIELD32(0x000000ff)
+#define ARCSR3_SERVICE			FIELD32(0x0000ff00)
+#define ARCSR3_LENGTH			FIELD32(0xffff0000)
+
+/*
+ * ARCSR4: 5.5 Mbps ACK/CTS PLCP.
+ */
+#define ARCSR4				0x0144
+#define ARCSR4_SIGNAL			FIELD32(0x000000ff)
+#define ARCSR4_SERVICE			FIELD32(0x0000ff00)
+#define ARCSR4_LENGTH			FIELD32(0xffff0000)
+
+/*
+ * ARCSR5: 11 Mbps ACK/CTS PLCP.
+ */
+#define ARCSR5				0x0148
+#define ARCSR5_SIGNAL			FIELD32(0x000000ff)
+#define ARCSR5_SERVICE			FIELD32(0x0000ff00)
+#define ARCSR5_LENGTH			FIELD32(0xffff0000)
+
+/*
+ * ARTCSR0: CCK ACK/CTS payload consumed time for 1/2/5.5/11 mbps.
+ */
+#define ARTCSR0				0x014c
+#define ARTCSR0_ACK_CTS_11MBS		FIELD32(0x000000ff)
+#define ARTCSR0_ACK_CTS_5_5MBS		FIELD32(0x0000ff00)
+#define ARTCSR0_ACK_CTS_2MBS		FIELD32(0x00ff0000)
+#define ARTCSR0_ACK_CTS_1MBS		FIELD32(0xff000000)
+
+
+/*
+ * ARTCSR1: OFDM ACK/CTS payload consumed time for 6/9/12/18 mbps.
+ */
+#define ARTCSR1				0x0150
+#define ARTCSR1_ACK_CTS_6MBS		FIELD32(0x000000ff)
+#define ARTCSR1_ACK_CTS_9MBS		FIELD32(0x0000ff00)
+#define ARTCSR1_ACK_CTS_12MBS		FIELD32(0x00ff0000)
+#define ARTCSR1_ACK_CTS_18MBS		FIELD32(0xff000000)
+
+/*
+ * ARTCSR2: OFDM ACK/CTS payload consumed time for 24/36/48/54 mbps.
+ */
+#define ARTCSR2				0x0154
+#define ARTCSR2_ACK_CTS_24MBS		FIELD32(0x000000ff)
+#define ARTCSR2_ACK_CTS_36MBS		FIELD32(0x0000ff00)
+#define ARTCSR2_ACK_CTS_48MBS		FIELD32(0x00ff0000)
+#define ARTCSR2_ACK_CTS_54MBS		FIELD32(0xff000000)
+
+/*
+ * SECCSR1_RT2509: WEP control register.
+ * KICK_ENCRYPT: Kick encryption engine, self-clear.
+ * ONE_SHOT: 0: ring mode, 1: One shot only mode.
+ * DESC_ADDRESS: Descriptor physical address of frame.
+ */
+#define SECCSR1				0x0158
+#define SECCSR1_KICK_ENCRYPT		FIELD32(0x00000001)
+#define SECCSR1_ONE_SHOT		FIELD32(0x00000002)
+#define SECCSR1_DESC_ADDRESS		FIELD32(0xfffffffc)
+
+/*
+ * BBPCSR1: BBP TX configuration.
+ */
+#define BBPCSR1				0x015c
+#define BBPCSR1_CCK			FIELD32(0x00000003)
+#define BBPCSR1_CCK_FLIP		FIELD32(0x00000004)
+#define BBPCSR1_OFDM			FIELD32(0x00030000)
+#define BBPCSR1_OFDM_FLIP		FIELD32(0x00040000)
+
+/*
+ * Dual band configuration registers.
+ * DBANDCSR0: Dual band configuration register 0.
+ * DBANDCSR1: Dual band configuration register 1.
+ */
+#define DBANDCSR0			0x0160
+#define DBANDCSR1			0x0164
+
+/*
+ * BBPPCSR: BBP Pin control register.
+ */
+#define BBPPCSR				0x0168
+
+/*
+ * MAC special debug mode selection registers.
+ * DBGSEL0: MAC special debug mode selection register 0.
+ * DBGSEL1: MAC special debug mode selection register 1.
+ */
+#define DBGSEL0				0x016c
+#define DBGSEL1				0x0170
+
+/*
+ * BISTCSR: BBP BIST register.
+ */
+#define BISTCSR				0x0174
+
+/*
+ * Multicast filter registers.
+ * MCAST0: Multicast filter register 0.
+ * MCAST1: Multicast filter register 1.
+ */
+#define MCAST0				0x0178
+#define MCAST1				0x017c
+
+/*
+ * UART registers.
+ * UARTCSR0: UART1 TX register.
+ * UARTCSR1: UART1 RX register.
+ * UARTCSR3: UART1 frame control register.
+ * UARTCSR4: UART1 buffer control register.
+ * UART2CSR0: UART2 TX register.
+ * UART2CSR1: UART2 RX register.
+ * UART2CSR3: UART2 frame control register.
+ * UART2CSR4: UART2 buffer control register.
+ */
+#define UARTCSR0			0x0180
+#define UARTCSR1			0x0184
+#define UARTCSR3			0x0188
+#define UARTCSR4			0x018c
+#define UART2CSR0			0x0190
+#define UART2CSR1			0x0194
+#define UART2CSR3			0x0198
+#define UART2CSR4			0x019c
+
+/*
+ * BBP registers.
+ * The wordsize of the BBP is 8 bits.
+ */
+
+/*
+ * R2: TX antenna control
+ */
+#define BBP_R2_TX_ANTENNA		FIELD8(0x03)
+#define BBP_R2_TX_IQ_FLIP		FIELD8(0x04)
+
+/*
+ * R14: RX antenna control
+ */
+#define BBP_R14_RX_ANTENNA		FIELD8(0x03)
+#define BBP_R14_RX_IQ_FLIP		FIELD8(0x04)
+
+/*
+ * BBP_R70
+ */
+#define BBP_R70_JAPAN_FILTER		FIELD8(0x08)
+
+/*
+ * RF registers
+ */
+
+/*
+ * RF 1
+ */
+#define RF1_TUNER			FIELD32(0x00020000)
+
+/*
+ * RF 3
+ */
+#define RF3_TUNER			FIELD32(0x00000100)
+#define RF3_TXPOWER			FIELD32(0x00003e00)
+
+/*
+ * EEPROM content.
+ * The wordsize of the EEPROM is 16 bits.
+ */
+
+/*
+ * HW MAC address.
+ */
+#define EEPROM_MAC_ADDR_0		0x0002
+#define EEPROM_MAC_ADDR_BYTE0		FIELD16(0x00ff)
+#define EEPROM_MAC_ADDR_BYTE1		FIELD16(0xff00)
+#define EEPROM_MAC_ADDR1		0x0003
+#define EEPROM_MAC_ADDR_BYTE2		FIELD16(0x00ff)
+#define EEPROM_MAC_ADDR_BYTE3		FIELD16(0xff00)
+#define EEPROM_MAC_ADDR_2		0x0004
+#define EEPROM_MAC_ADDR_BYTE4		FIELD16(0x00ff)
+#define EEPROM_MAC_ADDR_BYTE5		FIELD16(0xff00)
+
+/*
+ * EEPROM antenna.
+ * ANTENNA_NUM: Number of antenna's.
+ * TX_DEFAULT: Default antenna 0: diversity, 1: A, 2: B.
+ * RX_DEFAULT: Default antenna 0: diversity, 1: A, 2: B.
+ * LED_MODE: 0: default, 1: TX/RX activity,2: Single (ignore link), 3: rsvd.
+ * DYN_TXAGC: Dynamic TX AGC control.
+ * HARDWARE_RADIO: 1: Hardware controlled radio. Read GPIO0.
+ * RF_TYPE: Rf_type of this adapter.
+ */
+#define EEPROM_ANTENNA			0x10
+#define EEPROM_ANTENNA_NUM		FIELD16(0x0003)
+#define EEPROM_ANTENNA_TX_DEFAULT	FIELD16(0x000c)
+#define EEPROM_ANTENNA_RX_DEFAULT	FIELD16(0x0030)
+#define EEPROM_ANTENNA_LED_MODE		FIELD16(0x01c0)
+#define EEPROM_ANTENNA_DYN_TXAGC	FIELD16(0x0200)
+#define EEPROM_ANTENNA_HARDWARE_RADIO	FIELD16(0x0400)
+#define EEPROM_ANTENNA_RF_TYPE		FIELD16(0xf800)
+
+/*
+ * EEPROM NIC config.
+ * CARDBUS_ACCEL: 0: enable, 1: disable.
+ * DYN_BBP_TUNE: 0: enable, 1: disable.
+ * CCK_TX_POWER: CCK TX power compensation.
+ */
+#define EEPROM_NIC			0x11
+#define EEPROM_NIC_CARDBUS_ACCEL	FIELD16(0x0001)
+#define EEPROM_NIC_DYN_BBP_TUNE		FIELD16(0x0002)
+#define EEPROM_NIC_CCK_TX_POWER		FIELD16(0x000c)
+
+/*
+ * EEPROM geography.
+ * GEO: Default geography setting for device.
+ */
+#define EEPROM_GEOGRAPHY		0x12
+#define EEPROM_GEOGRAPHY_GEO		FIELD16(0x0f00)
+
+/*
+ * EEPROM BBP.
+ */
+#define EEPROM_BBP_START		0x13
+#define EEPROM_BBP_SIZE			16
+#define EEPROM_BBP_VALUE		FIELD16(0x00ff)
+#define EEPROM_BBP_REG_ID		FIELD16(0xff00)
+
+/*
+ * EEPROM TXPOWER
+ */
+#define EEPROM_TXPOWER_START		0x23
+#define EEPROM_TXPOWER_SIZE		7
+#define EEPROM_TXPOWER_1		FIELD16(0x00ff)
+#define EEPROM_TXPOWER_2		FIELD16(0xff00)
+
+/*
+ * RSSI <-> dBm offset calibration
+ */
+#define EEPROM_CALIBRATE_OFFSET		0x3e
+#define EEPROM_CALIBRATE_OFFSET_RSSI	FIELD16(0x00ff)
+
+/*
+ * DMA descriptor defines.
+ */
+#define TXD_DESC_SIZE			( 11 * sizeof(struct data_desc) )
+#define RXD_DESC_SIZE			( 11 * sizeof(struct data_desc) )
+
+/*
+ * TX descriptor format for TX, PRIO, ATIM and Beacon Ring.
+ */
+
+/*
+ * Word0
+ */
+#define TXD_W0_OWNER_NIC		FIELD32(0x00000001)
+#define TXD_W0_VALID			FIELD32(0x00000002)
+#define TXD_W0_RESULT			FIELD32(0x0000001c)
+#define TXD_W0_RETRY_COUNT		FIELD32(0x000000e0)
+#define TXD_W0_MORE_FRAG		FIELD32(0x00000100)
+#define TXD_W0_ACK			FIELD32(0x00000200)
+#define TXD_W0_TIMESTAMP		FIELD32(0x00000400)
+#define TXD_W0_OFDM			FIELD32(0x00000800)
+#define TXD_W0_CIPHER_OWNER		FIELD32(0x00001000)
+#define TXD_W0_IFS			FIELD32(0x00006000)
+#define TXD_W0_RETRY_MODE		FIELD32(0x00008000)
+#define TXD_W0_DATABYTE_COUNT		FIELD32(0x0fff0000)
+#define TXD_W0_CIPHER_ALG		FIELD32(0xe0000000)
+
+/*
+ * Word1
+ */
+#define TXD_W1_BUFFER_ADDRESS		FIELD32(0xffffffff)
+
+/*
+ * Word2
+ */
+#define TXD_W2_IV_OFFSET		FIELD32(0x0000003f)
+#define TXD_W2_AIFS			FIELD32(0x000000c0)
+#define TXD_W2_CWMIN			FIELD32(0x00000f00)
+#define TXD_W2_CWMAX			FIELD32(0x0000f000)
+
+/*
+ * Word3: PLCP information
+ */
+#define TXD_W3_PLCP_SIGNAL		FIELD32(0x000000ff)
+#define TXD_W3_PLCP_SERVICE		FIELD32(0x0000ff00)
+#define TXD_W3_PLCP_LENGTH_LOW		FIELD32(0x00ff0000)
+#define TXD_W3_PLCP_LENGTH_HIGH		FIELD32(0xff000000)
+
+/*
+ * Word4
+ */
+#define TXD_W4_IV			FIELD32(0xffffffff)
+
+/*
+ * Word5
+ */
+#define TXD_W5_EIV			FIELD32(0xffffffff)
+
+/*
+ * Word6-9: Key
+ */
+#define TXD_W6_KEY			FIELD32(0xffffffff)
+#define TXD_W7_KEY			FIELD32(0xffffffff)
+#define TXD_W8_KEY			FIELD32(0xffffffff)
+#define TXD_W9_KEY			FIELD32(0xffffffff)
+
+/*
+ * Word10
+ */
+#define TXD_W10_RTS			FIELD32(0x00000001)
+#define TXD_W10_TX_RATE			FIELD32(0x000000fe)
+
+/*
+ * RX descriptor format for RX Ring.
+ */
+
+/*
+ * Word0
+ */
+#define RXD_W0_OWNER_NIC		FIELD32(0x00000001)
+#define RXD_W0_UNICAST_TO_ME		FIELD32(0x00000002)
+#define RXD_W0_MULTICAST		FIELD32(0x00000004)
+#define RXD_W0_BROADCAST		FIELD32(0x00000008)
+#define RXD_W0_MY_BSS			FIELD32(0x00000010)
+#define RXD_W0_CRC_ERROR		FIELD32(0x00000020)
+#define RXD_W0_OFDM			FIELD32(0x00000040)
+#define RXD_W0_PHYSICAL_ERROR		FIELD32(0x00000080)
+#define RXD_W0_CIPHER_OWNER		FIELD32(0x00000100)
+#define RXD_W0_ICV_ERROR		FIELD32(0x00000200)
+#define RXD_W0_IV_OFFSET		FIELD32(0x0000fc00)
+#define RXD_W0_DATABYTE_COUNT		FIELD32(0x0fff0000)
+#define RXD_W0_CIPHER_ALG		FIELD32(0xe0000000)
+
+/*
+ * Word1
+ */
+#define RXD_W1_BUFFER_ADDRESS		FIELD32(0xffffffff)
+
+/*
+ * Word2
+ */
+#define RXD_W2_SIGNAL			FIELD32(0x000000ff)
+#define RXD_W2_RSSI			FIELD32(0x0000ff00)
+#define RXD_W2_TA			FIELD32(0xffff0000)
+
+/*
+ * Word3
+ */
+#define RXD_W3_TA			FIELD32(0xffffffff)
+
+/*
+ * Word4
+ */
+#define RXD_W4_IV			FIELD32(0xffffffff)
+
+/*
+ * Word5
+ */
+#define RXD_W5_EIV			FIELD32(0xffffffff)
+
+/*
+ * Word6-9: Key
+ */
+#define RXD_W6_KEY			FIELD32(0xffffffff)
+#define RXD_W7_KEY			FIELD32(0xffffffff)
+#define RXD_W8_KEY			FIELD32(0xffffffff)
+#define RXD_W9_KEY			FIELD32(0xffffffff)
+
+/*
+ * Word10
+ */
+#define RXD_W10_DROP			FIELD32(0x00000001)
+
+/*
+ * Macro's for converting txpower from EEPROM to dscape value
+ * and from dscape value to register value.
+ */
+#define MIN_TXPOWER	0
+#define MAX_TXPOWER	31
+#define DEFAULT_TXPOWER	24
+
+#define TXPOWER_FROM_DEV(__txpower)		\
+({						\
+	((__txpower) > MAX_TXPOWER) ?		\
+		DEFAULT_TXPOWER : (__txpower);	\
+})
+
+#define TXPOWER_TO_DEV(__txpower)			\
+({							\
+	((__txpower) <= MIN_TXPOWER) ? MIN_TXPOWER :	\
+	(((__txpower) >= MAX_TXPOWER) ? MAX_TXPOWER :	\
+	(__txpower));					\
+})
+
+#endif /* RT2500PCI_H */
diff --git a/drivers/net/wireless/rt2x00/rt2500usb.c b/drivers/net/wireless/rt2x00/rt2500usb.c
new file mode 100644
index 000000000000..7cdc80a122bb
--- /dev/null
+++ b/drivers/net/wireless/rt2x00/rt2500usb.c
@@ -0,0 +1,1832 @@
+/*
+	Copyright (C) 2004 - 2007 rt2x00 SourceForge Project
+	<http://rt2x00.serialmonkey.com>
+
+	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, or
+	(at your option) any later version.
+
+	This program is distributed in the hope that it will be useful,
+	but WITHOUT ANY WARRANTY; without even the implied warranty of
+	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+	GNU General Public License for more details.
+
+	You should have received a copy of the GNU General Public License
+	along with this program; if not, write to the
+	Free Software Foundation, Inc.,
+	59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+/*
+	Module: rt2500usb
+	Abstract: rt2500usb device specific routines.
+	Supported chipsets: RT2570.
+ */
+
+/*
+ * Set enviroment defines for rt2x00.h
+ */
+#define DRV_NAME "rt2500usb"
+
+#include <linux/delay.h>
+#include <linux/etherdevice.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/usb.h>
+
+#include "rt2x00.h"
+#include "rt2x00usb.h"
+#include "rt2500usb.h"
+
+/*
+ * Register access.
+ * All access to the CSR registers will go through the methods
+ * rt2500usb_register_read and rt2500usb_register_write.
+ * BBP and RF register require indirect register access,
+ * and use the CSR registers BBPCSR and RFCSR to achieve this.
+ * These indirect registers work with busy bits,
+ * and we will try maximal REGISTER_BUSY_COUNT times to access
+ * the register while taking a REGISTER_BUSY_DELAY us delay
+ * between each attampt. When the busy bit is still set at that time,
+ * the access attempt is considered to have failed,
+ * and we will print an error.
+ */
+static inline void rt2500usb_register_read(const struct rt2x00_dev *rt2x00dev,
+					   const unsigned int offset,
+					   u16 *value)
+{
+	__le16 reg;
+	rt2x00usb_vendor_request_buff(rt2x00dev, USB_MULTI_READ,
+				      USB_VENDOR_REQUEST_IN, offset,
+				      &reg, sizeof(u16), REGISTER_TIMEOUT);
+	*value = le16_to_cpu(reg);
+}
+
+static inline void rt2500usb_register_multiread(const struct rt2x00_dev
+						*rt2x00dev,
+						const unsigned int offset,
+						void *value, const u16 length)
+{
+	int timeout = REGISTER_TIMEOUT * (length / sizeof(u16));
+	rt2x00usb_vendor_request_buff(rt2x00dev, USB_MULTI_READ,
+				      USB_VENDOR_REQUEST_IN, offset,
+				      value, length, timeout);
+}
+
+static inline void rt2500usb_register_write(const struct rt2x00_dev *rt2x00dev,
+					    const unsigned int offset,
+					    u16 value)
+{
+	__le16 reg = cpu_to_le16(value);
+	rt2x00usb_vendor_request_buff(rt2x00dev, USB_MULTI_WRITE,
+				      USB_VENDOR_REQUEST_OUT, offset,
+				      &reg, sizeof(u16), REGISTER_TIMEOUT);
+}
+
+static inline void rt2500usb_register_multiwrite(const struct rt2x00_dev
+						 *rt2x00dev,
+						 const unsigned int offset,
+						 void *value, const u16 length)
+{
+	int timeout = REGISTER_TIMEOUT * (length / sizeof(u16));
+	rt2x00usb_vendor_request_buff(rt2x00dev, USB_MULTI_WRITE,
+				      USB_VENDOR_REQUEST_OUT, offset,
+				      value, length, timeout);
+}
+
+static u16 rt2500usb_bbp_check(const struct rt2x00_dev *rt2x00dev)
+{
+	u16 reg;
+	unsigned int i;
+
+	for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
+		rt2500usb_register_read(rt2x00dev, PHY_CSR8, &reg);
+		if (!rt2x00_get_field16(reg, PHY_CSR8_BUSY))
+			break;
+		udelay(REGISTER_BUSY_DELAY);
+	}
+
+	return reg;
+}
+
+static void rt2500usb_bbp_write(const struct rt2x00_dev *rt2x00dev,
+				const unsigned int word, const u8 value)
+{
+	u16 reg;
+
+	/*
+	 * Wait until the BBP becomes ready.
+	 */
+	reg = rt2500usb_bbp_check(rt2x00dev);
+	if (rt2x00_get_field16(reg, PHY_CSR8_BUSY)) {
+		ERROR(rt2x00dev, "PHY_CSR8 register busy. Write failed.\n");
+		return;
+	}
+
+	/*
+	 * Write the data into the BBP.
+	 */
+	reg = 0;
+	rt2x00_set_field16(&reg, PHY_CSR7_DATA, value);
+	rt2x00_set_field16(&reg, PHY_CSR7_REG_ID, word);
+	rt2x00_set_field16(&reg, PHY_CSR7_READ_CONTROL, 0);
+
+	rt2500usb_register_write(rt2x00dev, PHY_CSR7, reg);
+}
+
+static void rt2500usb_bbp_read(const struct rt2x00_dev *rt2x00dev,
+			       const unsigned int word, u8 *value)
+{
+	u16 reg;
+
+	/*
+	 * Wait until the BBP becomes ready.
+	 */
+	reg = rt2500usb_bbp_check(rt2x00dev);
+	if (rt2x00_get_field16(reg, PHY_CSR8_BUSY)) {
+		ERROR(rt2x00dev, "PHY_CSR8 register busy. Read failed.\n");
+		return;
+	}
+
+	/*
+	 * Write the request into the BBP.
+	 */
+	reg = 0;
+	rt2x00_set_field16(&reg, PHY_CSR7_REG_ID, word);
+	rt2x00_set_field16(&reg, PHY_CSR7_READ_CONTROL, 1);
+
+	rt2500usb_register_write(rt2x00dev, PHY_CSR7, reg);
+
+	/*
+	 * Wait until the BBP becomes ready.
+	 */
+	reg = rt2500usb_bbp_check(rt2x00dev);
+	if (rt2x00_get_field16(reg, PHY_CSR8_BUSY)) {
+		ERROR(rt2x00dev, "PHY_CSR8 register busy. Read failed.\n");
+		*value = 0xff;
+		return;
+	}
+
+	rt2500usb_register_read(rt2x00dev, PHY_CSR7, &reg);
+	*value = rt2x00_get_field16(reg, PHY_CSR7_DATA);
+}
+
+static void rt2500usb_rf_write(const struct rt2x00_dev *rt2x00dev,
+			       const unsigned int word, const u32 value)
+{
+	u16 reg;
+	unsigned int i;
+
+	if (!word)
+		return;
+
+	for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
+		rt2500usb_register_read(rt2x00dev, PHY_CSR10, &reg);
+		if (!rt2x00_get_field16(reg, PHY_CSR10_RF_BUSY))
+			goto rf_write;
+		udelay(REGISTER_BUSY_DELAY);
+	}
+
+	ERROR(rt2x00dev, "PHY_CSR10 register busy. Write failed.\n");
+	return;
+
+rf_write:
+	reg = 0;
+	rt2x00_set_field16(&reg, PHY_CSR9_RF_VALUE, value);
+	rt2500usb_register_write(rt2x00dev, PHY_CSR9, reg);
+
+	reg = 0;
+	rt2x00_set_field16(&reg, PHY_CSR10_RF_VALUE, value >> 16);
+	rt2x00_set_field16(&reg, PHY_CSR10_RF_NUMBER_OF_BITS, 20);
+	rt2x00_set_field16(&reg, PHY_CSR10_RF_IF_SELECT, 0);
+	rt2x00_set_field16(&reg, PHY_CSR10_RF_BUSY, 1);
+
+	rt2500usb_register_write(rt2x00dev, PHY_CSR10, reg);
+	rt2x00_rf_write(rt2x00dev, word, value);
+}
+
+#ifdef CONFIG_RT2X00_LIB_DEBUGFS
+#define CSR_OFFSET(__word)	( CSR_REG_BASE + ((__word) * sizeof(u16)) )
+
+static void rt2500usb_read_csr(const struct rt2x00_dev *rt2x00dev,
+			       const unsigned int word, u32 *data)
+{
+	rt2500usb_register_read(rt2x00dev, CSR_OFFSET(word), (u16 *) data);
+}
+
+static void rt2500usb_write_csr(const struct rt2x00_dev *rt2x00dev,
+				const unsigned int word, u32 data)
+{
+	rt2500usb_register_write(rt2x00dev, CSR_OFFSET(word), data);
+}
+
+static const struct rt2x00debug rt2500usb_rt2x00debug = {
+	.owner	= THIS_MODULE,
+	.csr	= {
+		.read		= rt2500usb_read_csr,
+		.write		= rt2500usb_write_csr,
+		.word_size	= sizeof(u16),
+		.word_count	= CSR_REG_SIZE / sizeof(u16),
+	},
+	.eeprom	= {
+		.read		= rt2x00_eeprom_read,
+		.write		= rt2x00_eeprom_write,
+		.word_size	= sizeof(u16),
+		.word_count	= EEPROM_SIZE / sizeof(u16),
+	},
+	.bbp	= {
+		.read		= rt2500usb_bbp_read,
+		.write		= rt2500usb_bbp_write,
+		.word_size	= sizeof(u8),
+		.word_count	= BBP_SIZE / sizeof(u8),
+	},
+	.rf	= {
+		.read		= rt2x00_rf_read,
+		.write		= rt2500usb_rf_write,
+		.word_size	= sizeof(u32),
+		.word_count	= RF_SIZE / sizeof(u32),
+	},
+};
+#endif /* CONFIG_RT2X00_LIB_DEBUGFS */
+
+/*
+ * Configuration handlers.
+ */
+static void rt2500usb_config_mac_addr(struct rt2x00_dev *rt2x00dev,
+				      __le32 *mac)
+{
+	rt2500usb_register_multiwrite(rt2x00dev, MAC_CSR2, &mac,
+				      (3 * sizeof(__le16)));
+}
+
+static void rt2500usb_config_bssid(struct rt2x00_dev *rt2x00dev,
+				   __le32 *bssid)
+{
+	rt2500usb_register_multiwrite(rt2x00dev, MAC_CSR5, bssid,
+				      (3 * sizeof(__le16)));
+}
+
+static void rt2500usb_config_type(struct rt2x00_dev *rt2x00dev, const int type,
+				  const int tsf_sync)
+{
+	u16 reg;
+
+	rt2500usb_register_write(rt2x00dev, TXRX_CSR19, 0);
+
+	/*
+	 * Enable beacon config
+	 */
+	rt2500usb_register_read(rt2x00dev, TXRX_CSR20, &reg);
+	rt2x00_set_field16(&reg, TXRX_CSR20_OFFSET,
+			   (PREAMBLE + get_duration(IEEE80211_HEADER, 20)) >> 6);
+	if (type == IEEE80211_IF_TYPE_STA)
+		rt2x00_set_field16(&reg, TXRX_CSR20_BCN_EXPECT_WINDOW, 0);
+	else
+		rt2x00_set_field16(&reg, TXRX_CSR20_BCN_EXPECT_WINDOW, 2);
+	rt2500usb_register_write(rt2x00dev, TXRX_CSR20, reg);
+
+	/*
+	 * Enable synchronisation.
+	 */
+	rt2500usb_register_read(rt2x00dev, TXRX_CSR18, &reg);
+	rt2x00_set_field16(&reg, TXRX_CSR18_OFFSET, 0);
+	rt2500usb_register_write(rt2x00dev, TXRX_CSR18, reg);
+
+	rt2500usb_register_read(rt2x00dev, TXRX_CSR19, &reg);
+	rt2x00_set_field16(&reg, TXRX_CSR19_TSF_COUNT, 1);
+	rt2x00_set_field16(&reg, TXRX_CSR19_TBCN, 1);
+	rt2x00_set_field16(&reg, TXRX_CSR19_BEACON_GEN, 0);
+	rt2x00_set_field16(&reg, TXRX_CSR19_TSF_SYNC, tsf_sync);
+	rt2500usb_register_write(rt2x00dev, TXRX_CSR19, reg);
+}
+
+static void rt2500usb_config_preamble(struct rt2x00_dev *rt2x00dev,
+				      const int short_preamble,
+				      const int ack_timeout,
+				      const int ack_consume_time)
+{
+	u16 reg;
+
+	/*
+	 * When in atomic context, reschedule and let rt2x00lib
+	 * call this function again.
+	 */
+	if (in_atomic()) {
+		queue_work(rt2x00dev->hw->workqueue, &rt2x00dev->config_work);
+		return;
+	}
+
+	rt2500usb_register_read(rt2x00dev, TXRX_CSR1, &reg);
+	rt2x00_set_field16(&reg, TXRX_CSR1_ACK_TIMEOUT, ack_timeout);
+	rt2500usb_register_write(rt2x00dev, TXRX_CSR1, reg);
+
+	rt2500usb_register_read(rt2x00dev, TXRX_CSR10, &reg);
+	rt2x00_set_field16(&reg, TXRX_CSR10_AUTORESPOND_PREAMBLE,
+			   !!short_preamble);
+	rt2500usb_register_write(rt2x00dev, TXRX_CSR10, reg);
+}
+
+static void rt2500usb_config_phymode(struct rt2x00_dev *rt2x00dev,
+				     const int phymode,
+				     const int basic_rate_mask)
+{
+	rt2500usb_register_write(rt2x00dev, TXRX_CSR11, basic_rate_mask);
+
+	if (phymode == HWMODE_B) {
+		rt2500usb_register_write(rt2x00dev, MAC_CSR11, 0x000b);
+		rt2500usb_register_write(rt2x00dev, MAC_CSR12, 0x0040);
+	} else {
+		rt2500usb_register_write(rt2x00dev, MAC_CSR11, 0x0005);
+		rt2500usb_register_write(rt2x00dev, MAC_CSR12, 0x016c);
+	}
+}
+
+static void rt2500usb_config_channel(struct rt2x00_dev *rt2x00dev,
+				     struct rf_channel *rf, const int txpower)
+{
+	/*
+	 * Set TXpower.
+	 */
+	rt2x00_set_field32(&rf->rf3, RF3_TXPOWER, TXPOWER_TO_DEV(txpower));
+
+	/*
+	 * For RT2525E we should first set the channel to half band higher.
+	 */
+	if (rt2x00_rf(&rt2x00dev->chip, RF2525E)) {
+		static const u32 vals[] = {
+			0x000008aa, 0x000008ae, 0x000008ae, 0x000008b2,
+			0x000008b2, 0x000008b6, 0x000008b6, 0x000008ba,
+			0x000008ba, 0x000008be, 0x000008b7, 0x00000902,
+			0x00000902, 0x00000906
+		};
+
+		rt2500usb_rf_write(rt2x00dev, 2, vals[rf->channel - 1]);
+		if (rf->rf4)
+			rt2500usb_rf_write(rt2x00dev, 4, rf->rf4);
+	}
+
+	rt2500usb_rf_write(rt2x00dev, 1, rf->rf1);
+	rt2500usb_rf_write(rt2x00dev, 2, rf->rf2);
+	rt2500usb_rf_write(rt2x00dev, 3, rf->rf3);
+	if (rf->rf4)
+		rt2500usb_rf_write(rt2x00dev, 4, rf->rf4);
+}
+
+static void rt2500usb_config_txpower(struct rt2x00_dev *rt2x00dev,
+				     const int txpower)
+{
+	u32 rf3;
+
+	rt2x00_rf_read(rt2x00dev, 3, &rf3);
+	rt2x00_set_field32(&rf3, RF3_TXPOWER, TXPOWER_TO_DEV(txpower));
+	rt2500usb_rf_write(rt2x00dev, 3, rf3);
+}
+
+static void rt2500usb_config_antenna(struct rt2x00_dev *rt2x00dev,
+				     const int antenna_tx, const int antenna_rx)
+{
+	u8 r2;
+	u8 r14;
+	u16 csr5;
+	u16 csr6;
+
+	rt2500usb_bbp_read(rt2x00dev, 2, &r2);
+	rt2500usb_bbp_read(rt2x00dev, 14, &r14);
+	rt2500usb_register_read(rt2x00dev, PHY_CSR5, &csr5);
+	rt2500usb_register_read(rt2x00dev, PHY_CSR6, &csr6);
+
+	/*
+	 * Configure the TX antenna.
+	 */
+	switch (antenna_tx) {
+	case ANTENNA_SW_DIVERSITY:
+	case ANTENNA_HW_DIVERSITY:
+		rt2x00_set_field8(&r2, BBP_R2_TX_ANTENNA, 1);
+		rt2x00_set_field16(&csr5, PHY_CSR5_CCK, 1);
+		rt2x00_set_field16(&csr6, PHY_CSR6_OFDM, 1);
+		break;
+	case ANTENNA_A:
+		rt2x00_set_field8(&r2, BBP_R2_TX_ANTENNA, 0);
+		rt2x00_set_field16(&csr5, PHY_CSR5_CCK, 0);
+		rt2x00_set_field16(&csr6, PHY_CSR6_OFDM, 0);
+		break;
+	case ANTENNA_B:
+		rt2x00_set_field8(&r2, BBP_R2_TX_ANTENNA, 2);
+		rt2x00_set_field16(&csr5, PHY_CSR5_CCK, 2);
+		rt2x00_set_field16(&csr6, PHY_CSR6_OFDM, 2);
+		break;
+	}
+
+	/*
+	 * Configure the RX antenna.
+	 */
+	switch (antenna_rx) {
+	case ANTENNA_SW_DIVERSITY:
+	case ANTENNA_HW_DIVERSITY:
+		rt2x00_set_field8(&r14, BBP_R14_RX_ANTENNA, 1);
+		break;
+	case ANTENNA_A:
+		rt2x00_set_field8(&r14, BBP_R14_RX_ANTENNA, 0);
+		break;
+	case ANTENNA_B:
+		rt2x00_set_field8(&r14, BBP_R14_RX_ANTENNA, 2);
+		break;
+	}
+
+	/*
+	 * RT2525E and RT5222 need to flip TX I/Q
+	 */
+	if (rt2x00_rf(&rt2x00dev->chip, RF2525E) ||
+	    rt2x00_rf(&rt2x00dev->chip, RF5222)) {
+		rt2x00_set_field8(&r2, BBP_R2_TX_IQ_FLIP, 1);
+		rt2x00_set_field16(&csr5, PHY_CSR5_CCK_FLIP, 1);
+		rt2x00_set_field16(&csr6, PHY_CSR6_OFDM_FLIP, 1);
+
+		/*
+		 * RT2525E does not need RX I/Q Flip.
+		 */
+		if (rt2x00_rf(&rt2x00dev->chip, RF2525E))
+			rt2x00_set_field8(&r14, BBP_R14_RX_IQ_FLIP, 0);
+	} else {
+		rt2x00_set_field16(&csr5, PHY_CSR5_CCK_FLIP, 0);
+		rt2x00_set_field16(&csr6, PHY_CSR6_OFDM_FLIP, 0);
+	}
+
+	rt2500usb_bbp_write(rt2x00dev, 2, r2);
+	rt2500usb_bbp_write(rt2x00dev, 14, r14);
+	rt2500usb_register_write(rt2x00dev, PHY_CSR5, csr5);
+	rt2500usb_register_write(rt2x00dev, PHY_CSR6, csr6);
+}
+
+static void rt2500usb_config_duration(struct rt2x00_dev *rt2x00dev,
+				      struct rt2x00lib_conf *libconf)
+{
+	u16 reg;
+
+	rt2500usb_register_write(rt2x00dev, MAC_CSR10, libconf->slot_time);
+
+	rt2500usb_register_read(rt2x00dev, TXRX_CSR18, &reg);
+	rt2x00_set_field16(&reg, TXRX_CSR18_INTERVAL,
+			   libconf->conf->beacon_int * 4);
+	rt2500usb_register_write(rt2x00dev, TXRX_CSR18, reg);
+}
+
+static void rt2500usb_config(struct rt2x00_dev *rt2x00dev,
+			     const unsigned int flags,
+			     struct rt2x00lib_conf *libconf)
+{
+	if (flags & CONFIG_UPDATE_PHYMODE)
+		rt2500usb_config_phymode(rt2x00dev, libconf->phymode,
+					 libconf->basic_rates);
+	if (flags & CONFIG_UPDATE_CHANNEL)
+		rt2500usb_config_channel(rt2x00dev, &libconf->rf,
+					 libconf->conf->power_level);
+	if ((flags & CONFIG_UPDATE_TXPOWER) && !(flags & CONFIG_UPDATE_CHANNEL))
+		rt2500usb_config_txpower(rt2x00dev,
+					 libconf->conf->power_level);
+	if (flags & CONFIG_UPDATE_ANTENNA)
+		rt2500usb_config_antenna(rt2x00dev,
+					 libconf->conf->antenna_sel_tx,
+					 libconf->conf->antenna_sel_rx);
+	if (flags & (CONFIG_UPDATE_SLOT_TIME | CONFIG_UPDATE_BEACON_INT))
+		rt2500usb_config_duration(rt2x00dev, libconf);
+}
+
+/*
+ * LED functions.
+ */
+static void rt2500usb_enable_led(struct rt2x00_dev *rt2x00dev)
+{
+	u16 reg;
+
+	rt2500usb_register_read(rt2x00dev, MAC_CSR21, &reg);
+	rt2x00_set_field16(&reg, MAC_CSR21_ON_PERIOD, 70);
+	rt2x00_set_field16(&reg, MAC_CSR21_OFF_PERIOD, 30);
+	rt2500usb_register_write(rt2x00dev, MAC_CSR21, reg);
+
+	rt2500usb_register_read(rt2x00dev, MAC_CSR20, &reg);
+
+	if (rt2x00dev->led_mode == LED_MODE_TXRX_ACTIVITY) {
+		rt2x00_set_field16(&reg, MAC_CSR20_LINK, 1);
+		rt2x00_set_field16(&reg, MAC_CSR20_ACTIVITY, 0);
+	} else if (rt2x00dev->led_mode == LED_MODE_ASUS) {
+		rt2x00_set_field16(&reg, MAC_CSR20_LINK, 0);
+		rt2x00_set_field16(&reg, MAC_CSR20_ACTIVITY, 1);
+	} else {
+		rt2x00_set_field16(&reg, MAC_CSR20_LINK, 1);
+		rt2x00_set_field16(&reg, MAC_CSR20_ACTIVITY, 1);
+	}
+
+	rt2500usb_register_write(rt2x00dev, MAC_CSR20, reg);
+}
+
+static void rt2500usb_disable_led(struct rt2x00_dev *rt2x00dev)
+{
+	u16 reg;
+
+	rt2500usb_register_read(rt2x00dev, MAC_CSR20, &reg);
+	rt2x00_set_field16(&reg, MAC_CSR20_LINK, 0);
+	rt2x00_set_field16(&reg, MAC_CSR20_ACTIVITY, 0);
+	rt2500usb_register_write(rt2x00dev, MAC_CSR20, reg);
+}
+
+/*
+ * Link tuning
+ */
+static void rt2500usb_link_stats(struct rt2x00_dev *rt2x00dev)
+{
+	u16 reg;
+
+	/*
+	 * Update FCS error count from register.
+	 */
+	rt2500usb_register_read(rt2x00dev, STA_CSR0, &reg);
+	rt2x00dev->link.rx_failed = rt2x00_get_field16(reg, STA_CSR0_FCS_ERROR);
+
+	/*
+	 * Update False CCA count from register.
+	 */
+	rt2500usb_register_read(rt2x00dev, STA_CSR3, &reg);
+	rt2x00dev->link.false_cca =
+	    rt2x00_get_field16(reg, STA_CSR3_FALSE_CCA_ERROR);
+}
+
+static void rt2500usb_reset_tuner(struct rt2x00_dev *rt2x00dev)
+{
+	u16 eeprom;
+	u16 value;
+
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE_R24, &eeprom);
+	value = rt2x00_get_field16(eeprom, EEPROM_BBPTUNE_R24_LOW);
+	rt2500usb_bbp_write(rt2x00dev, 24, value);
+
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE_R25, &eeprom);
+	value = rt2x00_get_field16(eeprom, EEPROM_BBPTUNE_R25_LOW);
+	rt2500usb_bbp_write(rt2x00dev, 25, value);
+
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE_R61, &eeprom);
+	value = rt2x00_get_field16(eeprom, EEPROM_BBPTUNE_R61_LOW);
+	rt2500usb_bbp_write(rt2x00dev, 61, value);
+
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE_VGC, &eeprom);
+	value = rt2x00_get_field16(eeprom, EEPROM_BBPTUNE_VGCUPPER);
+	rt2500usb_bbp_write(rt2x00dev, 17, value);
+
+	rt2x00dev->link.vgc_level = value;
+}
+
+static void rt2500usb_link_tuner(struct rt2x00_dev *rt2x00dev)
+{
+	int rssi = rt2x00_get_link_rssi(&rt2x00dev->link);
+	u16 bbp_thresh;
+	u16 vgc_bound;
+	u16 sens;
+	u16 r24;
+	u16 r25;
+	u16 r61;
+	u16 r17_sens;
+	u8 r17;
+	u8 up_bound;
+	u8 low_bound;
+
+	/*
+	 * Determine the BBP tuning threshold and correctly
+	 * set BBP 24, 25 and 61.
+	 */
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE, &bbp_thresh);
+	bbp_thresh = rt2x00_get_field16(bbp_thresh, EEPROM_BBPTUNE_THRESHOLD);
+
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE_R24, &r24);
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE_R25, &r25);
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE_R61, &r61);
+
+	if ((rssi + bbp_thresh) > 0) {
+		r24 = rt2x00_get_field16(r24, EEPROM_BBPTUNE_R24_HIGH);
+		r25 = rt2x00_get_field16(r25, EEPROM_BBPTUNE_R25_HIGH);
+		r61 = rt2x00_get_field16(r61, EEPROM_BBPTUNE_R61_HIGH);
+	} else {
+		r24 = rt2x00_get_field16(r24, EEPROM_BBPTUNE_R24_LOW);
+		r25 = rt2x00_get_field16(r25, EEPROM_BBPTUNE_R25_LOW);
+		r61 = rt2x00_get_field16(r61, EEPROM_BBPTUNE_R61_LOW);
+	}
+
+	rt2500usb_bbp_write(rt2x00dev, 24, r24);
+	rt2500usb_bbp_write(rt2x00dev, 25, r25);
+	rt2500usb_bbp_write(rt2x00dev, 61, r61);
+
+	/*
+	 * Read current r17 value, as well as the sensitivity values
+	 * for the r17 register.
+	 */
+	rt2500usb_bbp_read(rt2x00dev, 17, &r17);
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE_R17, &r17_sens);
+
+	/*
+	 * A too low RSSI will cause too much false CCA which will
+	 * then corrupt the R17 tuning. To remidy this the tuning should
+	 * be stopped (While making sure the R17 value will not exceed limits)
+	 */
+	if (rssi >= -40) {
+		if (r17 != 0x60)
+			rt2500usb_bbp_write(rt2x00dev, 17, 0x60);
+		return;
+	}
+
+	/*
+	 * Special big-R17 for short distance
+	 */
+	if (rssi >= -58) {
+		sens = rt2x00_get_field16(r17_sens, EEPROM_BBPTUNE_R17_LOW);
+		if (r17 != sens)
+			rt2500usb_bbp_write(rt2x00dev, 17, sens);
+		return;
+	}
+
+	/*
+	 * Special mid-R17 for middle distance
+	 */
+	if (rssi >= -74) {
+		sens = rt2x00_get_field16(r17_sens, EEPROM_BBPTUNE_R17_HIGH);
+		if (r17 != sens)
+			rt2500usb_bbp_write(rt2x00dev, 17, sens);
+		return;
+	}
+
+	/*
+	 * Leave short or middle distance condition, restore r17
+	 * to the dynamic tuning range.
+	 */
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE_VGC, &vgc_bound);
+	vgc_bound = rt2x00_get_field16(vgc_bound, EEPROM_BBPTUNE_VGCUPPER);
+
+	low_bound = 0x32;
+	if (rssi >= -77)
+		up_bound = vgc_bound;
+	else
+		up_bound = vgc_bound - (-77 - rssi);
+
+	if (up_bound < low_bound)
+		up_bound = low_bound;
+
+	if (r17 > up_bound) {
+		rt2500usb_bbp_write(rt2x00dev, 17, up_bound);
+		rt2x00dev->link.vgc_level = up_bound;
+	} else if (rt2x00dev->link.false_cca > 512 && r17 < up_bound) {
+		rt2500usb_bbp_write(rt2x00dev, 17, ++r17);
+		rt2x00dev->link.vgc_level = r17;
+	} else if (rt2x00dev->link.false_cca < 100 && r17 > low_bound) {
+		rt2500usb_bbp_write(rt2x00dev, 17, --r17);
+		rt2x00dev->link.vgc_level = r17;
+	}
+}
+
+/*
+ * Initialization functions.
+ */
+static int rt2500usb_init_registers(struct rt2x00_dev *rt2x00dev)
+{
+	u16 reg;
+
+	rt2x00usb_vendor_request_sw(rt2x00dev, USB_DEVICE_MODE, 0x0001,
+				    USB_MODE_TEST, REGISTER_TIMEOUT);
+	rt2x00usb_vendor_request_sw(rt2x00dev, USB_SINGLE_WRITE, 0x0308,
+				    0x00f0, REGISTER_TIMEOUT);
+
+	rt2500usb_register_read(rt2x00dev, TXRX_CSR2, &reg);
+	rt2x00_set_field16(&reg, TXRX_CSR2_DISABLE_RX, 1);
+	rt2500usb_register_write(rt2x00dev, TXRX_CSR2, reg);
+
+	rt2500usb_register_write(rt2x00dev, MAC_CSR13, 0x1111);
+	rt2500usb_register_write(rt2x00dev, MAC_CSR14, 0x1e11);
+
+	rt2500usb_register_read(rt2x00dev, MAC_CSR1, &reg);
+	rt2x00_set_field16(&reg, MAC_CSR1_SOFT_RESET, 1);
+	rt2x00_set_field16(&reg, MAC_CSR1_BBP_RESET, 1);
+	rt2x00_set_field16(&reg, MAC_CSR1_HOST_READY, 0);
+	rt2500usb_register_write(rt2x00dev, MAC_CSR1, reg);
+
+	rt2500usb_register_read(rt2x00dev, MAC_CSR1, &reg);
+	rt2x00_set_field16(&reg, MAC_CSR1_SOFT_RESET, 0);
+	rt2x00_set_field16(&reg, MAC_CSR1_BBP_RESET, 0);
+	rt2x00_set_field16(&reg, MAC_CSR1_HOST_READY, 0);
+	rt2500usb_register_write(rt2x00dev, MAC_CSR1, reg);
+
+	rt2500usb_register_read(rt2x00dev, TXRX_CSR5, &reg);
+	rt2x00_set_field16(&reg, TXRX_CSR5_BBP_ID0, 13);
+	rt2x00_set_field16(&reg, TXRX_CSR5_BBP_ID0_VALID, 1);
+	rt2x00_set_field16(&reg, TXRX_CSR5_BBP_ID1, 12);
+	rt2x00_set_field16(&reg, TXRX_CSR5_BBP_ID1_VALID, 1);
+	rt2500usb_register_write(rt2x00dev, TXRX_CSR5, reg);
+
+	rt2500usb_register_read(rt2x00dev, TXRX_CSR6, &reg);
+	rt2x00_set_field16(&reg, TXRX_CSR6_BBP_ID0, 10);
+	rt2x00_set_field16(&reg, TXRX_CSR6_BBP_ID0_VALID, 1);
+	rt2x00_set_field16(&reg, TXRX_CSR6_BBP_ID1, 11);
+	rt2x00_set_field16(&reg, TXRX_CSR6_BBP_ID1_VALID, 1);
+	rt2500usb_register_write(rt2x00dev, TXRX_CSR6, reg);
+
+	rt2500usb_register_read(rt2x00dev, TXRX_CSR7, &reg);
+	rt2x00_set_field16(&reg, TXRX_CSR7_BBP_ID0, 7);
+	rt2x00_set_field16(&reg, TXRX_CSR7_BBP_ID0_VALID, 1);
+	rt2x00_set_field16(&reg, TXRX_CSR7_BBP_ID1, 6);
+	rt2x00_set_field16(&reg, TXRX_CSR7_BBP_ID1_VALID, 1);
+	rt2500usb_register_write(rt2x00dev, TXRX_CSR7, reg);
+
+	rt2500usb_register_read(rt2x00dev, TXRX_CSR8, &reg);
+	rt2x00_set_field16(&reg, TXRX_CSR8_BBP_ID0, 5);
+	rt2x00_set_field16(&reg, TXRX_CSR8_BBP_ID0_VALID, 1);
+	rt2x00_set_field16(&reg, TXRX_CSR8_BBP_ID1, 0);
+	rt2x00_set_field16(&reg, TXRX_CSR8_BBP_ID1_VALID, 0);
+	rt2500usb_register_write(rt2x00dev, TXRX_CSR8, reg);
+
+	rt2500usb_register_write(rt2x00dev, TXRX_CSR21, 0xe78f);
+	rt2500usb_register_write(rt2x00dev, MAC_CSR9, 0xff1d);
+
+	if (rt2x00dev->ops->lib->set_device_state(rt2x00dev, STATE_AWAKE))
+		return -EBUSY;
+
+	rt2500usb_register_read(rt2x00dev, MAC_CSR1, &reg);
+	rt2x00_set_field16(&reg, MAC_CSR1_SOFT_RESET, 0);
+	rt2x00_set_field16(&reg, MAC_CSR1_BBP_RESET, 0);
+	rt2x00_set_field16(&reg, MAC_CSR1_HOST_READY, 1);
+	rt2500usb_register_write(rt2x00dev, MAC_CSR1, reg);
+
+	if (rt2x00_get_rev(&rt2x00dev->chip) >= RT2570_VERSION_C) {
+		rt2500usb_register_read(rt2x00dev, PHY_CSR2, &reg);
+		reg &= ~0x0002;
+	} else {
+		reg = 0x3002;
+	}
+	rt2500usb_register_write(rt2x00dev, PHY_CSR2, reg);
+
+	rt2500usb_register_write(rt2x00dev, MAC_CSR11, 0x0002);
+	rt2500usb_register_write(rt2x00dev, MAC_CSR22, 0x0053);
+	rt2500usb_register_write(rt2x00dev, MAC_CSR15, 0x01ee);
+	rt2500usb_register_write(rt2x00dev, MAC_CSR16, 0x0000);
+
+	rt2500usb_register_read(rt2x00dev, MAC_CSR8, &reg);
+	rt2x00_set_field16(&reg, MAC_CSR8_MAX_FRAME_UNIT,
+			   rt2x00dev->rx->data_size);
+	rt2500usb_register_write(rt2x00dev, MAC_CSR8, reg);
+
+	rt2500usb_register_read(rt2x00dev, TXRX_CSR0, &reg);
+	rt2x00_set_field16(&reg, TXRX_CSR0_IV_OFFSET, IEEE80211_HEADER);
+	rt2x00_set_field16(&reg, TXRX_CSR0_KEY_ID, 0xff);
+	rt2500usb_register_write(rt2x00dev, TXRX_CSR0, reg);
+
+	rt2500usb_register_read(rt2x00dev, MAC_CSR18, &reg);
+	rt2x00_set_field16(&reg, MAC_CSR18_DELAY_AFTER_BEACON, 90);
+	rt2500usb_register_write(rt2x00dev, MAC_CSR18, reg);
+
+	rt2500usb_register_read(rt2x00dev, PHY_CSR4, &reg);
+	rt2x00_set_field16(&reg, PHY_CSR4_LOW_RF_LE, 1);
+	rt2500usb_register_write(rt2x00dev, PHY_CSR4, reg);
+
+	rt2500usb_register_read(rt2x00dev, TXRX_CSR1, &reg);
+	rt2x00_set_field16(&reg, TXRX_CSR1_AUTO_SEQUENCE, 1);
+	rt2500usb_register_write(rt2x00dev, TXRX_CSR1, reg);
+
+	return 0;
+}
+
+static int rt2500usb_init_bbp(struct rt2x00_dev *rt2x00dev)
+{
+	unsigned int i;
+	u16 eeprom;
+	u8 value;
+	u8 reg_id;
+
+	for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
+		rt2500usb_bbp_read(rt2x00dev, 0, &value);
+		if ((value != 0xff) && (value != 0x00))
+			goto continue_csr_init;
+		NOTICE(rt2x00dev, "Waiting for BBP register.\n");
+		udelay(REGISTER_BUSY_DELAY);
+	}
+
+	ERROR(rt2x00dev, "BBP register access failed, aborting.\n");
+	return -EACCES;
+
+continue_csr_init:
+	rt2500usb_bbp_write(rt2x00dev, 3, 0x02);
+	rt2500usb_bbp_write(rt2x00dev, 4, 0x19);
+	rt2500usb_bbp_write(rt2x00dev, 14, 0x1c);
+	rt2500usb_bbp_write(rt2x00dev, 15, 0x30);
+	rt2500usb_bbp_write(rt2x00dev, 16, 0xac);
+	rt2500usb_bbp_write(rt2x00dev, 18, 0x18);
+	rt2500usb_bbp_write(rt2x00dev, 19, 0xff);
+	rt2500usb_bbp_write(rt2x00dev, 20, 0x1e);
+	rt2500usb_bbp_write(rt2x00dev, 21, 0x08);
+	rt2500usb_bbp_write(rt2x00dev, 22, 0x08);
+	rt2500usb_bbp_write(rt2x00dev, 23, 0x08);
+	rt2500usb_bbp_write(rt2x00dev, 24, 0x80);
+	rt2500usb_bbp_write(rt2x00dev, 25, 0x50);
+	rt2500usb_bbp_write(rt2x00dev, 26, 0x08);
+	rt2500usb_bbp_write(rt2x00dev, 27, 0x23);
+	rt2500usb_bbp_write(rt2x00dev, 30, 0x10);
+	rt2500usb_bbp_write(rt2x00dev, 31, 0x2b);
+	rt2500usb_bbp_write(rt2x00dev, 32, 0xb9);
+	rt2500usb_bbp_write(rt2x00dev, 34, 0x12);
+	rt2500usb_bbp_write(rt2x00dev, 35, 0x50);
+	rt2500usb_bbp_write(rt2x00dev, 39, 0xc4);
+	rt2500usb_bbp_write(rt2x00dev, 40, 0x02);
+	rt2500usb_bbp_write(rt2x00dev, 41, 0x60);
+	rt2500usb_bbp_write(rt2x00dev, 53, 0x10);
+	rt2500usb_bbp_write(rt2x00dev, 54, 0x18);
+	rt2500usb_bbp_write(rt2x00dev, 56, 0x08);
+	rt2500usb_bbp_write(rt2x00dev, 57, 0x10);
+	rt2500usb_bbp_write(rt2x00dev, 58, 0x08);
+	rt2500usb_bbp_write(rt2x00dev, 61, 0x60);
+	rt2500usb_bbp_write(rt2x00dev, 62, 0x10);
+	rt2500usb_bbp_write(rt2x00dev, 75, 0xff);
+
+	DEBUG(rt2x00dev, "Start initialization from EEPROM...\n");
+	for (i = 0; i < EEPROM_BBP_SIZE; i++) {
+		rt2x00_eeprom_read(rt2x00dev, EEPROM_BBP_START + i, &eeprom);
+
+		if (eeprom != 0xffff && eeprom != 0x0000) {
+			reg_id = rt2x00_get_field16(eeprom, EEPROM_BBP_REG_ID);
+			value = rt2x00_get_field16(eeprom, EEPROM_BBP_VALUE);
+			DEBUG(rt2x00dev, "BBP: 0x%02x, value: 0x%02x.\n",
+			      reg_id, value);
+			rt2500usb_bbp_write(rt2x00dev, reg_id, value);
+		}
+	}
+	DEBUG(rt2x00dev, "...End initialization from EEPROM.\n");
+
+	return 0;
+}
+
+/*
+ * Device state switch handlers.
+ */
+static void rt2500usb_toggle_rx(struct rt2x00_dev *rt2x00dev,
+				enum dev_state state)
+{
+	u16 reg;
+
+	rt2500usb_register_read(rt2x00dev, TXRX_CSR2, &reg);
+	rt2x00_set_field16(&reg, TXRX_CSR2_DISABLE_RX,
+			   state == STATE_RADIO_RX_OFF);
+	rt2500usb_register_write(rt2x00dev, TXRX_CSR2, reg);
+}
+
+static int rt2500usb_enable_radio(struct rt2x00_dev *rt2x00dev)
+{
+	/*
+	 * Initialize all registers.
+	 */
+	if (rt2500usb_init_registers(rt2x00dev) ||
+	    rt2500usb_init_bbp(rt2x00dev)) {
+		ERROR(rt2x00dev, "Register initialization failed.\n");
+		return -EIO;
+	}
+
+	rt2x00usb_enable_radio(rt2x00dev);
+
+	/*
+	 * Enable LED
+	 */
+	rt2500usb_enable_led(rt2x00dev);
+
+	return 0;
+}
+
+static void rt2500usb_disable_radio(struct rt2x00_dev *rt2x00dev)
+{
+	/*
+	 * Disable LED
+	 */
+	rt2500usb_disable_led(rt2x00dev);
+
+	rt2500usb_register_write(rt2x00dev, MAC_CSR13, 0x2121);
+	rt2500usb_register_write(rt2x00dev, MAC_CSR14, 0x2121);
+
+	/*
+	 * Disable synchronisation.
+	 */
+	rt2500usb_register_write(rt2x00dev, TXRX_CSR19, 0);
+
+	rt2x00usb_disable_radio(rt2x00dev);
+}
+
+static int rt2500usb_set_state(struct rt2x00_dev *rt2x00dev,
+			       enum dev_state state)
+{
+	u16 reg;
+	u16 reg2;
+	unsigned int i;
+	char put_to_sleep;
+	char bbp_state;
+	char rf_state;
+
+	put_to_sleep = (state != STATE_AWAKE);
+
+	reg = 0;
+	rt2x00_set_field16(&reg, MAC_CSR17_BBP_DESIRE_STATE, state);
+	rt2x00_set_field16(&reg, MAC_CSR17_RF_DESIRE_STATE, state);
+	rt2x00_set_field16(&reg, MAC_CSR17_PUT_TO_SLEEP, put_to_sleep);
+	rt2500usb_register_write(rt2x00dev, MAC_CSR17, reg);
+	rt2x00_set_field16(&reg, MAC_CSR17_SET_STATE, 1);
+	rt2500usb_register_write(rt2x00dev, MAC_CSR17, reg);
+
+	/*
+	 * Device is not guaranteed to be in the requested state yet.
+	 * We must wait until the register indicates that the
+	 * device has entered the correct state.
+	 */
+	for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
+		rt2500usb_register_read(rt2x00dev, MAC_CSR17, &reg2);
+		bbp_state = rt2x00_get_field16(reg2, MAC_CSR17_BBP_CURR_STATE);
+		rf_state = rt2x00_get_field16(reg2, MAC_CSR17_RF_CURR_STATE);
+		if (bbp_state == state && rf_state == state)
+			return 0;
+		rt2500usb_register_write(rt2x00dev, MAC_CSR17, reg);
+		msleep(30);
+	}
+
+	NOTICE(rt2x00dev, "Device failed to enter state %d, "
+	       "current device state: bbp %d and rf %d.\n",
+	       state, bbp_state, rf_state);
+
+	return -EBUSY;
+}
+
+static int rt2500usb_set_device_state(struct rt2x00_dev *rt2x00dev,
+				      enum dev_state state)
+{
+	int retval = 0;
+
+	switch (state) {
+	case STATE_RADIO_ON:
+		retval = rt2500usb_enable_radio(rt2x00dev);
+		break;
+	case STATE_RADIO_OFF:
+		rt2500usb_disable_radio(rt2x00dev);
+		break;
+	case STATE_RADIO_RX_ON:
+	case STATE_RADIO_RX_OFF:
+		rt2500usb_toggle_rx(rt2x00dev, state);
+		break;
+	case STATE_DEEP_SLEEP:
+	case STATE_SLEEP:
+	case STATE_STANDBY:
+	case STATE_AWAKE:
+		retval = rt2500usb_set_state(rt2x00dev, state);
+		break;
+	default:
+		retval = -ENOTSUPP;
+		break;
+	}
+
+	return retval;
+}
+
+/*
+ * TX descriptor initialization
+ */
+static void rt2500usb_write_tx_desc(struct rt2x00_dev *rt2x00dev,
+				    struct data_desc *txd,
+				    struct txdata_entry_desc *desc,
+				    struct ieee80211_hdr *ieee80211hdr,
+				    unsigned int length,
+				    struct ieee80211_tx_control *control)
+{
+	u32 word;
+
+	/*
+	 * Start writing the descriptor words.
+	 */
+	rt2x00_desc_read(txd, 1, &word);
+	rt2x00_set_field32(&word, TXD_W1_IV_OFFSET, IEEE80211_HEADER);
+	rt2x00_set_field32(&word, TXD_W1_AIFS, desc->aifs);
+	rt2x00_set_field32(&word, TXD_W1_CWMIN, desc->cw_min);
+	rt2x00_set_field32(&word, TXD_W1_CWMAX, desc->cw_max);
+	rt2x00_desc_write(txd, 1, word);
+
+	rt2x00_desc_read(txd, 2, &word);
+	rt2x00_set_field32(&word, TXD_W2_PLCP_SIGNAL, desc->signal);
+	rt2x00_set_field32(&word, TXD_W2_PLCP_SERVICE, desc->service);
+	rt2x00_set_field32(&word, TXD_W2_PLCP_LENGTH_LOW, desc->length_low);
+	rt2x00_set_field32(&word, TXD_W2_PLCP_LENGTH_HIGH, desc->length_high);
+	rt2x00_desc_write(txd, 2, word);
+
+	rt2x00_desc_read(txd, 0, &word);
+	rt2x00_set_field32(&word, TXD_W0_RETRY_LIMIT, control->retry_limit);
+	rt2x00_set_field32(&word, TXD_W0_MORE_FRAG,
+			   test_bit(ENTRY_TXD_MORE_FRAG, &desc->flags));
+	rt2x00_set_field32(&word, TXD_W0_ACK,
+			   !(control->flags & IEEE80211_TXCTL_NO_ACK));
+	rt2x00_set_field32(&word, TXD_W0_TIMESTAMP,
+			   test_bit(ENTRY_TXD_REQ_TIMESTAMP, &desc->flags));
+	rt2x00_set_field32(&word, TXD_W0_OFDM,
+			   test_bit(ENTRY_TXD_OFDM_RATE, &desc->flags));
+	rt2x00_set_field32(&word, TXD_W0_NEW_SEQ,
+			   !!(control->flags & IEEE80211_TXCTL_FIRST_FRAGMENT));
+	rt2x00_set_field32(&word, TXD_W0_IFS, desc->ifs);
+	rt2x00_set_field32(&word, TXD_W0_DATABYTE_COUNT, length);
+	rt2x00_set_field32(&word, TXD_W0_CIPHER, CIPHER_NONE);
+	rt2x00_desc_write(txd, 0, word);
+}
+
+static int rt2500usb_get_tx_data_len(struct rt2x00_dev *rt2x00dev,
+				     int maxpacket, struct sk_buff *skb)
+{
+	int length;
+
+	/*
+	 * The length _must_ be a multiple of 2,
+	 * but it must _not_ be a multiple of the USB packet size.
+	 */
+	length = roundup(skb->len, 2);
+	length += (2 * !(length % maxpacket));
+
+	return length;
+}
+
+/*
+ * TX data initialization
+ */
+static void rt2500usb_kick_tx_queue(struct rt2x00_dev *rt2x00dev,
+				    unsigned int queue)
+{
+	u16 reg;
+
+	if (queue != IEEE80211_TX_QUEUE_BEACON)
+		return;
+
+	rt2500usb_register_read(rt2x00dev, TXRX_CSR19, &reg);
+	if (!rt2x00_get_field16(reg, TXRX_CSR19_BEACON_GEN)) {
+		rt2x00_set_field16(&reg, TXRX_CSR19_BEACON_GEN, 1);
+		/*
+		 * Beacon generation will fail initially.
+		 * To prevent this we need to register the TXRX_CSR19
+		 * register several times.
+		 */
+		rt2500usb_register_write(rt2x00dev, TXRX_CSR19, reg);
+		rt2500usb_register_write(rt2x00dev, TXRX_CSR19, 0);
+		rt2500usb_register_write(rt2x00dev, TXRX_CSR19, reg);
+		rt2500usb_register_write(rt2x00dev, TXRX_CSR19, 0);
+		rt2500usb_register_write(rt2x00dev, TXRX_CSR19, reg);
+	}
+}
+
+/*
+ * RX control handlers
+ */
+static void rt2500usb_fill_rxdone(struct data_entry *entry,
+				  struct rxdata_entry_desc *desc)
+{
+	struct urb *urb = entry->priv;
+	struct data_desc *rxd = (struct data_desc *)(entry->skb->data +
+						     (urb->actual_length -
+						      entry->ring->desc_size));
+	u32 word0;
+	u32 word1;
+
+	rt2x00_desc_read(rxd, 0, &word0);
+	rt2x00_desc_read(rxd, 1, &word1);
+
+	desc->flags = 0;
+	if (rt2x00_get_field32(word0, RXD_W0_CRC_ERROR))
+		desc->flags |= RX_FLAG_FAILED_FCS_CRC;
+	if (rt2x00_get_field32(word0, RXD_W0_PHYSICAL_ERROR))
+		desc->flags |= RX_FLAG_FAILED_PLCP_CRC;
+
+	/*
+	 * Obtain the status about this packet.
+	 */
+	desc->signal = rt2x00_get_field32(word1, RXD_W1_SIGNAL);
+	desc->rssi = rt2x00_get_field32(word1, RXD_W1_RSSI) -
+	    entry->ring->rt2x00dev->rssi_offset;
+	desc->ofdm = rt2x00_get_field32(word0, RXD_W0_OFDM);
+	desc->size = rt2x00_get_field32(word0, RXD_W0_DATABYTE_COUNT);
+
+	return;
+}
+
+/*
+ * Interrupt functions.
+ */
+static void rt2500usb_beacondone(struct urb *urb)
+{
+	struct data_entry *entry = (struct data_entry *)urb->context;
+	struct data_ring *ring = entry->ring;
+
+	if (!test_bit(DEVICE_ENABLED_RADIO, &ring->rt2x00dev->flags))
+		return;
+
+	/*
+	 * Check if this was the guardian beacon,
+	 * if that was the case we need to send the real beacon now.
+	 * Otherwise we should free the sk_buffer, the device
+	 * should be doing the rest of the work now.
+	 */
+	if (ring->index == 1) {
+		rt2x00_ring_index_done_inc(ring);
+		entry = rt2x00_get_data_entry(ring);
+		usb_submit_urb(entry->priv, GFP_ATOMIC);
+		rt2x00_ring_index_inc(ring);
+	} else if (ring->index_done == 1) {
+		entry = rt2x00_get_data_entry_done(ring);
+		if (entry->skb) {
+			dev_kfree_skb(entry->skb);
+			entry->skb = NULL;
+		}
+		rt2x00_ring_index_done_inc(ring);
+	}
+}
+
+/*
+ * Device probe functions.
+ */
+static int rt2500usb_validate_eeprom(struct rt2x00_dev *rt2x00dev)
+{
+	u16 word;
+	u8 *mac;
+
+	rt2x00usb_eeprom_read(rt2x00dev, rt2x00dev->eeprom, EEPROM_SIZE);
+
+	/*
+	 * Start validation of the data that has been read.
+	 */
+	mac = rt2x00_eeprom_addr(rt2x00dev, EEPROM_MAC_ADDR_0);
+	if (!is_valid_ether_addr(mac)) {
+		DECLARE_MAC_BUF(macbuf);
+
+		random_ether_addr(mac);
+		EEPROM(rt2x00dev, "MAC: %s\n", print_mac(macbuf, mac));
+	}
+
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_ANTENNA, &word);
+	if (word == 0xffff) {
+		rt2x00_set_field16(&word, EEPROM_ANTENNA_NUM, 2);
+		rt2x00_set_field16(&word, EEPROM_ANTENNA_TX_DEFAULT, 0);
+		rt2x00_set_field16(&word, EEPROM_ANTENNA_RX_DEFAULT, 0);
+		rt2x00_set_field16(&word, EEPROM_ANTENNA_LED_MODE, 0);
+		rt2x00_set_field16(&word, EEPROM_ANTENNA_DYN_TXAGC, 0);
+		rt2x00_set_field16(&word, EEPROM_ANTENNA_HARDWARE_RADIO, 0);
+		rt2x00_set_field16(&word, EEPROM_ANTENNA_RF_TYPE, RF2522);
+		rt2x00_eeprom_write(rt2x00dev, EEPROM_ANTENNA, word);
+		EEPROM(rt2x00dev, "Antenna: 0x%04x\n", word);
+	}
+
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC, &word);
+	if (word == 0xffff) {
+		rt2x00_set_field16(&word, EEPROM_NIC_CARDBUS_ACCEL, 0);
+		rt2x00_set_field16(&word, EEPROM_NIC_DYN_BBP_TUNE, 0);
+		rt2x00_set_field16(&word, EEPROM_NIC_CCK_TX_POWER, 0);
+		rt2x00_eeprom_write(rt2x00dev, EEPROM_NIC, word);
+		EEPROM(rt2x00dev, "NIC: 0x%04x\n", word);
+	}
+
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_CALIBRATE_OFFSET, &word);
+	if (word == 0xffff) {
+		rt2x00_set_field16(&word, EEPROM_CALIBRATE_OFFSET_RSSI,
+				   DEFAULT_RSSI_OFFSET);
+		rt2x00_eeprom_write(rt2x00dev, EEPROM_CALIBRATE_OFFSET, word);
+		EEPROM(rt2x00dev, "Calibrate offset: 0x%04x\n", word);
+	}
+
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE, &word);
+	if (word == 0xffff) {
+		rt2x00_set_field16(&word, EEPROM_BBPTUNE_THRESHOLD, 45);
+		rt2x00_eeprom_write(rt2x00dev, EEPROM_BBPTUNE, word);
+		EEPROM(rt2x00dev, "BBPtune: 0x%04x\n", word);
+	}
+
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE_VGC, &word);
+	if (word == 0xffff) {
+		rt2x00_set_field16(&word, EEPROM_BBPTUNE_VGCUPPER, 0x40);
+		rt2x00_eeprom_write(rt2x00dev, EEPROM_BBPTUNE_VGC, word);
+		EEPROM(rt2x00dev, "BBPtune vgc: 0x%04x\n", word);
+	}
+
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE_R17, &word);
+	if (word == 0xffff) {
+		rt2x00_set_field16(&word, EEPROM_BBPTUNE_R17_LOW, 0x48);
+		rt2x00_set_field16(&word, EEPROM_BBPTUNE_R17_HIGH, 0x41);
+		rt2x00_eeprom_write(rt2x00dev, EEPROM_BBPTUNE_R17, word);
+		EEPROM(rt2x00dev, "BBPtune r17: 0x%04x\n", word);
+	}
+
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE_R24, &word);
+	if (word == 0xffff) {
+		rt2x00_set_field16(&word, EEPROM_BBPTUNE_R24_LOW, 0x40);
+		rt2x00_set_field16(&word, EEPROM_BBPTUNE_R24_HIGH, 0x80);
+		rt2x00_eeprom_write(rt2x00dev, EEPROM_BBPTUNE_R24, word);
+		EEPROM(rt2x00dev, "BBPtune r24: 0x%04x\n", word);
+	}
+
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE_R25, &word);
+	if (word == 0xffff) {
+		rt2x00_set_field16(&word, EEPROM_BBPTUNE_R25_LOW, 0x40);
+		rt2x00_set_field16(&word, EEPROM_BBPTUNE_R25_HIGH, 0x50);
+		rt2x00_eeprom_write(rt2x00dev, EEPROM_BBPTUNE_R25, word);
+		EEPROM(rt2x00dev, "BBPtune r25: 0x%04x\n", word);
+	}
+
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_BBPTUNE_R61, &word);
+	if (word == 0xffff) {
+		rt2x00_set_field16(&word, EEPROM_BBPTUNE_R61_LOW, 0x60);
+		rt2x00_set_field16(&word, EEPROM_BBPTUNE_R61_HIGH, 0x6d);
+		rt2x00_eeprom_write(rt2x00dev, EEPROM_BBPTUNE_R61, word);
+		EEPROM(rt2x00dev, "BBPtune r61: 0x%04x\n", word);
+	}
+
+	return 0;
+}
+
+static int rt2500usb_init_eeprom(struct rt2x00_dev *rt2x00dev)
+{
+	u16 reg;
+	u16 value;
+	u16 eeprom;
+
+	/*
+	 * Read EEPROM word for configuration.
+	 */
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_ANTENNA, &eeprom);
+
+	/*
+	 * Identify RF chipset.
+	 */
+	value = rt2x00_get_field16(eeprom, EEPROM_ANTENNA_RF_TYPE);
+	rt2500usb_register_read(rt2x00dev, MAC_CSR0, &reg);
+	rt2x00_set_chip(rt2x00dev, RT2570, value, reg);
+
+	if (rt2x00_rev(&rt2x00dev->chip, 0xffff0)) {
+		ERROR(rt2x00dev, "Invalid RT chipset detected.\n");
+		return -ENODEV;
+	}
+
+	if (!rt2x00_rf(&rt2x00dev->chip, RF2522) &&
+	    !rt2x00_rf(&rt2x00dev->chip, RF2523) &&
+	    !rt2x00_rf(&rt2x00dev->chip, RF2524) &&
+	    !rt2x00_rf(&rt2x00dev->chip, RF2525) &&
+	    !rt2x00_rf(&rt2x00dev->chip, RF2525E) &&
+	    !rt2x00_rf(&rt2x00dev->chip, RF5222)) {
+		ERROR(rt2x00dev, "Invalid RF chipset detected.\n");
+		return -ENODEV;
+	}
+
+	/*
+	 * Identify default antenna configuration.
+	 */
+	rt2x00dev->hw->conf.antenna_sel_tx =
+	    rt2x00_get_field16(eeprom, EEPROM_ANTENNA_TX_DEFAULT);
+	rt2x00dev->hw->conf.antenna_sel_rx =
+	    rt2x00_get_field16(eeprom, EEPROM_ANTENNA_RX_DEFAULT);
+
+	/*
+	 * Store led mode, for correct led behaviour.
+	 */
+	rt2x00dev->led_mode =
+	    rt2x00_get_field16(eeprom, EEPROM_ANTENNA_LED_MODE);
+
+	/*
+	 * Check if the BBP tuning should be disabled.
+	 */
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC, &eeprom);
+	if (rt2x00_get_field16(eeprom, EEPROM_NIC_DYN_BBP_TUNE))
+		__set_bit(CONFIG_DISABLE_LINK_TUNING, &rt2x00dev->flags);
+
+	/*
+	 * Read the RSSI <-> dBm offset information.
+	 */
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_CALIBRATE_OFFSET, &eeprom);
+	rt2x00dev->rssi_offset =
+	    rt2x00_get_field16(eeprom, EEPROM_CALIBRATE_OFFSET_RSSI);
+
+	return 0;
+}
+
+/*
+ * RF value list for RF2522
+ * Supports: 2.4 GHz
+ */
+static const struct rf_channel rf_vals_bg_2522[] = {
+	{ 1,  0x00002050, 0x000c1fda, 0x00000101, 0 },
+	{ 2,  0x00002050, 0x000c1fee, 0x00000101, 0 },
+	{ 3,  0x00002050, 0x000c2002, 0x00000101, 0 },
+	{ 4,  0x00002050, 0x000c2016, 0x00000101, 0 },
+	{ 5,  0x00002050, 0x000c202a, 0x00000101, 0 },
+	{ 6,  0x00002050, 0x000c203e, 0x00000101, 0 },
+	{ 7,  0x00002050, 0x000c2052, 0x00000101, 0 },
+	{ 8,  0x00002050, 0x000c2066, 0x00000101, 0 },
+	{ 9,  0x00002050, 0x000c207a, 0x00000101, 0 },
+	{ 10, 0x00002050, 0x000c208e, 0x00000101, 0 },
+	{ 11, 0x00002050, 0x000c20a2, 0x00000101, 0 },
+	{ 12, 0x00002050, 0x000c20b6, 0x00000101, 0 },
+	{ 13, 0x00002050, 0x000c20ca, 0x00000101, 0 },
+	{ 14, 0x00002050, 0x000c20fa, 0x00000101, 0 },
+};
+
+/*
+ * RF value list for RF2523
+ * Supports: 2.4 GHz
+ */
+static const struct rf_channel rf_vals_bg_2523[] = {
+	{ 1,  0x00022010, 0x00000c9e, 0x000e0111, 0x00000a1b },
+	{ 2,  0x00022010, 0x00000ca2, 0x000e0111, 0x00000a1b },
+	{ 3,  0x00022010, 0x00000ca6, 0x000e0111, 0x00000a1b },
+	{ 4,  0x00022010, 0x00000caa, 0x000e0111, 0x00000a1b },
+	{ 5,  0x00022010, 0x00000cae, 0x000e0111, 0x00000a1b },
+	{ 6,  0x00022010, 0x00000cb2, 0x000e0111, 0x00000a1b },
+	{ 7,  0x00022010, 0x00000cb6, 0x000e0111, 0x00000a1b },
+	{ 8,  0x00022010, 0x00000cba, 0x000e0111, 0x00000a1b },
+	{ 9,  0x00022010, 0x00000cbe, 0x000e0111, 0x00000a1b },
+	{ 10, 0x00022010, 0x00000d02, 0x000e0111, 0x00000a1b },
+	{ 11, 0x00022010, 0x00000d06, 0x000e0111, 0x00000a1b },
+	{ 12, 0x00022010, 0x00000d0a, 0x000e0111, 0x00000a1b },
+	{ 13, 0x00022010, 0x00000d0e, 0x000e0111, 0x00000a1b },
+	{ 14, 0x00022010, 0x00000d1a, 0x000e0111, 0x00000a03 },
+};
+
+/*
+ * RF value list for RF2524
+ * Supports: 2.4 GHz
+ */
+static const struct rf_channel rf_vals_bg_2524[] = {
+	{ 1,  0x00032020, 0x00000c9e, 0x00000101, 0x00000a1b },
+	{ 2,  0x00032020, 0x00000ca2, 0x00000101, 0x00000a1b },
+	{ 3,  0x00032020, 0x00000ca6, 0x00000101, 0x00000a1b },
+	{ 4,  0x00032020, 0x00000caa, 0x00000101, 0x00000a1b },
+	{ 5,  0x00032020, 0x00000cae, 0x00000101, 0x00000a1b },
+	{ 6,  0x00032020, 0x00000cb2, 0x00000101, 0x00000a1b },
+	{ 7,  0x00032020, 0x00000cb6, 0x00000101, 0x00000a1b },
+	{ 8,  0x00032020, 0x00000cba, 0x00000101, 0x00000a1b },
+	{ 9,  0x00032020, 0x00000cbe, 0x00000101, 0x00000a1b },
+	{ 10, 0x00032020, 0x00000d02, 0x00000101, 0x00000a1b },
+	{ 11, 0x00032020, 0x00000d06, 0x00000101, 0x00000a1b },
+	{ 12, 0x00032020, 0x00000d0a, 0x00000101, 0x00000a1b },
+	{ 13, 0x00032020, 0x00000d0e, 0x00000101, 0x00000a1b },
+	{ 14, 0x00032020, 0x00000d1a, 0x00000101, 0x00000a03 },
+};
+
+/*
+ * RF value list for RF2525
+ * Supports: 2.4 GHz
+ */
+static const struct rf_channel rf_vals_bg_2525[] = {
+	{ 1,  0x00022020, 0x00080c9e, 0x00060111, 0x00000a1b },
+	{ 2,  0x00022020, 0x00080ca2, 0x00060111, 0x00000a1b },
+	{ 3,  0x00022020, 0x00080ca6, 0x00060111, 0x00000a1b },
+	{ 4,  0x00022020, 0x00080caa, 0x00060111, 0x00000a1b },
+	{ 5,  0x00022020, 0x00080cae, 0x00060111, 0x00000a1b },
+	{ 6,  0x00022020, 0x00080cb2, 0x00060111, 0x00000a1b },
+	{ 7,  0x00022020, 0x00080cb6, 0x00060111, 0x00000a1b },
+	{ 8,  0x00022020, 0x00080cba, 0x00060111, 0x00000a1b },
+	{ 9,  0x00022020, 0x00080cbe, 0x00060111, 0x00000a1b },
+	{ 10, 0x00022020, 0x00080d02, 0x00060111, 0x00000a1b },
+	{ 11, 0x00022020, 0x00080d06, 0x00060111, 0x00000a1b },
+	{ 12, 0x00022020, 0x00080d0a, 0x00060111, 0x00000a1b },
+	{ 13, 0x00022020, 0x00080d0e, 0x00060111, 0x00000a1b },
+	{ 14, 0x00022020, 0x00080d1a, 0x00060111, 0x00000a03 },
+};
+
+/*
+ * RF value list for RF2525e
+ * Supports: 2.4 GHz
+ */
+static const struct rf_channel rf_vals_bg_2525e[] = {
+	{ 1,  0x00022010, 0x0000089a, 0x00060111, 0x00000e1b },
+	{ 2,  0x00022010, 0x0000089e, 0x00060111, 0x00000e07 },
+	{ 3,  0x00022010, 0x0000089e, 0x00060111, 0x00000e1b },
+	{ 4,  0x00022010, 0x000008a2, 0x00060111, 0x00000e07 },
+	{ 5,  0x00022010, 0x000008a2, 0x00060111, 0x00000e1b },
+	{ 6,  0x00022010, 0x000008a6, 0x00060111, 0x00000e07 },
+	{ 7,  0x00022010, 0x000008a6, 0x00060111, 0x00000e1b },
+	{ 8,  0x00022010, 0x000008aa, 0x00060111, 0x00000e07 },
+	{ 9,  0x00022010, 0x000008aa, 0x00060111, 0x00000e1b },
+	{ 10, 0x00022010, 0x000008ae, 0x00060111, 0x00000e07 },
+	{ 11, 0x00022010, 0x000008ae, 0x00060111, 0x00000e1b },
+	{ 12, 0x00022010, 0x000008b2, 0x00060111, 0x00000e07 },
+	{ 13, 0x00022010, 0x000008b2, 0x00060111, 0x00000e1b },
+	{ 14, 0x00022010, 0x000008b6, 0x00060111, 0x00000e23 },
+};
+
+/*
+ * RF value list for RF5222
+ * Supports: 2.4 GHz & 5.2 GHz
+ */
+static const struct rf_channel rf_vals_5222[] = {
+	{ 1,  0x00022020, 0x00001136, 0x00000101, 0x00000a0b },
+	{ 2,  0x00022020, 0x0000113a, 0x00000101, 0x00000a0b },
+	{ 3,  0x00022020, 0x0000113e, 0x00000101, 0x00000a0b },
+	{ 4,  0x00022020, 0x00001182, 0x00000101, 0x00000a0b },
+	{ 5,  0x00022020, 0x00001186, 0x00000101, 0x00000a0b },
+	{ 6,  0x00022020, 0x0000118a, 0x00000101, 0x00000a0b },
+	{ 7,  0x00022020, 0x0000118e, 0x00000101, 0x00000a0b },
+	{ 8,  0x00022020, 0x00001192, 0x00000101, 0x00000a0b },
+	{ 9,  0x00022020, 0x00001196, 0x00000101, 0x00000a0b },
+	{ 10, 0x00022020, 0x0000119a, 0x00000101, 0x00000a0b },
+	{ 11, 0x00022020, 0x0000119e, 0x00000101, 0x00000a0b },
+	{ 12, 0x00022020, 0x000011a2, 0x00000101, 0x00000a0b },
+	{ 13, 0x00022020, 0x000011a6, 0x00000101, 0x00000a0b },
+	{ 14, 0x00022020, 0x000011ae, 0x00000101, 0x00000a1b },
+
+	/* 802.11 UNI / HyperLan 2 */
+	{ 36, 0x00022010, 0x00018896, 0x00000101, 0x00000a1f },
+	{ 40, 0x00022010, 0x0001889a, 0x00000101, 0x00000a1f },
+	{ 44, 0x00022010, 0x0001889e, 0x00000101, 0x00000a1f },
+	{ 48, 0x00022010, 0x000188a2, 0x00000101, 0x00000a1f },
+	{ 52, 0x00022010, 0x000188a6, 0x00000101, 0x00000a1f },
+	{ 66, 0x00022010, 0x000188aa, 0x00000101, 0x00000a1f },
+	{ 60, 0x00022010, 0x000188ae, 0x00000101, 0x00000a1f },
+	{ 64, 0x00022010, 0x000188b2, 0x00000101, 0x00000a1f },
+
+	/* 802.11 HyperLan 2 */
+	{ 100, 0x00022010, 0x00008802, 0x00000101, 0x00000a0f },
+	{ 104, 0x00022010, 0x00008806, 0x00000101, 0x00000a0f },
+	{ 108, 0x00022010, 0x0000880a, 0x00000101, 0x00000a0f },
+	{ 112, 0x00022010, 0x0000880e, 0x00000101, 0x00000a0f },
+	{ 116, 0x00022010, 0x00008812, 0x00000101, 0x00000a0f },
+	{ 120, 0x00022010, 0x00008816, 0x00000101, 0x00000a0f },
+	{ 124, 0x00022010, 0x0000881a, 0x00000101, 0x00000a0f },
+	{ 128, 0x00022010, 0x0000881e, 0x00000101, 0x00000a0f },
+	{ 132, 0x00022010, 0x00008822, 0x00000101, 0x00000a0f },
+	{ 136, 0x00022010, 0x00008826, 0x00000101, 0x00000a0f },
+
+	/* 802.11 UNII */
+	{ 140, 0x00022010, 0x0000882a, 0x00000101, 0x00000a0f },
+	{ 149, 0x00022020, 0x000090a6, 0x00000101, 0x00000a07 },
+	{ 153, 0x00022020, 0x000090ae, 0x00000101, 0x00000a07 },
+	{ 157, 0x00022020, 0x000090b6, 0x00000101, 0x00000a07 },
+	{ 161, 0x00022020, 0x000090be, 0x00000101, 0x00000a07 },
+};
+
+static void rt2500usb_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
+{
+	struct hw_mode_spec *spec = &rt2x00dev->spec;
+	u8 *txpower;
+	unsigned int i;
+
+	/*
+	 * Initialize all hw fields.
+	 */
+	rt2x00dev->hw->flags =
+	    IEEE80211_HW_HOST_GEN_BEACON_TEMPLATE |
+	    IEEE80211_HW_RX_INCLUDES_FCS |
+	    IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING;
+	rt2x00dev->hw->extra_tx_headroom = TXD_DESC_SIZE;
+	rt2x00dev->hw->max_signal = MAX_SIGNAL;
+	rt2x00dev->hw->max_rssi = MAX_RX_SSI;
+	rt2x00dev->hw->queues = 2;
+
+	SET_IEEE80211_DEV(rt2x00dev->hw, &rt2x00dev_usb(rt2x00dev)->dev);
+	SET_IEEE80211_PERM_ADDR(rt2x00dev->hw,
+				rt2x00_eeprom_addr(rt2x00dev,
+						   EEPROM_MAC_ADDR_0));
+
+	/*
+	 * Convert tx_power array in eeprom.
+	 */
+	txpower = rt2x00_eeprom_addr(rt2x00dev, EEPROM_TXPOWER_START);
+	for (i = 0; i < 14; i++)
+		txpower[i] = TXPOWER_FROM_DEV(txpower[i]);
+
+	/*
+	 * Initialize hw_mode information.
+	 */
+	spec->num_modes = 2;
+	spec->num_rates = 12;
+	spec->tx_power_a = NULL;
+	spec->tx_power_bg = txpower;
+	spec->tx_power_default = DEFAULT_TXPOWER;
+
+	if (rt2x00_rf(&rt2x00dev->chip, RF2522)) {
+		spec->num_channels = ARRAY_SIZE(rf_vals_bg_2522);
+		spec->channels = rf_vals_bg_2522;
+	} else if (rt2x00_rf(&rt2x00dev->chip, RF2523)) {
+		spec->num_channels = ARRAY_SIZE(rf_vals_bg_2523);
+		spec->channels = rf_vals_bg_2523;
+	} else if (rt2x00_rf(&rt2x00dev->chip, RF2524)) {
+		spec->num_channels = ARRAY_SIZE(rf_vals_bg_2524);
+		spec->channels = rf_vals_bg_2524;
+	} else if (rt2x00_rf(&rt2x00dev->chip, RF2525)) {
+		spec->num_channels = ARRAY_SIZE(rf_vals_bg_2525);
+		spec->channels = rf_vals_bg_2525;
+	} else if (rt2x00_rf(&rt2x00dev->chip, RF2525E)) {
+		spec->num_channels = ARRAY_SIZE(rf_vals_bg_2525e);
+		spec->channels = rf_vals_bg_2525e;
+	} else if (rt2x00_rf(&rt2x00dev->chip, RF5222)) {
+		spec->num_channels = ARRAY_SIZE(rf_vals_5222);
+		spec->channels = rf_vals_5222;
+		spec->num_modes = 3;
+	}
+}
+
+static int rt2500usb_probe_hw(struct rt2x00_dev *rt2x00dev)
+{
+	int retval;
+
+	/*
+	 * Allocate eeprom data.
+	 */
+	retval = rt2500usb_validate_eeprom(rt2x00dev);
+	if (retval)
+		return retval;
+
+	retval = rt2500usb_init_eeprom(rt2x00dev);
+	if (retval)
+		return retval;
+
+	/*
+	 * Initialize hw specifications.
+	 */
+	rt2500usb_probe_hw_mode(rt2x00dev);
+
+	/*
+	 * This device requires the beacon ring
+	 */
+	__set_bit(DRIVER_REQUIRE_BEACON_RING, &rt2x00dev->flags);
+
+	/*
+	 * Set the rssi offset.
+	 */
+	rt2x00dev->rssi_offset = DEFAULT_RSSI_OFFSET;
+
+	return 0;
+}
+
+/*
+ * IEEE80211 stack callback functions.
+ */
+static void rt2500usb_configure_filter(struct ieee80211_hw *hw,
+				       unsigned int changed_flags,
+				       unsigned int *total_flags,
+				       int mc_count,
+				       struct dev_addr_list *mc_list)
+{
+	struct rt2x00_dev *rt2x00dev = hw->priv;
+	struct interface *intf = &rt2x00dev->interface;
+	u16 reg;
+
+	/*
+	 * Mask off any flags we are going to ignore from
+	 * the total_flags field.
+	 */
+	*total_flags &=
+	    FIF_ALLMULTI |
+	    FIF_FCSFAIL |
+	    FIF_PLCPFAIL |
+	    FIF_CONTROL |
+	    FIF_OTHER_BSS |
+	    FIF_PROMISC_IN_BSS;
+
+	/*
+	 * Apply some rules to the filters:
+	 * - Some filters imply different filters to be set.
+	 * - Some things we can't filter out at all.
+	 * - Some filters are set based on interface type.
+	 */
+	if (mc_count)
+		*total_flags |= FIF_ALLMULTI;
+	if (*total_flags & FIF_OTHER_BSS ||
+	    *total_flags & FIF_PROMISC_IN_BSS)
+		*total_flags |= FIF_PROMISC_IN_BSS | FIF_OTHER_BSS;
+	if (is_interface_type(intf, IEEE80211_IF_TYPE_AP))
+		*total_flags |= FIF_PROMISC_IN_BSS;
+
+	/*
+	 * Check if there is any work left for us.
+	 */
+	if (intf->filter == *total_flags)
+		return;
+	intf->filter = *total_flags;
+
+	/*
+	 * When in atomic context, reschedule and let rt2x00lib
+	 * call this function again.
+	 */
+	if (in_atomic()) {
+		queue_work(rt2x00dev->hw->workqueue, &rt2x00dev->filter_work);
+		return;
+	}
+
+	/*
+	 * Start configuration steps.
+	 * Note that the version error will always be dropped
+	 * and broadcast frames will always be accepted since
+	 * there is no filter for it at this time.
+	 */
+	rt2500usb_register_read(rt2x00dev, TXRX_CSR2, &reg);
+	rt2x00_set_field16(&reg, TXRX_CSR2_DROP_CRC,
+			   !(*total_flags & FIF_FCSFAIL));
+	rt2x00_set_field16(&reg, TXRX_CSR2_DROP_PHYSICAL,
+			   !(*total_flags & FIF_PLCPFAIL));
+	rt2x00_set_field16(&reg, TXRX_CSR2_DROP_CONTROL,
+			   !(*total_flags & FIF_CONTROL));
+	rt2x00_set_field16(&reg, TXRX_CSR2_DROP_NOT_TO_ME,
+			   !(*total_flags & FIF_PROMISC_IN_BSS));
+	rt2x00_set_field16(&reg, TXRX_CSR2_DROP_TODS,
+			   !(*total_flags & FIF_PROMISC_IN_BSS));
+	rt2x00_set_field16(&reg, TXRX_CSR2_DROP_VERSION_ERROR, 1);
+	rt2x00_set_field16(&reg, TXRX_CSR2_DROP_MULTICAST,
+			   !(*total_flags & FIF_ALLMULTI));
+	rt2x00_set_field16(&reg, TXRX_CSR2_DROP_BROADCAST, 0);
+	rt2500usb_register_write(rt2x00dev, TXRX_CSR2, reg);
+}
+
+static int rt2500usb_beacon_update(struct ieee80211_hw *hw,
+				   struct sk_buff *skb,
+				   struct ieee80211_tx_control *control)
+{
+	struct rt2x00_dev *rt2x00dev = hw->priv;
+	struct usb_device *usb_dev =
+	    interface_to_usbdev(rt2x00dev_usb(rt2x00dev));
+	struct data_ring *ring =
+	    rt2x00lib_get_ring(rt2x00dev, IEEE80211_TX_QUEUE_BEACON);
+	struct data_entry *beacon;
+	struct data_entry *guardian;
+	int pipe = usb_sndbulkpipe(usb_dev, 1);
+	int max_packet = usb_maxpacket(usb_dev, pipe, 1);
+	int length;
+
+	/*
+	 * Just in case the ieee80211 doesn't set this,
+	 * but we need this queue set for the descriptor
+	 * initialization.
+	 */
+	control->queue = IEEE80211_TX_QUEUE_BEACON;
+
+	/*
+	 * Obtain 2 entries, one for the guardian byte,
+	 * the second for the actual beacon.
+	 */
+	guardian = rt2x00_get_data_entry(ring);
+	rt2x00_ring_index_inc(ring);
+	beacon = rt2x00_get_data_entry(ring);
+
+	/*
+	 * First we create the beacon.
+	 */
+	skb_push(skb, ring->desc_size);
+	memset(skb->data, 0, ring->desc_size);
+
+	rt2x00lib_write_tx_desc(rt2x00dev, (struct data_desc *)skb->data,
+				(struct ieee80211_hdr *)(skb->data +
+							 ring->desc_size),
+				skb->len - ring->desc_size, control);
+
+	length = rt2500usb_get_tx_data_len(rt2x00dev, max_packet, skb);
+
+	usb_fill_bulk_urb(beacon->priv, usb_dev, pipe,
+			  skb->data, length, rt2500usb_beacondone, beacon);
+
+	beacon->skb = skb;
+
+	/*
+	 * Second we need to create the guardian byte.
+	 * We only need a single byte, so lets recycle
+	 * the 'flags' field we are not using for beacons.
+	 */
+	guardian->flags = 0;
+	usb_fill_bulk_urb(guardian->priv, usb_dev, pipe,
+			  &guardian->flags, 1, rt2500usb_beacondone, guardian);
+
+	/*
+	 * Send out the guardian byte.
+	 */
+	usb_submit_urb(guardian->priv, GFP_ATOMIC);
+
+	/*
+	 * Enable beacon generation.
+	 */
+	rt2500usb_kick_tx_queue(rt2x00dev, IEEE80211_TX_QUEUE_BEACON);
+
+	return 0;
+}
+
+static const struct ieee80211_ops rt2500usb_mac80211_ops = {
+	.tx			= rt2x00mac_tx,
+	.start			= rt2x00mac_start,
+	.stop			= rt2x00mac_stop,
+	.add_interface		= rt2x00mac_add_interface,
+	.remove_interface	= rt2x00mac_remove_interface,
+	.config			= rt2x00mac_config,
+	.config_interface	= rt2x00mac_config_interface,
+	.configure_filter	= rt2500usb_configure_filter,
+	.get_stats		= rt2x00mac_get_stats,
+	.erp_ie_changed		= rt2x00mac_erp_ie_changed,
+	.conf_tx		= rt2x00mac_conf_tx,
+	.get_tx_stats		= rt2x00mac_get_tx_stats,
+	.beacon_update		= rt2500usb_beacon_update,
+};
+
+static const struct rt2x00lib_ops rt2500usb_rt2x00_ops = {
+	.probe_hw		= rt2500usb_probe_hw,
+	.initialize		= rt2x00usb_initialize,
+	.uninitialize		= rt2x00usb_uninitialize,
+	.set_device_state	= rt2500usb_set_device_state,
+	.link_stats		= rt2500usb_link_stats,
+	.reset_tuner		= rt2500usb_reset_tuner,
+	.link_tuner		= rt2500usb_link_tuner,
+	.write_tx_desc		= rt2500usb_write_tx_desc,
+	.write_tx_data		= rt2x00usb_write_tx_data,
+	.get_tx_data_len	= rt2500usb_get_tx_data_len,
+	.kick_tx_queue		= rt2500usb_kick_tx_queue,
+	.fill_rxdone		= rt2500usb_fill_rxdone,
+	.config_mac_addr	= rt2500usb_config_mac_addr,
+	.config_bssid		= rt2500usb_config_bssid,
+	.config_type		= rt2500usb_config_type,
+	.config_preamble	= rt2500usb_config_preamble,
+	.config			= rt2500usb_config,
+};
+
+static const struct rt2x00_ops rt2500usb_ops = {
+	.name		= DRV_NAME,
+	.rxd_size	= RXD_DESC_SIZE,
+	.txd_size	= TXD_DESC_SIZE,
+	.eeprom_size	= EEPROM_SIZE,
+	.rf_size	= RF_SIZE,
+	.lib		= &rt2500usb_rt2x00_ops,
+	.hw		= &rt2500usb_mac80211_ops,
+#ifdef CONFIG_RT2X00_LIB_DEBUGFS
+	.debugfs	= &rt2500usb_rt2x00debug,
+#endif /* CONFIG_RT2X00_LIB_DEBUGFS */
+};
+
+/*
+ * rt2500usb module information.
+ */
+static struct usb_device_id rt2500usb_device_table[] = {
+	/* ASUS */
+	{ USB_DEVICE(0x0b05, 0x1706), USB_DEVICE_DATA(&rt2500usb_ops) },
+	{ USB_DEVICE(0x0b05, 0x1707), USB_DEVICE_DATA(&rt2500usb_ops) },
+	/* Belkin */
+	{ USB_DEVICE(0x050d, 0x7050), USB_DEVICE_DATA(&rt2500usb_ops) },
+	{ USB_DEVICE(0x050d, 0x7051), USB_DEVICE_DATA(&rt2500usb_ops) },
+	{ USB_DEVICE(0x050d, 0x705a), USB_DEVICE_DATA(&rt2500usb_ops) },
+	/* Cisco Systems */
+	{ USB_DEVICE(0x13b1, 0x000d), USB_DEVICE_DATA(&rt2500usb_ops) },
+	{ USB_DEVICE(0x13b1, 0x0011), USB_DEVICE_DATA(&rt2500usb_ops) },
+	{ USB_DEVICE(0x13b1, 0x001a), USB_DEVICE_DATA(&rt2500usb_ops) },
+	/* Conceptronic */
+	{ USB_DEVICE(0x14b2, 0x3c02), USB_DEVICE_DATA(&rt2500usb_ops) },
+	/* D-LINK */
+	{ USB_DEVICE(0x2001, 0x3c00), USB_DEVICE_DATA(&rt2500usb_ops) },
+	/* Gigabyte */
+	{ USB_DEVICE(0x1044, 0x8001), USB_DEVICE_DATA(&rt2500usb_ops) },
+	{ USB_DEVICE(0x1044, 0x8007), USB_DEVICE_DATA(&rt2500usb_ops) },
+	/* Hercules */
+	{ USB_DEVICE(0x06f8, 0xe000), USB_DEVICE_DATA(&rt2500usb_ops) },
+	/* Melco */
+	{ USB_DEVICE(0x0411, 0x0066), USB_DEVICE_DATA(&rt2500usb_ops) },
+	{ USB_DEVICE(0x0411, 0x0067), USB_DEVICE_DATA(&rt2500usb_ops) },
+	{ USB_DEVICE(0x0411, 0x008b), USB_DEVICE_DATA(&rt2500usb_ops) },
+	{ USB_DEVICE(0x0411, 0x0097), USB_DEVICE_DATA(&rt2500usb_ops) },
+
+	/* MSI */
+	{ USB_DEVICE(0x0db0, 0x6861), USB_DEVICE_DATA(&rt2500usb_ops) },
+	{ USB_DEVICE(0x0db0, 0x6865), USB_DEVICE_DATA(&rt2500usb_ops) },
+	{ USB_DEVICE(0x0db0, 0x6869), USB_DEVICE_DATA(&rt2500usb_ops) },
+	/* Ralink */
+	{ USB_DEVICE(0x148f, 0x1706), USB_DEVICE_DATA(&rt2500usb_ops) },
+	{ USB_DEVICE(0x148f, 0x2570), USB_DEVICE_DATA(&rt2500usb_ops) },
+	{ USB_DEVICE(0x148f, 0x2573), USB_DEVICE_DATA(&rt2500usb_ops) },
+	{ USB_DEVICE(0x148f, 0x9020), USB_DEVICE_DATA(&rt2500usb_ops) },
+	/* Siemens */
+	{ USB_DEVICE(0x0681, 0x3c06), USB_DEVICE_DATA(&rt2500usb_ops) },
+	/* SMC */
+	{ USB_DEVICE(0x0707, 0xee13), USB_DEVICE_DATA(&rt2500usb_ops) },
+	/* Spairon */
+	{ USB_DEVICE(0x114b, 0x0110), USB_DEVICE_DATA(&rt2500usb_ops) },
+	/* Trust */
+	{ USB_DEVICE(0x0eb0, 0x9020), USB_DEVICE_DATA(&rt2500usb_ops) },
+	/* Zinwell */
+	{ USB_DEVICE(0x5a57, 0x0260), USB_DEVICE_DATA(&rt2500usb_ops) },
+	{ 0, }
+};
+
+MODULE_AUTHOR(DRV_PROJECT);
+MODULE_VERSION(DRV_VERSION);
+MODULE_DESCRIPTION("Ralink RT2500 USB Wireless LAN driver.");
+MODULE_SUPPORTED_DEVICE("Ralink RT2570 USB chipset based cards");
+MODULE_DEVICE_TABLE(usb, rt2500usb_device_table);
+MODULE_LICENSE("GPL");
+
+static struct usb_driver rt2500usb_driver = {
+	.name		= DRV_NAME,
+	.id_table	= rt2500usb_device_table,
+	.probe		= rt2x00usb_probe,
+	.disconnect	= rt2x00usb_disconnect,
+	.suspend	= rt2x00usb_suspend,
+	.resume		= rt2x00usb_resume,
+};
+
+static int __init rt2500usb_init(void)
+{
+	return usb_register(&rt2500usb_driver);
+}
+
+static void __exit rt2500usb_exit(void)
+{
+	usb_deregister(&rt2500usb_driver);
+}
+
+module_init(rt2500usb_init);
+module_exit(rt2500usb_exit);
diff --git a/drivers/net/wireless/rt2x00/rt2500usb.h b/drivers/net/wireless/rt2x00/rt2500usb.h
new file mode 100644
index 000000000000..b18d56e73cf1
--- /dev/null
+++ b/drivers/net/wireless/rt2x00/rt2500usb.h
@@ -0,0 +1,798 @@
+/*
+	Copyright (C) 2004 - 2007 rt2x00 SourceForge Project
+	<http://rt2x00.serialmonkey.com>
+
+	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, or
+	(at your option) any later version.
+
+	This program is distributed in the hope that it will be useful,
+	but WITHOUT ANY WARRANTY; without even the implied warranty of
+	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+	GNU General Public License for more details.
+
+	You should have received a copy of the GNU General Public License
+	along with this program; if not, write to the
+	Free Software Foundation, Inc.,
+	59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+/*
+	Module: rt2500usb
+	Abstract: Data structures and registers for the rt2500usb module.
+	Supported chipsets: RT2570.
+ */
+
+#ifndef RT2500USB_H
+#define RT2500USB_H
+
+/*
+ * RF chip defines.
+ */
+#define RF2522				0x0000
+#define RF2523				0x0001
+#define RF2524				0x0002
+#define RF2525				0x0003
+#define RF2525E				0x0005
+#define RF5222				0x0010
+
+/*
+ * RT2570 version
+ */
+#define RT2570_VERSION_B		2
+#define RT2570_VERSION_C		3
+#define RT2570_VERSION_D		4
+
+/*
+ * Signal information.
+ * Defaul offset is required for RSSI <-> dBm conversion.
+ */
+#define MAX_SIGNAL			100
+#define MAX_RX_SSI			-1
+#define DEFAULT_RSSI_OFFSET		120
+
+/*
+ * Register layout information.
+ */
+#define CSR_REG_BASE			0x0400
+#define CSR_REG_SIZE			0x0100
+#define EEPROM_BASE			0x0000
+#define EEPROM_SIZE			0x006a
+#define BBP_SIZE			0x0060
+#define RF_SIZE				0x0014
+
+/*
+ * Control/Status Registers(CSR).
+ * Some values are set in TU, whereas 1 TU == 1024 us.
+ */
+
+/*
+ * MAC_CSR0: ASIC revision number.
+ */
+#define MAC_CSR0			0x0400
+
+/*
+ * MAC_CSR1: System control.
+ * SOFT_RESET: Software reset, 1: reset, 0: normal.
+ * BBP_RESET: Hardware reset, 1: reset, 0, release.
+ * HOST_READY: Host ready after initialization.
+ */
+#define MAC_CSR1			0x0402
+#define MAC_CSR1_SOFT_RESET		FIELD16(0x00000001)
+#define MAC_CSR1_BBP_RESET		FIELD16(0x00000002)
+#define MAC_CSR1_HOST_READY		FIELD16(0x00000004)
+
+/*
+ * MAC_CSR2: STA MAC register 0.
+ */
+#define MAC_CSR2			0x0404
+#define MAC_CSR2_BYTE0			FIELD16(0x00ff)
+#define MAC_CSR2_BYTE1			FIELD16(0xff00)
+
+/*
+ * MAC_CSR3: STA MAC register 1.
+ */
+#define MAC_CSR3			0x0406
+#define MAC_CSR3_BYTE2			FIELD16(0x00ff)
+#define MAC_CSR3_BYTE3			FIELD16(0xff00)
+
+/*
+ * MAC_CSR4: STA MAC register 2.
+ */
+#define MAC_CSR4			0X0408
+#define MAC_CSR4_BYTE4			FIELD16(0x00ff)
+#define MAC_CSR4_BYTE5			FIELD16(0xff00)
+
+/*
+ * MAC_CSR5: BSSID register 0.
+ */
+#define MAC_CSR5			0x040a
+#define MAC_CSR5_BYTE0			FIELD16(0x00ff)
+#define MAC_CSR5_BYTE1			FIELD16(0xff00)
+
+/*
+ * MAC_CSR6: BSSID register 1.
+ */
+#define MAC_CSR6			0x040c
+#define MAC_CSR6_BYTE2			FIELD16(0x00ff)
+#define MAC_CSR6_BYTE3			FIELD16(0xff00)
+
+/*
+ * MAC_CSR7: BSSID register 2.
+ */
+#define MAC_CSR7			0x040e
+#define MAC_CSR7_BYTE4			FIELD16(0x00ff)
+#define MAC_CSR7_BYTE5			FIELD16(0xff00)
+
+/*
+ * MAC_CSR8: Max frame length.
+ */
+#define MAC_CSR8			0x0410
+#define MAC_CSR8_MAX_FRAME_UNIT		FIELD16(0x0fff)
+
+/*
+ * Misc MAC_CSR registers.
+ * MAC_CSR9: Timer control.
+ * MAC_CSR10: Slot time.
+ * MAC_CSR11: IFS.
+ * MAC_CSR12: EIFS.
+ * MAC_CSR13: Power mode0.
+ * MAC_CSR14: Power mode1.
+ * MAC_CSR15: Power saving transition0
+ * MAC_CSR16: Power saving transition1
+ */
+#define MAC_CSR9			0x0412
+#define MAC_CSR10			0x0414
+#define MAC_CSR11			0x0416
+#define MAC_CSR12			0x0418
+#define MAC_CSR13			0x041a
+#define MAC_CSR14			0x041c
+#define MAC_CSR15			0x041e
+#define MAC_CSR16			0x0420
+
+/*
+ * MAC_CSR17: Manual power control / status register.
+ * Allowed state: 0 deep_sleep, 1: sleep, 2: standby, 3: awake.
+ * SET_STATE: Set state. Write 1 to trigger, self cleared.
+ * BBP_DESIRE_STATE: BBP desired state.
+ * RF_DESIRE_STATE: RF desired state.
+ * BBP_CURRENT_STATE: BBP current state.
+ * RF_CURRENT_STATE: RF current state.
+ * PUT_TO_SLEEP: Put to sleep. Write 1 to trigger, self cleared.
+ */
+#define MAC_CSR17			0x0422
+#define MAC_CSR17_SET_STATE		FIELD16(0x0001)
+#define MAC_CSR17_BBP_DESIRE_STATE	FIELD16(0x0006)
+#define MAC_CSR17_RF_DESIRE_STATE	FIELD16(0x0018)
+#define MAC_CSR17_BBP_CURR_STATE	FIELD16(0x0060)
+#define MAC_CSR17_RF_CURR_STATE		FIELD16(0x0180)
+#define MAC_CSR17_PUT_TO_SLEEP		FIELD16(0x0200)
+
+/*
+ * MAC_CSR18: Wakeup timer register.
+ * DELAY_AFTER_BEACON: Delay after Tbcn expired in units of 1/16 TU.
+ * BEACONS_BEFORE_WAKEUP: Number of beacon before wakeup.
+ * AUTO_WAKE: Enable auto wakeup / sleep mechanism.
+ */
+#define MAC_CSR18			0x0424
+#define MAC_CSR18_DELAY_AFTER_BEACON	FIELD16(0x00ff)
+#define MAC_CSR18_BEACONS_BEFORE_WAKEUP	FIELD16(0x7f00)
+#define MAC_CSR18_AUTO_WAKE		FIELD16(0x8000)
+
+/*
+ * MAC_CSR19: GPIO control register.
+ */
+#define MAC_CSR19			0x0426
+
+/*
+ * MAC_CSR20: LED control register.
+ * ACTIVITY: 0: idle, 1: active.
+ * LINK: 0: linkoff, 1: linkup.
+ * ACTIVITY_POLARITY: 0: active low, 1: active high.
+ */
+#define MAC_CSR20			0x0428
+#define MAC_CSR20_ACTIVITY		FIELD16(0x0001)
+#define MAC_CSR20_LINK			FIELD16(0x0002)
+#define MAC_CSR20_ACTIVITY_POLARITY	FIELD16(0x0004)
+
+/*
+ * MAC_CSR21: LED control register.
+ * ON_PERIOD: On period, default 70ms.
+ * OFF_PERIOD: Off period, default 30ms.
+ */
+#define MAC_CSR21			0x042a
+#define MAC_CSR21_ON_PERIOD		FIELD16(0x00ff)
+#define MAC_CSR21_OFF_PERIOD		FIELD16(0xff00)
+
+/*
+ * Collision window control register.
+ */
+#define MAC_CSR22			0x042c
+
+/*
+ * Transmit related CSRs.
+ * Some values are set in TU, whereas 1 TU == 1024 us.
+ */
+
+/*
+ * TXRX_CSR0: Security control register.
+ */
+#define TXRX_CSR0			0x0440
+#define TXRX_CSR0_ALGORITHM		FIELD16(0x0007)
+#define TXRX_CSR0_IV_OFFSET		FIELD16(0x01f8)
+#define TXRX_CSR0_KEY_ID		FIELD16(0x1e00)
+
+/*
+ * TXRX_CSR1: TX configuration.
+ * ACK_TIMEOUT: ACK Timeout in unit of 1-us.
+ * TSF_OFFSET: TSF offset in MAC header.
+ * AUTO_SEQUENCE: Let ASIC control frame sequence number.
+ */
+#define TXRX_CSR1			0x0442
+#define TXRX_CSR1_ACK_TIMEOUT		FIELD16(0x00ff)
+#define TXRX_CSR1_TSF_OFFSET		FIELD16(0x7f00)
+#define TXRX_CSR1_AUTO_SEQUENCE		FIELD16(0x8000)
+
+/*
+ * TXRX_CSR2: RX control.
+ * DISABLE_RX: Disable rx engine.
+ * DROP_CRC: Drop crc error.
+ * DROP_PHYSICAL: Drop physical error.
+ * DROP_CONTROL: Drop control frame.
+ * DROP_NOT_TO_ME: Drop not to me unicast frame.
+ * DROP_TODS: Drop frame tods bit is true.
+ * DROP_VERSION_ERROR: Drop version error frame.
+ * DROP_MCAST: Drop multicast frames.
+ * DROP_BCAST: Drop broadcast frames.
+ */
+#define TXRX_CSR2			0x0444
+#define	TXRX_CSR2_DISABLE_RX		FIELD16(0x0001)
+#define TXRX_CSR2_DROP_CRC		FIELD16(0x0002)
+#define TXRX_CSR2_DROP_PHYSICAL		FIELD16(0x0004)
+#define TXRX_CSR2_DROP_CONTROL		FIELD16(0x0008)
+#define TXRX_CSR2_DROP_NOT_TO_ME	FIELD16(0x0010)
+#define TXRX_CSR2_DROP_TODS		FIELD16(0x0020)
+#define TXRX_CSR2_DROP_VERSION_ERROR	FIELD16(0x0040)
+#define TXRX_CSR2_DROP_MULTICAST	FIELD16(0x0200)
+#define TXRX_CSR2_DROP_BROADCAST	FIELD16(0x0400)
+
+/*
+ * RX BBP ID registers
+ * TXRX_CSR3: CCK RX BBP ID.
+ * TXRX_CSR4: OFDM RX BBP ID.
+ */
+#define TXRX_CSR3			0x0446
+#define TXRX_CSR4			0x0448
+
+/*
+ * TXRX_CSR5: CCK TX BBP ID0.
+ */
+#define TXRX_CSR5			0x044a
+#define TXRX_CSR5_BBP_ID0		FIELD16(0x007f)
+#define TXRX_CSR5_BBP_ID0_VALID		FIELD16(0x0080)
+#define TXRX_CSR5_BBP_ID1		FIELD16(0x7f00)
+#define TXRX_CSR5_BBP_ID1_VALID		FIELD16(0x8000)
+
+/*
+ * TXRX_CSR6: CCK TX BBP ID1.
+ */
+#define TXRX_CSR6			0x044c
+#define TXRX_CSR6_BBP_ID0		FIELD16(0x007f)
+#define TXRX_CSR6_BBP_ID0_VALID		FIELD16(0x0080)
+#define TXRX_CSR6_BBP_ID1		FIELD16(0x7f00)
+#define TXRX_CSR6_BBP_ID1_VALID		FIELD16(0x8000)
+
+/*
+ * TXRX_CSR7: OFDM TX BBP ID0.
+ */
+#define TXRX_CSR7			0x044e
+#define TXRX_CSR7_BBP_ID0		FIELD16(0x007f)
+#define TXRX_CSR7_BBP_ID0_VALID		FIELD16(0x0080)
+#define TXRX_CSR7_BBP_ID1		FIELD16(0x7f00)
+#define TXRX_CSR7_BBP_ID1_VALID		FIELD16(0x8000)
+
+/*
+ * TXRX_CSR5: OFDM TX BBP ID1.
+ */
+#define TXRX_CSR8			0x0450
+#define TXRX_CSR8_BBP_ID0		FIELD16(0x007f)
+#define TXRX_CSR8_BBP_ID0_VALID		FIELD16(0x0080)
+#define TXRX_CSR8_BBP_ID1		FIELD16(0x7f00)
+#define TXRX_CSR8_BBP_ID1_VALID		FIELD16(0x8000)
+
+/*
+ * TXRX_CSR9: TX ACK time-out.
+ */
+#define TXRX_CSR9			0x0452
+
+/*
+ * TXRX_CSR10: Auto responder control.
+ */
+#define TXRX_CSR10			0x0454
+#define TXRX_CSR10_AUTORESPOND_PREAMBLE FIELD16(0x0004)
+
+/*
+ * TXRX_CSR11: Auto responder basic rate.
+ */
+#define TXRX_CSR11			0x0456
+
+/*
+ * ACK/CTS time registers.
+ */
+#define TXRX_CSR12			0x0458
+#define TXRX_CSR13			0x045a
+#define TXRX_CSR14			0x045c
+#define TXRX_CSR15			0x045e
+#define TXRX_CSR16			0x0460
+#define TXRX_CSR17			0x0462
+
+/*
+ * TXRX_CSR18: Synchronization control register.
+ */
+#define TXRX_CSR18			0x0464
+#define TXRX_CSR18_OFFSET		FIELD16(0x000f)
+#define TXRX_CSR18_INTERVAL		FIELD16(0xfff0)
+
+/*
+ * TXRX_CSR19: Synchronization control register.
+ * TSF_COUNT: Enable TSF auto counting.
+ * TSF_SYNC: Tsf sync, 0: disable, 1: infra, 2: ad-hoc/master mode.
+ * TBCN: Enable Tbcn with reload value.
+ * BEACON_GEN: Enable beacon generator.
+ */
+#define TXRX_CSR19			0x0466
+#define TXRX_CSR19_TSF_COUNT		FIELD16(0x0001)
+#define TXRX_CSR19_TSF_SYNC		FIELD16(0x0006)
+#define TXRX_CSR19_TBCN			FIELD16(0x0008)
+#define TXRX_CSR19_BEACON_GEN		FIELD16(0x0010)
+
+/*
+ * TXRX_CSR20: Tx BEACON offset time control register.
+ * OFFSET: In units of usec.
+ * BCN_EXPECT_WINDOW: Default: 2^CWmin
+ */
+#define TXRX_CSR20			0x0468
+#define TXRX_CSR20_OFFSET		FIELD16(0x1fff)
+#define TXRX_CSR20_BCN_EXPECT_WINDOW	FIELD16(0xe000)
+
+/*
+ * TXRX_CSR21
+ */
+#define TXRX_CSR21			0x046a
+
+/*
+ * Encryption related CSRs.
+ *
+ */
+
+/*
+ * SEC_CSR0-SEC_CSR7: Shared key 0, word 0-7
+ */
+#define SEC_CSR0			0x0480
+#define SEC_CSR1			0x0482
+#define SEC_CSR2			0x0484
+#define SEC_CSR3			0x0486
+#define SEC_CSR4			0x0488
+#define SEC_CSR5			0x048a
+#define SEC_CSR6			0x048c
+#define SEC_CSR7			0x048e
+
+/*
+ * SEC_CSR8-SEC_CSR15: Shared key 1, word 0-7
+ */
+#define SEC_CSR8			0x0490
+#define SEC_CSR9			0x0492
+#define SEC_CSR10			0x0494
+#define SEC_CSR11			0x0496
+#define SEC_CSR12			0x0498
+#define SEC_CSR13			0x049a
+#define SEC_CSR14			0x049c
+#define SEC_CSR15			0x049e
+
+/*
+ * SEC_CSR16-SEC_CSR23: Shared key 2, word 0-7
+ */
+#define SEC_CSR16			0x04a0
+#define SEC_CSR17			0x04a2
+#define SEC_CSR18			0X04A4
+#define SEC_CSR19			0x04a6
+#define SEC_CSR20			0x04a8
+#define SEC_CSR21			0x04aa
+#define SEC_CSR22			0x04ac
+#define SEC_CSR23			0x04ae
+
+/*
+ * SEC_CSR24-SEC_CSR31: Shared key 3, word 0-7
+ */
+#define SEC_CSR24			0x04b0
+#define SEC_CSR25			0x04b2
+#define SEC_CSR26			0x04b4
+#define SEC_CSR27			0x04b6
+#define SEC_CSR28			0x04b8
+#define SEC_CSR29			0x04ba
+#define SEC_CSR30			0x04bc
+#define SEC_CSR31			0x04be
+
+/*
+ * PHY control registers.
+ */
+
+/*
+ * PHY_CSR0: RF switching timing control.
+ */
+#define PHY_CSR0			0x04c0
+
+/*
+ * PHY_CSR1: TX PA configuration.
+ */
+#define PHY_CSR1			0x04c2
+
+/*
+ * MAC configuration registers.
+ * PHY_CSR2: TX MAC configuration.
+ * PHY_CSR3: RX MAC configuration.
+ */
+#define PHY_CSR2			0x04c4
+#define PHY_CSR3			0x04c6
+
+/*
+ * PHY_CSR4: Interface configuration.
+ */
+#define PHY_CSR4			0x04c8
+#define PHY_CSR4_LOW_RF_LE		FIELD16(0x0001)
+
+/*
+ * BBP pre-TX registers.
+ * PHY_CSR5: BBP pre-TX CCK.
+ */
+#define PHY_CSR5			0x04ca
+#define PHY_CSR5_CCK			FIELD16(0x0003)
+#define PHY_CSR5_CCK_FLIP		FIELD16(0x0004)
+
+/*
+ * BBP pre-TX registers.
+ * PHY_CSR6: BBP pre-TX OFDM.
+ */
+#define PHY_CSR6			0x04cc
+#define PHY_CSR6_OFDM			FIELD16(0x0003)
+#define PHY_CSR6_OFDM_FLIP		FIELD16(0x0004)
+
+/*
+ * PHY_CSR7: BBP access register 0.
+ * BBP_DATA: BBP data.
+ * BBP_REG_ID: BBP register ID.
+ * BBP_READ_CONTROL: 0: write, 1: read.
+ */
+#define PHY_CSR7			0x04ce
+#define PHY_CSR7_DATA			FIELD16(0x00ff)
+#define PHY_CSR7_REG_ID			FIELD16(0x7f00)
+#define PHY_CSR7_READ_CONTROL		FIELD16(0x8000)
+
+/*
+ * PHY_CSR8: BBP access register 1.
+ * BBP_BUSY: ASIC is busy execute BBP programming.
+ */
+#define PHY_CSR8			0x04d0
+#define PHY_CSR8_BUSY			FIELD16(0x0001)
+
+/*
+ * PHY_CSR9: RF access register.
+ * RF_VALUE: Register value + id to program into rf/if.
+ */
+#define PHY_CSR9			0x04d2
+#define PHY_CSR9_RF_VALUE		FIELD16(0xffff)
+
+/*
+ * PHY_CSR10: RF access register.
+ * RF_VALUE: Register value + id to program into rf/if.
+ * RF_NUMBER_OF_BITS: Number of bits used in value (i:20, rfmd:22).
+ * RF_IF_SELECT: Chip to program: 0: rf, 1: if.
+ * RF_PLL_LD: Rf pll_ld status.
+ * RF_BUSY: 1: asic is busy execute rf programming.
+ */
+#define PHY_CSR10			0x04d4
+#define PHY_CSR10_RF_VALUE		FIELD16(0x00ff)
+#define PHY_CSR10_RF_NUMBER_OF_BITS	FIELD16(0x1f00)
+#define PHY_CSR10_RF_IF_SELECT		FIELD16(0x2000)
+#define PHY_CSR10_RF_PLL_LD		FIELD16(0x4000)
+#define PHY_CSR10_RF_BUSY		FIELD16(0x8000)
+
+/*
+ * STA_CSR0: FCS error count.
+ * FCS_ERROR: FCS error count, cleared when read.
+ */
+#define STA_CSR0			0x04e0
+#define STA_CSR0_FCS_ERROR		FIELD16(0xffff)
+
+/*
+ * STA_CSR1: PLCP error count.
+ */
+#define STA_CSR1			0x04e2
+
+/*
+ * STA_CSR2: LONG error count.
+ */
+#define STA_CSR2			0x04e4
+
+/*
+ * STA_CSR3: CCA false alarm.
+ * FALSE_CCA_ERROR: False CCA error count, cleared when read.
+ */
+#define STA_CSR3			0x04e6
+#define STA_CSR3_FALSE_CCA_ERROR	FIELD16(0xffff)
+
+/*
+ * STA_CSR4: RX FIFO overflow.
+ */
+#define STA_CSR4			0x04e8
+
+/*
+ * STA_CSR5: Beacon sent counter.
+ */
+#define STA_CSR5			0x04ea
+
+/*
+ *  Statistics registers
+ */
+#define STA_CSR6			0x04ec
+#define STA_CSR7			0x04ee
+#define STA_CSR8			0x04f0
+#define STA_CSR9			0x04f2
+#define STA_CSR10			0x04f4
+
+/*
+ * BBP registers.
+ * The wordsize of the BBP is 8 bits.
+ */
+
+/*
+ * R2: TX antenna control
+ */
+#define BBP_R2_TX_ANTENNA		FIELD8(0x03)
+#define BBP_R2_TX_IQ_FLIP		FIELD8(0x04)
+
+/*
+ * R14: RX antenna control
+ */
+#define BBP_R14_RX_ANTENNA		FIELD8(0x03)
+#define BBP_R14_RX_IQ_FLIP		FIELD8(0x04)
+
+/*
+ * RF registers.
+ */
+
+/*
+ * RF 1
+ */
+#define RF1_TUNER			FIELD32(0x00020000)
+
+/*
+ * RF 3
+ */
+#define RF3_TUNER			FIELD32(0x00000100)
+#define RF3_TXPOWER			FIELD32(0x00003e00)
+
+/*
+ * EEPROM contents.
+ */
+
+/*
+ * HW MAC address.
+ */
+#define EEPROM_MAC_ADDR_0		0x0002
+#define EEPROM_MAC_ADDR_BYTE0		FIELD16(0x00ff)
+#define EEPROM_MAC_ADDR_BYTE1		FIELD16(0xff00)
+#define EEPROM_MAC_ADDR1		0x0003
+#define EEPROM_MAC_ADDR_BYTE2		FIELD16(0x00ff)
+#define EEPROM_MAC_ADDR_BYTE3		FIELD16(0xff00)
+#define EEPROM_MAC_ADDR_2		0x0004
+#define EEPROM_MAC_ADDR_BYTE4		FIELD16(0x00ff)
+#define EEPROM_MAC_ADDR_BYTE5		FIELD16(0xff00)
+
+/*
+ * EEPROM antenna.
+ * ANTENNA_NUM: Number of antenna's.
+ * TX_DEFAULT: Default antenna 0: diversity, 1: A, 2: B.
+ * RX_DEFAULT: Default antenna 0: diversity, 1: A, 2: B.
+ * LED_MODE: 0: default, 1: TX/RX activity, 2: Single (ignore link), 3: rsvd.
+ * DYN_TXAGC: Dynamic TX AGC control.
+ * HARDWARE_RADIO: 1: Hardware controlled radio. Read GPIO0.
+ * RF_TYPE: Rf_type of this adapter.
+ */
+#define EEPROM_ANTENNA			0x000b
+#define EEPROM_ANTENNA_NUM		FIELD16(0x0003)
+#define EEPROM_ANTENNA_TX_DEFAULT	FIELD16(0x000c)
+#define EEPROM_ANTENNA_RX_DEFAULT	FIELD16(0x0030)
+#define EEPROM_ANTENNA_LED_MODE		FIELD16(0x01c0)
+#define EEPROM_ANTENNA_DYN_TXAGC	FIELD16(0x0200)
+#define EEPROM_ANTENNA_HARDWARE_RADIO	FIELD16(0x0400)
+#define EEPROM_ANTENNA_RF_TYPE		FIELD16(0xf800)
+
+/*
+ * EEPROM NIC config.
+ * CARDBUS_ACCEL: 0: enable, 1: disable.
+ * DYN_BBP_TUNE: 0: enable, 1: disable.
+ * CCK_TX_POWER: CCK TX power compensation.
+ */
+#define EEPROM_NIC			0x000c
+#define EEPROM_NIC_CARDBUS_ACCEL	FIELD16(0x0001)
+#define EEPROM_NIC_DYN_BBP_TUNE		FIELD16(0x0002)
+#define EEPROM_NIC_CCK_TX_POWER		FIELD16(0x000c)
+
+/*
+ * EEPROM geography.
+ * GEO: Default geography setting for device.
+ */
+#define EEPROM_GEOGRAPHY		0x000d
+#define EEPROM_GEOGRAPHY_GEO		FIELD16(0x0f00)
+
+/*
+ * EEPROM BBP.
+ */
+#define EEPROM_BBP_START		0x000e
+#define EEPROM_BBP_SIZE			16
+#define EEPROM_BBP_VALUE		FIELD16(0x00ff)
+#define EEPROM_BBP_REG_ID		FIELD16(0xff00)
+
+/*
+ * EEPROM TXPOWER
+ */
+#define EEPROM_TXPOWER_START		0x001e
+#define EEPROM_TXPOWER_SIZE		7
+#define EEPROM_TXPOWER_1		FIELD16(0x00ff)
+#define EEPROM_TXPOWER_2		FIELD16(0xff00)
+
+/*
+ * EEPROM Tuning threshold
+ */
+#define EEPROM_BBPTUNE			0x0030
+#define EEPROM_BBPTUNE_THRESHOLD	FIELD16(0x00ff)
+
+/*
+ * EEPROM BBP R24 Tuning.
+ */
+#define EEPROM_BBPTUNE_R24		0x0031
+#define EEPROM_BBPTUNE_R24_LOW		FIELD16(0x00ff)
+#define EEPROM_BBPTUNE_R24_HIGH		FIELD16(0xff00)
+
+/*
+ * EEPROM BBP R25 Tuning.
+ */
+#define EEPROM_BBPTUNE_R25		0x0032
+#define EEPROM_BBPTUNE_R25_LOW		FIELD16(0x00ff)
+#define EEPROM_BBPTUNE_R25_HIGH		FIELD16(0xff00)
+
+/*
+ * EEPROM BBP R24 Tuning.
+ */
+#define EEPROM_BBPTUNE_R61		0x0033
+#define EEPROM_BBPTUNE_R61_LOW		FIELD16(0x00ff)
+#define EEPROM_BBPTUNE_R61_HIGH		FIELD16(0xff00)
+
+/*
+ * EEPROM BBP VGC Tuning.
+ */
+#define EEPROM_BBPTUNE_VGC		0x0034
+#define EEPROM_BBPTUNE_VGCUPPER		FIELD16(0x00ff)
+
+/*
+ * EEPROM BBP R17 Tuning.
+ */
+#define EEPROM_BBPTUNE_R17		0x0035
+#define EEPROM_BBPTUNE_R17_LOW		FIELD16(0x00ff)
+#define EEPROM_BBPTUNE_R17_HIGH		FIELD16(0xff00)
+
+/*
+ * RSSI <-> dBm offset calibration
+ */
+#define EEPROM_CALIBRATE_OFFSET		0x0036
+#define EEPROM_CALIBRATE_OFFSET_RSSI	FIELD16(0x00ff)
+
+/*
+ * DMA descriptor defines.
+ */
+#define TXD_DESC_SIZE			( 5 * sizeof(struct data_desc) )
+#define RXD_DESC_SIZE			( 4 * sizeof(struct data_desc) )
+
+/*
+ * TX descriptor format for TX, PRIO, ATIM and Beacon Ring.
+ */
+
+/*
+ * Word0
+ */
+#define TXD_W0_PACKET_ID		FIELD32(0x0000000f)
+#define TXD_W0_RETRY_LIMIT		FIELD32(0x000000f0)
+#define TXD_W0_MORE_FRAG		FIELD32(0x00000100)
+#define TXD_W0_ACK			FIELD32(0x00000200)
+#define TXD_W0_TIMESTAMP		FIELD32(0x00000400)
+#define TXD_W0_OFDM			FIELD32(0x00000800)
+#define TXD_W0_NEW_SEQ			FIELD32(0x00001000)
+#define TXD_W0_IFS			FIELD32(0x00006000)
+#define TXD_W0_DATABYTE_COUNT		FIELD32(0x0fff0000)
+#define TXD_W0_CIPHER			FIELD32(0x20000000)
+#define TXD_W0_KEY_ID			FIELD32(0xc0000000)
+
+/*
+ * Word1
+ */
+#define TXD_W1_IV_OFFSET		FIELD32(0x0000003f)
+#define TXD_W1_AIFS			FIELD32(0x000000c0)
+#define TXD_W1_CWMIN			FIELD32(0x00000f00)
+#define TXD_W1_CWMAX			FIELD32(0x0000f000)
+
+/*
+ * Word2: PLCP information
+ */
+#define TXD_W2_PLCP_SIGNAL		FIELD32(0x000000ff)
+#define TXD_W2_PLCP_SERVICE		FIELD32(0x0000ff00)
+#define TXD_W2_PLCP_LENGTH_LOW		FIELD32(0x00ff0000)
+#define TXD_W2_PLCP_LENGTH_HIGH		FIELD32(0xff000000)
+
+/*
+ * Word3
+ */
+#define TXD_W3_IV			FIELD32(0xffffffff)
+
+/*
+ * Word4
+ */
+#define TXD_W4_EIV			FIELD32(0xffffffff)
+
+/*
+ * RX descriptor format for RX Ring.
+ */
+
+/*
+ * Word0
+ */
+#define RXD_W0_UNICAST_TO_ME		FIELD32(0x00000002)
+#define RXD_W0_MULTICAST		FIELD32(0x00000004)
+#define RXD_W0_BROADCAST		FIELD32(0x00000008)
+#define RXD_W0_MY_BSS			FIELD32(0x00000010)
+#define RXD_W0_CRC_ERROR		FIELD32(0x00000020)
+#define RXD_W0_OFDM			FIELD32(0x00000040)
+#define RXD_W0_PHYSICAL_ERROR		FIELD32(0x00000080)
+#define RXD_W0_CIPHER			FIELD32(0x00000100)
+#define RXD_W0_CIPHER_ERROR		FIELD32(0x00000200)
+#define RXD_W0_DATABYTE_COUNT		FIELD32(0x0fff0000)
+
+/*
+ * Word1
+ */
+#define RXD_W1_RSSI			FIELD32(0x000000ff)
+#define RXD_W1_SIGNAL			FIELD32(0x0000ff00)
+
+/*
+ * Word2
+ */
+#define RXD_W2_IV			FIELD32(0xffffffff)
+
+/*
+ * Word3
+ */
+#define RXD_W3_EIV			FIELD32(0xffffffff)
+
+/*
+ * Macro's for converting txpower from EEPROM to dscape value
+ * and from dscape value to register value.
+ */
+#define MIN_TXPOWER	0
+#define MAX_TXPOWER	31
+#define DEFAULT_TXPOWER	24
+
+#define TXPOWER_FROM_DEV(__txpower)		\
+({						\
+	((__txpower) > MAX_TXPOWER) ?		\
+		DEFAULT_TXPOWER : (__txpower);	\
+})
+
+#define TXPOWER_TO_DEV(__txpower)			\
+({							\
+	((__txpower) <= MIN_TXPOWER) ? MIN_TXPOWER :	\
+	(((__txpower) >= MAX_TXPOWER) ? MAX_TXPOWER :	\
+	(__txpower));					\
+})
+
+#endif /* RT2500USB_H */
diff --git a/drivers/net/wireless/rt2x00/rt2x00.h b/drivers/net/wireless/rt2x00/rt2x00.h
new file mode 100644
index 000000000000..9845e584b731
--- /dev/null
+++ b/drivers/net/wireless/rt2x00/rt2x00.h
@@ -0,0 +1,838 @@
+/*
+	Copyright (C) 2004 - 2007 rt2x00 SourceForge Project
+	<http://rt2x00.serialmonkey.com>
+
+	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, or
+	(at your option) any later version.
+
+	This program is distributed in the hope that it will be useful,
+	but WITHOUT ANY WARRANTY; without even the implied warranty of
+	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+	GNU General Public License for more details.
+
+	You should have received a copy of the GNU General Public License
+	along with this program; if not, write to the
+	Free Software Foundation, Inc.,
+	59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+/*
+	Module: rt2x00
+	Abstract: rt2x00 global information.
+ */
+
+#ifndef RT2X00_H
+#define RT2X00_H
+
+#include <linux/bitops.h>
+#include <linux/prefetch.h>
+#include <linux/skbuff.h>
+#include <linux/workqueue.h>
+#include <linux/firmware.h>
+
+#include <net/mac80211.h>
+
+#include "rt2x00debug.h"
+#include "rt2x00reg.h"
+#include "rt2x00ring.h"
+
+/*
+ * Module information.
+ * DRV_NAME should be set within the individual module source files.
+ */
+#define DRV_VERSION	"2.0.10"
+#define DRV_PROJECT	"http://rt2x00.serialmonkey.com"
+
+/*
+ * Debug definitions.
+ * Debug output has to be enabled during compile time.
+ */
+#define DEBUG_PRINTK_MSG(__dev, __kernlvl, __lvl, __msg, __args...)	\
+	printk(__kernlvl "%s -> %s: %s - " __msg,			\
+	       wiphy_name((__dev)->hw->wiphy), __FUNCTION__, __lvl, ##__args)
+
+#define DEBUG_PRINTK_PROBE(__kernlvl, __lvl, __msg, __args...)	\
+	printk(__kernlvl "%s -> %s: %s - " __msg,		\
+	       DRV_NAME, __FUNCTION__, __lvl, ##__args)
+
+#ifdef CONFIG_RT2X00_DEBUG
+#define DEBUG_PRINTK(__dev, __kernlvl, __lvl, __msg, __args...)	\
+	DEBUG_PRINTK_MSG(__dev, __kernlvl, __lvl, __msg, ##__args);
+#else
+#define DEBUG_PRINTK(__dev, __kernlvl, __lvl, __msg, __args...)	\
+	do { } while (0)
+#endif /* CONFIG_RT2X00_DEBUG */
+
+/*
+ * Various debug levels.
+ * The debug levels PANIC and ERROR both indicate serious problems,
+ * for this reason they should never be ignored.
+ * The special ERROR_PROBE message is for messages that are generated
+ * when the rt2x00_dev is not yet initialized.
+ */
+#define PANIC(__dev, __msg, __args...) \
+	DEBUG_PRINTK_MSG(__dev, KERN_CRIT, "Panic", __msg, ##__args)
+#define ERROR(__dev, __msg, __args...)	\
+	DEBUG_PRINTK_MSG(__dev, KERN_ERR, "Error", __msg, ##__args)
+#define ERROR_PROBE(__msg, __args...) \
+	DEBUG_PRINTK_PROBE(KERN_ERR, "Error", __msg, ##__args)
+#define WARNING(__dev, __msg, __args...) \
+	DEBUG_PRINTK(__dev, KERN_WARNING, "Warning", __msg, ##__args)
+#define NOTICE(__dev, __msg, __args...) \
+	DEBUG_PRINTK(__dev, KERN_NOTICE, "Notice", __msg, ##__args)
+#define INFO(__dev, __msg, __args...) \
+	DEBUG_PRINTK(__dev, KERN_INFO, "Info", __msg, ##__args)
+#define DEBUG(__dev, __msg, __args...) \
+	DEBUG_PRINTK(__dev, KERN_DEBUG, "Debug", __msg, ##__args)
+#define EEPROM(__dev, __msg, __args...) \
+	DEBUG_PRINTK(__dev, KERN_DEBUG, "EEPROM recovery", __msg, ##__args)
+
+/*
+ * Ring sizes.
+ * Ralink PCI devices demand the Frame size to be a multiple of 128 bytes.
+ * DATA_FRAME_SIZE is used for TX, RX, ATIM and PRIO rings.
+ * MGMT_FRAME_SIZE is used for the BEACON ring.
+ */
+#define DATA_FRAME_SIZE	2432
+#define MGMT_FRAME_SIZE	256
+
+/*
+ * Number of entries in a packet ring.
+ * PCI devices only need 1 Beacon entry,
+ * but USB devices require a second because they
+ * have to send a Guardian byte first.
+ */
+#define RX_ENTRIES	12
+#define TX_ENTRIES	12
+#define ATIM_ENTRIES	1
+#define BEACON_ENTRIES	2
+
+/*
+ * Standard timing and size defines.
+ * These values should follow the ieee80211 specifications.
+ */
+#define ACK_SIZE		14
+#define IEEE80211_HEADER	24
+#define PLCP			48
+#define BEACON			100
+#define PREAMBLE		144
+#define SHORT_PREAMBLE		72
+#define SLOT_TIME		20
+#define SHORT_SLOT_TIME		9
+#define SIFS			10
+#define PIFS			( SIFS + SLOT_TIME )
+#define SHORT_PIFS		( SIFS + SHORT_SLOT_TIME )
+#define DIFS			( PIFS + SLOT_TIME )
+#define SHORT_DIFS		( SHORT_PIFS + SHORT_SLOT_TIME )
+#define EIFS			( SIFS + (8 * (IEEE80211_HEADER + ACK_SIZE)) )
+
+/*
+ * IEEE802.11 header defines
+ */
+static inline int is_rts_frame(u16 fc)
+{
+	return !!(((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_CTL) &&
+		  ((fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_RTS));
+}
+
+static inline int is_cts_frame(u16 fc)
+{
+	return !!(((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_CTL) &&
+		  ((fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_CTS));
+}
+
+static inline int is_probe_resp(u16 fc)
+{
+	return !!(((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT) &&
+		  ((fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_PROBE_RESP));
+}
+
+/*
+ * Chipset identification
+ * The chipset on the device is composed of a RT and RF chip.
+ * The chipset combination is important for determining device capabilities.
+ */
+struct rt2x00_chip {
+	u16 rt;
+#define RT2460		0x0101
+#define RT2560		0x0201
+#define RT2570		0x1201
+#define RT2561s		0x0301	/* Turbo */
+#define RT2561		0x0302
+#define RT2661		0x0401
+#define RT2571		0x1300
+
+	u16 rf;
+	u32 rev;
+};
+
+/*
+ * RF register values that belong to a particular channel.
+ */
+struct rf_channel {
+	int channel;
+	u32 rf1;
+	u32 rf2;
+	u32 rf3;
+	u32 rf4;
+};
+
+/*
+ * To optimize the quality of the link we need to store
+ * the quality of received frames and periodically
+ * optimize the link.
+ */
+struct link {
+	/*
+	 * Link tuner counter
+	 * The number of times the link has been tuned
+	 * since the radio has been switched on.
+	 */
+	u32 count;
+
+	/*
+	 * Statistics required for Link tuning.
+	 * For the average RSSI value we use the "Walking average" approach.
+	 * When adding RSSI to the average value the following calculation
+	 * is needed:
+	 *
+	 *        avg_rssi = ((avg_rssi * 7) + rssi) / 8;
+	 *
+	 * The advantage of this approach is that we only need 1 variable
+	 * to store the average in (No need for a count and a total).
+	 * But more importantly, normal average values will over time
+	 * move less and less towards newly added values this results
+	 * that with link tuning, the device can have a very good RSSI
+	 * for a few minutes but when the device is moved away from the AP
+	 * the average will not decrease fast enough to compensate.
+	 * The walking average compensates this and will move towards
+	 * the new values correctly allowing a effective link tuning.
+	 */
+	int avg_rssi;
+	int vgc_level;
+	int false_cca;
+
+	/*
+	 * Statistics required for Signal quality calculation.
+	 * For calculating the Signal quality we have to determine
+	 * the total number of success and failed RX and TX frames.
+	 * After that we also use the average RSSI value to help
+	 * determining the signal quality.
+	 * For the calculation we will use the following algorithm:
+	 *
+	 *         rssi_percentage = (avg_rssi * 100) / rssi_offset
+	 *         rx_percentage = (rx_success * 100) / rx_total
+	 *         tx_percentage = (tx_success * 100) / tx_total
+	 *         avg_signal = ((WEIGHT_RSSI * avg_rssi) +
+	 *                       (WEIGHT_TX * tx_percentage) +
+	 *                       (WEIGHT_RX * rx_percentage)) / 100
+	 *
+	 * This value should then be checked to not be greated then 100.
+	 */
+	int rx_percentage;
+	int rx_success;
+	int rx_failed;
+	int tx_percentage;
+	int tx_success;
+	int tx_failed;
+#define WEIGHT_RSSI	20
+#define WEIGHT_RX	40
+#define WEIGHT_TX	40
+
+	/*
+	 * Work structure for scheduling periodic link tuning.
+	 */
+	struct delayed_work work;
+};
+
+/*
+ * Clear all counters inside the link structure.
+ * This can be easiest achieved by memsetting everything
+ * except for the work structure at the end.
+ */
+static inline void rt2x00_clear_link(struct link *link)
+{
+	memset(link, 0x00, sizeof(*link) - sizeof(link->work));
+	link->rx_percentage = 50;
+	link->tx_percentage = 50;
+}
+
+/*
+ * Update the rssi using the walking average approach.
+ */
+static inline void rt2x00_update_link_rssi(struct link *link, int rssi)
+{
+	if (!link->avg_rssi)
+		link->avg_rssi = rssi;
+	else
+		link->avg_rssi = ((link->avg_rssi * 7) + rssi) / 8;
+}
+
+/*
+ * When the avg_rssi is unset or no frames  have been received),
+ * we need to return the default value which needs to be less
+ * than -80 so the device will select the maximum sensitivity.
+ */
+static inline int rt2x00_get_link_rssi(struct link *link)
+{
+	return (link->avg_rssi && link->rx_success) ? link->avg_rssi : -128;
+}
+
+/*
+ * Interface structure
+ * Configuration details about the current interface.
+ */
+struct interface {
+	/*
+	 * Interface identification. The value is assigned
+	 * to us by the 80211 stack, and is used to request
+	 * new beacons.
+	 */
+	int id;
+
+	/*
+	 * Current working type (IEEE80211_IF_TYPE_*).
+	 * When set to INVALID_INTERFACE, no interface is configured.
+	 */
+	int type;
+#define INVALID_INTERFACE	IEEE80211_IF_TYPE_INVALID
+
+	/*
+	 * MAC of the device.
+	 */
+	u8 mac[ETH_ALEN];
+
+	/*
+	 * BBSID of the AP to associate with.
+	 */
+	u8 bssid[ETH_ALEN];
+
+	/*
+	 * Store the packet filter mode for the current interface.
+	 */
+	unsigned int filter;
+};
+
+static inline int is_interface_present(struct interface *intf)
+{
+	return !!intf->id;
+}
+
+static inline int is_interface_type(struct interface *intf, int type)
+{
+	return intf->type == type;
+}
+
+/*
+ * Details about the supported modes, rates and channels
+ * of a particular chipset. This is used by rt2x00lib
+ * to build the ieee80211_hw_mode array for mac80211.
+ */
+struct hw_mode_spec {
+	/*
+	 * Number of modes, rates and channels.
+	 */
+	int num_modes;
+	int num_rates;
+	int num_channels;
+
+	/*
+	 * txpower values.
+	 */
+	const u8 *tx_power_a;
+	const u8 *tx_power_bg;
+	u8 tx_power_default;
+
+	/*
+	 * Device/chipset specific value.
+	 */
+	const struct rf_channel *channels;
+};
+
+/*
+ * Configuration structure wrapper around the
+ * mac80211 configuration structure.
+ * When mac80211 configures the driver, rt2x00lib
+ * can precalculate values which are equal for all
+ * rt2x00 drivers. Those values can be stored in here.
+ */
+struct rt2x00lib_conf {
+	struct ieee80211_conf *conf;
+	struct rf_channel rf;
+
+	int phymode;
+
+	int basic_rates;
+	int slot_time;
+
+	short sifs;
+	short pifs;
+	short difs;
+	short eifs;
+};
+
+/*
+ * rt2x00lib callback functions.
+ */
+struct rt2x00lib_ops {
+	/*
+	 * Interrupt handlers.
+	 */
+	irq_handler_t irq_handler;
+
+	/*
+	 * Device init handlers.
+	 */
+	int (*probe_hw) (struct rt2x00_dev *rt2x00dev);
+	char *(*get_firmware_name) (struct rt2x00_dev *rt2x00dev);
+	int (*load_firmware) (struct rt2x00_dev *rt2x00dev, void *data,
+			      const size_t len);
+
+	/*
+	 * Device initialization/deinitialization handlers.
+	 */
+	int (*initialize) (struct rt2x00_dev *rt2x00dev);
+	void (*uninitialize) (struct rt2x00_dev *rt2x00dev);
+
+	/*
+	 * Radio control handlers.
+	 */
+	int (*set_device_state) (struct rt2x00_dev *rt2x00dev,
+				 enum dev_state state);
+	int (*rfkill_poll) (struct rt2x00_dev *rt2x00dev);
+	void (*link_stats) (struct rt2x00_dev *rt2x00dev);
+	void (*reset_tuner) (struct rt2x00_dev *rt2x00dev);
+	void (*link_tuner) (struct rt2x00_dev *rt2x00dev);
+
+	/*
+	 * TX control handlers
+	 */
+	void (*write_tx_desc) (struct rt2x00_dev *rt2x00dev,
+			       struct data_desc *txd,
+			       struct txdata_entry_desc *desc,
+			       struct ieee80211_hdr *ieee80211hdr,
+			       unsigned int length,
+			       struct ieee80211_tx_control *control);
+	int (*write_tx_data) (struct rt2x00_dev *rt2x00dev,
+			      struct data_ring *ring, struct sk_buff *skb,
+			      struct ieee80211_tx_control *control);
+	int (*get_tx_data_len) (struct rt2x00_dev *rt2x00dev, int maxpacket,
+				struct sk_buff *skb);
+	void (*kick_tx_queue) (struct rt2x00_dev *rt2x00dev,
+			       unsigned int queue);
+
+	/*
+	 * RX control handlers
+	 */
+	void (*fill_rxdone) (struct data_entry *entry,
+			     struct rxdata_entry_desc *desc);
+
+	/*
+	 * Configuration handlers.
+	 */
+	void (*config_mac_addr) (struct rt2x00_dev *rt2x00dev, __le32 *mac);
+	void (*config_bssid) (struct rt2x00_dev *rt2x00dev, __le32 *bssid);
+	void (*config_type) (struct rt2x00_dev *rt2x00dev, const int type,
+							   const int tsf_sync);
+	void (*config_preamble) (struct rt2x00_dev *rt2x00dev,
+				 const int short_preamble,
+				 const int ack_timeout,
+				 const int ack_consume_time);
+	void (*config) (struct rt2x00_dev *rt2x00dev, const unsigned int flags,
+			struct rt2x00lib_conf *libconf);
+#define CONFIG_UPDATE_PHYMODE		( 1 << 1 )
+#define CONFIG_UPDATE_CHANNEL		( 1 << 2 )
+#define CONFIG_UPDATE_TXPOWER		( 1 << 3 )
+#define CONFIG_UPDATE_ANTENNA		( 1 << 4 )
+#define CONFIG_UPDATE_SLOT_TIME 	( 1 << 5 )
+#define CONFIG_UPDATE_BEACON_INT	( 1 << 6 )
+#define CONFIG_UPDATE_ALL		0xffff
+};
+
+/*
+ * rt2x00 driver callback operation structure.
+ */
+struct rt2x00_ops {
+	const char *name;
+	const unsigned int rxd_size;
+	const unsigned int txd_size;
+	const unsigned int eeprom_size;
+	const unsigned int rf_size;
+	const struct rt2x00lib_ops *lib;
+	const struct ieee80211_ops *hw;
+#ifdef CONFIG_RT2X00_LIB_DEBUGFS
+	const struct rt2x00debug *debugfs;
+#endif /* CONFIG_RT2X00_LIB_DEBUGFS */
+};
+
+/*
+ * rt2x00 device flags
+ */
+enum rt2x00_flags {
+	/*
+	 * Device state flags
+	 */
+	DEVICE_PRESENT,
+	DEVICE_REGISTERED_HW,
+	DEVICE_INITIALIZED,
+	DEVICE_STARTED,
+	DEVICE_STARTED_SUSPEND,
+	DEVICE_ENABLED_RADIO,
+	DEVICE_DISABLED_RADIO_HW,
+
+	/*
+	 * Driver features
+	 */
+	DRIVER_REQUIRE_FIRMWARE,
+	DRIVER_REQUIRE_BEACON_RING,
+
+	/*
+	 * Driver configuration
+	 */
+	CONFIG_SUPPORT_HW_BUTTON,
+	CONFIG_FRAME_TYPE,
+	CONFIG_RF_SEQUENCE,
+	CONFIG_EXTERNAL_LNA_A,
+	CONFIG_EXTERNAL_LNA_BG,
+	CONFIG_DOUBLE_ANTENNA,
+	CONFIG_DISABLE_LINK_TUNING,
+	CONFIG_SHORT_PREAMBLE,
+};
+
+/*
+ * rt2x00 device structure.
+ */
+struct rt2x00_dev {
+	/*
+	 * Device structure.
+	 * The structure stored in here depends on the
+	 * system bus (PCI or USB).
+	 * When accessing this variable, the rt2x00dev_{pci,usb}
+	 * macro's should be used for correct typecasting.
+	 */
+	void *dev;
+#define rt2x00dev_pci(__dev)	( (struct pci_dev*)(__dev)->dev )
+#define rt2x00dev_usb(__dev)	( (struct usb_interface*)(__dev)->dev )
+
+	/*
+	 * Callback functions.
+	 */
+	const struct rt2x00_ops *ops;
+
+	/*
+	 * IEEE80211 control structure.
+	 */
+	struct ieee80211_hw *hw;
+	struct ieee80211_hw_mode *hwmodes;
+	unsigned int curr_hwmode;
+#define HWMODE_B	0
+#define HWMODE_G	1
+#define HWMODE_A	2
+
+	/*
+	 * rfkill structure for RF state switching support.
+	 * This will only be compiled in when required.
+	 */
+#ifdef CONFIG_RT2X00_LIB_RFKILL
+	struct rfkill *rfkill;
+	struct input_polled_dev *poll_dev;
+#endif /* CONFIG_RT2X00_LIB_RFKILL */
+
+	/*
+	 * If enabled, the debugfs interface structures
+	 * required for deregistration of debugfs.
+	 */
+#ifdef CONFIG_RT2X00_LIB_DEBUGFS
+	const struct rt2x00debug_intf *debugfs_intf;
+#endif /* CONFIG_RT2X00_LIB_DEBUGFS */
+
+	/*
+	 * Device flags.
+	 * In these flags the current status and some
+	 * of the device capabilities are stored.
+	 */
+	unsigned long flags;
+
+	/*
+	 * Chipset identification.
+	 */
+	struct rt2x00_chip chip;
+
+	/*
+	 * hw capability specifications.
+	 */
+	struct hw_mode_spec spec;
+
+	/*
+	 * Register pointers
+	 * csr_addr: Base register address. (PCI)
+	 * csr_cache: CSR cache for usb_control_msg. (USB)
+	 */
+	void __iomem *csr_addr;
+	void *csr_cache;
+
+	/*
+	 * Interface configuration.
+	 */
+	struct interface interface;
+
+	/*
+	 * Link quality
+	 */
+	struct link link;
+
+	/*
+	 * EEPROM data.
+	 */
+	__le16 *eeprom;
+
+	/*
+	 * Active RF register values.
+	 * These are stored here so we don't need
+	 * to read the rf registers and can directly
+	 * use this value instead.
+	 * This field should be accessed by using
+	 * rt2x00_rf_read() and rt2x00_rf_write().
+	 */
+	u32 *rf;
+
+	/*
+	 * Current TX power value.
+	 */
+	u16 tx_power;
+
+	/*
+	 * LED register (for rt61pci & rt73usb).
+	 */
+	u16 led_reg;
+
+	/*
+	 * Led mode (LED_MODE_*)
+	 */
+	u8 led_mode;
+
+	/*
+	 * Rssi <-> Dbm offset
+	 */
+	u8 rssi_offset;
+
+	/*
+	 * Frequency offset (for rt61pci & rt73usb).
+	 */
+	u8 freq_offset;
+
+	/*
+	 * Low level statistics which will have
+	 * to be kept up to date while device is running.
+	 */
+	struct ieee80211_low_level_stats low_level_stats;
+
+	/*
+	 * RX configuration information.
+	 */
+	struct ieee80211_rx_status rx_status;
+
+	/*
+	 * Scheduled work.
+	 */
+	struct work_struct beacon_work;
+	struct work_struct filter_work;
+	struct work_struct config_work;
+
+	/*
+	 * Data ring arrays for RX, TX and Beacon.
+	 * The Beacon array also contains the Atim ring
+	 * if that is supported by the device.
+	 */
+	int data_rings;
+	struct data_ring *rx;
+	struct data_ring *tx;
+	struct data_ring *bcn;
+
+	/*
+	 * Firmware image.
+	 */
+	const struct firmware *fw;
+};
+
+/*
+ * For-each loop for the ring array.
+ * All rings have been allocated as a single array,
+ * this means we can create a very simply loop macro
+ * that is capable of looping through all rings.
+ * ring_end(), txring_end() and ring_loop() are helper macro's which
+ * should not be used directly. Instead the following should be used:
+ * ring_for_each() - Loops through all rings (RX, TX, Beacon & Atim)
+ * txring_for_each() - Loops through TX data rings (TX only)
+ * txringall_for_each() - Loops through all TX rings (TX, Beacon & Atim)
+ */
+#define ring_end(__dev) \
+	&(__dev)->rx[(__dev)->data_rings]
+
+#define txring_end(__dev) \
+	&(__dev)->tx[(__dev)->hw->queues]
+
+#define ring_loop(__entry, __start, __end)			\
+	for ((__entry) = (__start);				\
+	     prefetch(&(__entry)[1]), (__entry) != (__end);	\
+	     (__entry) = &(__entry)[1])
+
+#define ring_for_each(__dev, __entry) \
+	ring_loop(__entry, (__dev)->rx, ring_end(__dev))
+
+#define txring_for_each(__dev, __entry) \
+	ring_loop(__entry, (__dev)->tx, txring_end(__dev))
+
+#define txringall_for_each(__dev, __entry) \
+	ring_loop(__entry, (__dev)->tx, ring_end(__dev))
+
+/*
+ * Generic RF access.
+ * The RF is being accessed by word index.
+ */
+static inline void rt2x00_rf_read(const struct rt2x00_dev *rt2x00dev,
+				  const unsigned int word, u32 *data)
+{
+	*data = rt2x00dev->rf[word];
+}
+
+static inline void rt2x00_rf_write(const struct rt2x00_dev *rt2x00dev,
+				   const unsigned int word, u32 data)
+{
+	rt2x00dev->rf[word] = data;
+}
+
+/*
+ *  Generic EEPROM access.
+ * The EEPROM is being accessed by word index.
+ */
+static inline void *rt2x00_eeprom_addr(const struct rt2x00_dev *rt2x00dev,
+				       const unsigned int word)
+{
+	return (void *)&rt2x00dev->eeprom[word];
+}
+
+static inline void rt2x00_eeprom_read(const struct rt2x00_dev *rt2x00dev,
+				      const unsigned int word, u16 *data)
+{
+	*data = le16_to_cpu(rt2x00dev->eeprom[word]);
+}
+
+static inline void rt2x00_eeprom_write(const struct rt2x00_dev *rt2x00dev,
+				       const unsigned int word, u16 data)
+{
+	rt2x00dev->eeprom[word] = cpu_to_le16(data);
+}
+
+/*
+ * Chipset handlers
+ */
+static inline void rt2x00_set_chip(struct rt2x00_dev *rt2x00dev,
+				   const u16 rt, const u16 rf, const u32 rev)
+{
+	INFO(rt2x00dev,
+	     "Chipset detected - rt: %04x, rf: %04x, rev: %08x.\n",
+	     rt, rf, rev);
+
+	rt2x00dev->chip.rt = rt;
+	rt2x00dev->chip.rf = rf;
+	rt2x00dev->chip.rev = rev;
+}
+
+static inline char rt2x00_rt(const struct rt2x00_chip *chipset, const u16 chip)
+{
+	return (chipset->rt == chip);
+}
+
+static inline char rt2x00_rf(const struct rt2x00_chip *chipset, const u16 chip)
+{
+	return (chipset->rf == chip);
+}
+
+static inline u16 rt2x00_get_rev(const struct rt2x00_chip *chipset)
+{
+	return chipset->rev;
+}
+
+static inline u16 rt2x00_rev(const struct rt2x00_chip *chipset, const u32 mask)
+{
+	return chipset->rev & mask;
+}
+
+/*
+ * Duration calculations
+ * The rate variable passed is: 100kbs.
+ * To convert from bytes to bits we multiply size with 8,
+ * then the size is multiplied with 10 to make the
+ * real rate -> rate argument correction.
+ */
+static inline u16 get_duration(const unsigned int size, const u8 rate)
+{
+	return ((size * 8 * 10) / rate);
+}
+
+static inline u16 get_duration_res(const unsigned int size, const u8 rate)
+{
+	return ((size * 8 * 10) % rate);
+}
+
+/*
+ * Library functions.
+ */
+struct data_ring *rt2x00lib_get_ring(struct rt2x00_dev *rt2x00dev,
+				     const unsigned int queue);
+
+/*
+ * Interrupt context handlers.
+ */
+void rt2x00lib_beacondone(struct rt2x00_dev *rt2x00dev);
+void rt2x00lib_txdone(struct data_entry *entry,
+		      const int status, const int retry);
+void rt2x00lib_rxdone(struct data_entry *entry, struct sk_buff *skb,
+		      struct rxdata_entry_desc *desc);
+
+/*
+ * TX descriptor initializer
+ */
+void rt2x00lib_write_tx_desc(struct rt2x00_dev *rt2x00dev,
+			     struct data_desc *txd,
+			     struct ieee80211_hdr *ieee80211hdr,
+			     unsigned int length,
+			     struct ieee80211_tx_control *control);
+
+/*
+ * mac80211 handlers.
+ */
+int rt2x00mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb,
+		 struct ieee80211_tx_control *control);
+int rt2x00mac_start(struct ieee80211_hw *hw);
+void rt2x00mac_stop(struct ieee80211_hw *hw);
+int rt2x00mac_add_interface(struct ieee80211_hw *hw,
+			    struct ieee80211_if_init_conf *conf);
+void rt2x00mac_remove_interface(struct ieee80211_hw *hw,
+				struct ieee80211_if_init_conf *conf);
+int rt2x00mac_config(struct ieee80211_hw *hw, struct ieee80211_conf *conf);
+int rt2x00mac_config_interface(struct ieee80211_hw *hw, int if_id,
+			       struct ieee80211_if_conf *conf);
+int rt2x00mac_get_stats(struct ieee80211_hw *hw,
+			struct ieee80211_low_level_stats *stats);
+int rt2x00mac_get_tx_stats(struct ieee80211_hw *hw,
+			   struct ieee80211_tx_queue_stats *stats);
+void rt2x00mac_erp_ie_changed(struct ieee80211_hw *hw, u8 changes,
+			      int cts_protection, int preamble);
+int rt2x00mac_conf_tx(struct ieee80211_hw *hw, int queue,
+		      const struct ieee80211_tx_queue_params *params);
+
+/*
+ * Driver allocation handlers.
+ */
+int rt2x00lib_probe_dev(struct rt2x00_dev *rt2x00dev);
+void rt2x00lib_remove_dev(struct rt2x00_dev *rt2x00dev);
+#ifdef CONFIG_PM
+int rt2x00lib_suspend(struct rt2x00_dev *rt2x00dev, pm_message_t state);
+int rt2x00lib_resume(struct rt2x00_dev *rt2x00dev);
+#endif /* CONFIG_PM */
+
+#endif /* RT2X00_H */
diff --git a/drivers/net/wireless/rt2x00/rt2x00config.c b/drivers/net/wireless/rt2x00/rt2x00config.c
new file mode 100644
index 000000000000..12914cf7156c
--- /dev/null
+++ b/drivers/net/wireless/rt2x00/rt2x00config.c
@@ -0,0 +1,205 @@
+/*
+	Copyright (C) 2004 - 2007 rt2x00 SourceForge Project
+	<http://rt2x00.serialmonkey.com>
+
+	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, or
+	(at your option) any later version.
+
+	This program is distributed in the hope that it will be useful,
+	but WITHOUT ANY WARRANTY; without even the implied warranty of
+	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+	GNU General Public License for more details.
+
+	You should have received a copy of the GNU General Public License
+	along with this program; if not, write to the
+	Free Software Foundation, Inc.,
+	59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+/*
+	Module: rt2x00lib
+	Abstract: rt2x00 generic configuration routines.
+ */
+
+/*
+ * Set enviroment defines for rt2x00.h
+ */
+#define DRV_NAME "rt2x00lib"
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+
+#include "rt2x00.h"
+#include "rt2x00lib.h"
+
+
+/*
+ * The MAC and BSSID addressess are simple array of bytes,
+ * these arrays are little endian, so when sending the addressess
+ * to the drivers, copy the it into a endian-signed variable.
+ *
+ * Note that all devices (except rt2500usb) have 32 bits
+ * register word sizes. This means that whatever variable we
+ * pass _must_ be a multiple of 32 bits. Otherwise the device
+ * might not accept what we are sending to it.
+ * This will also make it easier for the driver to write
+ * the data to the device.
+ *
+ * Also note that when NULL is passed as address the
+ * we will send 00:00:00:00:00 to the device to clear the address.
+ * This will prevent the device being confused when it wants
+ * to ACK frames or consideres itself associated.
+ */
+void rt2x00lib_config_mac_addr(struct rt2x00_dev *rt2x00dev, u8 *mac)
+{
+	__le32 reg[2];
+
+	memset(&reg, 0, sizeof(reg));
+	if (mac)
+		memcpy(&reg, mac, ETH_ALEN);
+
+	rt2x00dev->ops->lib->config_mac_addr(rt2x00dev, &reg[0]);
+}
+
+void rt2x00lib_config_bssid(struct rt2x00_dev *rt2x00dev, u8 *bssid)
+{
+	__le32 reg[2];
+
+	memset(&reg, 0, sizeof(reg));
+	if (bssid)
+		memcpy(&reg, bssid, ETH_ALEN);
+
+	rt2x00dev->ops->lib->config_bssid(rt2x00dev, &reg[0]);
+}
+
+void rt2x00lib_config_type(struct rt2x00_dev *rt2x00dev, const int type)
+{
+	int tsf_sync;
+
+	switch (type) {
+	case IEEE80211_IF_TYPE_IBSS:
+	case IEEE80211_IF_TYPE_AP:
+		tsf_sync = TSF_SYNC_BEACON;
+		break;
+	case IEEE80211_IF_TYPE_STA:
+		tsf_sync = TSF_SYNC_INFRA;
+		break;
+	default:
+		tsf_sync = TSF_SYNC_NONE;
+		break;
+	}
+
+	rt2x00dev->ops->lib->config_type(rt2x00dev, type, tsf_sync);
+}
+
+void rt2x00lib_config(struct rt2x00_dev *rt2x00dev,
+		      struct ieee80211_conf *conf, const int force_config)
+{
+	struct rt2x00lib_conf libconf;
+	struct ieee80211_hw_mode *mode;
+	struct ieee80211_rate *rate;
+	int flags = 0;
+	int short_slot_time;
+
+	/*
+	 * In some situations we want to force all configurations
+	 * to be reloaded (When resuming for instance).
+	 */
+	if (force_config) {
+		flags = CONFIG_UPDATE_ALL;
+		goto config;
+	}
+
+	/*
+	 * Check which configuration options have been
+	 * updated and should be send to the device.
+	 */
+	if (rt2x00dev->rx_status.phymode != conf->phymode)
+		flags |= CONFIG_UPDATE_PHYMODE;
+	if (rt2x00dev->rx_status.channel != conf->channel)
+		flags |= CONFIG_UPDATE_CHANNEL;
+	if (rt2x00dev->tx_power != conf->power_level)
+		flags |= CONFIG_UPDATE_TXPOWER;
+	if (rt2x00dev->rx_status.antenna == conf->antenna_sel_rx)
+		flags |= CONFIG_UPDATE_ANTENNA;
+
+	/*
+	 * The following configuration options are never
+	 * stored anywhere and will always be updated.
+	 */
+	flags |= CONFIG_UPDATE_SLOT_TIME;
+	flags |= CONFIG_UPDATE_BEACON_INT;
+
+	/*
+	 * We have determined what options should be updated,
+	 * now precalculate device configuration values depending
+	 * on what configuration options need to be updated.
+	 */
+config:
+	memset(&libconf, 0, sizeof(libconf));
+
+	if (flags & CONFIG_UPDATE_PHYMODE) {
+		switch (conf->phymode) {
+		case MODE_IEEE80211A:
+			libconf.phymode = HWMODE_A;
+			break;
+		case MODE_IEEE80211B:
+			libconf.phymode = HWMODE_B;
+			break;
+		case MODE_IEEE80211G:
+			libconf.phymode = HWMODE_G;
+			break;
+		default:
+			ERROR(rt2x00dev,
+			      "Attempt to configure unsupported mode (%d)"
+			      "Defaulting to 802.11b", conf->phymode);
+			libconf.phymode = HWMODE_B;
+		}
+
+		mode = &rt2x00dev->hwmodes[libconf.phymode];
+		rate = &mode->rates[mode->num_rates - 1];
+
+		libconf.basic_rates =
+		    DEVICE_GET_RATE_FIELD(rate->val, RATEMASK) & DEV_BASIC_RATEMASK;
+	}
+
+	if (flags & CONFIG_UPDATE_CHANNEL) {
+		memcpy(&libconf.rf,
+		       &rt2x00dev->spec.channels[conf->channel_val],
+		       sizeof(libconf.rf));
+	}
+
+	if (flags & CONFIG_UPDATE_SLOT_TIME) {
+		short_slot_time = conf->flags & IEEE80211_CONF_SHORT_SLOT_TIME;
+
+		libconf.slot_time =
+		    short_slot_time ? SHORT_SLOT_TIME : SLOT_TIME;
+		libconf.sifs = SIFS;
+		libconf.pifs = short_slot_time ? SHORT_PIFS : PIFS;
+		libconf.difs = short_slot_time ? SHORT_DIFS : DIFS;
+		libconf.eifs = EIFS;
+	}
+
+	libconf.conf = conf;
+
+	/*
+	 * Start configuration.
+	 */
+	rt2x00dev->ops->lib->config(rt2x00dev, flags, &libconf);
+
+	/*
+	 * Some configuration changes affect the link quality
+	 * which means we need to reset the link tuner.
+	 */
+	if (flags & (CONFIG_UPDATE_CHANNEL | CONFIG_UPDATE_ANTENNA))
+		rt2x00lib_reset_link_tuner(rt2x00dev);
+
+	rt2x00dev->curr_hwmode = libconf.phymode;
+	rt2x00dev->rx_status.phymode = conf->phymode;
+	rt2x00dev->rx_status.freq = conf->freq;
+	rt2x00dev->rx_status.channel = conf->channel;
+	rt2x00dev->tx_power = conf->power_level;
+	rt2x00dev->rx_status.antenna = conf->antenna_sel_rx;
+}
diff --git a/drivers/net/wireless/rt2x00/rt2x00debug.c b/drivers/net/wireless/rt2x00/rt2x00debug.c
new file mode 100644
index 000000000000..9275d6f9517e
--- /dev/null
+++ b/drivers/net/wireless/rt2x00/rt2x00debug.c
@@ -0,0 +1,368 @@
+/*
+	Copyright (C) 2004 - 2007 rt2x00 SourceForge Project
+	<http://rt2x00.serialmonkey.com>
+
+	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, or
+	(at your option) any later version.
+
+	This program is distributed in the hope that it will be useful,
+	but WITHOUT ANY WARRANTY; without even the implied warranty of
+	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+	GNU General Public License for more details.
+
+	You should have received a copy of the GNU General Public License
+	along with this program; if not, write to the
+	Free Software Foundation, Inc.,
+	59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+/*
+	Module: rt2x00lib
+	Abstract: rt2x00 debugfs specific routines.
+ */
+
+/*
+ * Set enviroment defines for rt2x00.h
+ */
+#define DRV_NAME "rt2x00lib"
+
+#include <linux/debugfs.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/uaccess.h>
+
+#include "rt2x00.h"
+#include "rt2x00lib.h"
+
+#define PRINT_LINE_LEN_MAX 32
+
+struct rt2x00debug_intf {
+	/*
+	 * Pointer to driver structure where
+	 * this debugfs entry belongs to.
+	 */
+	struct rt2x00_dev *rt2x00dev;
+
+	/*
+	 * Reference to the rt2x00debug structure
+	 * which can be used to communicate with
+	 * the registers.
+	 */
+	const struct rt2x00debug *debug;
+
+	/*
+	 * Debugfs entries for:
+	 * - driver folder
+	 * - driver file
+	 * - chipset file
+	 * - device flags file
+	 * - register offset/value files
+	 * - eeprom offset/value files
+	 * - bbp offset/value files
+	 * - rf offset/value files
+	 */
+	struct dentry *driver_folder;
+	struct dentry *driver_entry;
+	struct dentry *chipset_entry;
+	struct dentry *dev_flags;
+	struct dentry *csr_off_entry;
+	struct dentry *csr_val_entry;
+	struct dentry *eeprom_off_entry;
+	struct dentry *eeprom_val_entry;
+	struct dentry *bbp_off_entry;
+	struct dentry *bbp_val_entry;
+	struct dentry *rf_off_entry;
+	struct dentry *rf_val_entry;
+
+	/*
+	 * Driver and chipset files will use a data buffer
+	 * that has been created in advance. This will simplify
+	 * the code since we can use the debugfs functions.
+	 */
+	struct debugfs_blob_wrapper driver_blob;
+	struct debugfs_blob_wrapper chipset_blob;
+
+	/*
+	 * Requested offset for each register type.
+	 */
+	unsigned int offset_csr;
+	unsigned int offset_eeprom;
+	unsigned int offset_bbp;
+	unsigned int offset_rf;
+};
+
+static int rt2x00debug_file_open(struct inode *inode, struct file *file)
+{
+	struct rt2x00debug_intf *intf = inode->i_private;
+
+	file->private_data = inode->i_private;
+
+	if (!try_module_get(intf->debug->owner))
+		return -EBUSY;
+
+	return 0;
+}
+
+static int rt2x00debug_file_release(struct inode *inode, struct file *file)
+{
+	struct rt2x00debug_intf *intf = file->private_data;
+
+	module_put(intf->debug->owner);
+
+	return 0;
+}
+
+#define RT2X00DEBUGFS_OPS_READ(__name, __format, __type)	\
+static ssize_t rt2x00debug_read_##__name(struct file *file,	\
+					 char __user *buf,	\
+					 size_t length,		\
+					 loff_t *offset)	\
+{								\
+	struct rt2x00debug_intf *intf =	file->private_data;	\
+	const struct rt2x00debug *debug = intf->debug;		\
+	char line[16];						\
+	size_t size;						\
+	__type value;						\
+								\
+	if (*offset)						\
+		return 0;					\
+								\
+	if (intf->offset_##__name >= debug->__name.word_count)	\
+		return -EINVAL;					\
+								\
+	debug->__name.read(intf->rt2x00dev,			\
+			   intf->offset_##__name, &value);	\
+								\
+	size = sprintf(line, __format, value);			\
+								\
+	if (copy_to_user(buf, line, size))			\
+		return -EFAULT;					\
+								\
+	*offset += size;					\
+	return size;						\
+}
+
+#define RT2X00DEBUGFS_OPS_WRITE(__name, __type)			\
+static ssize_t rt2x00debug_write_##__name(struct file *file,	\
+					  const char __user *buf,\
+					  size_t length,	\
+					  loff_t *offset)	\
+{								\
+	struct rt2x00debug_intf *intf =	file->private_data;	\
+	const struct rt2x00debug *debug = intf->debug;		\
+	char line[16];						\
+	size_t size;						\
+	__type value;						\
+								\
+	if (*offset)						\
+		return 0;					\
+								\
+	if (!capable(CAP_NET_ADMIN))				\
+		return -EPERM;					\
+								\
+	if (intf->offset_##__name >= debug->__name.word_count)	\
+		return -EINVAL;					\
+								\
+	if (copy_from_user(line, buf, length))			\
+		return -EFAULT;					\
+								\
+	size = strlen(line);					\
+	value = simple_strtoul(line, NULL, 0);			\
+								\
+	debug->__name.write(intf->rt2x00dev,			\
+			    intf->offset_##__name, value);	\
+								\
+	*offset += size;					\
+	return size;						\
+}
+
+#define RT2X00DEBUGFS_OPS(__name, __format, __type)		\
+RT2X00DEBUGFS_OPS_READ(__name, __format, __type);		\
+RT2X00DEBUGFS_OPS_WRITE(__name, __type);			\
+								\
+static const struct file_operations rt2x00debug_fop_##__name = {\
+	.owner		= THIS_MODULE,				\
+	.read		= rt2x00debug_read_##__name,		\
+	.write		= rt2x00debug_write_##__name,		\
+	.open		= rt2x00debug_file_open,		\
+	.release	= rt2x00debug_file_release,		\
+};
+
+RT2X00DEBUGFS_OPS(csr, "0x%.8x\n", u32);
+RT2X00DEBUGFS_OPS(eeprom, "0x%.4x\n", u16);
+RT2X00DEBUGFS_OPS(bbp, "0x%.2x\n", u8);
+RT2X00DEBUGFS_OPS(rf, "0x%.8x\n", u32);
+
+static ssize_t rt2x00debug_read_dev_flags(struct file *file,
+					  char __user *buf,
+					  size_t length,
+					  loff_t *offset)
+{
+	struct rt2x00debug_intf *intf =	file->private_data;
+	char line[16];
+	size_t size;
+
+	if (*offset)
+		return 0;
+
+	size = sprintf(line, "0x%.8x\n", (unsigned int)intf->rt2x00dev->flags);
+
+	if (copy_to_user(buf, line, size))
+		return -EFAULT;
+
+	*offset += size;
+	return size;
+}
+
+static const struct file_operations rt2x00debug_fop_dev_flags = {
+	.owner		= THIS_MODULE,
+	.read		= rt2x00debug_read_dev_flags,
+	.open		= rt2x00debug_file_open,
+	.release	= rt2x00debug_file_release,
+};
+
+static struct dentry *rt2x00debug_create_file_driver(const char *name,
+						     struct rt2x00debug_intf
+						     *intf,
+						     struct debugfs_blob_wrapper
+						     *blob)
+{
+	char *data;
+
+	data = kzalloc(3 * PRINT_LINE_LEN_MAX, GFP_KERNEL);
+	if (!data)
+		return NULL;
+
+	blob->data = data;
+	data += sprintf(data, "driver: %s\n", intf->rt2x00dev->ops->name);
+	data += sprintf(data, "version: %s\n", DRV_VERSION);
+	data += sprintf(data, "compiled: %s %s\n", __DATE__, __TIME__);
+	blob->size = strlen(blob->data);
+
+	return debugfs_create_blob(name, S_IRUGO, intf->driver_folder, blob);
+}
+
+static struct dentry *rt2x00debug_create_file_chipset(const char *name,
+						      struct rt2x00debug_intf
+						      *intf,
+						      struct
+						      debugfs_blob_wrapper
+						      *blob)
+{
+	const struct rt2x00debug *debug = intf->debug;
+	char *data;
+
+	data = kzalloc(4 * PRINT_LINE_LEN_MAX, GFP_KERNEL);
+	if (!data)
+		return NULL;
+
+	blob->data = data;
+	data += sprintf(data, "csr length: %d\n", debug->csr.word_count);
+	data += sprintf(data, "eeprom length: %d\n", debug->eeprom.word_count);
+	data += sprintf(data, "bbp length: %d\n", debug->bbp.word_count);
+	data += sprintf(data, "rf length: %d\n", debug->rf.word_count);
+	blob->size = strlen(blob->data);
+
+	return debugfs_create_blob(name, S_IRUGO, intf->driver_folder, blob);
+}
+
+void rt2x00debug_register(struct rt2x00_dev *rt2x00dev)
+{
+	const struct rt2x00debug *debug = rt2x00dev->ops->debugfs;
+	struct rt2x00debug_intf *intf;
+
+	intf = kzalloc(sizeof(struct rt2x00debug_intf), GFP_KERNEL);
+	if (!intf) {
+		ERROR(rt2x00dev, "Failed to allocate debug handler.\n");
+		return;
+	}
+
+	intf->debug = debug;
+	intf->rt2x00dev = rt2x00dev;
+	rt2x00dev->debugfs_intf = intf;
+
+	intf->driver_folder =
+	    debugfs_create_dir(intf->rt2x00dev->ops->name,
+			       rt2x00dev->hw->wiphy->debugfsdir);
+	if (IS_ERR(intf->driver_folder))
+		goto exit;
+
+	intf->driver_entry =
+	    rt2x00debug_create_file_driver("driver", intf, &intf->driver_blob);
+	if (IS_ERR(intf->driver_entry))
+		goto exit;
+
+	intf->chipset_entry =
+	    rt2x00debug_create_file_chipset("chipset",
+					    intf, &intf->chipset_blob);
+	if (IS_ERR(intf->chipset_entry))
+		goto exit;
+
+	intf->dev_flags = debugfs_create_file("dev_flags", S_IRUGO,
+					      intf->driver_folder, intf,
+					      &rt2x00debug_fop_dev_flags);
+	if (IS_ERR(intf->dev_flags))
+		goto exit;
+
+#define RT2X00DEBUGFS_CREATE_ENTRY(__intf, __name)		\
+({								\
+	(__intf)->__name##_off_entry =				\
+	    debugfs_create_u32(__stringify(__name) "_offset",	\
+			       S_IRUGO | S_IWUSR,		\
+			       (__intf)->driver_folder,		\
+			       &(__intf)->offset_##__name);	\
+	if (IS_ERR((__intf)->__name##_off_entry))		\
+		goto exit;					\
+								\
+	(__intf)->__name##_val_entry =				\
+	    debugfs_create_file(__stringify(__name) "_value",	\
+				S_IRUGO | S_IWUSR,		\
+				(__intf)->driver_folder,	\
+				(__intf), &rt2x00debug_fop_##__name);\
+	if (IS_ERR((__intf)->__name##_val_entry))		\
+		goto exit;					\
+})
+
+	RT2X00DEBUGFS_CREATE_ENTRY(intf, csr);
+	RT2X00DEBUGFS_CREATE_ENTRY(intf, eeprom);
+	RT2X00DEBUGFS_CREATE_ENTRY(intf, bbp);
+	RT2X00DEBUGFS_CREATE_ENTRY(intf, rf);
+
+#undef RT2X00DEBUGFS_CREATE_ENTRY
+
+	return;
+
+exit:
+	rt2x00debug_deregister(rt2x00dev);
+	ERROR(rt2x00dev, "Failed to register debug handler.\n");
+
+	return;
+}
+
+void rt2x00debug_deregister(struct rt2x00_dev *rt2x00dev)
+{
+	const struct rt2x00debug_intf *intf = rt2x00dev->debugfs_intf;
+
+	if (unlikely(!intf))
+		return;
+
+	debugfs_remove(intf->rf_val_entry);
+	debugfs_remove(intf->rf_off_entry);
+	debugfs_remove(intf->bbp_val_entry);
+	debugfs_remove(intf->bbp_off_entry);
+	debugfs_remove(intf->eeprom_val_entry);
+	debugfs_remove(intf->eeprom_off_entry);
+	debugfs_remove(intf->csr_val_entry);
+	debugfs_remove(intf->csr_off_entry);
+	debugfs_remove(intf->dev_flags);
+	debugfs_remove(intf->chipset_entry);
+	debugfs_remove(intf->driver_entry);
+	debugfs_remove(intf->driver_folder);
+	kfree(intf->chipset_blob.data);
+	kfree(intf->driver_blob.data);
+	kfree(intf);
+
+	rt2x00dev->debugfs_intf = NULL;
+}
diff --git a/drivers/net/wireless/rt2x00/rt2x00debug.h b/drivers/net/wireless/rt2x00/rt2x00debug.h
new file mode 100644
index 000000000000..860e8fa3a0da
--- /dev/null
+++ b/drivers/net/wireless/rt2x00/rt2x00debug.h
@@ -0,0 +1,57 @@
+/*
+	Copyright (C) 2004 - 2007 rt2x00 SourceForge Project
+	<http://rt2x00.serialmonkey.com>
+
+	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, or
+	(at your option) any later version.
+
+	This program is distributed in the hope that it will be useful,
+	but WITHOUT ANY WARRANTY; without even the implied warranty of
+	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+	GNU General Public License for more details.
+
+	You should have received a copy of the GNU General Public License
+	along with this program; if not, write to the
+	Free Software Foundation, Inc.,
+	59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+/*
+	Module: rt2x00debug
+	Abstract: Data structures for the rt2x00debug.
+ */
+
+#ifndef RT2X00DEBUG_H
+#define RT2X00DEBUG_H
+
+struct rt2x00_dev;
+
+#define RT2X00DEBUGFS_REGISTER_ENTRY(__name, __type)		\
+struct reg##__name {						\
+	void (*read)(const struct rt2x00_dev *rt2x00dev,	\
+		     const unsigned int word, __type *data);	\
+	void (*write)(const struct rt2x00_dev *rt2x00dev,	\
+		      const unsigned int word, __type data);	\
+								\
+	unsigned int word_size;					\
+	unsigned int word_count;				\
+} __name
+
+struct rt2x00debug {
+	/*
+	 * Reference to the modules structure.
+	 */
+	struct module *owner;
+
+	/*
+	 * Register access entries.
+	 */
+	RT2X00DEBUGFS_REGISTER_ENTRY(csr, u32);
+	RT2X00DEBUGFS_REGISTER_ENTRY(eeprom, u16);
+	RT2X00DEBUGFS_REGISTER_ENTRY(bbp, u8);
+	RT2X00DEBUGFS_REGISTER_ENTRY(rf, u32);
+};
+
+#endif /* RT2X00DEBUG_H */
diff --git a/drivers/net/wireless/rt2x00/rt2x00dev.c b/drivers/net/wireless/rt2x00/rt2x00dev.c
new file mode 100644
index 000000000000..bb6f46cfbb9f
--- /dev/null
+++ b/drivers/net/wireless/rt2x00/rt2x00dev.c
@@ -0,0 +1,1202 @@
+/*
+	Copyright (C) 2004 - 2007 rt2x00 SourceForge Project
+	<http://rt2x00.serialmonkey.com>
+
+	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, or
+	(at your option) any later version.
+
+	This program is distributed in the hope that it will be useful,
+	but WITHOUT ANY WARRANTY; without even the implied warranty of
+	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+	GNU General Public License for more details.
+
+	You should have received a copy of the GNU General Public License
+	along with this program; if not, write to the
+	Free Software Foundation, Inc.,
+	59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+/*
+	Module: rt2x00lib
+	Abstract: rt2x00 generic device routines.
+ */
+
+/*
+ * Set enviroment defines for rt2x00.h
+ */
+#define DRV_NAME "rt2x00lib"
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+
+#include "rt2x00.h"
+#include "rt2x00lib.h"
+
+/*
+ * Ring handler.
+ */
+struct data_ring *rt2x00lib_get_ring(struct rt2x00_dev *rt2x00dev,
+				     const unsigned int queue)
+{
+	int beacon = test_bit(DRIVER_REQUIRE_BEACON_RING, &rt2x00dev->flags);
+
+	/*
+	 * Check if we are requesting a reqular TX ring,
+	 * or if we are requesting a Beacon or Atim ring.
+	 * For Atim rings, we should check if it is supported.
+	 */
+	if (queue < rt2x00dev->hw->queues && rt2x00dev->tx)
+		return &rt2x00dev->tx[queue];
+
+	if (!rt2x00dev->bcn || !beacon)
+		return NULL;
+
+	if (queue == IEEE80211_TX_QUEUE_BEACON)
+		return &rt2x00dev->bcn[0];
+	else if (queue == IEEE80211_TX_QUEUE_AFTER_BEACON)
+		return &rt2x00dev->bcn[1];
+
+	return NULL;
+}
+EXPORT_SYMBOL_GPL(rt2x00lib_get_ring);
+
+/*
+ * Link tuning handlers
+ */
+static void rt2x00lib_start_link_tuner(struct rt2x00_dev *rt2x00dev)
+{
+	rt2x00_clear_link(&rt2x00dev->link);
+
+	/*
+	 * Reset the link tuner.
+	 */
+	rt2x00dev->ops->lib->reset_tuner(rt2x00dev);
+
+	queue_delayed_work(rt2x00dev->hw->workqueue,
+			   &rt2x00dev->link.work, LINK_TUNE_INTERVAL);
+}
+
+static void rt2x00lib_stop_link_tuner(struct rt2x00_dev *rt2x00dev)
+{
+	cancel_delayed_work_sync(&rt2x00dev->link.work);
+}
+
+void rt2x00lib_reset_link_tuner(struct rt2x00_dev *rt2x00dev)
+{
+	if (!test_bit(DEVICE_ENABLED_RADIO, &rt2x00dev->flags))
+		return;
+
+	rt2x00lib_stop_link_tuner(rt2x00dev);
+	rt2x00lib_start_link_tuner(rt2x00dev);
+}
+
+/*
+ * Radio control handlers.
+ */
+int rt2x00lib_enable_radio(struct rt2x00_dev *rt2x00dev)
+{
+	int status;
+
+	/*
+	 * Don't enable the radio twice.
+	 * And check if the hardware button has been disabled.
+	 */
+	if (test_bit(DEVICE_ENABLED_RADIO, &rt2x00dev->flags) ||
+	    test_bit(DEVICE_DISABLED_RADIO_HW, &rt2x00dev->flags))
+		return 0;
+
+	/*
+	 * Enable radio.
+	 */
+	status = rt2x00dev->ops->lib->set_device_state(rt2x00dev,
+						       STATE_RADIO_ON);
+	if (status)
+		return status;
+
+	__set_bit(DEVICE_ENABLED_RADIO, &rt2x00dev->flags);
+
+	/*
+	 * Enable RX.
+	 */
+	rt2x00lib_toggle_rx(rt2x00dev, STATE_RADIO_RX_ON);
+
+	/*
+	 * Start the TX queues.
+	 */
+	ieee80211_start_queues(rt2x00dev->hw);
+
+	return 0;
+}
+
+void rt2x00lib_disable_radio(struct rt2x00_dev *rt2x00dev)
+{
+	if (!__test_and_clear_bit(DEVICE_ENABLED_RADIO, &rt2x00dev->flags))
+		return;
+
+	/*
+	 * Stop all scheduled work.
+	 */
+	if (work_pending(&rt2x00dev->beacon_work))
+		cancel_work_sync(&rt2x00dev->beacon_work);
+	if (work_pending(&rt2x00dev->filter_work))
+		cancel_work_sync(&rt2x00dev->filter_work);
+	if (work_pending(&rt2x00dev->config_work))
+		cancel_work_sync(&rt2x00dev->config_work);
+
+	/*
+	 * Stop the TX queues.
+	 */
+	ieee80211_stop_queues(rt2x00dev->hw);
+
+	/*
+	 * Disable RX.
+	 */
+	rt2x00lib_toggle_rx(rt2x00dev, STATE_RADIO_RX_OFF);
+
+	/*
+	 * Disable radio.
+	 */
+	rt2x00dev->ops->lib->set_device_state(rt2x00dev, STATE_RADIO_OFF);
+}
+
+void rt2x00lib_toggle_rx(struct rt2x00_dev *rt2x00dev, enum dev_state state)
+{
+	/*
+	 * When we are disabling the RX, we should also stop the link tuner.
+	 */
+	if (state == STATE_RADIO_RX_OFF)
+		rt2x00lib_stop_link_tuner(rt2x00dev);
+
+	rt2x00dev->ops->lib->set_device_state(rt2x00dev, state);
+
+	/*
+	 * When we are enabling the RX, we should also start the link tuner.
+	 */
+	if (state == STATE_RADIO_RX_ON &&
+	    is_interface_present(&rt2x00dev->interface))
+		rt2x00lib_start_link_tuner(rt2x00dev);
+}
+
+static void rt2x00lib_precalculate_link_signal(struct link *link)
+{
+	if (link->rx_failed || link->rx_success)
+		link->rx_percentage =
+		    (link->rx_success * 100) /
+		    (link->rx_failed + link->rx_success);
+	else
+		link->rx_percentage = 50;
+
+	if (link->tx_failed || link->tx_success)
+		link->tx_percentage =
+		    (link->tx_success * 100) /
+		    (link->tx_failed + link->tx_success);
+	else
+		link->tx_percentage = 50;
+
+	link->rx_success = 0;
+	link->rx_failed = 0;
+	link->tx_success = 0;
+	link->tx_failed = 0;
+}
+
+static int rt2x00lib_calculate_link_signal(struct rt2x00_dev *rt2x00dev,
+					   int rssi)
+{
+	int rssi_percentage = 0;
+	int signal;
+
+	/*
+	 * We need a positive value for the RSSI.
+	 */
+	if (rssi < 0)
+		rssi += rt2x00dev->rssi_offset;
+
+	/*
+	 * Calculate the different percentages,
+	 * which will be used for the signal.
+	 */
+	if (rt2x00dev->rssi_offset)
+		rssi_percentage = (rssi * 100) / rt2x00dev->rssi_offset;
+
+	/*
+	 * Add the individual percentages and use the WEIGHT
+	 * defines to calculate the current link signal.
+	 */
+	signal = ((WEIGHT_RSSI * rssi_percentage) +
+		  (WEIGHT_TX * rt2x00dev->link.tx_percentage) +
+		  (WEIGHT_RX * rt2x00dev->link.rx_percentage)) / 100;
+
+	return (signal > 100) ? 100 : signal;
+}
+
+static void rt2x00lib_link_tuner(struct work_struct *work)
+{
+	struct rt2x00_dev *rt2x00dev =
+	    container_of(work, struct rt2x00_dev, link.work.work);
+
+	/*
+	 * When the radio is shutting down we should
+	 * immediately cease all link tuning.
+	 */
+	if (!test_bit(DEVICE_ENABLED_RADIO, &rt2x00dev->flags))
+		return;
+
+	/*
+	 * Update statistics.
+	 */
+	rt2x00dev->ops->lib->link_stats(rt2x00dev);
+
+	rt2x00dev->low_level_stats.dot11FCSErrorCount +=
+	    rt2x00dev->link.rx_failed;
+
+	/*
+	 * Only perform the link tuning when Link tuning
+	 * has been enabled (This could have been disabled from the EEPROM).
+	 */
+	if (!test_bit(CONFIG_DISABLE_LINK_TUNING, &rt2x00dev->flags))
+		rt2x00dev->ops->lib->link_tuner(rt2x00dev);
+
+	/*
+	 * Precalculate a portion of the link signal which is
+	 * in based on the tx/rx success/failure counters.
+	 */
+	rt2x00lib_precalculate_link_signal(&rt2x00dev->link);
+
+	/*
+	 * Increase tuner counter, and reschedule the next link tuner run.
+	 */
+	rt2x00dev->link.count++;
+	queue_delayed_work(rt2x00dev->hw->workqueue, &rt2x00dev->link.work,
+			   LINK_TUNE_INTERVAL);
+}
+
+static void rt2x00lib_packetfilter_scheduled(struct work_struct *work)
+{
+	struct rt2x00_dev *rt2x00dev =
+	    container_of(work, struct rt2x00_dev, filter_work);
+	unsigned int filter = rt2x00dev->interface.filter;
+
+	/*
+	 * Since we had stored the filter inside interface.filter,
+	 * we should now clear that field. Otherwise the driver will
+	 * assume nothing has changed (*total_flags will be compared
+	 * to interface.filter to determine if any action is required).
+	 */
+	rt2x00dev->interface.filter = 0;
+
+	rt2x00dev->ops->hw->configure_filter(rt2x00dev->hw,
+					     filter, &filter, 0, NULL);
+}
+
+static void rt2x00lib_configuration_scheduled(struct work_struct *work)
+{
+	struct rt2x00_dev *rt2x00dev =
+	    container_of(work, struct rt2x00_dev, config_work);
+	int preamble = !test_bit(CONFIG_SHORT_PREAMBLE, &rt2x00dev->flags);
+
+	rt2x00mac_erp_ie_changed(rt2x00dev->hw,
+				 IEEE80211_ERP_CHANGE_PREAMBLE, 0, preamble);
+}
+
+/*
+ * Interrupt context handlers.
+ */
+static void rt2x00lib_beacondone_scheduled(struct work_struct *work)
+{
+	struct rt2x00_dev *rt2x00dev =
+	    container_of(work, struct rt2x00_dev, beacon_work);
+	struct data_ring *ring =
+	    rt2x00lib_get_ring(rt2x00dev, IEEE80211_TX_QUEUE_BEACON);
+	struct data_entry *entry = rt2x00_get_data_entry(ring);
+	struct sk_buff *skb;
+
+	skb = ieee80211_beacon_get(rt2x00dev->hw,
+				   rt2x00dev->interface.id,
+				   &entry->tx_status.control);
+	if (!skb)
+		return;
+
+	rt2x00dev->ops->hw->beacon_update(rt2x00dev->hw, skb,
+					  &entry->tx_status.control);
+
+	dev_kfree_skb(skb);
+}
+
+void rt2x00lib_beacondone(struct rt2x00_dev *rt2x00dev)
+{
+	if (!test_bit(DEVICE_ENABLED_RADIO, &rt2x00dev->flags))
+		return;
+
+	queue_work(rt2x00dev->hw->workqueue, &rt2x00dev->beacon_work);
+}
+EXPORT_SYMBOL_GPL(rt2x00lib_beacondone);
+
+void rt2x00lib_txdone(struct data_entry *entry,
+		      const int status, const int retry)
+{
+	struct rt2x00_dev *rt2x00dev = entry->ring->rt2x00dev;
+	struct ieee80211_tx_status *tx_status = &entry->tx_status;
+	struct ieee80211_low_level_stats *stats = &rt2x00dev->low_level_stats;
+	int success = !!(status == TX_SUCCESS || status == TX_SUCCESS_RETRY);
+	int fail = !!(status == TX_FAIL_RETRY || status == TX_FAIL_INVALID ||
+		      status == TX_FAIL_OTHER);
+
+	/*
+	 * Update TX statistics.
+	 */
+	tx_status->flags = 0;
+	tx_status->ack_signal = 0;
+	tx_status->excessive_retries = (status == TX_FAIL_RETRY);
+	tx_status->retry_count = retry;
+	rt2x00dev->link.tx_success += success;
+	rt2x00dev->link.tx_failed += retry + fail;
+
+	if (!(tx_status->control.flags & IEEE80211_TXCTL_NO_ACK)) {
+		if (success)
+			tx_status->flags |= IEEE80211_TX_STATUS_ACK;
+		else
+			stats->dot11ACKFailureCount++;
+	}
+
+	tx_status->queue_length = entry->ring->stats.limit;
+	tx_status->queue_number = tx_status->control.queue;
+
+	if (tx_status->control.flags & IEEE80211_TXCTL_USE_RTS_CTS) {
+		if (success)
+			stats->dot11RTSSuccessCount++;
+		else
+			stats->dot11RTSFailureCount++;
+	}
+
+	/*
+	 * Send the tx_status to mac80211,
+	 * that method also cleans up the skb structure.
+	 */
+	ieee80211_tx_status_irqsafe(rt2x00dev->hw, entry->skb, tx_status);
+	entry->skb = NULL;
+}
+EXPORT_SYMBOL_GPL(rt2x00lib_txdone);
+
+void rt2x00lib_rxdone(struct data_entry *entry, struct sk_buff *skb,
+		      struct rxdata_entry_desc *desc)
+{
+	struct rt2x00_dev *rt2x00dev = entry->ring->rt2x00dev;
+	struct ieee80211_rx_status *rx_status = &rt2x00dev->rx_status;
+	struct ieee80211_hw_mode *mode;
+	struct ieee80211_rate *rate;
+	unsigned int i;
+	int val = 0;
+
+	/*
+	 * Update RX statistics.
+	 */
+	mode = &rt2x00dev->hwmodes[rt2x00dev->curr_hwmode];
+	for (i = 0; i < mode->num_rates; i++) {
+		rate = &mode->rates[i];
+
+		/*
+		 * When frame was received with an OFDM bitrate,
+		 * the signal is the PLCP value. If it was received with
+		 * a CCK bitrate the signal is the rate in 0.5kbit/s.
+		 */
+		if (!desc->ofdm)
+			val = DEVICE_GET_RATE_FIELD(rate->val, RATE);
+		else
+			val = DEVICE_GET_RATE_FIELD(rate->val, PLCP);
+
+		if (val == desc->signal) {
+			val = rate->val;
+			break;
+		}
+	}
+
+	rt2x00_update_link_rssi(&rt2x00dev->link, desc->rssi);
+	rt2x00dev->link.rx_success++;
+	rx_status->rate = val;
+	rx_status->signal =
+	    rt2x00lib_calculate_link_signal(rt2x00dev, desc->rssi);
+	rx_status->ssi = desc->rssi;
+	rx_status->flag = desc->flags;
+
+	/*
+	 * Send frame to mac80211
+	 */
+	ieee80211_rx_irqsafe(rt2x00dev->hw, skb, rx_status);
+}
+EXPORT_SYMBOL_GPL(rt2x00lib_rxdone);
+
+/*
+ * TX descriptor initializer
+ */
+void rt2x00lib_write_tx_desc(struct rt2x00_dev *rt2x00dev,
+			     struct data_desc *txd,
+			     struct ieee80211_hdr *ieee80211hdr,
+			     unsigned int length,
+			     struct ieee80211_tx_control *control)
+{
+	struct txdata_entry_desc desc;
+	struct data_ring *ring;
+	int tx_rate;
+	int bitrate;
+	int duration;
+	int residual;
+	u16 frame_control;
+	u16 seq_ctrl;
+
+	/*
+	 * Make sure the descriptor is properly cleared.
+	 */
+	memset(&desc, 0x00, sizeof(desc));
+
+	/*
+	 * Get ring pointer, if we fail to obtain the
+	 * correct ring, then use the first TX ring.
+	 */
+	ring = rt2x00lib_get_ring(rt2x00dev, control->queue);
+	if (!ring)
+		ring = rt2x00lib_get_ring(rt2x00dev, IEEE80211_TX_QUEUE_DATA0);
+
+	desc.cw_min = ring->tx_params.cw_min;
+	desc.cw_max = ring->tx_params.cw_max;
+	desc.aifs = ring->tx_params.aifs;
+
+	/*
+	 * Identify queue
+	 */
+	if (control->queue < rt2x00dev->hw->queues)
+		desc.queue = control->queue;
+	else if (control->queue == IEEE80211_TX_QUEUE_BEACON ||
+		 control->queue == IEEE80211_TX_QUEUE_AFTER_BEACON)
+		desc.queue = QUEUE_MGMT;
+	else
+		desc.queue = QUEUE_OTHER;
+
+	/*
+	 * Read required fields from ieee80211 header.
+	 */
+	frame_control = le16_to_cpu(ieee80211hdr->frame_control);
+	seq_ctrl = le16_to_cpu(ieee80211hdr->seq_ctrl);
+
+	tx_rate = control->tx_rate;
+
+	/*
+	 * Check if this is a RTS/CTS frame
+	 */
+	if (is_rts_frame(frame_control) || is_cts_frame(frame_control)) {
+		__set_bit(ENTRY_TXD_BURST, &desc.flags);
+		if (is_rts_frame(frame_control))
+			__set_bit(ENTRY_TXD_RTS_FRAME, &desc.flags);
+		if (control->rts_cts_rate)
+			tx_rate = control->rts_cts_rate;
+	}
+
+	/*
+	 * Check for OFDM
+	 */
+	if (DEVICE_GET_RATE_FIELD(tx_rate, RATEMASK) & DEV_OFDM_RATEMASK)
+		__set_bit(ENTRY_TXD_OFDM_RATE, &desc.flags);
+
+	/*
+	 * Check if more fragments are pending
+	 */
+	if (ieee80211_get_morefrag(ieee80211hdr)) {
+		__set_bit(ENTRY_TXD_BURST, &desc.flags);
+		__set_bit(ENTRY_TXD_MORE_FRAG, &desc.flags);
+	}
+
+	/*
+	 * Beacons and probe responses require the tsf timestamp
+	 * to be inserted into the frame.
+	 */
+	if (control->queue == IEEE80211_TX_QUEUE_BEACON ||
+	    is_probe_resp(frame_control))
+		__set_bit(ENTRY_TXD_REQ_TIMESTAMP, &desc.flags);
+
+	/*
+	 * Determine with what IFS priority this frame should be send.
+	 * Set ifs to IFS_SIFS when the this is not the first fragment,
+	 * or this fragment came after RTS/CTS.
+	 */
+	if ((seq_ctrl & IEEE80211_SCTL_FRAG) > 0 ||
+	    test_bit(ENTRY_TXD_RTS_FRAME, &desc.flags))
+		desc.ifs = IFS_SIFS;
+	else
+		desc.ifs = IFS_BACKOFF;
+
+	/*
+	 * PLCP setup
+	 * Length calculation depends on OFDM/CCK rate.
+	 */
+	desc.signal = DEVICE_GET_RATE_FIELD(tx_rate, PLCP);
+	desc.service = 0x04;
+
+	if (test_bit(ENTRY_TXD_OFDM_RATE, &desc.flags)) {
+		desc.length_high = ((length + FCS_LEN) >> 6) & 0x3f;
+		desc.length_low = ((length + FCS_LEN) & 0x3f);
+	} else {
+		bitrate = DEVICE_GET_RATE_FIELD(tx_rate, RATE);
+
+		/*
+		 * Convert length to microseconds.
+		 */
+		residual = get_duration_res(length + FCS_LEN, bitrate);
+		duration = get_duration(length + FCS_LEN, bitrate);
+
+		if (residual != 0) {
+			duration++;
+
+			/*
+			 * Check if we need to set the Length Extension
+			 */
+			if (bitrate == 110 && residual <= 3)
+				desc.service |= 0x80;
+		}
+
+		desc.length_high = (duration >> 8) & 0xff;
+		desc.length_low = duration & 0xff;
+
+		/*
+		 * When preamble is enabled we should set the
+		 * preamble bit for the signal.
+		 */
+		if (DEVICE_GET_RATE_FIELD(tx_rate, PREAMBLE))
+			desc.signal |= 0x08;
+	}
+
+	rt2x00dev->ops->lib->write_tx_desc(rt2x00dev, txd, &desc,
+					   ieee80211hdr, length, control);
+}
+EXPORT_SYMBOL_GPL(rt2x00lib_write_tx_desc);
+
+/*
+ * Driver initialization handlers.
+ */
+static void rt2x00lib_channel(struct ieee80211_channel *entry,
+			      const int channel, const int tx_power,
+			      const int value)
+{
+	entry->chan = channel;
+	if (channel <= 14)
+		entry->freq = 2407 + (5 * channel);
+	else
+		entry->freq = 5000 + (5 * channel);
+	entry->val = value;
+	entry->flag =
+	    IEEE80211_CHAN_W_IBSS |
+	    IEEE80211_CHAN_W_ACTIVE_SCAN |
+	    IEEE80211_CHAN_W_SCAN;
+	entry->power_level = tx_power;
+	entry->antenna_max = 0xff;
+}
+
+static void rt2x00lib_rate(struct ieee80211_rate *entry,
+			   const int rate, const int mask,
+			   const int plcp, const int flags)
+{
+	entry->rate = rate;
+	entry->val =
+	    DEVICE_SET_RATE_FIELD(rate, RATE) |
+	    DEVICE_SET_RATE_FIELD(mask, RATEMASK) |
+	    DEVICE_SET_RATE_FIELD(plcp, PLCP);
+	entry->flags = flags;
+	entry->val2 = entry->val;
+	if (entry->flags & IEEE80211_RATE_PREAMBLE2)
+		entry->val2 |= DEVICE_SET_RATE_FIELD(1, PREAMBLE);
+	entry->min_rssi_ack = 0;
+	entry->min_rssi_ack_delta = 0;
+}
+
+static int rt2x00lib_probe_hw_modes(struct rt2x00_dev *rt2x00dev,
+				    struct hw_mode_spec *spec)
+{
+	struct ieee80211_hw *hw = rt2x00dev->hw;
+	struct ieee80211_hw_mode *hwmodes;
+	struct ieee80211_channel *channels;
+	struct ieee80211_rate *rates;
+	unsigned int i;
+	unsigned char tx_power;
+
+	hwmodes = kzalloc(sizeof(*hwmodes) * spec->num_modes, GFP_KERNEL);
+	if (!hwmodes)
+		goto exit;
+
+	channels = kzalloc(sizeof(*channels) * spec->num_channels, GFP_KERNEL);
+	if (!channels)
+		goto exit_free_modes;
+
+	rates = kzalloc(sizeof(*rates) * spec->num_rates, GFP_KERNEL);
+	if (!rates)
+		goto exit_free_channels;
+
+	/*
+	 * Initialize Rate list.
+	 */
+	rt2x00lib_rate(&rates[0], 10, DEV_RATEMASK_1MB,
+		       0x00, IEEE80211_RATE_CCK);
+	rt2x00lib_rate(&rates[1], 20, DEV_RATEMASK_2MB,
+		       0x01, IEEE80211_RATE_CCK_2);
+	rt2x00lib_rate(&rates[2], 55, DEV_RATEMASK_5_5MB,
+		       0x02, IEEE80211_RATE_CCK_2);
+	rt2x00lib_rate(&rates[3], 110, DEV_RATEMASK_11MB,
+		       0x03, IEEE80211_RATE_CCK_2);
+
+	if (spec->num_rates > 4) {
+		rt2x00lib_rate(&rates[4], 60, DEV_RATEMASK_6MB,
+			       0x0b, IEEE80211_RATE_OFDM);
+		rt2x00lib_rate(&rates[5], 90, DEV_RATEMASK_9MB,
+			       0x0f, IEEE80211_RATE_OFDM);
+		rt2x00lib_rate(&rates[6], 120, DEV_RATEMASK_12MB,
+			       0x0a, IEEE80211_RATE_OFDM);
+		rt2x00lib_rate(&rates[7], 180, DEV_RATEMASK_18MB,
+			       0x0e, IEEE80211_RATE_OFDM);
+		rt2x00lib_rate(&rates[8], 240, DEV_RATEMASK_24MB,
+			       0x09, IEEE80211_RATE_OFDM);
+		rt2x00lib_rate(&rates[9], 360, DEV_RATEMASK_36MB,
+			       0x0d, IEEE80211_RATE_OFDM);
+		rt2x00lib_rate(&rates[10], 480, DEV_RATEMASK_48MB,
+			       0x08, IEEE80211_RATE_OFDM);
+		rt2x00lib_rate(&rates[11], 540, DEV_RATEMASK_54MB,
+			       0x0c, IEEE80211_RATE_OFDM);
+	}
+
+	/*
+	 * Initialize Channel list.
+	 */
+	for (i = 0; i < spec->num_channels; i++) {
+		if (spec->channels[i].channel <= 14)
+			tx_power = spec->tx_power_bg[i];
+		else if (spec->tx_power_a)
+			tx_power = spec->tx_power_a[i];
+		else
+			tx_power = spec->tx_power_default;
+
+		rt2x00lib_channel(&channels[i],
+				  spec->channels[i].channel, tx_power, i);
+	}
+
+	/*
+	 * Intitialize 802.11b
+	 * Rates: CCK.
+	 * Channels: OFDM.
+	 */
+	if (spec->num_modes > HWMODE_B) {
+		hwmodes[HWMODE_B].mode = MODE_IEEE80211B;
+		hwmodes[HWMODE_B].num_channels = 14;
+		hwmodes[HWMODE_B].num_rates = 4;
+		hwmodes[HWMODE_B].channels = channels;
+		hwmodes[HWMODE_B].rates = rates;
+	}
+
+	/*
+	 * Intitialize 802.11g
+	 * Rates: CCK, OFDM.
+	 * Channels: OFDM.
+	 */
+	if (spec->num_modes > HWMODE_G) {
+		hwmodes[HWMODE_G].mode = MODE_IEEE80211G;
+		hwmodes[HWMODE_G].num_channels = 14;
+		hwmodes[HWMODE_G].num_rates = spec->num_rates;
+		hwmodes[HWMODE_G].channels = channels;
+		hwmodes[HWMODE_G].rates = rates;
+	}
+
+	/*
+	 * Intitialize 802.11a
+	 * Rates: OFDM.
+	 * Channels: OFDM, UNII, HiperLAN2.
+	 */
+	if (spec->num_modes > HWMODE_A) {
+		hwmodes[HWMODE_A].mode = MODE_IEEE80211A;
+		hwmodes[HWMODE_A].num_channels = spec->num_channels - 14;
+		hwmodes[HWMODE_A].num_rates = spec->num_rates - 4;
+		hwmodes[HWMODE_A].channels = &channels[14];
+		hwmodes[HWMODE_A].rates = &rates[4];
+	}
+
+	if (spec->num_modes > HWMODE_G &&
+	    ieee80211_register_hwmode(hw, &hwmodes[HWMODE_G]))
+		goto exit_free_rates;
+
+	if (spec->num_modes > HWMODE_B &&
+	    ieee80211_register_hwmode(hw, &hwmodes[HWMODE_B]))
+		goto exit_free_rates;
+
+	if (spec->num_modes > HWMODE_A &&
+	    ieee80211_register_hwmode(hw, &hwmodes[HWMODE_A]))
+		goto exit_free_rates;
+
+	rt2x00dev->hwmodes = hwmodes;
+
+	return 0;
+
+exit_free_rates:
+	kfree(rates);
+
+exit_free_channels:
+	kfree(channels);
+
+exit_free_modes:
+	kfree(hwmodes);
+
+exit:
+	ERROR(rt2x00dev, "Allocation ieee80211 modes failed.\n");
+	return -ENOMEM;
+}
+
+static void rt2x00lib_remove_hw(struct rt2x00_dev *rt2x00dev)
+{
+	if (test_bit(DEVICE_REGISTERED_HW, &rt2x00dev->flags))
+		ieee80211_unregister_hw(rt2x00dev->hw);
+
+	if (likely(rt2x00dev->hwmodes)) {
+		kfree(rt2x00dev->hwmodes->channels);
+		kfree(rt2x00dev->hwmodes->rates);
+		kfree(rt2x00dev->hwmodes);
+		rt2x00dev->hwmodes = NULL;
+	}
+}
+
+static int rt2x00lib_probe_hw(struct rt2x00_dev *rt2x00dev)
+{
+	struct hw_mode_spec *spec = &rt2x00dev->spec;
+	int status;
+
+	/*
+	 * Initialize HW modes.
+	 */
+	status = rt2x00lib_probe_hw_modes(rt2x00dev, spec);
+	if (status)
+		return status;
+
+	/*
+	 * Register HW.
+	 */
+	status = ieee80211_register_hw(rt2x00dev->hw);
+	if (status) {
+		rt2x00lib_remove_hw(rt2x00dev);
+		return status;
+	}
+
+	__set_bit(DEVICE_REGISTERED_HW, &rt2x00dev->flags);
+
+	return 0;
+}
+
+/*
+ * Initialization/uninitialization handlers.
+ */
+static int rt2x00lib_alloc_entries(struct data_ring *ring,
+				   const u16 max_entries, const u16 data_size,
+				   const u16 desc_size)
+{
+	struct data_entry *entry;
+	unsigned int i;
+
+	ring->stats.limit = max_entries;
+	ring->data_size = data_size;
+	ring->desc_size = desc_size;
+
+	/*
+	 * Allocate all ring entries.
+	 */
+	entry = kzalloc(ring->stats.limit * sizeof(*entry), GFP_KERNEL);
+	if (!entry)
+		return -ENOMEM;
+
+	for (i = 0; i < ring->stats.limit; i++) {
+		entry[i].flags = 0;
+		entry[i].ring = ring;
+		entry[i].skb = NULL;
+	}
+
+	ring->entry = entry;
+
+	return 0;
+}
+
+static int rt2x00lib_alloc_ring_entries(struct rt2x00_dev *rt2x00dev)
+{
+	struct data_ring *ring;
+
+	/*
+	 * Allocate the RX ring.
+	 */
+	if (rt2x00lib_alloc_entries(rt2x00dev->rx, RX_ENTRIES, DATA_FRAME_SIZE,
+				    rt2x00dev->ops->rxd_size))
+		return -ENOMEM;
+
+	/*
+	 * First allocate the TX rings.
+	 */
+	txring_for_each(rt2x00dev, ring) {
+		if (rt2x00lib_alloc_entries(ring, TX_ENTRIES, DATA_FRAME_SIZE,
+					    rt2x00dev->ops->txd_size))
+			return -ENOMEM;
+	}
+
+	if (!test_bit(DRIVER_REQUIRE_BEACON_RING, &rt2x00dev->flags))
+		return 0;
+
+	/*
+	 * Allocate the BEACON ring.
+	 */
+	if (rt2x00lib_alloc_entries(&rt2x00dev->bcn[0], BEACON_ENTRIES,
+				    MGMT_FRAME_SIZE, rt2x00dev->ops->txd_size))
+		return -ENOMEM;
+
+	/*
+	 * Allocate the Atim ring.
+	 */
+	if (rt2x00lib_alloc_entries(&rt2x00dev->bcn[1], ATIM_ENTRIES,
+				    DATA_FRAME_SIZE, rt2x00dev->ops->txd_size))
+		return -ENOMEM;
+
+	return 0;
+}
+
+static void rt2x00lib_free_ring_entries(struct rt2x00_dev *rt2x00dev)
+{
+	struct data_ring *ring;
+
+	ring_for_each(rt2x00dev, ring) {
+		kfree(ring->entry);
+		ring->entry = NULL;
+	}
+}
+
+void rt2x00lib_uninitialize(struct rt2x00_dev *rt2x00dev)
+{
+	if (!__test_and_clear_bit(DEVICE_INITIALIZED, &rt2x00dev->flags))
+		return;
+
+	/*
+	 * Unregister rfkill.
+	 */
+	rt2x00rfkill_unregister(rt2x00dev);
+
+	/*
+	 * Allow the HW to uninitialize.
+	 */
+	rt2x00dev->ops->lib->uninitialize(rt2x00dev);
+
+	/*
+	 * Free allocated ring entries.
+	 */
+	rt2x00lib_free_ring_entries(rt2x00dev);
+}
+
+int rt2x00lib_initialize(struct rt2x00_dev *rt2x00dev)
+{
+	int status;
+
+	if (test_bit(DEVICE_INITIALIZED, &rt2x00dev->flags))
+		return 0;
+
+	/*
+	 * Allocate all ring entries.
+	 */
+	status = rt2x00lib_alloc_ring_entries(rt2x00dev);
+	if (status) {
+		ERROR(rt2x00dev, "Ring entries allocation failed.\n");
+		return status;
+	}
+
+	/*
+	 * Initialize the device.
+	 */
+	status = rt2x00dev->ops->lib->initialize(rt2x00dev);
+	if (status)
+		goto exit;
+
+	__set_bit(DEVICE_INITIALIZED, &rt2x00dev->flags);
+
+	/*
+	 * Register the rfkill handler.
+	 */
+	status = rt2x00rfkill_register(rt2x00dev);
+	if (status)
+		goto exit_unitialize;
+
+	return 0;
+
+exit_unitialize:
+	rt2x00lib_uninitialize(rt2x00dev);
+
+exit:
+	rt2x00lib_free_ring_entries(rt2x00dev);
+
+	return status;
+}
+
+/*
+ * driver allocation handlers.
+ */
+static int rt2x00lib_alloc_rings(struct rt2x00_dev *rt2x00dev)
+{
+	struct data_ring *ring;
+
+	/*
+	 * We need the following rings:
+	 * RX: 1
+	 * TX: hw->queues
+	 * Beacon: 1 (if required)
+	 * Atim: 1 (if required)
+	 */
+	rt2x00dev->data_rings = 1 + rt2x00dev->hw->queues +
+	    (2 * test_bit(DRIVER_REQUIRE_BEACON_RING, &rt2x00dev->flags));
+
+	ring = kzalloc(rt2x00dev->data_rings * sizeof(*ring), GFP_KERNEL);
+	if (!ring) {
+		ERROR(rt2x00dev, "Ring allocation failed.\n");
+		return -ENOMEM;
+	}
+
+	/*
+	 * Initialize pointers
+	 */
+	rt2x00dev->rx = ring;
+	rt2x00dev->tx = &rt2x00dev->rx[1];
+	if (test_bit(DRIVER_REQUIRE_BEACON_RING, &rt2x00dev->flags))
+		rt2x00dev->bcn = &rt2x00dev->tx[rt2x00dev->hw->queues];
+
+	/*
+	 * Initialize ring parameters.
+	 * cw_min: 2^5 = 32.
+	 * cw_max: 2^10 = 1024.
+	 */
+	ring_for_each(rt2x00dev, ring) {
+		ring->rt2x00dev = rt2x00dev;
+		ring->tx_params.aifs = 2;
+		ring->tx_params.cw_min = 5;
+		ring->tx_params.cw_max = 10;
+	}
+
+	return 0;
+}
+
+static void rt2x00lib_free_rings(struct rt2x00_dev *rt2x00dev)
+{
+	kfree(rt2x00dev->rx);
+	rt2x00dev->rx = NULL;
+	rt2x00dev->tx = NULL;
+	rt2x00dev->bcn = NULL;
+}
+
+int rt2x00lib_probe_dev(struct rt2x00_dev *rt2x00dev)
+{
+	int retval = -ENOMEM;
+
+	/*
+	 * Let the driver probe the device to detect the capabilities.
+	 */
+	retval = rt2x00dev->ops->lib->probe_hw(rt2x00dev);
+	if (retval) {
+		ERROR(rt2x00dev, "Failed to allocate device.\n");
+		goto exit;
+	}
+
+	/*
+	 * Initialize configuration work.
+	 */
+	INIT_WORK(&rt2x00dev->beacon_work, rt2x00lib_beacondone_scheduled);
+	INIT_WORK(&rt2x00dev->filter_work, rt2x00lib_packetfilter_scheduled);
+	INIT_WORK(&rt2x00dev->config_work, rt2x00lib_configuration_scheduled);
+	INIT_DELAYED_WORK(&rt2x00dev->link.work, rt2x00lib_link_tuner);
+
+	/*
+	 * Reset current working type.
+	 */
+	rt2x00dev->interface.type = INVALID_INTERFACE;
+
+	/*
+	 * Allocate ring array.
+	 */
+	retval = rt2x00lib_alloc_rings(rt2x00dev);
+	if (retval)
+		goto exit;
+
+	/*
+	 * Initialize ieee80211 structure.
+	 */
+	retval = rt2x00lib_probe_hw(rt2x00dev);
+	if (retval) {
+		ERROR(rt2x00dev, "Failed to initialize hw.\n");
+		goto exit;
+	}
+
+	/*
+	 * Allocatie rfkill.
+	 */
+	retval = rt2x00rfkill_allocate(rt2x00dev);
+	if (retval)
+		goto exit;
+
+	/*
+	 * Open the debugfs entry.
+	 */
+	rt2x00debug_register(rt2x00dev);
+
+	__set_bit(DEVICE_PRESENT, &rt2x00dev->flags);
+
+	return 0;
+
+exit:
+	rt2x00lib_remove_dev(rt2x00dev);
+
+	return retval;
+}
+EXPORT_SYMBOL_GPL(rt2x00lib_probe_dev);
+
+void rt2x00lib_remove_dev(struct rt2x00_dev *rt2x00dev)
+{
+	__clear_bit(DEVICE_PRESENT, &rt2x00dev->flags);
+
+	/*
+	 * Disable radio.
+	 */
+	rt2x00lib_disable_radio(rt2x00dev);
+
+	/*
+	 * Uninitialize device.
+	 */
+	rt2x00lib_uninitialize(rt2x00dev);
+
+	/*
+	 * Close debugfs entry.
+	 */
+	rt2x00debug_deregister(rt2x00dev);
+
+	/*
+	 * Free rfkill
+	 */
+	rt2x00rfkill_free(rt2x00dev);
+
+	/*
+	 * Free ieee80211_hw memory.
+	 */
+	rt2x00lib_remove_hw(rt2x00dev);
+
+	/*
+	 * Free firmware image.
+	 */
+	rt2x00lib_free_firmware(rt2x00dev);
+
+	/*
+	 * Free ring structures.
+	 */
+	rt2x00lib_free_rings(rt2x00dev);
+}
+EXPORT_SYMBOL_GPL(rt2x00lib_remove_dev);
+
+/*
+ * Device state handlers
+ */
+#ifdef CONFIG_PM
+int rt2x00lib_suspend(struct rt2x00_dev *rt2x00dev, pm_message_t state)
+{
+	int retval;
+
+	NOTICE(rt2x00dev, "Going to sleep.\n");
+	__clear_bit(DEVICE_PRESENT, &rt2x00dev->flags);
+
+	/*
+	 * Only continue if mac80211 has open interfaces.
+	 */
+	if (!test_bit(DEVICE_STARTED, &rt2x00dev->flags))
+		goto exit;
+	__set_bit(DEVICE_STARTED_SUSPEND, &rt2x00dev->flags);
+
+	/*
+	 * Disable radio and unitialize all items
+	 * that must be recreated on resume.
+	 */
+	rt2x00mac_stop(rt2x00dev->hw);
+	rt2x00lib_uninitialize(rt2x00dev);
+	rt2x00debug_deregister(rt2x00dev);
+
+exit:
+	/*
+	 * Set device mode to sleep for power management.
+	 */
+	retval = rt2x00dev->ops->lib->set_device_state(rt2x00dev, STATE_SLEEP);
+	if (retval)
+		return retval;
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(rt2x00lib_suspend);
+
+int rt2x00lib_resume(struct rt2x00_dev *rt2x00dev)
+{
+	struct interface *intf = &rt2x00dev->interface;
+	int retval;
+
+	NOTICE(rt2x00dev, "Waking up.\n");
+	__set_bit(DEVICE_PRESENT, &rt2x00dev->flags);
+
+	/*
+	 * Open the debugfs entry.
+	 */
+	rt2x00debug_register(rt2x00dev);
+
+	/*
+	 * Only continue if mac80211 had open interfaces.
+	 */
+	if (!__test_and_clear_bit(DEVICE_STARTED_SUSPEND, &rt2x00dev->flags))
+		return 0;
+
+	/*
+	 * Reinitialize device and all active interfaces.
+	 */
+	retval = rt2x00mac_start(rt2x00dev->hw);
+	if (retval)
+		goto exit;
+
+	/*
+	 * Reconfigure device.
+	 */
+	rt2x00lib_config(rt2x00dev, &rt2x00dev->hw->conf, 1);
+	if (!rt2x00dev->hw->conf.radio_enabled)
+		rt2x00lib_disable_radio(rt2x00dev);
+
+	rt2x00lib_config_mac_addr(rt2x00dev, intf->mac);
+	rt2x00lib_config_bssid(rt2x00dev, intf->bssid);
+	rt2x00lib_config_type(rt2x00dev, intf->type);
+
+	/*
+	 * It is possible that during that mac80211 has attempted
+	 * to send frames while we were suspending or resuming.
+	 * In that case we have disabled the TX queue and should
+	 * now enable it again
+	 */
+	ieee80211_start_queues(rt2x00dev->hw);
+
+	/*
+	 * When in Master or Ad-hoc mode,
+	 * restart Beacon transmitting by faking a beacondone event.
+	 */
+	if (intf->type == IEEE80211_IF_TYPE_AP ||
+	    intf->type == IEEE80211_IF_TYPE_IBSS)
+		rt2x00lib_beacondone(rt2x00dev);
+
+	return 0;
+
+exit:
+	rt2x00lib_disable_radio(rt2x00dev);
+	rt2x00lib_uninitialize(rt2x00dev);
+	rt2x00debug_deregister(rt2x00dev);
+
+	return retval;
+}
+EXPORT_SYMBOL_GPL(rt2x00lib_resume);
+#endif /* CONFIG_PM */
+
+/*
+ * rt2x00lib module information.
+ */
+MODULE_AUTHOR(DRV_PROJECT);
+MODULE_VERSION(DRV_VERSION);
+MODULE_DESCRIPTION("rt2x00 library");
+MODULE_LICENSE("GPL");
diff --git a/drivers/net/wireless/rt2x00/rt2x00firmware.c b/drivers/net/wireless/rt2x00/rt2x00firmware.c
new file mode 100644
index 000000000000..236025f8b90f
--- /dev/null
+++ b/drivers/net/wireless/rt2x00/rt2x00firmware.c
@@ -0,0 +1,124 @@
+/*
+	Copyright (C) 2004 - 2007 rt2x00 SourceForge Project
+	<http://rt2x00.serialmonkey.com>
+
+	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, or
+	(at your option) any later version.
+
+	This program is distributed in the hope that it will be useful,
+	but WITHOUT ANY WARRANTY; without even the implied warranty of
+	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+	GNU General Public License for more details.
+
+	You should have received a copy of the GNU General Public License
+	along with this program; if not, write to the
+	Free Software Foundation, Inc.,
+	59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+/*
+	Module: rt2x00lib
+	Abstract: rt2x00 firmware loading routines.
+ */
+
+/*
+ * Set enviroment defines for rt2x00.h
+ */
+#define DRV_NAME "rt2x00lib"
+
+#include <linux/crc-itu-t.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+
+#include "rt2x00.h"
+#include "rt2x00lib.h"
+
+static int rt2x00lib_request_firmware(struct rt2x00_dev *rt2x00dev)
+{
+	struct device *device = wiphy_dev(rt2x00dev->hw->wiphy);
+	const struct firmware *fw;
+	char *fw_name;
+	int retval;
+	u16 crc;
+	u16 tmp;
+
+	/*
+	 * Read correct firmware from harddisk.
+	 */
+	fw_name = rt2x00dev->ops->lib->get_firmware_name(rt2x00dev);
+	if (!fw_name) {
+		ERROR(rt2x00dev,
+		      "Invalid firmware filename.\n"
+		      "Please file bug report to %s.\n", DRV_PROJECT);
+		return -EINVAL;
+	}
+
+	INFO(rt2x00dev, "Loading firmware file '%s'.\n", fw_name);
+
+	retval = request_firmware(&fw, fw_name, device);
+	if (retval) {
+		ERROR(rt2x00dev, "Failed to request Firmware.\n");
+		return retval;
+	}
+
+	if (!fw || !fw->size || !fw->data) {
+		ERROR(rt2x00dev, "Failed to read Firmware.\n");
+		return -ENOENT;
+	}
+
+	/*
+	 * Validate the firmware using 16 bit CRC.
+	 * The last 2 bytes of the firmware are the CRC
+	 * so substract those 2 bytes from the CRC checksum,
+	 * and set those 2 bytes to 0 when calculating CRC.
+	 */
+	tmp = 0;
+	crc = crc_itu_t(0, fw->data, fw->size - 2);
+	crc = crc_itu_t(crc, (u8 *)&tmp, 2);
+
+	if (crc != (fw->data[fw->size - 2] << 8 | fw->data[fw->size - 1])) {
+		ERROR(rt2x00dev, "Firmware CRC error.\n");
+		retval = -ENOENT;
+		goto exit;
+	}
+
+	INFO(rt2x00dev, "Firmware detected - version: %d.%d.\n",
+	     fw->data[fw->size - 4], fw->data[fw->size - 3]);
+
+	rt2x00dev->fw = fw;
+
+	return 0;
+
+exit:
+	release_firmware(fw);
+
+	return retval;
+}
+
+int rt2x00lib_load_firmware(struct rt2x00_dev *rt2x00dev)
+{
+	int retval;
+
+	if (!rt2x00dev->fw) {
+		retval = rt2x00lib_request_firmware(rt2x00dev);
+		if (retval)
+			return retval;
+	}
+
+	/*
+	 * Send firmware to the device.
+	 */
+	retval = rt2x00dev->ops->lib->load_firmware(rt2x00dev,
+						    rt2x00dev->fw->data,
+						    rt2x00dev->fw->size);
+	return retval;
+}
+
+void rt2x00lib_free_firmware(struct rt2x00_dev *rt2x00dev)
+{
+	release_firmware(rt2x00dev->fw);
+	rt2x00dev->fw = NULL;
+}
+
diff --git a/drivers/net/wireless/rt2x00/rt2x00lib.h b/drivers/net/wireless/rt2x00/rt2x00lib.h
new file mode 100644
index 000000000000..298faa9d3f61
--- /dev/null
+++ b/drivers/net/wireless/rt2x00/rt2x00lib.h
@@ -0,0 +1,119 @@
+/*
+	Copyright (C) 2004 - 2007 rt2x00 SourceForge Project
+	<http://rt2x00.serialmonkey.com>
+
+	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, or
+	(at your option) any later version.
+
+	This program is distributed in the hope that it will be useful,
+	but WITHOUT ANY WARRANTY; without even the implied warranty of
+	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+	GNU General Public License for more details.
+
+	You should have received a copy of the GNU General Public License
+	along with this program; if not, write to the
+	Free Software Foundation, Inc.,
+	59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+/*
+	Module: rt2x00lib
+	Abstract: Data structures and definitions for the rt2x00lib module.
+ */
+
+#ifndef RT2X00LIB_H
+#define RT2X00LIB_H
+
+/*
+ * Interval defines
+ * Both the link tuner as the rfkill will be called once per second.
+ */
+#define LINK_TUNE_INTERVAL	( round_jiffies(HZ) )
+#define RFKILL_POLL_INTERVAL	( 1000 )
+
+/*
+ * Radio control handlers.
+ */
+int rt2x00lib_enable_radio(struct rt2x00_dev *rt2x00dev);
+void rt2x00lib_disable_radio(struct rt2x00_dev *rt2x00dev);
+void rt2x00lib_toggle_rx(struct rt2x00_dev *rt2x00dev, enum dev_state state);
+void rt2x00lib_reset_link_tuner(struct rt2x00_dev *rt2x00dev);
+
+/*
+ * Initialization handlers.
+ */
+int rt2x00lib_initialize(struct rt2x00_dev *rt2x00dev);
+void rt2x00lib_uninitialize(struct rt2x00_dev *rt2x00dev);
+
+/*
+ * Configuration handlers.
+ */
+void rt2x00lib_config_mac_addr(struct rt2x00_dev *rt2x00dev, u8 *mac);
+void rt2x00lib_config_bssid(struct rt2x00_dev *rt2x00dev, u8 *bssid);
+void rt2x00lib_config_type(struct rt2x00_dev *rt2x00dev, const int type);
+void rt2x00lib_config(struct rt2x00_dev *rt2x00dev,
+		      struct ieee80211_conf *conf, const int force_config);
+
+/*
+ * Firmware handlers.
+ */
+#ifdef CONFIG_RT2X00_LIB_FIRMWARE
+int rt2x00lib_load_firmware(struct rt2x00_dev *rt2x00dev);
+void rt2x00lib_free_firmware(struct rt2x00_dev *rt2x00dev);
+#else
+static inline int rt2x00lib_load_firmware(struct rt2x00_dev *rt2x00dev)
+{
+	return 0;
+}
+static inline void rt2x00lib_free_firmware(struct rt2x00_dev *rt2x00dev)
+{
+}
+#endif /* CONFIG_RT2X00_LIB_FIRMWARE */
+
+/*
+ * Debugfs handlers.
+ */
+#ifdef CONFIG_RT2X00_LIB_DEBUGFS
+void rt2x00debug_register(struct rt2x00_dev *rt2x00dev);
+void rt2x00debug_deregister(struct rt2x00_dev *rt2x00dev);
+#else
+static inline void rt2x00debug_register(struct rt2x00_dev *rt2x00dev)
+{
+}
+
+static inline void rt2x00debug_deregister(struct rt2x00_dev *rt2x00dev)
+{
+}
+#endif /* CONFIG_RT2X00_LIB_DEBUGFS */
+
+/*
+ * RFkill handlers.
+ */
+#ifdef CONFIG_RT2X00_LIB_RFKILL
+int rt2x00rfkill_register(struct rt2x00_dev *rt2x00dev);
+void rt2x00rfkill_unregister(struct rt2x00_dev *rt2x00dev);
+int rt2x00rfkill_allocate(struct rt2x00_dev *rt2x00dev);
+void rt2x00rfkill_free(struct rt2x00_dev *rt2x00dev);
+#else
+static inline int rt2x00rfkill_register(struct rt2x00_dev *rt2x00dev)
+{
+	return 0;
+}
+
+static inline void rt2x00rfkill_unregister(struct rt2x00_dev *rt2x00dev)
+{
+}
+
+static inline int rt2x00rfkill_allocate(struct rt2x00_dev *rt2x00dev)
+{
+	return 0;
+}
+
+static inline void rt2x00rfkill_free(struct rt2x00_dev *rt2x00dev)
+{
+}
+#endif /* CONFIG_RT2X00_LIB_RFKILL */
+
+#endif /* RT2X00LIB_H */
diff --git a/drivers/net/wireless/rt2x00/rt2x00mac.c b/drivers/net/wireless/rt2x00/rt2x00mac.c
new file mode 100644
index 000000000000..4a6a0bd01ff1
--- /dev/null
+++ b/drivers/net/wireless/rt2x00/rt2x00mac.c
@@ -0,0 +1,438 @@
+/*
+	Copyright (C) 2004 - 2007 rt2x00 SourceForge Project
+	<http://rt2x00.serialmonkey.com>
+
+	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, or
+	(at your option) any later version.
+
+	This program is distributed in the hope that it will be useful,
+	but WITHOUT ANY WARRANTY; without even the implied warranty of
+	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+	GNU General Public License for more details.
+
+	You should have received a copy of the GNU General Public License
+	along with this program; if not, write to the
+	Free Software Foundation, Inc.,
+	59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+/*
+	Module: rt2x00mac
+	Abstract: rt2x00 generic mac80211 routines.
+ */
+
+/*
+ * Set enviroment defines for rt2x00.h
+ */
+#define DRV_NAME "rt2x00lib"
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+
+#include "rt2x00.h"
+#include "rt2x00lib.h"
+
+static int rt2x00mac_tx_rts_cts(struct rt2x00_dev *rt2x00dev,
+				struct data_ring *ring,
+				struct sk_buff *frag_skb,
+				struct ieee80211_tx_control *control)
+{
+	struct sk_buff *skb;
+	int size;
+
+	if (control->flags & IEEE80211_TXCTL_USE_CTS_PROTECT)
+		size = sizeof(struct ieee80211_cts);
+	else
+		size = sizeof(struct ieee80211_rts);
+
+	skb = dev_alloc_skb(size + rt2x00dev->hw->extra_tx_headroom);
+	if (!skb) {
+		WARNING(rt2x00dev, "Failed to create RTS/CTS frame.\n");
+		return NETDEV_TX_BUSY;
+	}
+
+	skb_reserve(skb, rt2x00dev->hw->extra_tx_headroom);
+	skb_put(skb, size);
+
+	if (control->flags & IEEE80211_TXCTL_USE_CTS_PROTECT)
+		ieee80211_ctstoself_get(rt2x00dev->hw, rt2x00dev->interface.id,
+					frag_skb->data, frag_skb->len, control,
+					(struct ieee80211_cts *)(skb->data));
+	else
+		ieee80211_rts_get(rt2x00dev->hw, rt2x00dev->interface.id,
+				  frag_skb->data, frag_skb->len, control,
+				  (struct ieee80211_rts *)(skb->data));
+
+	if (rt2x00dev->ops->lib->write_tx_data(rt2x00dev, ring, skb, control)) {
+		WARNING(rt2x00dev, "Failed to send RTS/CTS frame.\n");
+		return NETDEV_TX_BUSY;
+	}
+
+	return NETDEV_TX_OK;
+}
+
+int rt2x00mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb,
+		 struct ieee80211_tx_control *control)
+{
+	struct rt2x00_dev *rt2x00dev = hw->priv;
+	struct ieee80211_hdr *ieee80211hdr = (struct ieee80211_hdr *)skb->data;
+	struct data_ring *ring;
+	u16 frame_control;
+
+	/*
+	 * Mac80211 might be calling this function while we are trying
+	 * to remove the device or perhaps suspending it.
+	 * Note that we can only stop the TX queues inside the TX path
+	 * due to possible race conditions in mac80211.
+	 */
+	if (!test_bit(DEVICE_PRESENT, &rt2x00dev->flags)) {
+		ieee80211_stop_queues(hw);
+		return 0;
+	}
+
+	/*
+	 * Determine which ring to put packet on.
+	 */
+	ring = rt2x00lib_get_ring(rt2x00dev, control->queue);
+	if (unlikely(!ring)) {
+		ERROR(rt2x00dev,
+		      "Attempt to send packet over invalid queue %d.\n"
+		      "Please file bug report to %s.\n",
+		      control->queue, DRV_PROJECT);
+		dev_kfree_skb_any(skb);
+		return NETDEV_TX_OK;
+	}
+
+	/*
+	 * If CTS/RTS is required. and this frame is not CTS or RTS,
+	 * create and queue that frame first. But make sure we have
+	 * at least enough entries available to send this CTS/RTS
+	 * frame as well as the data frame.
+	 */
+	frame_control = le16_to_cpu(ieee80211hdr->frame_control);
+	if (!is_rts_frame(frame_control) && !is_cts_frame(frame_control) &&
+	    (control->flags & (IEEE80211_TXCTL_USE_RTS_CTS |
+			       IEEE80211_TXCTL_USE_CTS_PROTECT))) {
+		if (rt2x00_ring_free(ring) <= 1)
+			return NETDEV_TX_BUSY;
+
+		if (rt2x00mac_tx_rts_cts(rt2x00dev, ring, skb, control))
+			return NETDEV_TX_BUSY;
+	}
+
+	if (rt2x00dev->ops->lib->write_tx_data(rt2x00dev, ring, skb, control))
+		return NETDEV_TX_BUSY;
+
+	if (rt2x00dev->ops->lib->kick_tx_queue)
+		rt2x00dev->ops->lib->kick_tx_queue(rt2x00dev, control->queue);
+
+	return NETDEV_TX_OK;
+}
+EXPORT_SYMBOL_GPL(rt2x00mac_tx);
+
+int rt2x00mac_start(struct ieee80211_hw *hw)
+{
+	struct rt2x00_dev *rt2x00dev = hw->priv;
+	int status;
+
+	if (!test_bit(DEVICE_PRESENT, &rt2x00dev->flags) ||
+	    test_bit(DEVICE_STARTED, &rt2x00dev->flags))
+		return 0;
+
+	/*
+	 * If this is the first interface which is added,
+	 * we should load the firmware now.
+	 */
+	if (test_bit(DRIVER_REQUIRE_FIRMWARE, &rt2x00dev->flags)) {
+		status = rt2x00lib_load_firmware(rt2x00dev);
+		if (status)
+			return status;
+	}
+
+	/*
+	 * Initialize the device.
+	 */
+	status = rt2x00lib_initialize(rt2x00dev);
+	if (status)
+		return status;
+
+	/*
+	 * Enable radio.
+	 */
+	status = rt2x00lib_enable_radio(rt2x00dev);
+	if (status) {
+		rt2x00lib_uninitialize(rt2x00dev);
+		return status;
+	}
+
+	__set_bit(DEVICE_STARTED, &rt2x00dev->flags);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(rt2x00mac_start);
+
+void rt2x00mac_stop(struct ieee80211_hw *hw)
+{
+	struct rt2x00_dev *rt2x00dev = hw->priv;
+
+	if (!test_bit(DEVICE_PRESENT, &rt2x00dev->flags))
+		return;
+
+	/*
+	 * Perhaps we can add something smarter here,
+	 * but for now just disabling the radio should do.
+	 */
+	rt2x00lib_disable_radio(rt2x00dev);
+
+	__clear_bit(DEVICE_STARTED, &rt2x00dev->flags);
+}
+EXPORT_SYMBOL_GPL(rt2x00mac_stop);
+
+int rt2x00mac_add_interface(struct ieee80211_hw *hw,
+			    struct ieee80211_if_init_conf *conf)
+{
+	struct rt2x00_dev *rt2x00dev = hw->priv;
+	struct interface *intf = &rt2x00dev->interface;
+
+	/*
+	 * Don't allow interfaces to be added while
+	 * either the device has disappeared or when
+	 * another interface is already present.
+	 */
+	if (!test_bit(DEVICE_PRESENT, &rt2x00dev->flags) ||
+	    is_interface_present(intf))
+		return -ENOBUFS;
+
+	intf->id = conf->if_id;
+	intf->type = conf->type;
+	if (conf->type == IEEE80211_IF_TYPE_AP)
+		memcpy(&intf->bssid, conf->mac_addr, ETH_ALEN);
+	memcpy(&intf->mac, conf->mac_addr, ETH_ALEN);
+
+	/*
+	 * The MAC adddress must be configured after the device
+	 * has been initialized. Otherwise the device can reset
+	 * the MAC registers.
+	 */
+	rt2x00lib_config_mac_addr(rt2x00dev, intf->mac);
+	rt2x00lib_config_type(rt2x00dev, conf->type);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(rt2x00mac_add_interface);
+
+void rt2x00mac_remove_interface(struct ieee80211_hw *hw,
+				struct ieee80211_if_init_conf *conf)
+{
+	struct rt2x00_dev *rt2x00dev = hw->priv;
+	struct interface *intf = &rt2x00dev->interface;
+
+	/*
+	 * Don't allow interfaces to be remove while
+	 * either the device has disappeared or when
+	 * no interface is present.
+	 */
+	if (!test_bit(DEVICE_PRESENT, &rt2x00dev->flags) ||
+	    !is_interface_present(intf))
+		return;
+
+	intf->id = 0;
+	intf->type = INVALID_INTERFACE;
+	memset(&intf->bssid, 0x00, ETH_ALEN);
+	memset(&intf->mac, 0x00, ETH_ALEN);
+
+	/*
+	 * Make sure the bssid and mac address registers
+	 * are cleared to prevent false ACKing of frames.
+	 */
+	rt2x00lib_config_mac_addr(rt2x00dev, intf->mac);
+	rt2x00lib_config_bssid(rt2x00dev, intf->bssid);
+	rt2x00lib_config_type(rt2x00dev, intf->type);
+}
+EXPORT_SYMBOL_GPL(rt2x00mac_remove_interface);
+
+int rt2x00mac_config(struct ieee80211_hw *hw, struct ieee80211_conf *conf)
+{
+	struct rt2x00_dev *rt2x00dev = hw->priv;
+
+	/*
+	 * Mac80211 might be calling this function while we are trying
+	 * to remove the device or perhaps suspending it.
+	 */
+	if (!test_bit(DEVICE_PRESENT, &rt2x00dev->flags))
+		return 0;
+
+	/*
+	 * Check if we need to disable the radio,
+	 * if this is not the case, at least the RX must be disabled.
+	 */
+	if (test_bit(DEVICE_ENABLED_RADIO, &rt2x00dev->flags)) {
+		if (!conf->radio_enabled)
+			rt2x00lib_disable_radio(rt2x00dev);
+		else
+			rt2x00lib_toggle_rx(rt2x00dev, STATE_RADIO_RX_OFF);
+	}
+
+	rt2x00lib_config(rt2x00dev, conf, 0);
+
+	/*
+	 * Reenable RX only if the radio should be on.
+	 */
+	if (test_bit(DEVICE_ENABLED_RADIO, &rt2x00dev->flags))
+		rt2x00lib_toggle_rx(rt2x00dev, STATE_RADIO_RX_ON);
+	else if (conf->radio_enabled)
+		return rt2x00lib_enable_radio(rt2x00dev);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(rt2x00mac_config);
+
+int rt2x00mac_config_interface(struct ieee80211_hw *hw, int if_id,
+			       struct ieee80211_if_conf *conf)
+{
+	struct rt2x00_dev *rt2x00dev = hw->priv;
+	struct interface *intf = &rt2x00dev->interface;
+	int status;
+
+	/*
+	 * Mac80211 might be calling this function while we are trying
+	 * to remove the device or perhaps suspending it.
+	 */
+	if (!test_bit(DEVICE_PRESENT, &rt2x00dev->flags))
+		return 0;
+
+	/*
+	 * If the given type does not match the configured type,
+	 * there has been a problem.
+	 */
+	if (conf->type != intf->type)
+		return -EINVAL;
+
+	/*
+	 * If the interface does not work in master mode,
+	 * then the bssid value in the interface structure
+	 * should now be set.
+	 */
+	if (conf->type != IEEE80211_IF_TYPE_AP)
+		memcpy(&intf->bssid, conf->bssid, ETH_ALEN);
+	rt2x00lib_config_bssid(rt2x00dev, intf->bssid);
+
+	/*
+	 * We only need to initialize the beacon when master mode is enabled.
+	 */
+	if (conf->type != IEEE80211_IF_TYPE_AP || !conf->beacon)
+		return 0;
+
+	status = rt2x00dev->ops->hw->beacon_update(rt2x00dev->hw,
+						   conf->beacon,
+						   conf->beacon_control);
+	if (status)
+		dev_kfree_skb(conf->beacon);
+
+	return status;
+}
+EXPORT_SYMBOL_GPL(rt2x00mac_config_interface);
+
+int rt2x00mac_get_stats(struct ieee80211_hw *hw,
+			struct ieee80211_low_level_stats *stats)
+{
+	struct rt2x00_dev *rt2x00dev = hw->priv;
+
+	/*
+	 * The dot11ACKFailureCount, dot11RTSFailureCount and
+	 * dot11RTSSuccessCount are updated in interrupt time.
+	 * dot11FCSErrorCount is updated in the link tuner.
+	 */
+	memcpy(stats, &rt2x00dev->low_level_stats, sizeof(*stats));
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(rt2x00mac_get_stats);
+
+int rt2x00mac_get_tx_stats(struct ieee80211_hw *hw,
+			   struct ieee80211_tx_queue_stats *stats)
+{
+	struct rt2x00_dev *rt2x00dev = hw->priv;
+	unsigned int i;
+
+	for (i = 0; i < hw->queues; i++)
+		memcpy(&stats->data[i], &rt2x00dev->tx[i].stats,
+		       sizeof(rt2x00dev->tx[i].stats));
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(rt2x00mac_get_tx_stats);
+
+void rt2x00mac_erp_ie_changed(struct ieee80211_hw *hw, u8 changes,
+			      int cts_protection, int preamble)
+{
+	struct rt2x00_dev *rt2x00dev = hw->priv;
+	int short_preamble;
+	int ack_timeout;
+	int ack_consume_time;
+	int difs;
+
+	/*
+	 * We only support changing preamble mode.
+	 */
+	if (!(changes & IEEE80211_ERP_CHANGE_PREAMBLE))
+		return;
+
+	short_preamble = !preamble;
+	preamble = !!(preamble) ? PREAMBLE : SHORT_PREAMBLE;
+
+	difs = (hw->conf.flags & IEEE80211_CONF_SHORT_SLOT_TIME) ?
+		SHORT_DIFS : DIFS;
+	ack_timeout = difs + PLCP + preamble + get_duration(ACK_SIZE, 10);
+
+	ack_consume_time = SIFS + PLCP + preamble + get_duration(ACK_SIZE, 10);
+
+	if (short_preamble)
+		__set_bit(CONFIG_SHORT_PREAMBLE, &rt2x00dev->flags);
+	else
+		__clear_bit(CONFIG_SHORT_PREAMBLE, &rt2x00dev->flags);
+
+	rt2x00dev->ops->lib->config_preamble(rt2x00dev, short_preamble,
+					     ack_timeout, ack_consume_time);
+}
+EXPORT_SYMBOL_GPL(rt2x00mac_erp_ie_changed);
+
+int rt2x00mac_conf_tx(struct ieee80211_hw *hw, int queue,
+		      const struct ieee80211_tx_queue_params *params)
+{
+	struct rt2x00_dev *rt2x00dev = hw->priv;
+	struct data_ring *ring;
+
+	ring = rt2x00lib_get_ring(rt2x00dev, queue);
+	if (unlikely(!ring))
+		return -EINVAL;
+
+	/*
+	 * The passed variables are stored as real value ((2^n)-1).
+	 * Ralink registers require to know the bit number 'n'.
+	 */
+	if (params->cw_min)
+		ring->tx_params.cw_min = fls(params->cw_min);
+	else
+		ring->tx_params.cw_min = 5; /* cw_min: 2^5 = 32. */
+
+	if (params->cw_max)
+		ring->tx_params.cw_max = fls(params->cw_max);
+	else
+		ring->tx_params.cw_max = 10; /* cw_min: 2^10 = 1024. */
+
+	if (params->aifs)
+		ring->tx_params.aifs = params->aifs;
+	else
+		ring->tx_params.aifs = 2;
+
+	INFO(rt2x00dev,
+	     "Configured TX ring %d - CWmin: %d, CWmax: %d, Aifs: %d.\n",
+	     queue, ring->tx_params.cw_min, ring->tx_params.cw_max,
+	     ring->tx_params.aifs);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(rt2x00mac_conf_tx);
diff --git a/drivers/net/wireless/rt2x00/rt2x00pci.c b/drivers/net/wireless/rt2x00/rt2x00pci.c
new file mode 100644
index 000000000000..2780df00623c
--- /dev/null
+++ b/drivers/net/wireless/rt2x00/rt2x00pci.c
@@ -0,0 +1,474 @@
+/*
+	Copyright (C) 2004 - 2007 rt2x00 SourceForge Project
+	<http://rt2x00.serialmonkey.com>
+
+	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, or
+	(at your option) any later version.
+
+	This program is distributed in the hope that it will be useful,
+	but WITHOUT ANY WARRANTY; without even the implied warranty of
+	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+	GNU General Public License for more details.
+
+	You should have received a copy of the GNU General Public License
+	along with this program; if not, write to the
+	Free Software Foundation, Inc.,
+	59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+/*
+	Module: rt2x00pci
+	Abstract: rt2x00 generic pci device routines.
+ */
+
+/*
+ * Set enviroment defines for rt2x00.h
+ */
+#define DRV_NAME "rt2x00pci"
+
+#include <linux/dma-mapping.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/pci.h>
+
+#include "rt2x00.h"
+#include "rt2x00pci.h"
+
+/*
+ * Beacon handlers.
+ */
+int rt2x00pci_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb,
+			    struct ieee80211_tx_control *control)
+{
+	struct rt2x00_dev *rt2x00dev = hw->priv;
+	struct data_ring *ring =
+	    rt2x00lib_get_ring(rt2x00dev, IEEE80211_TX_QUEUE_BEACON);
+	struct data_entry *entry = rt2x00_get_data_entry(ring);
+
+	/*
+	 * Just in case mac80211 doesn't set this correctly,
+	 * but we need this queue set for the descriptor
+	 * initialization.
+	 */
+	control->queue = IEEE80211_TX_QUEUE_BEACON;
+
+	/*
+	 * Update the beacon entry.
+	 */
+	memcpy(entry->data_addr, skb->data, skb->len);
+	rt2x00lib_write_tx_desc(rt2x00dev, entry->priv,
+				(struct ieee80211_hdr *)skb->data,
+				skb->len, control);
+
+	/*
+	 * Enable beacon generation.
+	 */
+	rt2x00dev->ops->lib->kick_tx_queue(rt2x00dev, control->queue);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(rt2x00pci_beacon_update);
+
+/*
+ * TX data handlers.
+ */
+int rt2x00pci_write_tx_data(struct rt2x00_dev *rt2x00dev,
+			    struct data_ring *ring, struct sk_buff *skb,
+			    struct ieee80211_tx_control *control)
+{
+	struct ieee80211_hdr *ieee80211hdr = (struct ieee80211_hdr *)skb->data;
+	struct data_entry *entry = rt2x00_get_data_entry(ring);
+	struct data_desc *txd = entry->priv;
+	u32 word;
+
+	if (rt2x00_ring_full(ring)) {
+		ieee80211_stop_queue(rt2x00dev->hw, control->queue);
+		return -EINVAL;
+	}
+
+	rt2x00_desc_read(txd, 0, &word);
+
+	if (rt2x00_get_field32(word, TXD_ENTRY_OWNER_NIC) ||
+	    rt2x00_get_field32(word, TXD_ENTRY_VALID)) {
+		ERROR(rt2x00dev,
+		      "Arrived at non-free entry in the non-full queue %d.\n"
+		      "Please file bug report to %s.\n",
+		      control->queue, DRV_PROJECT);
+		ieee80211_stop_queue(rt2x00dev->hw, control->queue);
+		return -EINVAL;
+	}
+
+	entry->skb = skb;
+	memcpy(&entry->tx_status.control, control, sizeof(*control));
+	memcpy(entry->data_addr, skb->data, skb->len);
+	rt2x00lib_write_tx_desc(rt2x00dev, txd, ieee80211hdr,
+				skb->len, control);
+
+	rt2x00_ring_index_inc(ring);
+
+	if (rt2x00_ring_full(ring))
+		ieee80211_stop_queue(rt2x00dev->hw, control->queue);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(rt2x00pci_write_tx_data);
+
+/*
+ * RX data handlers.
+ */
+void rt2x00pci_rxdone(struct rt2x00_dev *rt2x00dev)
+{
+	struct data_ring *ring = rt2x00dev->rx;
+	struct data_entry *entry;
+	struct data_desc *rxd;
+	struct sk_buff *skb;
+	struct rxdata_entry_desc desc;
+	u32 word;
+
+	while (1) {
+		entry = rt2x00_get_data_entry(ring);
+		rxd = entry->priv;
+		rt2x00_desc_read(rxd, 0, &word);
+
+		if (rt2x00_get_field32(word, RXD_ENTRY_OWNER_NIC))
+			break;
+
+		memset(&desc, 0x00, sizeof(desc));
+		rt2x00dev->ops->lib->fill_rxdone(entry, &desc);
+
+		/*
+		 * Allocate the sk_buffer, initialize it and copy
+		 * all data into it.
+		 */
+		skb = dev_alloc_skb(desc.size + NET_IP_ALIGN);
+		if (!skb)
+			return;
+
+		skb_reserve(skb, NET_IP_ALIGN);
+		skb_put(skb, desc.size);
+		memcpy(skb->data, entry->data_addr, desc.size);
+
+		/*
+		 * Send the frame to rt2x00lib for further processing.
+		 */
+		rt2x00lib_rxdone(entry, skb, &desc);
+
+		if (test_bit(DEVICE_ENABLED_RADIO, &ring->rt2x00dev->flags)) {
+			rt2x00_set_field32(&word, RXD_ENTRY_OWNER_NIC, 1);
+			rt2x00_desc_write(rxd, 0, word);
+		}
+
+		rt2x00_ring_index_inc(ring);
+	}
+}
+EXPORT_SYMBOL_GPL(rt2x00pci_rxdone);
+
+/*
+ * Device initialization handlers.
+ */
+#define priv_offset(__ring, __i)				\
+({								\
+	ring->data_addr + (i * ring->desc_size);		\
+})
+
+#define data_addr_offset(__ring, __i)				\
+({								\
+	(__ring)->data_addr +					\
+	    ((__ring)->stats.limit * (__ring)->desc_size) +	\
+	    ((__i) * (__ring)->data_size);			\
+})
+
+#define data_dma_offset(__ring, __i)				\
+({								\
+	(__ring)->data_dma +					\
+	    ((__ring)->stats.limit * (__ring)->desc_size) +	\
+	    ((__i) * (__ring)->data_size);			\
+})
+
+static int rt2x00pci_alloc_dma(struct rt2x00_dev *rt2x00dev,
+			       struct data_ring *ring)
+{
+	unsigned int i;
+
+	/*
+	 * Allocate DMA memory for descriptor and buffer.
+	 */
+	ring->data_addr = pci_alloc_consistent(rt2x00dev_pci(rt2x00dev),
+					       rt2x00_get_ring_size(ring),
+					       &ring->data_dma);
+	if (!ring->data_addr)
+		return -ENOMEM;
+
+	/*
+	 * Initialize all ring entries to contain valid
+	 * addresses.
+	 */
+	for (i = 0; i < ring->stats.limit; i++) {
+		ring->entry[i].priv = priv_offset(ring, i);
+		ring->entry[i].data_addr = data_addr_offset(ring, i);
+		ring->entry[i].data_dma = data_dma_offset(ring, i);
+	}
+
+	return 0;
+}
+
+static void rt2x00pci_free_dma(struct rt2x00_dev *rt2x00dev,
+			       struct data_ring *ring)
+{
+	if (ring->data_addr)
+		pci_free_consistent(rt2x00dev_pci(rt2x00dev),
+				    rt2x00_get_ring_size(ring),
+				    ring->data_addr, ring->data_dma);
+	ring->data_addr = NULL;
+}
+
+int rt2x00pci_initialize(struct rt2x00_dev *rt2x00dev)
+{
+	struct pci_dev *pci_dev = rt2x00dev_pci(rt2x00dev);
+	struct data_ring *ring;
+	int status;
+
+	/*
+	 * Allocate DMA
+	 */
+	ring_for_each(rt2x00dev, ring) {
+		status = rt2x00pci_alloc_dma(rt2x00dev, ring);
+		if (status)
+			goto exit;
+	}
+
+	/*
+	 * Register interrupt handler.
+	 */
+	status = request_irq(pci_dev->irq, rt2x00dev->ops->lib->irq_handler,
+			     IRQF_SHARED, pci_name(pci_dev), rt2x00dev);
+	if (status) {
+		ERROR(rt2x00dev, "IRQ %d allocation failed (error %d).\n",
+		      pci_dev->irq, status);
+		return status;
+	}
+
+	return 0;
+
+exit:
+	rt2x00pci_uninitialize(rt2x00dev);
+
+	return status;
+}
+EXPORT_SYMBOL_GPL(rt2x00pci_initialize);
+
+void rt2x00pci_uninitialize(struct rt2x00_dev *rt2x00dev)
+{
+	struct data_ring *ring;
+
+	/*
+	 * Free irq line.
+	 */
+	free_irq(rt2x00dev_pci(rt2x00dev)->irq, rt2x00dev);
+
+	/*
+	 * Free DMA
+	 */
+	ring_for_each(rt2x00dev, ring)
+		rt2x00pci_free_dma(rt2x00dev, ring);
+}
+EXPORT_SYMBOL_GPL(rt2x00pci_uninitialize);
+
+/*
+ * PCI driver handlers.
+ */
+static void rt2x00pci_free_reg(struct rt2x00_dev *rt2x00dev)
+{
+	kfree(rt2x00dev->rf);
+	rt2x00dev->rf = NULL;
+
+	kfree(rt2x00dev->eeprom);
+	rt2x00dev->eeprom = NULL;
+
+	if (rt2x00dev->csr_addr) {
+		iounmap(rt2x00dev->csr_addr);
+		rt2x00dev->csr_addr = NULL;
+	}
+}
+
+static int rt2x00pci_alloc_reg(struct rt2x00_dev *rt2x00dev)
+{
+	struct pci_dev *pci_dev = rt2x00dev_pci(rt2x00dev);
+
+	rt2x00dev->csr_addr = ioremap(pci_resource_start(pci_dev, 0),
+				      pci_resource_len(pci_dev, 0));
+	if (!rt2x00dev->csr_addr)
+		goto exit;
+
+	rt2x00dev->eeprom = kzalloc(rt2x00dev->ops->eeprom_size, GFP_KERNEL);
+	if (!rt2x00dev->eeprom)
+		goto exit;
+
+	rt2x00dev->rf = kzalloc(rt2x00dev->ops->rf_size, GFP_KERNEL);
+	if (!rt2x00dev->rf)
+		goto exit;
+
+	return 0;
+
+exit:
+	ERROR_PROBE("Failed to allocate registers.\n");
+
+	rt2x00pci_free_reg(rt2x00dev);
+
+	return -ENOMEM;
+}
+
+int rt2x00pci_probe(struct pci_dev *pci_dev, const struct pci_device_id *id)
+{
+	struct rt2x00_ops *ops = (struct rt2x00_ops *)id->driver_data;
+	struct ieee80211_hw *hw;
+	struct rt2x00_dev *rt2x00dev;
+	int retval;
+
+	retval = pci_request_regions(pci_dev, pci_name(pci_dev));
+	if (retval) {
+		ERROR_PROBE("PCI request regions failed.\n");
+		return retval;
+	}
+
+	retval = pci_enable_device(pci_dev);
+	if (retval) {
+		ERROR_PROBE("Enable device failed.\n");
+		goto exit_release_regions;
+	}
+
+	pci_set_master(pci_dev);
+
+	if (pci_set_mwi(pci_dev))
+		ERROR_PROBE("MWI not available.\n");
+
+	if (pci_set_dma_mask(pci_dev, DMA_64BIT_MASK) &&
+	    pci_set_dma_mask(pci_dev, DMA_32BIT_MASK)) {
+		ERROR_PROBE("PCI DMA not supported.\n");
+		retval = -EIO;
+		goto exit_disable_device;
+	}
+
+	hw = ieee80211_alloc_hw(sizeof(struct rt2x00_dev), ops->hw);
+	if (!hw) {
+		ERROR_PROBE("Failed to allocate hardware.\n");
+		retval = -ENOMEM;
+		goto exit_disable_device;
+	}
+
+	pci_set_drvdata(pci_dev, hw);
+
+	rt2x00dev = hw->priv;
+	rt2x00dev->dev = pci_dev;
+	rt2x00dev->ops = ops;
+	rt2x00dev->hw = hw;
+
+	retval = rt2x00pci_alloc_reg(rt2x00dev);
+	if (retval)
+		goto exit_free_device;
+
+	retval = rt2x00lib_probe_dev(rt2x00dev);
+	if (retval)
+		goto exit_free_reg;
+
+	return 0;
+
+exit_free_reg:
+	rt2x00pci_free_reg(rt2x00dev);
+
+exit_free_device:
+	ieee80211_free_hw(hw);
+
+exit_disable_device:
+	if (retval != -EBUSY)
+		pci_disable_device(pci_dev);
+
+exit_release_regions:
+	pci_release_regions(pci_dev);
+
+	pci_set_drvdata(pci_dev, NULL);
+
+	return retval;
+}
+EXPORT_SYMBOL_GPL(rt2x00pci_probe);
+
+void rt2x00pci_remove(struct pci_dev *pci_dev)
+{
+	struct ieee80211_hw *hw = pci_get_drvdata(pci_dev);
+	struct rt2x00_dev *rt2x00dev = hw->priv;
+
+	/*
+	 * Free all allocated data.
+	 */
+	rt2x00lib_remove_dev(rt2x00dev);
+	rt2x00pci_free_reg(rt2x00dev);
+	ieee80211_free_hw(hw);
+
+	/*
+	 * Free the PCI device data.
+	 */
+	pci_set_drvdata(pci_dev, NULL);
+	pci_disable_device(pci_dev);
+	pci_release_regions(pci_dev);
+}
+EXPORT_SYMBOL_GPL(rt2x00pci_remove);
+
+#ifdef CONFIG_PM
+int rt2x00pci_suspend(struct pci_dev *pci_dev, pm_message_t state)
+{
+	struct ieee80211_hw *hw = pci_get_drvdata(pci_dev);
+	struct rt2x00_dev *rt2x00dev = hw->priv;
+	int retval;
+
+	retval = rt2x00lib_suspend(rt2x00dev, state);
+	if (retval)
+		return retval;
+
+	rt2x00pci_free_reg(rt2x00dev);
+
+	pci_save_state(pci_dev);
+	pci_disable_device(pci_dev);
+	return pci_set_power_state(pci_dev, pci_choose_state(pci_dev, state));
+}
+EXPORT_SYMBOL_GPL(rt2x00pci_suspend);
+
+int rt2x00pci_resume(struct pci_dev *pci_dev)
+{
+	struct ieee80211_hw *hw = pci_get_drvdata(pci_dev);
+	struct rt2x00_dev *rt2x00dev = hw->priv;
+	int retval;
+
+	if (pci_set_power_state(pci_dev, PCI_D0) ||
+	    pci_enable_device(pci_dev) ||
+	    pci_restore_state(pci_dev)) {
+		ERROR(rt2x00dev, "Failed to resume device.\n");
+		return -EIO;
+	}
+
+	retval = rt2x00pci_alloc_reg(rt2x00dev);
+	if (retval)
+		return retval;
+
+	retval = rt2x00lib_resume(rt2x00dev);
+	if (retval)
+		goto exit_free_reg;
+
+	return 0;
+
+exit_free_reg:
+	rt2x00pci_free_reg(rt2x00dev);
+
+	return retval;
+}
+EXPORT_SYMBOL_GPL(rt2x00pci_resume);
+#endif /* CONFIG_PM */
+
+/*
+ * rt2x00pci module information.
+ */
+MODULE_AUTHOR(DRV_PROJECT);
+MODULE_VERSION(DRV_VERSION);
+MODULE_DESCRIPTION("rt2x00 library");
+MODULE_LICENSE("GPL");
diff --git a/drivers/net/wireless/rt2x00/rt2x00pci.h b/drivers/net/wireless/rt2x00/rt2x00pci.h
new file mode 100644
index 000000000000..82adeac061d0
--- /dev/null
+++ b/drivers/net/wireless/rt2x00/rt2x00pci.h
@@ -0,0 +1,127 @@
+/*
+	Copyright (C) 2004 - 2007 rt2x00 SourceForge Project
+	<http://rt2x00.serialmonkey.com>
+
+	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, or
+	(at your option) any later version.
+
+	This program is distributed in the hope that it will be useful,
+	but WITHOUT ANY WARRANTY; without even the implied warranty of
+	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+	GNU General Public License for more details.
+
+	You should have received a copy of the GNU General Public License
+	along with this program; if not, write to the
+	Free Software Foundation, Inc.,
+	59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+/*
+	Module: rt2x00pci
+	Abstract: Data structures for the rt2x00pci module.
+ */
+
+#ifndef RT2X00PCI_H
+#define RT2X00PCI_H
+
+#include <linux/io.h>
+
+/*
+ * This variable should be used with the
+ * pci_driver structure initialization.
+ */
+#define PCI_DEVICE_DATA(__ops)	.driver_data = (kernel_ulong_t)(__ops)
+
+/*
+ * Register defines.
+ * Some registers require multiple attempts before success,
+ * in those cases REGISTER_BUSY_COUNT attempts should be
+ * taken with a REGISTER_BUSY_DELAY interval.
+ */
+#define REGISTER_BUSY_COUNT	5
+#define REGISTER_BUSY_DELAY	100
+
+/*
+ * Descriptor availability flags.
+ * All PCI device descriptors have these 2 flags
+ * with the exact same definition.
+ * By storing them here we can use them inside rt2x00pci
+ * for some simple entry availability checking.
+ */
+#define TXD_ENTRY_OWNER_NIC	FIELD32(0x00000001)
+#define TXD_ENTRY_VALID		FIELD32(0x00000002)
+#define RXD_ENTRY_OWNER_NIC	FIELD32(0x00000001)
+
+/*
+ * Register access.
+ */
+static inline void rt2x00pci_register_read(const struct rt2x00_dev *rt2x00dev,
+					   const unsigned long offset,
+					   u32 *value)
+{
+	*value = readl(rt2x00dev->csr_addr + offset);
+}
+
+static inline void
+rt2x00pci_register_multiread(const struct rt2x00_dev *rt2x00dev,
+			     const unsigned long offset,
+			     void *value, const u16 length)
+{
+	memcpy_fromio(value, rt2x00dev->csr_addr + offset, length);
+}
+
+static inline void rt2x00pci_register_write(const struct rt2x00_dev *rt2x00dev,
+					    const unsigned long offset,
+					    u32 value)
+{
+	writel(value, rt2x00dev->csr_addr + offset);
+}
+
+static inline void
+rt2x00pci_register_multiwrite(const struct rt2x00_dev *rt2x00dev,
+			      const unsigned long offset,
+			      void *value, const u16 length)
+{
+	memcpy_toio(rt2x00dev->csr_addr + offset, value, length);
+}
+
+/*
+ * Beacon handlers.
+ */
+int rt2x00pci_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb,
+			    struct ieee80211_tx_control *control);
+
+/*
+ * TX data handlers.
+ */
+int rt2x00pci_write_tx_data(struct rt2x00_dev *rt2x00dev,
+			    struct data_ring *ring, struct sk_buff *skb,
+			    struct ieee80211_tx_control *control);
+
+/*
+ * RX data handlers.
+ */
+void rt2x00pci_rxdone(struct rt2x00_dev *rt2x00dev);
+
+/*
+ * Device initialization handlers.
+ */
+int rt2x00pci_initialize(struct rt2x00_dev *rt2x00dev);
+void rt2x00pci_uninitialize(struct rt2x00_dev *rt2x00dev);
+
+/*
+ * PCI driver handlers.
+ */
+int rt2x00pci_probe(struct pci_dev *pci_dev, const struct pci_device_id *id);
+void rt2x00pci_remove(struct pci_dev *pci_dev);
+#ifdef CONFIG_PM
+int rt2x00pci_suspend(struct pci_dev *pci_dev, pm_message_t state);
+int rt2x00pci_resume(struct pci_dev *pci_dev);
+#else
+#define rt2x00pci_suspend	NULL
+#define rt2x00pci_resume	NULL
+#endif /* CONFIG_PM */
+
+#endif /* RT2X00PCI_H */
diff --git a/drivers/net/wireless/rt2x00/rt2x00reg.h b/drivers/net/wireless/rt2x00/rt2x00reg.h
new file mode 100644
index 000000000000..838421216da0
--- /dev/null
+++ b/drivers/net/wireless/rt2x00/rt2x00reg.h
@@ -0,0 +1,292 @@
+/*
+	Copyright (C) 2004 - 2007 rt2x00 SourceForge Project
+	<http://rt2x00.serialmonkey.com>
+
+	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, or
+	(at your option) any later version.
+
+	This program is distributed in the hope that it will be useful,
+	but WITHOUT ANY WARRANTY; without even the implied warranty of
+	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+	GNU General Public License for more details.
+
+	You should have received a copy of the GNU General Public License
+	along with this program; if not, write to the
+	Free Software Foundation, Inc.,
+	59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+/*
+	Module: rt2x00
+	Abstract: rt2x00 generic register information.
+ */
+
+#ifndef RT2X00REG_H
+#define RT2X00REG_H
+
+/*
+ * TX result flags.
+ */
+enum TX_STATUS {
+	TX_SUCCESS = 0,
+	TX_SUCCESS_RETRY = 1,
+	TX_FAIL_RETRY = 2,
+	TX_FAIL_INVALID = 3,
+	TX_FAIL_OTHER = 4,
+};
+
+/*
+ * Antenna values
+ */
+enum antenna {
+	ANTENNA_SW_DIVERSITY = 0,
+	ANTENNA_A = 1,
+	ANTENNA_B = 2,
+	ANTENNA_HW_DIVERSITY = 3,
+};
+
+/*
+ * Led mode values.
+ */
+enum led_mode {
+	LED_MODE_DEFAULT = 0,
+	LED_MODE_TXRX_ACTIVITY = 1,
+	LED_MODE_SIGNAL_STRENGTH = 2,
+	LED_MODE_ASUS = 3,
+	LED_MODE_ALPHA = 4,
+};
+
+/*
+ * TSF sync values
+ */
+enum tsf_sync {
+	TSF_SYNC_NONE = 0,
+	TSF_SYNC_INFRA = 1,
+	TSF_SYNC_BEACON = 2,
+};
+
+/*
+ * Device states
+ */
+enum dev_state {
+	STATE_DEEP_SLEEP = 0,
+	STATE_SLEEP = 1,
+	STATE_STANDBY = 2,
+	STATE_AWAKE = 3,
+
+/*
+ * Additional device states, these values are
+ * not strict since they are not directly passed
+ * into the device.
+ */
+	STATE_RADIO_ON,
+	STATE_RADIO_OFF,
+	STATE_RADIO_RX_ON,
+	STATE_RADIO_RX_OFF,
+	STATE_RADIO_IRQ_ON,
+	STATE_RADIO_IRQ_OFF,
+};
+
+/*
+ * IFS backoff values
+ */
+enum ifs {
+	IFS_BACKOFF = 0,
+	IFS_SIFS = 1,
+	IFS_NEW_BACKOFF = 2,
+	IFS_NONE = 3,
+};
+
+/*
+ * Cipher types for hardware encryption
+ */
+enum cipher {
+	CIPHER_NONE = 0,
+	CIPHER_WEP64 = 1,
+	CIPHER_WEP128 = 2,
+	CIPHER_TKIP = 3,
+	CIPHER_AES = 4,
+/*
+ * The following fields were added by rt61pci and rt73usb.
+ */
+	CIPHER_CKIP64 = 5,
+	CIPHER_CKIP128 = 6,
+	CIPHER_TKIP_NO_MIC = 7,
+};
+
+/*
+ * Register handlers.
+ * We store the position of a register field inside a field structure,
+ * This will simplify the process of setting and reading a certain field
+ * inside the register while making sure the process remains byte order safe.
+ */
+struct rt2x00_field8 {
+	u8 bit_offset;
+	u8 bit_mask;
+};
+
+struct rt2x00_field16 {
+	u16 bit_offset;
+	u16 bit_mask;
+};
+
+struct rt2x00_field32 {
+	u32 bit_offset;
+	u32 bit_mask;
+};
+
+/*
+ * Power of two check, this will check
+ * if the mask that has been given contains
+ * and contiguous set of bits.
+ */
+#define is_power_of_two(x)	( !((x) & ((x)-1)) )
+#define low_bit_mask(x)		( ((x)-1) & ~(x) )
+#define is_valid_mask(x)	is_power_of_two(1 + (x) + low_bit_mask(x))
+
+#define FIELD8(__mask)				\
+({						\
+	BUILD_BUG_ON(!(__mask) ||		\
+		     !is_valid_mask(__mask) ||	\
+		     (__mask) != (u8)(__mask));	\
+	(struct rt2x00_field8) {		\
+		__ffs(__mask), (__mask)		\
+	};					\
+})
+
+#define FIELD16(__mask)				\
+({						\
+	BUILD_BUG_ON(!(__mask) ||		\
+		     !is_valid_mask(__mask) ||	\
+		     (__mask) != (u16)(__mask));\
+	(struct rt2x00_field16) {		\
+		__ffs(__mask), (__mask)		\
+	};					\
+})
+
+#define FIELD32(__mask)				\
+({						\
+	BUILD_BUG_ON(!(__mask) ||		\
+		     !is_valid_mask(__mask) ||	\
+		     (__mask) != (u32)(__mask));\
+	(struct rt2x00_field32) {		\
+		__ffs(__mask), (__mask)		\
+	};					\
+})
+
+static inline void rt2x00_set_field32(u32 *reg,
+				      const struct rt2x00_field32 field,
+				      const u32 value)
+{
+	*reg &= ~(field.bit_mask);
+	*reg |= (value << field.bit_offset) & field.bit_mask;
+}
+
+static inline u32 rt2x00_get_field32(const u32 reg,
+				     const struct rt2x00_field32 field)
+{
+	return (reg & field.bit_mask) >> field.bit_offset;
+}
+
+static inline void rt2x00_set_field16(u16 *reg,
+				      const struct rt2x00_field16 field,
+				      const u16 value)
+{
+	*reg &= ~(field.bit_mask);
+	*reg |= (value << field.bit_offset) & field.bit_mask;
+}
+
+static inline u16 rt2x00_get_field16(const u16 reg,
+				     const struct rt2x00_field16 field)
+{
+	return (reg & field.bit_mask) >> field.bit_offset;
+}
+
+static inline void rt2x00_set_field8(u8 *reg,
+				     const struct rt2x00_field8 field,
+				     const u8 value)
+{
+	*reg &= ~(field.bit_mask);
+	*reg |= (value << field.bit_offset) & field.bit_mask;
+}
+
+static inline u8 rt2x00_get_field8(const u8 reg,
+				   const struct rt2x00_field8 field)
+{
+	return (reg & field.bit_mask) >> field.bit_offset;
+}
+
+/*
+ * Device specific rate value.
+ * We will have to create the device specific rate value
+ * passed to the ieee80211 kernel. We need to make it a consist of
+ * multiple fields because we want to store more then 1 device specific
+ * values inside the value.
+ *	1 - rate, stored as 100 kbit/s.
+ *	2 - preamble, short_preamble enabled flag.
+ *	3 - MASK_RATE, which rates are enabled in this mode, this mask
+ *	corresponds with the TX register format for the current device.
+ *	4 - plcp, 802.11b rates are device specific,
+ *	802.11g rates are set according to the ieee802.11a-1999 p.14.
+ * The bit to enable preamble is set in a seperate define.
+ */
+#define DEV_RATE	FIELD32(0x000007ff)
+#define DEV_PREAMBLE	FIELD32(0x00000800)
+#define DEV_RATEMASK	FIELD32(0x00fff000)
+#define DEV_PLCP	FIELD32(0xff000000)
+
+/*
+ * Bitfields
+ */
+#define DEV_RATEBIT_1MB		( 1 << 0 )
+#define DEV_RATEBIT_2MB		( 1 << 1 )
+#define DEV_RATEBIT_5_5MB	( 1 << 2 )
+#define DEV_RATEBIT_11MB	( 1 << 3 )
+#define DEV_RATEBIT_6MB		( 1 << 4 )
+#define DEV_RATEBIT_9MB		( 1 << 5 )
+#define DEV_RATEBIT_12MB	( 1 << 6 )
+#define DEV_RATEBIT_18MB	( 1 << 7 )
+#define DEV_RATEBIT_24MB	( 1 << 8 )
+#define DEV_RATEBIT_36MB	( 1 << 9 )
+#define DEV_RATEBIT_48MB	( 1 << 10 )
+#define DEV_RATEBIT_54MB	( 1 << 11 )
+
+/*
+ * Bitmasks for DEV_RATEMASK
+ */
+#define DEV_RATEMASK_1MB	( (DEV_RATEBIT_1MB << 1) -1 )
+#define DEV_RATEMASK_2MB	( (DEV_RATEBIT_2MB << 1) -1 )
+#define DEV_RATEMASK_5_5MB	( (DEV_RATEBIT_5_5MB << 1) -1 )
+#define DEV_RATEMASK_11MB	( (DEV_RATEBIT_11MB << 1) -1 )
+#define DEV_RATEMASK_6MB	( (DEV_RATEBIT_6MB << 1) -1 )
+#define DEV_RATEMASK_9MB	( (DEV_RATEBIT_9MB << 1) -1 )
+#define DEV_RATEMASK_12MB	( (DEV_RATEBIT_12MB << 1) -1 )
+#define DEV_RATEMASK_18MB	( (DEV_RATEBIT_18MB << 1) -1 )
+#define DEV_RATEMASK_24MB	( (DEV_RATEBIT_24MB << 1) -1 )
+#define DEV_RATEMASK_36MB	( (DEV_RATEBIT_36MB << 1) -1 )
+#define DEV_RATEMASK_48MB	( (DEV_RATEBIT_48MB << 1) -1 )
+#define DEV_RATEMASK_54MB	( (DEV_RATEBIT_54MB << 1) -1 )
+
+/*
+ * Bitmask groups of bitrates
+ */
+#define DEV_BASIC_RATEMASK \
+	( DEV_RATEMASK_11MB | \
+	  DEV_RATEBIT_6MB | DEV_RATEBIT_12MB | DEV_RATEBIT_24MB )
+
+#define DEV_CCK_RATEMASK	( DEV_RATEMASK_11MB )
+#define DEV_OFDM_RATEMASK	( DEV_RATEMASK_54MB & ~DEV_CCK_RATEMASK )
+
+/*
+ * Macro's to set and get specific fields from the device specific val and val2
+ * fields inside the ieee80211_rate entry.
+ */
+#define DEVICE_SET_RATE_FIELD(__value, __mask) \
+	(int)( ((__value) << DEV_##__mask.bit_offset) & DEV_##__mask.bit_mask )
+
+#define DEVICE_GET_RATE_FIELD(__value, __mask) \
+	(int)( ((__value) & DEV_##__mask.bit_mask) >> DEV_##__mask.bit_offset )
+
+#endif /* RT2X00REG_H */
diff --git a/drivers/net/wireless/rt2x00/rt2x00rfkill.c b/drivers/net/wireless/rt2x00/rt2x00rfkill.c
new file mode 100644
index 000000000000..a0f8b8e0a24b
--- /dev/null
+++ b/drivers/net/wireless/rt2x00/rt2x00rfkill.c
@@ -0,0 +1,146 @@
+/*
+	Copyright (C) 2004 - 2007 rt2x00 SourceForge Project
+	<http://rt2x00.serialmonkey.com>
+
+	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, or
+	(at your option) any later version.
+
+	This program is distributed in the hope that it will be useful,
+	but WITHOUT ANY WARRANTY; without even the implied warranty of
+	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+	GNU General Public License for more details.
+
+	You should have received a copy of the GNU General Public License
+	along with this program; if not, write to the
+	Free Software Foundation, Inc.,
+	59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+/*
+	Module: rt2x00rfkill
+	Abstract: rt2x00 rfkill routines.
+ */
+
+/*
+ * Set enviroment defines for rt2x00.h
+ */
+#define DRV_NAME "rt2x00lib"
+
+#include <linux/input-polldev.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/rfkill.h>
+
+#include "rt2x00.h"
+#include "rt2x00lib.h"
+
+static int rt2x00rfkill_toggle_radio(void *data, enum rfkill_state state)
+{
+	struct rt2x00_dev *rt2x00dev = data;
+	int retval = 0;
+
+	if (unlikely(!rt2x00dev))
+		return 0;
+
+	/*
+	 * Only continue if there are enabled interfaces.
+	 */
+	if (!test_bit(DEVICE_STARTED, &rt2x00dev->flags))
+		return 0;
+
+	if (state == RFKILL_STATE_ON) {
+		INFO(rt2x00dev, "Hardware button pressed, enabling radio.\n");
+		__clear_bit(DEVICE_DISABLED_RADIO_HW, &rt2x00dev->flags);
+		retval = rt2x00lib_enable_radio(rt2x00dev);
+	} else if (state == RFKILL_STATE_OFF) {
+		INFO(rt2x00dev, "Hardware button pressed, disabling radio.\n");
+		__set_bit(DEVICE_DISABLED_RADIO_HW, &rt2x00dev->flags);
+		rt2x00lib_disable_radio(rt2x00dev);
+	}
+
+	return retval;
+}
+
+static void rt2x00rfkill_poll(struct input_polled_dev *poll_dev)
+{
+	struct rt2x00_dev *rt2x00dev = poll_dev->private;
+	int state = rt2x00dev->ops->lib->rfkill_poll(rt2x00dev);
+
+	if (rt2x00dev->rfkill->state != state)
+		input_report_key(poll_dev->input, KEY_WLAN, 1);
+}
+
+int rt2x00rfkill_register(struct rt2x00_dev *rt2x00dev)
+{
+	int retval;
+
+	if (!test_bit(CONFIG_SUPPORT_HW_BUTTON, &rt2x00dev->flags))
+		return 0;
+
+	retval = rfkill_register(rt2x00dev->rfkill);
+	if (retval) {
+		ERROR(rt2x00dev, "Failed to register rfkill handler.\n");
+		return retval;
+	}
+
+	retval = input_register_polled_device(rt2x00dev->poll_dev);
+	if (retval) {
+		ERROR(rt2x00dev, "Failed to register polled device.\n");
+		rfkill_unregister(rt2x00dev->rfkill);
+		return retval;
+	}
+
+	return 0;
+}
+
+void rt2x00rfkill_unregister(struct rt2x00_dev *rt2x00dev)
+{
+	if (!test_bit(CONFIG_SUPPORT_HW_BUTTON, &rt2x00dev->flags))
+		return;
+
+	input_unregister_polled_device(rt2x00dev->poll_dev);
+	rfkill_unregister(rt2x00dev->rfkill);
+}
+
+int rt2x00rfkill_allocate(struct rt2x00_dev *rt2x00dev)
+{
+	struct device *device = wiphy_dev(rt2x00dev->hw->wiphy);
+
+	if (!test_bit(CONFIG_SUPPORT_HW_BUTTON, &rt2x00dev->flags))
+		return 0;
+
+	rt2x00dev->rfkill = rfkill_allocate(device, RFKILL_TYPE_WLAN);
+	if (!rt2x00dev->rfkill) {
+		ERROR(rt2x00dev, "Failed to allocate rfkill handler.\n");
+		return -ENOMEM;
+	}
+
+	rt2x00dev->rfkill->name = rt2x00dev->ops->name;
+	rt2x00dev->rfkill->data = rt2x00dev;
+	rt2x00dev->rfkill->state = rt2x00dev->ops->lib->rfkill_poll(rt2x00dev);
+	rt2x00dev->rfkill->toggle_radio = rt2x00rfkill_toggle_radio;
+
+	rt2x00dev->poll_dev = input_allocate_polled_device();
+	if (!rt2x00dev->poll_dev) {
+		ERROR(rt2x00dev, "Failed to allocate polled device.\n");
+		rfkill_free(rt2x00dev->rfkill);
+		return -ENOMEM;
+	}
+
+	rt2x00dev->poll_dev->private = rt2x00dev;
+	rt2x00dev->poll_dev->poll = rt2x00rfkill_poll;
+	rt2x00dev->poll_dev->poll_interval = RFKILL_POLL_INTERVAL;
+
+	return 0;
+}
+
+void rt2x00rfkill_free(struct rt2x00_dev *rt2x00dev)
+{
+	if (!test_bit(CONFIG_SUPPORT_HW_BUTTON, &rt2x00dev->flags))
+		return;
+
+	input_free_polled_device(rt2x00dev->poll_dev);
+	rfkill_free(rt2x00dev->rfkill);
+}
diff --git a/drivers/net/wireless/rt2x00/rt2x00ring.h b/drivers/net/wireless/rt2x00/rt2x00ring.h
new file mode 100644
index 000000000000..1a864d32cfbd
--- /dev/null
+++ b/drivers/net/wireless/rt2x00/rt2x00ring.h
@@ -0,0 +1,268 @@
+/*
+	Copyright (C) 2004 - 2007 rt2x00 SourceForge Project
+	<http://rt2x00.serialmonkey.com>
+
+	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, or
+	(at your option) any later version.
+
+	This program is distributed in the hope that it will be useful,
+	but WITHOUT ANY WARRANTY; without even the implied warranty of
+	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+	GNU General Public License for more details.
+
+	You should have received a copy of the GNU General Public License
+	along with this program; if not, write to the
+	Free Software Foundation, Inc.,
+	59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+/*
+	Module: rt2x00
+	Abstract: rt2x00 ring datastructures and routines
+ */
+
+#ifndef RT2X00RING_H
+#define RT2X00RING_H
+
+/*
+ * data_desc
+ * Each data entry also contains a descriptor which is used by the
+ * device to determine what should be done with the packet and
+ * what the current status is.
+ * This structure is greatly simplified, but the descriptors
+ * are basically a list of little endian 32 bit values.
+ * Make the array by default 1 word big, this will allow us
+ * to use sizeof() correctly.
+ */
+struct data_desc {
+	__le32 word[1];
+};
+
+/*
+ * rxdata_entry_desc
+ * Summary of information that has been read from the
+ * RX frame descriptor.
+ */
+struct rxdata_entry_desc {
+	int signal;
+	int rssi;
+	int ofdm;
+	int size;
+	int flags;
+};
+
+/*
+ * txdata_entry_desc
+ * Summary of information that should be written into the
+ * descriptor for sending a TX frame.
+ */
+struct txdata_entry_desc {
+	unsigned long flags;
+#define ENTRY_TXDONE		1
+#define ENTRY_TXD_RTS_FRAME	2
+#define ENTRY_TXD_OFDM_RATE	3
+#define ENTRY_TXD_MORE_FRAG	4
+#define ENTRY_TXD_REQ_TIMESTAMP	5
+#define ENTRY_TXD_BURST		6
+
+/*
+ * Queue ID. ID's 0-4 are data TX rings
+ */
+	int queue;
+#define QUEUE_MGMT		13
+#define QUEUE_RX		14
+#define QUEUE_OTHER		15
+
+	/*
+	 * PLCP values.
+	 */
+	u16 length_high;
+	u16 length_low;
+	u16 signal;
+	u16 service;
+
+	/*
+	 * Timing information
+	 */
+	int aifs;
+	int ifs;
+	int cw_min;
+	int cw_max;
+};
+
+/*
+ * data_entry
+ * The data ring is a list of data entries.
+ * Each entry holds a reference to the descriptor
+ * and the data buffer. For TX rings the reference to the
+ * sk_buff of the packet being transmitted is also stored here.
+ */
+struct data_entry {
+	/*
+	 * Status flags
+	 */
+	unsigned long flags;
+#define ENTRY_OWNER_NIC		1
+
+	/*
+	 * Ring we belong to.
+	 */
+	struct data_ring *ring;
+
+	/*
+	 * sk_buff for the packet which is being transmitted
+	 * in this entry (Only used with TX related rings).
+	 */
+	struct sk_buff *skb;
+
+	/*
+	 * Store a ieee80211_tx_status structure in each
+	 * ring entry, this will optimize the txdone
+	 * handler.
+	 */
+	struct ieee80211_tx_status tx_status;
+
+	/*
+	 * private pointer specific to driver.
+	 */
+	void *priv;
+
+	/*
+	 * Data address for this entry.
+	 */
+	void *data_addr;
+	dma_addr_t data_dma;
+};
+
+/*
+ * data_ring
+ * Data rings are used by the device to send and receive packets.
+ * The data_addr is the base address of the data memory.
+ * To determine at which point in the ring we are,
+ * have to use the rt2x00_ring_index_*() functions.
+ */
+struct data_ring {
+	/*
+	 * Pointer to main rt2x00dev structure where this
+	 * ring belongs to.
+	 */
+	struct rt2x00_dev *rt2x00dev;
+
+	/*
+	 * Base address for the device specific data entries.
+	 */
+	struct data_entry *entry;
+
+	/*
+	 * TX queue statistic info.
+	 */
+	struct ieee80211_tx_queue_stats_data stats;
+
+	/*
+	 * TX Queue parameters.
+	 */
+	struct ieee80211_tx_queue_params tx_params;
+
+	/*
+	 * Base address for data ring.
+	 */
+	dma_addr_t data_dma;
+	void *data_addr;
+
+	/*
+	 * Index variables.
+	 */
+	u16 index;
+	u16 index_done;
+
+	/*
+	 * Size of packet and descriptor in bytes.
+	 */
+	u16 data_size;
+	u16 desc_size;
+};
+
+/*
+ * Handlers to determine the address of the current device specific
+ * data entry, where either index or index_done points to.
+ */
+static inline struct data_entry *rt2x00_get_data_entry(struct data_ring *ring)
+{
+	return &ring->entry[ring->index];
+}
+
+static inline struct data_entry *rt2x00_get_data_entry_done(struct data_ring
+							    *ring)
+{
+	return &ring->entry[ring->index_done];
+}
+
+/*
+ * Total ring memory
+ */
+static inline int rt2x00_get_ring_size(struct data_ring *ring)
+{
+	return ring->stats.limit * (ring->desc_size + ring->data_size);
+}
+
+/*
+ * Ring index manipulation functions.
+ */
+static inline void rt2x00_ring_index_inc(struct data_ring *ring)
+{
+	ring->index++;
+	if (ring->index >= ring->stats.limit)
+		ring->index = 0;
+	ring->stats.len++;
+}
+
+static inline void rt2x00_ring_index_done_inc(struct data_ring *ring)
+{
+	ring->index_done++;
+	if (ring->index_done >= ring->stats.limit)
+		ring->index_done = 0;
+	ring->stats.len--;
+	ring->stats.count++;
+}
+
+static inline void rt2x00_ring_index_clear(struct data_ring *ring)
+{
+	ring->index = 0;
+	ring->index_done = 0;
+	ring->stats.len = 0;
+	ring->stats.count = 0;
+}
+
+static inline int rt2x00_ring_empty(struct data_ring *ring)
+{
+	return ring->stats.len == 0;
+}
+
+static inline int rt2x00_ring_full(struct data_ring *ring)
+{
+	return ring->stats.len == ring->stats.limit;
+}
+
+static inline int rt2x00_ring_free(struct data_ring *ring)
+{
+	return ring->stats.limit - ring->stats.len;
+}
+
+/*
+ * TX/RX Descriptor access functions.
+ */
+static inline void rt2x00_desc_read(struct data_desc *desc,
+				    const u8 word, u32 *value)
+{
+	*value = le32_to_cpu(desc->word[word]);
+}
+
+static inline void rt2x00_desc_write(struct data_desc *desc,
+				     const u8 word, const u32 value)
+{
+	desc->word[word] = cpu_to_le32(value);
+}
+
+#endif /* RT2X00RING_H */
diff --git a/drivers/net/wireless/rt2x00/rt2x00usb.c b/drivers/net/wireless/rt2x00/rt2x00usb.c
new file mode 100644
index 000000000000..73cc726c4046
--- /dev/null
+++ b/drivers/net/wireless/rt2x00/rt2x00usb.c
@@ -0,0 +1,592 @@
+/*
+	Copyright (C) 2004 - 2007 rt2x00 SourceForge Project
+	<http://rt2x00.serialmonkey.com>
+
+	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, or
+	(at your option) any later version.
+
+	This program is distributed in the hope that it will be useful,
+	but WITHOUT ANY WARRANTY; without even the implied warranty of
+	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+	GNU General Public License for more details.
+
+	You should have received a copy of the GNU General Public License
+	along with this program; if not, write to the
+	Free Software Foundation, Inc.,
+	59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+/*
+	Module: rt2x00usb
+	Abstract: rt2x00 generic usb device routines.
+ */
+
+/*
+ * Set enviroment defines for rt2x00.h
+ */
+#define DRV_NAME "rt2x00usb"
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/usb.h>
+
+#include "rt2x00.h"
+#include "rt2x00usb.h"
+
+/*
+ * Interfacing with the HW.
+ */
+int rt2x00usb_vendor_request(const struct rt2x00_dev *rt2x00dev,
+			     const u8 request, const u8 requesttype,
+			     const u16 offset, const u16 value,
+			     void *buffer, const u16 buffer_length,
+			     const int timeout)
+{
+	struct usb_device *usb_dev =
+	    interface_to_usbdev(rt2x00dev_usb(rt2x00dev));
+	int status;
+	unsigned int i;
+	unsigned int pipe =
+	    (requesttype == USB_VENDOR_REQUEST_IN) ?
+	    usb_rcvctrlpipe(usb_dev, 0) : usb_sndctrlpipe(usb_dev, 0);
+
+	for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
+		status = usb_control_msg(usb_dev, pipe, request, requesttype,
+					 value, offset, buffer, buffer_length,
+					 timeout);
+		if (status >= 0)
+			return 0;
+
+		/*
+		 * Check for errors
+		 * -ENODEV: Device has disappeared, no point continuing.
+		 * All other errors: Try again.
+		 */
+		else if (status == -ENODEV)
+			break;
+	}
+
+	ERROR(rt2x00dev,
+	      "Vendor Request 0x%02x failed for offset 0x%04x with error %d.\n",
+	      request, offset, status);
+
+	return status;
+}
+EXPORT_SYMBOL_GPL(rt2x00usb_vendor_request);
+
+int rt2x00usb_vendor_request_buff(const struct rt2x00_dev *rt2x00dev,
+				  const u8 request, const u8 requesttype,
+				  const u16 offset, void *buffer,
+				  const u16 buffer_length, const int timeout)
+{
+	int status;
+
+	/*
+	 * Check for Cache availability.
+	 */
+	if (unlikely(!rt2x00dev->csr_cache || buffer_length > CSR_CACHE_SIZE)) {
+		ERROR(rt2x00dev, "CSR cache not available.\n");
+		return -ENOMEM;
+	}
+
+	if (requesttype == USB_VENDOR_REQUEST_OUT)
+		memcpy(rt2x00dev->csr_cache, buffer, buffer_length);
+
+	status = rt2x00usb_vendor_request(rt2x00dev, request, requesttype,
+					  offset, 0, rt2x00dev->csr_cache,
+					  buffer_length, timeout);
+
+	if (!status && requesttype == USB_VENDOR_REQUEST_IN)
+		memcpy(buffer, rt2x00dev->csr_cache, buffer_length);
+
+	return status;
+}
+EXPORT_SYMBOL_GPL(rt2x00usb_vendor_request_buff);
+
+/*
+ * TX data handlers.
+ */
+static void rt2x00usb_interrupt_txdone(struct urb *urb)
+{
+	struct data_entry *entry = (struct data_entry *)urb->context;
+	struct data_ring *ring = entry->ring;
+	struct rt2x00_dev *rt2x00dev = ring->rt2x00dev;
+	struct data_desc *txd = (struct data_desc *)entry->skb->data;
+	u32 word;
+	int tx_status;
+
+	if (!test_bit(DEVICE_ENABLED_RADIO, &rt2x00dev->flags) ||
+	    !__test_and_clear_bit(ENTRY_OWNER_NIC, &entry->flags))
+		return;
+
+	rt2x00_desc_read(txd, 0, &word);
+
+	/*
+	 * Remove the descriptor data from the buffer.
+	 */
+	skb_pull(entry->skb, ring->desc_size);
+
+	/*
+	 * Obtain the status about this packet.
+	 */
+	tx_status = !urb->status ? TX_SUCCESS : TX_FAIL_RETRY;
+
+	rt2x00lib_txdone(entry, tx_status, 0);
+
+	/*
+	 * Make this entry available for reuse.
+	 */
+	entry->flags = 0;
+	rt2x00_ring_index_done_inc(entry->ring);
+
+	/*
+	 * If the data ring was full before the txdone handler
+	 * we must make sure the packet queue in the mac80211 stack
+	 * is reenabled when the txdone handler has finished.
+	 */
+	if (!rt2x00_ring_full(ring))
+		ieee80211_wake_queue(rt2x00dev->hw,
+				     entry->tx_status.control.queue);
+}
+
+int rt2x00usb_write_tx_data(struct rt2x00_dev *rt2x00dev,
+			    struct data_ring *ring, struct sk_buff *skb,
+			    struct ieee80211_tx_control *control)
+{
+	struct usb_device *usb_dev =
+	    interface_to_usbdev(rt2x00dev_usb(rt2x00dev));
+	struct data_entry *entry = rt2x00_get_data_entry(ring);
+	int pipe = usb_sndbulkpipe(usb_dev, 1);
+	int max_packet = usb_maxpacket(usb_dev, pipe, 1);
+	u32 length;
+
+	if (rt2x00_ring_full(ring)) {
+		ieee80211_stop_queue(rt2x00dev->hw, control->queue);
+		return -EINVAL;
+	}
+
+	if (test_bit(ENTRY_OWNER_NIC, &entry->flags)) {
+		ERROR(rt2x00dev,
+		      "Arrived at non-free entry in the non-full queue %d.\n"
+		      "Please file bug report to %s.\n",
+		      control->queue, DRV_PROJECT);
+		ieee80211_stop_queue(rt2x00dev->hw, control->queue);
+		return -EINVAL;
+	}
+
+	/*
+	 * Add the descriptor in front of the skb.
+	 */
+	skb_push(skb, ring->desc_size);
+	memset(skb->data, 0, ring->desc_size);
+
+	rt2x00lib_write_tx_desc(rt2x00dev, (struct data_desc *)skb->data,
+				(struct ieee80211_hdr *)(skb->data +
+							 ring->desc_size),
+				skb->len - ring->desc_size, control);
+	memcpy(&entry->tx_status.control, control, sizeof(*control));
+	entry->skb = skb;
+
+	/*
+	 * USB devices cannot blindly pass the skb->len as the
+	 * length of the data to usb_fill_bulk_urb. Pass the skb
+	 * to the driver to determine what the length should be.
+	 */
+	length = rt2x00dev->ops->lib->get_tx_data_len(rt2x00dev,
+						      max_packet, skb);
+
+	/*
+	 * Initialize URB and send the frame to the device.
+	 */
+	__set_bit(ENTRY_OWNER_NIC, &entry->flags);
+	usb_fill_bulk_urb(entry->priv, usb_dev, pipe,
+			  skb->data, length, rt2x00usb_interrupt_txdone, entry);
+	usb_submit_urb(entry->priv, GFP_ATOMIC);
+
+	rt2x00_ring_index_inc(ring);
+
+	if (rt2x00_ring_full(ring))
+		ieee80211_stop_queue(rt2x00dev->hw, control->queue);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(rt2x00usb_write_tx_data);
+
+/*
+ * RX data handlers.
+ */
+static void rt2x00usb_interrupt_rxdone(struct urb *urb)
+{
+	struct data_entry *entry = (struct data_entry *)urb->context;
+	struct data_ring *ring = entry->ring;
+	struct rt2x00_dev *rt2x00dev = ring->rt2x00dev;
+	struct sk_buff *skb;
+	struct rxdata_entry_desc desc;
+	int frame_size;
+
+	if (!test_bit(DEVICE_ENABLED_RADIO, &rt2x00dev->flags) ||
+	    !test_and_clear_bit(ENTRY_OWNER_NIC, &entry->flags))
+		return;
+
+	/*
+	 * Check if the received data is simply too small
+	 * to be actually valid, or if the urb is signaling
+	 * a problem.
+	 */
+	if (urb->actual_length < entry->ring->desc_size || urb->status)
+		goto skip_entry;
+
+	memset(&desc, 0x00, sizeof(desc));
+	rt2x00dev->ops->lib->fill_rxdone(entry, &desc);
+
+	/*
+	 * Allocate a new sk buffer to replace the current one.
+	 * If allocation fails, we should drop the current frame
+	 * so we can recycle the existing sk buffer for the new frame.
+	 */
+	frame_size = entry->ring->data_size + entry->ring->desc_size;
+	skb = dev_alloc_skb(frame_size + NET_IP_ALIGN);
+	if (!skb)
+		goto skip_entry;
+
+	skb_reserve(skb, NET_IP_ALIGN);
+	skb_put(skb, frame_size);
+
+	/*
+	 * Trim the skb_buffer to only contain the valid
+	 * frame data (so ignore the device's descriptor).
+	 */
+	skb_trim(entry->skb, desc.size);
+
+	/*
+	 * Send the frame to rt2x00lib for further processing.
+	 */
+	rt2x00lib_rxdone(entry, entry->skb, &desc);
+
+	/*
+	 * Replace current entry's skb with the newly allocated one,
+	 * and reinitialize the urb.
+	 */
+	entry->skb = skb;
+	urb->transfer_buffer = entry->skb->data;
+	urb->transfer_buffer_length = entry->skb->len;
+
+skip_entry:
+	if (test_bit(DEVICE_ENABLED_RADIO, &ring->rt2x00dev->flags)) {
+		__set_bit(ENTRY_OWNER_NIC, &entry->flags);
+		usb_submit_urb(urb, GFP_ATOMIC);
+	}
+
+	rt2x00_ring_index_inc(ring);
+}
+
+/*
+ * Radio handlers
+ */
+void rt2x00usb_enable_radio(struct rt2x00_dev *rt2x00dev)
+{
+	struct usb_device *usb_dev =
+	    interface_to_usbdev(rt2x00dev_usb(rt2x00dev));
+	struct data_ring *ring;
+	struct data_entry *entry;
+	unsigned int i;
+
+	/*
+	 * Initialize the TX rings
+	 */
+	txringall_for_each(rt2x00dev, ring) {
+		for (i = 0; i < ring->stats.limit; i++)
+			ring->entry[i].flags = 0;
+
+		rt2x00_ring_index_clear(ring);
+	}
+
+	/*
+	 * Initialize and start the RX ring.
+	 */
+	rt2x00_ring_index_clear(rt2x00dev->rx);
+
+	for (i = 0; i < rt2x00dev->rx->stats.limit; i++) {
+		entry = &rt2x00dev->rx->entry[i];
+
+		usb_fill_bulk_urb(entry->priv, usb_dev,
+				  usb_rcvbulkpipe(usb_dev, 1),
+				  entry->skb->data, entry->skb->len,
+				  rt2x00usb_interrupt_rxdone, entry);
+
+		__set_bit(ENTRY_OWNER_NIC, &entry->flags);
+		usb_submit_urb(entry->priv, GFP_ATOMIC);
+	}
+}
+EXPORT_SYMBOL_GPL(rt2x00usb_enable_radio);
+
+void rt2x00usb_disable_radio(struct rt2x00_dev *rt2x00dev)
+{
+	struct data_ring *ring;
+	unsigned int i;
+
+	rt2x00usb_vendor_request_sw(rt2x00dev, USB_RX_CONTROL, 0x0000, 0x0000,
+				    REGISTER_TIMEOUT);
+
+	/*
+	 * Cancel all rings.
+	 */
+	ring_for_each(rt2x00dev, ring) {
+		for (i = 0; i < ring->stats.limit; i++)
+			usb_kill_urb(ring->entry[i].priv);
+	}
+}
+EXPORT_SYMBOL_GPL(rt2x00usb_disable_radio);
+
+/*
+ * Device initialization handlers.
+ */
+static int rt2x00usb_alloc_urb(struct rt2x00_dev *rt2x00dev,
+			       struct data_ring *ring)
+{
+	unsigned int i;
+
+	/*
+	 * Allocate the URB's
+	 */
+	for (i = 0; i < ring->stats.limit; i++) {
+		ring->entry[i].priv = usb_alloc_urb(0, GFP_KERNEL);
+		if (!ring->entry[i].priv)
+			return -ENOMEM;
+	}
+
+	return 0;
+}
+
+static void rt2x00usb_free_urb(struct rt2x00_dev *rt2x00dev,
+			       struct data_ring *ring)
+{
+	unsigned int i;
+
+	if (!ring->entry)
+		return;
+
+	for (i = 0; i < ring->stats.limit; i++) {
+		usb_kill_urb(ring->entry[i].priv);
+		usb_free_urb(ring->entry[i].priv);
+		if (ring->entry[i].skb)
+			kfree_skb(ring->entry[i].skb);
+	}
+}
+
+int rt2x00usb_initialize(struct rt2x00_dev *rt2x00dev)
+{
+	struct data_ring *ring;
+	struct sk_buff *skb;
+	unsigned int entry_size;
+	unsigned int i;
+	int status;
+
+	/*
+	 * Allocate DMA
+	 */
+	ring_for_each(rt2x00dev, ring) {
+		status = rt2x00usb_alloc_urb(rt2x00dev, ring);
+		if (status)
+			goto exit;
+	}
+
+	/*
+	 * For the RX ring, skb's should be allocated.
+	 */
+	entry_size = rt2x00dev->rx->data_size + rt2x00dev->rx->desc_size;
+	for (i = 0; i < rt2x00dev->rx->stats.limit; i++) {
+		skb = dev_alloc_skb(NET_IP_ALIGN + entry_size);
+		if (!skb)
+			goto exit;
+
+		skb_reserve(skb, NET_IP_ALIGN);
+		skb_put(skb, entry_size);
+
+		rt2x00dev->rx->entry[i].skb = skb;
+	}
+
+	return 0;
+
+exit:
+	rt2x00usb_uninitialize(rt2x00dev);
+
+	return status;
+}
+EXPORT_SYMBOL_GPL(rt2x00usb_initialize);
+
+void rt2x00usb_uninitialize(struct rt2x00_dev *rt2x00dev)
+{
+	struct data_ring *ring;
+
+	ring_for_each(rt2x00dev, ring)
+		rt2x00usb_free_urb(rt2x00dev, ring);
+}
+EXPORT_SYMBOL_GPL(rt2x00usb_uninitialize);
+
+/*
+ * USB driver handlers.
+ */
+static void rt2x00usb_free_reg(struct rt2x00_dev *rt2x00dev)
+{
+	kfree(rt2x00dev->rf);
+	rt2x00dev->rf = NULL;
+
+	kfree(rt2x00dev->eeprom);
+	rt2x00dev->eeprom = NULL;
+
+	kfree(rt2x00dev->csr_cache);
+	rt2x00dev->csr_cache = NULL;
+}
+
+static int rt2x00usb_alloc_reg(struct rt2x00_dev *rt2x00dev)
+{
+	rt2x00dev->csr_cache = kzalloc(CSR_CACHE_SIZE, GFP_KERNEL);
+	if (!rt2x00dev->csr_cache)
+		goto exit;
+
+	rt2x00dev->eeprom = kzalloc(rt2x00dev->ops->eeprom_size, GFP_KERNEL);
+	if (!rt2x00dev->eeprom)
+		goto exit;
+
+	rt2x00dev->rf = kzalloc(rt2x00dev->ops->rf_size, GFP_KERNEL);
+	if (!rt2x00dev->rf)
+		goto exit;
+
+	return 0;
+
+exit:
+	ERROR_PROBE("Failed to allocate registers.\n");
+
+	rt2x00usb_free_reg(rt2x00dev);
+
+	return -ENOMEM;
+}
+
+int rt2x00usb_probe(struct usb_interface *usb_intf,
+		    const struct usb_device_id *id)
+{
+	struct usb_device *usb_dev = interface_to_usbdev(usb_intf);
+	struct rt2x00_ops *ops = (struct rt2x00_ops *)id->driver_info;
+	struct ieee80211_hw *hw;
+	struct rt2x00_dev *rt2x00dev;
+	int retval;
+
+	usb_dev = usb_get_dev(usb_dev);
+
+	hw = ieee80211_alloc_hw(sizeof(struct rt2x00_dev), ops->hw);
+	if (!hw) {
+		ERROR_PROBE("Failed to allocate hardware.\n");
+		retval = -ENOMEM;
+		goto exit_put_device;
+	}
+
+	usb_set_intfdata(usb_intf, hw);
+
+	rt2x00dev = hw->priv;
+	rt2x00dev->dev = usb_intf;
+	rt2x00dev->ops = ops;
+	rt2x00dev->hw = hw;
+
+	retval = rt2x00usb_alloc_reg(rt2x00dev);
+	if (retval)
+		goto exit_free_device;
+
+	retval = rt2x00lib_probe_dev(rt2x00dev);
+	if (retval)
+		goto exit_free_reg;
+
+	return 0;
+
+exit_free_reg:
+	rt2x00usb_free_reg(rt2x00dev);
+
+exit_free_device:
+	ieee80211_free_hw(hw);
+
+exit_put_device:
+	usb_put_dev(usb_dev);
+
+	usb_set_intfdata(usb_intf, NULL);
+
+	return retval;
+}
+EXPORT_SYMBOL_GPL(rt2x00usb_probe);
+
+void rt2x00usb_disconnect(struct usb_interface *usb_intf)
+{
+	struct ieee80211_hw *hw = usb_get_intfdata(usb_intf);
+	struct rt2x00_dev *rt2x00dev = hw->priv;
+
+	/*
+	 * Free all allocated data.
+	 */
+	rt2x00lib_remove_dev(rt2x00dev);
+	rt2x00usb_free_reg(rt2x00dev);
+	ieee80211_free_hw(hw);
+
+	/*
+	 * Free the USB device data.
+	 */
+	usb_set_intfdata(usb_intf, NULL);
+	usb_put_dev(interface_to_usbdev(usb_intf));
+}
+EXPORT_SYMBOL_GPL(rt2x00usb_disconnect);
+
+#ifdef CONFIG_PM
+int rt2x00usb_suspend(struct usb_interface *usb_intf, pm_message_t state)
+{
+	struct ieee80211_hw *hw = usb_get_intfdata(usb_intf);
+	struct rt2x00_dev *rt2x00dev = hw->priv;
+	int retval;
+
+	retval = rt2x00lib_suspend(rt2x00dev, state);
+	if (retval)
+		return retval;
+
+	rt2x00usb_free_reg(rt2x00dev);
+
+	/*
+	 * Decrease usbdev refcount.
+	 */
+	usb_put_dev(interface_to_usbdev(usb_intf));
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(rt2x00usb_suspend);
+
+int rt2x00usb_resume(struct usb_interface *usb_intf)
+{
+	struct ieee80211_hw *hw = usb_get_intfdata(usb_intf);
+	struct rt2x00_dev *rt2x00dev = hw->priv;
+	int retval;
+
+	usb_get_dev(interface_to_usbdev(usb_intf));
+
+	retval = rt2x00usb_alloc_reg(rt2x00dev);
+	if (retval)
+		return retval;
+
+	retval = rt2x00lib_resume(rt2x00dev);
+	if (retval)
+		goto exit_free_reg;
+
+	return 0;
+
+exit_free_reg:
+	rt2x00usb_free_reg(rt2x00dev);
+
+	return retval;
+}
+EXPORT_SYMBOL_GPL(rt2x00usb_resume);
+#endif /* CONFIG_PM */
+
+/*
+ * rt2x00pci module information.
+ */
+MODULE_AUTHOR(DRV_PROJECT);
+MODULE_VERSION(DRV_VERSION);
+MODULE_DESCRIPTION("rt2x00 library");
+MODULE_LICENSE("GPL");
diff --git a/drivers/net/wireless/rt2x00/rt2x00usb.h b/drivers/net/wireless/rt2x00/rt2x00usb.h
new file mode 100644
index 000000000000..2681abe4d49e
--- /dev/null
+++ b/drivers/net/wireless/rt2x00/rt2x00usb.h
@@ -0,0 +1,180 @@
+/*
+	Copyright (C) 2004 - 2007 rt2x00 SourceForge Project
+	<http://rt2x00.serialmonkey.com>
+
+	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, or
+	(at your option) any later version.
+
+	This program is distributed in the hope that it will be useful,
+	but WITHOUT ANY WARRANTY; without even the implied warranty of
+	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+	GNU General Public License for more details.
+
+	You should have received a copy of the GNU General Public License
+	along with this program; if not, write to the
+	Free Software Foundation, Inc.,
+	59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+/*
+	Module: rt2x00usb
+	Abstract: Data structures for the rt2x00usb module.
+ */
+
+#ifndef RT2X00USB_H
+#define RT2X00USB_H
+
+/*
+ * This variable should be used with the
+ * usb_driver structure initialization.
+ */
+#define USB_DEVICE_DATA(__ops)	.driver_info = (kernel_ulong_t)(__ops)
+
+/*
+ * Register defines.
+ * Some registers require multiple attempts before success,
+ * in those cases REGISTER_BUSY_COUNT attempts should be
+ * taken with a REGISTER_BUSY_DELAY interval.
+ * For USB vendor requests we need to pass a timeout
+ * time in ms, for this we use the REGISTER_TIMEOUT,
+ * however when loading firmware a higher value is
+ * required. In that case we use the REGISTER_TIMEOUT_FIRMWARE.
+ */
+#define REGISTER_BUSY_COUNT		5
+#define REGISTER_BUSY_DELAY		100
+#define REGISTER_TIMEOUT		500
+#define REGISTER_TIMEOUT_FIRMWARE	1000
+
+/*
+ * Cache size
+ */
+#define CSR_CACHE_SIZE			8
+#define CSR_CACHE_SIZE_FIRMWARE		64
+
+/*
+ * USB request types.
+ */
+#define USB_VENDOR_REQUEST	( USB_TYPE_VENDOR | USB_RECIP_DEVICE )
+#define USB_VENDOR_REQUEST_IN	( USB_DIR_IN | USB_VENDOR_REQUEST )
+#define USB_VENDOR_REQUEST_OUT	( USB_DIR_OUT | USB_VENDOR_REQUEST )
+
+/*
+ * USB vendor commands.
+ */
+#define USB_DEVICE_MODE		0x01
+#define USB_SINGLE_WRITE	0x02
+#define USB_SINGLE_READ		0x03
+#define USB_MULTI_WRITE		0x06
+#define USB_MULTI_READ		0x07
+#define USB_EEPROM_WRITE	0x08
+#define USB_EEPROM_READ		0x09
+#define USB_LED_CONTROL		0x0a	/* RT73USB */
+#define USB_RX_CONTROL		0x0c
+
+/*
+ * Device modes offset
+ */
+#define USB_MODE_RESET		0x01
+#define USB_MODE_UNPLUG		0x02
+#define USB_MODE_FUNCTION	0x03
+#define USB_MODE_TEST		0x04
+#define USB_MODE_SLEEP		0x07	/* RT73USB */
+#define USB_MODE_FIRMWARE	0x08	/* RT73USB */
+#define USB_MODE_WAKEUP		0x09	/* RT73USB */
+
+/*
+ * Used to read/write from/to the device.
+ * This is the main function to communicate with the device,
+ * the buffer argument _must_ either be NULL or point to
+ * a buffer allocated by kmalloc. Failure to do so can lead
+ * to unexpected behavior depending on the architecture.
+ */
+int rt2x00usb_vendor_request(const struct rt2x00_dev *rt2x00dev,
+			     const u8 request, const u8 requesttype,
+			     const u16 offset, const u16 value,
+			     void *buffer, const u16 buffer_length,
+			     const int timeout);
+
+/*
+ * Used to read/write from/to the device.
+ * This function will use a previously with kmalloc allocated cache
+ * to communicate with the device. The contents of the buffer pointer
+ * will be copied to this cache when writing, or read from the cache
+ * when reading.
+ * Buffers send to rt2x00usb_vendor_request _must_ be allocated with
+ * kmalloc. Hence the reason for using a previously allocated cache
+ * which has been allocated properly.
+ */
+int rt2x00usb_vendor_request_buff(const struct rt2x00_dev *rt2x00dev,
+				  const u8 request, const u8 requesttype,
+				  const u16 offset, void *buffer,
+				  const u16 buffer_length, const int timeout);
+
+/*
+ * Simple wrapper around rt2x00usb_vendor_request to write a single
+ * command to the device. Since we don't use the buffer argument we
+ * don't have to worry about kmalloc here.
+ */
+static inline int rt2x00usb_vendor_request_sw(const struct rt2x00_dev
+					      *rt2x00dev,
+					      const u8 request,
+					      const u16 offset,
+					      const u16 value,
+					      const int timeout)
+{
+	return rt2x00usb_vendor_request(rt2x00dev, request,
+					USB_VENDOR_REQUEST_OUT, offset,
+					value, NULL, 0, timeout);
+}
+
+/*
+ * Simple wrapper around rt2x00usb_vendor_request to read the eeprom
+ * from the device. Note that the eeprom argument _must_ be allocated using
+ * kmalloc for correct handling inside the kernel USB layer.
+ */
+static inline int rt2x00usb_eeprom_read(const struct rt2x00_dev *rt2x00dev,
+					 __le16 *eeprom, const u16 lenght)
+{
+	int timeout = REGISTER_TIMEOUT * (lenght / sizeof(u16));
+
+	return rt2x00usb_vendor_request(rt2x00dev, USB_EEPROM_READ,
+					USB_VENDOR_REQUEST_IN, 0x0000,
+					0x0000, eeprom, lenght, timeout);
+}
+
+/*
+ * Radio handlers
+ */
+void rt2x00usb_enable_radio(struct rt2x00_dev *rt2x00dev);
+void rt2x00usb_disable_radio(struct rt2x00_dev *rt2x00dev);
+
+/*
+ * TX data handlers.
+ */
+int rt2x00usb_write_tx_data(struct rt2x00_dev *rt2x00dev,
+			    struct data_ring *ring, struct sk_buff *skb,
+			    struct ieee80211_tx_control *control);
+
+/*
+ * Device initialization handlers.
+ */
+int rt2x00usb_initialize(struct rt2x00_dev *rt2x00dev);
+void rt2x00usb_uninitialize(struct rt2x00_dev *rt2x00dev);
+
+/*
+ * USB driver handlers.
+ */
+int rt2x00usb_probe(struct usb_interface *usb_intf,
+		    const struct usb_device_id *id);
+void rt2x00usb_disconnect(struct usb_interface *usb_intf);
+#ifdef CONFIG_PM
+int rt2x00usb_suspend(struct usb_interface *usb_intf, pm_message_t state);
+int rt2x00usb_resume(struct usb_interface *usb_intf);
+#else
+#define rt2x00usb_suspend	NULL
+#define rt2x00usb_resume	NULL
+#endif /* CONFIG_PM */
+
+#endif /* RT2X00USB_H */
diff --git a/drivers/net/wireless/rt2x00/rt61pci.c b/drivers/net/wireless/rt2x00/rt61pci.c
new file mode 100644
index 000000000000..01dbef19d651
--- /dev/null
+++ b/drivers/net/wireless/rt2x00/rt61pci.c
@@ -0,0 +1,2557 @@
+/*
+	Copyright (C) 2004 - 2007 rt2x00 SourceForge Project
+	<http://rt2x00.serialmonkey.com>
+
+	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, or
+	(at your option) any later version.
+
+	This program is distributed in the hope that it will be useful,
+	but WITHOUT ANY WARRANTY; without even the implied warranty of
+	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+	GNU General Public License for more details.
+
+	You should have received a copy of the GNU General Public License
+	along with this program; if not, write to the
+	Free Software Foundation, Inc.,
+	59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+/*
+	Module: rt61pci
+	Abstract: rt61pci device specific routines.
+	Supported chipsets: RT2561, RT2561s, RT2661.
+ */
+
+/*
+ * Set enviroment defines for rt2x00.h
+ */
+#define DRV_NAME "rt61pci"
+
+#include <linux/delay.h>
+#include <linux/etherdevice.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/pci.h>
+#include <linux/eeprom_93cx6.h>
+
+#include "rt2x00.h"
+#include "rt2x00pci.h"
+#include "rt61pci.h"
+
+/*
+ * Register access.
+ * BBP and RF register require indirect register access,
+ * and use the CSR registers PHY_CSR3 and PHY_CSR4 to achieve this.
+ * These indirect registers work with busy bits,
+ * and we will try maximal REGISTER_BUSY_COUNT times to access
+ * the register while taking a REGISTER_BUSY_DELAY us delay
+ * between each attampt. When the busy bit is still set at that time,
+ * the access attempt is considered to have failed,
+ * and we will print an error.
+ */
+static u32 rt61pci_bbp_check(const struct rt2x00_dev *rt2x00dev)
+{
+	u32 reg;
+	unsigned int i;
+
+	for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
+		rt2x00pci_register_read(rt2x00dev, PHY_CSR3, &reg);
+		if (!rt2x00_get_field32(reg, PHY_CSR3_BUSY))
+			break;
+		udelay(REGISTER_BUSY_DELAY);
+	}
+
+	return reg;
+}
+
+static void rt61pci_bbp_write(const struct rt2x00_dev *rt2x00dev,
+			      const unsigned int word, const u8 value)
+{
+	u32 reg;
+
+	/*
+	 * Wait until the BBP becomes ready.
+	 */
+	reg = rt61pci_bbp_check(rt2x00dev);
+	if (rt2x00_get_field32(reg, PHY_CSR3_BUSY)) {
+		ERROR(rt2x00dev, "PHY_CSR3 register busy. Write failed.\n");
+		return;
+	}
+
+	/*
+	 * Write the data into the BBP.
+	 */
+	reg = 0;
+	rt2x00_set_field32(&reg, PHY_CSR3_VALUE, value);
+	rt2x00_set_field32(&reg, PHY_CSR3_REGNUM, word);
+	rt2x00_set_field32(&reg, PHY_CSR3_BUSY, 1);
+	rt2x00_set_field32(&reg, PHY_CSR3_READ_CONTROL, 0);
+
+	rt2x00pci_register_write(rt2x00dev, PHY_CSR3, reg);
+}
+
+static void rt61pci_bbp_read(const struct rt2x00_dev *rt2x00dev,
+			     const unsigned int word, u8 *value)
+{
+	u32 reg;
+
+	/*
+	 * Wait until the BBP becomes ready.
+	 */
+	reg = rt61pci_bbp_check(rt2x00dev);
+	if (rt2x00_get_field32(reg, PHY_CSR3_BUSY)) {
+		ERROR(rt2x00dev, "PHY_CSR3 register busy. Read failed.\n");
+		return;
+	}
+
+	/*
+	 * Write the request into the BBP.
+	 */
+	reg = 0;
+	rt2x00_set_field32(&reg, PHY_CSR3_REGNUM, word);
+	rt2x00_set_field32(&reg, PHY_CSR3_BUSY, 1);
+	rt2x00_set_field32(&reg, PHY_CSR3_READ_CONTROL, 1);
+
+	rt2x00pci_register_write(rt2x00dev, PHY_CSR3, reg);
+
+	/*
+	 * Wait until the BBP becomes ready.
+	 */
+	reg = rt61pci_bbp_check(rt2x00dev);
+	if (rt2x00_get_field32(reg, PHY_CSR3_BUSY)) {
+		ERROR(rt2x00dev, "PHY_CSR3 register busy. Read failed.\n");
+		*value = 0xff;
+		return;
+	}
+
+	*value = rt2x00_get_field32(reg, PHY_CSR3_VALUE);
+}
+
+static void rt61pci_rf_write(const struct rt2x00_dev *rt2x00dev,
+			     const unsigned int word, const u32 value)
+{
+	u32 reg;
+	unsigned int i;
+
+	if (!word)
+		return;
+
+	for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
+		rt2x00pci_register_read(rt2x00dev, PHY_CSR4, &reg);
+		if (!rt2x00_get_field32(reg, PHY_CSR4_BUSY))
+			goto rf_write;
+		udelay(REGISTER_BUSY_DELAY);
+	}
+
+	ERROR(rt2x00dev, "PHY_CSR4 register busy. Write failed.\n");
+	return;
+
+rf_write:
+	reg = 0;
+	rt2x00_set_field32(&reg, PHY_CSR4_VALUE, value);
+	rt2x00_set_field32(&reg, PHY_CSR4_NUMBER_OF_BITS, 21);
+	rt2x00_set_field32(&reg, PHY_CSR4_IF_SELECT, 0);
+	rt2x00_set_field32(&reg, PHY_CSR4_BUSY, 1);
+
+	rt2x00pci_register_write(rt2x00dev, PHY_CSR4, reg);
+	rt2x00_rf_write(rt2x00dev, word, value);
+}
+
+static void rt61pci_mcu_request(const struct rt2x00_dev *rt2x00dev,
+				const u8 command, const u8 token,
+				const u8 arg0, const u8 arg1)
+{
+	u32 reg;
+
+	rt2x00pci_register_read(rt2x00dev, H2M_MAILBOX_CSR, &reg);
+
+	if (rt2x00_get_field32(reg, H2M_MAILBOX_CSR_OWNER)) {
+		ERROR(rt2x00dev, "mcu request error. "
+		      "Request 0x%02x failed for token 0x%02x.\n",
+		      command, token);
+		return;
+	}
+
+	rt2x00_set_field32(&reg, H2M_MAILBOX_CSR_OWNER, 1);
+	rt2x00_set_field32(&reg, H2M_MAILBOX_CSR_CMD_TOKEN, token);
+	rt2x00_set_field32(&reg, H2M_MAILBOX_CSR_ARG0, arg0);
+	rt2x00_set_field32(&reg, H2M_MAILBOX_CSR_ARG1, arg1);
+	rt2x00pci_register_write(rt2x00dev, H2M_MAILBOX_CSR, reg);
+
+	rt2x00pci_register_read(rt2x00dev, HOST_CMD_CSR, &reg);
+	rt2x00_set_field32(&reg, HOST_CMD_CSR_HOST_COMMAND, command);
+	rt2x00_set_field32(&reg, HOST_CMD_CSR_INTERRUPT_MCU, 1);
+	rt2x00pci_register_write(rt2x00dev, HOST_CMD_CSR, reg);
+}
+
+static void rt61pci_eepromregister_read(struct eeprom_93cx6 *eeprom)
+{
+	struct rt2x00_dev *rt2x00dev = eeprom->data;
+	u32 reg;
+
+	rt2x00pci_register_read(rt2x00dev, E2PROM_CSR, &reg);
+
+	eeprom->reg_data_in = !!rt2x00_get_field32(reg, E2PROM_CSR_DATA_IN);
+	eeprom->reg_data_out = !!rt2x00_get_field32(reg, E2PROM_CSR_DATA_OUT);
+	eeprom->reg_data_clock =
+	    !!rt2x00_get_field32(reg, E2PROM_CSR_DATA_CLOCK);
+	eeprom->reg_chip_select =
+	    !!rt2x00_get_field32(reg, E2PROM_CSR_CHIP_SELECT);
+}
+
+static void rt61pci_eepromregister_write(struct eeprom_93cx6 *eeprom)
+{
+	struct rt2x00_dev *rt2x00dev = eeprom->data;
+	u32 reg = 0;
+
+	rt2x00_set_field32(&reg, E2PROM_CSR_DATA_IN, !!eeprom->reg_data_in);
+	rt2x00_set_field32(&reg, E2PROM_CSR_DATA_OUT, !!eeprom->reg_data_out);
+	rt2x00_set_field32(&reg, E2PROM_CSR_DATA_CLOCK,
+			   !!eeprom->reg_data_clock);
+	rt2x00_set_field32(&reg, E2PROM_CSR_CHIP_SELECT,
+			   !!eeprom->reg_chip_select);
+
+	rt2x00pci_register_write(rt2x00dev, E2PROM_CSR, reg);
+}
+
+#ifdef CONFIG_RT2X00_LIB_DEBUGFS
+#define CSR_OFFSET(__word)	( CSR_REG_BASE + ((__word) * sizeof(u32)) )
+
+static void rt61pci_read_csr(const struct rt2x00_dev *rt2x00dev,
+			     const unsigned int word, u32 *data)
+{
+	rt2x00pci_register_read(rt2x00dev, CSR_OFFSET(word), data);
+}
+
+static void rt61pci_write_csr(const struct rt2x00_dev *rt2x00dev,
+			      const unsigned int word, u32 data)
+{
+	rt2x00pci_register_write(rt2x00dev, CSR_OFFSET(word), data);
+}
+
+static const struct rt2x00debug rt61pci_rt2x00debug = {
+	.owner	= THIS_MODULE,
+	.csr	= {
+		.read		= rt61pci_read_csr,
+		.write		= rt61pci_write_csr,
+		.word_size	= sizeof(u32),
+		.word_count	= CSR_REG_SIZE / sizeof(u32),
+	},
+	.eeprom	= {
+		.read		= rt2x00_eeprom_read,
+		.write		= rt2x00_eeprom_write,
+		.word_size	= sizeof(u16),
+		.word_count	= EEPROM_SIZE / sizeof(u16),
+	},
+	.bbp	= {
+		.read		= rt61pci_bbp_read,
+		.write		= rt61pci_bbp_write,
+		.word_size	= sizeof(u8),
+		.word_count	= BBP_SIZE / sizeof(u8),
+	},
+	.rf	= {
+		.read		= rt2x00_rf_read,
+		.write		= rt61pci_rf_write,
+		.word_size	= sizeof(u32),
+		.word_count	= RF_SIZE / sizeof(u32),
+	},
+};
+#endif /* CONFIG_RT2X00_LIB_DEBUGFS */
+
+#ifdef CONFIG_RT61PCI_RFKILL
+static int rt61pci_rfkill_poll(struct rt2x00_dev *rt2x00dev)
+{
+	u32 reg;
+
+	rt2x00pci_register_read(rt2x00dev, MAC_CSR13, &reg);
+	return rt2x00_get_field32(reg, MAC_CSR13_BIT5);;
+}
+#else
+#define rt61pci_rfkill_poll	NULL
+#endif /* CONFIG_RT61PCI_RFKILL */
+
+/*
+ * Configuration handlers.
+ */
+static void rt61pci_config_mac_addr(struct rt2x00_dev *rt2x00dev, __le32 *mac)
+{
+	u32 tmp;
+
+	tmp = le32_to_cpu(mac[1]);
+	rt2x00_set_field32(&tmp, MAC_CSR3_UNICAST_TO_ME_MASK, 0xff);
+	mac[1] = cpu_to_le32(tmp);
+
+	rt2x00pci_register_multiwrite(rt2x00dev, MAC_CSR2, mac,
+				      (2 * sizeof(__le32)));
+}
+
+static void rt61pci_config_bssid(struct rt2x00_dev *rt2x00dev, __le32 *bssid)
+{
+	u32 tmp;
+
+	tmp = le32_to_cpu(bssid[1]);
+	rt2x00_set_field32(&tmp, MAC_CSR5_BSS_ID_MASK, 3);
+	bssid[1] = cpu_to_le32(tmp);
+
+	rt2x00pci_register_multiwrite(rt2x00dev, MAC_CSR4, bssid,
+				      (2 * sizeof(__le32)));
+}
+
+static void rt61pci_config_type(struct rt2x00_dev *rt2x00dev, const int type,
+				const int tsf_sync)
+{
+	u32 reg;
+
+	/*
+	 * Clear current synchronisation setup.
+	 * For the Beacon base registers we only need to clear
+	 * the first byte since that byte contains the VALID and OWNER
+	 * bits which (when set to 0) will invalidate the entire beacon.
+	 */
+	rt2x00pci_register_write(rt2x00dev, TXRX_CSR9, 0);
+	rt2x00pci_register_write(rt2x00dev, HW_BEACON_BASE0, 0);
+	rt2x00pci_register_write(rt2x00dev, HW_BEACON_BASE1, 0);
+	rt2x00pci_register_write(rt2x00dev, HW_BEACON_BASE2, 0);
+	rt2x00pci_register_write(rt2x00dev, HW_BEACON_BASE3, 0);
+
+	/*
+	 * Enable synchronisation.
+	 */
+	rt2x00pci_register_read(rt2x00dev, TXRX_CSR9, &reg);
+	rt2x00_set_field32(&reg, TXRX_CSR9_TSF_TICKING, 1);
+	rt2x00_set_field32(&reg, TXRX_CSR9_TBTT_ENABLE, 1);
+	rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_GEN, 0);
+	rt2x00_set_field32(&reg, TXRX_CSR9_TSF_SYNC, tsf_sync);
+	rt2x00pci_register_write(rt2x00dev, TXRX_CSR9, reg);
+}
+
+static void rt61pci_config_preamble(struct rt2x00_dev *rt2x00dev,
+				    const int short_preamble,
+				    const int ack_timeout,
+				    const int ack_consume_time)
+{
+	u32 reg;
+
+	rt2x00pci_register_read(rt2x00dev, TXRX_CSR0, &reg);
+	rt2x00_set_field32(&reg, TXRX_CSR0_RX_ACK_TIMEOUT, ack_timeout);
+	rt2x00pci_register_write(rt2x00dev, TXRX_CSR0, reg);
+
+	rt2x00pci_register_read(rt2x00dev, TXRX_CSR4, &reg);
+	rt2x00_set_field32(&reg, TXRX_CSR4_AUTORESPOND_PREAMBLE,
+			   !!short_preamble);
+	rt2x00pci_register_write(rt2x00dev, TXRX_CSR4, reg);
+}
+
+static void rt61pci_config_phymode(struct rt2x00_dev *rt2x00dev,
+				   const int basic_rate_mask)
+{
+	rt2x00pci_register_write(rt2x00dev, TXRX_CSR5, basic_rate_mask);
+}
+
+static void rt61pci_config_channel(struct rt2x00_dev *rt2x00dev,
+				   struct rf_channel *rf, const int txpower)
+{
+	u8 r3;
+	u8 r94;
+	u8 smart;
+
+	rt2x00_set_field32(&rf->rf3, RF3_TXPOWER, TXPOWER_TO_DEV(txpower));
+	rt2x00_set_field32(&rf->rf4, RF4_FREQ_OFFSET, rt2x00dev->freq_offset);
+
+	smart = !(rt2x00_rf(&rt2x00dev->chip, RF5225) ||
+		  rt2x00_rf(&rt2x00dev->chip, RF2527));
+
+	rt61pci_bbp_read(rt2x00dev, 3, &r3);
+	rt2x00_set_field8(&r3, BBP_R3_SMART_MODE, smart);
+	rt61pci_bbp_write(rt2x00dev, 3, r3);
+
+	r94 = 6;
+	if (txpower > MAX_TXPOWER && txpower <= (MAX_TXPOWER + r94))
+		r94 += txpower - MAX_TXPOWER;
+	else if (txpower < MIN_TXPOWER && txpower >= (MIN_TXPOWER - r94))
+		r94 += txpower;
+	rt61pci_bbp_write(rt2x00dev, 94, r94);
+
+	rt61pci_rf_write(rt2x00dev, 1, rf->rf1);
+	rt61pci_rf_write(rt2x00dev, 2, rf->rf2);
+	rt61pci_rf_write(rt2x00dev, 3, rf->rf3 & ~0x00000004);
+	rt61pci_rf_write(rt2x00dev, 4, rf->rf4);
+
+	udelay(200);
+
+	rt61pci_rf_write(rt2x00dev, 1, rf->rf1);
+	rt61pci_rf_write(rt2x00dev, 2, rf->rf2);
+	rt61pci_rf_write(rt2x00dev, 3, rf->rf3 | 0x00000004);
+	rt61pci_rf_write(rt2x00dev, 4, rf->rf4);
+
+	udelay(200);
+
+	rt61pci_rf_write(rt2x00dev, 1, rf->rf1);
+	rt61pci_rf_write(rt2x00dev, 2, rf->rf2);
+	rt61pci_rf_write(rt2x00dev, 3, rf->rf3 & ~0x00000004);
+	rt61pci_rf_write(rt2x00dev, 4, rf->rf4);
+
+	msleep(1);
+}
+
+static void rt61pci_config_txpower(struct rt2x00_dev *rt2x00dev,
+				   const int txpower)
+{
+	struct rf_channel rf;
+
+	rt2x00_rf_read(rt2x00dev, 1, &rf.rf1);
+	rt2x00_rf_read(rt2x00dev, 2, &rf.rf2);
+	rt2x00_rf_read(rt2x00dev, 3, &rf.rf3);
+	rt2x00_rf_read(rt2x00dev, 4, &rf.rf4);
+
+	rt61pci_config_channel(rt2x00dev, &rf, txpower);
+}
+
+static void rt61pci_config_antenna_5x(struct rt2x00_dev *rt2x00dev,
+				      const int antenna_tx,
+				      const int antenna_rx)
+{
+	u8 r3;
+	u8 r4;
+	u8 r77;
+
+	rt61pci_bbp_read(rt2x00dev, 3, &r3);
+	rt61pci_bbp_read(rt2x00dev, 4, &r4);
+	rt61pci_bbp_read(rt2x00dev, 77, &r77);
+
+	rt2x00_set_field8(&r3, BBP_R3_SMART_MODE,
+			  !rt2x00_rf(&rt2x00dev->chip, RF5225));
+
+	switch (antenna_rx) {
+	case ANTENNA_SW_DIVERSITY:
+	case ANTENNA_HW_DIVERSITY:
+		rt2x00_set_field8(&r4, BBP_R4_RX_ANTENNA, 2);
+		rt2x00_set_field8(&r4, BBP_R4_RX_FRAME_END,
+				  !!(rt2x00dev->curr_hwmode != HWMODE_A));
+		break;
+	case ANTENNA_A:
+		rt2x00_set_field8(&r4, BBP_R4_RX_ANTENNA, 1);
+		rt2x00_set_field8(&r4, BBP_R4_RX_FRAME_END, 0);
+
+		if (rt2x00dev->curr_hwmode == HWMODE_A)
+			rt2x00_set_field8(&r77, BBP_R77_PAIR, 0);
+		else
+			rt2x00_set_field8(&r77, BBP_R77_PAIR, 3);
+		break;
+	case ANTENNA_B:
+		rt2x00_set_field8(&r4, BBP_R4_RX_ANTENNA, 1);
+		rt2x00_set_field8(&r4, BBP_R4_RX_FRAME_END, 0);
+
+		if (rt2x00dev->curr_hwmode == HWMODE_A)
+			rt2x00_set_field8(&r77, BBP_R77_PAIR, 3);
+		else
+			rt2x00_set_field8(&r77, BBP_R77_PAIR, 0);
+		break;
+	}
+
+	rt61pci_bbp_write(rt2x00dev, 77, r77);
+	rt61pci_bbp_write(rt2x00dev, 3, r3);
+	rt61pci_bbp_write(rt2x00dev, 4, r4);
+}
+
+static void rt61pci_config_antenna_2x(struct rt2x00_dev *rt2x00dev,
+				      const int antenna_tx,
+				      const int antenna_rx)
+{
+	u8 r3;
+	u8 r4;
+	u8 r77;
+
+	rt61pci_bbp_read(rt2x00dev, 3, &r3);
+	rt61pci_bbp_read(rt2x00dev, 4, &r4);
+	rt61pci_bbp_read(rt2x00dev, 77, &r77);
+
+	rt2x00_set_field8(&r3, BBP_R3_SMART_MODE,
+			  !rt2x00_rf(&rt2x00dev->chip, RF2527));
+	rt2x00_set_field8(&r4, BBP_R4_RX_FRAME_END,
+			  !test_bit(CONFIG_FRAME_TYPE, &rt2x00dev->flags));
+
+	switch (antenna_rx) {
+	case ANTENNA_SW_DIVERSITY:
+	case ANTENNA_HW_DIVERSITY:
+		rt2x00_set_field8(&r4, BBP_R4_RX_ANTENNA, 2);
+		break;
+	case ANTENNA_A:
+		rt2x00_set_field8(&r4, BBP_R4_RX_ANTENNA, 1);
+		rt2x00_set_field8(&r77, BBP_R77_PAIR, 3);
+		break;
+	case ANTENNA_B:
+		rt2x00_set_field8(&r4, BBP_R4_RX_ANTENNA, 1);
+		rt2x00_set_field8(&r77, BBP_R77_PAIR, 0);
+		break;
+	}
+
+	rt61pci_bbp_write(rt2x00dev, 77, r77);
+	rt61pci_bbp_write(rt2x00dev, 3, r3);
+	rt61pci_bbp_write(rt2x00dev, 4, r4);
+}
+
+static void rt61pci_config_antenna_2529_rx(struct rt2x00_dev *rt2x00dev,
+					   const int p1, const int p2)
+{
+	u32 reg;
+
+	rt2x00pci_register_read(rt2x00dev, MAC_CSR13, &reg);
+
+	if (p1 != 0xff) {
+		rt2x00_set_field32(&reg, MAC_CSR13_BIT4, !!p1);
+		rt2x00_set_field32(&reg, MAC_CSR13_BIT12, 0);
+		rt2x00pci_register_write(rt2x00dev, MAC_CSR13, reg);
+	}
+	if (p2 != 0xff) {
+		rt2x00_set_field32(&reg, MAC_CSR13_BIT3, !p2);
+		rt2x00_set_field32(&reg, MAC_CSR13_BIT11, 0);
+		rt2x00pci_register_write(rt2x00dev, MAC_CSR13, reg);
+	}
+}
+
+static void rt61pci_config_antenna_2529(struct rt2x00_dev *rt2x00dev,
+					const int antenna_tx,
+					const int antenna_rx)
+{
+	u16 eeprom;
+	u8 r3;
+	u8 r4;
+	u8 r77;
+
+	rt61pci_bbp_read(rt2x00dev, 3, &r3);
+	rt61pci_bbp_read(rt2x00dev, 4, &r4);
+	rt61pci_bbp_read(rt2x00dev, 77, &r77);
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC, &eeprom);
+
+	rt2x00_set_field8(&r3, BBP_R3_SMART_MODE, 0);
+
+	if (rt2x00_get_field16(eeprom, EEPROM_NIC_ENABLE_DIVERSITY) &&
+	    rt2x00_get_field16(eeprom, EEPROM_NIC_TX_DIVERSITY)) {
+		rt2x00_set_field8(&r4, BBP_R4_RX_ANTENNA, 2);
+		rt2x00_set_field8(&r4, BBP_R4_RX_FRAME_END, 1);
+		rt61pci_config_antenna_2529_rx(rt2x00dev, 0, 1);
+	} else if (rt2x00_get_field16(eeprom, EEPROM_NIC_ENABLE_DIVERSITY)) {
+		if (rt2x00_get_field16(eeprom, EEPROM_NIC_TX_RX_FIXED) >= 2) {
+			rt2x00_set_field8(&r77, BBP_R77_PAIR, 3);
+			rt61pci_bbp_write(rt2x00dev, 77, r77);
+		}
+		rt2x00_set_field8(&r4, BBP_R4_RX_ANTENNA, 1);
+		rt61pci_config_antenna_2529_rx(rt2x00dev, 1, 1);
+	} else if (!rt2x00_get_field16(eeprom, EEPROM_NIC_ENABLE_DIVERSITY) &&
+		   rt2x00_get_field16(eeprom, EEPROM_NIC_TX_DIVERSITY)) {
+		rt2x00_set_field8(&r4, BBP_R4_RX_ANTENNA, 2);
+		rt2x00_set_field8(&r4, BBP_R4_RX_FRAME_END, 0);
+
+		switch (rt2x00_get_field16(eeprom, EEPROM_NIC_TX_RX_FIXED)) {
+		case 0:
+			rt61pci_config_antenna_2529_rx(rt2x00dev, 0, 1);
+			break;
+		case 1:
+			rt61pci_config_antenna_2529_rx(rt2x00dev, 1, 0);
+			break;
+		case 2:
+			rt61pci_config_antenna_2529_rx(rt2x00dev, 0, 0);
+			break;
+		case 3:
+			rt61pci_config_antenna_2529_rx(rt2x00dev, 1, 1);
+			break;
+		}
+	} else if (!rt2x00_get_field16(eeprom, EEPROM_NIC_ENABLE_DIVERSITY) &&
+		   !rt2x00_get_field16(eeprom, EEPROM_NIC_TX_DIVERSITY)) {
+		rt2x00_set_field8(&r4, BBP_R4_RX_ANTENNA, 1);
+		rt2x00_set_field8(&r4, BBP_R4_RX_FRAME_END, 0);
+
+		switch (rt2x00_get_field16(eeprom, EEPROM_NIC_TX_RX_FIXED)) {
+		case 0:
+			rt2x00_set_field8(&r77, BBP_R77_PAIR, 0);
+			rt61pci_bbp_write(rt2x00dev, 77, r77);
+			rt61pci_config_antenna_2529_rx(rt2x00dev, 0, 1);
+			break;
+		case 1:
+			rt2x00_set_field8(&r77, BBP_R77_PAIR, 0);
+			rt61pci_bbp_write(rt2x00dev, 77, r77);
+			rt61pci_config_antenna_2529_rx(rt2x00dev, 1, 0);
+			break;
+		case 2:
+			rt2x00_set_field8(&r77, BBP_R77_PAIR, 3);
+			rt61pci_bbp_write(rt2x00dev, 77, r77);
+			rt61pci_config_antenna_2529_rx(rt2x00dev, 0, 0);
+			break;
+		case 3:
+			rt2x00_set_field8(&r77, BBP_R77_PAIR, 3);
+			rt61pci_bbp_write(rt2x00dev, 77, r77);
+			rt61pci_config_antenna_2529_rx(rt2x00dev, 1, 1);
+			break;
+		}
+	}
+
+	rt61pci_bbp_write(rt2x00dev, 3, r3);
+	rt61pci_bbp_write(rt2x00dev, 4, r4);
+}
+
+struct antenna_sel {
+	u8 word;
+	/*
+	 * value[0] -> non-LNA
+	 * value[1] -> LNA
+	 */
+	u8 value[2];
+};
+
+static const struct antenna_sel antenna_sel_a[] = {
+	{ 96,  { 0x58, 0x78 } },
+	{ 104, { 0x38, 0x48 } },
+	{ 75,  { 0xfe, 0x80 } },
+	{ 86,  { 0xfe, 0x80 } },
+	{ 88,  { 0xfe, 0x80 } },
+	{ 35,  { 0x60, 0x60 } },
+	{ 97,  { 0x58, 0x58 } },
+	{ 98,  { 0x58, 0x58 } },
+};
+
+static const struct antenna_sel antenna_sel_bg[] = {
+	{ 96,  { 0x48, 0x68 } },
+	{ 104, { 0x2c, 0x3c } },
+	{ 75,  { 0xfe, 0x80 } },
+	{ 86,  { 0xfe, 0x80 } },
+	{ 88,  { 0xfe, 0x80 } },
+	{ 35,  { 0x50, 0x50 } },
+	{ 97,  { 0x48, 0x48 } },
+	{ 98,  { 0x48, 0x48 } },
+};
+
+static void rt61pci_config_antenna(struct rt2x00_dev *rt2x00dev,
+				   const int antenna_tx, const int antenna_rx)
+{
+	const struct antenna_sel *sel;
+	unsigned int lna;
+	unsigned int i;
+	u32 reg;
+
+	rt2x00pci_register_read(rt2x00dev, PHY_CSR0, &reg);
+
+	if (rt2x00dev->curr_hwmode == HWMODE_A) {
+		sel = antenna_sel_a;
+		lna = test_bit(CONFIG_EXTERNAL_LNA_A, &rt2x00dev->flags);
+
+		rt2x00_set_field32(&reg, PHY_CSR0_PA_PE_BG, 0);
+		rt2x00_set_field32(&reg, PHY_CSR0_PA_PE_A, 1);
+	} else {
+		sel = antenna_sel_bg;
+		lna = test_bit(CONFIG_EXTERNAL_LNA_BG, &rt2x00dev->flags);
+
+		rt2x00_set_field32(&reg, PHY_CSR0_PA_PE_BG, 1);
+		rt2x00_set_field32(&reg, PHY_CSR0_PA_PE_A, 0);
+	}
+
+	for (i = 0; i < ARRAY_SIZE(antenna_sel_a); i++)
+		rt61pci_bbp_write(rt2x00dev, sel[i].word, sel[i].value[lna]);
+
+	rt2x00pci_register_write(rt2x00dev, PHY_CSR0, reg);
+
+	if (rt2x00_rf(&rt2x00dev->chip, RF5225) ||
+	    rt2x00_rf(&rt2x00dev->chip, RF5325))
+		rt61pci_config_antenna_5x(rt2x00dev, antenna_tx, antenna_rx);
+	else if (rt2x00_rf(&rt2x00dev->chip, RF2527))
+		rt61pci_config_antenna_2x(rt2x00dev, antenna_tx, antenna_rx);
+	else if (rt2x00_rf(&rt2x00dev->chip, RF2529)) {
+		if (test_bit(CONFIG_DOUBLE_ANTENNA, &rt2x00dev->flags))
+			rt61pci_config_antenna_2x(rt2x00dev, antenna_tx,
+						  antenna_rx);
+		else
+			rt61pci_config_antenna_2529(rt2x00dev, antenna_tx,
+						    antenna_rx);
+	}
+}
+
+static void rt61pci_config_duration(struct rt2x00_dev *rt2x00dev,
+				    struct rt2x00lib_conf *libconf)
+{
+	u32 reg;
+
+	rt2x00pci_register_read(rt2x00dev, MAC_CSR9, &reg);
+	rt2x00_set_field32(&reg, MAC_CSR9_SLOT_TIME, libconf->slot_time);
+	rt2x00pci_register_write(rt2x00dev, MAC_CSR9, reg);
+
+	rt2x00pci_register_read(rt2x00dev, MAC_CSR8, &reg);
+	rt2x00_set_field32(&reg, MAC_CSR8_SIFS, libconf->sifs);
+	rt2x00_set_field32(&reg, MAC_CSR8_SIFS_AFTER_RX_OFDM, 3);
+	rt2x00_set_field32(&reg, MAC_CSR8_EIFS, libconf->eifs);
+	rt2x00pci_register_write(rt2x00dev, MAC_CSR8, reg);
+
+	rt2x00pci_register_read(rt2x00dev, TXRX_CSR0, &reg);
+	rt2x00_set_field32(&reg, TXRX_CSR0_TSF_OFFSET, IEEE80211_HEADER);
+	rt2x00pci_register_write(rt2x00dev, TXRX_CSR0, reg);
+
+	rt2x00pci_register_read(rt2x00dev, TXRX_CSR4, &reg);
+	rt2x00_set_field32(&reg, TXRX_CSR4_AUTORESPOND_ENABLE, 1);
+	rt2x00pci_register_write(rt2x00dev, TXRX_CSR4, reg);
+
+	rt2x00pci_register_read(rt2x00dev, TXRX_CSR9, &reg);
+	rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_INTERVAL,
+			   libconf->conf->beacon_int * 16);
+	rt2x00pci_register_write(rt2x00dev, TXRX_CSR9, reg);
+}
+
+static void rt61pci_config(struct rt2x00_dev *rt2x00dev,
+			   const unsigned int flags,
+			   struct rt2x00lib_conf *libconf)
+{
+	if (flags & CONFIG_UPDATE_PHYMODE)
+		rt61pci_config_phymode(rt2x00dev, libconf->basic_rates);
+	if (flags & CONFIG_UPDATE_CHANNEL)
+		rt61pci_config_channel(rt2x00dev, &libconf->rf,
+				       libconf->conf->power_level);
+	if ((flags & CONFIG_UPDATE_TXPOWER) && !(flags & CONFIG_UPDATE_CHANNEL))
+		rt61pci_config_txpower(rt2x00dev, libconf->conf->power_level);
+	if (flags & CONFIG_UPDATE_ANTENNA)
+		rt61pci_config_antenna(rt2x00dev, libconf->conf->antenna_sel_tx,
+				       libconf->conf->antenna_sel_rx);
+	if (flags & (CONFIG_UPDATE_SLOT_TIME | CONFIG_UPDATE_BEACON_INT))
+		rt61pci_config_duration(rt2x00dev, libconf);
+}
+
+/*
+ * LED functions.
+ */
+static void rt61pci_enable_led(struct rt2x00_dev *rt2x00dev)
+{
+	u32 reg;
+	u16 led_reg;
+	u8 arg0;
+	u8 arg1;
+
+	rt2x00pci_register_read(rt2x00dev, MAC_CSR14, &reg);
+	rt2x00_set_field32(&reg, MAC_CSR14_ON_PERIOD, 70);
+	rt2x00_set_field32(&reg, MAC_CSR14_OFF_PERIOD, 30);
+	rt2x00pci_register_write(rt2x00dev, MAC_CSR14, reg);
+
+	led_reg = rt2x00dev->led_reg;
+	rt2x00_set_field16(&led_reg, MCU_LEDCS_RADIO_STATUS, 1);
+	if (rt2x00dev->rx_status.phymode == MODE_IEEE80211A)
+		rt2x00_set_field16(&led_reg, MCU_LEDCS_LINK_A_STATUS, 1);
+	else
+		rt2x00_set_field16(&led_reg, MCU_LEDCS_LINK_BG_STATUS, 1);
+
+	arg0 = led_reg & 0xff;
+	arg1 = (led_reg >> 8) & 0xff;
+
+	rt61pci_mcu_request(rt2x00dev, MCU_LED, 0xff, arg0, arg1);
+}
+
+static void rt61pci_disable_led(struct rt2x00_dev *rt2x00dev)
+{
+	u16 led_reg;
+	u8 arg0;
+	u8 arg1;
+
+	led_reg = rt2x00dev->led_reg;
+	rt2x00_set_field16(&led_reg, MCU_LEDCS_RADIO_STATUS, 0);
+	rt2x00_set_field16(&led_reg, MCU_LEDCS_LINK_BG_STATUS, 0);
+	rt2x00_set_field16(&led_reg, MCU_LEDCS_LINK_A_STATUS, 0);
+
+	arg0 = led_reg & 0xff;
+	arg1 = (led_reg >> 8) & 0xff;
+
+	rt61pci_mcu_request(rt2x00dev, MCU_LED, 0xff, arg0, arg1);
+}
+
+static void rt61pci_activity_led(struct rt2x00_dev *rt2x00dev, int rssi)
+{
+	u8 led;
+
+	if (rt2x00dev->led_mode != LED_MODE_SIGNAL_STRENGTH)
+		return;
+
+	/*
+	 * Led handling requires a positive value for the rssi,
+	 * to do that correctly we need to add the correction.
+	 */
+	rssi += rt2x00dev->rssi_offset;
+
+	if (rssi <= 30)
+		led = 0;
+	else if (rssi <= 39)
+		led = 1;
+	else if (rssi <= 49)
+		led = 2;
+	else if (rssi <= 53)
+		led = 3;
+	else if (rssi <= 63)
+		led = 4;
+	else
+		led = 5;
+
+	rt61pci_mcu_request(rt2x00dev, MCU_LED_STRENGTH, 0xff, led, 0);
+}
+
+/*
+ * Link tuning
+ */
+static void rt61pci_link_stats(struct rt2x00_dev *rt2x00dev)
+{
+	u32 reg;
+
+	/*
+	 * Update FCS error count from register.
+	 */
+	rt2x00pci_register_read(rt2x00dev, STA_CSR0, &reg);
+	rt2x00dev->link.rx_failed = rt2x00_get_field32(reg, STA_CSR0_FCS_ERROR);
+
+	/*
+	 * Update False CCA count from register.
+	 */
+	rt2x00pci_register_read(rt2x00dev, STA_CSR1, &reg);
+	rt2x00dev->link.false_cca =
+	    rt2x00_get_field32(reg, STA_CSR1_FALSE_CCA_ERROR);
+}
+
+static void rt61pci_reset_tuner(struct rt2x00_dev *rt2x00dev)
+{
+	rt61pci_bbp_write(rt2x00dev, 17, 0x20);
+	rt2x00dev->link.vgc_level = 0x20;
+}
+
+static void rt61pci_link_tuner(struct rt2x00_dev *rt2x00dev)
+{
+	int rssi = rt2x00_get_link_rssi(&rt2x00dev->link);
+	u8 r17;
+	u8 up_bound;
+	u8 low_bound;
+
+	/*
+	 * Update Led strength
+	 */
+	rt61pci_activity_led(rt2x00dev, rssi);
+
+	rt61pci_bbp_read(rt2x00dev, 17, &r17);
+
+	/*
+	 * Determine r17 bounds.
+	 */
+	if (rt2x00dev->rx_status.phymode == MODE_IEEE80211A) {
+		low_bound = 0x28;
+		up_bound = 0x48;
+		if (test_bit(CONFIG_EXTERNAL_LNA_A, &rt2x00dev->flags)) {
+			low_bound += 0x10;
+			up_bound += 0x10;
+		}
+	} else {
+		low_bound = 0x20;
+		up_bound = 0x40;
+		if (test_bit(CONFIG_EXTERNAL_LNA_BG, &rt2x00dev->flags)) {
+			low_bound += 0x10;
+			up_bound += 0x10;
+		}
+	}
+
+	/*
+	 * Special big-R17 for very short distance
+	 */
+	if (rssi >= -35) {
+		if (r17 != 0x60)
+			rt61pci_bbp_write(rt2x00dev, 17, 0x60);
+		return;
+	}
+
+	/*
+	 * Special big-R17 for short distance
+	 */
+	if (rssi >= -58) {
+		if (r17 != up_bound)
+			rt61pci_bbp_write(rt2x00dev, 17, up_bound);
+		return;
+	}
+
+	/*
+	 * Special big-R17 for middle-short distance
+	 */
+	if (rssi >= -66) {
+		low_bound += 0x10;
+		if (r17 != low_bound)
+			rt61pci_bbp_write(rt2x00dev, 17, low_bound);
+		return;
+	}
+
+	/*
+	 * Special mid-R17 for middle distance
+	 */
+	if (rssi >= -74) {
+		low_bound += 0x08;
+		if (r17 != low_bound)
+			rt61pci_bbp_write(rt2x00dev, 17, low_bound);
+		return;
+	}
+
+	/*
+	 * Special case: Change up_bound based on the rssi.
+	 * Lower up_bound when rssi is weaker then -74 dBm.
+	 */
+	up_bound -= 2 * (-74 - rssi);
+	if (low_bound > up_bound)
+		up_bound = low_bound;
+
+	if (r17 > up_bound) {
+		rt61pci_bbp_write(rt2x00dev, 17, up_bound);
+		return;
+	}
+
+	/*
+	 * r17 does not yet exceed upper limit, continue and base
+	 * the r17 tuning on the false CCA count.
+	 */
+	if (rt2x00dev->link.false_cca > 512 && r17 < up_bound) {
+		if (++r17 > up_bound)
+			r17 = up_bound;
+		rt61pci_bbp_write(rt2x00dev, 17, r17);
+	} else if (rt2x00dev->link.false_cca < 100 && r17 > low_bound) {
+		if (--r17 < low_bound)
+			r17 = low_bound;
+		rt61pci_bbp_write(rt2x00dev, 17, r17);
+	}
+}
+
+/*
+ * Firmware name function.
+ */
+static char *rt61pci_get_firmware_name(struct rt2x00_dev *rt2x00dev)
+{
+	char *fw_name;
+
+	switch (rt2x00dev->chip.rt) {
+	case RT2561:
+		fw_name = FIRMWARE_RT2561;
+		break;
+	case RT2561s:
+		fw_name = FIRMWARE_RT2561s;
+		break;
+	case RT2661:
+		fw_name = FIRMWARE_RT2661;
+		break;
+	default:
+		fw_name = NULL;
+		break;
+	}
+
+	return fw_name;
+}
+
+/*
+ * Initialization functions.
+ */
+static int rt61pci_load_firmware(struct rt2x00_dev *rt2x00dev, void *data,
+				 const size_t len)
+{
+	int i;
+	u32 reg;
+
+	/*
+	 * Wait for stable hardware.
+	 */
+	for (i = 0; i < 100; i++) {
+		rt2x00pci_register_read(rt2x00dev, MAC_CSR0, &reg);
+		if (reg)
+			break;
+		msleep(1);
+	}
+
+	if (!reg) {
+		ERROR(rt2x00dev, "Unstable hardware.\n");
+		return -EBUSY;
+	}
+
+	/*
+	 * Prepare MCU and mailbox for firmware loading.
+	 */
+	reg = 0;
+	rt2x00_set_field32(&reg, MCU_CNTL_CSR_RESET, 1);
+	rt2x00pci_register_write(rt2x00dev, MCU_CNTL_CSR, reg);
+	rt2x00pci_register_write(rt2x00dev, M2H_CMD_DONE_CSR, 0xffffffff);
+	rt2x00pci_register_write(rt2x00dev, H2M_MAILBOX_CSR, 0);
+	rt2x00pci_register_write(rt2x00dev, HOST_CMD_CSR, 0);
+
+	/*
+	 * Write firmware to device.
+	 */
+	reg = 0;
+	rt2x00_set_field32(&reg, MCU_CNTL_CSR_RESET, 1);
+	rt2x00_set_field32(&reg, MCU_CNTL_CSR_SELECT_BANK, 1);
+	rt2x00pci_register_write(rt2x00dev, MCU_CNTL_CSR, reg);
+
+	rt2x00pci_register_multiwrite(rt2x00dev, FIRMWARE_IMAGE_BASE,
+				      data, len);
+
+	rt2x00_set_field32(&reg, MCU_CNTL_CSR_SELECT_BANK, 0);
+	rt2x00pci_register_write(rt2x00dev, MCU_CNTL_CSR, reg);
+
+	rt2x00_set_field32(&reg, MCU_CNTL_CSR_RESET, 0);
+	rt2x00pci_register_write(rt2x00dev, MCU_CNTL_CSR, reg);
+
+	for (i = 0; i < 100; i++) {
+		rt2x00pci_register_read(rt2x00dev, MCU_CNTL_CSR, &reg);
+		if (rt2x00_get_field32(reg, MCU_CNTL_CSR_READY))
+			break;
+		msleep(1);
+	}
+
+	if (i == 100) {
+		ERROR(rt2x00dev, "MCU Control register not ready.\n");
+		return -EBUSY;
+	}
+
+	/*
+	 * Reset MAC and BBP registers.
+	 */
+	reg = 0;
+	rt2x00_set_field32(&reg, MAC_CSR1_SOFT_RESET, 1);
+	rt2x00_set_field32(&reg, MAC_CSR1_BBP_RESET, 1);
+	rt2x00pci_register_write(rt2x00dev, MAC_CSR1, reg);
+
+	rt2x00pci_register_read(rt2x00dev, MAC_CSR1, &reg);
+	rt2x00_set_field32(&reg, MAC_CSR1_SOFT_RESET, 0);
+	rt2x00_set_field32(&reg, MAC_CSR1_BBP_RESET, 0);
+	rt2x00pci_register_write(rt2x00dev, MAC_CSR1, reg);
+
+	rt2x00pci_register_read(rt2x00dev, MAC_CSR1, &reg);
+	rt2x00_set_field32(&reg, MAC_CSR1_HOST_READY, 1);
+	rt2x00pci_register_write(rt2x00dev, MAC_CSR1, reg);
+
+	return 0;
+}
+
+static void rt61pci_init_rxring(struct rt2x00_dev *rt2x00dev)
+{
+	struct data_ring *ring = rt2x00dev->rx;
+	struct data_desc *rxd;
+	unsigned int i;
+	u32 word;
+
+	memset(ring->data_addr, 0x00, rt2x00_get_ring_size(ring));
+
+	for (i = 0; i < ring->stats.limit; i++) {
+		rxd = ring->entry[i].priv;
+
+		rt2x00_desc_read(rxd, 5, &word);
+		rt2x00_set_field32(&word, RXD_W5_BUFFER_PHYSICAL_ADDRESS,
+				   ring->entry[i].data_dma);
+		rt2x00_desc_write(rxd, 5, word);
+
+		rt2x00_desc_read(rxd, 0, &word);
+		rt2x00_set_field32(&word, RXD_W0_OWNER_NIC, 1);
+		rt2x00_desc_write(rxd, 0, word);
+	}
+
+	rt2x00_ring_index_clear(rt2x00dev->rx);
+}
+
+static void rt61pci_init_txring(struct rt2x00_dev *rt2x00dev, const int queue)
+{
+	struct data_ring *ring = rt2x00lib_get_ring(rt2x00dev, queue);
+	struct data_desc *txd;
+	unsigned int i;
+	u32 word;
+
+	memset(ring->data_addr, 0x00, rt2x00_get_ring_size(ring));
+
+	for (i = 0; i < ring->stats.limit; i++) {
+		txd = ring->entry[i].priv;
+
+		rt2x00_desc_read(txd, 1, &word);
+		rt2x00_set_field32(&word, TXD_W1_BUFFER_COUNT, 1);
+		rt2x00_desc_write(txd, 1, word);
+
+		rt2x00_desc_read(txd, 5, &word);
+		rt2x00_set_field32(&word, TXD_W5_PID_TYPE, queue);
+		rt2x00_set_field32(&word, TXD_W5_PID_SUBTYPE, i);
+		rt2x00_desc_write(txd, 5, word);
+
+		rt2x00_desc_read(txd, 6, &word);
+		rt2x00_set_field32(&word, TXD_W6_BUFFER_PHYSICAL_ADDRESS,
+				   ring->entry[i].data_dma);
+		rt2x00_desc_write(txd, 6, word);
+
+		rt2x00_desc_read(txd, 0, &word);
+		rt2x00_set_field32(&word, TXD_W0_VALID, 0);
+		rt2x00_set_field32(&word, TXD_W0_OWNER_NIC, 0);
+		rt2x00_desc_write(txd, 0, word);
+	}
+
+	rt2x00_ring_index_clear(ring);
+}
+
+static int rt61pci_init_rings(struct rt2x00_dev *rt2x00dev)
+{
+	u32 reg;
+
+	/*
+	 * Initialize rings.
+	 */
+	rt61pci_init_rxring(rt2x00dev);
+	rt61pci_init_txring(rt2x00dev, IEEE80211_TX_QUEUE_DATA0);
+	rt61pci_init_txring(rt2x00dev, IEEE80211_TX_QUEUE_DATA1);
+	rt61pci_init_txring(rt2x00dev, IEEE80211_TX_QUEUE_DATA2);
+	rt61pci_init_txring(rt2x00dev, IEEE80211_TX_QUEUE_DATA3);
+	rt61pci_init_txring(rt2x00dev, IEEE80211_TX_QUEUE_DATA4);
+
+	/*
+	 * Initialize registers.
+	 */
+	rt2x00pci_register_read(rt2x00dev, TX_RING_CSR0, &reg);
+	rt2x00_set_field32(&reg, TX_RING_CSR0_AC0_RING_SIZE,
+			   rt2x00dev->tx[IEEE80211_TX_QUEUE_DATA0].stats.limit);
+	rt2x00_set_field32(&reg, TX_RING_CSR0_AC1_RING_SIZE,
+			   rt2x00dev->tx[IEEE80211_TX_QUEUE_DATA1].stats.limit);
+	rt2x00_set_field32(&reg, TX_RING_CSR0_AC2_RING_SIZE,
+			   rt2x00dev->tx[IEEE80211_TX_QUEUE_DATA2].stats.limit);
+	rt2x00_set_field32(&reg, TX_RING_CSR0_AC3_RING_SIZE,
+			   rt2x00dev->tx[IEEE80211_TX_QUEUE_DATA3].stats.limit);
+	rt2x00pci_register_write(rt2x00dev, TX_RING_CSR0, reg);
+
+	rt2x00pci_register_read(rt2x00dev, TX_RING_CSR1, &reg);
+	rt2x00_set_field32(&reg, TX_RING_CSR1_MGMT_RING_SIZE,
+			   rt2x00dev->tx[IEEE80211_TX_QUEUE_DATA4].stats.limit);
+	rt2x00_set_field32(&reg, TX_RING_CSR1_TXD_SIZE,
+			   rt2x00dev->tx[IEEE80211_TX_QUEUE_DATA0].desc_size /
+			   4);
+	rt2x00pci_register_write(rt2x00dev, TX_RING_CSR1, reg);
+
+	rt2x00pci_register_read(rt2x00dev, AC0_BASE_CSR, &reg);
+	rt2x00_set_field32(&reg, AC0_BASE_CSR_RING_REGISTER,
+			   rt2x00dev->tx[IEEE80211_TX_QUEUE_DATA0].data_dma);
+	rt2x00pci_register_write(rt2x00dev, AC0_BASE_CSR, reg);
+
+	rt2x00pci_register_read(rt2x00dev, AC1_BASE_CSR, &reg);
+	rt2x00_set_field32(&reg, AC1_BASE_CSR_RING_REGISTER,
+			   rt2x00dev->tx[IEEE80211_TX_QUEUE_DATA1].data_dma);
+	rt2x00pci_register_write(rt2x00dev, AC1_BASE_CSR, reg);
+
+	rt2x00pci_register_read(rt2x00dev, AC2_BASE_CSR, &reg);
+	rt2x00_set_field32(&reg, AC2_BASE_CSR_RING_REGISTER,
+			   rt2x00dev->tx[IEEE80211_TX_QUEUE_DATA2].data_dma);
+	rt2x00pci_register_write(rt2x00dev, AC2_BASE_CSR, reg);
+
+	rt2x00pci_register_read(rt2x00dev, AC3_BASE_CSR, &reg);
+	rt2x00_set_field32(&reg, AC3_BASE_CSR_RING_REGISTER,
+			   rt2x00dev->tx[IEEE80211_TX_QUEUE_DATA3].data_dma);
+	rt2x00pci_register_write(rt2x00dev, AC3_BASE_CSR, reg);
+
+	rt2x00pci_register_read(rt2x00dev, MGMT_BASE_CSR, &reg);
+	rt2x00_set_field32(&reg, MGMT_BASE_CSR_RING_REGISTER,
+			   rt2x00dev->tx[IEEE80211_TX_QUEUE_DATA4].data_dma);
+	rt2x00pci_register_write(rt2x00dev, MGMT_BASE_CSR, reg);
+
+	rt2x00pci_register_read(rt2x00dev, RX_RING_CSR, &reg);
+	rt2x00_set_field32(&reg, RX_RING_CSR_RING_SIZE,
+			   rt2x00dev->rx->stats.limit);
+	rt2x00_set_field32(&reg, RX_RING_CSR_RXD_SIZE,
+			   rt2x00dev->rx->desc_size / 4);
+	rt2x00_set_field32(&reg, RX_RING_CSR_RXD_WRITEBACK_SIZE, 4);
+	rt2x00pci_register_write(rt2x00dev, RX_RING_CSR, reg);
+
+	rt2x00pci_register_read(rt2x00dev, RX_BASE_CSR, &reg);
+	rt2x00_set_field32(&reg, RX_BASE_CSR_RING_REGISTER,
+			   rt2x00dev->rx->data_dma);
+	rt2x00pci_register_write(rt2x00dev, RX_BASE_CSR, reg);
+
+	rt2x00pci_register_read(rt2x00dev, TX_DMA_DST_CSR, &reg);
+	rt2x00_set_field32(&reg, TX_DMA_DST_CSR_DEST_AC0, 2);
+	rt2x00_set_field32(&reg, TX_DMA_DST_CSR_DEST_AC1, 2);
+	rt2x00_set_field32(&reg, TX_DMA_DST_CSR_DEST_AC2, 2);
+	rt2x00_set_field32(&reg, TX_DMA_DST_CSR_DEST_AC3, 2);
+	rt2x00_set_field32(&reg, TX_DMA_DST_CSR_DEST_MGMT, 0);
+	rt2x00pci_register_write(rt2x00dev, TX_DMA_DST_CSR, reg);
+
+	rt2x00pci_register_read(rt2x00dev, LOAD_TX_RING_CSR, &reg);
+	rt2x00_set_field32(&reg, LOAD_TX_RING_CSR_LOAD_TXD_AC0, 1);
+	rt2x00_set_field32(&reg, LOAD_TX_RING_CSR_LOAD_TXD_AC1, 1);
+	rt2x00_set_field32(&reg, LOAD_TX_RING_CSR_LOAD_TXD_AC2, 1);
+	rt2x00_set_field32(&reg, LOAD_TX_RING_CSR_LOAD_TXD_AC3, 1);
+	rt2x00_set_field32(&reg, LOAD_TX_RING_CSR_LOAD_TXD_MGMT, 1);
+	rt2x00pci_register_write(rt2x00dev, LOAD_TX_RING_CSR, reg);
+
+	rt2x00pci_register_read(rt2x00dev, RX_CNTL_CSR, &reg);
+	rt2x00_set_field32(&reg, RX_CNTL_CSR_LOAD_RXD, 1);
+	rt2x00pci_register_write(rt2x00dev, RX_CNTL_CSR, reg);
+
+	return 0;
+}
+
+static int rt61pci_init_registers(struct rt2x00_dev *rt2x00dev)
+{
+	u32 reg;
+
+	rt2x00pci_register_read(rt2x00dev, TXRX_CSR0, &reg);
+	rt2x00_set_field32(&reg, TXRX_CSR0_AUTO_TX_SEQ, 1);
+	rt2x00_set_field32(&reg, TXRX_CSR0_DISABLE_RX, 0);
+	rt2x00_set_field32(&reg, TXRX_CSR0_TX_WITHOUT_WAITING, 0);
+	rt2x00pci_register_write(rt2x00dev, TXRX_CSR0, reg);
+
+	rt2x00pci_register_read(rt2x00dev, TXRX_CSR1, &reg);
+	rt2x00_set_field32(&reg, TXRX_CSR1_BBP_ID0, 47); /* CCK Signal */
+	rt2x00_set_field32(&reg, TXRX_CSR1_BBP_ID0_VALID, 1);
+	rt2x00_set_field32(&reg, TXRX_CSR1_BBP_ID1, 30); /* Rssi */
+	rt2x00_set_field32(&reg, TXRX_CSR1_BBP_ID1_VALID, 1);
+	rt2x00_set_field32(&reg, TXRX_CSR1_BBP_ID2, 42); /* OFDM Rate */
+	rt2x00_set_field32(&reg, TXRX_CSR1_BBP_ID2_VALID, 1);
+	rt2x00_set_field32(&reg, TXRX_CSR1_BBP_ID3, 30); /* Rssi */
+	rt2x00_set_field32(&reg, TXRX_CSR1_BBP_ID3_VALID, 1);
+	rt2x00pci_register_write(rt2x00dev, TXRX_CSR1, reg);
+
+	/*
+	 * CCK TXD BBP registers
+	 */
+	rt2x00pci_register_read(rt2x00dev, TXRX_CSR2, &reg);
+	rt2x00_set_field32(&reg, TXRX_CSR2_BBP_ID0, 13);
+	rt2x00_set_field32(&reg, TXRX_CSR2_BBP_ID0_VALID, 1);
+	rt2x00_set_field32(&reg, TXRX_CSR2_BBP_ID1, 12);
+	rt2x00_set_field32(&reg, TXRX_CSR2_BBP_ID1_VALID, 1);
+	rt2x00_set_field32(&reg, TXRX_CSR2_BBP_ID2, 11);
+	rt2x00_set_field32(&reg, TXRX_CSR2_BBP_ID2_VALID, 1);
+	rt2x00_set_field32(&reg, TXRX_CSR2_BBP_ID3, 10);
+	rt2x00_set_field32(&reg, TXRX_CSR2_BBP_ID3_VALID, 1);
+	rt2x00pci_register_write(rt2x00dev, TXRX_CSR2, reg);
+
+	/*
+	 * OFDM TXD BBP registers
+	 */
+	rt2x00pci_register_read(rt2x00dev, TXRX_CSR3, &reg);
+	rt2x00_set_field32(&reg, TXRX_CSR3_BBP_ID0, 7);
+	rt2x00_set_field32(&reg, TXRX_CSR3_BBP_ID0_VALID, 1);
+	rt2x00_set_field32(&reg, TXRX_CSR3_BBP_ID1, 6);
+	rt2x00_set_field32(&reg, TXRX_CSR3_BBP_ID1_VALID, 1);
+	rt2x00_set_field32(&reg, TXRX_CSR3_BBP_ID2, 5);
+	rt2x00_set_field32(&reg, TXRX_CSR3_BBP_ID2_VALID, 1);
+	rt2x00pci_register_write(rt2x00dev, TXRX_CSR3, reg);
+
+	rt2x00pci_register_read(rt2x00dev, TXRX_CSR7, &reg);
+	rt2x00_set_field32(&reg, TXRX_CSR7_ACK_CTS_6MBS, 59);
+	rt2x00_set_field32(&reg, TXRX_CSR7_ACK_CTS_9MBS, 53);
+	rt2x00_set_field32(&reg, TXRX_CSR7_ACK_CTS_12MBS, 49);
+	rt2x00_set_field32(&reg, TXRX_CSR7_ACK_CTS_18MBS, 46);
+	rt2x00pci_register_write(rt2x00dev, TXRX_CSR7, reg);
+
+	rt2x00pci_register_read(rt2x00dev, TXRX_CSR8, &reg);
+	rt2x00_set_field32(&reg, TXRX_CSR8_ACK_CTS_24MBS, 44);
+	rt2x00_set_field32(&reg, TXRX_CSR8_ACK_CTS_36MBS, 42);
+	rt2x00_set_field32(&reg, TXRX_CSR8_ACK_CTS_48MBS, 42);
+	rt2x00_set_field32(&reg, TXRX_CSR8_ACK_CTS_54MBS, 42);
+	rt2x00pci_register_write(rt2x00dev, TXRX_CSR8, reg);
+
+	rt2x00pci_register_write(rt2x00dev, TXRX_CSR15, 0x0000000f);
+
+	rt2x00pci_register_write(rt2x00dev, MAC_CSR6, 0x00000fff);
+
+	rt2x00pci_register_read(rt2x00dev, MAC_CSR9, &reg);
+	rt2x00_set_field32(&reg, MAC_CSR9_CW_SELECT, 0);
+	rt2x00pci_register_write(rt2x00dev, MAC_CSR9, reg);
+
+	rt2x00pci_register_write(rt2x00dev, MAC_CSR10, 0x0000071c);
+
+	if (rt2x00dev->ops->lib->set_device_state(rt2x00dev, STATE_AWAKE))
+		return -EBUSY;
+
+	rt2x00pci_register_write(rt2x00dev, MAC_CSR13, 0x0000e000);
+
+	/*
+	 * Invalidate all Shared Keys (SEC_CSR0),
+	 * and clear the Shared key Cipher algorithms (SEC_CSR1 & SEC_CSR5)
+	 */
+	rt2x00pci_register_write(rt2x00dev, SEC_CSR0, 0x00000000);
+	rt2x00pci_register_write(rt2x00dev, SEC_CSR1, 0x00000000);
+	rt2x00pci_register_write(rt2x00dev, SEC_CSR5, 0x00000000);
+
+	rt2x00pci_register_write(rt2x00dev, PHY_CSR1, 0x000023b0);
+	rt2x00pci_register_write(rt2x00dev, PHY_CSR5, 0x060a100c);
+	rt2x00pci_register_write(rt2x00dev, PHY_CSR6, 0x00080606);
+	rt2x00pci_register_write(rt2x00dev, PHY_CSR7, 0x00000a08);
+
+	rt2x00pci_register_write(rt2x00dev, PCI_CFG_CSR, 0x28ca4404);
+
+	rt2x00pci_register_write(rt2x00dev, TEST_MODE_CSR, 0x00000200);
+
+	rt2x00pci_register_write(rt2x00dev, M2H_CMD_DONE_CSR, 0xffffffff);
+
+	rt2x00pci_register_read(rt2x00dev, AC_TXOP_CSR0, &reg);
+	rt2x00_set_field32(&reg, AC_TXOP_CSR0_AC0_TX_OP, 0);
+	rt2x00_set_field32(&reg, AC_TXOP_CSR0_AC1_TX_OP, 0);
+	rt2x00pci_register_write(rt2x00dev, AC_TXOP_CSR0, reg);
+
+	rt2x00pci_register_read(rt2x00dev, AC_TXOP_CSR1, &reg);
+	rt2x00_set_field32(&reg, AC_TXOP_CSR1_AC2_TX_OP, 192);
+	rt2x00_set_field32(&reg, AC_TXOP_CSR1_AC3_TX_OP, 48);
+	rt2x00pci_register_write(rt2x00dev, AC_TXOP_CSR1, reg);
+
+	/*
+	 * We must clear the error counters.
+	 * These registers are cleared on read,
+	 * so we may pass a useless variable to store the value.
+	 */
+	rt2x00pci_register_read(rt2x00dev, STA_CSR0, &reg);
+	rt2x00pci_register_read(rt2x00dev, STA_CSR1, &reg);
+	rt2x00pci_register_read(rt2x00dev, STA_CSR2, &reg);
+
+	/*
+	 * Reset MAC and BBP registers.
+	 */
+	rt2x00pci_register_read(rt2x00dev, MAC_CSR1, &reg);
+	rt2x00_set_field32(&reg, MAC_CSR1_SOFT_RESET, 1);
+	rt2x00_set_field32(&reg, MAC_CSR1_BBP_RESET, 1);
+	rt2x00pci_register_write(rt2x00dev, MAC_CSR1, reg);
+
+	rt2x00pci_register_read(rt2x00dev, MAC_CSR1, &reg);
+	rt2x00_set_field32(&reg, MAC_CSR1_SOFT_RESET, 0);
+	rt2x00_set_field32(&reg, MAC_CSR1_BBP_RESET, 0);
+	rt2x00pci_register_write(rt2x00dev, MAC_CSR1, reg);
+
+	rt2x00pci_register_read(rt2x00dev, MAC_CSR1, &reg);
+	rt2x00_set_field32(&reg, MAC_CSR1_HOST_READY, 1);
+	rt2x00pci_register_write(rt2x00dev, MAC_CSR1, reg);
+
+	return 0;
+}
+
+static int rt61pci_init_bbp(struct rt2x00_dev *rt2x00dev)
+{
+	unsigned int i;
+	u16 eeprom;
+	u8 reg_id;
+	u8 value;
+
+	for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
+		rt61pci_bbp_read(rt2x00dev, 0, &value);
+		if ((value != 0xff) && (value != 0x00))
+			goto continue_csr_init;
+		NOTICE(rt2x00dev, "Waiting for BBP register.\n");
+		udelay(REGISTER_BUSY_DELAY);
+	}
+
+	ERROR(rt2x00dev, "BBP register access failed, aborting.\n");
+	return -EACCES;
+
+continue_csr_init:
+	rt61pci_bbp_write(rt2x00dev, 3, 0x00);
+	rt61pci_bbp_write(rt2x00dev, 15, 0x30);
+	rt61pci_bbp_write(rt2x00dev, 21, 0xc8);
+	rt61pci_bbp_write(rt2x00dev, 22, 0x38);
+	rt61pci_bbp_write(rt2x00dev, 23, 0x06);
+	rt61pci_bbp_write(rt2x00dev, 24, 0xfe);
+	rt61pci_bbp_write(rt2x00dev, 25, 0x0a);
+	rt61pci_bbp_write(rt2x00dev, 26, 0x0d);
+	rt61pci_bbp_write(rt2x00dev, 34, 0x12);
+	rt61pci_bbp_write(rt2x00dev, 37, 0x07);
+	rt61pci_bbp_write(rt2x00dev, 39, 0xf8);
+	rt61pci_bbp_write(rt2x00dev, 41, 0x60);
+	rt61pci_bbp_write(rt2x00dev, 53, 0x10);
+	rt61pci_bbp_write(rt2x00dev, 54, 0x18);
+	rt61pci_bbp_write(rt2x00dev, 60, 0x10);
+	rt61pci_bbp_write(rt2x00dev, 61, 0x04);
+	rt61pci_bbp_write(rt2x00dev, 62, 0x04);
+	rt61pci_bbp_write(rt2x00dev, 75, 0xfe);
+	rt61pci_bbp_write(rt2x00dev, 86, 0xfe);
+	rt61pci_bbp_write(rt2x00dev, 88, 0xfe);
+	rt61pci_bbp_write(rt2x00dev, 90, 0x0f);
+	rt61pci_bbp_write(rt2x00dev, 99, 0x00);
+	rt61pci_bbp_write(rt2x00dev, 102, 0x16);
+	rt61pci_bbp_write(rt2x00dev, 107, 0x04);
+
+	DEBUG(rt2x00dev, "Start initialization from EEPROM...\n");
+	for (i = 0; i < EEPROM_BBP_SIZE; i++) {
+		rt2x00_eeprom_read(rt2x00dev, EEPROM_BBP_START + i, &eeprom);
+
+		if (eeprom != 0xffff && eeprom != 0x0000) {
+			reg_id = rt2x00_get_field16(eeprom, EEPROM_BBP_REG_ID);
+			value = rt2x00_get_field16(eeprom, EEPROM_BBP_VALUE);
+			DEBUG(rt2x00dev, "BBP: 0x%02x, value: 0x%02x.\n",
+			      reg_id, value);
+			rt61pci_bbp_write(rt2x00dev, reg_id, value);
+		}
+	}
+	DEBUG(rt2x00dev, "...End initialization from EEPROM.\n");
+
+	return 0;
+}
+
+/*
+ * Device state switch handlers.
+ */
+static void rt61pci_toggle_rx(struct rt2x00_dev *rt2x00dev,
+			      enum dev_state state)
+{
+	u32 reg;
+
+	rt2x00pci_register_read(rt2x00dev, TXRX_CSR0, &reg);
+	rt2x00_set_field32(&reg, TXRX_CSR0_DISABLE_RX,
+			   state == STATE_RADIO_RX_OFF);
+	rt2x00pci_register_write(rt2x00dev, TXRX_CSR0, reg);
+}
+
+static void rt61pci_toggle_irq(struct rt2x00_dev *rt2x00dev,
+			       enum dev_state state)
+{
+	int mask = (state == STATE_RADIO_IRQ_OFF);
+	u32 reg;
+
+	/*
+	 * When interrupts are being enabled, the interrupt registers
+	 * should clear the register to assure a clean state.
+	 */
+	if (state == STATE_RADIO_IRQ_ON) {
+		rt2x00pci_register_read(rt2x00dev, INT_SOURCE_CSR, &reg);
+		rt2x00pci_register_write(rt2x00dev, INT_SOURCE_CSR, reg);
+
+		rt2x00pci_register_read(rt2x00dev, MCU_INT_SOURCE_CSR, &reg);
+		rt2x00pci_register_write(rt2x00dev, MCU_INT_SOURCE_CSR, reg);
+	}
+
+	/*
+	 * Only toggle the interrupts bits we are going to use.
+	 * Non-checked interrupt bits are disabled by default.
+	 */
+	rt2x00pci_register_read(rt2x00dev, INT_MASK_CSR, &reg);
+	rt2x00_set_field32(&reg, INT_MASK_CSR_TXDONE, mask);
+	rt2x00_set_field32(&reg, INT_MASK_CSR_RXDONE, mask);
+	rt2x00_set_field32(&reg, INT_MASK_CSR_ENABLE_MITIGATION, mask);
+	rt2x00_set_field32(&reg, INT_MASK_CSR_MITIGATION_PERIOD, 0xff);
+	rt2x00pci_register_write(rt2x00dev, INT_MASK_CSR, reg);
+
+	rt2x00pci_register_read(rt2x00dev, MCU_INT_MASK_CSR, &reg);
+	rt2x00_set_field32(&reg, MCU_INT_MASK_CSR_0, mask);
+	rt2x00_set_field32(&reg, MCU_INT_MASK_CSR_1, mask);
+	rt2x00_set_field32(&reg, MCU_INT_MASK_CSR_2, mask);
+	rt2x00_set_field32(&reg, MCU_INT_MASK_CSR_3, mask);
+	rt2x00_set_field32(&reg, MCU_INT_MASK_CSR_4, mask);
+	rt2x00_set_field32(&reg, MCU_INT_MASK_CSR_5, mask);
+	rt2x00_set_field32(&reg, MCU_INT_MASK_CSR_6, mask);
+	rt2x00_set_field32(&reg, MCU_INT_MASK_CSR_7, mask);
+	rt2x00pci_register_write(rt2x00dev, MCU_INT_MASK_CSR, reg);
+}
+
+static int rt61pci_enable_radio(struct rt2x00_dev *rt2x00dev)
+{
+	u32 reg;
+
+	/*
+	 * Initialize all registers.
+	 */
+	if (rt61pci_init_rings(rt2x00dev) ||
+	    rt61pci_init_registers(rt2x00dev) ||
+	    rt61pci_init_bbp(rt2x00dev)) {
+		ERROR(rt2x00dev, "Register initialization failed.\n");
+		return -EIO;
+	}
+
+	/*
+	 * Enable interrupts.
+	 */
+	rt61pci_toggle_irq(rt2x00dev, STATE_RADIO_IRQ_ON);
+
+	/*
+	 * Enable RX.
+	 */
+	rt2x00pci_register_read(rt2x00dev, RX_CNTL_CSR, &reg);
+	rt2x00_set_field32(&reg, RX_CNTL_CSR_ENABLE_RX_DMA, 1);
+	rt2x00pci_register_write(rt2x00dev, RX_CNTL_CSR, reg);
+
+	/*
+	 * Enable LED
+	 */
+	rt61pci_enable_led(rt2x00dev);
+
+	return 0;
+}
+
+static void rt61pci_disable_radio(struct rt2x00_dev *rt2x00dev)
+{
+	u32 reg;
+
+	/*
+	 * Disable LED
+	 */
+	rt61pci_disable_led(rt2x00dev);
+
+	rt2x00pci_register_write(rt2x00dev, MAC_CSR10, 0x00001818);
+
+	/*
+	 * Disable synchronisation.
+	 */
+	rt2x00pci_register_write(rt2x00dev, TXRX_CSR9, 0);
+
+	/*
+	 * Cancel RX and TX.
+	 */
+	rt2x00pci_register_read(rt2x00dev, TX_CNTL_CSR, &reg);
+	rt2x00_set_field32(&reg, TX_CNTL_CSR_ABORT_TX_AC0, 1);
+	rt2x00_set_field32(&reg, TX_CNTL_CSR_ABORT_TX_AC1, 1);
+	rt2x00_set_field32(&reg, TX_CNTL_CSR_ABORT_TX_AC2, 1);
+	rt2x00_set_field32(&reg, TX_CNTL_CSR_ABORT_TX_AC3, 1);
+	rt2x00_set_field32(&reg, TX_CNTL_CSR_ABORT_TX_MGMT, 1);
+	rt2x00pci_register_write(rt2x00dev, TX_CNTL_CSR, reg);
+
+	/*
+	 * Disable interrupts.
+	 */
+	rt61pci_toggle_irq(rt2x00dev, STATE_RADIO_IRQ_OFF);
+}
+
+static int rt61pci_set_state(struct rt2x00_dev *rt2x00dev, enum dev_state state)
+{
+	u32 reg;
+	unsigned int i;
+	char put_to_sleep;
+	char current_state;
+
+	put_to_sleep = (state != STATE_AWAKE);
+
+	rt2x00pci_register_read(rt2x00dev, MAC_CSR12, &reg);
+	rt2x00_set_field32(&reg, MAC_CSR12_FORCE_WAKEUP, !put_to_sleep);
+	rt2x00_set_field32(&reg, MAC_CSR12_PUT_TO_SLEEP, put_to_sleep);
+	rt2x00pci_register_write(rt2x00dev, MAC_CSR12, reg);
+
+	/*
+	 * Device is not guaranteed to be in the requested state yet.
+	 * We must wait until the register indicates that the
+	 * device has entered the correct state.
+	 */
+	for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
+		rt2x00pci_register_read(rt2x00dev, MAC_CSR12, &reg);
+		current_state =
+		    rt2x00_get_field32(reg, MAC_CSR12_BBP_CURRENT_STATE);
+		if (current_state == !put_to_sleep)
+			return 0;
+		msleep(10);
+	}
+
+	NOTICE(rt2x00dev, "Device failed to enter state %d, "
+	       "current device state %d.\n", !put_to_sleep, current_state);
+
+	return -EBUSY;
+}
+
+static int rt61pci_set_device_state(struct rt2x00_dev *rt2x00dev,
+				    enum dev_state state)
+{
+	int retval = 0;
+
+	switch (state) {
+	case STATE_RADIO_ON:
+		retval = rt61pci_enable_radio(rt2x00dev);
+		break;
+	case STATE_RADIO_OFF:
+		rt61pci_disable_radio(rt2x00dev);
+		break;
+	case STATE_RADIO_RX_ON:
+	case STATE_RADIO_RX_OFF:
+		rt61pci_toggle_rx(rt2x00dev, state);
+		break;
+	case STATE_DEEP_SLEEP:
+	case STATE_SLEEP:
+	case STATE_STANDBY:
+	case STATE_AWAKE:
+		retval = rt61pci_set_state(rt2x00dev, state);
+		break;
+	default:
+		retval = -ENOTSUPP;
+		break;
+	}
+
+	return retval;
+}
+
+/*
+ * TX descriptor initialization
+ */
+static void rt61pci_write_tx_desc(struct rt2x00_dev *rt2x00dev,
+				  struct data_desc *txd,
+				  struct txdata_entry_desc *desc,
+				  struct ieee80211_hdr *ieee80211hdr,
+				  unsigned int length,
+				  struct ieee80211_tx_control *control)
+{
+	u32 word;
+
+	/*
+	 * Start writing the descriptor words.
+	 */
+	rt2x00_desc_read(txd, 1, &word);
+	rt2x00_set_field32(&word, TXD_W1_HOST_Q_ID, desc->queue);
+	rt2x00_set_field32(&word, TXD_W1_AIFSN, desc->aifs);
+	rt2x00_set_field32(&word, TXD_W1_CWMIN, desc->cw_min);
+	rt2x00_set_field32(&word, TXD_W1_CWMAX, desc->cw_max);
+	rt2x00_set_field32(&word, TXD_W1_IV_OFFSET, IEEE80211_HEADER);
+	rt2x00_set_field32(&word, TXD_W1_HW_SEQUENCE, 1);
+	rt2x00_desc_write(txd, 1, word);
+
+	rt2x00_desc_read(txd, 2, &word);
+	rt2x00_set_field32(&word, TXD_W2_PLCP_SIGNAL, desc->signal);
+	rt2x00_set_field32(&word, TXD_W2_PLCP_SERVICE, desc->service);
+	rt2x00_set_field32(&word, TXD_W2_PLCP_LENGTH_LOW, desc->length_low);
+	rt2x00_set_field32(&word, TXD_W2_PLCP_LENGTH_HIGH, desc->length_high);
+	rt2x00_desc_write(txd, 2, word);
+
+	rt2x00_desc_read(txd, 5, &word);
+	rt2x00_set_field32(&word, TXD_W5_TX_POWER,
+			   TXPOWER_TO_DEV(control->power_level));
+	rt2x00_set_field32(&word, TXD_W5_WAITING_DMA_DONE_INT, 1);
+	rt2x00_desc_write(txd, 5, word);
+
+	rt2x00_desc_read(txd, 11, &word);
+	rt2x00_set_field32(&word, TXD_W11_BUFFER_LENGTH0, length);
+	rt2x00_desc_write(txd, 11, word);
+
+	rt2x00_desc_read(txd, 0, &word);
+	rt2x00_set_field32(&word, TXD_W0_OWNER_NIC, 1);
+	rt2x00_set_field32(&word, TXD_W0_VALID, 1);
+	rt2x00_set_field32(&word, TXD_W0_MORE_FRAG,
+			   test_bit(ENTRY_TXD_MORE_FRAG, &desc->flags));
+	rt2x00_set_field32(&word, TXD_W0_ACK,
+			   !(control->flags & IEEE80211_TXCTL_NO_ACK));
+	rt2x00_set_field32(&word, TXD_W0_TIMESTAMP,
+			   test_bit(ENTRY_TXD_REQ_TIMESTAMP, &desc->flags));
+	rt2x00_set_field32(&word, TXD_W0_OFDM,
+			   test_bit(ENTRY_TXD_OFDM_RATE, &desc->flags));
+	rt2x00_set_field32(&word, TXD_W0_IFS, desc->ifs);
+	rt2x00_set_field32(&word, TXD_W0_RETRY_MODE,
+			   !!(control->flags &
+			      IEEE80211_TXCTL_LONG_RETRY_LIMIT));
+	rt2x00_set_field32(&word, TXD_W0_TKIP_MIC, 0);
+	rt2x00_set_field32(&word, TXD_W0_DATABYTE_COUNT, length);
+	rt2x00_set_field32(&word, TXD_W0_BURST,
+			   test_bit(ENTRY_TXD_BURST, &desc->flags));
+	rt2x00_set_field32(&word, TXD_W0_CIPHER_ALG, CIPHER_NONE);
+	rt2x00_desc_write(txd, 0, word);
+}
+
+/*
+ * TX data initialization
+ */
+static void rt61pci_kick_tx_queue(struct rt2x00_dev *rt2x00dev,
+				  unsigned int queue)
+{
+	u32 reg;
+
+	if (queue == IEEE80211_TX_QUEUE_BEACON) {
+		/*
+		 * For Wi-Fi faily generated beacons between participating
+		 * stations. Set TBTT phase adaptive adjustment step to 8us.
+		 */
+		rt2x00pci_register_write(rt2x00dev, TXRX_CSR10, 0x00001008);
+
+		rt2x00pci_register_read(rt2x00dev, TXRX_CSR9, &reg);
+		if (!rt2x00_get_field32(reg, TXRX_CSR9_BEACON_GEN)) {
+			rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_GEN, 1);
+			rt2x00pci_register_write(rt2x00dev, TXRX_CSR9, reg);
+		}
+		return;
+	}
+
+	rt2x00pci_register_read(rt2x00dev, TX_CNTL_CSR, &reg);
+	if (queue == IEEE80211_TX_QUEUE_DATA0)
+		rt2x00_set_field32(&reg, TX_CNTL_CSR_KICK_TX_AC0, 1);
+	else if (queue == IEEE80211_TX_QUEUE_DATA1)
+		rt2x00_set_field32(&reg, TX_CNTL_CSR_KICK_TX_AC1, 1);
+	else if (queue == IEEE80211_TX_QUEUE_DATA2)
+		rt2x00_set_field32(&reg, TX_CNTL_CSR_KICK_TX_AC2, 1);
+	else if (queue == IEEE80211_TX_QUEUE_DATA3)
+		rt2x00_set_field32(&reg, TX_CNTL_CSR_KICK_TX_AC3, 1);
+	else if (queue == IEEE80211_TX_QUEUE_DATA4)
+		rt2x00_set_field32(&reg, TX_CNTL_CSR_KICK_TX_MGMT, 1);
+	rt2x00pci_register_write(rt2x00dev, TX_CNTL_CSR, reg);
+}
+
+/*
+ * RX control handlers
+ */
+static int rt61pci_agc_to_rssi(struct rt2x00_dev *rt2x00dev, int rxd_w1)
+{
+	u16 eeprom;
+	u8 offset;
+	u8 lna;
+
+	lna = rt2x00_get_field32(rxd_w1, RXD_W1_RSSI_LNA);
+	switch (lna) {
+	case 3:
+		offset = 90;
+		break;
+	case 2:
+		offset = 74;
+		break;
+	case 1:
+		offset = 64;
+		break;
+	default:
+		return 0;
+	}
+
+	if (rt2x00dev->rx_status.phymode == MODE_IEEE80211A) {
+		if (test_bit(CONFIG_EXTERNAL_LNA_A, &rt2x00dev->flags))
+			offset += 14;
+
+		if (lna == 3 || lna == 2)
+			offset += 10;
+
+		rt2x00_eeprom_read(rt2x00dev, EEPROM_RSSI_OFFSET_A, &eeprom);
+		offset -= rt2x00_get_field16(eeprom, EEPROM_RSSI_OFFSET_A_1);
+	} else {
+		if (test_bit(CONFIG_EXTERNAL_LNA_BG, &rt2x00dev->flags))
+			offset += 14;
+
+		rt2x00_eeprom_read(rt2x00dev, EEPROM_RSSI_OFFSET_BG, &eeprom);
+		offset -= rt2x00_get_field16(eeprom, EEPROM_RSSI_OFFSET_BG_1);
+	}
+
+	return rt2x00_get_field32(rxd_w1, RXD_W1_RSSI_AGC) * 2 - offset;
+}
+
+static void rt61pci_fill_rxdone(struct data_entry *entry,
+			        struct rxdata_entry_desc *desc)
+{
+	struct data_desc *rxd = entry->priv;
+	u32 word0;
+	u32 word1;
+
+	rt2x00_desc_read(rxd, 0, &word0);
+	rt2x00_desc_read(rxd, 1, &word1);
+
+	desc->flags = 0;
+	if (rt2x00_get_field32(word0, RXD_W0_CRC_ERROR))
+		desc->flags |= RX_FLAG_FAILED_FCS_CRC;
+
+	/*
+	 * Obtain the status about this packet.
+	 */
+	desc->signal = rt2x00_get_field32(word1, RXD_W1_SIGNAL);
+	desc->rssi = rt61pci_agc_to_rssi(entry->ring->rt2x00dev, word1);
+	desc->ofdm = rt2x00_get_field32(word0, RXD_W0_OFDM);
+	desc->size = rt2x00_get_field32(word0, RXD_W0_DATABYTE_COUNT);
+
+	return;
+}
+
+/*
+ * Interrupt functions.
+ */
+static void rt61pci_txdone(struct rt2x00_dev *rt2x00dev)
+{
+	struct data_ring *ring;
+	struct data_entry *entry;
+	struct data_desc *txd;
+	u32 word;
+	u32 reg;
+	u32 old_reg;
+	int type;
+	int index;
+	int tx_status;
+	int retry;
+
+	/*
+	 * During each loop we will compare the freshly read
+	 * STA_CSR4 register value with the value read from
+	 * the previous loop. If the 2 values are equal then
+	 * we should stop processing because the chance it
+	 * quite big that the device has been unplugged and
+	 * we risk going into an endless loop.
+	 */
+	old_reg = 0;
+
+	while (1) {
+		rt2x00pci_register_read(rt2x00dev, STA_CSR4, &reg);
+		if (!rt2x00_get_field32(reg, STA_CSR4_VALID))
+			break;
+
+		if (old_reg == reg)
+			break;
+		old_reg = reg;
+
+		/*
+		 * Skip this entry when it contains an invalid
+		 * ring identication number.
+		 */
+		type = rt2x00_get_field32(reg, STA_CSR4_PID_TYPE);
+		ring = rt2x00lib_get_ring(rt2x00dev, type);
+		if (unlikely(!ring))
+			continue;
+
+		/*
+		 * Skip this entry when it contains an invalid
+		 * index number.
+		 */
+		index = rt2x00_get_field32(reg, STA_CSR4_PID_SUBTYPE);
+		if (unlikely(index >= ring->stats.limit))
+			continue;
+
+		entry = &ring->entry[index];
+		txd = entry->priv;
+		rt2x00_desc_read(txd, 0, &word);
+
+		if (rt2x00_get_field32(word, TXD_W0_OWNER_NIC) ||
+		    !rt2x00_get_field32(word, TXD_W0_VALID))
+			return;
+
+		/*
+		 * Obtain the status about this packet.
+		 */
+		tx_status = rt2x00_get_field32(reg, STA_CSR4_TX_RESULT);
+		retry = rt2x00_get_field32(reg, STA_CSR4_RETRY_COUNT);
+
+		rt2x00lib_txdone(entry, tx_status, retry);
+
+		/*
+		 * Make this entry available for reuse.
+		 */
+		entry->flags = 0;
+		rt2x00_set_field32(&word, TXD_W0_VALID, 0);
+		rt2x00_desc_write(txd, 0, word);
+		rt2x00_ring_index_done_inc(entry->ring);
+
+		/*
+		 * If the data ring was full before the txdone handler
+		 * we must make sure the packet queue in the mac80211 stack
+		 * is reenabled when the txdone handler has finished.
+		 */
+		if (!rt2x00_ring_full(ring))
+			ieee80211_wake_queue(rt2x00dev->hw,
+					     entry->tx_status.control.queue);
+	}
+}
+
+static irqreturn_t rt61pci_interrupt(int irq, void *dev_instance)
+{
+	struct rt2x00_dev *rt2x00dev = dev_instance;
+	u32 reg_mcu;
+	u32 reg;
+
+	/*
+	 * Get the interrupt sources & saved to local variable.
+	 * Write register value back to clear pending interrupts.
+	 */
+	rt2x00pci_register_read(rt2x00dev, MCU_INT_SOURCE_CSR, &reg_mcu);
+	rt2x00pci_register_write(rt2x00dev, MCU_INT_SOURCE_CSR, reg_mcu);
+
+	rt2x00pci_register_read(rt2x00dev, INT_SOURCE_CSR, &reg);
+	rt2x00pci_register_write(rt2x00dev, INT_SOURCE_CSR, reg);
+
+	if (!reg && !reg_mcu)
+		return IRQ_NONE;
+
+	if (!test_bit(DEVICE_ENABLED_RADIO, &rt2x00dev->flags))
+		return IRQ_HANDLED;
+
+	/*
+	 * Handle interrupts, walk through all bits
+	 * and run the tasks, the bits are checked in order of
+	 * priority.
+	 */
+
+	/*
+	 * 1 - Rx ring done interrupt.
+	 */
+	if (rt2x00_get_field32(reg, INT_SOURCE_CSR_RXDONE))
+		rt2x00pci_rxdone(rt2x00dev);
+
+	/*
+	 * 2 - Tx ring done interrupt.
+	 */
+	if (rt2x00_get_field32(reg, INT_SOURCE_CSR_TXDONE))
+		rt61pci_txdone(rt2x00dev);
+
+	/*
+	 * 3 - Handle MCU command done.
+	 */
+	if (reg_mcu)
+		rt2x00pci_register_write(rt2x00dev,
+					 M2H_CMD_DONE_CSR, 0xffffffff);
+
+	return IRQ_HANDLED;
+}
+
+/*
+ * Device probe functions.
+ */
+static int rt61pci_validate_eeprom(struct rt2x00_dev *rt2x00dev)
+{
+	struct eeprom_93cx6 eeprom;
+	u32 reg;
+	u16 word;
+	u8 *mac;
+	s8 value;
+
+	rt2x00pci_register_read(rt2x00dev, E2PROM_CSR, &reg);
+
+	eeprom.data = rt2x00dev;
+	eeprom.register_read = rt61pci_eepromregister_read;
+	eeprom.register_write = rt61pci_eepromregister_write;
+	eeprom.width = rt2x00_get_field32(reg, E2PROM_CSR_TYPE_93C46) ?
+	    PCI_EEPROM_WIDTH_93C46 : PCI_EEPROM_WIDTH_93C66;
+	eeprom.reg_data_in = 0;
+	eeprom.reg_data_out = 0;
+	eeprom.reg_data_clock = 0;
+	eeprom.reg_chip_select = 0;
+
+	eeprom_93cx6_multiread(&eeprom, EEPROM_BASE, rt2x00dev->eeprom,
+			       EEPROM_SIZE / sizeof(u16));
+
+	/*
+	 * Start validation of the data that has been read.
+	 */
+	mac = rt2x00_eeprom_addr(rt2x00dev, EEPROM_MAC_ADDR_0);
+	if (!is_valid_ether_addr(mac)) {
+		DECLARE_MAC_BUF(macbuf);
+
+		random_ether_addr(mac);
+		EEPROM(rt2x00dev, "MAC: %s\n", print_mac(macbuf, mac));
+	}
+
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_ANTENNA, &word);
+	if (word == 0xffff) {
+		rt2x00_set_field16(&word, EEPROM_ANTENNA_NUM, 2);
+		rt2x00_set_field16(&word, EEPROM_ANTENNA_TX_DEFAULT, 2);
+		rt2x00_set_field16(&word, EEPROM_ANTENNA_RX_DEFAULT, 2);
+		rt2x00_set_field16(&word, EEPROM_ANTENNA_FRAME_TYPE, 0);
+		rt2x00_set_field16(&word, EEPROM_ANTENNA_DYN_TXAGC, 0);
+		rt2x00_set_field16(&word, EEPROM_ANTENNA_HARDWARE_RADIO, 0);
+		rt2x00_set_field16(&word, EEPROM_ANTENNA_RF_TYPE, RF5225);
+		rt2x00_eeprom_write(rt2x00dev, EEPROM_ANTENNA, word);
+		EEPROM(rt2x00dev, "Antenna: 0x%04x\n", word);
+	}
+
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC, &word);
+	if (word == 0xffff) {
+		rt2x00_set_field16(&word, EEPROM_NIC_ENABLE_DIVERSITY, 0);
+		rt2x00_set_field16(&word, EEPROM_NIC_TX_DIVERSITY, 0);
+		rt2x00_set_field16(&word, EEPROM_NIC_TX_RX_FIXED, 0);
+		rt2x00_set_field16(&word, EEPROM_NIC_EXTERNAL_LNA_BG, 0);
+		rt2x00_set_field16(&word, EEPROM_NIC_CARDBUS_ACCEL, 0);
+		rt2x00_set_field16(&word, EEPROM_NIC_EXTERNAL_LNA_A, 0);
+		rt2x00_eeprom_write(rt2x00dev, EEPROM_NIC, word);
+		EEPROM(rt2x00dev, "NIC: 0x%04x\n", word);
+	}
+
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_LED, &word);
+	if (word == 0xffff) {
+		rt2x00_set_field16(&word, EEPROM_LED_LED_MODE,
+				   LED_MODE_DEFAULT);
+		rt2x00_eeprom_write(rt2x00dev, EEPROM_LED, word);
+		EEPROM(rt2x00dev, "Led: 0x%04x\n", word);
+	}
+
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_FREQ, &word);
+	if (word == 0xffff) {
+		rt2x00_set_field16(&word, EEPROM_FREQ_OFFSET, 0);
+		rt2x00_set_field16(&word, EEPROM_FREQ_SEQ, 0);
+		rt2x00_eeprom_write(rt2x00dev, EEPROM_FREQ, word);
+		EEPROM(rt2x00dev, "Freq: 0x%04x\n", word);
+	}
+
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_RSSI_OFFSET_BG, &word);
+	if (word == 0xffff) {
+		rt2x00_set_field16(&word, EEPROM_RSSI_OFFSET_BG_1, 0);
+		rt2x00_set_field16(&word, EEPROM_RSSI_OFFSET_BG_2, 0);
+		rt2x00_eeprom_write(rt2x00dev, EEPROM_RSSI_OFFSET_BG, word);
+		EEPROM(rt2x00dev, "RSSI OFFSET BG: 0x%04x\n", word);
+	} else {
+		value = rt2x00_get_field16(word, EEPROM_RSSI_OFFSET_BG_1);
+		if (value < -10 || value > 10)
+			rt2x00_set_field16(&word, EEPROM_RSSI_OFFSET_BG_1, 0);
+		value = rt2x00_get_field16(word, EEPROM_RSSI_OFFSET_BG_2);
+		if (value < -10 || value > 10)
+			rt2x00_set_field16(&word, EEPROM_RSSI_OFFSET_BG_2, 0);
+		rt2x00_eeprom_write(rt2x00dev, EEPROM_RSSI_OFFSET_BG, word);
+	}
+
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_RSSI_OFFSET_A, &word);
+	if (word == 0xffff) {
+		rt2x00_set_field16(&word, EEPROM_RSSI_OFFSET_A_1, 0);
+		rt2x00_set_field16(&word, EEPROM_RSSI_OFFSET_A_2, 0);
+		rt2x00_eeprom_write(rt2x00dev, EEPROM_RSSI_OFFSET_A, word);
+		EEPROM(rt2x00dev, "RSSI OFFSET BG: 0x%04x\n", word);
+	} else {
+		value = rt2x00_get_field16(word, EEPROM_RSSI_OFFSET_A_1);
+		if (value < -10 || value > 10)
+			rt2x00_set_field16(&word, EEPROM_RSSI_OFFSET_A_1, 0);
+		value = rt2x00_get_field16(word, EEPROM_RSSI_OFFSET_A_2);
+		if (value < -10 || value > 10)
+			rt2x00_set_field16(&word, EEPROM_RSSI_OFFSET_A_2, 0);
+		rt2x00_eeprom_write(rt2x00dev, EEPROM_RSSI_OFFSET_A, word);
+	}
+
+	return 0;
+}
+
+static int rt61pci_init_eeprom(struct rt2x00_dev *rt2x00dev)
+{
+	u32 reg;
+	u16 value;
+	u16 eeprom;
+	u16 device;
+
+	/*
+	 * Read EEPROM word for configuration.
+	 */
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_ANTENNA, &eeprom);
+
+	/*
+	 * Identify RF chipset.
+	 * To determine the RT chip we have to read the
+	 * PCI header of the device.
+	 */
+	pci_read_config_word(rt2x00dev_pci(rt2x00dev),
+			     PCI_CONFIG_HEADER_DEVICE, &device);
+	value = rt2x00_get_field16(eeprom, EEPROM_ANTENNA_RF_TYPE);
+	rt2x00pci_register_read(rt2x00dev, MAC_CSR0, &reg);
+	rt2x00_set_chip(rt2x00dev, device, value, reg);
+
+	if (!rt2x00_rf(&rt2x00dev->chip, RF5225) &&
+	    !rt2x00_rf(&rt2x00dev->chip, RF5325) &&
+	    !rt2x00_rf(&rt2x00dev->chip, RF2527) &&
+	    !rt2x00_rf(&rt2x00dev->chip, RF2529)) {
+		ERROR(rt2x00dev, "Invalid RF chipset detected.\n");
+		return -ENODEV;
+	}
+
+	/*
+	 * Identify default antenna configuration.
+	 */
+	rt2x00dev->hw->conf.antenna_sel_tx =
+	    rt2x00_get_field16(eeprom, EEPROM_ANTENNA_TX_DEFAULT);
+	rt2x00dev->hw->conf.antenna_sel_rx =
+	    rt2x00_get_field16(eeprom, EEPROM_ANTENNA_RX_DEFAULT);
+
+	/*
+	 * Read the Frame type.
+	 */
+	if (rt2x00_get_field16(eeprom, EEPROM_ANTENNA_FRAME_TYPE))
+		__set_bit(CONFIG_FRAME_TYPE, &rt2x00dev->flags);
+
+	/*
+	 * Determine number of antenna's.
+	 */
+	if (rt2x00_get_field16(eeprom, EEPROM_ANTENNA_NUM) == 2)
+		__set_bit(CONFIG_DOUBLE_ANTENNA, &rt2x00dev->flags);
+
+	/*
+	 * Detect if this device has an hardware controlled radio.
+	 */
+#ifdef CONFIG_RT61PCI_RFKILL
+	if (rt2x00_get_field16(eeprom, EEPROM_ANTENNA_HARDWARE_RADIO))
+		__set_bit(CONFIG_SUPPORT_HW_BUTTON, &rt2x00dev->flags);
+#endif /* CONFIG_RT61PCI_RFKILL */
+
+	/*
+	 * Read frequency offset and RF programming sequence.
+	 */
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_FREQ, &eeprom);
+	if (rt2x00_get_field16(eeprom, EEPROM_FREQ_SEQ))
+		__set_bit(CONFIG_RF_SEQUENCE, &rt2x00dev->flags);
+
+	rt2x00dev->freq_offset = rt2x00_get_field16(eeprom, EEPROM_FREQ_OFFSET);
+
+	/*
+	 * Read external LNA informations.
+	 */
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC, &eeprom);
+
+	if (rt2x00_get_field16(eeprom, EEPROM_NIC_EXTERNAL_LNA_A))
+		__set_bit(CONFIG_EXTERNAL_LNA_A, &rt2x00dev->flags);
+	if (rt2x00_get_field16(eeprom, EEPROM_NIC_EXTERNAL_LNA_BG))
+		__set_bit(CONFIG_EXTERNAL_LNA_BG, &rt2x00dev->flags);
+
+	/*
+	 * Store led settings, for correct led behaviour.
+	 * If the eeprom value is invalid,
+	 * switch to default led mode.
+	 */
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_LED, &eeprom);
+
+	rt2x00dev->led_mode = rt2x00_get_field16(eeprom, EEPROM_LED_LED_MODE);
+
+	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_LED_MODE,
+			   rt2x00dev->led_mode);
+	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_POLARITY_GPIO_0,
+			   rt2x00_get_field16(eeprom,
+					      EEPROM_LED_POLARITY_GPIO_0));
+	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_POLARITY_GPIO_1,
+			   rt2x00_get_field16(eeprom,
+					      EEPROM_LED_POLARITY_GPIO_1));
+	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_POLARITY_GPIO_2,
+			   rt2x00_get_field16(eeprom,
+					      EEPROM_LED_POLARITY_GPIO_2));
+	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_POLARITY_GPIO_3,
+			   rt2x00_get_field16(eeprom,
+					      EEPROM_LED_POLARITY_GPIO_3));
+	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_POLARITY_GPIO_4,
+			   rt2x00_get_field16(eeprom,
+					      EEPROM_LED_POLARITY_GPIO_4));
+	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_POLARITY_ACT,
+			   rt2x00_get_field16(eeprom, EEPROM_LED_POLARITY_ACT));
+	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_POLARITY_READY_BG,
+			   rt2x00_get_field16(eeprom,
+					      EEPROM_LED_POLARITY_RDY_G));
+	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_POLARITY_READY_A,
+			   rt2x00_get_field16(eeprom,
+					      EEPROM_LED_POLARITY_RDY_A));
+
+	return 0;
+}
+
+/*
+ * RF value list for RF5225 & RF5325
+ * Supports: 2.4 GHz & 5.2 GHz, rf_sequence disabled
+ */
+static const struct rf_channel rf_vals_noseq[] = {
+	{ 1,  0x00002ccc, 0x00004786, 0x00068455, 0x000ffa0b },
+	{ 2,  0x00002ccc, 0x00004786, 0x00068455, 0x000ffa1f },
+	{ 3,  0x00002ccc, 0x0000478a, 0x00068455, 0x000ffa0b },
+	{ 4,  0x00002ccc, 0x0000478a, 0x00068455, 0x000ffa1f },
+	{ 5,  0x00002ccc, 0x0000478e, 0x00068455, 0x000ffa0b },
+	{ 6,  0x00002ccc, 0x0000478e, 0x00068455, 0x000ffa1f },
+	{ 7,  0x00002ccc, 0x00004792, 0x00068455, 0x000ffa0b },
+	{ 8,  0x00002ccc, 0x00004792, 0x00068455, 0x000ffa1f },
+	{ 9,  0x00002ccc, 0x00004796, 0x00068455, 0x000ffa0b },
+	{ 10, 0x00002ccc, 0x00004796, 0x00068455, 0x000ffa1f },
+	{ 11, 0x00002ccc, 0x0000479a, 0x00068455, 0x000ffa0b },
+	{ 12, 0x00002ccc, 0x0000479a, 0x00068455, 0x000ffa1f },
+	{ 13, 0x00002ccc, 0x0000479e, 0x00068455, 0x000ffa0b },
+	{ 14, 0x00002ccc, 0x000047a2, 0x00068455, 0x000ffa13 },
+
+	/* 802.11 UNI / HyperLan 2 */
+	{ 36, 0x00002ccc, 0x0000499a, 0x0009be55, 0x000ffa23 },
+	{ 40, 0x00002ccc, 0x000049a2, 0x0009be55, 0x000ffa03 },
+	{ 44, 0x00002ccc, 0x000049a6, 0x0009be55, 0x000ffa0b },
+	{ 48, 0x00002ccc, 0x000049aa, 0x0009be55, 0x000ffa13 },
+	{ 52, 0x00002ccc, 0x000049ae, 0x0009ae55, 0x000ffa1b },
+	{ 56, 0x00002ccc, 0x000049b2, 0x0009ae55, 0x000ffa23 },
+	{ 60, 0x00002ccc, 0x000049ba, 0x0009ae55, 0x000ffa03 },
+	{ 64, 0x00002ccc, 0x000049be, 0x0009ae55, 0x000ffa0b },
+
+	/* 802.11 HyperLan 2 */
+	{ 100, 0x00002ccc, 0x00004a2a, 0x000bae55, 0x000ffa03 },
+	{ 104, 0x00002ccc, 0x00004a2e, 0x000bae55, 0x000ffa0b },
+	{ 108, 0x00002ccc, 0x00004a32, 0x000bae55, 0x000ffa13 },
+	{ 112, 0x00002ccc, 0x00004a36, 0x000bae55, 0x000ffa1b },
+	{ 116, 0x00002ccc, 0x00004a3a, 0x000bbe55, 0x000ffa23 },
+	{ 120, 0x00002ccc, 0x00004a82, 0x000bbe55, 0x000ffa03 },
+	{ 124, 0x00002ccc, 0x00004a86, 0x000bbe55, 0x000ffa0b },
+	{ 128, 0x00002ccc, 0x00004a8a, 0x000bbe55, 0x000ffa13 },
+	{ 132, 0x00002ccc, 0x00004a8e, 0x000bbe55, 0x000ffa1b },
+	{ 136, 0x00002ccc, 0x00004a92, 0x000bbe55, 0x000ffa23 },
+
+	/* 802.11 UNII */
+	{ 140, 0x00002ccc, 0x00004a9a, 0x000bbe55, 0x000ffa03 },
+	{ 149, 0x00002ccc, 0x00004aa2, 0x000bbe55, 0x000ffa1f },
+	{ 153, 0x00002ccc, 0x00004aa6, 0x000bbe55, 0x000ffa27 },
+	{ 157, 0x00002ccc, 0x00004aae, 0x000bbe55, 0x000ffa07 },
+	{ 161, 0x00002ccc, 0x00004ab2, 0x000bbe55, 0x000ffa0f },
+	{ 165, 0x00002ccc, 0x00004ab6, 0x000bbe55, 0x000ffa17 },
+
+	/* MMAC(Japan)J52 ch 34,38,42,46 */
+	{ 34, 0x00002ccc, 0x0000499a, 0x0009be55, 0x000ffa0b },
+	{ 38, 0x00002ccc, 0x0000499e, 0x0009be55, 0x000ffa13 },
+	{ 42, 0x00002ccc, 0x000049a2, 0x0009be55, 0x000ffa1b },
+	{ 46, 0x00002ccc, 0x000049a6, 0x0009be55, 0x000ffa23 },
+};
+
+/*
+ * RF value list for RF5225 & RF5325
+ * Supports: 2.4 GHz & 5.2 GHz, rf_sequence enabled
+ */
+static const struct rf_channel rf_vals_seq[] = {
+	{ 1,  0x00002ccc, 0x00004786, 0x00068455, 0x000ffa0b },
+	{ 2,  0x00002ccc, 0x00004786, 0x00068455, 0x000ffa1f },
+	{ 3,  0x00002ccc, 0x0000478a, 0x00068455, 0x000ffa0b },
+	{ 4,  0x00002ccc, 0x0000478a, 0x00068455, 0x000ffa1f },
+	{ 5,  0x00002ccc, 0x0000478e, 0x00068455, 0x000ffa0b },
+	{ 6,  0x00002ccc, 0x0000478e, 0x00068455, 0x000ffa1f },
+	{ 7,  0x00002ccc, 0x00004792, 0x00068455, 0x000ffa0b },
+	{ 8,  0x00002ccc, 0x00004792, 0x00068455, 0x000ffa1f },
+	{ 9,  0x00002ccc, 0x00004796, 0x00068455, 0x000ffa0b },
+	{ 10, 0x00002ccc, 0x00004796, 0x00068455, 0x000ffa1f },
+	{ 11, 0x00002ccc, 0x0000479a, 0x00068455, 0x000ffa0b },
+	{ 12, 0x00002ccc, 0x0000479a, 0x00068455, 0x000ffa1f },
+	{ 13, 0x00002ccc, 0x0000479e, 0x00068455, 0x000ffa0b },
+	{ 14, 0x00002ccc, 0x000047a2, 0x00068455, 0x000ffa13 },
+
+	/* 802.11 UNI / HyperLan 2 */
+	{ 36, 0x00002cd4, 0x0004481a, 0x00098455, 0x000c0a03 },
+	{ 40, 0x00002cd0, 0x00044682, 0x00098455, 0x000c0a03 },
+	{ 44, 0x00002cd0, 0x00044686, 0x00098455, 0x000c0a1b },
+	{ 48, 0x00002cd0, 0x0004468e, 0x00098655, 0x000c0a0b },
+	{ 52, 0x00002cd0, 0x00044692, 0x00098855, 0x000c0a23 },
+	{ 56, 0x00002cd0, 0x0004469a, 0x00098c55, 0x000c0a13 },
+	{ 60, 0x00002cd0, 0x000446a2, 0x00098e55, 0x000c0a03 },
+	{ 64, 0x00002cd0, 0x000446a6, 0x00099255, 0x000c0a1b },
+
+	/* 802.11 HyperLan 2 */
+	{ 100, 0x00002cd4, 0x0004489a, 0x000b9855, 0x000c0a03 },
+	{ 104, 0x00002cd4, 0x000448a2, 0x000b9855, 0x000c0a03 },
+	{ 108, 0x00002cd4, 0x000448aa, 0x000b9855, 0x000c0a03 },
+	{ 112, 0x00002cd4, 0x000448b2, 0x000b9a55, 0x000c0a03 },
+	{ 116, 0x00002cd4, 0x000448ba, 0x000b9a55, 0x000c0a03 },
+	{ 120, 0x00002cd0, 0x00044702, 0x000b9a55, 0x000c0a03 },
+	{ 124, 0x00002cd0, 0x00044706, 0x000b9a55, 0x000c0a1b },
+	{ 128, 0x00002cd0, 0x0004470e, 0x000b9c55, 0x000c0a0b },
+	{ 132, 0x00002cd0, 0x00044712, 0x000b9c55, 0x000c0a23 },
+	{ 136, 0x00002cd0, 0x0004471a, 0x000b9e55, 0x000c0a13 },
+
+	/* 802.11 UNII */
+	{ 140, 0x00002cd0, 0x00044722, 0x000b9e55, 0x000c0a03 },
+	{ 149, 0x00002cd0, 0x0004472e, 0x000ba255, 0x000c0a1b },
+	{ 153, 0x00002cd0, 0x00044736, 0x000ba255, 0x000c0a0b },
+	{ 157, 0x00002cd4, 0x0004490a, 0x000ba255, 0x000c0a17 },
+	{ 161, 0x00002cd4, 0x00044912, 0x000ba255, 0x000c0a17 },
+	{ 165, 0x00002cd4, 0x0004491a, 0x000ba255, 0x000c0a17 },
+
+	/* MMAC(Japan)J52 ch 34,38,42,46 */
+	{ 34, 0x00002ccc, 0x0000499a, 0x0009be55, 0x000c0a0b },
+	{ 38, 0x00002ccc, 0x0000499e, 0x0009be55, 0x000c0a13 },
+	{ 42, 0x00002ccc, 0x000049a2, 0x0009be55, 0x000c0a1b },
+	{ 46, 0x00002ccc, 0x000049a6, 0x0009be55, 0x000c0a23 },
+};
+
+static void rt61pci_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
+{
+	struct hw_mode_spec *spec = &rt2x00dev->spec;
+	u8 *txpower;
+	unsigned int i;
+
+	/*
+	 * Initialize all hw fields.
+	 */
+	rt2x00dev->hw->flags =
+	    IEEE80211_HW_HOST_GEN_BEACON_TEMPLATE |
+	    IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING;
+	rt2x00dev->hw->extra_tx_headroom = 0;
+	rt2x00dev->hw->max_signal = MAX_SIGNAL;
+	rt2x00dev->hw->max_rssi = MAX_RX_SSI;
+	rt2x00dev->hw->queues = 5;
+
+	SET_IEEE80211_DEV(rt2x00dev->hw, &rt2x00dev_pci(rt2x00dev)->dev);
+	SET_IEEE80211_PERM_ADDR(rt2x00dev->hw,
+				rt2x00_eeprom_addr(rt2x00dev,
+						   EEPROM_MAC_ADDR_0));
+
+	/*
+	 * Convert tx_power array in eeprom.
+	 */
+	txpower = rt2x00_eeprom_addr(rt2x00dev, EEPROM_TXPOWER_G_START);
+	for (i = 0; i < 14; i++)
+		txpower[i] = TXPOWER_FROM_DEV(txpower[i]);
+
+	/*
+	 * Initialize hw_mode information.
+	 */
+	spec->num_modes = 2;
+	spec->num_rates = 12;
+	spec->tx_power_a = NULL;
+	spec->tx_power_bg = txpower;
+	spec->tx_power_default = DEFAULT_TXPOWER;
+
+	if (!test_bit(CONFIG_RF_SEQUENCE, &rt2x00dev->flags)) {
+		spec->num_channels = 14;
+		spec->channels = rf_vals_noseq;
+	} else {
+		spec->num_channels = 14;
+		spec->channels = rf_vals_seq;
+	}
+
+	if (rt2x00_rf(&rt2x00dev->chip, RF5225) ||
+	    rt2x00_rf(&rt2x00dev->chip, RF5325)) {
+		spec->num_modes = 3;
+		spec->num_channels = ARRAY_SIZE(rf_vals_seq);
+
+		txpower = rt2x00_eeprom_addr(rt2x00dev, EEPROM_TXPOWER_A_START);
+		for (i = 0; i < 14; i++)
+			txpower[i] = TXPOWER_FROM_DEV(txpower[i]);
+
+		spec->tx_power_a = txpower;
+	}
+}
+
+static int rt61pci_probe_hw(struct rt2x00_dev *rt2x00dev)
+{
+	int retval;
+
+	/*
+	 * Allocate eeprom data.
+	 */
+	retval = rt61pci_validate_eeprom(rt2x00dev);
+	if (retval)
+		return retval;
+
+	retval = rt61pci_init_eeprom(rt2x00dev);
+	if (retval)
+		return retval;
+
+	/*
+	 * Initialize hw specifications.
+	 */
+	rt61pci_probe_hw_mode(rt2x00dev);
+
+	/*
+	 * This device requires firmware
+	 */
+	__set_bit(DRIVER_REQUIRE_FIRMWARE, &rt2x00dev->flags);
+
+	/*
+	 * Set the rssi offset.
+	 */
+	rt2x00dev->rssi_offset = DEFAULT_RSSI_OFFSET;
+
+	return 0;
+}
+
+/*
+ * IEEE80211 stack callback functions.
+ */
+static void rt61pci_configure_filter(struct ieee80211_hw *hw,
+				     unsigned int changed_flags,
+				     unsigned int *total_flags,
+				     int mc_count,
+				     struct dev_addr_list *mc_list)
+{
+	struct rt2x00_dev *rt2x00dev = hw->priv;
+	struct interface *intf = &rt2x00dev->interface;
+	u32 reg;
+
+	/*
+	 * Mask off any flags we are going to ignore from
+	 * the total_flags field.
+	 */
+	*total_flags &=
+	    FIF_ALLMULTI |
+	    FIF_FCSFAIL |
+	    FIF_PLCPFAIL |
+	    FIF_CONTROL |
+	    FIF_OTHER_BSS |
+	    FIF_PROMISC_IN_BSS;
+
+	/*
+	 * Apply some rules to the filters:
+	 * - Some filters imply different filters to be set.
+	 * - Some things we can't filter out at all.
+	 * - Some filters are set based on interface type.
+	 */
+	if (mc_count)
+		*total_flags |= FIF_ALLMULTI;
+	if (*total_flags & FIF_OTHER_BSS ||
+	    *total_flags & FIF_PROMISC_IN_BSS)
+		*total_flags |= FIF_PROMISC_IN_BSS | FIF_OTHER_BSS;
+	if (is_interface_type(intf, IEEE80211_IF_TYPE_AP))
+		*total_flags |= FIF_PROMISC_IN_BSS;
+
+	/*
+	 * Check if there is any work left for us.
+	 */
+	if (intf->filter == *total_flags)
+		return;
+	intf->filter = *total_flags;
+
+	/*
+	 * Start configuration steps.
+	 * Note that the version error will always be dropped
+	 * and broadcast frames will always be accepted since
+	 * there is no filter for it at this time.
+	 */
+	rt2x00pci_register_read(rt2x00dev, TXRX_CSR0, &reg);
+	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_CRC,
+			   !(*total_flags & FIF_FCSFAIL));
+	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_PHYSICAL,
+			   !(*total_flags & FIF_PLCPFAIL));
+	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_CONTROL,
+			   !(*total_flags & FIF_CONTROL));
+	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_NOT_TO_ME,
+			   !(*total_flags & FIF_PROMISC_IN_BSS));
+	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_TO_DS,
+			   !(*total_flags & FIF_PROMISC_IN_BSS));
+	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_VERSION_ERROR, 1);
+	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_MULTICAST,
+			   !(*total_flags & FIF_ALLMULTI));
+	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_BORADCAST, 0);
+	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_ACK_CTS, 1);
+	rt2x00pci_register_write(rt2x00dev, TXRX_CSR0, reg);
+}
+
+static int rt61pci_set_retry_limit(struct ieee80211_hw *hw,
+				   u32 short_retry, u32 long_retry)
+{
+	struct rt2x00_dev *rt2x00dev = hw->priv;
+	u32 reg;
+
+	rt2x00pci_register_read(rt2x00dev, TXRX_CSR4, &reg);
+	rt2x00_set_field32(&reg, TXRX_CSR4_LONG_RETRY_LIMIT, long_retry);
+	rt2x00_set_field32(&reg, TXRX_CSR4_SHORT_RETRY_LIMIT, short_retry);
+	rt2x00pci_register_write(rt2x00dev, TXRX_CSR4, reg);
+
+	return 0;
+}
+
+static u64 rt61pci_get_tsf(struct ieee80211_hw *hw)
+{
+	struct rt2x00_dev *rt2x00dev = hw->priv;
+	u64 tsf;
+	u32 reg;
+
+	rt2x00pci_register_read(rt2x00dev, TXRX_CSR13, &reg);
+	tsf = (u64) rt2x00_get_field32(reg, TXRX_CSR13_HIGH_TSFTIMER) << 32;
+	rt2x00pci_register_read(rt2x00dev, TXRX_CSR12, &reg);
+	tsf |= rt2x00_get_field32(reg, TXRX_CSR12_LOW_TSFTIMER);
+
+	return tsf;
+}
+
+static void rt61pci_reset_tsf(struct ieee80211_hw *hw)
+{
+	struct rt2x00_dev *rt2x00dev = hw->priv;
+
+	rt2x00pci_register_write(rt2x00dev, TXRX_CSR12, 0);
+	rt2x00pci_register_write(rt2x00dev, TXRX_CSR13, 0);
+}
+
+static int rt61pci_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb,
+			  struct ieee80211_tx_control *control)
+{
+	struct rt2x00_dev *rt2x00dev = hw->priv;
+
+	/*
+	 * Just in case the ieee80211 doesn't set this,
+	 * but we need this queue set for the descriptor
+	 * initialization.
+	 */
+	control->queue = IEEE80211_TX_QUEUE_BEACON;
+
+	/*
+	 * We need to append the descriptor in front of the
+	 * beacon frame.
+	 */
+	if (skb_headroom(skb) < TXD_DESC_SIZE) {
+		if (pskb_expand_head(skb, TXD_DESC_SIZE, 0, GFP_ATOMIC)) {
+			dev_kfree_skb(skb);
+			return -ENOMEM;
+		}
+	}
+
+	/*
+	 * First we create the beacon.
+	 */
+	skb_push(skb, TXD_DESC_SIZE);
+	memset(skb->data, 0, TXD_DESC_SIZE);
+
+	rt2x00lib_write_tx_desc(rt2x00dev, (struct data_desc *)skb->data,
+				(struct ieee80211_hdr *)(skb->data +
+							 TXD_DESC_SIZE),
+				skb->len - TXD_DESC_SIZE, control);
+
+	/*
+	 * Write entire beacon with descriptor to register,
+	 * and kick the beacon generator.
+	 */
+	rt2x00pci_register_multiwrite(rt2x00dev, HW_BEACON_BASE0,
+				      skb->data, skb->len);
+	rt61pci_kick_tx_queue(rt2x00dev, IEEE80211_TX_QUEUE_BEACON);
+
+	return 0;
+}
+
+static const struct ieee80211_ops rt61pci_mac80211_ops = {
+	.tx			= rt2x00mac_tx,
+	.start			= rt2x00mac_start,
+	.stop			= rt2x00mac_stop,
+	.add_interface		= rt2x00mac_add_interface,
+	.remove_interface	= rt2x00mac_remove_interface,
+	.config			= rt2x00mac_config,
+	.config_interface	= rt2x00mac_config_interface,
+	.configure_filter	= rt61pci_configure_filter,
+	.get_stats		= rt2x00mac_get_stats,
+	.set_retry_limit	= rt61pci_set_retry_limit,
+	.erp_ie_changed		= rt2x00mac_erp_ie_changed,
+	.conf_tx		= rt2x00mac_conf_tx,
+	.get_tx_stats		= rt2x00mac_get_tx_stats,
+	.get_tsf		= rt61pci_get_tsf,
+	.reset_tsf		= rt61pci_reset_tsf,
+	.beacon_update		= rt61pci_beacon_update,
+};
+
+static const struct rt2x00lib_ops rt61pci_rt2x00_ops = {
+	.irq_handler		= rt61pci_interrupt,
+	.probe_hw		= rt61pci_probe_hw,
+	.get_firmware_name	= rt61pci_get_firmware_name,
+	.load_firmware		= rt61pci_load_firmware,
+	.initialize		= rt2x00pci_initialize,
+	.uninitialize		= rt2x00pci_uninitialize,
+	.set_device_state	= rt61pci_set_device_state,
+	.rfkill_poll		= rt61pci_rfkill_poll,
+	.link_stats		= rt61pci_link_stats,
+	.reset_tuner		= rt61pci_reset_tuner,
+	.link_tuner		= rt61pci_link_tuner,
+	.write_tx_desc		= rt61pci_write_tx_desc,
+	.write_tx_data		= rt2x00pci_write_tx_data,
+	.kick_tx_queue		= rt61pci_kick_tx_queue,
+	.fill_rxdone		= rt61pci_fill_rxdone,
+	.config_mac_addr	= rt61pci_config_mac_addr,
+	.config_bssid		= rt61pci_config_bssid,
+	.config_type		= rt61pci_config_type,
+	.config_preamble	= rt61pci_config_preamble,
+	.config			= rt61pci_config,
+};
+
+static const struct rt2x00_ops rt61pci_ops = {
+	.name		= DRV_NAME,
+	.rxd_size	= RXD_DESC_SIZE,
+	.txd_size	= TXD_DESC_SIZE,
+	.eeprom_size	= EEPROM_SIZE,
+	.rf_size	= RF_SIZE,
+	.lib		= &rt61pci_rt2x00_ops,
+	.hw		= &rt61pci_mac80211_ops,
+#ifdef CONFIG_RT2X00_LIB_DEBUGFS
+	.debugfs	= &rt61pci_rt2x00debug,
+#endif /* CONFIG_RT2X00_LIB_DEBUGFS */
+};
+
+/*
+ * RT61pci module information.
+ */
+static struct pci_device_id rt61pci_device_table[] = {
+	/* RT2561s */
+	{ PCI_DEVICE(0x1814, 0x0301), PCI_DEVICE_DATA(&rt61pci_ops) },
+	/* RT2561 v2 */
+	{ PCI_DEVICE(0x1814, 0x0302), PCI_DEVICE_DATA(&rt61pci_ops) },
+	/* RT2661 */
+	{ PCI_DEVICE(0x1814, 0x0401), PCI_DEVICE_DATA(&rt61pci_ops) },
+	{ 0, }
+};
+
+MODULE_AUTHOR(DRV_PROJECT);
+MODULE_VERSION(DRV_VERSION);
+MODULE_DESCRIPTION("Ralink RT61 PCI & PCMCIA Wireless LAN driver.");
+MODULE_SUPPORTED_DEVICE("Ralink RT2561, RT2561s & RT2661 "
+			"PCI & PCMCIA chipset based cards");
+MODULE_DEVICE_TABLE(pci, rt61pci_device_table);
+MODULE_FIRMWARE(FIRMWARE_RT2561);
+MODULE_FIRMWARE(FIRMWARE_RT2561s);
+MODULE_FIRMWARE(FIRMWARE_RT2661);
+MODULE_LICENSE("GPL");
+
+static struct pci_driver rt61pci_driver = {
+	.name		= DRV_NAME,
+	.id_table	= rt61pci_device_table,
+	.probe		= rt2x00pci_probe,
+	.remove		= __devexit_p(rt2x00pci_remove),
+	.suspend	= rt2x00pci_suspend,
+	.resume		= rt2x00pci_resume,
+};
+
+static int __init rt61pci_init(void)
+{
+	return pci_register_driver(&rt61pci_driver);
+}
+
+static void __exit rt61pci_exit(void)
+{
+	pci_unregister_driver(&rt61pci_driver);
+}
+
+module_init(rt61pci_init);
+module_exit(rt61pci_exit);
diff --git a/drivers/net/wireless/rt2x00/rt61pci.h b/drivers/net/wireless/rt2x00/rt61pci.h
new file mode 100644
index 000000000000..6721d7dd32bc
--- /dev/null
+++ b/drivers/net/wireless/rt2x00/rt61pci.h
@@ -0,0 +1,1457 @@
+/*
+	Copyright (C) 2004 - 2007 rt2x00 SourceForge Project
+	<http://rt2x00.serialmonkey.com>
+
+	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, or
+	(at your option) any later version.
+
+	This program is distributed in the hope that it will be useful,
+	but WITHOUT ANY WARRANTY; without even the implied warranty of
+	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+	GNU General Public License for more details.
+
+	You should have received a copy of the GNU General Public License
+	along with this program; if not, write to the
+	Free Software Foundation, Inc.,
+	59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+/*
+	Module: rt61pci
+	Abstract: Data structures and registers for the rt61pci module.
+	Supported chipsets: RT2561, RT2561s, RT2661.
+ */
+
+#ifndef RT61PCI_H
+#define RT61PCI_H
+
+/*
+ * RF chip defines.
+ */
+#define RF5225				0x0001
+#define RF5325				0x0002
+#define RF2527				0x0003
+#define RF2529				0x0004
+
+/*
+ * Signal information.
+ * Defaul offset is required for RSSI <-> dBm conversion.
+ */
+#define MAX_SIGNAL			100
+#define MAX_RX_SSI			-1
+#define DEFAULT_RSSI_OFFSET		120
+
+/*
+ * Register layout information.
+ */
+#define CSR_REG_BASE			0x3000
+#define CSR_REG_SIZE			0x04b0
+#define EEPROM_BASE			0x0000
+#define EEPROM_SIZE			0x0100
+#define BBP_SIZE			0x0080
+#define RF_SIZE				0x0014
+
+/*
+ * PCI registers.
+ */
+
+/*
+ * PCI Configuration Header
+ */
+#define PCI_CONFIG_HEADER_VENDOR	0x0000
+#define PCI_CONFIG_HEADER_DEVICE	0x0002
+
+/*
+ * HOST_CMD_CSR: For HOST to interrupt embedded processor
+ */
+#define HOST_CMD_CSR			0x0008
+#define HOST_CMD_CSR_HOST_COMMAND	FIELD32(0x0000007f)
+#define HOST_CMD_CSR_INTERRUPT_MCU	FIELD32(0x00000080)
+
+/*
+ * MCU_CNTL_CSR
+ * SELECT_BANK: Select 8051 program bank.
+ * RESET: Enable 8051 reset state.
+ * READY: Ready state for 8051.
+ */
+#define MCU_CNTL_CSR			0x000c
+#define MCU_CNTL_CSR_SELECT_BANK	FIELD32(0x00000001)
+#define MCU_CNTL_CSR_RESET		FIELD32(0x00000002)
+#define MCU_CNTL_CSR_READY		FIELD32(0x00000004)
+
+/*
+ * SOFT_RESET_CSR
+ */
+#define SOFT_RESET_CSR			0x0010
+
+/*
+ * MCU_INT_SOURCE_CSR: MCU interrupt source/mask register.
+ */
+#define MCU_INT_SOURCE_CSR		0x0014
+#define MCU_INT_SOURCE_CSR_0		FIELD32(0x00000001)
+#define MCU_INT_SOURCE_CSR_1		FIELD32(0x00000002)
+#define MCU_INT_SOURCE_CSR_2		FIELD32(0x00000004)
+#define MCU_INT_SOURCE_CSR_3		FIELD32(0x00000008)
+#define MCU_INT_SOURCE_CSR_4		FIELD32(0x00000010)
+#define MCU_INT_SOURCE_CSR_5		FIELD32(0x00000020)
+#define MCU_INT_SOURCE_CSR_6		FIELD32(0x00000040)
+#define MCU_INT_SOURCE_CSR_7		FIELD32(0x00000080)
+#define MCU_INT_SOURCE_CSR_TWAKEUP	FIELD32(0x00000100)
+#define MCU_INT_SOURCE_CSR_TBTT_EXPIRE	FIELD32(0x00000200)
+
+/*
+ * MCU_INT_MASK_CSR: MCU interrupt source/mask register.
+ */
+#define MCU_INT_MASK_CSR		0x0018
+#define MCU_INT_MASK_CSR_0		FIELD32(0x00000001)
+#define MCU_INT_MASK_CSR_1		FIELD32(0x00000002)
+#define MCU_INT_MASK_CSR_2		FIELD32(0x00000004)
+#define MCU_INT_MASK_CSR_3		FIELD32(0x00000008)
+#define MCU_INT_MASK_CSR_4		FIELD32(0x00000010)
+#define MCU_INT_MASK_CSR_5		FIELD32(0x00000020)
+#define MCU_INT_MASK_CSR_6		FIELD32(0x00000040)
+#define MCU_INT_MASK_CSR_7		FIELD32(0x00000080)
+#define MCU_INT_MASK_CSR_TWAKEUP	FIELD32(0x00000100)
+#define MCU_INT_MASK_CSR_TBTT_EXPIRE	FIELD32(0x00000200)
+
+/*
+ * PCI_USEC_CSR
+ */
+#define PCI_USEC_CSR			0x001c
+
+/*
+ * Security key table memory.
+ * 16 entries 32-byte for shared key table
+ * 64 entries 32-byte for pairwise key table
+ * 64 entries 8-byte for pairwise ta key table
+ */
+#define SHARED_KEY_TABLE_BASE		0x1000
+#define PAIRWISE_KEY_TABLE_BASE		0x1200
+#define PAIRWISE_TA_TABLE_BASE		0x1a00
+
+struct hw_key_entry {
+	u8 key[16];
+	u8 tx_mic[8];
+	u8 rx_mic[8];
+} __attribute__ ((packed));
+
+struct hw_pairwise_ta_entry {
+	u8 address[6];
+	u8 reserved[2];
+} __attribute__ ((packed));
+
+/*
+ * Other on-chip shared memory space.
+ */
+#define HW_CIS_BASE			0x2000
+#define HW_NULL_BASE			0x2b00
+
+/*
+ * Since NULL frame won't be that long (256 byte),
+ * We steal 16 tail bytes to save debugging settings.
+ */
+#define HW_DEBUG_SETTING_BASE		0x2bf0
+
+/*
+ * On-chip BEACON frame space.
+ */
+#define HW_BEACON_BASE0			0x2c00
+#define HW_BEACON_BASE1			0x2d00
+#define HW_BEACON_BASE2			0x2e00
+#define HW_BEACON_BASE3			0x2f00
+#define HW_BEACON_OFFSET		0x0100
+
+/*
+ * HOST-MCU shared memory.
+ */
+
+/*
+ * H2M_MAILBOX_CSR: Host-to-MCU Mailbox.
+ */
+#define H2M_MAILBOX_CSR			0x2100
+#define H2M_MAILBOX_CSR_ARG0		FIELD32(0x000000ff)
+#define H2M_MAILBOX_CSR_ARG1		FIELD32(0x0000ff00)
+#define H2M_MAILBOX_CSR_CMD_TOKEN	FIELD32(0x00ff0000)
+#define H2M_MAILBOX_CSR_OWNER		FIELD32(0xff000000)
+
+/*
+ * MCU_LEDCS: LED control for MCU Mailbox.
+ */
+#define MCU_LEDCS_LED_MODE		FIELD16(0x001f)
+#define MCU_LEDCS_RADIO_STATUS		FIELD16(0x0020)
+#define MCU_LEDCS_LINK_BG_STATUS	FIELD16(0x0040)
+#define MCU_LEDCS_LINK_A_STATUS		FIELD16(0x0080)
+#define MCU_LEDCS_POLARITY_GPIO_0	FIELD16(0x0100)
+#define MCU_LEDCS_POLARITY_GPIO_1	FIELD16(0x0200)
+#define MCU_LEDCS_POLARITY_GPIO_2	FIELD16(0x0400)
+#define MCU_LEDCS_POLARITY_GPIO_3	FIELD16(0x0800)
+#define MCU_LEDCS_POLARITY_GPIO_4	FIELD16(0x1000)
+#define MCU_LEDCS_POLARITY_ACT		FIELD16(0x2000)
+#define MCU_LEDCS_POLARITY_READY_BG	FIELD16(0x4000)
+#define MCU_LEDCS_POLARITY_READY_A	FIELD16(0x8000)
+
+/*
+ * M2H_CMD_DONE_CSR.
+ */
+#define M2H_CMD_DONE_CSR		0x2104
+
+/*
+ * MCU_TXOP_ARRAY_BASE.
+ */
+#define MCU_TXOP_ARRAY_BASE		0x2110
+
+/*
+ * MAC Control/Status Registers(CSR).
+ * Some values are set in TU, whereas 1 TU == 1024 us.
+ */
+
+/*
+ * MAC_CSR0: ASIC revision number.
+ */
+#define MAC_CSR0			0x3000
+
+/*
+ * MAC_CSR1: System control register.
+ * SOFT_RESET: Software reset bit, 1: reset, 0: normal.
+ * BBP_RESET: Hardware reset BBP.
+ * HOST_READY: Host is ready after initialization, 1: ready.
+ */
+#define MAC_CSR1			0x3004
+#define MAC_CSR1_SOFT_RESET		FIELD32(0x00000001)
+#define MAC_CSR1_BBP_RESET		FIELD32(0x00000002)
+#define MAC_CSR1_HOST_READY		FIELD32(0x00000004)
+
+/*
+ * MAC_CSR2: STA MAC register 0.
+ */
+#define MAC_CSR2			0x3008
+#define MAC_CSR2_BYTE0			FIELD32(0x000000ff)
+#define MAC_CSR2_BYTE1			FIELD32(0x0000ff00)
+#define MAC_CSR2_BYTE2			FIELD32(0x00ff0000)
+#define MAC_CSR2_BYTE3			FIELD32(0xff000000)
+
+/*
+ * MAC_CSR3: STA MAC register 1.
+ */
+#define MAC_CSR3			0x300c
+#define MAC_CSR3_BYTE4			FIELD32(0x000000ff)
+#define MAC_CSR3_BYTE5			FIELD32(0x0000ff00)
+#define MAC_CSR3_UNICAST_TO_ME_MASK	FIELD32(0x00ff0000)
+
+/*
+ * MAC_CSR4: BSSID register 0.
+ */
+#define MAC_CSR4			0x3010
+#define MAC_CSR4_BYTE0			FIELD32(0x000000ff)
+#define MAC_CSR4_BYTE1			FIELD32(0x0000ff00)
+#define MAC_CSR4_BYTE2			FIELD32(0x00ff0000)
+#define MAC_CSR4_BYTE3			FIELD32(0xff000000)
+
+/*
+ * MAC_CSR5: BSSID register 1.
+ * BSS_ID_MASK: 3: one BSSID, 0: 4 BSSID, 2 or 1: 2 BSSID.
+ */
+#define MAC_CSR5			0x3014
+#define MAC_CSR5_BYTE4			FIELD32(0x000000ff)
+#define MAC_CSR5_BYTE5			FIELD32(0x0000ff00)
+#define MAC_CSR5_BSS_ID_MASK		FIELD32(0x00ff0000)
+
+/*
+ * MAC_CSR6: Maximum frame length register.
+ */
+#define MAC_CSR6			0x3018
+#define MAC_CSR6_MAX_FRAME_UNIT		FIELD32(0x00000fff)
+
+/*
+ * MAC_CSR7: Reserved
+ */
+#define MAC_CSR7			0x301c
+
+/*
+ * MAC_CSR8: SIFS/EIFS register.
+ * All units are in US.
+ */
+#define MAC_CSR8			0x3020
+#define MAC_CSR8_SIFS			FIELD32(0x000000ff)
+#define MAC_CSR8_SIFS_AFTER_RX_OFDM	FIELD32(0x0000ff00)
+#define MAC_CSR8_EIFS			FIELD32(0xffff0000)
+
+/*
+ * MAC_CSR9: Back-Off control register.
+ * SLOT_TIME: Slot time, default is 20us for 802.11BG.
+ * CWMIN: Bit for Cwmin. default Cwmin is 31 (2^5 - 1).
+ * CWMAX: Bit for Cwmax, default Cwmax is 1023 (2^10 - 1).
+ * CW_SELECT: 1: CWmin/Cwmax select from register, 0:select from TxD.
+ */
+#define MAC_CSR9			0x3024
+#define MAC_CSR9_SLOT_TIME		FIELD32(0x000000ff)
+#define MAC_CSR9_CWMIN			FIELD32(0x00000f00)
+#define MAC_CSR9_CWMAX			FIELD32(0x0000f000)
+#define MAC_CSR9_CW_SELECT		FIELD32(0x00010000)
+
+/*
+ * MAC_CSR10: Power state configuration.
+ */
+#define MAC_CSR10			0x3028
+
+/*
+ * MAC_CSR11: Power saving transition time register.
+ * DELAY_AFTER_TBCN: Delay after Tbcn expired in units of TU.
+ * TBCN_BEFORE_WAKEUP: Number of beacon before wakeup.
+ * WAKEUP_LATENCY: In unit of TU.
+ */
+#define MAC_CSR11			0x302c
+#define MAC_CSR11_DELAY_AFTER_TBCN	FIELD32(0x000000ff)
+#define MAC_CSR11_TBCN_BEFORE_WAKEUP	FIELD32(0x00007f00)
+#define MAC_CSR11_AUTOWAKE		FIELD32(0x00008000)
+#define MAC_CSR11_WAKEUP_LATENCY	FIELD32(0x000f0000)
+
+/*
+ * MAC_CSR12: Manual power control / status register (merge CSR20 & PWRCSR1).
+ * CURRENT_STATE: 0:sleep, 1:awake.
+ * FORCE_WAKEUP: This has higher priority than PUT_TO_SLEEP.
+ * BBP_CURRENT_STATE: 0: BBP sleep, 1: BBP awake.
+ */
+#define MAC_CSR12			0x3030
+#define MAC_CSR12_CURRENT_STATE		FIELD32(0x00000001)
+#define MAC_CSR12_PUT_TO_SLEEP		FIELD32(0x00000002)
+#define MAC_CSR12_FORCE_WAKEUP		FIELD32(0x00000004)
+#define MAC_CSR12_BBP_CURRENT_STATE	FIELD32(0x00000008)
+
+/*
+ * MAC_CSR13: GPIO.
+ */
+#define MAC_CSR13			0x3034
+#define MAC_CSR13_BIT0			FIELD32(0x00000001)
+#define MAC_CSR13_BIT1			FIELD32(0x00000002)
+#define MAC_CSR13_BIT2			FIELD32(0x00000004)
+#define MAC_CSR13_BIT3			FIELD32(0x00000008)
+#define MAC_CSR13_BIT4			FIELD32(0x00000010)
+#define MAC_CSR13_BIT5			FIELD32(0x00000020)
+#define MAC_CSR13_BIT6			FIELD32(0x00000040)
+#define MAC_CSR13_BIT7			FIELD32(0x00000080)
+#define MAC_CSR13_BIT8			FIELD32(0x00000100)
+#define MAC_CSR13_BIT9			FIELD32(0x00000200)
+#define MAC_CSR13_BIT10			FIELD32(0x00000400)
+#define MAC_CSR13_BIT11			FIELD32(0x00000800)
+#define MAC_CSR13_BIT12			FIELD32(0x00001000)
+
+/*
+ * MAC_CSR14: LED control register.
+ * ON_PERIOD: On period, default 70ms.
+ * OFF_PERIOD: Off period, default 30ms.
+ * HW_LED: HW TX activity, 1: normal OFF, 0: normal ON.
+ * SW_LED: s/w LED, 1: ON, 0: OFF.
+ * HW_LED_POLARITY: 0: active low, 1: active high.
+ */
+#define MAC_CSR14			0x3038
+#define MAC_CSR14_ON_PERIOD		FIELD32(0x000000ff)
+#define MAC_CSR14_OFF_PERIOD		FIELD32(0x0000ff00)
+#define MAC_CSR14_HW_LED		FIELD32(0x00010000)
+#define MAC_CSR14_SW_LED		FIELD32(0x00020000)
+#define MAC_CSR14_HW_LED_POLARITY	FIELD32(0x00040000)
+#define MAC_CSR14_SW_LED2		FIELD32(0x00080000)
+
+/*
+ * MAC_CSR15: NAV control.
+ */
+#define MAC_CSR15			0x303c
+
+/*
+ * TXRX control registers.
+ * Some values are set in TU, whereas 1 TU == 1024 us.
+ */
+
+/*
+ * TXRX_CSR0: TX/RX configuration register.
+ * TSF_OFFSET: Default is 24.
+ * AUTO_TX_SEQ: 1: ASIC auto replace sequence nr in outgoing frame.
+ * DISABLE_RX: Disable Rx engine.
+ * DROP_CRC: Drop CRC error.
+ * DROP_PHYSICAL: Drop physical error.
+ * DROP_CONTROL: Drop control frame.
+ * DROP_NOT_TO_ME: Drop not to me unicast frame.
+ * DROP_TO_DS: Drop fram ToDs bit is true.
+ * DROP_VERSION_ERROR: Drop version error frame.
+ * DROP_MULTICAST: Drop multicast frames.
+ * DROP_BORADCAST: Drop broadcast frames.
+ * ROP_ACK_CTS: Drop received ACK and CTS.
+ */
+#define TXRX_CSR0			0x3040
+#define TXRX_CSR0_RX_ACK_TIMEOUT	FIELD32(0x000001ff)
+#define TXRX_CSR0_TSF_OFFSET		FIELD32(0x00007e00)
+#define TXRX_CSR0_AUTO_TX_SEQ		FIELD32(0x00008000)
+#define TXRX_CSR0_DISABLE_RX		FIELD32(0x00010000)
+#define TXRX_CSR0_DROP_CRC		FIELD32(0x00020000)
+#define TXRX_CSR0_DROP_PHYSICAL		FIELD32(0x00040000)
+#define TXRX_CSR0_DROP_CONTROL		FIELD32(0x00080000)
+#define TXRX_CSR0_DROP_NOT_TO_ME	FIELD32(0x00100000)
+#define TXRX_CSR0_DROP_TO_DS		FIELD32(0x00200000)
+#define TXRX_CSR0_DROP_VERSION_ERROR	FIELD32(0x00400000)
+#define TXRX_CSR0_DROP_MULTICAST	FIELD32(0x00800000)
+#define TXRX_CSR0_DROP_BORADCAST	FIELD32(0x01000000)
+#define TXRX_CSR0_DROP_ACK_CTS		FIELD32(0x02000000)
+#define TXRX_CSR0_TX_WITHOUT_WAITING	FIELD32(0x04000000)
+
+/*
+ * TXRX_CSR1
+ */
+#define TXRX_CSR1			0x3044
+#define TXRX_CSR1_BBP_ID0		FIELD32(0x0000007f)
+#define TXRX_CSR1_BBP_ID0_VALID		FIELD32(0x00000080)
+#define TXRX_CSR1_BBP_ID1		FIELD32(0x00007f00)
+#define TXRX_CSR1_BBP_ID1_VALID		FIELD32(0x00008000)
+#define TXRX_CSR1_BBP_ID2		FIELD32(0x007f0000)
+#define TXRX_CSR1_BBP_ID2_VALID		FIELD32(0x00800000)
+#define TXRX_CSR1_BBP_ID3		FIELD32(0x7f000000)
+#define TXRX_CSR1_BBP_ID3_VALID		FIELD32(0x80000000)
+
+/*
+ * TXRX_CSR2
+ */
+#define TXRX_CSR2			0x3048
+#define TXRX_CSR2_BBP_ID0		FIELD32(0x0000007f)
+#define TXRX_CSR2_BBP_ID0_VALID		FIELD32(0x00000080)
+#define TXRX_CSR2_BBP_ID1		FIELD32(0x00007f00)
+#define TXRX_CSR2_BBP_ID1_VALID		FIELD32(0x00008000)
+#define TXRX_CSR2_BBP_ID2		FIELD32(0x007f0000)
+#define TXRX_CSR2_BBP_ID2_VALID		FIELD32(0x00800000)
+#define TXRX_CSR2_BBP_ID3		FIELD32(0x7f000000)
+#define TXRX_CSR2_BBP_ID3_VALID		FIELD32(0x80000000)
+
+/*
+ * TXRX_CSR3
+ */
+#define TXRX_CSR3			0x304c
+#define TXRX_CSR3_BBP_ID0		FIELD32(0x0000007f)
+#define TXRX_CSR3_BBP_ID0_VALID		FIELD32(0x00000080)
+#define TXRX_CSR3_BBP_ID1		FIELD32(0x00007f00)
+#define TXRX_CSR3_BBP_ID1_VALID		FIELD32(0x00008000)
+#define TXRX_CSR3_BBP_ID2		FIELD32(0x007f0000)
+#define TXRX_CSR3_BBP_ID2_VALID		FIELD32(0x00800000)
+#define TXRX_CSR3_BBP_ID3		FIELD32(0x7f000000)
+#define TXRX_CSR3_BBP_ID3_VALID		FIELD32(0x80000000)
+
+/*
+ * TXRX_CSR4: Auto-Responder/Tx-retry register.
+ * AUTORESPOND_PREAMBLE: 0:long, 1:short preamble.
+ * OFDM_TX_RATE_DOWN: 1:enable.
+ * OFDM_TX_RATE_STEP: 0:1-step, 1: 2-step, 2:3-step, 3:4-step.
+ * OFDM_TX_FALLBACK_CCK: 0: Fallback to OFDM 6M only, 1: Fallback to CCK 1M,2M.
+ */
+#define TXRX_CSR4			0x3050
+#define TXRX_CSR4_TX_ACK_TIMEOUT	FIELD32(0x000000ff)
+#define TXRX_CSR4_CNTL_ACK_POLICY	FIELD32(0x00000700)
+#define TXRX_CSR4_ACK_CTS_PSM		FIELD32(0x00010000)
+#define TXRX_CSR4_AUTORESPOND_ENABLE	FIELD32(0x00020000)
+#define TXRX_CSR4_AUTORESPOND_PREAMBLE	FIELD32(0x00040000)
+#define TXRX_CSR4_OFDM_TX_RATE_DOWN	FIELD32(0x00080000)
+#define TXRX_CSR4_OFDM_TX_RATE_STEP	FIELD32(0x00300000)
+#define TXRX_CSR4_OFDM_TX_FALLBACK_CCK	FIELD32(0x00400000)
+#define TXRX_CSR4_LONG_RETRY_LIMIT	FIELD32(0x0f000000)
+#define TXRX_CSR4_SHORT_RETRY_LIMIT	FIELD32(0xf0000000)
+
+/*
+ * TXRX_CSR5
+ */
+#define TXRX_CSR5			0x3054
+
+/*
+ * TXRX_CSR6: ACK/CTS payload consumed time
+ */
+#define TXRX_CSR6			0x3058
+
+/*
+ * TXRX_CSR7: OFDM ACK/CTS payload consumed time for 6/9/12/18 mbps.
+ */
+#define TXRX_CSR7			0x305c
+#define TXRX_CSR7_ACK_CTS_6MBS		FIELD32(0x000000ff)
+#define TXRX_CSR7_ACK_CTS_9MBS		FIELD32(0x0000ff00)
+#define TXRX_CSR7_ACK_CTS_12MBS		FIELD32(0x00ff0000)
+#define TXRX_CSR7_ACK_CTS_18MBS		FIELD32(0xff000000)
+
+/*
+ * TXRX_CSR8: OFDM ACK/CTS payload consumed time for 24/36/48/54 mbps.
+ */
+#define TXRX_CSR8			0x3060
+#define TXRX_CSR8_ACK_CTS_24MBS		FIELD32(0x000000ff)
+#define TXRX_CSR8_ACK_CTS_36MBS		FIELD32(0x0000ff00)
+#define TXRX_CSR8_ACK_CTS_48MBS		FIELD32(0x00ff0000)
+#define TXRX_CSR8_ACK_CTS_54MBS		FIELD32(0xff000000)
+
+/*
+ * TXRX_CSR9: Synchronization control register.
+ * BEACON_INTERVAL: In unit of 1/16 TU.
+ * TSF_TICKING: Enable TSF auto counting.
+ * TSF_SYNC: Tsf sync, 0: disable, 1: infra, 2: ad-hoc/master mode.
+ * BEACON_GEN: Enable beacon generator.
+ */
+#define TXRX_CSR9			0x3064
+#define TXRX_CSR9_BEACON_INTERVAL	FIELD32(0x0000ffff)
+#define TXRX_CSR9_TSF_TICKING		FIELD32(0x00010000)
+#define TXRX_CSR9_TSF_SYNC		FIELD32(0x00060000)
+#define TXRX_CSR9_TBTT_ENABLE		FIELD32(0x00080000)
+#define TXRX_CSR9_BEACON_GEN		FIELD32(0x00100000)
+#define TXRX_CSR9_TIMESTAMP_COMPENSATE	FIELD32(0xff000000)
+
+/*
+ * TXRX_CSR10: BEACON alignment.
+ */
+#define TXRX_CSR10			0x3068
+
+/*
+ * TXRX_CSR11: AES mask.
+ */
+#define TXRX_CSR11			0x306c
+
+/*
+ * TXRX_CSR12: TSF low 32.
+ */
+#define TXRX_CSR12			0x3070
+#define TXRX_CSR12_LOW_TSFTIMER		FIELD32(0xffffffff)
+
+/*
+ * TXRX_CSR13: TSF high 32.
+ */
+#define TXRX_CSR13			0x3074
+#define TXRX_CSR13_HIGH_TSFTIMER	FIELD32(0xffffffff)
+
+/*
+ * TXRX_CSR14: TBTT timer.
+ */
+#define TXRX_CSR14			0x3078
+
+/*
+ * TXRX_CSR15: TKIP MIC priority byte "AND" mask.
+ */
+#define TXRX_CSR15			0x307c
+
+/*
+ * PHY control registers.
+ * Some values are set in TU, whereas 1 TU == 1024 us.
+ */
+
+/*
+ * PHY_CSR0: RF/PS control.
+ */
+#define PHY_CSR0			0x3080
+#define PHY_CSR0_PA_PE_BG		FIELD32(0x00010000)
+#define PHY_CSR0_PA_PE_A		FIELD32(0x00020000)
+
+/*
+ * PHY_CSR1
+ */
+#define PHY_CSR1			0x3084
+
+/*
+ * PHY_CSR2: Pre-TX BBP control.
+ */
+#define PHY_CSR2			0x3088
+
+/*
+ * PHY_CSR3: BBP serial control register.
+ * VALUE: Register value to program into BBP.
+ * REG_NUM: Selected BBP register.
+ * READ_CONTROL: 0: Write BBP, 1: Read BBP.
+ * BUSY: 1: ASIC is busy execute BBP programming.
+ */
+#define PHY_CSR3			0x308c
+#define PHY_CSR3_VALUE			FIELD32(0x000000ff)
+#define PHY_CSR3_REGNUM			FIELD32(0x00007f00)
+#define PHY_CSR3_READ_CONTROL		FIELD32(0x00008000)
+#define PHY_CSR3_BUSY			FIELD32(0x00010000)
+
+/*
+ * PHY_CSR4: RF serial control register
+ * VALUE: Register value (include register id) serial out to RF/IF chip.
+ * NUMBER_OF_BITS: Number of bits used in RFRegValue (I:20, RFMD:22).
+ * IF_SELECT: 1: select IF to program, 0: select RF to program.
+ * PLL_LD: RF PLL_LD status.
+ * BUSY: 1: ASIC is busy execute RF programming.
+ */
+#define PHY_CSR4			0x3090
+#define PHY_CSR4_VALUE			FIELD32(0x00ffffff)
+#define PHY_CSR4_NUMBER_OF_BITS		FIELD32(0x1f000000)
+#define PHY_CSR4_IF_SELECT		FIELD32(0x20000000)
+#define PHY_CSR4_PLL_LD			FIELD32(0x40000000)
+#define PHY_CSR4_BUSY			FIELD32(0x80000000)
+
+/*
+ * PHY_CSR5: RX to TX signal switch timing control.
+ */
+#define PHY_CSR5			0x3094
+#define PHY_CSR5_IQ_FLIP		FIELD32(0x00000004)
+
+/*
+ * PHY_CSR6: TX to RX signal timing control.
+ */
+#define PHY_CSR6			0x3098
+#define PHY_CSR6_IQ_FLIP		FIELD32(0x00000004)
+
+/*
+ * PHY_CSR7: TX DAC switching timing control.
+ */
+#define PHY_CSR7			0x309c
+
+/*
+ * Security control register.
+ */
+
+/*
+ * SEC_CSR0: Shared key table control.
+ */
+#define SEC_CSR0			0x30a0
+#define SEC_CSR0_BSS0_KEY0_VALID	FIELD32(0x00000001)
+#define SEC_CSR0_BSS0_KEY1_VALID	FIELD32(0x00000002)
+#define SEC_CSR0_BSS0_KEY2_VALID	FIELD32(0x00000004)
+#define SEC_CSR0_BSS0_KEY3_VALID	FIELD32(0x00000008)
+#define SEC_CSR0_BSS1_KEY0_VALID	FIELD32(0x00000010)
+#define SEC_CSR0_BSS1_KEY1_VALID	FIELD32(0x00000020)
+#define SEC_CSR0_BSS1_KEY2_VALID	FIELD32(0x00000040)
+#define SEC_CSR0_BSS1_KEY3_VALID	FIELD32(0x00000080)
+#define SEC_CSR0_BSS2_KEY0_VALID	FIELD32(0x00000100)
+#define SEC_CSR0_BSS2_KEY1_VALID	FIELD32(0x00000200)
+#define SEC_CSR0_BSS2_KEY2_VALID	FIELD32(0x00000400)
+#define SEC_CSR0_BSS2_KEY3_VALID	FIELD32(0x00000800)
+#define SEC_CSR0_BSS3_KEY0_VALID	FIELD32(0x00001000)
+#define SEC_CSR0_BSS3_KEY1_VALID	FIELD32(0x00002000)
+#define SEC_CSR0_BSS3_KEY2_VALID	FIELD32(0x00004000)
+#define SEC_CSR0_BSS3_KEY3_VALID	FIELD32(0x00008000)
+
+/*
+ * SEC_CSR1: Shared key table security mode register.
+ */
+#define SEC_CSR1			0x30a4
+#define SEC_CSR1_BSS0_KEY0_CIPHER_ALG	FIELD32(0x00000007)
+#define SEC_CSR1_BSS0_KEY1_CIPHER_ALG	FIELD32(0x00000070)
+#define SEC_CSR1_BSS0_KEY2_CIPHER_ALG	FIELD32(0x00000700)
+#define SEC_CSR1_BSS0_KEY3_CIPHER_ALG	FIELD32(0x00007000)
+#define SEC_CSR1_BSS1_KEY0_CIPHER_ALG	FIELD32(0x00070000)
+#define SEC_CSR1_BSS1_KEY1_CIPHER_ALG	FIELD32(0x00700000)
+#define SEC_CSR1_BSS1_KEY2_CIPHER_ALG	FIELD32(0x07000000)
+#define SEC_CSR1_BSS1_KEY3_CIPHER_ALG	FIELD32(0x70000000)
+
+/*
+ * Pairwise key table valid bitmap registers.
+ * SEC_CSR2: pairwise key table valid bitmap 0.
+ * SEC_CSR3: pairwise key table valid bitmap 1.
+ */
+#define SEC_CSR2			0x30a8
+#define SEC_CSR3			0x30ac
+
+/*
+ * SEC_CSR4: Pairwise key table lookup control.
+ */
+#define SEC_CSR4			0x30b0
+
+/*
+ * SEC_CSR5: shared key table security mode register.
+ */
+#define SEC_CSR5			0x30b4
+#define SEC_CSR5_BSS2_KEY0_CIPHER_ALG	FIELD32(0x00000007)
+#define SEC_CSR5_BSS2_KEY1_CIPHER_ALG	FIELD32(0x00000070)
+#define SEC_CSR5_BSS2_KEY2_CIPHER_ALG	FIELD32(0x00000700)
+#define SEC_CSR5_BSS2_KEY3_CIPHER_ALG	FIELD32(0x00007000)
+#define SEC_CSR5_BSS3_KEY0_CIPHER_ALG	FIELD32(0x00070000)
+#define SEC_CSR5_BSS3_KEY1_CIPHER_ALG	FIELD32(0x00700000)
+#define SEC_CSR5_BSS3_KEY2_CIPHER_ALG	FIELD32(0x07000000)
+#define SEC_CSR5_BSS3_KEY3_CIPHER_ALG	FIELD32(0x70000000)
+
+/*
+ * STA control registers.
+ */
+
+/*
+ * STA_CSR0: RX PLCP error count & RX FCS error count.
+ */
+#define STA_CSR0			0x30c0
+#define STA_CSR0_FCS_ERROR		FIELD32(0x0000ffff)
+#define STA_CSR0_PLCP_ERROR		FIELD32(0xffff0000)
+
+/*
+ * STA_CSR1: RX False CCA count & RX LONG frame count.
+ */
+#define STA_CSR1			0x30c4
+#define STA_CSR1_PHYSICAL_ERROR		FIELD32(0x0000ffff)
+#define STA_CSR1_FALSE_CCA_ERROR	FIELD32(0xffff0000)
+
+/*
+ * STA_CSR2: TX Beacon count and RX FIFO overflow count.
+ */
+#define STA_CSR2			0x30c8
+#define STA_CSR2_RX_FIFO_OVERFLOW_COUNT	FIELD32(0x0000ffff)
+#define STA_CSR2_RX_OVERFLOW_COUNT	FIELD32(0xffff0000)
+
+/*
+ * STA_CSR3: TX Beacon count.
+ */
+#define STA_CSR3			0x30cc
+#define STA_CSR3_TX_BEACON_COUNT	FIELD32(0x0000ffff)
+
+/*
+ * STA_CSR4: TX Result status register.
+ * VALID: 1:This register contains a valid TX result.
+ */
+#define STA_CSR4			0x30d0
+#define STA_CSR4_VALID			FIELD32(0x00000001)
+#define STA_CSR4_TX_RESULT		FIELD32(0x0000000e)
+#define STA_CSR4_RETRY_COUNT		FIELD32(0x000000f0)
+#define STA_CSR4_PID_SUBTYPE		FIELD32(0x00001f00)
+#define STA_CSR4_PID_TYPE		FIELD32(0x0000e000)
+#define STA_CSR4_TXRATE			FIELD32(0x000f0000)
+
+/*
+ * QOS control registers.
+ */
+
+/*
+ * QOS_CSR0: TXOP holder MAC address register.
+ */
+#define QOS_CSR0			0x30e0
+#define QOS_CSR0_BYTE0			FIELD32(0x000000ff)
+#define QOS_CSR0_BYTE1			FIELD32(0x0000ff00)
+#define QOS_CSR0_BYTE2			FIELD32(0x00ff0000)
+#define QOS_CSR0_BYTE3			FIELD32(0xff000000)
+
+/*
+ * QOS_CSR1: TXOP holder MAC address register.
+ */
+#define QOS_CSR1			0x30e4
+#define QOS_CSR1_BYTE4			FIELD32(0x000000ff)
+#define QOS_CSR1_BYTE5			FIELD32(0x0000ff00)
+
+/*
+ * QOS_CSR2: TXOP holder timeout register.
+ */
+#define QOS_CSR2			0x30e8
+
+/*
+ * RX QOS-CFPOLL MAC address register.
+ * QOS_CSR3: RX QOS-CFPOLL MAC address 0.
+ * QOS_CSR4: RX QOS-CFPOLL MAC address 1.
+ */
+#define QOS_CSR3			0x30ec
+#define QOS_CSR4			0x30f0
+
+/*
+ * QOS_CSR5: "QosControl" field of the RX QOS-CFPOLL.
+ */
+#define QOS_CSR5			0x30f4
+
+/*
+ * Host DMA registers.
+ */
+
+/*
+ * AC0_BASE_CSR: AC_BK base address.
+ */
+#define AC0_BASE_CSR			0x3400
+#define AC0_BASE_CSR_RING_REGISTER	FIELD32(0xffffffff)
+
+/*
+ * AC1_BASE_CSR: AC_BE base address.
+ */
+#define AC1_BASE_CSR			0x3404
+#define AC1_BASE_CSR_RING_REGISTER	FIELD32(0xffffffff)
+
+/*
+ * AC2_BASE_CSR: AC_VI base address.
+ */
+#define AC2_BASE_CSR			0x3408
+#define AC2_BASE_CSR_RING_REGISTER	FIELD32(0xffffffff)
+
+/*
+ * AC3_BASE_CSR: AC_VO base address.
+ */
+#define AC3_BASE_CSR			0x340c
+#define AC3_BASE_CSR_RING_REGISTER	FIELD32(0xffffffff)
+
+/*
+ * MGMT_BASE_CSR: MGMT ring base address.
+ */
+#define MGMT_BASE_CSR			0x3410
+#define MGMT_BASE_CSR_RING_REGISTER	FIELD32(0xffffffff)
+
+/*
+ * TX_RING_CSR0: TX Ring size for AC_BK, AC_BE, AC_VI, AC_VO.
+ */
+#define TX_RING_CSR0			0x3418
+#define TX_RING_CSR0_AC0_RING_SIZE	FIELD32(0x000000ff)
+#define TX_RING_CSR0_AC1_RING_SIZE	FIELD32(0x0000ff00)
+#define TX_RING_CSR0_AC2_RING_SIZE	FIELD32(0x00ff0000)
+#define TX_RING_CSR0_AC3_RING_SIZE	FIELD32(0xff000000)
+
+/*
+ * TX_RING_CSR1: TX Ring size for MGMT Ring, HCCA Ring
+ * TXD_SIZE: In unit of 32-bit.
+ */
+#define TX_RING_CSR1			0x341c
+#define TX_RING_CSR1_MGMT_RING_SIZE	FIELD32(0x000000ff)
+#define TX_RING_CSR1_HCCA_RING_SIZE	FIELD32(0x0000ff00)
+#define TX_RING_CSR1_TXD_SIZE		FIELD32(0x003f0000)
+
+/*
+ * AIFSN_CSR: AIFSN for each EDCA AC.
+ * AIFSN0: For AC_BK.
+ * AIFSN1: For AC_BE.
+ * AIFSN2: For AC_VI.
+ * AIFSN3: For AC_VO.
+ */
+#define AIFSN_CSR			0x3420
+#define AIFSN_CSR_AIFSN0		FIELD32(0x0000000f)
+#define AIFSN_CSR_AIFSN1		FIELD32(0x000000f0)
+#define AIFSN_CSR_AIFSN2		FIELD32(0x00000f00)
+#define AIFSN_CSR_AIFSN3		FIELD32(0x0000f000)
+
+/*
+ * CWMIN_CSR: CWmin for each EDCA AC.
+ * CWMIN0: For AC_BK.
+ * CWMIN1: For AC_BE.
+ * CWMIN2: For AC_VI.
+ * CWMIN3: For AC_VO.
+ */
+#define CWMIN_CSR			0x3424
+#define CWMIN_CSR_CWMIN0		FIELD32(0x0000000f)
+#define CWMIN_CSR_CWMIN1		FIELD32(0x000000f0)
+#define CWMIN_CSR_CWMIN2		FIELD32(0x00000f00)
+#define CWMIN_CSR_CWMIN3		FIELD32(0x0000f000)
+
+/*
+ * CWMAX_CSR: CWmax for each EDCA AC.
+ * CWMAX0: For AC_BK.
+ * CWMAX1: For AC_BE.
+ * CWMAX2: For AC_VI.
+ * CWMAX3: For AC_VO.
+ */
+#define CWMAX_CSR			0x3428
+#define CWMAX_CSR_CWMAX0		FIELD32(0x0000000f)
+#define CWMAX_CSR_CWMAX1		FIELD32(0x000000f0)
+#define CWMAX_CSR_CWMAX2		FIELD32(0x00000f00)
+#define CWMAX_CSR_CWMAX3		FIELD32(0x0000f000)
+
+/*
+ * TX_DMA_DST_CSR: TX DMA destination
+ * 0: TX ring0, 1: TX ring1, 2: TX ring2 3: invalid
+ */
+#define TX_DMA_DST_CSR			0x342c
+#define TX_DMA_DST_CSR_DEST_AC0		FIELD32(0x00000003)
+#define TX_DMA_DST_CSR_DEST_AC1		FIELD32(0x0000000c)
+#define TX_DMA_DST_CSR_DEST_AC2		FIELD32(0x00000030)
+#define TX_DMA_DST_CSR_DEST_AC3		FIELD32(0x000000c0)
+#define TX_DMA_DST_CSR_DEST_MGMT	FIELD32(0x00000300)
+
+/*
+ * TX_CNTL_CSR: KICK/Abort TX.
+ * KICK_TX_AC0: For AC_BK.
+ * KICK_TX_AC1: For AC_BE.
+ * KICK_TX_AC2: For AC_VI.
+ * KICK_TX_AC3: For AC_VO.
+ * ABORT_TX_AC0: For AC_BK.
+ * ABORT_TX_AC1: For AC_BE.
+ * ABORT_TX_AC2: For AC_VI.
+ * ABORT_TX_AC3: For AC_VO.
+ */
+#define TX_CNTL_CSR			0x3430
+#define TX_CNTL_CSR_KICK_TX_AC0		FIELD32(0x00000001)
+#define TX_CNTL_CSR_KICK_TX_AC1		FIELD32(0x00000002)
+#define TX_CNTL_CSR_KICK_TX_AC2		FIELD32(0x00000004)
+#define TX_CNTL_CSR_KICK_TX_AC3		FIELD32(0x00000008)
+#define TX_CNTL_CSR_KICK_TX_MGMT	FIELD32(0x00000010)
+#define TX_CNTL_CSR_ABORT_TX_AC0	FIELD32(0x00010000)
+#define TX_CNTL_CSR_ABORT_TX_AC1	FIELD32(0x00020000)
+#define TX_CNTL_CSR_ABORT_TX_AC2	FIELD32(0x00040000)
+#define TX_CNTL_CSR_ABORT_TX_AC3	FIELD32(0x00080000)
+#define TX_CNTL_CSR_ABORT_TX_MGMT	FIELD32(0x00100000)
+
+/*
+ * LOAD_TX_RING_CSR: Load RX de
+ */
+#define LOAD_TX_RING_CSR		0x3434
+#define LOAD_TX_RING_CSR_LOAD_TXD_AC0	FIELD32(0x00000001)
+#define LOAD_TX_RING_CSR_LOAD_TXD_AC1	FIELD32(0x00000002)
+#define LOAD_TX_RING_CSR_LOAD_TXD_AC2	FIELD32(0x00000004)
+#define LOAD_TX_RING_CSR_LOAD_TXD_AC3	FIELD32(0x00000008)
+#define LOAD_TX_RING_CSR_LOAD_TXD_MGMT	FIELD32(0x00000010)
+
+/*
+ * Several read-only registers, for debugging.
+ */
+#define AC0_TXPTR_CSR			0x3438
+#define AC1_TXPTR_CSR			0x343c
+#define AC2_TXPTR_CSR			0x3440
+#define AC3_TXPTR_CSR			0x3444
+#define MGMT_TXPTR_CSR			0x3448
+
+/*
+ * RX_BASE_CSR
+ */
+#define RX_BASE_CSR			0x3450
+#define RX_BASE_CSR_RING_REGISTER	FIELD32(0xffffffff)
+
+/*
+ * RX_RING_CSR.
+ * RXD_SIZE: In unit of 32-bit.
+ */
+#define RX_RING_CSR			0x3454
+#define RX_RING_CSR_RING_SIZE		FIELD32(0x000000ff)
+#define RX_RING_CSR_RXD_SIZE		FIELD32(0x00003f00)
+#define RX_RING_CSR_RXD_WRITEBACK_SIZE	FIELD32(0x00070000)
+
+/*
+ * RX_CNTL_CSR
+ */
+#define RX_CNTL_CSR			0x3458
+#define RX_CNTL_CSR_ENABLE_RX_DMA	FIELD32(0x00000001)
+#define RX_CNTL_CSR_LOAD_RXD		FIELD32(0x00000002)
+
+/*
+ * RXPTR_CSR: Read-only, for debugging.
+ */
+#define RXPTR_CSR			0x345c
+
+/*
+ * PCI_CFG_CSR
+ */
+#define PCI_CFG_CSR			0x3460
+
+/*
+ * BUF_FORMAT_CSR
+ */
+#define BUF_FORMAT_CSR			0x3464
+
+/*
+ * INT_SOURCE_CSR: Interrupt source register.
+ * Write one to clear corresponding bit.
+ */
+#define INT_SOURCE_CSR			0x3468
+#define INT_SOURCE_CSR_TXDONE		FIELD32(0x00000001)
+#define INT_SOURCE_CSR_RXDONE		FIELD32(0x00000002)
+#define INT_SOURCE_CSR_BEACON_DONE	FIELD32(0x00000004)
+#define INT_SOURCE_CSR_TX_ABORT_DONE	FIELD32(0x00000010)
+#define INT_SOURCE_CSR_AC0_DMA_DONE	FIELD32(0x00010000)
+#define INT_SOURCE_CSR_AC1_DMA_DONE	FIELD32(0x00020000)
+#define INT_SOURCE_CSR_AC2_DMA_DONE	FIELD32(0x00040000)
+#define INT_SOURCE_CSR_AC3_DMA_DONE	FIELD32(0x00080000)
+#define INT_SOURCE_CSR_MGMT_DMA_DONE	FIELD32(0x00100000)
+#define INT_SOURCE_CSR_HCCA_DMA_DONE	FIELD32(0x00200000)
+
+/*
+ * INT_MASK_CSR: Interrupt MASK register. 1: the interrupt is mask OFF.
+ * MITIGATION_PERIOD: Interrupt mitigation in unit of 32 PCI clock.
+ */
+#define INT_MASK_CSR			0x346c
+#define INT_MASK_CSR_TXDONE		FIELD32(0x00000001)
+#define INT_MASK_CSR_RXDONE		FIELD32(0x00000002)
+#define INT_MASK_CSR_BEACON_DONE	FIELD32(0x00000004)
+#define INT_MASK_CSR_TX_ABORT_DONE	FIELD32(0x00000010)
+#define INT_MASK_CSR_ENABLE_MITIGATION	FIELD32(0x00000080)
+#define INT_MASK_CSR_MITIGATION_PERIOD	FIELD32(0x0000ff00)
+#define INT_MASK_CSR_AC0_DMA_DONE	FIELD32(0x00010000)
+#define INT_MASK_CSR_AC1_DMA_DONE	FIELD32(0x00020000)
+#define INT_MASK_CSR_AC2_DMA_DONE	FIELD32(0x00040000)
+#define INT_MASK_CSR_AC3_DMA_DONE	FIELD32(0x00080000)
+#define INT_MASK_CSR_MGMT_DMA_DONE	FIELD32(0x00100000)
+#define INT_MASK_CSR_HCCA_DMA_DONE	FIELD32(0x00200000)
+
+/*
+ * E2PROM_CSR: EEPROM control register.
+ * RELOAD: Write 1 to reload eeprom content.
+ * TYPE_93C46: 1: 93c46, 0:93c66.
+ * LOAD_STATUS: 1:loading, 0:done.
+ */
+#define E2PROM_CSR			0x3470
+#define E2PROM_CSR_RELOAD		FIELD32(0x00000001)
+#define E2PROM_CSR_DATA_CLOCK		FIELD32(0x00000002)
+#define E2PROM_CSR_CHIP_SELECT		FIELD32(0x00000004)
+#define E2PROM_CSR_DATA_IN		FIELD32(0x00000008)
+#define E2PROM_CSR_DATA_OUT		FIELD32(0x00000010)
+#define E2PROM_CSR_TYPE_93C46		FIELD32(0x00000020)
+#define E2PROM_CSR_LOAD_STATUS		FIELD32(0x00000040)
+
+/*
+ * AC_TXOP_CSR0: AC_BK/AC_BE TXOP register.
+ * AC0_TX_OP: For AC_BK, in unit of 32us.
+ * AC1_TX_OP: For AC_BE, in unit of 32us.
+ */
+#define AC_TXOP_CSR0			0x3474
+#define AC_TXOP_CSR0_AC0_TX_OP		FIELD32(0x0000ffff)
+#define AC_TXOP_CSR0_AC1_TX_OP		FIELD32(0xffff0000)
+
+/*
+ * AC_TXOP_CSR1: AC_VO/AC_VI TXOP register.
+ * AC2_TX_OP: For AC_VI, in unit of 32us.
+ * AC3_TX_OP: For AC_VO, in unit of 32us.
+ */
+#define AC_TXOP_CSR1			0x3478
+#define AC_TXOP_CSR1_AC2_TX_OP		FIELD32(0x0000ffff)
+#define AC_TXOP_CSR1_AC3_TX_OP		FIELD32(0xffff0000)
+
+/*
+ * DMA_STATUS_CSR
+ */
+#define DMA_STATUS_CSR			0x3480
+
+/*
+ * TEST_MODE_CSR
+ */
+#define TEST_MODE_CSR			0x3484
+
+/*
+ * UART0_TX_CSR
+ */
+#define UART0_TX_CSR			0x3488
+
+/*
+ * UART0_RX_CSR
+ */
+#define UART0_RX_CSR			0x348c
+
+/*
+ * UART0_FRAME_CSR
+ */
+#define UART0_FRAME_CSR			0x3490
+
+/*
+ * UART0_BUFFER_CSR
+ */
+#define UART0_BUFFER_CSR		0x3494
+
+/*
+ * IO_CNTL_CSR
+ */
+#define IO_CNTL_CSR			0x3498
+
+/*
+ * UART_INT_SOURCE_CSR
+ */
+#define UART_INT_SOURCE_CSR		0x34a8
+
+/*
+ * UART_INT_MASK_CSR
+ */
+#define UART_INT_MASK_CSR		0x34ac
+
+/*
+ * PBF_QUEUE_CSR
+ */
+#define PBF_QUEUE_CSR			0x34b0
+
+/*
+ * Firmware DMA registers.
+ * Firmware DMA registers are dedicated for MCU usage
+ * and should not be touched by host driver.
+ * Therefore we skip the definition of these registers.
+ */
+#define FW_TX_BASE_CSR			0x34c0
+#define FW_TX_START_CSR			0x34c4
+#define FW_TX_LAST_CSR			0x34c8
+#define FW_MODE_CNTL_CSR		0x34cc
+#define FW_TXPTR_CSR			0x34d0
+
+/*
+ * 8051 firmware image.
+ */
+#define FIRMWARE_RT2561			"rt2561.bin"
+#define FIRMWARE_RT2561s		"rt2561s.bin"
+#define FIRMWARE_RT2661			"rt2661.bin"
+#define FIRMWARE_IMAGE_BASE		0x4000
+
+/*
+ * BBP registers.
+ * The wordsize of the BBP is 8 bits.
+ */
+
+/*
+ * R2
+ */
+#define BBP_R2_BG_MODE			FIELD8(0x20)
+
+/*
+ * R3
+ */
+#define BBP_R3_SMART_MODE		FIELD8(0x01)
+
+/*
+ * R4: RX antenna control
+ * FRAME_END: 1 - DPDT, 0 - SPDT (Only valid for 802.11G, RF2527 & RF2529)
+ */
+#define BBP_R4_RX_ANTENNA		FIELD8(0x03)
+#define BBP_R4_RX_FRAME_END		FIELD8(0x20)
+
+/*
+ * R77
+ */
+#define BBP_R77_PAIR			FIELD8(0x03)
+
+/*
+ * RF registers
+ */
+
+/*
+ * RF 3
+ */
+#define RF3_TXPOWER			FIELD32(0x00003e00)
+
+/*
+ * RF 4
+ */
+#define RF4_FREQ_OFFSET			FIELD32(0x0003f000)
+
+/*
+ * EEPROM content.
+ * The wordsize of the EEPROM is 16 bits.
+ */
+
+/*
+ * HW MAC address.
+ */
+#define EEPROM_MAC_ADDR_0		0x0002
+#define EEPROM_MAC_ADDR_BYTE0		FIELD16(0x00ff)
+#define EEPROM_MAC_ADDR_BYTE1		FIELD16(0xff00)
+#define EEPROM_MAC_ADDR1		0x0004
+#define EEPROM_MAC_ADDR_BYTE2		FIELD16(0x00ff)
+#define EEPROM_MAC_ADDR_BYTE3		FIELD16(0xff00)
+#define EEPROM_MAC_ADDR_2		0x0006
+#define EEPROM_MAC_ADDR_BYTE4		FIELD16(0x00ff)
+#define EEPROM_MAC_ADDR_BYTE5		FIELD16(0xff00)
+
+/*
+ * EEPROM antenna.
+ * ANTENNA_NUM: Number of antenna's.
+ * TX_DEFAULT: Default antenna 0: diversity, 1: A, 2: B.
+ * RX_DEFAULT: Default antenna 0: diversity, 1: A, 2: B.
+ * FRAME_TYPE: 0: DPDT , 1: SPDT , noted this bit is valid for g only.
+ * DYN_TXAGC: Dynamic TX AGC control.
+ * HARDWARE_RADIO: 1: Hardware controlled radio. Read GPIO0.
+ * RF_TYPE: Rf_type of this adapter.
+ */
+#define EEPROM_ANTENNA			0x0010
+#define EEPROM_ANTENNA_NUM		FIELD16(0x0003)
+#define EEPROM_ANTENNA_TX_DEFAULT	FIELD16(0x000c)
+#define EEPROM_ANTENNA_RX_DEFAULT	FIELD16(0x0030)
+#define EEPROM_ANTENNA_FRAME_TYPE	FIELD16(0x0040)
+#define EEPROM_ANTENNA_DYN_TXAGC	FIELD16(0x0200)
+#define EEPROM_ANTENNA_HARDWARE_RADIO	FIELD16(0x0400)
+#define EEPROM_ANTENNA_RF_TYPE		FIELD16(0xf800)
+
+/*
+ * EEPROM NIC config.
+ * ENABLE_DIVERSITY: 1:enable, 0:disable.
+ * EXTERNAL_LNA_BG: External LNA enable for 2.4G.
+ * CARDBUS_ACCEL: 0:enable, 1:disable.
+ * EXTERNAL_LNA_A: External LNA enable for 5G.
+ */
+#define EEPROM_NIC			0x0011
+#define EEPROM_NIC_ENABLE_DIVERSITY	FIELD16(0x0001)
+#define EEPROM_NIC_TX_DIVERSITY		FIELD16(0x0002)
+#define EEPROM_NIC_TX_RX_FIXED		FIELD16(0x000c)
+#define EEPROM_NIC_EXTERNAL_LNA_BG	FIELD16(0x0010)
+#define EEPROM_NIC_CARDBUS_ACCEL	FIELD16(0x0020)
+#define EEPROM_NIC_EXTERNAL_LNA_A	FIELD16(0x0040)
+
+/*
+ * EEPROM geography.
+ * GEO_A: Default geographical setting for 5GHz band
+ * GEO: Default geographical setting.
+ */
+#define EEPROM_GEOGRAPHY		0x0012
+#define EEPROM_GEOGRAPHY_GEO_A		FIELD16(0x00ff)
+#define EEPROM_GEOGRAPHY_GEO		FIELD16(0xff00)
+
+/*
+ * EEPROM BBP.
+ */
+#define EEPROM_BBP_START		0x0013
+#define EEPROM_BBP_SIZE			16
+#define EEPROM_BBP_VALUE		FIELD16(0x00ff)
+#define EEPROM_BBP_REG_ID		FIELD16(0xff00)
+
+/*
+ * EEPROM TXPOWER 802.11G
+ */
+#define EEPROM_TXPOWER_G_START		0x0023
+#define EEPROM_TXPOWER_G_SIZE		7
+#define EEPROM_TXPOWER_G_1		FIELD16(0x00ff)
+#define EEPROM_TXPOWER_G_2		FIELD16(0xff00)
+
+/*
+ * EEPROM Frequency
+ */
+#define EEPROM_FREQ			0x002f
+#define EEPROM_FREQ_OFFSET		FIELD16(0x00ff)
+#define EEPROM_FREQ_SEQ_MASK		FIELD16(0xff00)
+#define EEPROM_FREQ_SEQ			FIELD16(0x0300)
+
+/*
+ * EEPROM LED.
+ * POLARITY_RDY_G: Polarity RDY_G setting.
+ * POLARITY_RDY_A: Polarity RDY_A setting.
+ * POLARITY_ACT: Polarity ACT setting.
+ * POLARITY_GPIO_0: Polarity GPIO0 setting.
+ * POLARITY_GPIO_1: Polarity GPIO1 setting.
+ * POLARITY_GPIO_2: Polarity GPIO2 setting.
+ * POLARITY_GPIO_3: Polarity GPIO3 setting.
+ * POLARITY_GPIO_4: Polarity GPIO4 setting.
+ * LED_MODE: Led mode.
+ */
+#define EEPROM_LED			0x0030
+#define EEPROM_LED_POLARITY_RDY_G	FIELD16(0x0001)
+#define EEPROM_LED_POLARITY_RDY_A	FIELD16(0x0002)
+#define EEPROM_LED_POLARITY_ACT		FIELD16(0x0004)
+#define EEPROM_LED_POLARITY_GPIO_0	FIELD16(0x0008)
+#define EEPROM_LED_POLARITY_GPIO_1	FIELD16(0x0010)
+#define EEPROM_LED_POLARITY_GPIO_2	FIELD16(0x0020)
+#define EEPROM_LED_POLARITY_GPIO_3	FIELD16(0x0040)
+#define EEPROM_LED_POLARITY_GPIO_4	FIELD16(0x0080)
+#define EEPROM_LED_LED_MODE		FIELD16(0x1f00)
+
+/*
+ * EEPROM TXPOWER 802.11A
+ */
+#define EEPROM_TXPOWER_A_START		0x0031
+#define EEPROM_TXPOWER_A_SIZE		12
+#define EEPROM_TXPOWER_A_1		FIELD16(0x00ff)
+#define EEPROM_TXPOWER_A_2		FIELD16(0xff00)
+
+/*
+ * EEPROM RSSI offset 802.11BG
+ */
+#define EEPROM_RSSI_OFFSET_BG		0x004d
+#define EEPROM_RSSI_OFFSET_BG_1		FIELD16(0x00ff)
+#define EEPROM_RSSI_OFFSET_BG_2		FIELD16(0xff00)
+
+/*
+ * EEPROM RSSI offset 802.11A
+ */
+#define EEPROM_RSSI_OFFSET_A		0x004e
+#define EEPROM_RSSI_OFFSET_A_1		FIELD16(0x00ff)
+#define EEPROM_RSSI_OFFSET_A_2		FIELD16(0xff00)
+
+/*
+ * MCU mailbox commands.
+ */
+#define MCU_SLEEP			0x30
+#define MCU_WAKEUP			0x31
+#define MCU_LED				0x50
+#define MCU_LED_STRENGTH		0x52
+
+/*
+ * DMA descriptor defines.
+ */
+#define TXD_DESC_SIZE			( 16 * sizeof(struct data_desc) )
+#define RXD_DESC_SIZE			( 16 * sizeof(struct data_desc) )
+
+/*
+ * TX descriptor format for TX, PRIO and Beacon Ring.
+ */
+
+/*
+ * Word0
+ * TKIP_MIC: ASIC appends TKIP MIC if TKIP is used.
+ * KEY_TABLE: Use per-client pairwise KEY table.
+ * KEY_INDEX:
+ * Key index (0~31) to the pairwise KEY table.
+ * 0~3 to shared KEY table 0 (BSS0).
+ * 4~7 to shared KEY table 1 (BSS1).
+ * 8~11 to shared KEY table 2 (BSS2).
+ * 12~15 to shared KEY table 3 (BSS3).
+ * BURST: Next frame belongs to same "burst" event.
+ */
+#define TXD_W0_OWNER_NIC		FIELD32(0x00000001)
+#define TXD_W0_VALID			FIELD32(0x00000002)
+#define TXD_W0_MORE_FRAG		FIELD32(0x00000004)
+#define TXD_W0_ACK			FIELD32(0x00000008)
+#define TXD_W0_TIMESTAMP		FIELD32(0x00000010)
+#define TXD_W0_OFDM			FIELD32(0x00000020)
+#define TXD_W0_IFS			FIELD32(0x00000040)
+#define TXD_W0_RETRY_MODE		FIELD32(0x00000080)
+#define TXD_W0_TKIP_MIC			FIELD32(0x00000100)
+#define TXD_W0_KEY_TABLE		FIELD32(0x00000200)
+#define TXD_W0_KEY_INDEX		FIELD32(0x0000fc00)
+#define TXD_W0_DATABYTE_COUNT		FIELD32(0x0fff0000)
+#define TXD_W0_BURST			FIELD32(0x10000000)
+#define TXD_W0_CIPHER_ALG		FIELD32(0xe0000000)
+
+/*
+ * Word1
+ * HOST_Q_ID: EDCA/HCCA queue ID.
+ * HW_SEQUENCE: MAC overwrites the frame sequence number.
+ * BUFFER_COUNT: Number of buffers in this TXD.
+ */
+#define TXD_W1_HOST_Q_ID		FIELD32(0x0000000f)
+#define TXD_W1_AIFSN			FIELD32(0x000000f0)
+#define TXD_W1_CWMIN			FIELD32(0x00000f00)
+#define TXD_W1_CWMAX			FIELD32(0x0000f000)
+#define TXD_W1_IV_OFFSET		FIELD32(0x003f0000)
+#define TXD_W1_PIGGY_BACK		FIELD32(0x01000000)
+#define TXD_W1_HW_SEQUENCE		FIELD32(0x10000000)
+#define TXD_W1_BUFFER_COUNT		FIELD32(0xe0000000)
+
+/*
+ * Word2: PLCP information
+ */
+#define TXD_W2_PLCP_SIGNAL		FIELD32(0x000000ff)
+#define TXD_W2_PLCP_SERVICE		FIELD32(0x0000ff00)
+#define TXD_W2_PLCP_LENGTH_LOW		FIELD32(0x00ff0000)
+#define TXD_W2_PLCP_LENGTH_HIGH		FIELD32(0xff000000)
+
+/*
+ * Word3
+ */
+#define TXD_W3_IV			FIELD32(0xffffffff)
+
+/*
+ * Word4
+ */
+#define TXD_W4_EIV			FIELD32(0xffffffff)
+
+/*
+ * Word5
+ * FRAME_OFFSET: Frame start offset inside ASIC TXFIFO (after TXINFO field).
+ * TXD_W5_PID_SUBTYPE: Driver assigned packet ID index for txdone handler.
+ * TXD_W5_PID_TYPE: Driver assigned packet ID type for txdone handler.
+ * WAITING_DMA_DONE_INT: TXD been filled with data
+ * and waiting for TxDoneISR housekeeping.
+ */
+#define TXD_W5_FRAME_OFFSET		FIELD32(0x000000ff)
+#define TXD_W5_PID_SUBTYPE		FIELD32(0x00001f00)
+#define TXD_W5_PID_TYPE			FIELD32(0x0000e000)
+#define TXD_W5_TX_POWER			FIELD32(0x00ff0000)
+#define TXD_W5_WAITING_DMA_DONE_INT	FIELD32(0x01000000)
+
+/*
+ * the above 24-byte is called TXINFO and will be DMAed to MAC block
+ * through TXFIFO. MAC block use this TXINFO to control the transmission
+ * behavior of this frame.
+ * The following fields are not used by MAC block.
+ * They are used by DMA block and HOST driver only.
+ * Once a frame has been DMA to ASIC, all the following fields are useless
+ * to ASIC.
+ */
+
+/*
+ * Word6-10: Buffer physical address
+ */
+#define TXD_W6_BUFFER_PHYSICAL_ADDRESS	FIELD32(0xffffffff)
+#define TXD_W7_BUFFER_PHYSICAL_ADDRESS	FIELD32(0xffffffff)
+#define TXD_W8_BUFFER_PHYSICAL_ADDRESS	FIELD32(0xffffffff)
+#define TXD_W9_BUFFER_PHYSICAL_ADDRESS	FIELD32(0xffffffff)
+#define TXD_W10_BUFFER_PHYSICAL_ADDRESS	FIELD32(0xffffffff)
+
+/*
+ * Word11-13: Buffer length
+ */
+#define TXD_W11_BUFFER_LENGTH0		FIELD32(0x00000fff)
+#define TXD_W11_BUFFER_LENGTH1		FIELD32(0x0fff0000)
+#define TXD_W12_BUFFER_LENGTH2		FIELD32(0x00000fff)
+#define TXD_W12_BUFFER_LENGTH3		FIELD32(0x0fff0000)
+#define TXD_W13_BUFFER_LENGTH4		FIELD32(0x00000fff)
+
+/*
+ * Word14
+ */
+#define TXD_W14_SK_BUFFER		FIELD32(0xffffffff)
+
+/*
+ * Word15
+ */
+#define TXD_W15_NEXT_SK_BUFFER		FIELD32(0xffffffff)
+
+/*
+ * RX descriptor format for RX Ring.
+ */
+
+/*
+ * Word0
+ * CIPHER_ERROR: 1:ICV error, 2:MIC error, 3:invalid key.
+ * KEY_INDEX: Decryption key actually used.
+ */
+#define RXD_W0_OWNER_NIC		FIELD32(0x00000001)
+#define RXD_W0_DROP			FIELD32(0x00000002)
+#define RXD_W0_UNICAST_TO_ME		FIELD32(0x00000004)
+#define RXD_W0_MULTICAST		FIELD32(0x00000008)
+#define RXD_W0_BROADCAST		FIELD32(0x00000010)
+#define RXD_W0_MY_BSS			FIELD32(0x00000020)
+#define RXD_W0_CRC_ERROR		FIELD32(0x00000040)
+#define RXD_W0_OFDM			FIELD32(0x00000080)
+#define RXD_W0_CIPHER_ERROR		FIELD32(0x00000300)
+#define RXD_W0_KEY_INDEX		FIELD32(0x0000fc00)
+#define RXD_W0_DATABYTE_COUNT		FIELD32(0x0fff0000)
+#define RXD_W0_CIPHER_ALG		FIELD32(0xe0000000)
+
+/*
+ * Word1
+ * SIGNAL: RX raw data rate reported by BBP.
+ */
+#define RXD_W1_SIGNAL			FIELD32(0x000000ff)
+#define RXD_W1_RSSI_AGC			FIELD32(0x00001f00)
+#define RXD_W1_RSSI_LNA			FIELD32(0x00006000)
+#define RXD_W1_FRAME_OFFSET		FIELD32(0x7f000000)
+
+/*
+ * Word2
+ * IV: Received IV of originally encrypted.
+ */
+#define RXD_W2_IV			FIELD32(0xffffffff)
+
+/*
+ * Word3
+ * EIV: Received EIV of originally encrypted.
+ */
+#define RXD_W3_EIV			FIELD32(0xffffffff)
+
+/*
+ * Word4
+ */
+#define RXD_W4_RESERVED			FIELD32(0xffffffff)
+
+/*
+ * the above 20-byte is called RXINFO and will be DMAed to MAC RX block
+ * and passed to the HOST driver.
+ * The following fields are for DMA block and HOST usage only.
+ * Can't be touched by ASIC MAC block.
+ */
+
+/*
+ * Word5
+ */
+#define RXD_W5_BUFFER_PHYSICAL_ADDRESS	FIELD32(0xffffffff)
+
+/*
+ * Word6-15: Reserved
+ */
+#define RXD_W6_RESERVED			FIELD32(0xffffffff)
+#define RXD_W7_RESERVED			FIELD32(0xffffffff)
+#define RXD_W8_RESERVED			FIELD32(0xffffffff)
+#define RXD_W9_RESERVED			FIELD32(0xffffffff)
+#define RXD_W10_RESERVED		FIELD32(0xffffffff)
+#define RXD_W11_RESERVED		FIELD32(0xffffffff)
+#define RXD_W12_RESERVED		FIELD32(0xffffffff)
+#define RXD_W13_RESERVED		FIELD32(0xffffffff)
+#define RXD_W14_RESERVED		FIELD32(0xffffffff)
+#define RXD_W15_RESERVED		FIELD32(0xffffffff)
+
+/*
+ * Macro's for converting txpower from EEPROM to dscape value
+ * and from dscape value to register value.
+ */
+#define MIN_TXPOWER	0
+#define MAX_TXPOWER	31
+#define DEFAULT_TXPOWER	24
+
+#define TXPOWER_FROM_DEV(__txpower)		\
+({						\
+	((__txpower) > MAX_TXPOWER) ?		\
+		DEFAULT_TXPOWER : (__txpower);	\
+})
+
+#define TXPOWER_TO_DEV(__txpower)			\
+({							\
+	((__txpower) <= MIN_TXPOWER) ? MIN_TXPOWER :	\
+	(((__txpower) >= MAX_TXPOWER) ? MAX_TXPOWER :	\
+	(__txpower));					\
+})
+
+#endif /* RT61PCI_H */
diff --git a/drivers/net/wireless/rt2x00/rt73usb.c b/drivers/net/wireless/rt2x00/rt73usb.c
new file mode 100644
index 000000000000..3e42759473c3
--- /dev/null
+++ b/drivers/net/wireless/rt2x00/rt73usb.c
@@ -0,0 +1,2110 @@
+/*
+	Copyright (C) 2004 - 2007 rt2x00 SourceForge Project
+	<http://rt2x00.serialmonkey.com>
+
+	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, or
+	(at your option) any later version.
+
+	This program is distributed in the hope that it will be useful,
+	but WITHOUT ANY WARRANTY; without even the implied warranty of
+	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+	GNU General Public License for more details.
+
+	You should have received a copy of the GNU General Public License
+	along with this program; if not, write to the
+	Free Software Foundation, Inc.,
+	59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+/*
+	Module: rt73usb
+	Abstract: rt73usb device specific routines.
+	Supported chipsets: rt2571W & rt2671.
+ */
+
+/*
+ * Set enviroment defines for rt2x00.h
+ */
+#define DRV_NAME "rt73usb"
+
+#include <linux/delay.h>
+#include <linux/etherdevice.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/usb.h>
+
+#include "rt2x00.h"
+#include "rt2x00usb.h"
+#include "rt73usb.h"
+
+/*
+ * Register access.
+ * All access to the CSR registers will go through the methods
+ * rt73usb_register_read and rt73usb_register_write.
+ * BBP and RF register require indirect register access,
+ * and use the CSR registers BBPCSR and RFCSR to achieve this.
+ * These indirect registers work with busy bits,
+ * and we will try maximal REGISTER_BUSY_COUNT times to access
+ * the register while taking a REGISTER_BUSY_DELAY us delay
+ * between each attampt. When the busy bit is still set at that time,
+ * the access attempt is considered to have failed,
+ * and we will print an error.
+ */
+static inline void rt73usb_register_read(const struct rt2x00_dev *rt2x00dev,
+					 const unsigned int offset, u32 *value)
+{
+	__le32 reg;
+	rt2x00usb_vendor_request_buff(rt2x00dev, USB_MULTI_READ,
+				      USB_VENDOR_REQUEST_IN, offset,
+				      &reg, sizeof(u32), REGISTER_TIMEOUT);
+	*value = le32_to_cpu(reg);
+}
+
+static inline void rt73usb_register_multiread(const struct rt2x00_dev
+					      *rt2x00dev,
+					      const unsigned int offset,
+					      void *value, const u32 length)
+{
+	int timeout = REGISTER_TIMEOUT * (length / sizeof(u32));
+	rt2x00usb_vendor_request_buff(rt2x00dev, USB_MULTI_READ,
+				      USB_VENDOR_REQUEST_IN, offset,
+				      value, length, timeout);
+}
+
+static inline void rt73usb_register_write(const struct rt2x00_dev *rt2x00dev,
+					  const unsigned int offset, u32 value)
+{
+	__le32 reg = cpu_to_le32(value);
+	rt2x00usb_vendor_request_buff(rt2x00dev, USB_MULTI_WRITE,
+				      USB_VENDOR_REQUEST_OUT, offset,
+				      &reg, sizeof(u32), REGISTER_TIMEOUT);
+}
+
+static inline void rt73usb_register_multiwrite(const struct rt2x00_dev
+					       *rt2x00dev,
+					       const unsigned int offset,
+					       void *value, const u32 length)
+{
+	int timeout = REGISTER_TIMEOUT * (length / sizeof(u32));
+	rt2x00usb_vendor_request_buff(rt2x00dev, USB_MULTI_WRITE,
+				      USB_VENDOR_REQUEST_OUT, offset,
+				      value, length, timeout);
+}
+
+static u32 rt73usb_bbp_check(const struct rt2x00_dev *rt2x00dev)
+{
+	u32 reg;
+	unsigned int i;
+
+	for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
+		rt73usb_register_read(rt2x00dev, PHY_CSR3, &reg);
+		if (!rt2x00_get_field32(reg, PHY_CSR3_BUSY))
+			break;
+		udelay(REGISTER_BUSY_DELAY);
+	}
+
+	return reg;
+}
+
+static void rt73usb_bbp_write(const struct rt2x00_dev *rt2x00dev,
+			      const unsigned int word, const u8 value)
+{
+	u32 reg;
+
+	/*
+	 * Wait until the BBP becomes ready.
+	 */
+	reg = rt73usb_bbp_check(rt2x00dev);
+	if (rt2x00_get_field32(reg, PHY_CSR3_BUSY)) {
+		ERROR(rt2x00dev, "PHY_CSR3 register busy. Write failed.\n");
+		return;
+	}
+
+	/*
+	 * Write the data into the BBP.
+	 */
+	reg = 0;
+	rt2x00_set_field32(&reg, PHY_CSR3_VALUE, value);
+	rt2x00_set_field32(&reg, PHY_CSR3_REGNUM, word);
+	rt2x00_set_field32(&reg, PHY_CSR3_BUSY, 1);
+	rt2x00_set_field32(&reg, PHY_CSR3_READ_CONTROL, 0);
+
+	rt73usb_register_write(rt2x00dev, PHY_CSR3, reg);
+}
+
+static void rt73usb_bbp_read(const struct rt2x00_dev *rt2x00dev,
+			     const unsigned int word, u8 *value)
+{
+	u32 reg;
+
+	/*
+	 * Wait until the BBP becomes ready.
+	 */
+	reg = rt73usb_bbp_check(rt2x00dev);
+	if (rt2x00_get_field32(reg, PHY_CSR3_BUSY)) {
+		ERROR(rt2x00dev, "PHY_CSR3 register busy. Read failed.\n");
+		return;
+	}
+
+	/*
+	 * Write the request into the BBP.
+	 */
+	reg = 0;
+	rt2x00_set_field32(&reg, PHY_CSR3_REGNUM, word);
+	rt2x00_set_field32(&reg, PHY_CSR3_BUSY, 1);
+	rt2x00_set_field32(&reg, PHY_CSR3_READ_CONTROL, 1);
+
+	rt73usb_register_write(rt2x00dev, PHY_CSR3, reg);
+
+	/*
+	 * Wait until the BBP becomes ready.
+	 */
+	reg = rt73usb_bbp_check(rt2x00dev);
+	if (rt2x00_get_field32(reg, PHY_CSR3_BUSY)) {
+		ERROR(rt2x00dev, "PHY_CSR3 register busy. Read failed.\n");
+		*value = 0xff;
+		return;
+	}
+
+	*value = rt2x00_get_field32(reg, PHY_CSR3_VALUE);
+}
+
+static void rt73usb_rf_write(const struct rt2x00_dev *rt2x00dev,
+			     const unsigned int word, const u32 value)
+{
+	u32 reg;
+	unsigned int i;
+
+	if (!word)
+		return;
+
+	for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
+		rt73usb_register_read(rt2x00dev, PHY_CSR4, &reg);
+		if (!rt2x00_get_field32(reg, PHY_CSR4_BUSY))
+			goto rf_write;
+		udelay(REGISTER_BUSY_DELAY);
+	}
+
+	ERROR(rt2x00dev, "PHY_CSR4 register busy. Write failed.\n");
+	return;
+
+rf_write:
+	reg = 0;
+	rt2x00_set_field32(&reg, PHY_CSR4_VALUE, value);
+
+	/*
+	 * RF5225 and RF2527 contain 21 bits per RF register value,
+	 * all others contain 20 bits.
+	 */
+	rt2x00_set_field32(&reg, PHY_CSR4_NUMBER_OF_BITS,
+			   20 + !!(rt2x00_rf(&rt2x00dev->chip, RF5225) ||
+				   rt2x00_rf(&rt2x00dev->chip, RF2527)));
+	rt2x00_set_field32(&reg, PHY_CSR4_IF_SELECT, 0);
+	rt2x00_set_field32(&reg, PHY_CSR4_BUSY, 1);
+
+	rt73usb_register_write(rt2x00dev, PHY_CSR4, reg);
+	rt2x00_rf_write(rt2x00dev, word, value);
+}
+
+#ifdef CONFIG_RT2X00_LIB_DEBUGFS
+#define CSR_OFFSET(__word)	( CSR_REG_BASE + ((__word) * sizeof(u32)) )
+
+static void rt73usb_read_csr(const struct rt2x00_dev *rt2x00dev,
+			     const unsigned int word, u32 *data)
+{
+	rt73usb_register_read(rt2x00dev, CSR_OFFSET(word), data);
+}
+
+static void rt73usb_write_csr(const struct rt2x00_dev *rt2x00dev,
+			      const unsigned int word, u32 data)
+{
+	rt73usb_register_write(rt2x00dev, CSR_OFFSET(word), data);
+}
+
+static const struct rt2x00debug rt73usb_rt2x00debug = {
+	.owner	= THIS_MODULE,
+	.csr	= {
+		.read		= rt73usb_read_csr,
+		.write		= rt73usb_write_csr,
+		.word_size	= sizeof(u32),
+		.word_count	= CSR_REG_SIZE / sizeof(u32),
+	},
+	.eeprom	= {
+		.read		= rt2x00_eeprom_read,
+		.write		= rt2x00_eeprom_write,
+		.word_size	= sizeof(u16),
+		.word_count	= EEPROM_SIZE / sizeof(u16),
+	},
+	.bbp	= {
+		.read		= rt73usb_bbp_read,
+		.write		= rt73usb_bbp_write,
+		.word_size	= sizeof(u8),
+		.word_count	= BBP_SIZE / sizeof(u8),
+	},
+	.rf	= {
+		.read		= rt2x00_rf_read,
+		.write		= rt73usb_rf_write,
+		.word_size	= sizeof(u32),
+		.word_count	= RF_SIZE / sizeof(u32),
+	},
+};
+#endif /* CONFIG_RT2X00_LIB_DEBUGFS */
+
+/*
+ * Configuration handlers.
+ */
+static void rt73usb_config_mac_addr(struct rt2x00_dev *rt2x00dev, __le32 *mac)
+{
+	u32 tmp;
+
+	tmp = le32_to_cpu(mac[1]);
+	rt2x00_set_field32(&tmp, MAC_CSR3_UNICAST_TO_ME_MASK, 0xff);
+	mac[1] = cpu_to_le32(tmp);
+
+	rt73usb_register_multiwrite(rt2x00dev, MAC_CSR2, mac,
+				    (2 * sizeof(__le32)));
+}
+
+static void rt73usb_config_bssid(struct rt2x00_dev *rt2x00dev, __le32 *bssid)
+{
+	u32 tmp;
+
+	tmp = le32_to_cpu(bssid[1]);
+	rt2x00_set_field32(&tmp, MAC_CSR5_BSS_ID_MASK, 3);
+	bssid[1] = cpu_to_le32(tmp);
+
+	rt73usb_register_multiwrite(rt2x00dev, MAC_CSR4, bssid,
+				    (2 * sizeof(__le32)));
+}
+
+static void rt73usb_config_type(struct rt2x00_dev *rt2x00dev, const int type,
+				const int tsf_sync)
+{
+	u32 reg;
+
+	/*
+	 * Clear current synchronisation setup.
+	 * For the Beacon base registers we only need to clear
+	 * the first byte since that byte contains the VALID and OWNER
+	 * bits which (when set to 0) will invalidate the entire beacon.
+	 */
+	rt73usb_register_write(rt2x00dev, TXRX_CSR9, 0);
+	rt73usb_register_write(rt2x00dev, HW_BEACON_BASE0, 0);
+	rt73usb_register_write(rt2x00dev, HW_BEACON_BASE1, 0);
+	rt73usb_register_write(rt2x00dev, HW_BEACON_BASE2, 0);
+	rt73usb_register_write(rt2x00dev, HW_BEACON_BASE3, 0);
+
+	/*
+	 * Enable synchronisation.
+	 */
+	rt73usb_register_read(rt2x00dev, TXRX_CSR9, &reg);
+	rt2x00_set_field32(&reg, TXRX_CSR9_TSF_TICKING, 1);
+	rt2x00_set_field32(&reg, TXRX_CSR9_TBTT_ENABLE, 1);
+	rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_GEN, 0);
+	rt2x00_set_field32(&reg, TXRX_CSR9_TSF_SYNC, tsf_sync);
+	rt73usb_register_write(rt2x00dev, TXRX_CSR9, reg);
+}
+
+static void rt73usb_config_preamble(struct rt2x00_dev *rt2x00dev,
+				      const int short_preamble,
+				      const int ack_timeout,
+				      const int ack_consume_time)
+{
+	u32 reg;
+
+	/*
+	 * When in atomic context, reschedule and let rt2x00lib
+	 * call this function again.
+	 */
+	if (in_atomic()) {
+		queue_work(rt2x00dev->hw->workqueue, &rt2x00dev->config_work);
+		return;
+	}
+
+	rt73usb_register_read(rt2x00dev, TXRX_CSR0, &reg);
+	rt2x00_set_field32(&reg, TXRX_CSR0_RX_ACK_TIMEOUT, ack_timeout);
+	rt73usb_register_write(rt2x00dev, TXRX_CSR0, reg);
+
+	rt73usb_register_read(rt2x00dev, TXRX_CSR4, &reg);
+	rt2x00_set_field32(&reg, TXRX_CSR4_AUTORESPOND_PREAMBLE,
+			   !!short_preamble);
+	rt73usb_register_write(rt2x00dev, TXRX_CSR4, reg);
+}
+
+static void rt73usb_config_phymode(struct rt2x00_dev *rt2x00dev,
+				   const int basic_rate_mask)
+{
+	rt73usb_register_write(rt2x00dev, TXRX_CSR5, basic_rate_mask);
+}
+
+static void rt73usb_config_channel(struct rt2x00_dev *rt2x00dev,
+				   struct rf_channel *rf, const int txpower)
+{
+	u8 r3;
+	u8 r94;
+	u8 smart;
+
+	rt2x00_set_field32(&rf->rf3, RF3_TXPOWER, TXPOWER_TO_DEV(txpower));
+	rt2x00_set_field32(&rf->rf4, RF4_FREQ_OFFSET, rt2x00dev->freq_offset);
+
+	smart = !(rt2x00_rf(&rt2x00dev->chip, RF5225) ||
+		  rt2x00_rf(&rt2x00dev->chip, RF2527));
+
+	rt73usb_bbp_read(rt2x00dev, 3, &r3);
+	rt2x00_set_field8(&r3, BBP_R3_SMART_MODE, smart);
+	rt73usb_bbp_write(rt2x00dev, 3, r3);
+
+	r94 = 6;
+	if (txpower > MAX_TXPOWER && txpower <= (MAX_TXPOWER + r94))
+		r94 += txpower - MAX_TXPOWER;
+	else if (txpower < MIN_TXPOWER && txpower >= (MIN_TXPOWER - r94))
+		r94 += txpower;
+	rt73usb_bbp_write(rt2x00dev, 94, r94);
+
+	rt73usb_rf_write(rt2x00dev, 1, rf->rf1);
+	rt73usb_rf_write(rt2x00dev, 2, rf->rf2);
+	rt73usb_rf_write(rt2x00dev, 3, rf->rf3 & ~0x00000004);
+	rt73usb_rf_write(rt2x00dev, 4, rf->rf4);
+
+	rt73usb_rf_write(rt2x00dev, 1, rf->rf1);
+	rt73usb_rf_write(rt2x00dev, 2, rf->rf2);
+	rt73usb_rf_write(rt2x00dev, 3, rf->rf3 | 0x00000004);
+	rt73usb_rf_write(rt2x00dev, 4, rf->rf4);
+
+	rt73usb_rf_write(rt2x00dev, 1, rf->rf1);
+	rt73usb_rf_write(rt2x00dev, 2, rf->rf2);
+	rt73usb_rf_write(rt2x00dev, 3, rf->rf3 & ~0x00000004);
+	rt73usb_rf_write(rt2x00dev, 4, rf->rf4);
+
+	udelay(10);
+}
+
+static void rt73usb_config_txpower(struct rt2x00_dev *rt2x00dev,
+				   const int txpower)
+{
+	struct rf_channel rf;
+
+	rt2x00_rf_read(rt2x00dev, 1, &rf.rf1);
+	rt2x00_rf_read(rt2x00dev, 2, &rf.rf2);
+	rt2x00_rf_read(rt2x00dev, 3, &rf.rf3);
+	rt2x00_rf_read(rt2x00dev, 4, &rf.rf4);
+
+	rt73usb_config_channel(rt2x00dev, &rf, txpower);
+}
+
+static void rt73usb_config_antenna_5x(struct rt2x00_dev *rt2x00dev,
+				      const int antenna_tx,
+				      const int antenna_rx)
+{
+	u8 r3;
+	u8 r4;
+	u8 r77;
+
+	rt73usb_bbp_read(rt2x00dev, 3, &r3);
+	rt73usb_bbp_read(rt2x00dev, 4, &r4);
+	rt73usb_bbp_read(rt2x00dev, 77, &r77);
+
+	rt2x00_set_field8(&r3, BBP_R3_SMART_MODE, 0);
+
+	switch (antenna_rx) {
+	case ANTENNA_SW_DIVERSITY:
+	case ANTENNA_HW_DIVERSITY:
+		rt2x00_set_field8(&r4, BBP_R4_RX_ANTENNA, 2);
+		rt2x00_set_field8(&r4, BBP_R4_RX_FRAME_END,
+				  !!(rt2x00dev->curr_hwmode != HWMODE_A));
+		break;
+	case ANTENNA_A:
+		rt2x00_set_field8(&r4, BBP_R4_RX_ANTENNA, 1);
+		rt2x00_set_field8(&r4, BBP_R4_RX_FRAME_END, 0);
+
+		if (rt2x00dev->curr_hwmode == HWMODE_A)
+			rt2x00_set_field8(&r77, BBP_R77_PAIR, 0);
+		else
+			rt2x00_set_field8(&r77, BBP_R77_PAIR, 3);
+		break;
+	case ANTENNA_B:
+		rt2x00_set_field8(&r4, BBP_R4_RX_ANTENNA, 1);
+		rt2x00_set_field8(&r4, BBP_R4_RX_FRAME_END, 0);
+
+		if (rt2x00dev->curr_hwmode == HWMODE_A)
+			rt2x00_set_field8(&r77, BBP_R77_PAIR, 3);
+		else
+			rt2x00_set_field8(&r77, BBP_R77_PAIR, 0);
+		break;
+	}
+
+	rt73usb_bbp_write(rt2x00dev, 77, r77);
+	rt73usb_bbp_write(rt2x00dev, 3, r3);
+	rt73usb_bbp_write(rt2x00dev, 4, r4);
+}
+
+static void rt73usb_config_antenna_2x(struct rt2x00_dev *rt2x00dev,
+				      const int antenna_tx,
+				      const int antenna_rx)
+{
+	u8 r3;
+	u8 r4;
+	u8 r77;
+
+	rt73usb_bbp_read(rt2x00dev, 3, &r3);
+	rt73usb_bbp_read(rt2x00dev, 4, &r4);
+	rt73usb_bbp_read(rt2x00dev, 77, &r77);
+
+	rt2x00_set_field8(&r3, BBP_R3_SMART_MODE, 0);
+	rt2x00_set_field8(&r4, BBP_R4_RX_FRAME_END,
+			  !test_bit(CONFIG_FRAME_TYPE, &rt2x00dev->flags));
+
+	switch (antenna_rx) {
+	case ANTENNA_SW_DIVERSITY:
+	case ANTENNA_HW_DIVERSITY:
+		rt2x00_set_field8(&r4, BBP_R4_RX_ANTENNA, 2);
+		break;
+	case ANTENNA_A:
+		rt2x00_set_field8(&r4, BBP_R4_RX_ANTENNA, 1);
+		rt2x00_set_field8(&r77, BBP_R77_PAIR, 3);
+		break;
+	case ANTENNA_B:
+		rt2x00_set_field8(&r4, BBP_R4_RX_ANTENNA, 1);
+		rt2x00_set_field8(&r77, BBP_R77_PAIR, 0);
+		break;
+	}
+
+	rt73usb_bbp_write(rt2x00dev, 77, r77);
+	rt73usb_bbp_write(rt2x00dev, 3, r3);
+	rt73usb_bbp_write(rt2x00dev, 4, r4);
+}
+
+struct antenna_sel {
+	u8 word;
+	/*
+	 * value[0] -> non-LNA
+	 * value[1] -> LNA
+	 */
+	u8 value[2];
+};
+
+static const struct antenna_sel antenna_sel_a[] = {
+	{ 96,  { 0x58, 0x78 } },
+	{ 104, { 0x38, 0x48 } },
+	{ 75,  { 0xfe, 0x80 } },
+	{ 86,  { 0xfe, 0x80 } },
+	{ 88,  { 0xfe, 0x80 } },
+	{ 35,  { 0x60, 0x60 } },
+	{ 97,  { 0x58, 0x58 } },
+	{ 98,  { 0x58, 0x58 } },
+};
+
+static const struct antenna_sel antenna_sel_bg[] = {
+	{ 96,  { 0x48, 0x68 } },
+	{ 104, { 0x2c, 0x3c } },
+	{ 75,  { 0xfe, 0x80 } },
+	{ 86,  { 0xfe, 0x80 } },
+	{ 88,  { 0xfe, 0x80 } },
+	{ 35,  { 0x50, 0x50 } },
+	{ 97,  { 0x48, 0x48 } },
+	{ 98,  { 0x48, 0x48 } },
+};
+
+static void rt73usb_config_antenna(struct rt2x00_dev *rt2x00dev,
+				   const int antenna_tx, const int antenna_rx)
+{
+	const struct antenna_sel *sel;
+	unsigned int lna;
+	unsigned int i;
+	u32 reg;
+
+	rt73usb_register_read(rt2x00dev, PHY_CSR0, &reg);
+
+	if (rt2x00dev->curr_hwmode == HWMODE_A) {
+		sel = antenna_sel_a;
+		lna = test_bit(CONFIG_EXTERNAL_LNA_A, &rt2x00dev->flags);
+
+		rt2x00_set_field32(&reg, PHY_CSR0_PA_PE_BG, 0);
+		rt2x00_set_field32(&reg, PHY_CSR0_PA_PE_A, 1);
+	} else {
+		sel = antenna_sel_bg;
+		lna = test_bit(CONFIG_EXTERNAL_LNA_BG, &rt2x00dev->flags);
+
+		rt2x00_set_field32(&reg, PHY_CSR0_PA_PE_BG, 1);
+		rt2x00_set_field32(&reg, PHY_CSR0_PA_PE_A, 0);
+	}
+
+	for (i = 0; i < ARRAY_SIZE(antenna_sel_a); i++)
+		rt73usb_bbp_write(rt2x00dev, sel[i].word, sel[i].value[lna]);
+
+	rt73usb_register_write(rt2x00dev, PHY_CSR0, reg);
+
+	if (rt2x00_rf(&rt2x00dev->chip, RF5226) ||
+	    rt2x00_rf(&rt2x00dev->chip, RF5225))
+		rt73usb_config_antenna_5x(rt2x00dev, antenna_tx, antenna_rx);
+	else if (rt2x00_rf(&rt2x00dev->chip, RF2528) ||
+		 rt2x00_rf(&rt2x00dev->chip, RF2527))
+		rt73usb_config_antenna_2x(rt2x00dev, antenna_tx, antenna_rx);
+}
+
+static void rt73usb_config_duration(struct rt2x00_dev *rt2x00dev,
+				    struct rt2x00lib_conf *libconf)
+{
+	u32 reg;
+
+	rt73usb_register_read(rt2x00dev, MAC_CSR9, &reg);
+	rt2x00_set_field32(&reg, MAC_CSR9_SLOT_TIME, libconf->slot_time);
+	rt73usb_register_write(rt2x00dev, MAC_CSR9, reg);
+
+	rt73usb_register_read(rt2x00dev, MAC_CSR8, &reg);
+	rt2x00_set_field32(&reg, MAC_CSR8_SIFS, libconf->sifs);
+	rt2x00_set_field32(&reg, MAC_CSR8_SIFS_AFTER_RX_OFDM, 3);
+	rt2x00_set_field32(&reg, MAC_CSR8_EIFS, libconf->eifs);
+	rt73usb_register_write(rt2x00dev, MAC_CSR8, reg);
+
+	rt73usb_register_read(rt2x00dev, TXRX_CSR0, &reg);
+	rt2x00_set_field32(&reg, TXRX_CSR0_TSF_OFFSET, IEEE80211_HEADER);
+	rt73usb_register_write(rt2x00dev, TXRX_CSR0, reg);
+
+	rt73usb_register_read(rt2x00dev, TXRX_CSR4, &reg);
+	rt2x00_set_field32(&reg, TXRX_CSR4_AUTORESPOND_ENABLE, 1);
+	rt73usb_register_write(rt2x00dev, TXRX_CSR4, reg);
+
+	rt73usb_register_read(rt2x00dev, TXRX_CSR9, &reg);
+	rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_INTERVAL,
+			   libconf->conf->beacon_int * 16);
+	rt73usb_register_write(rt2x00dev, TXRX_CSR9, reg);
+}
+
+static void rt73usb_config(struct rt2x00_dev *rt2x00dev,
+			   const unsigned int flags,
+			   struct rt2x00lib_conf *libconf)
+{
+	if (flags & CONFIG_UPDATE_PHYMODE)
+		rt73usb_config_phymode(rt2x00dev, libconf->basic_rates);
+	if (flags & CONFIG_UPDATE_CHANNEL)
+		rt73usb_config_channel(rt2x00dev, &libconf->rf,
+				       libconf->conf->power_level);
+	if ((flags & CONFIG_UPDATE_TXPOWER) && !(flags & CONFIG_UPDATE_CHANNEL))
+		rt73usb_config_txpower(rt2x00dev, libconf->conf->power_level);
+	if (flags & CONFIG_UPDATE_ANTENNA)
+		rt73usb_config_antenna(rt2x00dev, libconf->conf->antenna_sel_tx,
+				       libconf->conf->antenna_sel_rx);
+	if (flags & (CONFIG_UPDATE_SLOT_TIME | CONFIG_UPDATE_BEACON_INT))
+		rt73usb_config_duration(rt2x00dev, libconf);
+}
+
+/*
+ * LED functions.
+ */
+static void rt73usb_enable_led(struct rt2x00_dev *rt2x00dev)
+{
+	u32 reg;
+
+	rt73usb_register_read(rt2x00dev, MAC_CSR14, &reg);
+	rt2x00_set_field32(&reg, MAC_CSR14_ON_PERIOD, 70);
+	rt2x00_set_field32(&reg, MAC_CSR14_OFF_PERIOD, 30);
+	rt73usb_register_write(rt2x00dev, MAC_CSR14, reg);
+
+	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_RADIO_STATUS, 1);
+	if (rt2x00dev->rx_status.phymode == MODE_IEEE80211A)
+		rt2x00_set_field16(&rt2x00dev->led_reg,
+				   MCU_LEDCS_LINK_A_STATUS, 1);
+	else
+		rt2x00_set_field16(&rt2x00dev->led_reg,
+				   MCU_LEDCS_LINK_BG_STATUS, 1);
+
+	rt2x00usb_vendor_request_sw(rt2x00dev, USB_LED_CONTROL, 0x0000,
+				    rt2x00dev->led_reg, REGISTER_TIMEOUT);
+}
+
+static void rt73usb_disable_led(struct rt2x00_dev *rt2x00dev)
+{
+	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_RADIO_STATUS, 0);
+	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_LINK_BG_STATUS, 0);
+	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_LINK_A_STATUS, 0);
+
+	rt2x00usb_vendor_request_sw(rt2x00dev, USB_LED_CONTROL, 0x0000,
+				    rt2x00dev->led_reg, REGISTER_TIMEOUT);
+}
+
+static void rt73usb_activity_led(struct rt2x00_dev *rt2x00dev, int rssi)
+{
+	u32 led;
+
+	if (rt2x00dev->led_mode != LED_MODE_SIGNAL_STRENGTH)
+		return;
+
+	/*
+	 * Led handling requires a positive value for the rssi,
+	 * to do that correctly we need to add the correction.
+	 */
+	rssi += rt2x00dev->rssi_offset;
+
+	if (rssi <= 30)
+		led = 0;
+	else if (rssi <= 39)
+		led = 1;
+	else if (rssi <= 49)
+		led = 2;
+	else if (rssi <= 53)
+		led = 3;
+	else if (rssi <= 63)
+		led = 4;
+	else
+		led = 5;
+
+	rt2x00usb_vendor_request_sw(rt2x00dev, USB_LED_CONTROL, led,
+				    rt2x00dev->led_reg, REGISTER_TIMEOUT);
+}
+
+/*
+ * Link tuning
+ */
+static void rt73usb_link_stats(struct rt2x00_dev *rt2x00dev)
+{
+	u32 reg;
+
+	/*
+	 * Update FCS error count from register.
+	 */
+	rt73usb_register_read(rt2x00dev, STA_CSR0, &reg);
+	rt2x00dev->link.rx_failed = rt2x00_get_field32(reg, STA_CSR0_FCS_ERROR);
+
+	/*
+	 * Update False CCA count from register.
+	 */
+	rt73usb_register_read(rt2x00dev, STA_CSR1, &reg);
+	reg = rt2x00_get_field32(reg, STA_CSR1_FALSE_CCA_ERROR);
+	rt2x00dev->link.false_cca =
+	    rt2x00_get_field32(reg, STA_CSR1_FALSE_CCA_ERROR);
+}
+
+static void rt73usb_reset_tuner(struct rt2x00_dev *rt2x00dev)
+{
+	rt73usb_bbp_write(rt2x00dev, 17, 0x20);
+	rt2x00dev->link.vgc_level = 0x20;
+}
+
+static void rt73usb_link_tuner(struct rt2x00_dev *rt2x00dev)
+{
+	int rssi = rt2x00_get_link_rssi(&rt2x00dev->link);
+	u8 r17;
+	u8 up_bound;
+	u8 low_bound;
+
+	/*
+	 * Update Led strength
+	 */
+	rt73usb_activity_led(rt2x00dev, rssi);
+
+	rt73usb_bbp_read(rt2x00dev, 17, &r17);
+
+	/*
+	 * Determine r17 bounds.
+	 */
+	if (rt2x00dev->rx_status.phymode == MODE_IEEE80211A) {
+		low_bound = 0x28;
+		up_bound = 0x48;
+
+		if (test_bit(CONFIG_EXTERNAL_LNA_A, &rt2x00dev->flags)) {
+			low_bound += 0x10;
+			up_bound += 0x10;
+		}
+	} else {
+		if (rssi > -82) {
+			low_bound = 0x1c;
+			up_bound = 0x40;
+		} else if (rssi > -84) {
+			low_bound = 0x1c;
+			up_bound = 0x20;
+		} else {
+			low_bound = 0x1c;
+			up_bound = 0x1c;
+		}
+
+		if (test_bit(CONFIG_EXTERNAL_LNA_BG, &rt2x00dev->flags)) {
+			low_bound += 0x14;
+			up_bound += 0x10;
+		}
+	}
+
+	/*
+	 * Special big-R17 for very short distance
+	 */
+	if (rssi > -35) {
+		if (r17 != 0x60)
+			rt73usb_bbp_write(rt2x00dev, 17, 0x60);
+		return;
+	}
+
+	/*
+	 * Special big-R17 for short distance
+	 */
+	if (rssi >= -58) {
+		if (r17 != up_bound)
+			rt73usb_bbp_write(rt2x00dev, 17, up_bound);
+		return;
+	}
+
+	/*
+	 * Special big-R17 for middle-short distance
+	 */
+	if (rssi >= -66) {
+		low_bound += 0x10;
+		if (r17 != low_bound)
+			rt73usb_bbp_write(rt2x00dev, 17, low_bound);
+		return;
+	}
+
+	/*
+	 * Special mid-R17 for middle distance
+	 */
+	if (rssi >= -74) {
+		if (r17 != (low_bound + 0x10))
+			rt73usb_bbp_write(rt2x00dev, 17, low_bound + 0x08);
+		return;
+	}
+
+	/*
+	 * Special case: Change up_bound based on the rssi.
+	 * Lower up_bound when rssi is weaker then -74 dBm.
+	 */
+	up_bound -= 2 * (-74 - rssi);
+	if (low_bound > up_bound)
+		up_bound = low_bound;
+
+	if (r17 > up_bound) {
+		rt73usb_bbp_write(rt2x00dev, 17, up_bound);
+		return;
+	}
+
+	/*
+	 * r17 does not yet exceed upper limit, continue and base
+	 * the r17 tuning on the false CCA count.
+	 */
+	if (rt2x00dev->link.false_cca > 512 && r17 < up_bound) {
+		r17 += 4;
+		if (r17 > up_bound)
+			r17 = up_bound;
+		rt73usb_bbp_write(rt2x00dev, 17, r17);
+	} else if (rt2x00dev->link.false_cca < 100 && r17 > low_bound) {
+		r17 -= 4;
+		if (r17 < low_bound)
+			r17 = low_bound;
+		rt73usb_bbp_write(rt2x00dev, 17, r17);
+	}
+}
+
+/*
+ * Firmware name function.
+ */
+static char *rt73usb_get_firmware_name(struct rt2x00_dev *rt2x00dev)
+{
+	return FIRMWARE_RT2571;
+}
+
+/*
+ * Initialization functions.
+ */
+static int rt73usb_load_firmware(struct rt2x00_dev *rt2x00dev, void *data,
+				 const size_t len)
+{
+	unsigned int i;
+	int status;
+	u32 reg;
+	char *ptr = data;
+	char *cache;
+	int buflen;
+	int timeout;
+
+	/*
+	 * Wait for stable hardware.
+	 */
+	for (i = 0; i < 100; i++) {
+		rt73usb_register_read(rt2x00dev, MAC_CSR0, &reg);
+		if (reg)
+			break;
+		msleep(1);
+	}
+
+	if (!reg) {
+		ERROR(rt2x00dev, "Unstable hardware.\n");
+		return -EBUSY;
+	}
+
+	/*
+	 * Write firmware to device.
+	 * We setup a seperate cache for this action,
+	 * since we are going to write larger chunks of data
+	 * then normally used cache size.
+	 */
+	cache = kmalloc(CSR_CACHE_SIZE_FIRMWARE, GFP_KERNEL);
+	if (!cache) {
+		ERROR(rt2x00dev, "Failed to allocate firmware cache.\n");
+		return -ENOMEM;
+	}
+
+	for (i = 0; i < len; i += CSR_CACHE_SIZE_FIRMWARE) {
+		buflen = min_t(int, len - i, CSR_CACHE_SIZE_FIRMWARE);
+		timeout = REGISTER_TIMEOUT * (buflen / sizeof(u32));
+
+		memcpy(cache, ptr, buflen);
+
+		rt2x00usb_vendor_request(rt2x00dev, USB_MULTI_WRITE,
+					 USB_VENDOR_REQUEST_OUT,
+					 FIRMWARE_IMAGE_BASE + i, 0x0000,
+					 cache, buflen, timeout);
+
+		ptr += buflen;
+	}
+
+	kfree(cache);
+
+	/*
+	 * Send firmware request to device to load firmware,
+	 * we need to specify a long timeout time.
+	 */
+	status = rt2x00usb_vendor_request_sw(rt2x00dev, USB_DEVICE_MODE,
+					     0x0000, USB_MODE_FIRMWARE,
+					     REGISTER_TIMEOUT_FIRMWARE);
+	if (status < 0) {
+		ERROR(rt2x00dev, "Failed to write Firmware to device.\n");
+		return status;
+	}
+
+	rt73usb_disable_led(rt2x00dev);
+
+	return 0;
+}
+
+static int rt73usb_init_registers(struct rt2x00_dev *rt2x00dev)
+{
+	u32 reg;
+
+	rt73usb_register_read(rt2x00dev, TXRX_CSR0, &reg);
+	rt2x00_set_field32(&reg, TXRX_CSR0_AUTO_TX_SEQ, 1);
+	rt2x00_set_field32(&reg, TXRX_CSR0_DISABLE_RX, 0);
+	rt2x00_set_field32(&reg, TXRX_CSR0_TX_WITHOUT_WAITING, 0);
+	rt73usb_register_write(rt2x00dev, TXRX_CSR0, reg);
+
+	rt73usb_register_read(rt2x00dev, TXRX_CSR1, &reg);
+	rt2x00_set_field32(&reg, TXRX_CSR1_BBP_ID0, 47); /* CCK Signal */
+	rt2x00_set_field32(&reg, TXRX_CSR1_BBP_ID0_VALID, 1);
+	rt2x00_set_field32(&reg, TXRX_CSR1_BBP_ID1, 30); /* Rssi */
+	rt2x00_set_field32(&reg, TXRX_CSR1_BBP_ID1_VALID, 1);
+	rt2x00_set_field32(&reg, TXRX_CSR1_BBP_ID2, 42); /* OFDM Rate */
+	rt2x00_set_field32(&reg, TXRX_CSR1_BBP_ID2_VALID, 1);
+	rt2x00_set_field32(&reg, TXRX_CSR1_BBP_ID3, 30); /* Rssi */
+	rt2x00_set_field32(&reg, TXRX_CSR1_BBP_ID3_VALID, 1);
+	rt73usb_register_write(rt2x00dev, TXRX_CSR1, reg);
+
+	/*
+	 * CCK TXD BBP registers
+	 */
+	rt73usb_register_read(rt2x00dev, TXRX_CSR2, &reg);
+	rt2x00_set_field32(&reg, TXRX_CSR2_BBP_ID0, 13);
+	rt2x00_set_field32(&reg, TXRX_CSR2_BBP_ID0_VALID, 1);
+	rt2x00_set_field32(&reg, TXRX_CSR2_BBP_ID1, 12);
+	rt2x00_set_field32(&reg, TXRX_CSR2_BBP_ID1_VALID, 1);
+	rt2x00_set_field32(&reg, TXRX_CSR2_BBP_ID2, 11);
+	rt2x00_set_field32(&reg, TXRX_CSR2_BBP_ID2_VALID, 1);
+	rt2x00_set_field32(&reg, TXRX_CSR2_BBP_ID3, 10);
+	rt2x00_set_field32(&reg, TXRX_CSR2_BBP_ID3_VALID, 1);
+	rt73usb_register_write(rt2x00dev, TXRX_CSR2, reg);
+
+	/*
+	 * OFDM TXD BBP registers
+	 */
+	rt73usb_register_read(rt2x00dev, TXRX_CSR3, &reg);
+	rt2x00_set_field32(&reg, TXRX_CSR3_BBP_ID0, 7);
+	rt2x00_set_field32(&reg, TXRX_CSR3_BBP_ID0_VALID, 1);
+	rt2x00_set_field32(&reg, TXRX_CSR3_BBP_ID1, 6);
+	rt2x00_set_field32(&reg, TXRX_CSR3_BBP_ID1_VALID, 1);
+	rt2x00_set_field32(&reg, TXRX_CSR3_BBP_ID2, 5);
+	rt2x00_set_field32(&reg, TXRX_CSR3_BBP_ID2_VALID, 1);
+	rt73usb_register_write(rt2x00dev, TXRX_CSR3, reg);
+
+	rt73usb_register_read(rt2x00dev, TXRX_CSR7, &reg);
+	rt2x00_set_field32(&reg, TXRX_CSR7_ACK_CTS_6MBS, 59);
+	rt2x00_set_field32(&reg, TXRX_CSR7_ACK_CTS_9MBS, 53);
+	rt2x00_set_field32(&reg, TXRX_CSR7_ACK_CTS_12MBS, 49);
+	rt2x00_set_field32(&reg, TXRX_CSR7_ACK_CTS_18MBS, 46);
+	rt73usb_register_write(rt2x00dev, TXRX_CSR7, reg);
+
+	rt73usb_register_read(rt2x00dev, TXRX_CSR8, &reg);
+	rt2x00_set_field32(&reg, TXRX_CSR8_ACK_CTS_24MBS, 44);
+	rt2x00_set_field32(&reg, TXRX_CSR8_ACK_CTS_36MBS, 42);
+	rt2x00_set_field32(&reg, TXRX_CSR8_ACK_CTS_48MBS, 42);
+	rt2x00_set_field32(&reg, TXRX_CSR8_ACK_CTS_54MBS, 42);
+	rt73usb_register_write(rt2x00dev, TXRX_CSR8, reg);
+
+	rt73usb_register_write(rt2x00dev, TXRX_CSR15, 0x0000000f);
+
+	rt73usb_register_read(rt2x00dev, MAC_CSR6, &reg);
+	rt2x00_set_field32(&reg, MAC_CSR6_MAX_FRAME_UNIT, 0xfff);
+	rt73usb_register_write(rt2x00dev, MAC_CSR6, reg);
+
+	rt73usb_register_write(rt2x00dev, MAC_CSR10, 0x00000718);
+
+	if (rt2x00dev->ops->lib->set_device_state(rt2x00dev, STATE_AWAKE))
+		return -EBUSY;
+
+	rt73usb_register_write(rt2x00dev, MAC_CSR13, 0x00007f00);
+
+	/*
+	 * Invalidate all Shared Keys (SEC_CSR0),
+	 * and clear the Shared key Cipher algorithms (SEC_CSR1 & SEC_CSR5)
+	 */
+	rt73usb_register_write(rt2x00dev, SEC_CSR0, 0x00000000);
+	rt73usb_register_write(rt2x00dev, SEC_CSR1, 0x00000000);
+	rt73usb_register_write(rt2x00dev, SEC_CSR5, 0x00000000);
+
+	reg = 0x000023b0;
+	if (rt2x00_rf(&rt2x00dev->chip, RF5225) ||
+	    rt2x00_rf(&rt2x00dev->chip, RF2527))
+		rt2x00_set_field32(&reg, PHY_CSR1_RF_RPI, 1);
+	rt73usb_register_write(rt2x00dev, PHY_CSR1, reg);
+
+	rt73usb_register_write(rt2x00dev, PHY_CSR5, 0x00040a06);
+	rt73usb_register_write(rt2x00dev, PHY_CSR6, 0x00080606);
+	rt73usb_register_write(rt2x00dev, PHY_CSR7, 0x00000408);
+
+	rt73usb_register_read(rt2x00dev, AC_TXOP_CSR0, &reg);
+	rt2x00_set_field32(&reg, AC_TXOP_CSR0_AC0_TX_OP, 0);
+	rt2x00_set_field32(&reg, AC_TXOP_CSR0_AC1_TX_OP, 0);
+	rt73usb_register_write(rt2x00dev, AC_TXOP_CSR0, reg);
+
+	rt73usb_register_read(rt2x00dev, AC_TXOP_CSR1, &reg);
+	rt2x00_set_field32(&reg, AC_TXOP_CSR1_AC2_TX_OP, 192);
+	rt2x00_set_field32(&reg, AC_TXOP_CSR1_AC3_TX_OP, 48);
+	rt73usb_register_write(rt2x00dev, AC_TXOP_CSR1, reg);
+
+	rt73usb_register_read(rt2x00dev, MAC_CSR9, &reg);
+	rt2x00_set_field32(&reg, MAC_CSR9_CW_SELECT, 0);
+	rt73usb_register_write(rt2x00dev, MAC_CSR9, reg);
+
+	/*
+	 * We must clear the error counters.
+	 * These registers are cleared on read,
+	 * so we may pass a useless variable to store the value.
+	 */
+	rt73usb_register_read(rt2x00dev, STA_CSR0, &reg);
+	rt73usb_register_read(rt2x00dev, STA_CSR1, &reg);
+	rt73usb_register_read(rt2x00dev, STA_CSR2, &reg);
+
+	/*
+	 * Reset MAC and BBP registers.
+	 */
+	rt73usb_register_read(rt2x00dev, MAC_CSR1, &reg);
+	rt2x00_set_field32(&reg, MAC_CSR1_SOFT_RESET, 1);
+	rt2x00_set_field32(&reg, MAC_CSR1_BBP_RESET, 1);
+	rt73usb_register_write(rt2x00dev, MAC_CSR1, reg);
+
+	rt73usb_register_read(rt2x00dev, MAC_CSR1, &reg);
+	rt2x00_set_field32(&reg, MAC_CSR1_SOFT_RESET, 0);
+	rt2x00_set_field32(&reg, MAC_CSR1_BBP_RESET, 0);
+	rt73usb_register_write(rt2x00dev, MAC_CSR1, reg);
+
+	rt73usb_register_read(rt2x00dev, MAC_CSR1, &reg);
+	rt2x00_set_field32(&reg, MAC_CSR1_HOST_READY, 1);
+	rt73usb_register_write(rt2x00dev, MAC_CSR1, reg);
+
+	return 0;
+}
+
+static int rt73usb_init_bbp(struct rt2x00_dev *rt2x00dev)
+{
+	unsigned int i;
+	u16 eeprom;
+	u8 reg_id;
+	u8 value;
+
+	for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
+		rt73usb_bbp_read(rt2x00dev, 0, &value);
+		if ((value != 0xff) && (value != 0x00))
+			goto continue_csr_init;
+		NOTICE(rt2x00dev, "Waiting for BBP register.\n");
+		udelay(REGISTER_BUSY_DELAY);
+	}
+
+	ERROR(rt2x00dev, "BBP register access failed, aborting.\n");
+	return -EACCES;
+
+continue_csr_init:
+	rt73usb_bbp_write(rt2x00dev, 3, 0x80);
+	rt73usb_bbp_write(rt2x00dev, 15, 0x30);
+	rt73usb_bbp_write(rt2x00dev, 21, 0xc8);
+	rt73usb_bbp_write(rt2x00dev, 22, 0x38);
+	rt73usb_bbp_write(rt2x00dev, 23, 0x06);
+	rt73usb_bbp_write(rt2x00dev, 24, 0xfe);
+	rt73usb_bbp_write(rt2x00dev, 25, 0x0a);
+	rt73usb_bbp_write(rt2x00dev, 26, 0x0d);
+	rt73usb_bbp_write(rt2x00dev, 32, 0x0b);
+	rt73usb_bbp_write(rt2x00dev, 34, 0x12);
+	rt73usb_bbp_write(rt2x00dev, 37, 0x07);
+	rt73usb_bbp_write(rt2x00dev, 39, 0xf8);
+	rt73usb_bbp_write(rt2x00dev, 41, 0x60);
+	rt73usb_bbp_write(rt2x00dev, 53, 0x10);
+	rt73usb_bbp_write(rt2x00dev, 54, 0x18);
+	rt73usb_bbp_write(rt2x00dev, 60, 0x10);
+	rt73usb_bbp_write(rt2x00dev, 61, 0x04);
+	rt73usb_bbp_write(rt2x00dev, 62, 0x04);
+	rt73usb_bbp_write(rt2x00dev, 75, 0xfe);
+	rt73usb_bbp_write(rt2x00dev, 86, 0xfe);
+	rt73usb_bbp_write(rt2x00dev, 88, 0xfe);
+	rt73usb_bbp_write(rt2x00dev, 90, 0x0f);
+	rt73usb_bbp_write(rt2x00dev, 99, 0x00);
+	rt73usb_bbp_write(rt2x00dev, 102, 0x16);
+	rt73usb_bbp_write(rt2x00dev, 107, 0x04);
+
+	DEBUG(rt2x00dev, "Start initialization from EEPROM...\n");
+	for (i = 0; i < EEPROM_BBP_SIZE; i++) {
+		rt2x00_eeprom_read(rt2x00dev, EEPROM_BBP_START + i, &eeprom);
+
+		if (eeprom != 0xffff && eeprom != 0x0000) {
+			reg_id = rt2x00_get_field16(eeprom, EEPROM_BBP_REG_ID);
+			value = rt2x00_get_field16(eeprom, EEPROM_BBP_VALUE);
+			DEBUG(rt2x00dev, "BBP: 0x%02x, value: 0x%02x.\n",
+			      reg_id, value);
+			rt73usb_bbp_write(rt2x00dev, reg_id, value);
+		}
+	}
+	DEBUG(rt2x00dev, "...End initialization from EEPROM.\n");
+
+	return 0;
+}
+
+/*
+ * Device state switch handlers.
+ */
+static void rt73usb_toggle_rx(struct rt2x00_dev *rt2x00dev,
+			      enum dev_state state)
+{
+	u32 reg;
+
+	rt73usb_register_read(rt2x00dev, TXRX_CSR0, &reg);
+	rt2x00_set_field32(&reg, TXRX_CSR0_DISABLE_RX,
+			   state == STATE_RADIO_RX_OFF);
+	rt73usb_register_write(rt2x00dev, TXRX_CSR0, reg);
+}
+
+static int rt73usb_enable_radio(struct rt2x00_dev *rt2x00dev)
+{
+	/*
+	 * Initialize all registers.
+	 */
+	if (rt73usb_init_registers(rt2x00dev) ||
+	    rt73usb_init_bbp(rt2x00dev)) {
+		ERROR(rt2x00dev, "Register initialization failed.\n");
+		return -EIO;
+	}
+
+	rt2x00usb_enable_radio(rt2x00dev);
+
+	/*
+	 * Enable LED
+	 */
+	rt73usb_enable_led(rt2x00dev);
+
+	return 0;
+}
+
+static void rt73usb_disable_radio(struct rt2x00_dev *rt2x00dev)
+{
+	/*
+	 * Disable LED
+	 */
+	rt73usb_disable_led(rt2x00dev);
+
+	rt73usb_register_write(rt2x00dev, MAC_CSR10, 0x00001818);
+
+	/*
+	 * Disable synchronisation.
+	 */
+	rt73usb_register_write(rt2x00dev, TXRX_CSR9, 0);
+
+	rt2x00usb_disable_radio(rt2x00dev);
+}
+
+static int rt73usb_set_state(struct rt2x00_dev *rt2x00dev, enum dev_state state)
+{
+	u32 reg;
+	unsigned int i;
+	char put_to_sleep;
+	char current_state;
+
+	put_to_sleep = (state != STATE_AWAKE);
+
+	rt73usb_register_read(rt2x00dev, MAC_CSR12, &reg);
+	rt2x00_set_field32(&reg, MAC_CSR12_FORCE_WAKEUP, !put_to_sleep);
+	rt2x00_set_field32(&reg, MAC_CSR12_PUT_TO_SLEEP, put_to_sleep);
+	rt73usb_register_write(rt2x00dev, MAC_CSR12, reg);
+
+	/*
+	 * Device is not guaranteed to be in the requested state yet.
+	 * We must wait until the register indicates that the
+	 * device has entered the correct state.
+	 */
+	for (i = 0; i < REGISTER_BUSY_COUNT; i++) {
+		rt73usb_register_read(rt2x00dev, MAC_CSR12, &reg);
+		current_state =
+		    rt2x00_get_field32(reg, MAC_CSR12_BBP_CURRENT_STATE);
+		if (current_state == !put_to_sleep)
+			return 0;
+		msleep(10);
+	}
+
+	NOTICE(rt2x00dev, "Device failed to enter state %d, "
+	       "current device state %d.\n", !put_to_sleep, current_state);
+
+	return -EBUSY;
+}
+
+static int rt73usb_set_device_state(struct rt2x00_dev *rt2x00dev,
+				    enum dev_state state)
+{
+	int retval = 0;
+
+	switch (state) {
+	case STATE_RADIO_ON:
+		retval = rt73usb_enable_radio(rt2x00dev);
+		break;
+	case STATE_RADIO_OFF:
+		rt73usb_disable_radio(rt2x00dev);
+		break;
+	case STATE_RADIO_RX_ON:
+	case STATE_RADIO_RX_OFF:
+		rt73usb_toggle_rx(rt2x00dev, state);
+		break;
+	case STATE_DEEP_SLEEP:
+	case STATE_SLEEP:
+	case STATE_STANDBY:
+	case STATE_AWAKE:
+		retval = rt73usb_set_state(rt2x00dev, state);
+		break;
+	default:
+		retval = -ENOTSUPP;
+		break;
+	}
+
+	return retval;
+}
+
+/*
+ * TX descriptor initialization
+ */
+static void rt73usb_write_tx_desc(struct rt2x00_dev *rt2x00dev,
+				  struct data_desc *txd,
+				  struct txdata_entry_desc *desc,
+				  struct ieee80211_hdr *ieee80211hdr,
+				  unsigned int length,
+				  struct ieee80211_tx_control *control)
+{
+	u32 word;
+
+	/*
+	 * Start writing the descriptor words.
+	 */
+	rt2x00_desc_read(txd, 1, &word);
+	rt2x00_set_field32(&word, TXD_W1_HOST_Q_ID, desc->queue);
+	rt2x00_set_field32(&word, TXD_W1_AIFSN, desc->aifs);
+	rt2x00_set_field32(&word, TXD_W1_CWMIN, desc->cw_min);
+	rt2x00_set_field32(&word, TXD_W1_CWMAX, desc->cw_max);
+	rt2x00_set_field32(&word, TXD_W1_IV_OFFSET, IEEE80211_HEADER);
+	rt2x00_set_field32(&word, TXD_W1_HW_SEQUENCE, 1);
+	rt2x00_desc_write(txd, 1, word);
+
+	rt2x00_desc_read(txd, 2, &word);
+	rt2x00_set_field32(&word, TXD_W2_PLCP_SIGNAL, desc->signal);
+	rt2x00_set_field32(&word, TXD_W2_PLCP_SERVICE, desc->service);
+	rt2x00_set_field32(&word, TXD_W2_PLCP_LENGTH_LOW, desc->length_low);
+	rt2x00_set_field32(&word, TXD_W2_PLCP_LENGTH_HIGH, desc->length_high);
+	rt2x00_desc_write(txd, 2, word);
+
+	rt2x00_desc_read(txd, 5, &word);
+	rt2x00_set_field32(&word, TXD_W5_TX_POWER,
+			   TXPOWER_TO_DEV(control->power_level));
+	rt2x00_set_field32(&word, TXD_W5_WAITING_DMA_DONE_INT, 1);
+	rt2x00_desc_write(txd, 5, word);
+
+	rt2x00_desc_read(txd, 0, &word);
+	rt2x00_set_field32(&word, TXD_W0_BURST,
+			   test_bit(ENTRY_TXD_BURST, &desc->flags));
+	rt2x00_set_field32(&word, TXD_W0_VALID, 1);
+	rt2x00_set_field32(&word, TXD_W0_MORE_FRAG,
+			   test_bit(ENTRY_TXD_MORE_FRAG, &desc->flags));
+	rt2x00_set_field32(&word, TXD_W0_ACK,
+			   !(control->flags & IEEE80211_TXCTL_NO_ACK));
+	rt2x00_set_field32(&word, TXD_W0_TIMESTAMP,
+			   test_bit(ENTRY_TXD_REQ_TIMESTAMP, &desc->flags));
+	rt2x00_set_field32(&word, TXD_W0_OFDM,
+			   test_bit(ENTRY_TXD_OFDM_RATE, &desc->flags));
+	rt2x00_set_field32(&word, TXD_W0_IFS, desc->ifs);
+	rt2x00_set_field32(&word, TXD_W0_RETRY_MODE,
+			   !!(control->flags &
+			      IEEE80211_TXCTL_LONG_RETRY_LIMIT));
+	rt2x00_set_field32(&word, TXD_W0_TKIP_MIC, 0);
+	rt2x00_set_field32(&word, TXD_W0_DATABYTE_COUNT, length);
+	rt2x00_set_field32(&word, TXD_W0_BURST2,
+			   test_bit(ENTRY_TXD_BURST, &desc->flags));
+	rt2x00_set_field32(&word, TXD_W0_CIPHER_ALG, CIPHER_NONE);
+	rt2x00_desc_write(txd, 0, word);
+}
+
+static int rt73usb_get_tx_data_len(struct rt2x00_dev *rt2x00dev,
+				   int maxpacket, struct sk_buff *skb)
+{
+	int length;
+
+	/*
+	 * The length _must_ be a multiple of 4,
+	 * but it must _not_ be a multiple of the USB packet size.
+	 */
+	length = roundup(skb->len, 4);
+	length += (4 * !(length % maxpacket));
+
+	return length;
+}
+
+/*
+ * TX data initialization
+ */
+static void rt73usb_kick_tx_queue(struct rt2x00_dev *rt2x00dev,
+				  unsigned int queue)
+{
+	u32 reg;
+
+	if (queue != IEEE80211_TX_QUEUE_BEACON)
+		return;
+
+	/*
+	 * For Wi-Fi faily generated beacons between participating stations.
+	 * Set TBTT phase adaptive adjustment step to 8us (default 16us)
+	 */
+	rt73usb_register_write(rt2x00dev, TXRX_CSR10, 0x00001008);
+
+	rt73usb_register_read(rt2x00dev, TXRX_CSR9, &reg);
+	if (!rt2x00_get_field32(reg, TXRX_CSR9_BEACON_GEN)) {
+		rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_GEN, 1);
+		rt73usb_register_write(rt2x00dev, TXRX_CSR9, reg);
+	}
+}
+
+/*
+ * RX control handlers
+ */
+static int rt73usb_agc_to_rssi(struct rt2x00_dev *rt2x00dev, int rxd_w1)
+{
+	u16 eeprom;
+	u8 offset;
+	u8 lna;
+
+	lna = rt2x00_get_field32(rxd_w1, RXD_W1_RSSI_LNA);
+	switch (lna) {
+	case 3:
+		offset = 90;
+		break;
+	case 2:
+		offset = 74;
+		break;
+	case 1:
+		offset = 64;
+		break;
+	default:
+		return 0;
+	}
+
+	if (rt2x00dev->rx_status.phymode == MODE_IEEE80211A) {
+		if (test_bit(CONFIG_EXTERNAL_LNA_A, &rt2x00dev->flags)) {
+			if (lna == 3 || lna == 2)
+				offset += 10;
+		} else {
+			if (lna == 3)
+				offset += 6;
+			else if (lna == 2)
+				offset += 8;
+		}
+
+		rt2x00_eeprom_read(rt2x00dev, EEPROM_RSSI_OFFSET_A, &eeprom);
+		offset -= rt2x00_get_field16(eeprom, EEPROM_RSSI_OFFSET_A_1);
+	} else {
+		if (test_bit(CONFIG_EXTERNAL_LNA_BG, &rt2x00dev->flags))
+			offset += 14;
+
+		rt2x00_eeprom_read(rt2x00dev, EEPROM_RSSI_OFFSET_BG, &eeprom);
+		offset -= rt2x00_get_field16(eeprom, EEPROM_RSSI_OFFSET_BG_1);
+	}
+
+	return rt2x00_get_field32(rxd_w1, RXD_W1_RSSI_AGC) * 2 - offset;
+}
+
+static void rt73usb_fill_rxdone(struct data_entry *entry,
+			        struct rxdata_entry_desc *desc)
+{
+	struct data_desc *rxd = (struct data_desc *)entry->skb->data;
+	u32 word0;
+	u32 word1;
+
+	rt2x00_desc_read(rxd, 0, &word0);
+	rt2x00_desc_read(rxd, 1, &word1);
+
+	desc->flags = 0;
+	if (rt2x00_get_field32(word0, RXD_W0_CRC_ERROR))
+		desc->flags |= RX_FLAG_FAILED_FCS_CRC;
+
+	/*
+	 * Obtain the status about this packet.
+	 */
+	desc->signal = rt2x00_get_field32(word1, RXD_W1_SIGNAL);
+	desc->rssi = rt73usb_agc_to_rssi(entry->ring->rt2x00dev, word1);
+	desc->ofdm = rt2x00_get_field32(word0, RXD_W0_OFDM);
+	desc->size = rt2x00_get_field32(word0, RXD_W0_DATABYTE_COUNT);
+
+	/*
+	 * Pull the skb to clear the descriptor area.
+	 */
+	skb_pull(entry->skb, entry->ring->desc_size);
+
+	return;
+}
+
+/*
+ * Device probe functions.
+ */
+static int rt73usb_validate_eeprom(struct rt2x00_dev *rt2x00dev)
+{
+	u16 word;
+	u8 *mac;
+	s8 value;
+
+	rt2x00usb_eeprom_read(rt2x00dev, rt2x00dev->eeprom, EEPROM_SIZE);
+
+	/*
+	 * Start validation of the data that has been read.
+	 */
+	mac = rt2x00_eeprom_addr(rt2x00dev, EEPROM_MAC_ADDR_0);
+	if (!is_valid_ether_addr(mac)) {
+		DECLARE_MAC_BUF(macbuf);
+
+		random_ether_addr(mac);
+		EEPROM(rt2x00dev, "MAC: %s\n", print_mac(macbuf, mac));
+	}
+
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_ANTENNA, &word);
+	if (word == 0xffff) {
+		rt2x00_set_field16(&word, EEPROM_ANTENNA_NUM, 2);
+		rt2x00_set_field16(&word, EEPROM_ANTENNA_TX_DEFAULT, 2);
+		rt2x00_set_field16(&word, EEPROM_ANTENNA_RX_DEFAULT, 2);
+		rt2x00_set_field16(&word, EEPROM_ANTENNA_FRAME_TYPE, 0);
+		rt2x00_set_field16(&word, EEPROM_ANTENNA_DYN_TXAGC, 0);
+		rt2x00_set_field16(&word, EEPROM_ANTENNA_HARDWARE_RADIO, 0);
+		rt2x00_set_field16(&word, EEPROM_ANTENNA_RF_TYPE, RF5226);
+		rt2x00_eeprom_write(rt2x00dev, EEPROM_ANTENNA, word);
+		EEPROM(rt2x00dev, "Antenna: 0x%04x\n", word);
+	}
+
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC, &word);
+	if (word == 0xffff) {
+		rt2x00_set_field16(&word, EEPROM_NIC_EXTERNAL_LNA, 0);
+		rt2x00_eeprom_write(rt2x00dev, EEPROM_NIC, word);
+		EEPROM(rt2x00dev, "NIC: 0x%04x\n", word);
+	}
+
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_LED, &word);
+	if (word == 0xffff) {
+		rt2x00_set_field16(&word, EEPROM_LED_POLARITY_RDY_G, 0);
+		rt2x00_set_field16(&word, EEPROM_LED_POLARITY_RDY_A, 0);
+		rt2x00_set_field16(&word, EEPROM_LED_POLARITY_ACT, 0);
+		rt2x00_set_field16(&word, EEPROM_LED_POLARITY_GPIO_0, 0);
+		rt2x00_set_field16(&word, EEPROM_LED_POLARITY_GPIO_1, 0);
+		rt2x00_set_field16(&word, EEPROM_LED_POLARITY_GPIO_2, 0);
+		rt2x00_set_field16(&word, EEPROM_LED_POLARITY_GPIO_3, 0);
+		rt2x00_set_field16(&word, EEPROM_LED_POLARITY_GPIO_4, 0);
+		rt2x00_set_field16(&word, EEPROM_LED_LED_MODE,
+				   LED_MODE_DEFAULT);
+		rt2x00_eeprom_write(rt2x00dev, EEPROM_LED, word);
+		EEPROM(rt2x00dev, "Led: 0x%04x\n", word);
+	}
+
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_FREQ, &word);
+	if (word == 0xffff) {
+		rt2x00_set_field16(&word, EEPROM_FREQ_OFFSET, 0);
+		rt2x00_set_field16(&word, EEPROM_FREQ_SEQ, 0);
+		rt2x00_eeprom_write(rt2x00dev, EEPROM_FREQ, word);
+		EEPROM(rt2x00dev, "Freq: 0x%04x\n", word);
+	}
+
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_RSSI_OFFSET_BG, &word);
+	if (word == 0xffff) {
+		rt2x00_set_field16(&word, EEPROM_RSSI_OFFSET_BG_1, 0);
+		rt2x00_set_field16(&word, EEPROM_RSSI_OFFSET_BG_2, 0);
+		rt2x00_eeprom_write(rt2x00dev, EEPROM_RSSI_OFFSET_BG, word);
+		EEPROM(rt2x00dev, "RSSI OFFSET BG: 0x%04x\n", word);
+	} else {
+		value = rt2x00_get_field16(word, EEPROM_RSSI_OFFSET_BG_1);
+		if (value < -10 || value > 10)
+			rt2x00_set_field16(&word, EEPROM_RSSI_OFFSET_BG_1, 0);
+		value = rt2x00_get_field16(word, EEPROM_RSSI_OFFSET_BG_2);
+		if (value < -10 || value > 10)
+			rt2x00_set_field16(&word, EEPROM_RSSI_OFFSET_BG_2, 0);
+		rt2x00_eeprom_write(rt2x00dev, EEPROM_RSSI_OFFSET_BG, word);
+	}
+
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_RSSI_OFFSET_A, &word);
+	if (word == 0xffff) {
+		rt2x00_set_field16(&word, EEPROM_RSSI_OFFSET_A_1, 0);
+		rt2x00_set_field16(&word, EEPROM_RSSI_OFFSET_A_2, 0);
+		rt2x00_eeprom_write(rt2x00dev, EEPROM_RSSI_OFFSET_A, word);
+		EEPROM(rt2x00dev, "RSSI OFFSET BG: 0x%04x\n", word);
+	} else {
+		value = rt2x00_get_field16(word, EEPROM_RSSI_OFFSET_A_1);
+		if (value < -10 || value > 10)
+			rt2x00_set_field16(&word, EEPROM_RSSI_OFFSET_A_1, 0);
+		value = rt2x00_get_field16(word, EEPROM_RSSI_OFFSET_A_2);
+		if (value < -10 || value > 10)
+			rt2x00_set_field16(&word, EEPROM_RSSI_OFFSET_A_2, 0);
+		rt2x00_eeprom_write(rt2x00dev, EEPROM_RSSI_OFFSET_A, word);
+	}
+
+	return 0;
+}
+
+static int rt73usb_init_eeprom(struct rt2x00_dev *rt2x00dev)
+{
+	u32 reg;
+	u16 value;
+	u16 eeprom;
+
+	/*
+	 * Read EEPROM word for configuration.
+	 */
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_ANTENNA, &eeprom);
+
+	/*
+	 * Identify RF chipset.
+	 */
+	value = rt2x00_get_field16(eeprom, EEPROM_ANTENNA_RF_TYPE);
+	rt73usb_register_read(rt2x00dev, MAC_CSR0, &reg);
+	rt2x00_set_chip(rt2x00dev, RT2571, value, reg);
+
+	if (!rt2x00_rev(&rt2x00dev->chip, 0x25730)) {
+		ERROR(rt2x00dev, "Invalid RT chipset detected.\n");
+		return -ENODEV;
+	}
+
+	if (!rt2x00_rf(&rt2x00dev->chip, RF5226) &&
+	    !rt2x00_rf(&rt2x00dev->chip, RF2528) &&
+	    !rt2x00_rf(&rt2x00dev->chip, RF5225) &&
+	    !rt2x00_rf(&rt2x00dev->chip, RF2527)) {
+		ERROR(rt2x00dev, "Invalid RF chipset detected.\n");
+		return -ENODEV;
+	}
+
+	/*
+	 * Identify default antenna configuration.
+	 */
+	rt2x00dev->hw->conf.antenna_sel_tx =
+	    rt2x00_get_field16(eeprom, EEPROM_ANTENNA_TX_DEFAULT);
+	rt2x00dev->hw->conf.antenna_sel_rx =
+	    rt2x00_get_field16(eeprom, EEPROM_ANTENNA_RX_DEFAULT);
+
+	/*
+	 * Read the Frame type.
+	 */
+	if (rt2x00_get_field16(eeprom, EEPROM_ANTENNA_FRAME_TYPE))
+		__set_bit(CONFIG_FRAME_TYPE, &rt2x00dev->flags);
+
+	/*
+	 * Read frequency offset.
+	 */
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_FREQ, &eeprom);
+	rt2x00dev->freq_offset = rt2x00_get_field16(eeprom, EEPROM_FREQ_OFFSET);
+
+	/*
+	 * Read external LNA informations.
+	 */
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC, &eeprom);
+
+	if (rt2x00_get_field16(eeprom, EEPROM_NIC_EXTERNAL_LNA)) {
+		__set_bit(CONFIG_EXTERNAL_LNA_A, &rt2x00dev->flags);
+		__set_bit(CONFIG_EXTERNAL_LNA_BG, &rt2x00dev->flags);
+	}
+
+	/*
+	 * Store led settings, for correct led behaviour.
+	 */
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_LED, &eeprom);
+
+	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_LED_MODE,
+			   rt2x00dev->led_mode);
+	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_POLARITY_GPIO_0,
+			   rt2x00_get_field16(eeprom,
+					      EEPROM_LED_POLARITY_GPIO_0));
+	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_POLARITY_GPIO_1,
+			   rt2x00_get_field16(eeprom,
+					      EEPROM_LED_POLARITY_GPIO_1));
+	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_POLARITY_GPIO_2,
+			   rt2x00_get_field16(eeprom,
+					      EEPROM_LED_POLARITY_GPIO_2));
+	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_POLARITY_GPIO_3,
+			   rt2x00_get_field16(eeprom,
+					      EEPROM_LED_POLARITY_GPIO_3));
+	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_POLARITY_GPIO_4,
+			   rt2x00_get_field16(eeprom,
+					      EEPROM_LED_POLARITY_GPIO_4));
+	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_POLARITY_ACT,
+			   rt2x00_get_field16(eeprom, EEPROM_LED_POLARITY_ACT));
+	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_POLARITY_READY_BG,
+			   rt2x00_get_field16(eeprom,
+					      EEPROM_LED_POLARITY_RDY_G));
+	rt2x00_set_field16(&rt2x00dev->led_reg, MCU_LEDCS_POLARITY_READY_A,
+			   rt2x00_get_field16(eeprom,
+					      EEPROM_LED_POLARITY_RDY_A));
+
+	return 0;
+}
+
+/*
+ * RF value list for RF2528
+ * Supports: 2.4 GHz
+ */
+static const struct rf_channel rf_vals_bg_2528[] = {
+	{ 1,  0x00002c0c, 0x00000786, 0x00068255, 0x000fea0b },
+	{ 2,  0x00002c0c, 0x00000786, 0x00068255, 0x000fea1f },
+	{ 3,  0x00002c0c, 0x0000078a, 0x00068255, 0x000fea0b },
+	{ 4,  0x00002c0c, 0x0000078a, 0x00068255, 0x000fea1f },
+	{ 5,  0x00002c0c, 0x0000078e, 0x00068255, 0x000fea0b },
+	{ 6,  0x00002c0c, 0x0000078e, 0x00068255, 0x000fea1f },
+	{ 7,  0x00002c0c, 0x00000792, 0x00068255, 0x000fea0b },
+	{ 8,  0x00002c0c, 0x00000792, 0x00068255, 0x000fea1f },
+	{ 9,  0x00002c0c, 0x00000796, 0x00068255, 0x000fea0b },
+	{ 10, 0x00002c0c, 0x00000796, 0x00068255, 0x000fea1f },
+	{ 11, 0x00002c0c, 0x0000079a, 0x00068255, 0x000fea0b },
+	{ 12, 0x00002c0c, 0x0000079a, 0x00068255, 0x000fea1f },
+	{ 13, 0x00002c0c, 0x0000079e, 0x00068255, 0x000fea0b },
+	{ 14, 0x00002c0c, 0x000007a2, 0x00068255, 0x000fea13 },
+};
+
+/*
+ * RF value list for RF5226
+ * Supports: 2.4 GHz & 5.2 GHz
+ */
+static const struct rf_channel rf_vals_5226[] = {
+	{ 1,  0x00002c0c, 0x00000786, 0x00068255, 0x000fea0b },
+	{ 2,  0x00002c0c, 0x00000786, 0x00068255, 0x000fea1f },
+	{ 3,  0x00002c0c, 0x0000078a, 0x00068255, 0x000fea0b },
+	{ 4,  0x00002c0c, 0x0000078a, 0x00068255, 0x000fea1f },
+	{ 5,  0x00002c0c, 0x0000078e, 0x00068255, 0x000fea0b },
+	{ 6,  0x00002c0c, 0x0000078e, 0x00068255, 0x000fea1f },
+	{ 7,  0x00002c0c, 0x00000792, 0x00068255, 0x000fea0b },
+	{ 8,  0x00002c0c, 0x00000792, 0x00068255, 0x000fea1f },
+	{ 9,  0x00002c0c, 0x00000796, 0x00068255, 0x000fea0b },
+	{ 10, 0x00002c0c, 0x00000796, 0x00068255, 0x000fea1f },
+	{ 11, 0x00002c0c, 0x0000079a, 0x00068255, 0x000fea0b },
+	{ 12, 0x00002c0c, 0x0000079a, 0x00068255, 0x000fea1f },
+	{ 13, 0x00002c0c, 0x0000079e, 0x00068255, 0x000fea0b },
+	{ 14, 0x00002c0c, 0x000007a2, 0x00068255, 0x000fea13 },
+
+	/* 802.11 UNI / HyperLan 2 */
+	{ 36, 0x00002c0c, 0x0000099a, 0x00098255, 0x000fea23 },
+	{ 40, 0x00002c0c, 0x000009a2, 0x00098255, 0x000fea03 },
+	{ 44, 0x00002c0c, 0x000009a6, 0x00098255, 0x000fea0b },
+	{ 48, 0x00002c0c, 0x000009aa, 0x00098255, 0x000fea13 },
+	{ 52, 0x00002c0c, 0x000009ae, 0x00098255, 0x000fea1b },
+	{ 56, 0x00002c0c, 0x000009b2, 0x00098255, 0x000fea23 },
+	{ 60, 0x00002c0c, 0x000009ba, 0x00098255, 0x000fea03 },
+	{ 64, 0x00002c0c, 0x000009be, 0x00098255, 0x000fea0b },
+
+	/* 802.11 HyperLan 2 */
+	{ 100, 0x00002c0c, 0x00000a2a, 0x000b8255, 0x000fea03 },
+	{ 104, 0x00002c0c, 0x00000a2e, 0x000b8255, 0x000fea0b },
+	{ 108, 0x00002c0c, 0x00000a32, 0x000b8255, 0x000fea13 },
+	{ 112, 0x00002c0c, 0x00000a36, 0x000b8255, 0x000fea1b },
+	{ 116, 0x00002c0c, 0x00000a3a, 0x000b8255, 0x000fea23 },
+	{ 120, 0x00002c0c, 0x00000a82, 0x000b8255, 0x000fea03 },
+	{ 124, 0x00002c0c, 0x00000a86, 0x000b8255, 0x000fea0b },
+	{ 128, 0x00002c0c, 0x00000a8a, 0x000b8255, 0x000fea13 },
+	{ 132, 0x00002c0c, 0x00000a8e, 0x000b8255, 0x000fea1b },
+	{ 136, 0x00002c0c, 0x00000a92, 0x000b8255, 0x000fea23 },
+
+	/* 802.11 UNII */
+	{ 140, 0x00002c0c, 0x00000a9a, 0x000b8255, 0x000fea03 },
+	{ 149, 0x00002c0c, 0x00000aa2, 0x000b8255, 0x000fea1f },
+	{ 153, 0x00002c0c, 0x00000aa6, 0x000b8255, 0x000fea27 },
+	{ 157, 0x00002c0c, 0x00000aae, 0x000b8255, 0x000fea07 },
+	{ 161, 0x00002c0c, 0x00000ab2, 0x000b8255, 0x000fea0f },
+	{ 165, 0x00002c0c, 0x00000ab6, 0x000b8255, 0x000fea17 },
+
+	/* MMAC(Japan)J52 ch 34,38,42,46 */
+	{ 34, 0x00002c0c, 0x0008099a, 0x000da255, 0x000d3a0b },
+	{ 38, 0x00002c0c, 0x0008099e, 0x000da255, 0x000d3a13 },
+	{ 42, 0x00002c0c, 0x000809a2, 0x000da255, 0x000d3a1b },
+	{ 46, 0x00002c0c, 0x000809a6, 0x000da255, 0x000d3a23 },
+};
+
+/*
+ * RF value list for RF5225 & RF2527
+ * Supports: 2.4 GHz & 5.2 GHz
+ */
+static const struct rf_channel rf_vals_5225_2527[] = {
+	{ 1,  0x00002ccc, 0x00004786, 0x00068455, 0x000ffa0b },
+	{ 2,  0x00002ccc, 0x00004786, 0x00068455, 0x000ffa1f },
+	{ 3,  0x00002ccc, 0x0000478a, 0x00068455, 0x000ffa0b },
+	{ 4,  0x00002ccc, 0x0000478a, 0x00068455, 0x000ffa1f },
+	{ 5,  0x00002ccc, 0x0000478e, 0x00068455, 0x000ffa0b },
+	{ 6,  0x00002ccc, 0x0000478e, 0x00068455, 0x000ffa1f },
+	{ 7,  0x00002ccc, 0x00004792, 0x00068455, 0x000ffa0b },
+	{ 8,  0x00002ccc, 0x00004792, 0x00068455, 0x000ffa1f },
+	{ 9,  0x00002ccc, 0x00004796, 0x00068455, 0x000ffa0b },
+	{ 10, 0x00002ccc, 0x00004796, 0x00068455, 0x000ffa1f },
+	{ 11, 0x00002ccc, 0x0000479a, 0x00068455, 0x000ffa0b },
+	{ 12, 0x00002ccc, 0x0000479a, 0x00068455, 0x000ffa1f },
+	{ 13, 0x00002ccc, 0x0000479e, 0x00068455, 0x000ffa0b },
+	{ 14, 0x00002ccc, 0x000047a2, 0x00068455, 0x000ffa13 },
+
+	/* 802.11 UNI / HyperLan 2 */
+	{ 36, 0x00002ccc, 0x0000499a, 0x0009be55, 0x000ffa23 },
+	{ 40, 0x00002ccc, 0x000049a2, 0x0009be55, 0x000ffa03 },
+	{ 44, 0x00002ccc, 0x000049a6, 0x0009be55, 0x000ffa0b },
+	{ 48, 0x00002ccc, 0x000049aa, 0x0009be55, 0x000ffa13 },
+	{ 52, 0x00002ccc, 0x000049ae, 0x0009ae55, 0x000ffa1b },
+	{ 56, 0x00002ccc, 0x000049b2, 0x0009ae55, 0x000ffa23 },
+	{ 60, 0x00002ccc, 0x000049ba, 0x0009ae55, 0x000ffa03 },
+	{ 64, 0x00002ccc, 0x000049be, 0x0009ae55, 0x000ffa0b },
+
+	/* 802.11 HyperLan 2 */
+	{ 100, 0x00002ccc, 0x00004a2a, 0x000bae55, 0x000ffa03 },
+	{ 104, 0x00002ccc, 0x00004a2e, 0x000bae55, 0x000ffa0b },
+	{ 108, 0x00002ccc, 0x00004a32, 0x000bae55, 0x000ffa13 },
+	{ 112, 0x00002ccc, 0x00004a36, 0x000bae55, 0x000ffa1b },
+	{ 116, 0x00002ccc, 0x00004a3a, 0x000bbe55, 0x000ffa23 },
+	{ 120, 0x00002ccc, 0x00004a82, 0x000bbe55, 0x000ffa03 },
+	{ 124, 0x00002ccc, 0x00004a86, 0x000bbe55, 0x000ffa0b },
+	{ 128, 0x00002ccc, 0x00004a8a, 0x000bbe55, 0x000ffa13 },
+	{ 132, 0x00002ccc, 0x00004a8e, 0x000bbe55, 0x000ffa1b },
+	{ 136, 0x00002ccc, 0x00004a92, 0x000bbe55, 0x000ffa23 },
+
+	/* 802.11 UNII */
+	{ 140, 0x00002ccc, 0x00004a9a, 0x000bbe55, 0x000ffa03 },
+	{ 149, 0x00002ccc, 0x00004aa2, 0x000bbe55, 0x000ffa1f },
+	{ 153, 0x00002ccc, 0x00004aa6, 0x000bbe55, 0x000ffa27 },
+	{ 157, 0x00002ccc, 0x00004aae, 0x000bbe55, 0x000ffa07 },
+	{ 161, 0x00002ccc, 0x00004ab2, 0x000bbe55, 0x000ffa0f },
+	{ 165, 0x00002ccc, 0x00004ab6, 0x000bbe55, 0x000ffa17 },
+
+	/* MMAC(Japan)J52 ch 34,38,42,46 */
+	{ 34, 0x00002ccc, 0x0000499a, 0x0009be55, 0x000ffa0b },
+	{ 38, 0x00002ccc, 0x0000499e, 0x0009be55, 0x000ffa13 },
+	{ 42, 0x00002ccc, 0x000049a2, 0x0009be55, 0x000ffa1b },
+	{ 46, 0x00002ccc, 0x000049a6, 0x0009be55, 0x000ffa23 },
+};
+
+
+static void rt73usb_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
+{
+	struct hw_mode_spec *spec = &rt2x00dev->spec;
+	u8 *txpower;
+	unsigned int i;
+
+	/*
+	 * Initialize all hw fields.
+	 */
+	rt2x00dev->hw->flags =
+	    IEEE80211_HW_HOST_GEN_BEACON_TEMPLATE |
+	    IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING;
+	rt2x00dev->hw->extra_tx_headroom = TXD_DESC_SIZE;
+	rt2x00dev->hw->max_signal = MAX_SIGNAL;
+	rt2x00dev->hw->max_rssi = MAX_RX_SSI;
+	rt2x00dev->hw->queues = 5;
+
+	SET_IEEE80211_DEV(rt2x00dev->hw, &rt2x00dev_usb(rt2x00dev)->dev);
+	SET_IEEE80211_PERM_ADDR(rt2x00dev->hw,
+				rt2x00_eeprom_addr(rt2x00dev,
+						   EEPROM_MAC_ADDR_0));
+
+	/*
+	 * Convert tx_power array in eeprom.
+	 */
+	txpower = rt2x00_eeprom_addr(rt2x00dev, EEPROM_TXPOWER_G_START);
+	for (i = 0; i < 14; i++)
+		txpower[i] = TXPOWER_FROM_DEV(txpower[i]);
+
+	/*
+	 * Initialize hw_mode information.
+	 */
+	spec->num_modes = 2;
+	spec->num_rates = 12;
+	spec->tx_power_a = NULL;
+	spec->tx_power_bg = txpower;
+	spec->tx_power_default = DEFAULT_TXPOWER;
+
+	if (rt2x00_rf(&rt2x00dev->chip, RF2528)) {
+		spec->num_channels = ARRAY_SIZE(rf_vals_bg_2528);
+		spec->channels = rf_vals_bg_2528;
+	} else if (rt2x00_rf(&rt2x00dev->chip, RF5226)) {
+		spec->num_channels = ARRAY_SIZE(rf_vals_5226);
+		spec->channels = rf_vals_5226;
+	} else if (rt2x00_rf(&rt2x00dev->chip, RF2527)) {
+		spec->num_channels = 14;
+		spec->channels = rf_vals_5225_2527;
+	} else if (rt2x00_rf(&rt2x00dev->chip, RF5225)) {
+		spec->num_channels = ARRAY_SIZE(rf_vals_5225_2527);
+		spec->channels = rf_vals_5225_2527;
+	}
+
+	if (rt2x00_rf(&rt2x00dev->chip, RF5225) ||
+	    rt2x00_rf(&rt2x00dev->chip, RF5226)) {
+		spec->num_modes = 3;
+
+		txpower = rt2x00_eeprom_addr(rt2x00dev, EEPROM_TXPOWER_A_START);
+		for (i = 0; i < 14; i++)
+			txpower[i] = TXPOWER_FROM_DEV(txpower[i]);
+
+		spec->tx_power_a = txpower;
+	}
+}
+
+static int rt73usb_probe_hw(struct rt2x00_dev *rt2x00dev)
+{
+	int retval;
+
+	/*
+	 * Allocate eeprom data.
+	 */
+	retval = rt73usb_validate_eeprom(rt2x00dev);
+	if (retval)
+		return retval;
+
+	retval = rt73usb_init_eeprom(rt2x00dev);
+	if (retval)
+		return retval;
+
+	/*
+	 * Initialize hw specifications.
+	 */
+	rt73usb_probe_hw_mode(rt2x00dev);
+
+	/*
+	 * This device requires firmware
+	 */
+	__set_bit(DRIVER_REQUIRE_FIRMWARE, &rt2x00dev->flags);
+
+	/*
+	 * Set the rssi offset.
+	 */
+	rt2x00dev->rssi_offset = DEFAULT_RSSI_OFFSET;
+
+	return 0;
+}
+
+/*
+ * IEEE80211 stack callback functions.
+ */
+static void rt73usb_configure_filter(struct ieee80211_hw *hw,
+				     unsigned int changed_flags,
+				     unsigned int *total_flags,
+				     int mc_count,
+				     struct dev_addr_list *mc_list)
+{
+	struct rt2x00_dev *rt2x00dev = hw->priv;
+	struct interface *intf = &rt2x00dev->interface;
+	u32 reg;
+
+	/*
+	 * Mask off any flags we are going to ignore from
+	 * the total_flags field.
+	 */
+	*total_flags &=
+	    FIF_ALLMULTI |
+	    FIF_FCSFAIL |
+	    FIF_PLCPFAIL |
+	    FIF_CONTROL |
+	    FIF_OTHER_BSS |
+	    FIF_PROMISC_IN_BSS;
+
+	/*
+	 * Apply some rules to the filters:
+	 * - Some filters imply different filters to be set.
+	 * - Some things we can't filter out at all.
+	 * - Some filters are set based on interface type.
+	 */
+	if (mc_count)
+		*total_flags |= FIF_ALLMULTI;
+	if (*total_flags & FIF_OTHER_BSS ||
+	    *total_flags & FIF_PROMISC_IN_BSS)
+		*total_flags |= FIF_PROMISC_IN_BSS | FIF_OTHER_BSS;
+	if (is_interface_type(intf, IEEE80211_IF_TYPE_AP))
+		*total_flags |= FIF_PROMISC_IN_BSS;
+
+	/*
+	 * Check if there is any work left for us.
+	 */
+	if (intf->filter == *total_flags)
+		return;
+	intf->filter = *total_flags;
+
+	/*
+	 * When in atomic context, reschedule and let rt2x00lib
+	 * call this function again.
+	 */
+	if (in_atomic()) {
+		queue_work(rt2x00dev->hw->workqueue, &rt2x00dev->filter_work);
+		return;
+	}
+
+	/*
+	 * Start configuration steps.
+	 * Note that the version error will always be dropped
+	 * and broadcast frames will always be accepted since
+	 * there is no filter for it at this time.
+	 */
+	rt73usb_register_read(rt2x00dev, TXRX_CSR0, &reg);
+	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_CRC,
+			   !(*total_flags & FIF_FCSFAIL));
+	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_PHYSICAL,
+			   !(*total_flags & FIF_PLCPFAIL));
+	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_CONTROL,
+			   !(*total_flags & FIF_CONTROL));
+	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_NOT_TO_ME,
+			   !(*total_flags & FIF_PROMISC_IN_BSS));
+	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_TO_DS,
+			   !(*total_flags & FIF_PROMISC_IN_BSS));
+	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_VERSION_ERROR, 1);
+	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_MULTICAST,
+			   !(*total_flags & FIF_ALLMULTI));
+	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_BROADCAST, 0);
+	rt2x00_set_field32(&reg, TXRX_CSR0_DROP_ACK_CTS, 1);
+	rt73usb_register_write(rt2x00dev, TXRX_CSR0, reg);
+}
+
+static int rt73usb_set_retry_limit(struct ieee80211_hw *hw,
+				   u32 short_retry, u32 long_retry)
+{
+	struct rt2x00_dev *rt2x00dev = hw->priv;
+	u32 reg;
+
+	rt73usb_register_read(rt2x00dev, TXRX_CSR4, &reg);
+	rt2x00_set_field32(&reg, TXRX_CSR4_LONG_RETRY_LIMIT, long_retry);
+	rt2x00_set_field32(&reg, TXRX_CSR4_SHORT_RETRY_LIMIT, short_retry);
+	rt73usb_register_write(rt2x00dev, TXRX_CSR4, reg);
+
+	return 0;
+}
+
+#if 0
+/*
+ * Mac80211 demands get_tsf must be atomic.
+ * This is not possible for rt73usb since all register access
+ * functions require sleeping. Untill mac80211 no longer needs
+ * get_tsf to be atomic, this function should be disabled.
+ */
+static u64 rt73usb_get_tsf(struct ieee80211_hw *hw)
+{
+	struct rt2x00_dev *rt2x00dev = hw->priv;
+	u64 tsf;
+	u32 reg;
+
+	rt73usb_register_read(rt2x00dev, TXRX_CSR13, &reg);
+	tsf = (u64) rt2x00_get_field32(reg, TXRX_CSR13_HIGH_TSFTIMER) << 32;
+	rt73usb_register_read(rt2x00dev, TXRX_CSR12, &reg);
+	tsf |= rt2x00_get_field32(reg, TXRX_CSR12_LOW_TSFTIMER);
+
+	return tsf;
+}
+#else
+#define rt73usb_get_tsf	NULL
+#endif
+
+static void rt73usb_reset_tsf(struct ieee80211_hw *hw)
+{
+	struct rt2x00_dev *rt2x00dev = hw->priv;
+
+	rt73usb_register_write(rt2x00dev, TXRX_CSR12, 0);
+	rt73usb_register_write(rt2x00dev, TXRX_CSR13, 0);
+}
+
+static int rt73usb_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb,
+			  struct ieee80211_tx_control *control)
+{
+	struct rt2x00_dev *rt2x00dev = hw->priv;
+	int timeout;
+
+	/*
+	 * Just in case the ieee80211 doesn't set this,
+	 * but we need this queue set for the descriptor
+	 * initialization.
+	 */
+	control->queue = IEEE80211_TX_QUEUE_BEACON;
+
+	/*
+	 * First we create the beacon.
+	 */
+	skb_push(skb, TXD_DESC_SIZE);
+	memset(skb->data, 0, TXD_DESC_SIZE);
+
+	rt2x00lib_write_tx_desc(rt2x00dev, (struct data_desc *)skb->data,
+				(struct ieee80211_hdr *)(skb->data +
+							 TXD_DESC_SIZE),
+				skb->len - TXD_DESC_SIZE, control);
+
+	/*
+	 * Write entire beacon with descriptor to register,
+	 * and kick the beacon generator.
+	 */
+	timeout = REGISTER_TIMEOUT * (skb->len / sizeof(u32));
+	rt2x00usb_vendor_request(rt2x00dev, USB_MULTI_WRITE,
+				 USB_VENDOR_REQUEST_OUT,
+				 HW_BEACON_BASE0, 0x0000,
+				 skb->data, skb->len, timeout);
+	rt73usb_kick_tx_queue(rt2x00dev, IEEE80211_TX_QUEUE_BEACON);
+
+	return 0;
+}
+
+static const struct ieee80211_ops rt73usb_mac80211_ops = {
+	.tx			= rt2x00mac_tx,
+	.start			= rt2x00mac_start,
+	.stop			= rt2x00mac_stop,
+	.add_interface		= rt2x00mac_add_interface,
+	.remove_interface	= rt2x00mac_remove_interface,
+	.config			= rt2x00mac_config,
+	.config_interface	= rt2x00mac_config_interface,
+	.configure_filter	= rt73usb_configure_filter,
+	.get_stats		= rt2x00mac_get_stats,
+	.set_retry_limit	= rt73usb_set_retry_limit,
+	.erp_ie_changed		= rt2x00mac_erp_ie_changed,
+	.conf_tx		= rt2x00mac_conf_tx,
+	.get_tx_stats		= rt2x00mac_get_tx_stats,
+	.get_tsf		= rt73usb_get_tsf,
+	.reset_tsf		= rt73usb_reset_tsf,
+	.beacon_update		= rt73usb_beacon_update,
+};
+
+static const struct rt2x00lib_ops rt73usb_rt2x00_ops = {
+	.probe_hw		= rt73usb_probe_hw,
+	.get_firmware_name	= rt73usb_get_firmware_name,
+	.load_firmware		= rt73usb_load_firmware,
+	.initialize		= rt2x00usb_initialize,
+	.uninitialize		= rt2x00usb_uninitialize,
+	.set_device_state	= rt73usb_set_device_state,
+	.link_stats		= rt73usb_link_stats,
+	.reset_tuner		= rt73usb_reset_tuner,
+	.link_tuner		= rt73usb_link_tuner,
+	.write_tx_desc		= rt73usb_write_tx_desc,
+	.write_tx_data		= rt2x00usb_write_tx_data,
+	.get_tx_data_len	= rt73usb_get_tx_data_len,
+	.kick_tx_queue		= rt73usb_kick_tx_queue,
+	.fill_rxdone		= rt73usb_fill_rxdone,
+	.config_mac_addr	= rt73usb_config_mac_addr,
+	.config_bssid		= rt73usb_config_bssid,
+	.config_type		= rt73usb_config_type,
+	.config_preamble	= rt73usb_config_preamble,
+	.config			= rt73usb_config,
+};
+
+static const struct rt2x00_ops rt73usb_ops = {
+	.name		= DRV_NAME,
+	.rxd_size	= RXD_DESC_SIZE,
+	.txd_size	= TXD_DESC_SIZE,
+	.eeprom_size	= EEPROM_SIZE,
+	.rf_size	= RF_SIZE,
+	.lib		= &rt73usb_rt2x00_ops,
+	.hw		= &rt73usb_mac80211_ops,
+#ifdef CONFIG_RT2X00_LIB_DEBUGFS
+	.debugfs	= &rt73usb_rt2x00debug,
+#endif /* CONFIG_RT2X00_LIB_DEBUGFS */
+};
+
+/*
+ * rt73usb module information.
+ */
+static struct usb_device_id rt73usb_device_table[] = {
+	/* AboCom */
+	{ USB_DEVICE(0x07b8, 0xb21d), USB_DEVICE_DATA(&rt73usb_ops) },
+	/* Askey */
+	{ USB_DEVICE(0x1690, 0x0722), USB_DEVICE_DATA(&rt73usb_ops) },
+	/* ASUS */
+	{ USB_DEVICE(0x0b05, 0x1723), USB_DEVICE_DATA(&rt73usb_ops) },
+	{ USB_DEVICE(0x0b05, 0x1724), USB_DEVICE_DATA(&rt73usb_ops) },
+	/* Belkin */
+	{ USB_DEVICE(0x050d, 0x7050), USB_DEVICE_DATA(&rt73usb_ops) },
+	{ USB_DEVICE(0x050d, 0x705a), USB_DEVICE_DATA(&rt73usb_ops) },
+	{ USB_DEVICE(0x050d, 0x905b), USB_DEVICE_DATA(&rt73usb_ops) },
+	/* Billionton */
+	{ USB_DEVICE(0x1631, 0xc019), USB_DEVICE_DATA(&rt73usb_ops) },
+	/* Buffalo */
+	{ USB_DEVICE(0x0411, 0x00f4), USB_DEVICE_DATA(&rt73usb_ops) },
+	/* CNet */
+	{ USB_DEVICE(0x1371, 0x9022), USB_DEVICE_DATA(&rt73usb_ops) },
+	{ USB_DEVICE(0x1371, 0x9032), USB_DEVICE_DATA(&rt73usb_ops) },
+	/* Conceptronic */
+	{ USB_DEVICE(0x14b2, 0x3c22), USB_DEVICE_DATA(&rt73usb_ops) },
+	/* D-Link */
+	{ USB_DEVICE(0x07d1, 0x3c03), USB_DEVICE_DATA(&rt73usb_ops) },
+	{ USB_DEVICE(0x07d1, 0x3c04), USB_DEVICE_DATA(&rt73usb_ops) },
+	/* Gemtek */
+	{ USB_DEVICE(0x15a9, 0x0004), USB_DEVICE_DATA(&rt73usb_ops) },
+	/* Gigabyte */
+	{ USB_DEVICE(0x1044, 0x8008), USB_DEVICE_DATA(&rt73usb_ops) },
+	{ USB_DEVICE(0x1044, 0x800a), USB_DEVICE_DATA(&rt73usb_ops) },
+	/* Huawei-3Com */
+	{ USB_DEVICE(0x1472, 0x0009), USB_DEVICE_DATA(&rt73usb_ops) },
+	/* Hercules */
+	{ USB_DEVICE(0x06f8, 0xe010), USB_DEVICE_DATA(&rt73usb_ops) },
+	{ USB_DEVICE(0x06f8, 0xe020), USB_DEVICE_DATA(&rt73usb_ops) },
+	/* Linksys */
+	{ USB_DEVICE(0x13b1, 0x0020), USB_DEVICE_DATA(&rt73usb_ops) },
+	{ USB_DEVICE(0x13b1, 0x0023), USB_DEVICE_DATA(&rt73usb_ops) },
+	/* MSI */
+	{ USB_DEVICE(0x0db0, 0x6877), USB_DEVICE_DATA(&rt73usb_ops) },
+	{ USB_DEVICE(0x0db0, 0x6874), USB_DEVICE_DATA(&rt73usb_ops) },
+	{ USB_DEVICE(0x0db0, 0xa861), USB_DEVICE_DATA(&rt73usb_ops) },
+	{ USB_DEVICE(0x0db0, 0xa874), USB_DEVICE_DATA(&rt73usb_ops) },
+	/* Ralink */
+	{ USB_DEVICE(0x148f, 0x2573), USB_DEVICE_DATA(&rt73usb_ops) },
+	{ USB_DEVICE(0x148f, 0x2671), USB_DEVICE_DATA(&rt73usb_ops) },
+	/* Qcom */
+	{ USB_DEVICE(0x18e8, 0x6196), USB_DEVICE_DATA(&rt73usb_ops) },
+	{ USB_DEVICE(0x18e8, 0x6229), USB_DEVICE_DATA(&rt73usb_ops) },
+	{ USB_DEVICE(0x18e8, 0x6238), USB_DEVICE_DATA(&rt73usb_ops) },
+	/* Senao */
+	{ USB_DEVICE(0x1740, 0x7100), USB_DEVICE_DATA(&rt73usb_ops) },
+	/* Sitecom */
+	{ USB_DEVICE(0x0df6, 0x9712), USB_DEVICE_DATA(&rt73usb_ops) },
+	{ USB_DEVICE(0x0df6, 0x90ac), USB_DEVICE_DATA(&rt73usb_ops) },
+	/* Surecom */
+	{ USB_DEVICE(0x0769, 0x31f3), USB_DEVICE_DATA(&rt73usb_ops) },
+	/* Planex */
+	{ USB_DEVICE(0x2019, 0xab01), USB_DEVICE_DATA(&rt73usb_ops) },
+	{ USB_DEVICE(0x2019, 0xab50), USB_DEVICE_DATA(&rt73usb_ops) },
+	{ 0, }
+};
+
+MODULE_AUTHOR(DRV_PROJECT);
+MODULE_VERSION(DRV_VERSION);
+MODULE_DESCRIPTION("Ralink RT73 USB Wireless LAN driver.");
+MODULE_SUPPORTED_DEVICE("Ralink RT2571W & RT2671 USB chipset based cards");
+MODULE_DEVICE_TABLE(usb, rt73usb_device_table);
+MODULE_FIRMWARE(FIRMWARE_RT2571);
+MODULE_LICENSE("GPL");
+
+static struct usb_driver rt73usb_driver = {
+	.name		= DRV_NAME,
+	.id_table	= rt73usb_device_table,
+	.probe		= rt2x00usb_probe,
+	.disconnect	= rt2x00usb_disconnect,
+	.suspend	= rt2x00usb_suspend,
+	.resume		= rt2x00usb_resume,
+};
+
+static int __init rt73usb_init(void)
+{
+	return usb_register(&rt73usb_driver);
+}
+
+static void __exit rt73usb_exit(void)
+{
+	usb_deregister(&rt73usb_driver);
+}
+
+module_init(rt73usb_init);
+module_exit(rt73usb_exit);
diff --git a/drivers/net/wireless/rt2x00/rt73usb.h b/drivers/net/wireless/rt2x00/rt73usb.h
new file mode 100644
index 000000000000..f0951519f74b
--- /dev/null
+++ b/drivers/net/wireless/rt2x00/rt73usb.h
@@ -0,0 +1,1024 @@
+/*
+	Copyright (C) 2004 - 2007 rt2x00 SourceForge Project
+	<http://rt2x00.serialmonkey.com>
+
+	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, or
+	(at your option) any later version.
+
+	This program is distributed in the hope that it will be useful,
+	but WITHOUT ANY WARRANTY; without even the implied warranty of
+	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+	GNU General Public License for more details.
+
+	You should have received a copy of the GNU General Public License
+	along with this program; if not, write to the
+	Free Software Foundation, Inc.,
+	59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+/*
+	Module: rt73usb
+	Abstract: Data structures and registers for the rt73usb module.
+	Supported chipsets: rt2571W & rt2671.
+ */
+
+#ifndef RT73USB_H
+#define RT73USB_H
+
+/*
+ * RF chip defines.
+ */
+#define RF5226				0x0001
+#define RF2528				0x0002
+#define RF5225				0x0003
+#define RF2527				0x0004
+
+/*
+ * Signal information.
+ * Defaul offset is required for RSSI <-> dBm conversion.
+ */
+#define MAX_SIGNAL			100
+#define MAX_RX_SSI			-1
+#define DEFAULT_RSSI_OFFSET		120
+
+/*
+ * Register layout information.
+ */
+#define CSR_REG_BASE			0x3000
+#define CSR_REG_SIZE			0x04b0
+#define EEPROM_BASE			0x0000
+#define EEPROM_SIZE			0x0100
+#define BBP_SIZE			0x0080
+#define RF_SIZE				0x0014
+
+/*
+ * USB registers.
+ */
+
+/*
+ * MCU_LEDCS: LED control for MCU Mailbox.
+ */
+#define MCU_LEDCS_LED_MODE		FIELD16(0x001f)
+#define MCU_LEDCS_RADIO_STATUS		FIELD16(0x0020)
+#define MCU_LEDCS_LINK_BG_STATUS	FIELD16(0x0040)
+#define MCU_LEDCS_LINK_A_STATUS		FIELD16(0x0080)
+#define MCU_LEDCS_POLARITY_GPIO_0	FIELD16(0x0100)
+#define MCU_LEDCS_POLARITY_GPIO_1	FIELD16(0x0200)
+#define MCU_LEDCS_POLARITY_GPIO_2	FIELD16(0x0400)
+#define MCU_LEDCS_POLARITY_GPIO_3	FIELD16(0x0800)
+#define MCU_LEDCS_POLARITY_GPIO_4	FIELD16(0x1000)
+#define MCU_LEDCS_POLARITY_ACT		FIELD16(0x2000)
+#define MCU_LEDCS_POLARITY_READY_BG	FIELD16(0x4000)
+#define MCU_LEDCS_POLARITY_READY_A	FIELD16(0x8000)
+
+/*
+ * 8051 firmware image.
+ */
+#define FIRMWARE_RT2571			"rt73.bin"
+#define FIRMWARE_IMAGE_BASE		0x0800
+
+/*
+ * Security key table memory.
+ * 16 entries 32-byte for shared key table
+ * 64 entries 32-byte for pairwise key table
+ * 64 entries 8-byte for pairwise ta key table
+ */
+#define SHARED_KEY_TABLE_BASE		0x1000
+#define PAIRWISE_KEY_TABLE_BASE		0x1200
+#define PAIRWISE_TA_TABLE_BASE		0x1a00
+
+struct hw_key_entry {
+	u8 key[16];
+	u8 tx_mic[8];
+	u8 rx_mic[8];
+} __attribute__ ((packed));
+
+struct hw_pairwise_ta_entry {
+	u8 address[6];
+	u8 reserved[2];
+} __attribute__ ((packed));
+
+/*
+ * Since NULL frame won't be that long (256 byte),
+ * We steal 16 tail bytes to save debugging settings.
+ */
+#define HW_DEBUG_SETTING_BASE		0x2bf0
+
+/*
+ * On-chip BEACON frame space.
+ */
+#define HW_BEACON_BASE0			0x2400
+#define HW_BEACON_BASE1			0x2500
+#define HW_BEACON_BASE2			0x2600
+#define HW_BEACON_BASE3			0x2700
+
+/*
+ * MAC Control/Status Registers(CSR).
+ * Some values are set in TU, whereas 1 TU == 1024 us.
+ */
+
+/*
+ * MAC_CSR0: ASIC revision number.
+ */
+#define MAC_CSR0			0x3000
+
+/*
+ * MAC_CSR1: System control register.
+ * SOFT_RESET: Software reset bit, 1: reset, 0: normal.
+ * BBP_RESET: Hardware reset BBP.
+ * HOST_READY: Host is ready after initialization, 1: ready.
+ */
+#define MAC_CSR1			0x3004
+#define MAC_CSR1_SOFT_RESET		FIELD32(0x00000001)
+#define MAC_CSR1_BBP_RESET		FIELD32(0x00000002)
+#define MAC_CSR1_HOST_READY		FIELD32(0x00000004)
+
+/*
+ * MAC_CSR2: STA MAC register 0.
+ */
+#define MAC_CSR2			0x3008
+#define MAC_CSR2_BYTE0			FIELD32(0x000000ff)
+#define MAC_CSR2_BYTE1			FIELD32(0x0000ff00)
+#define MAC_CSR2_BYTE2			FIELD32(0x00ff0000)
+#define MAC_CSR2_BYTE3			FIELD32(0xff000000)
+
+/*
+ * MAC_CSR3: STA MAC register 1.
+ */
+#define MAC_CSR3			0x300c
+#define MAC_CSR3_BYTE4			FIELD32(0x000000ff)
+#define MAC_CSR3_BYTE5			FIELD32(0x0000ff00)
+#define MAC_CSR3_UNICAST_TO_ME_MASK	FIELD32(0x00ff0000)
+
+/*
+ * MAC_CSR4: BSSID register 0.
+ */
+#define MAC_CSR4			0x3010
+#define MAC_CSR4_BYTE0			FIELD32(0x000000ff)
+#define MAC_CSR4_BYTE1			FIELD32(0x0000ff00)
+#define MAC_CSR4_BYTE2			FIELD32(0x00ff0000)
+#define MAC_CSR4_BYTE3			FIELD32(0xff000000)
+
+/*
+ * MAC_CSR5: BSSID register 1.
+ * BSS_ID_MASK: 3: one BSSID, 0: 4 BSSID, 2 or 1: 2 BSSID.
+ */
+#define MAC_CSR5			0x3014
+#define MAC_CSR5_BYTE4			FIELD32(0x000000ff)
+#define MAC_CSR5_BYTE5			FIELD32(0x0000ff00)
+#define MAC_CSR5_BSS_ID_MASK		FIELD32(0x00ff0000)
+
+/*
+ * MAC_CSR6: Maximum frame length register.
+ */
+#define MAC_CSR6			0x3018
+#define MAC_CSR6_MAX_FRAME_UNIT		FIELD32(0x00000fff)
+
+/*
+ * MAC_CSR7: Reserved
+ */
+#define MAC_CSR7			0x301c
+
+/*
+ * MAC_CSR8: SIFS/EIFS register.
+ * All units are in US.
+ */
+#define MAC_CSR8			0x3020
+#define MAC_CSR8_SIFS			FIELD32(0x000000ff)
+#define MAC_CSR8_SIFS_AFTER_RX_OFDM	FIELD32(0x0000ff00)
+#define MAC_CSR8_EIFS			FIELD32(0xffff0000)
+
+/*
+ * MAC_CSR9: Back-Off control register.
+ * SLOT_TIME: Slot time, default is 20us for 802.11BG.
+ * CWMIN: Bit for Cwmin. default Cwmin is 31 (2^5 - 1).
+ * CWMAX: Bit for Cwmax, default Cwmax is 1023 (2^10 - 1).
+ * CW_SELECT: 1: CWmin/Cwmax select from register, 0:select from TxD.
+ */
+#define MAC_CSR9			0x3024
+#define MAC_CSR9_SLOT_TIME		FIELD32(0x000000ff)
+#define MAC_CSR9_CWMIN			FIELD32(0x00000f00)
+#define MAC_CSR9_CWMAX			FIELD32(0x0000f000)
+#define MAC_CSR9_CW_SELECT		FIELD32(0x00010000)
+
+/*
+ * MAC_CSR10: Power state configuration.
+ */
+#define MAC_CSR10			0x3028
+
+/*
+ * MAC_CSR11: Power saving transition time register.
+ * DELAY_AFTER_TBCN: Delay after Tbcn expired in units of TU.
+ * TBCN_BEFORE_WAKEUP: Number of beacon before wakeup.
+ * WAKEUP_LATENCY: In unit of TU.
+ */
+#define MAC_CSR11			0x302c
+#define MAC_CSR11_DELAY_AFTER_TBCN	FIELD32(0x000000ff)
+#define MAC_CSR11_TBCN_BEFORE_WAKEUP	FIELD32(0x00007f00)
+#define MAC_CSR11_AUTOWAKE		FIELD32(0x00008000)
+#define MAC_CSR11_WAKEUP_LATENCY	FIELD32(0x000f0000)
+
+/*
+ * MAC_CSR12: Manual power control / status register (merge CSR20 & PWRCSR1).
+ * CURRENT_STATE: 0:sleep, 1:awake.
+ * FORCE_WAKEUP: This has higher priority than PUT_TO_SLEEP.
+ * BBP_CURRENT_STATE: 0: BBP sleep, 1: BBP awake.
+ */
+#define MAC_CSR12			0x3030
+#define MAC_CSR12_CURRENT_STATE		FIELD32(0x00000001)
+#define MAC_CSR12_PUT_TO_SLEEP		FIELD32(0x00000002)
+#define MAC_CSR12_FORCE_WAKEUP		FIELD32(0x00000004)
+#define MAC_CSR12_BBP_CURRENT_STATE	FIELD32(0x00000008)
+
+/*
+ * MAC_CSR13: GPIO.
+ */
+#define MAC_CSR13			0x3034
+
+/*
+ * MAC_CSR14: LED control register.
+ * ON_PERIOD: On period, default 70ms.
+ * OFF_PERIOD: Off period, default 30ms.
+ * HW_LED: HW TX activity, 1: normal OFF, 0: normal ON.
+ * SW_LED: s/w LED, 1: ON, 0: OFF.
+ * HW_LED_POLARITY: 0: active low, 1: active high.
+ */
+#define MAC_CSR14			0x3038
+#define MAC_CSR14_ON_PERIOD		FIELD32(0x000000ff)
+#define MAC_CSR14_OFF_PERIOD		FIELD32(0x0000ff00)
+#define MAC_CSR14_HW_LED		FIELD32(0x00010000)
+#define MAC_CSR14_SW_LED		FIELD32(0x00020000)
+#define MAC_CSR14_HW_LED_POLARITY	FIELD32(0x00040000)
+#define MAC_CSR14_SW_LED2		FIELD32(0x00080000)
+
+/*
+ * MAC_CSR15: NAV control.
+ */
+#define MAC_CSR15			0x303c
+
+/*
+ * TXRX control registers.
+ * Some values are set in TU, whereas 1 TU == 1024 us.
+ */
+
+/*
+ * TXRX_CSR0: TX/RX configuration register.
+ * TSF_OFFSET: Default is 24.
+ * AUTO_TX_SEQ: 1: ASIC auto replace sequence nr in outgoing frame.
+ * DISABLE_RX: Disable Rx engine.
+ * DROP_CRC: Drop CRC error.
+ * DROP_PHYSICAL: Drop physical error.
+ * DROP_CONTROL: Drop control frame.
+ * DROP_NOT_TO_ME: Drop not to me unicast frame.
+ * DROP_TO_DS: Drop fram ToDs bit is true.
+ * DROP_VERSION_ERROR: Drop version error frame.
+ * DROP_MULTICAST: Drop multicast frames.
+ * DROP_BORADCAST: Drop broadcast frames.
+ * ROP_ACK_CTS: Drop received ACK and CTS.
+ */
+#define TXRX_CSR0			0x3040
+#define TXRX_CSR0_RX_ACK_TIMEOUT	FIELD32(0x000001ff)
+#define TXRX_CSR0_TSF_OFFSET		FIELD32(0x00007e00)
+#define TXRX_CSR0_AUTO_TX_SEQ		FIELD32(0x00008000)
+#define TXRX_CSR0_DISABLE_RX		FIELD32(0x00010000)
+#define TXRX_CSR0_DROP_CRC		FIELD32(0x00020000)
+#define TXRX_CSR0_DROP_PHYSICAL		FIELD32(0x00040000)
+#define TXRX_CSR0_DROP_CONTROL		FIELD32(0x00080000)
+#define TXRX_CSR0_DROP_NOT_TO_ME	FIELD32(0x00100000)
+#define TXRX_CSR0_DROP_TO_DS		FIELD32(0x00200000)
+#define TXRX_CSR0_DROP_VERSION_ERROR	FIELD32(0x00400000)
+#define TXRX_CSR0_DROP_MULTICAST	FIELD32(0x00800000)
+#define TXRX_CSR0_DROP_BROADCAST	FIELD32(0x01000000)
+#define TXRX_CSR0_DROP_ACK_CTS		FIELD32(0x02000000)
+#define TXRX_CSR0_TX_WITHOUT_WAITING	FIELD32(0x04000000)
+
+/*
+ * TXRX_CSR1
+ */
+#define TXRX_CSR1			0x3044
+#define TXRX_CSR1_BBP_ID0		FIELD32(0x0000007f)
+#define TXRX_CSR1_BBP_ID0_VALID		FIELD32(0x00000080)
+#define TXRX_CSR1_BBP_ID1		FIELD32(0x00007f00)
+#define TXRX_CSR1_BBP_ID1_VALID		FIELD32(0x00008000)
+#define TXRX_CSR1_BBP_ID2		FIELD32(0x007f0000)
+#define TXRX_CSR1_BBP_ID2_VALID		FIELD32(0x00800000)
+#define TXRX_CSR1_BBP_ID3		FIELD32(0x7f000000)
+#define TXRX_CSR1_BBP_ID3_VALID		FIELD32(0x80000000)
+
+/*
+ * TXRX_CSR2
+ */
+#define TXRX_CSR2			0x3048
+#define TXRX_CSR2_BBP_ID0		FIELD32(0x0000007f)
+#define TXRX_CSR2_BBP_ID0_VALID		FIELD32(0x00000080)
+#define TXRX_CSR2_BBP_ID1		FIELD32(0x00007f00)
+#define TXRX_CSR2_BBP_ID1_VALID		FIELD32(0x00008000)
+#define TXRX_CSR2_BBP_ID2		FIELD32(0x007f0000)
+#define TXRX_CSR2_BBP_ID2_VALID		FIELD32(0x00800000)
+#define TXRX_CSR2_BBP_ID3		FIELD32(0x7f000000)
+#define TXRX_CSR2_BBP_ID3_VALID		FIELD32(0x80000000)
+
+/*
+ * TXRX_CSR3
+ */
+#define TXRX_CSR3			0x304c
+#define TXRX_CSR3_BBP_ID0		FIELD32(0x0000007f)
+#define TXRX_CSR3_BBP_ID0_VALID		FIELD32(0x00000080)
+#define TXRX_CSR3_BBP_ID1		FIELD32(0x00007f00)
+#define TXRX_CSR3_BBP_ID1_VALID		FIELD32(0x00008000)
+#define TXRX_CSR3_BBP_ID2		FIELD32(0x007f0000)
+#define TXRX_CSR3_BBP_ID2_VALID		FIELD32(0x00800000)
+#define TXRX_CSR3_BBP_ID3		FIELD32(0x7f000000)
+#define TXRX_CSR3_BBP_ID3_VALID		FIELD32(0x80000000)
+
+/*
+ * TXRX_CSR4: Auto-Responder/Tx-retry register.
+ * AUTORESPOND_PREAMBLE: 0:long, 1:short preamble.
+ * OFDM_TX_RATE_DOWN: 1:enable.
+ * OFDM_TX_RATE_STEP: 0:1-step, 1: 2-step, 2:3-step, 3:4-step.
+ * OFDM_TX_FALLBACK_CCK: 0: Fallback to OFDM 6M only, 1: Fallback to CCK 1M,2M.
+ */
+#define TXRX_CSR4			0x3050
+#define TXRX_CSR4_TX_ACK_TIMEOUT	FIELD32(0x000000ff)
+#define TXRX_CSR4_CNTL_ACK_POLICY	FIELD32(0x00000700)
+#define TXRX_CSR4_ACK_CTS_PSM		FIELD32(0x00010000)
+#define TXRX_CSR4_AUTORESPOND_ENABLE	FIELD32(0x00020000)
+#define TXRX_CSR4_AUTORESPOND_PREAMBLE	FIELD32(0x00040000)
+#define TXRX_CSR4_OFDM_TX_RATE_DOWN	FIELD32(0x00080000)
+#define TXRX_CSR4_OFDM_TX_RATE_STEP	FIELD32(0x00300000)
+#define TXRX_CSR4_OFDM_TX_FALLBACK_CCK	FIELD32(0x00400000)
+#define TXRX_CSR4_LONG_RETRY_LIMIT	FIELD32(0x0f000000)
+#define TXRX_CSR4_SHORT_RETRY_LIMIT	FIELD32(0xf0000000)
+
+/*
+ * TXRX_CSR5
+ */
+#define TXRX_CSR5			0x3054
+
+/*
+ * TXRX_CSR6: ACK/CTS payload consumed time
+ */
+#define TXRX_CSR6			0x3058
+
+/*
+ * TXRX_CSR7: OFDM ACK/CTS payload consumed time for 6/9/12/18 mbps.
+ */
+#define TXRX_CSR7			0x305c
+#define TXRX_CSR7_ACK_CTS_6MBS		FIELD32(0x000000ff)
+#define TXRX_CSR7_ACK_CTS_9MBS		FIELD32(0x0000ff00)
+#define TXRX_CSR7_ACK_CTS_12MBS		FIELD32(0x00ff0000)
+#define TXRX_CSR7_ACK_CTS_18MBS		FIELD32(0xff000000)
+
+/*
+ * TXRX_CSR8: OFDM ACK/CTS payload consumed time for 24/36/48/54 mbps.
+ */
+#define TXRX_CSR8			0x3060
+#define TXRX_CSR8_ACK_CTS_24MBS		FIELD32(0x000000ff)
+#define TXRX_CSR8_ACK_CTS_36MBS		FIELD32(0x0000ff00)
+#define TXRX_CSR8_ACK_CTS_48MBS		FIELD32(0x00ff0000)
+#define TXRX_CSR8_ACK_CTS_54MBS		FIELD32(0xff000000)
+
+/*
+ * TXRX_CSR9: Synchronization control register.
+ * BEACON_INTERVAL: In unit of 1/16 TU.
+ * TSF_TICKING: Enable TSF auto counting.
+ * TSF_SYNC: Tsf sync, 0: disable, 1: infra, 2: ad-hoc/master mode.
+ * BEACON_GEN: Enable beacon generator.
+ */
+#define TXRX_CSR9			0x3064
+#define TXRX_CSR9_BEACON_INTERVAL	FIELD32(0x0000ffff)
+#define TXRX_CSR9_TSF_TICKING		FIELD32(0x00010000)
+#define TXRX_CSR9_TSF_SYNC		FIELD32(0x00060000)
+#define TXRX_CSR9_TBTT_ENABLE		FIELD32(0x00080000)
+#define TXRX_CSR9_BEACON_GEN		FIELD32(0x00100000)
+#define TXRX_CSR9_TIMESTAMP_COMPENSATE	FIELD32(0xff000000)
+
+/*
+ * TXRX_CSR10: BEACON alignment.
+ */
+#define TXRX_CSR10			0x3068
+
+/*
+ * TXRX_CSR11: AES mask.
+ */
+#define TXRX_CSR11			0x306c
+
+/*
+ * TXRX_CSR12: TSF low 32.
+ */
+#define TXRX_CSR12			0x3070
+#define TXRX_CSR12_LOW_TSFTIMER		FIELD32(0xffffffff)
+
+/*
+ * TXRX_CSR13: TSF high 32.
+ */
+#define TXRX_CSR13			0x3074
+#define TXRX_CSR13_HIGH_TSFTIMER	FIELD32(0xffffffff)
+
+/*
+ * TXRX_CSR14: TBTT timer.
+ */
+#define TXRX_CSR14			0x3078
+
+/*
+ * TXRX_CSR15: TKIP MIC priority byte "AND" mask.
+ */
+#define TXRX_CSR15			0x307c
+
+/*
+ * PHY control registers.
+ * Some values are set in TU, whereas 1 TU == 1024 us.
+ */
+
+/*
+ * PHY_CSR0: RF/PS control.
+ */
+#define PHY_CSR0			0x3080
+#define PHY_CSR0_PA_PE_BG		FIELD32(0x00010000)
+#define PHY_CSR0_PA_PE_A		FIELD32(0x00020000)
+
+/*
+ * PHY_CSR1
+ */
+#define PHY_CSR1			0x3084
+#define PHY_CSR1_RF_RPI			FIELD32(0x00010000)
+
+/*
+ * PHY_CSR2: Pre-TX BBP control.
+ */
+#define PHY_CSR2			0x3088
+
+/*
+ * PHY_CSR3: BBP serial control register.
+ * VALUE: Register value to program into BBP.
+ * REG_NUM: Selected BBP register.
+ * READ_CONTROL: 0: Write BBP, 1: Read BBP.
+ * BUSY: 1: ASIC is busy execute BBP programming.
+ */
+#define PHY_CSR3			0x308c
+#define PHY_CSR3_VALUE			FIELD32(0x000000ff)
+#define PHY_CSR3_REGNUM			FIELD32(0x00007f00)
+#define PHY_CSR3_READ_CONTROL		FIELD32(0x00008000)
+#define PHY_CSR3_BUSY			FIELD32(0x00010000)
+
+/*
+ * PHY_CSR4: RF serial control register
+ * VALUE: Register value (include register id) serial out to RF/IF chip.
+ * NUMBER_OF_BITS: Number of bits used in RFRegValue (I:20, RFMD:22).
+ * IF_SELECT: 1: select IF to program, 0: select RF to program.
+ * PLL_LD: RF PLL_LD status.
+ * BUSY: 1: ASIC is busy execute RF programming.
+ */
+#define PHY_CSR4			0x3090
+#define PHY_CSR4_VALUE			FIELD32(0x00ffffff)
+#define PHY_CSR4_NUMBER_OF_BITS		FIELD32(0x1f000000)
+#define PHY_CSR4_IF_SELECT		FIELD32(0x20000000)
+#define PHY_CSR4_PLL_LD			FIELD32(0x40000000)
+#define PHY_CSR4_BUSY			FIELD32(0x80000000)
+
+/*
+ * PHY_CSR5: RX to TX signal switch timing control.
+ */
+#define PHY_CSR5			0x3094
+#define PHY_CSR5_IQ_FLIP		FIELD32(0x00000004)
+
+/*
+ * PHY_CSR6: TX to RX signal timing control.
+ */
+#define PHY_CSR6			0x3098
+#define PHY_CSR6_IQ_FLIP		FIELD32(0x00000004)
+
+/*
+ * PHY_CSR7: TX DAC switching timing control.
+ */
+#define PHY_CSR7			0x309c
+
+/*
+ * Security control register.
+ */
+
+/*
+ * SEC_CSR0: Shared key table control.
+ */
+#define SEC_CSR0			0x30a0
+#define SEC_CSR0_BSS0_KEY0_VALID	FIELD32(0x00000001)
+#define SEC_CSR0_BSS0_KEY1_VALID	FIELD32(0x00000002)
+#define SEC_CSR0_BSS0_KEY2_VALID	FIELD32(0x00000004)
+#define SEC_CSR0_BSS0_KEY3_VALID	FIELD32(0x00000008)
+#define SEC_CSR0_BSS1_KEY0_VALID	FIELD32(0x00000010)
+#define SEC_CSR0_BSS1_KEY1_VALID	FIELD32(0x00000020)
+#define SEC_CSR0_BSS1_KEY2_VALID	FIELD32(0x00000040)
+#define SEC_CSR0_BSS1_KEY3_VALID	FIELD32(0x00000080)
+#define SEC_CSR0_BSS2_KEY0_VALID	FIELD32(0x00000100)
+#define SEC_CSR0_BSS2_KEY1_VALID	FIELD32(0x00000200)
+#define SEC_CSR0_BSS2_KEY2_VALID	FIELD32(0x00000400)
+#define SEC_CSR0_BSS2_KEY3_VALID	FIELD32(0x00000800)
+#define SEC_CSR0_BSS3_KEY0_VALID	FIELD32(0x00001000)
+#define SEC_CSR0_BSS3_KEY1_VALID	FIELD32(0x00002000)
+#define SEC_CSR0_BSS3_KEY2_VALID	FIELD32(0x00004000)
+#define SEC_CSR0_BSS3_KEY3_VALID	FIELD32(0x00008000)
+
+/*
+ * SEC_CSR1: Shared key table security mode register.
+ */
+#define SEC_CSR1			0x30a4
+#define SEC_CSR1_BSS0_KEY0_CIPHER_ALG	FIELD32(0x00000007)
+#define SEC_CSR1_BSS0_KEY1_CIPHER_ALG	FIELD32(0x00000070)
+#define SEC_CSR1_BSS0_KEY2_CIPHER_ALG	FIELD32(0x00000700)
+#define SEC_CSR1_BSS0_KEY3_CIPHER_ALG	FIELD32(0x00007000)
+#define SEC_CSR1_BSS1_KEY0_CIPHER_ALG	FIELD32(0x00070000)
+#define SEC_CSR1_BSS1_KEY1_CIPHER_ALG	FIELD32(0x00700000)
+#define SEC_CSR1_BSS1_KEY2_CIPHER_ALG	FIELD32(0x07000000)
+#define SEC_CSR1_BSS1_KEY3_CIPHER_ALG	FIELD32(0x70000000)
+
+/*
+ * Pairwise key table valid bitmap registers.
+ * SEC_CSR2: pairwise key table valid bitmap 0.
+ * SEC_CSR3: pairwise key table valid bitmap 1.
+ */
+#define SEC_CSR2			0x30a8
+#define SEC_CSR3			0x30ac
+
+/*
+ * SEC_CSR4: Pairwise key table lookup control.
+ */
+#define SEC_CSR4			0x30b0
+
+/*
+ * SEC_CSR5: shared key table security mode register.
+ */
+#define SEC_CSR5			0x30b4
+#define SEC_CSR5_BSS2_KEY0_CIPHER_ALG	FIELD32(0x00000007)
+#define SEC_CSR5_BSS2_KEY1_CIPHER_ALG	FIELD32(0x00000070)
+#define SEC_CSR5_BSS2_KEY2_CIPHER_ALG	FIELD32(0x00000700)
+#define SEC_CSR5_BSS2_KEY3_CIPHER_ALG	FIELD32(0x00007000)
+#define SEC_CSR5_BSS3_KEY0_CIPHER_ALG	FIELD32(0x00070000)
+#define SEC_CSR5_BSS3_KEY1_CIPHER_ALG	FIELD32(0x00700000)
+#define SEC_CSR5_BSS3_KEY2_CIPHER_ALG	FIELD32(0x07000000)
+#define SEC_CSR5_BSS3_KEY3_CIPHER_ALG	FIELD32(0x70000000)
+
+/*
+ * STA control registers.
+ */
+
+/*
+ * STA_CSR0: RX PLCP error count & RX FCS error count.
+ */
+#define STA_CSR0			0x30c0
+#define STA_CSR0_FCS_ERROR		FIELD32(0x0000ffff)
+#define STA_CSR0_PLCP_ERROR		FIELD32(0xffff0000)
+
+/*
+ * STA_CSR1: RX False CCA count & RX LONG frame count.
+ */
+#define STA_CSR1			0x30c4
+#define STA_CSR1_PHYSICAL_ERROR		FIELD32(0x0000ffff)
+#define STA_CSR1_FALSE_CCA_ERROR	FIELD32(0xffff0000)
+
+/*
+ * STA_CSR2: TX Beacon count and RX FIFO overflow count.
+ */
+#define STA_CSR2			0x30c8
+#define STA_CSR2_RX_FIFO_OVERFLOW_COUNT	FIELD32(0x0000ffff)
+#define STA_CSR2_RX_OVERFLOW_COUNT	FIELD32(0xffff0000)
+
+/*
+ * STA_CSR3: TX Beacon count.
+ */
+#define STA_CSR3			0x30cc
+#define STA_CSR3_TX_BEACON_COUNT	FIELD32(0x0000ffff)
+
+/*
+ * STA_CSR4: TX Retry count.
+ */
+#define STA_CSR4			0x30d0
+#define STA_CSR4_TX_NO_RETRY_COUNT	FIELD32(0x0000ffff)
+#define STA_CSR4_TX_ONE_RETRY_COUNT	FIELD32(0xffff0000)
+
+/*
+ * STA_CSR5: TX Retry count.
+ */
+#define STA_CSR5			0x30d4
+#define STA_CSR4_TX_MULTI_RETRY_COUNT	FIELD32(0x0000ffff)
+#define STA_CSR4_TX_RETRY_FAIL_COUNT	FIELD32(0xffff0000)
+
+/*
+ * QOS control registers.
+ */
+
+/*
+ * QOS_CSR1: TXOP holder MAC address register.
+ */
+#define QOS_CSR1			0x30e4
+#define QOS_CSR1_BYTE4			FIELD32(0x000000ff)
+#define QOS_CSR1_BYTE5			FIELD32(0x0000ff00)
+
+/*
+ * QOS_CSR2: TXOP holder timeout register.
+ */
+#define QOS_CSR2			0x30e8
+
+/*
+ * RX QOS-CFPOLL MAC address register.
+ * QOS_CSR3: RX QOS-CFPOLL MAC address 0.
+ * QOS_CSR4: RX QOS-CFPOLL MAC address 1.
+ */
+#define QOS_CSR3			0x30ec
+#define QOS_CSR4			0x30f0
+
+/*
+ * QOS_CSR5: "QosControl" field of the RX QOS-CFPOLL.
+ */
+#define QOS_CSR5			0x30f4
+
+/*
+ * WMM Scheduler Register
+ */
+
+/*
+ * AIFSN_CSR: AIFSN for each EDCA AC.
+ * AIFSN0: For AC_BK.
+ * AIFSN1: For AC_BE.
+ * AIFSN2: For AC_VI.
+ * AIFSN3: For AC_VO.
+ */
+#define AIFSN_CSR			0x0400
+#define AIFSN_CSR_AIFSN0		FIELD32(0x0000000f)
+#define AIFSN_CSR_AIFSN1		FIELD32(0x000000f0)
+#define AIFSN_CSR_AIFSN2		FIELD32(0x00000f00)
+#define AIFSN_CSR_AIFSN3		FIELD32(0x0000f000)
+
+/*
+ * CWMIN_CSR: CWmin for each EDCA AC.
+ * CWMIN0: For AC_BK.
+ * CWMIN1: For AC_BE.
+ * CWMIN2: For AC_VI.
+ * CWMIN3: For AC_VO.
+ */
+#define CWMIN_CSR			0x0404
+#define CWMIN_CSR_CWMIN0		FIELD32(0x0000000f)
+#define CWMIN_CSR_CWMIN1		FIELD32(0x000000f0)
+#define CWMIN_CSR_CWMIN2		FIELD32(0x00000f00)
+#define CWMIN_CSR_CWMIN3		FIELD32(0x0000f000)
+
+/*
+ * CWMAX_CSR: CWmax for each EDCA AC.
+ * CWMAX0: For AC_BK.
+ * CWMAX1: For AC_BE.
+ * CWMAX2: For AC_VI.
+ * CWMAX3: For AC_VO.
+ */
+#define CWMAX_CSR			0x0408
+#define CWMAX_CSR_CWMAX0		FIELD32(0x0000000f)
+#define CWMAX_CSR_CWMAX1		FIELD32(0x000000f0)
+#define CWMAX_CSR_CWMAX2		FIELD32(0x00000f00)
+#define CWMAX_CSR_CWMAX3		FIELD32(0x0000f000)
+
+/*
+ * AC_TXOP_CSR0: AC_BK/AC_BE TXOP register.
+ * AC0_TX_OP: For AC_BK, in unit of 32us.
+ * AC1_TX_OP: For AC_BE, in unit of 32us.
+ */
+#define AC_TXOP_CSR0			0x040c
+#define AC_TXOP_CSR0_AC0_TX_OP		FIELD32(0x0000ffff)
+#define AC_TXOP_CSR0_AC1_TX_OP		FIELD32(0xffff0000)
+
+/*
+ * AC_TXOP_CSR1: AC_VO/AC_VI TXOP register.
+ * AC2_TX_OP: For AC_VI, in unit of 32us.
+ * AC3_TX_OP: For AC_VO, in unit of 32us.
+ */
+#define AC_TXOP_CSR1			0x0410
+#define AC_TXOP_CSR1_AC2_TX_OP		FIELD32(0x0000ffff)
+#define AC_TXOP_CSR1_AC3_TX_OP		FIELD32(0xffff0000)
+
+/*
+ * BBP registers.
+ * The wordsize of the BBP is 8 bits.
+ */
+
+/*
+ * R2
+ */
+#define BBP_R2_BG_MODE			FIELD8(0x20)
+
+/*
+ * R3
+ */
+#define BBP_R3_SMART_MODE		FIELD8(0x01)
+
+/*
+ * R4: RX antenna control
+ * FRAME_END: 1 - DPDT, 0 - SPDT (Only valid for 802.11G, RF2527 & RF2529)
+ */
+#define BBP_R4_RX_ANTENNA		FIELD8(0x03)
+#define BBP_R4_RX_FRAME_END		FIELD8(0x20)
+
+/*
+ * R77
+ */
+#define BBP_R77_PAIR			FIELD8(0x03)
+
+/*
+ * RF registers
+ */
+
+/*
+ * RF 3
+ */
+#define RF3_TXPOWER			FIELD32(0x00003e00)
+
+/*
+ * RF 4
+ */
+#define RF4_FREQ_OFFSET			FIELD32(0x0003f000)
+
+/*
+ * EEPROM content.
+ * The wordsize of the EEPROM is 16 bits.
+ */
+
+/*
+ * HW MAC address.
+ */
+#define EEPROM_MAC_ADDR_0		0x0002
+#define EEPROM_MAC_ADDR_BYTE0		FIELD16(0x00ff)
+#define EEPROM_MAC_ADDR_BYTE1		FIELD16(0xff00)
+#define EEPROM_MAC_ADDR1		0x0003
+#define EEPROM_MAC_ADDR_BYTE2		FIELD16(0x00ff)
+#define EEPROM_MAC_ADDR_BYTE3		FIELD16(0xff00)
+#define EEPROM_MAC_ADDR_2		0x0004
+#define EEPROM_MAC_ADDR_BYTE4		FIELD16(0x00ff)
+#define EEPROM_MAC_ADDR_BYTE5		FIELD16(0xff00)
+
+/*
+ * EEPROM antenna.
+ * ANTENNA_NUM: Number of antenna's.
+ * TX_DEFAULT: Default antenna 0: diversity, 1: A, 2: B.
+ * RX_DEFAULT: Default antenna 0: diversity, 1: A, 2: B.
+ * FRAME_TYPE: 0: DPDT , 1: SPDT , noted this bit is valid for g only.
+ * DYN_TXAGC: Dynamic TX AGC control.
+ * HARDWARE_RADIO: 1: Hardware controlled radio. Read GPIO0.
+ * RF_TYPE: Rf_type of this adapter.
+ */
+#define EEPROM_ANTENNA			0x0010
+#define EEPROM_ANTENNA_NUM		FIELD16(0x0003)
+#define EEPROM_ANTENNA_TX_DEFAULT	FIELD16(0x000c)
+#define EEPROM_ANTENNA_RX_DEFAULT	FIELD16(0x0030)
+#define EEPROM_ANTENNA_FRAME_TYPE	FIELD16(0x0040)
+#define EEPROM_ANTENNA_DYN_TXAGC	FIELD16(0x0200)
+#define EEPROM_ANTENNA_HARDWARE_RADIO	FIELD16(0x0400)
+#define EEPROM_ANTENNA_RF_TYPE		FIELD16(0xf800)
+
+/*
+ * EEPROM NIC config.
+ * EXTERNAL_LNA: External LNA.
+ */
+#define EEPROM_NIC			0x0011
+#define EEPROM_NIC_EXTERNAL_LNA		FIELD16(0x0010)
+
+/*
+ * EEPROM geography.
+ * GEO_A: Default geographical setting for 5GHz band
+ * GEO: Default geographical setting.
+ */
+#define EEPROM_GEOGRAPHY		0x0012
+#define EEPROM_GEOGRAPHY_GEO_A		FIELD16(0x00ff)
+#define EEPROM_GEOGRAPHY_GEO		FIELD16(0xff00)
+
+/*
+ * EEPROM BBP.
+ */
+#define EEPROM_BBP_START		0x0013
+#define EEPROM_BBP_SIZE			16
+#define EEPROM_BBP_VALUE		FIELD16(0x00ff)
+#define EEPROM_BBP_REG_ID		FIELD16(0xff00)
+
+/*
+ * EEPROM TXPOWER 802.11G
+ */
+#define EEPROM_TXPOWER_G_START		0x0023
+#define EEPROM_TXPOWER_G_SIZE		7
+#define EEPROM_TXPOWER_G_1		FIELD16(0x00ff)
+#define EEPROM_TXPOWER_G_2		FIELD16(0xff00)
+
+/*
+ * EEPROM Frequency
+ */
+#define EEPROM_FREQ			0x002f
+#define EEPROM_FREQ_OFFSET		FIELD16(0x00ff)
+#define EEPROM_FREQ_SEQ_MASK		FIELD16(0xff00)
+#define EEPROM_FREQ_SEQ			FIELD16(0x0300)
+
+/*
+ * EEPROM LED.
+ * POLARITY_RDY_G: Polarity RDY_G setting.
+ * POLARITY_RDY_A: Polarity RDY_A setting.
+ * POLARITY_ACT: Polarity ACT setting.
+ * POLARITY_GPIO_0: Polarity GPIO0 setting.
+ * POLARITY_GPIO_1: Polarity GPIO1 setting.
+ * POLARITY_GPIO_2: Polarity GPIO2 setting.
+ * POLARITY_GPIO_3: Polarity GPIO3 setting.
+ * POLARITY_GPIO_4: Polarity GPIO4 setting.
+ * LED_MODE: Led mode.
+ */
+#define EEPROM_LED			0x0030
+#define EEPROM_LED_POLARITY_RDY_G	FIELD16(0x0001)
+#define EEPROM_LED_POLARITY_RDY_A	FIELD16(0x0002)
+#define EEPROM_LED_POLARITY_ACT		FIELD16(0x0004)
+#define EEPROM_LED_POLARITY_GPIO_0	FIELD16(0x0008)
+#define EEPROM_LED_POLARITY_GPIO_1	FIELD16(0x0010)
+#define EEPROM_LED_POLARITY_GPIO_2	FIELD16(0x0020)
+#define EEPROM_LED_POLARITY_GPIO_3	FIELD16(0x0040)
+#define EEPROM_LED_POLARITY_GPIO_4	FIELD16(0x0080)
+#define EEPROM_LED_LED_MODE		FIELD16(0x1f00)
+
+/*
+ * EEPROM TXPOWER 802.11A
+ */
+#define EEPROM_TXPOWER_A_START		0x0031
+#define EEPROM_TXPOWER_A_SIZE		12
+#define EEPROM_TXPOWER_A_1		FIELD16(0x00ff)
+#define EEPROM_TXPOWER_A_2		FIELD16(0xff00)
+
+/*
+ * EEPROM RSSI offset 802.11BG
+ */
+#define EEPROM_RSSI_OFFSET_BG		0x004d
+#define EEPROM_RSSI_OFFSET_BG_1		FIELD16(0x00ff)
+#define EEPROM_RSSI_OFFSET_BG_2		FIELD16(0xff00)
+
+/*
+ * EEPROM RSSI offset 802.11A
+ */
+#define EEPROM_RSSI_OFFSET_A		0x004e
+#define EEPROM_RSSI_OFFSET_A_1		FIELD16(0x00ff)
+#define EEPROM_RSSI_OFFSET_A_2		FIELD16(0xff00)
+
+/*
+ * DMA descriptor defines.
+ */
+#define TXD_DESC_SIZE			( 6 * sizeof(struct data_desc) )
+#define RXD_DESC_SIZE			( 6 * sizeof(struct data_desc) )
+
+/*
+ * TX descriptor format for TX, PRIO and Beacon Ring.
+ */
+
+/*
+ * Word0
+ * BURST: Next frame belongs to same "burst" event.
+ * TKIP_MIC: ASIC appends TKIP MIC if TKIP is used.
+ * KEY_TABLE: Use per-client pairwise KEY table.
+ * KEY_INDEX:
+ * Key index (0~31) to the pairwise KEY table.
+ * 0~3 to shared KEY table 0 (BSS0).
+ * 4~7 to shared KEY table 1 (BSS1).
+ * 8~11 to shared KEY table 2 (BSS2).
+ * 12~15 to shared KEY table 3 (BSS3).
+ * BURST2: For backward compatibility, set to same value as BURST.
+ */
+#define TXD_W0_BURST			FIELD32(0x00000001)
+#define TXD_W0_VALID			FIELD32(0x00000002)
+#define TXD_W0_MORE_FRAG		FIELD32(0x00000004)
+#define TXD_W0_ACK			FIELD32(0x00000008)
+#define TXD_W0_TIMESTAMP		FIELD32(0x00000010)
+#define TXD_W0_OFDM			FIELD32(0x00000020)
+#define TXD_W0_IFS			FIELD32(0x00000040)
+#define TXD_W0_RETRY_MODE		FIELD32(0x00000080)
+#define TXD_W0_TKIP_MIC			FIELD32(0x00000100)
+#define TXD_W0_KEY_TABLE		FIELD32(0x00000200)
+#define TXD_W0_KEY_INDEX		FIELD32(0x0000fc00)
+#define TXD_W0_DATABYTE_COUNT		FIELD32(0x0fff0000)
+#define TXD_W0_BURST2			FIELD32(0x10000000)
+#define TXD_W0_CIPHER_ALG		FIELD32(0xe0000000)
+
+/*
+ * Word1
+ * HOST_Q_ID: EDCA/HCCA queue ID.
+ * HW_SEQUENCE: MAC overwrites the frame sequence number.
+ * BUFFER_COUNT: Number of buffers in this TXD.
+ */
+#define TXD_W1_HOST_Q_ID		FIELD32(0x0000000f)
+#define TXD_W1_AIFSN			FIELD32(0x000000f0)
+#define TXD_W1_CWMIN			FIELD32(0x00000f00)
+#define TXD_W1_CWMAX			FIELD32(0x0000f000)
+#define TXD_W1_IV_OFFSET		FIELD32(0x003f0000)
+#define TXD_W1_HW_SEQUENCE		FIELD32(0x10000000)
+#define TXD_W1_BUFFER_COUNT		FIELD32(0xe0000000)
+
+/*
+ * Word2: PLCP information
+ */
+#define TXD_W2_PLCP_SIGNAL		FIELD32(0x000000ff)
+#define TXD_W2_PLCP_SERVICE		FIELD32(0x0000ff00)
+#define TXD_W2_PLCP_LENGTH_LOW		FIELD32(0x00ff0000)
+#define TXD_W2_PLCP_LENGTH_HIGH		FIELD32(0xff000000)
+
+/*
+ * Word3
+ */
+#define TXD_W3_IV			FIELD32(0xffffffff)
+
+/*
+ * Word4
+ */
+#define TXD_W4_EIV			FIELD32(0xffffffff)
+
+/*
+ * Word5
+ * FRAME_OFFSET: Frame start offset inside ASIC TXFIFO (after TXINFO field).
+ * PACKET_ID: Driver assigned packet ID to categorize TXResult in interrupt.
+ * WAITING_DMA_DONE_INT: TXD been filled with data
+ * and waiting for TxDoneISR housekeeping.
+ */
+#define TXD_W5_FRAME_OFFSET		FIELD32(0x000000ff)
+#define TXD_W5_PACKET_ID		FIELD32(0x0000ff00)
+#define TXD_W5_TX_POWER			FIELD32(0x00ff0000)
+#define TXD_W5_WAITING_DMA_DONE_INT	FIELD32(0x01000000)
+
+/*
+ * RX descriptor format for RX Ring.
+ */
+
+/*
+ * Word0
+ * CIPHER_ERROR: 1:ICV error, 2:MIC error, 3:invalid key.
+ * KEY_INDEX: Decryption key actually used.
+ */
+#define RXD_W0_OWNER_NIC		FIELD32(0x00000001)
+#define RXD_W0_DROP			FIELD32(0x00000002)
+#define RXD_W0_UNICAST_TO_ME		FIELD32(0x00000004)
+#define RXD_W0_MULTICAST		FIELD32(0x00000008)
+#define RXD_W0_BROADCAST		FIELD32(0x00000010)
+#define RXD_W0_MY_BSS			FIELD32(0x00000020)
+#define RXD_W0_CRC_ERROR		FIELD32(0x00000040)
+#define RXD_W0_OFDM			FIELD32(0x00000080)
+#define RXD_W0_CIPHER_ERROR		FIELD32(0x00000300)
+#define RXD_W0_KEY_INDEX		FIELD32(0x0000fc00)
+#define RXD_W0_DATABYTE_COUNT		FIELD32(0x0fff0000)
+#define RXD_W0_CIPHER_ALG		FIELD32(0xe0000000)
+
+/*
+ * WORD1
+ * SIGNAL: RX raw data rate reported by BBP.
+ * RSSI: RSSI reported by BBP.
+ */
+#define RXD_W1_SIGNAL			FIELD32(0x000000ff)
+#define RXD_W1_RSSI_AGC			FIELD32(0x00001f00)
+#define RXD_W1_RSSI_LNA			FIELD32(0x00006000)
+#define RXD_W1_FRAME_OFFSET		FIELD32(0x7f000000)
+
+/*
+ * Word2
+ * IV: Received IV of originally encrypted.
+ */
+#define RXD_W2_IV			FIELD32(0xffffffff)
+
+/*
+ * Word3
+ * EIV: Received EIV of originally encrypted.
+ */
+#define RXD_W3_EIV			FIELD32(0xffffffff)
+
+/*
+ * Word4
+ */
+#define RXD_W4_RESERVED			FIELD32(0xffffffff)
+
+/*
+ * the above 20-byte is called RXINFO and will be DMAed to MAC RX block
+ * and passed to the HOST driver.
+ * The following fields are for DMA block and HOST usage only.
+ * Can't be touched by ASIC MAC block.
+ */
+
+/*
+ * Word5
+ */
+#define RXD_W5_RESERVED			FIELD32(0xffffffff)
+
+/*
+ * Macro's for converting txpower from EEPROM to dscape value
+ * and from dscape value to register value.
+ */
+#define MIN_TXPOWER	0
+#define MAX_TXPOWER	31
+#define DEFAULT_TXPOWER	24
+
+#define TXPOWER_FROM_DEV(__txpower)		\
+({						\
+	((__txpower) > MAX_TXPOWER) ?		\
+		DEFAULT_TXPOWER : (__txpower);	\
+})
+
+#define TXPOWER_TO_DEV(__txpower)			\
+({							\
+	((__txpower) <= MIN_TXPOWER) ? MIN_TXPOWER :	\
+	(((__txpower) >= MAX_TXPOWER) ? MAX_TXPOWER :	\
+	(__txpower));					\
+})
+
+#endif /* RT73USB_H */
diff --git a/drivers/net/wireless/rtl8187.h b/drivers/net/wireless/rtl8187.h
index 6124e467b156..6ad322ef0da1 100644
--- a/drivers/net/wireless/rtl8187.h
+++ b/drivers/net/wireless/rtl8187.h
@@ -36,8 +36,7 @@ struct rtl8187_rx_info {
 };
 
 struct rtl8187_rx_hdr {
-	__le16 len;
-	__le16 rate;
+	__le32 flags;
 	u8 noise;
 	u8 signal;
 	u8 agc;
@@ -67,13 +66,14 @@ struct rtl8187_priv {
 	struct rtl818x_csr *map;
 	void (*rf_init)(struct ieee80211_hw *);
 	int mode;
+	int if_id;
 
 	/* rtl8187 specific */
 	struct ieee80211_channel channels[14];
 	struct ieee80211_rate rates[12];
 	struct ieee80211_hw_mode modes[2];
 	struct usb_device *udev;
-	u8 *hwaddr;
+	u32 rx_conf;
 	u16 txpwr_base;
 	u8 asic_rev;
 	struct sk_buff_head rx_queue;
diff --git a/drivers/net/wireless/rtl8187_dev.c b/drivers/net/wireless/rtl8187_dev.c
index e61c6d5ba1a9..0ef887dd2867 100644
--- a/drivers/net/wireless/rtl8187_dev.c
+++ b/drivers/net/wireless/rtl8187_dev.c
@@ -36,11 +36,64 @@ static struct usb_device_id rtl8187_table[] __devinitdata = {
 	/* Netgear */
 	{USB_DEVICE(0x0846, 0x6100)},
 	{USB_DEVICE(0x0846, 0x6a00)},
+	/* HP */
+	{USB_DEVICE(0x03f0, 0xca02)},
 	{}
 };
 
 MODULE_DEVICE_TABLE(usb, rtl8187_table);
 
+static void rtl8187_iowrite_async_cb(struct urb *urb)
+{
+	kfree(urb->context);
+	usb_free_urb(urb);
+}
+
+static void rtl8187_iowrite_async(struct rtl8187_priv *priv, __le16 addr,
+				  void *data, u16 len)
+{
+	struct usb_ctrlrequest *dr;
+	struct urb *urb;
+	struct rtl8187_async_write_data {
+		u8 data[4];
+		struct usb_ctrlrequest dr;
+	} *buf;
+
+	buf = kmalloc(sizeof(*buf), GFP_ATOMIC);
+	if (!buf)
+		return;
+
+	urb = usb_alloc_urb(0, GFP_ATOMIC);
+	if (!urb) {
+		kfree(buf);
+		return;
+	}
+
+	dr = &buf->dr;
+
+	dr->bRequestType = RTL8187_REQT_WRITE;
+	dr->bRequest = RTL8187_REQ_SET_REG;
+	dr->wValue = addr;
+	dr->wIndex = 0;
+	dr->wLength = cpu_to_le16(len);
+
+	memcpy(buf, data, len);
+
+	usb_fill_control_urb(urb, priv->udev, usb_sndctrlpipe(priv->udev, 0),
+			     (unsigned char *)dr, buf, len,
+			     rtl8187_iowrite_async_cb, buf);
+	usb_submit_urb(urb, GFP_ATOMIC);
+}
+
+static inline void rtl818x_iowrite32_async(struct rtl8187_priv *priv,
+					   __le32 *addr, u32 val)
+{
+	__le32 buf = cpu_to_le32(val);
+
+	rtl8187_iowrite_async(priv, cpu_to_le16((unsigned long)addr),
+			      &buf, sizeof(buf));
+}
+
 void rtl8187_write_phy(struct ieee80211_hw *dev, u8 addr, u32 data)
 {
 	struct rtl8187_priv *priv = dev->priv;
@@ -96,7 +149,7 @@ static int rtl8187_tx(struct ieee80211_hw *dev, struct sk_buff *skb,
 	if (control->flags & IEEE80211_TXCTL_USE_RTS_CTS) {
 		tmp |= RTL8187_TX_FLAG_RTS;
 		hdr->rts_duration =
-			ieee80211_rts_duration(dev, skb->len, control);
+			ieee80211_rts_duration(dev, priv->if_id, skb->len, control);
 	}
 	if (control->flags & IEEE80211_TXCTL_USE_CTS_PROTECT)
 		tmp |= RTL8187_TX_FLAG_CTS;
@@ -125,6 +178,7 @@ static void rtl8187_rx_cb(struct urb *urb)
 	struct rtl8187_rx_hdr *hdr;
 	struct ieee80211_rx_status rx_status = { 0 };
 	int rate, signal;
+	u32 flags;
 
 	spin_lock(&priv->rx_queue.lock);
 	if (skb->next)
@@ -143,10 +197,11 @@ static void rtl8187_rx_cb(struct urb *urb)
 
 	skb_put(skb, urb->actual_length);
 	hdr = (struct rtl8187_rx_hdr *)(skb_tail_pointer(skb) - sizeof(*hdr));
-	skb_trim(skb, le16_to_cpu(hdr->len) & 0x0FFF);
+	flags = le32_to_cpu(hdr->flags);
+	skb_trim(skb, flags & 0x0FFF);
 
 	signal = hdr->agc >> 1;
-	rate = (le16_to_cpu(hdr->rate) >> 4) & 0xF;
+	rate = (flags >> 20) & 0xF;
 	if (rate > 3) {	/* OFDM rate */
 		if (signal > 90)
 			signal = 90;
@@ -169,6 +224,8 @@ static void rtl8187_rx_cb(struct urb *urb)
 	rx_status.channel = dev->conf.channel;
 	rx_status.phymode = dev->conf.phymode;
 	rx_status.mactime = le64_to_cpu(hdr->mac_time);
+	if (flags & (1 << 13))
+		rx_status.flag |= RX_FLAG_FAILED_FCS_CRC;
 	ieee80211_rx_irqsafe(dev, skb, &rx_status);
 
 	skb = dev_alloc_skb(RTL8187_MAX_RX);
@@ -293,8 +350,6 @@ static int rtl8187_init_hw(struct ieee80211_hw *dev)
 	rtl818x_iowrite8(priv, &priv->map->GP_ENABLE, 0);
 
 	rtl818x_iowrite8(priv, &priv->map->EEPROM_CMD, RTL818X_EEPROM_CMD_CONFIG);
-	for (i = 0; i < ETH_ALEN; i++)
-		rtl818x_iowrite8(priv, &priv->map->MAC[i], priv->hwaddr[i]);
 
 	rtl818x_iowrite16(priv, (__le16 *)0xFFF4, 0xFFFF);
 	reg = rtl818x_ioread8(priv, &priv->map->CONFIG1);
@@ -365,7 +420,7 @@ static void rtl8187_set_channel(struct ieee80211_hw *dev, int channel)
 	rtl818x_iowrite32(priv, &priv->map->TX_CONF, reg);
 }
 
-static int rtl8187_open(struct ieee80211_hw *dev)
+static int rtl8187_start(struct ieee80211_hw *dev)
 {
 	struct rtl8187_priv *priv = dev->priv;
 	u32 reg;
@@ -383,16 +438,13 @@ static int rtl8187_open(struct ieee80211_hw *dev)
 	      RTL818X_RX_CONF_RX_AUTORESETPHY |
 	      RTL818X_RX_CONF_BSSID |
 	      RTL818X_RX_CONF_MGMT |
-	      RTL818X_RX_CONF_CTRL |
 	      RTL818X_RX_CONF_DATA |
 	      (7 << 13 /* RX FIFO threshold NONE */) |
 	      (7 << 10 /* MAX RX DMA */) |
 	      RTL818X_RX_CONF_BROADCAST |
-	      RTL818X_RX_CONF_MULTICAST |
 	      RTL818X_RX_CONF_NICMAC;
-	if (priv->mode == IEEE80211_IF_TYPE_MNTR)
-		reg |= RTL818X_RX_CONF_MONITOR;
 
+	priv->rx_conf = reg;
 	rtl818x_iowrite32(priv, &priv->map->RX_CONF, reg);
 
 	reg = rtl818x_ioread8(priv, &priv->map->CW_CONF);
@@ -419,7 +471,7 @@ static int rtl8187_open(struct ieee80211_hw *dev)
 	return 0;
 }
 
-static int rtl8187_stop(struct ieee80211_hw *dev)
+static void rtl8187_stop(struct ieee80211_hw *dev)
 {
 	struct rtl8187_priv *priv = dev->priv;
 	struct rtl8187_rx_info *info;
@@ -445,28 +497,31 @@ static int rtl8187_stop(struct ieee80211_hw *dev)
 		usb_kill_urb(info->urb);
 		kfree_skb(skb);
 	}
-	return 0;
+	return;
 }
 
 static int rtl8187_add_interface(struct ieee80211_hw *dev,
 				 struct ieee80211_if_init_conf *conf)
 {
 	struct rtl8187_priv *priv = dev->priv;
+	int i;
 
-	/* NOTE: using IEEE80211_IF_TYPE_MGMT to indicate no mode selected */
-	if (priv->mode != IEEE80211_IF_TYPE_MGMT)
-		return -1;
+	if (priv->mode != IEEE80211_IF_TYPE_MNTR)
+		return -EOPNOTSUPP;
 
 	switch (conf->type) {
 	case IEEE80211_IF_TYPE_STA:
-	case IEEE80211_IF_TYPE_MNTR:
 		priv->mode = conf->type;
 		break;
 	default:
 		return -EOPNOTSUPP;
 	}
 
-	priv->hwaddr = conf->mac_addr ? conf->mac_addr : dev->wiphy->perm_addr;
+	rtl818x_iowrite8(priv, &priv->map->EEPROM_CMD, RTL818X_EEPROM_CMD_CONFIG);
+	for (i = 0; i < ETH_ALEN; i++)
+		rtl818x_iowrite8(priv, &priv->map->MAC[i],
+				 ((u8 *)conf->mac_addr)[i]);
+	rtl818x_iowrite8(priv, &priv->map->EEPROM_CMD, RTL818X_EEPROM_CMD_NORMAL);
 
 	return 0;
 }
@@ -475,7 +530,7 @@ static void rtl8187_remove_interface(struct ieee80211_hw *dev,
 				     struct ieee80211_if_init_conf *conf)
 {
 	struct rtl8187_priv *priv = dev->priv;
-	priv->mode = IEEE80211_IF_TYPE_MGMT;
+	priv->mode = IEEE80211_IF_TYPE_MNTR;
 }
 
 static int rtl8187_config(struct ieee80211_hw *dev, struct ieee80211_conf *conf)
@@ -510,6 +565,8 @@ static int rtl8187_config_interface(struct ieee80211_hw *dev, int if_id,
 	struct rtl8187_priv *priv = dev->priv;
 	int i;
 
+	priv->if_id = if_id;
+
 	for (i = 0; i < ETH_ALEN; i++)
 		rtl818x_iowrite8(priv, &priv->map->BSSID[i], conf->bssid[i]);
 
@@ -521,14 +578,52 @@ static int rtl8187_config_interface(struct ieee80211_hw *dev, int if_id,
 	return 0;
 }
 
+static void rtl8187_configure_filter(struct ieee80211_hw *dev,
+				     unsigned int changed_flags,
+				     unsigned int *total_flags,
+				     int mc_count, struct dev_addr_list *mc_list)
+{
+	struct rtl8187_priv *priv = dev->priv;
+
+	*total_flags = 0;
+
+	if (changed_flags & FIF_PROMISC_IN_BSS)
+		priv->rx_conf ^= RTL818X_RX_CONF_NICMAC;
+	if (changed_flags & FIF_ALLMULTI)
+		priv->rx_conf ^= RTL818X_RX_CONF_MULTICAST;
+	if (changed_flags & FIF_FCSFAIL)
+		priv->rx_conf ^= RTL818X_RX_CONF_FCS;
+	if (changed_flags & FIF_CONTROL)
+		priv->rx_conf ^= RTL818X_RX_CONF_CTRL;
+	if (changed_flags & FIF_OTHER_BSS)
+		priv->rx_conf ^= RTL818X_RX_CONF_MONITOR;
+
+	if (mc_count > 0)
+		priv->rx_conf |= RTL818X_RX_CONF_MULTICAST;
+
+	if (priv->rx_conf & RTL818X_RX_CONF_NICMAC)
+		*total_flags |= FIF_PROMISC_IN_BSS;
+	if (priv->rx_conf & RTL818X_RX_CONF_MULTICAST)
+		*total_flags |= FIF_ALLMULTI;
+	if (priv->rx_conf & RTL818X_RX_CONF_FCS)
+		*total_flags |= FIF_FCSFAIL;
+	if (priv->rx_conf & RTL818X_RX_CONF_CTRL)
+		*total_flags |= FIF_CONTROL;
+	if (priv->rx_conf & RTL818X_RX_CONF_MONITOR)
+		*total_flags |= FIF_OTHER_BSS;
+
+	rtl818x_iowrite32_async(priv, &priv->map->RX_CONF, priv->rx_conf);
+}
+
 static const struct ieee80211_ops rtl8187_ops = {
 	.tx			= rtl8187_tx,
-	.open			= rtl8187_open,
+	.start			= rtl8187_start,
 	.stop			= rtl8187_stop,
 	.add_interface		= rtl8187_add_interface,
 	.remove_interface	= rtl8187_remove_interface,
 	.config			= rtl8187_config,
 	.config_interface	= rtl8187_config_interface,
+	.configure_filter	= rtl8187_configure_filter,
 };
 
 static void rtl8187_eeprom_register_read(struct eeprom_93cx6 *eeprom)
@@ -572,6 +667,7 @@ static int __devinit rtl8187_probe(struct usb_interface *intf,
 	struct ieee80211_channel *channel;
 	u16 txpwr, reg;
 	int err, i;
+	DECLARE_MAC_BUF(mac);
 
 	dev = ieee80211_alloc_hw(sizeof(*priv), &rtl8187_ops);
 	if (!dev) {
@@ -601,11 +697,9 @@ static int __devinit rtl8187_probe(struct usb_interface *intf,
 	priv->modes[1].rates = priv->rates;
 	priv->modes[1].num_channels = ARRAY_SIZE(rtl818x_channels);
 	priv->modes[1].channels = priv->channels;
-	priv->mode = IEEE80211_IF_TYPE_MGMT;
+	priv->mode = IEEE80211_IF_TYPE_MNTR;
 	dev->flags = IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING |
-		     IEEE80211_HW_RX_INCLUDES_FCS |
-		     IEEE80211_HW_WEP_INCLUDE_IV |
-		     IEEE80211_HW_DATA_NULLFUNC_ACK;
+		     IEEE80211_HW_RX_INCLUDES_FCS;
 	dev->extra_tx_headroom = sizeof(struct rtl8187_tx_hdr);
 	dev->queues = 1;
 	dev->max_rssi = 65;
@@ -681,8 +775,8 @@ static int __devinit rtl8187_probe(struct usb_interface *intf,
 		goto err_free_dev;
 	}
 
-	printk(KERN_INFO "%s: hwaddr " MAC_FMT ", rtl8187 V%d + %s\n",
-	       wiphy_name(dev->wiphy), MAC_ARG(dev->wiphy->perm_addr),
+	printk(KERN_INFO "%s: hwaddr %s, rtl8187 V%d + %s\n",
+	       wiphy_name(dev->wiphy), print_mac(mac, dev->wiphy->perm_addr),
 	       priv->asic_rev, priv->rf_init == rtl8225_rf_init ?
 	       "rtl8225" : "rtl8225z2");
 
diff --git a/drivers/net/wireless/rtl818x.h b/drivers/net/wireless/rtl818x.h
index 283de30628e1..880d4becae31 100644
--- a/drivers/net/wireless/rtl818x.h
+++ b/drivers/net/wireless/rtl818x.h
@@ -71,6 +71,7 @@ struct rtl818x_csr {
 #define RTL818X_RX_CONF_NICMAC		(1 <<  1)
 #define RTL818X_RX_CONF_MULTICAST	(1 <<  2)
 #define RTL818X_RX_CONF_BROADCAST	(1 <<  3)
+#define RTL818X_RX_CONF_FCS		(1 <<  5)
 #define RTL818X_RX_CONF_DATA		(1 << 18)
 #define RTL818X_RX_CONF_CTRL		(1 << 19)
 #define RTL818X_RX_CONF_MGMT		(1 << 20)
diff --git a/drivers/net/wireless/spectrum_cs.c b/drivers/net/wireless/spectrum_cs.c
index af70460f008a..98df9bc7836a 100644
--- a/drivers/net/wireless/spectrum_cs.c
+++ b/drivers/net/wireless/spectrum_cs.c
@@ -782,7 +782,6 @@ spectrum_cs_config(struct pcmcia_device *link)
 	/* Ok, we have the configuration, prepare to register the netdev */
 	dev->base_addr = link->io.BasePort1;
 	dev->irq = link->irq.AssignedIRQ;
-	SET_MODULE_OWNER(dev);
 	card->node.major = card->node.minor = 0;
 
 	/* Reset card and download firmware */
diff --git a/drivers/net/wireless/strip.c b/drivers/net/wireless/strip.c
index ef32a5c1e818..4bd14b331862 100644
--- a/drivers/net/wireless/strip.c
+++ b/drivers/net/wireless/strip.c
@@ -107,6 +107,7 @@ static const char StripVersion[] = "1.3A-STUART.CHESHIRE";
 #include <linux/serialP.h>
 #include <linux/rcupdate.h>
 #include <net/arp.h>
+#include <net/net_namespace.h>
 
 #include <linux/ip.h>
 #include <linux/tcp.h>
@@ -1630,8 +1631,8 @@ static void strip_IdleTask(unsigned long parameter)
  */
 
 static int strip_header(struct sk_buff *skb, struct net_device *dev,
-			unsigned short type, void *daddr, void *saddr,
-			unsigned len)
+			unsigned short type, const void *daddr,
+			const void *saddr, unsigned len)
 {
 	struct strip *strip_info = netdev_priv(dev);
 	STRIP_Header *header = (STRIP_Header *) skb_push(skb, sizeof(STRIP_Header));
@@ -1971,7 +1972,7 @@ static struct net_device *get_strip_dev(struct strip *strip_info)
 		      sizeof(zero_address))) {
 		struct net_device *dev;
 		read_lock_bh(&dev_base_lock);
-		for_each_netdev(dev) {
+		for_each_netdev(&init_net, dev) {
 			if (dev->type == strip_info->dev->type &&
 			    !memcmp(dev->dev_addr,
 				    &strip_info->true_dev_addr,
@@ -2496,6 +2497,11 @@ static int strip_close_low(struct net_device *dev)
 	return 0;
 }
 
+static const struct header_ops strip_header_ops = {
+	.create = strip_header,
+	.rebuild = strip_rebuild_header,
+};
+
 /*
  * This routine is called by DDI when the
  * (dynamically assigned) device is registered
@@ -2507,8 +2513,6 @@ static void strip_dev_setup(struct net_device *dev)
 	 * Finish setting up the DEVICE info.
 	 */
 
-	SET_MODULE_OWNER(dev);
-
 	dev->trans_start = 0;
 	dev->last_rx = 0;
 	dev->tx_queue_len = 30;	/* Drop after 30 frames queued */
@@ -2532,8 +2536,8 @@ static void strip_dev_setup(struct net_device *dev)
 	dev->open = strip_open_low;
 	dev->stop = strip_close_low;
 	dev->hard_start_xmit = strip_xmit;
-	dev->hard_header = strip_header;
-	dev->rebuild_header = strip_rebuild_header;
+	dev->header_ops = &strip_header_ops;
+
 	dev->set_mac_address = strip_set_mac_address;
 	dev->get_stats = strip_get_stats;
 	dev->change_mtu = strip_change_mtu;
@@ -2571,7 +2575,7 @@ static struct strip *strip_alloc(void)
 		return NULL;	/* If no more memory, return */
 
 
-	strip_info = dev->priv;
+	strip_info = netdev_priv(dev);
 	strip_info->dev = dev;
 
 	strip_info->magic = STRIP_MAGIC;
@@ -2787,7 +2791,7 @@ static int __init strip_init_driver(void)
 	/*
 	 * Register the status file with /proc
 	 */
-	proc_net_fops_create("strip", S_IFREG | S_IRUGO, &strip_seq_fops);
+	proc_net_fops_create(&init_net, "strip", S_IFREG | S_IRUGO, &strip_seq_fops);
 
 	return status;
 }
@@ -2809,7 +2813,7 @@ static void __exit strip_exit_driver(void)
 	}
 
 	/* Unregister with the /proc/net file here. */
-	proc_net_remove("strip");
+	proc_net_remove(&init_net, "strip");
 
 	if ((i = tty_unregister_ldisc(N_STRIP)))
 		printk(KERN_ERR "STRIP: can't unregister line discipline (err = %d)\n", i);
diff --git a/drivers/net/wireless/wavelan.c b/drivers/net/wireless/wavelan.c
index 1cf090d60edc..a1f8a1687842 100644
--- a/drivers/net/wireless/wavelan.c
+++ b/drivers/net/wireless/wavelan.c
@@ -880,6 +880,8 @@ static void wv_82586_reconfig(struct net_device * dev)
  */
 static void wv_psa_show(psa_t * p)
 {
+	DECLARE_MAC_BUF(mac);
+
 	printk(KERN_DEBUG "##### WaveLAN PSA contents: #####\n");
 	printk(KERN_DEBUG "psa_io_base_addr_1: 0x%02X %02X %02X %02X\n",
 	       p->psa_io_base_addr_1,
@@ -891,22 +893,13 @@ static void wv_psa_show(psa_t * p)
 	printk(KERN_DEBUG "psa_holi_params: 0x%02x, ", p->psa_holi_params);
 	printk("psa_int_req_no: %d\n", p->psa_int_req_no);
 #ifdef DEBUG_SHOW_UNUSED
-	printk(KERN_DEBUG
-	       "psa_unused0[]: %02X:%02X:%02X:%02X:%02X:%02X:%02X\n",
-	       p->psa_unused0[0], p->psa_unused0[1], p->psa_unused0[2],
-	       p->psa_unused0[3], p->psa_unused0[4], p->psa_unused0[5],
-	       p->psa_unused0[6]);
+	printk(KERN_DEBUG "psa_unused0[]: %s\n",
+	       print_mac(mac, p->psa_unused0));
 #endif				/* DEBUG_SHOW_UNUSED */
-	printk(KERN_DEBUG
-	       "psa_univ_mac_addr[]: %02x:%02x:%02x:%02x:%02x:%02x\n",
-	       p->psa_univ_mac_addr[0], p->psa_univ_mac_addr[1],
-	       p->psa_univ_mac_addr[2], p->psa_univ_mac_addr[3],
-	       p->psa_univ_mac_addr[4], p->psa_univ_mac_addr[5]);
-	printk(KERN_DEBUG
-	       "psa_local_mac_addr[]: %02x:%02x:%02x:%02x:%02x:%02x\n",
-	       p->psa_local_mac_addr[0], p->psa_local_mac_addr[1],
-	       p->psa_local_mac_addr[2], p->psa_local_mac_addr[3],
-	       p->psa_local_mac_addr[4], p->psa_local_mac_addr[5]);
+	printk(KERN_DEBUG "psa_univ_mac_addr[]: %s\n",
+	       print_mac(mac, p->psa_univ_mac_addr));
+	printk(KERN_DEBUG "psa_local_mac_addr[]: %s\n",
+	       print_mac(mac, p->psa_local_mac_addr));
 	printk(KERN_DEBUG "psa_univ_local_sel: %d, ",
 	       p->psa_univ_local_sel);
 	printk("psa_comp_number: %d, ", p->psa_comp_number);
@@ -1248,14 +1241,14 @@ static inline void wv_packet_info(u8 * p,	/* Packet to dump */
 {				/* Name of the function */
 	int i;
 	int maxi;
+	DECLARE_MAC_BUF(mac);
 
 	printk(KERN_DEBUG
-	       "%s: %s(): dest %02X:%02X:%02X:%02X:%02X:%02X, length %d\n",
-	       msg1, msg2, p[0], p[1], p[2], p[3], p[4], p[5], length);
+	       "%s: %s(): dest %s, length %d\n",
+	       msg1, msg2, print_mac(mac, p), length);
 	printk(KERN_DEBUG
-	       "%s: %s(): src %02X:%02X:%02X:%02X:%02X:%02X, type 0x%02X%02X\n",
-	       msg1, msg2, p[6], p[7], p[8], p[9], p[10], p[11], p[12],
-	       p[13]);
+	       "%s: %s(): src %s, type 0x%02X%02X\n",
+	       msg1, msg2, print_mac(mac, &p[6]), p[12], p[13]);
 
 #ifdef DEBUG_PACKET_DUMP
 
@@ -1286,7 +1279,9 @@ static void wv_init_info(struct net_device * dev)
 	short ioaddr = dev->base_addr;
 	net_local *lp = (net_local *) dev->priv;
 	psa_t psa;
-	int i;
+#ifdef DEBUG_BASIC_SHOW
+	DECLARE_MAC_BUF(mac);
+#endif
 
 	/* Read the parameter storage area */
 	psa_read(ioaddr, lp->hacr, 0, (unsigned char *) &psa, sizeof(psa));
@@ -1303,10 +1298,8 @@ static void wv_init_info(struct net_device * dev)
 
 #ifdef DEBUG_BASIC_SHOW
 	/* Now, let's go for the basic stuff. */
-	printk(KERN_NOTICE "%s: WaveLAN at %#x,", dev->name, ioaddr);
-	for (i = 0; i < WAVELAN_ADDR_SIZE; i++)
-		printk("%s%02X", (i == 0) ? " " : ":", dev->dev_addr[i]);
-	printk(", IRQ %d", dev->irq);
+	printk(KERN_NOTICE "%s: WaveLAN at %#x, %s, IRQ %d",
+	       dev->name, ioaddr, print_mac(mac, dev->dev_addr), dev->irq);
 
 	/* Print current network ID. */
 	if (psa.psa_nwid_select)
@@ -2400,9 +2393,9 @@ static const struct iw_priv_args wavelan_private_args[] = {
 
 static const struct iw_handler_def	wavelan_handler_def =
 {
-	.num_standard	= sizeof(wavelan_handler)/sizeof(iw_handler),
-	.num_private	= sizeof(wavelan_private_handler)/sizeof(iw_handler),
-	.num_private_args = sizeof(wavelan_private_args)/sizeof(struct iw_priv_args),
+	.num_standard	= ARRAY_SIZE(wavelan_handler),
+	.num_private	= ARRAY_SIZE(wavelan_private_handler),
+	.num_private_args = ARRAY_SIZE(wavelan_private_args),
 	.standard	= wavelan_handler,
 	.private	= wavelan_private_handler,
 	.private_args	= wavelan_private_args,
@@ -3596,15 +3589,15 @@ static void wv_82586_config(struct net_device * dev)
 			      WAVELAN_ADDR_SIZE >> 1);
 
 #ifdef DEBUG_CONFIG_INFO
+ {
+		DECLARE_MAC_BUF(mac);
 		printk(KERN_DEBUG
 		       "%s: wv_82586_config(): set %d multicast addresses:\n",
 		       dev->name, lp->mc_count);
 		for (dmi = dev->mc_list; dmi; dmi = dmi->next)
-			printk(KERN_DEBUG
-			       " %02x:%02x:%02x:%02x:%02x:%02x\n",
-			       dmi->dmi_addr[0], dmi->dmi_addr[1],
-			       dmi->dmi_addr[2], dmi->dmi_addr[3],
-			       dmi->dmi_addr[4], dmi->dmi_addr[5]);
+			printk(KERN_DEBUG " %s\n",
+			       print_mac(mac, dmi->dmi_addr));
+ }
 #endif
 	}
 
@@ -4177,7 +4170,6 @@ static int __init wavelan_config(struct net_device *dev, unsigned short ioaddr)
 	/* Init spinlock */
 	spin_lock_init(&lp->spinlock);
 
-	SET_MODULE_OWNER(dev);
 	dev->open = wavelan_open;
 	dev->stop = wavelan_close;
 	dev->hard_start_xmit = wavelan_packet_xmit;
diff --git a/drivers/net/wireless/wavelan_cs.c b/drivers/net/wireless/wavelan_cs.c
index 5740d4d4267c..577c647824fe 100644
--- a/drivers/net/wireless/wavelan_cs.c
+++ b/drivers/net/wireless/wavelan_cs.c
@@ -1042,6 +1042,7 @@ wv_82593_reconfig(struct net_device *	dev)
 static void
 wv_psa_show(psa_t *	p)
 {
+  DECLARE_MAC_BUF(mac);
   printk(KERN_DEBUG "##### wavelan psa contents: #####\n");
   printk(KERN_DEBUG "psa_io_base_addr_1: 0x%02X %02X %02X %02X\n",
 	 p->psa_io_base_addr_1,
@@ -1055,29 +1056,13 @@ wv_psa_show(psa_t *	p)
   printk(KERN_DEBUG "psa_holi_params: 0x%02x, ", p->psa_holi_params);
   printk("psa_int_req_no: %d\n", p->psa_int_req_no);
 #ifdef DEBUG_SHOW_UNUSED
-  printk(KERN_DEBUG "psa_unused0[]: %02X:%02X:%02X:%02X:%02X:%02X:%02X\n",
-	 p->psa_unused0[0],
-	 p->psa_unused0[1],
-	 p->psa_unused0[2],
-	 p->psa_unused0[3],
-	 p->psa_unused0[4],
-	 p->psa_unused0[5],
-	 p->psa_unused0[6]);
+  printk(KERN_DEBUG "psa_unused0[]: %s\n",
+	 print_mac(mac, p->psa_unused0));
 #endif	/* DEBUG_SHOW_UNUSED */
-  printk(KERN_DEBUG "psa_univ_mac_addr[]: %02x:%02x:%02x:%02x:%02x:%02x\n",
-	 p->psa_univ_mac_addr[0],
-	 p->psa_univ_mac_addr[1],
-	 p->psa_univ_mac_addr[2],
-	 p->psa_univ_mac_addr[3],
-	 p->psa_univ_mac_addr[4],
-	 p->psa_univ_mac_addr[5]);
-  printk(KERN_DEBUG "psa_local_mac_addr[]: %02x:%02x:%02x:%02x:%02x:%02x\n",
-	 p->psa_local_mac_addr[0],
-	 p->psa_local_mac_addr[1],
-	 p->psa_local_mac_addr[2],
-	 p->psa_local_mac_addr[3],
-	 p->psa_local_mac_addr[4],
-	 p->psa_local_mac_addr[5]);
+  printk(KERN_DEBUG "psa_univ_mac_addr[]: %s\n",
+	 print_mac(mac, p->psa_univ_mac_addr));
+  printk(KERN_DEBUG "psa_local_mac_addr[]: %s\n",
+	 print_mac(mac, p->psa_local_mac_addr));
   printk(KERN_DEBUG "psa_univ_local_sel: %d, ", p->psa_univ_local_sel);
   printk("psa_comp_number: %d, ", p->psa_comp_number);
   printk("psa_thr_pre_set: 0x%02x\n", p->psa_thr_pre_set);
@@ -1277,11 +1262,12 @@ wv_packet_info(u_char *		p,		/* Packet to dump */
 {
   int		i;
   int		maxi;
+  DECLARE_MAC_BUF(mac);
 
-  printk(KERN_DEBUG "%s: %s(): dest %02X:%02X:%02X:%02X:%02X:%02X, length %d\n",
-	 msg1, msg2, p[0], p[1], p[2], p[3], p[4], p[5], length);
-  printk(KERN_DEBUG "%s: %s(): src %02X:%02X:%02X:%02X:%02X:%02X, type 0x%02X%02X\n",
-	 msg1, msg2, p[6], p[7], p[8], p[9], p[10], p[11], p[12], p[13]);
+  printk(KERN_DEBUG "%s: %s(): dest %s, length %d\n",
+	 msg1, msg2, print_mac(mac, p), length);
+  printk(KERN_DEBUG "%s: %s(): src %s, type 0x%02X%02X\n",
+	 msg1, msg2, print_mac(mac, &p[6]), p[12], p[13]);
 
 #ifdef DEBUG_PACKET_DUMP
 
@@ -1312,7 +1298,7 @@ wv_init_info(struct net_device *	dev)
 {
   kio_addr_t	base = dev->base_addr;
   psa_t		psa;
-  int		i;
+  DECLARE_MAC_BUF(mac);
 
   /* Read the parameter storage area */
   psa_read(dev, 0, (unsigned char *) &psa, sizeof(psa));
@@ -1329,10 +1315,10 @@ wv_init_info(struct net_device *	dev)
 
 #ifdef DEBUG_BASIC_SHOW
   /* Now, let's go for the basic stuff */
-  printk(KERN_NOTICE "%s: WaveLAN: port %#lx, irq %d, hw_addr",
-	 dev->name, base, dev->irq);
-  for(i = 0; i < WAVELAN_ADDR_SIZE; i++)
-    printk("%s%02X", (i == 0) ? " " : ":", dev->dev_addr[i]);
+  printk(KERN_NOTICE "%s: WaveLAN: port %#lx, irq %d, "
+	 "hw_addr %s",
+	 dev->name, base, dev->irq,
+	 print_mac(mac, dev->dev_addr));
 
   /* Print current network id */
   if(psa.psa_nwid_select)
@@ -2719,9 +2705,9 @@ static const iw_handler		wavelan_private_handler[] =
 
 static const struct iw_handler_def	wavelan_handler_def =
 {
-	.num_standard	= sizeof(wavelan_handler)/sizeof(iw_handler),
-	.num_private	= sizeof(wavelan_private_handler)/sizeof(iw_handler),
-	.num_private_args = sizeof(wavelan_private_args)/sizeof(struct iw_priv_args),
+	.num_standard	= ARRAY_SIZE(wavelan_handler),
+	.num_private	= ARRAY_SIZE(wavelan_private_handler),
+	.num_private_args = ARRAY_SIZE(wavelan_private_args),
 	.standard	= wavelan_handler,
 	.private	= wavelan_private_handler,
 	.private_args	= wavelan_private_args,
@@ -3691,12 +3677,12 @@ wv_82593_config(struct net_device *	dev)
       int			addrs_len = WAVELAN_ADDR_SIZE * lp->mc_count;
 
 #ifdef DEBUG_CONFIG_INFO
+      DECLARE_MAC_BUF(mac);
       printk(KERN_DEBUG "%s: wv_hw_config(): set %d multicast addresses:\n",
 	     dev->name, lp->mc_count);
       for(dmi=dev->mc_list; dmi; dmi=dmi->next)
-	printk(KERN_DEBUG " %02x:%02x:%02x:%02x:%02x:%02x\n",
-	       dmi->dmi_addr[0], dmi->dmi_addr[1], dmi->dmi_addr[2],
-	       dmi->dmi_addr[3], dmi->dmi_addr[4], dmi->dmi_addr[5] );
+	printk(KERN_DEBUG " %s\n",
+	       print_mac(mac, dmi->dmi_addr));
 #endif
 
       /* Initialize adapter's ethernet multicast addresses */
@@ -4577,7 +4563,6 @@ wavelan_probe(struct pcmcia_device *p_dev)
   lp->dev = dev;
 
   /* wavelan NET3 callbacks */
-  SET_MODULE_OWNER(dev);
   dev->open = &wavelan_open;
   dev->stop = &wavelan_close;
   dev->hard_start_xmit = &wavelan_packet_xmit;
diff --git a/drivers/net/wireless/wl3501_cs.c b/drivers/net/wireless/wl3501_cs.c
index c8b5c2271938..42a36b3f3ff7 100644
--- a/drivers/net/wireless/wl3501_cs.c
+++ b/drivers/net/wireless/wl3501_cs.c
@@ -859,12 +859,11 @@ static int wl3501_esbq_confirm(struct wl3501_card *this)
 
 static void wl3501_online(struct net_device *dev)
 {
-	struct wl3501_card *this = dev->priv;
+	struct wl3501_card *this = netdev_priv(dev);
+	DECLARE_MAC_BUF(mac);
 
-	printk(KERN_INFO "%s: Wireless LAN online. BSSID: "
-	       "%02X %02X %02X %02X %02X %02X\n", dev->name,
-	       this->bssid[0], this->bssid[1], this->bssid[2],
-	       this->bssid[3], this->bssid[4], this->bssid[5]);
+	printk(KERN_INFO "%s: Wireless LAN online. BSSID: %s\n",
+	       dev->name, print_mac(mac, this->bssid));
 	netif_wake_queue(dev);
 }
 
@@ -907,7 +906,7 @@ static int wl3501_mgmt_association(struct wl3501_card *this)
 
 static void wl3501_mgmt_join_confirm(struct net_device *dev, u16 addr)
 {
-	struct wl3501_card *this = dev->priv;
+	struct wl3501_card *this = netdev_priv(dev);
 	struct wl3501_join_confirm sig;
 
 	dprintk(3, "entry");
@@ -1046,7 +1045,7 @@ static inline void wl3501_start_confirm_interrupt(struct net_device *dev,
 static inline void wl3501_assoc_confirm_interrupt(struct net_device *dev,
 						  u16 addr)
 {
-	struct wl3501_card *this = dev->priv;
+	struct wl3501_card *this = netdev_priv(dev);
 	struct wl3501_assoc_confirm sig;
 
 	dprintk(3, "entry");
@@ -1075,7 +1074,7 @@ static inline void wl3501_rx_interrupt(struct net_device *dev)
 	int morepkts;
 	u16 addr;
 	u8 sig_id;
-	struct wl3501_card *this = dev->priv;
+	struct wl3501_card *this = netdev_priv(dev);
 
 	dprintk(3, "entry");
 loop:
@@ -1257,7 +1256,7 @@ fail:
 
 static int wl3501_close(struct net_device *dev)
 {
-	struct wl3501_card *this = dev->priv;
+	struct wl3501_card *this = netdev_priv(dev);
 	int rc = -ENODEV;
 	unsigned long flags;
 	struct pcmcia_device *link;
@@ -1289,7 +1288,7 @@ static int wl3501_close(struct net_device *dev)
  */
 static int wl3501_reset(struct net_device *dev)
 {
-	struct wl3501_card *this = dev->priv;
+	struct wl3501_card *this = netdev_priv(dev);
 	int rc = -ENODEV;
 
 	wl3501_block_interrupt(this);
@@ -1318,7 +1317,7 @@ out:
 
 static void wl3501_tx_timeout(struct net_device *dev)
 {
-	struct wl3501_card *this = dev->priv;
+	struct wl3501_card *this = netdev_priv(dev);
 	struct net_device_stats *stats = &this->stats;
 	unsigned long flags;
 	int rc;
@@ -1344,7 +1343,7 @@ static void wl3501_tx_timeout(struct net_device *dev)
 static int wl3501_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
 {
 	int enabled, rc;
-	struct wl3501_card *this = dev->priv;
+	struct wl3501_card *this = netdev_priv(dev);
 	unsigned long flags;
 
 	spin_lock_irqsave(&this->lock, flags);
@@ -1371,7 +1370,7 @@ static int wl3501_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
 static int wl3501_open(struct net_device *dev)
 {
 	int rc = -ENODEV;
-	struct wl3501_card *this = dev->priv;
+	struct wl3501_card *this = netdev_priv(dev);
 	unsigned long flags;
 	struct pcmcia_device *link;
 	link = this->p_dev;
@@ -1410,14 +1409,14 @@ fail:
 
 static struct net_device_stats *wl3501_get_stats(struct net_device *dev)
 {
-	struct wl3501_card *this = dev->priv;
+	struct wl3501_card *this = netdev_priv(dev);
 
 	return &this->stats;
 }
 
 static struct iw_statistics *wl3501_get_wireless_stats(struct net_device *dev)
 {
-	struct wl3501_card *this = dev->priv;
+	struct wl3501_card *this = netdev_priv(dev);
 	struct iw_statistics *wstats = &this->wstats;
 	u32 value; /* size checked: it is u32 */
 
@@ -1497,7 +1496,7 @@ static int wl3501_get_name(struct net_device *dev, struct iw_request_info *info,
 static int wl3501_set_freq(struct net_device *dev, struct iw_request_info *info,
 			   union iwreq_data *wrqu, char *extra)
 {
-	struct wl3501_card *this = dev->priv;
+	struct wl3501_card *this = netdev_priv(dev);
 	int channel = wrqu->freq.m;
 	int rc = -EINVAL;
 
@@ -1511,7 +1510,7 @@ static int wl3501_set_freq(struct net_device *dev, struct iw_request_info *info,
 static int wl3501_get_freq(struct net_device *dev, struct iw_request_info *info,
 			   union iwreq_data *wrqu, char *extra)
 {
-	struct wl3501_card *this = dev->priv;
+	struct wl3501_card *this = netdev_priv(dev);
 
 	wrqu->freq.m = wl3501_chan2freq[this->chan - 1] * 100000;
 	wrqu->freq.e = 1;
@@ -1526,7 +1525,7 @@ static int wl3501_set_mode(struct net_device *dev, struct iw_request_info *info,
 	if (wrqu->mode == IW_MODE_INFRA ||
 	    wrqu->mode == IW_MODE_ADHOC ||
 	    wrqu->mode == IW_MODE_AUTO) {
-		struct wl3501_card *this = dev->priv;
+		struct wl3501_card *this = netdev_priv(dev);
 
 		this->net_type = wrqu->mode;
 		rc = wl3501_reset(dev);
@@ -1537,7 +1536,7 @@ static int wl3501_set_mode(struct net_device *dev, struct iw_request_info *info,
 static int wl3501_get_mode(struct net_device *dev, struct iw_request_info *info,
 			   union iwreq_data *wrqu, char *extra)
 {
-	struct wl3501_card *this = dev->priv;
+	struct wl3501_card *this = netdev_priv(dev);
 
 	wrqu->mode = this->net_type;
 	return 0;
@@ -1546,7 +1545,7 @@ static int wl3501_get_mode(struct net_device *dev, struct iw_request_info *info,
 static int wl3501_get_sens(struct net_device *dev, struct iw_request_info *info,
 			   union iwreq_data *wrqu, char *extra)
 {
-	struct wl3501_card *this = dev->priv;
+	struct wl3501_card *this = netdev_priv(dev);
 
 	wrqu->sens.value = this->rssi;
 	wrqu->sens.disabled = !wrqu->sens.value;
@@ -1577,7 +1576,7 @@ static int wl3501_get_range(struct net_device *dev,
 static int wl3501_set_wap(struct net_device *dev, struct iw_request_info *info,
 			  union iwreq_data *wrqu, char *extra)
 {
-	struct wl3501_card *this = dev->priv;
+	struct wl3501_card *this = netdev_priv(dev);
 	static const u8 bcast[ETH_ALEN] = { 255, 255, 255, 255, 255, 255 };
 	int rc = -EINVAL;
 
@@ -1597,7 +1596,7 @@ out:
 static int wl3501_get_wap(struct net_device *dev, struct iw_request_info *info,
 			  union iwreq_data *wrqu, char *extra)
 {
-	struct wl3501_card *this = dev->priv;
+	struct wl3501_card *this = netdev_priv(dev);
 
 	wrqu->ap_addr.sa_family = ARPHRD_ETHER;
 	memcpy(wrqu->ap_addr.sa_data, this->bssid, ETH_ALEN);
@@ -1616,7 +1615,7 @@ static int wl3501_set_scan(struct net_device *dev, struct iw_request_info *info,
 static int wl3501_get_scan(struct net_device *dev, struct iw_request_info *info,
 			   union iwreq_data *wrqu, char *extra)
 {
-	struct wl3501_card *this = dev->priv;
+	struct wl3501_card *this = netdev_priv(dev);
 	int i;
 	char *current_ev = extra;
 	struct iw_event iwe;
@@ -1666,7 +1665,7 @@ static int wl3501_set_essid(struct net_device *dev,
 			    struct iw_request_info *info,
 			    union iwreq_data *wrqu, char *extra)
 {
-	struct wl3501_card *this = dev->priv;
+	struct wl3501_card *this = netdev_priv(dev);
 
 	if (wrqu->data.flags) {
 		iw_set_mgmt_info_element(IW_MGMT_INFO_ELEMENT_SSID,
@@ -1683,7 +1682,7 @@ static int wl3501_get_essid(struct net_device *dev,
 			    struct iw_request_info *info,
 			    union iwreq_data *wrqu, char *extra)
 {
-	struct wl3501_card *this = dev->priv;
+	struct wl3501_card *this = netdev_priv(dev);
 	unsigned long flags;
 
 	spin_lock_irqsave(&this->lock, flags);
@@ -1697,7 +1696,7 @@ static int wl3501_get_essid(struct net_device *dev,
 static int wl3501_set_nick(struct net_device *dev, struct iw_request_info *info,
 			   union iwreq_data *wrqu, char *extra)
 {
-	struct wl3501_card *this = dev->priv;
+	struct wl3501_card *this = netdev_priv(dev);
 
 	if (wrqu->data.length > sizeof(this->nick))
 		return -E2BIG;
@@ -1708,7 +1707,7 @@ static int wl3501_set_nick(struct net_device *dev, struct iw_request_info *info,
 static int wl3501_get_nick(struct net_device *dev, struct iw_request_info *info,
 			   union iwreq_data *wrqu, char *extra)
 {
-	struct wl3501_card *this = dev->priv;
+	struct wl3501_card *this = netdev_priv(dev);
 
 	strlcpy(extra, this->nick, 32);
 	wrqu->data.length = strlen(extra);
@@ -1733,7 +1732,7 @@ static int wl3501_get_rts_threshold(struct net_device *dev,
 				    union iwreq_data *wrqu, char *extra)
 {
 	u16 threshold; /* size checked: it is u16 */
-	struct wl3501_card *this = dev->priv;
+	struct wl3501_card *this = netdev_priv(dev);
 	int rc = wl3501_get_mib_value(this, WL3501_MIB_ATTR_RTS_THRESHOLD,
 				      &threshold, sizeof(threshold));
 	if (!rc) {
@@ -1749,7 +1748,7 @@ static int wl3501_get_frag_threshold(struct net_device *dev,
 				     union iwreq_data *wrqu, char *extra)
 {
 	u16 threshold; /* size checked: it is u16 */
-	struct wl3501_card *this = dev->priv;
+	struct wl3501_card *this = netdev_priv(dev);
 	int rc = wl3501_get_mib_value(this, WL3501_MIB_ATTR_FRAG_THRESHOLD,
 				      &threshold, sizeof(threshold));
 	if (!rc) {
@@ -1765,7 +1764,7 @@ static int wl3501_get_txpow(struct net_device *dev,
 			    union iwreq_data *wrqu, char *extra)
 {
 	u16 txpow;
-	struct wl3501_card *this = dev->priv;
+	struct wl3501_card *this = netdev_priv(dev);
 	int rc = wl3501_get_mib_value(this,
 				      WL3501_MIB_ATTR_CURRENT_TX_PWR_LEVEL,
 				      &txpow, sizeof(txpow));
@@ -1787,7 +1786,7 @@ static int wl3501_get_retry(struct net_device *dev,
 			    union iwreq_data *wrqu, char *extra)
 {
 	u8 retry; /* size checked: it is u8 */
-	struct wl3501_card *this = dev->priv;
+	struct wl3501_card *this = netdev_priv(dev);
 	int rc = wl3501_get_mib_value(this,
 				      WL3501_MIB_ATTR_LONG_RETRY_LIMIT,
 				      &retry, sizeof(retry));
@@ -1814,7 +1813,7 @@ static int wl3501_get_encode(struct net_device *dev,
 			     union iwreq_data *wrqu, char *extra)
 {
 	u8 implemented, restricted, keys[100], len_keys, tocopy;
-	struct wl3501_card *this = dev->priv;
+	struct wl3501_card *this = netdev_priv(dev);
 	int rc = wl3501_get_mib_value(this,
 				      WL3501_MIB_ATTR_PRIV_OPT_IMPLEMENTED,
 				      &implemented, sizeof(implemented));
@@ -1841,7 +1840,6 @@ static int wl3501_get_encode(struct net_device *dev,
 	tocopy = min_t(u8, len_keys, wrqu->encoding.length);
 	tocopy = min_t(u8, tocopy, 100);
 	wrqu->encoding.length = tocopy;
-	memset(extra, 0, tocopy);
 	memcpy(extra, keys, tocopy);
 out:
 	return rc;
@@ -1852,7 +1850,7 @@ static int wl3501_get_power(struct net_device *dev,
 			    union iwreq_data *wrqu, char *extra)
 {
 	u8 pwr_state;
-	struct wl3501_card *this = dev->priv;
+	struct wl3501_card *this = netdev_priv(dev);
 	int rc = wl3501_get_mib_value(this,
 				      WL3501_MIB_ATTR_CURRENT_PWR_STATE,
 				      &pwr_state, sizeof(pwr_state));
@@ -1894,7 +1892,7 @@ static const iw_handler	wl3501_handler[] = {
 };
 
 static const struct iw_handler_def wl3501_handler_def = {
-	.num_standard	= sizeof(wl3501_handler) / sizeof(iw_handler),
+	.num_standard	= ARRAY_SIZE(wl3501_handler),
 	.standard	= (iw_handler *)wl3501_handler,
 	.get_wireless_stats = wl3501_get_wireless_stats,
 };
@@ -1937,7 +1935,7 @@ static int wl3501_probe(struct pcmcia_device *p_dev)
 	dev->tx_timeout		= wl3501_tx_timeout;
 	dev->watchdog_timeo	= 5 * HZ;
 	dev->get_stats		= wl3501_get_stats;
-	this = dev->priv;
+	this = netdev_priv(dev);
 	this->wireless_data.spy_data = &this->spy_data;
 	this->p_dev = p_dev;
 	dev->wireless_data	= &this->wireless_data;
@@ -1967,6 +1965,7 @@ static int wl3501_config(struct pcmcia_device *link)
 	struct net_device *dev = link->priv;
 	int i = 0, j, last_fn, last_ret;
 	struct wl3501_card *this;
+	DECLARE_MAC_BUF(mac);
 
 	/* Try allocating IO ports.  This tries a few fixed addresses.  If you
 	 * want, you can also read the card's config table to pick addresses --
@@ -2004,9 +2003,7 @@ static int wl3501_config(struct pcmcia_device *link)
 		goto failed;
 	}
 
-	SET_MODULE_OWNER(dev);
-
-	this = dev->priv;
+	this = netdev_priv(dev);
 	/*
 	 * At this point, the dev_node_t structure(s) should be initialized and
 	 * arranged in a linked list at link->dev_node.
@@ -2022,14 +2019,14 @@ static int wl3501_config(struct pcmcia_device *link)
 	}
 	strcpy(this->node.dev_name, dev->name);
 
-	/* print probe information */
-	printk(KERN_INFO "%s: wl3501 @ 0x%3.3x, IRQ %d, MAC addr in flash ROM:",
-	       dev->name, this->base_addr, (int)dev->irq);
-	for (i = 0; i < 6; i++) {
+	for (i = 0; i < 6; i++)
 		dev->dev_addr[i] = ((char *)&this->mac_addr)[i];
-		printk("%c%02x", i ? ':' : ' ', dev->dev_addr[i]);
-	}
-	printk("\n");
+
+	/* print probe information */
+	printk(KERN_INFO "%s: wl3501 @ 0x%3.3x, IRQ %d, "
+	       "MAC addr in flash ROM:%s\n",
+	       dev->name, this->base_addr, (int)dev->irq,
+	       print_mac(mac, dev->dev_addr));
 	/*
 	 * Initialize card parameters - added by jss
 	 */
@@ -2079,7 +2076,7 @@ static int wl3501_suspend(struct pcmcia_device *link)
 {
 	struct net_device *dev = link->priv;
 
-	wl3501_pwr_mgmt(dev->priv, WL3501_SUSPEND);
+	wl3501_pwr_mgmt(netdev_priv(dev), WL3501_SUSPEND);
 	if (link->open)
 		netif_device_detach(dev);
 
@@ -2090,7 +2087,7 @@ static int wl3501_resume(struct pcmcia_device *link)
 {
 	struct net_device *dev = link->priv;
 
-	wl3501_pwr_mgmt(dev->priv, WL3501_RESUME);
+	wl3501_pwr_mgmt(netdev_priv(dev), WL3501_RESUME);
 	if (link->open) {
 		wl3501_reset(dev);
 		netif_device_attach(dev);
diff --git a/drivers/net/wireless/zd1211rw/Makefile b/drivers/net/wireless/zd1211rw/Makefile
index 4d505903352c..7a2f2a98edab 100644
--- a/drivers/net/wireless/zd1211rw/Makefile
+++ b/drivers/net/wireless/zd1211rw/Makefile
@@ -4,7 +4,7 @@ zd1211rw-objs := zd_chip.o zd_ieee80211.o \
 		zd_mac.o zd_netdev.o \
 		zd_rf_al2230.o zd_rf_rf2959.o \
 		zd_rf_al7230b.o zd_rf_uw2453.o \
-		zd_rf.o zd_usb.o zd_util.o
+		zd_rf.o zd_usb.o
 
 ifeq ($(CONFIG_ZD1211RW_DEBUG),y)
 EXTRA_CFLAGS += -DDEBUG
diff --git a/drivers/net/wireless/zd1211rw/zd_chip.c b/drivers/net/wireless/zd1211rw/zd_chip.c
index c39f1984b84d..f831b68f1b9c 100644
--- a/drivers/net/wireless/zd1211rw/zd_chip.c
+++ b/drivers/net/wireless/zd1211rw/zd_chip.c
@@ -28,7 +28,6 @@
 #include "zd_ieee80211.h"
 #include "zd_mac.h"
 #include "zd_rf.h"
-#include "zd_util.h"
 
 void zd_chip_init(struct zd_chip *chip,
 	         struct net_device *netdev,
@@ -106,7 +105,7 @@ int zd_ioread32v_locked(struct zd_chip *chip, u32 *values, const zd_addr_t *addr
 {
 	int r;
 	int i;
-	zd_addr_t *a16 = (zd_addr_t *)NULL;
+	zd_addr_t *a16;
 	u16 *v16;
 	unsigned int count16;
 
@@ -377,6 +376,7 @@ int zd_write_mac_addr(struct zd_chip *chip, const u8 *mac_addr)
 		[0] = { .addr = CR_MAC_ADDR_P1 },
 		[1] = { .addr = CR_MAC_ADDR_P2 },
 	};
+	DECLARE_MAC_BUF(mac);
 
 	reqs[0].value = (mac_addr[3] << 24)
 		      | (mac_addr[2] << 16)
@@ -386,7 +386,7 @@ int zd_write_mac_addr(struct zd_chip *chip, const u8 *mac_addr)
 		      |  mac_addr[4];
 
 	dev_dbg_f(zd_chip_dev(chip),
-		"mac addr " MAC_FMT "\n", MAC_ARG(mac_addr));
+		"mac addr %s\n", print_mac(mac, mac_addr));
 
 	mutex_lock(&chip->mutex);
 	r = zd_iowrite32a_locked(chip, reqs, ARRAY_SIZE(reqs));
@@ -500,8 +500,6 @@ int zd_chip_lock_phy_regs(struct zd_chip *chip)
 		return r;
 	}
 
-	dev_dbg_f(zd_chip_dev(chip),
-		"CR_REG1: 0x%02x -> 0x%02x\n", tmp, tmp & ~UNLOCK_PHY_REGS);
 	tmp &= ~UNLOCK_PHY_REGS;
 
 	r = zd_iowrite32_locked(chip, tmp, CR_REG1);
@@ -523,8 +521,6 @@ int zd_chip_unlock_phy_regs(struct zd_chip *chip)
 		return r;
 	}
 
-	dev_dbg_f(zd_chip_dev(chip),
-		"CR_REG1: 0x%02x -> 0x%02x\n", tmp, tmp | UNLOCK_PHY_REGS);
 	tmp |= UNLOCK_PHY_REGS;
 
 	r = zd_iowrite32_locked(chip, tmp, CR_REG1);
@@ -841,8 +837,6 @@ static int get_aw_pt_bi(struct zd_chip *chip, struct aw_pt_bi *s)
 	s->atim_wnd_period = values[0];
 	s->pre_tbtt = values[1];
 	s->beacon_interval = values[2];
-	dev_dbg_f(zd_chip_dev(chip), "aw %u pt %u bi %u\n",
-		s->atim_wnd_period, s->pre_tbtt, s->beacon_interval);
 	return 0;
 }
 
@@ -864,9 +858,6 @@ static int set_aw_pt_bi(struct zd_chip *chip, struct aw_pt_bi *s)
 	reqs[2].addr = CR_BCN_INTERVAL;
 	reqs[2].value = s->beacon_interval;
 
-	dev_dbg_f(zd_chip_dev(chip),
-		"aw %u pt %u bi %u\n", s->atim_wnd_period, s->pre_tbtt,
-		                       s->beacon_interval);
 	return zd_iowrite32a_locked(chip, reqs, ARRAY_SIZE(reqs));
 }
 
@@ -1018,19 +1009,19 @@ int zd_chip_set_rts_cts_rate_locked(struct zd_chip *chip,
 	u32 value = 0;
 
 	/* Modulation bit */
-	if (ZD_CS_TYPE(rts_rate) == ZD_CS_OFDM)
+	if (ZD_MODULATION_TYPE(rts_rate) == ZD_OFDM)
 		rts_mod = ZD_RX_OFDM;
 
 	dev_dbg_f(zd_chip_dev(chip), "rts_rate=%x preamble=%x\n",
 		rts_rate, preamble);
 
-	value |= rts_rate << RTSCTS_SH_RTS_RATE;
+	value |= ZD_PURE_RATE(rts_rate) << RTSCTS_SH_RTS_RATE;
 	value |= rts_mod << RTSCTS_SH_RTS_MOD_TYPE;
 	value |= preamble << RTSCTS_SH_RTS_PMB_TYPE;
 	value |= preamble << RTSCTS_SH_CTS_PMB_TYPE;
 
 	/* We always send 11M self-CTS messages, like the vendor driver. */
-	value |= ZD_CCK_RATE_11M << RTSCTS_SH_CTS_RATE;
+	value |= ZD_PURE_RATE(ZD_CCK_RATE_11M) << RTSCTS_SH_CTS_RATE;
 	value |= ZD_RX_CCK << RTSCTS_SH_CTS_MOD_TYPE;
 
 	return zd_iowrite32_locked(chip, value, CR_RTS_CTS_RATE);
@@ -1160,16 +1151,12 @@ out:
 static int update_pwr_int(struct zd_chip *chip, u8 channel)
 {
 	u8 value = chip->pwr_int_values[channel - 1];
-	dev_dbg_f(zd_chip_dev(chip), "channel %d pwr_int %#04x\n",
-		 channel, value);
 	return zd_iowrite16_locked(chip, value, CR31);
 }
 
 static int update_pwr_cal(struct zd_chip *chip, u8 channel)
 {
 	u8 value = chip->pwr_cal_values[channel-1];
-	dev_dbg_f(zd_chip_dev(chip), "channel %d pwr_cal %#04x\n",
-		 channel, value);
 	return zd_iowrite16_locked(chip, value, CR68);
 }
 
@@ -1184,9 +1171,6 @@ static int update_ofdm_cal(struct zd_chip *chip, u8 channel)
 	ioreqs[2].addr = CR65;
 	ioreqs[2].value = chip->ofdm_cal_values[OFDM_54M_INDEX][channel-1];
 
-	dev_dbg_f(zd_chip_dev(chip),
-		"channel %d ofdm_cal 36M %#04x 48M %#04x 54M %#04x\n",
-		channel, ioreqs[0].value, ioreqs[1].value, ioreqs[2].value);
 	return zd_iowrite16a_locked(chip, ioreqs, ARRAY_SIZE(ioreqs));
 }
 
@@ -1344,7 +1328,7 @@ int zd_chip_set_basic_rates_locked(struct zd_chip *chip, u16 cr_rates)
 	return zd_iowrite32_locked(chip, cr_rates, CR_BASIC_RATE_TBL);
 }
 
-static int ofdm_qual_db(u8 status_quality, u8 rate, unsigned int size)
+static int ofdm_qual_db(u8 status_quality, u8 zd_rate, unsigned int size)
 {
 	static const u16 constants[] = {
 		715, 655, 585, 540, 470, 410, 360, 315,
@@ -1358,7 +1342,7 @@ static int ofdm_qual_db(u8 status_quality, u8 rate, unsigned int size)
 	/* It seems that their quality parameter is somehow per signal
 	 * and is now transferred per bit.
 	 */
-	switch (rate) {
+	switch (zd_rate) {
 	case ZD_OFDM_RATE_6M:
 	case ZD_OFDM_RATE_12M:
 	case ZD_OFDM_RATE_24M:
@@ -1385,7 +1369,7 @@ static int ofdm_qual_db(u8 status_quality, u8 rate, unsigned int size)
 			break;
 	}
 
-	switch (rate) {
+	switch (zd_rate) {
 	case ZD_OFDM_RATE_6M:
 	case ZD_OFDM_RATE_9M:
 		i += 3;
@@ -1409,11 +1393,11 @@ static int ofdm_qual_db(u8 status_quality, u8 rate, unsigned int size)
 	return i;
 }
 
-static int ofdm_qual_percent(u8 status_quality, u8 rate, unsigned int size)
+static int ofdm_qual_percent(u8 status_quality, u8 zd_rate, unsigned int size)
 {
 	int r;
 
-	r = ofdm_qual_db(status_quality, rate, size);
+	r = ofdm_qual_db(status_quality, zd_rate, size);
 	ZD_ASSERT(r >= 0);
 	if (r < 0)
 		r = 0;
@@ -1474,12 +1458,17 @@ static int cck_qual_percent(u8 status_quality)
 	return r <= 100 ? r : 100;
 }
 
+static inline u8 zd_rate_from_ofdm_plcp_header(const void *rx_frame)
+{
+	return ZD_OFDM | zd_ofdm_plcp_header_rate(rx_frame);
+}
+
 u8 zd_rx_qual_percent(const void *rx_frame, unsigned int size,
 	              const struct rx_status *status)
 {
 	return (status->frame_status&ZD_RX_OFDM) ?
 		ofdm_qual_percent(status->signal_quality_ofdm,
-			          zd_ofdm_plcp_header_rate(rx_frame),
+					  zd_rate_from_ofdm_plcp_header(rx_frame),
 			          size) :
 		cck_qual_percent(status->signal_quality_cck);
 }
@@ -1495,32 +1484,32 @@ u8 zd_rx_strength_percent(u8 rssi)
 u16 zd_rx_rate(const void *rx_frame, const struct rx_status *status)
 {
 	static const u16 ofdm_rates[] = {
-		[ZD_OFDM_RATE_6M]  = 60,
-		[ZD_OFDM_RATE_9M]  = 90,
-		[ZD_OFDM_RATE_12M] = 120,
-		[ZD_OFDM_RATE_18M] = 180,
-		[ZD_OFDM_RATE_24M] = 240,
-		[ZD_OFDM_RATE_36M] = 360,
-		[ZD_OFDM_RATE_48M] = 480,
-		[ZD_OFDM_RATE_54M] = 540,
+		[ZD_OFDM_PLCP_RATE_6M]  = 60,
+		[ZD_OFDM_PLCP_RATE_9M]  = 90,
+		[ZD_OFDM_PLCP_RATE_12M] = 120,
+		[ZD_OFDM_PLCP_RATE_18M] = 180,
+		[ZD_OFDM_PLCP_RATE_24M] = 240,
+		[ZD_OFDM_PLCP_RATE_36M] = 360,
+		[ZD_OFDM_PLCP_RATE_48M] = 480,
+		[ZD_OFDM_PLCP_RATE_54M] = 540,
 	};
 	u16 rate;
 	if (status->frame_status & ZD_RX_OFDM) {
+		/* Deals with PLCP OFDM rate (not zd_rates) */
 		u8 ofdm_rate = zd_ofdm_plcp_header_rate(rx_frame);
 		rate = ofdm_rates[ofdm_rate & 0xf];
 	} else {
-		u8 cck_rate = zd_cck_plcp_header_rate(rx_frame);
-		switch (cck_rate) {
-		case ZD_CCK_SIGNAL_1M:
+		switch (zd_cck_plcp_header_signal(rx_frame)) {
+		case ZD_CCK_PLCP_SIGNAL_1M:
 			rate = 10;
 			break;
-		case ZD_CCK_SIGNAL_2M:
+		case ZD_CCK_PLCP_SIGNAL_2M:
 			rate = 20;
 			break;
-		case ZD_CCK_SIGNAL_5M5:
+		case ZD_CCK_PLCP_SIGNAL_5M5:
 			rate = 55;
 			break;
-		case ZD_CCK_SIGNAL_11M:
+		case ZD_CCK_PLCP_SIGNAL_11M:
 			rate = 110;
 			break;
 		default:
@@ -1638,7 +1627,5 @@ int zd_chip_set_multicast_hash(struct zd_chip *chip,
 		{ CR_GROUP_HASH_P2, hash->high },
 	};
 
-	dev_dbg_f(zd_chip_dev(chip), "hash l 0x%08x h 0x%08x\n",
-		ioreqs[0].value, ioreqs[1].value);
 	return zd_iowrite32a(chip, ioreqs, ARRAY_SIZE(ioreqs));
 }
diff --git a/drivers/net/wireless/zd1211rw/zd_chip.h b/drivers/net/wireless/zd1211rw/zd_chip.h
index f4698576ab71..8009b70213e2 100644
--- a/drivers/net/wireless/zd1211rw/zd_chip.h
+++ b/drivers/net/wireless/zd1211rw/zd_chip.h
@@ -871,11 +871,6 @@ static inline int zd_chip_set_basic_rates(struct zd_chip *chip, u16 cr_rates)
 	return r;
 }
 
-static inline int zd_chip_set_rx_filter(struct zd_chip *chip, u32 filter)
-{
-	return zd_iowrite32(chip, CR_RX_FILTER, filter);
-}
-
 int zd_chip_lock_phy_regs(struct zd_chip *chip);
 int zd_chip_unlock_phy_regs(struct zd_chip *chip);
 
diff --git a/drivers/net/wireless/zd1211rw/zd_def.h b/drivers/net/wireless/zd1211rw/zd_def.h
index deb99d1eaa77..505b4d7dd0e2 100644
--- a/drivers/net/wireless/zd1211rw/zd_def.h
+++ b/drivers/net/wireless/zd1211rw/zd_def.h
@@ -21,7 +21,6 @@
 #include <linux/kernel.h>
 #include <linux/stringify.h>
 #include <linux/device.h>
-#include <linux/kernel.h>
 
 typedef u16 __nocast zd_addr_t;
 
diff --git a/drivers/net/wireless/zd1211rw/zd_ieee80211.h b/drivers/net/wireless/zd1211rw/zd_ieee80211.h
index c4f36d39642b..fbf6491dce7e 100644
--- a/drivers/net/wireless/zd1211rw/zd_ieee80211.h
+++ b/drivers/net/wireless/zd1211rw/zd_ieee80211.h
@@ -43,21 +43,25 @@ struct ofdm_plcp_header {
 	__le16 service;
 } __attribute__((packed));
 
-static inline u8 zd_ofdm_plcp_header_rate(
-	const struct ofdm_plcp_header *header)
+static inline u8 zd_ofdm_plcp_header_rate(const struct ofdm_plcp_header *header)
 {
 	return header->prefix[0] & 0xf;
 }
 
-/* These are referred to as zd_rates */
-#define ZD_OFDM_RATE_6M		0xb
-#define ZD_OFDM_RATE_9M		0xf
-#define ZD_OFDM_RATE_12M	0xa
-#define ZD_OFDM_RATE_18M	0xe
-#define ZD_OFDM_RATE_24M	0x9
-#define ZD_OFDM_RATE_36M	0xd
-#define ZD_OFDM_RATE_48M	0x8
-#define ZD_OFDM_RATE_54M	0xc
+/* The following defines give the encoding of the 4-bit rate field in the
+ * OFDM (802.11a/802.11g) PLCP header. Notify that these values are used to
+ * define the zd-rate values for OFDM.
+ *
+ * See the struct zd_ctrlset definition in zd_mac.h.
+ */
+#define ZD_OFDM_PLCP_RATE_6M		0xb
+#define ZD_OFDM_PLCP_RATE_9M		0xf
+#define ZD_OFDM_PLCP_RATE_12M	0xa
+#define ZD_OFDM_PLCP_RATE_18M	0xe
+#define ZD_OFDM_PLCP_RATE_24M	0x9
+#define ZD_OFDM_PLCP_RATE_36M	0xd
+#define ZD_OFDM_PLCP_RATE_48M	0x8
+#define ZD_OFDM_PLCP_RATE_54M	0xc
 
 struct cck_plcp_header {
 	u8 signal;
@@ -66,15 +70,22 @@ struct cck_plcp_header {
 	__le16 crc16;
 } __attribute__((packed));
 
-static inline u8 zd_cck_plcp_header_rate(const struct cck_plcp_header *header)
+static inline u8 zd_cck_plcp_header_signal(const struct cck_plcp_header *header)
 {
 	return header->signal;
 }
 
-#define ZD_CCK_SIGNAL_1M	0x0a
-#define ZD_CCK_SIGNAL_2M	0x14
-#define ZD_CCK_SIGNAL_5M5	0x37
-#define ZD_CCK_SIGNAL_11M	0x6e
+/* These defines give the encodings of the signal field in the 802.11b PLCP
+ * header. The signal field gives the bit rate of the following packet. Even
+ * if technically wrong we use CCK here also for the 1 MBit/s and 2 MBit/s
+ * rate to stay consistent with Zydas and our use of the term.
+ *
+ * Notify that these values are *not* used in the zd-rates.
+ */
+#define ZD_CCK_PLCP_SIGNAL_1M	0x0a
+#define ZD_CCK_PLCP_SIGNAL_2M	0x14
+#define ZD_CCK_PLCP_SIGNAL_5M5	0x37
+#define ZD_CCK_PLCP_SIGNAL_11M	0x6e
 
 enum ieee80211_std {
 	IEEE80211B = 0x01,
diff --git a/drivers/net/wireless/zd1211rw/zd_mac.c b/drivers/net/wireless/zd1211rw/zd_mac.c
index 26869d107e52..a903645e157a 100644
--- a/drivers/net/wireless/zd1211rw/zd_mac.c
+++ b/drivers/net/wireless/zd1211rw/zd_mac.c
@@ -28,7 +28,6 @@
 #include "zd_ieee80211.h"
 #include "zd_netdev.h"
 #include "zd_rf.h"
-#include "zd_util.h"
 
 static void ieee_init(struct ieee80211_device *ieee);
 static void softmac_init(struct ieee80211softmac_device *sm);
@@ -161,13 +160,33 @@ void zd_mac_clear(struct zd_mac *mac)
 	ZD_MEMCLEAR(mac, sizeof(struct zd_mac));
 }
 
-static int reset_mode(struct zd_mac *mac)
+static int set_rx_filter(struct zd_mac *mac)
 {
 	struct ieee80211_device *ieee = zd_mac_to_ieee80211(mac);
 	u32 filter = (ieee->iw_mode == IW_MODE_MONITOR) ? ~0 : STA_RX_FILTER;
 	return zd_iowrite32(&mac->chip, CR_RX_FILTER, filter);
 }
 
+static int set_sniffer(struct zd_mac *mac)
+{
+	struct ieee80211_device *ieee = zd_mac_to_ieee80211(mac);
+	return zd_iowrite32(&mac->chip, CR_SNIFFER_ON,
+		ieee->iw_mode == IW_MODE_MONITOR ? 1 : 0);
+	return 0;
+}
+
+static int set_mc_hash(struct zd_mac *mac)
+{
+	struct zd_mc_hash hash;
+	struct ieee80211_device *ieee = zd_mac_to_ieee80211(mac);
+
+	zd_mc_clear(&hash);
+	if (ieee->iw_mode == IW_MODE_MONITOR)
+		zd_mc_add_all(&hash);
+
+	return zd_chip_set_multicast_hash(&mac->chip, &hash);
+}
+
 int zd_mac_open(struct net_device *netdev)
 {
 	struct zd_mac *mac = zd_netdev_mac(netdev);
@@ -194,7 +213,13 @@ int zd_mac_open(struct net_device *netdev)
 	r = zd_chip_set_basic_rates(chip, CR_RATES_80211B | CR_RATES_80211G);
 	if (r < 0)
 		goto disable_int;
-	r = reset_mode(mac);
+	r = set_rx_filter(mac);
+	if (r)
+		goto disable_int;
+	r = set_sniffer(mac);
+	if (r)
+		goto disable_int;
+	r = set_mc_hash(mac);
 	if (r)
 		goto disable_int;
 	r = zd_chip_switch_radio_on(chip);
@@ -263,12 +288,13 @@ int zd_mac_set_mac_address(struct net_device *netdev, void *p)
 	struct sockaddr *addr = p;
 	struct zd_mac *mac = zd_netdev_mac(netdev);
 	struct zd_chip *chip = &mac->chip;
+	DECLARE_MAC_BUF(mac2);
 
 	if (!is_valid_ether_addr(addr->sa_data))
 		return -EADDRNOTAVAIL;
 
 	dev_dbg_f(zd_mac_dev(mac),
-		  "Setting MAC to " MAC_FMT "\n", MAC_ARG(addr->sa_data));
+		  "Setting MAC to %s\n", print_mac(mac2, addr->sa_data));
 
 	if (netdev->flags & IFF_UP) {
 		r = zd_write_mac_addr(chip, addr->sa_data);
@@ -298,18 +324,21 @@ static void set_multicast_hash_handler(struct work_struct *work)
 
 void zd_mac_set_multicast_list(struct net_device *dev)
 {
-	struct zd_mc_hash hash;
 	struct zd_mac *mac = zd_netdev_mac(dev);
+	struct ieee80211_device *ieee = zd_mac_to_ieee80211(mac);
+	struct zd_mc_hash hash;
 	struct dev_mc_list *mc;
 	unsigned long flags;
+	DECLARE_MAC_BUF(mac2);
 
-	if (dev->flags & (IFF_PROMISC|IFF_ALLMULTI)) {
+	if (dev->flags & (IFF_PROMISC|IFF_ALLMULTI) ||
+			ieee->iw_mode == IW_MODE_MONITOR) {
 		zd_mc_add_all(&hash);
 	} else {
 		zd_mc_clear(&hash);
 		for (mc = dev->mc_list; mc; mc = mc->next) {
-			dev_dbg_f(zd_mac_dev(mac), "mc addr " MAC_FMT "\n",
-				  MAC_ARG(mc->dmi_addr));
+			dev_dbg_f(zd_mac_dev(mac), "mc addr %s\n",
+				  print_mac(mac2, mc->dmi_addr));
 			zd_mc_add_addr(&hash, mc->dmi_addr);
 		}
 	}
@@ -582,28 +611,6 @@ u8 zd_mac_get_channel(struct zd_mac *mac)
 	return channel;
 }
 
-/* If wrong rate is given, we are falling back to the slowest rate: 1MBit/s */
-static u8 zd_rate_typed(u8 zd_rate)
-{
-	static const u8 typed_rates[16] = {
-		[ZD_CCK_RATE_1M]	= ZD_CS_CCK|ZD_CCK_RATE_1M,
-		[ZD_CCK_RATE_2M]	= ZD_CS_CCK|ZD_CCK_RATE_2M,
-		[ZD_CCK_RATE_5_5M]	= ZD_CS_CCK|ZD_CCK_RATE_5_5M,
-		[ZD_CCK_RATE_11M]	= ZD_CS_CCK|ZD_CCK_RATE_11M,
-		[ZD_OFDM_RATE_6M]	= ZD_CS_OFDM|ZD_OFDM_RATE_6M,
-		[ZD_OFDM_RATE_9M]	= ZD_CS_OFDM|ZD_OFDM_RATE_9M,
-		[ZD_OFDM_RATE_12M]	= ZD_CS_OFDM|ZD_OFDM_RATE_12M,
-		[ZD_OFDM_RATE_18M]	= ZD_CS_OFDM|ZD_OFDM_RATE_18M,
-		[ZD_OFDM_RATE_24M]	= ZD_CS_OFDM|ZD_OFDM_RATE_24M,
-		[ZD_OFDM_RATE_36M]	= ZD_CS_OFDM|ZD_OFDM_RATE_36M,
-		[ZD_OFDM_RATE_48M]	= ZD_CS_OFDM|ZD_OFDM_RATE_48M,
-		[ZD_OFDM_RATE_54M]	= ZD_CS_OFDM|ZD_OFDM_RATE_54M,
-	};
-
-	ZD_ASSERT(ZD_CS_RATE_MASK == 0x0f);
-	return typed_rates[zd_rate & ZD_CS_RATE_MASK];
-}
-
 int zd_mac_set_mode(struct zd_mac *mac, u32 mode)
 {
 	struct ieee80211_device *ieee;
@@ -628,8 +635,12 @@ int zd_mac_set_mode(struct zd_mac *mac, u32 mode)
 	ieee->iw_mode = mode;
 	spin_unlock_irq(&ieee->lock);
 
-	if (netif_running(mac->netdev))
-		return reset_mode(mac);
+	if (netif_running(mac->netdev)) {
+		int r = set_rx_filter(mac);
+		if (r)
+			return r;
+		return set_sniffer(mac);
+	}
 
 	return 0;
 }
@@ -707,25 +718,30 @@ int zd_mac_get_range(struct zd_mac *mac, struct iw_range *range)
 
 static int zd_calc_tx_length_us(u8 *service, u8 zd_rate, u16 tx_length)
 {
+	/* ZD_PURE_RATE() must be used to remove the modulation type flag of
+	 * the zd-rate values. */
 	static const u8 rate_divisor[] = {
-		[ZD_CCK_RATE_1M]	=  1,
-		[ZD_CCK_RATE_2M]	=  2,
-		[ZD_CCK_RATE_5_5M]	= 11, /* bits must be doubled */
-		[ZD_CCK_RATE_11M]	= 11,
-		[ZD_OFDM_RATE_6M]	=  6,
-		[ZD_OFDM_RATE_9M]	=  9,
-		[ZD_OFDM_RATE_12M]	= 12,
-		[ZD_OFDM_RATE_18M]	= 18,
-		[ZD_OFDM_RATE_24M]	= 24,
-		[ZD_OFDM_RATE_36M]	= 36,
-		[ZD_OFDM_RATE_48M]	= 48,
-		[ZD_OFDM_RATE_54M]	= 54,
+		[ZD_PURE_RATE(ZD_CCK_RATE_1M)]		=  1,
+		[ZD_PURE_RATE(ZD_CCK_RATE_2M)]		=  2,
+
+		/* bits must be doubled */
+		[ZD_PURE_RATE(ZD_CCK_RATE_5_5M)]	= 11,
+
+		[ZD_PURE_RATE(ZD_CCK_RATE_11M)]		= 11,
+		[ZD_PURE_RATE(ZD_OFDM_RATE_6M)]		=  6,
+		[ZD_PURE_RATE(ZD_OFDM_RATE_9M)]		=  9,
+		[ZD_PURE_RATE(ZD_OFDM_RATE_12M)]	= 12,
+		[ZD_PURE_RATE(ZD_OFDM_RATE_18M)]	= 18,
+		[ZD_PURE_RATE(ZD_OFDM_RATE_24M)]	= 24,
+		[ZD_PURE_RATE(ZD_OFDM_RATE_36M)]	= 36,
+		[ZD_PURE_RATE(ZD_OFDM_RATE_48M)]	= 48,
+		[ZD_PURE_RATE(ZD_OFDM_RATE_54M)]	= 54,
 	};
 
 	u32 bits = (u32)tx_length * 8;
 	u32 divisor;
 
-	divisor = rate_divisor[zd_rate];
+	divisor = rate_divisor[ZD_PURE_RATE(zd_rate)];
 	if (divisor == 0)
 		return -EINVAL;
 
@@ -748,52 +764,24 @@ static int zd_calc_tx_length_us(u8 *service, u8 zd_rate, u16 tx_length)
 	return bits/divisor;
 }
 
-enum {
-	R2M_SHORT_PREAMBLE = 0x01,
-	R2M_11A		   = 0x02,
-};
-
-static u8 zd_rate_to_modulation(u8 zd_rate, int flags)
-{
-	u8 modulation;
-
-	modulation = zd_rate_typed(zd_rate);
-	if (flags & R2M_SHORT_PREAMBLE) {
-		switch (ZD_CS_RATE(modulation)) {
-		case ZD_CCK_RATE_2M:
-		case ZD_CCK_RATE_5_5M:
-		case ZD_CCK_RATE_11M:
-			modulation |= ZD_CS_CCK_PREA_SHORT;
-			return modulation;
-		}
-	}
-	if (flags & R2M_11A) {
-		if (ZD_CS_TYPE(modulation) == ZD_CS_OFDM)
-			modulation |= ZD_CS_OFDM_MODE_11A;
-	}
-	return modulation;
-}
-
 static void cs_set_modulation(struct zd_mac *mac, struct zd_ctrlset *cs,
 	                      struct ieee80211_hdr_4addr *hdr)
 {
 	struct ieee80211softmac_device *softmac = ieee80211_priv(mac->netdev);
 	u16 ftype = WLAN_FC_GET_TYPE(le16_to_cpu(hdr->frame_ctl));
-	u8 rate, zd_rate;
+	u8 rate;
 	int is_mgt = (ftype == IEEE80211_FTYPE_MGMT) != 0;
 	int is_multicast = is_multicast_ether_addr(hdr->addr1);
 	int short_preamble = ieee80211softmac_short_preamble_ok(softmac,
 		is_multicast, is_mgt);
-	int flags = 0;
 
-	/* FIXME: 802.11a? */
 	rate = ieee80211softmac_suggest_txrate(softmac, is_multicast, is_mgt);
+	cs->modulation = rate_to_zd_rate(rate);
 
-	if (short_preamble)
-		flags |= R2M_SHORT_PREAMBLE;
-
-	zd_rate = rate_to_zd_rate(rate);
-	cs->modulation = zd_rate_to_modulation(zd_rate, flags);
+	/* Set short preamble bit when appropriate */
+	if (short_preamble && ZD_MODULATION_TYPE(cs->modulation) == ZD_CCK
+	    && cs->modulation != ZD_CCK_RATE_1M)
+		cs->modulation |= ZD_CCK_PREA_SHORT;
 }
 
 static void cs_set_control(struct zd_mac *mac, struct zd_ctrlset *cs,
@@ -832,7 +820,7 @@ static void cs_set_control(struct zd_mac *mac, struct zd_ctrlset *cs,
 		cs->control |= ZD_CS_RTS;
 
 	/* Use CTS-to-self protection if required */
-	if (ZD_CS_TYPE(cs->modulation) == ZD_CS_OFDM &&
+	if (ZD_MODULATION_TYPE(cs->modulation) == ZD_OFDM &&
 			ieee80211softmac_protection_needed(softmac)) {
 		/* FIXME: avoid sending RTS *and* self-CTS, is that correct? */
 		cs->control &= ~ZD_CS_RTS;
@@ -893,7 +881,7 @@ static int fill_ctrlset(struct zd_mac *mac,
 	 * - see line 53 of zdinlinef.h
 	 */
 	cs->service = 0;
-	r = zd_calc_tx_length_us(&cs->service, ZD_CS_RATE(cs->modulation),
+	r = zd_calc_tx_length_us(&cs->service, ZD_RATE(cs->modulation),
 		                 le16_to_cpu(cs->tx_length));
 	if (r < 0)
 		return r;
@@ -902,7 +890,7 @@ static int fill_ctrlset(struct zd_mac *mac,
 	if (next_frag_len == 0) {
 		cs->next_frame_length = 0;
 	} else {
-		r = zd_calc_tx_length_us(NULL, ZD_CS_RATE(cs->modulation),
+		r = zd_calc_tx_length_us(NULL, ZD_RATE(cs->modulation),
 			                 next_frag_len);
 		if (r < 0)
 			return r;
@@ -1077,7 +1065,8 @@ static int fill_rx_stats(struct ieee80211_rx_stats *stats,
 {
 	const struct rx_status *status;
 
-	*pstatus = status = zd_tail(buffer, length, sizeof(struct rx_status));
+	*pstatus = status = (struct rx_status *)
+		(buffer + (length - sizeof(struct rx_status)));
 	if (status->frame_status & ZD_RX_ERROR) {
 		struct ieee80211_device *ieee = zd_mac_to_ieee80211(mac);
 		ieee->stats.rx_errors++;
diff --git a/drivers/net/wireless/zd1211rw/zd_mac.h b/drivers/net/wireless/zd1211rw/zd_mac.h
index 9f9344eb50f9..1b15bde3ff60 100644
--- a/drivers/net/wireless/zd1211rw/zd_mac.h
+++ b/drivers/net/wireless/zd1211rw/zd_mac.h
@@ -40,28 +40,51 @@ struct zd_ctrlset {
 
 #define ZD_CS_RESERVED_SIZE	25
 
-/* zd_crtlset field modulation */
-#define ZD_CS_RATE_MASK		0x0f
-#define ZD_CS_TYPE_MASK		0x10
-#define ZD_CS_RATE(modulation) ((modulation) & ZD_CS_RATE_MASK)
-#define ZD_CS_TYPE(modulation) ((modulation) & ZD_CS_TYPE_MASK)
-
-#define ZD_CS_CCK		0x00
-#define ZD_CS_OFDM		0x10
-
-/* These are referred to as zd_rates */
-#define ZD_CCK_RATE_1M	0x00
-#define ZD_CCK_RATE_2M	0x01
-#define ZD_CCK_RATE_5_5M	0x02
-#define ZD_CCK_RATE_11M	0x03
-/* The rates for OFDM are encoded as in the PLCP header. Use ZD_OFDM_RATE_*.
+/* The field modulation of struct zd_ctrlset controls the bit rate, the use
+ * of short or long preambles in 802.11b (CCK mode) or the use of 802.11a or
+ * 802.11g in OFDM mode.
+ *
+ * The term zd-rate is used for the combination of the modulation type flag
+ * and the "pure" rate value.
  */
-
-/* bit 5 is preamble (when in CCK mode), or a/g selection (when in OFDM mode) */
-#define ZD_CS_CCK_PREA_LONG	0x00
-#define ZD_CS_CCK_PREA_SHORT	0x20
-#define ZD_CS_OFDM_MODE_11G	0x00
-#define ZD_CS_OFDM_MODE_11A	0x20
+#define ZD_PURE_RATE_MASK       0x0f
+#define ZD_MODULATION_TYPE_MASK 0x10
+#define ZD_RATE_MASK            (ZD_PURE_RATE_MASK|ZD_MODULATION_TYPE_MASK)
+#define ZD_PURE_RATE(modulation) ((modulation) & ZD_PURE_RATE_MASK)
+#define ZD_MODULATION_TYPE(modulation) ((modulation) & ZD_MODULATION_TYPE_MASK)
+#define ZD_RATE(modulation) ((modulation) & ZD_RATE_MASK)
+
+/* The two possible modulation types. Notify that 802.11b doesn't use the CCK
+ * codeing for the 1 and 2 MBit/s rate. We stay with the term here to remain
+ * consistent with uses the term at other places.
+  */
+#define ZD_CCK                  0x00
+#define ZD_OFDM                 0x10
+
+/* The ZD1211 firmware uses proprietary encodings of the 802.11b (CCK) rates.
+ * For OFDM the PLCP rate encodings are used. We combine these "pure" rates
+ * with the modulation type flag and call the resulting values zd-rates.
+ */
+#define ZD_CCK_RATE_1M          (ZD_CCK|0x00)
+#define ZD_CCK_RATE_2M          (ZD_CCK|0x01)
+#define ZD_CCK_RATE_5_5M        (ZD_CCK|0x02)
+#define ZD_CCK_RATE_11M         (ZD_CCK|0x03)
+#define ZD_OFDM_RATE_6M         (ZD_OFDM|ZD_OFDM_PLCP_RATE_6M)
+#define ZD_OFDM_RATE_9M         (ZD_OFDM|ZD_OFDM_PLCP_RATE_9M)
+#define ZD_OFDM_RATE_12M        (ZD_OFDM|ZD_OFDM_PLCP_RATE_12M)
+#define ZD_OFDM_RATE_18M        (ZD_OFDM|ZD_OFDM_PLCP_RATE_18M)
+#define ZD_OFDM_RATE_24M        (ZD_OFDM|ZD_OFDM_PLCP_RATE_24M)
+#define ZD_OFDM_RATE_36M        (ZD_OFDM|ZD_OFDM_PLCP_RATE_36M)
+#define ZD_OFDM_RATE_48M        (ZD_OFDM|ZD_OFDM_PLCP_RATE_48M)
+#define ZD_OFDM_RATE_54M        (ZD_OFDM|ZD_OFDM_PLCP_RATE_54M)
+
+/* The bit 5 of the zd_ctrlset modulation field controls the preamble in CCK
+ * mode or the 802.11a/802.11g selection in OFDM mode.
+ */
+#define ZD_CCK_PREA_LONG        0x00
+#define ZD_CCK_PREA_SHORT       0x20
+#define ZD_OFDM_MODE_11G        0x00
+#define ZD_OFDM_MODE_11A        0x20
 
 /* zd_ctrlset control field */
 #define ZD_CS_NEED_RANDOM_BACKOFF	0x01
diff --git a/drivers/net/wireless/zd1211rw/zd_netdev.c b/drivers/net/wireless/zd1211rw/zd_netdev.c
index 8bda48de31ef..047cab3d87df 100644
--- a/drivers/net/wireless/zd1211rw/zd_netdev.c
+++ b/drivers/net/wireless/zd1211rw/zd_netdev.c
@@ -233,7 +233,6 @@ struct net_device *zd_netdev_alloc(struct usb_interface *intf)
 		return NULL;
 	}
 
-	SET_MODULE_OWNER(netdev);
 	SET_NETDEV_DEV(netdev, &intf->dev);
 
 	dev_dbg_f(&intf->dev, "netdev->flags %#06hx\n", netdev->flags);
diff --git a/drivers/net/wireless/zd1211rw/zd_usb.c b/drivers/net/wireless/zd1211rw/zd_usb.c
index a9c339ef116a..b0684f965761 100644
--- a/drivers/net/wireless/zd1211rw/zd_usb.c
+++ b/drivers/net/wireless/zd1211rw/zd_usb.c
@@ -31,7 +31,6 @@
 #include "zd_netdev.h"
 #include "zd_mac.h"
 #include "zd_usb.h"
-#include "zd_util.h"
 
 static struct usb_device_id usb_ids[] = {
 	/* ZD1211 */
@@ -55,6 +54,7 @@ static struct usb_device_id usb_ids[] = {
 	{ USB_DEVICE(0x14ea, 0xab13), .driver_info = DEVICE_ZD1211 },
 	{ USB_DEVICE(0x13b1, 0x001e), .driver_info = DEVICE_ZD1211 },
 	{ USB_DEVICE(0x0586, 0x3407), .driver_info = DEVICE_ZD1211 },
+	{ USB_DEVICE(0x129b, 0x1666), .driver_info = DEVICE_ZD1211 },
 	/* ZD1211B */
 	{ USB_DEVICE(0x0ace, 0x1215), .driver_info = DEVICE_ZD1211B },
 	{ USB_DEVICE(0x157e, 0x300d), .driver_info = DEVICE_ZD1211B },
@@ -74,6 +74,9 @@ static struct usb_device_id usb_ids[] = {
 	{ USB_DEVICE(0x0411, 0x00da), .driver_info = DEVICE_ZD1211B },
 	{ USB_DEVICE(0x2019, 0x5303), .driver_info = DEVICE_ZD1211B },
 	{ USB_DEVICE(0x129b, 0x1667), .driver_info = DEVICE_ZD1211B },
+	{ USB_DEVICE(0x0cde, 0x001a), .driver_info = DEVICE_ZD1211B },
+	{ USB_DEVICE(0x0586, 0x340a), .driver_info = DEVICE_ZD1211B },
+	{ USB_DEVICE(0x0471, 0x1237), .driver_info = DEVICE_ZD1211B },
 	/* "Driverless" devices that need ejecting */
 	{ USB_DEVICE(0x0ace, 0x2011), .driver_info = DEVICE_INSTALLER },
 	{ USB_DEVICE(0x0ace, 0x20ff), .driver_info = DEVICE_INSTALLER },
diff --git a/drivers/net/wireless/zd1211rw/zd_util.c b/drivers/net/wireless/zd1211rw/zd_util.c
deleted file mode 100644
index d20036c15d11..000000000000
--- a/drivers/net/wireless/zd1211rw/zd_util.c
+++ /dev/null
@@ -1,82 +0,0 @@
-/* zd_util.c
- *
- * 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, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- *
- * Utility program
- */
-
-#include "zd_def.h"
-#include "zd_util.h"
-
-#ifdef DEBUG
-static char hex(u8 v)
-{
-	v &= 0xf;
-	return (v < 10 ? '0' : 'a' - 10) + v;
-}
-
-static char hex_print(u8 c)
-{
-	return (0x20 <= c && c < 0x7f) ? c : '.';
-}
-
-static void dump_line(const u8 *bytes, size_t size)
-{
-	char c;
-	size_t i;
-
-	size = size <= 8 ? size : 8;
-	printk(KERN_DEBUG "zd1211 %p ", bytes);
-	for (i = 0; i < 8; i++) {
-		switch (i) {
-		case 1:
-		case 5:
-			c = '.';
-			break;
-		case 3:
-			c = ':';
-			break;
-		default:
-			c = ' ';
-		}
-		if (i < size) {
-			printk("%c%c%c", hex(bytes[i] >> 4), hex(bytes[i]), c);
-		} else {
-			printk("  %c", c);
-		}
-	}
-
-	for (i = 0; i < size; i++)
-		printk("%c", hex_print(bytes[i]));
-	printk("\n");
-}
-
-void zd_hexdump(const void *bytes, size_t size)
-{
-	size_t i = 0;
-
-	do {
-		dump_line((u8 *)bytes + i, size-i);
-		i += 8;
-	} while (i < size);
-}
-#endif /* DEBUG */
-
-void *zd_tail(const void *buffer, size_t buffer_size, size_t tail_size)
-{
-	if (buffer_size < tail_size)
-		return NULL;
-	return (u8 *)buffer + (buffer_size - tail_size);
-}
diff --git a/drivers/net/wireless/zd1211rw/zd_util.h b/drivers/net/wireless/zd1211rw/zd_util.h
deleted file mode 100644
index ce26f7adea92..000000000000
--- a/drivers/net/wireless/zd1211rw/zd_util.h
+++ /dev/null
@@ -1,29 +0,0 @@
-/* zd_util.h
- *
- * 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, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the Free Software
- * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
- */
-
-#ifndef _ZD_UTIL_H
-#define _ZD_UTIL_H
-
-void *zd_tail(const void *buffer, size_t buffer_size, size_t tail_size);
-
-#ifdef DEBUG
-void zd_hexdump(const void *bytes, size_t size);
-#else
-#define zd_hexdump(bytes, size)
-#endif /* DEBUG */
-
-#endif /* _ZD_UTIL_H */