Merge branch 'linux-next' of git://git.infradead.org/~dedekind/ubi-2.6

156 files changed, 20086 insertions, 15290 deletions

diff --git a/drivers/net/wireless/Kconfig b/drivers/net/wireless/Kconfig
index fdf5aa8b8429..91fc2c765d90 100644
--- a/drivers/net/wireless/Kconfig
+++ b/drivers/net/wireless/Kconfig
@@ -635,14 +635,20 @@ config RTL8180
 	  Thanks to Realtek for their support!
 
 config RTL8187
-	tristate "Realtek 8187 USB support"
+	tristate "Realtek 8187 and 8187B USB support"
 	depends on MAC80211 && USB && WLAN_80211 && EXPERIMENTAL
 	select EEPROM_93CX6
 	---help---
-	  This is a driver for RTL8187 based cards.
-	  These are USB based chips found in cards such as:
+	  This is a driver for RTL8187 and RTL8187B based cards.
+	  These are USB based chips found in devices such as:
 
 	  Netgear WG111v2
+	  Level 1 WNC-0301USB
+	  Micronet SP907GK V5
+	  Encore ENUWI-G2
+	  Trendnet TEW-424UB
+	  ASUS P5B Deluxe
+	  Toshiba Satellite Pro series of laptops
 
 	  Thanks to Realtek for their support!
 
@@ -673,6 +679,19 @@ config ADM8211
 
 	  Thanks to Infineon-ADMtek for their support of this driver.
 
+config MAC80211_HWSIM
+	tristate "Simulated radio testing tool for mac80211"
+	depends on MAC80211 && WLAN_80211
+	---help---
+	  This driver is a developer testing tool that can be used to test
+	  IEEE 802.11 networking stack (mac80211) functionality. This is not
+	  needed for normal wireless LAN usage and is only for testing. See
+	  Documentation/networking/mac80211_hwsim for more information on how
+	  to use this tool.
+
+	  To compile this driver as a module, choose M here: the module will be
+	  called mac80211_hwsim.  If unsure, say N.
+
 source "drivers/net/wireless/p54/Kconfig"
 source "drivers/net/wireless/ath5k/Kconfig"
 source "drivers/net/wireless/iwlwifi/Kconfig"
diff --git a/drivers/net/wireless/Makefile b/drivers/net/wireless/Makefile
index 2c343aae38d4..54a4f6f1db67 100644
--- a/drivers/net/wireless/Makefile
+++ b/drivers/net/wireless/Makefile
@@ -62,3 +62,5 @@ obj-$(CONFIG_RT2X00)	+= rt2x00/
 obj-$(CONFIG_P54_COMMON)	+= p54/
 
 obj-$(CONFIG_ATH5K)	+= ath5k/
+
+obj-$(CONFIG_MAC80211_HWSIM)	+= mac80211_hwsim.o
diff --git a/drivers/net/wireless/adm8211.c b/drivers/net/wireless/adm8211.c
index 5c0d2b082750..3333d4596b8d 100644
--- a/drivers/net/wireless/adm8211.c
+++ b/drivers/net/wireless/adm8211.c
@@ -306,11 +306,10 @@ 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;
+	stats[0].len = priv->cur_tx - priv->dirty_tx;
+	stats[0].limit = priv->tx_ring_size - 2;
+	stats[0].count = priv->dirty_tx;
 
 	return 0;
 }
@@ -325,7 +324,7 @@ static void adm8211_interrupt_tci(struct ieee80211_hw *dev)
 	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 ieee80211_tx_info *txi;
 		struct adm8211_tx_ring_info *info;
 		struct sk_buff *skb;
 
@@ -335,24 +334,23 @@ static void adm8211_interrupt_tci(struct ieee80211_hw *dev)
 
 		info = &priv->tx_buffers[entry];
 		skb = info->skb;
+		txi = IEEE80211_SKB_CB(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));
+		memset(&txi->status, 0, sizeof(txi->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 (!(txi->flags & IEEE80211_TX_CTL_NO_ACK)) {
 			if (status & TDES0_STATUS_ES)
-				tx_status.excessive_retries = 1;
+				txi->status.excessive_retries = 1;
 			else
-				tx_status.flags |= IEEE80211_TX_STATUS_ACK;
+				txi->flags |= IEEE80211_TX_STAT_ACK;
 		}
-		ieee80211_tx_status_irqsafe(dev, skb, &tx_status);
+		ieee80211_tx_status_irqsafe(dev, skb);
 
 		info->skb = NULL;
 	}
@@ -446,9 +444,9 @@ static void adm8211_interrupt_rci(struct ieee80211_hw *dev)
 			struct ieee80211_rx_status rx_status = {0};
 
 			if (priv->pdev->revision < ADM8211_REV_CA)
-				rx_status.ssi = rssi;
+				rx_status.signal = rssi;
 			else
-				rx_status.ssi = 100 - rssi;
+				rx_status.signal = 100 - rssi;
 
 			rx_status.rate_idx = rate;
 
@@ -1639,7 +1637,6 @@ static void adm8211_calc_durations(int *dur, int *plcp, size_t payload_len, int
 /* 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;
@@ -1665,7 +1662,6 @@ static void adm8211_tx_raw(struct ieee80211_hw *dev, struct sk_buff *skb,
 
 	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);
 
@@ -1686,22 +1682,20 @@ static void adm8211_tx_raw(struct ieee80211_hw *dev, struct sk_buff *skb,
 }
 
 /* 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)
+static int adm8211_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
 {
 	struct adm8211_tx_hdr *txhdr;
-	u16 fc;
 	size_t payload_len, hdrlen;
 	int plcp, dur, len, plcp_signal, short_preamble;
 	struct ieee80211_hdr *hdr;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+	struct ieee80211_rate *txrate = ieee80211_get_tx_rate(dev, info);
 
-	short_preamble = !!(control->tx_rate->flags &
-					IEEE80211_TXCTL_SHORT_PREAMBLE);
-	plcp_signal = control->tx_rate->bitrate;
+	short_preamble = !!(txrate->flags & IEEE80211_TX_CTL_SHORT_PREAMBLE);
+	plcp_signal = txrate->bitrate;
 
 	hdr = (struct ieee80211_hdr *)skb->data;
-	fc = le16_to_cpu(hdr->frame_control) & ~IEEE80211_FCTL_PROTECTED;
-	hdrlen = ieee80211_get_hdrlen(fc);
+	hdrlen = ieee80211_hdrlen(hdr->frame_control);
 	memcpy(skb->cb, skb->data, hdrlen);
 	hdr = (struct ieee80211_hdr *)skb->cb;
 	skb_pull(skb, hdrlen);
@@ -1715,8 +1709,6 @@ static int adm8211_tx(struct ieee80211_hw *dev, struct sk_buff *skb,
 	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,
@@ -1731,15 +1723,12 @@ static int adm8211_tx(struct ieee80211_hw *dev, struct sk_buff *skb,
 	if (short_preamble)
 		txhdr->header_control |= cpu_to_le16(ADM8211_TXHDRCTL_SHORT_PREAMBLE);
 
-	if (control->flags & IEEE80211_TXCTL_USE_RTS_CTS)
+	if (info->flags & IEEE80211_TX_CTL_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 = info->control.retry_limit;
 
-	txhdr->retry_limit = control->retry_limit;
-
-	adm8211_tx_raw(dev, skb, plcp_signal, control, hdrlen);
+	adm8211_tx_raw(dev, skb, plcp_signal, hdrlen);
 
 	return NETDEV_TX_OK;
 }
@@ -1894,9 +1883,10 @@ static int __devinit adm8211_probe(struct pci_dev *pdev,
 
 	dev->extra_tx_headroom = sizeof(struct adm8211_tx_hdr);
 	/* dev->flags = IEEE80211_HW_RX_INCLUDES_FCS in promisc mode */
+	dev->flags = IEEE80211_HW_SIGNAL_UNSPEC;
 
 	dev->channel_change_time = 1000;
-	dev->max_rssi = 100;	/* FIXME: find better value */
+	dev->max_signal = 100;    /* FIXME: find better value */
 
 	dev->queues = 1; /* ADM8211C supports more, maybe ADM8211B too */
 
@@ -2015,7 +2005,7 @@ static int adm8211_resume(struct pci_dev *pdev)
 
 	if (priv->mode != IEEE80211_IF_TYPE_INVALID) {
 		adm8211_start(dev);
-		ieee80211_start_queues(dev);
+		ieee80211_wake_queues(dev);
 	}
 
 	return 0;
diff --git a/drivers/net/wireless/adm8211.h b/drivers/net/wireless/adm8211.h
index 8d7c564b3b04..9b190ee26e90 100644
--- a/drivers/net/wireless/adm8211.h
+++ b/drivers/net/wireless/adm8211.h
@@ -443,7 +443,6 @@ struct adm8211_rx_ring_info {
 struct adm8211_tx_ring_info {
 	struct sk_buff *skb;
 	dma_addr_t mapping;
-	struct ieee80211_tx_control tx_control;
 	size_t hdrlen;
 };
 
diff --git a/drivers/net/wireless/airo.c b/drivers/net/wireless/airo.c
index 32019fb878d8..b5cd850a4a59 100644
--- a/drivers/net/wireless/airo.c
+++ b/drivers/net/wireless/airo.c
@@ -85,10 +85,10 @@ static struct pci_driver airo_driver = {
 
 /* Include Wireless Extension definition and check version - Jean II */
 #include <linux/wireless.h>
-#define WIRELESS_SPY		// enable iwspy support
-#include <net/iw_handler.h>	// New driver API
+#define WIRELESS_SPY		/* enable iwspy support */
+#include <net/iw_handler.h>	/* New driver API */
 
-#define CISCO_EXT		// enable Cisco extensions
+#define CISCO_EXT		/* enable Cisco extensions */
 #ifdef CISCO_EXT
 #include <linux/delay.h>
 #endif
@@ -281,7 +281,7 @@ MODULE_PARM_DESC(proc_perm, "The permission bits of the files in /proc");
 /* This is a kind of sloppy hack to get this information to OUT4500 and
    IN4500.  I would be extremely interested in the situation where this
    doesn't work though!!! */
-static int do8bitIO = 0;
+static int do8bitIO /* = 0 */;
 
 /* Return codes */
 #define SUCCESS 0
@@ -398,8 +398,8 @@ static int do8bitIO = 0;
 #define MAXTXQ 64
 
 /* BAP selectors */
-#define BAP0 0 // Used for receiving packets
-#define BAP1 2 // Used for xmiting packets and working with RIDS
+#define BAP0 0 /* Used for receiving packets */
+#define BAP1 2 /* Used for xmiting packets and working with RIDS */
 
 /* Flags */
 #define COMMAND_BUSY 0x8000
@@ -1148,7 +1148,6 @@ static u8 airo_dbm_to_pct (tdsRssiEntry *rssi_rid, u8 dbm);
 static void airo_networks_free(struct airo_info *ai);
 
 struct airo_info {
-	struct net_device_stats	stats;
 	struct net_device             *dev;
 	struct list_head              dev_list;
 	/* Note, we can have MAX_FIDS outstanding.  FIDs are 16-bits, so we
@@ -1924,7 +1923,7 @@ static int mpi_start_xmit(struct sk_buff *skb, struct net_device *dev) {
 	if (npacks >= MAXTXQ - 1) {
 		netif_stop_queue (dev);
 		if (npacks > MAXTXQ) {
-			ai->stats.tx_fifo_errors++;
+			dev->stats.tx_fifo_errors++;
 			return 1;
 		}
 		skb_queue_tail (&ai->txq, skb);
@@ -2044,13 +2043,13 @@ static void get_tx_error(struct airo_info *ai, s32 fid)
 		bap_read(ai, &status, 2, BAP0);
 	}
 	if (le16_to_cpu(status) & 2) /* Too many retries */
-		ai->stats.tx_aborted_errors++;
+		ai->dev->stats.tx_aborted_errors++;
 	if (le16_to_cpu(status) & 4) /* Transmit lifetime exceeded */
-		ai->stats.tx_heartbeat_errors++;
+		ai->dev->stats.tx_heartbeat_errors++;
 	if (le16_to_cpu(status) & 8) /* Aid fail */
 		{ }
 	if (le16_to_cpu(status) & 0x10) /* MAC disabled */
-		ai->stats.tx_carrier_errors++;
+		ai->dev->stats.tx_carrier_errors++;
 	if (le16_to_cpu(status) & 0x20) /* Association lost */
 		{ }
 	/* We produce a TXDROP event only for retry or lifetime
@@ -2102,7 +2101,7 @@ static void airo_end_xmit(struct net_device *dev) {
 		for (; i < MAX_FIDS / 2 && (priv->fids[i] & 0xffff0000); i++);
 	} else {
 		priv->fids[fid] &= 0xffff;
-		priv->stats.tx_window_errors++;
+		dev->stats.tx_window_errors++;
 	}
 	if (i < MAX_FIDS / 2)
 		netif_wake_queue(dev);
@@ -2128,7 +2127,7 @@ static int airo_start_xmit(struct sk_buff *skb, struct net_device *dev) {
 		netif_stop_queue(dev);
 
 		if (i == MAX_FIDS / 2) {
-			priv->stats.tx_fifo_errors++;
+			dev->stats.tx_fifo_errors++;
 			return 1;
 		}
 	}
@@ -2167,7 +2166,7 @@ static void airo_end_xmit11(struct net_device *dev) {
 		for (; i < MAX_FIDS && (priv->fids[i] & 0xffff0000); i++);
 	} else {
 		priv->fids[fid] &= 0xffff;
-		priv->stats.tx_window_errors++;
+		dev->stats.tx_window_errors++;
 	}
 	if (i < MAX_FIDS)
 		netif_wake_queue(dev);
@@ -2199,7 +2198,7 @@ static int airo_start_xmit11(struct sk_buff *skb, struct net_device *dev) {
 		netif_stop_queue(dev);
 
 		if (i == MAX_FIDS) {
-			priv->stats.tx_fifo_errors++;
+			dev->stats.tx_fifo_errors++;
 			return 1;
 		}
 	}
@@ -2219,8 +2218,9 @@ static int airo_start_xmit11(struct sk_buff *skb, struct net_device *dev) {
 	return 0;
 }
 
-static void airo_read_stats(struct airo_info *ai)
+static void airo_read_stats(struct net_device *dev)
 {
+	struct airo_info *ai = dev->priv;
 	StatsRid stats_rid;
 	__le32 *vals = stats_rid.vals;
 
@@ -2232,23 +2232,24 @@ static void airo_read_stats(struct airo_info *ai)
 	readStatsRid(ai, &stats_rid, RID_STATS, 0);
 	up(&ai->sem);
 
-	ai->stats.rx_packets = le32_to_cpu(vals[43]) + le32_to_cpu(vals[44]) +
+	dev->stats.rx_packets = le32_to_cpu(vals[43]) + le32_to_cpu(vals[44]) +
 			       le32_to_cpu(vals[45]);
-	ai->stats.tx_packets = le32_to_cpu(vals[39]) + le32_to_cpu(vals[40]) +
+	dev->stats.tx_packets = le32_to_cpu(vals[39]) + le32_to_cpu(vals[40]) +
 			       le32_to_cpu(vals[41]);
-	ai->stats.rx_bytes = le32_to_cpu(vals[92]);
-	ai->stats.tx_bytes = le32_to_cpu(vals[91]);
-	ai->stats.rx_errors = le32_to_cpu(vals[0]) + le32_to_cpu(vals[2]) +
+	dev->stats.rx_bytes = le32_to_cpu(vals[92]);
+	dev->stats.tx_bytes = le32_to_cpu(vals[91]);
+	dev->stats.rx_errors = le32_to_cpu(vals[0]) + le32_to_cpu(vals[2]) +
 			      le32_to_cpu(vals[3]) + le32_to_cpu(vals[4]);
-	ai->stats.tx_errors = le32_to_cpu(vals[42]) + ai->stats.tx_fifo_errors;
-	ai->stats.multicast = le32_to_cpu(vals[43]);
-	ai->stats.collisions = le32_to_cpu(vals[89]);
+	dev->stats.tx_errors = le32_to_cpu(vals[42]) +
+			      dev->stats.tx_fifo_errors;
+	dev->stats.multicast = le32_to_cpu(vals[43]);
+	dev->stats.collisions = le32_to_cpu(vals[89]);
 
 	/* detailed rx_errors: */
-	ai->stats.rx_length_errors = le32_to_cpu(vals[3]);
-	ai->stats.rx_crc_errors = le32_to_cpu(vals[4]);
-	ai->stats.rx_frame_errors = le32_to_cpu(vals[2]);
-	ai->stats.rx_fifo_errors = le32_to_cpu(vals[0]);
+	dev->stats.rx_length_errors = le32_to_cpu(vals[3]);
+	dev->stats.rx_crc_errors = le32_to_cpu(vals[4]);
+	dev->stats.rx_frame_errors = le32_to_cpu(vals[2]);
+	dev->stats.rx_fifo_errors = le32_to_cpu(vals[0]);
 }
 
 static struct net_device_stats *airo_get_stats(struct net_device *dev)
@@ -2261,10 +2262,10 @@ static struct net_device_stats *airo_get_stats(struct net_device *dev)
 			set_bit(JOB_STATS, &local->jobs);
 			wake_up_interruptible(&local->thr_wait);
 		} else
-			airo_read_stats(local);
+			airo_read_stats(dev);
 	}
 
-	return &local->stats;
+	return &dev->stats;
 }
 
 static void airo_set_promisc(struct airo_info *ai) {
@@ -3093,7 +3094,7 @@ static int airo_thread(void *data) {
 		else if (test_bit(JOB_XMIT11, &ai->jobs))
 			airo_end_xmit11(dev);
 		else if (test_bit(JOB_STATS, &ai->jobs))
-			airo_read_stats(ai);
+			airo_read_stats(dev);
 		else if (test_bit(JOB_WSTATS, &ai->jobs))
 			airo_read_wireless_stats(ai);
 		else if (test_bit(JOB_PROMISC, &ai->jobs))
@@ -3289,7 +3290,7 @@ static irqreturn_t airo_interrupt(int irq, void *dev_id)
 
 			skb = dev_alloc_skb( len + hdrlen + 2 + 2 );
 			if ( !skb ) {
-				apriv->stats.rx_dropped++;
+				dev->stats.rx_dropped++;
 				goto badrx;
 			}
 			skb_reserve(skb, 2); /* This way the IP header is aligned */
@@ -3557,7 +3558,7 @@ static void mpi_receive_802_3(struct airo_info *ai)
 
 		skb = dev_alloc_skb(len);
 		if (!skb) {
-			ai->stats.rx_dropped++;
+			ai->dev->stats.rx_dropped++;
 			goto badrx;
 		}
 		buffer = skb_put(skb,len);
@@ -3650,7 +3651,7 @@ void mpi_receive_802_11 (struct airo_info *ai)
 
 	skb = dev_alloc_skb( len + hdrlen + 2 );
 	if ( !skb ) {
-		ai->stats.rx_dropped++;
+		ai->dev->stats.rx_dropped++;
 		goto badrx;
 	}
 	buffer = (u16*)skb_put (skb, len + hdrlen);
@@ -4560,22 +4561,13 @@ static ssize_t proc_read( struct file *file,
 			  size_t len,
 			  loff_t *offset )
 {
-	loff_t pos = *offset;
-	struct proc_data *priv = (struct proc_data*)file->private_data;
+	struct proc_data *priv = file->private_data;
 
 	if (!priv->rbuffer)
 		return -EINVAL;
 
-	if (pos < 0)
-		return -EINVAL;
-	if (pos >= priv->readlen)
-		return 0;
-	if (len > priv->readlen - pos)
-		len = priv->readlen - pos;
-	if (copy_to_user(buffer, priv->rbuffer + pos, len))
-		return -EFAULT;
-	*offset = pos + len;
-	return len;
+	return simple_read_from_buffer(buffer, len, offset, priv->rbuffer,
+					priv->readlen);
 }
 
 /*
@@ -5530,11 +5522,13 @@ static int airo_pci_suspend(struct pci_dev *pdev, pm_message_t state)
 	Cmd cmd;
 	Resp rsp;
 
-	if ((ai->APList == NULL) &&
-		(ai->APList = kmalloc(sizeof(APListRid), GFP_KERNEL)) == NULL)
+	if (!ai->APList)
+		ai->APList = kmalloc(sizeof(APListRid), GFP_KERNEL);
+	if (!ai->APList)
 		return -ENOMEM;
-	if ((ai->SSID == NULL) &&
-		(ai->SSID = kmalloc(sizeof(SsidRid), GFP_KERNEL)) == NULL)
+	if (!ai->SSID)
+		ai->SSID = kmalloc(sizeof(SsidRid), GFP_KERNEL);
+	if (!ai->SSID)
 		return -ENOMEM;
 	readAPListRid(ai, ai->APList);
 	readSsidRid(ai, ai->SSID);
@@ -5545,7 +5539,7 @@ static int airo_pci_suspend(struct pci_dev *pdev, pm_message_t state)
 	disable_MAC(ai, 0);
 	netif_device_detach(dev);
 	ai->power = state;
-	cmd.cmd=HOSTSLEEP;
+	cmd.cmd = HOSTSLEEP;
 	issuecommand(ai, &cmd, &rsp);
 
 	pci_enable_wake(pdev, pci_choose_state(pdev, state), 1);
@@ -5575,7 +5569,7 @@ static int airo_pci_resume(struct pci_dev *pdev)
 		msleep(100);
 	}
 
-	set_bit (FLAG_COMMIT, &ai->flags);
+	set_bit(FLAG_COMMIT, &ai->flags);
 	disable_MAC(ai, 0);
         msleep(200);
 	if (ai->SSID) {
@@ -5602,9 +5596,6 @@ static int airo_pci_resume(struct pci_dev *pdev)
 static int __init airo_init_module( void )
 {
 	int i;
-#if 0
-	int have_isa_dev = 0;
-#endif
 
 	airo_entry = create_proc_entry("driver/aironet",
 				       S_IFDIR | airo_perm,
@@ -5615,15 +5606,11 @@ static int __init airo_init_module( void )
 		airo_entry->gid = proc_gid;
 	}
 
-	for( i = 0; i < 4 && io[i] && irq[i]; i++ ) {
+	for (i = 0; i < 4 && io[i] && irq[i]; i++) {
 		airo_print_info("", "Trying to configure ISA adapter at irq=%d "
 			"io=0x%x", irq[i], io[i] );
 		if (init_airo_card( irq[i], io[i], 0, NULL ))
-#if 0
-			have_isa_dev = 1;
-#else
 			/* do nothing */ ;
-#endif
 	}
 
 #ifdef CONFIG_PCI
@@ -5669,7 +5656,7 @@ static void __exit airo_cleanup_module( void )
 
 static u8 airo_rssi_to_dbm (tdsRssiEntry *rssi_rid, u8 rssi)
 {
-	if( !rssi_rid )
+	if (!rssi_rid)
 		return 0;
 
 	return (0x100 - rssi_rid[rssi].rssidBm);
@@ -5679,10 +5666,10 @@ static u8 airo_dbm_to_pct (tdsRssiEntry *rssi_rid, u8 dbm)
 {
 	int i;
 
-	if( !rssi_rid )
+	if (!rssi_rid)
 		return 0;
 
-	for( i = 0; i < 256; i++ )
+	for (i = 0; i < 256; i++)
 		if (rssi_rid[i].rssidBm == dbm)
 			return rssi_rid[i].rssipct;
 
@@ -7164,6 +7151,7 @@ out:
  * format that the Wireless Tools will understand - Jean II
  */
 static inline char *airo_translate_scan(struct net_device *dev,
+					struct iw_request_info *info,
 					char *current_ev,
 					char *end_buf,
 					BSSListRid *bss)
@@ -7180,7 +7168,8 @@ static inline char *airo_translate_scan(struct net_device *dev,
 	iwe.cmd = SIOCGIWAP;
 	iwe.u.ap_addr.sa_family = ARPHRD_ETHER;
 	memcpy(iwe.u.ap_addr.sa_data, bss->bssid, ETH_ALEN);
-	current_ev = iwe_stream_add_event(current_ev, end_buf, &iwe, IW_EV_ADDR_LEN);
+	current_ev = iwe_stream_add_event(info, current_ev, end_buf,
+					  &iwe, IW_EV_ADDR_LEN);
 
 	/* Other entries will be displayed in the order we give them */
 
@@ -7190,7 +7179,8 @@ static inline char *airo_translate_scan(struct net_device *dev,
 		iwe.u.data.length = 32;
 	iwe.cmd = SIOCGIWESSID;
 	iwe.u.data.flags = 1;
-	current_ev = iwe_stream_add_point(current_ev, end_buf, &iwe, bss->ssid);
+	current_ev = iwe_stream_add_point(info, current_ev, end_buf,
+					  &iwe, bss->ssid);
 
 	/* Add mode */
 	iwe.cmd = SIOCGIWMODE;
@@ -7200,7 +7190,8 @@ static inline char *airo_translate_scan(struct net_device *dev,
 			iwe.u.mode = IW_MODE_MASTER;
 		else
 			iwe.u.mode = IW_MODE_ADHOC;
-		current_ev = iwe_stream_add_event(current_ev, end_buf, &iwe, IW_EV_UINT_LEN);
+		current_ev = iwe_stream_add_event(info, current_ev, end_buf,
+						  &iwe, IW_EV_UINT_LEN);
 	}
 
 	/* Add frequency */
@@ -7211,7 +7202,8 @@ static inline char *airo_translate_scan(struct net_device *dev,
 	 */
 	iwe.u.freq.m = frequency_list[iwe.u.freq.m - 1] * 100000;
 	iwe.u.freq.e = 1;
-	current_ev = iwe_stream_add_event(current_ev, end_buf, &iwe, IW_EV_FREQ_LEN);
+	current_ev = iwe_stream_add_event(info, current_ev, end_buf,
+					  &iwe, IW_EV_FREQ_LEN);
 
 	dBm = le16_to_cpu(bss->dBm);
 
@@ -7231,7 +7223,8 @@ static inline char *airo_translate_scan(struct net_device *dev,
 				| IW_QUAL_DBM;
 	}
 	iwe.u.qual.noise = ai->wstats.qual.noise;
-	current_ev = iwe_stream_add_event(current_ev, end_buf, &iwe, IW_EV_QUAL_LEN);
+	current_ev = iwe_stream_add_event(info, current_ev, end_buf,
+					  &iwe, IW_EV_QUAL_LEN);
 
 	/* Add encryption capability */
 	iwe.cmd = SIOCGIWENCODE;
@@ -7240,11 +7233,12 @@ static inline char *airo_translate_scan(struct net_device *dev,
 	else
 		iwe.u.data.flags = IW_ENCODE_DISABLED;
 	iwe.u.data.length = 0;
-	current_ev = iwe_stream_add_point(current_ev, end_buf, &iwe, bss->ssid);
+	current_ev = iwe_stream_add_point(info, current_ev, end_buf,
+					  &iwe, bss->ssid);
 
 	/* Rate : stuffing multiple values in a single event require a bit
 	 * more of magic - Jean II */
-	current_val = current_ev + IW_EV_LCP_LEN;
+	current_val = current_ev + iwe_stream_lcp_len(info);
 
 	iwe.cmd = SIOCGIWRATE;
 	/* Those two flags are ignored... */
@@ -7257,10 +7251,12 @@ static inline char *airo_translate_scan(struct net_device *dev,
 		/* Bit rate given in 500 kb/s units (+ 0x80) */
 		iwe.u.bitrate.value = ((bss->rates[i] & 0x7f) * 500000);
 		/* Add new value to event */
-		current_val = iwe_stream_add_value(current_ev, current_val, end_buf, &iwe, IW_EV_PARAM_LEN);
+		current_val = iwe_stream_add_value(info, current_ev,
+						   current_val, end_buf,
+						   &iwe, IW_EV_PARAM_LEN);
 	}
 	/* Check if we added any event */
-	if((current_val - current_ev) > IW_EV_LCP_LEN)
+	if ((current_val - current_ev) > iwe_stream_lcp_len(info))
 		current_ev = current_val;
 
 	/* Beacon interval */
@@ -7269,7 +7265,8 @@ static inline char *airo_translate_scan(struct net_device *dev,
 		iwe.cmd = IWEVCUSTOM;
 		sprintf(buf, "bcn_int=%d", bss->beaconInterval);
 		iwe.u.data.length = strlen(buf);
-		current_ev = iwe_stream_add_point(current_ev, end_buf, &iwe, buf);
+		current_ev = iwe_stream_add_point(info, current_ev, end_buf,
+						  &iwe, buf);
 		kfree(buf);
 	}
 
@@ -7303,8 +7300,10 @@ static inline char *airo_translate_scan(struct net_device *dev,
 					iwe.cmd = IWEVGENIE;
 					iwe.u.data.length = min(info_element->len + 2,
 								  MAX_WPA_IE_LEN);
-					current_ev = iwe_stream_add_point(current_ev, end_buf,
-							&iwe, (char *) info_element);
+					current_ev = iwe_stream_add_point(
+							info, current_ev,
+							end_buf, &iwe,
+							(char *) info_element);
 				}
 				break;
 
@@ -7312,8 +7311,9 @@ static inline char *airo_translate_scan(struct net_device *dev,
 				iwe.cmd = IWEVGENIE;
 				iwe.u.data.length = min(info_element->len + 2,
 							  MAX_WPA_IE_LEN);
-				current_ev = iwe_stream_add_point(current_ev, end_buf,
-						&iwe, (char *) info_element);
+				current_ev = iwe_stream_add_point(
+					info, current_ev, end_buf,
+					&iwe, (char *) info_element);
 				break;
 
 			default:
@@ -7352,7 +7352,7 @@ static int airo_get_scan(struct net_device *dev,
 
 	list_for_each_entry (net, &ai->network_list, list) {
 		/* Translate to WE format this entry */
-		current_ev = airo_translate_scan(dev, current_ev,
+		current_ev = airo_translate_scan(dev, info, current_ev,
 						 extra + dwrq->length,
 						 &net->bss);
 
diff --git a/drivers/net/wireless/arlan-main.c b/drivers/net/wireless/arlan-main.c
index dbdfc9e39d20..dec5e874a54d 100644
--- a/drivers/net/wireless/arlan-main.c
+++ b/drivers/net/wireless/arlan-main.c
@@ -125,7 +125,7 @@ static inline int arlan_drop_tx(struct net_device *dev)
 {
 	struct arlan_private *priv = netdev_priv(dev);
 
-	priv->stats.tx_errors++;
+	dev->stats.tx_errors++;
 	if (priv->Conf->tx_delay_ms)
 	{
 		priv->tx_done_delayed = jiffies + priv->Conf->tx_delay_ms * HZ / 1000 + 1;
@@ -1269,7 +1269,7 @@ static void arlan_tx_done_interrupt(struct net_device *dev, int status)
 		{
 			IFDEBUG(ARLAN_DEBUG_TX_CHAIN)
 				printk("arlan intr: transmit OK\n");
-			priv->stats.tx_packets++;
+			dev->stats.tx_packets++;
 			priv->bad = 0;
 			priv->reset = 0;
 			priv->retransmissions = 0;
@@ -1496,7 +1496,7 @@ static void arlan_rx_interrupt(struct net_device *dev, u_char rxStatus, u_short
 			if (skb == NULL)
 			{
 				printk(KERN_ERR "%s: Memory squeeze, dropping packet.\n", dev->name);
-				priv->stats.rx_dropped++;
+				dev->stats.rx_dropped++;
 				break;
 			}
 			skb_reserve(skb, 2);
@@ -1536,14 +1536,14 @@ static void arlan_rx_interrupt(struct net_device *dev, u_char rxStatus, u_short
 				}
 			netif_rx(skb);
 			dev->last_rx = jiffies;
-			priv->stats.rx_packets++;
-			priv->stats.rx_bytes += pkt_len;
+			dev->stats.rx_packets++;
+			dev->stats.rx_bytes += pkt_len;
 		}
 		break;
 		
 		default:
 			printk(KERN_ERR "arlan intr: received unknown status\n");
-			priv->stats.rx_crc_errors++;
+			dev->stats.rx_crc_errors++;
 			break;
 	}
 	ARLAN_DEBUG_EXIT("arlan_rx_interrupt");
@@ -1719,23 +1719,23 @@ static struct net_device_stats *arlan_statistics(struct net_device *dev)
 
 	/* Update the statistics from the device registers. */
 
-	READSHM(priv->stats.collisions, arlan->numReTransmissions, u_int);
-	READSHM(priv->stats.rx_crc_errors, arlan->numCRCErrors, u_int);
-	READSHM(priv->stats.rx_dropped, arlan->numFramesDiscarded, u_int);
-	READSHM(priv->stats.rx_fifo_errors, arlan->numRXBufferOverflows, u_int);
-	READSHM(priv->stats.rx_frame_errors, arlan->numReceiveFramesLost, u_int);
-	READSHM(priv->stats.rx_over_errors, arlan->numRXOverruns, u_int);
-	READSHM(priv->stats.rx_packets, arlan->numDatagramsReceived, u_int);
-	READSHM(priv->stats.tx_aborted_errors, arlan->numAbortErrors, u_int);
-	READSHM(priv->stats.tx_carrier_errors, arlan->numStatusTimeouts, u_int);
-	READSHM(priv->stats.tx_dropped, arlan->numDatagramsDiscarded, u_int);
-	READSHM(priv->stats.tx_fifo_errors, arlan->numTXUnderruns, u_int);
-	READSHM(priv->stats.tx_packets, arlan->numDatagramsTransmitted, u_int);
-	READSHM(priv->stats.tx_window_errors, arlan->numHoldOffs, u_int);
+	READSHM(dev->stats.collisions, arlan->numReTransmissions, u_int);
+	READSHM(dev->stats.rx_crc_errors, arlan->numCRCErrors, u_int);
+	READSHM(dev->stats.rx_dropped, arlan->numFramesDiscarded, u_int);
+	READSHM(dev->stats.rx_fifo_errors, arlan->numRXBufferOverflows, u_int);
+	READSHM(dev->stats.rx_frame_errors, arlan->numReceiveFramesLost, u_int);
+	READSHM(dev->stats.rx_over_errors, arlan->numRXOverruns, u_int);
+	READSHM(dev->stats.rx_packets, arlan->numDatagramsReceived, u_int);
+	READSHM(dev->stats.tx_aborted_errors, arlan->numAbortErrors, u_int);
+	READSHM(dev->stats.tx_carrier_errors, arlan->numStatusTimeouts, u_int);
+	READSHM(dev->stats.tx_dropped, arlan->numDatagramsDiscarded, u_int);
+	READSHM(dev->stats.tx_fifo_errors, arlan->numTXUnderruns, u_int);
+	READSHM(dev->stats.tx_packets, arlan->numDatagramsTransmitted, u_int);
+	READSHM(dev->stats.tx_window_errors, arlan->numHoldOffs, u_int);
 
 	ARLAN_DEBUG_EXIT("arlan_statistics");
 
-	return &priv->stats;
+	return &dev->stats;
 }
 
 
diff --git a/drivers/net/wireless/arlan.h b/drivers/net/wireless/arlan.h
index 3ed1df75900f..fb3ad51a1caf 100644
--- a/drivers/net/wireless/arlan.h
+++ b/drivers/net/wireless/arlan.h
@@ -330,7 +330,6 @@ struct TxParam
 #define TX_RING_SIZE 2
 /* Information that need to be kept for each board. */
 struct arlan_private {
-      struct net_device_stats stats;
       struct arlan_shmem __iomem * card;
       struct arlan_shmem * conf;
 
diff --git a/drivers/net/wireless/ath5k/Kconfig b/drivers/net/wireless/ath5k/Kconfig
index f1f2aea2eab4..75383a5df992 100644
--- a/drivers/net/wireless/ath5k/Kconfig
+++ b/drivers/net/wireless/ath5k/Kconfig
@@ -1,6 +1,9 @@
 config ATH5K
 	tristate "Atheros 5xxx wireless cards support"
 	depends on PCI && MAC80211 && WLAN_80211 && EXPERIMENTAL
+	select MAC80211_LEDS
+	select LEDS_CLASS
+	select NEW_LEDS
 	---help---
 	  This module adds support for wireless adapters based on
 	  Atheros 5xxx chipset.
diff --git a/drivers/net/wireless/ath5k/base.c b/drivers/net/wireless/ath5k/base.c
index 635b9ac9aaa1..217d506527a9 100644
--- a/drivers/net/wireless/ath5k/base.c
+++ b/drivers/net/wireless/ath5k/base.c
@@ -58,11 +58,6 @@
 #include "reg.h"
 #include "debug.h"
 
-enum {
-	ATH_LED_TX,
-	ATH_LED_RX,
-};
-
 static int ath5k_calinterval = 10; /* Calibrate PHY every 10 secs (TODO: Fixme) */
 
 
@@ -167,8 +162,7 @@ static struct pci_driver ath5k_pci_driver = {
 /*
  * Prototypes - MAC 802.11 stack related functions
  */
-static int ath5k_tx(struct ieee80211_hw *hw, struct sk_buff *skb,
-		struct ieee80211_tx_control *ctl);
+static int ath5k_tx(struct ieee80211_hw *hw, struct sk_buff *skb);
 static int ath5k_reset(struct ieee80211_hw *hw);
 static int ath5k_start(struct ieee80211_hw *hw);
 static void ath5k_stop(struct ieee80211_hw *hw);
@@ -196,8 +190,7 @@ static int ath5k_get_tx_stats(struct ieee80211_hw *hw,
 static u64 ath5k_get_tsf(struct ieee80211_hw *hw);
 static void ath5k_reset_tsf(struct ieee80211_hw *hw);
 static int ath5k_beacon_update(struct ieee80211_hw *hw,
-		struct sk_buff *skb,
-		struct ieee80211_tx_control *ctl);
+		struct sk_buff *skb);
 
 static struct ieee80211_ops ath5k_hw_ops = {
 	.tx 		= ath5k_tx,
@@ -214,7 +207,6 @@ static struct ieee80211_ops ath5k_hw_ops = {
 	.get_tx_stats 	= ath5k_get_tx_stats,
 	.get_tsf 	= ath5k_get_tsf,
 	.reset_tsf 	= ath5k_reset_tsf,
-	.beacon_update 	= ath5k_beacon_update,
 };
 
 /*
@@ -251,9 +243,7 @@ static void	ath5k_desc_free(struct ath5k_softc *sc,
 static int 	ath5k_rxbuf_setup(struct ath5k_softc *sc,
 				struct ath5k_buf *bf);
 static int 	ath5k_txbuf_setup(struct ath5k_softc *sc,
-				struct ath5k_buf *bf,
-				struct ieee80211_tx_control *ctl);
-
+				struct ath5k_buf *bf);
 static inline void ath5k_txbuf_free(struct ath5k_softc *sc,
 				struct ath5k_buf *bf)
 {
@@ -289,8 +279,7 @@ static void 	ath5k_tx_processq(struct ath5k_softc *sc,
 static void 	ath5k_tasklet_tx(unsigned long data);
 /* Beacon handling */
 static int 	ath5k_beacon_setup(struct ath5k_softc *sc,
-				struct ath5k_buf *bf,
-				struct ieee80211_tx_control *ctl);
+					struct ath5k_buf *bf);
 static void 	ath5k_beacon_send(struct ath5k_softc *sc);
 static void 	ath5k_beacon_config(struct ath5k_softc *sc);
 static void	ath5k_beacon_update_timers(struct ath5k_softc *sc, u64 bc_tsf);
@@ -314,13 +303,10 @@ static void 	ath5k_tasklet_reset(unsigned long data);
 
 static void 	ath5k_calibrate(unsigned long data);
 /* LED functions */
-static void 	ath5k_led_off(unsigned long data);
-static void 	ath5k_led_blink(struct ath5k_softc *sc,
-				unsigned int on,
-				unsigned int off);
-static void 	ath5k_led_event(struct ath5k_softc *sc,
-				int event);
-
+static int	ath5k_init_leds(struct ath5k_softc *sc);
+static void	ath5k_led_enable(struct ath5k_softc *sc);
+static void	ath5k_led_off(struct ath5k_softc *sc);
+static void	ath5k_unregister_leds(struct ath5k_softc *sc);
 
 /*
  * Module init/exit functions
@@ -458,13 +444,11 @@ ath5k_pci_probe(struct pci_dev *pdev,
 
 	/* Initialize driver private data */
 	SET_IEEE80211_DEV(hw, &pdev->dev);
-	hw->flags = IEEE80211_HW_RX_INCLUDES_FCS;
+	hw->flags = IEEE80211_HW_RX_INCLUDES_FCS |
+		    IEEE80211_HW_SIGNAL_DBM |
+		    IEEE80211_HW_NOISE_DBM;
 	hw->extra_tx_headroom = 2;
 	hw->channel_change_time = 5000;
-	/* these names are misleading */
-	hw->max_rssi = -110; /* signal in dBm */
-	hw->max_noise = -110; /* noise in dBm */
-	hw->max_signal = 100; /* we will provide a percentage based on rssi */
 	sc = hw->priv;
 	sc->hw = hw;
 	sc->pdev = pdev;
@@ -603,8 +587,7 @@ ath5k_pci_suspend(struct pci_dev *pdev, pm_message_t state)
 	struct ieee80211_hw *hw = pci_get_drvdata(pdev);
 	struct ath5k_softc *sc = hw->priv;
 
-	if (test_bit(ATH_STAT_LEDSOFT, sc->status))
-		ath5k_hw_set_gpio(sc->ah, sc->led_pin, 1);
+	ath5k_led_off(sc);
 
 	ath5k_stop_hw(sc);
 	pci_save_state(pdev);
@@ -639,10 +622,7 @@ ath5k_pci_resume(struct pci_dev *pdev)
 	pci_write_config_byte(pdev, 0x41, 0);
 
 	ath5k_init(sc);
-	if (test_bit(ATH_STAT_LEDSOFT, sc->status)) {
-		ath5k_hw_set_gpio_output(ah, sc->led_pin);
-		ath5k_hw_set_gpio(ah, sc->led_pin, 0);
-	}
+	ath5k_led_enable(sc);
 
 	/*
 	 * Reset the key cache since some parts do not
@@ -749,27 +729,6 @@ ath5k_attach(struct pci_dev *pdev, struct ieee80211_hw *hw)
 	tasklet_init(&sc->txtq, ath5k_tasklet_tx, (unsigned long)sc);
 	tasklet_init(&sc->restq, ath5k_tasklet_reset, (unsigned long)sc);
 	setup_timer(&sc->calib_tim, ath5k_calibrate, (unsigned long)sc);
-	setup_timer(&sc->led_tim, ath5k_led_off, (unsigned long)sc);
-
-	sc->led_on = 0; /* low true */
-	/*
-	 * Auto-enable soft led processing for IBM cards and for
-	 * 5211 minipci cards.
-	 */
-	if (pdev->device == PCI_DEVICE_ID_ATHEROS_AR5212_IBM ||
-			pdev->device == PCI_DEVICE_ID_ATHEROS_AR5211) {
-		__set_bit(ATH_STAT_LEDSOFT, sc->status);
-		sc->led_pin = 0;
-	}
-	/* Enable softled on PIN1 on HP Compaq nc6xx, nc4000 & nx5000 laptops */
-	if (pdev->subsystem_vendor == PCI_VENDOR_ID_COMPAQ) {
-		__set_bit(ATH_STAT_LEDSOFT, sc->status);
-		sc->led_pin = 0;
-	}
-	if (test_bit(ATH_STAT_LEDSOFT, sc->status)) {
-		ath5k_hw_set_gpio_output(ah, sc->led_pin);
-		ath5k_hw_set_gpio(ah, sc->led_pin, !sc->led_on);
-	}
 
 	ath5k_hw_get_lladdr(ah, mac);
 	SET_IEEE80211_PERM_ADDR(hw, mac);
@@ -783,6 +742,8 @@ ath5k_attach(struct pci_dev *pdev, struct ieee80211_hw *hw)
 		goto err_queues;
 	}
 
+	ath5k_init_leds(sc);
+
 	return 0;
 err_queues:
 	ath5k_txq_release(sc);
@@ -816,6 +777,7 @@ ath5k_detach(struct pci_dev *pdev, struct ieee80211_hw *hw)
 	ath5k_desc_free(sc, pdev);
 	ath5k_txq_release(sc);
 	ath5k_hw_release_tx_queue(sc->ah, sc->bhalq);
+	ath5k_unregister_leds(sc);
 
 	/*
 	 * NB: can't reclaim these until after ieee80211_ifdetach
@@ -1067,65 +1029,9 @@ ath5k_chan_set(struct ath5k_softc *sc, struct ieee80211_channel *chan)
 	return 0;
 }
 
-/*
- * TODO: CLEAN THIS !!!
- */
 static void
 ath5k_setcurmode(struct ath5k_softc *sc, unsigned int mode)
 {
-	if (unlikely(test_bit(ATH_STAT_LEDSOFT, sc->status))) {
-		/* from Atheros NDIS driver, w/ permission */
-		static const struct {
-			u16 rate;	/* tx/rx 802.11 rate */
-			u16 timeOn;	/* LED on time (ms) */
-			u16 timeOff;	/* LED off time (ms) */
-		} blinkrates[] = {
-			{ 108,  40,  10 },
-			{  96,  44,  11 },
-			{  72,  50,  13 },
-			{  48,  57,  14 },
-			{  36,  67,  16 },
-			{  24,  80,  20 },
-			{  22, 100,  25 },
-			{  18, 133,  34 },
-			{  12, 160,  40 },
-			{  10, 200,  50 },
-			{   6, 240,  58 },
-			{   4, 267,  66 },
-			{   2, 400, 100 },
-			{   0, 500, 130 }
-		};
-		const struct ath5k_rate_table *rt =
-				ath5k_hw_get_rate_table(sc->ah, mode);
-		unsigned int i, j;
-
-		BUG_ON(rt == NULL);
-
-		memset(sc->hwmap, 0, sizeof(sc->hwmap));
-		for (i = 0; i < 32; i++) {
-			u8 ix = rt->rate_code_to_index[i];
-			if (ix == 0xff) {
-				sc->hwmap[i].ledon = msecs_to_jiffies(500);
-				sc->hwmap[i].ledoff = msecs_to_jiffies(130);
-				continue;
-			}
-			sc->hwmap[i].txflags = IEEE80211_RADIOTAP_F_DATAPAD;
-			/* receive frames include FCS */
-			sc->hwmap[i].rxflags = sc->hwmap[i].txflags |
-					IEEE80211_RADIOTAP_F_FCS;
-			/* setup blink rate table to avoid per-packet lookup */
-			for (j = 0; j < ARRAY_SIZE(blinkrates) - 1; j++)
-				if (blinkrates[j].rate == /* XXX why 7f? */
-						(rt->rates[ix].dot11_rate&0x7f))
-					break;
-
-			sc->hwmap[i].ledon = msecs_to_jiffies(blinkrates[j].
-					timeOn);
-			sc->hwmap[i].ledoff = msecs_to_jiffies(blinkrates[j].
-					timeOff);
-		}
-	}
-
 	sc->curmode = mode;
 
 	if (mode == AR5K_MODE_11A) {
@@ -1297,36 +1203,36 @@ ath5k_rxbuf_setup(struct ath5k_softc *sc, struct ath5k_buf *bf)
 }
 
 static int
-ath5k_txbuf_setup(struct ath5k_softc *sc, struct ath5k_buf *bf,
-		struct ieee80211_tx_control *ctl)
+ath5k_txbuf_setup(struct ath5k_softc *sc, struct ath5k_buf *bf)
 {
 	struct ath5k_hw *ah = sc->ah;
 	struct ath5k_txq *txq = sc->txq;
 	struct ath5k_desc *ds = bf->desc;
 	struct sk_buff *skb = bf->skb;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 	unsigned int pktlen, flags, keyidx = AR5K_TXKEYIX_INVALID;
 	int ret;
 
 	flags = AR5K_TXDESC_INTREQ | AR5K_TXDESC_CLRDMASK;
-	bf->ctl = *ctl;
+
 	/* XXX endianness */
 	bf->skbaddr = pci_map_single(sc->pdev, skb->data, skb->len,
 			PCI_DMA_TODEVICE);
 
-	if (ctl->flags & IEEE80211_TXCTL_NO_ACK)
+	if (info->flags & IEEE80211_TX_CTL_NO_ACK)
 		flags |= AR5K_TXDESC_NOACK;
 
 	pktlen = skb->len;
 
-	if (!(ctl->flags & IEEE80211_TXCTL_DO_NOT_ENCRYPT)) {
-		keyidx = ctl->key_idx;
-		pktlen += ctl->icv_len;
+	if (!(info->flags & IEEE80211_TX_CTL_DO_NOT_ENCRYPT)) {
+		keyidx = info->control.hw_key->hw_key_idx;
+		pktlen += info->control.icv_len;
 	}
-
 	ret = ah->ah_setup_tx_desc(ah, ds, pktlen,
 		ieee80211_get_hdrlen_from_skb(skb), AR5K_PKT_TYPE_NORMAL,
-		(sc->power_level * 2), ctl->tx_rate->hw_value,
-		ctl->retry_limit, keyidx, 0, flags, 0, 0);
+		(sc->power_level * 2),
+		ieee80211_get_tx_rate(sc->hw, info)->hw_value,
+		info->control.retry_limit, keyidx, 0, flags, 0, 0);
 	if (ret)
 		goto err_unmap;
 
@@ -1335,7 +1241,7 @@ ath5k_txbuf_setup(struct ath5k_softc *sc, struct ath5k_buf *bf,
 
 	spin_lock_bh(&txq->lock);
 	list_add_tail(&bf->list, &txq->q);
-	sc->tx_stats.data[txq->qnum].len++;
+	sc->tx_stats[txq->qnum].len++;
 	if (txq->link == NULL) /* is this first packet? */
 		ath5k_hw_put_tx_buf(ah, txq->qnum, bf->daddr);
 	else /* no, so only link it */
@@ -1566,7 +1472,7 @@ ath5k_txq_drainq(struct ath5k_softc *sc, struct ath5k_txq *txq)
 		ath5k_txbuf_free(sc, bf);
 
 		spin_lock_bh(&sc->txbuflock);
-		sc->tx_stats.data[txq->qnum].len--;
+		sc->tx_stats[txq->qnum].len--;
 		list_move_tail(&bf->list, &sc->txbuf);
 		sc->txbuf_len++;
 		spin_unlock_bh(&sc->txbuflock);
@@ -1601,7 +1507,7 @@ ath5k_txq_cleanup(struct ath5k_softc *sc)
 					sc->txqs[i].link);
 			}
 	}
-	ieee80211_start_queues(sc->hw); /* XXX move to callers */
+	ieee80211_wake_queues(sc->hw); /* XXX move to callers */
 
 	for (i = 0; i < ARRAY_SIZE(sc->txqs); i++)
 		if (sc->txqs[i].setup)
@@ -1698,9 +1604,9 @@ ath5k_rx_decrypted(struct ath5k_softc *sc, struct ath5k_desc *ds,
 	/* Apparently when a default key is used to decrypt the packet
 	   the hw does not set the index used to decrypt.  In such cases
 	   get the index from the packet. */
-	if ((le16_to_cpu(hdr->frame_control) & IEEE80211_FCTL_PROTECTED) &&
-			!(rs->rs_status & AR5K_RXERR_DECRYPT) &&
-			skb->len >= hlen + 4) {
+	if (ieee80211_has_protected(hdr->frame_control) &&
+	    !(rs->rs_status & AR5K_RXERR_DECRYPT) &&
+	    skb->len >= hlen + 4) {
 		keyix = skb->data[hlen + 3] >> 6;
 
 		if (test_bit(keyix, sc->keymap))
@@ -1719,10 +1625,7 @@ ath5k_check_ibss_tsf(struct ath5k_softc *sc, struct sk_buff *skb,
 	u32 hw_tu;
 	struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)skb->data;
 
-	if ((le16_to_cpu(mgmt->frame_control) & IEEE80211_FCTL_FTYPE) ==
-		IEEE80211_FTYPE_MGMT &&
-	    (le16_to_cpu(mgmt->frame_control) & IEEE80211_FCTL_STYPE) ==
-		IEEE80211_STYPE_BEACON &&
+	if (ieee80211_is_beacon(mgmt->frame_control) &&
 	    le16_to_cpu(mgmt->u.beacon.capab_info) & WLAN_CAPABILITY_IBSS &&
 	    memcmp(mgmt->bssid, sc->ah->ah_bssid, ETH_ALEN) == 0) {
 		/*
@@ -1895,20 +1798,9 @@ accept:
 		rxs.freq = sc->curchan->center_freq;
 		rxs.band = sc->curband->band;
 
-		/*
-		 * signal quality:
-		 * the names here are misleading and the usage of these
-		 * values by iwconfig makes it even worse
-		 */
-		/* noise floor in dBm, from the last noise calibration */
 		rxs.noise = sc->ah->ah_noise_floor;
-		/* signal level in dBm */
-		rxs.ssi = rxs.noise + rs.rs_rssi;
-		/*
-		 * "signal" is actually displayed as Link Quality by iwconfig
-		 * we provide a percentage based on rssi (assuming max rssi 64)
-		 */
-		rxs.signal = rs.rs_rssi * 100 / 64;
+		rxs.signal = rxs.noise + rs.rs_rssi;
+		rxs.qual = rs.rs_rssi * 100 / 64;
 
 		rxs.antenna = rs.rs_antenna;
 		rxs.rate_idx = ath5k_hw_to_driver_rix(sc, rs.rs_rate);
@@ -1921,8 +1813,6 @@ accept:
 			ath5k_check_ibss_tsf(sc, skb, &rxs);
 
 		__ieee80211_rx(sc->hw, skb, &rxs);
-		sc->led_rxrate = rs.rs_rate;
-		ath5k_led_event(sc, ATH_LED_RX);
 next:
 		list_move_tail(&bf->list, &sc->rxbuf);
 	} while (ath5k_rxbuf_setup(sc, bf) == 0);
@@ -1939,11 +1829,11 @@ next:
 static void
 ath5k_tx_processq(struct ath5k_softc *sc, struct ath5k_txq *txq)
 {
-	struct ieee80211_tx_status txs = {};
 	struct ath5k_tx_status ts = {};
 	struct ath5k_buf *bf, *bf0;
 	struct ath5k_desc *ds;
 	struct sk_buff *skb;
+	struct ieee80211_tx_info *info;
 	int ret;
 
 	spin_lock(&txq->lock);
@@ -1963,28 +1853,29 @@ ath5k_tx_processq(struct ath5k_softc *sc, struct ath5k_txq *txq)
 		}
 
 		skb = bf->skb;
+		info = IEEE80211_SKB_CB(skb);
 		bf->skb = NULL;
+
 		pci_unmap_single(sc->pdev, bf->skbaddr, skb->len,
 				PCI_DMA_TODEVICE);
 
-		txs.control = bf->ctl;
-		txs.retry_count = ts.ts_shortretry + ts.ts_longretry / 6;
+		info->status.retry_count = ts.ts_shortretry + ts.ts_longretry / 6;
 		if (unlikely(ts.ts_status)) {
 			sc->ll_stats.dot11ACKFailureCount++;
 			if (ts.ts_status & AR5K_TXERR_XRETRY)
-				txs.excessive_retries = 1;
+				info->status.excessive_retries = 1;
 			else if (ts.ts_status & AR5K_TXERR_FILT)
-				txs.flags |= IEEE80211_TX_STATUS_TX_FILTERED;
+				info->flags |= IEEE80211_TX_STAT_TX_FILTERED;
 		} else {
-			txs.flags |= IEEE80211_TX_STATUS_ACK;
-			txs.ack_signal = ts.ts_rssi;
+			info->flags |= IEEE80211_TX_STAT_ACK;
+			info->status.ack_signal = ts.ts_rssi;
 		}
 
-		ieee80211_tx_status(sc->hw, skb, &txs);
-		sc->tx_stats.data[txq->qnum].count++;
+		ieee80211_tx_status(sc->hw, skb);
+		sc->tx_stats[txq->qnum].count++;
 
 		spin_lock(&sc->txbuflock);
-		sc->tx_stats.data[txq->qnum].len--;
+		sc->tx_stats[txq->qnum].len--;
 		list_move_tail(&bf->list, &sc->txbuf);
 		sc->txbuf_len++;
 		spin_unlock(&sc->txbuflock);
@@ -2002,13 +1893,9 @@ ath5k_tasklet_tx(unsigned long data)
 	struct ath5k_softc *sc = (void *)data;
 
 	ath5k_tx_processq(sc, sc->txq);
-
-	ath5k_led_event(sc, ATH_LED_TX);
 }
 
 
-
-
 /*****************\
 * Beacon handling *
 \*****************/
@@ -2017,10 +1904,10 @@ ath5k_tasklet_tx(unsigned long data)
  * Setup the beacon frame for transmit.
  */
 static int
-ath5k_beacon_setup(struct ath5k_softc *sc, struct ath5k_buf *bf,
-		struct ieee80211_tx_control *ctl)
+ath5k_beacon_setup(struct ath5k_softc *sc, struct ath5k_buf *bf)
 {
 	struct sk_buff *skb = bf->skb;
+	struct	ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 	struct ath5k_hw *ah = sc->ah;
 	struct ath5k_desc *ds;
 	int ret, antenna = 0;
@@ -2059,7 +1946,8 @@ ath5k_beacon_setup(struct ath5k_softc *sc, struct ath5k_buf *bf,
 	ret = ah->ah_setup_tx_desc(ah, ds, skb->len,
 			ieee80211_get_hdrlen_from_skb(skb),
 			AR5K_PKT_TYPE_BEACON, (sc->power_level * 2),
-			ctl->tx_rate->hw_value, 1, AR5K_TXKEYIX_INVALID,
+			ieee80211_get_tx_rate(sc->hw, info)->hw_value,
+			1, AR5K_TXKEYIX_INVALID,
 			antenna, flags, 0, 0);
 	if (ret)
 		goto err_unmap;
@@ -2382,11 +2270,7 @@ ath5k_stop_locked(struct ath5k_softc *sc)
 	ieee80211_stop_queues(sc->hw);
 
 	if (!test_bit(ATH_STAT_INVALID, sc->status)) {
-		if (test_bit(ATH_STAT_LEDSOFT, sc->status)) {
-			del_timer_sync(&sc->led_tim);
-			ath5k_hw_set_gpio(ah, sc->led_pin, !sc->led_on);
-			__clear_bit(ATH_STAT_LEDBLINKING, sc->status);
-		}
+		ath5k_led_off(sc);
 		ath5k_hw_set_intr(ah, 0);
 	}
 	ath5k_txq_cleanup(sc);
@@ -2582,54 +2466,123 @@ ath5k_calibrate(unsigned long data)
 \***************/
 
 static void
-ath5k_led_off(unsigned long data)
+ath5k_led_enable(struct ath5k_softc *sc)
 {
-	struct ath5k_softc *sc = (void *)data;
-
-	if (test_bit(ATH_STAT_LEDENDBLINK, sc->status))
-		__clear_bit(ATH_STAT_LEDBLINKING, sc->status);
-	else {
-		__set_bit(ATH_STAT_LEDENDBLINK, sc->status);
-		ath5k_hw_set_gpio(sc->ah, sc->led_pin, !sc->led_on);
-		mod_timer(&sc->led_tim, jiffies + sc->led_off);
+	if (test_bit(ATH_STAT_LEDSOFT, sc->status)) {
+		ath5k_hw_set_gpio_output(sc->ah, sc->led_pin);
+		ath5k_led_off(sc);
 	}
 }
 
-/*
- * Blink the LED according to the specified on/off times.
- */
 static void
-ath5k_led_blink(struct ath5k_softc *sc, unsigned int on,
-		unsigned int off)
+ath5k_led_on(struct ath5k_softc *sc)
 {
-	ATH5K_DBG(sc, ATH5K_DEBUG_LED, "on %u off %u\n", on, off);
+	if (!test_bit(ATH_STAT_LEDSOFT, sc->status))
+		return;
 	ath5k_hw_set_gpio(sc->ah, sc->led_pin, sc->led_on);
-	__set_bit(ATH_STAT_LEDBLINKING, sc->status);
-	__clear_bit(ATH_STAT_LEDENDBLINK, sc->status);
-	sc->led_off = off;
-	mod_timer(&sc->led_tim, jiffies + on);
 }
 
 static void
-ath5k_led_event(struct ath5k_softc *sc, int event)
+ath5k_led_off(struct ath5k_softc *sc)
 {
-	if (likely(!test_bit(ATH_STAT_LEDSOFT, sc->status)))
+	if (!test_bit(ATH_STAT_LEDSOFT, sc->status))
 		return;
-	if (unlikely(test_bit(ATH_STAT_LEDBLINKING, sc->status)))
-		return; /* don't interrupt active blink */
-	switch (event) {
-	case ATH_LED_TX:
-		ath5k_led_blink(sc, sc->hwmap[sc->led_txrate].ledon,
-			sc->hwmap[sc->led_txrate].ledoff);
-		break;
-	case ATH_LED_RX:
-		ath5k_led_blink(sc, sc->hwmap[sc->led_rxrate].ledon,
-			sc->hwmap[sc->led_rxrate].ledoff);
-		break;
+	ath5k_hw_set_gpio(sc->ah, sc->led_pin, !sc->led_on);
+}
+
+static void
+ath5k_led_brightness_set(struct led_classdev *led_dev,
+	enum led_brightness brightness)
+{
+	struct ath5k_led *led = container_of(led_dev, struct ath5k_led,
+		led_dev);
+
+	if (brightness == LED_OFF)
+		ath5k_led_off(led->sc);
+	else
+		ath5k_led_on(led->sc);
+}
+
+static int
+ath5k_register_led(struct ath5k_softc *sc, struct ath5k_led *led,
+		   const char *name, char *trigger)
+{
+	int err;
+
+	led->sc = sc;
+	strncpy(led->name, name, sizeof(led->name));
+	led->led_dev.name = led->name;
+	led->led_dev.default_trigger = trigger;
+	led->led_dev.brightness_set = ath5k_led_brightness_set;
+
+	err = led_classdev_register(&sc->pdev->dev, &led->led_dev);
+	if (err)
+	{
+		ATH5K_WARN(sc, "could not register LED %s\n", name);
+		led->sc = NULL;
 	}
+	return err;
 }
 
+static void
+ath5k_unregister_led(struct ath5k_led *led)
+{
+	if (!led->sc)
+		return;
+	led_classdev_unregister(&led->led_dev);
+	ath5k_led_off(led->sc);
+	led->sc = NULL;
+}
+
+static void
+ath5k_unregister_leds(struct ath5k_softc *sc)
+{
+	ath5k_unregister_led(&sc->rx_led);
+	ath5k_unregister_led(&sc->tx_led);
+}
+
+
+static int
+ath5k_init_leds(struct ath5k_softc *sc)
+{
+	int ret = 0;
+	struct ieee80211_hw *hw = sc->hw;
+	struct pci_dev *pdev = sc->pdev;
+	char name[ATH5K_LED_MAX_NAME_LEN + 1];
+
+	sc->led_on = 0;  /* active low */
 
+	/*
+	 * Auto-enable soft led processing for IBM cards and for
+	 * 5211 minipci cards.
+	 */
+	if (pdev->device == PCI_DEVICE_ID_ATHEROS_AR5212_IBM ||
+	    pdev->device == PCI_DEVICE_ID_ATHEROS_AR5211) {
+		__set_bit(ATH_STAT_LEDSOFT, sc->status);
+		sc->led_pin = 0;
+	}
+	/* Enable softled on PIN1 on HP Compaq nc6xx, nc4000 & nx5000 laptops */
+	if (pdev->subsystem_vendor == PCI_VENDOR_ID_COMPAQ) {
+		__set_bit(ATH_STAT_LEDSOFT, sc->status);
+		sc->led_pin = 1;
+	}
+	if (!test_bit(ATH_STAT_LEDSOFT, sc->status))
+		goto out;
+
+	ath5k_led_enable(sc);
+
+	snprintf(name, sizeof(name), "ath5k-%s::rx", wiphy_name(hw->wiphy));
+	ret = ath5k_register_led(sc, &sc->rx_led, name,
+		ieee80211_get_rx_led_name(hw));
+	if (ret)
+		goto out;
+
+	snprintf(name, sizeof(name), "ath5k-%s::tx", wiphy_name(hw->wiphy));
+	ret = ath5k_register_led(sc, &sc->tx_led, name,
+		ieee80211_get_tx_led_name(hw));
+out:
+	return ret;
+}
 
 
 /********************\
@@ -2637,8 +2590,7 @@ ath5k_led_event(struct ath5k_softc *sc, int event)
 \********************/
 
 static int
-ath5k_tx(struct ieee80211_hw *hw, struct sk_buff *skb,
-			struct ieee80211_tx_control *ctl)
+ath5k_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
 	struct ath5k_softc *sc = hw->priv;
 	struct ath5k_buf *bf;
@@ -2667,13 +2619,11 @@ ath5k_tx(struct ieee80211_hw *hw, struct sk_buff *skb,
 		memmove(skb->data, skb->data+pad, hdrlen);
 	}
 
-	sc->led_txrate = ctl->tx_rate->hw_value;
-
 	spin_lock_irqsave(&sc->txbuflock, flags);
 	if (list_empty(&sc->txbuf)) {
 		ATH5K_ERR(sc, "no further txbuf available, dropping packet\n");
 		spin_unlock_irqrestore(&sc->txbuflock, flags);
-		ieee80211_stop_queue(hw, ctl->queue);
+		ieee80211_stop_queue(hw, skb_get_queue_mapping(skb));
 		return -1;
 	}
 	bf = list_first_entry(&sc->txbuf, struct ath5k_buf, list);
@@ -2685,7 +2635,7 @@ ath5k_tx(struct ieee80211_hw *hw, struct sk_buff *skb,
 
 	bf->skb = skb;
 
-	if (ath5k_txbuf_setup(sc, bf, ctl)) {
+	if (ath5k_txbuf_setup(sc, bf)) {
 		bf->skb = NULL;
 		spin_lock_irqsave(&sc->txbuflock, flags);
 		list_add_tail(&bf->list, &sc->txbuf);
@@ -2834,6 +2784,18 @@ ath5k_config_interface(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 		 * a clean way of letting us retrieve this yet. */
 		ath5k_hw_set_associd(ah, ah->ah_bssid, 0);
 	}
+
+	if (conf->changed & IEEE80211_IFCC_BEACON &&
+	    vif->type == IEEE80211_IF_TYPE_IBSS) {
+		struct sk_buff *beacon = ieee80211_beacon_get(hw, vif);
+		if (!beacon) {
+			ret = -ENOMEM;
+			goto unlock;
+		}
+		/* call old handler for now */
+		ath5k_beacon_update(hw, beacon);
+	}
+
 	mutex_unlock(&sc->lock);
 
 	return ath5k_reset(hw);
@@ -3063,8 +3025,7 @@ ath5k_reset_tsf(struct ieee80211_hw *hw)
 }
 
 static int
-ath5k_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb,
-			struct ieee80211_tx_control *ctl)
+ath5k_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
 	struct ath5k_softc *sc = hw->priv;
 	int ret;
@@ -3080,7 +3041,7 @@ ath5k_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb,
 
 	ath5k_txbuf_free(sc, sc->bbuf);
 	sc->bbuf->skb = skb;
-	ret = ath5k_beacon_setup(sc, sc->bbuf, ctl);
+	ret = ath5k_beacon_setup(sc, sc->bbuf);
 	if (ret)
 		sc->bbuf->skb = NULL;
 	else
diff --git a/drivers/net/wireless/ath5k/base.h b/drivers/net/wireless/ath5k/base.h
index 3a9755893018..47f414b09e67 100644
--- a/drivers/net/wireless/ath5k/base.h
+++ b/drivers/net/wireless/ath5k/base.h
@@ -45,6 +45,7 @@
 #include <linux/list.h>
 #include <linux/wireless.h>
 #include <linux/if_ether.h>
+#include <linux/leds.h>
 
 #include "ath5k.h"
 #include "debug.h"
@@ -60,7 +61,6 @@ struct ath5k_buf {
 	dma_addr_t		daddr;	/* physical addr of desc */
 	struct sk_buff		*skb;	/* skbuff for buf */
 	dma_addr_t		skbaddr;/* physical addr of skb data */
-	struct ieee80211_tx_control ctl;
 };
 
 /*
@@ -80,6 +80,19 @@ struct ath5k_txq {
 	bool			setup;
 };
 
+#define ATH5K_LED_MAX_NAME_LEN 31
+
+/*
+ * State for LED triggers
+ */
+struct ath5k_led
+{
+	char name[ATH5K_LED_MAX_NAME_LEN + 1];	/* name of the LED in sysfs */
+	struct ath5k_softc *sc;			/* driver state */
+	struct led_classdev led_dev;		/* led classdev */
+};
+
+
 #if CHAN_DEBUG
 #define ATH_CHAN_MAX	(26+26+26+200+200)
 #else
@@ -92,7 +105,8 @@ struct ath5k_softc {
 	struct pci_dev		*pdev;		/* for dma mapping */
 	void __iomem		*iobase;	/* address of the device */
 	struct mutex		lock;		/* dev-level lock */
-	struct ieee80211_tx_queue_stats tx_stats;
+	/* FIXME: how many does it really need? */
+	struct ieee80211_tx_queue_stats tx_stats[16];
 	struct ieee80211_low_level_stats ll_stats;
 	struct ieee80211_hw	*hw;		/* IEEE 802.11 common */
 	struct ieee80211_supported_band sbands[IEEE80211_NUM_BANDS];
@@ -118,13 +132,11 @@ struct ath5k_softc {
 	size_t			desc_len;	/* size of TX/RX descriptors */
 	u16			cachelsz;	/* cache line size */
 
-	DECLARE_BITMAP(status, 6);
+	DECLARE_BITMAP(status, 4);
 #define ATH_STAT_INVALID	0		/* disable hardware accesses */
 #define ATH_STAT_MRRETRY	1		/* multi-rate retry support */
 #define ATH_STAT_PROMISC	2
-#define ATH_STAT_LEDBLINKING	3		/* LED blink operation active */
-#define ATH_STAT_LEDENDBLINK	4		/* finish LED blink operation */
-#define ATH_STAT_LEDSOFT	5		/* enable LED gpio status */
+#define ATH_STAT_LEDSOFT	3		/* enable LED gpio status */
 
 	unsigned int		filter_flags;	/* HW flags, AR5K_RX_FILTER_* */
 	unsigned int		curmode;	/* current phy mode */
@@ -132,13 +144,6 @@ struct ath5k_softc {
 
 	struct ieee80211_vif *vif;
 
-	struct {
-		u8	rxflags;	/* radiotap rx flags */
-		u8	txflags;	/* radiotap tx flags */
-		u16	ledon;		/* softled on time */
-		u16	ledoff;		/* softled off time */
-	} hwmap[32];				/* h/w rate ix mappings */
-
 	enum ath5k_int		imask;		/* interrupt mask copy */
 
 	DECLARE_BITMAP(keymap, AR5K_KEYCACHE_SIZE); /* key use bit map */
@@ -148,9 +153,6 @@ struct ath5k_softc {
 	unsigned int		led_pin,	/* GPIO pin for driving LED */
 				led_on,		/* pin setting for LED on */
 				led_off;	/* off time for current blink */
-	struct timer_list	led_tim;	/* led off timer */
-	u8			led_rxrate;	/* current rx rate for LED */
-	u8			led_txrate;	/* current tx rate for LED */
 
 	struct tasklet_struct	restq;		/* reset tasklet */
 
@@ -159,6 +161,7 @@ struct ath5k_softc {
 	spinlock_t		rxbuflock;
 	u32			*rxlink;	/* link ptr in last RX desc */
 	struct tasklet_struct	rxtq;		/* rx intr tasklet */
+	struct ath5k_led	rx_led;		/* rx led */
 
 	struct list_head	txbuf;		/* transmit buffer */
 	spinlock_t		txbuflock;
@@ -167,6 +170,7 @@ struct ath5k_softc {
 
 	struct ath5k_txq	*txq;		/* beacon and tx*/
 	struct tasklet_struct	txtq;		/* tx intr tasklet */
+	struct ath5k_led	tx_led;		/* tx led */
 
 	struct ath5k_buf	*bbuf;		/* beacon buffer */
 	unsigned int		bhalq,		/* SW q for outgoing beacons */
diff --git a/drivers/net/wireless/ath5k/hw.c b/drivers/net/wireless/ath5k/hw.c
index 77990b56860b..c6d12c53bda4 100644
--- a/drivers/net/wireless/ath5k/hw.c
+++ b/drivers/net/wireless/ath5k/hw.c
@@ -31,14 +31,14 @@
 #include "base.h"
 #include "debug.h"
 
-/*Rate tables*/
+/* Rate tables */
 static const struct ath5k_rate_table ath5k_rt_11a = AR5K_RATES_11A;
 static const struct ath5k_rate_table ath5k_rt_11b = AR5K_RATES_11B;
 static const struct ath5k_rate_table ath5k_rt_11g = AR5K_RATES_11G;
 static const struct ath5k_rate_table ath5k_rt_turbo = AR5K_RATES_TURBO;
 static const struct ath5k_rate_table ath5k_rt_xr = AR5K_RATES_XR;
 
-/*Prototypes*/
+/* Prototypes */
 static int ath5k_hw_nic_reset(struct ath5k_hw *, u32);
 static int ath5k_hw_nic_wakeup(struct ath5k_hw *, int, bool);
 static int ath5k_hw_setup_4word_tx_desc(struct ath5k_hw *, struct ath5k_desc *,
diff --git a/drivers/net/wireless/atmel.c b/drivers/net/wireless/atmel.c
index 438e63ecccf1..bd35bb0a1480 100644
--- a/drivers/net/wireless/atmel.c
+++ b/drivers/net/wireless/atmel.c
@@ -433,7 +433,6 @@ struct atmel_private {
 	struct net_device *dev;
 	struct device *sys_dev;
 	struct iw_statistics wstats;
-	struct net_device_stats	stats;	// device stats
 	spinlock_t irqlock, timerlock;	// spinlocks
 	enum { BUS_TYPE_PCCARD, BUS_TYPE_PCI } bus_type;
 	enum {
@@ -560,7 +559,7 @@ static const struct {
 static void build_wpa_mib(struct atmel_private *priv);
 static int atmel_ioctl(struct net_device *dev, struct ifreq *rq, int cmd);
 static void atmel_copy_to_card(struct net_device *dev, u16 dest,
-			       unsigned char *src, u16 len);
+			       const unsigned char *src, u16 len);
 static void atmel_copy_to_host(struct net_device *dev, unsigned char *dest,
 			       u16 src, u16 len);
 static void atmel_set_gcr(struct net_device *dev, u16 mask);
@@ -694,9 +693,9 @@ static void tx_done_irq(struct atmel_private *priv)
 
 		if (type == TX_PACKET_TYPE_DATA) {
 			if (status == TX_STATUS_SUCCESS)
-				priv->stats.tx_packets++;
+				priv->dev->stats.tx_packets++;
 			else
-				priv->stats.tx_errors++;
+				priv->dev->stats.tx_errors++;
 			netif_wake_queue(priv->dev);
 		}
 	}
@@ -792,13 +791,13 @@ static int start_tx(struct sk_buff *skb, struct net_device *dev)
 
 	if (priv->card && priv->present_callback &&
 	    !(*priv->present_callback)(priv->card)) {
-		priv->stats.tx_errors++;
+		dev->stats.tx_errors++;
 		dev_kfree_skb(skb);
 		return 0;
 	}
 
 	if (priv->station_state != STATION_STATE_READY) {
-		priv->stats.tx_errors++;
+		dev->stats.tx_errors++;
 		dev_kfree_skb(skb);
 		return 0;
 	}
@@ -815,7 +814,7 @@ static int start_tx(struct sk_buff *skb, struct net_device *dev)
 	   initial + 18 (+30-12) */
 
 	if (!(buff = find_tx_buff(priv, len + 18))) {
-		priv->stats.tx_dropped++;
+		dev->stats.tx_dropped++;
 		spin_unlock_irqrestore(&priv->irqlock, flags);
 		spin_unlock_bh(&priv->timerlock);
 		netif_stop_queue(dev);
@@ -851,7 +850,7 @@ static int start_tx(struct sk_buff *skb, struct net_device *dev)
 	/* low bit of first byte of destination tells us if broadcast */
 	tx_update_descriptor(priv, *(skb->data) & 0x01, len + 18, buff, TX_PACKET_TYPE_DATA);
 	dev->trans_start = jiffies;
-	priv->stats.tx_bytes += len;
+	dev->stats.tx_bytes += len;
 
 	spin_unlock_irqrestore(&priv->irqlock, flags);
 	spin_unlock_bh(&priv->timerlock);
@@ -895,7 +894,7 @@ static void fast_rx_path(struct atmel_private *priv,
 	}
 
 	if (!(skb = dev_alloc_skb(msdu_size + 14))) {
-		priv->stats.rx_dropped++;
+		priv->dev->stats.rx_dropped++;
 		return;
 	}
 
@@ -908,7 +907,7 @@ static void fast_rx_path(struct atmel_private *priv,
 		crc = crc32_le(crc, skbp + 12, msdu_size);
 		atmel_copy_to_host(priv->dev, (void *)&netcrc, rx_packet_loc + 30 + msdu_size, 4);
 		if ((crc ^ 0xffffffff) != netcrc) {
-			priv->stats.rx_crc_errors++;
+			priv->dev->stats.rx_crc_errors++;
 			dev_kfree_skb(skb);
 			return;
 		}
@@ -924,8 +923,8 @@ static void fast_rx_path(struct atmel_private *priv,
 	skb->protocol = eth_type_trans(skb, priv->dev);
 	skb->ip_summed = CHECKSUM_NONE;
 	netif_rx(skb);
-	priv->stats.rx_bytes += 12 + msdu_size;
-	priv->stats.rx_packets++;
+	priv->dev->stats.rx_bytes += 12 + msdu_size;
+	priv->dev->stats.rx_packets++;
 }
 
 /* Test to see if the packet in card memory at packet_loc has a valid CRC
@@ -991,7 +990,7 @@ static void frag_rx_path(struct atmel_private *priv,
 			crc = crc32_le(crc, &priv->rx_buf[12], msdu_size);
 			atmel_copy_to_host(priv->dev, (void *)&netcrc, rx_packet_loc + msdu_size, 4);
 			if ((crc ^ 0xffffffff) != netcrc) {
-				priv->stats.rx_crc_errors++;
+				priv->dev->stats.rx_crc_errors++;
 				memset(priv->frag_source, 0xff, 6);
 			}
 		}
@@ -1009,7 +1008,7 @@ static void frag_rx_path(struct atmel_private *priv,
 				       msdu_size);
 			atmel_copy_to_host(priv->dev, (void *)&netcrc, rx_packet_loc + msdu_size, 4);
 			if ((crc ^ 0xffffffff) != netcrc) {
-				priv->stats.rx_crc_errors++;
+				priv->dev->stats.rx_crc_errors++;
 				memset(priv->frag_source, 0xff, 6);
 				more_frags = 1; /* don't send broken assembly */
 			}
@@ -1021,7 +1020,7 @@ static void frag_rx_path(struct atmel_private *priv,
 		if (!more_frags) { /* last one */
 			memset(priv->frag_source, 0xff, 6);
 			if (!(skb = dev_alloc_skb(priv->frag_len + 14))) {
-				priv->stats.rx_dropped++;
+				priv->dev->stats.rx_dropped++;
 			} else {
 				skb_reserve(skb, 2);
 				memcpy(skb_put(skb, priv->frag_len + 12),
@@ -1031,8 +1030,8 @@ static void frag_rx_path(struct atmel_private *priv,
 				skb->protocol = eth_type_trans(skb, priv->dev);
 				skb->ip_summed = CHECKSUM_NONE;
 				netif_rx(skb);
-				priv->stats.rx_bytes += priv->frag_len + 12;
-				priv->stats.rx_packets++;
+				priv->dev->stats.rx_bytes += priv->frag_len + 12;
+				priv->dev->stats.rx_packets++;
 			}
 		}
 	} else
@@ -1057,7 +1056,7 @@ static void rx_done_irq(struct atmel_private *priv)
 			if (status == 0xc1) /* determined by experiment */
 				priv->wstats.discard.nwid++;
 			else
-				priv->stats.rx_errors++;
+				priv->dev->stats.rx_errors++;
 			goto next;
 		}
 
@@ -1065,7 +1064,7 @@ static void rx_done_irq(struct atmel_private *priv)
 		rx_packet_loc = atmel_rmem16(priv, atmel_rx(priv, RX_DESC_MSDU_POS_OFFSET, priv->rx_desc_head));
 
 		if (msdu_size < 30) {
-			priv->stats.rx_errors++;
+			priv->dev->stats.rx_errors++;
 			goto next;
 		}
 
@@ -1123,7 +1122,7 @@ static void rx_done_irq(struct atmel_private *priv)
 				msdu_size -= 4;
 				crc = crc32_le(crc, (unsigned char *)&priv->rx_buf, msdu_size);
 				if ((crc ^ 0xffffffff) != (*((u32 *)&priv->rx_buf[msdu_size]))) {
-					priv->stats.rx_crc_errors++;
+					priv->dev->stats.rx_crc_errors++;
 					goto next;
 				}
 			}
@@ -1250,12 +1249,6 @@ static irqreturn_t service_interrupt(int irq, void *dev_id)
 	}
 }
 
-static struct net_device_stats *atmel_get_stats(struct net_device *dev)
-{
-	struct atmel_private *priv = netdev_priv(dev);
-	return &priv->stats;
-}
-
 static struct iw_statistics *atmel_get_wireless_stats(struct net_device *dev)
 {
 	struct atmel_private *priv = netdev_priv(dev);
@@ -1518,8 +1511,6 @@ struct net_device *init_atmel_card(unsigned short irq, unsigned long port,
 		priv->crc_ok_cnt = priv->crc_ko_cnt = 0;
 	} else
 		priv->probe_crc = 0;
-	memset(&priv->stats, 0, sizeof(priv->stats));
-	memset(&priv->wstats, 0, sizeof(priv->wstats));
 	priv->last_qual = jiffies;
 	priv->last_beacon_timestamp = 0;
 	memset(priv->frag_source, 0xff, sizeof(priv->frag_source));
@@ -1568,7 +1559,6 @@ struct net_device *init_atmel_card(unsigned short irq, unsigned long port,
 	dev->change_mtu = atmel_change_mtu;
 	dev->set_mac_address = atmel_set_mac_address;
 	dev->hard_start_xmit = start_tx;
-	dev->get_stats = atmel_get_stats;
 	dev->wireless_handlers = (struct iw_handler_def *)&atmel_handler_def;
 	dev->do_ioctl = atmel_ioctl;
 	dev->irq = irq;
@@ -2320,30 +2310,40 @@ static int atmel_get_scan(struct net_device *dev,
 		iwe.cmd = SIOCGIWAP;
 		iwe.u.ap_addr.sa_family = ARPHRD_ETHER;
 		memcpy(iwe.u.ap_addr.sa_data, priv->BSSinfo[i].BSSID, 6);
-		current_ev = iwe_stream_add_event(current_ev, extra + IW_SCAN_MAX_DATA, &iwe, IW_EV_ADDR_LEN);
+		current_ev = iwe_stream_add_event(info, current_ev,
+						  extra + IW_SCAN_MAX_DATA,
+						  &iwe, IW_EV_ADDR_LEN);
 
 		iwe.u.data.length =  priv->BSSinfo[i].SSIDsize;
 		if (iwe.u.data.length > 32)
 			iwe.u.data.length = 32;
 		iwe.cmd = SIOCGIWESSID;
 		iwe.u.data.flags = 1;
-		current_ev = iwe_stream_add_point(current_ev, extra + IW_SCAN_MAX_DATA, &iwe, priv->BSSinfo[i].SSID);
+		current_ev = iwe_stream_add_point(info, current_ev,
+						  extra + IW_SCAN_MAX_DATA,
+						  &iwe, priv->BSSinfo[i].SSID);
 
 		iwe.cmd = SIOCGIWMODE;
 		iwe.u.mode = priv->BSSinfo[i].BSStype;
-		current_ev = iwe_stream_add_event(current_ev, extra + IW_SCAN_MAX_DATA, &iwe, IW_EV_UINT_LEN);
+		current_ev = iwe_stream_add_event(info, current_ev,
+						  extra + IW_SCAN_MAX_DATA,
+						  &iwe, IW_EV_UINT_LEN);
 
 		iwe.cmd = SIOCGIWFREQ;
 		iwe.u.freq.m = priv->BSSinfo[i].channel;
 		iwe.u.freq.e = 0;
-		current_ev = iwe_stream_add_event(current_ev, extra + IW_SCAN_MAX_DATA, &iwe, IW_EV_FREQ_LEN);
+		current_ev = iwe_stream_add_event(info, current_ev,
+						  extra + IW_SCAN_MAX_DATA,
+						  &iwe, IW_EV_FREQ_LEN);
 
 		/* Add quality statistics */
 		iwe.cmd = IWEVQUAL;
 		iwe.u.qual.level = priv->BSSinfo[i].RSSI;
 		iwe.u.qual.qual  = iwe.u.qual.level;
 		/* iwe.u.qual.noise  = SOMETHING */
-		current_ev = iwe_stream_add_event(current_ev, extra + IW_SCAN_MAX_DATA , &iwe, IW_EV_QUAL_LEN);
+		current_ev = iwe_stream_add_event(info, current_ev,
+						  extra + IW_SCAN_MAX_DATA,
+						  &iwe, IW_EV_QUAL_LEN);
 
 
 		iwe.cmd = SIOCGIWENCODE;
@@ -2352,7 +2352,9 @@ static int atmel_get_scan(struct net_device *dev,
 		else
 			iwe.u.data.flags = IW_ENCODE_DISABLED;
 		iwe.u.data.length = 0;
-		current_ev = iwe_stream_add_point(current_ev, extra + IW_SCAN_MAX_DATA, &iwe, NULL);
+		current_ev = iwe_stream_add_point(info, current_ev,
+						  extra + IW_SCAN_MAX_DATA,
+						  &iwe, NULL);
 	}
 
 	/* Length of data */
@@ -3853,7 +3855,7 @@ static int reset_atmel_card(struct net_device *dev)
 	if (priv->card_type == CARD_TYPE_EEPROM) {
 		/* copy in firmware if needed */
 		const struct firmware *fw_entry = NULL;
-		unsigned char *fw;
+		const unsigned char *fw;
 		int len = priv->firmware_length;
 		if (!(fw = priv->firmware)) {
 			if (priv->firmware_type == ATMEL_FW_TYPE_NONE) {
@@ -4120,7 +4122,7 @@ static void atmel_writeAR(struct net_device *dev, u16 data)
 }
 
 static void atmel_copy_to_card(struct net_device *dev, u16 dest,
-			       unsigned char *src, u16 len)
+			       const unsigned char *src, u16 len)
 {
 	int i;
 	atmel_writeAR(dev, dest);
diff --git a/drivers/net/wireless/b43/b43.h b/drivers/net/wireless/b43/b43.h
index d3db298c05fc..edcdfa366452 100644
--- a/drivers/net/wireless/b43/b43.h
+++ b/drivers/net/wireless/b43/b43.h
@@ -410,8 +410,7 @@ enum {
 #define B43_IRQ_TIMEOUT			0x80000000
 
 #define B43_IRQ_ALL			0xFFFFFFFF
-#define B43_IRQ_MASKTEMPLATE		(B43_IRQ_MAC_SUSPENDED | \
-					 B43_IRQ_TBTT_INDI | \
+#define B43_IRQ_MASKTEMPLATE		(B43_IRQ_TBTT_INDI | \
 					 B43_IRQ_ATIM_END | \
 					 B43_IRQ_PMQ | \
 					 B43_IRQ_MAC_TXERR | \
@@ -423,6 +422,28 @@ enum {
 					 B43_IRQ_RFKILL | \
 					 B43_IRQ_TX_OK)
 
+/* The firmware register to fetch the debug-IRQ reason from. */
+#define B43_DEBUGIRQ_REASON_REG		63
+/* Debug-IRQ reasons. */
+#define B43_DEBUGIRQ_PANIC		0	/* The firmware panic'ed */
+#define B43_DEBUGIRQ_DUMP_SHM		1	/* Dump shared SHM */
+#define B43_DEBUGIRQ_DUMP_REGS		2	/* Dump the microcode registers */
+#define B43_DEBUGIRQ_MARKER		3	/* A "marker" was thrown by the firmware. */
+#define B43_DEBUGIRQ_ACK		0xFFFF	/* The host writes that to ACK the IRQ */
+
+/* The firmware register that contains the "marker" line. */
+#define B43_MARKER_ID_REG		2
+#define B43_MARKER_LINE_REG		3
+
+/* The firmware register to fetch the panic reason from. */
+#define B43_FWPANIC_REASON_REG		3
+/* Firmware panic reason codes */
+#define B43_FWPANIC_DIE			0 /* Firmware died. Don't auto-restart it. */
+#define B43_FWPANIC_RESTART		1 /* Firmware died. Schedule a controller reset. */
+
+/* The firmware register that contains the watchdog counter. */
+#define B43_WATCHDOG_REG		1
+
 /* Device specific rate values.
  * The actual values defined here are (rate_in_mbps * 2).
  * Some code depends on this. Don't change it. */
@@ -733,7 +754,6 @@ struct b43_wl {
 	/* The beacon we are currently using (AP or IBSS mode).
 	 * This beacon stuff is protected by the irq_lock. */
 	struct sk_buff *current_beacon;
-	struct ieee80211_tx_control beacon_txctl;
 	bool beacon0_uploaded;
 	bool beacon1_uploaded;
 	bool beacon_templates_virgin; /* Never wrote the templates? */
@@ -767,6 +787,13 @@ struct b43_firmware {
 	u16 rev;
 	/* Firmware patchlevel */
 	u16 patch;
+
+	/* Set to true, if we are using an opensource firmware. */
+	bool opensource;
+	/* Set to true, if the core needs a PCM firmware, but
+	 * we failed to load one. This is always false for
+	 * core rev > 10, as these don't need PCM firmware. */
+	bool pcm_request_failed;
 };
 
 /* Device (802.11 core) initialization status. */
@@ -940,22 +967,6 @@ 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!) */
diff --git a/drivers/net/wireless/b43/debugfs.c b/drivers/net/wireless/b43/debugfs.c
index 7fca2ebc747f..29851bc1101f 100644
--- a/drivers/net/wireless/b43/debugfs.c
+++ b/drivers/net/wireless/b43/debugfs.c
@@ -74,70 +74,327 @@ struct b43_dfs_file * fops_to_dfs_file(struct b43_wldev *dev,
 	} while (0)
 
 
-/* wl->irq_lock is locked */
-static ssize_t tsf_read_file(struct b43_wldev *dev,
-			     char *buf, size_t bufsize)
+/* The biggest address values for SHM access from the debugfs files. */
+#define B43_MAX_SHM_ROUTING	4
+#define B43_MAX_SHM_ADDR	0xFFFF
+
+static ssize_t shm16read__read_file(struct b43_wldev *dev,
+				    char *buf, size_t bufsize)
 {
 	ssize_t count = 0;
-	u64 tsf;
+	unsigned int routing, addr;
+	u16 val;
 
-	b43_tsf_read(dev, &tsf);
-	fappend("0x%08x%08x\n",
-		(unsigned int)((tsf & 0xFFFFFFFF00000000ULL) >> 32),
-		(unsigned int)(tsf & 0xFFFFFFFFULL));
+	routing = dev->dfsentry->shm16read_routing_next;
+	addr = dev->dfsentry->shm16read_addr_next;
+	if ((routing > B43_MAX_SHM_ROUTING) ||
+	    (addr > B43_MAX_SHM_ADDR))
+		return -EDESTADDRREQ;
+
+	val = b43_shm_read16(dev, routing, addr);
+	fappend("0x%04X\n", val);
 
 	return count;
 }
 
-/* wl->irq_lock is locked */
-static int tsf_write_file(struct b43_wldev *dev,
-			  const char *buf, size_t count)
+static int shm16read__write_file(struct b43_wldev *dev,
+				 const char *buf, size_t count)
 {
-	u64 tsf;
+	unsigned int routing, addr;
+	int res;
 
-	if (sscanf(buf, "%llu", (unsigned long long *)(&tsf)) != 1)
+	res = sscanf(buf, "0x%X 0x%X", &routing, &addr);
+	if (res != 2)
 		return -EINVAL;
-	b43_tsf_write(dev, tsf);
+	if (routing > B43_MAX_SHM_ROUTING)
+		return -EADDRNOTAVAIL;
+	if (addr > B43_MAX_SHM_ADDR)
+		return -EADDRNOTAVAIL;
+	if (routing == B43_SHM_SHARED) {
+		if ((addr % 2) != 0)
+			return -EADDRNOTAVAIL;
+	}
+
+	dev->dfsentry->shm16read_routing_next = routing;
+	dev->dfsentry->shm16read_addr_next = addr;
 
 	return 0;
 }
 
-/* wl->irq_lock is locked */
-static ssize_t ucode_regs_read_file(struct b43_wldev *dev,
+static int shm16write__write_file(struct b43_wldev *dev,
+				  const char *buf, size_t count)
+{
+	unsigned int routing, addr, mask, set;
+	u16 val;
+	int res;
+	unsigned long flags;
+
+	res = sscanf(buf, "0x%X 0x%X 0x%X 0x%X",
+		     &routing, &addr, &mask, &set);
+	if (res != 4)
+		return -EINVAL;
+	if (routing > B43_MAX_SHM_ROUTING)
+		return -EADDRNOTAVAIL;
+	if (addr > B43_MAX_SHM_ADDR)
+		return -EADDRNOTAVAIL;
+	if (routing == B43_SHM_SHARED) {
+		if ((addr % 2) != 0)
+			return -EADDRNOTAVAIL;
+	}
+	if ((mask > 0xFFFF) || (set > 0xFFFF))
+		return -E2BIG;
+
+	spin_lock_irqsave(&dev->wl->shm_lock, flags);
+	if (mask == 0)
+		val = 0;
+	else
+		val = __b43_shm_read16(dev, routing, addr);
+	val &= mask;
+	val |= set;
+	__b43_shm_write16(dev, routing, addr, val);
+	spin_unlock_irqrestore(&dev->wl->shm_lock, flags);
+
+	return 0;
+}
+
+static ssize_t shm32read__read_file(struct b43_wldev *dev,
 				    char *buf, size_t bufsize)
 {
 	ssize_t count = 0;
-	int i;
+	unsigned int routing, addr;
+	u32 val;
+
+	routing = dev->dfsentry->shm32read_routing_next;
+	addr = dev->dfsentry->shm32read_addr_next;
+	if ((routing > B43_MAX_SHM_ROUTING) ||
+	    (addr > B43_MAX_SHM_ADDR))
+		return -EDESTADDRREQ;
+
+	val = b43_shm_read32(dev, routing, addr);
+	fappend("0x%08X\n", val);
 
-	for (i = 0; i < 64; i++) {
-		fappend("r%d = 0x%04x\n", i,
-			b43_shm_read16(dev, B43_SHM_SCRATCH, i));
+	return count;
+}
+
+static int shm32read__write_file(struct b43_wldev *dev,
+				 const char *buf, size_t count)
+{
+	unsigned int routing, addr;
+	int res;
+
+	res = sscanf(buf, "0x%X 0x%X", &routing, &addr);
+	if (res != 2)
+		return -EINVAL;
+	if (routing > B43_MAX_SHM_ROUTING)
+		return -EADDRNOTAVAIL;
+	if (addr > B43_MAX_SHM_ADDR)
+		return -EADDRNOTAVAIL;
+	if (routing == B43_SHM_SHARED) {
+		if ((addr % 2) != 0)
+			return -EADDRNOTAVAIL;
 	}
 
+	dev->dfsentry->shm32read_routing_next = routing;
+	dev->dfsentry->shm32read_addr_next = addr;
+
+	return 0;
+}
+
+static int shm32write__write_file(struct b43_wldev *dev,
+				  const char *buf, size_t count)
+{
+	unsigned int routing, addr, mask, set;
+	u32 val;
+	int res;
+	unsigned long flags;
+
+	res = sscanf(buf, "0x%X 0x%X 0x%X 0x%X",
+		     &routing, &addr, &mask, &set);
+	if (res != 4)
+		return -EINVAL;
+	if (routing > B43_MAX_SHM_ROUTING)
+		return -EADDRNOTAVAIL;
+	if (addr > B43_MAX_SHM_ADDR)
+		return -EADDRNOTAVAIL;
+	if (routing == B43_SHM_SHARED) {
+		if ((addr % 2) != 0)
+			return -EADDRNOTAVAIL;
+	}
+	if ((mask > 0xFFFFFFFF) || (set > 0xFFFFFFFF))
+		return -E2BIG;
+
+	spin_lock_irqsave(&dev->wl->shm_lock, flags);
+	if (mask == 0)
+		val = 0;
+	else
+		val = __b43_shm_read32(dev, routing, addr);
+	val &= mask;
+	val |= set;
+	__b43_shm_write32(dev, routing, addr, val);
+	spin_unlock_irqrestore(&dev->wl->shm_lock, flags);
+
+	return 0;
+}
+
+/* The biggest MMIO address that we allow access to from the debugfs files. */
+#define B43_MAX_MMIO_ACCESS	(0xF00 - 1)
+
+static ssize_t mmio16read__read_file(struct b43_wldev *dev,
+				     char *buf, size_t bufsize)
+{
+	ssize_t count = 0;
+	unsigned int addr;
+	u16 val;
+
+	addr = dev->dfsentry->mmio16read_next;
+	if (addr > B43_MAX_MMIO_ACCESS)
+		return -EDESTADDRREQ;
+
+	val = b43_read16(dev, addr);
+	fappend("0x%04X\n", val);
+
+	return count;
+}
+
+static int mmio16read__write_file(struct b43_wldev *dev,
+				  const char *buf, size_t count)
+{
+	unsigned int addr;
+	int res;
+
+	res = sscanf(buf, "0x%X", &addr);
+	if (res != 1)
+		return -EINVAL;
+	if (addr > B43_MAX_MMIO_ACCESS)
+		return -EADDRNOTAVAIL;
+	if ((addr % 2) != 0)
+		return -EINVAL;
+
+	dev->dfsentry->mmio16read_next = addr;
+
+	return 0;
+}
+
+static int mmio16write__write_file(struct b43_wldev *dev,
+				   const char *buf, size_t count)
+{
+	unsigned int addr, mask, set;
+	int res;
+	u16 val;
+
+	res = sscanf(buf, "0x%X 0x%X 0x%X", &addr, &mask, &set);
+	if (res != 3)
+		return -EINVAL;
+	if (addr > B43_MAX_MMIO_ACCESS)
+		return -EADDRNOTAVAIL;
+	if ((mask > 0xFFFF) || (set > 0xFFFF))
+		return -E2BIG;
+	if ((addr % 2) != 0)
+		return -EINVAL;
+
+	if (mask == 0)
+		val = 0;
+	else
+		val = b43_read16(dev, addr);
+	val &= mask;
+	val |= set;
+	b43_write16(dev, addr, val);
+
+	return 0;
+}
+
+static ssize_t mmio32read__read_file(struct b43_wldev *dev,
+				     char *buf, size_t bufsize)
+{
+	ssize_t count = 0;
+	unsigned int addr;
+	u32 val;
+
+	addr = dev->dfsentry->mmio32read_next;
+	if (addr > B43_MAX_MMIO_ACCESS)
+		return -EDESTADDRREQ;
+
+	val = b43_read32(dev, addr);
+	fappend("0x%08X\n", val);
+
 	return count;
 }
 
+static int mmio32read__write_file(struct b43_wldev *dev,
+				  const char *buf, size_t count)
+{
+	unsigned int addr;
+	int res;
+
+	res = sscanf(buf, "0x%X", &addr);
+	if (res != 1)
+		return -EINVAL;
+	if (addr > B43_MAX_MMIO_ACCESS)
+		return -EADDRNOTAVAIL;
+	if ((addr % 4) != 0)
+		return -EINVAL;
+
+	dev->dfsentry->mmio32read_next = addr;
+
+	return 0;
+}
+
+static int mmio32write__write_file(struct b43_wldev *dev,
+				   const char *buf, size_t count)
+{
+	unsigned int addr, mask, set;
+	int res;
+	u32 val;
+
+	res = sscanf(buf, "0x%X 0x%X 0x%X", &addr, &mask, &set);
+	if (res != 3)
+		return -EINVAL;
+	if (addr > B43_MAX_MMIO_ACCESS)
+		return -EADDRNOTAVAIL;
+	if ((mask > 0xFFFFFFFF) || (set > 0xFFFFFFFF))
+		return -E2BIG;
+	if ((addr % 4) != 0)
+		return -EINVAL;
+
+	if (mask == 0)
+		val = 0;
+	else
+		val = b43_read32(dev, addr);
+	val &= mask;
+	val |= set;
+	b43_write32(dev, addr, val);
+
+	return 0;
+}
+
 /* wl->irq_lock is locked */
-static ssize_t shm_read_file(struct b43_wldev *dev,
+static ssize_t tsf_read_file(struct b43_wldev *dev,
 			     char *buf, size_t bufsize)
 {
 	ssize_t count = 0;
-	int i;
-	u16 tmp;
-	__le16 *le16buf = (__le16 *)buf;
+	u64 tsf;
 
-	for (i = 0; i < 0x1000; i++) {
-		if (bufsize < sizeof(tmp))
-			break;
-		tmp = b43_shm_read16(dev, B43_SHM_SHARED, 2 * i);
-		le16buf[i] = cpu_to_le16(tmp);
-		count += sizeof(tmp);
-		bufsize -= sizeof(tmp);
-	}
+	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;
+}
+
 static ssize_t txstat_read_file(struct b43_wldev *dev,
 				char *buf, size_t bufsize)
 {
@@ -270,24 +527,22 @@ static int restart_write_file(struct b43_wldev *dev,
 	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])
+static unsigned long calc_expire_secs(unsigned long now,
+				      unsigned long time,
+				      unsigned long expire)
 {
-	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));
-		}
+	expire = time + expire;
+
+	if (time_after(now, expire))
+		return 0; /* expired */
+	if (expire < now) {
+		/* jiffies wrapped */
+		expire -= MAX_JIFFY_OFFSET;
+		now -= MAX_JIFFY_OFFSET;
 	}
+	B43_WARN_ON(expire < now);
 
-	return count;
+	return (expire - now) / HZ;
 }
 
 static ssize_t loctls_read_file(struct b43_wldev *dev,
@@ -296,27 +551,45 @@ static ssize_t loctls_read_file(struct b43_wldev *dev,
 	ssize_t count = 0;
 	struct b43_txpower_lo_control *lo;
 	int i, err = 0;
+	struct b43_lo_calib *cal;
+	unsigned long now = jiffies;
+	struct b43_phy *phy = &dev->phy;
 
-	if (dev->phy.type != B43_PHYTYPE_G) {
+	if (phy->type != B43_PHYTYPE_G) {
 		fappend("Device is not a G-PHY\n");
 		err = -ENODEV;
 		goto out;
 	}
-	lo = dev->phy.lo_control;
+	lo = 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,
+	fappend("HW-power-control enabled: %d\n",
 		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",
+	fappend("TX Bias: 0x%02X,  TX Magn: 0x%02X  (expire in %lu sec)\n",
+		lo->tx_bias, lo->tx_magn,
+		calc_expire_secs(now, lo->txctl_measured_time,
+				 B43_LO_TXCTL_EXPIRE));
+	fappend("Power Vector: 0x%08X%08X  (expires in %lu sec)\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);
+		(unsigned int)(lo->power_vector & 0x00000000FFFFFFFFULL),
+		calc_expire_secs(now, lo->pwr_vec_read_time,
+				 B43_LO_PWRVEC_EXPIRE));
+
+	fappend("\nCalibrated settings:\n");
+	list_for_each_entry(cal, &lo->calib_list, list) {
+		bool active;
+
+		active = (b43_compare_bbatt(&cal->bbatt, &phy->bbatt) &&
+			  b43_compare_rfatt(&cal->rfatt, &phy->rfatt));
+		fappend("BB(%d), RF(%d,%d)  ->  I=%d, Q=%d  "
+			"(expires in %lu sec)%s\n",
+			cal->bbatt.att,
+			cal->rfatt.att, cal->rfatt.with_padmix,
+			cal->ctl.i, cal->ctl.q,
+			calc_expire_secs(now, cal->calib_time,
+					 B43_LO_CALIB_EXPIRE),
+			active ? "  ACTIVE" : "");
+	}
+
 	fappend("\nUsed RF attenuation values:  Value(WithPadmix flag)\n");
 	for (i = 0; i < lo->rfatt_list.len; i++) {
 		fappend("%u(%d), ",
@@ -351,7 +624,7 @@ static ssize_t b43_debugfs_read(struct file *file, char __user *userbuf,
 	struct b43_dfs_file *dfile;
 	ssize_t uninitialized_var(ret);
 	char *buf;
-	const size_t bufsize = 1024 * 128;
+	const size_t bufsize = 1024 * 16; /* 16 kiB buffer */
 	const size_t buforder = get_order(bufsize);
 	int err = 0;
 
@@ -380,8 +653,6 @@ static ssize_t b43_debugfs_read(struct file *file, char __user *userbuf,
 			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);
@@ -482,9 +753,15 @@ out_unlock:
 		.take_irqlock	= _take_irqlock,		\
 	}
 
+B43_DEBUGFS_FOPS(shm16read, shm16read__read_file, shm16read__write_file, 1);
+B43_DEBUGFS_FOPS(shm16write, NULL, shm16write__write_file, 1);
+B43_DEBUGFS_FOPS(shm32read, shm32read__read_file, shm32read__write_file, 1);
+B43_DEBUGFS_FOPS(shm32write, NULL, shm32write__write_file, 1);
+B43_DEBUGFS_FOPS(mmio16read, mmio16read__read_file, mmio16read__write_file, 1);
+B43_DEBUGFS_FOPS(mmio16write, NULL, mmio16write__write_file, 1);
+B43_DEBUGFS_FOPS(mmio32read, mmio32read__read_file, mmio32read__write_file, 1);
+B43_DEBUGFS_FOPS(mmio32write, NULL, mmio32write__write_file, 1);
 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);
@@ -523,6 +800,8 @@ static void b43_add_dynamic_debug(struct b43_wldev *dev)
 	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);
+	add_dyn_dbg("debug_lo", B43_DBG_LO, 0);
+	add_dyn_dbg("debug_firmware", B43_DBG_FIRMWARE, 0);
 
 #undef add_dyn_dbg
 }
@@ -569,6 +848,13 @@ void b43_debugfs_add_device(struct b43_wldev *dev)
 		return;
 	}
 
+	e->mmio16read_next = 0xFFFF; /* invalid address */
+	e->mmio32read_next = 0xFFFF; /* invalid address */
+	e->shm16read_routing_next = 0xFFFFFFFF; /* invalid routing */
+	e->shm16read_addr_next = 0xFFFFFFFF; /* invalid address */
+	e->shm32read_routing_next = 0xFFFFFFFF; /* invalid routing */
+	e->shm32read_addr_next = 0xFFFFFFFF; /* invalid address */
+
 #define ADD_FILE(name, mode)	\
 	do {							\
 		struct dentry *d;				\
@@ -581,9 +867,15 @@ void b43_debugfs_add_device(struct b43_wldev *dev)
 	} while (0)
 
 
+	ADD_FILE(shm16read, 0600);
+	ADD_FILE(shm16write, 0200);
+	ADD_FILE(shm32read, 0600);
+	ADD_FILE(shm32write, 0200);
+	ADD_FILE(mmio16read, 0600);
+	ADD_FILE(mmio16write, 0200);
+	ADD_FILE(mmio32read, 0600);
+	ADD_FILE(mmio32write, 0200);
 	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);
@@ -605,9 +897,15 @@ void b43_debugfs_remove_device(struct b43_wldev *dev)
 		return;
 	b43_remove_dynamic_debug(dev);
 
+	debugfs_remove(e->file_shm16read.dentry);
+	debugfs_remove(e->file_shm16write.dentry);
+	debugfs_remove(e->file_shm32read.dentry);
+	debugfs_remove(e->file_shm32write.dentry);
+	debugfs_remove(e->file_mmio16read.dentry);
+	debugfs_remove(e->file_mmio16write.dentry);
+	debugfs_remove(e->file_mmio32read.dentry);
+	debugfs_remove(e->file_mmio32write.dentry);
 	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);
diff --git a/drivers/net/wireless/b43/debugfs.h b/drivers/net/wireless/b43/debugfs.h
index 6eebe858db5a..22ffd02ba554 100644
--- a/drivers/net/wireless/b43/debugfs.h
+++ b/drivers/net/wireless/b43/debugfs.h
@@ -10,6 +10,8 @@ enum b43_dyndbg {		/* Dynamic debugging features */
 	B43_DBG_DMAVERBOSE,
 	B43_DBG_PWORK_FAST,
 	B43_DBG_PWORK_STOP,
+	B43_DBG_LO,
+	B43_DBG_FIRMWARE,
 	__B43_NR_DYNDBG,
 };
 
@@ -35,9 +37,15 @@ struct b43_dfsentry {
 	struct b43_wldev *dev;
 	struct dentry *subdir;
 
+	struct b43_dfs_file file_shm16read;
+	struct b43_dfs_file file_shm16write;
+	struct b43_dfs_file file_shm32read;
+	struct b43_dfs_file file_shm32write;
+	struct b43_dfs_file file_mmio16read;
+	struct b43_dfs_file file_mmio16write;
+	struct b43_dfs_file file_mmio32read;
+	struct b43_dfs_file file_mmio32write;
 	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;
@@ -45,6 +53,18 @@ struct b43_dfsentry {
 
 	struct b43_txstatus_log txstatlog;
 
+	/* The cached address for the next mmio16read file read */
+	u16 mmio16read_next;
+	/* The cached address for the next mmio32read file read */
+	u16 mmio32read_next;
+
+	/* The cached address for the next shm16read file read */
+	u32 shm16read_routing_next;
+	u32 shm16read_addr_next;
+	/* The cached address for the next shm32read file read */
+	u32 shm32read_routing_next;
+	u32 shm32read_addr_next;
+
 	/* Enabled/Disabled list for the dynamic debugging features. */
 	u32 dyn_debug[__B43_NR_DYNDBG];
 	/* Dentries for the dynamic debugging entries. */
diff --git a/drivers/net/wireless/b43/dma.c b/drivers/net/wireless/b43/dma.c
index e23f2f172bd7..098f886976f6 100644
--- a/drivers/net/wireless/b43/dma.c
+++ b/drivers/net/wireless/b43/dma.c
@@ -328,11 +328,11 @@ static inline
 	dma_addr_t dmaaddr;
 
 	if (tx) {
-		dmaaddr = dma_map_single(ring->dev->dev->dma_dev,
-					 buf, len, DMA_TO_DEVICE);
+		dmaaddr = ssb_dma_map_single(ring->dev->dev,
+					     buf, len, DMA_TO_DEVICE);
 	} else {
-		dmaaddr = dma_map_single(ring->dev->dev->dma_dev,
-					 buf, len, DMA_FROM_DEVICE);
+		dmaaddr = ssb_dma_map_single(ring->dev->dev,
+					     buf, len, DMA_FROM_DEVICE);
 	}
 
 	return dmaaddr;
@@ -343,11 +343,11 @@ static inline
 			  dma_addr_t addr, size_t len, int tx)
 {
 	if (tx) {
-		dma_unmap_single(ring->dev->dev->dma_dev,
-				 addr, len, DMA_TO_DEVICE);
+		ssb_dma_unmap_single(ring->dev->dev,
+				     addr, len, DMA_TO_DEVICE);
 	} else {
-		dma_unmap_single(ring->dev->dev->dma_dev,
-				 addr, len, DMA_FROM_DEVICE);
+		ssb_dma_unmap_single(ring->dev->dev,
+				     addr, len, DMA_FROM_DEVICE);
 	}
 }
 
@@ -356,8 +356,8 @@ static inline
 				 dma_addr_t addr, size_t len)
 {
 	B43_WARN_ON(ring->tx);
-	dma_sync_single_for_cpu(ring->dev->dev->dma_dev,
-				addr, len, DMA_FROM_DEVICE);
+	ssb_dma_sync_single_for_cpu(ring->dev->dev,
+				    addr, len, DMA_FROM_DEVICE);
 }
 
 static inline
@@ -365,8 +365,8 @@ static inline
 				    dma_addr_t addr, size_t len)
 {
 	B43_WARN_ON(ring->tx);
-	dma_sync_single_for_device(ring->dev->dev->dma_dev,
-				   addr, len, DMA_FROM_DEVICE);
+	ssb_dma_sync_single_for_device(ring->dev->dev,
+				       addr, len, DMA_FROM_DEVICE);
 }
 
 static inline
@@ -381,7 +381,6 @@ static inline
 
 static int alloc_ringmemory(struct b43_dmaring *ring)
 {
-	struct device *dma_dev = ring->dev->dev->dma_dev;
 	gfp_t flags = GFP_KERNEL;
 
 	/* The specs call for 4K buffers for 30- and 32-bit DMA with 4K
@@ -392,11 +391,14 @@ static int alloc_ringmemory(struct b43_dmaring *ring)
 	 * For unknown reasons - possibly a hardware error - the BCM4311 rev
 	 * 02, which uses 64-bit DMA, needs the ring buffer in very low memory,
 	 * which accounts for the GFP_DMA flag below.
+	 *
+	 * The flags here must match the flags in free_ringmemory below!
 	 */
 	if (ring->type == B43_DMA_64BIT)
 		flags |= GFP_DMA;
-	ring->descbase = dma_alloc_coherent(dma_dev, B43_DMA_RINGMEMSIZE,
-					    &(ring->dmabase), flags);
+	ring->descbase = ssb_dma_alloc_consistent(ring->dev->dev,
+						  B43_DMA_RINGMEMSIZE,
+						  &(ring->dmabase), flags);
 	if (!ring->descbase) {
 		b43err(ring->dev->wl, "DMA ringmemory allocation failed\n");
 		return -ENOMEM;
@@ -408,10 +410,13 @@ static int alloc_ringmemory(struct b43_dmaring *ring)
 
 static void free_ringmemory(struct b43_dmaring *ring)
 {
-	struct device *dma_dev = ring->dev->dev->dma_dev;
+	gfp_t flags = GFP_KERNEL;
+
+	if (ring->type == B43_DMA_64BIT)
+		flags |= GFP_DMA;
 
-	dma_free_coherent(dma_dev, B43_DMA_RINGMEMSIZE,
-			  ring->descbase, ring->dmabase);
+	ssb_dma_free_consistent(ring->dev->dev, B43_DMA_RINGMEMSIZE,
+				ring->descbase, ring->dmabase, flags);
 }
 
 /* Reset the RX DMA channel */
@@ -518,7 +523,7 @@ static bool b43_dma_mapping_error(struct b43_dmaring *ring,
 				  dma_addr_t addr,
 				  size_t buffersize, bool dma_to_device)
 {
-	if (unlikely(dma_mapping_error(addr)))
+	if (unlikely(ssb_dma_mapping_error(ring->dev->dev, addr)))
 		return 1;
 
 	switch (ring->type) {
@@ -844,10 +849,10 @@ struct b43_dmaring *b43_setup_dmaring(struct b43_wldev *dev,
 			goto err_kfree_meta;
 
 		/* test for ability to dma to txhdr_cache */
-		dma_test = dma_map_single(dev->dev->dma_dev,
-					  ring->txhdr_cache,
-					  b43_txhdr_size(dev),
-					  DMA_TO_DEVICE);
+		dma_test = ssb_dma_map_single(dev->dev,
+					      ring->txhdr_cache,
+					      b43_txhdr_size(dev),
+					      DMA_TO_DEVICE);
 
 		if (b43_dma_mapping_error(ring, dma_test,
 					  b43_txhdr_size(dev), 1)) {
@@ -859,10 +864,10 @@ struct b43_dmaring *b43_setup_dmaring(struct b43_wldev *dev,
 			if (!ring->txhdr_cache)
 				goto err_kfree_meta;
 
-			dma_test = dma_map_single(dev->dev->dma_dev,
-						  ring->txhdr_cache,
-						  b43_txhdr_size(dev),
-						  DMA_TO_DEVICE);
+			dma_test = ssb_dma_map_single(dev->dev,
+						      ring->txhdr_cache,
+						      b43_txhdr_size(dev),
+						      DMA_TO_DEVICE);
 
 			if (b43_dma_mapping_error(ring, dma_test,
 						  b43_txhdr_size(dev), 1)) {
@@ -873,9 +878,9 @@ struct b43_dmaring *b43_setup_dmaring(struct b43_wldev *dev,
 			}
 		}
 
-		dma_unmap_single(dev->dev->dma_dev,
-				 dma_test, b43_txhdr_size(dev),
-				 DMA_TO_DEVICE);
+		ssb_dma_unmap_single(dev->dev,
+				     dma_test, b43_txhdr_size(dev),
+				     DMA_TO_DEVICE);
 	}
 
 	err = alloc_ringmemory(ring);
@@ -1130,10 +1135,10 @@ struct b43_dmaring *parse_cookie(struct b43_wldev *dev, u16 cookie, int *slot)
 }
 
 static int dma_tx_fragment(struct b43_dmaring *ring,
-			   struct sk_buff *skb,
-			   struct ieee80211_tx_control *ctl)
+			   struct sk_buff *skb)
 {
 	const struct b43_dma_ops *ops = ring->ops;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 	u8 *header;
 	int slot, old_top_slot, old_used_slots;
 	int err;
@@ -1157,7 +1162,7 @@ static int dma_tx_fragment(struct b43_dmaring *ring,
 	header = &(ring->txhdr_cache[slot * hdrsize]);
 	cookie = generate_cookie(ring, slot);
 	err = b43_generate_txhdr(ring->dev, header,
-				 skb->data, skb->len, ctl, cookie);
+				 skb->data, skb->len, info, cookie);
 	if (unlikely(err)) {
 		ring->current_slot = old_top_slot;
 		ring->used_slots = old_used_slots;
@@ -1179,7 +1184,6 @@ static int dma_tx_fragment(struct b43_dmaring *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;
 
@@ -1209,7 +1213,7 @@ static int dma_tx_fragment(struct b43_dmaring *ring,
 
 	ops->fill_descriptor(ring, desc, meta->dmaaddr, skb->len, 0, 1, 1);
 
-	if (ctl->flags & IEEE80211_TXCTL_SEND_AFTER_DTIM) {
+	if (info->flags & IEEE80211_TX_CTL_SEND_AFTER_DTIM) {
 		/* Tell the firmware about the cookie of the last
 		 * mcast frame, so it can clear the more-data bit in it. */
 		b43_shm_write16(ring->dev, B43_SHM_SHARED,
@@ -1280,16 +1284,16 @@ static struct b43_dmaring * select_ring_by_priority(struct b43_wldev *dev,
 	return ring;
 }
 
-int b43_dma_tx(struct b43_wldev *dev,
-	       struct sk_buff *skb, struct ieee80211_tx_control *ctl)
+int b43_dma_tx(struct b43_wldev *dev, struct sk_buff *skb)
 {
 	struct b43_dmaring *ring;
 	struct ieee80211_hdr *hdr;
 	int err = 0;
 	unsigned long flags;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 
 	hdr = (struct ieee80211_hdr *)skb->data;
-	if (ctl->flags & IEEE80211_TXCTL_SEND_AFTER_DTIM) {
+	if (info->flags & IEEE80211_TX_CTL_SEND_AFTER_DTIM) {
 		/* The multicast ring will be sent after the DTIM */
 		ring = dev->dma.tx_ring_mcast;
 		/* Set the more-data bit. Ucode will clear it on
@@ -1297,7 +1301,8 @@ int b43_dma_tx(struct b43_wldev *dev,
 		hdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_MOREDATA);
 	} else {
 		/* Decide by priority where to put this frame. */
-		ring = select_ring_by_priority(dev, ctl->queue);
+		ring = select_ring_by_priority(
+			dev, skb_get_queue_mapping(skb));
 	}
 
 	spin_lock_irqsave(&ring->lock, flags);
@@ -1315,9 +1320,9 @@ int b43_dma_tx(struct b43_wldev *dev,
 	/* Assign the queue number to the ring (if not already done before)
 	 * so TX status handling can use it. The queue to ring mapping is
 	 * static, so we don't need to store it per frame. */
-	ring->queue_prio = ctl->queue;
+	ring->queue_prio = skb_get_queue_mapping(skb);
 
-	err = dma_tx_fragment(ring, skb, ctl);
+	err = dma_tx_fragment(ring, skb);
 	if (unlikely(err == -ENOKEY)) {
 		/* Drop this packet, as we don't have the encryption key
 		 * anymore and must not transmit it unencrypted. */
@@ -1333,7 +1338,7 @@ int b43_dma_tx(struct b43_wldev *dev,
 	if ((free_slots(ring) < SLOTS_PER_PACKET) ||
 	    should_inject_overflow(ring)) {
 		/* This TX ring is full. */
-		ieee80211_stop_queue(dev->wl->hw, ctl->queue);
+		ieee80211_stop_queue(dev->wl->hw, skb_get_queue_mapping(skb));
 		ring->stopped = 1;
 		if (b43_debug(dev, B43_DBG_DMAVERBOSE)) {
 			b43dbg(dev->wl, "Stopped TX ring %d\n", ring->index);
@@ -1376,13 +1381,19 @@ void b43_dma_handle_txstatus(struct b43_wldev *dev,
 					 b43_txhdr_size(dev), 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().
+			struct ieee80211_tx_info *info;
+
+			BUG_ON(!meta->skb);
+
+			info = IEEE80211_SKB_CB(meta->skb);
+
+			memset(&info->status, 0, sizeof(info->status));
+
+			/*
+			 * Call back to inform the ieee80211 subsystem about
+			 * the status of the transmission.
 			 */
-			frame_succeed = b43_fill_txstatus_report(
-						&(meta->txstat), status);
+			frame_succeed = b43_fill_txstatus_report(info, status);
 #ifdef CONFIG_B43_DEBUG
 			if (frame_succeed)
 				ring->nr_succeed_tx_packets++;
@@ -1390,8 +1401,8 @@ void b43_dma_handle_txstatus(struct b43_wldev *dev,
 				ring->nr_failed_tx_packets++;
 			ring->nr_total_packet_tries += status->frame_count;
 #endif /* DEBUG */
-			ieee80211_tx_status_irqsafe(dev->wl->hw, meta->skb,
-						    &(meta->txstat));
+			ieee80211_tx_status_irqsafe(dev->wl->hw, meta->skb);
+
 			/* skb is freed by ieee80211_tx_status_irqsafe() */
 			meta->skb = NULL;
 		} else {
@@ -1426,18 +1437,16 @@ void b43_dma_get_tx_stats(struct b43_wldev *dev,
 {
 	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 = select_ring_by_priority(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;
+		stats[i].len = ring->used_slots / SLOTS_PER_PACKET;
+		stats[i].limit = ring->nr_slots / SLOTS_PER_PACKET;
+		stats[i].count = ring->nr_tx_packets;
 		spin_unlock_irqrestore(&ring->lock, flags);
 	}
 }
diff --git a/drivers/net/wireless/b43/dma.h b/drivers/net/wireless/b43/dma.h
index 20acf885dba5..d1eb5c0848a5 100644
--- a/drivers/net/wireless/b43/dma.h
+++ b/drivers/net/wireless/b43/dma.h
@@ -181,7 +181,6 @@ struct b43_dmadesc_meta {
 	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;
@@ -285,7 +284,7 @@ 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);
+	       struct sk_buff *skb);
 void b43_dma_handle_txstatus(struct b43_wldev *dev,
 			     const struct b43_txstatus *status);
 
diff --git a/drivers/net/wireless/b43/lo.c b/drivers/net/wireless/b43/lo.c
index d890f366a23b..9c854d6aae36 100644
--- a/drivers/net/wireless/b43/lo.c
+++ b/drivers/net/wireless/b43/lo.c
@@ -36,17 +36,28 @@
 #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
+static struct b43_lo_calib * b43_find_lo_calib(struct b43_txpower_lo_control *lo,
+					       const struct b43_bbatt *bbatt,
+					       const struct b43_rfatt *rfatt)
+{
+	struct b43_lo_calib *c;
+
+	list_for_each_entry(c, &lo->calib_list, list) {
+		if (!b43_compare_bbatt(&c->bbatt, bbatt))
+			continue;
+		if (!b43_compare_rfatt(&c->rfatt, rfatt))
+			continue;
+		return c;
+	}
 
+	return NULL;
+}
 
 /* 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)) {
@@ -56,189 +67,11 @@ static void b43_lo_write(struct b43_wldev *dev, struct b43_loctl *control)
 			return;
 		}
 	}
+	B43_WARN_ON(phy->type != B43_PHYTYPE_G);
 
 	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]);
+	b43_phy_write(dev, B43_PHY_LO_CTL, value);
 }
 
 static u16 lo_measure_feedthrough(struct b43_wldev *dev,
@@ -366,7 +199,7 @@ static void lo_measure_txctl_values(struct b43_wldev *dev)
 		if (lb_gain > 10) {
 			radio_pctl_reg = 0;
 			pga = abs(10 - lb_gain) / 6;
-			pga = limit_value(pga, 0, 15);
+			pga = clamp_val(pga, 0, 15);
 		} else {
 			int cmp_val;
 			int tmp;
@@ -438,48 +271,26 @@ static void lo_measure_txctl_values(struct b43_wldev *dev)
 		b43_radio_write16(dev, 0x52, b43_radio_read16(dev, 0x52)
 				  & 0xFFF0);	/* TX bias == 0 */
 	}
+	lo->txctl_measured_time = jiffies;
 }
 
 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;
+	int 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;
-		}
-	}
+	lo->pwr_vec_read_time = jiffies;
 }
 
 /* 802.11/LO/GPHY/MeasuringGains */
@@ -510,7 +321,7 @@ static void lo_measure_gain_values(struct b43_wldev *dev,
 			phy->lna_lod_gain = 1;
 			trsw_rx_gain -= 8;
 		}
-		trsw_rx_gain = limit_value(trsw_rx_gain, 0, 0x2D);
+		trsw_rx_gain = clamp_val(trsw_rx_gain, 0, 0x2D);
 		phy->pga_gain = trsw_rx_gain / 3;
 		if (phy->pga_gain >= 5) {
 			phy->pga_gain -= 5;
@@ -609,8 +420,6 @@ static void lo_measure_setup(struct b43_wldev *dev,
 		b43_phy_write(dev, B43_PHY_CCK(0x16), 0x410);
 		b43_phy_write(dev, B43_PHY_CCK(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 =
@@ -691,8 +500,12 @@ static void lo_measure_setup(struct b43_wldev *dev,
 	b43_radio_read16(dev, 0x51);	/* dummy read */
 	if (phy->type == B43_PHYTYPE_G)
 		b43_phy_write(dev, B43_PHY_CCK(0x2F), 0);
-	if (lo->rebuild)
+
+	/* Re-measure the txctl values, if needed. */
+	if (time_before(lo->txctl_measured_time,
+			jiffies - B43_LO_TXCTL_EXPIRE))
 		lo_measure_txctl_values(dev);
+
 	if (phy->type == B43_PHYTYPE_G && phy->rev >= 3) {
 		b43_phy_write(dev, B43_PHY_LO_MASK, 0xC078);
 	} else {
@@ -707,7 +520,6 @@ 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) {
@@ -722,14 +534,6 @@ static void lo_measure_restore(struct b43_wldev *dev,
 		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_CCK(0x2E), 0xC078);
@@ -793,7 +597,6 @@ static int lo_probe_possible_loctls(struct b43_wldev *dev,
 				    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 = {
@@ -852,7 +655,7 @@ static int lo_probe_possible_loctls(struct b43_wldev *dev,
 				found_lower = 1;
 				d->lowest_feedth = feedth;
 				if ((d->nr_measured < 2) &&
-				    (!has_loopback_gain(phy) || lo->rebuild))
+				    !has_loopback_gain(phy))
 					break;
 			}
 		}
@@ -874,7 +677,6 @@ static void lo_probe_loctls_statemachine(struct b43_wldev *dev,
 					 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;
@@ -883,18 +685,18 @@ static void lo_probe_loctls_statemachine(struct b43_wldev *dev,
 
 	d.nr_measured = 0;
 	d.state_val_multiplier = 1;
-	if (has_loopback_gain(phy) && !lo->rebuild)
+	if (has_loopback_gain(phy))
 		d.state_val_multiplier = 3;
 
 	memcpy(&d.min_loctl, loctl, sizeof(struct b43_loctl));
-	if (has_loopback_gain(phy) && lo->rebuild)
+	if (has_loopback_gain(phy))
 		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 < 0x258) {
 			if (feedth >= 0x12C)
 				*max_rx_gain += 6;
 			else
@@ -944,278 +746,188 @@ static void lo_probe_loctls_statemachine(struct b43_wldev *dev,
 	} 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)
+static
+struct b43_lo_calib * b43_calibrate_lo_setting(struct b43_wldev *dev,
+					       const struct b43_bbatt *bbatt,
+					       const struct b43_rfatt *rfatt)
 {
 	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;
-
+	struct b43_lo_calib *cal;
+	struct lo_g_saved_values uninitialized_var(saved_regs);
 	/* 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
+	saved_regs.old_channel = phy->channel;
+	b43_mac_suspend(dev);
+	lo_measure_setup(dev, &saved_regs);
 
 	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;
+	b43_radio_write16(dev, 0x43,
+			  (b43_radio_read16(dev, 0x43) & 0xFFF0)
+			  | rfatt->att);
+	b43_radio_write16(dev, txctl_reg,
+			  (b43_radio_read16(dev, txctl_reg) & ~txctl_value)
+			  | (rfatt->with_padmix) ? txctl_value : 0);
 
-	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;
-		}
+	max_rx_gain = rfatt->att * 2;
+	max_rx_gain += bbatt->att / 2;
+	if (rfatt->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->att);
+	lo_probe_loctls_statemachine(dev, &loctl, &max_rx_gain);
+
+	lo_measure_restore(dev, &saved_regs);
+	b43_mac_enable(dev);
+
+	if (b43_debug(dev, B43_DBG_LO)) {
+		b43dbg(dev->wl, "LO: Calibrated for BB(%u), RF(%u,%u) "
+		       "=> I=%d Q=%d\n",
+		       bbatt->att, rfatt->att, rfatt->with_padmix,
+		       loctl.i, loctl.q);
 	}
-	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;
+	cal = kmalloc(sizeof(*cal), GFP_KERNEL);
+	if (!cal) {
+		b43warn(dev->wl, "LO calib: out of memory\n");
+		return NULL;
 	}
+	memcpy(&cal->bbatt, bbatt, sizeof(*bbatt));
+	memcpy(&cal->rfatt, rfatt, sizeof(*rfatt));
+	memcpy(&cal->ctl, &loctl, sizeof(loctl));
+	cal->calib_time = jiffies;
+	INIT_LIST_HEAD(&cal->list);
+
+	return cal;
 }
 
-static void reset_all_loctl_calibration_states(struct b43_wldev *dev)
+/* Get a calibrated LO setting for the given attenuation values.
+ * Might return a NULL pointer under OOM! */
+static
+struct b43_lo_calib * b43_get_calib_lo_settings(struct b43_wldev *dev,
+						const struct b43_bbatt *bbatt,
+						const struct b43_rfatt *rfatt)
 {
-	b43_call_for_each_loctl(dev, do_reset_calib);
+	struct b43_txpower_lo_control *lo = dev->phy.lo_control;
+	struct b43_lo_calib *c;
+
+	c = b43_find_lo_calib(lo, bbatt, rfatt);
+	if (c)
+		return c;
+	/* Not in the list of calibrated LO settings.
+	 * Calibrate it now. */
+	c = b43_calibrate_lo_setting(dev, bbatt, rfatt);
+	if (!c)
+		return NULL;
+	list_add(&c->list, &lo->calib_list);
+
+	return c;
 }
 
-#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)
+void b43_gphy_dc_lt_init(struct b43_wldev *dev, bool update_all)
 {
 	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);
+	struct b43_txpower_lo_control *lo = phy->lo_control;
+	int i;
+	int rf_offset, bb_offset;
+	const struct b43_rfatt *rfatt;
+	const struct b43_bbatt *bbatt;
+	u64 power_vector;
+	bool table_changed = 0;
 
-	phy->lo_control->lo_measured = 1;
-	phy->lo_control->rebuild = 0;
-}
+	BUILD_BUG_ON(B43_DC_LT_SIZE != 32);
+	B43_WARN_ON(lo->rfatt_list.len * lo->bbatt_list.len > 64);
 
-#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;
+	power_vector = lo->power_vector;
+	if (!update_all && !power_vector)
+		return; /* Nothing to do. */
+
+	/* Suspend the MAC now to avoid continuous suspend/enable
+	 * cycles in the loop. */
+	b43_mac_suspend(dev);
+
+	for (i = 0; i < B43_DC_LT_SIZE * 2; i++) {
+		struct b43_lo_calib *cal;
+		int idx;
+		u16 val;
+
+		if (!update_all && !(power_vector & (((u64)1ULL) << i)))
+			continue;
+		/* Update the table entry for this power_vector bit.
+		 * The table rows are RFatt entries and columns are BBatt. */
+		bb_offset = i / lo->rfatt_list.len;
+		rf_offset = i % lo->rfatt_list.len;
+		bbatt = &(lo->bbatt_list.list[bb_offset]);
+		rfatt = &(lo->rfatt_list.list[rf_offset]);
+
+		cal = b43_calibrate_lo_setting(dev, bbatt, rfatt);
+		if (!cal) {
+			b43warn(dev->wl, "LO: Could not "
+				"calibrate DC table entry\n");
+			continue;
+		}
+		/*FIXME: Is Q really in the low nibble? */
+		val = (u8)(cal->ctl.q);
+		val |= ((u8)(cal->ctl.i)) << 4;
+		kfree(cal);
+
+		/* Get the index into the hardware DC LT. */
+		idx = i / 2;
+		/* Change the table in memory. */
+		if (i % 2) {
+			/* Change the high byte. */
+			lo->dc_lt[idx] = (lo->dc_lt[idx] & 0x00FF)
+					 | ((val & 0x00FF) << 8);
+		} else {
+			/* Change the low byte. */
+			lo->dc_lt[idx] = (lo->dc_lt[idx] & 0xFF00)
+					 | (val & 0x00FF);
+		}
+		table_changed = 1;
 	}
-	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)
-{
+	if (table_changed) {
+		/* The table changed in memory. Update the hardware table. */
+		for (i = 0; i < B43_DC_LT_SIZE; i++)
+			b43_phy_write(dev, 0x3A0 + i, lo->dc_lt[i]);
+	}
+	b43_mac_enable(dev);
 }
-#endif
 
-static inline void fixup_rfatt_for_txcontrol(struct b43_rfatt *rf,
-					     u8 tx_control)
+/* Fixup the RF attenuation value for the case where we are
+ * using the PAD mixer. */
+static inline void b43_lo_fixup_rfatt(struct b43_rfatt *rf)
 {
-	if (tx_control & B43_TXCTL_TXMIX) {
-		if (rf->att < 5)
-			rf->att = 4;
-	}
+	if (!rf->with_padmix)
+		return;
+	if ((rf->att != 1) && (rf->att != 2) && (rf->att != 3))
+		rf->att = 4;
 }
 
 void b43_lo_g_adjust(struct b43_wldev *dev)
 {
 	struct b43_phy *phy = &dev->phy;
+	struct b43_lo_calib *cal;
 	struct b43_rfatt rf;
-	struct b43_loctl *loctl;
 
 	memcpy(&rf, &phy->rfatt, sizeof(rf));
-	fixup_rfatt_for_txcontrol(&rf, phy->tx_control);
+	b43_lo_fixup_rfatt(&rf);
 
-	loctl = b43_get_lo_g_ctl(dev, &rf, &phy->bbatt);
-	validate_loctl_calibration(dev, loctl, &rf, &phy->bbatt);
-	b43_lo_write(dev, loctl);
+	cal = b43_get_calib_lo_settings(dev, &phy->bbatt, &rf);
+	if (!cal)
+		return;
+	b43_lo_write(dev, &cal->ctl);
 }
 
 void b43_lo_g_adjust_to(struct b43_wldev *dev,
@@ -1223,39 +935,102 @@ void b43_lo_g_adjust_to(struct b43_wldev *dev,
 {
 	struct b43_rfatt rf;
 	struct b43_bbatt bb;
-	struct b43_loctl *loctl;
+	struct b43_lo_calib *cal;
 
 	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);
+	b43_lo_fixup_rfatt(&rf);
+	cal = b43_get_calib_lo_settings(dev, &bb, &rf);
+	if (!cal)
+		return;
+	b43_lo_write(dev, &cal->ctl);
 }
 
-static void do_mark_unused(struct b43_wldev *dev, struct b43_loctl *control)
+/* Periodic LO maintanance work */
+void b43_lo_g_maintanance_work(struct b43_wldev *dev)
 {
-	control->used = 0;
+	struct b43_phy *phy = &dev->phy;
+	struct b43_txpower_lo_control *lo = phy->lo_control;
+	unsigned long now;
+	unsigned long expire;
+	struct b43_lo_calib *cal, *tmp;
+	bool current_item_expired = 0;
+	bool hwpctl;
+
+	if (!lo)
+		return;
+	now = jiffies;
+	hwpctl = b43_has_hardware_pctl(phy);
+
+	if (hwpctl) {
+		/* Read the power vector and update it, if needed. */
+		expire = now - B43_LO_PWRVEC_EXPIRE;
+		if (time_before(lo->pwr_vec_read_time, expire)) {
+			lo_read_power_vector(dev);
+			b43_gphy_dc_lt_init(dev, 0);
+		}
+		//FIXME Recalc the whole DC table from time to time?
+	}
+
+	if (hwpctl)
+		return;
+	/* Search for expired LO settings. Remove them.
+	 * Recalibrate the current setting, if expired. */
+	expire = now - B43_LO_CALIB_EXPIRE;
+	list_for_each_entry_safe(cal, tmp, &lo->calib_list, list) {
+		if (!time_before(cal->calib_time, expire))
+			continue;
+		/* This item expired. */
+		if (b43_compare_bbatt(&cal->bbatt, &phy->bbatt) &&
+		    b43_compare_rfatt(&cal->rfatt, &phy->rfatt)) {
+			B43_WARN_ON(current_item_expired);
+			current_item_expired = 1;
+		}
+		if (b43_debug(dev, B43_DBG_LO)) {
+			b43dbg(dev->wl, "LO: Item BB(%u), RF(%u,%u), "
+			       "I=%d, Q=%d expired\n",
+			       cal->bbatt.att, cal->rfatt.att,
+			       cal->rfatt.with_padmix,
+			       cal->ctl.i, cal->ctl.q);
+		}
+		list_del(&cal->list);
+		kfree(cal);
+	}
+	if (current_item_expired || unlikely(list_empty(&lo->calib_list))) {
+		/* Recalibrate currently used LO setting. */
+		if (b43_debug(dev, B43_DBG_LO))
+			b43dbg(dev->wl, "LO: Recalibrating current LO setting\n");
+		cal = b43_calibrate_lo_setting(dev, &phy->bbatt, &phy->rfatt);
+		if (cal) {
+			list_add(&cal->list, &lo->calib_list);
+			b43_lo_write(dev, &cal->ctl);
+		} else
+			b43warn(dev->wl, "Failed to recalibrate current LO setting\n");
+	}
 }
 
-void b43_lo_g_ctl_mark_all_unused(struct b43_wldev *dev)
+void b43_lo_g_cleanup(struct b43_wldev *dev)
 {
-	struct b43_phy *phy = &dev->phy;
-	struct b43_txpower_lo_control *lo = phy->lo_control;
+	struct b43_txpower_lo_control *lo = dev->phy.lo_control;
+	struct b43_lo_calib *cal, *tmp;
 
-	b43_call_for_each_loctl(dev, do_mark_unused);
-	lo->rebuild = 1;
+	if (!lo)
+		return;
+	list_for_each_entry_safe(cal, tmp, &lo->calib_list, list) {
+		list_del(&cal->list);
+		kfree(cal);
+	}
 }
 
-void b43_lo_g_ctl_mark_cur_used(struct b43_wldev *dev)
+/* LO Initialization */
+void b43_lo_g_init(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;
+	if (b43_has_hardware_pctl(phy)) {
+		lo_read_power_vector(dev);
+		b43_gphy_dc_lt_init(dev, 1);
+	}
 }
diff --git a/drivers/net/wireless/b43/lo.h b/drivers/net/wireless/b43/lo.h
index 455615d1f8c6..1da321cabc12 100644
--- a/drivers/net/wireless/b43/lo.h
+++ b/drivers/net/wireless/b43/lo.h
@@ -10,82 +10,63 @@ 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.
- */
+/* This struct holds calibrated LO settings for a set of
+ * Baseband and RF attenuation settings. */
+struct b43_lo_calib {
+	/* The set of attenuation values this set of LO
+	 * control values is calibrated for. */
+	struct b43_bbatt bbatt;
+	struct b43_rfatt rfatt;
+	/* The set of control values for the LO. */
+	struct b43_loctl ctl;
+	/* The time when these settings were calibrated (in jiffies) */
+	unsigned long calib_time;
+	/* List. */
+	struct list_head list;
+};
+
+/* Size of the DC Lookup Table in 16bit words. */
+#define B43_DC_LT_SIZE		32
+
+/* Local Oscillator calibration information */
 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. */
+	/* Lists of RF and BB attenuation values for this device.
+	 * Used for building hardware power control tables. */
 	struct b43_rfatt_list rfatt_list;
 	struct b43_bbatt_list bbatt_list;
 
+	/* The DC Lookup Table is cached in memory here.
+	 * Note that this is only used for Hardware Power Control. */
+	u16 dc_lt[B43_DC_LT_SIZE];
+
+	/* List of calibrated control values (struct b43_lo_calib). */
+	struct list_head calib_list;
+	/* Last time the power vector was read (jiffies). */
+	unsigned long pwr_vec_read_time;
+	/* Last time the txctl values were measured (jiffies). */
+	unsigned long txctl_measured_time;
+
 	/* 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);
+/* Calibration expire timeouts.
+ * Timeouts must be multiple of 15 seconds. To make sure
+ * the item really expired when the 15 second timer hits, we
+ * subtract two additional seconds from the timeout. */
+#define B43_LO_CALIB_EXPIRE	(HZ * (30 - 2))
+#define B43_LO_PWRVEC_EXPIRE	(HZ * (30 - 2))
+#define B43_LO_TXCTL_EXPIRE	(HZ * (180 - 4))
+
 
 /* Adjust the Local Oscillator to the saved attenuation
  * and txctl values.
@@ -95,18 +76,10 @@ void b43_lo_g_adjust(struct b43_wldev *dev);
 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);
+void b43_gphy_dc_lt_init(struct b43_wldev *dev, bool update_all);
 
-/* 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);
+void b43_lo_g_maintanance_work(struct b43_wldev *dev);
+void b43_lo_g_cleanup(struct b43_wldev *dev);
+void b43_lo_g_init(struct b43_wldev *dev);
 
 #endif /* B43_LO_H_ */
diff --git a/drivers/net/wireless/b43/main.c b/drivers/net/wireless/b43/main.c
index a70827793086..e78319aa47c1 100644
--- a/drivers/net/wireless/b43/main.c
+++ b/drivers/net/wireless/b43/main.c
@@ -373,13 +373,10 @@ static inline void b43_shm_control_word(struct b43_wldev *dev,
 	b43_write32(dev, B43_MMIO_SHM_CONTROL, control);
 }
 
-u32 b43_shm_read32(struct b43_wldev *dev, u16 routing, u16 offset)
+u32 __b43_shm_read32(struct b43_wldev *dev, u16 routing, u16 offset)
 {
-	struct b43_wl *wl = dev->wl;
-	unsigned long flags;
 	u32 ret;
 
-	spin_lock_irqsave(&wl->shm_lock, flags);
 	if (routing == B43_SHM_SHARED) {
 		B43_WARN_ON(offset & 0x0001);
 		if (offset & 0x0003) {
@@ -397,18 +394,26 @@ u32 b43_shm_read32(struct b43_wldev *dev, u16 routing, u16 offset)
 	b43_shm_control_word(dev, routing, offset);
 	ret = b43_read32(dev, B43_MMIO_SHM_DATA);
 out:
-	spin_unlock_irqrestore(&wl->shm_lock, flags);
-
 	return ret;
 }
 
-u16 b43_shm_read16(struct b43_wldev * dev, u16 routing, u16 offset)
+u32 b43_shm_read32(struct b43_wldev *dev, u16 routing, u16 offset)
 {
 	struct b43_wl *wl = dev->wl;
 	unsigned long flags;
-	u16 ret;
+	u32 ret;
 
 	spin_lock_irqsave(&wl->shm_lock, flags);
+	ret = __b43_shm_read32(dev, routing, offset);
+	spin_unlock_irqrestore(&wl->shm_lock, flags);
+
+	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) {
@@ -423,17 +428,24 @@ u16 b43_shm_read16(struct b43_wldev * dev, u16 routing, u16 offset)
 	b43_shm_control_word(dev, routing, offset);
 	ret = b43_read16(dev, B43_MMIO_SHM_DATA);
 out:
-	spin_unlock_irqrestore(&wl->shm_lock, flags);
-
 	return ret;
 }
 
-void b43_shm_write32(struct b43_wldev *dev, u16 routing, u16 offset, u32 value)
+u16 b43_shm_read16(struct b43_wldev *dev, u16 routing, u16 offset)
 {
 	struct b43_wl *wl = dev->wl;
 	unsigned long flags;
+	u16 ret;
 
 	spin_lock_irqsave(&wl->shm_lock, flags);
+	ret = __b43_shm_read16(dev, routing, offset);
+	spin_unlock_irqrestore(&wl->shm_lock, flags);
+
+	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) {
@@ -443,35 +455,47 @@ void b43_shm_write32(struct b43_wldev *dev, u16 routing, u16 offset, u32 value)
 				    (value >> 16) & 0xffff);
 			b43_shm_control_word(dev, routing, (offset >> 2) + 1);
 			b43_write16(dev, B43_MMIO_SHM_DATA, value & 0xffff);
-			goto out;
+			return;
 		}
 		offset >>= 2;
 	}
 	b43_shm_control_word(dev, routing, offset);
 	b43_write32(dev, B43_MMIO_SHM_DATA, value);
-out:
-	spin_unlock_irqrestore(&wl->shm_lock, flags);
 }
 
-void b43_shm_write16(struct b43_wldev *dev, u16 routing, u16 offset, u16 value)
+void b43_shm_write32(struct b43_wldev *dev, u16 routing, u16 offset, u32 value)
 {
 	struct b43_wl *wl = dev->wl;
 	unsigned long flags;
 
 	spin_lock_irqsave(&wl->shm_lock, flags);
+	__b43_shm_write32(dev, routing, offset, value);
+	spin_unlock_irqrestore(&wl->shm_lock, flags);
+}
+
+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);
 			b43_write16(dev, B43_MMIO_SHM_DATA_UNALIGNED, value);
-			goto out;
+			return;
 		}
 		offset >>= 2;
 	}
 	b43_shm_control_word(dev, routing, offset);
 	b43_write16(dev, B43_MMIO_SHM_DATA, value);
-out:
+}
+
+void b43_shm_write16(struct b43_wldev *dev, u16 routing, u16 offset, u16 value)
+{
+	struct b43_wl *wl = dev->wl;
+	unsigned long flags;
+
+	spin_lock_irqsave(&wl->shm_lock, flags);
+	__b43_shm_write16(dev, routing, offset, value);
 	spin_unlock_irqrestore(&wl->shm_lock, flags);
 }
 
@@ -1187,10 +1211,10 @@ static void handle_irq_noise(struct b43_wldev *dev)
 	/* 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);
+	noise[0] = clamp_val(noise[0], 0, ARRAY_SIZE(phy->nrssi_lt) - 1);
+	noise[1] = clamp_val(noise[1], 0, ARRAY_SIZE(phy->nrssi_lt) - 1);
+	noise[2] = clamp_val(noise[2], 0, ARRAY_SIZE(phy->nrssi_lt) - 1);
+	noise[3] = clamp_val(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]];
@@ -1372,18 +1396,18 @@ static void b43_write_beacon_template(struct b43_wldev *dev,
 	unsigned int rate;
 	u16 ctl;
 	int antenna;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(dev->wl->current_beacon);
 
 	bcn = (const struct ieee80211_mgmt *)(dev->wl->current_beacon->data);
 	len = min((size_t) dev->wl->current_beacon->len,
 		  0x200 - sizeof(struct b43_plcp_hdr6));
-	rate = dev->wl->beacon_txctl.tx_rate->hw_value;
+	rate = ieee80211_get_tx_rate(dev->wl->hw, info)->hw_value;
 
 	b43_write_template_common(dev, (const u8 *)bcn,
 				  len, ram_offset, shm_size_offset, rate);
 
 	/* Write the PHY TX control parameters. */
-	antenna = b43_antenna_from_ieee80211(dev,
-			dev->wl->beacon_txctl.antenna_sel_tx);
+	antenna = b43_antenna_from_ieee80211(dev, info->antenna_sel_tx);
 	antenna = b43_antenna_to_phyctl(antenna);
 	ctl = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_BEACPHYCTL);
 	/* We can't send beacons with short preamble. Would get PHY errors. */
@@ -1434,11 +1458,17 @@ static void b43_write_beacon_template(struct b43_wldev *dev,
 		i += ie_len + 2;
 	}
 	if (!tim_found) {
-		b43warn(dev->wl, "Did not find a valid TIM IE in "
-			"the beacon template packet. AP or IBSS operation "
-			"may be broken.\n");
-	} else
-		b43dbg(dev->wl, "Updated beacon template\n");
+		/*
+		 * If ucode wants to modify TIM do it behind the beacon, this
+		 * will happen, for example, when doing mesh networking.
+		 */
+		b43_shm_write16(dev, B43_SHM_SHARED,
+				B43_SHM_SH_TIMBPOS,
+				len + sizeof(struct b43_plcp_hdr6));
+		b43_shm_write16(dev, B43_SHM_SHARED,
+				B43_SHM_SH_DTIMPER, 0);
+	}
+	b43dbg(dev->wl, "Updated beacon template at 0x%x\n", ram_offset);
 }
 
 static void b43_write_probe_resp_plcp(struct b43_wldev *dev,
@@ -1577,7 +1607,8 @@ static void handle_irq_beacon(struct b43_wldev *dev)
 	struct b43_wl *wl = dev->wl;
 	u32 cmd, beacon0_valid, beacon1_valid;
 
-	if (!b43_is_mode(wl, IEEE80211_IF_TYPE_AP))
+	if (!b43_is_mode(wl, IEEE80211_IF_TYPE_AP) &&
+	    !b43_is_mode(wl, IEEE80211_IF_TYPE_MESH_POINT))
 		return;
 
 	/* This is the bottom half of the asynchronous beacon update. */
@@ -1644,19 +1675,27 @@ static void b43_beacon_update_trigger_work(struct work_struct *work)
 
 /* Asynchronously update the packet templates in template RAM.
  * Locking: Requires wl->irq_lock to be locked. */
-static void b43_update_templates(struct b43_wl *wl, struct sk_buff *beacon,
-				 const struct ieee80211_tx_control *txctl)
+static void b43_update_templates(struct b43_wl *wl)
 {
+	struct sk_buff *beacon;
+
 	/* This is the top half of the ansynchronous beacon update.
 	 * The bottom half is the beacon IRQ.
 	 * Beacon update must be asynchronous to avoid sending an
 	 * invalid beacon. This can happen for example, if the firmware
 	 * transmits a beacon while we are updating it. */
 
+	/* We could modify the existing beacon and set the aid bit in
+	 * the TIM field, but that would probably require resizing and
+	 * moving of data within the beacon template.
+	 * Simply request a new beacon and let mac80211 do the hard work. */
+	beacon = ieee80211_beacon_get(wl->hw, wl->vif);
+	if (unlikely(!beacon))
+		return;
+
 	if (wl->current_beacon)
 		dev_kfree_skb_any(wl->current_beacon);
 	wl->current_beacon = beacon;
-	memcpy(&wl->beacon_txctl, txctl, sizeof(wl->beacon_txctl));
 	wl->beacon0_uploaded = 0;
 	wl->beacon1_uploaded = 0;
 	queue_work(wl->hw->workqueue, &wl->beacon_update_trigger);
@@ -1695,9 +1734,100 @@ static void b43_set_beacon_int(struct b43_wldev *dev, u16 beacon_int)
 	b43dbg(dev->wl, "Set beacon interval to %u\n", beacon_int);
 }
 
+static void b43_handle_firmware_panic(struct b43_wldev *dev)
+{
+	u16 reason;
+
+	/* Read the register that contains the reason code for the panic. */
+	reason = b43_shm_read16(dev, B43_SHM_SCRATCH, B43_FWPANIC_REASON_REG);
+	b43err(dev->wl, "Whoopsy, firmware panic! Reason: %u\n", reason);
+
+	switch (reason) {
+	default:
+		b43dbg(dev->wl, "The panic reason is unknown.\n");
+		/* fallthrough */
+	case B43_FWPANIC_DIE:
+		/* Do not restart the controller or firmware.
+		 * The device is nonfunctional from now on.
+		 * Restarting would result in this panic to trigger again,
+		 * so we avoid that recursion. */
+		break;
+	case B43_FWPANIC_RESTART:
+		b43_controller_restart(dev, "Microcode panic");
+		break;
+	}
+}
+
 static void handle_irq_ucode_debug(struct b43_wldev *dev)
 {
-	//TODO
+	unsigned int i, cnt;
+	u16 reason, marker_id, marker_line;
+	__le16 *buf;
+
+	/* The proprietary firmware doesn't have this IRQ. */
+	if (!dev->fw.opensource)
+		return;
+
+	/* Read the register that contains the reason code for this IRQ. */
+	reason = b43_shm_read16(dev, B43_SHM_SCRATCH, B43_DEBUGIRQ_REASON_REG);
+
+	switch (reason) {
+	case B43_DEBUGIRQ_PANIC:
+		b43_handle_firmware_panic(dev);
+		break;
+	case B43_DEBUGIRQ_DUMP_SHM:
+		if (!B43_DEBUG)
+			break; /* Only with driver debugging enabled. */
+		buf = kmalloc(4096, GFP_ATOMIC);
+		if (!buf) {
+			b43dbg(dev->wl, "SHM-dump: Failed to allocate memory\n");
+			goto out;
+		}
+		for (i = 0; i < 4096; i += 2) {
+			u16 tmp = b43_shm_read16(dev, B43_SHM_SHARED, i);
+			buf[i / 2] = cpu_to_le16(tmp);
+		}
+		b43info(dev->wl, "Shared memory dump:\n");
+		print_hex_dump(KERN_INFO, "", DUMP_PREFIX_OFFSET,
+			       16, 2, buf, 4096, 1);
+		kfree(buf);
+		break;
+	case B43_DEBUGIRQ_DUMP_REGS:
+		if (!B43_DEBUG)
+			break; /* Only with driver debugging enabled. */
+		b43info(dev->wl, "Microcode register dump:\n");
+		for (i = 0, cnt = 0; i < 64; i++) {
+			u16 tmp = b43_shm_read16(dev, B43_SHM_SCRATCH, i);
+			if (cnt == 0)
+				printk(KERN_INFO);
+			printk("r%02u: 0x%04X  ", i, tmp);
+			cnt++;
+			if (cnt == 6) {
+				printk("\n");
+				cnt = 0;
+			}
+		}
+		printk("\n");
+		break;
+	case B43_DEBUGIRQ_MARKER:
+		if (!B43_DEBUG)
+			break; /* Only with driver debugging enabled. */
+		marker_id = b43_shm_read16(dev, B43_SHM_SCRATCH,
+					   B43_MARKER_ID_REG);
+		marker_line = b43_shm_read16(dev, B43_SHM_SCRATCH,
+					     B43_MARKER_LINE_REG);
+		b43info(dev->wl, "The firmware just executed the MARKER(%u) "
+			"at line number %u\n",
+			marker_id, marker_line);
+		break;
+	default:
+		b43dbg(dev->wl, "Debug-IRQ triggered for unknown reason: %u\n",
+		       reason);
+	}
+out:
+	/* Acknowledge the debug-IRQ, so the firmware can continue. */
+	b43_shm_write16(dev, B43_SHM_SCRATCH,
+			B43_DEBUGIRQ_REASON_REG, B43_DEBUGIRQ_ACK);
 }
 
 /* Interrupt handler bottom-half */
@@ -1884,7 +2014,8 @@ static void b43_print_fw_helptext(struct b43_wl *wl, bool error)
 
 static int do_request_fw(struct b43_wldev *dev,
 			 const char *name,
-			 struct b43_firmware_file *fw)
+			 struct b43_firmware_file *fw,
+			 bool silent)
 {
 	char path[sizeof(modparam_fwpostfix) + 32];
 	const struct firmware *blob;
@@ -1908,9 +2039,15 @@ static int do_request_fw(struct b43_wldev *dev,
 		 "b43%s/%s.fw",
 		 modparam_fwpostfix, name);
 	err = request_firmware(&blob, path, dev->dev->dev);
-	if (err) {
-		b43err(dev->wl, "Firmware file \"%s\" not found "
-		       "or load failed.\n", path);
+	if (err == -ENOENT) {
+		if (!silent) {
+			b43err(dev->wl, "Firmware file \"%s\" not found\n",
+			       path);
+		}
+		return err;
+	} else if (err) {
+		b43err(dev->wl, "Firmware file \"%s\" request failed (err=%d)\n",
+		       path, err);
 		return err;
 	}
 	if (blob->size < sizeof(struct b43_fw_header))
@@ -1961,7 +2098,7 @@ static int b43_request_firmware(struct b43_wldev *dev)
 		filename = "ucode13";
 	else
 		goto err_no_ucode;
-	err = do_request_fw(dev, filename, &fw->ucode);
+	err = do_request_fw(dev, filename, &fw->ucode, 0);
 	if (err)
 		goto err_load;
 
@@ -1972,8 +2109,13 @@ static int b43_request_firmware(struct b43_wldev *dev)
 		filename = NULL;
 	else
 		goto err_no_pcm;
-	err = do_request_fw(dev, filename, &fw->pcm);
-	if (err)
+	fw->pcm_request_failed = 0;
+	err = do_request_fw(dev, filename, &fw->pcm, 1);
+	if (err == -ENOENT) {
+		/* We did not find a PCM file? Not fatal, but
+		 * core rev <= 10 must do without hwcrypto then. */
+		fw->pcm_request_failed = 1;
+	} else if (err)
 		goto err_load;
 
 	/* Get initvals */
@@ -1991,7 +2133,7 @@ static int b43_request_firmware(struct b43_wldev *dev)
 		if ((rev >= 5) && (rev <= 10))
 			filename = "b0g0initvals5";
 		else if (rev >= 13)
-			filename = "lp0initvals13";
+			filename = "b0g0initvals13";
 		else
 			goto err_no_initvals;
 		break;
@@ -2004,7 +2146,7 @@ static int b43_request_firmware(struct b43_wldev *dev)
 	default:
 		goto err_no_initvals;
 	}
-	err = do_request_fw(dev, filename, &fw->initvals);
+	err = do_request_fw(dev, filename, &fw->initvals, 0);
 	if (err)
 		goto err_load;
 
@@ -2038,7 +2180,7 @@ static int b43_request_firmware(struct b43_wldev *dev)
 	default:
 		goto err_no_initvals;
 	}
-	err = do_request_fw(dev, filename, &fw->initvals_band);
+	err = do_request_fw(dev, filename, &fw->initvals_band, 0);
 	if (err)
 		goto err_load;
 
@@ -2155,14 +2297,28 @@ static int b43_upload_microcode(struct b43_wldev *dev)
 		err = -EOPNOTSUPP;
 		goto error;
 	}
-	b43info(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;
+	dev->fw.opensource = (fwdate == 0xFFFF);
+
+	if (dev->fw.opensource) {
+		/* Patchlevel info is encoded in the "time" field. */
+		dev->fw.patch = fwtime;
+		b43info(dev->wl, "Loading OpenSource firmware version %u.%u%s\n",
+			dev->fw.rev, dev->fw.patch,
+			dev->fw.pcm_request_failed ? " (Hardware crypto not supported)" : "");
+	} else {
+		b43info(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);
+		if (dev->fw.pcm_request_failed) {
+			b43warn(dev->wl, "No \"pcm5.fw\" firmware file found. "
+				"Hardware accelerated cryptography is disabled.\n");
+			b43_print_fw_helptext(dev->wl, 0);
+		}
+	}
 
 	if (b43_is_old_txhdr_format(dev)) {
 		b43warn(dev->wl, "You are using an old firmware image. "
@@ -2339,8 +2495,21 @@ static void b43_gpio_cleanup(struct b43_wldev *dev)
 }
 
 /* http://bcm-specs.sipsolutions.net/EnableMac */
-static void b43_mac_enable(struct b43_wldev *dev)
-{
+void b43_mac_enable(struct b43_wldev *dev)
+{
+	if (b43_debug(dev, B43_DBG_FIRMWARE)) {
+		u16 fwstate;
+
+		fwstate = b43_shm_read16(dev, B43_SHM_SHARED,
+					 B43_SHM_SH_UCODESTAT);
+		if ((fwstate != B43_SHM_SH_UCODESTAT_SUSP) &&
+		    (fwstate != B43_SHM_SH_UCODESTAT_SLEEP)) {
+			b43err(dev->wl, "b43_mac_enable(): The firmware "
+			       "should be suspended, but current state is %u\n",
+			       fwstate);
+		}
+	}
+
 	dev->mac_suspended--;
 	B43_WARN_ON(dev->mac_suspended < 0);
 	if (dev->mac_suspended == 0) {
@@ -2353,16 +2522,11 @@ static void b43_mac_enable(struct b43_wldev *dev)
 		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 */
-static void b43_mac_suspend(struct b43_wldev *dev)
+void b43_mac_suspend(struct b43_wldev *dev)
 {
 	int i;
 	u32 tmp;
@@ -2371,14 +2535,6 @@ static void b43_mac_suspend(struct b43_wldev *dev)
 	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)
@@ -2420,7 +2576,8 @@ static void b43_adjust_opmode(struct b43_wldev *dev)
 	ctl &= ~B43_MACCTL_BEACPROMISC;
 	ctl |= B43_MACCTL_INFRA;
 
-	if (b43_is_mode(wl, IEEE80211_IF_TYPE_AP))
+	if (b43_is_mode(wl, IEEE80211_IF_TYPE_AP) ||
+	    b43_is_mode(wl, IEEE80211_IF_TYPE_MESH_POINT))
 		ctl |= B43_MACCTL_AP;
 	else if (b43_is_mode(wl, IEEE80211_IF_TYPE_IBSS))
 		ctl &= ~B43_MACCTL_INFRA;
@@ -2534,6 +2691,7 @@ static void b43_chip_exit(struct b43_wldev *dev)
 {
 	b43_radio_turn_off(dev, 1);
 	b43_gpio_cleanup(dev);
+	b43_lo_g_cleanup(dev);
 	/* firmware is released later */
 }
 
@@ -2640,28 +2798,12 @@ err_gpio_clean:
 	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 (phy->type != B43_PHYTYPE_G)
 		return;
-	if (!b43_has_hardware_pctl(phy))
-		b43_lo_g_ctl_mark_all_unused(dev);
 	if (dev->dev->bus->sprom.boardflags_lo & B43_BFL_RSSI) {
 		b43_mac_suspend(dev);
 		b43_calc_nrssi_slope(dev);
@@ -2688,6 +2830,21 @@ static void b43_periodic_every30sec(struct b43_wldev *dev)
 static void b43_periodic_every15sec(struct b43_wldev *dev)
 {
 	struct b43_phy *phy = &dev->phy;
+	u16 wdr;
+
+	if (dev->fw.opensource) {
+		/* Check if the firmware is still alive.
+		 * It will reset the watchdog counter to 0 in its idle loop. */
+		wdr = b43_shm_read16(dev, B43_SHM_SCRATCH, B43_WATCHDOG_REG);
+		if (unlikely(wdr)) {
+			b43err(dev->wl, "Firmware watchdog: The firmware died!\n");
+			b43_controller_restart(dev, "Firmware watchdog");
+			return;
+		} else {
+			b43_shm_write16(dev, B43_SHM_SCRATCH,
+					B43_WATCHDOG_REG, 1);
+		}
+	}
 
 	if (phy->type == B43_PHYTYPE_G) {
 		//TODO: update_aci_moving_average
@@ -2713,6 +2870,7 @@ static void b43_periodic_every15sec(struct b43_wldev *dev)
 		}
 	}
 	b43_phy_xmitpower(dev);	//FIXME: unless scanning?
+	b43_lo_g_maintanance_work(dev);
 	//TODO for APHY (temperature?)
 
 	atomic_set(&phy->txerr_cnt, B43_PHY_TX_BADNESS_LIMIT);
@@ -2724,8 +2882,6 @@ 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)
@@ -2873,8 +3029,7 @@ static int b43_rng_init(struct b43_wl *wl)
 }
 
 static int b43_op_tx(struct ieee80211_hw *hw,
-		     struct sk_buff *skb,
-		     struct ieee80211_tx_control *ctl)
+		     struct sk_buff *skb)
 {
 	struct b43_wl *wl = hw_to_b43_wl(hw);
 	struct b43_wldev *dev = wl->current_dev;
@@ -2895,9 +3050,9 @@ static int b43_op_tx(struct ieee80211_hw *hw,
 	err = -ENODEV;
 	if (likely(b43_status(dev) >= B43_STAT_STARTED)) {
 		if (b43_using_pio_transfers(dev))
-			err = b43_pio_tx(dev, skb, ctl);
+			err = b43_pio_tx(dev, skb);
 		else
-			err = b43_dma_tx(dev, skb, ctl);
+			err = b43_dma_tx(dev, skb);
 	}
 
 	read_unlock_irqrestore(&wl->tx_lock, flags);
@@ -2918,53 +3073,20 @@ static void b43_qos_params_upload(struct b43_wldev *dev,
 				  u16 shm_offset)
 {
 	u16 params[B43_NR_QOSPARAMS];
-	int cw_min, cw_max, aifs, bslots, tmp;
+	int bslots, tmp;
 	unsigned int i;
 
-	const u16 aCWmin = 0x0001;
-	const u16 aCWmax = 0x03FF;
-
-	/* Calculate the default values for the parameters, if needed. */
-	switch (shm_offset) {
-	case B43_QOS_VOICE:
-		aifs = (p->aifs == -1) ? 2 : p->aifs;
-		cw_min = (p->cw_min == 0) ? ((aCWmin + 1) / 4 - 1) : p->cw_min;
-		cw_max = (p->cw_max == 0) ? ((aCWmin + 1) / 2 - 1) : p->cw_max;
-		break;
-	case B43_QOS_VIDEO:
-		aifs = (p->aifs == -1) ? 2 : p->aifs;
-		cw_min = (p->cw_min == 0) ? ((aCWmin + 1) / 2 - 1) : p->cw_min;
-		cw_max = (p->cw_max == 0) ? aCWmin : p->cw_max;
-		break;
-	case B43_QOS_BESTEFFORT:
-		aifs = (p->aifs == -1) ? 3 : p->aifs;
-		cw_min = (p->cw_min == 0) ? aCWmin : p->cw_min;
-		cw_max = (p->cw_max == 0) ? aCWmax : p->cw_max;
-		break;
-	case B43_QOS_BACKGROUND:
-		aifs = (p->aifs == -1) ? 7 : p->aifs;
-		cw_min = (p->cw_min == 0) ? aCWmin : p->cw_min;
-		cw_max = (p->cw_max == 0) ? aCWmax : p->cw_max;
-		break;
-	default:
-		B43_WARN_ON(1);
-		return;
-	}
-	if (cw_min <= 0)
-		cw_min = aCWmin;
-	if (cw_max <= 0)
-		cw_max = aCWmin;
-	bslots = b43_read16(dev, B43_MMIO_RNG) % cw_min;
+	bslots = b43_read16(dev, B43_MMIO_RNG) & p->cw_min;
 
 	memset(&params, 0, sizeof(params));
 
 	params[B43_QOSPARAM_TXOP] = p->txop * 32;
-	params[B43_QOSPARAM_CWMIN] = cw_min;
-	params[B43_QOSPARAM_CWMAX] = cw_max;
-	params[B43_QOSPARAM_CWCUR] = cw_min;
-	params[B43_QOSPARAM_AIFS] = aifs;
+	params[B43_QOSPARAM_CWMIN] = p->cw_min;
+	params[B43_QOSPARAM_CWMAX] = p->cw_max;
+	params[B43_QOSPARAM_CWCUR] = p->cw_min;
+	params[B43_QOSPARAM_AIFS] = p->aifs;
 	params[B43_QOSPARAM_BSLOTS] = bslots;
-	params[B43_QOSPARAM_REGGAP] = bslots + aifs;
+	params[B43_QOSPARAM_REGGAP] = bslots + p->aifs;
 
 	for (i = 0; i < ARRAY_SIZE(params); i++) {
 		if (i == B43_QOSPARAM_STATUS) {
@@ -3060,8 +3182,7 @@ static void b43_qos_update_work(struct work_struct *work)
 	mutex_unlock(&wl->mutex);
 }
 
-static int b43_op_conf_tx(struct ieee80211_hw *hw,
-			  int _queue,
+static int b43_op_conf_tx(struct ieee80211_hw *hw, u16 _queue,
 			  const struct ieee80211_tx_queue_params *params)
 {
 	struct b43_wl *wl = hw_to_b43_wl(hw);
@@ -3309,8 +3430,9 @@ static int b43_op_config(struct ieee80211_hw *hw, struct ieee80211_conf *conf)
 	antenna = b43_antenna_from_ieee80211(dev, conf->antenna_sel_rx);
 	b43_set_rx_antenna(dev, antenna);
 
-	/* Update templates for AP mode. */
-	if (b43_is_mode(wl, IEEE80211_IF_TYPE_AP))
+	/* Update templates for AP/mesh mode. */
+	if (b43_is_mode(wl, IEEE80211_IF_TYPE_AP) ||
+	    b43_is_mode(wl, IEEE80211_IF_TYPE_MESH_POINT))
 		b43_set_beacon_int(dev, conf->beacon_int);
 
 	if (!!conf->radio_enabled != phy->radio_on) {
@@ -3361,6 +3483,13 @@ static int b43_op_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 	if (!dev || b43_status(dev) < B43_STAT_INITIALIZED)
 		goto out_unlock;
 
+	if (dev->fw.pcm_request_failed) {
+		/* We don't have firmware for the crypto engine.
+		 * Must use software-crypto. */
+		err = -EOPNOTSUPP;
+		goto out_unlock;
+	}
+
 	err = -EINVAL;
 	switch (key->alg) {
 	case ALG_WEP:
@@ -3491,13 +3620,16 @@ static int b43_op_config_interface(struct ieee80211_hw *hw,
 	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_update_templates(wl, conf->beacon,
-						     conf->beacon_control);
-			}
+		if (b43_is_mode(wl, IEEE80211_IF_TYPE_AP) ||
+		    b43_is_mode(wl, IEEE80211_IF_TYPE_MESH_POINT)) {
+			B43_WARN_ON(vif->type != wl->if_type);
+			if (conf->changed & IEEE80211_IFCC_SSID)
+				b43_set_ssid(dev, conf->ssid, conf->ssid_len);
+			if (conf->changed & IEEE80211_IFCC_BEACON)
+				b43_update_templates(wl);
+		} else if (b43_is_mode(wl, IEEE80211_IF_TYPE_IBSS)) {
+			if (conf->changed & IEEE80211_IFCC_BEACON)
+				b43_update_templates(wl);
 		}
 		b43_write_mac_bssid_templates(dev);
 	}
@@ -3562,7 +3694,6 @@ static int b43_wireless_core_start(struct b43_wldev *dev)
 	/* 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);
@@ -3703,8 +3834,8 @@ static void setup_struct_phy_for_init(struct b43_wldev *dev,
 	lo = phy->lo_control;
 	if (lo) {
 		memset(lo, 0, sizeof(*(phy->lo_control)));
-		lo->rebuild = 1;
 		lo->tx_bias = 0xFF;
+		INIT_LIST_HEAD(&lo->calib_list);
 	}
 	phy->max_lb_gain = 0;
 	phy->trsw_rx_gain = 0;
@@ -4035,6 +4166,7 @@ static int b43_op_add_interface(struct ieee80211_hw *hw,
 	/* TODO: allow WDS/AP devices to coexist */
 
 	if (conf->type != IEEE80211_IF_TYPE_AP &&
+	    conf->type != IEEE80211_IF_TYPE_MESH_POINT &&
 	    conf->type != IEEE80211_IF_TYPE_STA &&
 	    conf->type != IEEE80211_IF_TYPE_WDS &&
 	    conf->type != IEEE80211_IF_TYPE_IBSS)
@@ -4185,33 +4317,10 @@ out_unlock:
 static int b43_op_beacon_set_tim(struct ieee80211_hw *hw, int aid, int set)
 {
 	struct b43_wl *wl = hw_to_b43_wl(hw);
-	struct sk_buff *beacon;
-	unsigned long flags;
-	struct ieee80211_tx_control txctl;
-
-	/* We could modify the existing beacon and set the aid bit in
-	 * the TIM field, but that would probably require resizing and
-	 * moving of data within the beacon template.
-	 * Simply request a new beacon and let mac80211 do the hard work. */
-	beacon = ieee80211_beacon_get(hw, wl->vif, &txctl);
-	if (unlikely(!beacon))
-		return -ENOMEM;
-	spin_lock_irqsave(&wl->irq_lock, flags);
-	b43_update_templates(wl, beacon, &txctl);
-	spin_unlock_irqrestore(&wl->irq_lock, flags);
-
-	return 0;
-}
-
-static int b43_op_ibss_beacon_update(struct ieee80211_hw *hw,
-				     struct sk_buff *beacon,
-				     struct ieee80211_tx_control *ctl)
-{
-	struct b43_wl *wl = hw_to_b43_wl(hw);
 	unsigned long flags;
 
 	spin_lock_irqsave(&wl->irq_lock, flags);
-	b43_update_templates(wl, beacon, ctl);
+	b43_update_templates(wl);
 	spin_unlock_irqrestore(&wl->irq_lock, flags);
 
 	return 0;
@@ -4242,7 +4351,6 @@ static const struct ieee80211_ops b43_hw_ops = {
 	.stop			= b43_op_stop,
 	.set_retry_limit	= b43_op_set_retry_limit,
 	.set_tim		= b43_op_beacon_set_tim,
-	.beacon_update		= b43_op_ibss_beacon_update,
 	.sta_notify		= b43_op_sta_notify,
 };
 
@@ -4538,10 +4646,10 @@ static int b43_wireless_init(struct ssb_device *dev)
 
 	/* 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;
+		    IEEE80211_HW_RX_INCLUDES_FCS |
+		    IEEE80211_HW_SIGNAL_DBM |
+		    IEEE80211_HW_NOISE_DBM;
+
 	hw->queues = b43_modparam_qos ? 4 : 1;
 	SET_IEEE80211_DEV(hw, dev->dev);
 	if (is_valid_ether_addr(sprom->et1mac))
diff --git a/drivers/net/wireless/b43/main.h b/drivers/net/wireless/b43/main.h
index 5230aeca78bf..f871a252cb55 100644
--- a/drivers/net/wireless/b43/main.h
+++ b/drivers/net/wireless/b43/main.h
@@ -95,9 +95,13 @@ 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);
+u32 __b43_shm_read32(struct b43_wldev *dev, u16 routing, u16 offset);
 u16 b43_shm_read16(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_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);
+void __b43_shm_write16(struct b43_wldev *dev, u16 routing, u16 offset, u16 value);
 
 u64 b43_hf_read(struct b43_wldev *dev);
 void b43_hf_write(struct b43_wldev *dev, u64 value);
@@ -114,4 +118,7 @@ void b43_controller_restart(struct b43_wldev *dev, const char *reason);
 #define B43_PS_ASLEEP	(1 << 3)	/* Force device asleep */
 void b43_power_saving_ctl_bits(struct b43_wldev *dev, unsigned int ps_flags);
 
+void b43_mac_suspend(struct b43_wldev *dev);
+void b43_mac_enable(struct b43_wldev *dev);
+
 #endif /* B43_MAIN_H_ */
diff --git a/drivers/net/wireless/b43/nphy.c b/drivers/net/wireless/b43/nphy.c
index 8695eb223476..644eed993bea 100644
--- a/drivers/net/wireless/b43/nphy.c
+++ b/drivers/net/wireless/b43/nphy.c
@@ -29,8 +29,6 @@
 #include "nphy.h"
 #include "tables_nphy.h"
 
-#include <linux/delay.h>
-
 
 void b43_nphy_set_rxantenna(struct b43_wldev *dev, int antenna)
 {//TODO
diff --git a/drivers/net/wireless/b43/phy.c b/drivers/net/wireless/b43/phy.c
index de024dc03718..305d4cd6fd03 100644
--- a/drivers/net/wireless/b43/phy.c
+++ b/drivers/net/wireless/b43/phy.c
@@ -28,6 +28,7 @@
 #include <linux/delay.h>
 #include <linux/io.h>
 #include <linux/types.h>
+#include <linux/bitrev.h>
 
 #include "b43.h"
 #include "phy.h"
@@ -83,25 +84,9 @@ const u8 b43_radio_channel_codes_bg[] = {
 	72, 84,
 };
 
+#define bitrev4(tmp) (bitrev8(tmp) >> 4)
 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)
 {
@@ -145,8 +130,7 @@ static void generate_rfatt_list(struct b43_wldev *dev,
 		{.att = 9,.with_padmix = 1,},
 	};
 
-	if ((phy->type == B43_PHYTYPE_A && phy->rev < 5) ||
-	    (phy->type == B43_PHYTYPE_G && phy->rev < 6)) {
+	if (!b43_has_hardware_pctl(phy)) {
 		/* Software pctl */
 		list->list = rfatt_0;
 		list->len = ARRAY_SIZE(rfatt_0);
@@ -158,7 +142,7 @@ static void generate_rfatt_list(struct b43_wldev *dev,
 		/* Hardware pctl */
 		list->list = rfatt_1;
 		list->len = ARRAY_SIZE(rfatt_1);
-		list->min_val = 2;
+		list->min_val = 0;
 		list->max_val = 14;
 		return;
 	}
@@ -346,6 +330,7 @@ void b43_set_txpower_g(struct b43_wldev *dev,
 	/* Save the values for later */
 	phy->tx_control = tx_control;
 	memcpy(&phy->rfatt, rfatt, sizeof(*rfatt));
+	phy->rfatt.with_padmix = !!(tx_control & B43_TXCTL_TXMIX);
 	memcpy(&phy->bbatt, bbatt, sizeof(*bbatt));
 
 	if (b43_debug(dev, B43_DBG_XMITPOWER)) {
@@ -559,11 +544,6 @@ static void b43_gphy_gain_lt_init(struct b43_wldev *dev)
 	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)
@@ -581,42 +561,6 @@ static void b43_gphy_gain_lt_init(struct b43_wldev *dev)
 	}
 }
 
-/* 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
@@ -643,7 +587,7 @@ static void hardware_pctl_init_gphy(struct b43_wldev *dev)
 	b43_phy_write(dev, 0x0801, b43_phy_read(dev, 0x0801)
 		      & 0xFFBF);
 
-	b43_gphy_dc_lt_init(dev);
+	b43_gphy_dc_lt_init(dev, 1);
 }
 
 /* HardwarePowerControl init for A and G PHY */
@@ -931,109 +875,6 @@ static void b43_phy_inita(struct b43_wldev *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.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;
@@ -1259,19 +1100,9 @@ static void b43_phy_initb6(struct b43_wldev *dev)
 		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.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)
+	if (phy->type == B43_PHYTYPE_B)
+		B43_WARN_ON(1);
+	else if (phy->type == B43_PHYTYPE_G)
 		b43_write16(dev, 0x03E6, 0x0);
 }
 
@@ -1534,34 +1365,31 @@ static void b43_phy_initg(struct b43_wldev *dev)
 		else
 			b43_radio_write16(dev, 0x0078, phy->initval);
 	}
-	if (phy->lo_control->tx_bias == 0xFF) {
-		b43_lo_g_measure(dev);
+	b43_lo_g_init(dev);
+	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 {
-		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_CCK(0x36),
-				      (b43_phy_read(dev, B43_PHY_CCK(0x36))
-				       & 0x0FFF) | (phy->lo_control->
-						    tx_bias << 12));
-		}
-		if (dev->dev->bus->sprom.boardflags_lo & B43_BFL_PACTRL)
-			b43_phy_write(dev, B43_PHY_CCK(0x2E), 0x8075);
-		else
-			b43_phy_write(dev, B43_PHY_CCK(0x2E), 0x807F);
-		if (phy->rev < 2)
-			b43_phy_write(dev, B43_PHY_CCK(0x2F), 0x101);
-		else
-			b43_phy_write(dev, B43_PHY_CCK(0x2F), 0x202);
+		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_CCK(0x36),
+			      (b43_phy_read(dev, B43_PHY_CCK(0x36))
+			       & 0x0FFF) | (phy->lo_control->
+					    tx_bias << 12));
+	}
+	if (dev->dev->bus->sprom.boardflags_lo & B43_BFL_PACTRL)
+		b43_phy_write(dev, B43_PHY_CCK(0x2E), 0x8075);
+	else
+		b43_phy_write(dev, B43_PHY_CCK(0x2E), 0x807F);
+	if (phy->rev < 2)
+		b43_phy_write(dev, B43_PHY_CCK(0x2F), 0x101);
+	else
+		b43_phy_write(dev, B43_PHY_CCK(0x2F), 0x202);
 	if (phy->gmode || phy->rev >= 2) {
 		b43_lo_g_adjust(dev);
 		b43_phy_write(dev, B43_PHY_LO_MASK, 0x8078);
@@ -1572,7 +1400,7 @@ static void b43_phy_initg(struct b43_wldev *dev)
 		 * 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())
+		 * entries to -32 (see the clamp_val() in nrssi_hw_update())
 		 */
 		b43_nrssi_hw_update(dev, 0xFFFF);	//FIXME?
 		b43_calc_nrssi_threshold(dev);
@@ -1634,13 +1462,13 @@ static s8 b43_phy_estimate_power_out(struct b43_wldev *dev, s8 tssi)
 	switch (phy->type) {
 	case B43_PHYTYPE_A:
 		tmp += 0x80;
-		tmp = limit_value(tmp, 0x00, 0xFF);
+		tmp = clamp_val(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);
+		tmp = clamp_val(tmp, 0x00, 0x3F);
 		dbm = phy->tssi2dbm[tmp];
 		break;
 	default:
@@ -1699,8 +1527,8 @@ void b43_put_attenuation_into_ranges(struct b43_wldev *dev,
 		break;
 	}
 
-	*_rfatt = limit_value(rfatt, rf_min, rf_max);
-	*_bbatt = limit_value(bbatt, bb_min, bb_max);
+	*_rfatt = clamp_val(rfatt, rf_min, rf_max);
+	*_bbatt = clamp_val(bbatt, bb_min, bb_max);
 }
 
 /* http://bcm-specs.sipsolutions.net/RecalculateTransmissionPower */
@@ -1795,7 +1623,7 @@ void b43_phy_xmitpower(struct b43_wldev *dev)
 			/* 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);
+			desired_pwr = clamp_val(desired_pwr, 0, max_pwr);
 			if (b43_debug(dev, B43_DBG_XMITPOWER)) {
 				b43dbg(dev->wl,
 				       "Current TX power output: " Q52_FMT
@@ -1821,10 +1649,8 @@ void b43_phy_xmitpower(struct b43_wldev *dev)
 			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);
+			if ((rfatt_delta == 0) && (bbatt_delta == 0))
 				return;
-			}
 
 			/* Calculate the new attenuation values. */
 			bbatt = phy->bbatt.att;
@@ -1870,7 +1696,6 @@ void b43_phy_xmitpower(struct b43_wldev *dev)
 			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);
 			break;
@@ -1908,7 +1733,7 @@ static inline
 		f = q;
 		i++;
 	} while (delta >= 2);
-	entry[index] = limit_value(b43_tssi2dbm_ad(m1 * f, 8192), -127, 128);
+	entry[index] = clamp_val(b43_tssi2dbm_ad(m1 * f, 8192), -127, 128);
 	return 0;
 }
 
@@ -2007,24 +1832,6 @@ int b43_phy_init(struct b43_wldev *dev)
 		else
 			unsupported = 1;
 		break;
-	case B43_PHYTYPE_B:
-		switch (phy->rev) {
-		case 2:
-			b43_phy_initb2(dev);
-			break;
-		case 4:
-			b43_phy_initb4(dev);
-			break;
-		case 5:
-			b43_phy_initb5(dev);
-			break;
-		case 6:
-			b43_phy_initb6(dev);
-			break;
-		default:
-			unsupported = 1;
-		}
-		break;
 	case B43_PHYTYPE_G:
 		b43_phy_initg(dev);
 		break;
@@ -2452,7 +2259,7 @@ void b43_nrssi_hw_update(struct b43_wldev *dev, u16 val)
 	for (i = 0; i < 64; i++) {
 		tmp = b43_nrssi_hw_read(dev, i);
 		tmp -= val;
-		tmp = limit_value(tmp, -32, 31);
+		tmp = clamp_val(tmp, -32, 31);
 		b43_nrssi_hw_write(dev, i, tmp);
 	}
 }
@@ -2469,7 +2276,7 @@ void b43_nrssi_mem_update(struct b43_wldev *dev)
 		tmp = (i - delta) * phy->nrssislope;
 		tmp /= 0x10000;
 		tmp += 0x3A;
-		tmp = limit_value(tmp, 0, 0x3F);
+		tmp = clamp_val(tmp, 0, 0x3F);
 		phy->nrssi_lt[i] = tmp;
 	}
 }
@@ -2906,7 +2713,7 @@ void b43_calc_nrssi_threshold(struct b43_wldev *dev)
 			} else
 				threshold = phy->nrssi[1] - 5;
 
-			threshold = limit_value(threshold, 0, 0x3E);
+			threshold = clamp_val(threshold, 0, 0x3E);
 			b43_phy_read(dev, 0x0020);	/* dummy read */
 			b43_phy_write(dev, 0x0020,
 				      (((u16) threshold) << 8) | 0x001C);
@@ -2957,7 +2764,7 @@ void b43_calc_nrssi_threshold(struct b43_wldev *dev)
 			else
 				a += 32;
 			a = a >> 6;
-			a = limit_value(a, -31, 31);
+			a = clamp_val(a, -31, 31);
 
 			b = b * (phy->nrssi[1] - phy->nrssi[0]);
 			b += (phy->nrssi[0] << 6);
@@ -2966,7 +2773,7 @@ void b43_calc_nrssi_threshold(struct b43_wldev *dev)
 			else
 				b += 32;
 			b = b >> 6;
-			b = limit_value(b, -31, 31);
+			b = clamp_val(b, -31, 31);
 
 			tmp_u16 = b43_phy_read(dev, 0x048A) & 0xF000;
 			tmp_u16 |= ((u32) b & 0x0000003F);
@@ -3069,13 +2876,13 @@ b43_radio_interference_mitigation_enable(struct b43_wldev *dev, int mode)
 		}
 		radio_stacksave(0x0078);
 		tmp = (b43_radio_read16(dev, 0x0078) & 0x001E);
-		flipped = flip_4bit(tmp);
+		B43_WARN_ON(tmp > 15);
+		flipped = bitrev4(tmp);
 		if (flipped < 10 && flipped >= 8)
 			flipped = 7;
 		else if (flipped >= 10)
 			flipped -= 3;
-		flipped = flip_4bit(flipped);
-		flipped = (flipped << 1) | 0x0020;
+		flipped = (bitrev4(flipped) << 1) | 0x0020;
 		b43_radio_write16(dev, 0x0078, flipped);
 
 		b43_calc_nrssi_threshold(dev);
@@ -3708,7 +3515,7 @@ u16 b43_radio_init2050(struct b43_wldev *dev)
 	tmp1 >>= 9;
 
 	for (i = 0; i < 16; i++) {
-		radio78 = ((flip_4bit(i) << 1) | 0x20);
+		radio78 = (bitrev4(i) << 1) | 0x0020;
 		b43_radio_write16(dev, 0x78, radio78);
 		udelay(10);
 		for (j = 0; j < 16; j++) {
diff --git a/drivers/net/wireless/b43/phy.h b/drivers/net/wireless/b43/phy.h
index 6d165d822175..4aab10903529 100644
--- a/drivers/net/wireless/b43/phy.h
+++ b/drivers/net/wireless/b43/phy.h
@@ -225,7 +225,6 @@ 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);
@@ -252,6 +251,14 @@ struct b43_rfatt_list {
 	u8 max_val;
 };
 
+/* Returns true, if the values are the same. */
+static inline bool b43_compare_rfatt(const struct b43_rfatt *a,
+				     const struct b43_rfatt *b)
+{
+	return ((a->att == b->att) &&
+		(a->with_padmix == b->with_padmix));
+}
+
 /* Baseband Attenuation */
 struct b43_bbatt {
 	u8 att;			/* Attenuation value */
@@ -265,6 +272,13 @@ struct b43_bbatt_list {
 	u8 max_val;
 };
 
+/* Returns true, if the values are the same. */
+static inline bool b43_compare_bbatt(const struct b43_bbatt *a,
+				     const struct b43_bbatt *b)
+{
+	return (a->att == b->att);
+}
+
 /* tx_control bits. */
 #define B43_TXCTL_PA3DB		0x40	/* PA Gain 3dB */
 #define B43_TXCTL_PA2DB		0x20	/* PA Gain 2dB */
diff --git a/drivers/net/wireless/b43/pio.c b/drivers/net/wireless/b43/pio.c
index fcacafb04346..401591267592 100644
--- a/drivers/net/wireless/b43/pio.c
+++ b/drivers/net/wireless/b43/pio.c
@@ -446,29 +446,27 @@ static void pio_tx_frame_4byte_queue(struct b43_pio_txpacket *pack,
 }
 
 static int pio_tx_frame(struct b43_pio_txqueue *q,
-			struct sk_buff *skb,
-			struct ieee80211_tx_control *ctl)
+			struct sk_buff *skb)
 {
 	struct b43_pio_txpacket *pack;
 	struct b43_txhdr txhdr;
 	u16 cookie;
 	int err;
 	unsigned int hdrlen;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 
 	B43_WARN_ON(list_empty(&q->packets_list));
 	pack = list_entry(q->packets_list.next,
 			  struct b43_pio_txpacket, list);
-	memset(&pack->txstat, 0, sizeof(pack->txstat));
-	memcpy(&pack->txstat.control, ctl, sizeof(*ctl));
 
 	cookie = generate_cookie(q, pack);
 	hdrlen = b43_txhdr_size(q->dev);
 	err = b43_generate_txhdr(q->dev, (u8 *)&txhdr, skb->data,
-				 skb->len, ctl, cookie);
+				 skb->len, info, cookie);
 	if (err)
 		return err;
 
-	if (ctl->flags & IEEE80211_TXCTL_SEND_AFTER_DTIM) {
+	if (info->flags & IEEE80211_TX_CTL_SEND_AFTER_DTIM) {
 		/* Tell the firmware about the cookie of the last
 		 * mcast frame, so it can clear the more-data bit in it. */
 		b43_shm_write16(q->dev, B43_SHM_SHARED,
@@ -492,17 +490,18 @@ static int pio_tx_frame(struct b43_pio_txqueue *q,
 	return 0;
 }
 
-int b43_pio_tx(struct b43_wldev *dev,
-	       struct sk_buff *skb, struct ieee80211_tx_control *ctl)
+int b43_pio_tx(struct b43_wldev *dev, struct sk_buff *skb)
 {
 	struct b43_pio_txqueue *q;
 	struct ieee80211_hdr *hdr;
 	unsigned long flags;
 	unsigned int hdrlen, total_len;
 	int err = 0;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 
 	hdr = (struct ieee80211_hdr *)skb->data;
-	if (ctl->flags & IEEE80211_TXCTL_SEND_AFTER_DTIM) {
+
+	if (info->flags & IEEE80211_TX_CTL_SEND_AFTER_DTIM) {
 		/* The multicast queue will be sent after the DTIM. */
 		q = dev->pio.tx_queue_mcast;
 		/* Set the frame More-Data bit. Ucode will clear it
@@ -510,7 +509,7 @@ int b43_pio_tx(struct b43_wldev *dev,
 		hdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_MOREDATA);
 	} else {
 		/* Decide by priority where to put this frame. */
-		q = select_queue_by_priority(dev, ctl->queue);
+		q = select_queue_by_priority(dev, skb_get_queue_mapping(skb));
 	}
 
 	spin_lock_irqsave(&q->lock, flags);
@@ -533,7 +532,7 @@ int b43_pio_tx(struct b43_wldev *dev,
 	if (total_len > (q->buffer_size - q->buffer_used)) {
 		/* Not enough memory on the queue. */
 		err = -EBUSY;
-		ieee80211_stop_queue(dev->wl->hw, ctl->queue);
+		ieee80211_stop_queue(dev->wl->hw, skb_get_queue_mapping(skb));
 		q->stopped = 1;
 		goto out_unlock;
 	}
@@ -541,9 +540,9 @@ int b43_pio_tx(struct b43_wldev *dev,
 	/* Assign the queue number to the ring (if not already done before)
 	 * so TX status handling can use it. The mac80211-queue to b43-queue
 	 * mapping is static, so we don't need to store it per frame. */
-	q->queue_prio = ctl->queue;
+	q->queue_prio = skb_get_queue_mapping(skb);
 
-	err = pio_tx_frame(q, skb, ctl);
+	err = pio_tx_frame(q, skb);
 	if (unlikely(err == -ENOKEY)) {
 		/* Drop this packet, as we don't have the encryption key
 		 * anymore and must not transmit it unencrypted. */
@@ -561,7 +560,7 @@ int b43_pio_tx(struct b43_wldev *dev,
 	if (((q->buffer_size - q->buffer_used) < roundup(2 + 2 + 6, 4)) ||
 	    (q->free_packet_slots == 0)) {
 		/* The queue is full. */
-		ieee80211_stop_queue(dev->wl->hw, ctl->queue);
+		ieee80211_stop_queue(dev->wl->hw, skb_get_queue_mapping(skb));
 		q->stopped = 1;
 	}
 
@@ -578,6 +577,7 @@ void b43_pio_handle_txstatus(struct b43_wldev *dev,
 	struct b43_pio_txqueue *q;
 	struct b43_pio_txpacket *pack = NULL;
 	unsigned int total_len;
+	struct ieee80211_tx_info *info;
 
 	q = parse_cookie(dev, status->cookie, &pack);
 	if (unlikely(!q))
@@ -586,15 +586,17 @@ void b43_pio_handle_txstatus(struct b43_wldev *dev,
 
 	spin_lock(&q->lock); /* IRQs are already disabled. */
 
-	b43_fill_txstatus_report(&(pack->txstat), status);
+	info = IEEE80211_SKB_CB(pack->skb);
+	memset(&info->status, 0, sizeof(info->status));
+
+	b43_fill_txstatus_report(info, status);
 
 	total_len = pack->skb->len + b43_txhdr_size(dev);
 	total_len = roundup(total_len, 4);
 	q->buffer_used -= total_len;
 	q->free_packet_slots += 1;
 
-	ieee80211_tx_status_irqsafe(dev->wl->hw, pack->skb,
-				    &(pack->txstat));
+	ieee80211_tx_status_irqsafe(dev->wl->hw, pack->skb);
 	pack->skb = NULL;
 	list_add(&pack->list, &q->packets_list);
 
@@ -611,18 +613,16 @@ void b43_pio_get_tx_stats(struct b43_wldev *dev,
 {
 	const int nr_queues = dev->wl->hw->queues;
 	struct b43_pio_txqueue *q;
-	struct ieee80211_tx_queue_stats_data *data;
 	unsigned long flags;
 	int i;
 
 	for (i = 0; i < nr_queues; i++) {
-		data = &(stats->data[i]);
 		q = select_queue_by_priority(dev, i);
 
 		spin_lock_irqsave(&q->lock, flags);
-		data->len = B43_PIO_MAX_NR_TXPACKETS - q->free_packet_slots;
-		data->limit = B43_PIO_MAX_NR_TXPACKETS;
-		data->count = q->nr_tx_packets;
+		stats[i].len = B43_PIO_MAX_NR_TXPACKETS - q->free_packet_slots;
+		stats[i].limit = B43_PIO_MAX_NR_TXPACKETS;
+		stats[i].count = q->nr_tx_packets;
 		spin_unlock_irqrestore(&q->lock, flags);
 	}
 }
diff --git a/drivers/net/wireless/b43/pio.h b/drivers/net/wireless/b43/pio.h
index e2ec676cc9e4..6c174c91ca20 100644
--- a/drivers/net/wireless/b43/pio.h
+++ b/drivers/net/wireless/b43/pio.h
@@ -62,8 +62,6 @@ struct b43_pio_txpacket {
 	struct b43_pio_txqueue *queue;
 	/* The TX data packet. */
 	struct sk_buff *skb;
-	/* The status meta data. */
-	struct ieee80211_tx_status txstat;
 	/* Index in the (struct b43_pio_txqueue)->packets array. */
 	u8 index;
 
@@ -167,8 +165,7 @@ int b43_pio_init(struct b43_wldev *dev);
 void b43_pio_stop(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);
+int b43_pio_tx(struct b43_wldev *dev, struct sk_buff *skb);
 void b43_pio_handle_txstatus(struct b43_wldev *dev,
 			     const struct b43_txstatus *status);
 void b43_pio_get_tx_stats(struct b43_wldev *dev,
@@ -193,8 +190,7 @@ static inline void b43_pio_stop(struct b43_wldev *dev)
 {
 }
 static inline int b43_pio_tx(struct b43_wldev *dev,
-			     struct sk_buff *skb,
-			     struct ieee80211_tx_control *ctl)
+			     struct sk_buff *skb)
 {
 	return 0;
 }
diff --git a/drivers/net/wireless/b43/rfkill.c b/drivers/net/wireless/b43/rfkill.c
index 11f53cb1139e..fec5645944a4 100644
--- a/drivers/net/wireless/b43/rfkill.c
+++ b/drivers/net/wireless/b43/rfkill.c
@@ -43,6 +43,23 @@ static bool b43_is_hw_radio_enabled(struct b43_wldev *dev)
 	return 0;
 }
 
+/* Update the rfkill state */
+static void b43_rfkill_update_state(struct b43_wldev *dev)
+{
+	struct b43_rfkill *rfk = &(dev->wl->rfkill);
+
+	if (!dev->radio_hw_enable) {
+		rfk->rfkill->state = RFKILL_STATE_HARD_BLOCKED;
+		return;
+	}
+
+	if (!dev->phy.radio_on)
+		rfk->rfkill->state = RFKILL_STATE_SOFT_BLOCKED;
+	else
+		rfk->rfkill->state = RFKILL_STATE_UNBLOCKED;
+
+}
+
 /* The poll callback for the hardware button. */
 static void b43_rfkill_poll(struct input_polled_dev *poll_dev)
 {
@@ -60,6 +77,7 @@ static void b43_rfkill_poll(struct input_polled_dev *poll_dev)
 	if (unlikely(enabled != dev->radio_hw_enable)) {
 		dev->radio_hw_enable = enabled;
 		report_change = 1;
+		b43_rfkill_update_state(dev);
 		b43info(wl, "Radio hardware status changed to %s\n",
 			enabled ? "ENABLED" : "DISABLED");
 	}
@@ -88,7 +106,7 @@ static int b43_rfkill_soft_toggle(void *data, enum rfkill_state state)
 		goto out_unlock;
 	err = 0;
 	switch (state) {
-	case RFKILL_STATE_ON:
+	case RFKILL_STATE_UNBLOCKED:
 		if (!dev->radio_hw_enable) {
 			/* No luck. We can't toggle the hardware RF-kill
 			 * button from software. */
@@ -98,10 +116,13 @@ static int b43_rfkill_soft_toggle(void *data, enum rfkill_state state)
 		if (!dev->phy.radio_on)
 			b43_radio_turn_on(dev);
 		break;
-	case RFKILL_STATE_OFF:
+	case RFKILL_STATE_SOFT_BLOCKED:
 		if (dev->phy.radio_on)
 			b43_radio_turn_off(dev, 0);
 		break;
+	default:
+		b43warn(wl, "Received unexpected rfkill state %d.\n", state);
+		break;
 	}
 out_unlock:
 	mutex_unlock(&wl->mutex);
@@ -132,7 +153,7 @@ void b43_rfkill_init(struct b43_wldev *dev)
 	snprintf(rfk->name, sizeof(rfk->name),
 		 "b43-%s", wiphy_name(wl->hw->wiphy));
 	rfk->rfkill->name = rfk->name;
-	rfk->rfkill->state = RFKILL_STATE_ON;
+	rfk->rfkill->state = RFKILL_STATE_UNBLOCKED;
 	rfk->rfkill->data = dev;
 	rfk->rfkill->toggle_radio = b43_rfkill_soft_toggle;
 	rfk->rfkill->user_claim_unsupported = 1;
diff --git a/drivers/net/wireless/b43/xmit.c b/drivers/net/wireless/b43/xmit.c
index 19aefbfb2c93..8d54502222a6 100644
--- a/drivers/net/wireless/b43/xmit.c
+++ b/drivers/net/wireless/b43/xmit.c
@@ -185,15 +185,15 @@ int b43_generate_txhdr(struct b43_wldev *dev,
 		       u8 *_txhdr,
 		       const unsigned char *fragment_data,
 		       unsigned int fragment_len,
-		       const struct ieee80211_tx_control *txctl,
+		       const struct ieee80211_tx_info *info,
 		       u16 cookie)
 {
 	struct b43_txhdr *txhdr = (struct b43_txhdr *)_txhdr;
 	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);
+	int use_encryption = (!(info->flags & IEEE80211_TX_CTL_DO_NOT_ENCRYPT));
+	__le16 fctl = wlhdr->frame_control;
 	struct ieee80211_rate *fbrate;
 	u8 rate, rate_fb;
 	int rate_ofdm, rate_fb_ofdm;
@@ -201,13 +201,14 @@ int b43_generate_txhdr(struct b43_wldev *dev,
 	u32 mac_ctl = 0;
 	u16 phy_ctl = 0;
 	u8 extra_ft = 0;
+	struct ieee80211_rate *txrate;
 
 	memset(txhdr, 0, sizeof(*txhdr));
 
-	WARN_ON(!txctl->tx_rate);
-	rate = txctl->tx_rate ? txctl->tx_rate->hw_value : B43_CCK_RATE_1MB;
+	txrate = ieee80211_get_tx_rate(dev->wl->hw, info);
+	rate = txrate ? txrate->hw_value : B43_CCK_RATE_1MB;
 	rate_ofdm = b43_is_ofdm_rate(rate);
-	fbrate = txctl->alt_retry_rate ? : txctl->tx_rate;
+	fbrate = ieee80211_get_alt_retry_rate(dev->wl->hw, info) ? : txrate;
 	rate_fb = fbrate->hw_value;
 	rate_fb_ofdm = b43_is_ofdm_rate(rate_fb);
 
@@ -227,15 +228,13 @@ int b43_generate_txhdr(struct b43_wldev *dev,
 		 * use the original dur_id field. */
 		txhdr->dur_fb = wlhdr->duration_id;
 	} else {
-		txhdr->dur_fb = ieee80211_generic_frame_duration(dev->wl->hw,
-								 txctl->vif,
-								 fragment_len,
-								 fbrate);
+		txhdr->dur_fb = ieee80211_generic_frame_duration(
+			dev->wl->hw, info->control.vif, fragment_len, fbrate);
 	}
 
 	plcp_fragment_len = fragment_len + FCS_LEN;
 	if (use_encryption) {
-		u8 key_idx = (u16) (txctl->key_idx);
+		u8 key_idx = info->control.hw_key->hw_key_idx;
 		struct b43_key *key;
 		int wlhdr_len;
 		size_t iv_len;
@@ -253,15 +252,15 @@ int b43_generate_txhdr(struct b43_wldev *dev,
 		}
 
 		/* Hardware appends ICV. */
-		plcp_fragment_len += txctl->icv_len;
+		plcp_fragment_len += info->control.icv_len;
 
 		key_idx = b43_kidx_to_fw(dev, key_idx);
 		mac_ctl |= (key_idx << B43_TXH_MAC_KEYIDX_SHIFT) &
 			   B43_TXH_MAC_KEYIDX;
 		mac_ctl |= (key->algorithm << B43_TXH_MAC_KEYALG_SHIFT) &
 			   B43_TXH_MAC_KEYALG;
-		wlhdr_len = ieee80211_get_hdrlen(fctl);
-		iv_len = min((size_t) txctl->iv_len,
+		wlhdr_len = ieee80211_hdrlen(fctl);
+		iv_len = min((size_t) info->control.iv_len,
 			     ARRAY_SIZE(txhdr->iv));
 		memcpy(txhdr->iv, ((u8 *) wlhdr) + wlhdr_len, iv_len);
 	}
@@ -292,10 +291,10 @@ int b43_generate_txhdr(struct b43_wldev *dev,
 		phy_ctl |= B43_TXH_PHY_ENC_OFDM;
 	else
 		phy_ctl |= B43_TXH_PHY_ENC_CCK;
-	if (txctl->flags & IEEE80211_TXCTL_SHORT_PREAMBLE)
+	if (info->flags & IEEE80211_TX_CTL_SHORT_PREAMBLE)
 		phy_ctl |= B43_TXH_PHY_SHORTPRMBL;
 
-	switch (b43_ieee80211_antenna_sanitize(dev, txctl->antenna_sel_tx)) {
+	switch (b43_ieee80211_antenna_sanitize(dev, info->antenna_sel_tx)) {
 	case 0: /* Default */
 		phy_ctl |= B43_TXH_PHY_ANT01AUTO;
 		break;
@@ -316,34 +315,36 @@ int b43_generate_txhdr(struct b43_wldev *dev,
 	}
 
 	/* MAC control */
-	if (!(txctl->flags & IEEE80211_TXCTL_NO_ACK))
+	if (!(info->flags & IEEE80211_TX_CTL_NO_ACK))
 		mac_ctl |= B43_TXH_MAC_ACK;
-	if (!(((fctl & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_CTL) &&
-	      ((fctl & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_PSPOLL)))
+	/* use hardware sequence counter as the non-TID counter */
+	if (info->flags & IEEE80211_TX_CTL_ASSIGN_SEQ)
 		mac_ctl |= B43_TXH_MAC_HWSEQ;
-	if (txctl->flags & IEEE80211_TXCTL_FIRST_FRAGMENT)
+	if (info->flags & IEEE80211_TX_CTL_FIRST_FRAGMENT)
 		mac_ctl |= B43_TXH_MAC_STMSDU;
 	if (phy->type == B43_PHYTYPE_A)
 		mac_ctl |= B43_TXH_MAC_5GHZ;
-	if (txctl->flags & IEEE80211_TXCTL_LONG_RETRY_LIMIT)
+	if (info->flags & IEEE80211_TX_CTL_LONG_RETRY_LIMIT)
 		mac_ctl |= B43_TXH_MAC_LONGFRAME;
 
 	/* Generate the RTS or CTS-to-self frame */
-	if ((txctl->flags & IEEE80211_TXCTL_USE_RTS_CTS) ||
-	    (txctl->flags & IEEE80211_TXCTL_USE_CTS_PROTECT)) {
+	if ((info->flags & IEEE80211_TX_CTL_USE_RTS_CTS) ||
+	    (info->flags & IEEE80211_TX_CTL_USE_CTS_PROTECT)) {
 		unsigned int len;
 		struct ieee80211_hdr *hdr;
 		int rts_rate, rts_rate_fb;
 		int rts_rate_ofdm, rts_rate_fb_ofdm;
 		struct b43_plcp_hdr6 *plcp;
+		struct ieee80211_rate *rts_cts_rate;
 
-		WARN_ON(!txctl->rts_cts_rate);
-		rts_rate = txctl->rts_cts_rate ? txctl->rts_cts_rate->hw_value : B43_CCK_RATE_1MB;
+		rts_cts_rate = ieee80211_get_rts_cts_rate(dev->wl->hw, info);
+
+		rts_rate = rts_cts_rate ? rts_cts_rate->hw_value : B43_CCK_RATE_1MB;
 		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) {
+		if (info->flags & IEEE80211_TX_CTL_USE_CTS_PROTECT) {
 			struct ieee80211_cts *cts;
 
 			if (b43_is_old_txhdr_format(dev)) {
@@ -353,9 +354,9 @@ int b43_generate_txhdr(struct b43_wldev *dev,
 				cts = (struct ieee80211_cts *)
 					(txhdr->new_format.rts_frame);
 			}
-			ieee80211_ctstoself_get(dev->wl->hw, txctl->vif,
+			ieee80211_ctstoself_get(dev->wl->hw, info->control.vif,
 						fragment_data, fragment_len,
-						txctl, cts);
+						info, cts);
 			mac_ctl |= B43_TXH_MAC_SENDCTS;
 			len = sizeof(struct ieee80211_cts);
 		} else {
@@ -368,9 +369,9 @@ int b43_generate_txhdr(struct b43_wldev *dev,
 				rts = (struct ieee80211_rts *)
 					(txhdr->new_format.rts_frame);
 			}
-			ieee80211_rts_get(dev->wl->hw, txctl->vif,
+			ieee80211_rts_get(dev->wl->hw, info->control.vif,
 					  fragment_data, fragment_len,
-					  txctl, rts);
+					  info, rts);
 			mac_ctl |= B43_TXH_MAC_SENDRTS;
 			len = sizeof(struct ieee80211_rts);
 		}
@@ -508,7 +509,7 @@ void b43_rx(struct b43_wldev *dev, struct sk_buff *skb, const void *_rxhdr)
 	struct b43_plcp_hdr6 *plcp;
 	struct ieee80211_hdr *wlhdr;
 	const struct b43_rxhdr_fw4 *rxhdr = _rxhdr;
-	u16 fctl;
+	__le16 fctl;
 	u16 phystat0, phystat3, chanstat, mactime;
 	u32 macstat;
 	u16 chanid;
@@ -548,7 +549,7 @@ void b43_rx(struct b43_wldev *dev, struct sk_buff *skb, const void *_rxhdr)
 		goto drop;
 	}
 	wlhdr = (struct ieee80211_hdr *)(skb->data);
-	fctl = le16_to_cpu(wlhdr->frame_control);
+	fctl = wlhdr->frame_control;
 
 	if (macstat & B43_RX_MAC_DEC) {
 		unsigned int keyidx;
@@ -563,7 +564,7 @@ void b43_rx(struct b43_wldev *dev, struct sk_buff *skb, const void *_rxhdr)
 		B43_WARN_ON(keyidx >= dev->max_nr_keys);
 
 		if (dev->key[keyidx].algorithm != B43_SEC_ALGO_NONE) {
-			wlhdr_len = ieee80211_get_hdrlen(fctl);
+			wlhdr_len = ieee80211_hdrlen(fctl);
 			if (unlikely(skb->len < (wlhdr_len + 3))) {
 				b43dbg(dev->wl,
 				       "RX: Packet size underrun (3)\n");
@@ -581,12 +582,11 @@ void b43_rx(struct b43_wldev *dev, struct sk_buff *skb, const void *_rxhdr)
 		//      and also find out what the maximum possible value is.
 		//      Fill status.ssi and status.signal fields.
 	} else {
-		status.ssi = b43_rssi_postprocess(dev, rxhdr->jssi,
+		status.signal = b43_rssi_postprocess(dev, rxhdr->jssi,
 						  (phystat0 & B43_RX_PHYST0_OFDM),
 						  (phystat0 & B43_RX_PHYST0_GAINCTL),
 						  (phystat3 & B43_RX_PHYST3_TRSTATE));
-		/* the next line looks wrong, but is what mac80211 wants */
-		status.signal = (rxhdr->jssi * 100) / B43_RX_MAX_SSI;
+		status.qual = (rxhdr->jssi * 100) / B43_RX_MAX_SSI;
 	}
 
 	if (phystat0 & B43_RX_PHYST0_OFDM)
@@ -604,9 +604,7 @@ void b43_rx(struct b43_wldev *dev, struct sk_buff *skb, const void *_rxhdr)
 	 * of timestamp, i.e. about 65 milliseconds after the PHY received
 	 * the first symbol.
 	 */
-	if (((fctl & (IEEE80211_FCTL_FTYPE | IEEE80211_FCTL_STYPE))
-	    == (IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_BEACON)) ||
-	    dev->wl->radiotap_enabled) {
+	if (ieee80211_is_beacon(fctl) || dev->wl->radiotap_enabled) {
 		u16 low_mactime_now;
 
 		b43_tsf_read(dev, &status.mactime);
@@ -685,27 +683,27 @@ void b43_handle_txstatus(struct b43_wldev *dev,
 /* Fill out the mac80211 TXstatus report based on the b43-specific
  * txstatus report data. This returns a boolean whether the frame was
  * successfully transmitted. */
-bool b43_fill_txstatus_report(struct ieee80211_tx_status *report,
+bool b43_fill_txstatus_report(struct ieee80211_tx_info *report,
 			      const struct b43_txstatus *status)
 {
 	bool frame_success = 1;
 
 	if (status->acked) {
 		/* The frame was ACKed. */
-		report->flags |= IEEE80211_TX_STATUS_ACK;
+		report->flags |= IEEE80211_TX_STAT_ACK;
 	} else {
 		/* The frame was not ACKed... */
-		if (!(report->control.flags & IEEE80211_TXCTL_NO_ACK)) {
+		if (!(report->flags & IEEE80211_TX_CTL_NO_ACK)) {
 			/* ...but we expected an ACK. */
 			frame_success = 0;
-			report->excessive_retries = 1;
+			report->status.excessive_retries = 1;
 		}
 	}
 	if (status->frame_count == 0) {
 		/* The frame was not transmitted at all. */
-		report->retry_count = 0;
+		report->status.retry_count = 0;
 	} else
-		report->retry_count = status->frame_count - 1;
+		report->status.retry_count = status->frame_count - 1;
 
 	return frame_success;
 }
diff --git a/drivers/net/wireless/b43/xmit.h b/drivers/net/wireless/b43/xmit.h
index b05f44e0d626..0215faf47541 100644
--- a/drivers/net/wireless/b43/xmit.h
+++ b/drivers/net/wireless/b43/xmit.h
@@ -178,7 +178,7 @@ int 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);
+		       const struct ieee80211_tx_info *txctl, u16 cookie);
 
 /* Transmit Status */
 struct b43_txstatus {
@@ -294,7 +294,7 @@ 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);
-bool b43_fill_txstatus_report(struct ieee80211_tx_status *report,
+bool b43_fill_txstatus_report(struct ieee80211_tx_info *report,
 			      const struct b43_txstatus *status);
 
 void b43_tx_suspend(struct b43_wldev *dev);
diff --git a/drivers/net/wireless/b43legacy/b43legacy.h b/drivers/net/wireless/b43legacy/b43legacy.h
index ded3cd31b3df..c40078e1fff9 100644
--- a/drivers/net/wireless/b43legacy/b43legacy.h
+++ b/drivers/net/wireless/b43legacy/b43legacy.h
@@ -823,23 +823,6 @@ void b43legacydbg(struct b43legacy_wl *wl, const char *fmt, ...)
 # 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)
diff --git a/drivers/net/wireless/b43legacy/dma.c b/drivers/net/wireless/b43legacy/dma.c
index 93ddc1cbcc8b..fb6819e40f38 100644
--- a/drivers/net/wireless/b43legacy/dma.c
+++ b/drivers/net/wireless/b43legacy/dma.c
@@ -393,13 +393,13 @@ dma_addr_t map_descbuffer(struct b43legacy_dmaring *ring,
 	dma_addr_t dmaaddr;
 
 	if (tx)
-		dmaaddr = dma_map_single(ring->dev->dev->dma_dev,
-					 buf, len,
-					 DMA_TO_DEVICE);
+		dmaaddr = ssb_dma_map_single(ring->dev->dev,
+					     buf, len,
+					     DMA_TO_DEVICE);
 	else
-		dmaaddr = dma_map_single(ring->dev->dev->dma_dev,
-					 buf, len,
-					 DMA_FROM_DEVICE);
+		dmaaddr = ssb_dma_map_single(ring->dev->dev,
+					     buf, len,
+					     DMA_FROM_DEVICE);
 
 	return dmaaddr;
 }
@@ -411,13 +411,13 @@ void unmap_descbuffer(struct b43legacy_dmaring *ring,
 		      int tx)
 {
 	if (tx)
-		dma_unmap_single(ring->dev->dev->dma_dev,
-				 addr, len,
-				 DMA_TO_DEVICE);
+		ssb_dma_unmap_single(ring->dev->dev,
+				     addr, len,
+				     DMA_TO_DEVICE);
 	else
-		dma_unmap_single(ring->dev->dev->dma_dev,
-				 addr, len,
-				 DMA_FROM_DEVICE);
+		ssb_dma_unmap_single(ring->dev->dev,
+				     addr, len,
+				     DMA_FROM_DEVICE);
 }
 
 static inline
@@ -427,8 +427,8 @@ void sync_descbuffer_for_cpu(struct b43legacy_dmaring *ring,
 {
 	B43legacy_WARN_ON(ring->tx);
 
-	dma_sync_single_for_cpu(ring->dev->dev->dma_dev,
-				addr, len, DMA_FROM_DEVICE);
+	ssb_dma_sync_single_for_cpu(ring->dev->dev,
+				    addr, len, DMA_FROM_DEVICE);
 }
 
 static inline
@@ -438,8 +438,8 @@ void sync_descbuffer_for_device(struct b43legacy_dmaring *ring,
 {
 	B43legacy_WARN_ON(ring->tx);
 
-	dma_sync_single_for_device(ring->dev->dev->dma_dev,
-				   addr, len, DMA_FROM_DEVICE);
+	ssb_dma_sync_single_for_device(ring->dev->dev,
+				       addr, len, DMA_FROM_DEVICE);
 }
 
 static inline
@@ -458,10 +458,11 @@ void free_descriptor_buffer(struct b43legacy_dmaring *ring,
 
 static int alloc_ringmemory(struct b43legacy_dmaring *ring)
 {
-	struct device *dma_dev = ring->dev->dev->dma_dev;
-
-	ring->descbase = dma_alloc_coherent(dma_dev, B43legacy_DMA_RINGMEMSIZE,
-					    &(ring->dmabase), GFP_KERNEL);
+	/* GFP flags must match the flags in free_ringmemory()! */
+	ring->descbase = ssb_dma_alloc_consistent(ring->dev->dev,
+						  B43legacy_DMA_RINGMEMSIZE,
+						  &(ring->dmabase),
+						  GFP_KERNEL);
 	if (!ring->descbase) {
 		b43legacyerr(ring->dev->wl, "DMA ringmemory allocation"
 			     " failed\n");
@@ -474,10 +475,8 @@ static int alloc_ringmemory(struct b43legacy_dmaring *ring)
 
 static void free_ringmemory(struct b43legacy_dmaring *ring)
 {
-	struct device *dma_dev = ring->dev->dev->dma_dev;
-
-	dma_free_coherent(dma_dev, B43legacy_DMA_RINGMEMSIZE,
-			  ring->descbase, ring->dmabase);
+	ssb_dma_free_consistent(ring->dev->dev, B43legacy_DMA_RINGMEMSIZE,
+				ring->descbase, ring->dmabase, GFP_KERNEL);
 }
 
 /* Reset the RX DMA channel */
@@ -589,7 +588,7 @@ static bool b43legacy_dma_mapping_error(struct b43legacy_dmaring *ring,
 					 size_t buffersize,
 					 bool dma_to_device)
 {
-	if (unlikely(dma_mapping_error(addr)))
+	if (unlikely(ssb_dma_mapping_error(ring->dev->dev, addr)))
 		return 1;
 
 	switch (ring->type) {
@@ -860,6 +859,18 @@ static u64 supported_dma_mask(struct b43legacy_wldev *dev)
 	return DMA_30BIT_MASK;
 }
 
+static enum b43legacy_dmatype dma_mask_to_engine_type(u64 dmamask)
+{
+	if (dmamask == DMA_30BIT_MASK)
+		return B43legacy_DMA_30BIT;
+	if (dmamask == DMA_32BIT_MASK)
+		return B43legacy_DMA_32BIT;
+	if (dmamask == DMA_64BIT_MASK)
+		return B43legacy_DMA_64BIT;
+	B43legacy_WARN_ON(1);
+	return B43legacy_DMA_30BIT;
+}
+
 /* Main initialization function. */
 static
 struct b43legacy_dmaring *b43legacy_setup_dmaring(struct b43legacy_wldev *dev,
@@ -894,9 +905,9 @@ struct b43legacy_dmaring *b43legacy_setup_dmaring(struct b43legacy_wldev *dev,
 			goto err_kfree_meta;
 
 		/* test for ability to dma to txhdr_cache */
-		dma_test = dma_map_single(dev->dev->dma_dev, ring->txhdr_cache,
-					  sizeof(struct b43legacy_txhdr_fw3),
-					  DMA_TO_DEVICE);
+		dma_test = ssb_dma_map_single(dev->dev, ring->txhdr_cache,
+					      sizeof(struct b43legacy_txhdr_fw3),
+					      DMA_TO_DEVICE);
 
 		if (b43legacy_dma_mapping_error(ring, dma_test,
 					sizeof(struct b43legacy_txhdr_fw3), 1)) {
@@ -908,7 +919,7 @@ struct b43legacy_dmaring *b43legacy_setup_dmaring(struct b43legacy_wldev *dev,
 			if (!ring->txhdr_cache)
 				goto err_kfree_meta;
 
-			dma_test = dma_map_single(dev->dev->dma_dev,
+				dma_test = ssb_dma_map_single(dev->dev,
 					ring->txhdr_cache,
 					sizeof(struct b43legacy_txhdr_fw3),
 					DMA_TO_DEVICE);
@@ -918,9 +929,9 @@ struct b43legacy_dmaring *b43legacy_setup_dmaring(struct b43legacy_wldev *dev,
 				goto err_kfree_txhdr_cache;
 		}
 
-		dma_unmap_single(dev->dev->dma_dev,
-				 dma_test, sizeof(struct b43legacy_txhdr_fw3),
-				 DMA_TO_DEVICE);
+		ssb_dma_unmap_single(dev->dev, dma_test,
+				     sizeof(struct b43legacy_txhdr_fw3),
+				     DMA_TO_DEVICE);
 	}
 
 	ring->nr_slots = nr_slots;
@@ -1019,6 +1030,43 @@ void b43legacy_dma_free(struct b43legacy_wldev *dev)
 	dma->tx_ring0 = NULL;
 }
 
+static int b43legacy_dma_set_mask(struct b43legacy_wldev *dev, u64 mask)
+{
+	u64 orig_mask = mask;
+	bool fallback = 0;
+	int err;
+
+	/* Try to set the DMA mask. If it fails, try falling back to a
+	 * lower mask, as we can always also support a lower one. */
+	while (1) {
+		err = ssb_dma_set_mask(dev->dev, mask);
+		if (!err)
+			break;
+		if (mask == DMA_64BIT_MASK) {
+			mask = DMA_32BIT_MASK;
+			fallback = 1;
+			continue;
+		}
+		if (mask == DMA_32BIT_MASK) {
+			mask = DMA_30BIT_MASK;
+			fallback = 1;
+			continue;
+		}
+		b43legacyerr(dev->wl, "The machine/kernel does not support "
+		       "the required %u-bit DMA mask\n",
+		       (unsigned int)dma_mask_to_engine_type(orig_mask));
+		return -EOPNOTSUPP;
+	}
+	if (fallback) {
+		b43legacyinfo(dev->wl, "DMA mask fallback from %u-bit to %u-"
+			"bit\n",
+			(unsigned int)dma_mask_to_engine_type(orig_mask),
+			(unsigned int)dma_mask_to_engine_type(mask));
+	}
+
+	return 0;
+}
+
 int b43legacy_dma_init(struct b43legacy_wldev *dev)
 {
 	struct b43legacy_dma *dma = &dev->dma;
@@ -1028,21 +1076,8 @@ int b43legacy_dma_init(struct b43legacy_wldev *dev)
 	enum b43legacy_dmatype type;
 
 	dmamask = supported_dma_mask(dev);
-	switch (dmamask) {
-	default:
-		B43legacy_WARN_ON(1);
-	case DMA_30BIT_MASK:
-		type = B43legacy_DMA_30BIT;
-		break;
-	case DMA_32BIT_MASK:
-		type = B43legacy_DMA_32BIT;
-		break;
-	case DMA_64BIT_MASK:
-		type = B43legacy_DMA_64BIT;
-		break;
-	}
-
-	err = ssb_dma_set_mask(dev->dev, dmamask);
+	type = dma_mask_to_engine_type(dmamask);
+	err = b43legacy_dma_set_mask(dev, dmamask);
 	if (err) {
 #ifdef CONFIG_B43LEGACY_PIO
 		b43legacywarn(dev->wl, "DMA for this device not supported. "
@@ -1205,10 +1240,10 @@ struct b43legacy_dmaring *parse_cookie(struct b43legacy_wldev *dev,
 }
 
 static int dma_tx_fragment(struct b43legacy_dmaring *ring,
-			    struct sk_buff *skb,
-			    struct ieee80211_tx_control *ctl)
+			    struct sk_buff *skb)
 {
 	const struct b43legacy_dma_ops *ops = ring->ops;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 	u8 *header;
 	int slot, old_top_slot, old_used_slots;
 	int err;
@@ -1231,7 +1266,7 @@ static int dma_tx_fragment(struct b43legacy_dmaring *ring,
 	header = &(ring->txhdr_cache[slot * sizeof(
 			       struct b43legacy_txhdr_fw3)]);
 	err = b43legacy_generate_txhdr(ring->dev, header,
-				 skb->data, skb->len, ctl,
+				 skb->data, skb->len, info,
 				 generate_cookie(ring, slot));
 	if (unlikely(err)) {
 		ring->current_slot = old_top_slot;
@@ -1255,7 +1290,6 @@ static int dma_tx_fragment(struct b43legacy_dmaring *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;
 
@@ -1323,14 +1357,13 @@ int should_inject_overflow(struct b43legacy_dmaring *ring)
 }
 
 int b43legacy_dma_tx(struct b43legacy_wldev *dev,
-		     struct sk_buff *skb,
-		     struct ieee80211_tx_control *ctl)
+		     struct sk_buff *skb)
 {
 	struct b43legacy_dmaring *ring;
 	int err = 0;
 	unsigned long flags;
 
-	ring = priority_to_txring(dev, ctl->queue);
+	ring = priority_to_txring(dev, skb_get_queue_mapping(skb));
 	spin_lock_irqsave(&ring->lock, flags);
 	B43legacy_WARN_ON(!ring->tx);
 	if (unlikely(free_slots(ring) < SLOTS_PER_PACKET)) {
@@ -1343,7 +1376,7 @@ int b43legacy_dma_tx(struct b43legacy_wldev *dev,
 	 * That would be a mac80211 bug. */
 	B43legacy_BUG_ON(ring->stopped);
 
-	err = dma_tx_fragment(ring, skb, ctl);
+	err = dma_tx_fragment(ring, skb);
 	if (unlikely(err == -ENOKEY)) {
 		/* Drop this packet, as we don't have the encryption key
 		 * anymore and must not transmit it unencrypted. */
@@ -1401,26 +1434,29 @@ void b43legacy_dma_handle_txstatus(struct b43legacy_wldev *dev,
 					 1);
 
 		if (meta->is_last_fragment) {
-			B43legacy_WARN_ON(!meta->skb);
+			struct ieee80211_tx_info *info;
+			BUG_ON(!meta->skb);
+			info = IEEE80211_SKB_CB(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().
 			 */
+
+			memset(&info->status, 0, sizeof(info->status));
+
 			if (status->acked) {
-				meta->txstat.flags |= IEEE80211_TX_STATUS_ACK;
+				info->flags |= IEEE80211_TX_STAT_ACK;
 			} else {
-				if (!(meta->txstat.control.flags
-				      & IEEE80211_TXCTL_NO_ACK))
-					 meta->txstat.excessive_retries = 1;
+				if (!(info->flags & IEEE80211_TX_CTL_NO_ACK))
+					 info->status.excessive_retries = 1;
 			}
 			if (status->frame_count == 0) {
 				/* The frame was not transmitted at all. */
-				meta->txstat.retry_count = 0;
+				info->status.retry_count = 0;
 			} else
-				meta->txstat.retry_count = status->frame_count
+				info->status.retry_count = status->frame_count
 							   - 1;
-			ieee80211_tx_status_irqsafe(dev->wl->hw, meta->skb,
-						    &(meta->txstat));
+			ieee80211_tx_status_irqsafe(dev->wl->hw, meta->skb);
 			/* skb is freed by ieee80211_tx_status_irqsafe() */
 			meta->skb = NULL;
 		} else {
@@ -1455,18 +1491,16 @@ void b43legacy_dma_get_tx_stats(struct b43legacy_wldev *dev,
 {
 	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;
+		stats[i].len = ring->used_slots / SLOTS_PER_PACKET;
+		stats[i].limit = ring->nr_slots / SLOTS_PER_PACKET;
+		stats[i].count = ring->nr_tx_packets;
 		spin_unlock_irqrestore(&ring->lock, flags);
 	}
 }
diff --git a/drivers/net/wireless/b43legacy/dma.h b/drivers/net/wireless/b43legacy/dma.h
index 2dd488c5be2d..2f186003c31e 100644
--- a/drivers/net/wireless/b43legacy/dma.h
+++ b/drivers/net/wireless/b43legacy/dma.h
@@ -195,7 +195,6 @@ struct b43legacy_dmadesc_meta {
 	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;
@@ -297,8 +296,7 @@ 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);
+		     struct sk_buff *skb);
 void b43legacy_dma_handle_txstatus(struct b43legacy_wldev *dev,
 				   const struct b43legacy_txstatus *status);
 
@@ -323,8 +321,7 @@ void b43legacy_dma_get_tx_stats(struct b43legacy_wldev *dev,
 }
 static inline
 int b43legacy_dma_tx(struct b43legacy_wldev *dev,
-		     struct sk_buff *skb,
-		     struct ieee80211_tx_control *ctl)
+		     struct sk_buff *skb)
 {
 	return 0;
 }
diff --git a/drivers/net/wireless/b43legacy/main.c b/drivers/net/wireless/b43legacy/main.c
index 3e612d0a13e8..a1b8bf3ee732 100644
--- a/drivers/net/wireless/b43legacy/main.c
+++ b/drivers/net/wireless/b43legacy/main.c
@@ -846,10 +846,10 @@ static void handle_irq_noise(struct b43legacy_wldev *dev)
 	/* 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);
+	noise[0] = clamp_val(noise[0], 0, ARRAY_SIZE(phy->nrssi_lt) - 1);
+	noise[1] = clamp_val(noise[1], 0, ARRAY_SIZE(phy->nrssi_lt) - 1);
+	noise[2] = clamp_val(noise[2], 0, ARRAY_SIZE(phy->nrssi_lt) - 1);
+	noise[3] = clamp_val(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]];
@@ -1138,14 +1138,22 @@ static void b43legacy_write_probe_resp_template(struct b43legacy_wldev *dev,
 
 /* Asynchronously update the packet templates in template RAM.
  * Locking: Requires wl->irq_lock to be locked. */
-static void b43legacy_update_templates(struct b43legacy_wl *wl,
-				       struct sk_buff *beacon)
+static void b43legacy_update_templates(struct b43legacy_wl *wl)
 {
+	struct sk_buff *beacon;
 	/* This is the top half of the ansynchronous beacon update. The bottom
 	 * half is the beacon IRQ. Beacon update must be asynchronous to avoid
 	 * sending an invalid beacon. This can happen for example, if the
 	 * firmware transmits a beacon while we are updating it. */
 
+	/* We could modify the existing beacon and set the aid bit in the TIM
+	 * field, but that would probably require resizing and moving of data
+	 * within the beacon template. Simply request a new beacon and let
+	 * mac80211 do the hard work. */
+	beacon = ieee80211_beacon_get(wl->hw, wl->vif);
+	if (unlikely(!beacon))
+		return;
+
 	if (wl->current_beacon)
 		dev_kfree_skb_any(wl->current_beacon);
 	wl->current_beacon = beacon;
@@ -2358,8 +2366,7 @@ static int b43legacy_rng_init(struct b43legacy_wl *wl)
 }
 
 static int b43legacy_op_tx(struct ieee80211_hw *hw,
-			   struct sk_buff *skb,
-			   struct ieee80211_tx_control *ctl)
+			   struct sk_buff *skb)
 {
 	struct b43legacy_wl *wl = hw_to_b43legacy_wl(hw);
 	struct b43legacy_wldev *dev = wl->current_dev;
@@ -2373,10 +2380,10 @@ static int b43legacy_op_tx(struct ieee80211_hw *hw,
 	/* 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);
+		err = b43legacy_pio_tx(dev, skb);
 		spin_unlock_irqrestore(&wl->irq_lock, flags);
 	} else
-		err = b43legacy_dma_tx(dev, skb, ctl);
+		err = b43legacy_dma_tx(dev, skb);
 out:
 	if (unlikely(err)) {
 		/* Drop the packet. */
@@ -2385,8 +2392,7 @@ out:
 	return NETDEV_TX_OK;
 }
 
-static int b43legacy_op_conf_tx(struct ieee80211_hw *hw,
-				int queue,
+static int b43legacy_op_conf_tx(struct ieee80211_hw *hw, u16 queue,
 				const struct ieee80211_tx_queue_params *params)
 {
 	return 0;
@@ -2729,10 +2735,13 @@ static int b43legacy_op_config_interface(struct ieee80211_hw *hw,
 		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_WARN_ON(vif->type != IEEE80211_IF_TYPE_AP);
 			b43legacy_set_ssid(dev, conf->ssid, conf->ssid_len);
-			if (conf->beacon)
-				b43legacy_update_templates(wl, conf->beacon);
+			if (conf->changed & IEEE80211_IFCC_BEACON)
+				b43legacy_update_templates(wl);
+		} else if (b43legacy_is_mode(wl, IEEE80211_IF_TYPE_IBSS)) {
+			if (conf->changed & IEEE80211_IFCC_BEACON)
+				b43legacy_update_templates(wl);
 		}
 		b43legacy_write_mac_bssid_templates(dev);
 	}
@@ -2797,7 +2806,6 @@ static int b43legacy_wireless_core_start(struct b43legacy_wldev *dev)
 	/* 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);
@@ -3399,32 +3407,10 @@ static int b43legacy_op_beacon_set_tim(struct ieee80211_hw *hw,
 				       int aid, int set)
 {
 	struct b43legacy_wl *wl = hw_to_b43legacy_wl(hw);
-	struct sk_buff *beacon;
-	unsigned long flags;
-
-	/* We could modify the existing beacon and set the aid bit in the TIM
-	 * field, but that would probably require resizing and moving of data
-	 * within the beacon template. Simply request a new beacon and let
-	 * mac80211 do the hard work. */
-	beacon = ieee80211_beacon_get(hw, wl->vif, NULL);
-	if (unlikely(!beacon))
-		return -ENOMEM;
-	spin_lock_irqsave(&wl->irq_lock, flags);
-	b43legacy_update_templates(wl, beacon);
-	spin_unlock_irqrestore(&wl->irq_lock, flags);
-
-	return 0;
-}
-
-static int b43legacy_op_ibss_beacon_update(struct ieee80211_hw *hw,
-					   struct sk_buff *beacon,
-					   struct ieee80211_tx_control *ctl)
-{
-	struct b43legacy_wl *wl = hw_to_b43legacy_wl(hw);
 	unsigned long flags;
 
 	spin_lock_irqsave(&wl->irq_lock, flags);
-	b43legacy_update_templates(wl, beacon);
+	b43legacy_update_templates(wl);
 	spin_unlock_irqrestore(&wl->irq_lock, flags);
 
 	return 0;
@@ -3444,7 +3430,6 @@ static const struct ieee80211_ops b43legacy_hw_ops = {
 	.stop			= b43legacy_op_stop,
 	.set_retry_limit	= b43legacy_op_set_retry_limit,
 	.set_tim		= b43legacy_op_beacon_set_tim,
-	.beacon_update		= b43legacy_op_ibss_beacon_update,
 };
 
 /* Hard-reset the chip. Do not call this directly.
@@ -3718,10 +3703,9 @@ static int b43legacy_wireless_init(struct ssb_device *dev)
 
 	/* 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;
+		    IEEE80211_HW_RX_INCLUDES_FCS |
+		    IEEE80211_HW_SIGNAL_DBM |
+		    IEEE80211_HW_NOISE_DBM;
 	hw->queues = 1; /* FIXME: hardware has more queues */
 	SET_IEEE80211_DEV(hw, dev->dev);
 	if (is_valid_ether_addr(sprom->et1mac))
diff --git a/drivers/net/wireless/b43legacy/phy.c b/drivers/net/wireless/b43legacy/phy.c
index 8e5c09b81871..768cccb9b1ba 100644
--- a/drivers/net/wireless/b43legacy/phy.c
+++ b/drivers/net/wireless/b43legacy/phy.c
@@ -1088,7 +1088,7 @@ static void b43legacy_phy_initg(struct b43legacy_wldev *dev)
 		 * 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())
+		 * entries to -32 (see the clamp_val() in nrssi_hw_update())
 		 */
 		b43legacy_nrssi_hw_update(dev, 0xFFFF);
 		b43legacy_calc_nrssi_threshold(dev);
@@ -1756,7 +1756,7 @@ static s8 b43legacy_phy_estimate_power_out(struct b43legacy_wldev *dev, s8 tssi)
 	switch (phy->type) {
 	case B43legacy_PHYTYPE_B:
 	case B43legacy_PHYTYPE_G:
-		tmp = limit_value(tmp, 0x00, 0x3F);
+		tmp = clamp_val(tmp, 0x00, 0x3F);
 		dbm = phy->tssi2dbm[tmp];
 		break;
 	default:
@@ -1859,7 +1859,7 @@ void b43legacy_phy_xmitpower(struct b43legacy_wldev *dev)
 
 	/* 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);
+	desired_pwr = clamp_val(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
@@ -1905,7 +1905,7 @@ void b43legacy_phy_xmitpower(struct b43legacy_wldev *dev)
 			radio_attenuation++;
 		}
 	}
-	baseband_attenuation = limit_value(baseband_attenuation, 0, 11);
+	baseband_attenuation = clamp_val(baseband_attenuation, 0, 11);
 
 	txpower = phy->txctl1;
 	if ((phy->radio_ver == 0x2050) && (phy->radio_rev == 2)) {
@@ -1933,8 +1933,8 @@ void b43legacy_phy_xmitpower(struct b43legacy_wldev *dev)
 	}
 	/* Save the control values */
 	phy->txctl1 = txpower;
-	baseband_attenuation = limit_value(baseband_attenuation, 0, 11);
-	radio_attenuation = limit_value(radio_attenuation, 0, 9);
+	baseband_attenuation = clamp_val(baseband_attenuation, 0, 11);
+	radio_attenuation = clamp_val(radio_attenuation, 0, 9);
 	phy->rfatt = radio_attenuation;
 	phy->bbatt = baseband_attenuation;
 
@@ -1979,7 +1979,7 @@ s8 b43legacy_tssi2dbm_entry(s8 entry [], u8 index, s16 pab0, s16 pab1, s16 pab2)
 		f = q;
 		i++;
 	} while (delta >= 2);
-	entry[index] = limit_value(b43legacy_tssi2dbm_ad(m1 * f, 8192),
+	entry[index] = clamp_val(b43legacy_tssi2dbm_ad(m1 * f, 8192),
 				   -127, 128);
 	return 0;
 }
diff --git a/drivers/net/wireless/b43legacy/pio.c b/drivers/net/wireless/b43legacy/pio.c
index bcdd54eb2edb..a86c7647fa2d 100644
--- a/drivers/net/wireless/b43legacy/pio.c
+++ b/drivers/net/wireless/b43legacy/pio.c
@@ -196,7 +196,7 @@ static int pio_tx_write_fragment(struct b43legacy_pioqueue *queue,
 	B43legacy_WARN_ON(skb_shinfo(skb)->nr_frags != 0);
 	err = b43legacy_generate_txhdr(queue->dev,
 				 txhdr, skb->data, skb->len,
-				 &packet->txstat.control,
+				 IEEE80211_SKB_CB(skb),
 				 generate_cookie(queue, packet));
 	if (err)
 		return err;
@@ -463,8 +463,7 @@ err_destroy0:
 }
 
 int b43legacy_pio_tx(struct b43legacy_wldev *dev,
-		     struct sk_buff *skb,
-		     struct ieee80211_tx_control *ctl)
+		     struct sk_buff *skb)
 {
 	struct b43legacy_pioqueue *queue = dev->pio.queue1;
 	struct b43legacy_pio_txpacket *packet;
@@ -476,9 +475,6 @@ int b43legacy_pio_tx(struct b43legacy_wldev *dev,
 			    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++;
@@ -494,6 +490,7 @@ void b43legacy_pio_handle_txstatus(struct b43legacy_wldev *dev,
 {
 	struct b43legacy_pioqueue *queue;
 	struct b43legacy_pio_txpacket *packet;
+	struct ieee80211_tx_info *info;
 
 	queue = parse_cookie(dev, status->cookie, &packet);
 	B43legacy_WARN_ON(!queue);
@@ -505,11 +502,13 @@ void b43legacy_pio_handle_txstatus(struct b43legacy_wldev *dev,
 	queue->tx_devq_used -= (packet->skb->len +
 				sizeof(struct b43legacy_txhdr_fw3));
 
+	info = IEEE80211_SKB_CB(packet->skb);
+	memset(&info->status, 0, sizeof(info->status));
+
 	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));
+		info->flags |= IEEE80211_TX_STAT_ACK;
+	info->status.retry_count = status->frame_count - 1;
+	ieee80211_tx_status_irqsafe(dev->wl->hw, packet->skb);
 	packet->skb = NULL;
 
 	free_txpacket(packet, 1);
@@ -525,13 +524,11 @@ void b43legacy_pio_get_tx_stats(struct b43legacy_wldev *dev,
 {
 	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;
+	stats[0].len = B43legacy_PIO_MAXTXPACKETS - queue->nr_txfree;
+	stats[0].limit = B43legacy_PIO_MAXTXPACKETS;
+	stats[0].count = queue->nr_tx_packets;
 }
 
 static void pio_rx_error(struct b43legacy_pioqueue *queue,
diff --git a/drivers/net/wireless/b43legacy/pio.h b/drivers/net/wireless/b43legacy/pio.h
index 5bfed0c40030..464fec05a06d 100644
--- a/drivers/net/wireless/b43legacy/pio.h
+++ b/drivers/net/wireless/b43legacy/pio.h
@@ -41,7 +41,6 @@ struct b43legacy_xmitstatus;
 struct b43legacy_pio_txpacket {
 	struct b43legacy_pioqueue *queue;
 	struct sk_buff *skb;
-	struct ieee80211_tx_status txstat;
 	struct list_head list;
 };
 
@@ -104,8 +103,7 @@ 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);
+		   struct sk_buff *skb);
 void b43legacy_pio_handle_txstatus(struct b43legacy_wldev *dev,
 				 const struct b43legacy_txstatus *status);
 void b43legacy_pio_get_tx_stats(struct b43legacy_wldev *dev,
@@ -132,8 +130,7 @@ 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)
+		   struct sk_buff *skb)
 {
 	return 0;
 }
diff --git a/drivers/net/wireless/b43legacy/radio.c b/drivers/net/wireless/b43legacy/radio.c
index 955832e8654f..2df545cfad14 100644
--- a/drivers/net/wireless/b43legacy/radio.c
+++ b/drivers/net/wireless/b43legacy/radio.c
@@ -357,7 +357,7 @@ void b43legacy_nrssi_hw_update(struct b43legacy_wldev *dev, u16 val)
 	for (i = 0; i < 64; i++) {
 		tmp = b43legacy_nrssi_hw_read(dev, i);
 		tmp -= val;
-		tmp = limit_value(tmp, -32, 31);
+		tmp = clamp_val(tmp, -32, 31);
 		b43legacy_nrssi_hw_write(dev, i, tmp);
 	}
 }
@@ -375,7 +375,7 @@ void b43legacy_nrssi_mem_update(struct b43legacy_wldev *dev)
 		tmp = (i - delta) * phy->nrssislope;
 		tmp /= 0x10000;
 		tmp += 0x3A;
-		tmp = limit_value(tmp, 0, 0x3F);
+		tmp = clamp_val(tmp, 0, 0x3F);
 		phy->nrssi_lt[i] = tmp;
 	}
 }
@@ -839,7 +839,7 @@ void b43legacy_calc_nrssi_threshold(struct b43legacy_wldev *dev)
 		} else
 			threshold = phy->nrssi[1] - 5;
 
-		threshold = limit_value(threshold, 0, 0x3E);
+		threshold = clamp_val(threshold, 0, 0x3E);
 		b43legacy_phy_read(dev, 0x0020); /* dummy read */
 		b43legacy_phy_write(dev, 0x0020, (((u16)threshold) << 8)
 				    | 0x001C);
@@ -892,7 +892,7 @@ void b43legacy_calc_nrssi_threshold(struct b43legacy_wldev *dev)
 			else
 				a += 32;
 			a = a >> 6;
-			a = limit_value(a, -31, 31);
+			a = clamp_val(a, -31, 31);
 
 			b = b * (phy->nrssi[1] - phy->nrssi[0]);
 			b += (phy->nrssi[0] << 6);
@@ -901,7 +901,7 @@ void b43legacy_calc_nrssi_threshold(struct b43legacy_wldev *dev)
 			else
 				b += 32;
 			b = b >> 6;
-			b = limit_value(b, -31, 31);
+			b = clamp_val(b, -31, 31);
 
 			tmp_u16 = b43legacy_phy_read(dev, 0x048A) & 0xF000;
 			tmp_u16 |= ((u32)b & 0x0000003F);
@@ -1905,7 +1905,7 @@ void b43legacy_radio_set_txpower_a(struct b43legacy_wldev *dev, u16 txpower)
 	u16 dac;
 	u16 ilt;
 
-	txpower = limit_value(txpower, 0, 63);
+	txpower = clamp_val(txpower, 0, 63);
 
 	pamp = b43legacy_get_txgain_freq_power_amp(txpower);
 	pamp <<= 5;
diff --git a/drivers/net/wireless/b43legacy/rfkill.c b/drivers/net/wireless/b43legacy/rfkill.c
index d178dfbb1c9f..476add97e974 100644
--- a/drivers/net/wireless/b43legacy/rfkill.c
+++ b/drivers/net/wireless/b43legacy/rfkill.c
@@ -44,6 +44,23 @@ static bool b43legacy_is_hw_radio_enabled(struct b43legacy_wldev *dev)
 	return 0;
 }
 
+/* Update the rfkill state */
+static void b43legacy_rfkill_update_state(struct b43legacy_wldev *dev)
+{
+	struct b43legacy_rfkill *rfk = &(dev->wl->rfkill);
+
+	if (!dev->radio_hw_enable) {
+		rfk->rfkill->state = RFKILL_STATE_HARD_BLOCKED;
+		return;
+	}
+
+	if (!dev->phy.radio_on)
+		rfk->rfkill->state = RFKILL_STATE_SOFT_BLOCKED;
+	else
+		rfk->rfkill->state = RFKILL_STATE_UNBLOCKED;
+
+}
+
 /* The poll callback for the hardware button. */
 static void b43legacy_rfkill_poll(struct input_polled_dev *poll_dev)
 {
@@ -61,6 +78,7 @@ static void b43legacy_rfkill_poll(struct input_polled_dev *poll_dev)
 	if (unlikely(enabled != dev->radio_hw_enable)) {
 		dev->radio_hw_enable = enabled;
 		report_change = 1;
+		b43legacy_rfkill_update_state(dev);
 		b43legacyinfo(wl, "Radio hardware status changed to %s\n",
 			enabled ? "ENABLED" : "DISABLED");
 	}
@@ -90,7 +108,7 @@ static int b43legacy_rfkill_soft_toggle(void *data, enum rfkill_state state)
 		goto out_unlock;
 	err = 0;
 	switch (state) {
-	case RFKILL_STATE_ON:
+	case RFKILL_STATE_UNBLOCKED:
 		if (!dev->radio_hw_enable) {
 			/* No luck. We can't toggle the hardware RF-kill
 			 * button from software. */
@@ -100,10 +118,14 @@ static int b43legacy_rfkill_soft_toggle(void *data, enum rfkill_state state)
 		if (!dev->phy.radio_on)
 			b43legacy_radio_turn_on(dev);
 		break;
-	case RFKILL_STATE_OFF:
+	case RFKILL_STATE_SOFT_BLOCKED:
 		if (dev->phy.radio_on)
 			b43legacy_radio_turn_off(dev, 0);
 		break;
+	default:
+		b43legacywarn(wl, "Received unexpected rfkill state %d.\n",
+			      state);
+		break;
 	}
 
 out_unlock:
@@ -135,7 +157,7 @@ void b43legacy_rfkill_init(struct b43legacy_wldev *dev)
 	snprintf(rfk->name, sizeof(rfk->name),
 		 "b43legacy-%s", wiphy_name(wl->hw->wiphy));
 	rfk->rfkill->name = rfk->name;
-	rfk->rfkill->state = RFKILL_STATE_ON;
+	rfk->rfkill->state = RFKILL_STATE_UNBLOCKED;
 	rfk->rfkill->data = dev;
 	rfk->rfkill->toggle_radio = b43legacy_rfkill_soft_toggle;
 	rfk->rfkill->user_claim_unsupported = 1;
diff --git a/drivers/net/wireless/b43legacy/xmit.c b/drivers/net/wireless/b43legacy/xmit.c
index dcad2491a606..e969ed8d412d 100644
--- a/drivers/net/wireless/b43legacy/xmit.c
+++ b/drivers/net/wireless/b43legacy/xmit.c
@@ -188,11 +188,11 @@ static int 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,
+			       const struct ieee80211_tx_info *info,
 			       u16 cookie)
 {
 	const struct ieee80211_hdr *wlhdr;
-	int use_encryption = (!(txctl->flags & IEEE80211_TXCTL_DO_NOT_ENCRYPT));
+	int use_encryption = (!(info->flags & IEEE80211_TX_CTL_DO_NOT_ENCRYPT));
 	u16 fctl;
 	u8 rate;
 	struct ieee80211_rate *rate_fb;
@@ -201,15 +201,18 @@ static int generate_txhdr_fw3(struct b43legacy_wldev *dev,
 	unsigned int plcp_fragment_len;
 	u32 mac_ctl = 0;
 	u16 phy_ctl = 0;
+	struct ieee80211_rate *tx_rate;
 
 	wlhdr = (const struct ieee80211_hdr *)fragment_data;
 	fctl = le16_to_cpu(wlhdr->frame_control);
 
 	memset(txhdr, 0, sizeof(*txhdr));
 
-	rate = txctl->tx_rate->hw_value;
+	tx_rate = ieee80211_get_tx_rate(dev->wl->hw, info);
+
+	rate = tx_rate->hw_value;
 	rate_ofdm = b43legacy_is_ofdm_rate(rate);
-	rate_fb = txctl->alt_retry_rate ? : txctl->tx_rate;
+	rate_fb = ieee80211_get_alt_retry_rate(dev->wl->hw, info) ? : tx_rate;
 	rate_fb_ofdm = b43legacy_is_ofdm_rate(rate_fb->hw_value);
 
 	txhdr->mac_frame_ctl = wlhdr->frame_control;
@@ -225,14 +228,14 @@ static int generate_txhdr_fw3(struct b43legacy_wldev *dev,
 		txhdr->dur_fb = wlhdr->duration_id;
 	} else {
 		txhdr->dur_fb = ieee80211_generic_frame_duration(dev->wl->hw,
-							 txctl->vif,
+							 info->control.vif,
 							 fragment_len,
 							 rate_fb);
 	}
 
 	plcp_fragment_len = fragment_len + FCS_LEN;
 	if (use_encryption) {
-		u8 key_idx = (u16)(txctl->key_idx);
+		u8 key_idx = info->control.hw_key->hw_key_idx;
 		struct b43legacy_key *key;
 		int wlhdr_len;
 		size_t iv_len;
@@ -242,7 +245,7 @@ static int generate_txhdr_fw3(struct b43legacy_wldev *dev,
 
 		if (key->enabled) {
 			/* Hardware appends ICV. */
-			plcp_fragment_len += txctl->icv_len;
+			plcp_fragment_len += info->control.icv_len;
 
 			key_idx = b43legacy_kidx_to_fw(dev, key_idx);
 			mac_ctl |= (key_idx << B43legacy_TX4_MAC_KEYIDX_SHIFT) &
@@ -251,7 +254,7 @@ static int generate_txhdr_fw3(struct b43legacy_wldev *dev,
 				   B43legacy_TX4_MAC_KEYALG_SHIFT) &
 				   B43legacy_TX4_MAC_KEYALG;
 			wlhdr_len = ieee80211_get_hdrlen(fctl);
-			iv_len = min((size_t)txctl->iv_len,
+			iv_len = min((size_t)info->control.iv_len,
 				     ARRAY_SIZE(txhdr->iv));
 			memcpy(txhdr->iv, ((u8 *)wlhdr) + wlhdr_len, iv_len);
 		} else {
@@ -275,7 +278,7 @@ static int generate_txhdr_fw3(struct b43legacy_wldev *dev,
 		phy_ctl |= B43legacy_TX4_PHY_OFDM;
 	if (dev->short_preamble)
 		phy_ctl |= B43legacy_TX4_PHY_SHORTPRMBL;
-	switch (txctl->antenna_sel_tx) {
+	switch (info->antenna_sel_tx) {
 	case 0:
 		phy_ctl |= B43legacy_TX4_PHY_ANTLAST;
 		break;
@@ -290,21 +293,20 @@ static int generate_txhdr_fw3(struct b43legacy_wldev *dev,
 	}
 
 	/* MAC control */
-	if (!(txctl->flags & IEEE80211_TXCTL_NO_ACK))
+	if (!(info->flags & IEEE80211_TX_CTL_NO_ACK))
 		mac_ctl |= B43legacy_TX4_MAC_ACK;
-	if (!(((fctl & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_CTL) &&
-	      ((fctl & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_PSPOLL)))
+	if (info->flags & IEEE80211_TX_CTL_ASSIGN_SEQ)
 		mac_ctl |= B43legacy_TX4_MAC_HWSEQ;
-	if (txctl->flags & IEEE80211_TXCTL_FIRST_FRAGMENT)
+	if (info->flags & IEEE80211_TX_CTL_FIRST_FRAGMENT)
 		mac_ctl |= B43legacy_TX4_MAC_STMSDU;
 	if (rate_fb_ofdm)
 		mac_ctl |= B43legacy_TX4_MAC_FALLBACKOFDM;
-	if (txctl->flags & IEEE80211_TXCTL_LONG_RETRY_LIMIT)
+	if (info->flags & IEEE80211_TX_CTL_LONG_RETRY_LIMIT)
 		mac_ctl |= B43legacy_TX4_MAC_LONGFRAME;
 
 	/* Generate the RTS or CTS-to-self frame */
-	if ((txctl->flags & IEEE80211_TXCTL_USE_RTS_CTS) ||
-	    (txctl->flags & IEEE80211_TXCTL_USE_CTS_PROTECT)) {
+	if ((info->flags & IEEE80211_TX_CTL_USE_RTS_CTS) ||
+	    (info->flags & IEEE80211_TX_CTL_USE_CTS_PROTECT)) {
 		unsigned int len;
 		struct ieee80211_hdr *hdr;
 		int rts_rate;
@@ -312,26 +314,26 @@ static int generate_txhdr_fw3(struct b43legacy_wldev *dev,
 		int rts_rate_ofdm;
 		int rts_rate_fb_ofdm;
 
-		rts_rate = txctl->rts_cts_rate->hw_value;
+		rts_rate = ieee80211_get_rts_cts_rate(dev->wl->hw, info)->hw_value;
 		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) {
+		if (info->flags & IEEE80211_TX_CTL_USE_CTS_PROTECT) {
 			ieee80211_ctstoself_get(dev->wl->hw,
-						txctl->vif,
+						info->control.vif,
 						fragment_data,
-						fragment_len, txctl,
+						fragment_len, info,
 						(struct ieee80211_cts *)
 						(txhdr->rts_frame));
 			mac_ctl |= B43legacy_TX4_MAC_SENDCTS;
 			len = sizeof(struct ieee80211_cts);
 		} else {
 			ieee80211_rts_get(dev->wl->hw,
-					  txctl->vif,
-					  fragment_data, fragment_len, txctl,
+					  info->control.vif,
+					  fragment_data, fragment_len, info,
 					  (struct ieee80211_rts *)
 					  (txhdr->rts_frame));
 			mac_ctl |= B43legacy_TX4_MAC_SENDRTS;
@@ -362,12 +364,12 @@ int b43legacy_generate_txhdr(struct b43legacy_wldev *dev,
 			      u8 *txhdr,
 			      const unsigned char *fragment_data,
 			      unsigned int fragment_len,
-			      const struct ieee80211_tx_control *txctl,
+			      const struct ieee80211_tx_info *info,
 			      u16 cookie)
 {
 	return generate_txhdr_fw3(dev, (struct b43legacy_txhdr_fw3 *)txhdr,
 			   fragment_data, fragment_len,
-			   txctl, cookie);
+			   info, cookie);
 }
 
 static s8 b43legacy_rssi_postprocess(struct b43legacy_wldev *dev,
@@ -439,7 +441,7 @@ void b43legacy_rx(struct b43legacy_wldev *dev,
 	struct b43legacy_plcp_hdr6 *plcp;
 	struct ieee80211_hdr *wlhdr;
 	const struct b43legacy_rxhdr_fw3 *rxhdr = _rxhdr;
-	u16 fctl;
+	__le16 fctl;
 	u16 phystat0;
 	u16 phystat3;
 	u16 chanstat;
@@ -477,7 +479,7 @@ void b43legacy_rx(struct b43legacy_wldev *dev,
 		goto drop;
 	}
 	wlhdr = (struct ieee80211_hdr *)(skb->data);
-	fctl = le16_to_cpu(wlhdr->frame_control);
+	fctl = wlhdr->frame_control;
 
 	if ((macstat & B43legacy_RX_MAC_DEC) &&
 	    !(macstat & B43legacy_RX_MAC_DECERR)) {
@@ -496,11 +498,11 @@ void b43legacy_rx(struct b43legacy_wldev *dev,
 
 		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);
+			B43legacy_WARN_ON(!ieee80211_has_protected(fctl));
+			fctl &= ~cpu_to_le16(IEEE80211_FCTL_PROTECTED);
+			wlhdr->frame_control = fctl;
 
-			wlhdr_len = ieee80211_get_hdrlen(fctl);
+			wlhdr_len = ieee80211_hdrlen(fctl);
 			if (unlikely(skb->len < (wlhdr_len + 3))) {
 				b43legacydbg(dev->wl, "RX: Packet size"
 					     " underrun3\n");
@@ -532,12 +534,12 @@ void b43legacy_rx(struct b43legacy_wldev *dev,
 		}
 	}
 
-	status.ssi = b43legacy_rssi_postprocess(dev, jssi,
+	status.signal = 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;
+	status.qual = (jssi * 100) / B43legacy_RX_MAX_SSI;
 	/* change to support A PHY */
 	if (phystat0 & B43legacy_RX_PHYST0_OFDM)
 		status.rate_idx = b43legacy_plcp_get_bitrate_idx_ofdm(plcp, false);
@@ -553,9 +555,7 @@ void b43legacy_rx(struct b43legacy_wldev *dev,
 	 * of timestamp, i.e. about 65 milliseconds after the PHY received
 	 * the first symbol.
 	 */
-	if (((fctl & (IEEE80211_FCTL_FTYPE | IEEE80211_FCTL_STYPE))
-	    == (IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_BEACON)) ||
-	    dev->wl->radiotap_enabled) {
+	if (ieee80211_is_beacon(fctl) || dev->wl->radiotap_enabled) {
 		u16 low_mactime_now;
 
 		b43legacy_tsf_read(dev, &status.mactime);
diff --git a/drivers/net/wireless/b43legacy/xmit.h b/drivers/net/wireless/b43legacy/xmit.h
index bab47928a0c9..e56777e0feab 100644
--- a/drivers/net/wireless/b43legacy/xmit.h
+++ b/drivers/net/wireless/b43legacy/xmit.h
@@ -80,7 +80,7 @@ int b43legacy_generate_txhdr(struct b43legacy_wldev *dev,
 			      u8 *txhdr,
 			      const unsigned char *fragment_data,
 			      unsigned int fragment_len,
-			      const struct ieee80211_tx_control *txctl,
+			      const struct ieee80211_tx_info *info,
 			      u16 cookie);
 
 
diff --git a/drivers/net/wireless/hostap/hostap.h b/drivers/net/wireless/hostap/hostap.h
index 547ba84dc797..3a386a636cca 100644
--- a/drivers/net/wireless/hostap/hostap.h
+++ b/drivers/net/wireless/hostap/hostap.h
@@ -67,7 +67,8 @@ void * ap_crypt_get_ptrs(struct ap_data *ap, u8 *addr, int permanent,
 int prism2_ap_get_sta_qual(local_info_t *local, struct sockaddr addr[],
 			   struct iw_quality qual[], int buf_size,
 			   int aplist);
-int prism2_ap_translate_scan(struct net_device *dev, char *buffer);
+int prism2_ap_translate_scan(struct net_device *dev,
+			     struct iw_request_info *info, char *buffer);
 int prism2_hostapd(struct ap_data *ap, struct prism2_hostapd_param *param);
 
 
diff --git a/drivers/net/wireless/hostap/hostap_80211_rx.c b/drivers/net/wireless/hostap/hostap_80211_rx.c
index 020f450e9dba..f106bc1585a4 100644
--- a/drivers/net/wireless/hostap/hostap_80211_rx.c
+++ b/drivers/net/wireless/hostap/hostap_80211_rx.c
@@ -78,6 +78,9 @@ int prism2_rx_80211(struct net_device *dev, struct sk_buff *skb,
 			prism_header = 2;
 			phdrlen = sizeof(struct linux_wlan_ng_cap_hdr);
 		}
+	} else if (dev->type == ARPHRD_IEEE80211_RADIOTAP) {
+		prism_header = 3;
+		phdrlen = sizeof(struct hostap_radiotap_rx);
 	} else {
 		prism_header = 0;
 		phdrlen = 0;
@@ -165,6 +168,24 @@ hdr->f.status = s; hdr->f.len = l; hdr->f.data = d
 		hdr->ssi_noise  = htonl(rx_stats->noise);
 		hdr->preamble   = htonl(0); /* unknown */
 		hdr->encoding   = htonl(1); /* cck */
+	} else if (prism_header == 3) {
+		struct hostap_radiotap_rx *hdr;
+		hdr = (struct hostap_radiotap_rx *)skb_push(skb, phdrlen);
+		memset(hdr, 0, phdrlen);
+		hdr->hdr.it_len = cpu_to_le16(phdrlen);
+		hdr->hdr.it_present =
+			cpu_to_le32((1 << IEEE80211_RADIOTAP_TSFT) |
+				    (1 << IEEE80211_RADIOTAP_CHANNEL) |
+				    (1 << IEEE80211_RADIOTAP_RATE) |
+				    (1 << IEEE80211_RADIOTAP_DBM_ANTSIGNAL) |
+				    (1 << IEEE80211_RADIOTAP_DBM_ANTNOISE));
+		hdr->tsft = cpu_to_le64(rx_stats->mac_time);
+		hdr->chan_freq = cpu_to_le16(freq_list[local->channel - 1]);
+		hdr->chan_flags = cpu_to_le16(IEEE80211_CHAN_CCK |
+						 IEEE80211_CHAN_2GHZ);
+		hdr->rate = rx_stats->rate / 5;
+		hdr->dbm_antsignal = rx_stats->signal;
+		hdr->dbm_antnoise = rx_stats->noise;
 	}
 
 	ret = skb->len - phdrlen;
diff --git a/drivers/net/wireless/hostap/hostap_ap.c b/drivers/net/wireless/hostap/hostap_ap.c
index ab981afd481d..af3d4ef2a80b 100644
--- a/drivers/net/wireless/hostap/hostap_ap.c
+++ b/drivers/net/wireless/hostap/hostap_ap.c
@@ -2420,7 +2420,8 @@ int prism2_ap_get_sta_qual(local_info_t *local, struct sockaddr addr[],
 
 /* Translate our list of Access Points & Stations to a card independant
  * format that the Wireless Tools will understand - Jean II */
-int prism2_ap_translate_scan(struct net_device *dev, char *buffer)
+int prism2_ap_translate_scan(struct net_device *dev,
+			     struct iw_request_info *info, char *buffer)
 {
 	struct hostap_interface *iface;
 	local_info_t *local;
@@ -2449,8 +2450,8 @@ int prism2_ap_translate_scan(struct net_device *dev, char *buffer)
 		iwe.u.ap_addr.sa_family = ARPHRD_ETHER;
 		memcpy(iwe.u.ap_addr.sa_data, sta->addr, ETH_ALEN);
 		iwe.len = IW_EV_ADDR_LEN;
-		current_ev = iwe_stream_add_event(current_ev, end_buf, &iwe,
-						  IW_EV_ADDR_LEN);
+		current_ev = iwe_stream_add_event(info, current_ev, end_buf,
+						  &iwe, IW_EV_ADDR_LEN);
 
 		/* Use the mode to indicate if it's a station or
 		 * an Access Point */
@@ -2461,8 +2462,8 @@ int prism2_ap_translate_scan(struct net_device *dev, char *buffer)
 		else
 			iwe.u.mode = IW_MODE_INFRA;
 		iwe.len = IW_EV_UINT_LEN;
-		current_ev = iwe_stream_add_event(current_ev, end_buf, &iwe,
-						  IW_EV_UINT_LEN);
+		current_ev = iwe_stream_add_event(info, current_ev, end_buf,
+						  &iwe, IW_EV_UINT_LEN);
 
 		/* Some quality */
 		memset(&iwe, 0, sizeof(iwe));
@@ -2477,8 +2478,8 @@ int prism2_ap_translate_scan(struct net_device *dev, char *buffer)
 		iwe.u.qual.noise = HFA384X_LEVEL_TO_dBm(sta->last_rx_silence);
 		iwe.u.qual.updated = sta->last_rx_updated;
 		iwe.len = IW_EV_QUAL_LEN;
-		current_ev = iwe_stream_add_event(current_ev, end_buf, &iwe,
-						  IW_EV_QUAL_LEN);
+		current_ev = iwe_stream_add_event(info, current_ev, end_buf,
+						  &iwe, IW_EV_QUAL_LEN);
 
 #ifndef PRISM2_NO_KERNEL_IEEE80211_MGMT
 		if (sta->ap) {
@@ -2486,8 +2487,8 @@ int prism2_ap_translate_scan(struct net_device *dev, char *buffer)
 			iwe.cmd = SIOCGIWESSID;
 			iwe.u.data.length = sta->u.ap.ssid_len;
 			iwe.u.data.flags = 1;
-			current_ev = iwe_stream_add_point(current_ev, end_buf,
-							  &iwe,
+			current_ev = iwe_stream_add_point(info, current_ev,
+							  end_buf, &iwe,
 							  sta->u.ap.ssid);
 
 			memset(&iwe, 0, sizeof(iwe));
@@ -2497,10 +2498,9 @@ int prism2_ap_translate_scan(struct net_device *dev, char *buffer)
 					IW_ENCODE_ENABLED | IW_ENCODE_NOKEY;
 			else
 				iwe.u.data.flags = IW_ENCODE_DISABLED;
-			current_ev = iwe_stream_add_point(current_ev, end_buf,
-							  &iwe,
-							  sta->u.ap.ssid
-							  /* 0 byte memcpy */);
+			current_ev = iwe_stream_add_point(info, current_ev,
+							  end_buf, &iwe,
+							  sta->u.ap.ssid);
 
 			if (sta->u.ap.channel > 0 &&
 			    sta->u.ap.channel <= FREQ_COUNT) {
@@ -2510,7 +2510,7 @@ int prism2_ap_translate_scan(struct net_device *dev, char *buffer)
 					* 100000;
 				iwe.u.freq.e = 1;
 				current_ev = iwe_stream_add_event(
-					current_ev, end_buf, &iwe,
+					info, current_ev, end_buf, &iwe,
 					IW_EV_FREQ_LEN);
 			}
 
@@ -2519,8 +2519,8 @@ int prism2_ap_translate_scan(struct net_device *dev, char *buffer)
 			sprintf(buf, "beacon_interval=%d",
 				sta->listen_interval);
 			iwe.u.data.length = strlen(buf);
-			current_ev = iwe_stream_add_point(current_ev, end_buf,
-							  &iwe, buf);
+			current_ev = iwe_stream_add_point(info, current_ev,
+							  end_buf, &iwe, buf);
 		}
 #endif /* PRISM2_NO_KERNEL_IEEE80211_MGMT */
 
diff --git a/drivers/net/wireless/hostap/hostap_hw.c b/drivers/net/wireless/hostap/hostap_hw.c
index 936f52e3d95c..3153fe9d7ce0 100644
--- a/drivers/net/wireless/hostap/hostap_hw.c
+++ b/drivers/net/wireless/hostap/hostap_hw.c
@@ -3101,7 +3101,22 @@ static void prism2_clear_set_tim_queue(local_info_t *local)
  * This is a natural nesting, which needs a split lock type.
  */
 static struct lock_class_key hostap_netdev_xmit_lock_key;
+static struct lock_class_key hostap_netdev_addr_lock_key;
 
+static void prism2_set_lockdep_class_one(struct net_device *dev,
+					 struct netdev_queue *txq,
+					 void *_unused)
+{
+	lockdep_set_class(&txq->_xmit_lock,
+			  &hostap_netdev_xmit_lock_key);
+}
+
+static void prism2_set_lockdep_class(struct net_device *dev)
+{
+	lockdep_set_class(&dev->addr_list_lock,
+			  &hostap_netdev_addr_lock_key);
+	netdev_for_each_tx_queue(dev, prism2_set_lockdep_class_one, NULL);
+}
 
 static struct net_device *
 prism2_init_local_data(struct prism2_helper_functions *funcs, int card_idx,
@@ -3204,6 +3219,7 @@ prism2_init_local_data(struct prism2_helper_functions *funcs, int card_idx,
 	local->auth_algs = PRISM2_AUTH_OPEN | PRISM2_AUTH_SHARED_KEY;
 	local->sram_type = -1;
 	local->scan_channel_mask = 0xffff;
+	local->monitor_type = PRISM2_MONITOR_RADIOTAP;
 
 	/* Initialize task queue structures */
 	INIT_WORK(&local->reset_queue, handle_reset_queue);
@@ -3267,7 +3283,7 @@ while (0)
 	if (ret >= 0)
 		ret = register_netdevice(dev);
 
-	lockdep_set_class(&dev->_xmit_lock, &hostap_netdev_xmit_lock_key);
+	prism2_set_lockdep_class(dev);
 	rtnl_unlock();
 	if (ret < 0) {
 		printk(KERN_WARNING "%s: register netdevice failed!\n",
@@ -3416,7 +3432,7 @@ static void prism2_free_local_data(struct net_device *dev)
 }
 
 
-#ifndef PRISM2_PLX
+#if (defined(PRISM2_PCI) && defined(CONFIG_PM)) || defined(PRISM2_PCCARD)
 static void prism2_suspend(struct net_device *dev)
 {
 	struct hostap_interface *iface;
@@ -3435,7 +3451,7 @@ static void prism2_suspend(struct net_device *dev)
 	/* Disable hardware and firmware */
 	prism2_hw_shutdown(dev, 0);
 }
-#endif /* PRISM2_PLX */
+#endif /* (PRISM2_PCI && CONFIG_PM) || PRISM2_PCCARD */
 
 
 /* These might at some point be compiled separately and used as separate
diff --git a/drivers/net/wireless/hostap/hostap_ioctl.c b/drivers/net/wireless/hostap/hostap_ioctl.c
index 0ca0bfeb0ada..3f8b1d7036e5 100644
--- a/drivers/net/wireless/hostap/hostap_ioctl.c
+++ b/drivers/net/wireless/hostap/hostap_ioctl.c
@@ -897,6 +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;
+	} else if (local->monitor_type == PRISM2_MONITOR_RADIOTAP) {
+		dev->type = ARPHRD_IEEE80211_RADIOTAP;
 	} else {
 		dev->type = ARPHRD_IEEE80211;
 	}
@@ -1793,6 +1795,7 @@ static int prism2_ioctl_siwscan(struct net_device *dev,
 
 #ifndef PRISM2_NO_STATION_MODES
 static char * __prism2_translate_scan(local_info_t *local,
+				      struct iw_request_info *info,
 				      struct hfa384x_hostscan_result *scan,
 				      struct hostap_bss_info *bss,
 				      char *current_ev, char *end_buf)
@@ -1823,7 +1826,7 @@ static char * __prism2_translate_scan(local_info_t *local,
 	iwe.cmd = SIOCGIWAP;
 	iwe.u.ap_addr.sa_family = ARPHRD_ETHER;
 	memcpy(iwe.u.ap_addr.sa_data, bssid, ETH_ALEN);
-	current_ev = iwe_stream_add_event(current_ev, end_buf, &iwe,
+	current_ev = iwe_stream_add_event(info, current_ev, end_buf, &iwe,
 					  IW_EV_ADDR_LEN);
 
 	/* Other entries will be displayed in the order we give them */
@@ -1832,7 +1835,8 @@ static char * __prism2_translate_scan(local_info_t *local,
 	iwe.cmd = SIOCGIWESSID;
 	iwe.u.data.length = ssid_len;
 	iwe.u.data.flags = 1;
-	current_ev = iwe_stream_add_point(current_ev, end_buf, &iwe, ssid);
+	current_ev = iwe_stream_add_point(info, current_ev, end_buf,
+					  &iwe, ssid);
 
 	memset(&iwe, 0, sizeof(iwe));
 	iwe.cmd = SIOCGIWMODE;
@@ -1847,8 +1851,8 @@ static char * __prism2_translate_scan(local_info_t *local,
 			iwe.u.mode = IW_MODE_MASTER;
 		else
 			iwe.u.mode = IW_MODE_ADHOC;
-		current_ev = iwe_stream_add_event(current_ev, end_buf, &iwe,
-						  IW_EV_UINT_LEN);
+		current_ev = iwe_stream_add_event(info, current_ev, end_buf,
+						  &iwe, IW_EV_UINT_LEN);
 	}
 
 	memset(&iwe, 0, sizeof(iwe));
@@ -1864,8 +1868,8 @@ static char * __prism2_translate_scan(local_info_t *local,
 	if (chan > 0) {
 		iwe.u.freq.m = freq_list[chan - 1] * 100000;
 		iwe.u.freq.e = 1;
-		current_ev = iwe_stream_add_event(current_ev, end_buf, &iwe,
-						  IW_EV_FREQ_LEN);
+		current_ev = iwe_stream_add_event(info, current_ev, end_buf,
+						  &iwe, IW_EV_FREQ_LEN);
 	}
 
 	if (scan) {
@@ -1884,8 +1888,8 @@ static char * __prism2_translate_scan(local_info_t *local,
 			| IW_QUAL_NOISE_UPDATED
 			| IW_QUAL_QUAL_INVALID
 			| IW_QUAL_DBM;
-		current_ev = iwe_stream_add_event(current_ev, end_buf, &iwe,
-						  IW_EV_QUAL_LEN);
+		current_ev = iwe_stream_add_event(info, current_ev, end_buf,
+						  &iwe, IW_EV_QUAL_LEN);
 	}
 
 	memset(&iwe, 0, sizeof(iwe));
@@ -1895,13 +1899,13 @@ static char * __prism2_translate_scan(local_info_t *local,
 	else
 		iwe.u.data.flags = IW_ENCODE_DISABLED;
 	iwe.u.data.length = 0;
-	current_ev = iwe_stream_add_point(current_ev, end_buf, &iwe, "");
+	current_ev = iwe_stream_add_point(info, current_ev, end_buf, &iwe, "");
 
 	/* TODO: add SuppRates into BSS table */
 	if (scan) {
 		memset(&iwe, 0, sizeof(iwe));
 		iwe.cmd = SIOCGIWRATE;
-		current_val = current_ev + IW_EV_LCP_LEN;
+		current_val = current_ev + iwe_stream_lcp_len(info);
 		pos = scan->sup_rates;
 		for (i = 0; i < sizeof(scan->sup_rates); i++) {
 			if (pos[i] == 0)
@@ -1909,11 +1913,11 @@ static char * __prism2_translate_scan(local_info_t *local,
 			/* Bit rate given in 500 kb/s units (+ 0x80) */
 			iwe.u.bitrate.value = ((pos[i] & 0x7f) * 500000);
 			current_val = iwe_stream_add_value(
-				current_ev, current_val, end_buf, &iwe,
+				info, current_ev, current_val, end_buf, &iwe,
 				IW_EV_PARAM_LEN);
 		}
 		/* Check if we added any event */
-		if ((current_val - current_ev) > IW_EV_LCP_LEN)
+		if ((current_val - current_ev) > iwe_stream_lcp_len(info))
 			current_ev = current_val;
 	}
 
@@ -1924,15 +1928,15 @@ static char * __prism2_translate_scan(local_info_t *local,
 		iwe.cmd = IWEVCUSTOM;
 		sprintf(buf, "bcn_int=%d", le16_to_cpu(scan->beacon_interval));
 		iwe.u.data.length = strlen(buf);
-		current_ev = iwe_stream_add_point(current_ev, end_buf, &iwe,
-						  buf);
+		current_ev = iwe_stream_add_point(info, current_ev, end_buf,
+						  &iwe, buf);
 
 		memset(&iwe, 0, sizeof(iwe));
 		iwe.cmd = IWEVCUSTOM;
 		sprintf(buf, "resp_rate=%d", le16_to_cpu(scan->rate));
 		iwe.u.data.length = strlen(buf);
-		current_ev = iwe_stream_add_point(current_ev, end_buf, &iwe,
-						  buf);
+		current_ev = iwe_stream_add_point(info, current_ev, end_buf,
+						  &iwe, buf);
 
 		if (local->last_scan_type == PRISM2_HOSTSCAN &&
 		    (capabilities & WLAN_CAPABILITY_IBSS)) {
@@ -1940,8 +1944,8 @@ static char * __prism2_translate_scan(local_info_t *local,
 			iwe.cmd = IWEVCUSTOM;
 			sprintf(buf, "atim=%d", le16_to_cpu(scan->atim));
 			iwe.u.data.length = strlen(buf);
-			current_ev = iwe_stream_add_point(current_ev, end_buf,
-							  &iwe, buf);
+			current_ev = iwe_stream_add_point(info, current_ev,
+							  end_buf, &iwe, buf);
 		}
 	}
 	kfree(buf);
@@ -1950,16 +1954,16 @@ static char * __prism2_translate_scan(local_info_t *local,
 		memset(&iwe, 0, sizeof(iwe));
 		iwe.cmd = IWEVGENIE;
 		iwe.u.data.length = bss->wpa_ie_len;
-		current_ev = iwe_stream_add_point(
-			current_ev, end_buf, &iwe, bss->wpa_ie);
+		current_ev = iwe_stream_add_point(info, current_ev, end_buf,
+						  &iwe, bss->wpa_ie);
 	}
 
 	if (bss && bss->rsn_ie_len > 0 && bss->rsn_ie_len <= MAX_WPA_IE_LEN) {
 		memset(&iwe, 0, sizeof(iwe));
 		iwe.cmd = IWEVGENIE;
 		iwe.u.data.length = bss->rsn_ie_len;
-		current_ev = iwe_stream_add_point(
-			current_ev, end_buf, &iwe, bss->rsn_ie);
+		current_ev = iwe_stream_add_point(info, current_ev, end_buf,
+						  &iwe, bss->rsn_ie);
 	}
 
 	return current_ev;
@@ -1969,6 +1973,7 @@ static char * __prism2_translate_scan(local_info_t *local,
 /* Translate scan data returned from the card to a card independant
  * format that the Wireless Tools will understand - Jean II */
 static inline int prism2_translate_scan(local_info_t *local,
+					struct iw_request_info *info,
 					char *buffer, int buflen)
 {
 	struct hfa384x_hostscan_result *scan;
@@ -1999,13 +2004,14 @@ static inline int prism2_translate_scan(local_info_t *local,
 			if (memcmp(bss->bssid, scan->bssid, ETH_ALEN) == 0) {
 				bss->included = 1;
 				current_ev = __prism2_translate_scan(
-					local, scan, bss, current_ev, end_buf);
+					local, info, scan, bss, current_ev,
+					end_buf);
 				found++;
 			}
 		}
 		if (!found) {
 			current_ev = __prism2_translate_scan(
-				local, scan, NULL, current_ev, end_buf);
+				local, info, scan, NULL, current_ev, end_buf);
 		}
 		/* Check if there is space for one more entry */
 		if ((end_buf - current_ev) <= IW_EV_ADDR_LEN) {
@@ -2023,7 +2029,7 @@ static inline int prism2_translate_scan(local_info_t *local,
 		bss = list_entry(ptr, struct hostap_bss_info, list);
 		if (bss->included)
 			continue;
-		current_ev = __prism2_translate_scan(local, NULL, bss,
+		current_ev = __prism2_translate_scan(local, info, NULL, bss,
 						     current_ev, end_buf);
 		/* Check if there is space for one more entry */
 		if ((end_buf - current_ev) <= IW_EV_ADDR_LEN) {
@@ -2070,7 +2076,7 @@ static inline int prism2_ioctl_giwscan_sta(struct net_device *dev,
 	}
 	local->scan_timestamp = 0;
 
-	res = prism2_translate_scan(local, extra, data->length);
+	res = prism2_translate_scan(local, info, extra, data->length);
 
 	if (res >= 0) {
 		data->length = res;
@@ -2103,7 +2109,7 @@ static int prism2_ioctl_giwscan(struct net_device *dev,
 		 * Jean II */
 
 		/* Translate to WE format */
-		res = prism2_ap_translate_scan(dev, extra);
+		res = prism2_ap_translate_scan(dev, info, extra);
 		if (res >= 0) {
 			printk(KERN_DEBUG "Scan result translation succeeded "
 			       "(length=%d)\n", res);
@@ -2516,7 +2522,8 @@ static int prism2_ioctl_priv_prism2_param(struct net_device *dev,
 	case PRISM2_PARAM_MONITOR_TYPE:
 		if (value != PRISM2_MONITOR_80211 &&
 		    value != PRISM2_MONITOR_CAPHDR &&
-		    value != PRISM2_MONITOR_PRISM) {
+		    value != PRISM2_MONITOR_PRISM &&
+		    value != PRISM2_MONITOR_RADIOTAP) {
 			ret = -EINVAL;
 			break;
 		}
diff --git a/drivers/net/wireless/hostap/hostap_main.c b/drivers/net/wireless/hostap/hostap_main.c
index a38e85f334df..756ab56c1f40 100644
--- a/drivers/net/wireless/hostap/hostap_main.c
+++ b/drivers/net/wireless/hostap/hostap_main.c
@@ -597,25 +597,7 @@ void hostap_dump_tx_header(const char *name, const struct hfa384x_tx_frame *tx)
 static int hostap_80211_header_parse(const struct sk_buff *skb,
 				     unsigned char *haddr)
 {
-	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 */
-
+	memcpy(haddr, skb_mac_header(skb) + 10, ETH_ALEN); /* addr2 */
 	return ETH_ALEN;
 }
 
diff --git a/drivers/net/wireless/hostap/hostap_wlan.h b/drivers/net/wireless/hostap/hostap_wlan.h
index 15445bce2ac7..ffdf4876121b 100644
--- a/drivers/net/wireless/hostap/hostap_wlan.h
+++ b/drivers/net/wireless/hostap/hostap_wlan.h
@@ -5,6 +5,7 @@
 #include <linux/netdevice.h>
 #include <linux/mutex.h>
 #include <net/iw_handler.h>
+#include <net/ieee80211_radiotap.h>
 
 #include "hostap_config.h"
 #include "hostap_common.h"
@@ -55,6 +56,17 @@ struct linux_wlan_ng_cap_hdr {
 	__be32 encoding;
 } __attribute__ ((packed));
 
+struct hostap_radiotap_rx {
+	struct ieee80211_radiotap_header hdr;
+	__le64 tsft;
+	u8 rate;
+	u8 padding;
+	__le16 chan_freq;
+	__le16 chan_flags;
+	s8 dbm_antsignal;
+	s8 dbm_antnoise;
+} __attribute__ ((packed));
+
 #define LWNG_CAP_DID_BASE   (4 | (1 << 6)) /* section 4, group 1 */
 #define LWNG_CAPHDR_VERSION 0x80211001
 
@@ -734,7 +746,7 @@ struct local_info {
 	unsigned long scan_timestamp; /* Time started to scan */
 	enum {
 		PRISM2_MONITOR_80211 = 0, PRISM2_MONITOR_PRISM = 1,
-		PRISM2_MONITOR_CAPHDR = 2
+		PRISM2_MONITOR_CAPHDR = 2, PRISM2_MONITOR_RADIOTAP = 3
 	} monitor_type;
 	int monitor_allow_fcserr;
 
diff --git a/drivers/net/wireless/ipw2200.c b/drivers/net/wireless/ipw2200.c
index 6e704608947c..1acfbcd3703c 100644
--- a/drivers/net/wireless/ipw2200.c
+++ b/drivers/net/wireless/ipw2200.c
@@ -4972,8 +4972,7 @@ static int ipw_queue_tx_reclaim(struct ipw_priv *priv,
 	}
       done:
 	if ((ipw_tx_queue_space(q) > q->low_mark) &&
-	    (qindex >= 0) &&
-	    (priv->status & STATUS_ASSOCIATED) && netif_running(priv->net_dev))
+	    (qindex >= 0))
 		netif_wake_queue(priv->net_dev);
 	used = q->first_empty - q->last_used;
 	if (used < 0)
@@ -10154,14 +10153,8 @@ static  void init_sys_config(struct ipw_sys_config *sys_config)
 
 static int ipw_net_open(struct net_device *dev)
 {
-	struct ipw_priv *priv = ieee80211_priv(dev);
 	IPW_DEBUG_INFO("dev->open\n");
-	/* we should be verifying the device is ready to be opened */
-	mutex_lock(&priv->mutex);
-	if (!(priv->status & STATUS_RF_KILL_MASK) &&
-	    (priv->status & STATUS_ASSOCIATED))
-		netif_start_queue(dev);
-	mutex_unlock(&priv->mutex);
+	netif_start_queue(dev);
 	return 0;
 }
 
@@ -10481,13 +10474,6 @@ static int ipw_net_hard_start_xmit(struct ieee80211_txb *txb,
 	IPW_DEBUG_TX("dev->xmit(%d bytes)\n", txb->payload_size);
 	spin_lock_irqsave(&priv->lock, flags);
 
-	if (!(priv->status & STATUS_ASSOCIATED)) {
-		IPW_DEBUG_INFO("Tx attempt while not associated.\n");
-		priv->ieee->stats.tx_carrier_errors++;
-		netif_stop_queue(dev);
-		goto fail_unlock;
-	}
-
 #ifdef CONFIG_IPW2200_PROMISCUOUS
 	if (rtap_iface && netif_running(priv->prom_net_dev))
 		ipw_handle_promiscuous_tx(priv, txb);
@@ -10499,10 +10485,6 @@ static int ipw_net_hard_start_xmit(struct ieee80211_txb *txb,
 	spin_unlock_irqrestore(&priv->lock, flags);
 
 	return ret;
-
-      fail_unlock:
-	spin_unlock_irqrestore(&priv->lock, flags);
-	return 1;
 }
 
 static struct net_device_stats *ipw_net_get_stats(struct net_device *dev)
@@ -10703,13 +10685,6 @@ static void ipw_link_up(struct ipw_priv *priv)
 	priv->last_packet_time = 0;
 
 	netif_carrier_on(priv->net_dev);
-	if (netif_queue_stopped(priv->net_dev)) {
-		IPW_DEBUG_NOTIF("waking queue\n");
-		netif_wake_queue(priv->net_dev);
-	} else {
-		IPW_DEBUG_NOTIF("starting queue\n");
-		netif_start_queue(priv->net_dev);
-	}
 
 	cancel_delayed_work(&priv->request_scan);
 	cancel_delayed_work(&priv->request_direct_scan);
@@ -10739,7 +10714,6 @@ static void ipw_link_down(struct ipw_priv *priv)
 {
 	ipw_led_link_down(priv);
 	netif_carrier_off(priv->net_dev);
-	netif_stop_queue(priv->net_dev);
 	notify_wx_assoc_event(priv);
 
 	/* Cancel any queued work ... */
@@ -11419,7 +11393,6 @@ static void ipw_down(struct ipw_priv *priv)
 	/* Clear all bits but the RF Kill */
 	priv->status &= STATUS_RF_KILL_MASK | STATUS_EXIT_PENDING;
 	netif_carrier_off(priv->net_dev);
-	netif_stop_queue(priv->net_dev);
 
 	ipw_stop_nic(priv);
 
@@ -11522,7 +11495,6 @@ static int ipw_prom_open(struct net_device *dev)
 
 	IPW_DEBUG_INFO("prom dev->open\n");
 	netif_carrier_off(dev);
-	netif_stop_queue(dev);
 
 	if (priv->ieee->iw_mode != IW_MODE_MONITOR) {
 		priv->sys_config.accept_all_data_frames = 1;
@@ -11558,7 +11530,6 @@ static int ipw_prom_stop(struct net_device *dev)
 static int ipw_prom_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
 {
 	IPW_DEBUG_INFO("prom dev->xmit\n");
-	netif_stop_queue(dev);
 	return -EOPNOTSUPP;
 }
 
diff --git a/drivers/net/wireless/iwlwifi/Kconfig b/drivers/net/wireless/iwlwifi/Kconfig
index 62fb89d82318..82b66a3d3a5d 100644
--- a/drivers/net/wireless/iwlwifi/Kconfig
+++ b/drivers/net/wireless/iwlwifi/Kconfig
@@ -8,7 +8,6 @@ config IWLCORE
 	select MAC80211_LEDS if IWLWIFI_LEDS
 	select LEDS_CLASS if IWLWIFI_LEDS
 	select RFKILL if IWLWIFI_RFKILL
-	select RFKILL_INPUT if IWLWIFI_RFKILL
 
 config IWLWIFI_LEDS
 	bool
@@ -45,14 +44,6 @@ config IWL4965
 	  say M here and read <file:Documentation/kbuild/modules.txt>.  The
 	  module will be called iwl4965.ko.
 
-config IWL4965_HT
-	bool "Enable 802.11n HT features in iwl4965 driver"
-	depends on EXPERIMENTAL
-	depends on IWL4965
-	---help---
-	  This option enables IEEE 802.11n High Throughput features
-	  for the iwl4965 driver.
-
 config IWL4965_LEDS
 	bool "Enable LEDS features in iwl4965 driver"
 	depends on IWL4965
@@ -67,13 +58,6 @@ config IWL4965_SPECTRUM_MEASUREMENT
 	---help---
 	  This option will enable spectrum measurement for the iwl4965 driver.
 
-config IWL4965_SENSITIVITY
-	bool "Enable Sensitivity Calibration in iwl4965 driver"
-	depends on IWL4965
-	---help---
-	  This option will enable sensitivity calibration for the iwl4965
-	  driver.
-
 config IWLWIFI_DEBUG
 	bool "Enable full debugging output in iwl4965 driver"
 	depends on IWL4965
@@ -85,13 +69,13 @@ config IWLWIFI_DEBUG
 	  control which debug output is sent to the kernel log by setting the
 	  value in
 
-	          /sys/bus/pci/drivers/${DRIVER}/debug_level
+		/sys/class/net/wlan0/device/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
+		  % echo 0x43fff > /sys/class/net/wlan0/device/debug_level
 
 	  You can find the list of debug mask values in:
 		  drivers/net/wireless/iwlwifi/iwl-4965-debug.h
@@ -100,6 +84,13 @@ config IWLWIFI_DEBUG
 	  as the debug information can assist others in helping you resolve
 	  any problems you may encounter.
 
+config IWL5000
+	bool "Intel Wireless WiFi 5000AGN"
+	depends on IWL4965
+	---help---
+	  This option enables support for Intel Wireless WiFi Link 5000AGN Family
+	  Dependency on 4965 is temporary
+
 config IWLWIFI_DEBUGFS
         bool "Iwlwifi debugfs support"
         depends on IWLCORE && IWLWIFI_DEBUG && MAC80211_DEBUGFS
@@ -113,6 +104,7 @@ config IWL3945
 	select IWLWIFI
 	select MAC80211_LEDS if IWL3945_LEDS
 	select LEDS_CLASS if IWL3945_LEDS
+	select RFKILL if IWL3945_RFKILL
 	---help---
 	  Select to build the driver supporting the:
 
@@ -135,6 +127,10 @@ config IWL3945
 	  say M here and read <file:Documentation/kbuild/modules.txt>.  The
 	  module will be called iwl3945.ko.
 
+config IWL3945_RFKILL
+	bool "Enable RF kill support in iwl3945 drivers"
+	depends on IWL3945
+
 config IWL3945_SPECTRUM_MEASUREMENT
 	bool "Enable Spectrum Measurement in iwl3945 drivers"
 	depends on IWL3945
diff --git a/drivers/net/wireless/iwlwifi/Makefile b/drivers/net/wireless/iwlwifi/Makefile
index ec6187b75c3b..1f52b92f08b5 100644
--- a/drivers/net/wireless/iwlwifi/Makefile
+++ b/drivers/net/wireless/iwlwifi/Makefile
@@ -1,5 +1,7 @@
 obj-$(CONFIG_IWLCORE)	+= iwlcore.o
-iwlcore-objs 		:= iwl-core.o iwl-eeprom.o iwl-hcmd.o
+iwlcore-objs 		:= iwl-core.o iwl-eeprom.o iwl-hcmd.o iwl-power.o
+iwlcore-objs 		+= iwl-rx.o iwl-tx.o iwl-sta.o iwl-calib.o
+iwlcore-objs 		+= iwl-scan.o
 iwlcore-$(CONFIG_IWLWIFI_DEBUGFS) += iwl-debugfs.o
 iwlcore-$(CONFIG_IWLWIFI_LEDS) += iwl-led.o
 iwlcore-$(CONFIG_IWLWIFI_RFKILL) += iwl-rfkill.o
@@ -9,5 +11,10 @@ iwl3945-objs		:= iwl3945-base.o iwl-3945.o iwl-3945-rs.o
 iwl3945-$(CONFIG_IWL3945_LEDS) += iwl-3945-led.o
 
 obj-$(CONFIG_IWL4965)	+= iwl4965.o
-iwl4965-objs		:= iwl4965-base.o iwl-4965.o iwl-4965-rs.o iwl-sta.o
+iwl4965-objs		:= iwl4965-base.o iwl-4965.o iwl-4965-rs.o
+
+ifeq ($(CONFIG_IWL5000),y)
+	iwl4965-objs += iwl-5000.o
+endif
+
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-3945-hw.h b/drivers/net/wireless/iwlwifi/iwl-3945-hw.h
index ad612a8719f4..644bd9e08052 100644
--- a/drivers/net/wireless/iwlwifi/iwl-3945-hw.h
+++ b/drivers/net/wireless/iwlwifi/iwl-3945-hw.h
@@ -126,7 +126,7 @@ enum {
 	EEPROM_CHANNEL_ACTIVE = (1 << 3),	/* active scanning allowed */
 	EEPROM_CHANNEL_RADAR = (1 << 4),	/* radar detection required */
 	EEPROM_CHANNEL_WIDE = (1 << 5),		/* 20 MHz channel okay */
-	EEPROM_CHANNEL_NARROW = (1 << 6),	/* 10 MHz channel (not used) */
+	/* Bit 6 Reserved (was Narrow Channel) */
 	EEPROM_CHANNEL_DFS = (1 << 7),	/* dynamic freq selection candidate */
 };
 
@@ -289,17 +289,6 @@ struct iwl3945_eeprom {
 #define PCI_REG_WUM8       0x0E8
 #define PCI_CFG_PMC_PME_FROM_D3COLD_SUPPORT         (0x80000000)
 
-/* SCD (3945 Tx Frame 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)
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-3945-led.c b/drivers/net/wireless/iwlwifi/iwl-3945-led.c
index 8b1528e52d43..6be1fe13fa57 100644
--- a/drivers/net/wireless/iwlwifi/iwl-3945-led.c
+++ b/drivers/net/wireless/iwlwifi/iwl-3945-led.c
@@ -42,14 +42,11 @@
 #include "iwl-3945.h"
 #include "iwl-helpers.h"
 
-#define IWL_1MB_RATE (128 * 1024)
-#define IWL_LED_THRESHOLD (16)
-#define IWL_MAX_BLINK_TBL (10)
 
 static const struct {
 	u16 brightness;
 	u8 on_time;
-	u8 of_time;
+	u8 off_time;
 } blink_tbl[] =
 {
 	{300, 25, 25},
@@ -61,9 +58,16 @@ static const struct {
 	{15, 95, 95 },
 	{10, 110, 110},
 	{5, 130, 130},
-	{0, 167, 167}
+	{0, 167, 167},
+	/*SOLID_ON*/
+	{-1, IWL_LED_SOLID, 0}
 };
 
+#define IWL_1MB_RATE (128 * 1024)
+#define IWL_LED_THRESHOLD (16)
+#define IWL_MAX_BLINK_TBL (ARRAY_SIZE(blink_tbl) - 1) /*Exclude Solid on*/
+#define IWL_SOLID_BLINK_IDX (ARRAY_SIZE(blink_tbl) - 1)
+
 static int iwl3945_led_cmd_callback(struct iwl3945_priv *priv,
 				    struct iwl3945_cmd *cmd,
 				    struct sk_buff *skb)
@@ -71,6 +75,10 @@ static int iwl3945_led_cmd_callback(struct iwl3945_priv *priv,
 	return 1;
 }
 
+static inline int iwl3945_brightness_to_idx(enum led_brightness brightness)
+{
+	return fls(0x000000FF & (u32)brightness);
+}
 
 /* Send led command */
 static int iwl_send_led_cmd(struct iwl3945_priv *priv,
@@ -81,49 +89,45 @@ static int iwl_send_led_cmd(struct iwl3945_priv *priv,
 		.len = sizeof(struct iwl3945_led_cmd),
 		.data = led_cmd,
 		.meta.flags = CMD_ASYNC,
-		.meta.u.callback = iwl3945_led_cmd_callback
+		.meta.u.callback = iwl3945_led_cmd_callback,
 	};
 
 	return iwl3945_send_cmd(priv, &cmd);
 }
 
 
+
 /* Set led on command */
-static int iwl3945_led_on(struct iwl3945_priv *priv, int led_id)
+static int iwl3945_led_pattern(struct iwl3945_priv *priv, int led_id,
+			       unsigned int idx)
 {
 	struct iwl3945_led_cmd led_cmd = {
 		.id = led_id,
-		.on = IWL_LED_SOLID,
-		.off = 0,
 		.interval = IWL_DEF_LED_INTRVL
 	};
+
+	BUG_ON(idx > IWL_MAX_BLINK_TBL);
+
+	led_cmd.on = blink_tbl[idx].on_time;
+	led_cmd.off = blink_tbl[idx].off_time;
+
 	return iwl_send_led_cmd(priv, &led_cmd);
 }
 
+
+#if 1
 /* Set led on command */
-static int iwl3945_led_pattern(struct iwl3945_priv *priv, int led_id,
-			       enum led_brightness brightness)
+static int iwl3945_led_on(struct iwl3945_priv *priv, int led_id)
 {
 	struct iwl3945_led_cmd led_cmd = {
 		.id = led_id,
-		.on = brightness,
-		.off = brightness,
+		.on = IWL_LED_SOLID,
+		.off = 0,
 		.interval = IWL_DEF_LED_INTRVL
 	};
-	if (brightness == LED_FULL) {
-		led_cmd.on = IWL_LED_SOLID;
-		led_cmd.off = 0;
-	}
 	return iwl_send_led_cmd(priv, &led_cmd);
 }
 
-/* Set led register off */
-static int iwl3945_led_on_reg(struct iwl3945_priv *priv, int led_id)
-{
-	IWL_DEBUG_LED("led on %d\n", led_id);
-	return iwl3945_led_on(priv, led_id);
-}
-
 /* Set led off command */
 static int iwl3945_led_off(struct iwl3945_priv *priv, int led_id)
 {
@@ -136,27 +140,7 @@ static int iwl3945_led_off(struct iwl3945_priv *priv, int led_id)
 	IWL_DEBUG_LED("led off %d\n", led_id);
 	return iwl_send_led_cmd(priv, &led_cmd);
 }
-
-/* Set led register off */
-static int iwl3945_led_off_reg(struct iwl3945_priv *priv, int led_id)
-{
-	iwl3945_led_off(priv, led_id);
-	return 0;
-}
-
-/* Set led blink command */
-static int iwl3945_led_not_solid(struct iwl3945_priv *priv, int led_id,
-			       u8 brightness)
-{
-	struct iwl3945_led_cmd led_cmd = {
-		.id = led_id,
-		.on = brightness,
-		.off = brightness,
-		.interval = IWL_DEF_LED_INTRVL
-	};
-
-	return iwl_send_led_cmd(priv, &led_cmd);
-}
+#endif
 
 
 /*
@@ -206,8 +190,10 @@ static void iwl3945_led_brightness_set(struct led_classdev *led_cdev,
 			led->led_off(priv, IWL_LED_LINK);
 		break;
 	default:
-		if (led->led_pattern)
-			led->led_pattern(priv, IWL_LED_LINK, brightness);
+		if (led->led_pattern) {
+			int idx = iwl3945_brightness_to_idx(brightness);
+			led->led_pattern(priv, IWL_LED_LINK, idx);
+		}
 		break;
 	}
 }
@@ -252,24 +238,20 @@ static int iwl3945_led_register_led(struct iwl3945_priv *priv,
 static inline u8 get_blink_rate(struct iwl3945_priv *priv)
 {
 	int index;
-	u8 blink_rate;
-
-	if (priv->rxtxpackets < IWL_LED_THRESHOLD)
-		index = 10;
-	else {
-		for (index = 0; index < IWL_MAX_BLINK_TBL; index++) {
-			if (priv->rxtxpackets > (blink_tbl[index].brightness *
-						 IWL_1MB_RATE))
-				break;
-		}
-	}
-	/* if 0 frame is transfered */
-	if ((index == IWL_MAX_BLINK_TBL) || !priv->allow_blinking)
-		blink_rate = IWL_LED_SOLID;
-	else
-		blink_rate = blink_tbl[index].on_time;
+	u64 current_tpt = priv->rxtxpackets;
+	s64 tpt = current_tpt - priv->led_tpt;
 
-	return blink_rate;
+	if (tpt < 0)
+		tpt = -tpt;
+	priv->led_tpt = current_tpt;
+
+	if (!priv->allow_blinking)
+		index = IWL_MAX_BLINK_TBL;
+	else
+		for (index = 0; index < IWL_MAX_BLINK_TBL; index++)
+			if (tpt > (blink_tbl[index].brightness * IWL_1MB_RATE))
+				break;
+	return index;
 }
 
 static inline int is_rf_kill(struct iwl3945_priv *priv)
@@ -285,7 +267,7 @@ static inline int is_rf_kill(struct iwl3945_priv *priv)
  */
 void iwl3945_led_background(struct iwl3945_priv *priv)
 {
-	u8 blink_rate;
+	u8 blink_idx;
 
 	if (test_bit(STATUS_EXIT_PENDING, &priv->status)) {
 		priv->last_blink_time = 0;
@@ -298,9 +280,10 @@ void iwl3945_led_background(struct iwl3945_priv *priv)
 
 	if (!priv->allow_blinking) {
 		priv->last_blink_time = 0;
-		if (priv->last_blink_rate != IWL_LED_SOLID) {
-			priv->last_blink_rate = IWL_LED_SOLID;
-			iwl3945_led_on(priv, IWL_LED_LINK);
+		if (priv->last_blink_rate != IWL_SOLID_BLINK_IDX) {
+			priv->last_blink_rate = IWL_SOLID_BLINK_IDX;
+			iwl3945_led_pattern(priv, IWL_LED_LINK,
+					    IWL_SOLID_BLINK_IDX);
 		}
 		return;
 	}
@@ -309,21 +292,14 @@ void iwl3945_led_background(struct iwl3945_priv *priv)
 			msecs_to_jiffies(1000)))
 		return;
 
-	blink_rate = get_blink_rate(priv);
+	blink_idx = get_blink_rate(priv);
 
 	/* call only if blink rate change */
-	if (blink_rate != priv->last_blink_rate) {
-		if (blink_rate != IWL_LED_SOLID) {
-			priv->last_blink_time = jiffies +
-						msecs_to_jiffies(1000);
-			iwl3945_led_not_solid(priv, IWL_LED_LINK, blink_rate);
-		} else {
-			priv->last_blink_time = 0;
-			iwl3945_led_on(priv, IWL_LED_LINK);
-		}
-	}
+	if (blink_idx != priv->last_blink_rate)
+		iwl3945_led_pattern(priv, IWL_LED_LINK, blink_idx);
 
-	priv->last_blink_rate = blink_rate;
+	priv->last_blink_time = jiffies;
+	priv->last_blink_rate = blink_idx;
 	priv->rxtxpackets = 0;
 }
 
@@ -337,6 +313,7 @@ int iwl3945_led_register(struct iwl3945_priv *priv)
 
 	priv->last_blink_rate = 0;
 	priv->rxtxpackets = 0;
+	priv->led_tpt = 0;
 	priv->last_blink_time = 0;
 	priv->allow_blinking = 0;
 
@@ -344,8 +321,8 @@ int iwl3945_led_register(struct iwl3945_priv *priv)
 	snprintf(name, sizeof(name), "iwl-%s:radio",
 		 wiphy_name(priv->hw->wiphy));
 
-	priv->led[IWL_LED_TRG_RADIO].led_on = iwl3945_led_on_reg;
-	priv->led[IWL_LED_TRG_RADIO].led_off = iwl3945_led_off_reg;
+	priv->led[IWL_LED_TRG_RADIO].led_on = iwl3945_led_on;
+	priv->led[IWL_LED_TRG_RADIO].led_off = iwl3945_led_off;
 	priv->led[IWL_LED_TRG_RADIO].led_pattern = NULL;
 
 	ret = iwl3945_led_register_led(priv,
@@ -364,8 +341,8 @@ int iwl3945_led_register(struct iwl3945_priv *priv)
 				   IWL_LED_TRG_ASSOC, 0,
 				   name, trigger);
 	/* for assoc always turn led on */
-	priv->led[IWL_LED_TRG_ASSOC].led_on = iwl3945_led_on_reg;
-	priv->led[IWL_LED_TRG_ASSOC].led_off = iwl3945_led_on_reg;
+	priv->led[IWL_LED_TRG_ASSOC].led_on = iwl3945_led_on;
+	priv->led[IWL_LED_TRG_ASSOC].led_off = iwl3945_led_on;
 	priv->led[IWL_LED_TRG_ASSOC].led_pattern = NULL;
 
 	if (ret)
@@ -391,6 +368,7 @@ int iwl3945_led_register(struct iwl3945_priv *priv)
 	trigger = ieee80211_get_tx_led_name(priv->hw);
 	snprintf(name, sizeof(name), "iwl-%s:TX",
 		 wiphy_name(priv->hw->wiphy));
+
 	ret = iwl3945_led_register_led(priv,
 				   &priv->led[IWL_LED_TRG_TX],
 				   IWL_LED_TRG_TX, 0,
diff --git a/drivers/net/wireless/iwlwifi/iwl-3945-led.h b/drivers/net/wireless/iwlwifi/iwl-3945-led.h
index b1d2f6b8b259..47b7e0bac802 100644
--- a/drivers/net/wireless/iwlwifi/iwl-3945-led.h
+++ b/drivers/net/wireless/iwlwifi/iwl-3945-led.h
@@ -54,7 +54,7 @@ struct iwl3945_led {
 	int (*led_on) (struct iwl3945_priv *priv, int led_id);
 	int (*led_off) (struct iwl3945_priv *priv, int led_id);
 	int (*led_pattern) (struct iwl3945_priv *priv, int led_id,
-			    enum led_brightness brightness);
+			    unsigned int idx);
 
 	enum led_type type;
 	unsigned int registered;
diff --git a/drivers/net/wireless/iwlwifi/iwl-3945-rs.c b/drivers/net/wireless/iwlwifi/iwl-3945-rs.c
index 85c22641542d..10c64bdb314c 100644
--- a/drivers/net/wireless/iwlwifi/iwl-3945-rs.c
+++ b/drivers/net/wireless/iwlwifi/iwl-3945-rs.c
@@ -29,7 +29,6 @@
 #include <linux/skbuff.h>
 #include <linux/wireless.h>
 #include <net/mac80211.h>
-#include <net/ieee80211.h>
 
 #include <linux/netdevice.h>
 #include <linux/etherdevice.h>
@@ -446,8 +445,7 @@ static int rs_adjust_next_rate(struct iwl3945_priv *priv, int rate)
  */
 static void rs_tx_status(void *priv_rate,
 			 struct net_device *dev,
-			 struct sk_buff *skb,
-			 struct ieee80211_tx_status *tx_resp)
+			 struct sk_buff *skb)
 {
 	u8 retries, current_count;
 	int scale_rate_index, first_index, last_index;
@@ -458,14 +456,15 @@ static void rs_tx_status(void *priv_rate,
 	struct ieee80211_local *local = wdev_priv(dev->ieee80211_ptr);
 	struct iwl3945_rs_sta *rs_sta;
 	struct ieee80211_supported_band *sband;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 
 	IWL_DEBUG_RATE("enter\n");
 
 	sband = local->hw.wiphy->bands[local->hw.conf.channel->band];
 
 
-	retries = tx_resp->retry_count;
-	first_index = tx_resp->control.tx_rate->hw_value;
+	retries = info->status.retry_count;
+	first_index = sband->bitrates[info->tx_rate_idx].hw_value;
 	if ((first_index < 0) || (first_index >= IWL_RATE_COUNT)) {
 		IWL_DEBUG_RATE("leave: Rate out of bounds: %d\n", first_index);
 		return;
@@ -526,11 +525,11 @@ static void rs_tx_status(void *priv_rate,
 	/* 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) ?
+		       (info->flags & IEEE80211_TX_STAT_ACK) ?
 		       "success" : "failure");
 	iwl3945_collect_tx_data(rs_sta,
 			    &rs_sta->win[last_index],
-			    tx_resp->flags & IEEE80211_TX_STATUS_ACK, 1);
+			    info->flags & IEEE80211_TX_STAT_ACK, 1);
 
 	/* We updated the rate scale window -- if its been more than
 	 * flush_time since the last run, schedule the flush
@@ -670,7 +669,7 @@ static void rs_get_rate(void *priv_rate, struct net_device *dev,
 	    is_multicast_ether_addr(hdr->addr1) ||
 	    !sta || !sta->rate_ctrl_priv) {
 		IWL_DEBUG_RATE("leave: No STA priv data to update!\n");
-		sel->rate = rate_lowest(local, sband, sta);
+		sel->rate_idx = rate_lowest_index(local, sband, sta);
 		rcu_read_unlock();
 		return;
 	}
@@ -814,7 +813,7 @@ static void rs_get_rate(void *priv_rate, struct net_device *dev,
 
 	IWL_DEBUG_RATE("leave: %d\n", index);
 
-	sel->rate = &sband->bitrates[sta->txrate_idx];
+	sel->rate_idx = sta->txrate_idx;
 }
 
 static struct rate_control_ops rs_ops = {
diff --git a/drivers/net/wireless/iwlwifi/iwl-3945.c b/drivers/net/wireless/iwlwifi/iwl-3945.c
index 55ac850744b3..c2a76785b665 100644
--- a/drivers/net/wireless/iwlwifi/iwl-3945.c
+++ b/drivers/net/wireless/iwlwifi/iwl-3945.c
@@ -283,8 +283,7 @@ static void iwl3945_tx_queue_reclaim(struct iwl3945_priv *priv,
 		q->read_ptr = iwl_queue_inc_wrap(q->read_ptr, q->n_bd)) {
 
 		tx_info = &txq->txb[txq->q.read_ptr];
-		ieee80211_tx_status_irqsafe(priv->hw, tx_info->skb[0],
-					    &tx_info->status);
+		ieee80211_tx_status_irqsafe(priv->hw, tx_info->skb[0]);
 		tx_info->skb[0] = NULL;
 		iwl3945_hw_txq_free_tfd(priv, txq);
 	}
@@ -306,7 +305,7 @@ static void iwl3945_rx_reply_tx(struct iwl3945_priv *priv,
 	int txq_id = SEQ_TO_QUEUE(sequence);
 	int index = SEQ_TO_INDEX(sequence);
 	struct iwl3945_tx_queue *txq = &priv->txq[txq_id];
-	struct ieee80211_tx_status *tx_status;
+	struct ieee80211_tx_info *info;
 	struct iwl3945_tx_resp *tx_resp = (void *)&pkt->u.raw[0];
 	u32  status = le32_to_cpu(tx_resp->status);
 	int rate_idx;
@@ -319,19 +318,22 @@ static void iwl3945_rx_reply_tx(struct iwl3945_priv *priv,
 		return;
 	}
 
-	tx_status = &(txq->txb[txq->q.read_ptr].status);
+	info = IEEE80211_SKB_CB(txq->txb[txq->q.read_ptr].skb[0]);
+	memset(&info->status, 0, sizeof(info->status));
 
-	tx_status->retry_count = tx_resp->failure_frame;
+	info->status.retry_count = tx_resp->failure_frame;
 	/* tx_status->rts_retry_count = tx_resp->failure_rts; */
-	tx_status->flags = ((status & TX_STATUS_MSK) == TX_STATUS_SUCCESS) ?
-				IEEE80211_TX_STATUS_ACK : 0;
+	info->flags |= ((status & TX_STATUS_MSK) == TX_STATUS_SUCCESS) ?
+				IEEE80211_TX_STAT_ACK : 0;
 
 	IWL_DEBUG_TX("Tx queue %d Status %s (0x%08x) plcp rate %d retries %d\n",
 			txq_id, iwl3945_get_tx_fail_reason(status), status,
 			tx_resp->rate, tx_resp->failure_frame);
 
 	rate_idx = iwl3945_hwrate_to_plcp_idx(tx_resp->rate);
-	tx_status->control.tx_rate = &priv->ieee_rates[rate_idx];
+	if (info->band == IEEE80211_BAND_5GHZ)
+		rate_idx -= IWL_FIRST_OFDM_RATE;
+	info->tx_rate_idx = rate_idx;
 	IWL_DEBUG_TX_REPLY("Tx queue reclaim %d\n", index);
 	iwl3945_tx_queue_reclaim(priv, txq_id, index);
 
@@ -386,7 +388,7 @@ static void iwl3945_dbg_report_frame(struct iwl3945_priv *priv,
 	u32 print_dump = 0;	/* set to 1 to dump all frames' contents */
 	u32 hundred = 0;
 	u32 dataframe = 0;
-	u16 fc;
+	__le16 fc;
 	u16 seq_ctl;
 	u16 channel;
 	u16 phy_flags;
@@ -405,7 +407,7 @@ static void iwl3945_dbg_report_frame(struct iwl3945_priv *priv,
 	u8 *data = IWL_RX_DATA(pkt);
 
 	/* MAC header */
-	fc = le16_to_cpu(header->frame_control);
+	fc = header->frame_control;
 	seq_ctl = le16_to_cpu(header->seq_ctrl);
 
 	/* metadata */
@@ -429,8 +431,8 @@ static void iwl3945_dbg_report_frame(struct iwl3945_priv *priv,
 
 	/* 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)) {
+	if (to_us && (fc & ~cpu_to_le16(IEEE80211_FCTL_PROTECTED)) ==
+	    cpu_to_le16(IEEE80211_FCTL_FROMDS | IEEE80211_FTYPE_DATA)) {
 		dataframe = 1;
 		if (!group100)
 			print_summary = 1;	/* print each frame */
@@ -453,13 +455,13 @@ static void iwl3945_dbg_report_frame(struct iwl3945_priv *priv,
 
 		if (hundred)
 			title = "100Frames";
-		else if (fc & IEEE80211_FCTL_RETRY)
+		else if (ieee80211_has_retry(fc))
 			title = "Retry";
-		else if (ieee80211_is_assoc_response(fc))
+		else if (ieee80211_is_assoc_resp(fc))
 			title = "AscRsp";
-		else if (ieee80211_is_reassoc_response(fc))
+		else if (ieee80211_is_reassoc_resp(fc))
 			title = "RasRsp";
-		else if (ieee80211_is_probe_response(fc)) {
+		else if (ieee80211_is_probe_resp(fc)) {
 			title = "PrbRsp";
 			print_dump = 1;	/* dump frame contents */
 		} else if (ieee80211_is_beacon(fc)) {
@@ -488,14 +490,14 @@ static void iwl3945_dbg_report_frame(struct iwl3945_priv *priv,
 		if (dataframe)
 			IWL_DEBUG_RX("%s: mhd=0x%04x, dst=0x%02x, "
 				     "len=%u, rssi=%d, chnl=%d, rate=%d, \n",
-				     title, fc, header->addr1[5],
+				     title, le16_to_cpu(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],
+				     title, le16_to_cpu(fc), header->addr1[5],
 				     header->addr3[5], rssi,
 				     tsf_low - priv->scan_start_tsf,
 				     phy_flags, channel);
@@ -512,6 +514,23 @@ static inline void iwl3945_dbg_report_frame(struct iwl3945_priv *priv,
 }
 #endif
 
+/* This is necessary only for a number of statistics, see the caller. */
+static int iwl3945_is_network_packet(struct iwl3945_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 to our IBSS update information */
+		return !compare_ether_addr(header->addr3, priv->bssid);
+	case IEEE80211_IF_TYPE_STA: /* Header: Dest. | AP{BSSID} | Source */
+		/* packets to our IBSS update information */
+		return !compare_ether_addr(header->addr2, priv->bssid);
+	default:
+		return 1;
+	}
+}
 
 static void iwl3945_add_radiotap(struct iwl3945_priv *priv,
 				 struct sk_buff *skb,
@@ -520,7 +539,7 @@ static void iwl3945_add_radiotap(struct iwl3945_priv *priv,
 {
 	/* First cache any information we need before we overwrite
 	 * the information provided in the skb from the hardware */
-	s8 signal = stats->ssi;
+	s8 signal = stats->signal;
 	s8 noise = 0;
 	int rate = stats->rate_idx;
 	u64 tsf = stats->mactime;
@@ -606,12 +625,12 @@ static void iwl3945_add_radiotap(struct iwl3945_priv *priv,
 	stats->flag |= RX_FLAG_RADIOTAP;
 }
 
-static void iwl3945_handle_data_packet(struct iwl3945_priv *priv, int is_data,
+static void iwl3945_pass_packet_to_mac80211(struct iwl3945_priv *priv,
 				   struct iwl3945_rx_mem_buffer *rxb,
 				   struct ieee80211_rx_status *stats)
 {
-	struct ieee80211_hdr *hdr;
 	struct iwl3945_rx_packet *pkt = (struct iwl3945_rx_packet *)rxb->skb->data;
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)IWL_RX_DATA(pkt);
 	struct iwl3945_rx_frame_hdr *rx_hdr = IWL_RX_HDR(pkt);
 	struct iwl3945_rx_frame_end *rx_end = IWL_RX_END(pkt);
 	short len = le16_to_cpu(rx_hdr->len);
@@ -633,8 +652,6 @@ static void iwl3945_handle_data_packet(struct iwl3945_priv *priv, int is_data,
 	/* 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 (iwl3945_param_hwcrypto)
 		iwl3945_set_decrypted_flag(priv, rxb->skb,
 				       le32_to_cpu(rx_end->status), stats);
@@ -643,7 +660,7 @@ static void iwl3945_handle_data_packet(struct iwl3945_priv *priv, int is_data,
 		iwl3945_add_radiotap(priv, rxb->skb, rx_hdr, stats);
 
 #ifdef CONFIG_IWL3945_LEDS
-	if (is_data)
+	if (ieee80211_is_data(hdr->frame_control))
 		priv->rxtxpackets += len;
 #endif
 	ieee80211_rx_irqsafe(priv->hw, rxb->skb, stats);
@@ -692,12 +709,12 @@ static void iwl3945_rx_reply_rx(struct iwl3945_priv *priv,
 	}
 
 	if (priv->iw_mode == IEEE80211_IF_TYPE_MNTR) {
-		iwl3945_handle_data_packet(priv, 1, rxb, &rx_status);
+		iwl3945_pass_packet_to_mac80211(priv, rxb, &rx_status);
 		return;
 	}
 
 	/* Convert 3945's rssi indicator to dBm */
-	rx_status.ssi = rx_stats->rssi - IWL_RSSI_OFFSET;
+	rx_status.signal = rx_stats->rssi - IWL_RSSI_OFFSET;
 
 	/* Set default noise value to -127 */
 	if (priv->last_rx_noise == 0)
@@ -716,21 +733,21 @@ static void iwl3945_rx_reply_rx(struct iwl3945_priv *priv,
 	 * Calculate rx_status.signal (quality indicator in %) based on SNR. */
 	if (rx_stats_noise_diff) {
 		snr = rx_stats_sig_avg / rx_stats_noise_diff;
-		rx_status.noise = rx_status.ssi -
+		rx_status.noise = rx_status.signal -
 					iwl3945_calc_db_from_ratio(snr);
-		rx_status.signal = iwl3945_calc_sig_qual(rx_status.ssi,
+		rx_status.qual = iwl3945_calc_sig_qual(rx_status.signal,
 							 rx_status.noise);
 
 	/* If noise info not available, calculate signal quality indicator (%)
 	 *   using just the dBm signal level. */
 	} else {
 		rx_status.noise = priv->last_rx_noise;
-		rx_status.signal = iwl3945_calc_sig_qual(rx_status.ssi, 0);
+		rx_status.qual = iwl3945_calc_sig_qual(rx_status.signal, 0);
 	}
 
 
 	IWL_DEBUG_STATS("Rssi %d noise %d qual %d sig_avg %d noise_diff %d\n",
-			rx_status.ssi, rx_status.noise, rx_status.signal,
+			rx_status.signal, rx_status.noise, rx_status.qual,
 			rx_stats_sig_avg, rx_stats_noise_diff);
 
 	header = (struct ieee80211_hdr *)IWL_RX_DATA(pkt);
@@ -740,8 +757,8 @@ static void iwl3945_rx_reply_rx(struct iwl3945_priv *priv,
 	IWL_DEBUG_STATS_LIMIT("[%c] %d RSSI:%d Signal:%u, Noise:%u, Rate:%u\n",
 			      network_packet ? '*' : ' ',
 			      le16_to_cpu(rx_hdr->channel),
-			      rx_status.ssi, rx_status.ssi,
-			      rx_status.ssi, rx_status.rate_idx);
+			      rx_status.signal, rx_status.signal,
+			      rx_status.noise, rx_status.rate_idx);
 
 #ifdef CONFIG_IWL3945_DEBUG
 	if (iwl3945_debug_level & (IWL_DL_RX))
@@ -752,7 +769,7 @@ static void iwl3945_rx_reply_rx(struct iwl3945_priv *priv,
 	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 = rx_status.ssi;
+		priv->last_rx_rssi = rx_status.signal;
 		priv->last_rx_noise = rx_status.noise;
 	}
 
@@ -840,27 +857,12 @@ static void iwl3945_rx_reply_rx(struct iwl3945_priv *priv,
 			}
 		}
 
-		iwl3945_handle_data_packet(priv, 0, rxb, &rx_status);
-		break;
-
-	case IEEE80211_FTYPE_CTL:
-		break;
-
-	case IEEE80211_FTYPE_DATA: {
-		DECLARE_MAC_BUF(mac1);
-		DECLARE_MAC_BUF(mac2);
-		DECLARE_MAC_BUF(mac3);
-
-		if (unlikely(iwl3945_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, &rx_status);
+	case IEEE80211_FTYPE_DATA:
+		/* fall through */
+	default:
+		iwl3945_pass_packet_to_mac80211(priv, rxb, &rx_status);
 		break;
 	}
-	}
 }
 
 int iwl3945_hw_txq_attach_buf_to_tfd(struct iwl3945_priv *priv, void *ptr,
@@ -962,23 +964,24 @@ u8 iwl3945_hw_find_station(struct iwl3945_priv *priv, const u8 *addr)
 */
 void iwl3945_hw_build_tx_cmd_rate(struct iwl3945_priv *priv,
 			      struct iwl3945_cmd *cmd,
-			      struct ieee80211_tx_control *ctrl,
+			      struct ieee80211_tx_info *info,
 			      struct ieee80211_hdr *hdr, int sta_id, int tx_id)
 {
 	unsigned long flags;
-	u16 rate_index = min(ctrl->tx_rate->hw_value & 0xffff, IWL_RATE_COUNT - 1);
+	u16 hw_value = ieee80211_get_tx_rate(priv->hw, info)->hw_value;
+	u16 rate_index = min(hw_value & 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);
+	__le16 fc = hdr->frame_control;
 
 	rate = iwl3945_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? */
+	 * in this running context */
 	rate_mask = IWL_RATES_MASK;
 
 	spin_lock_irqsave(&priv->sta_lock, flags);
@@ -997,7 +1000,7 @@ void iwl3945_hw_build_tx_cmd_rate(struct iwl3945_priv *priv,
 	else
 		rts_retry_limit = 7;
 
-	if (ieee80211_is_probe_response(fc)) {
+	if (ieee80211_is_probe_resp(fc)) {
 		data_retry_limit = 3;
 		if (data_retry_limit < rts_retry_limit)
 			rts_retry_limit = data_retry_limit;
@@ -1007,12 +1010,12 @@ void iwl3945_hw_build_tx_cmd_rate(struct iwl3945_priv *priv,
 	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 (ieee80211_is_mgmt(fc)) {
+		switch (fc & cpu_to_le16(IEEE80211_FCTL_STYPE)) {
+		case cpu_to_le16(IEEE80211_STYPE_AUTH):
+		case cpu_to_le16(IEEE80211_STYPE_DEAUTH):
+		case cpu_to_le16(IEEE80211_STYPE_ASSOC_REQ):
+		case cpu_to_le16(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;
@@ -1233,7 +1236,7 @@ int iwl3945_hw_nic_init(struct iwl3945_priv *priv)
 	iwl3945_power_init_handle(priv);
 
 	spin_lock_irqsave(&priv->lock, flags);
-	iwl3945_set_bit(priv, CSR_ANA_PLL_CFG, (1 << 24));
+	iwl3945_set_bit(priv, CSR_ANA_PLL_CFG, CSR39_ANA_PLL_CFG_VAL);
 	iwl3945_set_bit(priv, CSR_GIO_CHICKEN_BITS,
 		    CSR_GIO_CHICKEN_BITS_REG_BIT_L1A_NO_L0S_RX);
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-3945.h b/drivers/net/wireless/iwlwifi/iwl-3945.h
index c7695a215a39..fa81ba1af3d3 100644
--- a/drivers/net/wireless/iwlwifi/iwl-3945.h
+++ b/drivers/net/wireless/iwlwifi/iwl-3945.h
@@ -36,6 +36,10 @@
 #include <linux/kernel.h>
 #include <net/ieee80211_radiotap.h>
 
+/*used for rfkill*/
+#include <linux/rfkill.h>
+#include <linux/input.h>
+
 /* Hardware specific file defines the PCI IDs table for that hardware module */
 extern struct pci_device_id iwl3945_hw_card_ids[];
 
@@ -124,7 +128,6 @@ int iwl3945_x2_queue_used(const struct iwl3945_queue *q, int i);
 
 /* One for each TFD */
 struct iwl3945_tx_info {
-	struct ieee80211_tx_status status;
 	struct sk_buff *skb[MAX_NUM_OF_TBS];
 };
 
@@ -507,8 +510,6 @@ struct iwl3945_ucode {
 	u8 data[0];		/* data in same order as "size" elements */
 };
 
-#define IWL_IBSS_MAC_HASH_SIZE 32
-
 struct iwl3945_ibss_seq {
 	u8 mac[ETH_ALEN];
 	u16 seq_num;
@@ -566,17 +567,8 @@ extern int iwl3945_send_add_station(struct iwl3945_priv *priv,
 				struct iwl3945_addsta_cmd *sta, u8 flags);
 extern u8 iwl3945_add_station(struct iwl3945_priv *priv, const u8 *bssid,
 			  int is_ap, u8 flags);
-extern int iwl3945_is_network_packet(struct iwl3945_priv *priv,
-				 struct ieee80211_hdr *header);
 extern int iwl3945_power_init_handle(struct iwl3945_priv *priv);
 extern int iwl3945_eeprom_init(struct iwl3945_priv *priv);
-extern void iwl3945_handle_data_packet_monitor(struct iwl3945_priv *priv,
-					   struct iwl3945_rx_mem_buffer *rxb,
-					   void *data, short len,
-					   struct ieee80211_rx_status *stats,
-					   u16 phy_flags);
-extern int iwl3945_is_duplicate_packet(struct iwl3945_priv *priv,
-				       struct ieee80211_hdr *header);
 extern int iwl3945_rx_queue_alloc(struct iwl3945_priv *priv);
 extern void iwl3945_rx_queue_reset(struct iwl3945_priv *priv,
 			       struct iwl3945_rx_queue *rxq);
@@ -645,7 +637,7 @@ extern unsigned int iwl3945_hw_get_beacon_cmd(struct iwl3945_priv *priv,
 extern int iwl3945_hw_get_rx_read(struct iwl3945_priv *priv);
 extern void iwl3945_hw_build_tx_cmd_rate(struct iwl3945_priv *priv,
 				     struct iwl3945_cmd *cmd,
-				     struct ieee80211_tx_control *ctrl,
+				     struct ieee80211_tx_info *info,
 				     struct ieee80211_hdr *hdr,
 				     int sta_id, int tx_id);
 extern int iwl3945_hw_reg_send_txpower(struct iwl3945_priv *priv);
@@ -687,6 +679,18 @@ enum {
 
 #endif
 
+#ifdef CONFIG_IWL3945_RFKILL
+struct iwl3945_priv;
+
+void iwl3945_rfkill_set_hw_state(struct iwl3945_priv *priv);
+void iwl3945_rfkill_unregister(struct iwl3945_priv *priv);
+int iwl3945_rfkill_init(struct iwl3945_priv *priv);
+#else
+static inline void iwl3945_rfkill_set_hw_state(struct iwl3945_priv *priv) {}
+static inline void iwl3945_rfkill_unregister(struct iwl3945_priv *priv) {}
+static inline int iwl3945_rfkill_init(struct iwl3945_priv *priv) { return 0; }
+#endif
+
 #define IWL_MAX_NUM_QUEUES IWL39_MAX_NUM_QUEUES
 
 struct iwl3945_priv {
@@ -780,12 +784,17 @@ struct iwl3945_priv {
 	struct iwl3945_init_alive_resp card_alive_init;
 	struct iwl3945_alive_resp card_alive;
 
+#ifdef CONFIG_IWL3945_RFKILL
+	struct rfkill *rfkill;
+#endif
+
 #ifdef CONFIG_IWL3945_LEDS
 	struct iwl3945_led led[IWL_LED_TRG_MAX];
 	unsigned long last_blink_time;
 	u8 last_blink_rate;
 	u8 allow_blinking;
 	unsigned int rxtxpackets;
+	u64 led_tpt;
 #endif
 
 
@@ -836,20 +845,10 @@ struct iwl3945_priv {
 
 	u8 mac80211_registered;
 
-	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;
-
-	/* Hash table for finding stations in IBSS network */
-	struct list_head ibss_mac_hash[IWL_IBSS_MAC_HASH_SIZE];
-
 	/* eeprom */
 	struct iwl3945_eeprom eeprom;
 
@@ -886,6 +885,7 @@ struct iwl3945_priv {
 	struct work_struct report_work;
 	struct work_struct request_scan;
 	struct work_struct beacon_update;
+	struct work_struct set_monitor;
 
 	struct tasklet_struct irq_tasklet;
 
@@ -924,11 +924,6 @@ static inline int is_channel_valid(const struct iwl3945_channel_info *ch_info)
 	return (ch_info->flags & EEPROM_CHANNEL_VALID) ? 1 : 0;
 }
 
-static inline int is_channel_narrow(const struct iwl3945_channel_info *ch_info)
-{
-	return (ch_info->flags & EEPROM_CHANNEL_NARROW) ? 1 : 0;
-}
-
 static inline int is_channel_radar(const struct iwl3945_channel_info *ch_info)
 {
 	return (ch_info->flags & EEPROM_CHANNEL_RADAR) ? 1 : 0;
diff --git a/drivers/net/wireless/iwlwifi/iwl-4965-hw.h b/drivers/net/wireless/iwlwifi/iwl-4965-hw.h
index 1a66b508a8ea..fce950f4163c 100644
--- a/drivers/net/wireless/iwlwifi/iwl-4965-hw.h
+++ b/drivers/net/wireless/iwlwifi/iwl-4965-hw.h
@@ -62,13 +62,18 @@
  *****************************************************************************/
 /*
  * Please use this file (iwl-4965-hw.h) only for hardware-related definitions.
- * Use iwl-4965-commands.h for uCode API definitions.
- * Use iwl-4965.h for driver implementation definitions.
+ * Use iwl-commands.h for uCode API definitions.
+ * Use iwl-dev.h for driver implementation definitions.
  */
 
 #ifndef __iwl_4965_hw_h__
 #define __iwl_4965_hw_h__
 
+#include "iwl-fh.h"
+
+/* EERPROM */
+#define IWL4965_EEPROM_IMG_SIZE			1024
+
 /*
  * uCode queue management definitions ...
  * Queue #4 is the command queue for 3945 and 4965; map it to Tx FIFO chnl 4.
@@ -77,7 +82,7 @@
  */
 #define IWL_CMD_QUEUE_NUM       4
 #define IWL_CMD_FIFO_NUM        4
-#define IWL_BACK_QUEUE_FIRST_ID 7
+#define IWL49_FIRST_AMPDU_QUEUE	7
 
 /* Tx rates */
 #define IWL_CCK_RATES 4
@@ -93,11 +98,16 @@
 #define IWL_RSSI_OFFSET	44
 
 
-#include "iwl-4965-commands.h"
+#include "iwl-commands.h"
 
-#define PCI_LINK_CTRL      0x0F0
+/* PCI registers */
+#define PCI_LINK_CTRL      0x0F0	/* 1 byte */
 #define PCI_POWER_SOURCE   0x0C8
 #define PCI_REG_WUM8       0x0E8
+
+/* PCI register values */
+#define PCI_LINK_VAL_L0S_EN	0x01
+#define PCI_LINK_VAL_L1_EN	0x02
 #define PCI_CFG_PMC_PME_FROM_D3COLD_SUPPORT         (0x80000000)
 
 #define TFD_QUEUE_SIZE_MAX      (256)
@@ -131,10 +141,8 @@
 #define RTC_DATA_LOWER_BOUND			(0x800000)
 #define IWL49_RTC_DATA_UPPER_BOUND		(0x80A000)
 
-#define IWL49_RTC_INST_SIZE	\
-			(IWL49_RTC_INST_UPPER_BOUND - RTC_INST_LOWER_BOUND)
-#define IWL49_RTC_DATA_SIZE	\
-			(IWL49_RTC_DATA_UPPER_BOUND - RTC_DATA_LOWER_BOUND)
+#define IWL49_RTC_INST_SIZE  (IWL49_RTC_INST_UPPER_BOUND - RTC_INST_LOWER_BOUND)
+#define IWL49_RTC_DATA_SIZE  (IWL49_RTC_DATA_UPPER_BOUND - RTC_DATA_LOWER_BOUND)
 
 #define IWL_MAX_INST_SIZE IWL49_RTC_INST_SIZE
 #define IWL_MAX_DATA_SIZE IWL49_RTC_DATA_SIZE
@@ -785,585 +793,6 @@ enum {
 
 /********************* END TXPOWER *****************************************/
 
-/****************************/
-/* Flow Handler Definitions */
-/****************************/
-
-/**
- * This I/O area is directly read/writable by driver (e.g. Linux uses writel())
- * Addresses are offsets from device's PCI hardware base address.
- */
-#define FH_MEM_LOWER_BOUND                   (0x1000)
-#define FH_MEM_UPPER_BOUND                   (0x1EF0)
-
-/**
- * Keep-Warm (KW) buffer base address.
- *
- * Driver must allocate a 4KByte buffer that is used by 4965 for keeping the
- * host DRAM powered on (via dummy accesses to DRAM) to maintain low-latency
- * DRAM access when 4965 is Txing or Rxing.  The dummy accesses prevent host
- * from going into a power-savings mode that would cause higher DRAM latency,
- * and possible data over/under-runs, before all Tx/Rx is complete.
- *
- * Driver loads IWL_FH_KW_MEM_ADDR_REG with the physical address (bits 35:4)
- * of the buffer, which must be 4K aligned.  Once this is set up, the 4965
- * automatically invokes keep-warm accesses when normal accesses might not
- * be sufficient to maintain fast DRAM response.
- *
- * Bit fields:
- *  31-0:  Keep-warm buffer physical base address [35:4], must be 4K aligned
- */
-#define IWL_FH_KW_MEM_ADDR_REG		     (FH_MEM_LOWER_BOUND + 0x97C)
-
-
-/**
- * TFD Circular Buffers Base (CBBC) addresses
- *
- * 4965 has 16 base pointer registers, one for each of 16 host-DRAM-resident
- * circular buffers (CBs/queues) containing Transmit Frame Descriptors (TFDs)
- * (see struct iwl_tfd_frame).  These 16 pointer registers are offset by 0x04
- * bytes from one another.  Each TFD circular buffer in DRAM must be 256-byte
- * aligned (address bits 0-7 must be 0).
- *
- * Bit fields in each pointer register:
- *  27-0: TFD CB physical base address [35:8], must be 256-byte aligned
- */
-#define FH_MEM_CBBC_LOWER_BOUND              (FH_MEM_LOWER_BOUND + 0x9D0)
-#define FH_MEM_CBBC_UPPER_BOUND              (FH_MEM_LOWER_BOUND + 0xA10)
-
-/* Find TFD CB base pointer for given queue (range 0-15). */
-#define FH_MEM_CBBC_QUEUE(x)  (FH_MEM_CBBC_LOWER_BOUND + (x) * 0x4)
-
-
-/**
- * Rx SRAM Control and Status Registers (RSCSR)
- *
- * These registers provide handshake between driver and 4965 for the Rx queue
- * (this queue handles *all* command responses, notifications, Rx data, etc.
- * sent from 4965 uCode to host driver).  Unlike Tx, there is only one Rx
- * queue, and only one Rx DMA/FIFO channel.  Also unlike Tx, which can
- * concatenate up to 20 DRAM buffers to form a Tx frame, each Receive Buffer
- * Descriptor (RBD) points to only one Rx Buffer (RB); there is a 1:1
- * mapping between RBDs and RBs.
- *
- * Driver must allocate host DRAM memory for the following, and set the
- * physical address of each into 4965 registers:
- *
- * 1)  Receive Buffer Descriptor (RBD) circular buffer (CB), typically with 256
- *     entries (although any power of 2, up to 4096, is selectable by driver).
- *     Each entry (1 dword) points to a receive buffer (RB) of consistent size
- *     (typically 4K, although 8K or 16K are also selectable by driver).
- *     Driver sets up RB size and number of RBDs in the CB via Rx config
- *     register FH_MEM_RCSR_CHNL0_CONFIG_REG.
- *
- *     Bit fields within one RBD:
- *     27-0:  Receive Buffer physical address bits [35:8], 256-byte aligned
- *
- *     Driver sets physical address [35:8] of base of RBD circular buffer
- *     into FH_RSCSR_CHNL0_RBDCB_BASE_REG [27:0].
- *
- * 2)  Rx status buffer, 8 bytes, in which 4965 indicates which Rx Buffers
- *     (RBs) have been filled, via a "write pointer", actually the index of
- *     the RB's corresponding RBD within the circular buffer.  Driver sets
- *     physical address [35:4] into FH_RSCSR_CHNL0_STTS_WPTR_REG [31:0].
- *
- *     Bit fields in lower dword of Rx status buffer (upper dword not used
- *     by driver; see struct iwl4965_shared, val0):
- *     31-12:  Not used by driver
- *     11- 0:  Index of last filled Rx buffer descriptor
- *             (4965 writes, driver reads this value)
- *
- * As the driver prepares Receive Buffers (RBs) for 4965 to fill, driver must
- * enter pointers to these RBs into contiguous RBD circular buffer entries,
- * and update the 4965's "write" index register, FH_RSCSR_CHNL0_RBDCB_WPTR_REG.
- *
- * This "write" index corresponds to the *next* RBD that the driver will make
- * available, i.e. one RBD past the tail of the ready-to-fill RBDs within
- * the circular buffer.  This value should initially be 0 (before preparing any
- * RBs), should be 8 after preparing the first 8 RBs (for example), and must
- * wrap back to 0 at the end of the circular buffer (but don't wrap before
- * "read" index has advanced past 1!  See below).
- * NOTE:  4965 EXPECTS THE WRITE INDEX TO BE INCREMENTED IN MULTIPLES OF 8.
- *
- * As the 4965 fills RBs (referenced from contiguous RBDs within the circular
- * buffer), it updates the Rx status buffer in host DRAM, 2) described above,
- * to tell the driver the index of the latest filled RBD.  The driver must
- * read this "read" index from DRAM after receiving an Rx interrupt from 4965.
- *
- * The driver must also internally keep track of a third index, which is the
- * next RBD to process.  When receiving an Rx interrupt, driver should process
- * all filled but unprocessed RBs up to, but not including, the RB
- * corresponding to the "read" index.  For example, if "read" index becomes "1",
- * driver may process the RB pointed to by RBD 0.  Depending on volume of
- * traffic, there may be many RBs to process.
- *
- * If read index == write index, 4965 thinks there is no room to put new data.
- * Due to this, the maximum number of filled RBs is 255, instead of 256.  To
- * be safe, make sure that there is a gap of at least 2 RBDs between "write"
- * and "read" indexes; that is, make sure that there are no more than 254
- * buffers waiting to be filled.
- */
-#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)
-
-/**
- * Physical base address of 8-byte Rx Status buffer.
- * Bit fields:
- *  31-0: Rx status buffer physical base address [35:4], must 16-byte aligned.
- */
-#define FH_RSCSR_CHNL0_STTS_WPTR_REG		(FH_MEM_RSCSR_CHNL0)
-
-/**
- * Physical base address of Rx Buffer Descriptor Circular Buffer.
- * Bit fields:
- *  27-0:  RBD CD physical base address [35:8], must be 256-byte aligned.
- */
-#define FH_RSCSR_CHNL0_RBDCB_BASE_REG		(FH_MEM_RSCSR_CHNL0 + 0x004)
-
-/**
- * Rx write pointer (index, really!).
- * Bit fields:
- *  11-0:  Index of driver's most recent prepared-to-be-filled RBD, + 1.
- *         NOTE:  For 256-entry circular buffer, use only bits [7:0].
- */
-#define FH_RSCSR_CHNL0_RBDCB_WPTR_REG		(FH_MEM_RSCSR_CHNL0 + 0x008)
-#define FH_RSCSR_CHNL0_WPTR        (FH_RSCSR_CHNL0_RBDCB_WPTR_REG)
-
-
-/**
- * Rx Config/Status Registers (RCSR)
- * Rx Config Reg for channel 0 (only channel used)
- *
- * Driver must initialize FH_MEM_RCSR_CHNL0_CONFIG_REG as follows for
- * normal operation (see bit fields).
- *
- * Clearing FH_MEM_RCSR_CHNL0_CONFIG_REG to 0 turns off Rx DMA.
- * Driver should poll FH_MEM_RSSR_RX_STATUS_REG	for
- * FH_RSSR_CHNL0_RX_STATUS_CHNL_IDLE (bit 24) before continuing.
- *
- * Bit fields:
- * 31-30: Rx DMA channel enable: '00' off/pause, '01' pause at end of frame,
- *        '10' operate normally
- * 29-24: reserved
- * 23-20: # RBDs in circular buffer = 2^value; use "8" for 256 RBDs (normal),
- *        min "5" for 32 RBDs, max "12" for 4096 RBDs.
- * 19-18: reserved
- * 17-16: size of each receive buffer; '00' 4K (normal), '01' 8K,
- *        '10' 12K, '11' 16K.
- * 15-14: reserved
- * 13-12: IRQ destination; '00' none, '01' host driver (normal operation)
- * 11- 4: timeout for closing Rx buffer and interrupting host (units 32 usec)
- *        typical value 0x10 (about 1/2 msec)
- *  3- 0: reserved
- */
-#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)
-
-#define FH_RCSR_CHNL0_RX_CONFIG_RB_TIMEOUT_MASK   (0x00000FF0) /* bit 4-11 */
-#define FH_RCSR_CHNL0_RX_CONFIG_IRQ_DEST_MASK     (0x00001000) /* bit 12 */
-#define FH_RCSR_CHNL0_RX_CONFIG_SINGLE_FRAME_MASK (0x00008000) /* bit 15 */
-#define FH_RCSR_CHNL0_RX_CONFIG_RB_SIZE_MASK	  (0x00030000) /* bits 16-17 */
-#define FH_RCSR_CHNL0_RX_CONFIG_RBDBC_SIZE_MASK   (0x00F00000) /* bits 20-23 */
-#define FH_RCSR_CHNL0_RX_CONFIG_DMA_CHNL_EN_MASK  (0xC0000000) /* bits 30-31 */
-
-#define FH_RCSR_RX_CONFIG_RBDCB_SIZE_BITSHIFT	(20)
-#define FH_RCSR_RX_CONFIG_REG_IRQ_RBTH_BITSHIFT	(4)
-#define RX_RB_TIMEOUT	(0x10)
-
-#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 FH_RCSR_RX_CONFIG_REG_VAL_RB_SIZE_4K    (0x00000000)
-#define FH_RCSR_RX_CONFIG_REG_VAL_RB_SIZE_8K    (0x00010000)
-#define FH_RCSR_RX_CONFIG_REG_VAL_RB_SIZE_12K   (0x00020000)
-#define FH_RCSR_RX_CONFIG_REG_VAL_RB_SIZE_16K   (0x00030000)
-
-#define FH_RCSR_CHNL0_RX_CONFIG_IRQ_DEST_NO_INT_VAL       (0x00000000)
-#define FH_RCSR_CHNL0_RX_CONFIG_IRQ_DEST_INT_HOST_VAL     (0x00001000)
-
-
-/**
- * Rx Shared Status Registers (RSSR)
- *
- * After stopping Rx DMA channel (writing 0 to FH_MEM_RCSR_CHNL0_CONFIG_REG),
- * driver must poll FH_MEM_RSSR_RX_STATUS_REG until Rx channel is idle.
- *
- * Bit fields:
- *  24:  1 = Channel 0 is idle
- *
- * FH_MEM_RSSR_SHARED_CTRL_REG and FH_MEM_RSSR_RX_ENABLE_ERR_IRQ2DRV contain
- * default values that should not be altered by the driver.
- */
-#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)
-
-#define FH_RSSR_CHNL0_RX_STATUS_CHNL_IDLE	(0x01000000)
-
-
-/**
- * Transmit DMA Channel Control/Status Registers (TCSR)
- *
- * 4965 has one configuration register for each of 8 Tx DMA/FIFO channels
- * supported in hardware (don't confuse these with the 16 Tx queues in DRAM,
- * which feed the DMA/FIFO channels); config regs are separated by 0x20 bytes.
- *
- * To use a Tx DMA channel, driver must initialize its
- * IWL_FH_TCSR_CHNL_TX_CONFIG_REG(chnl) with:
- *
- * IWL_FH_TCSR_TX_CONFIG_REG_VAL_DMA_CHNL_ENABLE |
- * IWL_FH_TCSR_TX_CONFIG_REG_VAL_DMA_CREDIT_ENABLE_VAL
- *
- * All other bits should be 0.
- *
- * Bit fields:
- * 31-30: Tx DMA channel enable: '00' off/pause, '01' pause at end of frame,
- *        '10' operate normally
- * 29- 4: Reserved, set to "0"
- *     3: Enable internal DMA requests (1, normal operation), disable (0)
- *  2- 0: Reserved, set to "0"
- */
-#define IWL_FH_TCSR_LOWER_BOUND  (FH_MEM_LOWER_BOUND + 0xD00)
-#define IWL_FH_TCSR_UPPER_BOUND  (FH_MEM_LOWER_BOUND + 0xE60)
-
-/* Find Control/Status reg for given Tx DMA/FIFO channel */
-#define IWL_FH_TCSR_CHNL_TX_CONFIG_REG(_chnl) \
-	(IWL_FH_TCSR_LOWER_BOUND + 0x20 * _chnl)
-
-#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_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)
-
-/**
- * Tx Shared Status Registers (TSSR)
- *
- * After stopping Tx DMA channel (writing 0 to
- * IWL_FH_TCSR_CHNL_TX_CONFIG_REG(chnl)), driver must poll
- * IWL_FH_TSSR_TX_STATUS_REG until selected Tx channel is idle
- * (channel's buffers empty | no pending requests).
- *
- * Bit fields:
- * 31-24:  1 = Channel buffers empty (channel 7:0)
- * 23-16:  1 = No pending requests (channel 7:0)
- */
-#define IWL_FH_TSSR_LOWER_BOUND		(FH_MEM_LOWER_BOUND + 0xEA0)
-#define IWL_FH_TSSR_UPPER_BOUND		(FH_MEM_LOWER_BOUND + 0xEC0)
-
-#define IWL_FH_TSSR_TX_STATUS_REG	(IWL_FH_TSSR_LOWER_BOUND + 0x010)
-
-#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))
-
-
-/********************* START TX SCHEDULER *************************************/
-
-/**
- * 4965 Tx Scheduler
- *
- * The Tx Scheduler selects the next frame to be transmitted, chosing TFDs
- * (Transmit Frame Descriptors) from up to 16 circular Tx queues resident in
- * host DRAM.  It steers each frame's Tx command (which contains the frame
- * data) into one of up to 7 prioritized Tx DMA FIFO channels within the
- * device.  A queue maps to only one (selectable by driver) Tx DMA channel,
- * but one DMA channel may take input from several queues.
- *
- * Tx DMA channels have dedicated purposes.  For 4965, they are used as follows:
- *
- * 0 -- EDCA BK (background) frames, lowest priority
- * 1 -- EDCA BE (best effort) frames, normal priority
- * 2 -- EDCA VI (video) frames, higher priority
- * 3 -- EDCA VO (voice) and management frames, highest priority
- * 4 -- Commands (e.g. RXON, etc.)
- * 5 -- HCCA short frames
- * 6 -- HCCA long frames
- * 7 -- not used by driver (device-internal only)
- *
- * Driver should normally map queues 0-6 to Tx DMA/FIFO channels 0-6.
- * In addition, driver can map queues 7-15 to Tx DMA/FIFO channels 0-3 to
- * support 11n aggregation via EDCA DMA channels.
- *
- * The driver sets up each queue to work in one of two modes:
- *
- * 1)  Scheduler-Ack, in which the scheduler automatically supports a
- *     block-ack (BA) window of up to 64 TFDs.  In this mode, each queue
- *     contains TFDs for a unique combination of Recipient Address (RA)
- *     and Traffic Identifier (TID), that is, traffic of a given
- *     Quality-Of-Service (QOS) priority, destined for a single station.
- *
- *     In scheduler-ack mode, the scheduler keeps track of the Tx status of
- *     each frame within the BA window, including whether it's been transmitted,
- *     and whether it's been acknowledged by the receiving station.  The device
- *     automatically processes block-acks received from the receiving STA,
- *     and reschedules un-acked frames to be retransmitted (successful
- *     Tx completion may end up being out-of-order).
- *
- *     The driver must maintain the queue's Byte Count table in host DRAM
- *     (struct iwl4965_sched_queue_byte_cnt_tbl) for this mode.
- *     This mode does not support fragmentation.
- *
- * 2)  FIFO (a.k.a. non-Scheduler-ACK), in which each TFD is processed in order.
- *     The device may automatically retry Tx, but will retry only one frame
- *     at a time, until receiving ACK from receiving station, or reaching
- *     retry limit and giving up.
- *
- *     The command queue (#4) must use this mode!
- *     This mode does not require use of the Byte Count table in host DRAM.
- *
- * Driver controls scheduler operation via 3 means:
- * 1)  Scheduler registers
- * 2)  Shared scheduler data base in internal 4956 SRAM
- * 3)  Shared data in host DRAM
- *
- * Initialization:
- *
- * When loading, driver should allocate memory for:
- * 1)  16 TFD circular buffers, each with space for (typically) 256 TFDs.
- * 2)  16 Byte Count circular buffers in 16 KBytes contiguous memory
- *     (1024 bytes for each queue).
- *
- * After receiving "Alive" response from uCode, driver must initialize
- * the scheduler (especially for queue #4, the command queue, otherwise
- * the driver can't issue commands!):
- */
-
-/**
- * Max Tx window size is the max number of contiguous TFDs that the scheduler
- * can keep track of at one time when creating block-ack chains of frames.
- * Note that "64" matches the number of ack bits in a block-ack packet.
- * Driver should use SCD_WIN_SIZE and SCD_FRAME_LIMIT values to initialize
- * SCD_CONTEXT_QUEUE_OFFSET(x) values.
- */
-#define SCD_WIN_SIZE				64
-#define SCD_FRAME_LIMIT				64
-
-/* SCD registers are internal, must be accessed via HBUS_TARG_PRPH regs */
-#define SCD_START_OFFSET		0xa02c00
-
-/*
- * 4965 tells driver SRAM address for internal scheduler structs via this reg.
- * Value is valid only after "Alive" response from uCode.
- */
-#define SCD_SRAM_BASE_ADDR           (SCD_START_OFFSET + 0x0)
-
-/*
- * Driver may need to update queue-empty bits after changing queue's
- * write and read pointers (indexes) during (re-)initialization (i.e. when
- * scheduler is not tracking what's happening).
- * Bit fields:
- * 31-16:  Write mask -- 1: update empty bit, 0: don't change empty bit
- * 15-00:  Empty state, one for each queue -- 1: empty, 0: non-empty
- * NOTE:  This register is not used by Linux driver.
- */
-#define SCD_EMPTY_BITS               (SCD_START_OFFSET + 0x4)
-
-/*
- * Physical base address of array of byte count (BC) circular buffers (CBs).
- * Each Tx queue has a BC CB in host DRAM to support Scheduler-ACK mode.
- * This register points to BC CB for queue 0, must be on 1024-byte boundary.
- * Others are spaced by 1024 bytes.
- * Each BC CB is 2 bytes * (256 + 64) = 740 bytes, followed by 384 bytes pad.
- * (Index into a queue's BC CB) = (index into queue's TFD CB) = (SSN & 0xff).
- * Bit fields:
- * 25-00:  Byte Count CB physical address [35:10], must be 1024-byte aligned.
- */
-#define SCD_DRAM_BASE_ADDR           (SCD_START_OFFSET + 0x10)
-
-/*
- * Enables any/all Tx DMA/FIFO channels.
- * Scheduler generates requests for only the active channels.
- * Set this to 0xff to enable all 8 channels (normal usage).
- * Bit fields:
- *  7- 0:  Enable (1), disable (0), one bit for each channel 0-7
- */
-#define SCD_TXFACT                   (SCD_START_OFFSET + 0x1c)
-
-/* Mask to enable contiguous Tx DMA/FIFO channels between "lo" and "hi". */
-#define SCD_TXFACT_REG_TXFIFO_MASK(lo, hi) \
-       ((1 << (hi)) | ((1 << (hi)) - (1 << (lo))))
-
-/*
- * Queue (x) Write Pointers (indexes, really!), one for each Tx queue.
- * Initialized and updated by driver as new TFDs are added to queue.
- * NOTE:  If using Block Ack, index must correspond to frame's
- *        Start Sequence Number; index = (SSN & 0xff)
- * NOTE:  Alternative to HBUS_TARG_WRPTR, which is what Linux driver uses?
- */
-#define SCD_QUEUE_WRPTR(x)           (SCD_START_OFFSET + 0x24 + (x) * 4)
-
-/*
- * Queue (x) Read Pointers (indexes, really!), one for each Tx queue.
- * For FIFO mode, index indicates next frame to transmit.
- * For Scheduler-ACK mode, index indicates first frame in Tx window.
- * Initialized by driver, updated by scheduler.
- */
-#define SCD_QUEUE_RDPTR(x)           (SCD_START_OFFSET + 0x64 + (x) * 4)
-
-/*
- * Select which queues work in chain mode (1) vs. not (0).
- * Use chain mode to build chains of aggregated frames.
- * Bit fields:
- * 31-16:  Reserved
- * 15-00:  Mode, one bit for each queue -- 1: Chain mode, 0: one-at-a-time
- * NOTE:  If driver sets up queue for chain mode, it should be also set up
- *        Scheduler-ACK mode as well, via SCD_QUEUE_STATUS_BITS(x).
- */
-#define SCD_QUEUECHAIN_SEL           (SCD_START_OFFSET + 0xd0)
-
-/*
- * Select which queues interrupt driver when scheduler increments
- * a queue's read pointer (index).
- * Bit fields:
- * 31-16:  Reserved
- * 15-00:  Interrupt enable, one bit for each queue -- 1: enabled, 0: disabled
- * NOTE:  This functionality is apparently a no-op; driver relies on interrupts
- *        from Rx queue to read Tx command responses and update Tx queues.
- */
-#define SCD_INTERRUPT_MASK           (SCD_START_OFFSET + 0xe4)
-
-/*
- * Queue search status registers.  One for each queue.
- * Sets up queue mode and assigns queue to Tx DMA channel.
- * Bit fields:
- * 19-10: Write mask/enable bits for bits 0-9
- *     9: Driver should init to "0"
- *     8: Scheduler-ACK mode (1), non-Scheduler-ACK (i.e. FIFO) mode (0).
- *        Driver should init to "1" for aggregation mode, or "0" otherwise.
- *   7-6: Driver should init to "0"
- *     5: Window Size Left; indicates whether scheduler can request
- *        another TFD, based on window size, etc.  Driver should init
- *        this bit to "1" for aggregation mode, or "0" for non-agg.
- *   4-1: Tx FIFO to use (range 0-7).
- *     0: Queue is active (1), not active (0).
- * Other bits should be written as "0"
- *
- * NOTE:  If enabling Scheduler-ACK mode, chain mode should also be enabled
- *        via SCD_QUEUECHAIN_SEL.
- */
-#define SCD_QUEUE_STATUS_BITS(x)     (SCD_START_OFFSET + 0x104 + (x) * 4)
-
-/* Bit field positions */
-#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)
-
-/* Write masks */
-#define SCD_QUEUE_STTS_REG_POS_SCD_ACT_EN	(10)
-#define SCD_QUEUE_STTS_REG_MSK			(0x0007FC00)
-
-/**
- * 4965 internal SRAM structures for scheduler, shared with driver ...
- *
- * Driver should clear and initialize the following areas after receiving
- * "Alive" response from 4965 uCode, i.e. after initial
- * uCode load, or after a uCode load done for error recovery:
- *
- * SCD_CONTEXT_DATA_OFFSET (size 128 bytes)
- * SCD_TX_STTS_BITMAP_OFFSET (size 256 bytes)
- * SCD_TRANSLATE_TBL_OFFSET (size 32 bytes)
- *
- * Driver accesses SRAM via HBUS_TARG_MEM_* registers.
- * Driver reads base address of this scheduler area from SCD_SRAM_BASE_ADDR.
- * All OFFSET values must be added to this base address.
- */
-
-/*
- * Queue context.  One 8-byte entry for each of 16 queues.
- *
- * Driver should clear this entire area (size 0x80) to 0 after receiving
- * "Alive" notification from uCode.  Additionally, driver should init
- * each queue's entry as follows:
- *
- * LS Dword bit fields:
- *  0-06:  Max Tx window size for Scheduler-ACK.  Driver should init to 64.
- *
- * MS Dword bit fields:
- * 16-22:  Frame limit.  Driver should init to 10 (0xa).
- *
- * Driver should init all other bits to 0.
- *
- * Init must be done after driver receives "Alive" response from 4965 uCode,
- * and when setting up queue for aggregation.
- */
-#define SCD_CONTEXT_DATA_OFFSET			0x380
-#define SCD_CONTEXT_QUEUE_OFFSET(x)	(SCD_CONTEXT_DATA_OFFSET + ((x) * 8))
-
-#define SCD_QUEUE_CTX_REG1_WIN_SIZE_POS		(0)
-#define SCD_QUEUE_CTX_REG1_WIN_SIZE_MSK		(0x0000007F)
-#define SCD_QUEUE_CTX_REG2_FRAME_LIMIT_POS	(16)
-#define SCD_QUEUE_CTX_REG2_FRAME_LIMIT_MSK	(0x007F0000)
-
-/*
- * Tx Status Bitmap
- *
- * Driver should clear this entire area (size 0x100) to 0 after receiving
- * "Alive" notification from uCode.  Area is used only by device itself;
- * no other support (besides clearing) is required from driver.
- */
-#define SCD_TX_STTS_BITMAP_OFFSET		0x400
-
-/*
- * RAxTID to queue translation mapping.
- *
- * When queue is in Scheduler-ACK mode, frames placed in a that queue must be
- * for only one combination of receiver address (RA) and traffic ID (TID), i.e.
- * one QOS priority level destined for one station (for this wireless link,
- * not final destination).  The SCD_TRANSLATE_TABLE area provides 16 16-bit
- * mappings, one for each of the 16 queues.  If queue is not in Scheduler-ACK
- * mode, the device ignores the mapping value.
- *
- * Bit fields, for each 16-bit map:
- * 15-9:  Reserved, set to 0
- *  8-4:  Index into device's station table for recipient station
- *  3-0:  Traffic ID (tid), range 0-15
- *
- * Driver should clear this entire area (size 32 bytes) to 0 after receiving
- * "Alive" notification from uCode.  To update a 16-bit map value, driver
- * must read a dword-aligned value from device SRAM, replace the 16-bit map
- * value of interest, and write the dword value back into device SRAM.
- */
-#define SCD_TRANSLATE_TBL_OFFSET		0x500
-
-/* Find translation table dword to read/write for given queue */
-#define SCD_TRANSLATE_TBL_OFFSET_QUEUE(x) \
-	((SCD_TRANSLATE_TBL_OFFSET + ((x) * 2)) & 0xfffffffc)
-
-#define SCD_TXFIFO_POS_TID			(0)
-#define SCD_TXFIFO_POS_RA			(4)
-#define SCD_QUEUE_RA_TID_MAP_RATID_MSK		(0x01FF)
-
-/*********************** END TX SCHEDULER *************************************/
-
-static inline u8 iwl4965_hw_get_rate(__le32 rate_n_flags)
-{
-	return le32_to_cpu(rate_n_flags) & 0xFF;
-}
-static inline u16 iwl4965_hw_get_rate_n_flags(__le32 rate_n_flags)
-{
-	return le32_to_cpu(rate_n_flags) & 0xFFFF;
-}
-static inline __le32 iwl4965_hw_set_rate_n_flags(u8 rate, u16 flags)
-{
-	return cpu_to_le32(flags|(u16)rate);
-}
-
 
 /**
  * Tx/Rx Queues
@@ -1385,14 +814,15 @@ static inline __le32 iwl4965_hw_set_rate_n_flags(u8 rate, u16 flags)
  * up to 7 DMA channels (FIFOs).  Each Tx queue is supported by a circular array
  * in DRAM containing 256 Transmit Frame Descriptors (TFDs).
  */
-#define IWL4965_MAX_WIN_SIZE	64
-#define IWL4965_QUEUE_SIZE	256
-#define IWL4965_NUM_FIFOS	7
-#define IWL4965_MAX_NUM_QUEUES	16
-
+#define IWL49_MAX_WIN_SIZE	64
+#define IWL49_QUEUE_SIZE	256
+#define IWL49_NUM_FIFOS 	7
+#define IWL49_CMD_FIFO_NUM	4
+#define IWL49_NUM_QUEUES	16
+#define IWL49_NUM_AMPDU_QUEUES	8
 
 /**
- * struct iwl4965_tfd_frame_data
+ * struct iwl_tfd_frame_data
  *
  * Describes up to 2 buffers containing (contiguous) portions of a Tx frame.
  * Each buffer must be on dword boundary.
@@ -1411,7 +841,7 @@ static inline __le32 iwl4965_hw_set_rate_n_flags(u8 rate, u16 flags)
  * 31-20: Tx buffer 2 length (bytes)
  * 19- 0: Tx buffer 2 address bits [35:16]
  */
-struct iwl4965_tfd_frame_data {
+struct iwl_tfd_frame_data {
 	__le32 tb1_addr;
 
 	__le32 val1;
@@ -1441,7 +871,7 @@ struct iwl4965_tfd_frame_data {
 
 
 /**
- * struct iwl4965_tfd_frame
+ * struct iwl_tfd_frame
  *
  * Transmit Frame Descriptor (TFD)
  *
@@ -1468,7 +898,7 @@ struct iwl4965_tfd_frame_data {
  *
  * A maximum of 255 (not 256!) TFDs may be on a queue waiting for Tx.
  */
-struct iwl4965_tfd_frame {
+struct iwl_tfd_frame {
 	__le32 val0;
 	/* __le32 rsvd1:24; */
 	/* __le32 num_tbs:5; */
@@ -1477,7 +907,7 @@ struct iwl4965_tfd_frame {
 #define IWL_num_tbs_SYM val0
 	/* __le32 rsvd2:1; */
 	/* __le32 padding:2; */
-	struct iwl4965_tfd_frame_data pa[10];
+	struct iwl_tfd_frame_data pa[10];
 	__le32 reserved;
 } __attribute__ ((packed));
 
@@ -1520,10 +950,10 @@ struct iwl4965_queue_byte_cnt_entry {
  * 4965 assumes tables are separated by 1024 bytes.
  */
 struct iwl4965_sched_queue_byte_cnt_tbl {
-	struct iwl4965_queue_byte_cnt_entry tfd_offset[IWL4965_QUEUE_SIZE +
-						       IWL4965_MAX_WIN_SIZE];
+	struct iwl4965_queue_byte_cnt_entry tfd_offset[IWL49_QUEUE_SIZE +
+						       IWL49_MAX_WIN_SIZE];
 	u8 dont_care[1024 -
-		     (IWL4965_QUEUE_SIZE + IWL4965_MAX_WIN_SIZE) *
+		     (IWL49_QUEUE_SIZE + IWL49_MAX_WIN_SIZE) *
 		     sizeof(__le16)];
 } __attribute__ ((packed));
 
@@ -1553,7 +983,7 @@ struct iwl4965_sched_queue_byte_cnt_tbl {
  */
 struct iwl4965_shared {
 	struct iwl4965_sched_queue_byte_cnt_tbl
-	 queues_byte_cnt_tbls[IWL4965_MAX_NUM_QUEUES];
+	 queues_byte_cnt_tbls[IWL49_NUM_QUEUES];
 	__le32 rb_closed;
 
 	/* __le32 rb_closed_stts_rb_num:12; */
diff --git a/drivers/net/wireless/iwlwifi/iwl-4965-rs.c b/drivers/net/wireless/iwlwifi/iwl-4965-rs.c
index 3a7f0cb710ec..3ccb84aa5dbc 100644
--- a/drivers/net/wireless/iwlwifi/iwl-4965-rs.c
+++ b/drivers/net/wireless/iwlwifi/iwl-4965-rs.c
@@ -28,7 +28,6 @@
 #include <linux/skbuff.h>
 #include <linux/wireless.h>
 #include <net/mac80211.h>
-#include <net/ieee80211.h>
 
 #include <linux/netdevice.h>
 #include <linux/etherdevice.h>
@@ -38,13 +37,14 @@
 
 #include "../net/mac80211/rate.h"
 
-#include "iwl-4965.h"
+#include "iwl-dev.h"
+#include "iwl-sta.h"
 #include "iwl-core.h"
 #include "iwl-helpers.h"
 
 #define RS_NAME "iwl-4965-rs"
 
-#define NUM_TRY_BEFORE_ANTENNA_TOGGLE 1
+#define NUM_TRY_BEFORE_ANT_TOGGLE 1
 #define IWL_NUMBER_TRY      1
 #define IWL_HT_NUMBER_TRY   3
 
@@ -65,9 +65,16 @@ static u8 rs_ht_to_legacy[] = {
 	IWL_RATE_48M_INDEX, IWL_RATE_54M_INDEX
 };
 
-struct iwl4965_rate {
-	u32 rate_n_flags;
-} __attribute__ ((packed));
+static const u8 ant_toggle_lookup[] = {
+	/*ANT_NONE -> */ ANT_NONE,
+	/*ANT_A    -> */ ANT_B,
+	/*ANT_B    -> */ ANT_C,
+	/*ANT_AB   -> */ ANT_BC,
+	/*ANT_C    -> */ ANT_A,
+	/*ANT_AC   -> */ ANT_AB,
+	/*ANT_BC   -> */ ANT_AC,
+	/*ANT_ABC  -> */ ANT_ABC,
+};
 
 /**
  * struct iwl4965_rate_scale_data -- tx success history for one rate
@@ -88,19 +95,17 @@ struct iwl4965_rate_scale_data {
  * one for "active", and one for "search".
  */
 struct iwl4965_scale_tbl_info {
-	enum iwl4965_table_type lq_type;
-	enum iwl4965_antenna_type antenna_type;
+	enum iwl_table_type lq_type;
+	u8 ant_type;
 	u8 is_SGI;	/* 1 = short guard interval */
 	u8 is_fat;	/* 1 = 40 MHz channel width */
 	u8 is_dup;	/* 1 = duplicated data streams */
 	u8 action;	/* change modulation; IWL_[LEGACY/SISO/MIMO]_SWITCH_* */
 	s32 *expected_tpt;	/* throughput metrics; expected_tpt_G, etc. */
-	struct iwl4965_rate current_rate;  /* rate_n_flags, uCode API format */
+	u32 current_rate;  /* rate_n_flags, uCode API format */
 	struct iwl4965_rate_scale_data win[IWL_RATE_COUNT]; /* rate histories */
 };
 
-#ifdef CONFIG_IWL4965_HT
-
 struct iwl4965_traffic_load {
 	unsigned long time_stamp;	/* age of the oldest statistics */
 	u32 packet_count[TID_QUEUE_MAX_SIZE];   /* packet count in this time
@@ -112,8 +117,6 @@ struct iwl4965_traffic_load {
 	u8 head;			/* start of the circular buffer */
 };
 
-#endif /* CONFIG_IWL4965_HT */
-
 /**
  * struct iwl4965_lq_sta -- driver's rate scaling private structure
  *
@@ -136,8 +139,6 @@ struct iwl4965_lq_sta {
 	u32 flush_timer;	/* time staying in mode before new search */
 
 	u8 action_counter;	/* # mode-switch actions tried */
-	u8 antenna;
-	u8 valid_antenna;
 	u8 is_green;
 	u8 is_dup;
 	enum ieee80211_band band;
@@ -145,24 +146,21 @@ struct iwl4965_lq_sta {
 
 	/* The following are bitmaps of rates; IWL_RATE_6M_MASK, etc. */
 	u32 supp_rates;
-	u16 active_rate;
+	u16 active_legacy_rate;
 	u16 active_siso_rate;
-	u16 active_mimo_rate;
+	u16 active_mimo2_rate;
+	u16 active_mimo3_rate;
 	u16 active_rate_basic;
 
 	struct iwl_link_quality_cmd lq;
 	struct iwl4965_scale_tbl_info lq_info[LQ_SIZE]; /* "active", "search" */
-#ifdef CONFIG_IWL4965_HT
 	struct iwl4965_traffic_load load[TID_MAX_LOAD_COUNT];
 	u8 tx_agg_tid_en;
-#endif
 #ifdef CONFIG_MAC80211_DEBUGFS
 	struct dentry *rs_sta_dbgfs_scale_table_file;
 	struct dentry *rs_sta_dbgfs_stats_table_file;
-#ifdef CONFIG_IWL4965_HT
 	struct dentry *rs_sta_dbgfs_tx_agg_tid_en_file;
-#endif
-	struct iwl4965_rate dbg_fixed;
+	u32 dbg_fixed_rate;
 #endif
 	struct iwl_priv *drv;
 };
@@ -171,17 +169,17 @@ 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 iwl4965_lq_sta *lq_sta,
-			     struct iwl4965_rate *tx_mcs,
-			     struct iwl_link_quality_cmd *tbl);
+static void rs_fill_link_cmd(const struct iwl_priv *priv,
+			     struct iwl4965_lq_sta *lq_sta,
+			     u32 rate_n_flags);
 
 
 #ifdef CONFIG_MAC80211_DEBUGFS
 static void rs_dbgfs_set_mcs(struct iwl4965_lq_sta *lq_sta,
-				struct iwl4965_rate *mcs, int index);
+					u32 *rate_n_flags, int index);
 #else
 static void rs_dbgfs_set_mcs(struct iwl4965_lq_sta *lq_sta,
-				struct iwl4965_rate *mcs, int index)
+					u32 *rate_n_flags, int index)
 {}
 #endif
 
@@ -190,6 +188,7 @@ static void rs_dbgfs_set_mcs(struct iwl4965_lq_sta *lq_sta,
  * 1, 2, 5.5, 11, 6, 9, 12, 18, 24, 36, 48, 54, 60 MBits
  * "G" is the only table that supports CCK (the first 4 rates).
  */
+/*FIXME:RS:need to spearate tables for MIMO2/MIMO3*/
 static s32 expected_tpt_A[IWL_RATE_COUNT] = {
 	0, 0, 0, 0, 40, 57, 72, 98, 121, 154, 177, 186, 186
 };
@@ -230,7 +229,7 @@ static s32 expected_tpt_mimo40MHzSGI[IWL_RATE_COUNT] = {
 	0, 0, 0, 0, 131, 131, 191, 222, 242, 270, 284, 289, 293
 };
 
-static inline u8 iwl4965_rate_get_rate(u32 rate_n_flags)
+static inline u8 rs_extract_rate(u32 rate_n_flags)
 {
 	return (u8)(rate_n_flags & 0xFF);
 }
@@ -245,7 +244,11 @@ static void rs_rate_scale_clear_window(struct iwl4965_rate_scale_data *window)
 	window->stamp = 0;
 }
 
-#ifdef CONFIG_IWL4965_HT
+static inline u8 rs_is_valid_ant(u8 valid_antenna, u8 ant_type)
+{
+	return ((ant_type & valid_antenna) == ant_type);
+}
+
 /*
  *	removes the old data from the statistics. All data that is older than
  *	TID_MAX_TIME_DIFF, will be deleted.
@@ -271,15 +274,21 @@ static void rs_tl_rm_old_stats(struct iwl4965_traffic_load *tl, u32 curr_time)
  *	increment traffic load value for tid and also remove
  *	any old values if passed the certain time period
  */
-static void rs_tl_add_packet(struct iwl4965_lq_sta *lq_data, u8 tid)
+static u8 rs_tl_add_packet(struct iwl4965_lq_sta *lq_data,
+			   struct ieee80211_hdr *hdr)
 {
 	u32 curr_time = jiffies_to_msecs(jiffies);
 	u32 time_diff;
 	s32 index;
 	struct iwl4965_traffic_load *tl = NULL;
+	__le16 fc = hdr->frame_control;
+	u8 tid;
 
-	if (tid >= TID_MAX_LOAD_COUNT)
-		return;
+	if (ieee80211_is_data_qos(fc)) {
+		u8 *qc = ieee80211_get_qos_ctl(hdr);
+		tid = qc[0] & 0xf;
+	} else
+		return MAX_TID_COUNT;
 
 	tl = &lq_data->load[tid];
 
@@ -292,7 +301,7 @@ static void rs_tl_add_packet(struct iwl4965_lq_sta *lq_data, u8 tid)
 		tl->queue_count = 1;
 		tl->head = 0;
 		tl->packet_count[0] = 1;
-		return;
+		return MAX_TID_COUNT;
 	}
 
 	time_diff = TIME_WRAP_AROUND(tl->time_stamp, curr_time);
@@ -309,6 +318,8 @@ static void rs_tl_add_packet(struct iwl4965_lq_sta *lq_data, u8 tid)
 
 	if ((index + 1) > tl->queue_count)
 		tl->queue_count = index + 1;
+
+	return tid;
 }
 
 /*
@@ -349,9 +360,9 @@ static void rs_tl_turn_on_agg_for_tid(struct iwl_priv *priv,
 	unsigned long state;
 	DECLARE_MAC_BUF(mac);
 
-	spin_lock_bh(&sta->ampdu_mlme.ampdu_tx);
+	spin_lock_bh(&sta->lock);
 	state = sta->ampdu_mlme.tid_state_tx[tid];
-	spin_unlock_bh(&sta->ampdu_mlme.ampdu_tx);
+	spin_unlock_bh(&sta->lock);
 
 	if (state == HT_AGG_STATE_IDLE &&
 	    rs_tl_get_load(lq_data, tid) > IWL_AGG_LOAD_THRESHOLD) {
@@ -372,7 +383,12 @@ static void rs_tl_turn_on_agg(struct iwl_priv *priv, u8 tid,
 			rs_tl_turn_on_agg_for_tid(priv, lq_data, tid, sta);
 }
 
-#endif /* CONFIG_IWLWIFI_HT */
+static inline int get_num_of_ant_from_rate(u32 rate_n_flags)
+{
+	return (!!(rate_n_flags & RATE_MCS_ANT_A_MSK) +
+		!!(rate_n_flags & RATE_MCS_ANT_B_MSK) +
+		!!(rate_n_flags & RATE_MCS_ANT_C_MSK));
+}
 
 /**
  * rs_collect_tx_data - Update the success/failure sliding window
@@ -386,8 +402,7 @@ static int rs_collect_tx_data(struct iwl4965_rate_scale_data *windows,
 			      int successes)
 {
 	struct iwl4965_rate_scale_data *window = NULL;
-	u64 mask;
-	u8 win_size = IWL_RATE_MAX_WINDOW;
+	static const u64 mask = (((u64)1) << (IWL_RATE_MAX_WINDOW - 1));
 	s32 fail_count;
 
 	if (scale_index < 0 || scale_index >= IWL_RATE_COUNT)
@@ -405,14 +420,14 @@ static int rs_collect_tx_data(struct iwl4965_rate_scale_data *windows,
 	 * we keep these bitmaps!).
 	 */
 	while (retries > 0) {
-		if (window->counter >= win_size) {
-			window->counter = win_size - 1;
-			mask = 1;
-			mask = (mask << (win_size - 1));
+		if (window->counter >= IWL_RATE_MAX_WINDOW) {
+
+			/* remove earliest */
+			window->counter = IWL_RATE_MAX_WINDOW - 1;
+
 			if (window->data & mask) {
 				window->data &= ~mask;
-				window->success_counter =
-					window->success_counter - 1;
+				window->success_counter--;
 			}
 		}
 
@@ -422,10 +437,9 @@ static int rs_collect_tx_data(struct iwl4965_rate_scale_data *windows,
 		/* Shift bitmap by one frame (throw away oldest history),
 		 * OR in "1", and increment "success" if this
 		 * frame was successful. */
-		mask = window->data;
-		window->data = (mask << 1);
+		window->data <<= 1;;
 		if (successes > 0) {
-			window->success_counter = window->success_counter + 1;
+			window->success_counter++;
 			window->data |= 0x1;
 			successes--;
 		}
@@ -458,168 +472,162 @@ static int rs_collect_tx_data(struct iwl4965_rate_scale_data *windows,
 /*
  * Fill uCode API rate_n_flags field, based on "search" or "active" table.
  */
-static void rs_mcs_from_tbl(struct iwl4965_rate *mcs_rate,
-			   struct iwl4965_scale_tbl_info *tbl,
-			   int index, u8 use_green)
+/* FIXME:RS:remove this function and put the flags statically in the table */
+static u32 rate_n_flags_from_tbl(struct iwl4965_scale_tbl_info *tbl,
+				       int index, u8 use_green)
 {
+	u32 rate_n_flags = 0;
+
 	if (is_legacy(tbl->lq_type)) {
-		mcs_rate->rate_n_flags = iwl4965_rates[index].plcp;
+		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;
+			rate_n_flags |= RATE_MCS_CCK_MSK;
 
-	} else if (is_siso(tbl->lq_type)) {
-		if (index > IWL_LAST_OFDM_RATE)
+	} else if (is_Ht(tbl->lq_type)) {
+		if (index > IWL_LAST_OFDM_RATE) {
+			IWL_ERROR("invalid HT rate index %d\n", index);
 			index = IWL_LAST_OFDM_RATE;
-		 mcs_rate->rate_n_flags = iwl4965_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 = iwl4965_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;
+		}
+		rate_n_flags = RATE_MCS_HT_MSK;
 
-	if (tbl->is_fat) {
-		if (tbl->is_dup)
-			mcs_rate->rate_n_flags |= RATE_MCS_DUP_MSK;
+		if (is_siso(tbl->lq_type))
+			rate_n_flags |=	iwl_rates[index].plcp_siso;
+		else if (is_mimo2(tbl->lq_type))
+			rate_n_flags |=	iwl_rates[index].plcp_mimo2;
 		else
-			mcs_rate->rate_n_flags |= RATE_MCS_FAT_MSK;
+			rate_n_flags |=	iwl_rates[index].plcp_mimo3;
+	} else {
+		IWL_ERROR("Invalid tbl->lq_type %d\n", tbl->lq_type);
 	}
-	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;
+	rate_n_flags |= ((tbl->ant_type << RATE_MCS_ANT_POS) &
+						     RATE_MCS_ANT_ABC_MSK);
+
+	if (is_Ht(tbl->lq_type)) {
+		if (tbl->is_fat) {
+			if (tbl->is_dup)
+				rate_n_flags |= RATE_MCS_DUP_MSK;
+			else
+				rate_n_flags |= RATE_MCS_FAT_MSK;
+		}
+		if (tbl->is_SGI)
+			rate_n_flags |= RATE_MCS_SGI_MSK;
+
+		if (use_green) {
+			rate_n_flags |= RATE_MCS_GF_MSK;
+			if (is_siso(tbl->lq_type) && tbl->is_SGI) {
+				rate_n_flags &= ~RATE_MCS_SGI_MSK;
+				IWL_ERROR("GF was set with SGI:SISO\n");
+			}
+		}
 	}
+	return rate_n_flags;
 }
 
 /*
  * Interpret uCode API's rate_n_flags format,
  * fill "search" or "active" tx mode table.
  */
-static int rs_get_tbl_info_from_mcs(const struct iwl4965_rate *mcs_rate,
+static int rs_get_tbl_info_from_mcs(const u32 rate_n_flags,
 				    enum ieee80211_band band,
 				    struct iwl4965_scale_tbl_info *tbl,
 				    int *rate_idx)
 {
-	int index;
-	u32 ant_msk;
+	u32 ant_msk = (rate_n_flags & RATE_MCS_ANT_ABC_MSK);
+	u8 num_of_ant = get_num_of_ant_from_rate(rate_n_flags);
+	u8 mcs;
 
-	index = iwl4965_hwrate_to_plcp_idx(mcs_rate->rate_n_flags);
+	*rate_idx = iwl_hwrate_to_plcp_idx(rate_n_flags);
 
-	if (index  == IWL_RATE_INVALID) {
+	if (*rate_idx  == IWL_RATE_INVALID) {
 		*rate_idx = -1;
 		return -EINVAL;
 	}
 	tbl->is_SGI = 0;	/* default legacy setup */
 	tbl->is_fat = 0;
 	tbl->is_dup = 0;
-	tbl->antenna_type = ANT_BOTH;	/* default MIMO setup */
+	tbl->ant_type = (ant_msk >> RATE_MCS_ANT_POS);
+	tbl->lq_type = LQ_NONE;
 
 	/* legacy rate format */
-	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 (!(rate_n_flags & RATE_MCS_HT_MSK)) {
+		if (num_of_ant == 1) {
 			if (band == IEEE80211_BAND_5GHZ)
 				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;
-
-	/* HT rate format, SISO (might be 20 MHz legacy or 40 MHz fat width) */
-	} else if (iwl4965_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;
-
-	/* HT rate format, MIMO (might be 20 MHz legacy or 40 MHz fat width) */
+	/* HT rate format */
 	} else {
-		tbl->lq_type = LQ_MIMO;
-		if (mcs_rate->rate_n_flags & RATE_MCS_SGI_MSK)
+		if (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))
+		if ((rate_n_flags & RATE_MCS_FAT_MSK) ||
+		    (rate_n_flags & RATE_MCS_DUP_MSK))
 			tbl->is_fat = 1;
 
-		if (mcs_rate->rate_n_flags & RATE_MCS_DUP_MSK)
+		if (rate_n_flags & RATE_MCS_DUP_MSK)
 			tbl->is_dup = 1;
-		*rate_idx = index;
+
+		mcs = rs_extract_rate(rate_n_flags);
+
+		/* SISO */
+		if (mcs <= IWL_RATE_SISO_60M_PLCP) {
+			if (num_of_ant == 1)
+				tbl->lq_type = LQ_SISO; /*else NONE*/
+		/* MIMO2 */
+		} else if (mcs <= IWL_RATE_MIMO2_60M_PLCP) {
+			if (num_of_ant == 2)
+				tbl->lq_type = LQ_MIMO2;
+		/* MIMO3 */
+		} else {
+			if (num_of_ant == 3)
+				tbl->lq_type = LQ_MIMO3;
+		}
 	}
 	return 0;
 }
 
-static inline void rs_toggle_antenna(struct iwl4965_rate *new_rate,
-				     struct iwl4965_scale_tbl_info *tbl)
+/* switch to another antenna/antennas and return 1 */
+/* if no other valid antenna found, return 0 */
+static int rs_toggle_antenna(u32 valid_ant, u32 *rate_n_flags,
+			      struct iwl4965_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;
-	}
+	u8 new_ant_type;
+
+	if (!tbl->ant_type || tbl->ant_type > ANT_ABC)
+		return 0;
+
+	if (!rs_is_valid_ant(valid_ant, tbl->ant_type))
+		return 0;
+
+	new_ant_type = ant_toggle_lookup[tbl->ant_type];
+
+	while ((new_ant_type != tbl->ant_type) &&
+	       !rs_is_valid_ant(valid_ant, new_ant_type))
+		new_ant_type = ant_toggle_lookup[new_ant_type];
+
+	if (new_ant_type == tbl->ant_type)
+		return 0;
+
+	tbl->ant_type = new_ant_type;
+	*rate_n_flags &= ~RATE_MCS_ANT_ABC_MSK;
+	*rate_n_flags |= new_ant_type << RATE_MCS_ANT_POS;
+	return 1;
 }
 
-static inline u8 rs_use_green(struct iwl_priv *priv,
-			      struct ieee80211_conf *conf)
+/* FIXME:RS: in 4965 we don't use greenfield at all */
+/* FIXME:RS: don't use greenfield for now in TX */
+#if 0
+static inline u8 rs_use_green(struct iwl_priv *priv, struct ieee80211_conf *conf)
 {
-#ifdef CONFIG_IWL4965_HT
 	return ((conf->flags & IEEE80211_CONF_SUPPORT_HT_MODE) &&
 		priv->current_ht_config.is_green_field &&
 		!priv->current_ht_config.non_GF_STA_present);
-#endif	/* CONFIG_IWL4965_HT */
+}
+#endif
+static inline u8 rs_use_green(struct iwl_priv *priv, struct ieee80211_conf *conf)
+{
 	return 0;
 }
 
@@ -630,27 +638,28 @@ static inline u8 rs_use_green(struct iwl_priv *priv,
  * basic available rates.
  *
  */
-static void rs_get_supported_rates(struct iwl4965_lq_sta *lq_sta,
+static u16 rs_get_supported_rates(struct iwl4965_lq_sta *lq_sta,
 				   struct ieee80211_hdr *hdr,
-				   enum iwl4965_table_type rate_type,
-				   u16 *data_rate)
+				   enum iwl_table_type rate_type)
 {
-	if (is_legacy(rate_type))
-		*data_rate = lq_sta->active_rate;
-	else {
+	if (hdr && is_multicast_ether_addr(hdr->addr1) &&
+	    lq_sta->active_rate_basic)
+		return lq_sta->active_rate_basic;
+
+	if (is_legacy(rate_type)) {
+		return lq_sta->active_legacy_rate;
+	} else {
 		if (is_siso(rate_type))
-			*data_rate = lq_sta->active_siso_rate;
+			return lq_sta->active_siso_rate;
+		else if (is_mimo2(rate_type))
+			return lq_sta->active_mimo2_rate;
 		else
-			*data_rate = lq_sta->active_mimo_rate;
-	}
-
-	if (hdr && is_multicast_ether_addr(hdr->addr1) &&
-	    lq_sta->active_rate_basic) {
-		*data_rate = lq_sta->active_rate_basic;
+			return lq_sta->active_mimo3_rate;
 	}
 }
 
-static u16 rs_get_adjacent_rate(u8 index, u16 rate_mask, int rate_type)
+static u16 rs_get_adjacent_rate(struct iwl_priv *priv, u8 index, u16 rate_mask,
+				int rate_type)
 {
 	u8 high = IWL_RATE_INVALID;
 	u8 low = IWL_RATE_INVALID;
@@ -684,7 +693,7 @@ static u16 rs_get_adjacent_rate(u8 index, u16 rate_mask, int rate_type)
 
 	low = index;
 	while (low != IWL_RATE_INVALID) {
-		low = iwl4965_rates[low].prev_rs;
+		low = iwl_rates[low].prev_rs;
 		if (low == IWL_RATE_INVALID)
 			break;
 		if (rate_mask & (1 << low))
@@ -694,7 +703,7 @@ static u16 rs_get_adjacent_rate(u8 index, u16 rate_mask, int rate_type)
 
 	high = index;
 	while (high != IWL_RATE_INVALID) {
-		high = iwl4965_rates[high].next_rs;
+		high = iwl_rates[high].next_rs;
 		if (high == IWL_RATE_INVALID)
 			break;
 		if (rate_mask & (1 << high))
@@ -705,9 +714,9 @@ static u16 rs_get_adjacent_rate(u8 index, u16 rate_mask, int rate_type)
 	return (high << 8) | low;
 }
 
-static void rs_get_lower_rate(struct iwl4965_lq_sta *lq_sta,
+static u32 rs_get_lower_rate(struct iwl4965_lq_sta *lq_sta,
 			     struct iwl4965_scale_tbl_info *tbl, u8 scale_index,
-			     u8 ht_possible, struct iwl4965_rate *mcs_rate)
+			     u8 ht_possible)
 {
 	s32 low;
 	u16 rate_mask;
@@ -726,15 +735,14 @@ static void rs_get_lower_rate(struct iwl4965_lq_sta *lq_sta,
 		else
 			tbl->lq_type = LQ_G;
 
-		if ((tbl->antenna_type == ANT_BOTH) ||
-		    (tbl->antenna_type == ANT_NONE))
-			tbl->antenna_type = ANT_MAIN;
+		if (num_of_ant(tbl->ant_type) > 1)
+			tbl->ant_type = ANT_A;/*FIXME:RS*/
 
 		tbl->is_fat = 0;
 		tbl->is_SGI = 0;
 	}
 
-	rs_get_supported_rates(lq_sta, NULL, tbl->lq_type, &rate_mask);
+	rate_mask = rs_get_supported_rates(lq_sta, NULL, tbl->lq_type);
 
 	/* Mask with station rate restriction */
 	if (is_legacy(tbl->lq_type)) {
@@ -748,25 +756,26 @@ static void rs_get_lower_rate(struct iwl4965_lq_sta *lq_sta,
 
 	/* If we 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;
+		low = scale_index;
+		goto out;
 	}
 
-	high_low = rs_get_adjacent_rate(scale_index, rate_mask, tbl->lq_type);
+	high_low = rs_get_adjacent_rate(lq_sta->drv, 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);
+	if (low == IWL_RATE_INVALID)
+		low = scale_index;
+
+out:
+	return rate_n_flags_from_tbl(tbl, low, is_green);
 }
 
 /*
  * mac80211 sends us Tx status
  */
 static void rs_tx_status(void *priv_rate, struct net_device *dev,
-			 struct sk_buff *skb,
-			 struct ieee80211_tx_status *tx_resp)
+			 struct sk_buff *skb)
 {
 	int status;
 	u8 retries;
@@ -778,13 +787,14 @@ static void rs_tx_status(void *priv_rate, struct net_device *dev,
 	struct iwl_priv *priv = (struct iwl_priv *)priv_rate;
 	struct ieee80211_local *local = wdev_priv(dev->ieee80211_ptr);
 	struct ieee80211_hw *hw = local_to_hw(local);
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 	struct iwl4965_rate_scale_data *window = NULL;
 	struct iwl4965_rate_scale_data *search_win = NULL;
-	struct iwl4965_rate tx_mcs;
+	u32 tx_rate;
 	struct iwl4965_scale_tbl_info tbl_type;
 	struct iwl4965_scale_tbl_info *curr_tbl, *search_tbl;
 	u8 active_index = 0;
-	u16 fc = le16_to_cpu(hdr->frame_control);
+	__le16 fc = hdr->frame_control;
 	s32 tpt = 0;
 
 	IWL_DEBUG_RATE_LIMIT("get frame ack response, update rate scale window\n");
@@ -793,11 +803,11 @@ static void rs_tx_status(void *priv_rate, struct net_device *dev,
 		return;
 
 	/* This packet was aggregated but doesn't carry rate scale info */
-	if ((tx_resp->control.flags & IEEE80211_TXCTL_AMPDU) &&
-	    !(tx_resp->flags & IEEE80211_TX_STATUS_AMPDU))
+	if ((info->flags & IEEE80211_TX_CTL_AMPDU) &&
+	    !(info->flags & IEEE80211_TX_STAT_AMPDU))
 		return;
 
-	retries = tx_resp->retry_count;
+	retries = info->status.retry_count;
 
 	if (retries > 15)
 		retries = 15;
@@ -812,9 +822,6 @@ static void rs_tx_status(void *priv_rate, struct net_device *dev,
 
 	lq_sta = (struct iwl4965_lq_sta *)sta->rate_ctrl_priv;
 
-	if (!priv->lq_mngr.lq_ready)
-		goto out;
-
 	if ((priv->iw_mode == IEEE80211_IF_TYPE_IBSS) &&
 	    !lq_sta->ibss_sta_added)
 		goto out;
@@ -822,15 +829,6 @@ static void rs_tx_status(void *priv_rate, struct net_device *dev,
 	table = &lq_sta->lq;
 	active_index = lq_sta->active_tbl;
 
-	/* Get mac80211 antenna info */
-	lq_sta->antenna =
-		(lq_sta->valid_antenna & local->hw.conf.antenna_sel_tx);
-	if (!lq_sta->antenna)
-		lq_sta->antenna = lq_sta->valid_antenna;
-
-	/* Ignore mac80211 antenna info for now */
-	lq_sta->antenna = lq_sta->valid_antenna;
-
 	curr_tbl = &(lq_sta->lq_info[active_index]);
 	search_tbl = &(lq_sta->lq_info[(1 - active_index)]);
 	window = (struct iwl4965_rate_scale_data *)
@@ -846,28 +844,26 @@ static void rs_tx_status(void *priv_rate, struct net_device *dev,
 	 * to check "search" mode, or a prior "search" mode after we've moved
 	 * to a new "search" mode (which might become the new "active" mode).
 	 */
-	tx_mcs.rate_n_flags = le32_to_cpu(table->rs_table[0].rate_n_flags);
-	rs_get_tbl_info_from_mcs(&tx_mcs, priv->band, &tbl_type, &rs_index);
+	tx_rate = le32_to_cpu(table->rs_table[0].rate_n_flags);
+	rs_get_tbl_info_from_mcs(tx_rate, priv->band, &tbl_type, &rs_index);
 	if (priv->band == IEEE80211_BAND_5GHZ)
 		rs_index -= IWL_FIRST_OFDM_RATE;
 
-	if ((tx_resp->control.tx_rate == NULL) ||
+	if ((info->tx_rate_idx < 0) ||
 	    (tbl_type.is_SGI ^
-		!!(tx_resp->control.flags & IEEE80211_TXCTL_SHORT_GI)) ||
+		!!(info->flags & IEEE80211_TX_CTL_SHORT_GI)) ||
 	    (tbl_type.is_fat ^
-		!!(tx_resp->control.flags & IEEE80211_TXCTL_40_MHZ_WIDTH)) ||
+		!!(info->flags & IEEE80211_TX_CTL_40_MHZ_WIDTH)) ||
 	    (tbl_type.is_dup ^
-		!!(tx_resp->control.flags & IEEE80211_TXCTL_DUP_DATA)) ||
-	    (tbl_type.antenna_type ^
-		tx_resp->control.antenna_sel_tx) ||
-	    (!!(tx_mcs.rate_n_flags & RATE_MCS_HT_MSK) ^
-		!!(tx_resp->control.flags & IEEE80211_TXCTL_OFDM_HT)) ||
-	    (!!(tx_mcs.rate_n_flags & RATE_MCS_GF_MSK) ^
-		!!(tx_resp->control.flags & IEEE80211_TXCTL_GREEN_FIELD)) ||
+		!!(info->flags & IEEE80211_TX_CTL_DUP_DATA)) ||
+	    (tbl_type.ant_type ^ info->antenna_sel_tx) ||
+	    (!!(tx_rate & RATE_MCS_HT_MSK) ^
+		!!(info->flags & IEEE80211_TX_CTL_OFDM_HT)) ||
+	    (!!(tx_rate & RATE_MCS_GF_MSK) ^
+		!!(info->flags & IEEE80211_TX_CTL_GREEN_FIELD)) ||
 	    (hw->wiphy->bands[priv->band]->bitrates[rs_index].bitrate !=
-		tx_resp->control.tx_rate->bitrate)) {
-		IWL_DEBUG_RATE("initial rate does not match 0x%x\n",
-				tx_mcs.rate_n_flags);
+	     hw->wiphy->bands[info->band]->bitrates[info->tx_rate_idx].bitrate)) {
+		IWL_DEBUG_RATE("initial rate does not match 0x%x\n", tx_rate);
 		goto out;
 	}
 
@@ -875,15 +871,14 @@ static void rs_tx_status(void *priv_rate, struct net_device *dev,
 	while (retries) {
 		/* Look up the rate and other info used for each tx attempt.
 		 * Each tx attempt steps one entry deeper in the rate table. */
-		tx_mcs.rate_n_flags =
-		    le32_to_cpu(table->rs_table[index].rate_n_flags);
-		rs_get_tbl_info_from_mcs(&tx_mcs, priv->band,
+		tx_rate = le32_to_cpu(table->rs_table[index].rate_n_flags);
+		rs_get_tbl_info_from_mcs(tx_rate, priv->band,
 					  &tbl_type, &rs_index);
 
 		/* If type matches "search" table,
 		 * add failure to "search" history */
 		if ((tbl_type.lq_type == search_tbl->lq_type) &&
-		    (tbl_type.antenna_type == search_tbl->antenna_type) &&
+		    (tbl_type.ant_type == search_tbl->ant_type) &&
 		    (tbl_type.is_SGI == search_tbl->is_SGI)) {
 			if (search_tbl->expected_tpt)
 				tpt = search_tbl->expected_tpt[rs_index];
@@ -894,7 +889,7 @@ static void rs_tx_status(void *priv_rate, struct net_device *dev,
 		/* Else if type matches "current/active" table,
 		 * add failure to "current/active" history */
 		} else if ((tbl_type.lq_type == curr_tbl->lq_type) &&
-			   (tbl_type.antenna_type == curr_tbl->antenna_type) &&
+			   (tbl_type.ant_type == curr_tbl->ant_type) &&
 			   (tbl_type.is_SGI == curr_tbl->is_SGI)) {
 			if (curr_tbl->expected_tpt)
 				tpt = curr_tbl->expected_tpt[rs_index];
@@ -917,44 +912,41 @@ static void rs_tx_status(void *priv_rate, struct net_device *dev,
 	 * if Tx was successful first try, use original rate,
 	 * else look up the rate that was, finally, successful.
 	 */
-	tx_mcs.rate_n_flags = le32_to_cpu(table->rs_table[index].rate_n_flags);
-	rs_get_tbl_info_from_mcs(&tx_mcs, priv->band, &tbl_type, &rs_index);
+	tx_rate = le32_to_cpu(table->rs_table[index].rate_n_flags);
+	rs_get_tbl_info_from_mcs(tx_rate, priv->band, &tbl_type, &rs_index);
 
 	/* Update frame history window with "success" if Tx got ACKed ... */
-	if (tx_resp->flags & IEEE80211_TX_STATUS_ACK)
-		status = 1;
-	else
-		status = 0;
+	status = !!(info->flags & IEEE80211_TX_STAT_ACK);
 
 	/* If type matches "search" table,
 	 * add final tx status to "search" history */
 	if ((tbl_type.lq_type == search_tbl->lq_type) &&
-	    (tbl_type.antenna_type == search_tbl->antenna_type) &&
+	    (tbl_type.ant_type == search_tbl->ant_type) &&
 	    (tbl_type.is_SGI == search_tbl->is_SGI)) {
 		if (search_tbl->expected_tpt)
 			tpt = search_tbl->expected_tpt[rs_index];
 		else
 			tpt = 0;
-		if (tx_resp->control.flags & IEEE80211_TXCTL_AMPDU)
+		if (info->flags & IEEE80211_TX_CTL_AMPDU)
 			rs_collect_tx_data(search_win, rs_index, tpt,
-					   tx_resp->ampdu_ack_len,
-					   tx_resp->ampdu_ack_map);
+					   info->status.ampdu_ack_len,
+					   info->status.ampdu_ack_map);
 		else
 			rs_collect_tx_data(search_win, rs_index, tpt,
 					   1, status);
 	/* Else if type matches "current/active" table,
 	 * add final tx status to "current/active" history */
 	} else if ((tbl_type.lq_type == curr_tbl->lq_type) &&
-		   (tbl_type.antenna_type == curr_tbl->antenna_type) &&
+		   (tbl_type.ant_type == curr_tbl->ant_type) &&
 		   (tbl_type.is_SGI == curr_tbl->is_SGI)) {
 		if (curr_tbl->expected_tpt)
 			tpt = curr_tbl->expected_tpt[rs_index];
 		else
 			tpt = 0;
-		if (tx_resp->control.flags & IEEE80211_TXCTL_AMPDU)
+		if (info->flags & IEEE80211_TX_CTL_AMPDU)
 			rs_collect_tx_data(window, rs_index, tpt,
-					   tx_resp->ampdu_ack_len,
-					   tx_resp->ampdu_ack_map);
+					   info->status.ampdu_ack_len,
+					   info->status.ampdu_ack_map);
 		else
 			rs_collect_tx_data(window, rs_index, tpt,
 					   1, status);
@@ -963,10 +955,10 @@ static void rs_tx_status(void *priv_rate, struct net_device *dev,
 	/* If not searching for new mode, increment success/failed counter
 	 * ... these help determine when to start searching again */
 	if (lq_sta->stay_in_tbl) {
-		if (tx_resp->control.flags & IEEE80211_TXCTL_AMPDU) {
-			lq_sta->total_success += tx_resp->ampdu_ack_map;
+		if (info->flags & IEEE80211_TX_CTL_AMPDU) {
+			lq_sta->total_success += info->status.ampdu_ack_map;
 			lq_sta->total_failed +=
-			     (tx_resp->ampdu_ack_len - tx_resp->ampdu_ack_map);
+			     (info->status.ampdu_ack_len - info->status.ampdu_ack_map);
 		} else {
 			if (status)
 				lq_sta->total_success++;
@@ -982,30 +974,6 @@ out:
 	return;
 }
 
-static u8 rs_is_ant_connected(u8 valid_antenna,
-			      enum iwl4965_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)
-		return ((valid_antenna & 0x3) == 0x3);
-
-	return 1;
-}
-
-static u8 rs_is_other_ant_connected(u8 valid_antenna,
-				    enum iwl4965_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;
-}
-
 /*
  * Begin a period of staying with a selected modulation mode.
  * Set "stay_in_tbl" flag to prevent any mode switches.
@@ -1014,10 +982,10 @@ static u8 rs_is_other_ant_connected(u8 valid_antenna,
  * These control how long we stay using same modulation mode before
  * searching for a new mode.
  */
-static void rs_set_stay_in_table(u8 is_legacy,
+static void rs_set_stay_in_table(struct iwl_priv *priv, u8 is_legacy,
 				 struct iwl4965_lq_sta *lq_sta)
 {
-	IWL_DEBUG_HT("we are staying in the same table\n");
+	IWL_DEBUG_RATE("we are staying in the same table\n");
 	lq_sta->stay_in_tbl = 1;	/* only place this gets set */
 	if (is_legacy) {
 		lq_sta->table_count_limit = IWL_LEGACY_TABLE_COUNT;
@@ -1036,7 +1004,7 @@ static void rs_set_stay_in_table(u8 is_legacy,
 /*
  * Find correct throughput table for given mode of modulation
  */
-static void rs_get_expected_tpt_table(struct iwl4965_lq_sta *lq_sta,
+static void rs_set_expected_tpt_table(struct iwl4965_lq_sta *lq_sta,
 				      struct iwl4965_scale_tbl_info *tbl)
 {
 	if (is_legacy(tbl->lq_type)) {
@@ -1055,7 +1023,7 @@ static void rs_get_expected_tpt_table(struct iwl4965_lq_sta *lq_sta,
 		else
 			tbl->expected_tpt = expected_tpt_siso20MHz;
 
-	} else if (is_mimo(tbl->lq_type)) {
+	} else if (is_mimo(tbl->lq_type)) { /* FIXME:need to separate mimo2/3 */
 		if (tbl->is_fat && !lq_sta->is_dup)
 			if (tbl->is_SGI)
 				tbl->expected_tpt = expected_tpt_mimo40MHzSGI;
@@ -1069,7 +1037,6 @@ static void rs_get_expected_tpt_table(struct iwl4965_lq_sta *lq_sta,
 		tbl->expected_tpt = expected_tpt_G;
 }
 
-#ifdef CONFIG_IWL4965_HT
 /*
  * Find starting rate for new "search" high-throughput mode of modulation.
  * Goal is to find lowest expected rate (under perfect conditions) that is
@@ -1085,7 +1052,7 @@ static void rs_get_expected_tpt_table(struct iwl4965_lq_sta *lq_sta,
 static s32 rs_get_best_rate(struct iwl_priv *priv,
 			    struct iwl4965_lq_sta *lq_sta,
 			    struct iwl4965_scale_tbl_info *tbl,	/* "search" */
-			    u16 rate_mask, s8 index, s8 rate)
+			    u16 rate_mask, s8 index)
 {
 	/* "active" values */
 	struct iwl4965_scale_tbl_info *active_tbl =
@@ -1098,11 +1065,13 @@ static s32 rs_get_best_rate(struct iwl_priv *priv,
 
 	s32 new_rate, high, low, start_hi;
 	u16 high_low;
+	s8 rate = index;
 
 	new_rate = high = low = start_hi = IWL_RATE_INVALID;
 
 	for (; ;) {
-		high_low = rs_get_adjacent_rate(rate, rate_mask, tbl->lq_type);
+		high_low = rs_get_adjacent_rate(priv, rate, rate_mask,
+						tbl->lq_type);
 
 		low = high_low & 0xff;
 		high = (high_low >> 8) & 0xff;
@@ -1169,23 +1138,16 @@ static s32 rs_get_best_rate(struct iwl_priv *priv,
 
 	return new_rate;
 }
-#endif				/* CONFIG_IWL4965_HT */
-
-static inline u8 rs_is_both_ant_supp(u8 valid_antenna)
-{
-	return (rs_is_ant_connected(valid_antenna, ANT_BOTH));
-}
 
 /*
  * Set up search table for MIMO
  */
-static int rs_switch_to_mimo(struct iwl_priv *priv,
+static int rs_switch_to_mimo2(struct iwl_priv *priv,
 			     struct iwl4965_lq_sta *lq_sta,
 			     struct ieee80211_conf *conf,
 			     struct sta_info *sta,
 			     struct iwl4965_scale_tbl_info *tbl, int index)
 {
-#ifdef CONFIG_IWL4965_HT
 	u16 rate_mask;
 	s32 rate;
 	s8 is_green = lq_sta->is_green;
@@ -1194,26 +1156,27 @@ static int rs_switch_to_mimo(struct iwl_priv *priv,
 	    !sta->ht_info.ht_supported)
 		return -1;
 
-	IWL_DEBUG_HT("LQ: try to switch to MIMO\n");
-	tbl->lq_type = LQ_MIMO;
-	rs_get_supported_rates(lq_sta, NULL, tbl->lq_type,
-				&rate_mask);
-
 	if (priv->current_ht_config.tx_mimo_ps_mode == IWL_MIMO_PS_STATIC)
 		return -1;
 
 	/* Need both Tx chains/antennas to support MIMO */
-	if (!rs_is_both_ant_supp(lq_sta->antenna))
+	if (priv->hw_params.tx_chains_num < 2)
 		return -1;
 
+	IWL_DEBUG_RATE("LQ: try to switch to MIMO2\n");
+
+	tbl->lq_type = LQ_MIMO2;
 	tbl->is_dup = lq_sta->is_dup;
 	tbl->action = 0;
+	rate_mask = lq_sta->active_mimo2_rate;
+
 	if (priv->current_ht_config.supported_chan_width
-	    == IWL_CHANNEL_WIDTH_40MHZ)
+					== IWL_CHANNEL_WIDTH_40MHZ)
 		tbl->is_fat = 1;
 	else
 		tbl->is_fat = 0;
 
+	/* FIXME: - don't toggle SGI here
 	if (tbl->is_fat) {
 		if (priv->current_ht_config.sgf & HT_SHORT_GI_40MHZ_ONLY)
 			tbl->is_SGI = 1;
@@ -1223,22 +1186,24 @@ static int rs_switch_to_mimo(struct iwl_priv *priv,
 		tbl->is_SGI = 1;
 	else
 		tbl->is_SGI = 0;
+	*/
+
+	rs_set_expected_tpt_table(lq_sta, tbl);
 
-	rs_get_expected_tpt_table(lq_sta, tbl);
+	rate = rs_get_best_rate(priv, lq_sta, tbl, rate_mask, index);
 
-	rate = rs_get_best_rate(priv, lq_sta, tbl, rate_mask, index, index);
+	IWL_DEBUG_RATE("LQ: MIMO2 best rate %d mask %X\n", rate, rate_mask);
 
-	IWL_DEBUG_HT("LQ: MIMO best rate %d mask %X\n", rate, rate_mask);
-	if ((rate == IWL_RATE_INVALID) || !((1 << rate) & rate_mask))
+	if ((rate == IWL_RATE_INVALID) || !((1 << rate) & rate_mask)) {
+		IWL_DEBUG_RATE("Can't switch with index %d rate mask %x\n",
+						rate, rate_mask);
 		return -1;
-	rs_mcs_from_tbl(&tbl->current_rate, tbl, rate, is_green);
+	}
+	tbl->current_rate = rate_n_flags_from_tbl(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);
+	IWL_DEBUG_RATE("LQ: Switch to new mcs %X index is green %X\n",
+		     tbl->current_rate, is_green);
 	return 0;
-#else
-	return -1;
-#endif	/*CONFIG_IWL4965_HT */
 }
 
 /*
@@ -1250,21 +1215,20 @@ static int rs_switch_to_siso(struct iwl_priv *priv,
 			     struct sta_info *sta,
 			     struct iwl4965_scale_tbl_info *tbl, int index)
 {
-#ifdef CONFIG_IWL4965_HT
 	u16 rate_mask;
 	u8 is_green = lq_sta->is_green;
 	s32 rate;
 
-	IWL_DEBUG_HT("LQ: try to switch to SISO\n");
 	if (!(conf->flags & IEEE80211_CONF_SUPPORT_HT_MODE) ||
 	    !sta->ht_info.ht_supported)
 		return -1;
 
+	IWL_DEBUG_RATE("LQ: try to switch to SISO\n");
+
 	tbl->is_dup = lq_sta->is_dup;
 	tbl->lq_type = LQ_SISO;
 	tbl->action = 0;
-	rs_get_supported_rates(lq_sta, NULL, tbl->lq_type,
-				&rate_mask);
+	rate_mask = lq_sta->active_siso_rate;
 
 	if (priv->current_ht_config.supported_chan_width
 	    == IWL_CHANNEL_WIDTH_40MHZ)
@@ -1272,6 +1236,7 @@ static int rs_switch_to_siso(struct iwl_priv *priv,
 	else
 		tbl->is_fat = 0;
 
+	/* FIXME: - don't toggle SGI here
 	if (tbl->is_fat) {
 		if (priv->current_ht_config.sgf & HT_SHORT_GI_40MHZ_ONLY)
 			tbl->is_SGI = 1;
@@ -1281,27 +1246,24 @@ static int rs_switch_to_siso(struct iwl_priv *priv,
 		tbl->is_SGI = 1;
 	else
 		tbl->is_SGI = 0;
+	*/
 
 	if (is_green)
-		tbl->is_SGI = 0;
+		tbl->is_SGI = 0; /*11n spec: no SGI in SISO+Greenfield*/
 
-	rs_get_expected_tpt_table(lq_sta, tbl);
-	rate = rs_get_best_rate(priv, lq_sta, tbl, rate_mask, index, index);
+	rs_set_expected_tpt_table(lq_sta, tbl);
+	rate = rs_get_best_rate(priv, lq_sta, tbl, rate_mask, index);
 
-	IWL_DEBUG_HT("LQ: get best rate %d mask %X\n", rate, rate_mask);
+	IWL_DEBUG_RATE("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",
+		IWL_DEBUG_RATE("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);
+	tbl->current_rate = rate_n_flags_from_tbl(tbl, rate, is_green);
+	IWL_DEBUG_RATE("LQ: Switch to new mcs %X index is green %X\n",
+		     tbl->current_rate, is_green);
 	return 0;
-#else
-	return -1;
-
-#endif	/*CONFIG_IWL4965_HT */
 }
 
 /*
@@ -1313,7 +1275,6 @@ static int rs_move_legacy_other(struct iwl_priv *priv,
 				struct sta_info *sta,
 				int index)
 {
-	int ret = 0;
 	struct iwl4965_scale_tbl_info *tbl =
 	    &(lq_sta->lq_info[lq_sta->active_tbl]);
 	struct iwl4965_scale_tbl_info *search_tbl =
@@ -1322,41 +1283,35 @@ static int rs_move_legacy_other(struct iwl_priv *priv,
 	u32 sz = (sizeof(struct iwl4965_scale_tbl_info) -
 		  (sizeof(struct iwl4965_rate_scale_data) * IWL_RATE_COUNT));
 	u8 start_action = tbl->action;
+	u8 valid_tx_ant = priv->hw_params.valid_tx_ant;
+	int ret = 0;
 
 	for (; ;) {
 		switch (tbl->action) {
 		case IWL_LEGACY_SWITCH_ANTENNA:
-			IWL_DEBUG_HT("LQ Legacy switch Antenna\n");
+			IWL_DEBUG_RATE("LQ: Legacy toggle Antenna\n");
 
-			search_tbl->lq_type = LQ_NONE;
 			lq_sta->action_counter++;
 
 			/* Don't change antenna if success has been great */
 			if (window->success_ratio >= IWL_RS_GOOD_RATIO)
 				break;
 
-			/* Don't change antenna if other one is not connected */
-			if (!rs_is_other_ant_connected(lq_sta->antenna,
-							tbl->antenna_type))
-				break;
-
 			/* Set up search table to try other antenna */
 			memcpy(search_tbl, tbl, sz);
 
-			rs_toggle_antenna(&(search_tbl->current_rate),
-					   search_tbl);
-			rs_get_expected_tpt_table(lq_sta, search_tbl);
-			lq_sta->search_better_tbl = 1;
-			goto out;
-
+			if (rs_toggle_antenna(valid_tx_ant,
+				&search_tbl->current_rate, search_tbl)) {
+				lq_sta->search_better_tbl = 1;
+				goto out;
+			}
+			break;
 		case IWL_LEGACY_SWITCH_SISO:
-			IWL_DEBUG_HT("LQ: Legacy switch to SISO\n");
+			IWL_DEBUG_RATE("LQ: Legacy switch to SISO\n");
 
 			/* Set up search table to try SISO */
 			memcpy(search_tbl, tbl, sz);
-			search_tbl->lq_type = LQ_SISO;
 			search_tbl->is_SGI = 0;
-			search_tbl->is_fat = 0;
 			ret = rs_switch_to_siso(priv, lq_sta, conf, sta,
 						 search_tbl, index);
 			if (!ret) {
@@ -1366,16 +1321,15 @@ static int rs_move_legacy_other(struct iwl_priv *priv,
 			}
 
 			break;
-		case IWL_LEGACY_SWITCH_MIMO:
-			IWL_DEBUG_HT("LQ: Legacy switch MIMO\n");
+		case IWL_LEGACY_SWITCH_MIMO2:
+			IWL_DEBUG_RATE("LQ: Legacy switch to MIMO2\n");
 
 			/* Set up search table to try MIMO */
 			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;
-			ret = rs_switch_to_mimo(priv, lq_sta, conf, sta,
+			search_tbl->ant_type = ANT_AB;/*FIXME:RS*/
+				/*FIXME:RS:need to check ant validity*/
+			ret = rs_switch_to_mimo2(priv, lq_sta, conf, sta,
 						 search_tbl, index);
 			if (!ret) {
 				lq_sta->search_better_tbl = 1;
@@ -1385,7 +1339,7 @@ static int rs_move_legacy_other(struct iwl_priv *priv,
 			break;
 		}
 		tbl->action++;
-		if (tbl->action > IWL_LEGACY_SWITCH_MIMO)
+		if (tbl->action > IWL_LEGACY_SWITCH_MIMO2)
 			tbl->action = IWL_LEGACY_SWITCH_ANTENNA;
 
 		if (tbl->action == start_action)
@@ -1396,7 +1350,7 @@ static int rs_move_legacy_other(struct iwl_priv *priv,
 
  out:
 	tbl->action++;
-	if (tbl->action > IWL_LEGACY_SWITCH_MIMO)
+	if (tbl->action > IWL_LEGACY_SWITCH_MIMO2)
 		tbl->action = IWL_LEGACY_SWITCH_ANTENNA;
 	return 0;
 
@@ -1411,7 +1365,6 @@ static int rs_move_siso_to_other(struct iwl_priv *priv,
 				 struct sta_info *sta,
 				 int index)
 {
-	int ret;
 	u8 is_green = lq_sta->is_green;
 	struct iwl4965_scale_tbl_info *tbl =
 	    &(lq_sta->lq_info[lq_sta->active_tbl]);
@@ -1421,35 +1374,30 @@ static int rs_move_siso_to_other(struct iwl_priv *priv,
 	u32 sz = (sizeof(struct iwl4965_scale_tbl_info) -
 		  (sizeof(struct iwl4965_rate_scale_data) * IWL_RATE_COUNT));
 	u8 start_action = tbl->action;
+	u8 valid_tx_ant = priv->hw_params.valid_tx_ant;
+	int ret;
 
 	for (;;) {
 		lq_sta->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;
+			IWL_DEBUG_RATE("LQ: SISO toggle Antenna\n");
 			if (window->success_ratio >= IWL_RS_GOOD_RATIO)
 				break;
-			if (!rs_is_other_ant_connected(lq_sta->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_sta->search_better_tbl = 1;
-
-			goto out;
-
-		case IWL_SISO_SWITCH_MIMO:
-			IWL_DEBUG_HT("LQ: SISO SWITCH TO MIMO FROM SISO\n");
+			if (rs_toggle_antenna(valid_tx_ant,
+				       &search_tbl->current_rate, search_tbl)) {
+				lq_sta->search_better_tbl = 1;
+				goto out;
+			}
+			break;
+		case IWL_SISO_SWITCH_MIMO2:
+			IWL_DEBUG_RATE("LQ: SISO switch to MIMO2\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;
-			ret = rs_switch_to_mimo(priv, lq_sta, conf, sta,
+			search_tbl->ant_type = ANT_AB; /*FIXME:RS*/
+			ret = rs_switch_to_mimo2(priv, lq_sta, conf, sta,
 						 search_tbl, index);
 			if (!ret) {
 				lq_sta->search_better_tbl = 1;
@@ -1457,29 +1405,34 @@ static int rs_move_siso_to_other(struct iwl_priv *priv,
 			}
 			break;
 		case IWL_SISO_SWITCH_GI:
-			IWL_DEBUG_HT("LQ: SISO SWITCH TO GI\n");
+			if (!tbl->is_fat &&
+				!(priv->current_ht_config.sgf &
+						HT_SHORT_GI_20MHZ))
+				break;
+			if (tbl->is_fat &&
+				!(priv->current_ht_config.sgf &
+						HT_SHORT_GI_40MHZ))
+				break;
+
+			IWL_DEBUG_RATE("LQ: SISO toggle SGI/NGI\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_sta->search_better_tbl = 1;
-			if ((tbl->lq_type == LQ_SISO) &&
-			    (tbl->is_SGI)) {
+			if (is_green) {
+				if (!tbl->is_SGI)
+					break;
+				else
+					IWL_ERROR("SGI was set in GF+SISO\n");
+			}
+			search_tbl->is_SGI = !tbl->is_SGI;
+			rs_set_expected_tpt_table(lq_sta, search_tbl);
+			if (tbl->is_SGI) {
 				s32 tpt = lq_sta->last_tpt / 100;
-				if (((!tbl->is_fat) &&
-				     (tpt >= expected_tpt_siso20MHz[index])) ||
-				    ((tbl->is_fat) &&
-				     (tpt >= expected_tpt_siso40MHz[index])))
-					lq_sta->search_better_tbl = 0;
+				if (tpt >= search_tbl->expected_tpt[index])
+					break;
 			}
-			rs_get_expected_tpt_table(lq_sta, search_tbl);
-			rs_mcs_from_tbl(&search_tbl->current_rate,
-					     search_tbl, index, is_green);
+			search_tbl->current_rate = rate_n_flags_from_tbl(
+						search_tbl, index, is_green);
+			lq_sta->search_better_tbl = 1;
 			goto out;
 		}
 		tbl->action++;
@@ -1507,7 +1460,6 @@ static int rs_move_mimo_to_other(struct iwl_priv *priv,
 				 struct sta_info *sta,
 				 int index)
 {
-	int ret;
 	s8 is_green = lq_sta->is_green;
 	struct iwl4965_scale_tbl_info *tbl =
 	    &(lq_sta->lq_info[lq_sta->active_tbl]);
@@ -1516,24 +1468,24 @@ static int rs_move_mimo_to_other(struct iwl_priv *priv,
 	u32 sz = (sizeof(struct iwl4965_scale_tbl_info) -
 		  (sizeof(struct iwl4965_rate_scale_data) * IWL_RATE_COUNT));
 	u8 start_action = tbl->action;
+	/*u8 valid_tx_ant = priv->hw_params.valid_tx_ant;*/
+	int ret;
 
 	for (;;) {
 		lq_sta->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");
-
+			IWL_DEBUG_RATE("LQ: MIMO2 switch to SISO\n");
 
 			/* Set up new search table for SISO */
 			memcpy(search_tbl, tbl, sz);
-			search_tbl->lq_type = LQ_SISO;
-			search_tbl->is_SGI = 0;
-			search_tbl->is_fat = 0;
+
+			/*FIXME:RS:need to check ant validity + C*/
 			if (tbl->action == IWL_MIMO_SWITCH_ANTENNA_A)
-				search_tbl->antenna_type = ANT_MAIN;
+				search_tbl->ant_type = ANT_A;
 			else
-				search_tbl->antenna_type = ANT_AUX;
+				search_tbl->ant_type = ANT_B;
 
 			ret = rs_switch_to_siso(priv, lq_sta, conf, sta,
 						 search_tbl, index);
@@ -1544,37 +1496,35 @@ static int rs_move_mimo_to_other(struct iwl_priv *priv,
 			break;
 
 		case IWL_MIMO_SWITCH_GI:
-			IWL_DEBUG_HT("LQ: MIMO SWITCH TO GI\n");
+			if (!tbl->is_fat &&
+				!(priv->current_ht_config.sgf &
+						HT_SHORT_GI_20MHZ))
+				break;
+			if (tbl->is_fat &&
+				!(priv->current_ht_config.sgf &
+						HT_SHORT_GI_40MHZ))
+				break;
+
+			IWL_DEBUG_RATE("LQ: MIMO toggle SGI/NGI\n");
 
 			/* Set up new search table for MIMO */
 			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_sta->search_better_tbl = 1;
-
+			search_tbl->is_SGI = !tbl->is_SGI;
+			rs_set_expected_tpt_table(lq_sta, search_tbl);
 			/*
 			 * If active table already uses the fastest possible
 			 * modulation (dual stream with short guard interval),
 			 * and it's working well, there's no need to look
 			 * for a better type of modulation!
 			 */
-			if ((tbl->lq_type == LQ_MIMO) &&
-			    (tbl->is_SGI)) {
+			if (tbl->is_SGI) {
 				s32 tpt = lq_sta->last_tpt / 100;
-				if (((!tbl->is_fat) &&
-				     (tpt >= expected_tpt_mimo20MHz[index])) ||
-				    ((tbl->is_fat) &&
-				     (tpt >= expected_tpt_mimo40MHz[index])))
-					lq_sta->search_better_tbl = 0;
+				if (tpt >= search_tbl->expected_tpt[index])
+					break;
 			}
-			rs_get_expected_tpt_table(lq_sta, search_tbl);
-			rs_mcs_from_tbl(&search_tbl->current_rate,
-					     search_tbl, index, is_green);
+			search_tbl->current_rate = rate_n_flags_from_tbl(
+						search_tbl, index, is_green);
+			lq_sta->search_better_tbl = 1;
 			goto out;
 
 		}
@@ -1608,7 +1558,9 @@ static void rs_stay_in_table(struct iwl4965_lq_sta *lq_sta)
 	int i;
 	int active_tbl;
 	int flush_interval_passed = 0;
+	struct iwl_priv *priv;
 
+	priv = lq_sta->drv;
 	active_tbl = lq_sta->active_tbl;
 
 	tbl = &(lq_sta->lq_info[active_tbl]);
@@ -1623,9 +1575,6 @@ static void rs_stay_in_table(struct iwl4965_lq_sta *lq_sta)
 				       (unsigned long)(lq_sta->flush_timer +
 					IWL_RATE_SCALE_FLUSH_INTVL));
 
-		/* For now, disable the elapsed time criterion */
-		flush_interval_passed = 0;
-
 		/*
 		 * Check if we should allow search for new modulation mode.
 		 * If many frames have failed or succeeded, or we've used
@@ -1638,7 +1587,7 @@ static void rs_stay_in_table(struct iwl4965_lq_sta *lq_sta)
 		    (lq_sta->total_success > lq_sta->max_success_limit) ||
 		    ((!lq_sta->search_better_tbl) && (lq_sta->flush_timer)
 		     && (flush_interval_passed))) {
-			IWL_DEBUG_HT("LQ: stay is expired %d %d %d\n:",
+			IWL_DEBUG_RATE("LQ: stay is expired %d %d %d\n:",
 				     lq_sta->total_failed,
 				     lq_sta->total_success,
 				     flush_interval_passed);
@@ -1661,7 +1610,7 @@ static void rs_stay_in_table(struct iwl4965_lq_sta *lq_sta)
 			    lq_sta->table_count_limit) {
 				lq_sta->table_count = 0;
 
-				IWL_DEBUG_HT("LQ: stay in table clear win\n");
+				IWL_DEBUG_RATE("LQ: stay in table clear win\n");
 				for (i = 0; i < IWL_RATE_COUNT; i++)
 					rs_rate_scale_clear_window(
 						&(tbl->win[i]));
@@ -1699,24 +1648,23 @@ static void rs_rate_scale_perform(struct iwl_priv *priv,
 	int high_tpt = IWL_INVALID_VALUE;
 	u32 fail_count;
 	s8 scale_action = 0;
-	u16 fc, rate_mask;
+	__le16 fc;
+	u16 rate_mask;
 	u8 update_lq = 0;
 	struct iwl4965_lq_sta *lq_sta;
 	struct iwl4965_scale_tbl_info *tbl, *tbl1;
 	u16 rate_scale_index_msk = 0;
-	struct iwl4965_rate mcs_rate;
+	u32 rate;
 	u8 is_green = 0;
 	u8 active_tbl = 0;
 	u8 done_search = 0;
 	u16 high_low;
-#ifdef CONFIG_IWL4965_HT
+	s32 sr;
 	u8 tid = MAX_TID_COUNT;
-	__le16 *qc;
-#endif
 
 	IWL_DEBUG_RATE("rate scale calculate new rate for skb\n");
 
-	fc = le16_to_cpu(hdr->frame_control);
+	fc = hdr->frame_control;
 	if (!ieee80211_is_data(fc) || is_multicast_ether_addr(hdr->addr1)) {
 		/* Send management frames and broadcast/multicast data using
 		 * lowest rate. */
@@ -1727,19 +1675,10 @@ static void rs_rate_scale_perform(struct iwl_priv *priv,
 	if (!sta || !sta->rate_ctrl_priv)
 		return;
 
-	if (!priv->lq_mngr.lq_ready) {
-		IWL_DEBUG_RATE("still rate scaling not ready\n");
-		return;
-	}
 	lq_sta = (struct iwl4965_lq_sta *)sta->rate_ctrl_priv;
 
-#ifdef CONFIG_IWL4965_HT
-	qc = ieee80211_get_qos_ctrl(hdr);
-	if (qc) {
-		tid = (u8)(le16_to_cpu(*qc) & 0xf);
-		rs_tl_add_packet(lq_sta, tid);
-	}
-#endif
+	tid = rs_tl_add_packet(lq_sta, hdr);
+
 	/*
 	 * Select rate-scale / modulation-mode table to work with in
 	 * the rest of this function:  "search" if searching for better
@@ -1760,8 +1699,7 @@ static void rs_rate_scale_perform(struct iwl_priv *priv,
 		       tbl->lq_type);
 
 	/* rates available for this association, and for modulation mode */
-	rs_get_supported_rates(lq_sta, hdr, tbl->lq_type,
-				&rate_mask);
+	rate_mask = rs_get_supported_rates(lq_sta, hdr, tbl->lq_type);
 
 	IWL_DEBUG_RATE("mask 0x%04X \n", rate_mask);
 
@@ -1781,27 +1719,16 @@ static void rs_rate_scale_perform(struct iwl_priv *priv,
 	if (!rate_scale_index_msk)
 		rate_scale_index_msk = rate_mask;
 
-	/* If current rate is no longer supported on current association,
-	 * or user changed preferences for rates, find a new supported rate. */
-	if (index < 0 || !((1 << index) & rate_scale_index_msk)) {
-		index = IWL_INVALID_VALUE;
-		update_lq = 1;
-
-		/* get the highest available 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 (!((1 << index) & rate_scale_index_msk)) {
+		IWL_ERROR("Current Rate is not valid\n");
+		return;
 	}
 
 	/* Get expected throughput table and history window for current rate */
-	if (!tbl->expected_tpt)
-		rs_get_expected_tpt_table(lq_sta, tbl);
+	if (!tbl->expected_tpt) {
+		IWL_ERROR("tbl->expected_tpt is NULL\n");
+		return;
+	}
 
 	window = &(tbl->win[index]);
 
@@ -1813,10 +1740,9 @@ static void rs_rate_scale_perform(struct iwl_priv *priv,
 	 * in current association (use new rate found above).
 	 */
 	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 "
+	if ((fail_count < IWL_RATE_MIN_FAILURE_TH) &&
+			(window->success_counter < IWL_RATE_MIN_SUCCESS_TH)) {
+		IWL_DEBUG_RATE("LQ: still below TH. succ=%d total=%d "
 			       "for index %d\n",
 			       window->success_counter, window->counter, index);
 
@@ -1827,44 +1753,51 @@ static void rs_rate_scale_perform(struct iwl_priv *priv,
 		 * or search for a new one? */
 		rs_stay_in_table(lq_sta);
 
-		/* Set up new rate table in uCode, if needed */
-		if (update_lq) {
-			rs_mcs_from_tbl(&mcs_rate, tbl, index, is_green);
-			rs_fill_link_cmd(lq_sta, &mcs_rate, &lq_sta->lq);
-			iwl_send_lq_cmd(priv, &lq_sta->lq, CMD_ASYNC);
-		}
 		goto out;
 
 	/* Else we have enough samples; calculate estimate of
 	 * actual average throughput */
-	} else
-		window->average_tpt = ((window->success_ratio *
+	} else {
+		/*FIXME:RS remove this else if we don't get this error*/
+		if (window->average_tpt != ((window->success_ratio *
+				tbl->expected_tpt[index] + 64) / 128)) {
+			IWL_ERROR("expected_tpt should have been calculated"
+								" by now\n");
+			window->average_tpt = ((window->success_ratio *
 					tbl->expected_tpt[index] + 64) / 128);
+		}
+	}
 
 	/* If we are searching for better modulation mode, check success. */
 	if (lq_sta->search_better_tbl) {
-		int success_limit = IWL_RATE_SCALE_SWITCH;
 
 		/* If good success, continue using the "search" mode;
 		 * no need to send new link quality command, since we're
 		 * continuing to use the setup that we've been trying. */
-		if ((window->success_ratio > success_limit) ||
-		    (window->average_tpt > lq_sta->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_sta->last_tpt);
+		if (window->average_tpt > lq_sta->last_tpt) {
+
+			IWL_DEBUG_RATE("LQ: SWITCHING TO CURRENT TABLE "
+					"suc=%d cur-tpt=%d old-tpt=%d\n",
+					window->success_ratio,
+					window->average_tpt,
+					lq_sta->last_tpt);
+
+			if (!is_legacy(tbl->lq_type))
 				lq_sta->enable_counter = 1;
-			}
+
 			/* Swap tables; "search" becomes "active" */
 			lq_sta->active_tbl = active_tbl;
 			current_tpt = window->average_tpt;
 
 		/* Else poor success; go back to mode in "active" table */
 		} else {
+
+			IWL_DEBUG_RATE("LQ: GOING BACK TO THE OLD TABLE "
+					"suc=%d cur-tpt=%d old-tpt=%d\n",
+					window->success_ratio,
+					window->average_tpt,
+					lq_sta->last_tpt);
+
 			/* Nullify "search" table */
 			tbl->lq_type = LQ_NONE;
 
@@ -1873,13 +1806,11 @@ static void rs_rate_scale_perform(struct iwl_priv *priv,
 			tbl = &(lq_sta->lq_info[active_tbl]);
 
 			/* Revert to "active" rate and throughput info */
-			index = iwl4965_hwrate_to_plcp_idx(
-				tbl->current_rate.rate_n_flags);
+			index = iwl_hwrate_to_plcp_idx(tbl->current_rate);
 			current_tpt = lq_sta->last_tpt;
 
 			/* Need to set up a new rate table in uCode */
 			update_lq = 1;
-			IWL_DEBUG_HT("XXY GO BACK TO OLD TABLE\n");
 		}
 
 		/* Either way, we've made a decision; modulation mode
@@ -1891,11 +1822,13 @@ static void rs_rate_scale_perform(struct iwl_priv *priv,
 
 	/* (Else) not in search of better modulation mode, try for better
 	 * starting rate, while staying in this mode. */
-	high_low = rs_get_adjacent_rate(index, rate_scale_index_msk,
+	high_low = rs_get_adjacent_rate(priv, index, rate_scale_index_msk,
 					tbl->lq_type);
 	low = high_low & 0xff;
 	high = (high_low >> 8) & 0xff;
 
+	sr = window->success_ratio;
+
 	/* Collect measured throughputs for current and adjacent rates */
 	current_tpt = window->average_tpt;
 	if (low != IWL_RATE_INVALID)
@@ -1903,19 +1836,22 @@ static void rs_rate_scale_perform(struct iwl_priv *priv,
 	if (high != IWL_RATE_INVALID)
 		high_tpt = tbl->win[high].average_tpt;
 
-	/* Assume rate increase */
-	scale_action = 1;
+	scale_action = 0;
 
 	/* Too many failures, decrease rate */
-	if ((window->success_ratio <= IWL_RATE_DECREASE_TH) ||
-	    (current_tpt == 0)) {
+	if ((sr <= IWL_RATE_DECREASE_TH) || (current_tpt == 0)) {
 		IWL_DEBUG_RATE("decrease rate because of low success_ratio\n");
 		scale_action = -1;
 
 	/* No throughput measured yet for adjacent rates; try increase. */
 	} else if ((low_tpt == IWL_INVALID_VALUE) &&
-		   (high_tpt == IWL_INVALID_VALUE))
-		scale_action = 1;
+		   (high_tpt == IWL_INVALID_VALUE)) {
+
+		if (high != IWL_RATE_INVALID && sr >= IWL_RATE_INCREASE_TH)
+			scale_action = 1;
+		else if (low != IWL_RATE_INVALID)
+			scale_action = -1;
+	}
 
 	/* Both adjacent throughputs are measured, but neither one has better
 	 * throughput; we're using the best rate, don't change it! */
@@ -1931,9 +1867,10 @@ static void rs_rate_scale_perform(struct iwl_priv *priv,
 		/* Higher adjacent rate's throughput is measured */
 		if (high_tpt != IWL_INVALID_VALUE) {
 			/* Higher rate has better throughput */
-			if (high_tpt > current_tpt)
+			if (high_tpt > current_tpt &&
+					sr >= IWL_RATE_INCREASE_TH) {
 				scale_action = 1;
-			else {
+			} else {
 				IWL_DEBUG_RATE
 				    ("decrease rate because of high tpt\n");
 				scale_action = -1;
@@ -1946,23 +1883,17 @@ static void rs_rate_scale_perform(struct iwl_priv *priv,
 				IWL_DEBUG_RATE
 				    ("decrease rate because of low tpt\n");
 				scale_action = -1;
-			} else
+			} else if (sr >= IWL_RATE_INCREASE_TH) {
 				scale_action = 1;
+			}
 		}
 	}
 
 	/* Sanity check; asked for decrease, but success rate or throughput
 	 * has been good at old rate.  Don't change it. */
-	if (scale_action == -1) {
-		if ((low != IWL_RATE_INVALID) &&
-		    ((window->success_ratio > IWL_RATE_HIGH_TH) ||
+	if ((scale_action == -1) && (low != IWL_RATE_INVALID) &&
+		    ((sr > IWL_RATE_HIGH_TH) ||
 		     (current_tpt > (100 * tbl->expected_tpt[low]))))
-			scale_action = 0;
-
-	/* Sanity check; asked for increase, but success rate has not been great
-	 * even at old rate, higher rate will be worse.  Don't change it. */
-	} else if ((scale_action == 1) &&
-		   (window->success_ratio < IWL_RATE_INCREASE_TH))
 		scale_action = 0;
 
 	switch (scale_action) {
@@ -1987,15 +1918,15 @@ static void rs_rate_scale_perform(struct iwl_priv *priv,
 		break;
 	}
 
-	IWL_DEBUG_HT("choose rate scale index %d action %d low %d "
+	IWL_DEBUG_RATE("choose rate scale index %d action %d low %d "
 		    "high %d type %d\n",
 		     index, scale_action, low, high, tbl->lq_type);
 
- lq_update:
+lq_update:
 	/* Replace uCode's rate table for the destination station. */
 	if (update_lq) {
-		rs_mcs_from_tbl(&mcs_rate, tbl, index, is_green);
-		rs_fill_link_cmd(lq_sta, &mcs_rate, &lq_sta->lq);
+		rate = rate_n_flags_from_tbl(tbl, index, is_green);
+		rs_fill_link_cmd(priv, lq_sta, rate);
 		iwl_send_lq_cmd(priv, &lq_sta->lq, CMD_ASYNC);
 	}
 
@@ -2029,13 +1960,11 @@ static void rs_rate_scale_perform(struct iwl_priv *priv,
 				rs_rate_scale_clear_window(&(tbl->win[i]));
 
 			/* Use new "search" start rate */
-			index = iwl4965_hwrate_to_plcp_idx(
-					tbl->current_rate.rate_n_flags);
+			index = iwl_hwrate_to_plcp_idx(tbl->current_rate);
 
-			IWL_DEBUG_HT("Switch current  mcs: %X index: %d\n",
-				     tbl->current_rate.rate_n_flags, index);
-			rs_fill_link_cmd(lq_sta, &tbl->current_rate,
-					 &lq_sta->lq);
+			IWL_DEBUG_RATE("Switch current  mcs: %X index: %d\n",
+				     tbl->current_rate, index);
+			rs_fill_link_cmd(priv, lq_sta, tbl->current_rate);
 			iwl_send_lq_cmd(priv, &lq_sta->lq, CMD_ASYNC);
 		}
 
@@ -2046,13 +1975,11 @@ static void rs_rate_scale_perform(struct iwl_priv *priv,
 		 * before next round of mode comparisons. */
 		tbl1 = &(lq_sta->lq_info[lq_sta->active_tbl]);
 		if (is_legacy(tbl1->lq_type) &&
-#ifdef CONFIG_IWL4965_HT
 		   (!(conf->flags & IEEE80211_CONF_SUPPORT_HT_MODE)) &&
-#endif
 		    (lq_sta->action_counter >= 1)) {
 			lq_sta->action_counter = 0;
-			IWL_DEBUG_HT("LQ: STAY in legacy table\n");
-			rs_set_stay_in_table(1, lq_sta);
+			IWL_DEBUG_RATE("LQ: STAY in legacy table\n");
+			rs_set_stay_in_table(priv, 1, lq_sta);
 		}
 
 		/* If we're in an HT mode, and all 3 mode switch actions
@@ -2060,16 +1987,14 @@ static void rs_rate_scale_perform(struct iwl_priv *priv,
 		 * mode for a while before next round of mode comparisons. */
 		if (lq_sta->enable_counter &&
 		    (lq_sta->action_counter >= IWL_ACTION_LIMIT)) {
-#ifdef CONFIG_IWL4965_HT
 			if ((lq_sta->last_tpt > IWL_AGG_TPT_THREHOLD) &&
 			    (lq_sta->tx_agg_tid_en & (1 << tid)) &&
 			    (tid != MAX_TID_COUNT)) {
-				IWL_DEBUG_HT("try to aggregate tid %d\n", tid);
+				IWL_DEBUG_RATE("try to aggregate tid %d\n", tid);
 				rs_tl_turn_on_agg(priv, tid, lq_sta, sta);
 			}
-#endif /*CONFIG_IWL4965_HT */
 			lq_sta->action_counter = 0;
-			rs_set_stay_in_table(0, lq_sta);
+			rs_set_stay_in_table(priv, 0, lq_sta);
 		}
 
 	/*
@@ -2085,7 +2010,7 @@ static void rs_rate_scale_perform(struct iwl_priv *priv,
 	}
 
 out:
-	rs_mcs_from_tbl(&tbl->current_rate, tbl, index, is_green);
+	tbl->current_rate = rate_n_flags_from_tbl(tbl, index, is_green);
 	i = index;
 	sta->last_txrate_idx = i;
 
@@ -2105,13 +2030,14 @@ static void rs_initialize_lq(struct iwl_priv *priv,
 			     struct ieee80211_conf *conf,
 			     struct sta_info *sta)
 {
-	int i;
 	struct iwl4965_lq_sta *lq_sta;
 	struct iwl4965_scale_tbl_info *tbl;
-	u8 active_tbl = 0;
 	int rate_idx;
+	int i;
+	u32 rate;
 	u8 use_green = rs_use_green(priv, conf);
-	struct iwl4965_rate mcs_rate;
+	u8 active_tbl = 0;
+	u8 valid_tx_ant;
 
 	if (!sta || !sta->rate_ctrl_priv)
 		goto out;
@@ -2123,6 +2049,8 @@ static void rs_initialize_lq(struct iwl_priv *priv,
 	    (priv->iw_mode == IEEE80211_IF_TYPE_IBSS))
 		goto out;
 
+	valid_tx_ant = priv->hw_params.valid_tx_ant;
+
 	if (!lq_sta->search_better_tbl)
 		active_tbl = lq_sta->active_tbl;
 	else
@@ -2133,22 +2061,23 @@ static void rs_initialize_lq(struct iwl_priv *priv,
 	if ((i < 0) || (i >= IWL_RATE_COUNT))
 		i = 0;
 
-	mcs_rate.rate_n_flags = iwl4965_rates[i].plcp ;
-	mcs_rate.rate_n_flags |= RATE_MCS_ANT_B_MSK;
-	mcs_rate.rate_n_flags &= ~RATE_MCS_ANT_A_MSK;
+	/* FIXME:RS: This is also wrong in 4965 */
+	rate = iwl_rates[i].plcp;
+	rate |= RATE_MCS_ANT_B_MSK;
+	rate &= ~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;
+		rate |= RATE_MCS_CCK_MSK;
 
-	tbl->antenna_type = ANT_AUX;
-	rs_get_tbl_info_from_mcs(&mcs_rate, priv->band, tbl, &rate_idx);
-	if (!rs_is_ant_connected(priv->valid_antenna, tbl->antenna_type))
-	    rs_toggle_antenna(&mcs_rate, tbl);
+	tbl->ant_type = ANT_B;
+	rs_get_tbl_info_from_mcs(rate, priv->band, tbl, &rate_idx);
+	if (!rs_is_valid_ant(valid_tx_ant, tbl->ant_type))
+	    rs_toggle_antenna(valid_tx_ant, &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_sta, tbl);
-	rs_fill_link_cmd(lq_sta, &mcs_rate, &lq_sta->lq);
+	rate = rate_n_flags_from_tbl(tbl, rate_idx, use_green);
+	tbl->current_rate = rate;
+	rs_set_expected_tpt_table(lq_sta, tbl);
+	rs_fill_link_cmd(NULL, lq_sta, rate);
 	iwl_send_lq_cmd(priv, &lq_sta->lq, CMD_ASYNC);
  out:
 	return;
@@ -2165,7 +2094,7 @@ static void rs_get_rate(void *priv_rate, struct net_device *dev,
 	struct ieee80211_conf *conf = &local->hw.conf;
 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
 	struct sta_info *sta;
-	u16 fc;
+	__le16 fc;
 	struct iwl_priv *priv = (struct iwl_priv *)priv_rate;
 	struct iwl4965_lq_sta *lq_sta;
 
@@ -2177,10 +2106,10 @@ static void rs_get_rate(void *priv_rate, struct net_device *dev,
 
 	/* Send management frames and broadcast/multicast data using lowest
 	 * rate. */
-	fc = le16_to_cpu(hdr->frame_control);
+	fc = hdr->frame_control;
 	if (!ieee80211_is_data(fc) || is_multicast_ether_addr(hdr->addr1) ||
 	    !sta || !sta->rate_ctrl_priv) {
-		sel->rate = rate_lowest(local, sband, sta);
+		sel->rate_idx = rate_lowest_index(local, sband, sta);
 		goto out;
 	}
 
@@ -2189,13 +2118,13 @@ static void rs_get_rate(void *priv_rate, struct net_device *dev,
 
 	if ((priv->iw_mode == IEEE80211_IF_TYPE_IBSS) &&
 	    !lq_sta->ibss_sta_added) {
-		u8 sta_id = iwl4965_hw_find_station(priv, hdr->addr1);
+		u8 sta_id = iwl_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 = iwl4965_add_station_flags(priv, hdr->addr1,
+			sta_id = iwl_add_station_flags(priv, hdr->addr1,
 							0, CMD_ASYNC, NULL);
 		}
 		if ((sta_id != IWL_INVALID_STATION)) {
@@ -2204,26 +2133,27 @@ static void rs_get_rate(void *priv_rate, struct net_device *dev,
 			lq_sta->ibss_sta_added = 1;
 			rs_initialize_lq(priv, conf, sta);
 		}
-		if (!lq_sta->ibss_sta_added)
-			goto done;
 	}
 
-done:
 	if ((i < 0) || (i > IWL_RATE_COUNT)) {
-		sel->rate = rate_lowest(local, sband, sta);
+		sel->rate_idx = rate_lowest_index(local, sband, sta);
 		goto out;
 	}
 
-	sel->rate = &priv->ieee_rates[i];
+	if (sband->band == IEEE80211_BAND_5GHZ)
+		i -= IWL_FIRST_OFDM_RATE;
+	sel->rate_idx = i;
 out:
 	rcu_read_unlock();
 }
 
-static void *rs_alloc_sta(void *priv, gfp_t gfp)
+static void *rs_alloc_sta(void *priv_rate, gfp_t gfp)
 {
 	struct iwl4965_lq_sta *lq_sta;
+	struct iwl_priv *priv;
 	int i, j;
 
+	priv = (struct iwl_priv *)priv_rate;
 	IWL_DEBUG_RATE("create station rate scale window\n");
 
 	lq_sta = kzalloc(sizeof(struct iwl4965_lq_sta), gfp);
@@ -2259,7 +2189,7 @@ static void rs_rate_init(void *priv_rate, void *priv_sta,
 		for (i = 0; i < IWL_RATE_COUNT; i++)
 			rs_rate_scale_clear_window(&(lq_sta->lq_info[j].win[i]));
 
-	IWL_DEBUG_RATE("rate scale global init\n");
+	IWL_DEBUG_RATE("LQ: *** 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
@@ -2267,17 +2197,17 @@ static void rs_rate_init(void *priv_rate, void *priv_sta,
 
 	lq_sta->ibss_sta_added = 0;
 	if (priv->iw_mode == IEEE80211_IF_TYPE_AP) {
-		u8 sta_id = iwl4965_hw_find_station(priv, sta->addr);
+		u8 sta_id = iwl_find_station(priv, sta->addr);
 		DECLARE_MAC_BUF(mac);
 
 		/* for IBSS the call are from tasklet */
-		IWL_DEBUG_HT("LQ: ADD station %s\n",
+		IWL_DEBUG_RATE("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 = iwl4965_add_station_flags(priv, sta->addr,
+			sta_id = iwl_add_station_flags(priv, sta->addr,
 							0, CMD_ASYNC, NULL);
 		}
 		if ((sta_id != IWL_INVALID_STATION)) {
@@ -2300,92 +2230,95 @@ static void rs_rate_init(void *priv_rate, void *priv_sta,
 		sta->last_txrate_idx += IWL_FIRST_OFDM_RATE;
 
 	lq_sta->is_dup = 0;
-	lq_sta->valid_antenna = priv->valid_antenna;
-	lq_sta->antenna = priv->antenna;
 	lq_sta->is_green = rs_use_green(priv, conf);
-	lq_sta->active_rate = priv->active_rate;
-	lq_sta->active_rate &= ~(0x1000);
+	lq_sta->active_legacy_rate = priv->active_rate & ~(0x1000);
 	lq_sta->active_rate_basic = priv->active_rate_basic;
 	lq_sta->band = priv->band;
-#ifdef CONFIG_IWL4965_HT
 	/*
 	 * active_siso_rate mask includes 9 MBits (bit 5), and CCK (bits 0-3),
 	 * supp_rates[] does not; shift to convert format, force 9 MBits off.
 	 */
-	lq_sta->active_siso_rate = (priv->current_ht_config.supp_mcs_set[0] << 1);
-	lq_sta->active_siso_rate |=
-			(priv->current_ht_config.supp_mcs_set[0] & 0x1);
+	lq_sta->active_siso_rate = conf->ht_conf.supp_mcs_set[0] << 1;
+	lq_sta->active_siso_rate |= conf->ht_conf.supp_mcs_set[0] & 0x1;
 	lq_sta->active_siso_rate &= ~((u16)0x2);
-	lq_sta->active_siso_rate =
-			lq_sta->active_siso_rate << IWL_FIRST_OFDM_RATE;
+	lq_sta->active_siso_rate <<= IWL_FIRST_OFDM_RATE;
 
 	/* Same here */
-	lq_sta->active_mimo_rate = (priv->current_ht_config.supp_mcs_set[1] << 1);
-	lq_sta->active_mimo_rate |=
-			(priv->current_ht_config.supp_mcs_set[1] & 0x1);
-	lq_sta->active_mimo_rate &= ~((u16)0x2);
-	lq_sta->active_mimo_rate =
-			lq_sta->active_mimo_rate << IWL_FIRST_OFDM_RATE;
-	IWL_DEBUG_HT("SISO RATE 0x%X MIMO RATE 0x%X\n",
+	lq_sta->active_mimo2_rate = conf->ht_conf.supp_mcs_set[1] << 1;
+	lq_sta->active_mimo2_rate |= conf->ht_conf.supp_mcs_set[1] & 0x1;
+	lq_sta->active_mimo2_rate &= ~((u16)0x2);
+	lq_sta->active_mimo2_rate <<= IWL_FIRST_OFDM_RATE;
+
+	lq_sta->active_mimo3_rate = conf->ht_conf.supp_mcs_set[2] << 1;
+	lq_sta->active_mimo3_rate |= conf->ht_conf.supp_mcs_set[2] & 0x1;
+	lq_sta->active_mimo3_rate &= ~((u16)0x2);
+	lq_sta->active_mimo3_rate <<= IWL_FIRST_OFDM_RATE;
+
+	IWL_DEBUG_RATE("SISO-RATE=%X MIMO2-RATE=%X MIMO3-RATE=%X\n",
 		     lq_sta->active_siso_rate,
-		     lq_sta->active_mimo_rate);
+		     lq_sta->active_mimo2_rate,
+		     lq_sta->active_mimo3_rate);
+
+	/* These values will be overriden later */
+	lq_sta->lq.general_params.single_stream_ant_msk = ANT_A;
+	lq_sta->lq.general_params.dual_stream_ant_msk = ANT_AB;
+
 	/* as default allow aggregation for all tids */
 	lq_sta->tx_agg_tid_en = IWL_AGG_ALL_TID;
-#endif /*CONFIG_IWL4965_HT*/
-#ifdef CONFIG_MAC80211_DEBUGFS
 	lq_sta->drv = priv;
-#endif
-
-	if (priv->assoc_station_added)
-		priv->lq_mngr.lq_ready = 1;
 
 	rs_initialize_lq(priv, conf, sta);
 }
 
-static void rs_fill_link_cmd(struct iwl4965_lq_sta *lq_sta,
-			    struct iwl4965_rate *tx_mcs,
-			    struct iwl_link_quality_cmd *lq_cmd)
+static void rs_fill_link_cmd(const struct iwl_priv *priv,
+			     struct iwl4965_lq_sta *lq_sta,
+			     u32 new_rate)
 {
+	struct iwl4965_scale_tbl_info tbl_type;
 	int index = 0;
 	int rate_idx;
 	int repeat_rate = 0;
-	u8 ant_toggle_count = 0;
+	u8 ant_toggle_cnt = 0;
 	u8 use_ht_possible = 1;
-	struct iwl4965_rate new_rate;
-	struct iwl4965_scale_tbl_info tbl_type = { 0 };
+	u8 valid_tx_ant = 0;
+	struct iwl_link_quality_cmd *lq_cmd = &lq_sta->lq;
 
 	/* Override starting rate (index 0) if needed for debug purposes */
-	rs_dbgfs_set_mcs(lq_sta, tx_mcs, index);
+	rs_dbgfs_set_mcs(lq_sta, &new_rate, index);
 
-	/* Interpret rate_n_flags */
-	rs_get_tbl_info_from_mcs(tx_mcs, lq_sta->band,
+	/* Interpret new_rate (rate_n_flags) */
+	memset(&tbl_type, 0, sizeof(tbl_type));
+	rs_get_tbl_info_from_mcs(new_rate, lq_sta->band,
 				  &tbl_type, &rate_idx);
 
 	/* How many times should we repeat the initial rate? */
 	if (is_legacy(tbl_type.lq_type)) {
-		ant_toggle_count = 1;
+		ant_toggle_cnt = 1;
 		repeat_rate = IWL_NUMBER_TRY;
-	} else
+	} else {
 		repeat_rate = IWL_HT_NUMBER_TRY;
+	}
 
 	lq_cmd->general_params.mimo_delimiter =
 			is_mimo(tbl_type.lq_type) ? 1 : 0;
 
 	/* Fill 1st table entry (index 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;
+	lq_cmd->rs_table[index].rate_n_flags = cpu_to_le32(new_rate);
 
-	if (is_mimo(tbl_type.lq_type) || (tbl_type.antenna_type == ANT_MAIN))
-		lq_cmd->general_params.single_stream_ant_msk
-			= LINK_QUAL_ANT_A_MSK;
-	else
-		lq_cmd->general_params.single_stream_ant_msk
-			= LINK_QUAL_ANT_B_MSK;
+	if (num_of_ant(tbl_type.ant_type) == 1) {
+		lq_cmd->general_params.single_stream_ant_msk =
+						tbl_type.ant_type;
+	} else if (num_of_ant(tbl_type.ant_type) == 2) {
+		lq_cmd->general_params.dual_stream_ant_msk =
+						tbl_type.ant_type;
+	} /* otherwise we don't modify the existing value */
 
 	index++;
 	repeat_rate--;
 
+	if (priv)
+		valid_tx_ant = priv->hw_params.valid_tx_ant;
+
 	/* Fill rest of rate table */
 	while (index < LINK_QUAL_MAX_RETRY_NUM) {
 		/* Repeat initial/next rate.
@@ -2393,26 +2326,25 @@ static void rs_fill_link_cmd(struct iwl4965_lq_sta *lq_sta,
 		 * For HT IWL_HT_NUMBER_TRY == 3, this executes twice. */
 		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;
-				}
-			}
+				if (ant_toggle_cnt < NUM_TRY_BEFORE_ANT_TOGGLE)
+					ant_toggle_cnt++;
+				else if (priv &&
+					 rs_toggle_antenna(valid_tx_ant,
+							&new_rate, &tbl_type))
+					ant_toggle_cnt = 1;
+}
 
 			/* Override next rate if needed for debug purposes */
 			rs_dbgfs_set_mcs(lq_sta, &new_rate, index);
 
 			/* Fill next table entry */
 			lq_cmd->rs_table[index].rate_n_flags =
-					cpu_to_le32(new_rate.rate_n_flags);
+					cpu_to_le32(new_rate);
 			repeat_rate--;
 			index++;
 		}
 
-		rs_get_tbl_info_from_mcs(&new_rate, lq_sta->band, &tbl_type,
+		rs_get_tbl_info_from_mcs(new_rate, lq_sta->band, &tbl_type,
 						&rate_idx);
 
 		/* Indicate to uCode which entries might be MIMO.
@@ -2422,20 +2354,22 @@ static void rs_fill_link_cmd(struct iwl4965_lq_sta *lq_sta,
 			lq_cmd->general_params.mimo_delimiter = index;
 
 		/* Get next rate */
-		rs_get_lower_rate(lq_sta, &tbl_type, rate_idx,
-				  use_ht_possible, &new_rate);
+		new_rate = rs_get_lower_rate(lq_sta, &tbl_type, rate_idx,
+					     use_ht_possible);
 
 		/* How many times should we repeat the next 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;
-			}
+			if (ant_toggle_cnt < NUM_TRY_BEFORE_ANT_TOGGLE)
+				ant_toggle_cnt++;
+			else if (priv &&
+				 rs_toggle_antenna(valid_tx_ant,
+						   &new_rate, &tbl_type))
+				ant_toggle_cnt = 1;
+
 			repeat_rate = IWL_NUMBER_TRY;
-		} else
+		} else {
 			repeat_rate = IWL_HT_NUMBER_TRY;
+		}
 
 		/* Don't allow HT rates after next pass.
 		 * rs_get_lower_rate() will change type to LQ_A or LQ_G. */
@@ -2445,14 +2379,13 @@ static void rs_fill_link_cmd(struct iwl4965_lq_sta *lq_sta,
 		rs_dbgfs_set_mcs(lq_sta, &new_rate, index);
 
 		/* Fill next table entry */
-		lq_cmd->rs_table[index].rate_n_flags =
-				cpu_to_le32(new_rate.rate_n_flags);
+		lq_cmd->rs_table[index].rate_n_flags = cpu_to_le32(new_rate);
 
 		index++;
 		repeat_rate--;
 	}
 
-	lq_cmd->general_params.dual_stream_ant_msk = 3;
+	lq_cmd->agg_params.agg_frame_cnt_limit = 64;
 	lq_cmd->agg_params.agg_dis_start_th = 3;
 	lq_cmd->agg_params.agg_time_limit = cpu_to_le16(4000);
 }
@@ -2473,15 +2406,17 @@ static void rs_clear(void *priv_rate)
 
 	IWL_DEBUG_RATE("enter\n");
 
-	priv->lq_mngr.lq_ready = 0;
+	/* TODO - add rate scale state reset */
 
 	IWL_DEBUG_RATE("leave\n");
 }
 
-static void rs_free_sta(void *priv, void *priv_sta)
+static void rs_free_sta(void *priv_rate, void *priv_sta)
 {
 	struct iwl4965_lq_sta *lq_sta = priv_sta;
+	struct iwl_priv *priv;
 
+	priv = (struct iwl_priv *)priv_rate;
 	IWL_DEBUG_RATE("enter\n");
 	kfree(lq_sta);
 	IWL_DEBUG_RATE("leave\n");
@@ -2495,54 +2430,56 @@ static int open_file_generic(struct inode *inode, struct file *file)
 	return 0;
 }
 static void rs_dbgfs_set_mcs(struct iwl4965_lq_sta *lq_sta,
-				struct iwl4965_rate *mcs, int index)
+				u32 *rate_n_flags, int index)
 {
-	u32 base_rate;
+	struct iwl_priv *priv;
 
-	if (lq_sta->band == IEEE80211_BAND_5GHZ)
-		base_rate = 0x800D;
-	else
-		base_rate = 0x820A;
-
-	if (lq_sta->dbg_fixed.rate_n_flags) {
-		if (index < 12)
-			mcs->rate_n_flags = lq_sta->dbg_fixed.rate_n_flags;
-		else
-			mcs->rate_n_flags = base_rate;
+	priv = lq_sta->drv;
+	if (lq_sta->dbg_fixed_rate) {
+		if (index < 12) {
+			*rate_n_flags = lq_sta->dbg_fixed_rate;
+		} else {
+			if (lq_sta->band == IEEE80211_BAND_5GHZ)
+				*rate_n_flags = 0x800D;
+			else
+				*rate_n_flags = 0x820A;
+		}
 		IWL_DEBUG_RATE("Fixed rate ON\n");
-		return;
+	} else {
+		IWL_DEBUG_RATE("Fixed rate OFF\n");
 	}
-
-	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 iwl4965_lq_sta *lq_sta = file->private_data;
+	struct iwl_priv *priv;
 	char buf[64];
 	int buf_size;
 	u32 parsed_rate;
 
+	priv = lq_sta->drv;
 	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)
-		lq_sta->dbg_fixed.rate_n_flags = parsed_rate;
+		lq_sta->dbg_fixed_rate = parsed_rate;
 	else
-		lq_sta->dbg_fixed.rate_n_flags = 0;
+		lq_sta->dbg_fixed_rate = 0;
 
-	lq_sta->active_rate = 0x0FFF;	/* 1 - 54 MBits, includes CCK */
-	lq_sta->active_siso_rate = 0x1FD0;	/* 6 - 60 MBits, no 9, no CCK */
-	lq_sta->active_mimo_rate = 0x1FD0;	/* 6 - 60 MBits, no 9, no CCK */
+	lq_sta->active_legacy_rate = 0x0FFF;	/* 1 - 54 MBits, includes CCK */
+	lq_sta->active_siso_rate   = 0x1FD0;	/* 6 - 60 MBits, no 9, no CCK */
+	lq_sta->active_mimo2_rate  = 0x1FD0;	/* 6 - 60 MBits, no 9, no CCK */
+	lq_sta->active_mimo3_rate  = 0x1FD0;	/* 6 - 60 MBits, no 9, no CCK */
 
 	IWL_DEBUG_RATE("sta_id %d rate 0x%X\n",
-		lq_sta->lq.sta_id, lq_sta->dbg_fixed.rate_n_flags);
+		lq_sta->lq.sta_id, lq_sta->dbg_fixed_rate);
 
-	if (lq_sta->dbg_fixed.rate_n_flags) {
-		rs_fill_link_cmd(lq_sta, &lq_sta->dbg_fixed, &lq_sta->lq);
+	if (lq_sta->dbg_fixed_rate) {
+		rs_fill_link_cmd(NULL, lq_sta, lq_sta->dbg_fixed_rate);
 		iwl_send_lq_cmd(lq_sta->drv, &lq_sta->lq, CMD_ASYNC);
 	}
 
@@ -2561,9 +2498,9 @@ static ssize_t rs_sta_dbgfs_scale_table_read(struct file *file,
 	desc += sprintf(buff+desc, "sta_id %d\n", lq_sta->lq.sta_id);
 	desc += sprintf(buff+desc, "failed=%d success=%d rate=0%X\n",
 			lq_sta->total_failed, lq_sta->total_success,
-			lq_sta->active_rate);
+			lq_sta->active_legacy_rate);
 	desc += sprintf(buff+desc, "fixed rate 0x%X\n",
-			lq_sta->dbg_fixed.rate_n_flags);
+			lq_sta->dbg_fixed_rate);
 	desc += sprintf(buff+desc, "general:"
 		"flags=0x%X mimo-d=%d s-ant0x%x d-ant=0x%x\n",
 		lq_sta->lq.general_params.flags,
@@ -2613,7 +2550,7 @@ static ssize_t rs_sta_dbgfs_stats_table_read(struct file *file,
 				lq_sta->lq_info[i].is_SGI,
 				lq_sta->lq_info[i].is_fat,
 				lq_sta->lq_info[i].is_dup,
-				lq_sta->lq_info[i].current_rate.rate_n_flags);
+				lq_sta->lq_info[i].current_rate);
 		for (j = 0; j < IWL_RATE_COUNT; j++) {
 			desc += sprintf(buff+desc,
 				"counter=%d success=%d %%=%d\n",
@@ -2640,11 +2577,9 @@ static void rs_add_debugfs(void *priv, void *priv_sta,
 	lq_sta->rs_sta_dbgfs_stats_table_file =
 		debugfs_create_file("rate_stats_table", 0600, dir,
 			lq_sta, &rs_sta_dbgfs_stats_table_ops);
-#ifdef CONFIG_IWL4965_HT
 	lq_sta->rs_sta_dbgfs_tx_agg_tid_en_file =
 		debugfs_create_u8("tx_agg_tid_enable", 0600, dir,
 		&lq_sta->tx_agg_tid_en);
-#endif
 
 }
 
@@ -2653,9 +2588,7 @@ static void rs_remove_debugfs(void *priv, void *priv_sta)
 	struct iwl4965_lq_sta *lq_sta = priv_sta;
 	debugfs_remove(lq_sta->rs_sta_dbgfs_scale_table_file);
 	debugfs_remove(lq_sta->rs_sta_dbgfs_stats_table_file);
-#ifdef CONFIG_IWL4965_HT
 	debugfs_remove(lq_sta->rs_sta_dbgfs_tx_agg_tid_en_file);
-#endif
 }
 #endif
 
@@ -2703,7 +2636,7 @@ int iwl4965_fill_rs_info(struct ieee80211_hw *hw, char *buf, u8 sta_id)
 	lq_sta = (void *)sta->rate_ctrl_priv;
 
 	lq_type = lq_sta->lq_info[lq_sta->active_tbl].lq_type;
-	antenna = lq_sta->lq_info[lq_sta->active_tbl].antenna_type;
+	antenna = lq_sta->lq_info[lq_sta->active_tbl].ant_type;
 
 	if (is_legacy(lq_type))
 		i = IWL_RATE_54M_INDEX;
@@ -2715,7 +2648,7 @@ int iwl4965_fill_rs_info(struct ieee80211_hw *hw, char *buf, u8 sta_id)
 		int active = lq_sta->active_tbl;
 
 		cnt +=
-		    sprintf(&buf[cnt], " %2dMbs: ", iwl4965_rates[i].ieee / 2);
+		    sprintf(&buf[cnt], " %2dMbs: ", iwl_rates[i].ieee / 2);
 
 		mask = (1ULL << (IWL_RATE_MAX_WINDOW - 1));
 		for (j = 0; j < IWL_RATE_MAX_WINDOW; j++, mask >>= 1)
@@ -2726,7 +2659,7 @@ int iwl4965_fill_rs_info(struct ieee80211_hw *hw, char *buf, u8 sta_id)
 		samples += lq_sta->lq_info[active].win[i].counter;
 		good += lq_sta->lq_info[active].win[i].success_counter;
 		success += lq_sta->lq_info[active].win[i].success_counter *
-			   iwl4965_rates[i].ieee;
+			   iwl_rates[i].ieee;
 
 		if (lq_sta->lq_info[active].win[i].stamp) {
 			int delta =
@@ -2746,10 +2679,11 @@ int iwl4965_fill_rs_info(struct ieee80211_hw *hw, char *buf, u8 sta_id)
 		i = j;
 	}
 
-	/* Display the average rate of all samples taken.
-	 *
-	 * NOTE:  We multiply # of samples by 2 since the IEEE measurement
-	 * added from iwl4965_rates is actually 2X the rate */
+	/*
+	 * Display the average rate of all samples taken.
+	 * NOTE: We multiply # of samples by 2 since the IEEE measurement
+	 * added from iwl_rates is actually 2X the rate.
+	 */
 	if (samples)
 		cnt += sprintf(&buf[cnt],
 			 "\nAverage rate is %3d.%02dMbs over last %4dms\n"
@@ -2767,13 +2701,6 @@ int iwl4965_fill_rs_info(struct ieee80211_hw *hw, char *buf, u8 sta_id)
 	return cnt;
 }
 
-void iwl4965_rate_scale_init(struct ieee80211_hw *hw, s32 sta_id)
-{
-	struct iwl_priv *priv = hw->priv;
-
-	priv->lq_mngr.lq_ready = 1;
-}
-
 int iwl4965_rate_control_register(void)
 {
 	return ieee80211_rate_control_register(&rs_ops);
diff --git a/drivers/net/wireless/iwlwifi/iwl-4965-rs.h b/drivers/net/wireless/iwlwifi/iwl-4965-rs.h
index 866e378aa385..9b9972885aa5 100644
--- a/drivers/net/wireless/iwlwifi/iwl-4965-rs.h
+++ b/drivers/net/wireless/iwlwifi/iwl-4965-rs.h
@@ -27,12 +27,13 @@
 #ifndef __iwl_4965_rs_h__
 #define __iwl_4965_rs_h__
 
-#include "iwl-4965.h"
+#include "iwl-dev.h"
 
-struct iwl4965_rate_info {
+struct iwl_rate_info {
 	u8 plcp;	/* uCode API:  IWL_RATE_6M_PLCP, etc. */
 	u8 plcp_siso;	/* uCode API:  IWL_RATE_SISO_6M_PLCP, etc. */
-	u8 plcp_mimo;	/* uCode API:  IWL_RATE_MIMO_6M_PLCP, etc. */
+	u8 plcp_mimo2;	/* uCode API:  IWL_RATE_MIMO2_6M_PLCP, etc. */
+	u8 plcp_mimo3;  /* uCode API:  IWL_RATE_MIMO3_6M_PLCP, etc. */
 	u8 ieee;	/* MAC header:  IWL_RATE_6M_IEEE, etc. */
 	u8 prev_ieee;    /* previous rate in IEEE speeds */
 	u8 next_ieee;    /* next rate in IEEE speeds */
@@ -44,7 +45,7 @@ struct iwl4965_rate_info {
 
 /*
  * These serve as indexes into
- * struct iwl4965_rate_info iwl4965_rates[IWL_RATE_COUNT];
+ * struct iwl_rate_info iwl_rates[IWL_RATE_COUNT];
  */
 enum {
 	IWL_RATE_1M_INDEX = 0,
@@ -60,9 +61,9 @@ enum {
 	IWL_RATE_48M_INDEX,
 	IWL_RATE_54M_INDEX,
 	IWL_RATE_60M_INDEX,
-	IWL_RATE_COUNT,
+	IWL_RATE_COUNT, /*FIXME:RS:change to IWL_RATE_INDEX_COUNT,*/
 	IWL_RATE_INVM_INDEX = IWL_RATE_COUNT,
-	IWL_RATE_INVALID = IWL_RATE_INVM_INDEX
+	IWL_RATE_INVALID = IWL_RATE_COUNT,
 };
 
 enum {
@@ -97,11 +98,13 @@ enum {
 	IWL_RATE_36M_PLCP = 11,
 	IWL_RATE_48M_PLCP = 1,
 	IWL_RATE_54M_PLCP = 3,
-	IWL_RATE_60M_PLCP = 3,
+	IWL_RATE_60M_PLCP = 3,/*FIXME:RS:should be removed*/
 	IWL_RATE_1M_PLCP  = 10,
 	IWL_RATE_2M_PLCP  = 20,
 	IWL_RATE_5M_PLCP  = 55,
 	IWL_RATE_11M_PLCP = 110,
+	/*FIXME:RS:change to IWL_RATE_LEGACY_??M_PLCP */
+	/*FIXME:RS:add IWL_RATE_LEGACY_INVM_PLCP = 0,*/
 };
 
 /* 4965 uCode API values for OFDM high-throughput (HT) bit rates */
@@ -114,16 +117,25 @@ enum {
 	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_MIMO2_6M_PLCP  = 0x8,
+	IWL_RATE_MIMO2_12M_PLCP = 0x9,
+	IWL_RATE_MIMO2_18M_PLCP = 0xa,
+	IWL_RATE_MIMO2_24M_PLCP = 0xb,
+	IWL_RATE_MIMO2_36M_PLCP = 0xc,
+	IWL_RATE_MIMO2_48M_PLCP = 0xd,
+	IWL_RATE_MIMO2_54M_PLCP = 0xe,
+	IWL_RATE_MIMO2_60M_PLCP = 0xf,
+	IWL_RATE_MIMO3_6M_PLCP  = 0x10,
+	IWL_RATE_MIMO3_12M_PLCP = 0x11,
+	IWL_RATE_MIMO3_18M_PLCP = 0x12,
+	IWL_RATE_MIMO3_24M_PLCP = 0x13,
+	IWL_RATE_MIMO3_36M_PLCP = 0x14,
+	IWL_RATE_MIMO3_48M_PLCP = 0x15,
+	IWL_RATE_MIMO3_54M_PLCP = 0x16,
+	IWL_RATE_MIMO3_60M_PLCP = 0x17,
 	IWL_RATE_SISO_INVM_PLCP,
-	IWL_RATE_MIMO_INVM_PLCP = IWL_RATE_SISO_INVM_PLCP,
+	IWL_RATE_MIMO2_INVM_PLCP = IWL_RATE_SISO_INVM_PLCP,
+	IWL_RATE_MIMO3_INVM_PLCP = IWL_RATE_SISO_INVM_PLCP,
 };
 
 /* MAC header values for bit rates */
@@ -196,11 +208,11 @@ enum {
 /* possible actions when in legacy mode */
 #define IWL_LEGACY_SWITCH_ANTENNA	0
 #define IWL_LEGACY_SWITCH_SISO		1
-#define IWL_LEGACY_SWITCH_MIMO	        2
+#define IWL_LEGACY_SWITCH_MIMO2		2
 
 /* possible actions when in siso mode */
 #define IWL_SISO_SWITCH_ANTENNA		0
-#define IWL_SISO_SWITCH_MIMO		1
+#define IWL_SISO_SWITCH_MIMO2		1
 #define IWL_SISO_SWITCH_GI		2
 
 /* possible actions when in mimo mode */
@@ -208,6 +220,10 @@ enum {
 #define IWL_MIMO_SWITCH_ANTENNA_B	1
 #define IWL_MIMO_SWITCH_GI		2
 
+/*FIXME:RS:separate MIMO2/3 transitions*/
+
+/*FIXME:RS:add posible acctions for MIMO3*/
+
 #define IWL_ACTION_LIMIT		3	/* # possible actions */
 
 #define LQ_SIZE		2	/* 2 mode tables:  "Active" and "Search" */
@@ -224,43 +240,52 @@ enum {
 #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))
 
-extern const struct iwl4965_rate_info iwl4965_rates[IWL_RATE_COUNT];
+extern const struct iwl_rate_info iwl_rates[IWL_RATE_COUNT];
 
-enum iwl4965_table_type {
+enum iwl_table_type {
 	LQ_NONE,
 	LQ_G,		/* legacy types */
 	LQ_A,
 	LQ_SISO,	/* high-throughput types */
-	LQ_MIMO,
+	LQ_MIMO2,
+	LQ_MIMO3,
 	LQ_MAX,
 };
 
 #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_siso(tbl) ((tbl) == LQ_SISO)
+#define is_mimo2(tbl) ((tbl) == LQ_MIMO2)
+#define is_mimo3(tbl) ((tbl) == LQ_MIMO3)
+#define is_mimo(tbl) (is_mimo2(tbl) || is_mimo3(tbl))
 #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))
-
-/* 4965 has 2 antennas/chains for Tx (but 3 for Rx) */
-enum iwl4965_antenna_type {
-	ANT_NONE,
-	ANT_MAIN,
-	ANT_AUX,
-	ANT_BOTH,
-};
+#define is_a_band(tbl) ((tbl) == LQ_A)
+#define is_g_and(tbl) ((tbl) == LQ_G)
+
+#define	ANT_NONE	0x0
+#define	ANT_A		BIT(0)
+#define	ANT_B		BIT(1)
+#define	ANT_AB		(ANT_A | ANT_B)
+#define ANT_C		BIT(2)
+#define	ANT_AC		(ANT_A | ANT_C)
+#define ANT_BC		(ANT_B | ANT_C)
+#define ANT_ABC		(ANT_AB | ANT_C)
+
+static inline u8 num_of_ant(u8 mask)
+{
+	return  !!((mask) & ANT_A) +
+		!!((mask) & ANT_B) +
+		!!((mask) & ANT_C);
+}
 
 static inline u8 iwl4965_get_prev_ieee_rate(u8 rate_index)
 {
-	u8 rate = iwl4965_rates[rate_index].prev_ieee;
+	u8 rate = iwl_rates[rate_index].prev_ieee;
 
 	if (rate == IWL_RATE_INVALID)
 		rate = rate_index;
 	return rate;
 }
 
-extern int iwl4965_hwrate_to_plcp_idx(u32 rate_n_flags);
-
 /**
  * iwl4965_fill_rs_info - Fill an output text buffer with the rate representation
  *
@@ -271,14 +296,6 @@ extern int iwl4965_hwrate_to_plcp_idx(u32 rate_n_flags);
 extern int iwl4965_fill_rs_info(struct ieee80211_hw *, char *buf, u8 sta_id);
 
 /**
- * iwl4965_rate_scale_init - Initialize the rate scale table based on assoc info
- *
- * The specific throughput table used is based on the type of network
- * the associated with, including A, B, G, and G w/ TGG protection
- */
-extern void iwl4965_rate_scale_init(struct ieee80211_hw *hw, s32 sta_id);
-
-/**
  * iwl4965_rate_control_register - Register the rate control algorithm callbacks
  *
  * Since the rate control algorithm is hardware specific, there is no need
diff --git a/drivers/net/wireless/iwlwifi/iwl-4965.c b/drivers/net/wireless/iwlwifi/iwl-4965.c
index de330ae0ca95..9afecb813716 100644
--- a/drivers/net/wireless/iwlwifi/iwl-4965.c
+++ b/drivers/net/wireless/iwlwifi/iwl-4965.c
@@ -39,81 +39,33 @@
 #include <asm/unaligned.h>
 
 #include "iwl-eeprom.h"
-#include "iwl-4965.h"
+#include "iwl-dev.h"
 #include "iwl-core.h"
 #include "iwl-io.h"
 #include "iwl-helpers.h"
+#include "iwl-calib.h"
+#include "iwl-sta.h"
+
+static int iwl4965_send_tx_power(struct iwl_priv *priv);
+static int iwl4965_hw_get_temperature(const struct iwl_priv *priv);
+
+/* 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 "-2"
+
 
 /* module parameters */
 static struct iwl_mod_params iwl4965_mod_params = {
-	.num_of_queues = IWL4965_MAX_NUM_QUEUES,
+	.num_of_queues = IWL49_NUM_QUEUES,
+	.num_of_ampdu_queues = IWL49_NUM_AMPDU_QUEUES,
 	.enable_qos = 1,
 	.amsdu_size_8K = 1,
+	.restart_fw = 1,
 	/* the rest are 0 by default */
 };
 
-static void iwl4965_hw_card_show_info(struct iwl_priv *priv);
-
-#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 iwl4965_rate_info iwl4965_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 */
-};
-
-#ifdef CONFIG_IWL4965_HT
-
-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
-};
-
-#endif	/*CONFIG_IWL4965_HT */
-
 /* check contents of special bootstrap uCode SRAM */
 static int iwl4965_verify_bsm(struct iwl_priv *priv)
 {
@@ -192,15 +144,18 @@ static int iwl4965_load_bsm(struct iwl_priv *priv)
 
 	IWL_DEBUG_INFO("Begin load bsm\n");
 
+	priv->ucode_type = UCODE_RT;
+
 	/* 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 ... host DRAM physical address bits 35:4 for 4965.
-	 * NOTE:  iwl4965_initialize_alive_start() will replace these values,
+	 * NOTE:  iwl_init_alive_start() will replace these values,
 	 *        after the "initialize" uCode has run, to point to
-	 *        runtime/protocol instructions and backup data cache. */
+	 *        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;
@@ -259,271 +214,134 @@ static int iwl4965_load_bsm(struct iwl_priv *priv)
 	return 0;
 }
 
-static int iwl4965_init_drv(struct iwl_priv *priv)
+/**
+ * iwl4965_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 iwl4965_set_ucode_ptrs(struct iwl_priv *priv)
 {
-	int ret;
-	int i;
-
-	priv->antenna = (enum iwl4965_antenna)priv->cfg->mod_params->antenna;
-	priv->retry_rate = 1;
-	priv->ibss_beacon = NULL;
-
-	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);
-
-	priv->shared_virt = pci_alloc_consistent(priv->pci_dev,
-					sizeof(struct iwl4965_shared),
-					&priv->shared_phys);
-
-	if (!priv->shared_virt) {
-		ret = -ENOMEM;
-		goto err;
-	}
-
-	memset(priv->shared_virt, 0, sizeof(struct iwl4965_shared));
-
-
-	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);
-
-	/* Clear the driver's (not device's) station table */
-	iwlcore_clear_stations_table(priv);
-
-	priv->data_retry_limit = -1;
-	priv->ieee_channels = NULL;
-	priv->ieee_rates = NULL;
-	priv->band = IEEE80211_BAND_2GHZ;
-
-	priv->iw_mode = IEEE80211_IF_TYPE_STA;
-
-	priv->use_ant_b_for_management_frame = 1; /* start with ant B */
-	priv->valid_antenna = 0x7;	/* assume all 3 connected */
-	priv->ps_mode = IWL_MIMO_PS_NONE;
-
-	/* Choose which receivers/antennas to use */
-	iwl4965_set_rxon_chain(priv);
-
-	iwlcore_reset_qos(priv);
-
-	priv->qos_data.qos_active = 0;
-	priv->qos_data.qos_cap.val = 0;
-
-	iwlcore_set_rxon_channel(priv, IEEE80211_BAND_2GHZ, 6);
+	dma_addr_t pinst;
+	dma_addr_t pdata;
+	unsigned long flags;
+	int ret = 0;
 
-	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;
+	/* bits 35:4 for 4965 */
+	pinst = priv->ucode_code.p_addr >> 4;
+	pdata = priv->ucode_data_backup.p_addr >> 4;
 
-	ret = iwl_init_channel_map(priv);
+	spin_lock_irqsave(&priv->lock, flags);
+	ret = iwl_grab_nic_access(priv);
 	if (ret) {
-		IWL_ERROR("initializing regulatory failed: %d\n", ret);
-		goto err;
+		spin_unlock_irqrestore(&priv->lock, flags);
+		return ret;
 	}
 
-	ret = iwl4965_init_geos(priv);
-	if (ret) {
-		IWL_ERROR("initializing geos failed: %d\n", ret);
-		goto err_free_channel_map;
-	}
+	/* Tell bootstrap uCode where to find image to load */
+	iwl_write_prph(priv, BSM_DRAM_INST_PTR_REG, pinst);
+	iwl_write_prph(priv, BSM_DRAM_DATA_PTR_REG, pdata);
+	iwl_write_prph(priv, BSM_DRAM_DATA_BYTECOUNT_REG,
+				 priv->ucode_data.len);
 
-	ret = ieee80211_register_hw(priv->hw);
-	if (ret) {
-		IWL_ERROR("Failed to register network device (error %d)\n",
-				ret);
-		goto err_free_geos;
-	}
+	/* Inst bytecount must be last to set up, bit 31 signals uCode
+	 *   that all new ptr/size info is in place */
+	iwl_write_prph(priv, BSM_DRAM_INST_BYTECOUNT_REG,
+				 priv->ucode_code.len | BSM_DRAM_INST_LOAD);
+	iwl_release_nic_access(priv);
 
-	priv->hw->conf.beacon_int = 100;
-	priv->mac80211_registered = 1;
+	spin_unlock_irqrestore(&priv->lock, flags);
 
-	return 0;
+	IWL_DEBUG_INFO("Runtime uCode pointers are set.\n");
 
-err_free_geos:
-	iwl4965_free_geos(priv);
-err_free_channel_map:
-	iwl_free_channel_map(priv);
-err:
 	return ret;
 }
 
-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_IWL4965_HT
-	if (!priv->current_ht_config.is_ht ||
-	    (priv->current_ht_config.supp_mcs_set[1] == 0) ||
-	    (priv->ps_mode == IWL_MIMO_PS_STATIC))
-		return 1;
-#else
-	return 1;
-#endif	/*CONFIG_IWL4965_HT */
-	return 0;
-}
-
-int iwl4965_hwrate_to_plcp_idx(u32 rate_n_flags)
-{
-	int idx = 0;
-
-	/* 4965 HT rate format */
-	if (rate_n_flags & RATE_MCS_HT_MSK) {
-		idx = (rate_n_flags & 0xff);
-
-		if (idx >= IWL_RATE_MIMO_6M_PLCP)
-			idx = idx - IWL_RATE_MIMO_6M_PLCP;
-
-		idx += IWL_FIRST_OFDM_RATE;
-		/* skip 9M not supported in ht*/
-		if (idx >= IWL_RATE_9M_INDEX)
-			idx += 1;
-		if ((idx >= IWL_FIRST_OFDM_RATE) && (idx <= IWL_LAST_OFDM_RATE))
-			return idx;
-
-	/* 4965 legacy rate format, search for match in table */
-	} else {
-		for (idx = 0; idx < ARRAY_SIZE(iwl4965_rates); idx++)
-			if (iwl4965_rates[idx].plcp == (rate_n_flags & 0xFF))
-				return idx;
+/**
+ * iwl4965_init_alive_start - Called after REPLY_ALIVE notification received
+ *
+ * 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 iwl4965_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_hw_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 (iwl4965_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;
 
-	return -1;
+restart:
+	queue_work(priv->workqueue, &priv->restart);
 }
 
-/**
- * translate ucode response to mac80211 tx status control values
- */
-void iwl4965_hwrate_to_tx_control(struct iwl_priv *priv, u32 rate_n_flags,
-				  struct ieee80211_tx_control *control)
+static int is_fat_channel(__le32 rxon_flags)
 {
-	int rate_index;
-
-	control->antenna_sel_tx =
-		((rate_n_flags & RATE_MCS_ANT_AB_MSK) >> RATE_MCS_ANT_POS);
-	if (rate_n_flags & RATE_MCS_HT_MSK)
-		control->flags |= IEEE80211_TXCTL_OFDM_HT;
-	if (rate_n_flags & RATE_MCS_GF_MSK)
-		control->flags |= IEEE80211_TXCTL_GREEN_FIELD;
-	if (rate_n_flags & RATE_MCS_FAT_MSK)
-		control->flags |= IEEE80211_TXCTL_40_MHZ_WIDTH;
-	if (rate_n_flags & RATE_MCS_DUP_MSK)
-		control->flags |= IEEE80211_TXCTL_DUP_DATA;
-	if (rate_n_flags & RATE_MCS_SGI_MSK)
-		control->flags |= IEEE80211_TXCTL_SHORT_GI;
-	/* since iwl4965_hwrate_to_plcp_idx is band indifferent, we always use
-	 * IEEE80211_BAND_2GHZ band as it contains all the rates */
-	rate_index = iwl4965_hwrate_to_plcp_idx(rate_n_flags);
-	if (rate_index == -1)
-		control->tx_rate = NULL;
-	else
-		control->tx_rate =
-			&priv->bands[IEEE80211_BAND_2GHZ].bitrates[rate_index];
+	return (rxon_flags & RXON_FLG_CHANNEL_MODE_PURE_40_MSK) ||
+		(rxon_flags & RXON_FLG_CHANNEL_MODE_MIXED_MSK);
 }
 
 /*
- * 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.
+ * EEPROM handlers
  */
-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 iwl4965_hw_rxq_stop(struct iwl_priv *priv)
+static int iwl4965_eeprom_check_version(struct iwl_priv *priv)
 {
-	int rc;
-	unsigned long flags;
+	u16 eeprom_ver;
+	u16 calib_ver;
 
-	spin_lock_irqsave(&priv->lock, flags);
-	rc = iwl_grab_nic_access(priv);
-	if (rc) {
-		spin_unlock_irqrestore(&priv->lock, flags);
-		return rc;
-	}
+	eeprom_ver = iwl_eeprom_query16(priv, EEPROM_VERSION);
 
-	/* stop Rx DMA */
-	iwl_write_direct32(priv, FH_MEM_RCSR_CHNL0_CONFIG_REG, 0);
-	rc = iwl_poll_direct_bit(priv, FH_MEM_RSSR_RX_STATUS_REG,
-				     (1 << 24), 1000);
-	if (rc < 0)
-		IWL_ERROR("Can't stop Rx DMA.\n");
+	calib_ver = iwl_eeprom_query16(priv, EEPROM_4965_CALIB_VERSION_OFFSET);
 
-	iwl_release_nic_access(priv);
-	spin_unlock_irqrestore(&priv->lock, flags);
+	if (eeprom_ver < EEPROM_4965_EEPROM_VERSION ||
+	    calib_ver < EEPROM_4965_TX_POWER_VERSION)
+		goto err;
 
 	return 0;
-}
-
-u8 iwl4965_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 priv->hw_params.bcast_sta_id;
-
-	spin_lock_irqsave(&priv->sta_lock, flags);
-	for (i = start; i < priv->hw_params.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);
+err:
+	IWL_ERROR("Unsuported EEPROM VER=0x%x < 0x%x CALIB=0x%x < 0x%x\n",
+		  eeprom_ver, EEPROM_4965_EEPROM_VERSION,
+		  calib_ver, EEPROM_4965_TX_POWER_VERSION);
+	return -EINVAL;
 
- 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 iwl4965_set_pwr_src(struct iwl_priv *priv, enum iwl_pwr_src src)
 {
 	int ret;
 	unsigned long flags;
@@ -535,340 +353,130 @@ static int iwl4965_nic_set_pwr_src(struct iwl_priv *priv, int pwr_max)
 		return ret;
 	}
 
-	if (!pwr_max) {
+	if (src == IWL_PWR_SRC_VAUX) {
 		u32 val;
-
 		ret = pci_read_config_dword(priv->pci_dev, PCI_POWER_SOURCE,
-					   &val);
+					    &val);
 
-		if (val & PCI_CFG_PMC_PME_FROM_D3COLD_SUPPORT)
+		if (val & PCI_CFG_PMC_PME_FROM_D3COLD_SUPPORT) {
 			iwl_set_bits_mask_prph(priv, APMG_PS_CTRL_REG,
-				APMG_PS_CTRL_VAL_PWR_SRC_VAUX,
-				~APMG_PS_CTRL_MSK_PWR_SRC);
-	} else
+					       APMG_PS_CTRL_VAL_PWR_SRC_VAUX,
+					       ~APMG_PS_CTRL_MSK_PWR_SRC);
+		}
+	} else {
 		iwl_set_bits_mask_prph(priv, APMG_PS_CTRL_REG,
-			APMG_PS_CTRL_VAL_PWR_SRC_VMAIN,
-			~APMG_PS_CTRL_MSK_PWR_SRC);
-
-	iwl_release_nic_access(priv);
-	spin_unlock_irqrestore(&priv->lock, flags);
-
-	return ret;
-}
-
-static int iwl4965_rx_init(struct iwl_priv *priv, struct iwl4965_rx_queue *rxq)
-{
-	int ret;
-	unsigned long flags;
-	unsigned int rb_size;
-
-	spin_lock_irqsave(&priv->lock, flags);
-	ret = iwl_grab_nic_access(priv);
-	if (ret) {
-		spin_unlock_irqrestore(&priv->lock, flags);
-		return ret;
+				       APMG_PS_CTRL_VAL_PWR_SRC_VMAIN,
+				       ~APMG_PS_CTRL_MSK_PWR_SRC);
 	}
 
-	if (priv->cfg->mod_params->amsdu_size_8K)
-		rb_size = FH_RCSR_RX_CONFIG_REG_VAL_RB_SIZE_8K;
-	else
-		rb_size = FH_RCSR_RX_CONFIG_REG_VAL_RB_SIZE_4K;
-
-	/* Stop Rx DMA */
-	iwl_write_direct32(priv, FH_MEM_RCSR_CHNL0_CONFIG_REG, 0);
-
-	/* Reset driver's Rx queue write index */
-	iwl_write_direct32(priv, FH_RSCSR_CHNL0_RBDCB_WPTR_REG, 0);
-
-	/* Tell device where to find RBD circular buffer in DRAM */
-	iwl_write_direct32(priv, FH_RSCSR_CHNL0_RBDCB_BASE_REG,
-			   rxq->dma_addr >> 8);
-
-	/* Tell device where in DRAM to update its Rx status */
-	iwl_write_direct32(priv, FH_RSCSR_CHNL0_STTS_WPTR_REG,
-			   (priv->shared_phys +
-			    offsetof(struct iwl4965_shared, rb_closed)) >> 4);
-
-	/* Enable Rx DMA, enable host interrupt, Rx buffer size 4k, 256 RBDs */
-	iwl_write_direct32(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 |
-			   rb_size |
-			     /* 0x10 << 4 | */
-			   (RX_QUEUE_SIZE_LOG <<
-			      FH_RCSR_RX_CONFIG_RBDCB_SIZE_BITSHIFT));
-
-	/*
-	 * iwl_write32(priv,CSR_INT_COAL_REG,0);
-	 */
-
-	iwl_release_nic_access(priv);
-	spin_unlock_irqrestore(&priv->lock, flags);
-
-	return 0;
-}
-
-/* Tell 4965 where to find the "keep warm" buffer */
-static int iwl4965_kw_init(struct iwl_priv *priv)
-{
-	unsigned long flags;
-	int rc;
-
-	spin_lock_irqsave(&priv->lock, flags);
-	rc = iwl_grab_nic_access(priv);
-	if (rc)
-		goto out;
-
-	iwl_write_direct32(priv, IWL_FH_KW_MEM_ADDR_REG,
-			     priv->kw.dma_addr >> 4);
 	iwl_release_nic_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 iwl4965_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;
+	return ret;
 }
 
-/**
- * iwl4965_kw_free - Free the "keep warm" buffer
+/*
+ * Activate/Deactivat Tx DMA/FIFO channels according tx fifos mask
+ * must be called under priv->lock and mac access
  */
-static void iwl4965_kw_free(struct iwl_priv *priv)
+static void iwl4965_txq_set_sched(struct iwl_priv *priv, u32 mask)
 {
-	struct pci_dev *dev = priv->pci_dev;
-	struct iwl4965_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));
-	}
+	iwl_write_prph(priv, IWL49_SCD_TXFACT, mask);
 }
 
-/**
- * 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)
+static int iwl4965_apm_init(struct iwl_priv *priv)
 {
-	int rc = 0;
-	int txq_id, slots_num;
-	unsigned long flags;
-
-	iwl4965_kw_free(priv);
-
-	/* Free all tx/cmd queues and keep-warm buffer */
-	iwl4965_hw_txq_ctx_free(priv);
-
-	/* Alloc keep-warm buffer */
-	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_nic_access(priv);
-	if (unlikely(rc)) {
-		IWL_ERROR("TX reset failed");
-		spin_unlock_irqrestore(&priv->lock, flags);
-		goto error_reset;
-	}
-
-	/* Turn off all Tx DMA channels */
-	iwl_write_prph(priv, IWL49_SCD_TXFACT, 0);
-	iwl_release_nic_access(priv);
-	spin_unlock_irqrestore(&priv->lock, flags);
-
-	/* Tell 4965 where to find the keep-warm buffer */
-	rc = iwl4965_kw_init(priv);
-	if (rc) {
-		IWL_ERROR("kw_init failed\n");
-		goto error_reset;
-	}
-
-	/* Alloc and init all (default 16) Tx queues,
-	 * including the command queue (#4) */
-	for (txq_id = 0; txq_id < priv->hw_params.max_txq_num; txq_id++) {
-		slots_num = (txq_id == IWL_CMD_QUEUE_NUM) ?
-					TFD_CMD_SLOTS : TFD_TX_CMD_SLOTS;
-		rc = iwl4965_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:
-	iwl4965_hw_txq_ctx_free(priv);
- error_reset:
-	iwl4965_kw_free(priv);
- error_kw:
-	return rc;
-}
-
-int iwl4965_hw_nic_init(struct iwl_priv *priv)
-{
-	int rc;
-	unsigned long flags;
-	struct iwl4965_rx_queue *rxq = &priv->rxq;
-	u8 rev_id;
-	u32 val;
-	u8 val_link;
-
-	iwl4965_power_init_handle(priv);
+	int ret = 0;
 
-	/* 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);
 
+	/* disable L0s without affecting L1 :don't wait for ICH L0s bug W/A) */
 	iwl_set_bit(priv, CSR_GIO_CHICKEN_BITS,
-		    CSR_GIO_CHICKEN_BITS_REG_BIT_DIS_L0S_EXIT_TIMER);
+			  CSR_GIO_CHICKEN_BITS_REG_BIT_L1A_NO_L0S_RX);
 
+	/* set "initialization complete" bit to move adapter
+	 * D0U* --> D0A* state */
 	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_nic_access(priv);
-	if (rc) {
-		spin_unlock_irqrestore(&priv->lock, flags);
-		return rc;
+	/* wait for clock stabilization */
+	ret = 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 (ret < 0) {
+		IWL_DEBUG_INFO("Failed to init the card\n");
+		goto out;
 	}
 
-	iwl_read_prph(priv, APMG_CLK_CTRL_REG);
+	ret = iwl_grab_nic_access(priv);
+	if (ret)
+		goto out;
 
-	iwl_write_prph(priv, APMG_CLK_CTRL_REG,
-			APMG_CLK_VAL_DMA_CLK_RQT | APMG_CLK_VAL_BSM_CLK_RQT);
-	iwl_read_prph(priv, APMG_CLK_CTRL_REG);
+	/* enable DMA */
+	iwl_write_prph(priv, APMG_CLK_CTRL_REG, APMG_CLK_VAL_DMA_CLK_RQT |
+						APMG_CLK_VAL_BSM_CLK_RQT);
 
 	udelay(20);
 
+	/* disable L1-Active */
 	iwl_set_bits_prph(priv, APMG_PCIDEV_STT_REG,
-				APMG_PCIDEV_STT_VAL_L1_ACT_DIS);
+			  APMG_PCIDEV_STT_VAL_L1_ACT_DIS);
 
 	iwl_release_nic_access(priv);
-	iwl_write32(priv, CSR_INT_COALESCING, 512 / 32);
-	spin_unlock_irqrestore(&priv->lock, flags);
+out:
+	return ret;
+}
 
-	/* 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);
+static void iwl4965_nic_config(struct iwl_priv *priv)
+{
+	unsigned long flags;
+	u32 val;
+	u16 radio_cfg;
+	u8 val_link;
 
-	iwl4965_nic_set_pwr_src(priv, 1);
 	spin_lock_irqsave(&priv->lock, flags);
 
-	if ((rev_id & 0x80) == 0x80 && (rev_id & 0x7f) < 8) {
+	if ((priv->rev_id & 0x80) == 0x80 && (priv->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);
-
-	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);
+	/* L1 is enabled by BIOS */
+	if ((val_link & PCI_LINK_VAL_L1_EN) == PCI_LINK_VAL_L1_EN)
+		/* diable L0S disabled L1A enabled */
+		iwl_set_bit(priv, CSR_GIO_REG, CSR_GIO_REG_VAL_L0S_ENABLED);
+	else
+		/* L0S enabled L1A disabled */
+		iwl_clear_bit(priv, CSR_GIO_REG, CSR_GIO_REG_VAL_L0S_ENABLED);
 
-	spin_lock_irqsave(&priv->lock, flags);
+	radio_cfg = iwl_eeprom_query16(priv, EEPROM_RADIO_CONFIG);
 
-	/* set CSR_HW_CONFIG_REG for uCode use */
+	/* write radio config values to register */
+	if (EEPROM_RF_CFG_TYPE_MSK(radio_cfg) == EEPROM_4965_RF_CFG_TYPE_MAX)
+		iwl_set_bit(priv, CSR_HW_IF_CONFIG_REG,
+			    EEPROM_RF_CFG_TYPE_MSK(radio_cfg) |
+			    EEPROM_RF_CFG_STEP_MSK(radio_cfg) |
+			    EEPROM_RF_CFG_DASH_MSK(radio_cfg));
 
+	/* set CSR_HW_CONFIG_REG for uCode use */
 	iwl_set_bit(priv, CSR_HW_IF_CONFIG_REG,
-		    CSR49_HW_IF_CONFIG_REG_BIT_4965_R |
-		    CSR49_HW_IF_CONFIG_REG_BIT_RADIO_SI |
-		    CSR49_HW_IF_CONFIG_REG_BIT_MAC_SI);
+		    CSR_HW_IF_CONFIG_REG_BIT_RADIO_SI |
+		    CSR_HW_IF_CONFIG_REG_BIT_MAC_SI);
 
-	rc = iwl_grab_nic_access(priv);
-	if (rc < 0) {
-		spin_unlock_irqrestore(&priv->lock, flags);
-		IWL_DEBUG_INFO("Failed to init the card\n");
-		return rc;
-	}
+	priv->calib_info = (struct iwl_eeprom_calib_info *)
+		iwl_eeprom_query_addr(priv, EEPROM_4965_CALIB_TXPOWER_OFFSET);
 
-	iwl_read_prph(priv, APMG_PS_CTRL_REG);
-	iwl_set_bits_prph(priv, APMG_PS_CTRL_REG, APMG_PS_CTRL_VAL_RESET_REQ);
-	udelay(5);
-	iwl_clear_bits_prph(priv, APMG_PS_CTRL_REG, APMG_PS_CTRL_VAL_RESET_REQ);
-
-	iwl_release_nic_access(priv);
 	spin_unlock_irqrestore(&priv->lock, flags);
-
-	iwl4965_hw_card_show_info(priv);
-
-	/* end nic_init */
-
-	/* Allocate the RX queue, or reset if it is already allocated */
-	if (!rxq->bd) {
-		rc = iwl4965_rx_queue_alloc(priv);
-		if (rc) {
-			IWL_ERROR("Unable to initialize Rx queue\n");
-			return -ENOMEM;
-		}
-	} else
-		iwl4965_rx_queue_reset(priv, rxq);
-
-	iwl4965_rx_replenish(priv);
-
-	iwl4965_rx_init(priv, rxq);
-
-	spin_lock_irqsave(&priv->lock, flags);
-
-	rxq->need_update = 1;
-	iwl4965_rx_queue_update_write_ptr(priv, rxq);
-
-	spin_unlock_irqrestore(&priv->lock, flags);
-
-	/* Allocate and init all Tx and Command queues */
-	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 iwl4965_hw_nic_stop_master(struct iwl_priv *priv)
+static int iwl4965_apm_stop_master(struct iwl_priv *priv)
 {
-	int rc = 0;
-	u32 reg_val;
+	int ret = 0;
 	unsigned long flags;
 
 	spin_lock_irqsave(&priv->lock, flags);
@@ -876,64 +484,24 @@ int iwl4965_hw_nic_stop_master(struct iwl_priv *priv)
 	/* 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,
+	ret = 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;
-		}
-	}
+	if (ret < 0)
+		goto out;
 
+out:
 	spin_unlock_irqrestore(&priv->lock, flags);
 	IWL_DEBUG_INFO("stop master\n");
 
-	return rc;
-}
-
-/**
- * iwl4965_hw_txq_ctx_stop - Stop all Tx DMA channels, free Tx queue memory
- */
-void iwl4965_hw_txq_ctx_stop(struct iwl_priv *priv)
-{
-
-	int txq_id;
-	unsigned long flags;
-
-	/* Stop each Tx DMA channel, and wait for it to be idle */
-	for (txq_id = 0; txq_id < priv->hw_params.max_txq_num; txq_id++) {
-		spin_lock_irqsave(&priv->lock, flags);
-		if (iwl_grab_nic_access(priv)) {
-			spin_unlock_irqrestore(&priv->lock, flags);
-			continue;
-		}
-
-		iwl_write_direct32(priv,
-				   IWL_FH_TCSR_CHNL_TX_CONFIG_REG(txq_id), 0x0);
-		iwl_poll_direct_bit(priv, IWL_FH_TSSR_TX_STATUS_REG,
-				    IWL_FH_TSSR_TX_STATUS_REG_MSK_CHNL_IDLE
-				    (txq_id), 200);
-		iwl_release_nic_access(priv);
-		spin_unlock_irqrestore(&priv->lock, flags);
-	}
-
-	/* Deallocate memory for all Tx queues */
-	iwl4965_hw_txq_ctx_free(priv);
+	return ret;
 }
 
-int iwl4965_hw_nic_reset(struct iwl_priv *priv)
+static void iwl4965_apm_stop(struct iwl_priv *priv)
 {
-	int rc = 0;
 	unsigned long flags;
 
-	iwl4965_hw_nic_stop_master(priv);
+	iwl4965_apm_stop_master(priv);
 
 	spin_lock_irqsave(&priv->lock, flags);
 
@@ -942,508 +510,66 @@ int iwl4965_hw_nic_reset(struct iwl_priv *priv)
 	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_nic_access(priv);
-	if (!rc) {
-		iwl_write_prph(priv, APMG_CLK_EN_REG,
-				APMG_CLK_VAL_DMA_CLK_RQT |
-				APMG_CLK_VAL_BSM_CLK_RQT);
-
-		udelay(10);
-
-		iwl_set_bits_prph(priv, APMG_PCIDEV_STT_REG,
-					APMG_PCIDEV_STT_VAL_L1_ACT_DIS);
-
-		iwl_release_nic_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 send a statistics request.
- *
- * 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.
- */
-static void iwl4965_bg_statistics_periodic(unsigned long data)
+static int iwl4965_apm_reset(struct iwl_priv *priv)
 {
-	struct iwl_priv *priv = (struct iwl_priv *)data;
-
-	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
-		return;
-
-	iwl_send_statistics_request(priv, CMD_ASYNC);
-}
-
-#define CT_LIMIT_CONST		259
-#define TM_CT_KILL_THRESHOLD	110
-
-void iwl4965_rf_kill_ct_config(struct iwl_priv *priv)
-{
-	struct iwl4965_ct_kill_config cmd;
-	u32 R1, R2, R3;
-	u32 temp_th;
-	u32 crit_temperature;
-	unsigned long flags;
 	int ret = 0;
+	unsigned long flags;
 
-	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);
-	ret = iwl_send_cmd_pdu(priv, REPLY_CT_KILL_CONFIG_CMD,
-			       sizeof(cmd), &cmd);
-	if (ret)
-		IWL_ERROR("REPLY_CT_KILL_CONFIG_CMD failed\n");
-	else
-		IWL_DEBUG_INFO("REPLY_CT_KILL_CONFIG_CMD succeeded\n");
-}
-
-#ifdef CONFIG_IWL4965_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 iwl4965_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 iwl4965_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);
-	}
+	iwl4965_apm_stop_master(priv);
 
-	else
-		IWL_DEBUG_CALIB("min FA %u < norm FA %u < max FA %u OK\n",
-			 min_false_alarms, false_alarms, max_false_alarms);
+	spin_lock_irqsave(&priv->lock, flags);
 
-	return 0;
-}
+	iwl_set_bit(priv, CSR_RESET, CSR_RESET_REG_FLAG_SW_RESET);
 
-static int iwl4965_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;
-}
+	udelay(10);
 
-/* Prepare a SENSITIVITY_CMD, send to uCode if values have changed */
-static int iwl4965_sensitivity_write(struct iwl_priv *priv, u8 flags)
-{
-	struct iwl4965_sensitivity_cmd cmd ;
-	struct iwl4965_sensitivity_data *data = NULL;
-	struct iwl_host_cmd cmd_out = {
-		.id = SENSITIVITY_CMD,
-		.len = sizeof(struct iwl4965_sensitivity_cmd),
-		.meta.flags = flags,
-		.data = &cmd,
-	};
-	int ret;
+	/* FIXME: put here L1A -L0S w/a */
 
-	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);
-
-	/* Update uCode's "work" table, and copy it to DSP */
-	cmd.control = SENSITIVITY_CMD_CONTROL_WORK_TABLE;
-
-	if (flags & CMD_ASYNC)
-		cmd_out.meta.u.callback = iwl4965_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;
-	}
+	iwl_set_bit(priv, CSR_GP_CNTRL, CSR_GP_CNTRL_REG_FLAG_INIT_DONE);
 
-	/* Copy table for comparison next time */
-	memcpy(&(priv->sensitivity_tbl[0]), &(cmd.table[0]),
-	       sizeof(u16)*HD_TABLE_SIZE);
+	ret = iwl_poll_bit(priv, CSR_RESET,
+			  CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY,
+			  CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY, 25);
 
-	ret = iwl_send_cmd(priv, &cmd_out);
 	if (ret)
-		IWL_ERROR("SENSITIVITY_CMD failed\n");
-
-	return ret;
-}
-
-void iwl4965_init_sensitivity(struct iwl_priv *priv, u8 flags, u8 force)
-{
-	struct iwl4965_sensitivity_data *data = NULL;
-	int i;
-	int ret  = 0;
-
-	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 iwl4965_sensitivity_data));
+		goto out;
 
-	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;
+	udelay(10);
 
-	for (i = 0; i < 10; i++)
-		data->nrg_value[i] = 0;
+	ret = iwl_grab_nic_access(priv);
+	if (ret)
+		goto out;
+	/* Enable DMA and BSM Clock */
+	iwl_write_prph(priv, APMG_CLK_EN_REG, APMG_CLK_VAL_DMA_CLK_RQT |
+					      APMG_CLK_VAL_BSM_CLK_RQT);
 
-	for (i = 0; i < NRG_NUM_PREV_STAT_L; i++)
-		data->nrg_silence_rssi[i] = 0;
+	udelay(10);
 
-	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;
+	/* disable L1A */
+	iwl_set_bits_prph(priv, APMG_PCIDEV_STT_REG,
+			  APMG_PCIDEV_STT_VAL_L1_ACT_DIS);
 
-	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;
+	iwl_release_nic_access(priv);
 
-	/* Clear prior Sensitivity command data to force send to uCode */
-	if (force)
-		memset(&(priv->sensitivity_tbl[0]), 0,
-		    sizeof(u16)*HD_TABLE_SIZE);
+	clear_bit(STATUS_HCMD_ACTIVE, &priv->status);
+	wake_up_interruptible(&priv->wait_command_queue);
 
-	ret |= iwl4965_sensitivity_write(priv, flags);
-	IWL_DEBUG_CALIB("<<return 0x%X\n", ret);
+out:
+	spin_unlock_irqrestore(&priv->lock, flags);
 
-	return;
+	return ret;
 }
 
-
 /* 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)
+static void iwl4965_chain_noise_reset(struct iwl_priv *priv)
 {
-	struct iwl4965_chain_noise_data *data = NULL;
+	struct iwl_chain_noise_data *data = &(priv->chain_noise_data);
 
-	data = &(priv->chain_noise_data);
 	if ((data->state == IWL_CHAIN_NOISE_ALIVE) && iwl_is_associated(priv)) {
 		struct iwl4965_calibration_cmd cmd;
 
@@ -1452,388 +578,89 @@ void iwl4965_chain_noise_reset(struct iwl_priv *priv)
 		cmd.diff_gain_a = 0;
 		cmd.diff_gain_b = 0;
 		cmd.diff_gain_c = 0;
-		iwl_send_cmd_pdu_async(priv, REPLY_PHY_CALIBRATION_CMD,
-				 sizeof(cmd), &cmd, NULL);
-		msleep(4);
+		if (iwl_send_cmd_pdu(priv, REPLY_PHY_CALIBRATION_CMD,
+				 sizeof(cmd), &cmd))
+			IWL_ERROR("Could not send REPLY_PHY_CALIBRATION_CMD\n");
 		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 iwl4965_notif_statistics *stat_resp)
+static void iwl4965_gain_computation(struct iwl_priv *priv,
+		u32 *average_noise,
+		u16 min_average_noise_antenna_i,
+		u32 min_average_noise)
 {
-	struct iwl4965_chain_noise_data *data = NULL;
-	int ret = 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");
-		}
+	int i, ret;
+	struct iwl_chain_noise_data *data = &priv->chain_noise_data;
 
-		IWL_DEBUG_CALIB("active_chains (bitwise) = 0x%x\n",
-				active_chains);
+	data->delta_gain_code[min_average_noise_antenna_i] = 0;
 
-		/* Save for use within RXON, TX, SCAN commands, etc. */
-		priv->valid_antenna = active_chains;
+	for (i = 0; i < NUM_RX_CHAINS; i++) {
+		s32 delta_g = 0;
 
-		/* 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] ==
+		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 iwl4965_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];
-			ret = iwl_send_cmd_pdu(priv, REPLY_PHY_CALIBRATION_CMD,
-					      sizeof(cmd), &cmd);
-			if (ret)
-				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;
+			delta_g = average_noise[i] - min_average_noise;
+			data->delta_gain_code[i] = (u8)((delta_g * 10) / 15);
+			data->delta_gain_code[i] =
+				min(data->delta_gain_code[i],
+				(u8) 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;
 		}
-		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 iwl4965_notif_statistics *resp)
-{
-	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 iwl4965_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;
-	int ret;
-
-	data = &(priv->sensitivity_data);
-
-	if (!iwl_is_associated(priv)) {
-		IWL_DEBUG_CALIB("<< - not associated\n");
-		return;
 	}
+	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]);
 
-	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;
-	}
+	/* Differential gain gets sent to uCode only once */
+	if (!data->radio_write) {
+		struct iwl4965_calibration_cmd cmd;
+		data->radio_write = 1;
 
-	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;
-	}
+		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];
+		ret = iwl_send_cmd_pdu(priv, REPLY_PHY_CALIBRATION_CMD,
+				      sizeof(cmd), &cmd);
+		if (ret)
+			IWL_DEBUG_CALIB("fail sending cmd "
+				     "REPLY_PHY_CALIBRATION_CMD \n");
 
-	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;
-	}
+		/* TODO we might want recalculate
+		 * rx_chain in rxon cmd */
 
-	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;
+		/* Mark so we run this algo only once! */
+		data->state = IWL_CHAIN_NOISE_CALIBRATED;
 	}
-
-	/* 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);
-	ret = iwl4965_sensitivity_write(priv, CMD_ASYNC);
-
-	return;
+	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;
 }
 
-static void iwl4965_bg_sensitivity_work(struct work_struct *work)
+static void iwl4965_rts_tx_cmd_flag(struct ieee80211_tx_info *info,
+			__le32 *tx_flags)
 {
-	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);
+	if (info->flags & IEEE80211_TX_CTL_USE_RTS_CTS) {
+		*tx_flags |= TX_CMD_FLG_RTS_MSK;
+		*tx_flags &= ~TX_CMD_FLG_CTS_MSK;
+	} else if (info->flags & IEEE80211_TX_CTL_USE_CTS_PROTECT) {
+		*tx_flags &= ~TX_CMD_FLG_RTS_MSK;
+		*tx_flags |= TX_CMD_FLG_CTS_MSK;
 	}
-
-	mutex_unlock(&priv->mutex);
-	return;
 }
-#endif /*CONFIG_IWL4965_SENSITIVITY*/
 
 static void iwl4965_bg_txpower_work(struct work_struct *work)
 {
@@ -1853,7 +680,7 @@ static void iwl4965_bg_txpower_work(struct work_struct *work)
 	/* Regardless of if we are assocaited, we must reconfigure the
 	 * TX power since frames can be sent on non-radar channels while
 	 * not associated */
-	iwl4965_hw_reg_send_txpower(priv);
+	iwl4965_send_tx_power(priv);
 
 	/* Update last_temperature to keep is_calib_needed from running
 	 * when it isn't needed... */
@@ -1880,7 +707,7 @@ static void iwl4965_set_wr_ptrs(struct iwl_priv *priv, int txq_id, u32 index)
  * NOTE:  Acquire priv->lock before calling this function !
  */
 static void iwl4965_tx_queue_set_status(struct iwl_priv *priv,
-					struct iwl4965_tx_queue *txq,
+					struct iwl_tx_queue *txq,
 					int tx_fifo_id, int scd_retry)
 {
 	int txq_id = txq->q.id;
@@ -1890,11 +717,11 @@ static void iwl4965_tx_queue_set_status(struct iwl_priv *priv,
 
 	/* Set up and activate */
 	iwl_write_prph(priv, IWL49_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);
+			 (active << IWL49_SCD_QUEUE_STTS_REG_POS_ACTIVE) |
+			 (tx_fifo_id << IWL49_SCD_QUEUE_STTS_REG_POS_TXF) |
+			 (scd_retry << IWL49_SCD_QUEUE_STTS_REG_POS_WSL) |
+			 (scd_retry << IWL49_SCD_QUEUE_STTS_REG_POS_SCD_ACK) |
+			 IWL49_SCD_QUEUE_STTS_REG_MSK);
 
 	txq->sched_retry = scd_retry;
 
@@ -1908,22 +735,12 @@ static const u16 default_queue_to_tx_fifo[] = {
 	IWL_TX_FIFO_AC2,
 	IWL_TX_FIFO_AC1,
 	IWL_TX_FIFO_AC0,
-	IWL_CMD_FIFO_NUM,
+	IWL49_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)
+static int iwl4965_alive_notify(struct iwl_priv *priv)
 {
 	u32 a;
 	int i = 0;
@@ -1932,15 +749,6 @@ int iwl4965_alive_notify(struct iwl_priv *priv)
 
 	spin_lock_irqsave(&priv->lock, flags);
 
-#ifdef CONFIG_IWL4965_SENSITIVITY
-	memset(&(priv->sensitivity_data), 0,
-	       sizeof(struct iwl4965_sensitivity_data));
-	memset(&(priv->chain_noise_data), 0,
-	       sizeof(struct iwl4965_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_IWL4965_SENSITIVITY*/
 	ret = iwl_grab_nic_access(priv);
 	if (ret) {
 		spin_unlock_irqrestore(&priv->lock, flags);
@@ -1949,10 +757,10 @@ int iwl4965_alive_notify(struct iwl_priv *priv)
 
 	/* Clear 4965's internal Tx Scheduler data base */
 	priv->scd_base_addr = iwl_read_prph(priv, IWL49_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)
+	a = priv->scd_base_addr + IWL49_SCD_CONTEXT_DATA_OFFSET;
+	for (; a < priv->scd_base_addr + IWL49_SCD_TX_STTS_BITMAP_OFFSET; a += 4)
 		iwl_write_targ_mem(priv, a, 0);
-	for (; a < priv->scd_base_addr + SCD_TRANSLATE_TBL_OFFSET; a += 4)
+	for (; a < priv->scd_base_addr + IWL49_SCD_TRANSLATE_TBL_OFFSET; a += 4)
 		iwl_write_targ_mem(priv, a, 0);
 	for (; a < sizeof(u16) * priv->hw_params.max_txq_num; a += 4)
 		iwl_write_targ_mem(priv, a, 0);
@@ -1974,160 +782,109 @@ int iwl4965_alive_notify(struct iwl_priv *priv)
 
 		/* Max Tx Window size for Scheduler-ACK mode */
 		iwl_write_targ_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);
+				IWL49_SCD_CONTEXT_QUEUE_OFFSET(i),
+				(SCD_WIN_SIZE <<
+				IWL49_SCD_QUEUE_CTX_REG1_WIN_SIZE_POS) &
+				IWL49_SCD_QUEUE_CTX_REG1_WIN_SIZE_MSK);
 
 		/* Frame limit */
 		iwl_write_targ_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);
+				IWL49_SCD_CONTEXT_QUEUE_OFFSET(i) +
+				sizeof(u32),
+				(SCD_FRAME_LIMIT <<
+				IWL49_SCD_QUEUE_CTX_REG2_FRAME_LIMIT_POS) &
+				IWL49_SCD_QUEUE_CTX_REG2_FRAME_LIMIT_MSK);
 
 	}
 	iwl_write_prph(priv, IWL49_SCD_INTERRUPT_MASK,
 				 (1 << priv->hw_params.max_txq_num) - 1);
 
 	/* Activate all Tx DMA/FIFO channels */
-	iwl_write_prph(priv, IWL49_SCD_TXFACT,
-				 SCD_TXFACT_REG_TXFIFO_MASK(0, 7));
+	priv->cfg->ops->lib->txq_set_sched(priv, IWL_MASK(0, 7));
 
 	iwl4965_set_wr_ptrs(priv, IWL_CMD_QUEUE_NUM, 0);
 
 	/* Map each Tx/cmd queue to its corresponding fifo */
 	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);
+		iwl_txq_ctx_activate(priv, i);
 		iwl4965_tx_queue_set_status(priv, &priv->txq[i], ac, 0);
 	}
 
 	iwl_release_nic_access(priv);
 	spin_unlock_irqrestore(&priv->lock, flags);
 
-	/* Ask for statistics now, the uCode will send statistics notification
-	 * periodically after association */
-	iwl_send_statistics_request(priv, CMD_ASYNC);
 	return ret;
 }
 
+static struct iwl_sensitivity_ranges iwl4965_sensitivity = {
+	.min_nrg_cck = 97,
+	.max_nrg_cck = 0,
+
+	.auto_corr_min_ofdm = 85,
+	.auto_corr_min_ofdm_mrc = 170,
+	.auto_corr_min_ofdm_x1 = 105,
+	.auto_corr_min_ofdm_mrc_x1 = 220,
+
+	.auto_corr_max_ofdm = 120,
+	.auto_corr_max_ofdm_mrc = 210,
+	.auto_corr_max_ofdm_x1 = 140,
+	.auto_corr_max_ofdm_mrc_x1 = 270,
+
+	.auto_corr_min_cck = 125,
+	.auto_corr_max_cck = 200,
+	.auto_corr_min_cck_mrc = 200,
+	.auto_corr_max_cck_mrc = 400,
+
+	.nrg_th_cck = 100,
+	.nrg_th_ofdm = 100,
+};
+
 /**
  * iwl4965_hw_set_hw_params
  *
  * Called when initializing driver
  */
-int iwl4965_hw_set_hw_params(struct iwl_priv *priv)
+static int iwl4965_hw_set_hw_params(struct iwl_priv *priv)
 {
 
-	if ((priv->cfg->mod_params->num_of_queues > IWL4965_MAX_NUM_QUEUES) ||
+	if ((priv->cfg->mod_params->num_of_queues > IWL49_NUM_QUEUES) ||
 	    (priv->cfg->mod_params->num_of_queues < IWL_MIN_NUM_QUEUES)) {
 		IWL_ERROR("invalid queues_num, should be between %d and %d\n",
-			  IWL_MIN_NUM_QUEUES, IWL4965_MAX_NUM_QUEUES);
+			  IWL_MIN_NUM_QUEUES, IWL49_NUM_QUEUES);
 		return -EINVAL;
 	}
 
 	priv->hw_params.max_txq_num = priv->cfg->mod_params->num_of_queues;
-	priv->hw_params.tx_cmd_len = sizeof(struct iwl4965_tx_cmd);
-	priv->hw_params.max_rxq_size = RX_QUEUE_SIZE;
-	priv->hw_params.max_rxq_log = RX_QUEUE_SIZE_LOG;
-	if (priv->cfg->mod_params->amsdu_size_8K)
-		priv->hw_params.rx_buf_size = IWL_RX_BUF_SIZE_8K;
-	else
-		priv->hw_params.rx_buf_size = IWL_RX_BUF_SIZE_4K;
-	priv->hw_params.max_pkt_size = priv->hw_params.rx_buf_size - 256;
+	priv->hw_params.first_ampdu_q = IWL49_FIRST_AMPDU_QUEUE;
 	priv->hw_params.max_stations = IWL4965_STATION_COUNT;
 	priv->hw_params.bcast_sta_id = IWL4965_BROADCAST_ID;
+	priv->hw_params.max_data_size = IWL49_RTC_DATA_SIZE;
+	priv->hw_params.max_inst_size = IWL49_RTC_INST_SIZE;
+	priv->hw_params.max_bsm_size = BSM_SRAM_SIZE;
+	priv->hw_params.fat_channel = BIT(IEEE80211_BAND_5GHZ);
 
 	priv->hw_params.tx_chains_num = 2;
 	priv->hw_params.rx_chains_num = 2;
-	priv->hw_params.valid_tx_ant = (IWL_ANTENNA_MAIN | IWL_ANTENNA_AUX);
-	priv->hw_params.valid_rx_ant = (IWL_ANTENNA_MAIN | IWL_ANTENNA_AUX);
-
-	return 0;
-}
-
-/**
- * iwl4965_hw_txq_ctx_free - Free TXQ Context
- *
- * Destroy all TX DMA queues and structures
- */
-void iwl4965_hw_txq_ctx_free(struct iwl_priv *priv)
-{
-	int txq_id;
-
-	/* Tx queues */
-	for (txq_id = 0; txq_id < priv->hw_params.max_txq_num; txq_id++)
-		iwl4965_tx_queue_free(priv, &priv->txq[txq_id]);
-
-	/* Keep-warm buffer */
-	iwl4965_kw_free(priv);
-}
-
-/**
- * iwl4965_hw_txq_free_tfd - Free all chunks referenced by TFD [txq->q.read_ptr]
- *
- * Does NOT advance any TFD circular buffer read/write indexes
- * Does NOT free the TFD itself (which is within circular buffer)
- */
-int iwl4965_hw_txq_free_tfd(struct iwl_priv *priv, struct iwl4965_tx_queue *txq)
-{
-	struct iwl4965_tfd_frame *bd_tmp = (struct iwl4965_tfd_frame *)&txq->bd[0];
-	struct iwl4965_tfd_frame *bd = &bd_tmp[txq->q.read_ptr];
-	struct pci_dev *dev = priv->pci_dev;
-	int i;
-	int counter = 0;
-	int index, is_odd;
+	priv->hw_params.valid_tx_ant = ANT_A | ANT_B;
+	priv->hw_params.valid_rx_ant = ANT_A | ANT_B;
+	priv->hw_params.ct_kill_threshold = CELSIUS_TO_KELVIN(CT_KILL_THRESHOLD);
 
-	/* Host command buffers stay mapped in memory, nothing to clean */
-	if (txq->q.id == IWL_CMD_QUEUE_NUM)
-		return 0;
-
-	/* Sanity check on number of chunks */
-	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;
-	}
+	priv->hw_params.sens = &iwl4965_sensitivity;
 
-	/* Unmap chunks, if any.
-	 * TFD info for odd chunks is different format than for even chunks. */
-	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);
-
-		/* Free SKB, if any, for this chunk */
-		if (txq->txb[txq->q.read_ptr].skb[i]) {
-			struct sk_buff *skb = txq->txb[txq->q.read_ptr].skb[i];
-
-			dev_kfree_skb(skb);
-			txq->txb[txq->q.read_ptr].skb[i] = NULL;
-		}
-	}
 	return 0;
 }
 
-int iwl4965_hw_reg_set_txpower(struct iwl_priv *priv, s8 power)
+/* set card power command */
+static int iwl4965_set_power(struct iwl_priv *priv,
+		      void *cmd)
 {
-	IWL_ERROR("TODO: Implement iwl4965_hw_reg_set_txpower!\n");
-	return -EINVAL;
+	int ret = 0;
+
+	ret = iwl_send_cmd_pdu_async(priv, POWER_TABLE_CMD,
+				    sizeof(struct iwl4965_powertable_cmd),
+				    cmd, NULL);
+	return ret;
 }
 
 static s32 iwl4965_math_div_round(s32 num, s32 denom, s32 *res)
@@ -2179,20 +936,6 @@ static s32 iwl4965_get_voltage_compensation(s32 eeprom_voltage,
 	return comp;
 }
 
-static const struct iwl_channel_info *
-iwl4965_get_channel_txpower_info(struct iwl_priv *priv,
-				 enum ieee80211_band band, u16 channel)
-{
-	const struct iwl_channel_info *ch_info;
-
-	ch_info = iwl_get_channel_info(priv, band, 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 &&
@@ -2224,11 +967,11 @@ 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)
+		if (priv->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))
+		if ((channel >= priv->calib_info->band_info[b].ch_from)
+		    && (channel <= priv->calib_info->band_info[b].ch_to))
 			break;
 	}
 
@@ -2256,14 +999,14 @@ static s32 iwl4965_interpolate_value(s32 x, s32 x1, s32 y1, s32 x2, s32 y2)
  * in channel number.
  */
 static int iwl4965_interpolate_chan(struct iwl_priv *priv, u32 channel,
-				    struct iwl4965_eeprom_calib_ch_info *chan_info)
+				    struct iwl_eeprom_calib_ch_info *chan_info)
 {
 	s32 s = -1;
 	u32 c;
 	u32 m;
-	const struct iwl4965_eeprom_calib_measure *m1;
-	const struct iwl4965_eeprom_calib_measure *m2;
-	struct iwl4965_eeprom_calib_measure *omeas;
+	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;
 
@@ -2273,8 +1016,8 @@ static int iwl4965_interpolate_chan(struct iwl_priv *priv, u32 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;
+	ch_i1 = priv->calib_info->band_info[s].ch1.ch_num;
+	ch_i2 = priv->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",
@@ -2282,9 +1025,9 @@ static int iwl4965_interpolate_chan(struct iwl_priv *priv, u32 channel,
 
 	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.
+			m1 = &(priv->calib_info->band_info[s].ch1.
 			       measurements[c][m]);
-			m2 = &(priv->eeprom.calib_info.band_info[s].ch2.
+			m2 = &(priv->calib_info->band_info[s].ch2.
 			       measurements[c][m]);
 			omeas = &(chan_info->measurements[c][m]);
 
@@ -2603,8 +1346,8 @@ static int iwl4965_fill_txpower_tbl(struct iwl_priv *priv, u8 band, u16 channel,
 	int i;
 	int c;
 	const struct iwl_channel_info *ch_info = NULL;
-	struct iwl4965_eeprom_calib_ch_info ch_eeprom_info;
-	const struct iwl4965_eeprom_calib_measure *measurement;
+	struct iwl_eeprom_calib_ch_info ch_eeprom_info;
+	const struct iwl_eeprom_calib_measure *measurement;
 	s16 voltage;
 	s32 init_voltage;
 	s32 voltage_compensation;
@@ -2616,30 +1359,17 @@ static int iwl4965_fill_txpower_tbl(struct iwl_priv *priv, u8 band, u16 channel,
 	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;
+	user_target_power = 2 * priv->tx_power_user_lmt;
 
 	/* Get current (RXON) channel, band, width */
-	ch_info =
-		iwl4965_get_channel_txpower_info(priv, priv->band, channel);
-
 	IWL_DEBUG_TXPOWER("chan %d band %d is_fat %d\n", channel, band,
 			  is_fat);
 
-	if (!ch_info)
+	ch_info = iwl_get_channel_info(priv, priv->band, channel);
+
+	if (!is_channel_valid(ch_info))
 		return -EINVAL;
 
 	/* get txatten group, used to select 1) thermal txpower adjustment
@@ -2661,9 +1391,9 @@ static int iwl4965_fill_txpower_tbl(struct iwl_priv *priv, u8 band, u16 channel,
 	/* hardware txpower limits ...
 	 * saturation (clipping distortion) txpowers are in half-dBm */
 	if (band)
-		saturation_power = priv->eeprom.calib_info.saturation_power24;
+		saturation_power = priv->calib_info->saturation_power24;
 	else
-		saturation_power = priv->eeprom.calib_info.saturation_power52;
+		saturation_power = priv->calib_info->saturation_power52;
 
 	if (saturation_power < IWL_TX_POWER_SATURATION_MIN ||
 	    saturation_power > IWL_TX_POWER_SATURATION_MAX) {
@@ -2693,7 +1423,7 @@ static int iwl4965_fill_txpower_tbl(struct iwl_priv *priv, u8 band, u16 channel,
 	iwl4965_interpolate_chan(priv, channel, &ch_eeprom_info);
 
 	/* calculate tx gain adjustment based on power supply voltage */
-	voltage = priv->eeprom.calib_info.voltage;
+	voltage = priv->calib_info->voltage;
 	init_voltage = (s32)le32_to_cpu(priv->card_alive_init.voltage);
 	voltage_compensation =
 	    iwl4965_get_voltage_compensation(voltage, init_voltage);
@@ -2840,12 +1570,12 @@ static int iwl4965_fill_txpower_tbl(struct iwl_priv *priv, u8 band, u16 channel,
 }
 
 /**
- * iwl4965_hw_reg_send_txpower - Configure the TXPOWER level user limit
+ * iwl4965_send_tx_power - 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.
+ * The power limit is taken from priv->tx_power_user_lmt.
  */
-int iwl4965_hw_reg_send_txpower(struct iwl_priv *priv)
+static int iwl4965_send_tx_power(struct iwl_priv *priv)
 {
 	struct iwl4965_txpowertable_cmd cmd = { 0 };
 	int ret;
@@ -2888,8 +1618,8 @@ static int iwl4965_send_rxon_assoc(struct iwl_priv *priv)
 {
 	int ret = 0;
 	struct iwl4965_rxon_assoc_cmd rxon_assoc;
-	const struct iwl4965_rxon_cmd *rxon1 = &priv->staging_rxon;
-	const struct iwl4965_rxon_cmd *rxon2 = &priv->active_rxon;
+	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) &&
@@ -2965,89 +1695,14 @@ int iwl4965_hw_channel_switch(struct iwl_priv *priv, u16 channel)
 	return rc;
 }
 
-#define RTS_HCCA_RETRY_LIMIT		3
-#define RTS_DFAULT_RETRY_LIMIT		60
-
-void iwl4965_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)
-{
-	struct iwl4965_tx_cmd *tx = &cmd->cmd.tx;
-	u8 rts_retry_limit = 0;
-	u8 data_retry_limit = 0;
-	u16 fc = le16_to_cpu(hdr->frame_control);
-	u8 rate_plcp;
-	u16 rate_flags = 0;
-	int rate_idx = min(ctrl->tx_rate->hw_value & 0xffff, IWL_RATE_COUNT - 1);
-
-	rate_plcp = iwl4965_rates[rate_idx].plcp;
-
-	rts_retry_limit = (is_hcca) ?
-	    RTS_HCCA_RETRY_LIMIT : RTS_DFAULT_RETRY_LIMIT;
-
-	if ((rate_idx >= IWL_FIRST_CCK_RATE) && (rate_idx <= IWL_LAST_CCK_RATE))
-		rate_flags |= RATE_MCS_CCK_MSK;
-
-
-	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 (ieee80211_is_data(fc)) {
-		tx->initial_rate_index = 0;
-		tx->tx_flags |= TX_CMD_FLG_STA_RATE_MSK;
-	} else {
-		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->tx_flags & TX_CMD_FLG_RTS_MSK) {
-				tx->tx_flags &= ~TX_CMD_FLG_RTS_MSK;
-				tx->tx_flags |= TX_CMD_FLG_CTS_MSK;
-			}
-			break;
-		default:
-			break;
-		}
-
-		/* 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;
-		}
-	}
-
-	tx->rts_retry_limit = rts_retry_limit;
-	tx->data_retry_limit = data_retry_limit;
-	tx->rate_n_flags = iwl4965_hw_set_rate_n_flags(rate_plcp, rate_flags);
-}
-
-int iwl4965_hw_get_rx_read(struct iwl_priv *priv)
+static int iwl4965_shared_mem_rx_idx(struct iwl_priv *priv)
 {
 	struct iwl4965_shared *s = priv->shared_virt;
 	return le32_to_cpu(s->rb_closed) & 0xFFF;
 }
 
-int iwl4965_hw_get_temperature(struct iwl_priv *priv)
-{
-	return priv->temperature;
-}
-
 unsigned int iwl4965_hw_get_beacon_cmd(struct iwl_priv *priv,
-			  struct iwl4965_frame *frame, u8 rate)
+			  struct iwl_frame *frame, u8 rate)
 {
 	struct iwl4965_tx_beacon_cmd *tx_beacon_cmd;
 	unsigned int frame_size;
@@ -3060,7 +1715,7 @@ unsigned int iwl4965_hw_get_beacon_cmd(struct iwl_priv *priv,
 
 	frame_size = iwl4965_fill_beacon_frame(priv,
 				tx_beacon_cmd->frame,
-				iwl4965_broadcast_addr,
+				iwl_bcast_addr,
 				sizeof(frame->u) - sizeof(*tx_beacon_cmd));
 
 	BUG_ON(frame_size > MAX_MPDU_SIZE);
@@ -3068,105 +1723,45 @@ unsigned int iwl4965_hw_get_beacon_cmd(struct iwl_priv *priv,
 
 	if ((rate == IWL_RATE_1M_PLCP) || (rate >= IWL_RATE_2M_PLCP))
 		tx_beacon_cmd->tx.rate_n_flags =
-			iwl4965_hw_set_rate_n_flags(rate, RATE_MCS_CCK_MSK);
+			iwl_hw_set_rate_n_flags(rate, RATE_MCS_CCK_MSK);
 	else
 		tx_beacon_cmd->tx.rate_n_flags =
-			iwl4965_hw_set_rate_n_flags(rate, 0);
+			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);
 }
 
-/*
- * Tell 4965 where to find circular buffer of Tx Frame Descriptors for
- * given Tx queue, and enable the DMA channel used for that queue.
- *
- * 4965 supports up to 16 Tx queues in DRAM, mapped to up to 8 Tx DMA
- * channels supported in hardware.
- */
-int iwl4965_hw_tx_queue_init(struct iwl_priv *priv, struct iwl4965_tx_queue *txq)
-{
-	int rc;
-	unsigned long flags;
-	int txq_id = txq->q.id;
-
-	spin_lock_irqsave(&priv->lock, flags);
-	rc = iwl_grab_nic_access(priv);
-	if (rc) {
-		spin_unlock_irqrestore(&priv->lock, flags);
-		return rc;
-	}
-
-	/* Circular buffer (TFD queue in DRAM) physical base address */
-	iwl_write_direct32(priv, FH_MEM_CBBC_QUEUE(txq_id),
-			     txq->q.dma_addr >> 8);
-
-	/* Enable DMA channel, using same id as for TFD queue */
-	iwl_write_direct32(
-		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_nic_access(priv);
-	spin_unlock_irqrestore(&priv->lock, flags);
-
-	return 0;
-}
-
-int iwl4965_hw_txq_attach_buf_to_tfd(struct iwl_priv *priv, void *ptr,
-				 dma_addr_t addr, u16 len)
+static int iwl4965_alloc_shared_mem(struct iwl_priv *priv)
 {
-	int index, is_odd;
-	struct iwl4965_tfd_frame *tfd = ptr;
-	u32 num_tbs = IWL_GET_BITS(*tfd, num_tbs);
-
-	/* Each TFD can point to a maximum 20 Tx buffers */
-	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;
+	priv->shared_virt = pci_alloc_consistent(priv->pci_dev,
+					sizeof(struct iwl4965_shared),
+					&priv->shared_phys);
+	if (!priv->shared_virt)
+		return -ENOMEM;
 
-	if (!is_odd) {
-		tfd->pa[index].tb1_addr = cpu_to_le32(addr);
-		IWL_SET_BITS(tfd->pa[index], tb1_addr_hi,
-			     iwl_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);
-	}
+	memset(priv->shared_virt, 0, sizeof(struct iwl4965_shared));
 
-	IWL_SET_BITS(*tfd, num_tbs, num_tbs + 1);
+	priv->rb_closed_offset = offsetof(struct iwl4965_shared, rb_closed);
 
 	return 0;
 }
 
-static void iwl4965_hw_card_show_info(struct iwl_priv *priv)
+static void iwl4965_free_shared_mem(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);
+	if (priv->shared_virt)
+		pci_free_consistent(priv->pci_dev,
+				    sizeof(struct iwl4965_shared),
+				    priv->shared_virt,
+				    priv->shared_phys);
 }
 
-#define IWL_TX_CRC_SIZE		4
-#define IWL_TX_DELIMITER_SIZE	4
-
 /**
  * iwl4965_txq_update_byte_cnt_tbl - Set up entry in Tx byte-count array
  */
 static void iwl4965_txq_update_byte_cnt_tbl(struct iwl_priv *priv,
-					    struct iwl4965_tx_queue *txq,
+					    struct iwl_tx_queue *txq,
 					    u16 byte_cnt)
 {
 	int len;
@@ -3180,50 +1775,13 @@ static void iwl4965_txq_update_byte_cnt_tbl(struct iwl_priv *priv,
 		       tfd_offset[txq->q.write_ptr], byte_cnt, len);
 
 	/* If within first 64 entries, duplicate at end */
-	if (txq->q.write_ptr < IWL4965_MAX_WIN_SIZE)
+	if (txq->q.write_ptr < IWL49_MAX_WIN_SIZE)
 		IWL_SET_BITS16(shared_data->queues_byte_cnt_tbls[txq_id].
-			tfd_offset[IWL4965_QUEUE_SIZE + txq->q.write_ptr],
+			tfd_offset[IWL49_QUEUE_SIZE + txq->q.write_ptr],
 			byte_cnt, len);
 }
 
 /**
- * iwl4965_set_rxon_chain - Set up Rx chain usage in "staging" RXON image
- *
- * Selects how many and which Rx receivers/antennas/chains to use.
- * 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);
-}
-
-/**
  * 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
@@ -3240,12 +1798,12 @@ static s32 sign_extend(u32 oper, int index)
 }
 
 /**
- * iwl4965_get_temperature - return the calibrated temperature (in Kelvin)
+ * iwl4965_hw_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)
+static int iwl4965_hw_get_temperature(const struct iwl_priv *priv)
 {
 	s32 temperature;
 	s32 vt;
@@ -3280,8 +1838,7 @@ int iwl4965_get_temperature(const struct iwl_priv *priv)
 		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);
+	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");
@@ -3292,11 +1849,10 @@ int iwl4965_get_temperature(const struct iwl_priv *priv)
 	 * 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;
+	temperature = (temperature * 97) / 100 + TEMPERATURE_CALIB_KELVIN_OFFSET;
 
-	IWL_DEBUG_TEMP("Calibrated temperature: %dK, %dC\n", temperature,
-	    KELVIN_TO_CELSIUS(temperature));
+	IWL_DEBUG_TEMP("Calibrated temperature: %dK, %dC\n",
+			temperature, KELVIN_TO_CELSIUS(temperature));
 
 	return temperature;
 }
@@ -3343,89 +1899,11 @@ static int iwl4965_is_temp_calib_needed(struct iwl_priv *priv)
 	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 iwl4965_hw_rx_statistics(struct iwl_priv *priv, struct iwl4965_rx_mem_buffer *rxb)
+static void iwl4965_temperature_calib(struct iwl_priv *priv)
 {
-	struct iwl4965_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_IWL4965_SENSITIVITY
-		queue_work(priv->workqueue, &priv->sensitivity_work);
-#endif
-	}
-
-	iwl_leds_background(priv);
-
-	/* 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);
+	temp = iwl4965_hw_get_temperature(priv);
 	if (temp < 0)
 		return;
 
@@ -3444,810 +1922,12 @@ void iwl4965_hw_rx_statistics(struct iwl_priv *priv, struct iwl4965_rx_mem_buffe
 	priv->temperature = temp;
 	set_bit(STATUS_TEMPERATURE, &priv->status);
 
-	if (unlikely(!test_bit(STATUS_SCANNING, &priv->status)) &&
-		     iwl4965_is_temp_calib_needed(priv))
+	if (!priv->disable_tx_power_cal &&
+	     unlikely(!test_bit(STATUS_SCANNING, &priv->status)) &&
+	     iwl4965_is_temp_calib_needed(priv))
 		queue_work(priv->workqueue, &priv->txpower_work);
 }
 
-static void iwl4965_add_radiotap(struct iwl_priv *priv,
-				 struct sk_buff *skb,
-				 struct iwl4965_rx_phy_res *rx_start,
-				 struct ieee80211_rx_status *stats,
-				 u32 ampdu_status)
-{
-	s8 signal = stats->ssi;
-	s8 noise = 0;
-	int rate = stats->rate_idx;
-	u64 tsf = stats->mactime;
-	__le16 antenna;
-	__le16 phy_flags_hw = rx_start->phy_flags;
-	struct iwl4965_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 */
-	} __attribute__ ((packed)) *iwl4965_rt;
-
-	/* TODO: We won't have enough headroom for HT frames. Fix it later. */
-	if (skb_headroom(skb) < sizeof(*iwl4965_rt)) {
-		if (net_ratelimit())
-			printk(KERN_ERR "not enough headroom [%d] for "
-			       "radiotap head [%zd]\n",
-			       skb_headroom(skb), sizeof(*iwl4965_rt));
-		return;
-	}
-
-	/* put radiotap header in front of 802.11 header and data */
-	iwl4965_rt = (void *)skb_push(skb, sizeof(*iwl4965_rt));
-
-	/* initialise radiotap header */
-	iwl4965_rt->rt_hdr.it_version = PKTHDR_RADIOTAP_VERSION;
-	iwl4965_rt->rt_hdr.it_pad = 0;
-
-	/* total header + data */
-	put_unaligned(cpu_to_le16(sizeof(*iwl4965_rt)),
-		      &iwl4965_rt->rt_hdr.it_len);
-
-	/* Indicate all the fields we add to the radiotap header */
-	put_unaligned(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)),
-		      &iwl4965_rt->rt_hdr.it_present);
-
-	/* Zero the flags, we'll add to them as we go */
-	iwl4965_rt->rt_flags = 0;
-
-	put_unaligned(cpu_to_le64(tsf), &iwl4965_rt->rt_tsf);
-
-	iwl4965_rt->rt_dbmsignal = signal;
-	iwl4965_rt->rt_dbmnoise = noise;
-
-	/* Convert the channel frequency and set the flags */
-	put_unaligned(cpu_to_le16(stats->freq), &iwl4965_rt->rt_channelMHz);
-	if (!(phy_flags_hw & RX_RES_PHY_FLAGS_BAND_24_MSK))
-		put_unaligned(cpu_to_le16(IEEE80211_CHAN_OFDM |
-					  IEEE80211_CHAN_5GHZ),
-			      &iwl4965_rt->rt_chbitmask);
-	else if (phy_flags_hw & RX_RES_PHY_FLAGS_MOD_CCK_MSK)
-		put_unaligned(cpu_to_le16(IEEE80211_CHAN_CCK |
-					  IEEE80211_CHAN_2GHZ),
-			      &iwl4965_rt->rt_chbitmask);
-	else	/* 802.11g */
-		put_unaligned(cpu_to_le16(IEEE80211_CHAN_OFDM |
-					  IEEE80211_CHAN_2GHZ),
-			      &iwl4965_rt->rt_chbitmask);
-
-	if (rate == -1)
-		iwl4965_rt->rt_rate = 0;
-	else {
-		if (stats->band == IEEE80211_BAND_5GHZ)
-			rate += IWL_FIRST_OFDM_RATE;
-
-		iwl4965_rt->rt_rate = iwl4965_rates[rate].ieee;
-	}
-
-	/*
-	 * "antenna number"
-	 *
-	 * It seems that the antenna field in the phy flags value
-	 * is actually a bitfield. This is undefined by radiotap,
-	 * it wants an actual antenna number but I always get "7"
-	 * for most legacy frames I receive indicating that the
-	 * same frame was received on all three RX chains.
-	 *
-	 * I think this field should be removed in favour of a
-	 * new 802.11n radiotap field "RX chains" that is defined
-	 * as a bitmask.
-	 */
-	antenna = phy_flags_hw & RX_RES_PHY_FLAGS_ANTENNA_MSK;
-	iwl4965_rt->rt_antenna = le16_to_cpu(antenna) >> 4;
-
-	/* set the preamble flag if appropriate */
-	if (phy_flags_hw & RX_RES_PHY_FLAGS_SHORT_PREAMBLE_MSK)
-		iwl4965_rt->rt_flags |= IEEE80211_RADIOTAP_F_SHORTPRE;
-
-	stats->flag |= RX_FLAG_RADIOTAP;
-}
-
-static void iwl_update_rx_stats(struct iwl_priv *priv, u16 fc, u16 len)
-{
-	/* 0 - mgmt, 1 - cnt, 2 - data */
-	int idx = (fc & IEEE80211_FCTL_FTYPE) >> 2;
-	priv->rx_stats[idx].cnt++;
-	priv->rx_stats[idx].bytes += len;
-}
-
-static u32 iwl4965_translate_rx_status(u32 decrypt_in)
-{
-	u32 decrypt_out = 0;
-
-	if ((decrypt_in & RX_RES_STATUS_STATION_FOUND) ==
-					RX_RES_STATUS_STATION_FOUND)
-		decrypt_out |= (RX_RES_STATUS_STATION_FOUND |
-				RX_RES_STATUS_NO_STATION_INFO_MISMATCH);
-
-	decrypt_out |= (decrypt_in & RX_RES_STATUS_SEC_TYPE_MSK);
-
-	/* packet was not encrypted */
-	if ((decrypt_in & RX_RES_STATUS_SEC_TYPE_MSK) ==
-					RX_RES_STATUS_SEC_TYPE_NONE)
-		return decrypt_out;
-
-	/* packet was encrypted with unknown alg */
-	if ((decrypt_in & RX_RES_STATUS_SEC_TYPE_MSK) ==
-					RX_RES_STATUS_SEC_TYPE_ERR)
-		return decrypt_out;
-
-	/* decryption was not done in HW */
-	if ((decrypt_in & RX_MPDU_RES_STATUS_DEC_DONE_MSK) !=
-					RX_MPDU_RES_STATUS_DEC_DONE_MSK)
-		return decrypt_out;
-
-	switch (decrypt_in & RX_RES_STATUS_SEC_TYPE_MSK) {
-
-	case RX_RES_STATUS_SEC_TYPE_CCMP:
-		/* alg is CCM: check MIC only */
-		if (!(decrypt_in & RX_MPDU_RES_STATUS_MIC_OK))
-			/* Bad MIC */
-			decrypt_out |= RX_RES_STATUS_BAD_ICV_MIC;
-		else
-			decrypt_out |= RX_RES_STATUS_DECRYPT_OK;
-
-		break;
-
-	case RX_RES_STATUS_SEC_TYPE_TKIP:
-		if (!(decrypt_in & RX_MPDU_RES_STATUS_TTAK_OK)) {
-			/* Bad TTAK */
-			decrypt_out |= RX_RES_STATUS_BAD_KEY_TTAK;
-			break;
-		}
-		/* fall through if TTAK OK */
-	default:
-		if (!(decrypt_in & RX_MPDU_RES_STATUS_ICV_OK))
-			decrypt_out |= RX_RES_STATUS_BAD_ICV_MIC;
-		else
-			decrypt_out |= RX_RES_STATUS_DECRYPT_OK;
-		break;
-	};
-
-	IWL_DEBUG_RX("decrypt_in:0x%x  decrypt_out = 0x%x\n",
-					decrypt_in, decrypt_out);
-
-	return decrypt_out;
-}
-
-static void iwl4965_handle_data_packet(struct iwl_priv *priv, int is_data,
-				       int include_phy,
-				       struct iwl4965_rx_mem_buffer *rxb,
-				       struct ieee80211_rx_status *stats)
-{
-	struct iwl4965_rx_packet *pkt = (struct iwl4965_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;
-	u32 ampdu_status_legacy;
-
-	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 > priv->hw_params.max_pkt_size || len < 16) {
-		IWL_WARNING("byte count out of range [16,4K] : %d\n", len);
-		return;
-	}
-
-	ampdu_status = le32_to_cpu(*rx_end);
-	skblen = ((u8 *) rx_end - (u8 *) & pkt->u.raw[0]) + sizeof(u32);
-
-	if (!include_phy) {
-		/* New status scheme, need to translate */
-		ampdu_status_legacy = ampdu_status;
-		ampdu_status = iwl4965_translate_rx_status(ampdu_status);
-	}
-
-	/* 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;
-	}
-
-	stats->flag = 0;
-	hdr = (struct ieee80211_hdr *)rxb->skb->data;
-
-	if (!priv->cfg->mod_params->sw_crypto)
-		iwl4965_set_decrypted_flag(priv, rxb->skb, ampdu_status, stats);
-
-	if (priv->add_radiotap)
-		iwl4965_add_radiotap(priv, rxb->skb, rx_start, stats, ampdu_status);
-
-	iwl_update_rx_stats(priv, le16_to_cpu(hdr->frame_control), len);
-	ieee80211_rx_irqsafe(priv->hw, rxb->skb, stats);
-	priv->alloc_rxb_skb--;
-	rxb->skb = NULL;
-}
-
-/* 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_IWL4965_HT
-
-void iwl4965_init_ht_hw_capab(struct iwl_priv *priv,
-			      struct ieee80211_ht_info *ht_info,
-			      enum ieee80211_band band)
-{
-	ht_info->cap = 0;
-	memset(ht_info->supp_mcs_set, 0, 16);
-
-	ht_info->ht_supported = 1;
-
-	if (band == IEEE80211_BAND_5GHZ) {
-		ht_info->cap |= (u16)IEEE80211_HT_CAP_SUP_WIDTH;
-		ht_info->cap |= (u16)IEEE80211_HT_CAP_SGI_40;
-		ht_info->supp_mcs_set[4] = 0x01;
-	}
-	ht_info->cap |= (u16)IEEE80211_HT_CAP_GRN_FLD;
-	ht_info->cap |= (u16)IEEE80211_HT_CAP_SGI_20;
-	ht_info->cap |= (u16)(IEEE80211_HT_CAP_MIMO_PS &
-			     (IWL_MIMO_PS_NONE << 2));
-
-	if (priv->cfg->mod_params->amsdu_size_8K)
-		ht_info->cap |= (u16)IEEE80211_HT_CAP_MAX_AMSDU;
-
-	ht_info->ampdu_factor = CFG_HT_RX_AMPDU_FACTOR_DEF;
-	ht_info->ampdu_density = CFG_HT_MPDU_DENSITY_DEF;
-
-	ht_info->supp_mcs_set[0] = 0xFF;
-	ht_info->supp_mcs_set[1] = 0xFF;
-}
-#endif /* CONFIG_IWL4965_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);
-
-	iwl4965_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 = iwl4965_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;
-		}
-	}
-}
-#ifdef CONFIG_IWLWIFI_DEBUG
-
-/**
- * iwl4965_dbg_report_frame - dump frame to syslog during debug sessions
- *
- * You may 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:  This was originally written for 3945, need to audit for
- *        proper operation with 4965.
- */
-static void iwl4965_dbg_report_frame(struct iwl_priv *priv,
-		      struct iwl4965_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 iwl4965_rx_frame_stats *rx_stats = IWL_RX_STATS(pkt);
-	struct iwl4965_rx_frame_hdr *rx_hdr = IWL_RX_HDR(pkt);
-	struct iwl4965_rx_frame_end *rx_end = IWL_RX_END(pkt);
-	u8 *data = IWL_RX_DATA(pkt);
-
-	if (likely(!(iwl_debug_level & IWL_DL_RX)))
-		return;
-
-	/* 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;
-		int rate_idx;
-		u32 bitrate;
-
-		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_idx = iwl4965_hwrate_to_plcp_idx(rate_sym);
-		if (unlikely(rate_idx == -1))
-			bitrate = 0;
-		else
-			bitrate = iwl4965_rates[rate_idx].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, bitrate);
-		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);
-}
-#else
-static inline void iwl4965_dbg_report_frame(struct iwl_priv *priv,
-					    struct iwl4965_rx_packet *pkt,
-					    struct ieee80211_hdr *header,
-					    int group100)
-{
-}
-#endif
-
-
-
-/* Called for REPLY_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 iwl4965_rx_mem_buffer *rxb)
-{
-	struct ieee80211_hdr *header;
-	struct ieee80211_rx_status rx_status;
-	struct iwl4965_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_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;
-	u16 fc;
-	u8 network_packet;
-
-	rx_status.mactime = le64_to_cpu(rx_start->timestamp);
-	rx_status.freq =
-		ieee80211_channel_to_frequency(le16_to_cpu(rx_start->channel));
-	rx_status.band = (rx_start->phy_flags & RX_RES_PHY_FLAGS_BAND_24_MSK) ?
-				IEEE80211_BAND_2GHZ : IEEE80211_BAND_5GHZ;
-	rx_status.rate_idx =
-		iwl4965_hwrate_to_plcp_idx(le32_to_cpu(rx_start->rate_n_flags));
-	if (rx_status.band == IEEE80211_BAND_5GHZ)
-		rx_status.rate_idx -= IWL_FIRST_OFDM_RATE;
-
-	rx_status.antenna = 0;
-	rx_status.flag = 0;
-
-	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);
-
-	/* Find max signal strength (dBm) among 3 antenna/receiver chains */
-	rx_status.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)) {
-		rx_status.noise = priv->last_rx_noise;
-		rx_status.signal = iwl4965_calc_sig_qual(rx_status.ssi,
-							 rx_status.noise);
-	} else {
-		rx_status.noise = IWL_NOISE_MEAS_NOT_AVAILABLE;
-		rx_status.signal = iwl4965_calc_sig_qual(rx_status.ssi, 0);
-	}
-
-	/* Reset beacon noise level if not associated. */
-	if (!iwl_is_associated(priv))
-		priv->last_rx_noise = IWL_NOISE_MEAS_NOT_AVAILABLE;
-
-	/* Set "1" to report good data frames in groups of 100 */
-	/* FIXME: need to optimze the call: */
-	iwl4965_dbg_report_frame(priv, pkt, header, 1);
-
-	IWL_DEBUG_STATS_LIMIT("Rssi %d, noise %d, qual %d, TSF %llu\n",
-			      rx_status.ssi, rx_status.noise, rx_status.signal,
-			      (unsigned long long)rx_status.mactime);
-
-	network_packet = iwl4965_is_network_packet(priv, header);
-	if (network_packet) {
-		priv->last_rx_rssi = rx_status.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);
-		iwl4965_handle_data_packet(priv, 0, include_phy, rxb, &rx_status);
-		break;
-
-	case IEEE80211_FTYPE_CTL:
-#ifdef CONFIG_IWL4965_HT
-		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, &rx_status);
-			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(iwl4965_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,
-						   &rx_status);
-		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 iwl4965_rx_mem_buffer *rxb)
-{
-	struct iwl4965_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 iwl4965_rx_mem_buffer *rxb)
-
-{
-#ifdef CONFIG_IWL4965_SENSITIVITY
-	struct iwl4965_rx_packet *pkt = (void *)rxb->skb->data;
-	struct iwl4965_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_IWL4965_SENSITIVITY*/
-}
-#ifdef CONFIG_IWL4965_HT
-
-/**
- * iwl4965_sta_modify_enable_tid_tx - Enable Tx for this TID in station table
- */
-static void iwl4965_sta_modify_enable_tid_tx(struct iwl_priv *priv,
-					 int sta_id, int tid)
-{
-	unsigned long flags;
-
-	/* Remove "disable" flag, to enable Tx for this TID */
-	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);
-
-	iwl4965_send_add_station(priv, &priv->stations[sta_id].sta, CMD_ASYNC);
-}
-
-/**
- * iwl4965_tx_status_reply_compressed_ba - Update tx status from block-ack
- *
- * Go through block-ack's bitmap of ACK'd frames, update driver's record of
- * ACK vs. not.  This gets sent to mac80211, then to rate scaling algo.
- */
-static int iwl4965_tx_status_reply_compressed_ba(struct iwl_priv *priv,
-						 struct iwl4965_ht_agg *agg,
-						 struct iwl4965_compressed_ba_resp*
-						 ba_resp)
-
-{
-	int i, sh, ack;
-	u16 seq_ctl = le16_to_cpu(ba_resp->seq_ctl);
-	u16 scd_flow = le16_to_cpu(ba_resp->scd_flow);
-	u64 bitmap;
-	int successes = 0;
-	struct ieee80211_tx_status *tx_status;
-
-	if (unlikely(!agg->wait_for_ba))  {
-		IWL_ERROR("Received BA when not expected\n");
-		return -EINVAL;
-	}
-
-	/* Mark that the expected block-ack response arrived */
-	agg->wait_for_ba = 0;
-	IWL_DEBUG_TX_REPLY("BA %d %d\n", agg->start_idx, ba_resp->seq_ctl);
-
-	/* Calculate shift to align block-ack bits with our Tx window bits */
-	sh = agg->start_idx - SEQ_TO_INDEX(seq_ctl>>4);
-	if (sh < 0) /* tbw something is wrong with indices */
-		sh += 0x100;
-
-	/* don't use 64-bit values for now */
-	bitmap = le64_to_cpu(ba_resp->bitmap) >> 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 block-ack bitmap */
-	bitmap &= agg->bitmap;
-
-	/* For each frame attempted in aggregation,
-	 * update driver's record of tx frame's status. */
-	for (i = 0; i < agg->frame_count ; i++) {
-		ack = bitmap & (1 << i);
-		successes += !!ack;
-		IWL_DEBUG_TX_REPLY("%s ON i=%d idx=%d raw=%d\n",
-			ack? "ACK":"NACK", i, (agg->start_idx + i) & 0xff,
-			agg->start_idx + i);
-	}
-
-	tx_status = &priv->txq[scd_flow].txb[agg->start_idx].status;
-	tx_status->flags = IEEE80211_TX_STATUS_ACK;
-	tx_status->flags |= IEEE80211_TX_STATUS_AMPDU;
-	tx_status->ampdu_ack_map = successes;
-	tx_status->ampdu_ack_len = agg->frame_count;
-	iwl4965_hwrate_to_tx_control(priv, agg->rate_n_flags,
-				     &tx_status->control);
-
-	IWL_DEBUG_TX_REPLY("Bitmap %llx\n", (unsigned long long)bitmap);
-
-	return 0;
-}
-
 /**
  * iwl4965_tx_queue_stop_scheduler - Stop queue, but keep configuration
  */
@@ -4258,22 +1938,24 @@ static void iwl4965_tx_queue_stop_scheduler(struct iwl_priv *priv,
 	 * the SCD_ACT_EN bit is the write-enable mask for the ACTIVE bit. */
 	iwl_write_prph(priv,
 		IWL49_SCD_QUEUE_STATUS_BITS(txq_id),
-		(0 << SCD_QUEUE_STTS_REG_POS_ACTIVE)|
-		(1 << SCD_QUEUE_STTS_REG_POS_SCD_ACT_EN));
+		(0 << IWL49_SCD_QUEUE_STTS_REG_POS_ACTIVE)|
+		(1 << IWL49_SCD_QUEUE_STTS_REG_POS_SCD_ACT_EN));
 }
 
 /**
- * txq_id must be greater than IWL_BACK_QUEUE_FIRST_ID
+ * txq_id must be greater than IWL49_FIRST_AMPDU_QUEUE
  * priv->lock must be held by the caller
  */
-static int iwl4965_tx_queue_agg_disable(struct iwl_priv *priv, u16 txq_id,
-					u16 ssn_idx, u8 tx_fifo)
+static int iwl4965_txq_agg_disable(struct iwl_priv *priv, u16 txq_id,
+				   u16 ssn_idx, u8 tx_fifo)
 {
 	int ret = 0;
 
-	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);
+	if ((IWL49_FIRST_AMPDU_QUEUE > txq_id) ||
+	    (IWL49_FIRST_AMPDU_QUEUE + IWL49_NUM_AMPDU_QUEUES <= txq_id)) {
+		IWL_WARNING("queue number out of range: %d, must be %d to %d\n",
+			txq_id, IWL49_FIRST_AMPDU_QUEUE,
+			IWL49_FIRST_AMPDU_QUEUE + IWL49_NUM_AMPDU_QUEUES - 1);
 		return -EINVAL;
 	}
 
@@ -4291,7 +1973,7 @@ static int iwl4965_tx_queue_agg_disable(struct iwl_priv *priv, u16 txq_id,
 	iwl4965_set_wr_ptrs(priv, txq_id, ssn_idx);
 
 	iwl_clear_bits_prph(priv, IWL49_SCD_INTERRUPT_MASK, (1 << txq_id));
-	iwl4965_txq_ctx_deactivate(priv, txq_id);
+	iwl_txq_ctx_deactivate(priv, txq_id);
 	iwl4965_tx_queue_set_status(priv, &priv->txq[txq_id], tx_fifo, 0);
 
 	iwl_release_nic_access(priv);
@@ -4299,121 +1981,6 @@ static int iwl4965_tx_queue_agg_disable(struct iwl_priv *priv, u16 txq_id,
 	return 0;
 }
 
-int iwl4965_check_empty_hw_queue(struct iwl_priv *priv, int sta_id,
-					 u8 tid, int txq_id)
-{
-	struct iwl4965_queue *q = &priv->txq[txq_id].q;
-	u8 *addr = priv->stations[sta_id].sta.sta.addr;
-	struct iwl4965_tid_data *tid_data = &priv->stations[sta_id].tid[tid];
-
-	switch (priv->stations[sta_id].tid[tid].agg.state) {
-	case IWL_EMPTYING_HW_QUEUE_DELBA:
-		/* We are reclaiming the last packet of the */
-		/* aggregated HW queue */
-		if (txq_id  == tid_data->agg.txq_id &&
-		    q->read_ptr == q->write_ptr) {
-			u16 ssn = SEQ_TO_SN(tid_data->seq_number);
-			int tx_fifo = default_tid_to_tx_fifo[tid];
-			IWL_DEBUG_HT("HW queue empty: continue DELBA flow\n");
-			iwl4965_tx_queue_agg_disable(priv, txq_id,
-						     ssn, tx_fifo);
-			tid_data->agg.state = IWL_AGG_OFF;
-			ieee80211_stop_tx_ba_cb_irqsafe(priv->hw, addr, tid);
-		}
-		break;
-	case IWL_EMPTYING_HW_QUEUE_ADDBA:
-		/* We are reclaiming the last packet of the queue */
-		if (tid_data->tfds_in_queue == 0) {
-			IWL_DEBUG_HT("HW queue empty: continue ADDBA flow\n");
-			tid_data->agg.state = IWL_AGG_ON;
-			ieee80211_start_tx_ba_cb_irqsafe(priv->hw, addr, tid);
-		}
-		break;
-	}
-	return 0;
-}
-
-/**
- * iwl4965_queue_dec_wrap - Decrement queue index, wrap back to end if needed
- * @index -- current index
- * @n_bd -- total number of entries in queue (s/b power of 2)
- */
-static inline int iwl4965_queue_dec_wrap(int index, int n_bd)
-{
-	return (index == 0) ? n_bd - 1 : index - 1;
-}
-
-/**
- * iwl4965_rx_reply_compressed_ba - Handler for REPLY_COMPRESSED_BA
- *
- * Handles block-acknowledge notification from device, which reports success
- * of frames sent via aggregation.
- */
-static void iwl4965_rx_reply_compressed_ba(struct iwl_priv *priv,
-					   struct iwl4965_rx_mem_buffer *rxb)
-{
-	struct iwl4965_rx_packet *pkt = (void *)rxb->skb->data;
-	struct iwl4965_compressed_ba_resp *ba_resp = &pkt->u.compressed_ba;
-	int index;
-	struct iwl4965_tx_queue *txq = NULL;
-	struct iwl4965_ht_agg *agg;
-	DECLARE_MAC_BUF(mac);
-
-	/* "flow" corresponds to Tx queue */
-	u16 scd_flow = le16_to_cpu(ba_resp->scd_flow);
-
-	/* "ssn" is start of block-ack Tx window, corresponds to index
-	 * (in Tx queue's circular buffer) of first TFD/frame in window */
-	u16 ba_resp_scd_ssn = le16_to_cpu(ba_resp->scd_ssn);
-
-	if (scd_flow >= priv->hw_params.max_txq_num) {
-		IWL_ERROR("BUG_ON scd_flow is bigger than number of queues");
-		return;
-	}
-
-	txq = &priv->txq[scd_flow];
-	agg = &priv->stations[ba_resp->sta_id].tid[ba_resp->tid].agg;
-
-	/* Find index just before block-ack window */
-	index = iwl4965_queue_dec_wrap(ba_resp_scd_ssn & 0xff, txq->q.n_bd);
-
-	/* TODO: Need to get this copy more safely - now good for debug */
-
-	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%llx, scd_flow = "
-			   "%d, scd_ssn = %d\n",
-			   ba_resp->tid,
-			   ba_resp->seq_ctl,
-			   (unsigned long long)le64_to_cpu(ba_resp->bitmap),
-			   ba_resp->scd_flow,
-			   ba_resp->scd_ssn);
-	IWL_DEBUG_TX_REPLY("DAT start_idx = %d, bitmap = 0x%llx \n",
-			   agg->start_idx,
-			   (unsigned long long)agg->bitmap);
-
-	/* Update driver's record of ACK vs. not for each frame in window */
-	iwl4965_tx_status_reply_compressed_ba(priv, agg, ba_resp);
-
-	/* Release all TFDs before the SSN, i.e. all TFDs in front of
-	 * block-ack window (we assume that they've been successfully
-	 * transmitted ... if not, it's too late anyway). */
-	if (txq->q.read_ptr != (ba_resp_scd_ssn & 0xff)) {
-		int freed = iwl4965_tx_queue_reclaim(priv, scd_flow, index);
-		priv->stations[ba_resp->sta_id].
-			tid[ba_resp->tid].tfds_in_queue -= freed;
-		if (iwl4965_queue_space(&txq->q) > txq->q.low_mark &&
-			priv->mac80211_registered &&
-			agg->state != IWL_EMPTYING_HW_QUEUE_DELBA)
-			ieee80211_wake_queue(priv->hw, scd_flow);
-		iwl4965_check_empty_hw_queue(priv, ba_resp->sta_id,
-			ba_resp->tid, scd_flow);
-	}
-}
-
 /**
  * iwl4965_tx_queue_set_q2ratid - Map unique receiver/tid combination to a queue
  */
@@ -4424,10 +1991,10 @@ static int iwl4965_tx_queue_set_q2ratid(struct iwl_priv *priv, u16 ra_tid,
 	u32 tbl_dw;
 	u16 scd_q2ratid;
 
-	scd_q2ratid = ra_tid & SCD_QUEUE_RA_TID_MAP_RATID_MSK;
+	scd_q2ratid = ra_tid & IWL_SCD_QUEUE_RA_TID_MAP_RATID_MSK;
 
 	tbl_dw_addr = priv->scd_base_addr +
-			SCD_TRANSLATE_TBL_OFFSET_QUEUE(txq_id);
+			IWL49_SCD_TRANSLATE_TBL_OFFSET_QUEUE(txq_id);
 
 	tbl_dw = iwl_read_targ_mem(priv, tbl_dw_addr);
 
@@ -4445,31 +2012,34 @@ static int iwl4965_tx_queue_set_q2ratid(struct iwl_priv *priv, u16 ra_tid,
 /**
  * iwl4965_tx_queue_agg_enable - Set up & enable aggregation for selected queue
  *
- * NOTE:  txq_id must be greater than IWL_BACK_QUEUE_FIRST_ID,
+ * NOTE:  txq_id must be greater than IWL49_FIRST_AMPDU_QUEUE,
  *        i.e. it must be one of the higher queues used for aggregation
  */
-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)
+static int iwl4965_txq_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;
+	int ret;
 	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);
+	if ((IWL49_FIRST_AMPDU_QUEUE > txq_id) ||
+	    (IWL49_FIRST_AMPDU_QUEUE + IWL49_NUM_AMPDU_QUEUES <= txq_id)) {
+		IWL_WARNING("queue number out of range: %d, must be %d to %d\n",
+			txq_id, IWL49_FIRST_AMPDU_QUEUE,
+			IWL49_FIRST_AMPDU_QUEUE + IWL49_NUM_AMPDU_QUEUES - 1);
+		return -EINVAL;
+	}
 
 	ra_tid = BUILD_RAxTID(sta_id, tid);
 
 	/* Modify device's station table to Tx this TID */
-	iwl4965_sta_modify_enable_tid_tx(priv, sta_id, tid);
+	iwl_sta_modify_enable_tid_tx(priv, sta_id, tid);
 
 	spin_lock_irqsave(&priv->lock, flags);
-	rc = iwl_grab_nic_access(priv);
-	if (rc) {
+	ret = iwl_grab_nic_access(priv);
+	if (ret) {
 		spin_unlock_irqrestore(&priv->lock, flags);
-		return rc;
+		return ret;
 	}
 
 	/* Stop this Tx queue before configuring it */
@@ -4489,14 +2059,14 @@ static int iwl4965_tx_queue_agg_enable(struct iwl_priv *priv, int txq_id,
 
 	/* Set up Tx window size and frame limit for this queue */
 	iwl_write_targ_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);
+		priv->scd_base_addr + IWL49_SCD_CONTEXT_QUEUE_OFFSET(txq_id),
+		(SCD_WIN_SIZE << IWL49_SCD_QUEUE_CTX_REG1_WIN_SIZE_POS) &
+		IWL49_SCD_QUEUE_CTX_REG1_WIN_SIZE_MSK);
 
 	iwl_write_targ_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);
+		IWL49_SCD_CONTEXT_QUEUE_OFFSET(txq_id) + sizeof(u32),
+		(SCD_FRAME_LIMIT << IWL49_SCD_QUEUE_CTX_REG2_FRAME_LIMIT_POS)
+		& IWL49_SCD_QUEUE_CTX_REG2_FRAME_LIMIT_MSK);
 
 	iwl_set_bits_prph(priv, IWL49_SCD_INTERRUPT_MASK, (1 << txq_id));
 
@@ -4509,444 +2079,313 @@ static int iwl4965_tx_queue_agg_enable(struct iwl_priv *priv, int txq_id,
 	return 0;
 }
 
-#endif /* CONFIG_IWL4965_HT */
-
-/**
- * iwl4965_add_station - Initialize a station's hardware rate table
- *
- * The uCode's station table contains a table of fallback rates
- * for automatic fallback during transmission.
- *
- * NOTE: This sets up a default set of values.  These will be replaced later
- *       if the driver's iwl-4965-rs rate scaling algorithm is used, instead of
- *       rc80211_simple.
- *
- * NOTE: Run REPLY_ADD_STA command to set up station table entry, before
- *       calling this function (which runs REPLY_TX_LINK_QUALITY_CMD,
- *       which requires station table entry to exist).
- */
-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 selected rate, fall back
-	 * all the way down to 1M in IEEE order, and then spin on 1M */
-	if (is_ap)
-		r = IWL_RATE_54M_INDEX;
-	else if (priv->band == IEEE80211_BAND_5GHZ)
-		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;
-
-		/* Use Tx antenna B only */
-		rate_flags |= RATE_MCS_ANT_B_MSK;
-		rate_flags &= ~RATE_MCS_ANT_A_MSK;
-
-		link_cmd.rs_table[i].rate_n_flags =
-			iwl4965_hw_set_rate_n_flags(iwl4965_rates[r].plcp, rate_flags);
-		r = iwl4965_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 : priv->hw_params.bcast_sta_id;
-
-	iwl_send_cmd_pdu_async(priv, REPLY_TX_LINK_QUALITY_CMD,
-			       sizeof(link_cmd), &link_cmd, NULL);
-}
-
-#ifdef CONFIG_IWL4965_HT
-
-static u8 iwl4965_is_channel_extension(struct iwl_priv *priv,
-				       enum ieee80211_band band,
-				       u16 channel, u8 extension_chan_offset)
-{
-	const struct iwl_channel_info *ch_info;
-
-	ch_info = iwl_get_channel_info(priv, band, channel);
-	if (!is_channel_valid(ch_info))
-		return 0;
-
-	if (extension_chan_offset == IWL_EXT_CHANNEL_OFFSET_NONE)
-		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 iwl4965_is_fat_tx_allowed(struct iwl_priv *priv,
-				struct ieee80211_ht_info *sta_ht_inf)
-{
-	struct iwl_ht_info *iwl_ht_conf = &priv->current_ht_config;
-
-	if ((!iwl_ht_conf->is_ht) ||
-	   (iwl_ht_conf->supported_chan_width != IWL_CHANNEL_WIDTH_40MHZ) ||
-	   (iwl_ht_conf->extension_chan_offset == IWL_EXT_CHANNEL_OFFSET_NONE))
-		return 0;
-
-	if (sta_ht_inf) {
-		if ((!sta_ht_inf->ht_supported) ||
-		   (!(sta_ht_inf->cap & IEEE80211_HT_CAP_SUP_WIDTH)))
-			return 0;
-	}
-
-	return (iwl4965_is_channel_extension(priv, priv->band,
-					 iwl_ht_conf->control_channel,
-					 iwl_ht_conf->extension_chan_offset));
-}
-
-void iwl4965_set_rxon_ht(struct iwl_priv *priv, struct iwl_ht_info *ht_info)
+int iwl4965_mac_ampdu_action(struct ieee80211_hw *hw,
+			     enum ieee80211_ampdu_mlme_action action,
+			     const u8 *addr, u16 tid, u16 *ssn)
 {
-	struct iwl4965_rxon_cmd *rxon = &priv->staging_rxon;
-	u32 val;
+	struct iwl_priv *priv = hw->priv;
+	DECLARE_MAC_BUF(mac);
 
-	if (!ht_info->is_ht)
-		return;
+	IWL_DEBUG_HT("A-MPDU action on addr %s tid %d\n",
+		     print_mac(mac, addr), tid);
 
-	/* Set up channel bandwidth:  20 MHz only, or 20/40 mixed if fat ok */
-	if (iwl4965_is_fat_tx_allowed(priv, NULL))
-		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;
-	}
+	if (!(priv->cfg->sku & IWL_SKU_N))
+		return -EACCES;
 
-	/* Note: control channel is opposite of 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_NONE:
+	switch (action) {
+	case IEEE80211_AMPDU_RX_START:
+		IWL_DEBUG_HT("start Rx\n");
+		return iwl_rx_agg_start(priv, addr, tid, *ssn);
+	case IEEE80211_AMPDU_RX_STOP:
+		IWL_DEBUG_HT("stop Rx\n");
+		return iwl_rx_agg_stop(priv, addr, tid);
+	case IEEE80211_AMPDU_TX_START:
+		IWL_DEBUG_HT("start Tx\n");
+		return iwl_tx_agg_start(priv, addr, tid, ssn);
+	case IEEE80211_AMPDU_TX_STOP:
+		IWL_DEBUG_HT("stop Tx\n");
+		return iwl_tx_agg_stop(priv, addr, tid);
 	default:
-		rxon->flags &= ~RXON_FLG_CHANNEL_MODE_MIXED_MSK;
+		IWL_DEBUG_HT("unknown\n");
+		return -EINVAL;
 		break;
 	}
-
-	val = ht_info->ht_protection;
-
-	rxon->flags |= cpu_to_le32(val << RXON_FLG_HT_OPERATING_MODE_POS);
-
-	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",
-			ht_info->supp_mcs_set[0], ht_info->supp_mcs_set[1],
-			le32_to_cpu(rxon->flags), ht_info->ht_protection,
-			ht_info->extension_chan_offset,
-			ht_info->control_channel);
-	return;
+	return 0;
 }
 
-void iwl4965_set_ht_add_station(struct iwl_priv *priv, u8 index,
-				struct ieee80211_ht_info *sta_ht_inf)
+static u16 iwl4965_get_hcmd_size(u8 cmd_id, u16 len)
 {
-	__le32 sta_flags;
-	u8 mimo_ps_mode;
-
-	if (!sta_ht_inf || !sta_ht_inf->ht_supported)
-		goto done;
-
-	mimo_ps_mode = (sta_ht_inf->cap & IEEE80211_HT_CAP_MIMO_PS) >> 2;
-
-	sta_flags = priv->stations[index].sta.station_flags;
-
-	sta_flags &= ~(STA_FLG_RTS_MIMO_PROT_MSK | STA_FLG_MIMO_DIS_MSK);
-
-	switch (mimo_ps_mode) {
-	case WLAN_HT_CAP_MIMO_PS_STATIC:
-		sta_flags |= STA_FLG_MIMO_DIS_MSK;
-		break;
-	case WLAN_HT_CAP_MIMO_PS_DYNAMIC:
-		sta_flags |= STA_FLG_RTS_MIMO_PROT_MSK;
-		break;
-	case WLAN_HT_CAP_MIMO_PS_DISABLED:
-		break;
+	switch (cmd_id) {
+	case REPLY_RXON:
+		return (u16) sizeof(struct iwl4965_rxon_cmd);
 	default:
-		IWL_WARNING("Invalid MIMO PS mode %d", mimo_ps_mode);
-		break;
+		return len;
 	}
-
-	sta_flags |= cpu_to_le32(
-	      (u32)sta_ht_inf->ampdu_factor << STA_FLG_MAX_AGG_SIZE_POS);
-
-	sta_flags |= cpu_to_le32(
-	      (u32)sta_ht_inf->ampdu_density << STA_FLG_AGG_MPDU_DENSITY_POS);
-
-	if (iwl4965_is_fat_tx_allowed(priv, sta_ht_inf))
-		sta_flags |= STA_FLG_FAT_EN_MSK;
-	else
-		sta_flags &= ~STA_FLG_FAT_EN_MSK;
-
-	priv->stations[index].sta.station_flags = sta_flags;
- done:
-	return;
 }
 
-static void iwl4965_sta_modify_add_ba_tid(struct iwl_priv *priv,
-					  int sta_id, int tid, u16 ssn)
+static u16 iwl4965_build_addsta_hcmd(const struct iwl_addsta_cmd *cmd, u8 *data)
 {
-	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);
+	struct iwl4965_addsta_cmd *addsta = (struct iwl4965_addsta_cmd *)data;
+	addsta->mode = cmd->mode;
+	memcpy(&addsta->sta, &cmd->sta, sizeof(struct sta_id_modify));
+	memcpy(&addsta->key, &cmd->key, sizeof(struct iwl4965_keyinfo));
+	addsta->station_flags = cmd->station_flags;
+	addsta->station_flags_msk = cmd->station_flags_msk;
+	addsta->tid_disable_tx = cmd->tid_disable_tx;
+	addsta->add_immediate_ba_tid = cmd->add_immediate_ba_tid;
+	addsta->remove_immediate_ba_tid = cmd->remove_immediate_ba_tid;
+	addsta->add_immediate_ba_ssn = cmd->add_immediate_ba_ssn;
+	addsta->reserved1 = __constant_cpu_to_le16(0);
+	addsta->reserved2 = __constant_cpu_to_le32(0);
 
-	iwl4965_send_add_station(priv, &priv->stations[sta_id].sta, CMD_ASYNC);
+	return (u16)sizeof(struct iwl4965_addsta_cmd);
 }
 
-static void iwl4965_sta_modify_del_ba_tid(struct iwl_priv *priv,
-					  int sta_id, int tid)
+static inline u32 iwl4965_get_scd_ssn(struct iwl4965_tx_resp *tx_resp)
 {
-	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);
-
-	iwl4965_send_add_station(priv, &priv->stations[sta_id].sta, CMD_ASYNC);
+	return le32_to_cpup(&tx_resp->u.status + tx_resp->frame_count) & MAX_SN;
 }
 
-/*
- * Find first available (lowest unused) Tx Queue, mark it "active".
- * Called only when finding queue for aggregation.
- * Should never return anything < 7, because they should already
- * be in use as EDCA AC (0-3), Command (4), HCCA (5, 6).
+/**
+ * iwl4965_tx_status_reply_tx - Handle Tx rspnse for frames in aggregation queue
  */
-static int iwl4965_txq_ctx_activate_free(struct iwl_priv *priv)
-{
-	int txq_id;
-
-	for (txq_id = 0; txq_id < priv->hw_params.max_txq_num; txq_id++)
-		if (!test_and_set_bit(txq_id, &priv->txq_ctx_active_msk))
-			return txq_id;
-	return -1;
-}
-
-static int iwl4965_mac_ht_tx_agg_start(struct ieee80211_hw *hw, const u8 *da,
-				       u16 tid, u16 *start_seq_num)
+static int iwl4965_tx_status_reply_tx(struct iwl_priv *priv,
+				      struct iwl_ht_agg *agg,
+				      struct iwl4965_tx_resp *tx_resp,
+				      int txq_id, u16 start_idx)
 {
-	struct iwl_priv *priv = hw->priv;
-	int sta_id;
-	int tx_fifo;
-	int txq_id;
-	int ssn = -1;
-	int ret = 0;
-	unsigned long flags;
-	struct iwl4965_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("%s on da = %s tid = %d\n",
-			__func__, print_mac(mac, da), tid);
-
-	sta_id = iwl4965_hw_find_station(priv, da);
-	if (sta_id == IWL_INVALID_STATION)
-		return -ENXIO;
-
-	if (priv->stations[sta_id].tid[tid].agg.state != IWL_AGG_OFF) {
-		IWL_ERROR("Start AGG when state is not IWL_AGG_OFF !\n");
-		return -ENXIO;
-	}
-
-	txq_id = iwl4965_txq_ctx_activate_free(priv);
-	if (txq_id == -1)
-		return -ENXIO;
+	u16 status;
+	struct agg_tx_status *frame_status = tx_resp->u.agg_status;
+	struct ieee80211_tx_info *info = NULL;
+	struct ieee80211_hdr *hdr = NULL;
+	u32 rate_n_flags = le32_to_cpu(tx_resp->rate_n_flags);
+	int i, sh, idx;
+	u16 seq;
+	if (agg->wait_for_ba)
+		IWL_DEBUG_TX_REPLY("got tx response w/o block-ack\n");
+
+	agg->frame_count = tx_resp->frame_count;
+	agg->start_idx = start_idx;
+	agg->rate_n_flags = rate_n_flags;
+	agg->bitmap = 0;
+
+	/* # frames attempted by Tx command */
+	if (agg->frame_count == 1) {
+		/* Only one frame was attempted; no block-ack will arrive */
+		status = le16_to_cpu(frame_status[0].status);
+		idx = start_idx;
+
+		/* FIXME: code repetition */
+		IWL_DEBUG_TX_REPLY("FrameCnt = %d, StartIdx=%d idx=%d\n",
+				   agg->frame_count, agg->start_idx, idx);
+
+		info = IEEE80211_SKB_CB(priv->txq[txq_id].txb[idx].skb[0]);
+		info->status.retry_count = tx_resp->failure_frame;
+		info->flags &= ~IEEE80211_TX_CTL_AMPDU;
+		info->flags |= iwl_is_tx_success(status)?
+			IEEE80211_TX_STAT_ACK : 0;
+		iwl_hwrate_to_tx_control(priv, rate_n_flags, info);
+		/* 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", rate_n_flags);
+
+		agg->wait_for_ba = 0;
+	} else {
+		/* Two or more frames were attempted; expect block-ack */
+		u64 bitmap = 0;
+		int start = agg->start_idx;
+
+		/* Construct bit-map of pending frames within Tx window */
+		for (i = 0; i < agg->frame_count; i++) {
+			u16 sc;
+			status = le16_to_cpu(frame_status[i].status);
+			seq  = le16_to_cpu(frame_status[i].sequence);
+			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;
+			}
 
-	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);
+			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));
+		}
 
-	*start_seq_num = ssn;
-	ret = iwl4965_tx_queue_agg_enable(priv, txq_id, tx_fifo,
-					  sta_id, tid, ssn);
-	if (ret)
-		return ret;
+		agg->bitmap = bitmap;
+		agg->start_idx = start;
+		IWL_DEBUG_TX_REPLY("Frames %d start_idx=%d bitmap=0x%llx\n",
+				   agg->frame_count, agg->start_idx,
+				   (unsigned long long)agg->bitmap);
 
-	ret = 0;
-	if (tid_data->tfds_in_queue == 0) {
-		printk(KERN_ERR "HW queue is empty\n");
-		tid_data->agg.state = IWL_AGG_ON;
-		ieee80211_start_tx_ba_cb_irqsafe(hw, da, tid);
-	} else {
-		IWL_DEBUG_HT("HW queue is NOT empty: %d packets in HW queue\n",
-				tid_data->tfds_in_queue);
-		tid_data->agg.state = IWL_EMPTYING_HW_QUEUE_ADDBA;
+		if (bitmap)
+			agg->wait_for_ba = 1;
 	}
-	return ret;
+	return 0;
 }
 
-static int iwl4965_mac_ht_tx_agg_stop(struct ieee80211_hw *hw, const u8 *da,
-				      u16 tid)
-{
-
-	struct iwl_priv *priv = hw->priv;
-	int tx_fifo_id, txq_id, sta_id, ssn = -1;
-	struct iwl4965_tid_data *tid_data;
-	int ret, write_ptr, read_ptr;
-	unsigned long flags;
-	DECLARE_MAC_BUF(mac);
+/**
+ * iwl4965_rx_reply_tx - Handle standard (non-aggregation) Tx response
+ */
+static void iwl4965_rx_reply_tx(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);
+	struct iwl_tx_queue *txq = &priv->txq[txq_id];
+	struct ieee80211_tx_info *info;
+	struct iwl4965_tx_resp *tx_resp = (void *)&pkt->u.raw[0];
+	u32  status = le32_to_cpu(tx_resp->u.status);
+	int tid = MAX_TID_COUNT, sta_id = IWL_INVALID_STATION;
+	__le16 fc;
+	struct ieee80211_hdr *hdr;
+	u8 *qc = NULL;
 
-	if (!da) {
-		IWL_ERROR("da = NULL\n");
-		return -EINVAL;
+	if ((index >= txq->q.n_bd) || (iwl_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.write_ptr,
+			  txq->q.read_ptr);
+		return;
 	}
 
-	if (likely(tid < ARRAY_SIZE(default_tid_to_tx_fifo)))
-		tx_fifo_id = default_tid_to_tx_fifo[tid];
-	else
-		return -EINVAL;
-
-	sta_id = iwl4965_hw_find_station(priv, da);
-
-	if (sta_id == IWL_INVALID_STATION)
-		return -ENXIO;
-
-	if (priv->stations[sta_id].tid[tid].agg.state != IWL_AGG_ON)
-		IWL_WARNING("Stopping AGG while state not IWL_AGG_ON\n");
+	info = IEEE80211_SKB_CB(txq->txb[txq->q.read_ptr].skb[0]);
+	memset(&info->status, 0, sizeof(info->status));
 
-	tid_data = &priv->stations[sta_id].tid[tid];
-	ssn = (tid_data->seq_number & IEEE80211_SCTL_SEQ) >> 4;
-	txq_id = tid_data->agg.txq_id;
-	write_ptr = priv->txq[txq_id].q.write_ptr;
-	read_ptr = priv->txq[txq_id].q.read_ptr;
+	hdr = iwl_tx_queue_get_hdr(priv, txq_id, index);
+	fc = hdr->frame_control;
+	if (ieee80211_is_data_qos(fc)) {
+		qc = ieee80211_get_qos_ctl(hdr);
+		tid = qc[0] & 0xf;
+	}
 
-	/* The queue is not empty */
-	if (write_ptr != read_ptr) {
-		IWL_DEBUG_HT("Stopping a non empty AGG HW QUEUE\n");
-		priv->stations[sta_id].tid[tid].agg.state =
-				IWL_EMPTYING_HW_QUEUE_DELBA;
-		return 0;
+	sta_id = iwl_get_ra_sta_id(priv, hdr);
+	if (txq->sched_retry && unlikely(sta_id == IWL_INVALID_STATION)) {
+		IWL_ERROR("Station not known\n");
+		return;
 	}
 
-	IWL_DEBUG_HT("HW queue empty\n");;
-	priv->stations[sta_id].tid[tid].agg.state = IWL_AGG_OFF;
+	if (txq->sched_retry) {
+		const u32 scd_ssn = iwl4965_get_scd_ssn(tx_resp);
+		struct iwl_ht_agg *agg = NULL;
 
-	spin_lock_irqsave(&priv->lock, flags);
-	ret = iwl4965_tx_queue_agg_disable(priv, txq_id, ssn, tx_fifo_id);
-	spin_unlock_irqrestore(&priv->lock, flags);
+		if (!qc)
+			return;
 
-	if (ret)
-		return ret;
+		agg = &priv->stations[sta_id].tid[tid].agg;
 
-	ieee80211_stop_tx_ba_cb_irqsafe(priv->hw, da, tid);
+		iwl4965_tx_status_reply_tx(priv, agg, tx_resp, txq_id, index);
 
-	IWL_DEBUG_INFO("iwl4965_mac_ht_tx_agg_stop on da=%s tid=%d\n",
-			print_mac(mac, da), tid);
+		/* check if BAR is needed */
+		if ((tx_resp->frame_count == 1) && !iwl_is_tx_success(status))
+			info->flags |= IEEE80211_TX_STAT_AMPDU_NO_BACK;
 
-	return 0;
-}
+		if (txq->q.read_ptr != (scd_ssn & 0xff)) {
+			int freed, ampdu_q;
+			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);
+			freed = iwl_tx_queue_reclaim(priv, txq_id, index);
+			priv->stations[sta_id].tid[tid].tfds_in_queue -= freed;
 
-int iwl4965_mac_ampdu_action(struct ieee80211_hw *hw,
-			     enum ieee80211_ampdu_mlme_action action,
-			     const u8 *addr, u16 tid, u16 *ssn)
-{
-	struct iwl_priv *priv = hw->priv;
-	int sta_id;
-	DECLARE_MAC_BUF(mac);
-
-	IWL_DEBUG_HT("A-MPDU action on da=%s tid=%d ",
-			print_mac(mac, addr), tid);
-	sta_id = iwl4965_hw_find_station(priv, addr);
-	switch (action) {
-	case IEEE80211_AMPDU_RX_START:
-		IWL_DEBUG_HT("start Rx\n");
-		iwl4965_sta_modify_add_ba_tid(priv, sta_id, tid, *ssn);
-		break;
-	case IEEE80211_AMPDU_RX_STOP:
-		IWL_DEBUG_HT("stop Rx\n");
-		iwl4965_sta_modify_del_ba_tid(priv, sta_id, tid);
-		break;
-	case IEEE80211_AMPDU_TX_START:
-		IWL_DEBUG_HT("start Tx\n");
-		return iwl4965_mac_ht_tx_agg_start(hw, addr, tid, ssn);
-	case IEEE80211_AMPDU_TX_STOP:
-		IWL_DEBUG_HT("stop Tx\n");
-		return iwl4965_mac_ht_tx_agg_stop(hw, addr, tid);
-	default:
-		IWL_DEBUG_HT("unknown\n");
-		return -EINVAL;
-		break;
+			if (iwl_queue_space(&txq->q) > txq->q.low_mark &&
+			    txq_id >= 0 && priv->mac80211_registered &&
+			    agg->state != IWL_EMPTYING_HW_QUEUE_DELBA) {
+				/* calculate mac80211 ampdu sw queue to wake */
+				ampdu_q = txq_id - IWL49_FIRST_AMPDU_QUEUE +
+					  priv->hw->queues;
+				if (agg->state == IWL_AGG_OFF)
+					ieee80211_wake_queue(priv->hw, txq_id);
+				else
+					ieee80211_wake_queue(priv->hw, ampdu_q);
+			}
+			iwl_txq_check_empty(priv, sta_id, tid, txq_id);
+		}
+	} else {
+		info->status.retry_count = tx_resp->failure_frame;
+		info->flags |=
+			iwl_is_tx_success(status) ? IEEE80211_TX_STAT_ACK : 0;
+		iwl_hwrate_to_tx_control(priv,
+					le32_to_cpu(tx_resp->rate_n_flags),
+					info);
+
+		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) {
+		    int freed = iwl_tx_queue_reclaim(priv, txq_id, index);
+		    if (tid != MAX_TID_COUNT)
+			priv->stations[sta_id].tid[tid].tfds_in_queue -= freed;
+		    if (iwl_queue_space(&txq->q) > txq->q.low_mark &&
+			(txq_id >= 0) && priv->mac80211_registered)
+			ieee80211_wake_queue(priv->hw, txq_id);
+		    if (tid != MAX_TID_COUNT)
+			iwl_txq_check_empty(priv, sta_id, tid, txq_id);
+		}
 	}
-	return 0;
+
+	if (iwl_check_bits(status, TX_ABORT_REQUIRED_MSK))
+		IWL_ERROR("TODO:  Implement Tx ABORT REQUIRED!!!\n");
 }
 
-#endif /* CONFIG_IWL4965_HT */
 
 /* Set up 4965-specific Rx frame reply handlers */
-void iwl4965_hw_rx_handler_setup(struct iwl_priv *priv)
+static void iwl4965_rx_handler_setup(struct iwl_priv *priv)
 {
 	/* Legacy Rx frames */
-	priv->rx_handlers[REPLY_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_IWL4965_HT
-	priv->rx_handlers[REPLY_COMPRESSED_BA] = iwl4965_rx_reply_compressed_ba;
-#endif /* CONFIG_IWL4965_HT */
+	priv->rx_handlers[REPLY_RX] = iwl_rx_reply_rx;
+	/* Tx response */
+	priv->rx_handlers[REPLY_TX] = iwl4965_rx_reply_tx;
 }
 
-void iwl4965_hw_setup_deferred_work(struct iwl_priv *priv)
+static void iwl4965_setup_deferred_work(struct iwl_priv *priv)
 {
 	INIT_WORK(&priv->txpower_work, iwl4965_bg_txpower_work);
-#ifdef CONFIG_IWL4965_SENSITIVITY
-	INIT_WORK(&priv->sensitivity_work, iwl4965_bg_sensitivity_work);
-#endif
-	init_timer(&priv->statistics_periodic);
-	priv->statistics_periodic.data = (unsigned long)priv;
-	priv->statistics_periodic.function = iwl4965_bg_statistics_periodic;
 }
 
-void iwl4965_hw_cancel_deferred_work(struct iwl_priv *priv)
+static void iwl4965_cancel_deferred_work(struct iwl_priv *priv)
 {
-	del_timer_sync(&priv->statistics_periodic);
-
-	cancel_delayed_work(&priv->init_alive_start);
+	cancel_work_sync(&priv->txpower_work);
 }
 
 
@@ -4955,23 +2394,56 @@ static struct iwl_hcmd_ops iwl4965_hcmd = {
 };
 
 static struct iwl_hcmd_utils_ops iwl4965_hcmd_utils = {
-	.enqueue_hcmd = iwl4965_enqueue_hcmd,
+	.get_hcmd_size = iwl4965_get_hcmd_size,
+	.build_addsta_hcmd = iwl4965_build_addsta_hcmd,
+	.chain_noise_reset = iwl4965_chain_noise_reset,
+	.gain_computation = iwl4965_gain_computation,
+	.rts_tx_cmd_flag = iwl4965_rts_tx_cmd_flag,
 };
 
 static struct iwl_lib_ops iwl4965_lib = {
-	.init_drv = iwl4965_init_drv,
 	.set_hw_params = iwl4965_hw_set_hw_params,
+	.alloc_shared_mem = iwl4965_alloc_shared_mem,
+	.free_shared_mem = iwl4965_free_shared_mem,
+	.shared_mem_rx_idx = iwl4965_shared_mem_rx_idx,
 	.txq_update_byte_cnt_tbl = iwl4965_txq_update_byte_cnt_tbl,
-	.hw_nic_init = iwl4965_hw_nic_init,
+	.txq_set_sched = iwl4965_txq_set_sched,
+	.txq_agg_enable = iwl4965_txq_agg_enable,
+	.txq_agg_disable = iwl4965_txq_agg_disable,
+	.rx_handler_setup = iwl4965_rx_handler_setup,
+	.setup_deferred_work = iwl4965_setup_deferred_work,
+	.cancel_deferred_work = iwl4965_cancel_deferred_work,
 	.is_valid_rtc_data_addr = iwl4965_hw_valid_rtc_data_addr,
 	.alive_notify = iwl4965_alive_notify,
+	.init_alive_start = iwl4965_init_alive_start,
 	.load_ucode = iwl4965_load_bsm,
+	.apm_ops = {
+		.init = iwl4965_apm_init,
+		.reset = iwl4965_apm_reset,
+		.stop = iwl4965_apm_stop,
+		.config = iwl4965_nic_config,
+		.set_pwr_src = iwl4965_set_pwr_src,
+	},
 	.eeprom_ops = {
+		.regulatory_bands = {
+			EEPROM_REGULATORY_BAND_1_CHANNELS,
+			EEPROM_REGULATORY_BAND_2_CHANNELS,
+			EEPROM_REGULATORY_BAND_3_CHANNELS,
+			EEPROM_REGULATORY_BAND_4_CHANNELS,
+			EEPROM_REGULATORY_BAND_5_CHANNELS,
+			EEPROM_4965_REGULATORY_BAND_24_FAT_CHANNELS,
+			EEPROM_4965_REGULATORY_BAND_52_FAT_CHANNELS
+		},
 		.verify_signature  = iwlcore_eeprom_verify_signature,
 		.acquire_semaphore = iwlcore_eeprom_acquire_semaphore,
 		.release_semaphore = iwlcore_eeprom_release_semaphore,
+		.check_version = iwl4965_eeprom_check_version,
+		.query_addr = iwlcore_eeprom_query_addr,
 	},
-	.radio_kill_sw = iwl4965_radio_kill_sw,
+	.set_power = iwl4965_set_power,
+	.send_tx_power	= iwl4965_send_tx_power,
+	.update_chain_flags = iwl4965_update_chain_flags,
+	.temperature = iwl4965_temperature_calib,
 };
 
 static struct iwl_ops iwl4965_ops = {
@@ -4984,10 +2456,14 @@ struct iwl_cfg iwl4965_agn_cfg = {
 	.name = "4965AGN",
 	.fw_name = "iwlwifi-4965" IWL4965_UCODE_API ".ucode",
 	.sku = IWL_SKU_A|IWL_SKU_G|IWL_SKU_N,
+	.eeprom_size = IWL4965_EEPROM_IMG_SIZE,
 	.ops = &iwl4965_ops,
 	.mod_params = &iwl4965_mod_params,
 };
 
+/* Module firmware */
+MODULE_FIRMWARE("iwlwifi-4965" IWL4965_UCODE_API ".ucode");
+
 module_param_named(antenna, iwl4965_mod_params.antenna, int, 0444);
 MODULE_PARM_DESC(antenna, "select antenna (1=Main, 2=Aux, default 0 [both])");
 module_param_named(disable, iwl4965_mod_params.disable, int, 0444);
@@ -5002,10 +2478,14 @@ MODULE_PARM_DESC(disable_hw_scan, "disable hardware scanning (default 0)");
 
 module_param_named(queues_num, iwl4965_mod_params.num_of_queues, int, 0444);
 MODULE_PARM_DESC(queues_num, "number of hw queues.");
-
 /* QoS */
 module_param_named(qos_enable, iwl4965_mod_params.enable_qos, int, 0444);
 MODULE_PARM_DESC(qos_enable, "enable all QoS functionality");
+/* 11n */
+module_param_named(11n_disable, iwl4965_mod_params.disable_11n, int, 0444);
+MODULE_PARM_DESC(11n_disable, "disable 11n functionality");
 module_param_named(amsdu_size_8K, iwl4965_mod_params.amsdu_size_8K, int, 0444);
 MODULE_PARM_DESC(amsdu_size_8K, "enable 8K amsdu size");
 
+module_param_named(fw_restart4965, iwl4965_mod_params.restart_fw, int, 0444);
+MODULE_PARM_DESC(fw_restart4965, "restart firmware in case of error");
diff --git a/drivers/net/wireless/iwlwifi/iwl-5000-hw.h b/drivers/net/wireless/iwlwifi/iwl-5000-hw.h
new file mode 100644
index 000000000000..17d4f31c5934
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-5000-hw.h
@@ -0,0 +1,134 @@
+/******************************************************************************
+ *
+ * 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) 2007 - 2008 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.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 - 2008 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.
+ *
+ *****************************************************************************/
+/*
+ * Please use this file (iwl-5000-hw.h) only for hardware-related definitions.
+ * Use iwl-5000-commands.h for uCode API definitions.
+ */
+
+#ifndef __iwl_5000_hw_h__
+#define __iwl_5000_hw_h__
+
+#define IWL50_RTC_INST_UPPER_BOUND		(0x020000)
+#define IWL50_RTC_DATA_UPPER_BOUND		(0x80C000)
+#define IWL50_RTC_INST_SIZE (IWL50_RTC_INST_UPPER_BOUND - RTC_INST_LOWER_BOUND)
+#define IWL50_RTC_DATA_SIZE (IWL50_RTC_DATA_UPPER_BOUND - RTC_DATA_LOWER_BOUND)
+
+/* EERPROM */
+#define IWL_5000_EEPROM_IMG_SIZE			2048
+
+
+#define IWL50_MAX_WIN_SIZE                64
+#define IWL50_QUEUE_SIZE                 256
+#define IWL50_CMD_FIFO_NUM                 7
+#define IWL50_NUM_QUEUES                  20
+#define IWL50_NUM_AMPDU_QUEUES		  10
+#define IWL50_FIRST_AMPDU_QUEUE		  10
+
+#define IWL_sta_id_POS 12
+#define IWL_sta_id_LEN 4
+#define IWL_sta_id_SYM val
+
+/* Fixed (non-configurable) rx data from phy */
+
+/* Base physical address of iwl5000_shared is provided to SCD_DRAM_BASE_ADDR
+ * and &iwl5000_shared.val0 is provided to FH_RSCSR_CHNL0_STTS_WPTR_REG */
+struct iwl5000_sched_queue_byte_cnt_tbl {
+	struct iwl4965_queue_byte_cnt_entry tfd_offset[IWL50_QUEUE_SIZE +
+						       IWL50_MAX_WIN_SIZE];
+} __attribute__ ((packed));
+
+struct iwl5000_shared {
+	struct iwl5000_sched_queue_byte_cnt_tbl
+	 queues_byte_cnt_tbls[IWL50_NUM_QUEUES];
+	__le32 rb_closed;
+
+	/* __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 rb_closed
+	/* __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 rb_closed
+	/* __le32 rsrv2:4; */
+
+	__le32 frm_finished;
+	/* __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 frm_finished
+	/* __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 frm_finished
+	/* __le32 rsrv4:4; */
+
+	__le32 padding1;  /* so that allocation will be aligned to 16B */
+	__le32 padding2;
+} __attribute__ ((packed));
+
+
+#endif /* __iwl_5000_hw_h__ */
+
diff --git a/drivers/net/wireless/iwlwifi/iwl-5000.c b/drivers/net/wireless/iwlwifi/iwl-5000.c
new file mode 100644
index 000000000000..878d6193b232
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-5000.c
@@ -0,0 +1,1580 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007-2008 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:
+ * 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/etherdevice.h>
+#include <asm/unaligned.h>
+
+#include "iwl-eeprom.h"
+#include "iwl-dev.h"
+#include "iwl-core.h"
+#include "iwl-io.h"
+#include "iwl-sta.h"
+#include "iwl-helpers.h"
+#include "iwl-5000-hw.h"
+
+#define IWL5000_UCODE_API  "-1"
+
+static const u16 iwl5000_default_queue_to_tx_fifo[] = {
+	IWL_TX_FIFO_AC3,
+	IWL_TX_FIFO_AC2,
+	IWL_TX_FIFO_AC1,
+	IWL_TX_FIFO_AC0,
+	IWL50_CMD_FIFO_NUM,
+	IWL_TX_FIFO_HCCA_1,
+	IWL_TX_FIFO_HCCA_2
+};
+
+/* FIXME: same implementation as 4965 */
+static int iwl5000_apm_stop_master(struct iwl_priv *priv)
+{
+	int ret = 0;
+	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);
+
+	ret = iwl_poll_bit(priv, CSR_RESET,
+				  CSR_RESET_REG_FLAG_MASTER_DISABLED,
+				  CSR_RESET_REG_FLAG_MASTER_DISABLED, 100);
+	if (ret < 0)
+		goto out;
+
+out:
+	spin_unlock_irqrestore(&priv->lock, flags);
+	IWL_DEBUG_INFO("stop master\n");
+
+	return ret;
+}
+
+
+static int iwl5000_apm_init(struct iwl_priv *priv)
+{
+	int ret = 0;
+
+	iwl_set_bit(priv, CSR_GIO_CHICKEN_BITS,
+		    CSR_GIO_CHICKEN_BITS_REG_BIT_DIS_L0S_EXIT_TIMER);
+
+	/* disable L0s without affecting L1 :don't wait for ICH L0s bug W/A) */
+	iwl_set_bit(priv, CSR_GIO_CHICKEN_BITS,
+		    CSR_GIO_CHICKEN_BITS_REG_BIT_L1A_NO_L0S_RX);
+
+	iwl_set_bit(priv, CSR_ANA_PLL_CFG, CSR50_ANA_PLL_CFG_VAL);
+
+	/* set "initialization complete" bit to move adapter
+	 * D0U* --> D0A* state */
+	iwl_set_bit(priv, CSR_GP_CNTRL, CSR_GP_CNTRL_REG_FLAG_INIT_DONE);
+
+	/* wait for clock stabilization */
+	ret = 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 (ret < 0) {
+		IWL_DEBUG_INFO("Failed to init the card\n");
+		return ret;
+	}
+
+	ret = iwl_grab_nic_access(priv);
+	if (ret)
+		return ret;
+
+	/* enable DMA */
+	iwl_write_prph(priv, APMG_CLK_EN_REG, APMG_CLK_VAL_DMA_CLK_RQT);
+
+	udelay(20);
+
+	/* disable L1-Active */
+	iwl_set_bits_prph(priv, APMG_PCIDEV_STT_REG,
+			  APMG_PCIDEV_STT_VAL_L1_ACT_DIS);
+
+	iwl_release_nic_access(priv);
+
+	return ret;
+}
+
+/* FIXME: this is indentical to 4965 */
+static void iwl5000_apm_stop(struct iwl_priv *priv)
+{
+	unsigned long flags;
+
+	iwl5000_apm_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);
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+}
+
+
+static int iwl5000_apm_reset(struct iwl_priv *priv)
+{
+	int ret = 0;
+	unsigned long flags;
+
+	iwl5000_apm_stop_master(priv);
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	iwl_set_bit(priv, CSR_RESET, CSR_RESET_REG_FLAG_SW_RESET);
+
+	udelay(10);
+
+
+	/* FIXME: put here L1A -L0S w/a */
+
+	iwl_set_bit(priv, CSR_ANA_PLL_CFG, CSR50_ANA_PLL_CFG_VAL);
+
+	/* set "initialization complete" bit to move adapter
+	 * D0U* --> D0A* state */
+	iwl_set_bit(priv, CSR_GP_CNTRL, CSR_GP_CNTRL_REG_FLAG_INIT_DONE);
+
+	/* wait for clock stabilization */
+	ret = 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 (ret < 0) {
+		IWL_DEBUG_INFO("Failed to init the card\n");
+		goto out;
+	}
+
+	ret = iwl_grab_nic_access(priv);
+	if (ret)
+		goto out;
+
+	/* enable DMA */
+	iwl_write_prph(priv, APMG_CLK_EN_REG, APMG_CLK_VAL_DMA_CLK_RQT);
+
+	udelay(20);
+
+	/* disable L1-Active */
+	iwl_set_bits_prph(priv, APMG_PCIDEV_STT_REG,
+			  APMG_PCIDEV_STT_VAL_L1_ACT_DIS);
+
+	iwl_release_nic_access(priv);
+
+out:
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return ret;
+}
+
+
+static void iwl5000_nic_config(struct iwl_priv *priv)
+{
+	unsigned long flags;
+	u16 radio_cfg;
+	u8 val_link;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	pci_read_config_byte(priv->pci_dev, PCI_LINK_CTRL, &val_link);
+
+	/* L1 is enabled by BIOS */
+	if ((val_link & PCI_LINK_VAL_L1_EN) == PCI_LINK_VAL_L1_EN)
+		/* diable L0S disabled L1A enabled */
+		iwl_set_bit(priv, CSR_GIO_REG, CSR_GIO_REG_VAL_L0S_ENABLED);
+	else
+		/* L0S enabled L1A disabled */
+		iwl_clear_bit(priv, CSR_GIO_REG, CSR_GIO_REG_VAL_L0S_ENABLED);
+
+	radio_cfg = iwl_eeprom_query16(priv, EEPROM_RADIO_CONFIG);
+
+	/* write radio config values to register */
+	if (EEPROM_RF_CFG_TYPE_MSK(radio_cfg) < EEPROM_5000_RF_CFG_TYPE_MAX)
+		iwl_set_bit(priv, CSR_HW_IF_CONFIG_REG,
+			    EEPROM_RF_CFG_TYPE_MSK(radio_cfg) |
+			    EEPROM_RF_CFG_STEP_MSK(radio_cfg) |
+			    EEPROM_RF_CFG_DASH_MSK(radio_cfg));
+
+	/* set CSR_HW_CONFIG_REG for uCode use */
+	iwl_set_bit(priv, CSR_HW_IF_CONFIG_REG,
+		    CSR_HW_IF_CONFIG_REG_BIT_RADIO_SI |
+		    CSR_HW_IF_CONFIG_REG_BIT_MAC_SI);
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+}
+
+
+
+/*
+ * EEPROM
+ */
+static u32 eeprom_indirect_address(const struct iwl_priv *priv, u32 address)
+{
+	u16 offset = 0;
+
+	if ((address & INDIRECT_ADDRESS) == 0)
+		return address;
+
+	switch (address & INDIRECT_TYPE_MSK) {
+	case INDIRECT_HOST:
+		offset = iwl_eeprom_query16(priv, EEPROM_5000_LINK_HOST);
+		break;
+	case INDIRECT_GENERAL:
+		offset = iwl_eeprom_query16(priv, EEPROM_5000_LINK_GENERAL);
+		break;
+	case INDIRECT_REGULATORY:
+		offset = iwl_eeprom_query16(priv, EEPROM_5000_LINK_REGULATORY);
+		break;
+	case INDIRECT_CALIBRATION:
+		offset = iwl_eeprom_query16(priv, EEPROM_5000_LINK_CALIBRATION);
+		break;
+	case INDIRECT_PROCESS_ADJST:
+		offset = iwl_eeprom_query16(priv, EEPROM_5000_LINK_PROCESS_ADJST);
+		break;
+	case INDIRECT_OTHERS:
+		offset = iwl_eeprom_query16(priv, EEPROM_5000_LINK_OTHERS);
+		break;
+	default:
+		IWL_ERROR("illegal indirect type: 0x%X\n",
+		address & INDIRECT_TYPE_MSK);
+		break;
+	}
+
+	/* translate the offset from words to byte */
+	return (address & ADDRESS_MSK) + (offset << 1);
+}
+
+static int iwl5000_eeprom_check_version(struct iwl_priv *priv)
+{
+	u16 eeprom_ver;
+	struct iwl_eeprom_calib_hdr {
+		u8 version;
+		u8 pa_type;
+		u16 voltage;
+	} *hdr;
+
+	eeprom_ver = iwl_eeprom_query16(priv, EEPROM_VERSION);
+
+	hdr = (struct iwl_eeprom_calib_hdr *)iwl_eeprom_query_addr(priv,
+							EEPROM_5000_CALIB_ALL);
+
+	if (eeprom_ver < EEPROM_5000_EEPROM_VERSION ||
+	    hdr->version < EEPROM_5000_TX_POWER_VERSION)
+		goto err;
+
+	return 0;
+err:
+	IWL_ERROR("Unsuported EEPROM VER=0x%x < 0x%x CALIB=0x%x < 0x%x\n",
+		  eeprom_ver, EEPROM_5000_EEPROM_VERSION,
+		  hdr->version, EEPROM_5000_TX_POWER_VERSION);
+	return -EINVAL;
+
+}
+
+static void iwl5000_gain_computation(struct iwl_priv *priv,
+		u32 average_noise[NUM_RX_CHAINS],
+		u16 min_average_noise_antenna_i,
+		u32 min_average_noise)
+{
+	int i;
+	s32 delta_g;
+	struct iwl_chain_noise_data *data = &priv->chain_noise_data;
+
+	/* Find Gain Code for the antennas B and C */
+	for (i = 1; i < NUM_RX_CHAINS; i++) {
+		if ((data->disconn_array[i])) {
+			data->delta_gain_code[i] = 0;
+			continue;
+		}
+		delta_g = (1000 * ((s32)average_noise[0] -
+			(s32)average_noise[i])) / 1500;
+		/* bound gain by 2 bits value max, 3rd bit is sign */
+		data->delta_gain_code[i] =
+			min(abs(delta_g), CHAIN_NOISE_MAX_DELTA_GAIN_CODE);
+
+		if (delta_g < 0)
+			/* set negative sign */
+			data->delta_gain_code[i] |= (1 << 2);
+	}
+
+	IWL_DEBUG_CALIB("Delta gains: ANT_B = %d  ANT_C = %d\n",
+			data->delta_gain_code[1], data->delta_gain_code[2]);
+
+	if (!data->radio_write) {
+		struct iwl5000_calibration_chain_noise_gain_cmd cmd;
+		memset(&cmd, 0, sizeof(cmd));
+
+		cmd.op_code = IWL5000_PHY_CALIBRATE_CHAIN_NOISE_GAIN_CMD;
+		cmd.delta_gain_1 = data->delta_gain_code[1];
+		cmd.delta_gain_2 = data->delta_gain_code[2];
+		iwl_send_cmd_pdu_async(priv, REPLY_PHY_CALIBRATION_CMD,
+			sizeof(cmd), &cmd, NULL);
+
+		data->radio_write = 1;
+		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;
+}
+
+static void iwl5000_chain_noise_reset(struct iwl_priv *priv)
+{
+	struct iwl_chain_noise_data *data = &priv->chain_noise_data;
+
+	if ((data->state == IWL_CHAIN_NOISE_ALIVE) && iwl_is_associated(priv)) {
+		struct iwl5000_calibration_chain_noise_reset_cmd cmd;
+
+		memset(&cmd, 0, sizeof(cmd));
+		cmd.op_code = IWL5000_PHY_CALIBRATE_CHAIN_NOISE_RESET_CMD;
+		if (iwl_send_cmd_pdu(priv, REPLY_PHY_CALIBRATION_CMD,
+			sizeof(cmd), &cmd))
+			IWL_ERROR("Could not send REPLY_PHY_CALIBRATION_CMD\n");
+		data->state = IWL_CHAIN_NOISE_ACCUMULATE;
+		IWL_DEBUG_CALIB("Run chain_noise_calibrate\n");
+	}
+}
+
+static void iwl5000_rts_tx_cmd_flag(struct ieee80211_tx_info *info,
+			__le32 *tx_flags)
+{
+	if ((info->flags & IEEE80211_TX_CTL_USE_RTS_CTS) ||
+	    (info->flags & IEEE80211_TX_CTL_USE_CTS_PROTECT))
+		*tx_flags |= TX_CMD_FLG_RTS_CTS_MSK;
+	else
+		*tx_flags &= ~TX_CMD_FLG_RTS_CTS_MSK;
+}
+
+static struct iwl_sensitivity_ranges iwl5000_sensitivity = {
+	.min_nrg_cck = 95,
+	.max_nrg_cck = 0,
+	.auto_corr_min_ofdm = 90,
+	.auto_corr_min_ofdm_mrc = 170,
+	.auto_corr_min_ofdm_x1 = 120,
+	.auto_corr_min_ofdm_mrc_x1 = 240,
+
+	.auto_corr_max_ofdm = 120,
+	.auto_corr_max_ofdm_mrc = 210,
+	.auto_corr_max_ofdm_x1 = 155,
+	.auto_corr_max_ofdm_mrc_x1 = 290,
+
+	.auto_corr_min_cck = 125,
+	.auto_corr_max_cck = 200,
+	.auto_corr_min_cck_mrc = 170,
+	.auto_corr_max_cck_mrc = 400,
+	.nrg_th_cck = 95,
+	.nrg_th_ofdm = 95,
+};
+
+static const u8 *iwl5000_eeprom_query_addr(const struct iwl_priv *priv,
+					   size_t offset)
+{
+	u32 address = eeprom_indirect_address(priv, offset);
+	BUG_ON(address >= priv->cfg->eeprom_size);
+	return &priv->eeprom[address];
+}
+
+/*
+ *  Calibration
+ */
+static int iwl5000_send_Xtal_calib(struct iwl_priv *priv)
+{
+	u16 *xtal_calib = (u16 *)iwl_eeprom_query_addr(priv, EEPROM_5000_XTAL);
+
+	struct iwl5000_calibration cal_cmd = {
+		.op_code = IWL5000_PHY_CALIBRATE_CRYSTAL_FRQ_CMD,
+		.data = {
+			(u8)xtal_calib[0],
+			(u8)xtal_calib[1],
+		}
+	};
+
+	return iwl_send_cmd_pdu(priv, REPLY_PHY_CALIBRATION_CMD,
+				sizeof(cal_cmd), &cal_cmd);
+}
+
+static int iwl5000_send_calib_results(struct iwl_priv *priv)
+{
+	int ret = 0;
+
+	struct iwl_host_cmd hcmd = {
+		.id = REPLY_PHY_CALIBRATION_CMD,
+		.meta.flags = CMD_SIZE_HUGE,
+	};
+
+	if (priv->calib_results.lo_res) {
+		hcmd.len = priv->calib_results.lo_res_len;
+		hcmd.data = priv->calib_results.lo_res;
+		ret = iwl_send_cmd_sync(priv, &hcmd);
+
+		if (ret)
+			goto err;
+	}
+
+	if (priv->calib_results.tx_iq_res) {
+		hcmd.len = priv->calib_results.tx_iq_res_len;
+		hcmd.data = priv->calib_results.tx_iq_res;
+		ret = iwl_send_cmd_sync(priv, &hcmd);
+
+		if (ret)
+			goto err;
+	}
+
+	if (priv->calib_results.tx_iq_perd_res) {
+		hcmd.len = priv->calib_results.tx_iq_perd_res_len;
+		hcmd.data = priv->calib_results.tx_iq_perd_res;
+		ret = iwl_send_cmd_sync(priv, &hcmd);
+
+		if (ret)
+			goto err;
+	}
+
+	return 0;
+err:
+	IWL_ERROR("Error %d\n", ret);
+	return ret;
+}
+
+static int iwl5000_send_calib_cfg(struct iwl_priv *priv)
+{
+	struct iwl5000_calib_cfg_cmd calib_cfg_cmd;
+	struct iwl_host_cmd cmd = {
+		.id = CALIBRATION_CFG_CMD,
+		.len = sizeof(struct iwl5000_calib_cfg_cmd),
+		.data = &calib_cfg_cmd,
+	};
+
+	memset(&calib_cfg_cmd, 0, sizeof(calib_cfg_cmd));
+	calib_cfg_cmd.ucd_calib_cfg.once.is_enable = IWL_CALIB_INIT_CFG_ALL;
+	calib_cfg_cmd.ucd_calib_cfg.once.start = IWL_CALIB_INIT_CFG_ALL;
+	calib_cfg_cmd.ucd_calib_cfg.once.send_res = IWL_CALIB_INIT_CFG_ALL;
+	calib_cfg_cmd.ucd_calib_cfg.flags = IWL_CALIB_INIT_CFG_ALL;
+
+	return iwl_send_cmd(priv, &cmd);
+}
+
+static void iwl5000_rx_calib_result(struct iwl_priv *priv,
+			     struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = (void *)rxb->skb->data;
+	struct iwl5000_calib_hdr *hdr = (struct iwl5000_calib_hdr *)pkt->u.raw;
+	int len = le32_to_cpu(pkt->len) & FH_RSCSR_FRAME_SIZE_MSK;
+
+	iwl_free_calib_results(priv);
+
+	/* reduce the size of the length field itself */
+	len -= 4;
+
+	switch (hdr->op_code) {
+	case IWL5000_PHY_CALIBRATE_LO_CMD:
+		priv->calib_results.lo_res = kzalloc(len, GFP_ATOMIC);
+		priv->calib_results.lo_res_len = len;
+		memcpy(priv->calib_results.lo_res, pkt->u.raw, len);
+		break;
+	case IWL5000_PHY_CALIBRATE_TX_IQ_CMD:
+		priv->calib_results.tx_iq_res = kzalloc(len, GFP_ATOMIC);
+		priv->calib_results.tx_iq_res_len = len;
+		memcpy(priv->calib_results.tx_iq_res, pkt->u.raw, len);
+		break;
+	case IWL5000_PHY_CALIBRATE_TX_IQ_PERD_CMD:
+		priv->calib_results.tx_iq_perd_res = kzalloc(len, GFP_ATOMIC);
+		priv->calib_results.tx_iq_perd_res_len = len;
+		memcpy(priv->calib_results.tx_iq_perd_res, pkt->u.raw, len);
+		break;
+	default:
+		IWL_ERROR("Unknown calibration notification %d\n",
+			  hdr->op_code);
+		return;
+	}
+}
+
+static void iwl5000_rx_calib_complete(struct iwl_priv *priv,
+			       struct iwl_rx_mem_buffer *rxb)
+{
+	IWL_DEBUG_INFO("Init. calibration is completed, restarting fw.\n");
+	queue_work(priv->workqueue, &priv->restart);
+}
+
+/*
+ * ucode
+ */
+static int iwl5000_load_section(struct iwl_priv *priv,
+				struct fw_desc *image,
+				u32 dst_addr)
+{
+	int ret = 0;
+	unsigned long flags;
+
+	dma_addr_t phy_addr = image->p_addr;
+	u32 byte_cnt = image->len;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	ret = iwl_grab_nic_access(priv);
+	if (ret) {
+		spin_unlock_irqrestore(&priv->lock, flags);
+		return ret;
+	}
+
+	iwl_write_direct32(priv,
+		FH_TCSR_CHNL_TX_CONFIG_REG(FH_SRVC_CHNL),
+		FH_TCSR_TX_CONFIG_REG_VAL_DMA_CHNL_PAUSE);
+
+	iwl_write_direct32(priv,
+		FH_SRVC_CHNL_SRAM_ADDR_REG(FH_SRVC_CHNL), dst_addr);
+
+	iwl_write_direct32(priv,
+		FH_TFDIB_CTRL0_REG(FH_SRVC_CHNL),
+		phy_addr & FH_MEM_TFDIB_DRAM_ADDR_LSB_MSK);
+
+	/* FIME: write the MSB of the phy_addr in CTRL1
+	 * iwl_write_direct32(priv,
+		IWL_FH_TFDIB_CTRL1_REG(IWL_FH_SRVC_CHNL),
+		((phy_addr & MSB_MSK)
+			<< FH_MEM_TFDIB_REG1_ADDR_BITSHIFT) | byte_count);
+	 */
+	iwl_write_direct32(priv,
+		FH_TFDIB_CTRL1_REG(FH_SRVC_CHNL), byte_cnt);
+	iwl_write_direct32(priv,
+		FH_TCSR_CHNL_TX_BUF_STS_REG(FH_SRVC_CHNL),
+		1 << FH_TCSR_CHNL_TX_BUF_STS_REG_POS_TB_NUM |
+		1 << FH_TCSR_CHNL_TX_BUF_STS_REG_POS_TB_IDX |
+		FH_TCSR_CHNL_TX_BUF_STS_REG_VAL_TFDB_VALID);
+
+	iwl_write_direct32(priv,
+		FH_TCSR_CHNL_TX_CONFIG_REG(FH_SRVC_CHNL),
+		FH_TCSR_TX_CONFIG_REG_VAL_DMA_CHNL_ENABLE	|
+		FH_TCSR_TX_CONFIG_REG_VAL_DMA_CREDIT_DISABLE_VAL |
+		FH_TCSR_TX_CONFIG_REG_VAL_CIRQ_HOST_ENDTFD);
+
+	iwl_release_nic_access(priv);
+	spin_unlock_irqrestore(&priv->lock, flags);
+	return 0;
+}
+
+static int iwl5000_load_given_ucode(struct iwl_priv *priv,
+		struct fw_desc *inst_image,
+		struct fw_desc *data_image)
+{
+	int ret = 0;
+
+	ret = iwl5000_load_section(
+		priv, inst_image, RTC_INST_LOWER_BOUND);
+	if (ret)
+		return ret;
+
+	IWL_DEBUG_INFO("INST uCode section being loaded...\n");
+	ret = wait_event_interruptible_timeout(priv->wait_command_queue,
+				priv->ucode_write_complete, 5 * HZ);
+	if (ret == -ERESTARTSYS) {
+		IWL_ERROR("Could not load the INST uCode section due "
+			"to interrupt\n");
+		return ret;
+	}
+	if (!ret) {
+		IWL_ERROR("Could not load the INST uCode section\n");
+		return -ETIMEDOUT;
+	}
+
+	priv->ucode_write_complete = 0;
+
+	ret = iwl5000_load_section(
+		priv, data_image, RTC_DATA_LOWER_BOUND);
+	if (ret)
+		return ret;
+
+	IWL_DEBUG_INFO("DATA uCode section being loaded...\n");
+
+	ret = wait_event_interruptible_timeout(priv->wait_command_queue,
+				priv->ucode_write_complete, 5 * HZ);
+	if (ret == -ERESTARTSYS) {
+		IWL_ERROR("Could not load the INST uCode section due "
+			"to interrupt\n");
+		return ret;
+	} else if (!ret) {
+		IWL_ERROR("Could not load the DATA uCode section\n");
+		return -ETIMEDOUT;
+	} else
+		ret = 0;
+
+	priv->ucode_write_complete = 0;
+
+	return ret;
+}
+
+static int iwl5000_load_ucode(struct iwl_priv *priv)
+{
+	int ret = 0;
+
+	/* check whether init ucode should be loaded, or rather runtime ucode */
+	if (priv->ucode_init.len && (priv->ucode_type == UCODE_NONE)) {
+		IWL_DEBUG_INFO("Init ucode found. Loading init ucode...\n");
+		ret = iwl5000_load_given_ucode(priv,
+			&priv->ucode_init, &priv->ucode_init_data);
+		if (!ret) {
+			IWL_DEBUG_INFO("Init ucode load complete.\n");
+			priv->ucode_type = UCODE_INIT;
+		}
+	} else {
+		IWL_DEBUG_INFO("Init ucode not found, or already loaded. "
+			"Loading runtime ucode...\n");
+		ret = iwl5000_load_given_ucode(priv,
+			&priv->ucode_code, &priv->ucode_data);
+		if (!ret) {
+			IWL_DEBUG_INFO("Runtime ucode load complete.\n");
+			priv->ucode_type = UCODE_RT;
+		}
+	}
+
+	return ret;
+}
+
+static void iwl5000_init_alive_start(struct iwl_priv *priv)
+{
+	int ret = 0;
+
+	/* 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;
+	}
+
+	/* initialize uCode was loaded... 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;
+	}
+
+	iwl_clear_stations_table(priv);
+	ret = priv->cfg->ops->lib->alive_notify(priv);
+	if (ret) {
+		IWL_WARNING("Could not complete ALIVE transition: %d\n", ret);
+		goto restart;
+	}
+
+	iwl5000_send_calib_cfg(priv);
+	return;
+
+restart:
+	/* real restart (first load init_ucode) */
+	queue_work(priv->workqueue, &priv->restart);
+}
+
+static void iwl5000_set_wr_ptrs(struct iwl_priv *priv,
+				int txq_id, u32 index)
+{
+	iwl_write_direct32(priv, HBUS_TARG_WRPTR,
+			(index & 0xff) | (txq_id << 8));
+	iwl_write_prph(priv, IWL50_SCD_QUEUE_RDPTR(txq_id), index);
+}
+
+static void iwl5000_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_prph(priv, IWL50_SCD_QUEUE_STATUS_BITS(txq_id),
+			(active << IWL50_SCD_QUEUE_STTS_REG_POS_ACTIVE) |
+			(tx_fifo_id << IWL50_SCD_QUEUE_STTS_REG_POS_TXF) |
+			(1 << IWL50_SCD_QUEUE_STTS_REG_POS_WSL) |
+			IWL50_SCD_QUEUE_STTS_REG_MSK);
+
+	txq->sched_retry = scd_retry;
+
+	IWL_DEBUG_INFO("%s %s Queue %d on AC %d\n",
+		       active ? "Activate" : "Deactivate",
+		       scd_retry ? "BA" : "AC", txq_id, tx_fifo_id);
+}
+
+static int iwl5000_send_wimax_coex(struct iwl_priv *priv)
+{
+	struct iwl_wimax_coex_cmd coex_cmd;
+
+	memset(&coex_cmd, 0, sizeof(coex_cmd));
+
+	return iwl_send_cmd_pdu(priv, COEX_PRIORITY_TABLE_CMD,
+				sizeof(coex_cmd), &coex_cmd);
+}
+
+static int iwl5000_alive_notify(struct iwl_priv *priv)
+{
+	u32 a;
+	int i = 0;
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	ret = iwl_grab_nic_access(priv);
+	if (ret) {
+		spin_unlock_irqrestore(&priv->lock, flags);
+		return ret;
+	}
+
+	priv->scd_base_addr = iwl_read_prph(priv, IWL50_SCD_SRAM_BASE_ADDR);
+	a = priv->scd_base_addr + IWL50_SCD_CONTEXT_DATA_OFFSET;
+	for (; a < priv->scd_base_addr + IWL50_SCD_TX_STTS_BITMAP_OFFSET;
+		a += 4)
+		iwl_write_targ_mem(priv, a, 0);
+	for (; a < priv->scd_base_addr + IWL50_SCD_TRANSLATE_TBL_OFFSET;
+		a += 4)
+		iwl_write_targ_mem(priv, a, 0);
+	for (; a < sizeof(u16) * priv->hw_params.max_txq_num; a += 4)
+		iwl_write_targ_mem(priv, a, 0);
+
+	iwl_write_prph(priv, IWL50_SCD_DRAM_BASE_ADDR,
+		(priv->shared_phys +
+		 offsetof(struct iwl5000_shared, queues_byte_cnt_tbls)) >> 10);
+	iwl_write_prph(priv, IWL50_SCD_QUEUECHAIN_SEL,
+		IWL50_SCD_QUEUECHAIN_SEL_ALL(
+			priv->hw_params.max_txq_num));
+	iwl_write_prph(priv, IWL50_SCD_AGGR_SEL, 0);
+
+	/* initiate the queues */
+	for (i = 0; i < priv->hw_params.max_txq_num; i++) {
+		iwl_write_prph(priv, IWL50_SCD_QUEUE_RDPTR(i), 0);
+		iwl_write_direct32(priv, HBUS_TARG_WRPTR, 0 | (i << 8));
+		iwl_write_targ_mem(priv, priv->scd_base_addr +
+				IWL50_SCD_CONTEXT_QUEUE_OFFSET(i), 0);
+		iwl_write_targ_mem(priv, priv->scd_base_addr +
+				IWL50_SCD_CONTEXT_QUEUE_OFFSET(i) +
+				sizeof(u32),
+				((SCD_WIN_SIZE <<
+				IWL50_SCD_QUEUE_CTX_REG2_WIN_SIZE_POS) &
+				IWL50_SCD_QUEUE_CTX_REG2_WIN_SIZE_MSK) |
+				((SCD_FRAME_LIMIT <<
+				IWL50_SCD_QUEUE_CTX_REG2_FRAME_LIMIT_POS) &
+				IWL50_SCD_QUEUE_CTX_REG2_FRAME_LIMIT_MSK));
+	}
+
+	iwl_write_prph(priv, IWL50_SCD_INTERRUPT_MASK,
+			IWL_MASK(0, priv->hw_params.max_txq_num));
+
+	/* Activate all Tx DMA/FIFO channels */
+	priv->cfg->ops->lib->txq_set_sched(priv, IWL_MASK(0, 7));
+
+	iwl5000_set_wr_ptrs(priv, IWL_CMD_QUEUE_NUM, 0);
+	/* map qos queues to fifos one-to-one */
+	for (i = 0; i < ARRAY_SIZE(iwl5000_default_queue_to_tx_fifo); i++) {
+		int ac = iwl5000_default_queue_to_tx_fifo[i];
+		iwl_txq_ctx_activate(priv, i);
+		iwl5000_tx_queue_set_status(priv, &priv->txq[i], ac, 0);
+	}
+	/* TODO - need to initialize those FIFOs inside the loop above,
+	 * not only mark them as active */
+	iwl_txq_ctx_activate(priv, 4);
+	iwl_txq_ctx_activate(priv, 7);
+	iwl_txq_ctx_activate(priv, 8);
+	iwl_txq_ctx_activate(priv, 9);
+
+	iwl_release_nic_access(priv);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+
+	iwl5000_send_wimax_coex(priv);
+
+	iwl5000_send_Xtal_calib(priv);
+
+	if (priv->ucode_type == UCODE_RT)
+		iwl5000_send_calib_results(priv);
+
+	return 0;
+}
+
+static int iwl5000_hw_set_hw_params(struct iwl_priv *priv)
+{
+	if ((priv->cfg->mod_params->num_of_queues > IWL50_NUM_QUEUES) ||
+	    (priv->cfg->mod_params->num_of_queues < IWL_MIN_NUM_QUEUES)) {
+		IWL_ERROR("invalid queues_num, should be between %d and %d\n",
+			  IWL_MIN_NUM_QUEUES, IWL50_NUM_QUEUES);
+		return -EINVAL;
+	}
+
+	priv->hw_params.max_txq_num = priv->cfg->mod_params->num_of_queues;
+	priv->hw_params.first_ampdu_q = IWL50_FIRST_AMPDU_QUEUE;
+	priv->hw_params.max_stations = IWL5000_STATION_COUNT;
+	priv->hw_params.bcast_sta_id = IWL5000_BROADCAST_ID;
+	priv->hw_params.max_data_size = IWL50_RTC_DATA_SIZE;
+	priv->hw_params.max_inst_size = IWL50_RTC_INST_SIZE;
+	priv->hw_params.max_bsm_size = 0;
+	priv->hw_params.fat_channel =  BIT(IEEE80211_BAND_2GHZ) |
+					BIT(IEEE80211_BAND_5GHZ);
+	priv->hw_params.sens = &iwl5000_sensitivity;
+
+	switch (priv->hw_rev & CSR_HW_REV_TYPE_MSK) {
+	case CSR_HW_REV_TYPE_5100:
+	case CSR_HW_REV_TYPE_5150:
+		priv->hw_params.tx_chains_num = 1;
+		priv->hw_params.rx_chains_num = 2;
+		/* FIXME: move to ANT_A, ANT_B, ANT_C enum */
+		priv->hw_params.valid_tx_ant = ANT_A;
+		priv->hw_params.valid_rx_ant = ANT_AB;
+		break;
+	case CSR_HW_REV_TYPE_5300:
+	case CSR_HW_REV_TYPE_5350:
+		priv->hw_params.tx_chains_num = 3;
+		priv->hw_params.rx_chains_num = 3;
+		priv->hw_params.valid_tx_ant = ANT_ABC;
+		priv->hw_params.valid_rx_ant = ANT_ABC;
+		break;
+	}
+
+	switch (priv->hw_rev & CSR_HW_REV_TYPE_MSK) {
+	case CSR_HW_REV_TYPE_5100:
+	case CSR_HW_REV_TYPE_5300:
+		/* 5X00 wants in Celsius */
+		priv->hw_params.ct_kill_threshold = CT_KILL_THRESHOLD;
+		break;
+	case CSR_HW_REV_TYPE_5150:
+	case CSR_HW_REV_TYPE_5350:
+		/* 5X50 wants in Kelvin */
+		priv->hw_params.ct_kill_threshold =
+				CELSIUS_TO_KELVIN(CT_KILL_THRESHOLD);
+		break;
+	}
+
+	return 0;
+}
+
+static int iwl5000_alloc_shared_mem(struct iwl_priv *priv)
+{
+	priv->shared_virt = pci_alloc_consistent(priv->pci_dev,
+					sizeof(struct iwl5000_shared),
+					&priv->shared_phys);
+	if (!priv->shared_virt)
+		return -ENOMEM;
+
+	memset(priv->shared_virt, 0, sizeof(struct iwl5000_shared));
+
+	priv->rb_closed_offset = offsetof(struct iwl5000_shared, rb_closed);
+
+	return 0;
+}
+
+static void iwl5000_free_shared_mem(struct iwl_priv *priv)
+{
+	if (priv->shared_virt)
+		pci_free_consistent(priv->pci_dev,
+				    sizeof(struct iwl5000_shared),
+				    priv->shared_virt,
+				    priv->shared_phys);
+}
+
+static int iwl5000_shared_mem_rx_idx(struct iwl_priv *priv)
+{
+	struct iwl5000_shared *s = priv->shared_virt;
+	return le32_to_cpu(s->rb_closed) & 0xFFF;
+}
+
+/**
+ * iwl5000_txq_update_byte_cnt_tbl - Set up entry in Tx byte-count array
+ */
+static void iwl5000_txq_update_byte_cnt_tbl(struct iwl_priv *priv,
+					    struct iwl_tx_queue *txq,
+					    u16 byte_cnt)
+{
+	struct iwl5000_shared *shared_data = priv->shared_virt;
+	int txq_id = txq->q.id;
+	u8 sec_ctl = 0;
+	u8 sta = 0;
+	int len;
+
+	len = byte_cnt + IWL_TX_CRC_SIZE + IWL_TX_DELIMITER_SIZE;
+
+	if (txq_id != IWL_CMD_QUEUE_NUM) {
+		sta = txq->cmd[txq->q.write_ptr].cmd.tx.sta_id;
+		sec_ctl = txq->cmd[txq->q.write_ptr].cmd.tx.sec_ctl;
+
+		switch (sec_ctl & TX_CMD_SEC_MSK) {
+		case TX_CMD_SEC_CCM:
+			len += CCMP_MIC_LEN;
+			break;
+		case TX_CMD_SEC_TKIP:
+			len += TKIP_ICV_LEN;
+			break;
+		case TX_CMD_SEC_WEP:
+			len += WEP_IV_LEN + WEP_ICV_LEN;
+			break;
+		}
+	}
+
+	IWL_SET_BITS16(shared_data->queues_byte_cnt_tbls[txq_id].
+		       tfd_offset[txq->q.write_ptr], byte_cnt, len);
+
+	IWL_SET_BITS16(shared_data->queues_byte_cnt_tbls[txq_id].
+		       tfd_offset[txq->q.write_ptr], sta_id, sta);
+
+	if (txq->q.write_ptr < IWL50_MAX_WIN_SIZE) {
+		IWL_SET_BITS16(shared_data->queues_byte_cnt_tbls[txq_id].
+			tfd_offset[IWL50_QUEUE_SIZE + txq->q.write_ptr],
+			byte_cnt, len);
+		IWL_SET_BITS16(shared_data->queues_byte_cnt_tbls[txq_id].
+			tfd_offset[IWL50_QUEUE_SIZE + txq->q.write_ptr],
+			sta_id, sta);
+	}
+}
+
+static void iwl5000_txq_inval_byte_cnt_tbl(struct iwl_priv *priv,
+					   struct iwl_tx_queue *txq)
+{
+	int txq_id = txq->q.id;
+	struct iwl5000_shared *shared_data = priv->shared_virt;
+	u8 sta = 0;
+
+	if (txq_id != IWL_CMD_QUEUE_NUM)
+		sta = txq->cmd[txq->q.read_ptr].cmd.tx.sta_id;
+
+	shared_data->queues_byte_cnt_tbls[txq_id].tfd_offset[txq->q.read_ptr].
+					val = cpu_to_le16(1 | (sta << 12));
+
+	if (txq->q.write_ptr < IWL50_MAX_WIN_SIZE) {
+		shared_data->queues_byte_cnt_tbls[txq_id].
+			tfd_offset[IWL50_QUEUE_SIZE + txq->q.read_ptr].
+				val = cpu_to_le16(1 | (sta << 12));
+	}
+}
+
+static int iwl5000_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 & IWL_SCD_QUEUE_RA_TID_MAP_RATID_MSK;
+
+	tbl_dw_addr = priv->scd_base_addr +
+			IWL50_SCD_TRANSLATE_TBL_OFFSET_QUEUE(txq_id);
+
+	tbl_dw = iwl_read_targ_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_targ_mem(priv, tbl_dw_addr, tbl_dw);
+
+	return 0;
+}
+static void iwl5000_tx_queue_stop_scheduler(struct iwl_priv *priv, u16 txq_id)
+{
+	/* Simply stop the queue, but don't change any configuration;
+	 * the SCD_ACT_EN bit is the write-enable mask for the ACTIVE bit. */
+	iwl_write_prph(priv,
+		IWL50_SCD_QUEUE_STATUS_BITS(txq_id),
+		(0 << IWL50_SCD_QUEUE_STTS_REG_POS_ACTIVE)|
+		(1 << IWL50_SCD_QUEUE_STTS_REG_POS_SCD_ACT_EN));
+}
+
+static int iwl5000_txq_agg_enable(struct iwl_priv *priv, int txq_id,
+				  int tx_fifo, int sta_id, int tid, u16 ssn_idx)
+{
+	unsigned long flags;
+	int ret;
+	u16 ra_tid;
+
+	if ((IWL50_FIRST_AMPDU_QUEUE > txq_id) ||
+	    (IWL50_FIRST_AMPDU_QUEUE + IWL50_NUM_AMPDU_QUEUES <= txq_id)) {
+		IWL_WARNING("queue number out of range: %d, must be %d to %d\n",
+			txq_id, IWL50_FIRST_AMPDU_QUEUE,
+			IWL50_FIRST_AMPDU_QUEUE + IWL50_NUM_AMPDU_QUEUES - 1);
+		return -EINVAL;
+	}
+
+	ra_tid = BUILD_RAxTID(sta_id, tid);
+
+	/* Modify device's station table to Tx this TID */
+	iwl_sta_modify_enable_tid_tx(priv, sta_id, tid);
+
+	spin_lock_irqsave(&priv->lock, flags);
+	ret = iwl_grab_nic_access(priv);
+	if (ret) {
+		spin_unlock_irqrestore(&priv->lock, flags);
+		return ret;
+	}
+
+	/* Stop this Tx queue before configuring it */
+	iwl5000_tx_queue_stop_scheduler(priv, txq_id);
+
+	/* Map receiver-address / traffic-ID to this queue */
+	iwl5000_tx_queue_set_q2ratid(priv, ra_tid, txq_id);
+
+	/* Set this queue as a chain-building queue */
+	iwl_set_bits_prph(priv, IWL50_SCD_QUEUECHAIN_SEL, (1<<txq_id));
+
+	/* enable aggregations for the queue */
+	iwl_set_bits_prph(priv, IWL50_SCD_AGGR_SEL, (1<<txq_id));
+
+	/* Place first TFD at index corresponding to start sequence number.
+	 * Assumes that ssn_idx is valid (!= 0xFFF) */
+	priv->txq[txq_id].q.read_ptr = (ssn_idx & 0xff);
+	priv->txq[txq_id].q.write_ptr = (ssn_idx & 0xff);
+	iwl5000_set_wr_ptrs(priv, txq_id, ssn_idx);
+
+	/* Set up Tx window size and frame limit for this queue */
+	iwl_write_targ_mem(priv, priv->scd_base_addr +
+			IWL50_SCD_CONTEXT_QUEUE_OFFSET(txq_id) +
+			sizeof(u32),
+			((SCD_WIN_SIZE <<
+			IWL50_SCD_QUEUE_CTX_REG2_WIN_SIZE_POS) &
+			IWL50_SCD_QUEUE_CTX_REG2_WIN_SIZE_MSK) |
+			((SCD_FRAME_LIMIT <<
+			IWL50_SCD_QUEUE_CTX_REG2_FRAME_LIMIT_POS) &
+			IWL50_SCD_QUEUE_CTX_REG2_FRAME_LIMIT_MSK));
+
+	iwl_set_bits_prph(priv, IWL50_SCD_INTERRUPT_MASK, (1 << txq_id));
+
+	/* Set up Status area in SRAM, map to Tx DMA/FIFO, activate the queue */
+	iwl5000_tx_queue_set_status(priv, &priv->txq[txq_id], tx_fifo, 1);
+
+	iwl_release_nic_access(priv);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return 0;
+}
+
+static int iwl5000_txq_agg_disable(struct iwl_priv *priv, u16 txq_id,
+				   u16 ssn_idx, u8 tx_fifo)
+{
+	int ret;
+
+	if ((IWL50_FIRST_AMPDU_QUEUE > txq_id) ||
+	    (IWL50_FIRST_AMPDU_QUEUE + IWL50_NUM_AMPDU_QUEUES <= txq_id)) {
+		IWL_WARNING("queue number out of range: %d, must be %d to %d\n",
+			txq_id, IWL50_FIRST_AMPDU_QUEUE,
+			IWL50_FIRST_AMPDU_QUEUE + IWL50_NUM_AMPDU_QUEUES - 1);
+		return -EINVAL;
+	}
+
+	ret = iwl_grab_nic_access(priv);
+	if (ret)
+		return ret;
+
+	iwl5000_tx_queue_stop_scheduler(priv, txq_id);
+
+	iwl_clear_bits_prph(priv, IWL50_SCD_AGGR_SEL, (1 << txq_id));
+
+	priv->txq[txq_id].q.read_ptr = (ssn_idx & 0xff);
+	priv->txq[txq_id].q.write_ptr = (ssn_idx & 0xff);
+	/* supposes that ssn_idx is valid (!= 0xFFF) */
+	iwl5000_set_wr_ptrs(priv, txq_id, ssn_idx);
+
+	iwl_clear_bits_prph(priv, IWL50_SCD_INTERRUPT_MASK, (1 << txq_id));
+	iwl_txq_ctx_deactivate(priv, txq_id);
+	iwl5000_tx_queue_set_status(priv, &priv->txq[txq_id], tx_fifo, 0);
+
+	iwl_release_nic_access(priv);
+
+	return 0;
+}
+
+static u16 iwl5000_build_addsta_hcmd(const struct iwl_addsta_cmd *cmd, u8 *data)
+{
+	u16 size = (u16)sizeof(struct iwl_addsta_cmd);
+	memcpy(data, cmd, size);
+	return size;
+}
+
+
+/*
+ * Activate/Deactivat Tx DMA/FIFO channels according tx fifos mask
+ * must be called under priv->lock and mac access
+ */
+static void iwl5000_txq_set_sched(struct iwl_priv *priv, u32 mask)
+{
+	iwl_write_prph(priv, IWL50_SCD_TXFACT, mask);
+}
+
+
+static inline u32 iwl5000_get_scd_ssn(struct iwl5000_tx_resp *tx_resp)
+{
+	return le32_to_cpup((__le32*)&tx_resp->status +
+			    tx_resp->frame_count) & MAX_SN;
+}
+
+static int iwl5000_tx_status_reply_tx(struct iwl_priv *priv,
+				      struct iwl_ht_agg *agg,
+				      struct iwl5000_tx_resp *tx_resp,
+				      int txq_id, u16 start_idx)
+{
+	u16 status;
+	struct agg_tx_status *frame_status = &tx_resp->status;
+	struct ieee80211_tx_info *info = NULL;
+	struct ieee80211_hdr *hdr = NULL;
+	u32 rate_n_flags = le32_to_cpu(tx_resp->rate_n_flags);
+	int i, sh, idx;
+	u16 seq;
+
+	if (agg->wait_for_ba)
+		IWL_DEBUG_TX_REPLY("got tx response w/o block-ack\n");
+
+	agg->frame_count = tx_resp->frame_count;
+	agg->start_idx = start_idx;
+	agg->rate_n_flags = rate_n_flags;
+	agg->bitmap = 0;
+
+	/* # frames attempted by Tx command */
+	if (agg->frame_count == 1) {
+		/* Only one frame was attempted; no block-ack will arrive */
+		status = le16_to_cpu(frame_status[0].status);
+		idx = start_idx;
+
+		/* FIXME: code repetition */
+		IWL_DEBUG_TX_REPLY("FrameCnt = %d, StartIdx=%d idx=%d\n",
+				   agg->frame_count, agg->start_idx, idx);
+
+		info = IEEE80211_SKB_CB(priv->txq[txq_id].txb[idx].skb[0]);
+		info->status.retry_count = tx_resp->failure_frame;
+		info->flags &= ~IEEE80211_TX_CTL_AMPDU;
+		info->flags |= iwl_is_tx_success(status)?
+			IEEE80211_TX_STAT_ACK : 0;
+		iwl_hwrate_to_tx_control(priv, rate_n_flags, info);
+
+		/* 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", rate_n_flags);
+
+		agg->wait_for_ba = 0;
+	} else {
+		/* Two or more frames were attempted; expect block-ack */
+		u64 bitmap = 0;
+		int start = agg->start_idx;
+
+		/* Construct bit-map of pending frames within Tx window */
+		for (i = 0; i < agg->frame_count; i++) {
+			u16 sc;
+			status = le16_to_cpu(frame_status[i].status);
+			seq  = le16_to_cpu(frame_status[i].sequence);
+			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->bitmap = bitmap;
+		agg->start_idx = start;
+		IWL_DEBUG_TX_REPLY("Frames %d start_idx=%d bitmap=0x%llx\n",
+				   agg->frame_count, agg->start_idx,
+				   (unsigned long long)agg->bitmap);
+
+		if (bitmap)
+			agg->wait_for_ba = 1;
+	}
+	return 0;
+}
+
+static void iwl5000_rx_reply_tx(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);
+	struct iwl_tx_queue *txq = &priv->txq[txq_id];
+	struct ieee80211_tx_info *info;
+	struct iwl5000_tx_resp *tx_resp = (void *)&pkt->u.raw[0];
+	u32  status = le16_to_cpu(tx_resp->status.status);
+	int tid = MAX_TID_COUNT, sta_id = IWL_INVALID_STATION;
+	struct ieee80211_hdr *hdr;
+	u8 *qc = NULL;
+
+	if ((index >= txq->q.n_bd) || (iwl_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.write_ptr,
+			  txq->q.read_ptr);
+		return;
+	}
+
+	info = IEEE80211_SKB_CB(txq->txb[txq->q.read_ptr].skb[0]);
+	memset(&info->status, 0, sizeof(info->status));
+
+	hdr = iwl_tx_queue_get_hdr(priv, txq_id, index);
+	if (ieee80211_is_data_qos(hdr->frame_control)) {
+		qc = ieee80211_get_qos_ctl(hdr);
+		tid = qc[0] & 0xf;
+	}
+
+	sta_id = iwl_get_ra_sta_id(priv, hdr);
+	if (txq->sched_retry && unlikely(sta_id == IWL_INVALID_STATION)) {
+		IWL_ERROR("Station not known\n");
+		return;
+	}
+
+	if (txq->sched_retry) {
+		const u32 scd_ssn = iwl5000_get_scd_ssn(tx_resp);
+		struct iwl_ht_agg *agg = NULL;
+
+		if (!qc)
+			return;
+
+		agg = &priv->stations[sta_id].tid[tid].agg;
+
+		iwl5000_tx_status_reply_tx(priv, agg, tx_resp, txq_id, index);
+
+		/* check if BAR is needed */
+		if ((tx_resp->frame_count == 1) && !iwl_is_tx_success(status))
+			info->flags |= IEEE80211_TX_STAT_AMPDU_NO_BACK;
+
+		if (txq->q.read_ptr != (scd_ssn & 0xff)) {
+			int freed, ampdu_q;
+			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);
+			freed = iwl_tx_queue_reclaim(priv, txq_id, index);
+			priv->stations[sta_id].tid[tid].tfds_in_queue -= freed;
+
+			if (iwl_queue_space(&txq->q) > txq->q.low_mark &&
+			    txq_id >= 0 && priv->mac80211_registered &&
+			    agg->state != IWL_EMPTYING_HW_QUEUE_DELBA) {
+				/* calculate mac80211 ampdu sw queue to wake */
+				ampdu_q = txq_id - IWL50_FIRST_AMPDU_QUEUE +
+					  priv->hw->queues;
+				if (agg->state == IWL_AGG_OFF)
+					ieee80211_wake_queue(priv->hw, txq_id);
+				else
+					ieee80211_wake_queue(priv->hw, ampdu_q);
+			}
+			iwl_txq_check_empty(priv, sta_id, tid, txq_id);
+		}
+	} else {
+		info->status.retry_count = tx_resp->failure_frame;
+		info->flags =
+			iwl_is_tx_success(status) ? IEEE80211_TX_STAT_ACK : 0;
+		iwl_hwrate_to_tx_control(priv,
+					le32_to_cpu(tx_resp->rate_n_flags),
+					info);
+
+		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) {
+		    int freed = iwl_tx_queue_reclaim(priv, txq_id, index);
+		    if (tid != MAX_TID_COUNT)
+			priv->stations[sta_id].tid[tid].tfds_in_queue -= freed;
+		    if (iwl_queue_space(&txq->q) > txq->q.low_mark &&
+			(txq_id >= 0) && priv->mac80211_registered)
+			ieee80211_wake_queue(priv->hw, txq_id);
+		    if (tid != MAX_TID_COUNT)
+			iwl_txq_check_empty(priv, sta_id, tid, txq_id);
+		}
+	}
+
+	if (iwl_check_bits(status, TX_ABORT_REQUIRED_MSK))
+		IWL_ERROR("TODO:  Implement Tx ABORT REQUIRED!!!\n");
+}
+
+/* Currently 5000 is the supperset of everything */
+static u16 iwl5000_get_hcmd_size(u8 cmd_id, u16 len)
+{
+	return len;
+}
+
+static void iwl5000_setup_deferred_work(struct iwl_priv *priv)
+{
+	/* in 5000 the tx power calibration is done in uCode */
+	priv->disable_tx_power_cal = 1;
+}
+
+static void iwl5000_rx_handler_setup(struct iwl_priv *priv)
+{
+	/* init calibration handlers */
+	priv->rx_handlers[CALIBRATION_RES_NOTIFICATION] =
+					iwl5000_rx_calib_result;
+	priv->rx_handlers[CALIBRATION_COMPLETE_NOTIFICATION] =
+					iwl5000_rx_calib_complete;
+	priv->rx_handlers[REPLY_TX] = iwl5000_rx_reply_tx;
+}
+
+
+static int iwl5000_hw_valid_rtc_data_addr(u32 addr)
+{
+	return (addr >= RTC_DATA_LOWER_BOUND) &&
+		(addr < IWL50_RTC_DATA_UPPER_BOUND);
+}
+
+static int iwl5000_send_rxon_assoc(struct iwl_priv *priv)
+{
+	int ret = 0;
+	struct iwl5000_rxon_assoc_cmd 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->ofdm_ht_triple_stream_basic_rates ==
+	     rxon2->ofdm_ht_triple_stream_basic_rates) &&
+	    (rxon1->acquisition_data == rxon2->acquisition_data) &&
+	    (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.reserved1 = 0;
+	rxon_assoc.reserved2 = 0;
+	rxon_assoc.reserved3 = 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;
+	rxon_assoc.ofdm_ht_triple_stream_basic_rates =
+		 priv->staging_rxon.ofdm_ht_triple_stream_basic_rates;
+	rxon_assoc.acquisition_data = priv->staging_rxon.acquisition_data;
+
+	ret = iwl_send_cmd_pdu_async(priv, REPLY_RXON_ASSOC,
+				     sizeof(rxon_assoc), &rxon_assoc, NULL);
+	if (ret)
+		return ret;
+
+	return ret;
+}
+static int  iwl5000_send_tx_power(struct iwl_priv *priv)
+{
+	struct iwl5000_tx_power_dbm_cmd tx_power_cmd;
+
+	/* half dBm need to multiply */
+	tx_power_cmd.global_lmt = (s8)(2 * priv->tx_power_user_lmt);
+	tx_power_cmd.flags = IWL50_TX_POWER_NO_CLOSED;
+	tx_power_cmd.srv_chan_lmt = IWL50_TX_POWER_AUTO;
+	return  iwl_send_cmd_pdu_async(priv, REPLY_TX_POWER_DBM_CMD,
+				       sizeof(tx_power_cmd), &tx_power_cmd,
+				       NULL);
+}
+
+static void iwl5000_temperature(struct iwl_priv *priv)
+{
+	/* store temperature from statistics (in Celsius) */
+	priv->temperature = le32_to_cpu(priv->statistics.general.temperature);
+}
+
+static struct iwl_hcmd_ops iwl5000_hcmd = {
+	.rxon_assoc = iwl5000_send_rxon_assoc,
+};
+
+static struct iwl_hcmd_utils_ops iwl5000_hcmd_utils = {
+	.get_hcmd_size = iwl5000_get_hcmd_size,
+	.build_addsta_hcmd = iwl5000_build_addsta_hcmd,
+	.gain_computation = iwl5000_gain_computation,
+	.chain_noise_reset = iwl5000_chain_noise_reset,
+	.rts_tx_cmd_flag = iwl5000_rts_tx_cmd_flag,
+};
+
+static struct iwl_lib_ops iwl5000_lib = {
+	.set_hw_params = iwl5000_hw_set_hw_params,
+	.alloc_shared_mem = iwl5000_alloc_shared_mem,
+	.free_shared_mem = iwl5000_free_shared_mem,
+	.shared_mem_rx_idx = iwl5000_shared_mem_rx_idx,
+	.txq_update_byte_cnt_tbl = iwl5000_txq_update_byte_cnt_tbl,
+	.txq_inval_byte_cnt_tbl = iwl5000_txq_inval_byte_cnt_tbl,
+	.txq_set_sched = iwl5000_txq_set_sched,
+	.txq_agg_enable = iwl5000_txq_agg_enable,
+	.txq_agg_disable = iwl5000_txq_agg_disable,
+	.rx_handler_setup = iwl5000_rx_handler_setup,
+	.setup_deferred_work = iwl5000_setup_deferred_work,
+	.is_valid_rtc_data_addr = iwl5000_hw_valid_rtc_data_addr,
+	.load_ucode = iwl5000_load_ucode,
+	.init_alive_start = iwl5000_init_alive_start,
+	.alive_notify = iwl5000_alive_notify,
+	.send_tx_power = iwl5000_send_tx_power,
+	.temperature = iwl5000_temperature,
+	.apm_ops = {
+		.init =	iwl5000_apm_init,
+		.reset = iwl5000_apm_reset,
+		.stop = iwl5000_apm_stop,
+		.config = iwl5000_nic_config,
+		.set_pwr_src = iwl4965_set_pwr_src,
+	},
+	.eeprom_ops = {
+		.regulatory_bands = {
+			EEPROM_5000_REG_BAND_1_CHANNELS,
+			EEPROM_5000_REG_BAND_2_CHANNELS,
+			EEPROM_5000_REG_BAND_3_CHANNELS,
+			EEPROM_5000_REG_BAND_4_CHANNELS,
+			EEPROM_5000_REG_BAND_5_CHANNELS,
+			EEPROM_5000_REG_BAND_24_FAT_CHANNELS,
+			EEPROM_5000_REG_BAND_52_FAT_CHANNELS
+		},
+		.verify_signature  = iwlcore_eeprom_verify_signature,
+		.acquire_semaphore = iwlcore_eeprom_acquire_semaphore,
+		.release_semaphore = iwlcore_eeprom_release_semaphore,
+		.check_version	= iwl5000_eeprom_check_version,
+		.query_addr = iwl5000_eeprom_query_addr,
+	},
+};
+
+static struct iwl_ops iwl5000_ops = {
+	.lib = &iwl5000_lib,
+	.hcmd = &iwl5000_hcmd,
+	.utils = &iwl5000_hcmd_utils,
+};
+
+static struct iwl_mod_params iwl50_mod_params = {
+	.num_of_queues = IWL50_NUM_QUEUES,
+	.num_of_ampdu_queues = IWL50_NUM_AMPDU_QUEUES,
+	.enable_qos = 1,
+	.amsdu_size_8K = 1,
+	.restart_fw = 1,
+	/* the rest are 0 by default */
+};
+
+
+struct iwl_cfg iwl5300_agn_cfg = {
+	.name = "5300AGN",
+	.fw_name = "iwlwifi-5000" IWL5000_UCODE_API ".ucode",
+	.sku = IWL_SKU_A|IWL_SKU_G|IWL_SKU_N,
+	.ops = &iwl5000_ops,
+	.eeprom_size = IWL_5000_EEPROM_IMG_SIZE,
+	.mod_params = &iwl50_mod_params,
+};
+
+struct iwl_cfg iwl5100_bg_cfg = {
+	.name = "5100BG",
+	.fw_name = "iwlwifi-5000" IWL5000_UCODE_API ".ucode",
+	.sku = IWL_SKU_G,
+	.ops = &iwl5000_ops,
+	.eeprom_size = IWL_5000_EEPROM_IMG_SIZE,
+	.mod_params = &iwl50_mod_params,
+};
+
+struct iwl_cfg iwl5100_abg_cfg = {
+	.name = "5100ABG",
+	.fw_name = "iwlwifi-5000" IWL5000_UCODE_API ".ucode",
+	.sku = IWL_SKU_A|IWL_SKU_G,
+	.ops = &iwl5000_ops,
+	.eeprom_size = IWL_5000_EEPROM_IMG_SIZE,
+	.mod_params = &iwl50_mod_params,
+};
+
+struct iwl_cfg iwl5100_agn_cfg = {
+	.name = "5100AGN",
+	.fw_name = "iwlwifi-5000" IWL5000_UCODE_API ".ucode",
+	.sku = IWL_SKU_A|IWL_SKU_G|IWL_SKU_N,
+	.ops = &iwl5000_ops,
+	.eeprom_size = IWL_5000_EEPROM_IMG_SIZE,
+	.mod_params = &iwl50_mod_params,
+};
+
+struct iwl_cfg iwl5350_agn_cfg = {
+	.name = "5350AGN",
+	.fw_name = "iwlwifi-5000" IWL5000_UCODE_API ".ucode",
+	.sku = IWL_SKU_A|IWL_SKU_G|IWL_SKU_N,
+	.ops = &iwl5000_ops,
+	.eeprom_size = IWL_5000_EEPROM_IMG_SIZE,
+	.mod_params = &iwl50_mod_params,
+};
+
+module_param_named(disable50, iwl50_mod_params.disable, int, 0444);
+MODULE_PARM_DESC(disable50,
+		  "manually disable the 50XX radio (default 0 [radio on])");
+module_param_named(swcrypto50, iwl50_mod_params.sw_crypto, bool, 0444);
+MODULE_PARM_DESC(swcrypto50,
+		  "using software crypto engine (default 0 [hardware])\n");
+module_param_named(debug50, iwl50_mod_params.debug, int, 0444);
+MODULE_PARM_DESC(debug50, "50XX debug output mask");
+module_param_named(queues_num50, iwl50_mod_params.num_of_queues, int, 0444);
+MODULE_PARM_DESC(queues_num50, "number of hw queues in 50xx series");
+module_param_named(qos_enable50, iwl50_mod_params.enable_qos, int, 0444);
+MODULE_PARM_DESC(qos_enable50, "enable all 50XX QoS functionality");
+module_param_named(11n_disable50, iwl50_mod_params.disable_11n, int, 0444);
+MODULE_PARM_DESC(11n_disable50, "disable 50XX 11n functionality");
+module_param_named(amsdu_size_8K50, iwl50_mod_params.amsdu_size_8K, int, 0444);
+MODULE_PARM_DESC(amsdu_size_8K50, "enable 8K amsdu size in 50XX series");
+module_param_named(fw_restart50, iwl50_mod_params.restart_fw, int, 0444);
+MODULE_PARM_DESC(fw_restart50, "restart firmware in case of error");
diff --git a/drivers/net/wireless/iwlwifi/iwl-calib.c b/drivers/net/wireless/iwlwifi/iwl-calib.c
new file mode 100644
index 000000000000..ef49440bd7f6
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-calib.c
@@ -0,0 +1,802 @@
+/******************************************************************************
+ *
+ * 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) 2008 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.GPL.
+ *
+ * Contact Information:
+ * Tomas Winkler <tomas.winkler@intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ * BSD LICENSE
+ *
+ * Copyright(c) 2005 - 2008 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.
+ *****************************************************************************/
+
+#include <net/mac80211.h>
+
+#include "iwl-dev.h"
+#include "iwl-core.h"
+#include "iwl-calib.h"
+
+/* "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 iwl_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;
+	const struct iwl_sensitivity_ranges *ranges = priv->hw_params.sens;
+
+	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) &&
+		(data->auto_corr_cck > AUTO_CORR_MAX_TH_CCK)) {
+		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;
+		/* 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 >
+			(ranges->max_nrg_cck + NRG_STEP_CCK))
+			data->nrg_th_cck = data->nrg_th_cck
+						 - NRG_STEP_CCK;
+		else
+			data->nrg_th_cck = ranges->max_nrg_cck;
+	/* 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)ranges->min_nrg_cck, 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");
+			if (data->nrg_th_cck > (max_nrg_cck + NRG_MARGIN))
+				data->nrg_th_cck -= NRG_MARGIN;
+			else
+				data->nrg_th_cck = max_nrg_cck;
+		}
+	}
+
+	/* 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;
+
+	/* Auto-correlation CCK algorithm */
+	if (false_alarms > min_false_alarms) {
+
+		/* increase auto_corr values to decrease sensitivity
+		 * so the DSP won't be disturbed by the noise
+		 */
+		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)ranges->auto_corr_max_cck, val);
+		}
+		val = data->auto_corr_cck_mrc + AUTO_CORR_STEP_CCK;
+		data->auto_corr_cck_mrc =
+			min((u32)ranges->auto_corr_max_cck_mrc, val);
+	} else if ((false_alarms < min_false_alarms) &&
+	   ((data->nrg_auto_corr_silence_diff > NRG_DIFF) ||
+	   (data->num_in_cck_no_fa > MAX_NUMBER_CCK_NO_FA))) {
+
+		/* Decrease auto_corr values to increase sensitivity */
+		val = data->auto_corr_cck - AUTO_CORR_STEP_CCK;
+		data->auto_corr_cck =
+			max((u32)ranges->auto_corr_min_cck, val);
+		val = data->auto_corr_cck_mrc - AUTO_CORR_STEP_CCK;
+		data->auto_corr_cck_mrc =
+			max((u32)ranges->auto_corr_min_cck_mrc, val);
+	}
+
+	return 0;
+}
+
+
+static int iwl_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;
+	const struct iwl_sensitivity_ranges *ranges = priv->hw_params.sens;
+
+	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)ranges->auto_corr_max_ofdm, val);
+
+		val = data->auto_corr_ofdm_mrc + AUTO_CORR_STEP_OFDM;
+		data->auto_corr_ofdm_mrc =
+			min((u32)ranges->auto_corr_max_ofdm_mrc, val);
+
+		val = data->auto_corr_ofdm_x1 + AUTO_CORR_STEP_OFDM;
+		data->auto_corr_ofdm_x1 =
+			min((u32)ranges->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)ranges->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)ranges->auto_corr_min_ofdm, val);
+
+		val = data->auto_corr_ofdm_mrc - AUTO_CORR_STEP_OFDM;
+		data->auto_corr_ofdm_mrc =
+			max((u32)ranges->auto_corr_min_ofdm_mrc, val);
+
+		val = data->auto_corr_ofdm_x1 - AUTO_CORR_STEP_OFDM;
+		data->auto_corr_ofdm_x1 =
+			max((u32)ranges->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)ranges->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;
+}
+
+/* Prepare a SENSITIVITY_CMD, send to uCode if values have changed */
+static int iwl_sensitivity_write(struct iwl_priv *priv)
+{
+	int ret = 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 = CMD_ASYNC,
+		.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);
+
+	/* Update uCode's "work" table, and copy it to DSP */
+	cmd.control = SENSITIVITY_CMD_CONTROL_WORK_TABLE;
+
+	/* 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);
+
+	ret = iwl_send_cmd(priv, &cmd_out);
+	if (ret)
+		IWL_ERROR("SENSITIVITY_CMD failed\n");
+
+	return ret;
+}
+
+void iwl_init_sensitivity(struct iwl_priv *priv)
+{
+	int ret = 0;
+	int i;
+	struct iwl_sensitivity_data *data = NULL;
+	const struct iwl_sensitivity_ranges *ranges = priv->hw_params.sens;
+
+	if (priv->disable_sens_cal)
+		return;
+
+	IWL_DEBUG_CALIB("Start iwl_init_sensitivity\n");
+
+	/* Clear driver's sensitivity algo data */
+	data = &(priv->sensitivity_data);
+
+	if (ranges == NULL)
+		return;
+
+	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 = ranges->auto_corr_min_ofdm_mrc;
+	data->auto_corr_ofdm_x1  = ranges->auto_corr_min_ofdm_x1;
+	data->auto_corr_ofdm_mrc_x1 = ranges->auto_corr_min_ofdm_mrc_x1;
+	data->auto_corr_cck = AUTO_CORR_CCK_MIN_VAL_DEF;
+	data->auto_corr_cck_mrc = ranges->auto_corr_min_cck_mrc;
+	data->nrg_th_cck = ranges->nrg_th_cck;
+	data->nrg_th_ofdm = ranges->nrg_th_ofdm;
+
+	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;
+
+	ret |= iwl_sensitivity_write(priv);
+	IWL_DEBUG_CALIB("<<return 0x%X\n", ret);
+}
+EXPORT_SYMBOL(iwl_init_sensitivity);
+
+void iwl_sensitivity_calibration(struct iwl_priv *priv,
+				    struct iwl_notif_statistics *resp)
+{
+	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;
+
+	if (priv->disable_sens_cal)
+		return;
+
+	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);
+
+	iwl_sens_auto_corr_ofdm(priv, norm_fa_ofdm, rx_enable_time);
+	iwl_sens_energy_cck(priv, norm_fa_cck, rx_enable_time, &statis);
+	iwl_sensitivity_write(priv);
+
+	return;
+}
+EXPORT_SYMBOL(iwl_sensitivity_calibration);
+
+/*
+ * 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.
+ */
+void iwl_chain_noise_calibration(struct iwl_priv *priv,
+				 struct iwl_notif_statistics *stat_resp)
+{
+	struct iwl_chain_noise_data *data = NULL;
+
+	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 rxon_chnum = INITIALIZATION_VALUE;
+	u16 stat_chnum = INITIALIZATION_VALUE;
+	u8 rxon_band24;
+	u8 stat_band24;
+	u32 active_chains = 0;
+	u8 num_tx_chains;
+	unsigned long flags;
+	struct statistics_rx_non_phy *rx_info = &(stat_resp->rx.general);
+
+	if (priv->disable_chain_noise_cal)
+		return;
+
+	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;
+	}
+
+	rxon_band24 = !!(priv->staging_rxon.flags & RXON_FLG_BAND_24G_MSK);
+	rxon_chnum = le16_to_cpu(priv->staging_rxon.channel);
+	stat_band24 = !!(stat_resp->flag & STATISTICS_REPLY_FLG_BAND_24G_MSK);
+	stat_chnum = le32_to_cpu(stat_resp->flag) >> 16;
+
+	/* Make sure we accumulate data for just the associated channel
+	 *   (even if scanning). */
+	if ((rxon_chnum != stat_chnum) || (rxon_band24 != stat_band24)) {
+		IWL_DEBUG_CALIB("Stats not from chan=%d, band24=%d\n",
+				rxon_chnum, rxon_band24);
+		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, band24=%d, beacon=%d\n",
+			rxon_chnum, rxon_band24, 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)
+		return;
+
+	/* 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]);
+		}
+	}
+
+	num_tx_chains = 0;
+	for (i = 0; i < NUM_RX_CHAINS; i++) {
+		/* loops on all the bits of
+		 * priv->hw_setting.valid_tx_ant */
+		u8 ant_msk = (1 << i);
+		if (!(priv->hw_params.valid_tx_ant & ant_msk))
+			continue;
+
+		num_tx_chains++;
+		if (data->disconn_array[i] == 0)
+			/* there is a Tx antenna connected */
+			break;
+		if (num_tx_chains == priv->hw_params.tx_chains_num &&
+		data->disconn_array[i]) {
+			/* This is the last TX antenna and is also
+			 * disconnected connect it anyway */
+			data->disconn_array[i] = 0;
+			active_chains |= ant_msk;
+			IWL_DEBUG_CALIB("All Tx chains are disconnected W/A - "
+				"declare %d as connected\n", i);
+			break;
+		}
+	}
+
+	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;*/
+	/*FIXME: should be reflected in RX chains in RXON */
+
+	/* 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;
+		}
+	}
+
+	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);
+
+	priv->cfg->ops->utils->gain_computation(priv, average_noise,
+		min_average_noise_antenna_i, min_average_noise);
+}
+EXPORT_SYMBOL(iwl_chain_noise_calibration);
+
+
+void iwl_reset_run_time_calib(struct iwl_priv *priv)
+{
+	int i;
+	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;
+
+	/* Ask for statistics now, the uCode will send notification
+	 * periodically after association */
+	iwl_send_statistics_request(priv, CMD_ASYNC);
+}
+EXPORT_SYMBOL(iwl_reset_run_time_calib);
+
diff --git a/drivers/net/wireless/iwlwifi/iwl-calib.h b/drivers/net/wireless/iwlwifi/iwl-calib.h
new file mode 100644
index 000000000000..94c8e316382a
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-calib.h
@@ -0,0 +1,84 @@
+/******************************************************************************
+ *
+ * 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) 2008 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.GPL.
+ *
+ * Contact Information:
+ * Tomas Winkler <tomas.winkler@intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *
+ * BSD LICENSE
+ *
+ * Copyright(c) 2005 - 2008 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_calib_h__
+#define __iwl_calib_h__
+
+#include "iwl-dev.h"
+#include "iwl-core.h"
+#include "iwl-commands.h"
+
+void iwl_chain_noise_calibration(struct iwl_priv *priv,
+				struct iwl_notif_statistics *stat_resp);
+void iwl_sensitivity_calibration(struct iwl_priv *priv,
+				struct iwl_notif_statistics *resp);
+
+void iwl_init_sensitivity(struct iwl_priv *priv);
+void iwl_reset_run_time_calib(struct iwl_priv *priv);
+static inline void iwl_chain_noise_reset(struct iwl_priv *priv)
+{
+
+	if (!priv->disable_chain_noise_cal &&
+	    priv->cfg->ops->utils->chain_noise_reset)
+		priv->cfg->ops->utils->chain_noise_reset(priv);
+}
+
+#endif /* __iwl_calib_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-4965-commands.h b/drivers/net/wireless/iwlwifi/iwl-commands.h
index 3bcd107e2d71..e9bb1de0ce3f 100644
--- a/drivers/net/wireless/iwlwifi/iwl-4965-commands.h
+++ b/drivers/net/wireless/iwlwifi/iwl-commands.h
@@ -61,9 +61,9 @@
  *
  *****************************************************************************/
 /*
- * Please use this file (iwl-4965-commands.h) only for uCode API definitions.
+ * Please use this file (iwl-commands.h) only for uCode API definitions.
  * Please use iwl-4965-hw.h for hardware-related definitions.
- * Please use iwl-4965.h for driver implementation definitions.
+ * Please use iwl-dev.h for driver implementation definitions.
  */
 
 #ifndef __iwl4965_commands_h__
@@ -93,6 +93,11 @@ enum {
 	REPLY_LEDS_CMD = 0x48,
 	REPLY_TX_LINK_QUALITY_CMD = 0x4e, /* 4965 only */
 
+	/* WiMAX coexistence */
+	COEX_PRIORITY_TABLE_CMD = 0x5a,	/*5000 only */
+	COEX_MEDIUM_NOTIFICATION = 0x5b,
+	COEX_EVENT_CMD = 0x5c,
+
 	/* 802.11h related */
 	RADAR_NOTIFICATION = 0x70,	/* not used */
 	REPLY_QUIET_CMD = 0x71,		/* not used */
@@ -121,6 +126,7 @@ enum {
 	/* Miscellaneous commands */
 	QUIET_NOTIFICATION = 0x96,		/* not used */
 	REPLY_TX_PWR_TABLE_CMD = 0x97,
+	REPLY_TX_POWER_DBM_CMD = 0x98,
 	MEASURE_ABORT_NOTIFICATION = 0x99,	/* not used */
 
 	/* Bluetooth device coexistance config command */
@@ -269,21 +275,13 @@ struct iwl_cmd_header {
  *          10 B active, A inactive
  *          11 Both active
  */
-#define RATE_MCS_ANT_POS       14
-#define RATE_MCS_ANT_A_MSK     0x04000
-#define RATE_MCS_ANT_B_MSK     0x08000
-#define RATE_MCS_ANT_AB_MSK    0x0C000
+#define RATE_MCS_ANT_POS      14
+#define RATE_MCS_ANT_A_MSK    0x04000
+#define RATE_MCS_ANT_B_MSK    0x08000
+#define RATE_MCS_ANT_C_MSK    0x10000
+#define RATE_MCS_ANT_ABC_MSK  0x1C000
 
-
-/**
- * struct iwl4965_tx_power - txpower format used in REPLY_SCAN_CMD
- *
- * Scan uses only one transmitter, so only one analog/dsp gain pair is needed.
- */
-struct iwl4965_tx_power {
-	u8 tx_gain;		/* gain for analog radio */
-	u8 dsp_atten;		/* gain for DSP */
-} __attribute__ ((packed));
+#define RATE_MCS_ANT_INIT_IND   1
 
 #define POWER_TABLE_NUM_ENTRIES			33
 #define POWER_TABLE_NUM_HT_OFDM_ENTRIES		32
@@ -333,6 +331,19 @@ struct iwl4965_tx_power_db {
 	struct tx_power_dual_stream power_tbl[POWER_TABLE_NUM_ENTRIES];
 } __attribute__ ((packed));
 
+/**
+ * Commad REPLY_TX_POWER_DBM_CMD = 0x98
+ * struct iwl5000_tx_power_dbm_cmd
+ */
+#define IWL50_TX_POWER_AUTO 0x7f
+#define IWL50_TX_POWER_NO_CLOSED (0x1 << 6)
+
+struct iwl5000_tx_power_dbm_cmd {
+	s8 global_lmt; /*in half-dBm (e.g. 30 = 15 dBm) */
+	u8 flags;
+	s8 srv_chan_lmt; /*in half-dBm (e.g. 30 = 15 dBm) */
+	u8 reserved;
+} __attribute__ ((packed));
 
 /******************************************************************************
  * (0a)
@@ -367,7 +378,7 @@ struct iwl4965_tx_power_db {
  * 3)  Tx gain compensation to balance 4965's 2 Tx chains for MIMO operation,
  *     for each of 5 frequency ranges.
  */
-struct iwl4965_init_alive_resp {
+struct iwl_init_alive_resp {
 	u8 ucode_minor;
 	u8 ucode_major;
 	__le16 reserved1;
@@ -443,7 +454,7 @@ struct iwl4965_init_alive_resp {
  * The Linux driver can print both logs to the system log when a uCode error
  * occurs.
  */
-struct iwl4965_alive_resp {
+struct iwl_alive_resp {
 	u8 ucode_minor;
 	u8 ucode_major;
 	__le16 reserved1;
@@ -467,7 +478,7 @@ union tsf {
 /*
  * REPLY_ERROR = 0x2 (response only, not a command)
  */
-struct iwl4965_error_resp {
+struct iwl_error_resp {
 	__le32 error_type;
 	u8 cmd_id;
 	u8 reserved1;
@@ -545,6 +556,8 @@ enum {
 #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)
+/* CTS to self (if spec allows) flag */
+#define RXON_FLG_SELF_CTS_EN			__constant_cpu_to_le32(0x1<<30)
 
 /* rx_config filter flags */
 /* accept all data frames */
@@ -599,6 +612,46 @@ struct iwl4965_rxon_cmd {
 	u8 ofdm_ht_dual_stream_basic_rates;
 } __attribute__ ((packed));
 
+/* 5000 HW just extend this cmmand */
+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;
+	__le16 rx_chain;
+	u8 ofdm_basic_rates;
+	u8 cck_basic_rates;
+	__le16 assoc_id;
+	__le32 flags;
+	__le32 filter_flags;
+	__le16 channel;
+	u8 ofdm_ht_single_stream_basic_rates;
+	u8 ofdm_ht_dual_stream_basic_rates;
+	u8 ofdm_ht_triple_stream_basic_rates;
+	u8 reserved5;
+	__le16 acquisition_data;
+	__le16 reserved6;
+} __attribute__ ((packed));
+
+struct iwl5000_rxon_assoc_cmd {
+	__le32 flags;
+	__le32 filter_flags;
+	u8 ofdm_basic_rates;
+	u8 cck_basic_rates;
+	__le16 reserved1;
+	u8 ofdm_ht_single_stream_basic_rates;
+	u8 ofdm_ht_dual_stream_basic_rates;
+	u8 ofdm_ht_triple_stream_basic_rates;
+	u8 reserved2;
+	__le16 rx_chain_select_flags;
+	__le16 acquisition_data;
+	__le32 reserved3;
+} __attribute__ ((packed));
+
 /*
  * REPLY_RXON_ASSOC = 0x11 (command, has simple generic response)
  */
@@ -613,6 +666,9 @@ struct iwl4965_rxon_assoc_cmd {
 	__le16 reserved;
 } __attribute__ ((packed));
 
+
+
+
 /*
  * REPLY_RXON_TIMING = 0x14 (command, has simple generic response)
  */
@@ -669,7 +725,7 @@ struct iwl4965_csa_notification {
  * transmission retry.  Device uses cw_max as a bit mask, ANDed with new CW
  * value, to cap the CW value.
  */
-struct iwl4965_ac_qos {
+struct iwl_ac_qos {
 	__le16 cw_min;
 	__le16 cw_max;
 	u8 aifsn;
@@ -691,9 +747,9 @@ struct iwl4965_ac_qos {
  * This command sets up timings for each of the 4 prioritized EDCA Tx FIFOs
  * 0: Background, 1: Best Effort, 2: Video, 3: Voice.
  */
-struct iwl4965_qosparam_cmd {
+struct iwl_qosparam_cmd {
 	__le32 qos_flags;
-	struct iwl4965_ac_qos ac[AC_NUM];
+	struct iwl_ac_qos ac[AC_NUM];
 } __attribute__ ((packed));
 
 /******************************************************************************
@@ -711,6 +767,8 @@ struct iwl4965_qosparam_cmd {
 #define	IWL_STA_ID		2
 #define IWL4965_BROADCAST_ID	31
 #define	IWL4965_STATION_COUNT	32
+#define IWL5000_BROADCAST_ID	15
+#define	IWL5000_STATION_COUNT	16
 
 #define	IWL_STATION_COUNT	32 	/* MAX(3945,4965)*/
 #define	IWL_INVALID_STATION 	255
@@ -766,6 +824,20 @@ struct iwl4965_keyinfo {
 	u8 key[16];		/* 16-byte unicast decryption key */
 } __attribute__ ((packed));
 
+/* 5000 */
+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 */
+	u8 key_offset;
+	u8 reserved2;
+	u8 key[16];		/* 16-byte unicast decryption key */
+	__le64 tx_secur_seq_cnt;
+	__le64 hw_tkip_mic_rx_key;
+	__le64 hw_tkip_mic_tx_key;
+} __attribute__ ((packed));
+
 /**
  * struct sta_id_modify
  * @addr[ETH_ALEN]: station's MAC address
@@ -841,6 +913,38 @@ struct iwl4965_addsta_cmd {
 	__le32 reserved2;
 } __attribute__ ((packed));
 
+/* 5000 */
+struct iwl_addsta_cmd {
+	u8 mode;		/* 1: modify existing, 0: add new station */
+	u8 reserved[3];
+	struct sta_id_modify sta;
+	struct iwl_keyinfo key;
+	__le32 station_flags;		/* STA_FLG_* */
+	__le32 station_flags_msk;	/* STA_FLG_* */
+
+	/* bit field to disable (1) or enable (0) Tx for Traffic ID (TID)
+	 * corresponding to bit (e.g. bit 5 controls TID 5).
+	 * Set modify_mask bit STA_MODIFY_TID_DISABLE_TX to use this field. */
+	__le16 tid_disable_tx;
+
+	__le16	reserved1;
+
+	/* TID for which to add block-ack support.
+	 * Set modify_mask bit STA_MODIFY_ADDBA_TID_MSK to use this field. */
+	u8 add_immediate_ba_tid;
+
+	/* TID for which to remove block-ack support.
+	 * Set modify_mask bit STA_MODIFY_DELBA_TID_MSK to use this field. */
+	u8 remove_immediate_ba_tid;
+
+	/* Starting Sequence Number for added block-ack support.
+	 * Set modify_mask bit STA_MODIFY_ADDBA_TID_MSK to use this field. */
+	__le16 add_immediate_ba_ssn;
+
+	__le32 reserved2;
+} __attribute__ ((packed));
+
+
 #define ADD_STA_SUCCESS_MSK		0x1
 #define ADD_STA_NO_ROOM_IN_TABLE	0x2
 #define ADD_STA_NO_BLOCK_ACK_RESOURCE	0x4
@@ -848,10 +952,28 @@ struct iwl4965_addsta_cmd {
 /*
  * REPLY_ADD_STA = 0x18 (response)
  */
-struct iwl4965_add_sta_resp {
+struct iwl_add_sta_resp {
 	u8 status;	/* ADD_STA_* */
 } __attribute__ ((packed));
 
+#define REM_STA_SUCCESS_MSK              0x1
+/*
+ *  REPLY_REM_STA = 0x19 (response)
+ */
+struct iwl_rem_sta_resp {
+	u8 status;
+} __attribute__ ((packed));
+
+/*
+ *  REPLY_REM_STA = 0x19 (command)
+ */
+struct iwl_rem_sta_cmd {
+	u8 num_sta;     /* number of removed stations */
+	u8 reserved[3];
+	u8 addr[ETH_ALEN]; /* MAC addr of the first station */
+	u8 reserved2[2];
+} __attribute__ ((packed));
+
 /*
  * REPLY_WEP_KEY = 0x20
  */
@@ -875,6 +997,7 @@ struct iwl_wep_cmd {
 #define WEP_KEY_WEP_TYPE 1
 #define WEP_KEYS_MAX 4
 #define WEP_INVALID_OFFSET 0xff
+#define WEP_KEY_LEN_64 5
 #define WEP_KEY_LEN_128 13
 
 /******************************************************************************
@@ -1018,6 +1141,11 @@ struct iwl4965_rx_mpdu_res_start {
 
 /* REPLY_TX Tx flags field */
 
+/* 1: Use RTS/CTS protocol or CTS-to-self if spec alows it
+ * before this frame. if CTS-to-self required check
+ * RXON_FLG_SELF_CTS_EN status. */
+#define TX_CMD_FLG_RTS_CTS_MSK __constant_cpu_to_le32(1 << 0)
+
 /* 1: Use Request-To-Send protocol before this frame.
  * Mutually exclusive vs. TX_CMD_FLG_CTS_MSK. */
 #define TX_CMD_FLG_RTS_MSK __constant_cpu_to_le32(1 << 1)
@@ -1100,6 +1228,14 @@ struct iwl4965_rx_mpdu_res_start {
 #define TX_CMD_SEC_KEY128	0x08
 
 /*
+ * security overhead sizes
+ */
+#define WEP_IV_LEN 4
+#define WEP_ICV_LEN 4
+#define CCMP_MIC_LEN 8
+#define TKIP_ICV_LEN 4
+
+/*
  * 4965 uCode updates these Tx attempt count values in host DRAM.
  * Used for managing Tx retries when expecting block-acks.
  * Driver should set these fields to 0.
@@ -1113,7 +1249,7 @@ struct iwl4965_dram_scratch {
 /*
  * REPLY_TX = 0x1c (command)
  */
-struct iwl4965_tx_cmd {
+struct iwl_tx_cmd {
 	/*
 	 * MPDU byte count:
 	 * MAC header (24/26/30/32 bytes) + 2 bytes pad if 26/30 header size,
@@ -1259,6 +1395,15 @@ enum {
 	TX_ABORT_REQUIRED_MSK = 0x80000000,	/* bits 31:31 */
 };
 
+static inline int iwl_is_tx_success(u32 status)
+{
+	status &= TX_STATUS_MSK;
+	return (status == TX_STATUS_SUCCESS)
+	    || (status == TX_STATUS_DIRECT_DONE);
+}
+
+
+
 /* *******************************
  * TX aggregation status
  ******************************* */
@@ -1313,6 +1458,11 @@ enum {
  *     within the sending station (this 4965), rather than whether it was
  *     received successfully by the destination station.
  */
+struct agg_tx_status {
+	__le16 status;
+	__le16 sequence;
+} __attribute__ ((packed));
+
 struct iwl4965_tx_resp {
 	u8 frame_count;		/* 1 no aggregation, >1 aggregation */
 	u8 bt_kill_count;	/* # blocked by bluetooth (unused for agg) */
@@ -1344,34 +1494,56 @@ struct iwl4965_tx_resp {
 	 *                   table entry used for all frames in the new agg.
 	 *           31-16:  Sequence # for this frame's Tx cmd (not SSN!)
 	 */
-	__le32 status;	/* TX status (for aggregation status of 1st frame) */
+	union {
+		__le32 status;
+		struct agg_tx_status agg_status[0]; /* for each agg frame */
+	} u;
 } __attribute__ ((packed));
 
-struct agg_tx_status {
-	__le16 status;
-	__le16 sequence;
-} __attribute__ ((packed));
+struct iwl5000_tx_resp {
+	u8 frame_count;		/* 1 no aggregation, >1 aggregation */
+	u8 bt_kill_count;	/* # blocked by bluetooth (unused for agg) */
+	u8 failure_rts;		/* # failures due to unsuccessful RTS */
+	u8 failure_frame;	/* # failures due to no ACK (unused for agg) */
 
-struct iwl4965_tx_resp_agg {
-	u8 frame_count;         /* 1 no aggregation, >1 aggregation */
-	u8 reserved1;
-	u8 failure_rts;
-	u8 failure_frame;
-	__le32 rate_n_flags;
-	__le16 wireless_media_time;
-	__le16 reserved3;
-	__le32 pa_power1;
+	/* For non-agg:  Rate at which frame was successful.
+	 * For agg:  Rate at which all frames were transmitted. */
+	__le32 rate_n_flags;	/* RATE_MCS_*  */
+
+	/* For non-agg:  RTS + CTS + frame tx attempts time + ACK.
+	 * For agg:  RTS + CTS + aggregation tx time + block-ack time. */
+	__le16 wireless_media_time;	/* uSecs */
+
+	__le16 reserved;
+	__le32 pa_power1;	/* RF power amplifier measurement (not used) */
 	__le32 pa_power2;
-	struct agg_tx_status status;    /* TX status (for aggregation status */
-					/* of 1st frame) */
-} __attribute__ ((packed));
 
+	__le32 tfd_info;
+	__le16 seq_ctl;
+	__le16 byte_cnt;
+	__le32 tlc_info;
+	/*
+	 * For non-agg:  frame status TX_STATUS_*
+	 * For agg:  status of 1st frame, AGG_TX_STATE_*; other frame status
+	 *           fields follow this one, up to frame_count.
+	 *           Bit fields:
+	 *           11- 0:  AGG_TX_STATE_* status code
+	 *           15-12:  Retry count for 1st frame in aggregation (retries
+	 *                   occur if tx failed for this frame when it was a
+	 *                   member of a previous aggregation block).  If rate
+	 *                   scaling is used, retry count indicates the rate
+	 *                   table entry used for all frames in the new agg.
+	 *           31-16:  Sequence # for this frame's Tx cmd (not SSN!)
+	 */
+	struct agg_tx_status status;	/* TX status (in aggregation -
+					 * status of 1st frame) */
+} __attribute__ ((packed));
 /*
  * REPLY_COMPRESSED_BA = 0xc5 (response only, not a command)
  *
  * Reports Block-Acknowledge from recipient station
  */
-struct iwl4965_compressed_ba_resp {
+struct iwl_compressed_ba_resp {
 	__le32 sta_addr_lo32;
 	__le16 sta_addr_hi16;
 	__le16 reserved;
@@ -1853,6 +2025,7 @@ struct iwl4965_spectrum_notification {
 #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)
+#define IWL_POWER_FAST_PD			__constant_cpu_to_le16(1 << 4)
 
 struct iwl4965_powertable_cmd {
 	__le16 flags;
@@ -1914,7 +2087,7 @@ struct iwl4965_card_state_notif {
 #define RF_CARD_DISABLED   0x04
 #define RXON_CARD_DISABLED 0x10
 
-struct iwl4965_ct_kill_config {
+struct iwl_ct_kill_config {
 	__le32   reserved;
 	__le32   critical_temperature_M;
 	__le32   critical_temperature_R;
@@ -1926,8 +2099,11 @@ struct iwl4965_ct_kill_config {
  *
  *****************************************************************************/
 
+#define SCAN_CHANNEL_TYPE_PASSIVE __constant_cpu_to_le32(0)
+#define SCAN_CHANNEL_TYPE_ACTIVE  __constant_cpu_to_le32(1)
+
 /**
- * struct iwl4965_scan_channel - entry in REPLY_SCAN_CMD channel table
+ * struct iwl_scan_channel - entry in REPLY_SCAN_CMD channel table
  *
  * One for each channel in the scan list.
  * Each channel can independently select:
@@ -1937,7 +2113,7 @@ struct iwl4965_ct_kill_config {
  *     quiet_plcp_th, good_CRC_th)
  *
  * To avoid uCode errors, make sure the following are true (see comments
- * under struct iwl4965_scan_cmd about max_out_time and quiet_time):
+ * under struct iwl_scan_cmd about max_out_time and quiet_time):
  * 1)  If using passive_dwell (i.e. passive_dwell != 0):
  *     active_dwell <= passive_dwell (< max_out_time if max_out_time != 0)
  * 2)  quiet_time <= active_dwell
@@ -1945,37 +2121,38 @@ struct iwl4965_ct_kill_config {
  *     passive_dwell < max_out_time
  *     active_dwell < max_out_time
  */
-struct iwl4965_scan_channel {
+struct iwl_scan_channel {
 	/*
 	 * type is defined as:
 	 * 0:0 1 = active, 0 = passive
-	 * 1:4 SSID direct bit map; if a bit is set, then corresponding
+	 * 1:20 SSID direct bit map; if a bit is set, then corresponding
 	 *     SSID IE is transmitted in probe request.
-	 * 5:7 reserved
+	 * 21:31 reserved
 	 */
-	u8 type;
-	u8 channel;	/* band is selected by iwl4965_scan_cmd "flags" field */
-	struct iwl4965_tx_power tpc;
+	__le32 type;
+	__le16 channel;	/* band is selected by iwl_scan_cmd "flags" field */
+	u8 tx_gain;		/* gain for analog radio */
+	u8 dsp_atten;		/* gain for DSP */
 	__le16 active_dwell;	/* in 1024-uSec TU (time units), typ 5-50 */
 	__le16 passive_dwell;	/* in 1024-uSec TU (time units), typ 20-500 */
 } __attribute__ ((packed));
 
 /**
- * struct iwl4965_ssid_ie - directed scan network information element
+ * struct iwl_ssid_ie - directed scan network information element
  *
  * Up to 4 of these may appear in REPLY_SCAN_CMD, selected by "type" field
  * in struct iwl4965_scan_channel; each channel may select different ssids from
  * among the 4 entries.  SSID IEs get transmitted in reverse order of entry.
  */
-struct iwl4965_ssid_ie {
+struct iwl_ssid_ie {
 	u8 id;
 	u8 len;
 	u8 ssid[32];
 } __attribute__ ((packed));
 
-#define PROBE_OPTION_MAX        0x4
+#define PROBE_OPTION_MAX        	0x14
 #define TX_CMD_LIFE_TIME_INFINITE	__constant_cpu_to_le32(0xFFFFFFFF)
-#define IWL_GOOD_CRC_TH		__constant_cpu_to_le16(1)
+#define IWL_GOOD_CRC_TH			__constant_cpu_to_le16(1)
 #define IWL_MAX_SCAN_SIZE 1024
 
 /*
@@ -2028,9 +2205,9 @@ struct iwl4965_ssid_ie {
  * Driver must use separate scan commands for 2.4 vs. 5 GHz bands.
  *
  * To avoid uCode errors, see timing restrictions described under
- * struct iwl4965_scan_channel.
+ * struct iwl_scan_channel.
  */
-struct iwl4965_scan_cmd {
+struct iwl_scan_cmd {
 	__le16 len;
 	u8 reserved0;
 	u8 channel_count;	/* # channels in channel list */
@@ -2051,10 +2228,10 @@ struct iwl4965_scan_cmd {
 
 	/* For active scans (set to all-0s for passive scans).
 	 * Does not include payload.  Must specify Tx rate; no rate scaling. */
-	struct iwl4965_tx_cmd tx_cmd;
+	struct iwl_tx_cmd tx_cmd;
 
 	/* For directed active scans (set to all-0s otherwise) */
-	struct iwl4965_ssid_ie direct_scan[PROBE_OPTION_MAX];
+	struct iwl_ssid_ie direct_scan[PROBE_OPTION_MAX];
 
 	/*
 	 * Probe request frame, followed by channel list.
@@ -2082,14 +2259,14 @@ struct iwl4965_scan_cmd {
 /*
  * REPLY_SCAN_CMD = 0x80 (response)
  */
-struct iwl4965_scanreq_notification {
+struct iwl_scanreq_notification {
 	__le32 status;		/* 1: okay, 2: cannot fulfill request */
 } __attribute__ ((packed));
 
 /*
  * SCAN_START_NOTIFICATION = 0x82 (notification only, not a command)
  */
-struct iwl4965_scanstart_notification {
+struct iwl_scanstart_notification {
 	__le32 tsf_low;
 	__le32 tsf_high;
 	__le32 beacon_timer;
@@ -2106,7 +2283,7 @@ struct iwl4965_scanstart_notification {
 /*
  * SCAN_RESULTS_NOTIFICATION = 0x83 (notification only, not a command)
  */
-struct iwl4965_scanresults_notification {
+struct iwl_scanresults_notification {
 	u8 channel;
 	u8 band;
 	u8 reserved[2];
@@ -2118,7 +2295,7 @@ struct iwl4965_scanresults_notification {
 /*
  * SCAN_COMPLETE_NOTIFICATION = 0x84 (notification only, not a command)
  */
-struct iwl4965_scancomplete_notification {
+struct iwl_scancomplete_notification {
 	u8 scanned_channels;
 	u8 status;
 	u8 reserved;
@@ -2148,7 +2325,7 @@ struct iwl4965_beacon_notif {
  * REPLY_TX_BEACON = 0x91 (command, has simple generic response)
  */
 struct iwl4965_tx_beacon_cmd {
-	struct iwl4965_tx_cmd tx;
+	struct iwl_tx_cmd tx;
 	__le16 tim_idx;
 	u8 tim_size;
 	u8 reserved1;
@@ -2339,7 +2516,7 @@ struct statistics_general {
  */
 #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 iwl4965_statistics_cmd {
+struct iwl_statistics_cmd {
 	__le32 configuration_flags;	/* IWL_STATS_CONF_* */
 } __attribute__ ((packed));
 
@@ -2360,7 +2537,7 @@ struct iwl4965_statistics_cmd {
  */
 #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 iwl4965_notif_statistics {
+struct iwl_notif_statistics {
 	__le32 flag;
 	struct statistics_rx rx;
 	struct statistics_tx tx;
@@ -2559,7 +2736,7 @@ struct iwl4965_missed_beacon_notif {
  */
 
 /*
- * Table entries in SENSITIVITY_CMD (struct iwl4965_sensitivity_cmd)
+ * Table entries in SENSITIVITY_CMD (struct iwl_sensitivity_cmd)
  */
 #define HD_TABLE_SIZE  (11)	/* number of entries */
 #define HD_MIN_ENERGY_CCK_DET_INDEX                 (0)	/* table indexes */
@@ -2574,18 +2751,18 @@ struct iwl4965_missed_beacon_notif {
 #define HD_AUTO_CORR40_X4_TH_ADD_MIN_INDEX          (9)
 #define HD_OFDM_ENERGY_TH_IN_INDEX                  (10)
 
-/* Control field in struct iwl4965_sensitivity_cmd */
+/* Control field in struct iwl_sensitivity_cmd */
 #define SENSITIVITY_CMD_CONTROL_DEFAULT_TABLE	__constant_cpu_to_le16(0)
 #define SENSITIVITY_CMD_CONTROL_WORK_TABLE	__constant_cpu_to_le16(1)
 
 /**
- * struct iwl4965_sensitivity_cmd
+ * struct iwl_sensitivity_cmd
  * @control:  (1) updates working table, (0) updates default table
  * @table:  energy threshold values, use HD_* as index into table
  *
  * Always use "1" in "control" to update uCode's working table and DSP.
  */
-struct iwl4965_sensitivity_cmd {
+struct iwl_sensitivity_cmd {
 	__le16 control;			/* always use "1" */
 	__le16 table[HD_TABLE_SIZE];	/* use HD_* as index */
 } __attribute__ ((packed));
@@ -2659,6 +2836,86 @@ struct iwl4965_calibration_cmd {
 	u8 reserved1;
 } __attribute__ ((packed));
 
+/* Phy calibration command for 5000 series */
+
+enum {
+	IWL5000_PHY_CALIBRATE_DC_CMD		= 8,
+	IWL5000_PHY_CALIBRATE_LO_CMD		= 9,
+	IWL5000_PHY_CALIBRATE_RX_BB_CMD		= 10,
+	IWL5000_PHY_CALIBRATE_TX_IQ_CMD		= 11,
+	IWL5000_PHY_CALIBRATE_RX_IQ_CMD		= 12,
+	IWL5000_PHY_CALIBRATION_NOISE_CMD	= 13,
+	IWL5000_PHY_CALIBRATE_AGC_TABLE_CMD	= 14,
+	IWL5000_PHY_CALIBRATE_CRYSTAL_FRQ_CMD	= 15,
+	IWL5000_PHY_CALIBRATE_BASE_BAND_CMD	= 16,
+	IWL5000_PHY_CALIBRATE_TX_IQ_PERD_CMD	= 17,
+	IWL5000_PHY_CALIBRATE_CHAIN_NOISE_RESET_CMD = 18,
+	IWL5000_PHY_CALIBRATE_CHAIN_NOISE_GAIN_CMD = 19,
+};
+
+enum {
+	CALIBRATION_CFG_CMD = 0x65,
+	CALIBRATION_RES_NOTIFICATION = 0x66,
+	CALIBRATION_COMPLETE_NOTIFICATION = 0x67
+};
+
+struct iwl_cal_crystal_freq_cmd {
+	u8 cap_pin1;
+	u8 cap_pin2;
+} __attribute__ ((packed));
+
+struct iwl5000_calibration {
+	u8 op_code;
+	u8 first_group;
+	u8 num_groups;
+	u8 all_data_valid;
+	struct iwl_cal_crystal_freq_cmd data;
+} __attribute__ ((packed));
+
+#define IWL_CALIB_INIT_CFG_ALL	__constant_cpu_to_le32(0xffffffff)
+
+struct iwl_calib_cfg_elmnt_s {
+	__le32 is_enable;
+	__le32 start;
+	__le32 send_res;
+	__le32 apply_res;
+	__le32 reserved;
+} __attribute__ ((packed));
+
+struct iwl_calib_cfg_status_s {
+	struct iwl_calib_cfg_elmnt_s once;
+	struct iwl_calib_cfg_elmnt_s perd;
+	__le32 flags;
+} __attribute__ ((packed));
+
+struct iwl5000_calib_cfg_cmd {
+	struct iwl_calib_cfg_status_s ucd_calib_cfg;
+	struct iwl_calib_cfg_status_s drv_calib_cfg;
+	__le32 reserved1;
+} __attribute__ ((packed));
+
+struct iwl5000_calib_hdr {
+	u8 op_code;
+	u8 first_group;
+	u8 groups_num;
+	u8 data_valid;
+} __attribute__ ((packed));
+
+struct iwl5000_calibration_chain_noise_reset_cmd {
+	u8 op_code;	/* IWL5000_PHY_CALIBRATE_CHAIN_NOISE_RESET_CMD */
+	u8 flags;	/* not used */
+	__le16 reserved;
+} __attribute__ ((packed));
+
+struct iwl5000_calibration_chain_noise_gain_cmd {
+	u8 op_code;	/* IWL5000_PHY_CALIBRATE_CHAIN_NOISE_GAIN_CMD */
+	u8 flags;	/* not used */
+	__le16 reserved;
+	u8 delta_gain_1;
+	u8 delta_gain_2;
+	__le16 reserved1;
+} __attribute__ ((packed));
+
 /******************************************************************************
  * (12)
  * Miscellaneous Commands:
@@ -2672,7 +2929,7 @@ struct iwl4965_calibration_cmd {
  * 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 iwl4965_led_cmd {
+struct iwl_led_cmd {
 	__le32 interval;	/* "interval" in uSec */
 	u8 id;			/* 1: Activity, 2: Link, 3: Tech */
 	u8 off;			/* # intervals off while blinking;
@@ -2682,30 +2939,81 @@ struct iwl4965_led_cmd {
 	u8 reserved;
 } __attribute__ ((packed));
 
+/*
+ * Coexistence WIFI/WIMAX  Command
+ * COEX_PRIORITY_TABLE_CMD = 0x5a
+ *
+ */
+enum {
+	COEX_UNASSOC_IDLE		= 0,
+	COEX_UNASSOC_MANUAL_SCAN	= 1,
+	COEX_UNASSOC_AUTO_SCAN		= 2,
+	COEX_CALIBRATION		= 3,
+	COEX_PERIODIC_CALIBRATION	= 4,
+	COEX_CONNECTION_ESTAB		= 5,
+	COEX_ASSOCIATED_IDLE		= 6,
+	COEX_ASSOC_MANUAL_SCAN		= 7,
+	COEX_ASSOC_AUTO_SCAN		= 8,
+	COEX_ASSOC_ACTIVE_LEVEL		= 9,
+	COEX_RF_ON			= 10,
+	COEX_RF_OFF			= 11,
+	COEX_STAND_ALONE_DEBUG		= 12,
+	COEX_IPAN_ASSOC_LEVEL		= 13,
+	COEX_RSRVD1			= 14,
+	COEX_RSRVD2			= 15,
+	COEX_NUM_OF_EVENTS		= 16
+};
+
+struct iwl_wimax_coex_event_entry {
+	u8 request_prio;
+	u8 win_medium_prio;
+	u8 reserved;
+	u8 flags;
+} __attribute__ ((packed));
+
+/* COEX flag masks */
+
+/* Staion table is valid */
+#define COEX_FLAGS_STA_TABLE_VALID_MSK      (0x1)
+/* UnMask wakeup src at unassociated sleep */
+#define COEX_FLAGS_UNASSOC_WA_UNMASK_MSK    (0x4)
+/* UnMask wakeup src at associated sleep */
+#define COEX_FLAGS_ASSOC_WA_UNMASK_MSK      (0x8)
+/* Enable CoEx feature. */
+#define COEX_FLAGS_COEX_ENABLE_MSK          (0x80)
+
+struct iwl_wimax_coex_cmd {
+	u8 flags;
+	u8 reserved[3];
+	struct iwl_wimax_coex_event_entry sta_prio[COEX_NUM_OF_EVENTS];
+} __attribute__ ((packed));
+
 /******************************************************************************
  * (13)
  * Union of all expected notifications/responses:
  *
  *****************************************************************************/
 
-struct iwl4965_rx_packet {
+struct iwl_rx_packet {
 	__le32 len;
 	struct iwl_cmd_header hdr;
 	union {
-		struct iwl4965_alive_resp alive_frame;
+		struct iwl_alive_resp alive_frame;
 		struct iwl4965_rx_frame rx_frame;
 		struct iwl4965_tx_resp tx_resp;
 		struct iwl4965_spectrum_notification spectrum_notif;
 		struct iwl4965_csa_notification csa_notif;
-		struct iwl4965_error_resp err_resp;
+		struct iwl_error_resp err_resp;
 		struct iwl4965_card_state_notif card_state_notif;
 		struct iwl4965_beacon_notif beacon_status;
-		struct iwl4965_add_sta_resp add_sta;
+		struct iwl_add_sta_resp add_sta;
+		struct iwl_rem_sta_resp rem_sta;
 		struct iwl4965_sleep_notification sleep_notif;
 		struct iwl4965_spectrum_resp spectrum;
-		struct iwl4965_notif_statistics stats;
-		struct iwl4965_compressed_ba_resp compressed_ba;
+		struct iwl_notif_statistics stats;
+		struct iwl_compressed_ba_resp compressed_ba;
 		struct iwl4965_missed_beacon_notif missed_beacon;
+		struct iwl5000_calibration calib;
 		__le32 status;
 		u8 raw[0];
 	} u;
diff --git a/drivers/net/wireless/iwlwifi/iwl-core.c b/drivers/net/wireless/iwlwifi/iwl-core.c
index 2dfd982d7d1f..a44188bf4459 100644
--- a/drivers/net/wireless/iwlwifi/iwl-core.c
+++ b/drivers/net/wireless/iwlwifi/iwl-core.c
@@ -34,9 +34,11 @@
 struct iwl_priv; /* FIXME: remove */
 #include "iwl-debug.h"
 #include "iwl-eeprom.h"
-#include "iwl-4965.h" /* FIXME: remove */
+#include "iwl-dev.h" /* FIXME: remove */
 #include "iwl-core.h"
+#include "iwl-io.h"
 #include "iwl-rfkill.h"
+#include "iwl-power.h"
 
 
 MODULE_DESCRIPTION("iwl core");
@@ -44,10 +46,106 @@ MODULE_VERSION(IWLWIFI_VERSION);
 MODULE_AUTHOR(DRV_COPYRIGHT);
 MODULE_LICENSE("GPL");
 
-#ifdef CONFIG_IWLWIFI_DEBUG
-u32 iwl_debug_level;
-EXPORT_SYMBOL(iwl_debug_level);
-#endif
+#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_MIMO2_##s##M_PLCP,\
+				    IWL_RATE_MIMO3_##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 */
+	/* FIXME:RS:          ^^    should be INV (legacy) */
+};
+EXPORT_SYMBOL(iwl_rates);
+
+/**
+ * translate ucode response to mac80211 tx status control values
+ */
+void iwl_hwrate_to_tx_control(struct iwl_priv *priv, u32 rate_n_flags,
+				  struct ieee80211_tx_info *control)
+{
+	int rate_index;
+
+	control->antenna_sel_tx =
+		((rate_n_flags & RATE_MCS_ANT_ABC_MSK) >> RATE_MCS_ANT_POS);
+	if (rate_n_flags & RATE_MCS_HT_MSK)
+		control->flags |= IEEE80211_TX_CTL_OFDM_HT;
+	if (rate_n_flags & RATE_MCS_GF_MSK)
+		control->flags |= IEEE80211_TX_CTL_GREEN_FIELD;
+	if (rate_n_flags & RATE_MCS_FAT_MSK)
+		control->flags |= IEEE80211_TX_CTL_40_MHZ_WIDTH;
+	if (rate_n_flags & RATE_MCS_DUP_MSK)
+		control->flags |= IEEE80211_TX_CTL_DUP_DATA;
+	if (rate_n_flags & RATE_MCS_SGI_MSK)
+		control->flags |= IEEE80211_TX_CTL_SHORT_GI;
+	rate_index = iwl_hwrate_to_plcp_idx(rate_n_flags);
+	if (control->band == IEEE80211_BAND_5GHZ)
+		rate_index -= IWL_FIRST_OFDM_RATE;
+	control->tx_rate_idx = rate_index;
+}
+EXPORT_SYMBOL(iwl_hwrate_to_tx_control);
+
+int iwl_hwrate_to_plcp_idx(u32 rate_n_flags)
+{
+	int idx = 0;
+
+	/* HT rate format */
+	if (rate_n_flags & RATE_MCS_HT_MSK) {
+		idx = (rate_n_flags & 0xff);
+
+		if (idx >= IWL_RATE_MIMO2_6M_PLCP)
+			idx = idx - IWL_RATE_MIMO2_6M_PLCP;
+
+		idx += IWL_FIRST_OFDM_RATE;
+		/* skip 9M not supported in ht*/
+		if (idx >= IWL_RATE_9M_INDEX)
+			idx += 1;
+		if ((idx >= IWL_FIRST_OFDM_RATE) && (idx <= IWL_LAST_OFDM_RATE))
+			return idx;
+
+	/* legacy rate format, search for match in table */
+	} else {
+		for (idx = 0; idx < ARRAY_SIZE(iwl_rates); idx++)
+			if (iwl_rates[idx].plcp == (rate_n_flags & 0xFF))
+				return idx;
+	}
+
+	return -1;
+}
+EXPORT_SYMBOL(iwl_hwrate_to_plcp_idx);
+
+
+
+const u8 iwl_bcast_addr[ETH_ALEN] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };
+EXPORT_SYMBOL(iwl_bcast_addr);
+
 
 /* This function both allocates and initializes hw and priv. */
 struct ieee80211_hw *iwl_alloc_all(struct iwl_cfg *cfg,
@@ -72,25 +170,132 @@ out:
 }
 EXPORT_SYMBOL(iwl_alloc_all);
 
+void iwl_hw_detect(struct iwl_priv *priv)
+{
+	priv->hw_rev = _iwl_read32(priv, CSR_HW_REV);
+	priv->hw_wa_rev = _iwl_read32(priv, CSR_HW_REV_WA_REG);
+	pci_read_config_byte(priv->pci_dev, PCI_REVISION_ID, &priv->rev_id);
+}
+EXPORT_SYMBOL(iwl_hw_detect);
+
+/* Tell nic where to find the "keep warm" buffer */
+int iwl_kw_init(struct iwl_priv *priv)
+{
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	ret = iwl_grab_nic_access(priv);
+	if (ret)
+		goto out;
+
+	iwl_write_direct32(priv, FH_KW_MEM_ADDR_REG,
+			     priv->kw.dma_addr >> 4);
+	iwl_release_nic_access(priv);
+out:
+	spin_unlock_irqrestore(&priv->lock, flags);
+	return ret;
+}
+
+int iwl_kw_alloc(struct iwl_priv *priv)
+{
+	struct pci_dev *dev = priv->pci_dev;
+	struct iwl_kw *kw = &priv->kw;
+
+	kw->size = IWL_KW_SIZE;
+	kw->v_addr = pci_alloc_consistent(dev, kw->size, &kw->dma_addr);
+	if (!kw->v_addr)
+		return -ENOMEM;
+
+	return 0;
+}
+
+/**
+ * iwl_kw_free - Free the "keep warm" buffer
+ */
+void iwl_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));
+	}
+}
+
+int iwl_hw_nic_init(struct iwl_priv *priv)
+{
+	unsigned long flags;
+	struct iwl_rx_queue *rxq = &priv->rxq;
+	int ret;
+
+	/* nic_init */
+	spin_lock_irqsave(&priv->lock, flags);
+	priv->cfg->ops->lib->apm_ops.init(priv);
+	iwl_write32(priv, CSR_INT_COALESCING, 512 / 32);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	ret = priv->cfg->ops->lib->apm_ops.set_pwr_src(priv, IWL_PWR_SRC_VMAIN);
+
+	priv->cfg->ops->lib->apm_ops.config(priv);
+
+	/* Allocate the RX queue, or reset if it is already allocated */
+	if (!rxq->bd) {
+		ret = iwl_rx_queue_alloc(priv);
+		if (ret) {
+			IWL_ERROR("Unable to initialize Rx queue\n");
+			return -ENOMEM;
+		}
+	} else
+		iwl_rx_queue_reset(priv, rxq);
+
+	iwl_rx_replenish(priv);
+
+	iwl_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);
+
+	/* Allocate and init all Tx and Command queues */
+	ret = iwl_txq_ctx_reset(priv);
+	if (ret)
+		return ret;
+
+	set_bit(STATUS_INIT, &priv->status);
+
+	return 0;
+}
+EXPORT_SYMBOL(iwl_hw_nic_init);
+
 /**
- * iwlcore_clear_stations_table - Clear the driver's station table
+ * iwl_clear_stations_table - Clear the driver's station table
  *
  * NOTE:  This does not clear or otherwise alter the device's station table.
  */
-void iwlcore_clear_stations_table(struct iwl_priv *priv)
+void iwl_clear_stations_table(struct iwl_priv *priv)
 {
 	unsigned long flags;
 
 	spin_lock_irqsave(&priv->sta_lock, flags);
 
+	if (iwl_is_alive(priv) &&
+	   !test_bit(STATUS_EXIT_PENDING, &priv->status) &&
+	   iwl_send_cmd_pdu_async(priv, REPLY_REMOVE_ALL_STA, 0, NULL, NULL))
+		IWL_ERROR("Couldn't clear the station table\n");
+
 	priv->num_stations = 0;
 	memset(priv->stations, 0, sizeof(priv->stations));
 
 	spin_unlock_irqrestore(&priv->sta_lock, flags);
 }
-EXPORT_SYMBOL(iwlcore_clear_stations_table);
+EXPORT_SYMBOL(iwl_clear_stations_table);
 
-void iwlcore_reset_qos(struct iwl_priv *priv)
+void iwl_reset_qos(struct iwl_priv *priv)
 {
 	u16 cw_min = 15;
 	u16 cw_max = 1023;
@@ -176,7 +381,397 @@ void iwlcore_reset_qos(struct iwl_priv *priv)
 
 	spin_unlock_irqrestore(&priv->lock, flags);
 }
-EXPORT_SYMBOL(iwlcore_reset_qos);
+EXPORT_SYMBOL(iwl_reset_qos);
+
+#define MAX_BIT_RATE_40_MHZ 0x96; /* 150 Mbps */
+#define MAX_BIT_RATE_20_MHZ 0x48; /* 72 Mbps */
+static void iwlcore_init_ht_hw_capab(const struct iwl_priv *priv,
+			      struct ieee80211_ht_info *ht_info,
+			      enum ieee80211_band band)
+{
+	u16 max_bit_rate = 0;
+	u8 rx_chains_num = priv->hw_params.rx_chains_num;
+	u8 tx_chains_num = priv->hw_params.tx_chains_num;
+
+	ht_info->cap = 0;
+	memset(ht_info->supp_mcs_set, 0, 16);
+
+	ht_info->ht_supported = 1;
+
+	ht_info->cap |= (u16)IEEE80211_HT_CAP_GRN_FLD;
+	ht_info->cap |= (u16)IEEE80211_HT_CAP_SGI_20;
+	ht_info->cap |= (u16)(IEEE80211_HT_CAP_MIMO_PS &
+			     (IWL_MIMO_PS_NONE << 2));
+
+	max_bit_rate = MAX_BIT_RATE_20_MHZ;
+	if (priv->hw_params.fat_channel & BIT(band)) {
+		ht_info->cap |= (u16)IEEE80211_HT_CAP_SUP_WIDTH;
+		ht_info->cap |= (u16)IEEE80211_HT_CAP_SGI_40;
+		ht_info->supp_mcs_set[4] = 0x01;
+		max_bit_rate = MAX_BIT_RATE_40_MHZ;
+	}
+
+	if (priv->cfg->mod_params->amsdu_size_8K)
+		ht_info->cap |= (u16)IEEE80211_HT_CAP_MAX_AMSDU;
+
+	ht_info->ampdu_factor = CFG_HT_RX_AMPDU_FACTOR_DEF;
+	ht_info->ampdu_density = CFG_HT_MPDU_DENSITY_DEF;
+
+	ht_info->supp_mcs_set[0] = 0xFF;
+	if (rx_chains_num >= 2)
+		ht_info->supp_mcs_set[1] = 0xFF;
+	if (rx_chains_num >= 3)
+		ht_info->supp_mcs_set[2] = 0xFF;
+
+	/* Highest supported Rx data rate */
+	max_bit_rate *= rx_chains_num;
+	ht_info->supp_mcs_set[10] = (u8)(max_bit_rate & 0x00FF);
+	ht_info->supp_mcs_set[11] = (u8)((max_bit_rate & 0xFF00) >> 8);
+
+	/* Tx MCS capabilities */
+	ht_info->supp_mcs_set[12] = IEEE80211_HT_CAP_MCS_TX_DEFINED;
+	if (tx_chains_num != rx_chains_num) {
+		ht_info->supp_mcs_set[12] |= IEEE80211_HT_CAP_MCS_TX_RX_DIFF;
+		ht_info->supp_mcs_set[12] |= ((tx_chains_num - 1) << 2);
+	}
+}
+
+static void iwlcore_init_hw_rates(struct iwl_priv *priv,
+			      struct ieee80211_rate *rates)
+{
+	int i;
+
+	for (i = 0; i < IWL_RATE_COUNT; i++) {
+		rates[i].bitrate = iwl_rates[i].ieee * 5;
+		rates[i].hw_value = i; /* Rate scaling will work on indexes */
+		rates[i].hw_value_short = i;
+		rates[i].flags = 0;
+		if ((i > IWL_LAST_OFDM_RATE) || (i < IWL_FIRST_OFDM_RATE)) {
+			/*
+			 * If CCK != 1M then set short preamble rate flag.
+			 */
+			rates[i].flags |=
+				(iwl_rates[i].plcp == IWL_RATE_1M_PLCP) ?
+					0 : IEEE80211_RATE_SHORT_PREAMBLE;
+		}
+	}
+}
+
+/**
+ * iwlcore_init_geos - Initialize mac80211's geo/channel info based from eeprom
+ */
+static int iwlcore_init_geos(struct iwl_priv *priv)
+{
+	struct iwl_channel_info *ch;
+	struct ieee80211_supported_band *sband;
+	struct ieee80211_channel *channels;
+	struct ieee80211_channel *geo_ch;
+	struct ieee80211_rate *rates;
+	int i = 0;
+
+	if (priv->bands[IEEE80211_BAND_2GHZ].n_bitrates ||
+	    priv->bands[IEEE80211_BAND_5GHZ].n_bitrates) {
+		IWL_DEBUG_INFO("Geography modes already initialized.\n");
+		set_bit(STATUS_GEO_CONFIGURED, &priv->status);
+		return 0;
+	}
+
+	channels = kzalloc(sizeof(struct ieee80211_channel) *
+			   priv->channel_count, GFP_KERNEL);
+	if (!channels)
+		return -ENOMEM;
+
+	rates = kzalloc((sizeof(struct ieee80211_rate) * (IWL_RATE_COUNT + 1)),
+			GFP_KERNEL);
+	if (!rates) {
+		kfree(channels);
+		return -ENOMEM;
+	}
+
+	/* 5.2GHz channels start after the 2.4GHz channels */
+	sband = &priv->bands[IEEE80211_BAND_5GHZ];
+	sband->channels = &channels[ARRAY_SIZE(iwl_eeprom_band_1)];
+	/* just OFDM */
+	sband->bitrates = &rates[IWL_FIRST_OFDM_RATE];
+	sband->n_bitrates = IWL_RATE_COUNT - IWL_FIRST_OFDM_RATE;
+
+	if (priv->cfg->sku & IWL_SKU_N)
+		iwlcore_init_ht_hw_capab(priv, &sband->ht_info,
+					 IEEE80211_BAND_5GHZ);
+
+	sband = &priv->bands[IEEE80211_BAND_2GHZ];
+	sband->channels = channels;
+	/* OFDM & CCK */
+	sband->bitrates = rates;
+	sband->n_bitrates = IWL_RATE_COUNT;
+
+	if (priv->cfg->sku & IWL_SKU_N)
+		iwlcore_init_ht_hw_capab(priv, &sband->ht_info,
+					 IEEE80211_BAND_2GHZ);
+
+	priv->ieee_channels = channels;
+	priv->ieee_rates = rates;
+
+	iwlcore_init_hw_rates(priv, rates);
+
+	for (i = 0;  i < priv->channel_count; i++) {
+		ch = &priv->channel_info[i];
+
+		/* FIXME: might be removed if scan is OK */
+		if (!is_channel_valid(ch))
+			continue;
+
+		if (is_channel_a_band(ch))
+			sband =  &priv->bands[IEEE80211_BAND_5GHZ];
+		else
+			sband =  &priv->bands[IEEE80211_BAND_2GHZ];
+
+		geo_ch = &sband->channels[sband->n_channels++];
+
+		geo_ch->center_freq =
+				ieee80211_channel_to_frequency(ch->channel);
+		geo_ch->max_power = ch->max_power_avg;
+		geo_ch->max_antenna_gain = 0xff;
+		geo_ch->hw_value = ch->channel;
+
+		if (is_channel_valid(ch)) {
+			if (!(ch->flags & EEPROM_CHANNEL_IBSS))
+				geo_ch->flags |= IEEE80211_CHAN_NO_IBSS;
+
+			if (!(ch->flags & EEPROM_CHANNEL_ACTIVE))
+				geo_ch->flags |= IEEE80211_CHAN_PASSIVE_SCAN;
+
+			if (ch->flags & EEPROM_CHANNEL_RADAR)
+				geo_ch->flags |= IEEE80211_CHAN_RADAR;
+
+			geo_ch->flags |= ch->fat_extension_channel;
+
+			if (ch->max_power_avg > priv->tx_power_channel_lmt)
+				priv->tx_power_channel_lmt = ch->max_power_avg;
+		} else {
+			geo_ch->flags |= IEEE80211_CHAN_DISABLED;
+		}
+
+		/* Save flags for reg domain usage */
+		geo_ch->orig_flags = geo_ch->flags;
+
+		IWL_DEBUG_INFO("Channel %d Freq=%d[%sGHz] %s flag=0x%X\n",
+				ch->channel, geo_ch->center_freq,
+				is_channel_a_band(ch) ?  "5.2" : "2.4",
+				geo_ch->flags & IEEE80211_CHAN_DISABLED ?
+				"restricted" : "valid",
+				 geo_ch->flags);
+	}
+
+	if ((priv->bands[IEEE80211_BAND_5GHZ].n_channels == 0) &&
+	     priv->cfg->sku & IWL_SKU_A) {
+		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->cfg->sku &= ~IWL_SKU_A;
+	}
+
+	printk(KERN_INFO DRV_NAME
+	       ": Tunable channels: %d 802.11bg, %d 802.11a channels\n",
+	       priv->bands[IEEE80211_BAND_2GHZ].n_channels,
+	       priv->bands[IEEE80211_BAND_5GHZ].n_channels);
+
+
+	set_bit(STATUS_GEO_CONFIGURED, &priv->status);
+
+	return 0;
+}
+
+/*
+ * iwlcore_free_geos - undo allocations in iwlcore_init_geos
+ */
+static void iwlcore_free_geos(struct iwl_priv *priv)
+{
+	kfree(priv->ieee_channels);
+	kfree(priv->ieee_rates);
+	clear_bit(STATUS_GEO_CONFIGURED, &priv->status);
+}
+
+static u8 is_single_rx_stream(struct iwl_priv *priv)
+{
+	return !priv->current_ht_config.is_ht ||
+	       ((priv->current_ht_config.supp_mcs_set[1] == 0) &&
+		(priv->current_ht_config.supp_mcs_set[2] == 0)) ||
+	       priv->ps_mode == IWL_MIMO_PS_STATIC;
+}
+
+static u8 iwl_is_channel_extension(struct iwl_priv *priv,
+				   enum ieee80211_band band,
+				   u16 channel, u8 extension_chan_offset)
+{
+	const struct iwl_channel_info *ch_info;
+
+	ch_info = iwl_get_channel_info(priv, band, channel);
+	if (!is_channel_valid(ch_info))
+		return 0;
+
+	if (extension_chan_offset == IEEE80211_HT_IE_CHA_SEC_ABOVE)
+		return !(ch_info->fat_extension_channel &
+					IEEE80211_CHAN_NO_FAT_ABOVE);
+	else if (extension_chan_offset == IEEE80211_HT_IE_CHA_SEC_BELOW)
+		return !(ch_info->fat_extension_channel &
+					IEEE80211_CHAN_NO_FAT_BELOW);
+
+	return 0;
+}
+
+u8 iwl_is_fat_tx_allowed(struct iwl_priv *priv,
+			     struct ieee80211_ht_info *sta_ht_inf)
+{
+	struct iwl_ht_info *iwl_ht_conf = &priv->current_ht_config;
+
+	if ((!iwl_ht_conf->is_ht) ||
+	   (iwl_ht_conf->supported_chan_width != IWL_CHANNEL_WIDTH_40MHZ) ||
+	   (iwl_ht_conf->extension_chan_offset == IEEE80211_HT_IE_CHA_SEC_NONE))
+		return 0;
+
+	if (sta_ht_inf) {
+		if ((!sta_ht_inf->ht_supported) ||
+		   (!(sta_ht_inf->cap & IEEE80211_HT_CAP_SUP_WIDTH)))
+			return 0;
+	}
+
+	return iwl_is_channel_extension(priv, priv->band,
+					 iwl_ht_conf->control_channel,
+					 iwl_ht_conf->extension_chan_offset);
+}
+EXPORT_SYMBOL(iwl_is_fat_tx_allowed);
+
+void iwl_set_rxon_ht(struct iwl_priv *priv, struct iwl_ht_info *ht_info)
+{
+	struct iwl_rxon_cmd *rxon = &priv->staging_rxon;
+	u32 val;
+
+	if (!ht_info->is_ht)
+		return;
+
+	/* Set up channel bandwidth:  20 MHz only, or 20/40 mixed if fat ok */
+	if (iwl_is_fat_tx_allowed(priv, NULL))
+		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);
+		return;
+	}
+
+	/* Note: control channel is opposite of extension channel */
+	switch (ht_info->extension_chan_offset) {
+	case IEEE80211_HT_IE_CHA_SEC_ABOVE:
+		rxon->flags &= ~(RXON_FLG_CTRL_CHANNEL_LOC_HI_MSK);
+		break;
+	case IEEE80211_HT_IE_CHA_SEC_BELOW:
+		rxon->flags |= RXON_FLG_CTRL_CHANNEL_LOC_HI_MSK;
+		break;
+	case IEEE80211_HT_IE_CHA_SEC_NONE:
+	default:
+		rxon->flags &= ~RXON_FLG_CHANNEL_MODE_MIXED_MSK;
+		break;
+	}
+
+	val = ht_info->ht_protection;
+
+	rxon->flags |= cpu_to_le32(val << RXON_FLG_HT_OPERATING_MODE_POS);
+
+	iwl_set_rxon_chain(priv);
+
+	IWL_DEBUG_ASSOC("supported HT rate 0x%X 0x%X 0x%X "
+			"rxon flags 0x%X operation mode :0x%X "
+			"extension channel offset 0x%x "
+			"control chan %d\n",
+			ht_info->supp_mcs_set[0],
+			ht_info->supp_mcs_set[1],
+			ht_info->supp_mcs_set[2],
+			le32_to_cpu(rxon->flags), ht_info->ht_protection,
+			ht_info->extension_chan_offset,
+			ht_info->control_channel);
+	return;
+}
+EXPORT_SYMBOL(iwl_set_rxon_ht);
+
+/*
+ * 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 iwlcore_get_rx_chain_counter(struct iwl_priv *priv,
+					u8 *idle_state, u8 *rx_state)
+{
+	u8 is_single = is_single_rx_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;
+}
+
+/**
+ * iwl_set_rxon_chain - Set up Rx chain usage in "staging" RXON image
+ *
+ * Selects how many and which Rx receivers/antennas/chains to use.
+ * This should not be used for scan command ... it puts data in wrong place.
+ */
+void iwl_set_rxon_chain(struct iwl_priv *priv)
+{
+	u8 is_single = is_single_rx_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, iwl_chain_noise_calibration()
+	 *    checks which antennas actually *are* connected. */
+	priv->staging_rxon.rx_chain |=
+		    cpu_to_le16(priv->hw_params.valid_rx_ant <<
+						 RXON_RX_CHAIN_VALID_POS);
+
+	/* How many receivers should we use? */
+	iwlcore_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);
+}
+EXPORT_SYMBOL(iwl_set_rxon_chain);
 
 /**
  * iwlcore_set_rxon_channel - Set the phymode and channel values in staging RXON
@@ -188,7 +783,7 @@ EXPORT_SYMBOL(iwlcore_reset_qos);
  * NOTE:  Does not commit to the hardware; it sets appropriate bit fields
  * in the staging RXON flag structure based on the phymode
  */
-int iwlcore_set_rxon_channel(struct iwl_priv *priv,
+int iwl_set_rxon_channel(struct iwl_priv *priv,
 				enum ieee80211_band band,
 				u16 channel)
 {
@@ -214,68 +809,185 @@ int iwlcore_set_rxon_channel(struct iwl_priv *priv,
 
 	return 0;
 }
-EXPORT_SYMBOL(iwlcore_set_rxon_channel);
+EXPORT_SYMBOL(iwl_set_rxon_channel);
 
-static void iwlcore_init_hw(struct iwl_priv *priv)
+int iwl_setup_mac(struct iwl_priv *priv)
 {
+	int ret;
 	struct ieee80211_hw *hw = priv->hw;
 	hw->rate_control_algorithm = "iwl-4965-rs";
 
-	/* 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;
-
+	/* Tell mac80211 our characteristics */
+	hw->flags = IEEE80211_HW_HOST_GEN_BEACON_TEMPLATE |
+		    IEEE80211_HW_SIGNAL_DBM |
+		    IEEE80211_HW_NOISE_DBM;
 	/* Default value; 4 EDCA QOS priorities */
 	hw->queues = 4;
-#ifdef CONFIG_IWL4965_HT
-	/* Enhanced value; more queues, to support 11n aggregation */
-	hw->queues = 16;
-#endif /* CONFIG_IWL4965_HT */
+	/* queues to support 11n aggregation */
+	if (priv->cfg->sku & IWL_SKU_N)
+		hw->ampdu_queues = priv->cfg->mod_params->num_of_ampdu_queues;
+
+	hw->conf.beacon_int = 100;
+
+	if (priv->bands[IEEE80211_BAND_2GHZ].n_channels)
+		priv->hw->wiphy->bands[IEEE80211_BAND_2GHZ] =
+			&priv->bands[IEEE80211_BAND_2GHZ];
+	if (priv->bands[IEEE80211_BAND_5GHZ].n_channels)
+		priv->hw->wiphy->bands[IEEE80211_BAND_5GHZ] =
+			&priv->bands[IEEE80211_BAND_5GHZ];
+
+	ret = ieee80211_register_hw(priv->hw);
+	if (ret) {
+		IWL_ERROR("Failed to register hw (error %d)\n", ret);
+		return ret;
+	}
+	priv->mac80211_registered = 1;
+
+	return 0;
 }
+EXPORT_SYMBOL(iwl_setup_mac);
 
-int iwl_setup(struct iwl_priv *priv)
+int iwl_set_hw_params(struct iwl_priv *priv)
 {
-	int ret = 0;
-	iwlcore_init_hw(priv);
-	ret = priv->cfg->ops->lib->init_drv(priv);
-	return ret;
+	priv->hw_params.sw_crypto = priv->cfg->mod_params->sw_crypto;
+	priv->hw_params.max_rxq_size = RX_QUEUE_SIZE;
+	priv->hw_params.max_rxq_log = RX_QUEUE_SIZE_LOG;
+	if (priv->cfg->mod_params->amsdu_size_8K)
+		priv->hw_params.rx_buf_size = IWL_RX_BUF_SIZE_8K;
+	else
+		priv->hw_params.rx_buf_size = IWL_RX_BUF_SIZE_4K;
+	priv->hw_params.max_pkt_size = priv->hw_params.rx_buf_size - 256;
+
+	if (priv->cfg->mod_params->disable_11n)
+		priv->cfg->sku &= ~IWL_SKU_N;
+
+	/* Device-specific setup */
+	return priv->cfg->ops->lib->set_hw_params(priv);
 }
-EXPORT_SYMBOL(iwl_setup);
+EXPORT_SYMBOL(iwl_set_hw_params);
 
-/* Low level driver call this function to update iwlcore with
- * driver status.
- */
-int iwlcore_low_level_notify(struct iwl_priv *priv,
-			      enum iwlcore_card_notify notify)
+int iwl_init_drv(struct iwl_priv *priv)
 {
 	int ret;
-	switch (notify) {
-	case IWLCORE_INIT_EVT:
-		ret = iwl_rfkill_init(priv);
-		if (ret)
-			IWL_ERROR("Unable to initialize RFKILL system. "
-				  "Ignoring error: %d\n", ret);
-		break;
-	case IWLCORE_START_EVT:
-		break;
-	case IWLCORE_STOP_EVT:
-		break;
-	case IWLCORE_REMOVE_EVT:
-		iwl_rfkill_unregister(priv);
-		break;
+
+	priv->retry_rate = 1;
+	priv->ibss_beacon = NULL;
+
+	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);
+
+	INIT_LIST_HEAD(&priv->free_frames);
+
+	mutex_init(&priv->mutex);
+
+	/* Clear the driver's (not device's) station table */
+	iwl_clear_stations_table(priv);
+
+	priv->data_retry_limit = -1;
+	priv->ieee_channels = NULL;
+	priv->ieee_rates = NULL;
+	priv->band = IEEE80211_BAND_2GHZ;
+
+	priv->iw_mode = IEEE80211_IF_TYPE_STA;
+
+	priv->use_ant_b_for_management_frame = 1; /* start with ant B */
+	priv->ps_mode = IWL_MIMO_PS_NONE;
+
+	/* Choose which receivers/antennas to use */
+	iwl_set_rxon_chain(priv);
+	iwl_init_scan_params(priv);
+
+	if (priv->cfg->mod_params->enable_qos)
+		priv->qos_data.qos_enable = 1;
+
+	iwl_reset_qos(priv);
+
+	priv->qos_data.qos_active = 0;
+	priv->qos_data.qos_cap.val = 0;
+
+	iwl_set_rxon_channel(priv, IEEE80211_BAND_2GHZ, 6);
+
+	priv->rates_mask = IWL_RATES_MASK;
+	/* If power management is turned on, default to AC mode */
+	priv->power_mode = IWL_POWER_AC;
+	priv->tx_power_user_lmt = IWL_TX_POWER_TARGET_POWER_MAX;
+
+	ret = iwl_init_channel_map(priv);
+	if (ret) {
+		IWL_ERROR("initializing regulatory failed: %d\n", ret);
+		goto err;
+	}
+
+	ret = iwlcore_init_geos(priv);
+	if (ret) {
+		IWL_ERROR("initializing geos failed: %d\n", ret);
+		goto err_free_channel_map;
 	}
 
 	return 0;
+
+err_free_channel_map:
+	iwl_free_channel_map(priv);
+err:
+	return ret;
+}
+EXPORT_SYMBOL(iwl_init_drv);
+
+void iwl_free_calib_results(struct iwl_priv *priv)
+{
+	kfree(priv->calib_results.lo_res);
+	priv->calib_results.lo_res = NULL;
+	priv->calib_results.lo_res_len = 0;
+
+	kfree(priv->calib_results.tx_iq_res);
+	priv->calib_results.tx_iq_res = NULL;
+	priv->calib_results.tx_iq_res_len = 0;
+
+	kfree(priv->calib_results.tx_iq_perd_res);
+	priv->calib_results.tx_iq_perd_res = NULL;
+	priv->calib_results.tx_iq_perd_res_len = 0;
+}
+EXPORT_SYMBOL(iwl_free_calib_results);
+
+int iwl_set_tx_power(struct iwl_priv *priv, s8 tx_power, bool force)
+{
+	int ret = 0;
+	if (tx_power < IWL_TX_POWER_TARGET_POWER_MIN) {
+		IWL_WARNING("Requested user TXPOWER %d below limit.\n",
+			    priv->tx_power_user_lmt);
+		return -EINVAL;
+	}
+
+	if (tx_power > IWL_TX_POWER_TARGET_POWER_MAX) {
+		IWL_WARNING("Requested user TXPOWER %d above limit.\n",
+			    priv->tx_power_user_lmt);
+		return -EINVAL;
+	}
+
+	if (priv->tx_power_user_lmt != tx_power)
+		force = true;
+
+	priv->tx_power_user_lmt = tx_power;
+
+	if (force && priv->cfg->ops->lib->send_tx_power)
+		ret = priv->cfg->ops->lib->send_tx_power(priv);
+
+	return ret;
+}
+EXPORT_SYMBOL(iwl_set_tx_power);
+
+
+void iwl_uninit_drv(struct iwl_priv *priv)
+{
+	iwl_free_calib_results(priv);
+	iwlcore_free_geos(priv);
+	iwl_free_channel_map(priv);
+	kfree(priv->scan);
 }
-EXPORT_SYMBOL(iwlcore_low_level_notify);
+EXPORT_SYMBOL(iwl_uninit_drv);
 
 int iwl_send_statistics_request(struct iwl_priv *priv, u8 flags)
 {
@@ -290,3 +1002,440 @@ int iwl_send_statistics_request(struct iwl_priv *priv, u8 flags)
 }
 EXPORT_SYMBOL(iwl_send_statistics_request);
 
+/**
+ * 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 iwlcore_verify_inst_sparse(struct iwl_priv *priv, __le32 *image, u32 len)
+{
+	u32 val;
+	int ret = 0;
+	u32 errcnt = 0;
+	u32 i;
+
+	IWL_DEBUG_INFO("ucode inst image size is %u\n", len);
+
+	ret = iwl_grab_nic_access(priv);
+	if (ret)
+		return ret;
+
+	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_direct32(priv, HBUS_TARG_MEM_RADDR,
+			i + RTC_INST_LOWER_BOUND);
+		val = _iwl_read_direct32(priv, HBUS_TARG_MEM_RDAT);
+		if (val != le32_to_cpu(*image)) {
+			ret = -EIO;
+			errcnt++;
+			if (errcnt >= 3)
+				break;
+		}
+	}
+
+	iwl_release_nic_access(priv);
+
+	return ret;
+}
+
+/**
+ * iwlcore_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 ret = 0;
+	u32 errcnt;
+
+	IWL_DEBUG_INFO("ucode inst image size is %u\n", len);
+
+	ret = iwl_grab_nic_access(priv);
+	if (ret)
+		return ret;
+
+	iwl_write_direct32(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_direct32(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));
+			ret = -EIO;
+			errcnt++;
+			if (errcnt >= 20)
+				break;
+		}
+	}
+
+	iwl_release_nic_access(priv);
+
+	if (!errcnt)
+		IWL_DEBUG_INFO
+		    ("ucode image in INSTRUCTION memory is good\n");
+
+	return ret;
+}
+
+/**
+ * iwl_verify_ucode - determine which instruction image is in SRAM,
+ *    and verify its contents
+ */
+int iwl_verify_ucode(struct iwl_priv *priv)
+{
+	__le32 *image;
+	u32 len;
+	int ret;
+
+	/* Try bootstrap */
+	image = (__le32 *)priv->ucode_boot.v_addr;
+	len = priv->ucode_boot.len;
+	ret = iwlcore_verify_inst_sparse(priv, image, len);
+	if (!ret) {
+		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;
+	ret = iwlcore_verify_inst_sparse(priv, image, len);
+	if (!ret) {
+		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;
+	ret = iwlcore_verify_inst_sparse(priv, image, len);
+	if (!ret) {
+		IWL_DEBUG_INFO("Runtime uCode is good in inst SRAM\n");
+		return 0;
+	}
+
+	IWL_ERROR("NO VALID UCODE IMAGE IN INSTRUCTION SRAM!!\n");
+
+	/* Since nothing seems to match, show first several data entries in
+	 * instruction SRAM, so maybe visual inspection will give a clue.
+	 * Selection of bootstrap image (vs. other images) is arbitrary. */
+	image = (__le32 *)priv->ucode_boot.v_addr;
+	len = priv->ucode_boot.len;
+	ret = iwl_verify_inst_full(priv, image, len);
+
+	return ret;
+}
+EXPORT_SYMBOL(iwl_verify_ucode);
+
+
+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))
+
+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 ret;
+
+	if (priv->ucode_type == UCODE_INIT)
+		base = le32_to_cpu(priv->card_alive_init.error_event_table_ptr);
+	else
+		base = le32_to_cpu(priv->card_alive.error_event_table_ptr);
+
+	if (!priv->cfg->ops->lib->is_valid_rtc_data_addr(base)) {
+		IWL_ERROR("Not valid error log pointer 0x%08X\n", base);
+		return;
+	}
+
+	ret = iwl_grab_nic_access(priv);
+	if (ret) {
+		IWL_WARNING("Can not read from adapter at this time.\n");
+		return;
+	}
+
+	count = iwl_read_targ_mem(priv, base);
+
+	if (ERROR_START_OFFSET <= count * ERROR_ELEM_SIZE) {
+		IWL_ERROR("Start IWL Error Log Dump:\n");
+		IWL_ERROR("Status: 0x%08lX, count: %d\n", priv->status, count);
+	}
+
+	desc = iwl_read_targ_mem(priv, base + 1 * sizeof(u32));
+	blink1 = iwl_read_targ_mem(priv, base + 3 * sizeof(u32));
+	blink2 = iwl_read_targ_mem(priv, base + 4 * sizeof(u32));
+	ilink1 = iwl_read_targ_mem(priv, base + 5 * sizeof(u32));
+	ilink2 = iwl_read_targ_mem(priv, base + 6 * sizeof(u32));
+	data1 = iwl_read_targ_mem(priv, base + 7 * sizeof(u32));
+	data2 = iwl_read_targ_mem(priv, base + 8 * sizeof(u32));
+	line = iwl_read_targ_mem(priv, base + 9 * sizeof(u32));
+	time = iwl_read_targ_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_nic_access(priv);
+}
+EXPORT_SYMBOL(iwl_dump_nic_error_log);
+
+#define EVENT_START_OFFSET  (4 * sizeof(u32))
+
+/**
+ * iwl_print_event_log - Dump error event log to syslog
+ *
+ * NOTE: Must be called with iwl4965_grab_nic_access() already obtained!
+ */
+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;
+	if (priv->ucode_type == UCODE_INIT)
+		base = le32_to_cpu(priv->card_alive_init.log_event_table_ptr);
+	else
+		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_targ_mem(priv, ptr);
+		ptr += sizeof(u32);
+		time = iwl_read_targ_mem(priv, ptr);
+		ptr += sizeof(u32);
+		if (mode == 0) {
+			/* data, ev */
+			IWL_ERROR("EVT_LOG:0x%08x:%04u\n", time, ev);
+		} else {
+			data = iwl_read_targ_mem(priv, ptr);
+			ptr += sizeof(u32);
+			IWL_ERROR("EVT_LOGT:%010u:0x%08x:%04u\n",
+					time, data, ev);
+		}
+	}
+}
+EXPORT_SYMBOL(iwl_print_event_log);
+
+
+void iwl_dump_nic_event_log(struct iwl_priv *priv)
+{
+	int ret;
+	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 */
+
+	if (priv->ucode_type == UCODE_INIT)
+		base = le32_to_cpu(priv->card_alive_init.log_event_table_ptr);
+	else
+		base = le32_to_cpu(priv->card_alive.log_event_table_ptr);
+
+	if (!priv->cfg->ops->lib->is_valid_rtc_data_addr(base)) {
+		IWL_ERROR("Invalid event log pointer 0x%08X\n", base);
+		return;
+	}
+
+	ret = iwl_grab_nic_access(priv);
+	if (ret) {
+		IWL_WARNING("Can not read from adapter at this time.\n");
+		return;
+	}
+
+	/* event log header */
+	capacity = iwl_read_targ_mem(priv, base);
+	mode = iwl_read_targ_mem(priv, base + (1 * sizeof(u32)));
+	num_wraps = iwl_read_targ_mem(priv, base + (2 * sizeof(u32)));
+	next_entry = iwl_read_targ_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_nic_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_nic_access(priv);
+}
+EXPORT_SYMBOL(iwl_dump_nic_event_log);
+
+void iwl_rf_kill_ct_config(struct iwl_priv *priv)
+{
+	struct iwl_ct_kill_config cmd;
+	unsigned long flags;
+	int ret = 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);
+
+	cmd.critical_temperature_R =
+		cpu_to_le32(priv->hw_params.ct_kill_threshold);
+
+	ret = iwl_send_cmd_pdu(priv, REPLY_CT_KILL_CONFIG_CMD,
+			       sizeof(cmd), &cmd);
+	if (ret)
+		IWL_ERROR("REPLY_CT_KILL_CONFIG_CMD failed\n");
+	else
+		IWL_DEBUG_INFO("REPLY_CT_KILL_CONFIG_CMD succeeded, "
+			"critical temperature is %d\n",
+			cmd.critical_temperature_R);
+}
+EXPORT_SYMBOL(iwl_rf_kill_ct_config);
+
+/*
+ * CARD_STATE_CMD
+ *
+ * Use: Sets the device's 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,
+	};
+
+	return iwl_send_cmd(priv, &cmd);
+}
+
+void iwl_radio_kill_sw_disable_radio(struct iwl_priv *priv)
+{
+	unsigned long flags;
+
+	if (test_bit(STATUS_RF_KILL_SW, &priv->status))
+		return;
+
+	IWL_DEBUG_RF_KILL("Manual SW RF KILL set to: RADIO OFF\n");
+
+	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);
+		/* call the host command only if no hw rf-kill set */
+		if (!test_bit(STATUS_RF_KILL_HW, &priv->status) &&
+		    iwl_is_ready(priv))
+			iwl_send_card_state(priv,
+				CARD_STATE_CMD_DISABLE, 0);
+		set_bit(STATUS_RF_KILL_SW, &priv->status);
+			/* make sure mac80211 stop sending Tx frame */
+		if (priv->mac80211_registered)
+			ieee80211_stop_queues(priv->hw);
+	}
+}
+EXPORT_SYMBOL(iwl_radio_kill_sw_disable_radio);
+
+int iwl_radio_kill_sw_enable_radio(struct iwl_priv *priv)
+{
+	unsigned long flags;
+
+	if (!test_bit(STATUS_RF_KILL_SW, &priv->status))
+		return 0;
+
+	IWL_DEBUG_RF_KILL("Manual SW RF KILL set to: RADIO ON\n");
+
+	spin_lock_irqsave(&priv->lock, flags);
+	iwl_write32(priv, CSR_UCODE_DRV_GP1_CLR, CSR_UCODE_SW_BIT_RFKILL);
+
+	/* If the driver is up it will receive CARD_STATE_NOTIFICATION
+	 * notification where it will clear SW rfkill status.
+	 * Setting it here would break the handler. Only if the
+	 * interface is down we can set here since we don't
+	 * receive any further notification.
+	 */
+	if (!priv->is_open)
+		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_nic_access(priv))
+		iwl_release_nic_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 0;
+	}
+
+	/* If the driver is already loaded, it will receive
+	 * CARD_STATE_NOTIFICATION notifications and the handler will
+	 * call restart to reload the driver.
+	 */
+	return 1;
+}
+EXPORT_SYMBOL(iwl_radio_kill_sw_enable_radio);
diff --git a/drivers/net/wireless/iwlwifi/iwl-core.h b/drivers/net/wireless/iwlwifi/iwl-core.h
index 7193d97630dc..db66114f1e56 100644
--- a/drivers/net/wireless/iwlwifi/iwl-core.h
+++ b/drivers/net/wireless/iwlwifi/iwl-core.h
@@ -70,7 +70,7 @@ struct iwl_host_cmd;
 struct iwl_cmd;
 
 
-#define IWLWIFI_VERSION "1.2.26k"
+#define IWLWIFI_VERSION "1.3.27k"
 #define DRV_COPYRIGHT	"Copyright(c) 2003-2008 Intel Corporation"
 
 #define IWL_PCI_DEVICE(dev, subdev, cfg) \
@@ -86,28 +86,63 @@ struct iwl_hcmd_ops {
 	int (*rxon_assoc)(struct iwl_priv *priv);
 };
 struct iwl_hcmd_utils_ops {
-	int (*enqueue_hcmd)(struct iwl_priv *priv, struct iwl_host_cmd *cmd);
+	u16 (*get_hcmd_size)(u8 cmd_id, u16 len);
+	u16 (*build_addsta_hcmd)(const struct iwl_addsta_cmd *cmd, u8 *data);
+	void (*gain_computation)(struct iwl_priv *priv,
+			u32 *average_noise,
+			u16 min_average_noise_antennat_i,
+			u32 min_average_noise);
+	void (*chain_noise_reset)(struct iwl_priv *priv);
+	void (*rts_tx_cmd_flag)(struct ieee80211_tx_info *info,
+			__le32 *tx_flags);
 };
 
 struct iwl_lib_ops {
-	/* iwlwifi driver (priv) init */
-	int (*init_drv)(struct iwl_priv *priv);
 	/* set hw dependant perameters */
 	int (*set_hw_params)(struct iwl_priv *priv);
-
+	/* ucode shared memory */
+	int (*alloc_shared_mem)(struct iwl_priv *priv);
+	void (*free_shared_mem)(struct iwl_priv *priv);
+	int (*shared_mem_rx_idx)(struct iwl_priv *priv);
+	/* Handling TX */
 	void (*txq_update_byte_cnt_tbl)(struct iwl_priv *priv,
-					struct iwl4965_tx_queue *txq,
+					struct iwl_tx_queue *txq,
 					u16 byte_cnt);
-	/* nic init */
-	int (*hw_nic_init)(struct iwl_priv *priv);
+	void (*txq_inval_byte_cnt_tbl)(struct iwl_priv *priv,
+				       struct iwl_tx_queue *txq);
+	void (*txq_set_sched)(struct iwl_priv *priv, u32 mask);
+	/* aggregations */
+	int (*txq_agg_enable)(struct iwl_priv *priv, int txq_id, int tx_fifo,
+			      int sta_id, int tid, u16 ssn_idx);
+	int (*txq_agg_disable)(struct iwl_priv *priv, u16 txq_id, u16 ssn_idx,
+			       u8 tx_fifo);
+	/* setup Rx handler */
+	void (*rx_handler_setup)(struct iwl_priv *priv);
+	/* setup deferred work */
+	void (*setup_deferred_work)(struct iwl_priv *priv);
+	/* cancel deferred work */
+	void (*cancel_deferred_work)(struct iwl_priv *priv);
+	/* alive notification after init uCode load */
+	void (*init_alive_start)(struct iwl_priv *priv);
 	/* alive notification */
 	int (*alive_notify)(struct iwl_priv *priv);
 	/* check validity of rtc data address */
 	int (*is_valid_rtc_data_addr)(u32 addr);
 	/* 1st ucode load */
 	int (*load_ucode)(struct iwl_priv *priv);
-	/* rfkill */
-	void (*radio_kill_sw)(struct iwl_priv *priv, int disable_radio);
+	 /* power management */
+	struct {
+		int (*init)(struct iwl_priv *priv);
+		int (*reset)(struct iwl_priv *priv);
+		void (*stop)(struct iwl_priv *priv);
+		void (*config)(struct iwl_priv *priv);
+		int (*set_pwr_src)(struct iwl_priv *priv, enum iwl_pwr_src src);
+	} apm_ops;
+	/* power */
+	int (*set_power)(struct iwl_priv *priv, void *cmd);
+	int (*send_tx_power) (struct iwl_priv *priv);
+	void (*update_chain_flags)(struct iwl_priv *priv);
+	void (*temperature) (struct iwl_priv *priv);
 	/* eeprom operations (as defined in iwl-eeprom.h) */
 	struct iwl_eeprom_ops eeprom_ops;
 };
@@ -124,15 +159,19 @@ struct iwl_mod_params {
 	int debug;		/* def: 0 = minimal debug log messages */
 	int disable_hw_scan;	/* def: 0 = use h/w scan */
 	int num_of_queues;	/* def: HW dependent */
+	int num_of_ampdu_queues;/* def: HW dependent */
 	int enable_qos;		/* def: 1 = use quality of service */
+	int disable_11n;	/* def: 0 = disable 11n capabilities */
 	int amsdu_size_8K;	/* def: 1 = enable 8K amsdu size */
 	int antenna;  		/* def: 0 = both antennas (use diversity) */
+	int restart_fw;		/* def: 1 = restart firmware */
 };
 
 struct iwl_cfg {
 	const char *name;
 	const char *fw_name;
 	unsigned int sku;
+	int eeprom_size;
 	const struct iwl_ops *ops;
 	const struct iwl_mod_params *mod_params;
 };
@@ -143,14 +182,113 @@ struct iwl_cfg {
 
 struct ieee80211_hw *iwl_alloc_all(struct iwl_cfg *cfg,
 		struct ieee80211_ops *hw_ops);
+void iwl_hw_detect(struct iwl_priv *priv);
 
-void iwlcore_clear_stations_table(struct iwl_priv *priv);
-void iwlcore_reset_qos(struct iwl_priv *priv);
-int iwlcore_set_rxon_channel(struct iwl_priv *priv,
+void iwl_clear_stations_table(struct iwl_priv *priv);
+void iwl_free_calib_results(struct iwl_priv *priv);
+void iwl_reset_qos(struct iwl_priv *priv);
+void iwl_set_rxon_chain(struct iwl_priv *priv);
+int iwl_set_rxon_channel(struct iwl_priv *priv,
 				enum ieee80211_band band,
 				u16 channel);
+void iwl_set_rxon_ht(struct iwl_priv *priv, struct iwl_ht_info *ht_info);
+u8 iwl_is_fat_tx_allowed(struct iwl_priv *priv,
+			 struct ieee80211_ht_info *sta_ht_inf);
+int iwl_hw_nic_init(struct iwl_priv *priv);
+int iwl_setup_mac(struct iwl_priv *priv);
+int iwl_set_hw_params(struct iwl_priv *priv);
+int iwl_init_drv(struct iwl_priv *priv);
+void iwl_uninit_drv(struct iwl_priv *priv);
+/* "keep warm" functions */
+int iwl_kw_init(struct iwl_priv *priv);
+int iwl_kw_alloc(struct iwl_priv *priv);
+void iwl_kw_free(struct iwl_priv *priv);
+
+/*****************************************************
+* RX
+******************************************************/
+void iwl_rx_queue_free(struct iwl_priv *priv, struct iwl_rx_queue *rxq);
+int iwl_rx_queue_alloc(struct iwl_priv *priv);
+void iwl_rx_handle(struct iwl_priv *priv);
+int iwl_rx_queue_update_write_ptr(struct iwl_priv *priv,
+				  struct iwl_rx_queue *q);
+void iwl_rx_queue_reset(struct iwl_priv *priv, struct iwl_rx_queue *rxq);
+void iwl_rx_replenish(struct iwl_priv *priv);
+int iwl_rx_init(struct iwl_priv *priv, struct iwl_rx_queue *rxq);
+int iwl_rx_agg_start(struct iwl_priv *priv, const u8 *addr, int tid, u16 ssn);
+int iwl_rx_agg_stop(struct iwl_priv *priv, const u8 *addr, int tid);
+/* FIXME: remove when TX is moved to iwl core */
+int iwl_rx_queue_restock(struct iwl_priv *priv);
+int iwl_rx_queue_space(const struct iwl_rx_queue *q);
+void iwl_rx_allocate(struct iwl_priv *priv);
+void iwl_tx_cmd_complete(struct iwl_priv *priv, struct iwl_rx_mem_buffer *rxb);
+int iwl_tx_queue_reclaim(struct iwl_priv *priv, int txq_id, int index);
+/* Handlers */
+void iwl_rx_missed_beacon_notif(struct iwl_priv *priv,
+			       struct iwl_rx_mem_buffer *rxb);
+void iwl_rx_statistics(struct iwl_priv *priv,
+			      struct iwl_rx_mem_buffer *rxb);
+
+/* TX helpers */
+
+/*****************************************************
+* TX
+******************************************************/
+int iwl_txq_ctx_reset(struct iwl_priv *priv);
+int iwl_tx_skb(struct iwl_priv *priv, struct sk_buff *skb);
+/* FIXME: remove when free Tx is fully merged into iwlcore */
+int iwl_hw_txq_free_tfd(struct iwl_priv *priv, struct iwl_tx_queue *txq);
+void iwl_hw_txq_ctx_free(struct iwl_priv *priv);
+int iwl_hw_txq_attach_buf_to_tfd(struct iwl_priv *priv, void *tfd,
+					dma_addr_t addr, u16 len);
+int iwl_txq_update_write_ptr(struct iwl_priv *priv, struct iwl_tx_queue *txq);
+int iwl_tx_agg_start(struct iwl_priv *priv, const u8 *ra, u16 tid, u16 *ssn);
+int iwl_tx_agg_stop(struct iwl_priv *priv , const u8 *ra, u16 tid);
+int iwl_txq_check_empty(struct iwl_priv *priv, int sta_id, u8 tid, int txq_id);
+
+/*****************************************************
+ * TX power
+ ****************************************************/
+int iwl_set_tx_power(struct iwl_priv *priv, s8 tx_power, bool force);
+
+/*****************************************************
+ * RF -Kill - here and not in iwl-rfkill.h to be available when
+ * RF-kill subsystem is not compiled.
+ ****************************************************/
+void iwl_radio_kill_sw_disable_radio(struct iwl_priv *priv);
+int iwl_radio_kill_sw_enable_radio(struct iwl_priv *priv);
+
+/*******************************************************************************
+ * Rate
+ ******************************************************************************/
+
+void iwl_hwrate_to_tx_control(struct iwl_priv *priv, u32 rate_n_flags,
+			      struct ieee80211_tx_info *info);
+int iwl_hwrate_to_plcp_idx(u32 rate_n_flags);
+
+static inline u8 iwl_hw_get_rate(__le32 rate_n_flags)
+{
+	return le32_to_cpu(rate_n_flags) & 0xFF;
+}
+static inline u32 iwl_hw_get_rate_n_flags(__le32 rate_n_flags)
+{
+	return le32_to_cpu(rate_n_flags) & 0x1FFFF;
+}
+static inline __le32 iwl_hw_set_rate_n_flags(u8 rate, u32 flags)
+{
+	return cpu_to_le32(flags|(u32)rate);
+}
 
-int iwl_setup(struct iwl_priv *priv);
+/*******************************************************************************
+ * Scanning
+ ******************************************************************************/
+void iwl_init_scan_params(struct iwl_priv *priv);
+int iwl_scan_cancel(struct iwl_priv *priv);
+int iwl_scan_cancel_timeout(struct iwl_priv *priv, unsigned long ms);
+const char *iwl_escape_essid(const char *essid, u8 essid_len);
+int iwl_scan_initiate(struct iwl_priv *priv);
+void iwl_setup_rx_scan_handlers(struct iwl_priv *priv);
+void iwl_setup_scan_deferred_work(struct iwl_priv *priv);
 
 /*****************************************************
  *   S e n d i n g     H o s t     C o m m a n d s   *
@@ -167,6 +305,17 @@ int iwl_send_cmd_pdu_async(struct iwl_priv *priv, u8 id, u16 len,
 			   int (*callback)(struct iwl_priv *priv,
 					   struct iwl_cmd *cmd,
 					   struct sk_buff *skb));
+
+int iwl_enqueue_hcmd(struct iwl_priv *priv, struct iwl_host_cmd *cmd);
+
+/*****************************************************
+*  Error Handling Debugging
+******************************************************/
+void iwl_print_event_log(struct iwl_priv *priv, u32 start_idx,
+			 u32 num_events, u32 mode);
+void iwl_dump_nic_error_log(struct iwl_priv *priv);
+void iwl_dump_nic_event_log(struct iwl_priv *priv);
+
 /*************** DRIVER STATUS FUNCTIONS   *****/
 
 #define STATUS_HCMD_ACTIVE	0	/* host command in progress */
@@ -188,6 +337,7 @@ int iwl_send_cmd_pdu_async(struct iwl_priv *priv, u8 id, u16 len,
 #define STATUS_POWER_PMI	16
 #define STATUS_FW_ERROR		17
 #define STATUS_CONF_PENDING	18
+#define STATUS_MODE_PENDING	19
 
 
 static inline int iwl_is_ready(struct iwl_priv *priv)
@@ -209,10 +359,19 @@ static inline int iwl_is_init(struct iwl_priv *priv)
 	return test_bit(STATUS_INIT, &priv->status);
 }
 
+static inline int iwl_is_rfkill_sw(struct iwl_priv *priv)
+{
+	return test_bit(STATUS_RF_KILL_SW, &priv->status);
+}
+
+static inline int iwl_is_rfkill_hw(struct iwl_priv *priv)
+{
+	return test_bit(STATUS_RF_KILL_HW, &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);
+	return iwl_is_rfkill_hw(priv) || iwl_is_rfkill_sw(priv);
 }
 
 static inline int iwl_is_ready_rf(struct iwl_priv *priv)
@@ -224,23 +383,27 @@ static inline int iwl_is_ready_rf(struct iwl_priv *priv)
 	return iwl_is_ready(priv);
 }
 
-
-enum iwlcore_card_notify {
-	IWLCORE_INIT_EVT = 0,
-	IWLCORE_START_EVT = 1,
-	IWLCORE_STOP_EVT = 2,
-	IWLCORE_REMOVE_EVT = 3,
-};
-
-int iwlcore_low_level_notify(struct iwl_priv *priv,
-			     enum iwlcore_card_notify notify);
+extern void iwl_rf_kill_ct_config(struct iwl_priv *priv);
 extern int iwl_send_statistics_request(struct iwl_priv *priv, u8 flags);
-int iwl_send_lq_cmd(struct iwl_priv *priv,
-		    struct iwl_link_quality_cmd *lq, u8 flags);
+extern int iwl_verify_ucode(struct iwl_priv *priv);
+extern int iwl_send_lq_cmd(struct iwl_priv *priv,
+		struct iwl_link_quality_cmd *lq, u8 flags);
+extern void iwl_rx_reply_rx(struct iwl_priv *priv,
+		struct iwl_rx_mem_buffer *rxb);
+extern void iwl_rx_reply_rx_phy(struct iwl_priv *priv,
+				    struct iwl_rx_mem_buffer *rxb);
+void iwl_rx_reply_compressed_ba(struct iwl_priv *priv,
+					   struct iwl_rx_mem_buffer *rxb);
 
 static inline int iwl_send_rxon_assoc(struct iwl_priv *priv)
 {
 	return priv->cfg->ops->hcmd->rxon_assoc(priv);
 }
 
+static inline const struct ieee80211_supported_band *iwl_get_hw_mode(
+			struct iwl_priv *priv, enum ieee80211_band band)
+{
+	return priv->hw->wiphy->bands[band];
+}
+
 #endif /* __iwl_core_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-csr.h b/drivers/net/wireless/iwlwifi/iwl-csr.h
index 12725796ea5f..545ed692d889 100644
--- a/drivers/net/wireless/iwlwifi/iwl-csr.h
+++ b/drivers/net/wireless/iwlwifi/iwl-csr.h
@@ -87,16 +87,16 @@
 /* EEPROM reads */
 #define CSR_EEPROM_REG          (CSR_BASE+0x02c)
 #define CSR_EEPROM_GP           (CSR_BASE+0x030)
+#define CSR_GIO_REG		(CSR_BASE+0x03C)
 #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_GIO_CHICKEN_BITS    (CSR_BASE+0x100)
 #define CSR_LED_REG             (CSR_BASE+0x094)
+#define CSR_GIO_CHICKEN_BITS    (CSR_BASE+0x100)
 
-/* Analog phase-lock-loop configuration (3945 only)
- * Set bit 24. */
+/* Analog phase-lock-loop configuration  */
 #define CSR_ANA_PLL_CFG         (CSR_BASE+0x20c)
 /*
  * Indicates hardware rev, to determine CCK backoff for txpower calculation.
@@ -107,9 +107,9 @@
 
 /* Bits for CSR_HW_IF_CONFIG_REG */
 #define CSR49_HW_IF_CONFIG_REG_BIT_4965_R	(0x00000010)
-#define CSR49_HW_IF_CONFIG_REG_MSK_BOARD_VER	(0x00000C00)
-#define CSR49_HW_IF_CONFIG_REG_BIT_MAC_SI		(0x00000100)
-#define CSR49_HW_IF_CONFIG_REG_BIT_RADIO_SI	(0x00000200)
+#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)
 
 #define CSR39_HW_IF_CONFIG_REG_BIT_3945_MB         (0x00000100)
 #define CSR39_HW_IF_CONFIG_REG_BIT_3945_MM         (0x00000200)
@@ -170,6 +170,10 @@
 #define CSR49_FH_INT_TX_MASK	(CSR_FH_INT_BIT_TX_CHNL1 | \
 				 CSR_FH_INT_BIT_TX_CHNL0)
 
+/* 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               (0x00000200)
 
 /* RESET */
 #define CSR_RESET_REG_FLAG_NEVO_RESET                (0x00000001)
@@ -191,6 +195,16 @@
 #define CSR_GP_CNTRL_REG_FLAG_HW_RF_KILL_SW          (0x08000000)
 
 
+/* HW REV */
+#define CSR_HW_REV_TYPE_MSK            (0x00000F0)
+#define CSR_HW_REV_TYPE_3945           (0x00000D0)
+#define CSR_HW_REV_TYPE_4965           (0x0000000)
+#define CSR_HW_REV_TYPE_5300           (0x0000020)
+#define CSR_HW_REV_TYPE_5350           (0x0000030)
+#define CSR_HW_REV_TYPE_5100           (0x0000050)
+#define CSR_HW_REV_TYPE_5150           (0x0000040)
+#define CSR_HW_REV_TYPE_NONE           (0x00000F0)
+
 /* EEPROM REG */
 #define CSR_EEPROM_REG_READ_VALID_MSK	(0x00000001)
 #define CSR_EEPROM_REG_BIT_CMD		(0x00000002)
@@ -200,17 +214,15 @@
 #define CSR_EEPROM_GP_BAD_SIGNATURE	(0x00000000)
 #define CSR_EEPROM_GP_IF_OWNER_MSK	(0x00000180)
 
+/* CSR GIO */
+#define CSR_GIO_REG_VAL_L0S_ENABLED	(0x00000002)
+
 /* 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)
@@ -220,6 +232,10 @@
 #define CSR_LED_REG_TRUN_ON (0x78)
 #define CSR_LED_REG_TRUN_OFF (0x38)
 
+/* ANA_PLL */
+#define CSR39_ANA_PLL_CFG_VAL        (0x01000000)
+#define CSR50_ANA_PLL_CFG_VAL        (0x00880300)
+
 /*=== HBUS (Host-side Bus) ===*/
 #define HBUS_BASE	(0x400)
 /*
diff --git a/drivers/net/wireless/iwlwifi/iwl-debug.h b/drivers/net/wireless/iwlwifi/iwl-debug.h
index c60724c21db8..58384805a494 100644
--- a/drivers/net/wireless/iwlwifi/iwl-debug.h
+++ b/drivers/net/wireless/iwlwifi/iwl-debug.h
@@ -30,37 +30,35 @@
 #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, \
+do { if (priv->debug_level & (level)) \
+  dev_printk(KERN_ERR, &(priv->hw->wiphy->dev), "%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, \
+do { if ((priv->debug_level & (level)) && net_ratelimit()) \
+  dev_printk(KERN_ERR, &(priv->hw->wiphy->dev), "%c %s " fmt, \
 	 in_interrupt() ? 'I' : 'U', __FUNCTION__ , ## args); } while (0)
 
-static inline void iwl_print_hex_dump(int level, void *p, u32 len)
-{
-	if (!(iwl_debug_level & level))
-		return;
-
-	print_hex_dump(KERN_DEBUG, "iwl data: ", DUMP_PREFIX_OFFSET, 16, 1,
-			p, len, 1);
-}
-
 #ifdef CONFIG_IWLWIFI_DEBUGFS
 struct iwl_debugfs {
 	const char *name;
 	struct dentry *dir_drv;
 	struct dentry *dir_data;
-	struct dir_data_files{
+	struct dentry *dir_rf;
+	struct dir_data_files {
 		struct dentry *file_sram;
+		struct dentry *file_eeprom;
 		struct dentry *file_stations;
 		struct dentry *file_rx_statistics;
 		struct dentry *file_tx_statistics;
+		struct dentry *file_log_event;
 	} dbgfs_data_files;
+	struct dir_rf_files {
+		struct dentry *file_disable_sensitivity;
+		struct dentry *file_disable_chain_noise;
+		struct dentry *file_disable_tx_power;
+	} dbgfs_rf_files;
 	u32 sram_offset;
 	u32 sram_len;
 };
@@ -76,9 +74,6 @@ static inline void IWL_DEBUG(int level, const char *fmt, ...)
 static inline void IWL_DEBUG_LIMIT(int level, const char *fmt, ...)
 {
 }
-static inline void iwl_print_hex_dump(int level, void *p, u32 len)
-{
-}
 #endif				/* CONFIG_IWLWIFI_DEBUG */
 
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-debugfs.c b/drivers/net/wireless/iwlwifi/iwl-debugfs.c
index 9a30e1df311d..ed948dc59b3d 100644
--- a/drivers/net/wireless/iwlwifi/iwl-debugfs.c
+++ b/drivers/net/wireless/iwlwifi/iwl-debugfs.c
@@ -34,7 +34,7 @@
 #include <net/mac80211.h>
 
 
-#include "iwl-4965.h"
+#include "iwl-dev.h"
 #include "iwl-debug.h"
 #include "iwl-core.h"
 #include "iwl-io.h"
@@ -55,6 +55,13 @@
 		goto err;                                               \
 } while (0)
 
+#define DEBUGFS_ADD_BOOL(name, parent, ptr) do {                        \
+	dbgfs->dbgfs_##parent##_files.file_##name =                     \
+	debugfs_create_bool(#name, 0644, dbgfs->dir_##parent, ptr);     \
+	if (IS_ERR(dbgfs->dbgfs_##parent##_files.file_##name))          \
+		goto err;                                               \
+} while (0)
+
 #define DEBUGFS_REMOVE(name)  do {              \
 	debugfs_remove(name);                   \
 	name = NULL;                            \
@@ -85,6 +92,14 @@ static const struct file_operations iwl_dbgfs_##name##_ops = {          \
 	.open = iwl_dbgfs_open_file_generic,                    	\
 };
 
+#define DEBUGFS_WRITE_FILE_OPS(name)                                    \
+	DEBUGFS_WRITE_FUNC(name);                                       \
+static const struct file_operations iwl_dbgfs_##name##_ops = {          \
+	.write = iwl_dbgfs_##name##_write,                              \
+	.open = iwl_dbgfs_open_file_generic,                    	\
+};
+
+
 #define DEBUGFS_READ_WRITE_FILE_OPS(name)                               \
 	DEBUGFS_READ_FUNC(name);                                        \
 	DEBUGFS_WRITE_FUNC(name);                                       \
@@ -206,7 +221,7 @@ static ssize_t iwl_dbgfs_stations_read(struct file *file, char __user *user_buf,
 					size_t count, loff_t *ppos)
 {
 	struct iwl_priv *priv = (struct iwl_priv *)file->private_data;
-	struct iwl4965_station_entry *station;
+	struct iwl_station_entry *station;
 	int max_sta = priv->hw_params.max_stations;
 	char *buf;
 	int i, j, pos = 0;
@@ -240,21 +255,18 @@ static ssize_t iwl_dbgfs_stations_read(struct file *file, char __user *user_buf,
 			pos += scnprintf(buf + pos, bufsz - pos, "tid data:\n");
 			pos += scnprintf(buf + pos, bufsz - pos,
 					"seq_num\t\ttxq_id");
-#ifdef CONFIG_IWL4965_HT
 			pos += scnprintf(buf + pos, bufsz - pos,
 					"\tframe_count\twait_for_ba\t");
 			pos += scnprintf(buf + pos, bufsz - pos,
 					"start_idx\tbitmap0\t");
 			pos += scnprintf(buf + pos, bufsz - pos,
 					"bitmap1\trate_n_flags");
-#endif
 			pos += scnprintf(buf + pos, bufsz - pos, "\n");
 
 			for (j = 0; j < MAX_TID_COUNT; j++) {
 				pos += scnprintf(buf + pos, bufsz - pos,
 						"[%d]:\t\t%u", j,
 						station->tid[j].seq_number);
-#ifdef CONFIG_IWL4965_HT
 				pos += scnprintf(buf + pos, bufsz - pos,
 						"\t%u\t\t%u\t\t%u\t\t",
 						station->tid[j].agg.txq_id,
@@ -265,7 +277,6 @@ static ssize_t iwl_dbgfs_stations_read(struct file *file, char __user *user_buf,
 						station->tid[j].agg.start_idx,
 						(unsigned long long)station->tid[j].agg.bitmap,
 						station->tid[j].agg.rate_n_flags);
-#endif
 				pos += scnprintf(buf + pos, bufsz - pos, "\n");
 			}
 			pos += scnprintf(buf + pos, bufsz - pos, "\n");
@@ -277,8 +288,70 @@ static ssize_t iwl_dbgfs_stations_read(struct file *file, char __user *user_buf,
 	return ret;
 }
 
+static ssize_t iwl_dbgfs_eeprom_read(struct file *file,
+				       char __user *user_buf,
+				       size_t count,
+				       loff_t *ppos)
+{
+	ssize_t ret;
+	struct iwl_priv *priv = (struct iwl_priv *)file->private_data;
+	int pos = 0, ofs = 0, buf_size = 0;
+	const u8 *ptr;
+	char *buf;
+	size_t eeprom_len = priv->cfg->eeprom_size;
+	buf_size = 4 * eeprom_len + 256;
+
+	if (eeprom_len % 16) {
+		IWL_ERROR("EEPROM size is not multiple of 16.\n");
+		return -ENODATA;
+	}
+
+	/* 4 characters for byte 0xYY */
+	buf = kzalloc(buf_size, GFP_KERNEL);
+	if (!buf) {
+		IWL_ERROR("Can not allocate Buffer\n");
+		return -ENOMEM;
+	}
+
+	ptr = priv->eeprom;
+	for (ofs = 0 ; ofs < eeprom_len ; ofs += 16) {
+		pos += scnprintf(buf + pos, buf_size - pos, "0x%.4x ", ofs);
+		hex_dump_to_buffer(ptr + ofs, 16 , 16, 2, buf + pos,
+				   buf_size - pos, 0);
+		pos += strlen(buf);
+		if (buf_size - pos > 0)
+			buf[pos++] = '\n';
+	}
+
+	ret = simple_read_from_buffer(user_buf, count, ppos, buf, pos);
+	kfree(buf);
+	return ret;
+}
+
+static ssize_t iwl_dbgfs_log_event_write(struct file *file,
+					const char __user *user_buf,
+					size_t count, loff_t *ppos)
+{
+	struct iwl_priv *priv = file->private_data;
+	u32 event_log_flag;
+	char buf[8];
+	int buf_size;
+
+	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, "%d", &event_log_flag) != 1)
+		return -EFAULT;
+	if (event_log_flag == 1)
+		iwl_dump_nic_event_log(priv);
+
+	return count;
+}
 
 DEBUGFS_READ_WRITE_FILE_OPS(sram);
+DEBUGFS_WRITE_FILE_OPS(log_event);
+DEBUGFS_READ_FILE_OPS(eeprom);
 DEBUGFS_READ_FILE_OPS(stations);
 DEBUGFS_READ_FILE_OPS(rx_statistics);
 DEBUGFS_READ_FILE_OPS(tx_statistics);
@@ -290,6 +363,7 @@ DEBUGFS_READ_FILE_OPS(tx_statistics);
 int iwl_dbgfs_register(struct iwl_priv *priv, const char *name)
 {
 	struct iwl_debugfs *dbgfs;
+	struct dentry *phyd = priv->hw->wiphy->debugfsdir;
 
 	dbgfs = kzalloc(sizeof(struct iwl_debugfs), GFP_KERNEL);
 	if (!dbgfs) {
@@ -298,17 +372,23 @@ int iwl_dbgfs_register(struct iwl_priv *priv, const char *name)
 
 	priv->dbgfs = dbgfs;
 	dbgfs->name = name;
-	dbgfs->dir_drv = debugfs_create_dir(name, NULL);
+	dbgfs->dir_drv = debugfs_create_dir(name, phyd);
 	if (!dbgfs->dir_drv || IS_ERR(dbgfs->dir_drv)){
 		goto err;
 	}
 
 	DEBUGFS_ADD_DIR(data, dbgfs->dir_drv);
+	DEBUGFS_ADD_DIR(rf, dbgfs->dir_drv);
+	DEBUGFS_ADD_FILE(eeprom, data);
 	DEBUGFS_ADD_FILE(sram, data);
+	DEBUGFS_ADD_FILE(log_event, data);
 	DEBUGFS_ADD_FILE(stations, data);
 	DEBUGFS_ADD_FILE(rx_statistics, data);
 	DEBUGFS_ADD_FILE(tx_statistics, data);
-
+	DEBUGFS_ADD_BOOL(disable_sensitivity, rf, &priv->disable_sens_cal);
+	DEBUGFS_ADD_BOOL(disable_chain_noise, rf,
+			 &priv->disable_chain_noise_cal);
+	DEBUGFS_ADD_BOOL(disable_tx_power, rf, &priv->disable_tx_power_cal);
 	return 0;
 
 err:
@@ -327,11 +407,17 @@ void iwl_dbgfs_unregister(struct iwl_priv *priv)
 	if (!(priv->dbgfs))
 		return;
 
+	DEBUGFS_REMOVE(priv->dbgfs->dbgfs_data_files.file_eeprom);
 	DEBUGFS_REMOVE(priv->dbgfs->dbgfs_data_files.file_rx_statistics);
 	DEBUGFS_REMOVE(priv->dbgfs->dbgfs_data_files.file_tx_statistics);
 	DEBUGFS_REMOVE(priv->dbgfs->dbgfs_data_files.file_sram);
+	DEBUGFS_REMOVE(priv->dbgfs->dbgfs_data_files.file_log_event);
 	DEBUGFS_REMOVE(priv->dbgfs->dbgfs_data_files.file_stations);
 	DEBUGFS_REMOVE(priv->dbgfs->dir_data);
+	DEBUGFS_REMOVE(priv->dbgfs->dbgfs_rf_files.file_disable_sensitivity);
+	DEBUGFS_REMOVE(priv->dbgfs->dbgfs_rf_files.file_disable_chain_noise);
+	DEBUGFS_REMOVE(priv->dbgfs->dbgfs_rf_files.file_disable_tx_power);
+	DEBUGFS_REMOVE(priv->dbgfs->dir_rf);
 	DEBUGFS_REMOVE(priv->dbgfs->dir_drv);
 	kfree(priv->dbgfs);
 	priv->dbgfs = NULL;
@@ -339,3 +425,4 @@ void iwl_dbgfs_unregister(struct iwl_priv *priv)
 EXPORT_SYMBOL(iwl_dbgfs_unregister);
 
 
+
diff --git a/drivers/net/wireless/iwlwifi/iwl-4965.h b/drivers/net/wireless/iwlwifi/iwl-dev.h
index 581b98556c86..4d789e353e3a 100644
--- a/drivers/net/wireless/iwlwifi/iwl-4965.h
+++ b/drivers/net/wireless/iwlwifi/iwl-dev.h
@@ -24,13 +24,13 @@
  *
  *****************************************************************************/
 /*
- * Please use this file (iwl-4965.h) for driver implementation definitions.
- * Please use iwl-4965-commands.h for uCode API definitions.
+ * Please use this file (iwl-dev.h) for driver implementation definitions.
+ * Please use iwl-commands.h for uCode API definitions.
  * Please use iwl-4965-hw.h for hardware-related definitions.
  */
 
-#ifndef __iwl_4965_h__
-#define __iwl_4965_h__
+#ifndef __iwl_dev_h__
+#define __iwl_dev_h__
 
 #include <linux/pci.h> /* for struct pci_device_id */
 #include <linux/kernel.h>
@@ -44,16 +44,18 @@
 #include "iwl-prph.h"
 #include "iwl-debug.h"
 #include "iwl-led.h"
+#include "iwl-power.h"
 
 /* configuration for the iwl4965 */
 extern struct iwl_cfg iwl4965_agn_cfg;
+extern struct iwl_cfg iwl5300_agn_cfg;
+extern struct iwl_cfg iwl5100_agn_cfg;
+extern struct iwl_cfg iwl5350_agn_cfg;
+extern struct iwl_cfg iwl5100_bg_cfg;
+extern struct iwl_cfg iwl5100_abg_cfg;
 
-/* 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"
-
+/* CT-KILL constants */
+#define CT_KILL_THRESHOLD	110 /* in Celsius */
 
 /* Default noise level to report when noise measurement is not available.
  *   This may be because we're:
@@ -68,12 +70,6 @@ extern struct iwl_cfg iwl4965_agn_cfg;
  *   averages within an s8's (used in some apps) range of negative values. */
 #define IWL_NOISE_MEAS_NOT_AVAILABLE (-127)
 
-enum iwl4965_antenna {
-	IWL_ANTENNA_DIVERSITY,
-	IWL_ANTENNA_MAIN,
-	IWL_ANTENNA_AUX
-};
-
 /*
  * RTS threshold here is total size [2347] minus 4 FCS bytes
  * Per spec:
@@ -91,7 +87,7 @@ enum iwl4965_antenna {
 #define	DEFAULT_SHORT_RETRY_LIMIT 7U
 #define	DEFAULT_LONG_RETRY_LIMIT  4U
 
-struct iwl4965_rx_mem_buffer {
+struct iwl_rx_mem_buffer {
 	dma_addr_t dma_addr;
 	struct sk_buff *skb;
 	struct list_head list;
@@ -102,7 +98,7 @@ struct iwl4965_rx_mem_buffer {
  *
  * Contains common data for Rx and Tx queues
  */
-struct iwl4965_queue {
+struct iwl_queue {
 	int n_bd;              /* number of BDs in this queue */
 	int write_ptr;       /* 1-st empty entry (index) host_w*/
 	int read_ptr;         /* last used entry (index) host_r*/
@@ -118,13 +114,12 @@ struct iwl4965_queue {
 #define MAX_NUM_OF_TBS          (20)
 
 /* One for each TFD */
-struct iwl4965_tx_info {
-	struct ieee80211_tx_status status;
+struct iwl_tx_info {
 	struct sk_buff *skb[MAX_NUM_OF_TBS];
 };
 
 /**
- * struct iwl4965_tx_queue - Tx Queue for DMA
+ * struct iwl_tx_queue - Tx Queue for DMA
  * @q: generic Rx/Tx queue descriptor
  * @bd: base of circular buffer of TFDs
  * @cmd: array of command/Tx buffers
@@ -136,12 +131,12 @@ struct iwl4965_tx_info {
  * A Tx queue consists of circular buffer of BDs (a.k.a. TFDs, transmit frame
  * descriptors) and required locking structures.
  */
-struct iwl4965_tx_queue {
-	struct iwl4965_queue q;
-	struct iwl4965_tfd_frame *bd;
+struct iwl_tx_queue {
+	struct iwl_queue q;
+	struct iwl_tfd_frame *bd;
 	struct iwl_cmd *cmd;
 	dma_addr_t dma_addr_cmd;
-	struct iwl4965_tx_info *txb;
+	struct iwl_tx_info *txb;
 	int need_update;
 	int sched_retry;
 	int active;
@@ -158,50 +153,17 @@ struct iwl4965_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 iwl4965_channel_power_info {
-	struct iwl4965_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 iwl4965_scan_power_info {
-	struct iwl4965_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 */
-};
-
-/* For fat_extension_channel */
-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
-};
-
 /*
  * 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 iwl4965_channel_tgd_info tgd;
 	struct iwl4965_channel_tgh_info tgh;
-	struct iwl4965_eeprom_channel eeprom;	  /* EEPROM regulatory limit */
-	struct iwl4965_eeprom_channel fat_eeprom; /* EEPROM regulatory limit for
-						   * FAT channel */
+	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 */
@@ -214,11 +176,6 @@ struct iwl_channel_info {
 	u8 band_index;	  /* 0-4, maps channel to band1/2/3/4/5 */
 	enum ieee80211_band band;
 
-	/* 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 iwl4965_channel_power_info power_info[IWL4965_MAX_RATE];
-
 	/* 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) */
@@ -226,9 +183,6 @@ struct iwl_channel_info {
 	s8 fat_scan_power;	/* (dBm) eeprom, direct scans, any rate */
 	u8 fat_flags;		/* flags copied from EEPROM */
 	u8 fat_extension_channel; /* HT_IE_EXT_CHANNEL_* */
-
-	/* Radio/DSP gain settings for each scan rate, for directed scans. */
-	struct iwl4965_scan_power_info scan_pwr_info[IWL_NUM_SCAN_RATES];
 };
 
 struct iwl4965_clip_group {
@@ -252,29 +206,9 @@ struct iwl4965_clip_group {
 
 /* Power management (not Tx power) structures */
 
-struct iwl4965_power_vec_entry {
-	struct iwl4965_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 iwl4965_power_mgr {
-	spinlock_t lock;
-	struct iwl4965_power_vec_entry pwr_range_0[IWL_POWER_AC];
-	struct iwl4965_power_vec_entry pwr_range_1[IWL_POWER_AC];
-	u8 active_index;
-	u32 dtim_val;
+enum iwl_pwr_src {
+	IWL_PWR_SRC_VMAIN,
+	IWL_PWR_SRC_VAUX,
 };
 
 #define IEEE80211_DATA_LEN              2304
@@ -282,7 +216,7 @@ struct iwl4965_power_mgr {
 #define IEEE80211_HLEN                  (IEEE80211_4ADDR_LEN)
 #define IEEE80211_FRAME_LEN             (IEEE80211_DATA_LEN + IEEE80211_HLEN)
 
-struct iwl4965_frame {
+struct iwl_frame {
 	union {
 		struct ieee80211_hdr frame;
 		struct iwl4965_tx_beacon_cmd beacon;
@@ -328,6 +262,8 @@ struct iwl_cmd_meta {
 
 } __attribute__ ((packed));
 
+#define IWL_CMD_MAX_PAYLOAD 320
+
 /**
  * struct iwl_cmd
  *
@@ -339,8 +275,8 @@ struct iwl_cmd {
 	struct iwl_cmd_meta meta;	/* driver data */
 	struct iwl_cmd_header hdr;	/* uCode API */
 	union {
-		struct iwl4965_addsta_cmd addsta;
-		struct iwl4965_led_cmd led;
+		struct iwl_addsta_cmd addsta;
+		struct iwl_led_cmd led;
 		u32 flags;
 		u8 val8;
 		u16 val16;
@@ -348,12 +284,13 @@ struct iwl_cmd {
 		struct iwl4965_bt_cmd bt;
 		struct iwl4965_rxon_time_cmd rxon_time;
 		struct iwl4965_powertable_cmd powertable;
-		struct iwl4965_qosparam_cmd qosparam;
-		struct iwl4965_tx_cmd tx;
+		struct iwl_qosparam_cmd qosparam;
+		struct iwl_tx_cmd tx;
 		struct iwl4965_tx_beacon_cmd tx_beacon;
 		struct iwl4965_rxon_assoc_cmd rxon_assoc;
+		struct iwl_rem_sta_cmd rm_sta;
 		u8 *indirect;
-		u8 payload[360];
+		u8 payload[IWL_CMD_MAX_PAYLOAD];
 	} __attribute__ ((packed)) cmd;
 } __attribute__ ((packed));
 
@@ -378,7 +315,7 @@ struct iwl_host_cmd {
 #define SUP_RATE_11G_MAX_NUM_CHANNELS  12
 
 /**
- * struct iwl4965_rx_queue - Rx queue
+ * 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
@@ -387,13 +324,13 @@ struct iwl_host_cmd {
  * @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 iwl4965_rx_mem_buffers
+ * NOTE:  rx_free and rx_used are used as a FIFO for iwl_rx_mem_buffers
  */
-struct iwl4965_rx_queue {
+struct iwl_rx_queue {
 	__le32 *bd;
 	dma_addr_t dma_addr;
-	struct iwl4965_rx_mem_buffer pool[RX_QUEUE_SIZE + RX_FREE_BUFFERS];
-	struct iwl4965_rx_mem_buffer *queue[RX_QUEUE_SIZE];
+	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;
@@ -419,9 +356,8 @@ struct iwl4965_rx_queue {
 #define IWL_INVALID_RATE     0xFF
 #define IWL_INVALID_VALUE    -1
 
-#ifdef CONFIG_IWL4965_HT
 /**
- * struct iwl4965_ht_agg -- aggregation status while waiting for block-ack
+ * struct iwl_ht_agg -- aggregation status while waiting for block-ack
  * @txq_id: Tx queue used for Tx attempt
  * @frame_count: # frames attempted by Tx command
  * @wait_for_ba: Expect block-ack before next Tx reply
@@ -434,7 +370,7 @@ struct iwl4965_rx_queue {
  * for block ack (REPLY_COMPRESSED_BA).  This struct stores tx reply info
  * until block ack arrives.
  */
-struct iwl4965_ht_agg {
+struct iwl_ht_agg {
 	u16 txq_id;
 	u16 frame_count;
 	u16 wait_for_ba;
@@ -448,21 +384,17 @@ struct iwl4965_ht_agg {
 	u8 state;
 };
 
-#endif /* CONFIG_IWL4965_HT */
 
-struct iwl4965_tid_data {
+struct iwl_tid_data {
 	u16 seq_number;
 	u16 tfds_in_queue;
-#ifdef CONFIG_IWL4965_HT
-	struct iwl4965_ht_agg agg;
-#endif /* CONFIG_IWL4965_HT */
+	struct iwl_ht_agg agg;
 };
 
-struct iwl4965_hw_key {
+struct iwl_hw_key {
 	enum ieee80211_key_alg alg;
 	int keylen;
 	u8 keyidx;
-	struct ieee80211_key_conf *conf;
 	u8 key[32];
 };
 
@@ -474,7 +406,6 @@ union iwl4965_ht_rate_supp {
 	};
 };
 
-#ifdef CONFIG_IWL4965_HT
 #define CFG_HT_RX_AMPDU_FACTOR_DEF  (0x3)
 #define CFG_HT_MPDU_DENSITY_2USEC   (0x5)
 #define CFG_HT_MPDU_DENSITY_DEF CFG_HT_MPDU_DENSITY_2USEC
@@ -497,9 +428,8 @@ struct iwl_ht_info {
 	u8 ht_protection;
 	u8 non_GF_STA_present;
 };
-#endif				/*CONFIG_IWL4965_HT */
 
-union iwl4965_qos_capabity {
+union iwl_qos_capabity {
 	struct {
 		u8 edca_count:4;	/* bit 0-3 */
 		u8 q_ack:1;		/* bit 4 */
@@ -520,22 +450,22 @@ union iwl4965_qos_capabity {
 };
 
 /* QoS structures */
-struct iwl4965_qos_info {
+struct iwl_qos_info {
 	int qos_enable;
 	int qos_active;
-	union iwl4965_qos_capabity qos_cap;
-	struct iwl4965_qosparam_cmd def_qos_parm;
+	union iwl_qos_capabity qos_cap;
+	struct iwl_qosparam_cmd def_qos_parm;
 };
 
 #define STA_PS_STATUS_WAKE             0
 #define STA_PS_STATUS_SLEEP            1
 
-struct iwl4965_station_entry {
-	struct iwl4965_addsta_cmd sta;
-	struct iwl4965_tid_data tid[MAX_TID_COUNT];
+struct iwl_station_entry {
+	struct iwl_addsta_cmd sta;
+	struct iwl_tid_data tid[MAX_TID_COUNT];
 	u8 used;
 	u8 ps_status;
-	struct iwl4965_hw_key keyinfo;
+	struct iwl_hw_key keyinfo;
 };
 
 /* one for each uCode image (inst/data, boot/init/runtime) */
@@ -546,7 +476,7 @@ struct fw_desc {
 };
 
 /* uCode file layout */
-struct iwl4965_ucode {
+struct iwl_ucode {
 	__le32 ver;		/* major/minor/subminor */
 	__le32 inst_size;	/* bytes of runtime instructions */
 	__le32 data_size;	/* bytes of runtime data */
@@ -556,8 +486,6 @@ struct iwl4965_ucode {
 	u8 data[0];		/* data in same order as "size" elements */
 };
 
-#define IWL_IBSS_MAC_HASH_SIZE 32
-
 struct iwl4965_ibss_seq {
 	u8 mac[ETH_ALEN];
 	u16 seq_num;
@@ -566,20 +494,52 @@ struct iwl4965_ibss_seq {
 	struct list_head list;
 };
 
+struct iwl_sensitivity_ranges {
+	u16 min_nrg_cck;
+	u16 max_nrg_cck;
+
+	u16 nrg_th_cck;
+	u16 nrg_th_ofdm;
+
+	u16 auto_corr_min_ofdm;
+	u16 auto_corr_min_ofdm_mrc;
+	u16 auto_corr_min_ofdm_x1;
+	u16 auto_corr_min_ofdm_mrc_x1;
+
+	u16 auto_corr_max_ofdm;
+	u16 auto_corr_max_ofdm_mrc;
+	u16 auto_corr_max_ofdm_x1;
+	u16 auto_corr_max_ofdm_mrc_x1;
+
+	u16 auto_corr_max_cck;
+	u16 auto_corr_max_cck_mrc;
+	u16 auto_corr_min_cck;
+	u16 auto_corr_min_cck_mrc;
+};
+
+
+#define IWL_FAT_CHANNEL_52 BIT(IEEE80211_BAND_5GHZ)
+
 /**
  * struct iwl_hw_params
  * @max_txq_num: Max # Tx queues supported
- * @tx_cmd_len: Size of Tx command (but not including frame itself)
- * @tx_ant_num: Number of TX antennas
+ * @tx/rx_chains_num: Number of TX/RX chains
+ * @valid_tx/rx_ant: usable antennas
  * @max_rxq_size: Max # Rx frames in Rx queue (must be power-of-2)
- * @rx_buffer_size:
  * @max_rxq_log: Log-base-2 of max_rxq_size
+ * @rx_buf_size: Rx buffer size
  * @max_stations:
  * @bcast_sta_id:
+ * @fat_channel: is 40MHz width possible in band 2.4
+ * BIT(IEEE80211_BAND_5GHZ) BIT(IEEE80211_BAND_5GHZ)
+ * @sw_crypto: 0 for hw, 1 for sw
+ * @max_xxx_size: for ucode uses
+ * @ct_kill_threshold: temperature threshold
+ * @struct iwl_sensitivity_ranges: range of sensitivity values
+ * @first_ampdu_q: first HW queue available for ampdu
  */
 struct iwl_hw_params {
 	u16 max_txq_num;
-	u16 tx_cmd_len;
 	u8  tx_chains_num;
 	u8  rx_chains_num;
 	u8  valid_tx_ant;
@@ -590,10 +550,18 @@ struct iwl_hw_params {
 	u32 max_pkt_size;
 	u8  max_stations;
 	u8  bcast_sta_id;
+	u8 fat_channel;
+	u8 sw_crypto;
+	u32 max_inst_size;
+	u32 max_data_size;
+	u32 max_bsm_size;
+	u32 ct_kill_threshold; /* value in hw-dependent units */
+	const struct iwl_sensitivity_ranges *sens;
+	u8 first_ampdu_q;
 };
 
-#define HT_SHORT_GI_20MHZ_ONLY          (1 << 0)
-#define HT_SHORT_GI_40MHZ_ONLY          (1 << 1)
+#define HT_SHORT_GI_20MHZ	(1 << 0)
+#define HT_SHORT_GI_40MHZ	(1 << 1)
 
 
 #define IWL_RX_HDR(x) ((struct iwl4965_rx_frame_hdr *)(\
@@ -612,51 +580,18 @@ struct iwl_hw_params {
  * for use by iwl-*.c
  *
  *****************************************************************************/
-struct iwl4965_addsta_cmd;
-extern int iwl4965_send_add_station(struct iwl_priv *priv,
-				struct iwl4965_addsta_cmd *sta, u8 flags);
-extern u8 iwl4965_add_station_flags(struct iwl_priv *priv, const u8 *addr,
-			  int is_ap, u8 flags, void *ht_data);
-extern int iwl4965_is_network_packet(struct iwl_priv *priv,
-				 struct ieee80211_hdr *header);
-extern int iwl4965_power_init_handle(struct iwl_priv *priv);
-extern void iwl4965_handle_data_packet_monitor(struct iwl_priv *priv,
-					   struct iwl4965_rx_mem_buffer *rxb,
-					   void *data, short len,
-					   struct ieee80211_rx_status *stats,
-					   u16 phy_flags);
-extern int iwl4965_is_duplicate_packet(struct iwl_priv *priv,
-				       struct ieee80211_hdr *header);
-extern int iwl4965_rx_queue_alloc(struct iwl_priv *priv);
-extern void iwl4965_rx_queue_reset(struct iwl_priv *priv,
-			       struct iwl4965_rx_queue *rxq);
-extern int iwl4965_calc_db_from_ratio(int sig_ratio);
-extern int iwl4965_calc_sig_qual(int rssi_dbm, int noise_dbm);
-extern int iwl4965_tx_queue_init(struct iwl_priv *priv,
-			     struct iwl4965_tx_queue *txq, int count, u32 id);
-extern void iwl4965_rx_replenish(void *data);
-extern void iwl4965_tx_queue_free(struct iwl_priv *priv, struct iwl4965_tx_queue *txq);
+struct iwl_addsta_cmd;
+extern int iwl_send_add_sta(struct iwl_priv *priv,
+			    struct iwl_addsta_cmd *sta, u8 flags);
+u8 iwl_add_station_flags(struct iwl_priv *priv, const u8 *addr, int is_ap,
+			 u8 flags, struct ieee80211_ht_info *ht_info);
 extern unsigned int iwl4965_fill_beacon_frame(struct iwl_priv *priv,
 					struct ieee80211_hdr *hdr,
 					const u8 *dest, int left);
-extern int iwl4965_rx_queue_update_write_ptr(struct iwl_priv *priv,
-					 struct iwl4965_rx_queue *q);
-extern void iwl4965_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);
-int iwl4965_init_geos(struct iwl_priv *priv);
-void iwl4965_free_geos(struct iwl_priv *priv);
-
-extern const u8 iwl4965_broadcast_addr[ETH_ALEN];
-int iwl4965_enqueue_hcmd(struct iwl_priv *priv, struct iwl_host_cmd *cmd);
+extern void iwl4965_update_chain_flags(struct iwl_priv *priv);
+int iwl4965_set_pwr_src(struct iwl_priv *priv, enum iwl_pwr_src src);
 
-/*
- * Currently used by iwl-3945-rs... look at restructuring so that it doesn't
- * call this... todo... fix that.
-*/
-extern u8 iwl4965_sync_station(struct iwl_priv *priv, int sta_id,
-			   u16 tx_rate, u8 flags);
+extern const u8 iwl_bcast_addr[ETH_ALEN];
 
 /******************************************************************************
  *
@@ -674,96 +609,51 @@ extern u8 iwl4965_sync_station(struct iwl_priv *priv, int sta_id,
  * iwl4965_mac_     <-- mac80211 callback
  *
  ****************************************************************************/
-extern void iwl4965_hw_rx_handler_setup(struct iwl_priv *priv);
-extern void iwl4965_hw_setup_deferred_work(struct iwl_priv *priv);
-extern void iwl4965_hw_cancel_deferred_work(struct iwl_priv *priv);
-extern int iwl4965_hw_rxq_stop(struct iwl_priv *priv);
-extern int iwl4965_hw_set_hw_params(struct iwl_priv *priv);
-extern int iwl4965_hw_nic_init(struct iwl_priv *priv);
-extern int iwl4965_hw_nic_stop_master(struct iwl_priv *priv);
-extern void iwl4965_hw_txq_ctx_free(struct iwl_priv *priv);
-extern void iwl4965_hw_txq_ctx_stop(struct iwl_priv *priv);
-extern int iwl4965_hw_nic_reset(struct iwl_priv *priv);
-extern int iwl4965_hw_txq_attach_buf_to_tfd(struct iwl_priv *priv, void *tfd,
-					dma_addr_t addr, u16 len);
-extern int iwl4965_hw_txq_free_tfd(struct iwl_priv *priv, struct iwl4965_tx_queue *txq);
-extern int iwl4965_hw_get_temperature(struct iwl_priv *priv);
-extern int iwl4965_hw_tx_queue_init(struct iwl_priv *priv,
-				struct iwl4965_tx_queue *txq);
+extern int iwl_rxq_stop(struct iwl_priv *priv);
+extern void iwl_txq_ctx_stop(struct iwl_priv *priv);
 extern unsigned int iwl4965_hw_get_beacon_cmd(struct iwl_priv *priv,
-				 struct iwl4965_frame *frame, u8 rate);
-extern int iwl4965_hw_get_rx_read(struct iwl_priv *priv);
-extern void iwl4965_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 iwl4965_hw_reg_send_txpower(struct iwl_priv *priv);
-extern int iwl4965_hw_reg_set_txpower(struct iwl_priv *priv, s8 power);
-extern void iwl4965_hw_rx_statistics(struct iwl_priv *priv,
-				 struct iwl4965_rx_mem_buffer *rxb);
+				 struct iwl_frame *frame, u8 rate);
 extern void iwl4965_disable_events(struct iwl_priv *priv);
-extern int iwl4965_get_temperature(const struct iwl_priv *priv);
-
-/**
- * iwl4965_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 iwl4965_hw_find_station(struct iwl_priv *priv, const u8 *bssid);
 
 extern int iwl4965_hw_channel_switch(struct iwl_priv *priv, u16 channel);
-extern int iwl4965_tx_queue_reclaim(struct iwl_priv *priv, int txq_id, int index);
-extern int iwl4965_queue_space(const struct iwl4965_queue *q);
+extern int iwl_queue_space(const struct iwl_queue *q);
+static inline int iwl_queue_used(const struct iwl_queue *q, int i)
+{
+	return q->write_ptr > q->read_ptr ?
+		(i >= q->read_ptr && i < q->write_ptr) :
+		!(i < q->read_ptr && i >= q->write_ptr);
+}
+
+
+static inline u8 get_cmd_index(struct iwl_queue *q, u32 index, int is_huge)
+{
+	/* This is for scan command, the big buffer at end of command array */
+	if (is_huge)
+		return q->n_window;	/* must be power of 2 */
+
+	/* Otherwise, use normal size buffers */
+	return index & (q->n_window - 1);
+}
+
+
 struct iwl_priv;
 
-extern void iwl4965_radio_kill_sw(struct iwl_priv *priv, int disable_radio);
 /*
  * Forward declare iwl-4965.c functions for iwl-base.c
  */
-extern int iwl4965_tx_queue_update_wr_ptr(struct iwl_priv *priv,
-					  struct iwl4965_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_chain(struct iwl_priv *priv);
-extern int iwl4965_alive_notify(struct iwl_priv *priv);
-extern void iwl4965_update_rate_scaling(struct iwl_priv *priv, u8 mode);
-extern void iwl4965_chain_noise_reset(struct iwl_priv *priv);
-extern void iwl4965_init_sensitivity(struct iwl_priv *priv, u8 flags,
-				     u8 force);
 extern void iwl4965_rf_kill_ct_config(struct iwl_priv *priv);
-extern void iwl4965_hwrate_to_tx_control(struct iwl_priv *priv,
-					 u32 rate_n_flags,
-					 struct ieee80211_tx_control *control);
-
-#ifdef CONFIG_IWL4965_HT
-void iwl4965_init_ht_hw_capab(struct iwl_priv *priv,
-			      struct ieee80211_ht_info *ht_info,
-			      enum ieee80211_band band);
-void iwl4965_set_rxon_ht(struct iwl_priv *priv,
-			 struct iwl_ht_info *ht_info);
-void iwl4965_set_ht_add_station(struct iwl_priv *priv, u8 index,
-				struct ieee80211_ht_info *sta_ht_inf);
+
 int iwl4965_mac_ampdu_action(struct ieee80211_hw *hw,
 				    enum ieee80211_ampdu_mlme_action action,
 				    const u8 *addr, u16 tid, u16 *ssn);
 int iwl4965_check_empty_hw_queue(struct iwl_priv *priv, int sta_id,
 					u8 tid, int txq_id);
-#else
-static inline void iwl4965_init_ht_hw_capab(struct iwl_priv *priv,
-					    struct ieee80211_ht_info *ht_info,
-					    enum ieee80211_band band) {}
 
-#endif /*CONFIG_IWL4965_HT */
 /* Structures, enum, and defines specific to the 4965 */
 
-#define IWL4965_KW_SIZE 0x1000	/*4k */
+#define IWL_KW_SIZE 0x1000	/*4k */
 
-struct iwl4965_kw {
+struct iwl_kw {
 	dma_addr_t dma_addr;
 	void *v_addr;
 	size_t size;
@@ -782,13 +672,8 @@ struct iwl4965_kw {
 #define IWL_OPERATION_MODE_MIXED    2
 #define IWL_OPERATION_MODE_20MHZ    3
 
-#define IWL_EXT_CHANNEL_OFFSET_NONE      0
-#define IWL_EXT_CHANNEL_OFFSET_ABOVE     1
-#define IWL_EXT_CHANNEL_OFFSET_RESERVE1  2
-#define IWL_EXT_CHANNEL_OFFSET_BELOW     3
-
-#define NRG_NUM_PREV_STAT_L     20
-#define NUM_RX_CHAINS           (3)
+#define IWL_TX_CRC_SIZE 4
+#define IWL_TX_DELIMITER_SIZE 4
 
 #define TX_POWER_IWL_ILLEGAL_VOLTAGE -10000
 
@@ -802,7 +687,6 @@ struct iwl4965_lq_mngr {
 	unsigned long stamp_last;
 	u32 flush_time;
 	u32 tx_packets;
-	u8 lq_ready;
 };
 
 /* Sensitivity and chain noise calibration */
@@ -818,23 +702,8 @@ struct iwl4965_lq_mngr {
 #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
 
@@ -853,6 +722,9 @@ struct iwl4965_lq_mngr {
 #define IN_BAND_FILTER			0xFF
 #define MIN_AVERAGE_NOISE_MAX_VALUE	0xFFFFFFFF
 
+#define NRG_NUM_PREV_STAT_L     20
+#define NUM_RX_CHAINS           3
+
 enum iwl4965_false_alarm_state {
 	IWL_FA_TOO_MANY = 0,
 	IWL_FA_TOO_FEW = 1,
@@ -865,11 +737,6 @@ enum iwl4965_chain_noise_state {
 	IWL_CHAIN_NOISE_CALIBRATED = 2,
 };
 
-enum iwl4965_sensitivity_state {
-	IWL_SENS_CALIB_ALLOWED = 0,
-	IWL_SENS_CALIB_NEED_REINIT = 1,
-};
-
 enum iwl4965_calib_enabled_state {
 	IWL_CALIB_DISABLED = 0,  /* must be 0 */
 	IWL_CALIB_ENABLED = 1,
@@ -884,8 +751,23 @@ struct statistics_general_data {
 	u32 beacon_energy_c;
 };
 
+struct iwl_calib_results {
+	void *tx_iq_res;
+	void *tx_iq_perd_res;
+	void *lo_res;
+	u32 tx_iq_res_len;
+	u32 tx_iq_perd_res_len;
+	u32 lo_res_len;
+};
+
+enum ucode_type {
+	UCODE_NONE = 0,
+	UCODE_INIT,
+	UCODE_RT
+};
+
 /* Sensitivity calib data */
-struct iwl4965_sensitivity_data {
+struct iwl_sensitivity_data {
 	u32 auto_corr_ofdm;
 	u32 auto_corr_ofdm_mrc;
 	u32 auto_corr_ofdm_x1;
@@ -909,12 +791,10 @@ struct iwl4965_sensitivity_data {
 	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 iwl4965_chain_noise_data {
+struct iwl_chain_noise_data {
 	u8 state;
 	u16 beacon_count;
 	u32 chain_noise_a;
@@ -960,7 +840,7 @@ struct iwl_priv {
 	bool add_radiotap;
 
 	void (*rx_handlers[REPLY_MAX])(struct iwl_priv *priv,
-				       struct iwl4965_rx_mem_buffer *rxb);
+				       struct iwl_rx_mem_buffer *rxb);
 
 	struct ieee80211_supported_band bands[IEEE80211_NUM_BANDS];
 
@@ -985,6 +865,9 @@ struct iwl_priv {
 	s32 temperature;	/* degrees Kelvin */
 	s32 last_temperature;
 
+	/* init calibration results */
+	struct iwl_calib_results calib_results;
+
 	/* Scan related variables */
 	unsigned long last_scan_jiffies;
 	unsigned long next_scan_jiffies;
@@ -995,7 +878,8 @@ struct iwl_priv {
 	int one_direct_scan;
 	u8 direct_ssid_len;
 	u8 direct_ssid[IW_ESSID_MAX_SIZE];
-	struct iwl4965_scan_cmd *scan;
+	struct iwl_scan_cmd *scan;
+	u32 scan_tx_ant[IEEE80211_NUM_BANDS];
 
 	/* spinlock */
 	spinlock_t lock;	/* protect general shared data */
@@ -1007,6 +891,9 @@ struct iwl_priv {
 
 	/* pci hardware address support */
 	void __iomem *hw_base;
+	u32  hw_rev;
+	u32  hw_wa_rev;
+	u8   rev_id;
 
 	/* uCode images, save to reload in case of failure */
 	struct fw_desc ucode_code;	/* runtime inst */
@@ -1015,6 +902,8 @@ struct iwl_priv {
 	struct fw_desc ucode_init;	/* initialization inst */
 	struct fw_desc ucode_init_data;	/* initialization data */
 	struct fw_desc ucode_boot;	/* bootstrap inst */
+	enum ucode_type ucode_type;
+	u8 ucode_write_complete;	/* the image write is complete */
 
 
 	struct iwl4965_rxon_time_cmd rxon_timing;
@@ -1023,22 +912,22 @@ struct iwl_priv {
 	 * changed via explicit cast within the
 	 * routines that actually update the physical
 	 * hardware */
-	const struct iwl4965_rxon_cmd active_rxon;
-	struct iwl4965_rxon_cmd staging_rxon;
+	const struct iwl_rxon_cmd active_rxon;
+	struct iwl_rxon_cmd staging_rxon;
 
 	int error_recovering;
-	struct iwl4965_rxon_cmd recovery_rxon;
+	struct iwl_rxon_cmd recovery_rxon;
 
 	/* 1st responses from initialize and runtime uCode images.
 	 * 4965's initialize alive response contains some calibration data. */
-	struct iwl4965_init_alive_resp card_alive_init;
-	struct iwl4965_alive_resp card_alive;
+	struct iwl_init_alive_resp card_alive_init;
+	struct iwl_alive_resp card_alive;
 #ifdef CONFIG_IWLWIFI_RFKILL
-	struct iwl_rfkill_mngr rfkill_mngr;
+	struct rfkill *rfkill;
 #endif
 
 #ifdef CONFIG_IWLWIFI_LEDS
-	struct iwl4965_led led[IWL_LED_TRG_MAX];
+	struct iwl_led led[IWL_LED_TRG_MAX];
 	unsigned long last_blink_time;
 	u8 last_blink_rate;
 	u8 allow_blinking;
@@ -1050,17 +939,12 @@ struct iwl_priv {
 
 	u8 assoc_station_added;
 	u8 use_ant_b_for_management_frame;	/* Tx antenna selection */
-	u8 valid_antenna;	/* Bit mask of antennas actually connected */
-#ifdef CONFIG_IWL4965_SENSITIVITY
-	struct iwl4965_sensitivity_data sensitivity_data;
-	struct iwl4965_chain_noise_data chain_noise_data;
 	u8 start_calib;
+	struct iwl_sensitivity_data sensitivity_data;
+	struct iwl_chain_noise_data chain_noise_data;
 	__le16 sensitivity_tbl[HD_TABLE_SIZE];
-#endif /*CONFIG_IWL4965_SENSITIVITY*/
 
-#ifdef CONFIG_IWL4965_HT
 	struct iwl_ht_info current_ht_config;
-#endif
 	u8 last_phy_res[100];
 
 	/* Rate scaling data */
@@ -1075,10 +959,10 @@ struct iwl_priv {
 	int activity_timer_active;
 
 	/* Rx and Tx DMA processing queues */
-	struct iwl4965_rx_queue rxq;
-	struct iwl4965_tx_queue txq[IWL_MAX_NUM_QUEUES];
+	struct iwl_rx_queue rxq;
+	struct iwl_tx_queue txq[IWL_MAX_NUM_QUEUES];
 	unsigned long txq_ctx_active_msk;
-	struct iwl4965_kw kw;	/* keep warm address */
+	struct iwl_kw kw;	/* keep warm address */
 	u32 scd_base_addr;	/* scheduler sram base address */
 
 	unsigned long status;
@@ -1092,9 +976,9 @@ struct iwl_priv {
 		u64 bytes;
 	} tx_stats[3], rx_stats[3];
 
-	struct iwl4965_power_mgr power_data;
+	struct iwl_power_mgr power_data;
 
-	struct iwl4965_notif_statistics statistics;
+	struct iwl_notif_statistics statistics;
 	unsigned long last_statistics_time;
 
 	/* context information */
@@ -1111,7 +995,7 @@ struct iwl_priv {
 	/*station table variables */
 	spinlock_t sta_lock;
 	int num_stations;
-	struct iwl4965_station_entry stations[IWL_STATION_COUNT];
+	struct iwl_station_entry stations[IWL_STATION_COUNT];
 	struct iwl_wep_key wep_keys[WEP_KEYS_MAX];
 	u8 default_wep_key;
 	u8 key_mapping_key;
@@ -1122,22 +1006,13 @@ struct iwl_priv {
 
 	u8 mac80211_registered;
 
-	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;
-
-	/* Hash table for finding stations in IBSS network */
-	struct list_head ibss_mac_hash[IWL_IBSS_MAC_HASH_SIZE];
-
 	/* eeprom */
-	struct iwl4965_eeprom eeprom;
+	u8 *eeprom;
+	struct iwl_eeprom_calib_info *calib_info;
 
 	enum ieee80211_if_types iw_mode;
 
@@ -1151,6 +1026,7 @@ struct iwl_priv {
 	struct iwl_hw_params hw_params;
 	/* driver/uCode shared Tx Byte Counts and Rx status */
 	void *shared_virt;
+	int rb_closed_offset;
 	/* Physical Pointer to Tx Byte Counts and Rx status */
 	dma_addr_t shared_phys;
 
@@ -1160,7 +1036,7 @@ struct iwl_priv {
 	u16 assoc_capability;
 	u8 ps_mode;
 
-	struct iwl4965_qos_info qos_data;
+	struct iwl_qos_info qos_data;
 
 	struct workqueue_struct *workqueue;
 
@@ -1176,20 +1052,16 @@ struct iwl_priv {
 	struct work_struct report_work;
 	struct work_struct request_scan;
 	struct work_struct beacon_update;
+	struct work_struct set_monitor;
 
 	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;
+	/* TX Power */
+	s8 tx_power_user_lmt;
+	s8 tx_power_channel_lmt;
 
 #ifdef CONFIG_PM
 	u32 pm_state[16];
@@ -1197,6 +1069,7 @@ struct iwl_priv {
 
 #ifdef CONFIG_IWLWIFI_DEBUG
 	/* debugging info */
+	u32 debug_level;
 	u32 framecnt_to_us;
 	atomic_t restrict_refcnt;
 #ifdef CONFIG_IWLWIFI_DEBUGFS
@@ -1206,12 +1079,40 @@ struct iwl_priv {
 #endif /* CONFIG_IWLWIFI_DEBUG */
 
 	struct work_struct txpower_work;
-#ifdef CONFIG_IWL4965_SENSITIVITY
-	struct work_struct sensitivity_work;
-#endif
+	u32 disable_sens_cal;
+	u32 disable_chain_noise_cal;
+	u32 disable_tx_power_cal;
+	struct work_struct run_time_calib_work;
 	struct timer_list statistics_periodic;
 }; /*iwl_priv */
 
+static inline void iwl_txq_ctx_activate(struct iwl_priv *priv, int txq_id)
+{
+	set_bit(txq_id, &priv->txq_ctx_active_msk);
+}
+
+static inline void iwl_txq_ctx_deactivate(struct iwl_priv *priv, int txq_id)
+{
+	clear_bit(txq_id, &priv->txq_ctx_active_msk);
+}
+
+#ifdef CONFIG_IWLWIFI_DEBUG
+const char *iwl_get_tx_fail_reason(u32 status);
+#else
+static inline const char *iwl_get_tx_fail_reason(u32 status) { return ""; }
+#endif
+
+
+static inline 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 int iwl_is_associated(struct iwl_priv *priv)
 {
 	return (priv->active_rxon.filter_flags & RXON_FILTER_ASSOC_MSK) ? 1 : 0;
@@ -1224,11 +1125,6 @@ static inline int is_channel_valid(const struct iwl_channel_info *ch_info)
 	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;
@@ -1254,9 +1150,26 @@ static inline int is_channel_ibss(const struct iwl_channel_info *ch)
 	return ((ch->flags & EEPROM_CHANNEL_IBSS)) ? 1 : 0;
 }
 
+#ifdef CONFIG_IWLWIFI_DEBUG
+static inline void iwl_print_hex_dump(struct iwl_priv *priv, int level,
+				      void *p, u32 len)
+{
+	if (!(priv->debug_level & level))
+		return;
+
+	print_hex_dump(KERN_DEBUG, "iwl data: ", DUMP_PREFIX_OFFSET, 16, 1,
+			p, len, 1);
+}
+#else
+static inline void iwl_print_hex_dump(struct iwl_priv *priv, int level,
+				      void *p, u32 len)
+{
+}
+#endif
+
 extern const struct iwl_channel_info *iwl_get_channel_info(
 	const struct iwl_priv *priv, enum ieee80211_band band, u16 channel);
 
 /* Requires full declaration of iwl_priv before including */
 
-#endif				/* __iwl4965_4965_h__ */
+#endif				/* __iwl_dev_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-eeprom.c b/drivers/net/wireless/iwlwifi/iwl-eeprom.c
index a07d5dcb7abc..4a08a1b50979 100644
--- a/drivers/net/wireless/iwlwifi/iwl-eeprom.c
+++ b/drivers/net/wireless/iwlwifi/iwl-eeprom.c
@@ -68,8 +68,8 @@
 
 #include <net/mac80211.h>
 
-#include "iwl-4965-commands.h"
-#include "iwl-4965.h"
+#include "iwl-commands.h"
+#include "iwl-dev.h"
 #include "iwl-core.h"
 #include "iwl-debug.h"
 #include "iwl-eeprom.h"
@@ -193,6 +193,12 @@ void iwlcore_eeprom_release_semaphore(struct iwl_priv *priv)
 }
 EXPORT_SYMBOL(iwlcore_eeprom_release_semaphore);
 
+const u8 *iwlcore_eeprom_query_addr(const struct iwl_priv *priv, size_t offset)
+{
+	BUG_ON(offset >= priv->cfg->eeprom_size);
+	return &priv->eeprom[offset];
+}
+EXPORT_SYMBOL(iwlcore_eeprom_query_addr);
 
 /**
  * iwl_eeprom_init - read EEPROM contents
@@ -203,30 +209,35 @@ EXPORT_SYMBOL(iwlcore_eeprom_release_semaphore);
  */
 int iwl_eeprom_init(struct iwl_priv *priv)
 {
-	u16 *e = (u16 *)&priv->eeprom;
+	u16 *e;
 	u32 gp = iwl_read32(priv, CSR_EEPROM_GP);
 	u32 r;
-	int sz = sizeof(priv->eeprom);
+	int sz = priv->cfg->eeprom_size;
 	int ret;
 	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);
+	/* allocate eeprom */
+	priv->eeprom = kzalloc(sz, GFP_KERNEL);
+	if (!priv->eeprom) {
+		ret = -ENOMEM;
+		goto alloc_err;
+	}
+	e = (u16 *)priv->eeprom;
 
-	if ((gp & CSR_EEPROM_GP_VALID_MSK) == CSR_EEPROM_GP_BAD_SIGNATURE) {
+	ret = priv->cfg->ops->lib->eeprom_ops.verify_signature(priv);
+	if (ret < 0) {
 		IWL_ERROR("EEPROM not found, EEPROM_GP=0x%08x", gp);
-		return -ENOENT;
+		ret = -ENOENT;
+		goto err;
 	}
 
 	/* Make sure driver (instead of uCode) is allowed to read EEPROM */
 	ret = priv->cfg->ops->lib->eeprom_ops.acquire_semaphore(priv);
 	if (ret < 0) {
 		IWL_ERROR("Failed to acquire EEPROM semaphore.\n");
-		return -ENOENT;
+		ret = -ENOENT;
+		goto err;
 	}
 
 	/* eeprom is an array of 16bit values */
@@ -250,61 +261,98 @@ int iwl_eeprom_init(struct iwl_priv *priv)
 		e[addr / 2] = le16_to_cpu((__force __le16)(r >> 16));
 	}
 	ret = 0;
-
 done:
 	priv->cfg->ops->lib->eeprom_ops.release_semaphore(priv);
+err:
+	if (ret)
+		kfree(priv->eeprom);
+alloc_err:
 	return ret;
 }
 EXPORT_SYMBOL(iwl_eeprom_init);
 
+void iwl_eeprom_free(struct iwl_priv *priv)
+{
+	if(priv->eeprom)
+		kfree(priv->eeprom);
+	priv->eeprom = NULL;
+}
+EXPORT_SYMBOL(iwl_eeprom_free);
+
+int iwl_eeprom_check_version(struct iwl_priv *priv)
+{
+	return priv->cfg->ops->lib->eeprom_ops.check_version(priv);
+}
+EXPORT_SYMBOL(iwl_eeprom_check_version);
+
+const u8 *iwl_eeprom_query_addr(const struct iwl_priv *priv, size_t offset)
+{
+	return priv->cfg->ops->lib->eeprom_ops.query_addr(priv, offset);
+}
+EXPORT_SYMBOL(iwl_eeprom_query_addr);
+
+u16 iwl_eeprom_query16(const struct iwl_priv *priv, size_t offset)
+{
+	return (u16)priv->eeprom[offset] | ((u16)priv->eeprom[offset + 1] << 8);
+}
+EXPORT_SYMBOL(iwl_eeprom_query16);
 
 void iwl_eeprom_get_mac(const struct iwl_priv *priv, u8 *mac)
 {
-	memcpy(mac, priv->eeprom.mac_address, 6);
+	const u8 *addr = priv->cfg->ops->lib->eeprom_ops.query_addr(priv,
+					EEPROM_MAC_ADDRESS);
+	memcpy(mac, addr, ETH_ALEN);
 }
 EXPORT_SYMBOL(iwl_eeprom_get_mac);
 
 static void iwl_init_band_reference(const struct iwl_priv *priv,
-				    int band,
-				    int *eeprom_ch_count,
-				    const struct iwl4965_eeprom_channel
-				    **eeprom_ch_info,
-				    const u8 **eeprom_ch_index)
+			int eep_band, int *eeprom_ch_count,
+			const struct iwl_eeprom_channel **eeprom_ch_info,
+			const u8 **eeprom_ch_index)
 {
-	switch (band) {
+	u32 offset = priv->cfg->ops->lib->
+			eeprom_ops.regulatory_bands[eep_band - 1];
+	switch (eep_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_info = (struct iwl_eeprom_channel *)
+				iwl_eeprom_query_addr(priv, offset);
 		*eeprom_ch_index = iwl_eeprom_band_1;
 		break;
 	case 2:		/* 4.9GHz band */
 		*eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_2);
-		*eeprom_ch_info = priv->eeprom.band_2_channels;
+		*eeprom_ch_info = (struct iwl_eeprom_channel *)
+				iwl_eeprom_query_addr(priv, offset);
 		*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_info = (struct iwl_eeprom_channel *)
+				iwl_eeprom_query_addr(priv, offset);
 		*eeprom_ch_index = iwl_eeprom_band_3;
 		break;
 	case 4:		/* 5.5GHz band */
 		*eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_4);
-		*eeprom_ch_info = priv->eeprom.band_4_channels;
+		*eeprom_ch_info = (struct iwl_eeprom_channel *)
+				iwl_eeprom_query_addr(priv, offset);
 		*eeprom_ch_index = iwl_eeprom_band_4;
 		break;
 	case 5:		/* 5.7GHz band */
 		*eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_5);
-		*eeprom_ch_info = priv->eeprom.band_5_channels;
+		*eeprom_ch_info = (struct iwl_eeprom_channel *)
+				iwl_eeprom_query_addr(priv, offset);
 		*eeprom_ch_index = iwl_eeprom_band_5;
 		break;
 	case 6:		/* 2.4GHz FAT channels */
 		*eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_6);
-		*eeprom_ch_info = priv->eeprom.band_24_channels;
+		*eeprom_ch_info = (struct iwl_eeprom_channel *)
+				iwl_eeprom_query_addr(priv, offset);
 		*eeprom_ch_index = iwl_eeprom_band_6;
 		break;
 	case 7:		/* 5 GHz FAT channels */
 		*eeprom_ch_count = ARRAY_SIZE(iwl_eeprom_band_7);
-		*eeprom_ch_info = priv->eeprom.band_52_channels;
+		*eeprom_ch_info = (struct iwl_eeprom_channel *)
+				iwl_eeprom_query_addr(priv, offset);
 		*eeprom_ch_index = iwl_eeprom_band_7;
 		break;
 	default:
@@ -317,13 +365,13 @@ static void iwl_init_band_reference(const struct iwl_priv *priv,
 			    ? # x " " : "")
 
 /**
- * iwl4965_set_fat_chan_info - Copy fat channel info into driver's priv.
+ * iwl_set_fat_chan_info - Copy fat channel info into driver's priv.
  *
  * Does not set up a command, or touch hardware.
  */
-static int iwl4965_set_fat_chan_info(struct iwl_priv *priv,
+static int iwl_set_fat_chan_info(struct iwl_priv *priv,
 			      enum ieee80211_band band, u16 channel,
-			      const struct iwl4965_eeprom_channel *eeprom_ch,
+			      const struct iwl_eeprom_channel *eeprom_ch,
 			      u8 fat_extension_channel)
 {
 	struct iwl_channel_info *ch_info;
@@ -334,8 +382,8 @@ static int iwl4965_set_fat_chan_info(struct iwl_priv *priv,
 	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",
+	IWL_DEBUG_INFO("FAT Ch. %d [%sGHz] %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",
@@ -343,7 +391,6 @@ static int iwl4965_set_fat_chan_info(struct iwl_priv *priv,
 			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,
@@ -372,7 +419,7 @@ int iwl_init_channel_map(struct iwl_priv *priv)
 {
 	int eeprom_ch_count = 0;
 	const u8 *eeprom_ch_index = NULL;
-	const struct iwl4965_eeprom_channel *eeprom_ch_info = NULL;
+	const struct iwl_eeprom_channel *eeprom_ch_info = NULL;
 	int band, ch;
 	struct iwl_channel_info *ch_info;
 
@@ -381,12 +428,6 @@ int iwl_init_channel_map(struct iwl_priv *priv)
 		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 =
@@ -429,6 +470,11 @@ int iwl_init_channel_map(struct iwl_priv *priv)
 			/* Copy the run-time flags so they are there even on
 			 * invalid channels */
 			ch_info->flags = eeprom_ch_info[ch].flags;
+			/* First write that fat is not enabled, and then enable
+			 * one by one */
+			ch_info->fat_extension_channel =
+				(IEEE80211_CHAN_NO_FAT_ABOVE |
+				 IEEE80211_CHAN_NO_FAT_BELOW);
 
 			if (!(is_channel_valid(ch_info))) {
 				IWL_DEBUG_INFO("Ch. %d Flags %x [%sGHz] - "
@@ -447,8 +493,8 @@ int iwl_init_channel_map(struct iwl_priv *priv)
 			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%s(0x%02x"
-				       " %ddBm): Ad-Hoc %ssupported\n",
+			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",
@@ -457,7 +503,6 @@ int iwl_init_channel_map(struct iwl_priv *priv)
 				       CHECK_AND_PRINT_I(ACTIVE),
 				       CHECK_AND_PRINT_I(RADAR),
 				       CHECK_AND_PRINT_I(WIDE),
-				       CHECK_AND_PRINT_I(NARROW),
 				       CHECK_AND_PRINT_I(DFS),
 				       eeprom_ch_info[ch].flags,
 				       eeprom_ch_info[ch].max_power_avg,
@@ -470,8 +515,8 @@ int iwl_init_channel_map(struct iwl_priv *priv)
 			/* 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 =
+						priv->tx_power_user_lmt)
+				priv->tx_power_user_lmt =
 				    eeprom_ch_info[ch].max_power_avg;
 
 			ch_info++;
@@ -494,24 +539,26 @@ int iwl_init_channel_map(struct iwl_priv *priv)
 		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;
+				((eeprom_ch_index[ch] == 5) ||
+				 (eeprom_ch_index[ch] == 6) ||
+				 (eeprom_ch_index[ch] == 7)))
+				/* both are allowed: above and below */
+				fat_extension_chan = 0;
 			else
-				fat_extension_chan = HT_IE_EXT_CHANNEL_ABOVE;
+				fat_extension_chan =
+					IEEE80211_CHAN_NO_FAT_BELOW;
 
 			/* Set up driver's info for lower half */
-			iwl4965_set_fat_chan_info(priv, ieeeband,
-						  eeprom_ch_index[ch],
-						  &(eeprom_ch_info[ch]),
-						  fat_extension_chan);
+			iwl_set_fat_chan_info(priv, ieeeband,
+						eeprom_ch_index[ch],
+						&(eeprom_ch_info[ch]),
+						fat_extension_chan);
 
 			/* Set up driver's info for upper half */
-			iwl4965_set_fat_chan_info(priv, ieeeband,
-						  (eeprom_ch_index[ch] + 4),
-						  &(eeprom_ch_info[ch]),
-						  HT_IE_EXT_CHANNEL_BELOW);
+			iwl_set_fat_chan_info(priv, ieeeband,
+						(eeprom_ch_index[ch] + 4),
+						&(eeprom_ch_info[ch]),
+						IEEE80211_CHAN_NO_FAT_ABOVE);
 		}
 	}
 
@@ -520,23 +567,21 @@ int iwl_init_channel_map(struct iwl_priv *priv)
 EXPORT_SYMBOL(iwl_init_channel_map);
 
 /*
- * iwl_free_channel_map - undo allocations in iwl4965_init_channel_map
+ * iwl_free_channel_map - undo allocations in iwl_init_channel_map
  */
 void iwl_free_channel_map(struct iwl_priv *priv)
 {
 	kfree(priv->channel_info);
 	priv->channel_count = 0;
 }
-EXPORT_SYMBOL(iwl_free_channel_map);
 
 /**
  * iwl_get_channel_info - Find driver's private channel info
  *
  * Based on band and channel number.
  */
-const struct iwl_channel_info *iwl_get_channel_info(
-		const struct iwl_priv *priv,
-		enum ieee80211_band band, u16 channel)
+const struct iwl_channel_info *iwl_get_channel_info(const struct iwl_priv *priv,
+					enum ieee80211_band band, u16 channel)
 {
 	int i;
 
diff --git a/drivers/net/wireless/iwlwifi/iwl-eeprom.h b/drivers/net/wireless/iwlwifi/iwl-eeprom.h
index bd0a042ca77f..d3a2a5b4ac56 100644
--- a/drivers/net/wireless/iwlwifi/iwl-eeprom.h
+++ b/drivers/net/wireless/iwlwifi/iwl-eeprom.h
@@ -106,7 +106,7 @@ enum {
 	EEPROM_CHANNEL_ACTIVE = (1 << 3),	/* active scanning allowed */
 	EEPROM_CHANNEL_RADAR = (1 << 4),	/* radar detection required */
 	EEPROM_CHANNEL_WIDE = (1 << 5),		/* 20 MHz channel okay */
-	EEPROM_CHANNEL_NARROW = (1 << 6),	/* 10 MHz channel (not used) */
+	/* Bit 6 Reserved (was Narrow Channel) */
 	EEPROM_CHANNEL_DFS = (1 << 7),	/* dynamic freq selection candidate */
 };
 
@@ -116,7 +116,7 @@ enum {
 
 /* *regulatory* channel data format in eeprom, one for each channel.
  * There are separate entries for FAT (40 MHz) vs. normal (20 MHz) channels. */
-struct iwl4965_eeprom_channel {
+struct iwl_eeprom_channel {
 	u8 flags;		/* EEPROM_CHANNEL_* flags copied from EEPROM */
 	s8 max_power_avg;	/* max power (dBm) on this chnl, limit 31 */
 } __attribute__ ((packed));
@@ -131,17 +131,55 @@ struct iwl4965_eeprom_channel {
  * each of 3 target output levels */
 #define EEPROM_TX_POWER_MEASUREMENTS   (3)
 
-#define EEPROM_4965_TX_POWER_VERSION        (2)
+/* 4965 Specific */
+/* 4965 driver does not work with txpower calibration version < 5 */
+#define EEPROM_4965_TX_POWER_VERSION    (5)
+#define EEPROM_4965_EEPROM_VERSION	(0x2f)
+#define EEPROM_4965_CALIB_VERSION_OFFSET       (2*0xB6) /* 2 bytes */
+#define EEPROM_4965_CALIB_TXPOWER_OFFSET       (2*0xE8) /* 48  bytes */
+#define EEPROM_4965_BOARD_REVISION             (2*0x4F) /* 2 bytes */
+#define EEPROM_4965_BOARD_PBA                  (2*0x56+1) /* 9 bytes */
+
+/* 5000 Specific */
+#define EEPROM_5000_TX_POWER_VERSION    (4)
+#define EEPROM_5000_EEPROM_VERSION	(0x11A)
+
+/*5000 calibrations */
+#define EEPROM_5000_CALIB_ALL	(INDIRECT_ADDRESS | INDIRECT_CALIBRATION)
+#define EEPROM_5000_XTAL	((2*0x128) | EEPROM_5000_CALIB_ALL)
+
+/* 5000 links */
+#define EEPROM_5000_LINK_HOST             (2*0x64)
+#define EEPROM_5000_LINK_GENERAL          (2*0x65)
+#define EEPROM_5000_LINK_REGULATORY       (2*0x66)
+#define EEPROM_5000_LINK_CALIBRATION      (2*0x67)
+#define EEPROM_5000_LINK_PROCESS_ADJST    (2*0x68)
+#define EEPROM_5000_LINK_OTHERS           (2*0x69)
+
+/* 5000 regulatory - indirect access */
+#define EEPROM_5000_REG_SKU_ID ((0x02)\
+		| INDIRECT_ADDRESS | INDIRECT_REGULATORY)   /* 4  bytes */
+#define EEPROM_5000_REG_BAND_1_CHANNELS       ((0x08)\
+		| INDIRECT_ADDRESS | INDIRECT_REGULATORY)   /* 28 bytes */
+#define EEPROM_5000_REG_BAND_2_CHANNELS       ((0x26)\
+		| INDIRECT_ADDRESS | INDIRECT_REGULATORY)   /* 26 bytes */
+#define EEPROM_5000_REG_BAND_3_CHANNELS       ((0x42)\
+		| INDIRECT_ADDRESS | INDIRECT_REGULATORY)   /* 24 bytes */
+#define EEPROM_5000_REG_BAND_4_CHANNELS       ((0x5C)\
+		| INDIRECT_ADDRESS | INDIRECT_REGULATORY)   /* 22 bytes */
+#define EEPROM_5000_REG_BAND_5_CHANNELS       ((0x74)\
+		| INDIRECT_ADDRESS | INDIRECT_REGULATORY)   /* 12 bytes */
+#define EEPROM_5000_REG_BAND_24_FAT_CHANNELS  ((0x82)\
+		| INDIRECT_ADDRESS | INDIRECT_REGULATORY)   /* 14  bytes */
+#define EEPROM_5000_REG_BAND_52_FAT_CHANNELS  ((0x92)\
+		| INDIRECT_ADDRESS | INDIRECT_REGULATORY)   /* 22  bytes */
 
-/* 4965 driver does not work with txpower calibration version < 5.
- * Look for this in calib_version member of struct iwl4965_eeprom. */
-#define EEPROM_TX_POWER_VERSION_NEW    (5)
 
 /* 2.4 GHz */
 extern const u8 iwl_eeprom_band_1[14];
 
 /*
- * 4965 factory calibration data for one txpower level, on one channel,
+ * factory calibration data for one txpower level, on one channel,
  * measured on one of the 2 tx chains (radio transmitter and associated
  * antenna).  EEPROM contains:
  *
@@ -154,7 +192,7 @@ extern const u8 iwl_eeprom_band_1[14];
  *
  * 4)  RF power amplifier detector level measurement (not used).
  */
-struct iwl4965_eeprom_calib_measure {
+struct iwl_eeprom_calib_measure {
 	u8 temperature;		/* Device temperature (Celsius) */
 	u8 gain_idx;		/* Index into gain table */
 	u8 actual_pow;		/* Measured RF output power, half-dBm */
@@ -163,22 +201,22 @@ struct iwl4965_eeprom_calib_measure {
 
 
 /*
- * 4965 measurement set for one channel.  EEPROM contains:
+ * measurement set for one channel.  EEPROM contains:
  *
  * 1)  Channel number measured
  *
  * 2)  Measurements for each of 3 power levels for each of 2 radio transmitters
  *     (a.k.a. "tx chains") (6 measurements altogether)
  */
-struct iwl4965_eeprom_calib_ch_info {
+struct iwl_eeprom_calib_ch_info {
 	u8 ch_num;
-	struct iwl4965_eeprom_calib_measure
+	struct iwl_eeprom_calib_measure
 		measurements[EEPROM_TX_POWER_TX_CHAINS]
 			[EEPROM_TX_POWER_MEASUREMENTS];
 } __attribute__ ((packed));
 
 /*
- * 4965 txpower subband info.
+ * txpower subband info.
  *
  * For each frequency subband, EEPROM contains the following:
  *
@@ -187,16 +225,16 @@ struct iwl4965_eeprom_calib_ch_info {
  *
  * 2)  Sample measurement sets for 2 channels close to the range endpoints.
  */
-struct iwl4965_eeprom_calib_subband_info {
+struct iwl_eeprom_calib_subband_info {
 	u8 ch_from;	/* channel number of lowest channel in subband */
 	u8 ch_to;	/* channel number of highest channel in subband */
-	struct iwl4965_eeprom_calib_ch_info ch1;
-	struct iwl4965_eeprom_calib_ch_info ch2;
+	struct iwl_eeprom_calib_ch_info ch1;
+	struct iwl_eeprom_calib_ch_info ch2;
 } __attribute__ ((packed));
 
 
 /*
- * 4965 txpower calibration info.  EEPROM contains:
+ * txpower calibration info.  EEPROM contains:
  *
  * 1)  Factory-measured saturation power levels (maximum levels at which
  *     tx power amplifier can output a signal without too much distortion).
@@ -212,55 +250,58 @@ struct iwl4965_eeprom_calib_subband_info {
  *     characteristics of the analog radio circuitry vary with frequency.
  *
  *     Not all sets need to be filled with data;
- *     struct iwl4965_eeprom_calib_subband_info contains range of channels
+ *     struct iwl_eeprom_calib_subband_info contains range of channels
  *     (0 if unused) for each set of data.
  */
-struct iwl4965_eeprom_calib_info {
+struct iwl_eeprom_calib_info {
 	u8 saturation_power24;	/* half-dBm (e.g. "34" = 17 dBm) */
 	u8 saturation_power52;	/* half-dBm */
 	s16 voltage;		/* signed */
-	struct iwl4965_eeprom_calib_subband_info
+	struct iwl_eeprom_calib_subband_info
 		band_info[EEPROM_TX_POWER_BANDS];
 } __attribute__ ((packed));
 
 
-
-/*
- * 4965 EEPROM map
- */
-struct iwl4965_eeprom {
-	u8 reserved0[16];
-	u16 device_id;		/* abs.ofs: 16 */
-	u8 reserved1[2];
-	u16 pmc;		/* abs.ofs: 20 */
-	u8 reserved2[20];
-	u8 mac_address[6];	/* abs.ofs: 42 */
-	u8 reserved3[58];
-	u16 board_revision;	/* abs.ofs: 106 */
-	u8 reserved4[11];
-	u8 board_pba_number[9];	/* abs.ofs: 119 */
-	u8 reserved5[8];
-	u16 version;		/* abs.ofs: 136 */
-	u8 sku_cap;		/* abs.ofs: 138 */
-	u8 leds_mode;		/* abs.ofs: 139 */
-	u16 oem_mode;
-	u16 wowlan_mode;	/* abs.ofs: 142 */
-	u16 leds_time_interval;	/* abs.ofs: 144 */
-	u8 leds_off_time;	/* abs.ofs: 146 */
-	u8 leds_on_time;	/* abs.ofs: 147 */
-	u8 almgor_m_version;	/* abs.ofs: 148 */
-	u8 antenna_switch_type;	/* abs.ofs: 149 */
-	u8 reserved6[8];
-	u16 board_revision_4965;	/* abs.ofs: 158 */
-	u8 reserved7[13];
-	u8 board_pba_number_4965[9];	/* abs.ofs: 173 */
-	u8 reserved8[10];
-	u8 sku_id[4];		/* abs.ofs: 192 */
+#define ADDRESS_MSK                 0x0000FFFF
+#define INDIRECT_TYPE_MSK           0x000F0000
+#define INDIRECT_HOST               0x00010000
+#define INDIRECT_GENERAL            0x00020000
+#define INDIRECT_REGULATORY         0x00030000
+#define INDIRECT_CALIBRATION        0x00040000
+#define INDIRECT_PROCESS_ADJST      0x00050000
+#define INDIRECT_OTHERS             0x00060000
+#define INDIRECT_ADDRESS            0x00100000
+
+/* General */
+#define EEPROM_DEVICE_ID                    (2*0x08)	/* 2 bytes */
+#define EEPROM_MAC_ADDRESS                  (2*0x15)	/* 6  bytes */
+#define EEPROM_BOARD_REVISION               (2*0x35)	/* 2  bytes */
+#define EEPROM_BOARD_PBA_NUMBER             (2*0x3B+1)	/* 9  bytes */
+#define EEPROM_VERSION                      (2*0x44)	/* 2  bytes */
+#define EEPROM_SKU_CAP                      (2*0x45)	/* 1  bytes */
+#define EEPROM_LEDS_MODE                    (2*0x45+1)	/* 1  bytes */
+#define EEPROM_OEM_MODE                     (2*0x46)	/* 2  bytes */
+#define EEPROM_WOWLAN_MODE                  (2*0x47)	/* 2  bytes */
+#define EEPROM_RADIO_CONFIG                 (2*0x48)	/* 2  bytes */
+#define EEPROM_3945_M_VERSION               (2*0x4A)	/* 1  bytes */
+#define EEPROM_ANTENNA_SWITCH_TYPE          (2*0x4A+1)	/* 1  bytes */
+
+/* The following masks are to be applied on EEPROM_RADIO_CONFIG */
+#define EEPROM_RF_CFG_TYPE_MSK(x)   (x & 0x3)         /* bits 0-1   */
+#define EEPROM_RF_CFG_STEP_MSK(x)   ((x >> 2)  & 0x3) /* bits 2-3   */
+#define EEPROM_RF_CFG_DASH_MSK(x)   ((x >> 4)  & 0x3) /* bits 4-5   */
+#define EEPROM_RF_CFG_PNUM_MSK(x)   ((x >> 6)  & 0x3) /* bits 6-7   */
+#define EEPROM_RF_CFG_TX_ANT_MSK(x) ((x >> 8)  & 0xF) /* bits 8-11  */
+#define EEPROM_RF_CFG_RX_ANT_MSK(x) ((x >> 12) & 0xF) /* bits 12-15 */
+
+#define EEPROM_3945_RF_CFG_TYPE_MAX  0x0
+#define EEPROM_4965_RF_CFG_TYPE_MAX  0x1
+#define EEPROM_5000_RF_CFG_TYPE_MAX  0x3
 
 /*
  * Per-channel regulatory data.
  *
- * Each channel that *might* be supported by 3945 or 4965 has a fixed location
+ * Each channel that *might* be supported by iwl has a fixed location
  * in EEPROM containing EEPROM_CHANNEL_* usage flags (LSB) and max regulatory
  * txpower (MSB).
  *
@@ -269,40 +310,38 @@ struct iwl4965_eeprom {
  *
  * 2.4 GHz channels 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14
  */
-	u16 band_1_count;	/* abs.ofs: 196 */
-	struct iwl4965_eeprom_channel band_1_channels[14]; /* abs.ofs: 196 */
+#define EEPROM_REGULATORY_SKU_ID            (2*0x60)    /* 4  bytes */
+#define EEPROM_REGULATORY_BAND_1            (2*0x62)	/* 2  bytes */
+#define EEPROM_REGULATORY_BAND_1_CHANNELS   (2*0x63)	/* 28 bytes */
 
 /*
  * 4.9 GHz channels 183, 184, 185, 187, 188, 189, 192, 196,
  * 5.0 GHz channels 7, 8, 11, 12, 16
  * (4915-5080MHz) (none of these is ever supported)
  */
-	u16 band_2_count;	/* abs.ofs: 226 */
-	struct iwl4965_eeprom_channel band_2_channels[13]; /* abs.ofs: 228 */
+#define EEPROM_REGULATORY_BAND_2            (2*0x71)	/* 2  bytes */
+#define EEPROM_REGULATORY_BAND_2_CHANNELS   (2*0x72)	/* 26 bytes */
 
 /*
  * 5.2 GHz channels 34, 36, 38, 40, 42, 44, 46, 48, 52, 56, 60, 64
  * (5170-5320MHz)
  */
-	u16 band_3_count;	/* abs.ofs: 254 */
-	struct iwl4965_eeprom_channel band_3_channels[12]; /* abs.ofs: 256 */
+#define EEPROM_REGULATORY_BAND_3            (2*0x7F)	/* 2  bytes */
+#define EEPROM_REGULATORY_BAND_3_CHANNELS   (2*0x80)	/* 24 bytes */
 
 /*
  * 5.5 GHz channels 100, 104, 108, 112, 116, 120, 124, 128, 132, 136, 140
  * (5500-5700MHz)
  */
-	u16 band_4_count;	/* abs.ofs: 280 */
-	struct iwl4965_eeprom_channel band_4_channels[11]; /* abs.ofs: 282 */
+#define EEPROM_REGULATORY_BAND_4            (2*0x8C)	/* 2  bytes */
+#define EEPROM_REGULATORY_BAND_4_CHANNELS   (2*0x8D)	/* 22 bytes */
 
 /*
  * 5.7 GHz channels 145, 149, 153, 157, 161, 165
  * (5725-5825MHz)
  */
-	u16 band_5_count;	/* abs.ofs: 304 */
-	struct iwl4965_eeprom_channel band_5_channels[6]; /* abs.ofs: 306 */
-
-	u8 reserved10[2];
-
+#define EEPROM_REGULATORY_BAND_5            (2*0x98)	/* 2  bytes */
+#define EEPROM_REGULATORY_BAND_5_CHANNELS   (2*0x99)	/* 12 bytes */
 
 /*
  * 2.4 GHz FAT channels 1 (5), 2 (6), 3 (7), 4 (8), 5 (9), 6 (10), 7 (11)
@@ -319,52 +358,35 @@ struct iwl4965_eeprom {
  *
  * NOTE:  4965 does not support FAT channels on 2.4 GHz.
  */
-	struct iwl4965_eeprom_channel band_24_channels[7]; /* abs.ofs: 320 */
-	u8 reserved11[2];
+#define EEPROM_4965_REGULATORY_BAND_24_FAT_CHANNELS (2*0xA0)	/* 14 bytes */
 
 /*
  * 5.2 GHz FAT channels 36 (40), 44 (48), 52 (56), 60 (64),
  * 100 (104), 108 (112), 116 (120), 124 (128), 132 (136), 149 (153), 157 (161)
  */
-	struct iwl4965_eeprom_channel band_52_channels[11]; /* abs.ofs: 336 */
-	u8 reserved12[6];
-
-/*
- * 4965 driver requires txpower calibration format version 5 or greater.
- * Driver does not work with txpower calibration version < 5.
- * This value is simply a 16-bit number, no major/minor versions here.
- */
-	u16 calib_version;	/* abs.ofs: 364 */
-	u8 reserved13[2];
-	u8 reserved14[96];	/* abs.ofs: 368 */
-
-/*
- * 4965 Txpower calibration data.
- */
-	struct iwl4965_eeprom_calib_info calib_info;	/* abs.ofs: 464 */
-
-	u8 reserved16[140];	/* fill out to full 1024 byte block */
-
-
-} __attribute__ ((packed));
-
-#define IWL_EEPROM_IMAGE_SIZE 1024
-
-/* End of EEPROM */
+#define EEPROM_4965_REGULATORY_BAND_52_FAT_CHANNELS (2*0xA8)	/* 22 bytes */
 
 struct iwl_eeprom_ops {
+	const u32 regulatory_bands[7];
 	int (*verify_signature) (struct iwl_priv *priv);
 	int (*acquire_semaphore) (struct iwl_priv *priv);
 	void (*release_semaphore) (struct iwl_priv *priv);
+	int (*check_version) (struct iwl_priv *priv);
+	const u8* (*query_addr) (const struct iwl_priv *priv, size_t offset);
 };
 
 
 void iwl_eeprom_get_mac(const struct iwl_priv *priv, u8 *mac);
 int iwl_eeprom_init(struct iwl_priv *priv);
+void iwl_eeprom_free(struct iwl_priv *priv);
+int  iwl_eeprom_check_version(struct iwl_priv *priv);
+const u8 *iwl_eeprom_query_addr(const struct iwl_priv *priv, size_t offset);
+u16 iwl_eeprom_query16(const struct iwl_priv *priv, size_t offset);
 
 int iwlcore_eeprom_verify_signature(struct iwl_priv *priv);
 int iwlcore_eeprom_acquire_semaphore(struct iwl_priv *priv);
 void iwlcore_eeprom_release_semaphore(struct iwl_priv *priv);
+const u8 *iwlcore_eeprom_query_addr(const struct iwl_priv *priv, size_t offset);
 
 int iwl_init_channel_map(struct iwl_priv *priv);
 void iwl_free_channel_map(struct iwl_priv *priv);
diff --git a/drivers/net/wireless/iwlwifi/iwl-fh.h b/drivers/net/wireless/iwlwifi/iwl-fh.h
new file mode 100644
index 000000000000..944642450d3d
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-fh.h
@@ -0,0 +1,391 @@
+/******************************************************************************
+ *
+ * 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 - 2008 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.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 - 2008 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.
+ *
+ *****************************************************************************/
+
+/****************************/
+/* Flow Handler Definitions */
+/****************************/
+
+/**
+ * This I/O area is directly read/writable by driver (e.g. Linux uses writel())
+ * Addresses are offsets from device's PCI hardware base address.
+ */
+#define FH_MEM_LOWER_BOUND                   (0x1000)
+#define FH_MEM_UPPER_BOUND                   (0x1EF0)
+
+/**
+ * Keep-Warm (KW) buffer base address.
+ *
+ * Driver must allocate a 4KByte buffer that is used by 4965 for keeping the
+ * host DRAM powered on (via dummy accesses to DRAM) to maintain low-latency
+ * DRAM access when 4965 is Txing or Rxing.  The dummy accesses prevent host
+ * from going into a power-savings mode that would cause higher DRAM latency,
+ * and possible data over/under-runs, before all Tx/Rx is complete.
+ *
+ * Driver loads FH_KW_MEM_ADDR_REG with the physical address (bits 35:4)
+ * of the buffer, which must be 4K aligned.  Once this is set up, the 4965
+ * automatically invokes keep-warm accesses when normal accesses might not
+ * be sufficient to maintain fast DRAM response.
+ *
+ * Bit fields:
+ *  31-0:  Keep-warm buffer physical base address [35:4], must be 4K aligned
+ */
+#define FH_KW_MEM_ADDR_REG		     (FH_MEM_LOWER_BOUND + 0x97C)
+
+
+/**
+ * TFD Circular Buffers Base (CBBC) addresses
+ *
+ * 4965 has 16 base pointer registers, one for each of 16 host-DRAM-resident
+ * circular buffers (CBs/queues) containing Transmit Frame Descriptors (TFDs)
+ * (see struct iwl_tfd_frame).  These 16 pointer registers are offset by 0x04
+ * bytes from one another.  Each TFD circular buffer in DRAM must be 256-byte
+ * aligned (address bits 0-7 must be 0).
+ *
+ * Bit fields in each pointer register:
+ *  27-0: TFD CB physical base address [35:8], must be 256-byte aligned
+ */
+#define FH_MEM_CBBC_LOWER_BOUND          (FH_MEM_LOWER_BOUND + 0x9D0)
+#define FH_MEM_CBBC_UPPER_BOUND          (FH_MEM_LOWER_BOUND + 0xA10)
+
+/* Find TFD CB base pointer for given queue (range 0-15). */
+#define FH_MEM_CBBC_QUEUE(x)  (FH_MEM_CBBC_LOWER_BOUND + (x) * 0x4)
+
+
+/**
+ * Rx SRAM Control and Status Registers (RSCSR)
+ *
+ * These registers provide handshake between driver and 4965 for the Rx queue
+ * (this queue handles *all* command responses, notifications, Rx data, etc.
+ * sent from 4965 uCode to host driver).  Unlike Tx, there is only one Rx
+ * queue, and only one Rx DMA/FIFO channel.  Also unlike Tx, which can
+ * concatenate up to 20 DRAM buffers to form a Tx frame, each Receive Buffer
+ * Descriptor (RBD) points to only one Rx Buffer (RB); there is a 1:1
+ * mapping between RBDs and RBs.
+ *
+ * Driver must allocate host DRAM memory for the following, and set the
+ * physical address of each into 4965 registers:
+ *
+ * 1)  Receive Buffer Descriptor (RBD) circular buffer (CB), typically with 256
+ *     entries (although any power of 2, up to 4096, is selectable by driver).
+ *     Each entry (1 dword) points to a receive buffer (RB) of consistent size
+ *     (typically 4K, although 8K or 16K are also selectable by driver).
+ *     Driver sets up RB size and number of RBDs in the CB via Rx config
+ *     register FH_MEM_RCSR_CHNL0_CONFIG_REG.
+ *
+ *     Bit fields within one RBD:
+ *     27-0:  Receive Buffer physical address bits [35:8], 256-byte aligned
+ *
+ *     Driver sets physical address [35:8] of base of RBD circular buffer
+ *     into FH_RSCSR_CHNL0_RBDCB_BASE_REG [27:0].
+ *
+ * 2)  Rx status buffer, 8 bytes, in which 4965 indicates which Rx Buffers
+ *     (RBs) have been filled, via a "write pointer", actually the index of
+ *     the RB's corresponding RBD within the circular buffer.  Driver sets
+ *     physical address [35:4] into FH_RSCSR_CHNL0_STTS_WPTR_REG [31:0].
+ *
+ *     Bit fields in lower dword of Rx status buffer (upper dword not used
+ *     by driver; see struct iwl4965_shared, val0):
+ *     31-12:  Not used by driver
+ *     11- 0:  Index of last filled Rx buffer descriptor
+ *             (4965 writes, driver reads this value)
+ *
+ * As the driver prepares Receive Buffers (RBs) for 4965 to fill, driver must
+ * enter pointers to these RBs into contiguous RBD circular buffer entries,
+ * and update the 4965's "write" index register,
+ * FH_RSCSR_CHNL0_RBDCB_WPTR_REG.
+ *
+ * This "write" index corresponds to the *next* RBD that the driver will make
+ * available, i.e. one RBD past the tail of the ready-to-fill RBDs within
+ * the circular buffer.  This value should initially be 0 (before preparing any
+ * RBs), should be 8 after preparing the first 8 RBs (for example), and must
+ * wrap back to 0 at the end of the circular buffer (but don't wrap before
+ * "read" index has advanced past 1!  See below).
+ * NOTE:  4965 EXPECTS THE WRITE INDEX TO BE INCREMENTED IN MULTIPLES OF 8.
+ *
+ * As the 4965 fills RBs (referenced from contiguous RBDs within the circular
+ * buffer), it updates the Rx status buffer in host DRAM, 2) described above,
+ * to tell the driver the index of the latest filled RBD.  The driver must
+ * read this "read" index from DRAM after receiving an Rx interrupt from 4965.
+ *
+ * The driver must also internally keep track of a third index, which is the
+ * next RBD to process.  When receiving an Rx interrupt, driver should process
+ * all filled but unprocessed RBs up to, but not including, the RB
+ * corresponding to the "read" index.  For example, if "read" index becomes "1",
+ * driver may process the RB pointed to by RBD 0.  Depending on volume of
+ * traffic, there may be many RBs to process.
+ *
+ * If read index == write index, 4965 thinks there is no room to put new data.
+ * Due to this, the maximum number of filled RBs is 255, instead of 256.  To
+ * be safe, make sure that there is a gap of at least 2 RBDs between "write"
+ * and "read" indexes; that is, make sure that there are no more than 254
+ * buffers waiting to be filled.
+ */
+#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)
+
+/**
+ * Physical base address of 8-byte Rx Status buffer.
+ * Bit fields:
+ *  31-0: Rx status buffer physical base address [35:4], must 16-byte aligned.
+ */
+#define FH_RSCSR_CHNL0_STTS_WPTR_REG	(FH_MEM_RSCSR_CHNL0)
+
+/**
+ * Physical base address of Rx Buffer Descriptor Circular Buffer.
+ * Bit fields:
+ *  27-0:  RBD CD physical base address [35:8], must be 256-byte aligned.
+ */
+#define FH_RSCSR_CHNL0_RBDCB_BASE_REG	(FH_MEM_RSCSR_CHNL0 + 0x004)
+
+/**
+ * Rx write pointer (index, really!).
+ * Bit fields:
+ *  11-0:  Index of driver's most recent prepared-to-be-filled RBD, + 1.
+ *         NOTE:  For 256-entry circular buffer, use only bits [7:0].
+ */
+#define FH_RSCSR_CHNL0_RBDCB_WPTR_REG	(FH_MEM_RSCSR_CHNL0 + 0x008)
+#define FH_RSCSR_CHNL0_WPTR        (FH_RSCSR_CHNL0_RBDCB_WPTR_REG)
+
+
+/**
+ * Rx Config/Status Registers (RCSR)
+ * Rx Config Reg for channel 0 (only channel used)
+ *
+ * Driver must initialize FH_MEM_RCSR_CHNL0_CONFIG_REG as follows for
+ * normal operation (see bit fields).
+ *
+ * Clearing FH_MEM_RCSR_CHNL0_CONFIG_REG to 0 turns off Rx DMA.
+ * Driver should poll FH_MEM_RSSR_RX_STATUS_REG	for
+ * FH_RSSR_CHNL0_RX_STATUS_CHNL_IDLE (bit 24) before continuing.
+ *
+ * Bit fields:
+ * 31-30: Rx DMA channel enable: '00' off/pause, '01' pause at end of frame,
+ *        '10' operate normally
+ * 29-24: reserved
+ * 23-20: # RBDs in circular buffer = 2^value; use "8" for 256 RBDs (normal),
+ *        min "5" for 32 RBDs, max "12" for 4096 RBDs.
+ * 19-18: reserved
+ * 17-16: size of each receive buffer; '00' 4K (normal), '01' 8K,
+ *        '10' 12K, '11' 16K.
+ * 15-14: reserved
+ * 13-12: IRQ destination; '00' none, '01' host driver (normal operation)
+ * 11- 4: timeout for closing Rx buffer and interrupting host (units 32 usec)
+ *        typical value 0x10 (about 1/2 msec)
+ *  3- 0: reserved
+ */
+#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)
+
+#define FH_RCSR_CHNL0_RX_CONFIG_RB_TIMEOUT_MSK (0x00000FF0) /* bits 4-11 */
+#define FH_RCSR_CHNL0_RX_CONFIG_IRQ_DEST_MSK   (0x00001000) /* bits 12 */
+#define FH_RCSR_CHNL0_RX_CONFIG_SINGLE_FRAME_MSK (0x00008000) /* bit 15 */
+#define FH_RCSR_CHNL0_RX_CONFIG_RB_SIZE_MSK   (0x00030000) /* bits 16-17 */
+#define FH_RCSR_CHNL0_RX_CONFIG_RBDBC_SIZE_MSK (0x00F00000) /* bits 20-23 */
+#define FH_RCSR_CHNL0_RX_CONFIG_DMA_CHNL_EN_MSK (0xC0000000) /* bits 30-31*/
+
+#define FH_RCSR_RX_CONFIG_RBDCB_SIZE_BITSHIFT	(20)
+#define FH_RCSR_RX_CONFIG_REG_IRQ_RBTH_BITSHIFT	(4)
+#define RX_RB_TIMEOUT	(0x10)
+
+#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 FH_RCSR_RX_CONFIG_REG_VAL_RB_SIZE_4K    (0x00000000)
+#define FH_RCSR_RX_CONFIG_REG_VAL_RB_SIZE_8K    (0x00010000)
+#define FH_RCSR_RX_CONFIG_REG_VAL_RB_SIZE_12K   (0x00020000)
+#define FH_RCSR_RX_CONFIG_REG_VAL_RB_SIZE_16K   (0x00030000)
+
+#define FH_RCSR_CHNL0_RX_CONFIG_IRQ_DEST_NO_INT_VAL       (0x00000000)
+#define FH_RCSR_CHNL0_RX_CONFIG_IRQ_DEST_INT_HOST_VAL     (0x00001000)
+
+
+/**
+ * Rx Shared Status Registers (RSSR)
+ *
+ * After stopping Rx DMA channel (writing 0 to
+ * FH_MEM_RCSR_CHNL0_CONFIG_REG), driver must poll
+ * FH_MEM_RSSR_RX_STATUS_REG until Rx channel is idle.
+ *
+ * Bit fields:
+ *  24:  1 = Channel 0 is idle
+ *
+ * FH_MEM_RSSR_SHARED_CTRL_REG and FH_MEM_RSSR_RX_ENABLE_ERR_IRQ2DRV
+ * contain default values that should not be altered by the driver.
+ */
+#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)
+
+#define FH_RSSR_CHNL0_RX_STATUS_CHNL_IDLE	(0x01000000)
+
+
+/**
+ * Transmit DMA Channel Control/Status Registers (TCSR)
+ *
+ * 4965 has one configuration register for each of 8 Tx DMA/FIFO channels
+ * supported in hardware (don't confuse these with the 16 Tx queues in DRAM,
+ * which feed the DMA/FIFO channels); config regs are separated by 0x20 bytes.
+ *
+ * To use a Tx DMA channel, driver must initialize its
+ * FH_TCSR_CHNL_TX_CONFIG_REG(chnl) with:
+ *
+ * FH_TCSR_TX_CONFIG_REG_VAL_DMA_CHNL_ENABLE |
+ * FH_TCSR_TX_CONFIG_REG_VAL_DMA_CREDIT_ENABLE_VAL
+ *
+ * All other bits should be 0.
+ *
+ * Bit fields:
+ * 31-30: Tx DMA channel enable: '00' off/pause, '01' pause at end of frame,
+ *        '10' operate normally
+ * 29- 4: Reserved, set to "0"
+ *     3: Enable internal DMA requests (1, normal operation), disable (0)
+ *  2- 0: Reserved, set to "0"
+ */
+#define FH_TCSR_LOWER_BOUND  (FH_MEM_LOWER_BOUND + 0xD00)
+#define FH_TCSR_UPPER_BOUND  (FH_MEM_LOWER_BOUND + 0xE60)
+
+/* Find Control/Status reg for given Tx DMA/FIFO channel */
+#define FH_TCSR_CHNL_TX_CONFIG_REG(_chnl) \
+	(FH_TCSR_LOWER_BOUND + 0x20 * _chnl)
+
+#define FH_TCSR_TX_CONFIG_REG_VAL_DMA_CREDIT_DISABLE_VAL    (0x00000000)
+#define FH_TCSR_TX_CONFIG_REG_VAL_DMA_CREDIT_ENABLE_VAL     (0x00000008)
+
+#define FH_TCSR_TX_CONFIG_REG_VAL_DMA_CHNL_PAUSE            (0x00000000)
+#define FH_TCSR_TX_CONFIG_REG_VAL_DMA_CHNL_PAUSE_EOF        (0x40000000)
+#define FH_TCSR_TX_CONFIG_REG_VAL_DMA_CHNL_ENABLE           (0x80000000)
+
+#define FH_TCSR_CHNL_NUM                            (7)
+
+#define FH_TCSR_CHNL_TX_BUF_STS_REG_VAL_TFDB_EMPTY          (0x00000000)
+#define FH_TCSR_CHNL_TX_BUF_STS_REG_VAL_TFDB_WAIT           (0x00002000)
+#define FH_TCSR_CHNL_TX_BUF_STS_REG_VAL_TFDB_VALID          (0x00000003)
+
+#define FH_TCSR_TX_CONFIG_REG_VAL_CIRQ_HOST_NOINT           (0x00000000)
+#define FH_TCSR_TX_CONFIG_REG_VAL_CIRQ_HOST_ENDTFD          (0x00100000)
+#define FH_TCSR_TX_CONFIG_REG_VAL_CIRQ_HOST_IFTFD           (0x00200000)
+
+#define FH_TCSR_CHNL_TX_BUF_STS_REG_POS_TB_NUM      (20)
+#define FH_TCSR_CHNL_TX_BUF_STS_REG_POS_TB_IDX      (12)
+#define FH_TCSR_CHNL_TX_CONFIG_REG(_chnl) \
+	(FH_TCSR_LOWER_BOUND + 0x20 * _chnl)
+#define FH_TCSR_CHNL_TX_CREDIT_REG(_chnl) \
+	  (FH_TCSR_LOWER_BOUND + 0x20 * _chnl + 0x4)
+#define FH_TCSR_CHNL_TX_BUF_STS_REG(_chnl) \
+	 (FH_TCSR_LOWER_BOUND + 0x20 * _chnl + 0x8)
+
+/**
+ * Tx Shared Status Registers (TSSR)
+ *
+ * After stopping Tx DMA channel (writing 0 to
+ * FH_TCSR_CHNL_TX_CONFIG_REG(chnl)), driver must poll
+ * FH_TSSR_TX_STATUS_REG until selected Tx channel is idle
+ * (channel's buffers empty | no pending requests).
+ *
+ * Bit fields:
+ * 31-24:  1 = Channel buffers empty (channel 7:0)
+ * 23-16:  1 = No pending requests (channel 7:0)
+ */
+#define FH_TSSR_LOWER_BOUND		(FH_MEM_LOWER_BOUND + 0xEA0)
+#define FH_TSSR_UPPER_BOUND		(FH_MEM_LOWER_BOUND + 0xEC0)
+
+#define FH_TSSR_TX_STATUS_REG	(FH_TSSR_LOWER_BOUND + 0x010)
+
+#define FH_TSSR_TX_STATUS_REG_BIT_BUFS_EMPTY(_chnl) ((1 << (_chnl)) << 24)
+#define FH_TSSR_TX_STATUS_REG_BIT_NO_PEND_REQ(_chnl) ((1 << (_chnl)) << 16)
+
+#define FH_TSSR_TX_STATUS_REG_MSK_CHNL_IDLE(_chnl) \
+	(FH_TSSR_TX_STATUS_REG_BIT_BUFS_EMPTY(_chnl) | \
+	FH_TSSR_TX_STATUS_REG_BIT_NO_PEND_REQ(_chnl))
+
+
+
+#define FH_REGS_LOWER_BOUND		     (0x1000)
+#define FH_REGS_UPPER_BOUND		     (0x2000)
+
+/* Tx service channels */
+#define FH_SRVC_CHNL                                (9)
+#define FH_SRVC_LOWER_BOUND          (FH_REGS_LOWER_BOUND + 0x9C8)
+#define FH_SRVC_UPPER_BOUND          (FH_REGS_LOWER_BOUND + 0x9D0)
+#define FH_SRVC_CHNL_SRAM_ADDR_REG(_chnl) \
+		(FH_SRVC_LOWER_BOUND + ((_chnl) - 9) * 0x4)
+
+/* TFDB  Area - TFDs buffer table */
+#define FH_MEM_TFDIB_DRAM_ADDR_LSB_MSK      (0xFFFFFFFF)
+#define FH_TFDIB_LOWER_BOUND       (FH_REGS_LOWER_BOUND + 0x900)
+#define FH_TFDIB_UPPER_BOUND       (FH_REGS_LOWER_BOUND + 0x958)
+#define FH_TFDIB_CTRL0_REG(_chnl)  (FH_TFDIB_LOWER_BOUND + 0x8 * (_chnl))
+#define FH_TFDIB_CTRL1_REG(_chnl)  (FH_TFDIB_LOWER_BOUND + 0x8 * (_chnl) + 0x4)
+
+/* TCSR: tx_config register values */
+#define FH_RSCSR_FRAME_SIZE_MSK	(0x00003FFF)	/* bits 0-13 */
+
diff --git a/drivers/net/wireless/iwlwifi/iwl-hcmd.c b/drivers/net/wireless/iwlwifi/iwl-hcmd.c
index fdb27f1cdc08..8fa991b7202a 100644
--- a/drivers/net/wireless/iwlwifi/iwl-hcmd.c
+++ b/drivers/net/wireless/iwlwifi/iwl-hcmd.c
@@ -31,7 +31,7 @@
 #include <linux/version.h>
 #include <net/mac80211.h>
 
-#include "iwl-4965.h" /* FIXME: remove */
+#include "iwl-dev.h" /* FIXME: remove */
 #include "iwl-debug.h"
 #include "iwl-eeprom.h"
 #include "iwl-core.h"
@@ -56,6 +56,7 @@ const char *get_cmd_string(u8 cmd)
 		IWL_CMD(REPLY_RATE_SCALE);
 		IWL_CMD(REPLY_LEDS_CMD);
 		IWL_CMD(REPLY_TX_LINK_QUALITY_CMD);
+		IWL_CMD(COEX_PRIORITY_TABLE_CMD);
 		IWL_CMD(RADAR_NOTIFICATION);
 		IWL_CMD(REPLY_QUIET_CMD);
 		IWL_CMD(REPLY_CHANNEL_SWITCH);
@@ -89,6 +90,10 @@ const char *get_cmd_string(u8 cmd)
 		IWL_CMD(REPLY_RX_MPDU_CMD);
 		IWL_CMD(REPLY_RX);
 		IWL_CMD(REPLY_COMPRESSED_BA);
+		IWL_CMD(CALIBRATION_CFG_CMD);
+		IWL_CMD(CALIBRATION_RES_NOTIFICATION);
+		IWL_CMD(CALIBRATION_COMPLETE_NOTIFICATION);
+		IWL_CMD(REPLY_TX_POWER_DBM_CMD);
 	default:
 		return "UNKNOWN";
 
@@ -101,7 +106,7 @@ EXPORT_SYMBOL(get_cmd_string);
 static int iwl_generic_cmd_callback(struct iwl_priv *priv,
 				    struct iwl_cmd *cmd, struct sk_buff *skb)
 {
-	struct iwl4965_rx_packet *pkt = NULL;
+	struct iwl_rx_packet *pkt = NULL;
 
 	if (!skb) {
 		IWL_ERROR("Error: Response NULL in %s.\n",
@@ -109,7 +114,7 @@ static int iwl_generic_cmd_callback(struct iwl_priv *priv,
 		return 1;
 	}
 
-	pkt = (struct iwl4965_rx_packet *)skb->data;
+	pkt = (struct iwl_rx_packet *)skb->data;
 	if (pkt->hdr.flags & IWL_CMD_FAILED_MSK) {
 		IWL_ERROR("Bad return from %s (0x%08X)\n",
 			get_cmd_string(cmd->hdr.cmd), pkt->hdr.flags);
@@ -139,7 +144,7 @@ static int iwl_send_cmd_async(struct iwl_priv *priv, struct iwl_host_cmd *cmd)
 	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
 		return -EBUSY;
 
-	ret = priv->cfg->ops->utils->enqueue_hcmd(priv, cmd);
+	ret = iwl_enqueue_hcmd(priv, cmd);
 	if (ret < 0) {
 		IWL_ERROR("Error sending %s: enqueue_hcmd failed: %d\n",
 			  get_cmd_string(cmd->id), ret);
@@ -170,7 +175,7 @@ int iwl_send_cmd_sync(struct iwl_priv *priv, struct iwl_host_cmd *cmd)
 	if (cmd->meta.flags & CMD_WANT_SKB)
 		cmd->meta.source = &cmd->meta;
 
-	cmd_idx = priv->cfg->ops->utils->enqueue_hcmd(priv, cmd);
+	cmd_idx = iwl_enqueue_hcmd(priv, cmd);
 	if (cmd_idx < 0) {
 		ret = cmd_idx;
 		IWL_ERROR("Error sending %s: enqueue_hcmd failed: %d\n",
diff --git a/drivers/net/wireless/iwlwifi/iwl-helpers.h b/drivers/net/wireless/iwlwifi/iwl-helpers.h
index a443472bea62..41eed6793328 100644
--- a/drivers/net/wireless/iwlwifi/iwl-helpers.h
+++ b/drivers/net/wireless/iwlwifi/iwl-helpers.h
@@ -136,8 +136,8 @@ static inline void iwl_set_bits16(__le16 *dst, u8 pos, u8 len, int val)
 
 #define KELVIN_TO_CELSIUS(x) ((x)-273)
 #define CELSIUS_TO_KELVIN(x) ((x)+273)
+#define IWL_MASK(lo, hi) ((1 << (hi)) | ((1 << (hi)) - (1 << (lo))))
 
-#define IEEE80211_CHAN_W_RADAR_DETECT 0x00000010
 
 static inline struct ieee80211_conf *ieee80211_get_hw_conf(
 	struct ieee80211_hw *hw)
@@ -145,96 +145,6 @@ static inline struct ieee80211_conf *ieee80211_get_hw_conf(
 	return &hw->conf;
 }
 
-#define QOS_CONTROL_LEN 2
-
-
-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;
diff --git a/drivers/net/wireless/iwlwifi/iwl-led.c b/drivers/net/wireless/iwlwifi/iwl-led.c
index 03fdf5b434a1..899d7a2567a8 100644
--- a/drivers/net/wireless/iwlwifi/iwl-led.c
+++ b/drivers/net/wireless/iwlwifi/iwl-led.c
@@ -39,19 +39,26 @@
 #include <linux/etherdevice.h>
 #include <asm/unaligned.h>
 
-#include "iwl-4965.h"
+#include "iwl-dev.h"
 #include "iwl-core.h"
 #include "iwl-io.h"
 #include "iwl-helpers.h"
 
-#define IWL_1MB_RATE (128 * 1024)
-#define IWL_LED_THRESHOLD (16)
-#define IWL_MAX_BLINK_TBL (10)
+#ifdef CONFIG_IWLWIFI_DEBUG
+static const char *led_type_str[] = {
+	__stringify(IWL_LED_TRG_TX),
+	__stringify(IWL_LED_TRG_RX),
+	__stringify(IWL_LED_TRG_ASSOC),
+	__stringify(IWL_LED_TRG_RADIO),
+	NULL
+};
+#endif /* CONFIG_IWLWIFI_DEBUG */
+
 
 static const struct {
 	u16 tpt;
 	u8 on_time;
-	u8 of_time;
+	u8 off_time;
 } blink_tbl[] =
 {
 	{300, 25, 25},
@@ -63,26 +70,31 @@ static const struct {
 	{15, 95, 95 },
 	{10, 110, 110},
 	{5, 130, 130},
-	{0, 167, 167}
+	{0, 167, 167},
+/* SOLID_ON */
+	{-1, IWL_LED_SOLID, 0}
 };
 
-static int iwl_led_cmd_callback(struct iwl_priv *priv,
-				struct iwl_cmd *cmd, struct sk_buff *skb)
+#define IWL_1MB_RATE (128 * 1024)
+#define IWL_LED_THRESHOLD (16)
+#define IWL_MAX_BLINK_TBL (ARRAY_SIZE(blink_tbl) - 1) /* exclude SOLID_ON */
+#define IWL_SOLID_BLINK_IDX (ARRAY_SIZE(blink_tbl) - 1)
+
+/*  [0-256] -> [0..8] FIXME: we need [0..10] */
+static inline int iwl_brightness_to_idx(enum led_brightness brightness)
 {
-	return 1;
+	return fls(0x000000FF & (u32)brightness);
 }
 
-
 /* Send led command */
-static int iwl_send_led_cmd(struct iwl_priv *priv,
-			    struct iwl4965_led_cmd *led_cmd)
+static int iwl_send_led_cmd(struct iwl_priv *priv, struct iwl_led_cmd *led_cmd)
 {
 	struct iwl_host_cmd cmd = {
 		.id = REPLY_LEDS_CMD,
-		.len = sizeof(struct iwl4965_led_cmd),
+		.len = sizeof(struct iwl_led_cmd),
 		.data = led_cmd,
 		.meta.flags = CMD_ASYNC,
-		.meta.u.callback = iwl_led_cmd_callback
+		.meta.u.callback = NULL,
 	};
 	u32 reg;
 
@@ -93,33 +105,20 @@ static int iwl_send_led_cmd(struct iwl_priv *priv,
 	return iwl_send_cmd(priv, &cmd);
 }
 
-
-/* Set led on command */
-static int iwl4965_led_on(struct iwl_priv *priv, int led_id)
-{
-	struct iwl4965_led_cmd led_cmd = {
-		.id = led_id,
-		.on = IWL_LED_SOLID,
-		.off = 0,
-		.interval = IWL_DEF_LED_INTRVL
-	};
-	return iwl_send_led_cmd(priv, &led_cmd);
-}
-
-/* Set led on command */
+/* Set led pattern command */
 static int iwl4965_led_pattern(struct iwl_priv *priv, int led_id,
-			       enum led_brightness brightness)
+			       unsigned int idx)
 {
-	struct iwl4965_led_cmd led_cmd = {
+	struct iwl_led_cmd led_cmd = {
 		.id = led_id,
-		.on = brightness,
-		.off = brightness,
 		.interval = IWL_DEF_LED_INTRVL
 	};
-	if (brightness == LED_FULL) {
-		led_cmd.on = IWL_LED_SOLID;
-		led_cmd.off = 0;
-	}
+
+	BUG_ON(idx > IWL_MAX_BLINK_TBL);
+
+	led_cmd.on = blink_tbl[idx].on_time;
+	led_cmd.off = blink_tbl[idx].off_time;
+
 	return iwl_send_led_cmd(priv, &led_cmd);
 }
 
@@ -132,10 +131,22 @@ static int iwl4965_led_on_reg(struct iwl_priv *priv, int led_id)
 }
 
 #if 0
+/* Set led on command */
+static int iwl4965_led_on(struct iwl_priv *priv, int led_id)
+{
+	struct iwl_led_cmd led_cmd = {
+		.id = led_id,
+		.on = IWL_LED_SOLID,
+		.off = 0,
+		.interval = IWL_DEF_LED_INTRVL
+	};
+	return iwl_send_led_cmd(priv, &led_cmd);
+}
+
 /* Set led off command */
 int iwl4965_led_off(struct iwl_priv *priv, int led_id)
 {
-	struct iwl4965_led_cmd led_cmd = {
+	struct iwl_led_cmd led_cmd = {
 		.id = led_id,
 		.on = 0,
 		.off = 0,
@@ -155,25 +166,10 @@ static int iwl4965_led_off_reg(struct iwl_priv *priv, int led_id)
 	return 0;
 }
 
-/* Set led blink command */
-static int iwl4965_led_not_solid(struct iwl_priv *priv, int led_id,
-			       u8 brightness)
-{
-	struct iwl4965_led_cmd led_cmd = {
-		.id = led_id,
-		.on = brightness,
-		.off = brightness,
-		.interval = IWL_DEF_LED_INTRVL
-	};
-
-	return iwl_send_led_cmd(priv, &led_cmd);
-}
-
-
 /*
  * brightness call back function for Tx/Rx LED
  */
-static int iwl4965_led_associated(struct iwl_priv *priv, int led_id)
+static int iwl_led_associated(struct iwl_priv *priv, int led_id)
 {
 	if (test_bit(STATUS_EXIT_PENDING, &priv->status) ||
 	    !test_bit(STATUS_READY, &priv->status))
@@ -189,16 +185,18 @@ static int iwl4965_led_associated(struct iwl_priv *priv, int led_id)
 /*
  * brightness call back for association and radio
  */
-static void iwl4965_led_brightness_set(struct led_classdev *led_cdev,
+static void iwl_led_brightness_set(struct led_classdev *led_cdev,
 				       enum led_brightness brightness)
 {
-	struct iwl4965_led *led = container_of(led_cdev,
-					       struct iwl4965_led, led_dev);
+	struct iwl_led *led = container_of(led_cdev, struct iwl_led, led_dev);
 	struct iwl_priv *priv = led->priv;
 
 	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
 		return;
 
+
+	IWL_DEBUG_LED("Led type = %s brightness = %d\n",
+			led_type_str[led->type], brightness);
 	switch (brightness) {
 	case LED_FULL:
 		if (led->type == IWL_LED_TRG_ASSOC)
@@ -215,8 +213,10 @@ static void iwl4965_led_brightness_set(struct led_classdev *led_cdev,
 			led->led_off(priv, IWL_LED_LINK);
 		break;
 	default:
-		if (led->led_pattern)
-			led->led_pattern(priv, IWL_LED_LINK, brightness);
+		if (led->led_pattern) {
+			int idx = iwl_brightness_to_idx(brightness);
+			led->led_pattern(priv, IWL_LED_LINK, idx);
+		}
 		break;
 	}
 }
@@ -226,8 +226,7 @@ static void iwl4965_led_brightness_set(struct led_classdev *led_cdev,
 /*
  * Register led class with the system
  */
-static int iwl_leds_register_led(struct iwl_priv *priv,
-				   struct iwl4965_led *led,
+static int iwl_leds_register_led(struct iwl_priv *priv, struct iwl_led *led,
 				   enum led_type type, u8 set_led,
 				   const char *name, char *trigger)
 {
@@ -235,7 +234,7 @@ static int iwl_leds_register_led(struct iwl_priv *priv,
 	int ret;
 
 	led->led_dev.name = name;
-	led->led_dev.brightness_set = iwl4965_led_brightness_set;
+	led->led_dev.brightness_set = iwl_led_brightness_set;
 	led->led_dev.default_trigger = trigger;
 
 	led->priv = priv;
@@ -259,32 +258,28 @@ static int iwl_leds_register_led(struct iwl_priv *priv,
 /*
  * calculate blink rate according to last 2 sec Tx/Rx activities
  */
-static inline u8 get_blink_rate(struct iwl_priv *priv)
+static int iwl_get_blink_rate(struct iwl_priv *priv)
 {
 	int i;
-	u8 blink_rate;
-	u64 current_tpt = priv->tx_stats[2].bytes + priv->rx_stats[2].bytes;
+	u64 current_tpt = priv->tx_stats[2].bytes;
+	/* FIXME: + priv->rx_stats[2].bytes; */
 	s64 tpt = current_tpt - priv->led_tpt;
 
 	if (tpt < 0) /* wrapparound */
 		tpt = -tpt;
 
+	IWL_DEBUG_LED("tpt %lld current_tpt %lld\n", tpt, current_tpt);
 	priv->led_tpt = current_tpt;
 
-	if (tpt < IWL_LED_THRESHOLD) {
+	if (!priv->allow_blinking)
 		i = IWL_MAX_BLINK_TBL;
-	} else {
+	else
 		for (i = 0; i < IWL_MAX_BLINK_TBL; i++)
 			if (tpt  > (blink_tbl[i].tpt * IWL_1MB_RATE))
 				break;
-	}
-	/* if 0 frame is transfered */
-	if ((i == IWL_MAX_BLINK_TBL) || !priv->allow_blinking)
-		blink_rate = IWL_LED_SOLID;
-	else
-		blink_rate = blink_tbl[i].on_time;
 
-	return blink_rate;
+	IWL_DEBUG_LED("LED BLINK IDX=%d", i);
+	return i;
 }
 
 static inline int is_rf_kill(struct iwl_priv *priv)
@@ -300,7 +295,7 @@ static inline int is_rf_kill(struct iwl_priv *priv)
  */
 void iwl_leds_background(struct iwl_priv *priv)
 {
-	u8 blink_rate;
+	u8 blink_idx;
 
 	if (test_bit(STATUS_EXIT_PENDING, &priv->status)) {
 		priv->last_blink_time = 0;
@@ -313,9 +308,10 @@ void iwl_leds_background(struct iwl_priv *priv)
 
 	if (!priv->allow_blinking) {
 		priv->last_blink_time = 0;
-		if (priv->last_blink_rate != IWL_LED_SOLID) {
-			priv->last_blink_rate = IWL_LED_SOLID;
-			iwl4965_led_on(priv, IWL_LED_LINK);
+		if (priv->last_blink_rate != IWL_SOLID_BLINK_IDX) {
+			priv->last_blink_rate = IWL_SOLID_BLINK_IDX;
+			iwl4965_led_pattern(priv, IWL_LED_LINK,
+					    IWL_SOLID_BLINK_IDX);
 		}
 		return;
 	}
@@ -324,21 +320,14 @@ void iwl_leds_background(struct iwl_priv *priv)
 			msecs_to_jiffies(1000)))
 		return;
 
-	blink_rate = get_blink_rate(priv);
+	blink_idx = iwl_get_blink_rate(priv);
 
 	/* call only if blink rate change */
-	if (blink_rate != priv->last_blink_rate) {
-		if (blink_rate != IWL_LED_SOLID) {
-			priv->last_blink_time = jiffies +
-						msecs_to_jiffies(1000);
-			iwl4965_led_not_solid(priv, IWL_LED_LINK, blink_rate);
-		} else {
-			priv->last_blink_time = 0;
-			iwl4965_led_on(priv, IWL_LED_LINK);
-		}
-	}
+	if (blink_idx != priv->last_blink_rate)
+		iwl4965_led_pattern(priv, IWL_LED_LINK, blink_idx);
 
-	priv->last_blink_rate = blink_rate;
+	priv->last_blink_time = jiffies;
+	priv->last_blink_rate = blink_idx;
 }
 EXPORT_SYMBOL(iwl_leds_background);
 
@@ -362,10 +351,8 @@ int iwl_leds_register(struct iwl_priv *priv)
 	priv->led[IWL_LED_TRG_RADIO].led_off = iwl4965_led_off_reg;
 	priv->led[IWL_LED_TRG_RADIO].led_pattern = NULL;
 
-	ret = iwl_leds_register_led(priv,
-				   &priv->led[IWL_LED_TRG_RADIO],
-				   IWL_LED_TRG_RADIO, 1,
-				   name, trigger);
+	ret = iwl_leds_register_led(priv, &priv->led[IWL_LED_TRG_RADIO],
+				   IWL_LED_TRG_RADIO, 1, name, trigger);
 	if (ret)
 		goto exit_fail;
 
@@ -373,10 +360,9 @@ int iwl_leds_register(struct iwl_priv *priv)
 	snprintf(name, sizeof(name), "iwl-%s:assoc",
 		 wiphy_name(priv->hw->wiphy));
 
-	ret = iwl_leds_register_led(priv,
-				   &priv->led[IWL_LED_TRG_ASSOC],
-				   IWL_LED_TRG_ASSOC, 0,
-				   name, trigger);
+	ret = iwl_leds_register_led(priv, &priv->led[IWL_LED_TRG_ASSOC],
+				   IWL_LED_TRG_ASSOC, 0, name, trigger);
+
 	/* for assoc always turn led on */
 	priv->led[IWL_LED_TRG_ASSOC].led_on = iwl4965_led_on_reg;
 	priv->led[IWL_LED_TRG_ASSOC].led_off = iwl4965_led_on_reg;
@@ -386,31 +372,26 @@ int iwl_leds_register(struct iwl_priv *priv)
 		goto exit_fail;
 
 	trigger = ieee80211_get_rx_led_name(priv->hw);
-	snprintf(name, sizeof(name), "iwl-%s:RX",
-		 wiphy_name(priv->hw->wiphy));
+	snprintf(name, sizeof(name), "iwl-%s:RX", wiphy_name(priv->hw->wiphy));
 
 
-	ret = iwl_leds_register_led(priv,
-				   &priv->led[IWL_LED_TRG_RX],
-				   IWL_LED_TRG_RX, 0,
-				   name, trigger);
+	ret = iwl_leds_register_led(priv, &priv->led[IWL_LED_TRG_RX],
+				   IWL_LED_TRG_RX, 0, name, trigger);
 
-	priv->led[IWL_LED_TRG_RX].led_on = iwl4965_led_associated;
-	priv->led[IWL_LED_TRG_RX].led_off = iwl4965_led_associated;
+	priv->led[IWL_LED_TRG_RX].led_on = iwl_led_associated;
+	priv->led[IWL_LED_TRG_RX].led_off = iwl_led_associated;
 	priv->led[IWL_LED_TRG_RX].led_pattern = iwl4965_led_pattern;
 
 	if (ret)
 		goto exit_fail;
 
 	trigger = ieee80211_get_tx_led_name(priv->hw);
-	snprintf(name, sizeof(name), "iwl-%s:TX",
-		 wiphy_name(priv->hw->wiphy));
-	ret = iwl_leds_register_led(priv,
-				   &priv->led[IWL_LED_TRG_TX],
-				   IWL_LED_TRG_TX, 0,
-				   name, trigger);
-	priv->led[IWL_LED_TRG_TX].led_on = iwl4965_led_associated;
-	priv->led[IWL_LED_TRG_TX].led_off = iwl4965_led_associated;
+	snprintf(name, sizeof(name), "iwl-%s:TX", wiphy_name(priv->hw->wiphy));
+	ret = iwl_leds_register_led(priv, &priv->led[IWL_LED_TRG_TX],
+				   IWL_LED_TRG_TX, 0, name, trigger);
+
+	priv->led[IWL_LED_TRG_TX].led_on = iwl_led_associated;
+	priv->led[IWL_LED_TRG_TX].led_off = iwl_led_associated;
 	priv->led[IWL_LED_TRG_TX].led_pattern = iwl4965_led_pattern;
 
 	if (ret)
@@ -425,7 +406,7 @@ exit_fail:
 EXPORT_SYMBOL(iwl_leds_register);
 
 /* unregister led class */
-static void iwl_leds_unregister_led(struct iwl4965_led *led, u8 set_led)
+static void iwl_leds_unregister_led(struct iwl_led *led, u8 set_led)
 {
 	if (!led->registered)
 		return;
diff --git a/drivers/net/wireless/iwlwifi/iwl-led.h b/drivers/net/wireless/iwlwifi/iwl-led.h
index 5bb04128cd65..1980ae5a7e82 100644
--- a/drivers/net/wireless/iwlwifi/iwl-led.h
+++ b/drivers/net/wireless/iwlwifi/iwl-led.h
@@ -49,14 +49,13 @@ enum led_type {
 };
 
 
-struct iwl4965_led {
+struct iwl_led {
 	struct iwl_priv *priv;
 	struct led_classdev led_dev;
 
 	int (*led_on) (struct iwl_priv *priv, int led_id);
 	int (*led_off) (struct iwl_priv *priv, int led_id);
-	int (*led_pattern) (struct iwl_priv *priv, int led_id,
-			    enum led_brightness brightness);
+	int (*led_pattern) (struct iwl_priv *priv, int led_id, unsigned int idx);
 
 	enum led_type type;
 	unsigned int registered;
diff --git a/drivers/net/wireless/iwlwifi/iwl-power.c b/drivers/net/wireless/iwlwifi/iwl-power.c
new file mode 100644
index 000000000000..2e71803e09ba
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-power.c
@@ -0,0 +1,423 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2008 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
+ *****************************************************************************/
+
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/version.h>
+#include <linux/init.h>
+
+#include <net/mac80211.h>
+
+#include "iwl-eeprom.h"
+#include "iwl-dev.h"
+#include "iwl-core.h"
+#include "iwl-commands.h"
+#include "iwl-debug.h"
+#include "iwl-power.h"
+#include "iwl-helpers.h"
+
+/*
+ * Setting power level allow the card to go to sleep when not busy
+ * there are three factor that decide the power level to go to, they
+ * are list here with its priority
+ *  1- critical_power_setting this will be set according to card temperature.
+ *  2- system_power_setting this will be set by system PM manager.
+ *  3- user_power_setting this will be set by user either by writing to sys or
+ *  	mac80211
+ *
+ * if system_power_setting and user_power_setting is set to auto
+ * the power level will be decided according to association status and battery
+ * status.
+ *
+ */
+
+#define MSEC_TO_USEC 1024
+#define IWL_POWER_RANGE_0_MAX  (2)
+#define IWL_POWER_RANGE_1_MAX  (10)
+
+
+#define NOSLP __constant_cpu_to_le16(0), 0, 0
+#define SLP IWL_POWER_DRIVER_ALLOW_SLEEP_MSK, 0, 0
+#define SLP_TOUT(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)}
+
+#define IWL_POWER_ON_BATTERY		IWL_POWER_INDEX_5
+#define IWL_POWER_ON_AC_DISASSOC	IWL_POWER_MODE_CAM
+#define IWL_POWER_ON_AC_ASSOC		IWL_POWER_MODE_CAM
+
+
+#define IWL_CT_KILL_TEMPERATURE		110
+#define IWL_MIN_POWER_TEMPERATURE	100
+#define IWL_REDUCED_POWER_TEMPERATURE	95
+
+/* 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_TOUT(0), SLP_TOUT(0), SLP_VEC(0, 0, 0, 0, 0)}, 0},
+	{{SLP, SLP_TOUT(200), SLP_TOUT(500), SLP_VEC(1, 2, 2, 2, 0xFF)}, 0},
+	{{SLP, SLP_TOUT(200), SLP_TOUT(300), SLP_VEC(1, 2, 2, 2, 0xFF)}, 0},
+	{{SLP, SLP_TOUT(50), SLP_TOUT(100), SLP_VEC(2, 2, 2, 2, 0xFF)}, 0},
+	{{SLP, SLP_TOUT(50), SLP_TOUT(25), SLP_VEC(2, 2, 4, 4, 0xFF)}, 1},
+	{{SLP, SLP_TOUT(25), SLP_TOUT(25), SLP_VEC(2, 2, 4, 6, 0xFF)}, 2}
+};
+
+
+/* for tim = 3-10 */
+static struct iwl_power_vec_entry range_1[IWL_POWER_AC] = {
+	{{NOSLP, SLP_TOUT(0), SLP_TOUT(0), SLP_VEC(0, 0, 0, 0, 0)}, 0},
+	{{SLP, SLP_TOUT(200), SLP_TOUT(500), SLP_VEC(1, 2, 3, 4, 4)}, 0},
+	{{SLP, SLP_TOUT(200), SLP_TOUT(300), SLP_VEC(1, 2, 3, 4, 7)}, 0},
+	{{SLP, SLP_TOUT(50), SLP_TOUT(100), SLP_VEC(2, 4, 6, 7, 9)}, 0},
+	{{SLP, SLP_TOUT(50), SLP_TOUT(25), SLP_VEC(2, 4, 6, 9, 10)}, 1},
+	{{SLP, SLP_TOUT(25), SLP_TOUT(25), SLP_VEC(2, 4, 7, 10, 10)}, 2}
+};
+
+/* for tim > 11 */
+static struct iwl_power_vec_entry range_2[IWL_POWER_AC] = {
+	{{NOSLP, SLP_TOUT(0), SLP_TOUT(0), SLP_VEC(0, 0, 0, 0, 0)}, 0},
+	{{SLP, SLP_TOUT(200), SLP_TOUT(500), SLP_VEC(1, 2, 3, 4, 0xFF)}, 0},
+	{{SLP, SLP_TOUT(200), SLP_TOUT(300), SLP_VEC(2, 4, 6, 7, 0xFF)}, 0},
+	{{SLP, SLP_TOUT(50), SLP_TOUT(100), SLP_VEC(2, 7, 9, 9, 0xFF)}, 0},
+	{{SLP, SLP_TOUT(50), SLP_TOUT(25), SLP_VEC(2, 7, 9, 9, 0xFF)}, 0},
+	{{SLP, SLP_TOUT(25), SLP_TOUT(25), SLP_VEC(4, 7, 10, 10, 0xFF)}, 0}
+};
+
+/* decide the right power level according to association status
+ * and battery status
+ */
+static u16 iwl_get_auto_power_mode(struct iwl_priv *priv)
+{
+	u16 mode = priv->power_data.user_power_setting;
+
+	switch (priv->power_data.user_power_setting) {
+	case IWL_POWER_AUTO:
+		/* if running on battery */
+		if (priv->power_data.is_battery_active)
+			mode = IWL_POWER_ON_BATTERY;
+		else if (iwl_is_associated(priv))
+			mode = IWL_POWER_ON_AC_ASSOC;
+		else
+			mode = IWL_POWER_ON_AC_DISASSOC;
+		break;
+	case IWL_POWER_BATTERY:
+		mode = IWL_POWER_INDEX_3;
+		break;
+	case IWL_POWER_AC:
+		mode = IWL_POWER_MODE_CAM;
+		break;
+	}
+	return mode;
+}
+
+/* initialize to default */
+static int iwl_power_init_handle(struct iwl_priv *priv)
+{
+	int ret = 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));
+
+	memcpy(&pow_data->pwr_range_0[0], &range_0[0], size);
+	memcpy(&pow_data->pwr_range_1[0], &range_1[0], size);
+	memcpy(&pow_data->pwr_range_2[0], &range_2[0], size);
+
+	ret = pci_read_config_word(priv->pci_dev,
+				  PCI_LINK_CTRL, &pci_pm);
+	if (ret != 0)
+		return 0;
+	else {
+		struct iwl4965_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 ret;
+}
+
+/* adjust power command according to dtim period and power level*/
+static int iwl_update_power_command(struct iwl_priv *priv,
+				    struct iwl4965_powertable_cmd *cmd,
+				    u16 mode)
+{
+	int ret = 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->dtim_period <= IWL_POWER_RANGE_0_MAX)
+		range = &pow_data->pwr_range_0[0];
+	else if (pow_data->dtim_period <= IWL_POWER_RANGE_1_MAX)
+		range = &pow_data->pwr_range_1[0];
+	else
+		range = &pow_data->pwr_range_2[0];
+
+	period = pow_data->dtim_period;
+	memcpy(cmd, &range[mode].cmd, sizeof(struct iwl4965_powertable_cmd));
+
+	if (period == 0) {
+		period = 1;
+		skip = 0;
+	} else
+		skip = range[mode].no_dtim;
+
+	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);
+		if (max_sleep == 0xFF)
+			max_sleep = period * (skip + 1);
+		else if (max_sleep >  period)
+			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 ret;
+}
+
+
+/*
+ * calucaute the final power mode index
+ */
+int iwl_power_update_mode(struct iwl_priv *priv, u8 refresh)
+{
+	struct iwl_power_mgr *setting = &(priv->power_data);
+	int ret = 0;
+	u16 uninitialized_var(final_mode);
+
+       /* If on battery, set to 3,
+	* if plugged into AC power, set to CAM ("continuously aware mode"),
+	* else user level */
+
+	switch (setting->system_power_setting) {
+	case IWL_POWER_AUTO:
+		final_mode = iwl_get_auto_power_mode(priv);
+		break;
+	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 = setting->system_power_setting;
+	}
+
+	if (setting->critical_power_setting > final_mode)
+		final_mode = setting->critical_power_setting;
+
+	/* driver only support CAM for non STA network */
+	if (priv->iw_mode != IEEE80211_IF_TYPE_STA)
+		final_mode = IWL_POWER_MODE_CAM;
+
+	if (!iwl_is_rfkill(priv) && !setting->power_disabled &&
+	    ((setting->power_mode != final_mode) || refresh)) {
+		struct iwl4965_powertable_cmd cmd;
+
+		if (final_mode != IWL_POWER_MODE_CAM)
+			set_bit(STATUS_POWER_PMI, &priv->status);
+
+		iwl_update_power_command(priv, &cmd, final_mode);
+		cmd.keep_alive_beacons = 0;
+
+		if (final_mode == IWL_POWER_INDEX_5)
+			cmd.flags |= IWL_POWER_FAST_PD;
+
+		if (priv->cfg->ops->lib->set_power)
+			ret = priv->cfg->ops->lib->set_power(priv, &cmd);
+
+		if (final_mode == IWL_POWER_MODE_CAM)
+			clear_bit(STATUS_POWER_PMI, &priv->status);
+		else
+			set_bit(STATUS_POWER_PMI, &priv->status);
+
+		if (priv->cfg->ops->lib->update_chain_flags)
+			priv->cfg->ops->lib->update_chain_flags(priv);
+
+		if (!ret)
+			setting->power_mode = final_mode;
+	}
+
+	return ret;
+}
+EXPORT_SYMBOL(iwl_power_update_mode);
+
+/* Allow other iwl code to disable/enable power management active
+ * this will be usefull for rate scale to disable PM during heavy
+ * Tx/Rx activities
+ */
+int iwl_power_disable_management(struct iwl_priv *priv)
+{
+	u16 prev_mode;
+	int ret = 0;
+
+	if (priv->power_data.power_disabled)
+		return -EBUSY;
+
+	prev_mode = priv->power_data.user_power_setting;
+	priv->power_data.user_power_setting = IWL_POWER_MODE_CAM;
+	ret = iwl_power_update_mode(priv, 0);
+	priv->power_data.power_disabled = 1;
+	priv->power_data.user_power_setting = prev_mode;
+
+	return ret;
+}
+EXPORT_SYMBOL(iwl_power_disable_management);
+
+/* Allow other iwl code to disable/enable power management active
+ * this will be usefull for rate scale to disable PM during hight
+ * valume activities
+ */
+int iwl_power_enable_management(struct iwl_priv *priv)
+{
+	int ret = 0;
+
+	priv->power_data.power_disabled = 0;
+	ret = iwl_power_update_mode(priv, 0);
+	return ret;
+}
+EXPORT_SYMBOL(iwl_power_enable_management);
+
+/* set user_power_setting */
+int iwl_power_set_user_mode(struct iwl_priv *priv, u16 mode)
+{
+	int ret = 0;
+
+	if (mode > IWL_POWER_LIMIT)
+		return -EINVAL;
+
+	priv->power_data.user_power_setting = mode;
+
+	ret = iwl_power_update_mode(priv, 0);
+
+	return ret;
+}
+EXPORT_SYMBOL(iwl_power_set_user_mode);
+
+
+/* set system_power_setting. This should be set by over all
+ * PM application.
+ */
+int iwl_power_set_system_mode(struct iwl_priv *priv, u16 mode)
+{
+	int ret = 0;
+
+	if (mode > IWL_POWER_LIMIT)
+		return -EINVAL;
+
+	priv->power_data.system_power_setting = mode;
+
+	ret = iwl_power_update_mode(priv, 0);
+
+	return ret;
+}
+EXPORT_SYMBOL(iwl_power_set_system_mode);
+
+/* initilize to default */
+void iwl_power_initialize(struct iwl_priv *priv)
+{
+
+	iwl_power_init_handle(priv);
+	priv->power_data.user_power_setting = IWL_POWER_AUTO;
+	priv->power_data.power_disabled = 0;
+	priv->power_data.system_power_setting = IWL_POWER_AUTO;
+	priv->power_data.is_battery_active = 0;
+	priv->power_data.power_disabled = 0;
+	priv->power_data.critical_power_setting = 0;
+}
+EXPORT_SYMBOL(iwl_power_initialize);
+
+/* set critical_power_setting according to temperature value */
+int iwl_power_temperature_change(struct iwl_priv *priv)
+{
+	int ret = 0;
+	u16 new_critical = priv->power_data.critical_power_setting;
+	s32 temperature = KELVIN_TO_CELSIUS(priv->last_temperature);
+
+	if (temperature > IWL_CT_KILL_TEMPERATURE)
+		return 0;
+	else if (temperature > IWL_MIN_POWER_TEMPERATURE)
+		new_critical = IWL_POWER_INDEX_5;
+	else if (temperature > IWL_REDUCED_POWER_TEMPERATURE)
+		new_critical = IWL_POWER_INDEX_3;
+	else
+		new_critical = IWL_POWER_MODE_CAM;
+
+	if (new_critical != priv->power_data.critical_power_setting)
+		priv->power_data.critical_power_setting = new_critical;
+
+	if (priv->power_data.critical_power_setting >
+				priv->power_data.power_mode)
+		ret = iwl_power_update_mode(priv, 0);
+
+	return ret;
+}
+EXPORT_SYMBOL(iwl_power_temperature_change);
diff --git a/drivers/net/wireless/iwlwifi/iwl-power.h b/drivers/net/wireless/iwlwifi/iwl-power.h
new file mode 100644
index 000000000000..b066724a1c2b
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-power.h
@@ -0,0 +1,76 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2007 - 2008 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_power_setting_h__
+#define __iwl_power_setting_h__
+
+#include <net/mac80211.h>
+#include "iwl-commands.h"
+
+struct iwl_priv;
+
+#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_AUTO		0x08
+#define IWL_POWER_LIMIT		0x08
+#define IWL_POWER_MASK		0x0F
+#define IWL_POWER_ENABLED	0x10
+
+/* Power management (not Tx power) structures */
+
+struct iwl_power_vec_entry {
+	struct iwl4965_powertable_cmd cmd;
+	u8 no_dtim;
+};
+
+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];
+	struct iwl_power_vec_entry pwr_range_2[IWL_POWER_AC];
+	u32 dtim_period;
+	/* final power level that used to calculate final power command */
+	u8 power_mode;
+	u8 user_power_setting; /* set by user through mac80211 or sysfs */
+	u8 system_power_setting; /* set by kernel syatem tools */
+	u8 critical_power_setting; /* set if driver over heated */
+	u8 is_battery_active; /* DC/AC power */
+	u8 power_disabled; /* flag to disable using power saving level */
+};
+
+int iwl_power_update_mode(struct iwl_priv *priv, u8 refresh);
+int iwl_power_disable_management(struct iwl_priv *priv);
+int iwl_power_enable_management(struct iwl_priv *priv);
+int iwl_power_set_user_mode(struct iwl_priv *priv, u16 mode);
+int iwl_power_set_system_mode(struct iwl_priv *priv, u16 mode);
+void iwl_power_initialize(struct iwl_priv *priv);
+int iwl_power_temperature_change(struct iwl_priv *priv);
+
+#endif  /* __iwl_power_setting_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-prph.h b/drivers/net/wireless/iwlwifi/iwl-prph.h
index c9cf8eef1a90..70d9c7568b98 100644
--- a/drivers/net/wireless/iwlwifi/iwl-prph.h
+++ b/drivers/net/wireless/iwlwifi/iwl-prph.h
@@ -239,40 +239,307 @@
 #define ALM_SCD_SBYP_MODE_1_REG             (ALM_SCD_BASE + 0x02C)
 #define ALM_SCD_SBYP_MODE_2_REG             (ALM_SCD_BASE + 0x030)
 
+/**
+ * Tx Scheduler
+ *
+ * The Tx Scheduler selects the next frame to be transmitted, chosing TFDs
+ * (Transmit Frame Descriptors) from up to 16 circular Tx queues resident in
+ * host DRAM.  It steers each frame's Tx command (which contains the frame
+ * data) into one of up to 7 prioritized Tx DMA FIFO channels within the
+ * device.  A queue maps to only one (selectable by driver) Tx DMA channel,
+ * but one DMA channel may take input from several queues.
+ *
+ * Tx DMA channels have dedicated purposes.  For 4965, they are used as follows:
+ *
+ * 0 -- EDCA BK (background) frames, lowest priority
+ * 1 -- EDCA BE (best effort) frames, normal priority
+ * 2 -- EDCA VI (video) frames, higher priority
+ * 3 -- EDCA VO (voice) and management frames, highest priority
+ * 4 -- Commands (e.g. RXON, etc.)
+ * 5 -- HCCA short frames
+ * 6 -- HCCA long frames
+ * 7 -- not used by driver (device-internal only)
+ *
+ * Driver should normally map queues 0-6 to Tx DMA/FIFO channels 0-6.
+ * In addition, driver can map queues 7-15 to Tx DMA/FIFO channels 0-3 to
+ * support 11n aggregation via EDCA DMA channels.
+ *
+ * The driver sets up each queue to work in one of two modes:
+ *
+ * 1)  Scheduler-Ack, in which the scheduler automatically supports a
+ *     block-ack (BA) window of up to 64 TFDs.  In this mode, each queue
+ *     contains TFDs for a unique combination of Recipient Address (RA)
+ *     and Traffic Identifier (TID), that is, traffic of a given
+ *     Quality-Of-Service (QOS) priority, destined for a single station.
+ *
+ *     In scheduler-ack mode, the scheduler keeps track of the Tx status of
+ *     each frame within the BA window, including whether it's been transmitted,
+ *     and whether it's been acknowledged by the receiving station.  The device
+ *     automatically processes block-acks received from the receiving STA,
+ *     and reschedules un-acked frames to be retransmitted (successful
+ *     Tx completion may end up being out-of-order).
+ *
+ *     The driver must maintain the queue's Byte Count table in host DRAM
+ *     (struct iwl4965_sched_queue_byte_cnt_tbl) for this mode.
+ *     This mode does not support fragmentation.
+ *
+ * 2)  FIFO (a.k.a. non-Scheduler-ACK), in which each TFD is processed in order.
+ *     The device may automatically retry Tx, but will retry only one frame
+ *     at a time, until receiving ACK from receiving station, or reaching
+ *     retry limit and giving up.
+ *
+ *     The command queue (#4) must use this mode!
+ *     This mode does not require use of the Byte Count table in host DRAM.
+ *
+ * Driver controls scheduler operation via 3 means:
+ * 1)  Scheduler registers
+ * 2)  Shared scheduler data base in internal 4956 SRAM
+ * 3)  Shared data in host DRAM
+ *
+ * Initialization:
+ *
+ * When loading, driver should allocate memory for:
+ * 1)  16 TFD circular buffers, each with space for (typically) 256 TFDs.
+ * 2)  16 Byte Count circular buffers in 16 KBytes contiguous memory
+ *     (1024 bytes for each queue).
+ *
+ * After receiving "Alive" response from uCode, driver must initialize
+ * the scheduler (especially for queue #4, the command queue, otherwise
+ * the driver can't issue commands!):
+ */
+
+/**
+ * Max Tx window size is the max number of contiguous TFDs that the scheduler
+ * can keep track of at one time when creating block-ack chains of frames.
+ * Note that "64" matches the number of ack bits in a block-ack packet.
+ * Driver should use SCD_WIN_SIZE and SCD_FRAME_LIMIT values to initialize
+ * IWL49_SCD_CONTEXT_QUEUE_OFFSET(x) values.
+ */
+#define SCD_WIN_SIZE				64
+#define SCD_FRAME_LIMIT				64
+
+/* SCD registers are internal, must be accessed via HBUS_TARG_PRPH regs */
+#define IWL49_SCD_START_OFFSET		0xa02c00
+
+/*
+ * 4965 tells driver SRAM address for internal scheduler structs via this reg.
+ * Value is valid only after "Alive" response from uCode.
+ */
+#define IWL49_SCD_SRAM_BASE_ADDR           (IWL49_SCD_START_OFFSET + 0x0)
+
+/*
+ * Driver may need to update queue-empty bits after changing queue's
+ * write and read pointers (indexes) during (re-)initialization (i.e. when
+ * scheduler is not tracking what's happening).
+ * Bit fields:
+ * 31-16:  Write mask -- 1: update empty bit, 0: don't change empty bit
+ * 15-00:  Empty state, one for each queue -- 1: empty, 0: non-empty
+ * NOTE:  This register is not used by Linux driver.
+ */
+#define IWL49_SCD_EMPTY_BITS               (IWL49_SCD_START_OFFSET + 0x4)
+
+/*
+ * Physical base address of array of byte count (BC) circular buffers (CBs).
+ * Each Tx queue has a BC CB in host DRAM to support Scheduler-ACK mode.
+ * This register points to BC CB for queue 0, must be on 1024-byte boundary.
+ * Others are spaced by 1024 bytes.
+ * Each BC CB is 2 bytes * (256 + 64) = 740 bytes, followed by 384 bytes pad.
+ * (Index into a queue's BC CB) = (index into queue's TFD CB) = (SSN & 0xff).
+ * Bit fields:
+ * 25-00:  Byte Count CB physical address [35:10], must be 1024-byte aligned.
+ */
+#define IWL49_SCD_DRAM_BASE_ADDR           (IWL49_SCD_START_OFFSET + 0x10)
+
+/*
+ * Enables any/all Tx DMA/FIFO channels.
+ * Scheduler generates requests for only the active channels.
+ * Set this to 0xff to enable all 8 channels (normal usage).
+ * Bit fields:
+ *  7- 0:  Enable (1), disable (0), one bit for each channel 0-7
+ */
+#define IWL49_SCD_TXFACT                   (IWL49_SCD_START_OFFSET + 0x1c)
+/*
+ * Queue (x) Write Pointers (indexes, really!), one for each Tx queue.
+ * Initialized and updated by driver as new TFDs are added to queue.
+ * NOTE:  If using Block Ack, index must correspond to frame's
+ *        Start Sequence Number; index = (SSN & 0xff)
+ * NOTE:  Alternative to HBUS_TARG_WRPTR, which is what Linux driver uses?
+ */
+#define IWL49_SCD_QUEUE_WRPTR(x)  (IWL49_SCD_START_OFFSET + 0x24 + (x) * 4)
+
+/*
+ * Queue (x) Read Pointers (indexes, really!), one for each Tx queue.
+ * For FIFO mode, index indicates next frame to transmit.
+ * For Scheduler-ACK mode, index indicates first frame in Tx window.
+ * Initialized by driver, updated by scheduler.
+ */
+#define IWL49_SCD_QUEUE_RDPTR(x)  (IWL49_SCD_START_OFFSET + 0x64 + (x) * 4)
+
+/*
+ * Select which queues work in chain mode (1) vs. not (0).
+ * Use chain mode to build chains of aggregated frames.
+ * Bit fields:
+ * 31-16:  Reserved
+ * 15-00:  Mode, one bit for each queue -- 1: Chain mode, 0: one-at-a-time
+ * NOTE:  If driver sets up queue for chain mode, it should be also set up
+ *        Scheduler-ACK mode as well, via SCD_QUEUE_STATUS_BITS(x).
+ */
+#define IWL49_SCD_QUEUECHAIN_SEL  (IWL49_SCD_START_OFFSET + 0xd0)
+
+/*
+ * Select which queues interrupt driver when scheduler increments
+ * a queue's read pointer (index).
+ * Bit fields:
+ * 31-16:  Reserved
+ * 15-00:  Interrupt enable, one bit for each queue -- 1: enabled, 0: disabled
+ * NOTE:  This functionality is apparently a no-op; driver relies on interrupts
+ *        from Rx queue to read Tx command responses and update Tx queues.
+ */
+#define IWL49_SCD_INTERRUPT_MASK  (IWL49_SCD_START_OFFSET + 0xe4)
+
+/*
+ * Queue search status registers.  One for each queue.
+ * Sets up queue mode and assigns queue to Tx DMA channel.
+ * Bit fields:
+ * 19-10: Write mask/enable bits for bits 0-9
+ *     9: Driver should init to "0"
+ *     8: Scheduler-ACK mode (1), non-Scheduler-ACK (i.e. FIFO) mode (0).
+ *        Driver should init to "1" for aggregation mode, or "0" otherwise.
+ *   7-6: Driver should init to "0"
+ *     5: Window Size Left; indicates whether scheduler can request
+ *        another TFD, based on window size, etc.  Driver should init
+ *        this bit to "1" for aggregation mode, or "0" for non-agg.
+ *   4-1: Tx FIFO to use (range 0-7).
+ *     0: Queue is active (1), not active (0).
+ * Other bits should be written as "0"
+ *
+ * NOTE:  If enabling Scheduler-ACK mode, chain mode should also be enabled
+ *        via SCD_QUEUECHAIN_SEL.
+ */
+#define IWL49_SCD_QUEUE_STATUS_BITS(x)\
+	(IWL49_SCD_START_OFFSET + 0x104 + (x) * 4)
+
+/* Bit field positions */
+#define IWL49_SCD_QUEUE_STTS_REG_POS_ACTIVE	(0)
+#define IWL49_SCD_QUEUE_STTS_REG_POS_TXF	(1)
+#define IWL49_SCD_QUEUE_STTS_REG_POS_WSL	(5)
+#define IWL49_SCD_QUEUE_STTS_REG_POS_SCD_ACK	(8)
+
+/* Write masks */
+#define IWL49_SCD_QUEUE_STTS_REG_POS_SCD_ACT_EN	(10)
+#define IWL49_SCD_QUEUE_STTS_REG_MSK		(0x0007FC00)
+
+/**
+ * 4965 internal SRAM structures for scheduler, shared with driver ...
+ *
+ * Driver should clear and initialize the following areas after receiving
+ * "Alive" response from 4965 uCode, i.e. after initial
+ * uCode load, or after a uCode load done for error recovery:
+ *
+ * SCD_CONTEXT_DATA_OFFSET (size 128 bytes)
+ * SCD_TX_STTS_BITMAP_OFFSET (size 256 bytes)
+ * SCD_TRANSLATE_TBL_OFFSET (size 32 bytes)
+ *
+ * Driver accesses SRAM via HBUS_TARG_MEM_* registers.
+ * Driver reads base address of this scheduler area from SCD_SRAM_BASE_ADDR.
+ * All OFFSET values must be added to this base address.
+ */
+
+/*
+ * Queue context.  One 8-byte entry for each of 16 queues.
+ *
+ * Driver should clear this entire area (size 0x80) to 0 after receiving
+ * "Alive" notification from uCode.  Additionally, driver should init
+ * each queue's entry as follows:
+ *
+ * LS Dword bit fields:
+ *  0-06:  Max Tx window size for Scheduler-ACK.  Driver should init to 64.
+ *
+ * MS Dword bit fields:
+ * 16-22:  Frame limit.  Driver should init to 10 (0xa).
+ *
+ * Driver should init all other bits to 0.
+ *
+ * Init must be done after driver receives "Alive" response from 4965 uCode,
+ * and when setting up queue for aggregation.
+ */
+#define IWL49_SCD_CONTEXT_DATA_OFFSET			0x380
+#define IWL49_SCD_CONTEXT_QUEUE_OFFSET(x) \
+			(IWL49_SCD_CONTEXT_DATA_OFFSET + ((x) * 8))
+
+#define IWL49_SCD_QUEUE_CTX_REG1_WIN_SIZE_POS		(0)
+#define IWL49_SCD_QUEUE_CTX_REG1_WIN_SIZE_MSK		(0x0000007F)
+#define IWL49_SCD_QUEUE_CTX_REG2_FRAME_LIMIT_POS	(16)
+#define IWL49_SCD_QUEUE_CTX_REG2_FRAME_LIMIT_MSK	(0x007F0000)
+
+/*
+ * Tx Status Bitmap
+ *
+ * Driver should clear this entire area (size 0x100) to 0 after receiving
+ * "Alive" notification from uCode.  Area is used only by device itself;
+ * no other support (besides clearing) is required from driver.
+ */
+#define IWL49_SCD_TX_STTS_BITMAP_OFFSET		0x400
+
 /*
- * 4965 Tx Scheduler registers.
- * Details are documented in iwl-4965-hw.h
+ * RAxTID to queue translation mapping.
+ *
+ * When queue is in Scheduler-ACK mode, frames placed in a that queue must be
+ * for only one combination of receiver address (RA) and traffic ID (TID), i.e.
+ * one QOS priority level destined for one station (for this wireless link,
+ * not final destination).  The SCD_TRANSLATE_TABLE area provides 16 16-bit
+ * mappings, one for each of the 16 queues.  If queue is not in Scheduler-ACK
+ * mode, the device ignores the mapping value.
+ *
+ * Bit fields, for each 16-bit map:
+ * 15-9:  Reserved, set to 0
+ *  8-4:  Index into device's station table for recipient station
+ *  3-0:  Traffic ID (tid), range 0-15
+ *
+ * Driver should clear this entire area (size 32 bytes) to 0 after receiving
+ * "Alive" notification from uCode.  To update a 16-bit map value, driver
+ * must read a dword-aligned value from device SRAM, replace the 16-bit map
+ * value of interest, and write the dword value back into device SRAM.
  */
-#define IWL49_SCD_BASE		(PRPH_BASE + 0xa02c00)
-
-#define IWL49_SCD_SRAM_BASE_ADDR         (IWL49_SCD_BASE + 0x0)
-#define IWL49_SCD_EMPTY_BITS             (IWL49_SCD_BASE + 0x4)
-#define IWL49_SCD_DRAM_BASE_ADDR         (IWL49_SCD_BASE + 0x10)
-#define IWL49_SCD_AIT                    (IWL49_SCD_BASE + 0x18)
-#define IWL49_SCD_TXFACT                 (IWL49_SCD_BASE + 0x1c)
-#define IWL49_SCD_QUEUE_WRPTR(x)         (IWL49_SCD_BASE + 0x24 + (x) * 4)
-#define IWL49_SCD_QUEUE_RDPTR(x)         (IWL49_SCD_BASE + 0x64 + (x) * 4)
-#define IWL49_SCD_SETQUEUENUM            (IWL49_SCD_BASE + 0xa4)
-#define IWL49_SCD_SET_TXSTAT_TXED        (IWL49_SCD_BASE + 0xa8)
-#define IWL49_SCD_SET_TXSTAT_DONE        (IWL49_SCD_BASE + 0xac)
-#define IWL49_SCD_SET_TXSTAT_NOT_SCHD    (IWL49_SCD_BASE + 0xb0)
-#define IWL49_SCD_DECREASE_CREDIT        (IWL49_SCD_BASE + 0xb4)
-#define IWL49_SCD_DECREASE_SCREDIT       (IWL49_SCD_BASE + 0xb8)
-#define IWL49_SCD_LOAD_CREDIT            (IWL49_SCD_BASE + 0xbc)
-#define IWL49_SCD_LOAD_SCREDIT           (IWL49_SCD_BASE + 0xc0)
-#define IWL49_SCD_BAR                    (IWL49_SCD_BASE + 0xc4)
-#define IWL49_SCD_BAR_DW0                (IWL49_SCD_BASE + 0xc8)
-#define IWL49_SCD_BAR_DW1                (IWL49_SCD_BASE + 0xcc)
-#define IWL49_SCD_QUEUECHAIN_SEL         (IWL49_SCD_BASE + 0xd0)
-#define IWL49_SCD_QUERY_REQ              (IWL49_SCD_BASE + 0xd8)
-#define IWL49_SCD_QUERY_RES              (IWL49_SCD_BASE + 0xdc)
-#define IWL49_SCD_PENDING_FRAMES         (IWL49_SCD_BASE + 0xe0)
-#define IWL49_SCD_INTERRUPT_MASK         (IWL49_SCD_BASE + 0xe4)
-#define IWL49_SCD_INTERRUPT_THRESHOLD    (IWL49_SCD_BASE + 0xe8)
-#define IWL49_SCD_QUERY_MIN_FRAME_SIZE   (IWL49_SCD_BASE + 0x100)
-#define IWL49_SCD_QUEUE_STATUS_BITS(x)   (IWL49_SCD_BASE + 0x104 + (x) * 4)
-
-/* SP SCD */
+#define IWL49_SCD_TRANSLATE_TBL_OFFSET		0x500
+
+/* Find translation table dword to read/write for given queue */
+#define IWL49_SCD_TRANSLATE_TBL_OFFSET_QUEUE(x) \
+	((IWL49_SCD_TRANSLATE_TBL_OFFSET + ((x) * 2)) & 0xfffffffc)
+
+#define IWL_SCD_TXFIFO_POS_TID			(0)
+#define IWL_SCD_TXFIFO_POS_RA			(4)
+#define IWL_SCD_QUEUE_RA_TID_MAP_RATID_MSK	(0x01FF)
+
+/* 5000 SCD */
+#define IWL50_SCD_QUEUE_STTS_REG_POS_TXF	(0)
+#define IWL50_SCD_QUEUE_STTS_REG_POS_ACTIVE	(3)
+#define IWL50_SCD_QUEUE_STTS_REG_POS_WSL	(4)
+#define IWL50_SCD_QUEUE_STTS_REG_POS_SCD_ACT_EN (19)
+#define IWL50_SCD_QUEUE_STTS_REG_MSK		(0x00FF0000)
+
+#define IWL50_SCD_QUEUE_CTX_REG1_CREDIT_POS		(8)
+#define IWL50_SCD_QUEUE_CTX_REG1_CREDIT_MSK		(0x00FFFF00)
+#define IWL50_SCD_QUEUE_CTX_REG1_SUPER_CREDIT_POS	(24)
+#define IWL50_SCD_QUEUE_CTX_REG1_SUPER_CREDIT_MSK	(0xFF000000)
+#define IWL50_SCD_QUEUE_CTX_REG2_WIN_SIZE_POS		(0)
+#define IWL50_SCD_QUEUE_CTX_REG2_WIN_SIZE_MSK		(0x0000007F)
+#define IWL50_SCD_QUEUE_CTX_REG2_FRAME_LIMIT_POS	(16)
+#define IWL50_SCD_QUEUE_CTX_REG2_FRAME_LIMIT_MSK	(0x007F0000)
+
+#define IWL50_SCD_CONTEXT_DATA_OFFSET		(0x600)
+#define IWL50_SCD_TX_STTS_BITMAP_OFFSET		(0x7B1)
+#define IWL50_SCD_TRANSLATE_TBL_OFFSET		(0x7E0)
+
+#define IWL50_SCD_CONTEXT_QUEUE_OFFSET(x)\
+	(IWL50_SCD_CONTEXT_DATA_OFFSET + ((x) * 8))
+
+#define IWL50_SCD_TRANSLATE_TBL_OFFSET_QUEUE(x) \
+	((IWL50_SCD_TRANSLATE_TBL_OFFSET + ((x) * 2)) & 0xfffc)
+
+#define IWL50_SCD_QUEUECHAIN_SEL_ALL(x)		(((1<<(x)) - 1) &\
+	(~(1<<IWL_CMD_QUEUE_NUM)))
+
 #define IWL50_SCD_BASE			(PRPH_BASE + 0xa02c00)
 
 #define IWL50_SCD_SRAM_BASE_ADDR         (IWL50_SCD_BASE + 0x0)
@@ -287,4 +554,6 @@
 #define IWL50_SCD_INTERRUPT_MASK         (IWL50_SCD_BASE + 0x108)
 #define IWL50_SCD_QUEUE_STATUS_BITS(x)   (IWL50_SCD_BASE + 0x10c + (x) * 4)
 
+/*********************** END TX SCHEDULER *************************************/
+
 #endif				/* __iwl_prph_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-rfkill.c b/drivers/net/wireless/iwlwifi/iwl-rfkill.c
index 5980a5621cb8..e5e5846e9f25 100644
--- a/drivers/net/wireless/iwlwifi/iwl-rfkill.c
+++ b/drivers/net/wireless/iwlwifi/iwl-rfkill.c
@@ -33,7 +33,7 @@
 #include <net/mac80211.h>
 
 #include "iwl-eeprom.h"
-#include "iwl-4965.h"
+#include "iwl-dev.h"
 #include "iwl-core.h"
 #include "iwl-helpers.h"
 
@@ -44,28 +44,31 @@ static int iwl_rfkill_soft_rf_kill(void *data, enum rfkill_state state)
 	struct iwl_priv *priv = data;
 	int err = 0;
 
-	if (!priv->rfkill_mngr.rfkill)
+	if (!priv->rfkill)
 		return 0;
 
 	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
 		return 0;
 
-	IWL_DEBUG_RF_KILL("we recieved soft RFKILL set to state %d\n", state);
+	IWL_DEBUG_RF_KILL("we received soft RFKILL set to state %d\n", state);
 	mutex_lock(&priv->mutex);
 
 	switch (state) {
-	case RFKILL_STATE_ON:
-		priv->cfg->ops->lib->radio_kill_sw(priv, 0);
-		/* if HW rf-kill is set dont allow ON state */
-		if (iwl_is_rfkill(priv))
+	case RFKILL_STATE_UNBLOCKED:
+		if (iwl_is_rfkill_hw(priv)) {
 			err = -EBUSY;
+			goto out_unlock;
+		}
+		iwl_radio_kill_sw_enable_radio(priv);
 		break;
-	case RFKILL_STATE_OFF:
-		priv->cfg->ops->lib->radio_kill_sw(priv, 1);
-		if (!iwl_is_rfkill(priv))
-			err = -EBUSY;
+	case RFKILL_STATE_SOFT_BLOCKED:
+		iwl_radio_kill_sw_disable_radio(priv);
+		break;
+	default:
+		IWL_WARNING("we recieved unexpected RFKILL state %d\n", state);
 		break;
 	}
+out_unlock:
 	mutex_unlock(&priv->mutex);
 
 	return err;
@@ -79,64 +82,35 @@ int iwl_rfkill_init(struct iwl_priv *priv)
 	BUG_ON(device == NULL);
 
 	IWL_DEBUG_RF_KILL("Initializing RFKILL.\n");
-	priv->rfkill_mngr.rfkill = rfkill_allocate(device, RFKILL_TYPE_WLAN);
-	if (!priv->rfkill_mngr.rfkill) {
+	priv->rfkill = rfkill_allocate(device, RFKILL_TYPE_WLAN);
+	if (!priv->rfkill) {
 		IWL_ERROR("Unable to allocate rfkill device.\n");
 		ret = -ENOMEM;
 		goto error;
 	}
 
-	priv->rfkill_mngr.rfkill->name = priv->cfg->name;
-	priv->rfkill_mngr.rfkill->data = priv;
-	priv->rfkill_mngr.rfkill->state = RFKILL_STATE_ON;
-	priv->rfkill_mngr.rfkill->toggle_radio = iwl_rfkill_soft_rf_kill;
-	priv->rfkill_mngr.rfkill->user_claim_unsupported = 1;
+	priv->rfkill->name = priv->cfg->name;
+	priv->rfkill->data = priv;
+	priv->rfkill->state = RFKILL_STATE_UNBLOCKED;
+	priv->rfkill->toggle_radio = iwl_rfkill_soft_rf_kill;
+	priv->rfkill->user_claim_unsupported = 1;
 
-	priv->rfkill_mngr.rfkill->dev.class->suspend = NULL;
-	priv->rfkill_mngr.rfkill->dev.class->resume = NULL;
+	priv->rfkill->dev.class->suspend = NULL;
+	priv->rfkill->dev.class->resume = NULL;
 
-	priv->rfkill_mngr.input_dev = input_allocate_device();
-	if (!priv->rfkill_mngr.input_dev) {
-		IWL_ERROR("Unable to allocate rfkill input device.\n");
-		ret = -ENOMEM;
-		goto freed_rfkill;
-	}
-
-	priv->rfkill_mngr.input_dev->name = priv->cfg->name;
-	priv->rfkill_mngr.input_dev->phys = wiphy_name(priv->hw->wiphy);
-	priv->rfkill_mngr.input_dev->id.bustype = BUS_HOST;
-	priv->rfkill_mngr.input_dev->id.vendor = priv->pci_dev->vendor;
-	priv->rfkill_mngr.input_dev->dev.parent = device;
-	priv->rfkill_mngr.input_dev->evbit[0] = BIT(EV_KEY);
-	set_bit(KEY_WLAN, priv->rfkill_mngr.input_dev->keybit);
-
-	ret = rfkill_register(priv->rfkill_mngr.rfkill);
+	ret = rfkill_register(priv->rfkill);
 	if (ret) {
 		IWL_ERROR("Unable to register rfkill: %d\n", ret);
-		goto free_input_dev;
-	}
-
-	ret = input_register_device(priv->rfkill_mngr.input_dev);
-	if (ret) {
-		IWL_ERROR("Unable to register rfkill input device: %d\n", ret);
-		goto unregister_rfkill;
+		goto free_rfkill;
 	}
 
 	IWL_DEBUG_RF_KILL("RFKILL initialization complete.\n");
 	return ret;
 
-unregister_rfkill:
-	rfkill_unregister(priv->rfkill_mngr.rfkill);
-	priv->rfkill_mngr.rfkill = NULL;
-
-free_input_dev:
-	input_free_device(priv->rfkill_mngr.input_dev);
-	priv->rfkill_mngr.input_dev = NULL;
-
-freed_rfkill:
-	if (priv->rfkill_mngr.rfkill != NULL)
-		rfkill_free(priv->rfkill_mngr.rfkill);
-	priv->rfkill_mngr.rfkill = NULL;
+free_rfkill:
+	if (priv->rfkill != NULL)
+		rfkill_free(priv->rfkill);
+	priv->rfkill = NULL;
 
 error:
 	IWL_DEBUG_RF_KILL("RFKILL initialization complete.\n");
@@ -147,27 +121,27 @@ EXPORT_SYMBOL(iwl_rfkill_init);
 void iwl_rfkill_unregister(struct iwl_priv *priv)
 {
 
-	if (priv->rfkill_mngr.input_dev)
-		input_unregister_device(priv->rfkill_mngr.input_dev);
+	if (priv->rfkill)
+		rfkill_unregister(priv->rfkill);
 
-	if (priv->rfkill_mngr.rfkill)
-		rfkill_unregister(priv->rfkill_mngr.rfkill);
-
-	priv->rfkill_mngr.input_dev = NULL;
-	priv->rfkill_mngr.rfkill = NULL;
+	priv->rfkill = NULL;
 }
 EXPORT_SYMBOL(iwl_rfkill_unregister);
 
 /* set rf-kill to the right state. */
 void iwl_rfkill_set_hw_state(struct iwl_priv *priv)
 {
+	if (!priv->rfkill)
+		return;
 
-	if (!priv->rfkill_mngr.rfkill)
+	if (iwl_is_rfkill_hw(priv)) {
+		rfkill_force_state(priv->rfkill, RFKILL_STATE_HARD_BLOCKED);
 		return;
+	}
 
-	if (!iwl_is_rfkill(priv))
-		priv->rfkill_mngr.rfkill->state = RFKILL_STATE_ON;
+	if (!iwl_is_rfkill_sw(priv))
+		rfkill_force_state(priv->rfkill, RFKILL_STATE_UNBLOCKED);
 	else
-		priv->rfkill_mngr.rfkill->state = RFKILL_STATE_OFF;
+		rfkill_force_state(priv->rfkill, RFKILL_STATE_SOFT_BLOCKED);
 }
 EXPORT_SYMBOL(iwl_rfkill_set_hw_state);
diff --git a/drivers/net/wireless/iwlwifi/iwl-rfkill.h b/drivers/net/wireless/iwlwifi/iwl-rfkill.h
index a7f04b855403..402fd4c781da 100644
--- a/drivers/net/wireless/iwlwifi/iwl-rfkill.h
+++ b/drivers/net/wireless/iwlwifi/iwl-rfkill.h
@@ -31,14 +31,8 @@
 struct iwl_priv;
 
 #include <linux/rfkill.h>
-#include <linux/input.h>
-
 
 #ifdef CONFIG_IWLWIFI_RFKILL
-struct iwl_rfkill_mngr {
-	struct rfkill *rfkill;
-	struct input_dev *input_dev;
-};
 
 void iwl_rfkill_set_hw_state(struct iwl_priv *priv);
 void iwl_rfkill_unregister(struct iwl_priv *priv);
diff --git a/drivers/net/wireless/iwlwifi/iwl-rx.c b/drivers/net/wireless/iwlwifi/iwl-rx.c
new file mode 100644
index 000000000000..e2d9afba38a5
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-rx.c
@@ -0,0 +1,1321 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2003 - 2008 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
+ *
+ *****************************************************************************/
+
+#include <linux/etherdevice.h>
+#include <net/mac80211.h>
+#include <asm/unaligned.h>
+#include "iwl-eeprom.h"
+#include "iwl-dev.h"
+#include "iwl-core.h"
+#include "iwl-sta.h"
+#include "iwl-io.h"
+#include "iwl-calib.h"
+#include "iwl-helpers.h"
+/************************** RX-FUNCTIONS ****************************/
+/*
+ * Rx theory of operation
+ *
+ * Driver allocates a circular buffer of Receive Buffer Descriptors (RBDs),
+ * each of which point to Receive Buffers to be filled by the NIC.  These get
+ * used not only for Rx frames, but for any command response or notification
+ * from the NIC.  The driver and NIC manage the Rx buffers by means
+ * of indexes into the circular buffer.
+ *
+ * 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 replenish 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.
+ */
+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;
+}
+EXPORT_SYMBOL(iwl_rx_queue_space);
+
+/**
+ * iwl_rx_queue_update_write_ptr - Update the write pointer for the RX queue
+ */
+int iwl_rx_queue_update_write_ptr(struct iwl_priv *priv, struct iwl_rx_queue *q)
+{
+	u32 reg = 0;
+	int ret = 0;
+	unsigned long flags;
+
+	spin_lock_irqsave(&q->lock, flags);
+
+	if (q->need_update == 0)
+		goto exit_unlock;
+
+	/* If power-saving is in use, make sure device is awake */
+	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;
+		}
+
+		ret = iwl_grab_nic_access(priv);
+		if (ret)
+			goto exit_unlock;
+
+		/* Device expects a multiple of 8 */
+		iwl_write_direct32(priv, FH_RSCSR_CHNL0_WPTR,
+				     q->write & ~0x7);
+		iwl_release_nic_access(priv);
+
+	/* Else device is assumed to be awake */
+	} else
+		/* Device expects a multiple of 8 */
+		iwl_write32(priv, FH_RSCSR_CHNL0_WPTR, q->write & ~0x7);
+
+
+	q->need_update = 0;
+
+ exit_unlock:
+	spin_unlock_irqrestore(&q->lock, flags);
+	return ret;
+}
+EXPORT_SYMBOL(iwl_rx_queue_update_write_ptr);
+/**
+ * iwl_dma_addr2rbd_ptr - convert a DMA address to a uCode read buffer ptr
+ */
+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;
+	int ret = 0;
+
+	spin_lock_irqsave(&rxq->lock, flags);
+	write = rxq->write & ~0x7;
+	while ((iwl_rx_queue_space(rxq) > 0) && (rxq->free_count)) {
+		/* Get next free Rx buffer, remove from free list */
+		element = rxq->rx_free.next;
+		rxb = list_entry(element, struct iwl_rx_mem_buffer, list);
+		list_del(element);
+
+		/* Point to Rx buffer via next RBD in circular buffer */
+		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.
+	 * Increment device's write pointer in multiples of 8. */
+	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);
+		ret = iwl_rx_queue_update_write_ptr(priv, rxq);
+	}
+
+	return ret;
+}
+EXPORT_SYMBOL(iwl_rx_queue_restock);
+
+
+/**
+ * iwl_rx_replenish - 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 initialization)
+ */
+void iwl_rx_allocate(struct iwl_priv *priv)
+{
+	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);
+
+		/* Alloc a new receive buffer */
+		rxb->skb = alloc_skb(priv->hw_params.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);
+
+		/* Get physical address of RB/SKB */
+		rxb->dma_addr =
+		    pci_map_single(priv->pci_dev, rxb->skb->data,
+			   priv->hw_params.rx_buf_size, PCI_DMA_FROMDEVICE);
+		list_add_tail(&rxb->list, &rxq->rx_free);
+		rxq->free_count++;
+	}
+	spin_unlock_irqrestore(&rxq->lock, flags);
+}
+EXPORT_SYMBOL(iwl_rx_allocate);
+
+void iwl_rx_replenish(struct iwl_priv *priv)
+{
+	unsigned long flags;
+
+	iwl_rx_allocate(priv);
+
+	spin_lock_irqsave(&priv->lock, flags);
+	iwl_rx_queue_restock(priv);
+	spin_unlock_irqrestore(&priv->lock, flags);
+}
+EXPORT_SYMBOL(iwl_rx_replenish);
+
+
+/* Assumes that the skb field of the buffers in 'pool' is kept accurate.
+ * If an SKB has been detached, the POOL needs to have its 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,
+					 priv->hw_params.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;
+}
+EXPORT_SYMBOL(iwl_rx_queue_free);
+
+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);
+
+	/* Alloc the circular buffer of Read Buffer Descriptors (RBDs) */
+	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;
+}
+EXPORT_SYMBOL(iwl_rx_queue_alloc);
+
+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,
+					 priv->hw_params.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);
+}
+EXPORT_SYMBOL(iwl_rx_queue_reset);
+
+int iwl_rx_init(struct iwl_priv *priv, struct iwl_rx_queue *rxq)
+{
+	int ret;
+	unsigned long flags;
+	unsigned int rb_size;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	ret = iwl_grab_nic_access(priv);
+	if (ret) {
+		spin_unlock_irqrestore(&priv->lock, flags);
+		return ret;
+	}
+
+	if (priv->cfg->mod_params->amsdu_size_8K)
+		rb_size = FH_RCSR_RX_CONFIG_REG_VAL_RB_SIZE_8K;
+	else
+		rb_size = FH_RCSR_RX_CONFIG_REG_VAL_RB_SIZE_4K;
+
+	/* Stop Rx DMA */
+	iwl_write_direct32(priv, FH_MEM_RCSR_CHNL0_CONFIG_REG, 0);
+
+	/* Reset driver's Rx queue write index */
+	iwl_write_direct32(priv, FH_RSCSR_CHNL0_RBDCB_WPTR_REG, 0);
+
+	/* Tell device where to find RBD circular buffer in DRAM */
+	iwl_write_direct32(priv, FH_RSCSR_CHNL0_RBDCB_BASE_REG,
+			   rxq->dma_addr >> 8);
+
+	/* Tell device where in DRAM to update its Rx status */
+	iwl_write_direct32(priv, FH_RSCSR_CHNL0_STTS_WPTR_REG,
+			   (priv->shared_phys + priv->rb_closed_offset) >> 4);
+
+	/* Enable Rx DMA, enable host interrupt, Rx buffer size 4k, 256 RBDs */
+	iwl_write_direct32(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 |
+			   rb_size |
+			     /* 0x10 << 4 | */
+			   (RX_QUEUE_SIZE_LOG <<
+			      FH_RCSR_RX_CONFIG_RBDCB_SIZE_BITSHIFT));
+
+	/*
+	 * iwl_write32(priv,CSR_INT_COAL_REG,0);
+	 */
+
+	iwl_release_nic_access(priv);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return 0;
+}
+
+int iwl_rxq_stop(struct iwl_priv *priv)
+{
+	int ret;
+	unsigned long flags;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	ret = iwl_grab_nic_access(priv);
+	if (unlikely(ret)) {
+		spin_unlock_irqrestore(&priv->lock, flags);
+		return ret;
+	}
+
+	/* stop Rx DMA */
+	iwl_write_direct32(priv, FH_MEM_RCSR_CHNL0_CONFIG_REG, 0);
+	ret = iwl_poll_direct_bit(priv, FH_MEM_RSSR_RX_STATUS_REG,
+				     (1 << 24), 1000);
+	if (ret < 0)
+		IWL_ERROR("Can't stop Rx DMA.\n");
+
+	iwl_release_nic_access(priv);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return 0;
+}
+EXPORT_SYMBOL(iwl_rxq_stop);
+
+void iwl_rx_missed_beacon_notif(struct iwl_priv *priv,
+				struct iwl_rx_mem_buffer *rxb)
+
+{
+	struct iwl_rx_packet *pkt = (struct iwl_rx_packet *)rxb->skb->data;
+	struct iwl4965_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));
+		if (!test_bit(STATUS_SCANNING, &priv->status))
+			iwl_init_sensitivity(priv);
+	}
+}
+EXPORT_SYMBOL(iwl_rx_missed_beacon_notif);
+
+int iwl_rx_agg_start(struct iwl_priv *priv, const u8 *addr, int tid, u16 ssn)
+{
+	unsigned long flags;
+	int sta_id;
+
+	sta_id = iwl_find_station(priv, addr);
+	if (sta_id == IWL_INVALID_STATION)
+		return -ENXIO;
+
+	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);
+
+	return iwl_send_add_sta(priv, &priv->stations[sta_id].sta,
+					CMD_ASYNC);
+}
+EXPORT_SYMBOL(iwl_rx_agg_start);
+
+int iwl_rx_agg_stop(struct iwl_priv *priv, const u8 *addr, int tid)
+{
+	unsigned long flags;
+	int sta_id;
+
+	sta_id = iwl_find_station(priv, addr);
+	if (sta_id == IWL_INVALID_STATION)
+		return -ENXIO;
+
+	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);
+
+	return iwl_send_add_sta(priv, &priv->stations[sta_id].sta,
+					CMD_ASYNC);
+}
+EXPORT_SYMBOL(iwl_rx_agg_stop);
+
+
+/* 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 iwl_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);
+}
+
+#define REG_RECALIB_PERIOD (60)
+
+void iwl_rx_statistics(struct iwl_priv *priv,
+			      struct iwl_rx_mem_buffer *rxb)
+{
+	int change;
+	struct iwl_rx_packet *pkt = (struct iwl_rx_packet *)rxb->skb->data;
+
+	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)) {
+		iwl_rx_calc_noise(priv);
+		queue_work(priv->workqueue, &priv->run_time_calib_work);
+	}
+
+	iwl_leds_background(priv);
+
+	if (priv->cfg->ops->lib->temperature && change)
+		priv->cfg->ops->lib->temperature(priv);
+}
+EXPORT_SYMBOL(iwl_rx_statistics);
+
+#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. */
+static 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;
+}
+
+#ifdef CONFIG_IWLWIFI_DEBUG
+
+/**
+ * iwl_dbg_report_frame - dump frame to syslog during debug sessions
+ *
+ * You may 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:  This was originally written for 3945, need to audit for
+ *        proper operation with 4965.
+ */
+static void iwl_dbg_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;
+	__le16 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 iwl4965_rx_frame_stats *rx_stats = IWL_RX_STATS(pkt);
+	struct iwl4965_rx_frame_hdr *rx_hdr = IWL_RX_HDR(pkt);
+	struct iwl4965_rx_frame_end *rx_end = IWL_RX_END(pkt);
+	u8 *data = IWL_RX_DATA(pkt);
+
+	if (likely(!(priv->debug_level & IWL_DL_RX)))
+		return;
+
+	/* MAC header */
+	fc = 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 & ~cpu_to_le16(IEEE80211_FCTL_PROTECTED)) ==
+	    cpu_to_le16(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;
+		int rate_idx;
+		u32 bitrate;
+
+		if (hundred)
+			title = "100Frames";
+		else if (ieee80211_has_retry(fc))
+			title = "Retry";
+		else if (ieee80211_is_assoc_resp(fc))
+			title = "AscRsp";
+		else if (ieee80211_is_reassoc_resp(fc))
+			title = "RasRsp";
+		else if (ieee80211_is_probe_resp(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_idx = iwl_hwrate_to_plcp_idx(rate_sym);
+		if (unlikely(rate_idx == -1))
+			bitrate = 0;
+		else
+			bitrate = iwl_rates[rate_idx].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, le16_to_cpu(fc), header->addr1[5],
+				     length, rssi, channel, bitrate);
+		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, le16_to_cpu(fc), header->addr1[5],
+				     header->addr3[5], rssi,
+				     tsf_low - priv->scan_start_tsf,
+				     phy_flags, channel);
+		}
+	}
+	if (print_dump)
+		iwl_print_hex_dump(priv, IWL_DL_RX, data, length);
+}
+#else
+static inline void iwl_dbg_report_frame(struct iwl_priv *priv,
+					    struct iwl_rx_packet *pkt,
+					    struct ieee80211_hdr *header,
+					    int group100)
+{
+}
+#endif
+
+static void iwl_add_radiotap(struct iwl_priv *priv,
+				 struct sk_buff *skb,
+				 struct iwl4965_rx_phy_res *rx_start,
+				 struct ieee80211_rx_status *stats,
+				 u32 ampdu_status)
+{
+	s8 signal = stats->signal;
+	s8 noise = 0;
+	int rate = stats->rate_idx;
+	u64 tsf = stats->mactime;
+	__le16 antenna;
+	__le16 phy_flags_hw = rx_start->phy_flags;
+	struct iwl4965_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 */
+	} __attribute__ ((packed)) *iwl4965_rt;
+
+	/* TODO: We won't have enough headroom for HT frames. Fix it later. */
+	if (skb_headroom(skb) < sizeof(*iwl4965_rt)) {
+		if (net_ratelimit())
+			printk(KERN_ERR "not enough headroom [%d] for "
+			       "radiotap head [%zd]\n",
+			       skb_headroom(skb), sizeof(*iwl4965_rt));
+		return;
+	}
+
+	/* put radiotap header in front of 802.11 header and data */
+	iwl4965_rt = (void *)skb_push(skb, sizeof(*iwl4965_rt));
+
+	/* initialise radiotap header */
+	iwl4965_rt->rt_hdr.it_version = PKTHDR_RADIOTAP_VERSION;
+	iwl4965_rt->rt_hdr.it_pad = 0;
+
+	/* total header + data */
+	put_unaligned_le16(sizeof(*iwl4965_rt), &iwl4965_rt->rt_hdr.it_len);
+
+	/* Indicate all the fields we add to the radiotap header */
+	put_unaligned_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),
+			   &(iwl4965_rt->rt_hdr.it_present));
+
+	/* Zero the flags, we'll add to them as we go */
+	iwl4965_rt->rt_flags = 0;
+
+	put_unaligned_le64(tsf, &iwl4965_rt->rt_tsf);
+
+	iwl4965_rt->rt_dbmsignal = signal;
+	iwl4965_rt->rt_dbmnoise = noise;
+
+	/* Convert the channel frequency and set the flags */
+	put_unaligned(cpu_to_le16(stats->freq), &iwl4965_rt->rt_channelMHz);
+	if (!(phy_flags_hw & RX_RES_PHY_FLAGS_BAND_24_MSK))
+		put_unaligned_le16(IEEE80211_CHAN_OFDM | IEEE80211_CHAN_5GHZ,
+				   &iwl4965_rt->rt_chbitmask);
+	else if (phy_flags_hw & RX_RES_PHY_FLAGS_MOD_CCK_MSK)
+		put_unaligned_le16(IEEE80211_CHAN_CCK | IEEE80211_CHAN_2GHZ,
+				   &iwl4965_rt->rt_chbitmask);
+	else	/* 802.11g */
+		put_unaligned_le16(IEEE80211_CHAN_OFDM | IEEE80211_CHAN_2GHZ,
+				   &iwl4965_rt->rt_chbitmask);
+
+	if (rate == -1)
+		iwl4965_rt->rt_rate = 0;
+	else
+		iwl4965_rt->rt_rate = iwl_rates[rate].ieee;
+
+	/*
+	 * "antenna number"
+	 *
+	 * It seems that the antenna field in the phy flags value
+	 * is actually a bitfield. This is undefined by radiotap,
+	 * it wants an actual antenna number but I always get "7"
+	 * for most legacy frames I receive indicating that the
+	 * same frame was received on all three RX chains.
+	 *
+	 * I think this field should be removed in favour of a
+	 * new 802.11n radiotap field "RX chains" that is defined
+	 * as a bitmask.
+	 */
+	antenna = phy_flags_hw & RX_RES_PHY_FLAGS_ANTENNA_MSK;
+	iwl4965_rt->rt_antenna = le16_to_cpu(antenna) >> 4;
+
+	/* set the preamble flag if appropriate */
+	if (phy_flags_hw & RX_RES_PHY_FLAGS_SHORT_PREAMBLE_MSK)
+		iwl4965_rt->rt_flags |= IEEE80211_RADIOTAP_F_SHORTPRE;
+
+	stats->flag |= RX_FLAG_RADIOTAP;
+}
+
+static void iwl_update_rx_stats(struct iwl_priv *priv, u16 fc, u16 len)
+{
+	/* 0 - mgmt, 1 - cnt, 2 - data */
+	int idx = (fc & IEEE80211_FCTL_FTYPE) >> 2;
+	priv->rx_stats[idx].cnt++;
+	priv->rx_stats[idx].bytes += len;
+}
+
+/*
+ * returns non-zero if packet should be dropped
+ */
+static int iwl_set_decrypted_flag(struct iwl_priv *priv,
+				      struct ieee80211_hdr *hdr,
+				      u32 decrypt_res,
+				      struct ieee80211_rx_status *stats)
+{
+	u16 fc = le16_to_cpu(hdr->frame_control);
+
+	if (priv->active_rxon.filter_flags & RXON_FILTER_DIS_DECRYPT_MSK)
+		return 0;
+
+	if (!(fc & IEEE80211_FCTL_PROTECTED))
+		return 0;
+
+	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:
+		/* The uCode has got a bad phase 1 Key, pushes the packet.
+		 * Decryption will be done in SW. */
+		if ((decrypt_res & RX_RES_STATUS_DECRYPT_TYPE_MSK) ==
+		    RX_RES_STATUS_BAD_KEY_TTAK)
+			break;
+
+	case RX_RES_STATUS_SEC_TYPE_WEP:
+		if ((decrypt_res & RX_RES_STATUS_DECRYPT_TYPE_MSK) ==
+		    RX_RES_STATUS_BAD_ICV_MIC) {
+			/* bad ICV, the packet is destroyed since the
+			 * decryption is inplace, drop it */
+			IWL_DEBUG_RX("Packet destroyed\n");
+			return -1;
+		}
+	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;
+	}
+	return 0;
+}
+
+static u32 iwl_translate_rx_status(struct iwl_priv *priv, u32 decrypt_in)
+{
+	u32 decrypt_out = 0;
+
+	if ((decrypt_in & RX_RES_STATUS_STATION_FOUND) ==
+					RX_RES_STATUS_STATION_FOUND)
+		decrypt_out |= (RX_RES_STATUS_STATION_FOUND |
+				RX_RES_STATUS_NO_STATION_INFO_MISMATCH);
+
+	decrypt_out |= (decrypt_in & RX_RES_STATUS_SEC_TYPE_MSK);
+
+	/* packet was not encrypted */
+	if ((decrypt_in & RX_RES_STATUS_SEC_TYPE_MSK) ==
+					RX_RES_STATUS_SEC_TYPE_NONE)
+		return decrypt_out;
+
+	/* packet was encrypted with unknown alg */
+	if ((decrypt_in & RX_RES_STATUS_SEC_TYPE_MSK) ==
+					RX_RES_STATUS_SEC_TYPE_ERR)
+		return decrypt_out;
+
+	/* decryption was not done in HW */
+	if ((decrypt_in & RX_MPDU_RES_STATUS_DEC_DONE_MSK) !=
+					RX_MPDU_RES_STATUS_DEC_DONE_MSK)
+		return decrypt_out;
+
+	switch (decrypt_in & RX_RES_STATUS_SEC_TYPE_MSK) {
+
+	case RX_RES_STATUS_SEC_TYPE_CCMP:
+		/* alg is CCM: check MIC only */
+		if (!(decrypt_in & RX_MPDU_RES_STATUS_MIC_OK))
+			/* Bad MIC */
+			decrypt_out |= RX_RES_STATUS_BAD_ICV_MIC;
+		else
+			decrypt_out |= RX_RES_STATUS_DECRYPT_OK;
+
+		break;
+
+	case RX_RES_STATUS_SEC_TYPE_TKIP:
+		if (!(decrypt_in & RX_MPDU_RES_STATUS_TTAK_OK)) {
+			/* Bad TTAK */
+			decrypt_out |= RX_RES_STATUS_BAD_KEY_TTAK;
+			break;
+		}
+		/* fall through if TTAK OK */
+	default:
+		if (!(decrypt_in & RX_MPDU_RES_STATUS_ICV_OK))
+			decrypt_out |= RX_RES_STATUS_BAD_ICV_MIC;
+		else
+			decrypt_out |= RX_RES_STATUS_DECRYPT_OK;
+		break;
+	};
+
+	IWL_DEBUG_RX("decrypt_in:0x%x  decrypt_out = 0x%x\n",
+					decrypt_in, decrypt_out);
+
+	return decrypt_out;
+}
+
+static void iwl_pass_packet_to_mac80211(struct iwl_priv *priv,
+				       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;
+	u32 ampdu_status_legacy;
+
+	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);
+	}
+
+	ampdu_status = le32_to_cpu(*rx_end);
+	skblen = ((u8 *) rx_end - (u8 *) &pkt->u.raw[0]) + sizeof(u32);
+
+	if (!include_phy) {
+		/* New status scheme, need to translate */
+		ampdu_status_legacy = ampdu_status;
+		ampdu_status = iwl_translate_rx_status(priv, ampdu_status);
+	}
+
+	/* 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;
+	}
+
+	hdr = (struct ieee80211_hdr *)rxb->skb->data;
+
+	/*  in case of HW accelerated crypto and bad decryption, drop */
+	if (!priv->hw_params.sw_crypto &&
+	    iwl_set_decrypted_flag(priv, hdr, ampdu_status, stats))
+		return;
+
+	if (priv->add_radiotap)
+		iwl_add_radiotap(priv, rxb->skb, rx_start, stats, ampdu_status);
+
+	iwl_update_rx_stats(priv, le16_to_cpu(hdr->frame_control), len);
+	ieee80211_rx_irqsafe(priv->hw, rxb->skb, stats);
+	priv->alloc_rxb_skb--;
+	rxb->skb = NULL;
+}
+
+/* Calc max signal level (dBm) among 3 possible receivers */
+static int iwl_calc_rssi(struct iwl_priv *priv,
+			     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;
+}
+
+static void iwl_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_sta(priv, &priv->stations[sta_id].sta, CMD_ASYNC);
+}
+
+static void iwl_update_ps_mode(struct iwl_priv *priv, u16 ps_bit, u8 *addr)
+{
+	/* FIXME: need locking over ps_status ??? */
+	u8 sta_id = iwl_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) {
+			iwl_sta_modify_ps_wake(priv, sta_id);
+			priv->stations[sta_id].ps_status = STA_PS_STATUS_WAKE;
+		}
+	}
+}
+
+/* This is necessary only for a number of statistics, see the caller. */
+static 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 to our IBSS update information */
+		return !compare_ether_addr(header->addr3, priv->bssid);
+	case IEEE80211_IF_TYPE_STA: /* Header: Dest. | AP{BSSID} | Source */
+		/* packets to our IBSS update information */
+		return !compare_ether_addr(header->addr2, priv->bssid);
+	default:
+		return 1;
+	}
+}
+
+/* Called for REPLY_RX (legacy ABG frames), or
+ * REPLY_RX_MPDU_CMD (HT high-throughput N frames). */
+void iwl_rx_reply_rx(struct iwl_priv *priv,
+				struct iwl_rx_mem_buffer *rxb)
+{
+	struct ieee80211_hdr *header;
+	struct ieee80211_rx_status rx_status;
+	struct iwl_rx_packet *pkt = (struct iwl_rx_packet *)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_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;
+	u16 fc;
+	u8 network_packet;
+
+	rx_status.mactime = le64_to_cpu(rx_start->timestamp);
+	rx_status.freq =
+		ieee80211_channel_to_frequency(le16_to_cpu(rx_start->channel));
+	rx_status.band = (rx_start->phy_flags & RX_RES_PHY_FLAGS_BAND_24_MSK) ?
+				IEEE80211_BAND_2GHZ : IEEE80211_BAND_5GHZ;
+	rx_status.rate_idx =
+		iwl_hwrate_to_plcp_idx(le32_to_cpu(rx_start->rate_n_flags));
+	if (rx_status.band == IEEE80211_BAND_5GHZ)
+		rx_status.rate_idx -= IWL_FIRST_OFDM_RATE;
+
+	rx_status.antenna = 0;
+	rx_status.flag = 0;
+	rx_status.flag |= RX_FLAG_TSFT;
+
+	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);
+
+	/* Find max signal strength (dBm) among 3 antenna/receiver chains */
+	rx_status.signal = iwl_calc_rssi(priv, 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)) {
+		rx_status.noise = priv->last_rx_noise;
+		rx_status.qual = iwl_calc_sig_qual(rx_status.signal,
+							 rx_status.noise);
+	} else {
+		rx_status.noise = IWL_NOISE_MEAS_NOT_AVAILABLE;
+		rx_status.qual = iwl_calc_sig_qual(rx_status.signal, 0);
+	}
+
+	/* Reset beacon noise level if not associated. */
+	if (!iwl_is_associated(priv))
+		priv->last_rx_noise = IWL_NOISE_MEAS_NOT_AVAILABLE;
+
+	/* Set "1" to report good data frames in groups of 100 */
+	/* FIXME: need to optimze the call: */
+	iwl_dbg_report_frame(priv, pkt, header, 1);
+
+	IWL_DEBUG_STATS_LIMIT("Rssi %d, noise %d, qual %d, TSF %llu\n",
+		rx_status.signal, rx_status.noise, rx_status.signal,
+		(unsigned long long)rx_status.mactime);
+
+	/* Take shortcut when only in monitor mode */
+	if (priv->iw_mode == IEEE80211_IF_TYPE_MNTR) {
+		iwl_pass_packet_to_mac80211(priv, include_phy,
+						 rxb, &rx_status);
+		return;
+	}
+
+	network_packet = iwl_is_network_packet(priv, header);
+	if (network_packet) {
+		priv->last_rx_rssi = rx_status.signal;
+		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:
+	case IEEE80211_FTYPE_DATA:
+		if (priv->iw_mode == IEEE80211_IF_TYPE_AP)
+			iwl_update_ps_mode(priv, fc  & IEEE80211_FCTL_PM,
+						header->addr2);
+		/* fall through */
+	default:
+			iwl_pass_packet_to_mac80211(priv, include_phy, rxb,
+				   &rx_status);
+		break;
+
+	}
+}
+EXPORT_SYMBOL(iwl_rx_reply_rx);
+
+/* Cache phy data (Rx signal strength, etc) for HT frame (REPLY_RX_PHY_CMD).
+ * This will be used later in iwl_rx_reply_rx() for REPLY_RX_MPDU_CMD. */
+void iwl_rx_reply_rx_phy(struct iwl_priv *priv,
+				    struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = (struct iwl_rx_packet *)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));
+}
+EXPORT_SYMBOL(iwl_rx_reply_rx_phy);
diff --git a/drivers/net/wireless/iwlwifi/iwl-scan.c b/drivers/net/wireless/iwlwifi/iwl-scan.c
new file mode 100644
index 000000000000..efc750d2fc5c
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-scan.c
@@ -0,0 +1,931 @@
+/******************************************************************************
+ *
+ * GPL LICENSE SUMMARY
+ *
+ * Copyright(c) 2008 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.GPL.
+ *
+ * Contact Information:
+ * Tomas Winkler <tomas.winkler@intel.com>
+ * Intel Corporation, 5200 N.E. Elam Young Parkway, Hillsboro, OR 97124-6497
+ *****************************************************************************/
+#include <net/mac80211.h>
+#include <linux/etherdevice.h>
+
+#include "iwl-eeprom.h"
+#include "iwl-dev.h"
+#include "iwl-core.h"
+#include "iwl-sta.h"
+#include "iwl-io.h"
+#include "iwl-helpers.h"
+
+/* 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    (30)       /* all times in msec */
+#define IWL_ACTIVE_DWELL_TIME_52    (20)
+
+#define IWL_ACTIVE_DWELL_FACTOR_24GHZ (3)
+#define IWL_ACTIVE_DWELL_FACTOR_52GHZ (2)
+
+/* 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(10)  /* 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
+
+#define IWL_SCAN_PROBE_MASK(n) 	cpu_to_le32((BIT(n) | (BIT(n) - BIT(1))))
+
+
+static int scan_tx_ant[3] = {
+	RATE_MCS_ANT_A_MSK, RATE_MCS_ANT_B_MSK, RATE_MCS_ANT_C_MSK
+};
+
+
+
+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;
+}
+
+
+
+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;
+}
+EXPORT_SYMBOL(iwl_escape_essid);
+
+/**
+ * iwl_scan_cancel - Cancel any currently executing HW scan
+ *
+ * NOTE: priv->mutex is not required before calling this function
+ */
+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;
+}
+EXPORT_SYMBOL(iwl_scan_cancel);
+/**
+ * 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
+ */
+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;
+}
+EXPORT_SYMBOL(iwl_scan_cancel_timeout);
+
+static int iwl_send_scan_abort(struct iwl_priv *priv)
+{
+	int ret = 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;
+	}
+
+	ret = iwl_send_cmd_sync(priv, &cmd);
+	if (ret) {
+		clear_bit(STATUS_SCAN_ABORTING, &priv->status);
+		return ret;
+	}
+
+	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 ret;
+}
+
+
+/* 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 = (struct iwl_rx_packet *)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 = (struct iwl_rx_packet *)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",
+		       le32_to_cpu(notif->tsf_high),
+		       le32_to_cpu(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)
+{
+#ifdef CONFIG_IWLWIFI_DEBUG
+	struct iwl_rx_packet *pkt = (struct iwl_rx_packet *)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)));
+#endif
+
+	priv->last_scan_jiffies = jiffies;
+	priv->next_scan_jiffies = 0;
+}
+
+/* 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 = (struct iwl_rx_packet *)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 & BIT(IEEE80211_BAND_2GHZ)) ?
+						"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, band G precedes A in order of scanning
+	 * as seen in iwl_bg_request_scan */
+	if (priv->scan_bands & BIT(IEEE80211_BAND_2GHZ))
+		priv->scan_bands &= ~BIT(IEEE80211_BAND_2GHZ);
+	else if (priv->scan_bands &  BIT(IEEE80211_BAND_5GHZ))
+		priv->scan_bands &= ~BIT(IEEE80211_BAND_5GHZ);
+
+	/* 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)
+			goto reschedule;
+	}
+
+	priv->last_scan_jiffies = jiffies;
+	priv->next_scan_jiffies = 0;
+	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);
+}
+
+void iwl_setup_rx_scan_handlers(struct iwl_priv *priv)
+{
+	/* scan handlers */
+	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;
+}
+EXPORT_SYMBOL(iwl_setup_rx_scan_handlers);
+
+static inline u16 iwl_get_active_dwell_time(struct iwl_priv *priv,
+					    enum ieee80211_band band,
+					    u8 n_probes)
+{
+	if (band == IEEE80211_BAND_5GHZ)
+		return IWL_ACTIVE_DWELL_TIME_52 +
+			IWL_ACTIVE_DWELL_FACTOR_52GHZ * (n_probes + 1);
+	else
+		return IWL_ACTIVE_DWELL_TIME_24 +
+			IWL_ACTIVE_DWELL_FACTOR_24GHZ * (n_probes + 1);
+}
+
+static u16 iwl_get_passive_dwell_time(struct iwl_priv *priv,
+				      enum ieee80211_band band)
+{
+	u16 passive = (band == IEEE80211_BAND_2GHZ) ?
+	    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;
+	}
+
+	return passive;
+}
+
+static int iwl_get_channels_for_scan(struct iwl_priv *priv,
+				     enum ieee80211_band band,
+				     u8 is_active, u8 n_probes,
+				     struct iwl_scan_channel *scan_ch)
+{
+	const struct ieee80211_channel *channels = NULL;
+	const struct ieee80211_supported_band *sband;
+	const struct iwl_channel_info *ch_info;
+	u16 passive_dwell = 0;
+	u16 active_dwell = 0;
+	int added, i;
+	u16 channel;
+
+	sband = iwl_get_hw_mode(priv, band);
+	if (!sband)
+		return 0;
+
+	channels = sband->channels;
+
+	active_dwell = iwl_get_active_dwell_time(priv, band, n_probes);
+	passive_dwell = iwl_get_passive_dwell_time(priv, band);
+
+	if (passive_dwell <= active_dwell)
+		passive_dwell = active_dwell + 1;
+
+	for (i = 0, added = 0; i < sband->n_channels; i++) {
+		if (channels[i].flags & IEEE80211_CHAN_DISABLED)
+			continue;
+
+		channel =
+			ieee80211_frequency_to_channel(channels[i].center_freq);
+		scan_ch->channel = cpu_to_le16(channel);
+
+		ch_info = iwl_get_channel_info(priv, band, channel);
+		if (!is_channel_valid(ch_info)) {
+			IWL_DEBUG_SCAN("Channel %d is INVALID for this band.\n",
+					channel);
+			continue;
+		}
+
+		if (!is_active || is_channel_passive(ch_info) ||
+		    (channels[i].flags & IEEE80211_CHAN_PASSIVE_SCAN))
+			scan_ch->type = SCAN_CHANNEL_TYPE_PASSIVE;
+		else
+			scan_ch->type = SCAN_CHANNEL_TYPE_ACTIVE;
+
+		if ((scan_ch->type & SCAN_CHANNEL_TYPE_ACTIVE) && n_probes)
+			scan_ch->type |= IWL_SCAN_PROBE_MASK(n_probes);
+
+		scan_ch->active_dwell = cpu_to_le16(active_dwell);
+		scan_ch->passive_dwell = cpu_to_le16(passive_dwell);
+
+		/* Set txpower levels to defaults */
+		scan_ch->dsp_atten = 110;
+
+		/* NOTE: if we were doing 6Mb OFDM for scans we'd use
+		 * power level:
+		 * scan_ch->tx_gain = ((1 << 5) | (2 << 3)) | 3;
+		 */
+		if (band == IEEE80211_BAND_5GHZ)
+			scan_ch->tx_gain = ((1 << 5) | (3 << 3)) | 3;
+		else
+			scan_ch->tx_gain = ((1 << 5) | (5 << 3));
+
+		IWL_DEBUG_SCAN("Scanning ch=%d prob=0x%X [%s %d]\n",
+			       channel, le32_to_cpu(scan_ch->type),
+			       (scan_ch->type & SCAN_CHANNEL_TYPE_ACTIVE) ?
+				"ACTIVE" : "PASSIVE",
+			       (scan_ch->type & SCAN_CHANNEL_TYPE_ACTIVE) ?
+			       active_dwell : passive_dwell);
+
+		scan_ch++;
+		added++;
+	}
+
+	IWL_DEBUG_SCAN("total channels to scan %d \n", added);
+	return added;
+}
+
+void iwl_init_scan_params(struct iwl_priv *priv)
+{
+	if (!priv->scan_tx_ant[IEEE80211_BAND_5GHZ])
+		priv->scan_tx_ant[IEEE80211_BAND_5GHZ] = RATE_MCS_ANT_INIT_IND;
+	if (!priv->scan_tx_ant[IEEE80211_BAND_2GHZ])
+		priv->scan_tx_ant[IEEE80211_BAND_2GHZ] = RATE_MCS_ANT_INIT_IND;
+}
+
+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");
+	if (priv->cfg->sku & IWL_SKU_G)
+		priv->scan_bands |= BIT(IEEE80211_BAND_2GHZ);
+	if (priv->cfg->sku & IWL_SKU_A)
+		priv->scan_bands |= BIT(IEEE80211_BAND_5GHZ);
+	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;
+}
+EXPORT_SYMBOL(iwl_scan_initiate);
+
+#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_SCAN, "Scan completion watchdog resetting "
+			"adapter (%dms)\n",
+			jiffies_to_msecs(IWL_SCAN_CHECK_WATCHDOG));
+
+		if (!test_bit(STATUS_EXIT_PENDING, &priv->status))
+			iwl_send_scan_abort(priv);
+	}
+	mutex_unlock(&priv->mutex);
+}
+/**
+ * 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 *left)
+{
+	u16 ret_rates = 0, bit;
+	int i;
+	u8 *cnt = ie;
+	u8 *rates = ie + 1;
+
+	for (bit = 1, i = 0; i < IWL_RATE_COUNT; i++, bit <<= 1) {
+		if (bit & supported_rate) {
+			ret_rates |= bit;
+			rates[*cnt] = iwl_rates[i].ieee |
+				((bit & basic_rate) ? 0x80 : 0x00);
+			(*cnt)++;
+			(*left)--;
+			if ((*left <= 0) ||
+			    (*cnt >= IWL_SUPPORTED_RATES_IE_LEN))
+				break;
+		}
+	}
+
+	return ret_rates;
+}
+
+
+static void iwl_ht_cap_to_ie(const struct ieee80211_supported_band *sband,
+			     u8 *pos, int *left)
+{
+	struct ieee80211_ht_cap *ht_cap;
+
+	if (!sband || !sband->ht_info.ht_supported)
+		return;
+
+	if (*left < sizeof(struct ieee80211_ht_cap))
+		return;
+
+	*pos++ = sizeof(struct ieee80211_ht_cap);
+	ht_cap = (struct ieee80211_ht_cap *) pos;
+
+	ht_cap->cap_info = cpu_to_le16(sband->ht_info.cap);
+	memcpy(ht_cap->supp_mcs_set, sband->ht_info.supp_mcs_set, 16);
+	ht_cap->ampdu_params_info =
+		(sband->ht_info.ampdu_factor & IEEE80211_HT_CAP_AMPDU_FACTOR) |
+		((sband->ht_info.ampdu_density << 2) &
+			IEEE80211_HT_CAP_AMPDU_DENSITY);
+	*left -= sizeof(struct ieee80211_ht_cap);
+}
+
+/**
+ * iwl_fill_probe_req - fill in all required fields and IE for probe request
+ */
+
+static u16 iwl_fill_probe_req(struct iwl_priv *priv,
+				  enum ieee80211_band band,
+				  struct ieee80211_mgmt *frame,
+				  int left)
+{
+	int len = 0;
+	u8 *pos = NULL;
+	u16 active_rates, ret_rates, cck_rates, active_rate_basic;
+	const struct ieee80211_supported_band *sband =
+						iwl_get_hw_mode(priv, band);
+
+
+	/* Make sure there is enough space for the probe request,
+	 * two mandatory IEs and the data */
+	left -= 24;
+	if (left < 0)
+		return 0;
+
+	frame->frame_control = cpu_to_le16(IEEE80211_STYPE_PROBE_REQ);
+	memcpy(frame->da, iwl_bcast_addr, ETH_ALEN);
+	memcpy(frame->sa, priv->mac_addr, ETH_ALEN);
+	memcpy(frame->bssid, iwl_bcast_addr, ETH_ALEN);
+	frame->seq_ctrl = 0;
+
+	len += 24;
+
+	/* ...next IE... */
+	pos = &frame->u.probe_req.variable[0];
+
+	/* fill in our indirect SSID IE */
+	left -= 2;
+	if (left < 0)
+		return 0;
+	*pos++ = WLAN_EID_SSID;
+	*pos++ = 0;
+
+	len += 2;
+
+	/* fill in supported rate */
+	left -= 2;
+	if (left < 0)
+		return 0;
+
+	*pos++ = WLAN_EID_SUPP_RATES;
+	*pos = 0;
+
+	/* exclude 60M rate */
+	active_rates = priv->rates_mask;
+	active_rates &= ~IWL_RATE_60M_MASK;
+
+	active_rate_basic = active_rates & IWL_BASIC_RATES_MASK;
+
+	cck_rates = IWL_CCK_RATES_MASK & active_rates;
+	ret_rates = iwl_supported_rate_to_ie(pos, cck_rates,
+					     active_rate_basic, &left);
+	active_rates &= ~ret_rates;
+
+	ret_rates = iwl_supported_rate_to_ie(pos, active_rates,
+					     active_rate_basic, &left);
+	active_rates &= ~ret_rates;
+
+	len += 2 + *pos;
+	pos += (*pos) + 1;
+
+	if (active_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, active_rates, active_rate_basic, &left);
+	if (*pos > 0) {
+		len += 2 + *pos;
+		pos += (*pos) + 1;
+	} else {
+		pos--;
+	}
+
+ fill_end:
+
+	left -= 2;
+	if (left < 0)
+		return 0;
+
+	*pos++ = WLAN_EID_HT_CAPABILITY;
+	*pos = 0;
+	iwl_ht_cap_to_ie(sband, pos, &left);
+	if (*pos > 0)
+		len += 2 + *pos;
+
+	return (u16)len;
+}
+
+static u32 iwl_scan_tx_ant(struct iwl_priv *priv, enum ieee80211_band band)
+{
+	int i, ind;
+
+	ind = priv->scan_tx_ant[band];
+	for (i = 0; i < priv->hw_params.tx_chains_num; i++) {
+		ind = (ind+1) >= priv->hw_params.tx_chains_num ? 0 : ind+1;
+		if (priv->hw_params.valid_tx_ant & (1 << ind)) {
+			priv->scan_tx_ant[band] = ind;
+			break;
+		}
+	}
+	IWL_DEBUG_SCAN("select TX ANT = %c\n", 'A' + ind);
+	return scan_tx_ant[ind];
+}
+
+
+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,
+	};
+	struct iwl_scan_cmd *scan;
+	struct ieee80211_conf *conf = NULL;
+	int ret = 0;
+	u32 tx_ant;
+	u16 cmd_len;
+	enum ieee80211_band band;
+	u8 n_probes = 2;
+	u8 rx_chain = 0x7; /* bitmap: ABC chains */
+
+	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");
+		ret = -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) {
+			ret = -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(200 * 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("Start 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);
+		n_probes++;
+	} else if (!iwl_is_associated(priv) && priv->essid_len) {
+		IWL_DEBUG_SCAN("Start direct scan for '%s' (not associated)\n",
+				iwl_escape_essid(priv->essid, priv->essid_len));
+		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);
+		n_probes++;
+	} else {
+		IWL_DEBUG_SCAN("Start indirect scan.\n");
+	}
+
+	scan->tx_cmd.tx_flags = TX_CMD_FLG_SEQ_CTL_MSK;
+	scan->tx_cmd.sta_id = priv->hw_params.bcast_sta_id;
+	scan->tx_cmd.stop_time.life_time = TX_CMD_LIFE_TIME_INFINITE;
+
+
+	if (priv->scan_bands & BIT(IEEE80211_BAND_2GHZ)) {
+		band = IEEE80211_BAND_2GHZ;
+		scan->flags = RXON_FLG_BAND_24G_MSK | RXON_FLG_AUTO_DETECT_MSK;
+		tx_ant = iwl_scan_tx_ant(priv, band);
+		if (priv->active_rxon.flags & RXON_FLG_CHANNEL_MODE_PURE_40_MSK)
+			scan->tx_cmd.rate_n_flags =
+				iwl_hw_set_rate_n_flags(IWL_RATE_6M_PLCP,
+							tx_ant);
+		else
+			scan->tx_cmd.rate_n_flags =
+				iwl_hw_set_rate_n_flags(IWL_RATE_1M_PLCP,
+							tx_ant |
+							RATE_MCS_CCK_MSK);
+		scan->good_CRC_th = 0;
+	} else if (priv->scan_bands & BIT(IEEE80211_BAND_5GHZ)) {
+		band = IEEE80211_BAND_5GHZ;
+		tx_ant = iwl_scan_tx_ant(priv, band);
+		scan->tx_cmd.rate_n_flags =
+				iwl_hw_set_rate_n_flags(IWL_RATE_6M_PLCP,
+							tx_ant);
+		scan->good_CRC_th = IWL_GOOD_CRC_TH;
+
+		/* Force use of chains B and C (0x6) for scan Rx for 4965
+		 * Avoid A (0x1) because of its off-channel reception on A-band.
+		 * MIMO is not used here, but value is required */
+		if ((priv->hw_rev & CSR_HW_REV_TYPE_MSK) == CSR_HW_REV_TYPE_4965)
+			rx_chain = 0x6;
+	} else {
+		IWL_WARNING("Invalid scan band count\n");
+		goto done;
+	}
+
+	scan->rx_chain = RXON_RX_CHAIN_DRIVER_FORCE_MSK |
+				cpu_to_le16((0x7 << RXON_RX_CHAIN_VALID_POS) |
+				(rx_chain << RXON_RX_CHAIN_FORCE_SEL_POS) |
+				(0x7 << RXON_RX_CHAIN_FORCE_MIMO_SEL_POS));
+
+	cmd_len = iwl_fill_probe_req(priv, band,
+				     (struct ieee80211_mgmt *)scan->data,
+				     IWL_MAX_SCAN_SIZE - sizeof(*scan));
+
+	scan->tx_cmd.len = cpu_to_le16(cmd_len);
+
+	if (priv->iw_mode == IEEE80211_IF_TYPE_MNTR)
+		scan->filter_flags = RXON_FILTER_PROMISC_MSK;
+
+	scan->filter_flags |= (RXON_FILTER_ACCEPT_GRP_MSK |
+			       RXON_FILTER_BCON_AWARE_MSK);
+
+	scan->channel_count =
+		iwl_get_channels_for_scan(priv, band, 1, /* active */
+			n_probes,
+			(void *)&scan->data[le16_to_cpu(scan->tx_cmd.len)]);
+
+	if (scan->channel_count == 0) {
+		IWL_DEBUG_SCAN("channel count %d\n", scan->channel_count);
+		goto done;
+	}
+
+	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);
+	ret = iwl_send_cmd_sync(priv, &cmd);
+	if (ret)
+		goto done;
+
+	queue_delayed_work(priv->workqueue, &priv->scan_check,
+			   IWL_SCAN_CHECK_WATCHDOG);
+
+	mutex_unlock(&priv->mutex);
+	return;
+
+ done:
+	/* inform mac80211 scan aborted */
+	queue_work(priv->workqueue, &priv->scan_completed);
+	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);
+}
+
+void iwl_setup_scan_deferred_work(struct iwl_priv *priv)
+{
+	/*  FIXME: move here when resolved PENDING
+	 *  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_DELAYED_WORK(&priv->scan_check, iwl_bg_scan_check);
+}
+EXPORT_SYMBOL(iwl_setup_scan_deferred_work);
+
diff --git a/drivers/net/wireless/iwlwifi/iwl-sta.c b/drivers/net/wireless/iwlwifi/iwl-sta.c
index e4fdfaa2b9b2..6d1467d0bd9d 100644
--- a/drivers/net/wireless/iwlwifi/iwl-sta.c
+++ b/drivers/net/wireless/iwlwifi/iwl-sta.c
@@ -28,17 +28,446 @@
  *****************************************************************************/
 
 #include <net/mac80211.h>
+#include <linux/etherdevice.h>
 
-#include "iwl-eeprom.h"
-#include "iwl-4965.h"
+#include "iwl-dev.h"
 #include "iwl-core.h"
 #include "iwl-sta.h"
-#include "iwl-io.h"
 #include "iwl-helpers.h"
-#include "iwl-4965.h"
-#include "iwl-sta.h"
 
-int iwl_get_free_ucode_key_index(struct iwl_priv *priv)
+
+#define IWL_STA_DRIVER_ACTIVE BIT(0) /* driver entry is active */
+#define IWL_STA_UCODE_ACTIVE  BIT(1) /* ucode entry is active */
+
+u8 iwl_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 priv->hw_params.bcast_sta_id;
+
+	spin_lock_irqsave(&priv->sta_lock, flags);
+	for (i = start; i < priv->hw_params.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;
+}
+EXPORT_SYMBOL(iwl_find_station);
+
+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_find_station(priv, da);
+	}
+}
+EXPORT_SYMBOL(iwl_get_ra_sta_id);
+
+static void iwl_sta_ucode_activate(struct iwl_priv *priv, u8 sta_id)
+{
+	unsigned long flags;
+	DECLARE_MAC_BUF(mac);
+
+	spin_lock_irqsave(&priv->sta_lock, flags);
+
+	if (!(priv->stations[sta_id].used & IWL_STA_DRIVER_ACTIVE))
+		IWL_ERROR("ACTIVATE a non DRIVER active station %d\n", sta_id);
+
+	priv->stations[sta_id].used |= IWL_STA_UCODE_ACTIVE;
+	IWL_DEBUG_ASSOC("Added STA to Ucode: %s\n",
+			print_mac(mac, priv->stations[sta_id].sta.sta.addr));
+
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+}
+
+static int iwl_add_sta_callback(struct iwl_priv *priv,
+				   struct iwl_cmd *cmd, struct sk_buff *skb)
+{
+	struct iwl_rx_packet *res = NULL;
+	u8 sta_id = cmd->cmd.addsta.sta.sta_id;
+
+	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:
+		iwl_sta_ucode_activate(priv, sta_id);
+		 /* fall through */
+	default:
+		IWL_DEBUG_HC("Received REPLY_ADD_STA:(0x%08X)\n",
+			     res->u.add_sta.status);
+		break;
+	}
+
+	/* We didn't cache the SKB; let the caller free it */
+	return 1;
+}
+
+int iwl_send_add_sta(struct iwl_priv *priv,
+		     struct iwl_addsta_cmd *sta, u8 flags)
+{
+	struct iwl_rx_packet *res = NULL;
+	int ret = 0;
+	u8 data[sizeof(*sta)];
+	struct iwl_host_cmd cmd = {
+		.id = REPLY_ADD_STA,
+		.meta.flags = flags,
+		.data = data,
+	};
+
+	if (flags & CMD_ASYNC)
+		cmd.meta.u.callback = iwl_add_sta_callback;
+	else
+		cmd.meta.flags |= CMD_WANT_SKB;
+
+	cmd.len = priv->cfg->ops->utils->build_addsta_hcmd(sta, data);
+	ret = iwl_send_cmd(priv, &cmd);
+
+	if (ret || (flags & CMD_ASYNC))
+		return ret;
+
+	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);
+		ret = -EIO;
+	}
+
+	if (ret == 0) {
+		switch (res->u.add_sta.status) {
+		case ADD_STA_SUCCESS_MSK:
+			iwl_sta_ucode_activate(priv, sta->sta.sta_id);
+			IWL_DEBUG_INFO("REPLY_ADD_STA PASSED\n");
+			break;
+		default:
+			ret = -EIO;
+			IWL_WARNING("REPLY_ADD_STA failed\n");
+			break;
+		}
+	}
+
+	priv->alloc_rxb_skb--;
+	dev_kfree_skb_any(cmd.meta.u.skb);
+
+	return ret;
+}
+EXPORT_SYMBOL(iwl_send_add_sta);
+
+static void iwl_set_ht_add_station(struct iwl_priv *priv, u8 index,
+				   struct ieee80211_ht_info *sta_ht_inf)
+{
+	__le32 sta_flags;
+	u8 mimo_ps_mode;
+
+	if (!sta_ht_inf || !sta_ht_inf->ht_supported)
+		goto done;
+
+	mimo_ps_mode = (sta_ht_inf->cap & IEEE80211_HT_CAP_MIMO_PS) >> 2;
+
+	sta_flags = priv->stations[index].sta.station_flags;
+
+	sta_flags &= ~(STA_FLG_RTS_MIMO_PROT_MSK | STA_FLG_MIMO_DIS_MSK);
+
+	switch (mimo_ps_mode) {
+	case WLAN_HT_CAP_MIMO_PS_STATIC:
+		sta_flags |= STA_FLG_MIMO_DIS_MSK;
+		break;
+	case WLAN_HT_CAP_MIMO_PS_DYNAMIC:
+		sta_flags |= STA_FLG_RTS_MIMO_PROT_MSK;
+		break;
+	case WLAN_HT_CAP_MIMO_PS_DISABLED:
+		break;
+	default:
+		IWL_WARNING("Invalid MIMO PS mode %d", mimo_ps_mode);
+		break;
+	}
+
+	sta_flags |= cpu_to_le32(
+	      (u32)sta_ht_inf->ampdu_factor << STA_FLG_MAX_AGG_SIZE_POS);
+
+	sta_flags |= cpu_to_le32(
+	      (u32)sta_ht_inf->ampdu_density << STA_FLG_AGG_MPDU_DENSITY_POS);
+
+	if (iwl_is_fat_tx_allowed(priv, sta_ht_inf))
+		sta_flags |= STA_FLG_FAT_EN_MSK;
+	else
+		sta_flags &= ~STA_FLG_FAT_EN_MSK;
+
+	priv->stations[index].sta.station_flags = sta_flags;
+ done:
+	return;
+}
+
+/**
+ * iwl_add_station_flags - Add station to tables in driver and device
+ */
+u8 iwl_add_station_flags(struct iwl_priv *priv, const u8 *addr, int is_ap,
+			 u8 flags, struct ieee80211_ht_info *ht_info)
+{
+	int i;
+	int sta_id = 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)
+		sta_id = IWL_AP_ID;
+	else if (is_broadcast_ether_addr(addr))
+		sta_id = priv->hw_params.bcast_sta_id;
+	else
+		for (i = IWL_STA_ID; i < priv->hw_params.max_stations; i++) {
+			if (!compare_ether_addr(priv->stations[i].sta.sta.addr,
+						addr)) {
+				sta_id = i;
+				break;
+			}
+
+			if (!priv->stations[i].used &&
+			    sta_id == IWL_INVALID_STATION)
+				sta_id = i;
+		}
+
+	/* These two conditions have the same outcome, but keep them separate
+	   since they have different meanings */
+	if (unlikely(sta_id == IWL_INVALID_STATION)) {
+		spin_unlock_irqrestore(&priv->sta_lock, flags_spin);
+		return sta_id;
+	}
+
+	if (priv->stations[sta_id].used &&
+	    !compare_ether_addr(priv->stations[sta_id].sta.sta.addr, addr)) {
+		spin_unlock_irqrestore(&priv->sta_lock, flags_spin);
+		return sta_id;
+	}
+
+
+	station = &priv->stations[sta_id];
+	station->used = IWL_STA_DRIVER_ACTIVE;
+	IWL_DEBUG_ASSOC("Add STA to driver ID %d: %s\n",
+			sta_id, print_mac(mac, addr));
+	priv->num_stations++;
+
+	/* Set up the REPLY_ADD_STA command to send to device */
+	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 = sta_id;
+	station->sta.station_flags = 0;
+
+	/* BCAST station and IBSS stations do not work in HT mode */
+	if (sta_id != priv->hw_params.bcast_sta_id &&
+	    priv->iw_mode != IEEE80211_IF_TYPE_IBSS)
+		iwl_set_ht_add_station(priv, sta_id, ht_info);
+
+	spin_unlock_irqrestore(&priv->sta_lock, flags_spin);
+
+	/* Add station to device's station table */
+	iwl_send_add_sta(priv, &station->sta, flags);
+	return sta_id;
+
+}
+EXPORT_SYMBOL(iwl_add_station_flags);
+
+static void iwl_sta_ucode_deactivate(struct iwl_priv *priv, const char *addr)
+{
+	unsigned long flags;
+	DECLARE_MAC_BUF(mac);
+
+	u8 sta_id = iwl_find_station(priv, addr);
+
+	BUG_ON(sta_id == IWL_INVALID_STATION);
+
+	IWL_DEBUG_ASSOC("Removed STA from Ucode: %s\n",
+			print_mac(mac, addr));
+
+	spin_lock_irqsave(&priv->sta_lock, flags);
+
+	/* Ucode must be active and driver must be non active */
+	if (priv->stations[sta_id].used != IWL_STA_UCODE_ACTIVE)
+		IWL_ERROR("removed non active STA %d\n", sta_id);
+
+	priv->stations[sta_id].used &= ~IWL_STA_UCODE_ACTIVE;
+
+	memset(&priv->stations[sta_id], 0, sizeof(struct iwl_station_entry));
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+}
+
+static int iwl_remove_sta_callback(struct iwl_priv *priv,
+				   struct iwl_cmd *cmd, struct sk_buff *skb)
+{
+	struct iwl_rx_packet *res = NULL;
+	const char *addr = cmd->cmd.rm_sta.addr;
+
+	if (!skb) {
+		IWL_ERROR("Error: Response NULL in REPLY_REMOVE_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_REMOVE_STA (0x%08X)\n",
+		res->hdr.flags);
+		return 1;
+	}
+
+	switch (res->u.rem_sta.status) {
+	case REM_STA_SUCCESS_MSK:
+		iwl_sta_ucode_deactivate(priv, addr);
+		break;
+	default:
+		IWL_ERROR("REPLY_REMOVE_STA failed\n");
+		break;
+	}
+
+	/* We didn't cache the SKB; let the caller free it */
+	return 1;
+}
+
+static int iwl_send_remove_station(struct iwl_priv *priv, const u8 *addr,
+				   u8 flags)
+{
+	struct iwl_rx_packet *res = NULL;
+	int ret;
+
+	struct iwl_rem_sta_cmd rm_sta_cmd;
+
+	struct iwl_host_cmd cmd = {
+		.id = REPLY_REMOVE_STA,
+		.len = sizeof(struct iwl_rem_sta_cmd),
+		.meta.flags = flags,
+		.data = &rm_sta_cmd,
+	};
+
+	memset(&rm_sta_cmd, 0, sizeof(rm_sta_cmd));
+	rm_sta_cmd.num_sta = 1;
+	memcpy(&rm_sta_cmd.addr, addr , ETH_ALEN);
+
+	if (flags & CMD_ASYNC)
+		cmd.meta.u.callback = iwl_remove_sta_callback;
+	else
+		cmd.meta.flags |= CMD_WANT_SKB;
+	ret = iwl_send_cmd(priv, &cmd);
+
+	if (ret || (flags & CMD_ASYNC))
+		return ret;
+
+	res = (struct iwl_rx_packet *)cmd.meta.u.skb->data;
+	if (res->hdr.flags & IWL_CMD_FAILED_MSK) {
+		IWL_ERROR("Bad return from REPLY_REMOVE_STA (0x%08X)\n",
+			  res->hdr.flags);
+		ret = -EIO;
+	}
+
+	if (!ret) {
+		switch (res->u.rem_sta.status) {
+		case REM_STA_SUCCESS_MSK:
+			iwl_sta_ucode_deactivate(priv, addr);
+			IWL_DEBUG_ASSOC("REPLY_REMOVE_STA PASSED\n");
+			break;
+		default:
+			ret = -EIO;
+			IWL_ERROR("REPLY_REMOVE_STA failed\n");
+			break;
+		}
+	}
+
+	priv->alloc_rxb_skb--;
+	dev_kfree_skb_any(cmd.meta.u.skb);
+
+	return ret;
+}
+
+/**
+ * iwl_remove_station - Remove driver's knowledge of station.
+ */
+int iwl_remove_station(struct iwl_priv *priv, const u8 *addr, int is_ap)
+{
+	int sta_id = IWL_INVALID_STATION;
+	int i, ret = -EINVAL;
+	unsigned long flags;
+	DECLARE_MAC_BUF(mac);
+
+	spin_lock_irqsave(&priv->sta_lock, flags);
+
+	if (is_ap)
+		sta_id = IWL_AP_ID;
+	else if (is_broadcast_ether_addr(addr))
+		sta_id = priv->hw_params.bcast_sta_id;
+	else
+		for (i = IWL_STA_ID; i < priv->hw_params.max_stations; i++)
+			if (priv->stations[i].used &&
+			    !compare_ether_addr(priv->stations[i].sta.sta.addr,
+						addr)) {
+				sta_id = i;
+				break;
+			}
+
+	if (unlikely(sta_id == IWL_INVALID_STATION))
+		goto out;
+
+	IWL_DEBUG_ASSOC("Removing STA from driver:%d  %s\n",
+		sta_id, print_mac(mac, addr));
+
+	if (!(priv->stations[sta_id].used & IWL_STA_DRIVER_ACTIVE)) {
+		IWL_ERROR("Removing %s but non DRIVER active\n",
+				print_mac(mac, addr));
+		goto out;
+	}
+
+	if (!(priv->stations[sta_id].used & IWL_STA_UCODE_ACTIVE)) {
+		IWL_ERROR("Removing %s but non UCODE active\n",
+				print_mac(mac, addr));
+		goto out;
+	}
+
+
+	priv->stations[sta_id].used &= ~IWL_STA_DRIVER_ACTIVE;
+
+	priv->num_stations--;
+
+	BUG_ON(priv->num_stations < 0);
+
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+
+	ret = iwl_send_remove_station(priv, addr, CMD_ASYNC);
+	return ret;
+out:
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+	return ret;
+}
+EXPORT_SYMBOL(iwl_remove_station);
+
+static int iwl_get_free_ucode_key_index(struct iwl_priv *priv)
 {
 	int i;
 
@@ -91,6 +520,7 @@ int iwl_send_static_wepkey_cmd(struct iwl_priv *priv, u8 send_if_empty)
 	else
 		return 0;
 }
+EXPORT_SYMBOL(iwl_send_static_wepkey_cmd);
 
 int iwl_remove_default_wep_key(struct iwl_priv *priv,
 			       struct ieee80211_key_conf *keyconf)
@@ -107,10 +537,13 @@ int iwl_remove_default_wep_key(struct iwl_priv *priv,
 	priv->default_wep_key--;
 	memset(&priv->wep_keys[keyconf->keyidx], 0, sizeof(priv->wep_keys[0]));
 	ret = iwl_send_static_wepkey_cmd(priv, 1);
+	IWL_DEBUG_WEP("Remove default WEP key: idx=%d ret=%d\n",
+		      keyconf->keyidx, ret);
 	spin_unlock_irqrestore(&priv->sta_lock, flags);
 
 	return ret;
 }
+EXPORT_SYMBOL(iwl_remove_default_wep_key);
 
 int iwl_set_default_wep_key(struct iwl_priv *priv,
 			    struct ieee80211_key_conf *keyconf)
@@ -118,8 +551,14 @@ int iwl_set_default_wep_key(struct iwl_priv *priv,
 	int ret;
 	unsigned long flags;
 
+	if (keyconf->keylen != WEP_KEY_LEN_128 &&
+	    keyconf->keylen != WEP_KEY_LEN_64) {
+		IWL_DEBUG_WEP("Bad WEP key length %d\n", keyconf->keylen);
+		return -EINVAL;
+	}
+
 	keyconf->flags &= ~IEEE80211_KEY_FLAG_GENERATE_IV;
-	keyconf->hw_key_idx = keyconf->keyidx;
+	keyconf->hw_key_idx = HW_KEY_DEFAULT;
 	priv->stations[IWL_AP_ID].keyinfo.alg = ALG_WEP;
 
 	spin_lock_irqsave(&priv->sta_lock, flags);
@@ -134,10 +573,13 @@ int iwl_set_default_wep_key(struct iwl_priv *priv,
 							keyconf->keylen);
 
 	ret = iwl_send_static_wepkey_cmd(priv, 0);
+	IWL_DEBUG_WEP("Set default WEP key: len=%d idx=%d ret=%d\n",
+		keyconf->keylen, keyconf->keyidx, ret);
 	spin_unlock_irqrestore(&priv->sta_lock, flags);
 
 	return ret;
 }
+EXPORT_SYMBOL(iwl_set_default_wep_key);
 
 static int iwl_set_wep_dynamic_key_info(struct iwl_priv *priv,
 				struct ieee80211_key_conf *keyconf,
@@ -148,7 +590,6 @@ static int iwl_set_wep_dynamic_key_info(struct iwl_priv *priv,
 	int ret;
 
 	keyconf->flags &= ~IEEE80211_KEY_FLAG_GENERATE_IV;
-	keyconf->hw_key_idx = keyconf->keyidx;
 
 	key_flags |= (STA_KEY_FLG_WEP | STA_KEY_FLG_MAP_KEY_MSK);
 	key_flags |= cpu_to_le16(keyconf->keyidx << STA_KEY_FLG_KEYID_POS);
@@ -172,15 +613,18 @@ static int iwl_set_wep_dynamic_key_info(struct iwl_priv *priv,
 	memcpy(&priv->stations[sta_id].sta.key.key[3],
 				keyconf->key, keyconf->keylen);
 
-	priv->stations[sta_id].sta.key.key_offset =
+	if ((priv->stations[sta_id].sta.key.key_flags & STA_KEY_FLG_ENCRYPT_MSK)
+			== STA_KEY_FLG_NO_ENC)
+		priv->stations[sta_id].sta.key.key_offset =
 				 iwl_get_free_ucode_key_index(priv);
-	priv->stations[sta_id].sta.key.key_flags = key_flags;
+	/* else, we are overriding an existing key => no need to allocated room
+	 * in uCode. */
 
+	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;
 
-	ret = iwl4965_send_add_station(priv,
-		&priv->stations[sta_id].sta, CMD_ASYNC);
+	ret = iwl_send_add_sta(priv, &priv->stations[sta_id].sta, CMD_ASYNC);
 
 	spin_unlock_irqrestore(&priv->sta_lock, flags);
 
@@ -202,7 +646,6 @@ static int iwl_set_ccmp_dynamic_key_info(struct iwl_priv *priv,
 		key_flags |= STA_KEY_MULTICAST_MSK;
 
 	keyconf->flags |= IEEE80211_KEY_FLAG_GENERATE_IV;
-	keyconf->hw_key_idx = keyconf->keyidx;
 
 	spin_lock_irqsave(&priv->sta_lock, flags);
 	priv->stations[sta_id].keyinfo.alg = keyconf->alg;
@@ -214,8 +657,13 @@ static int iwl_set_ccmp_dynamic_key_info(struct iwl_priv *priv,
 	memcpy(priv->stations[sta_id].sta.key.key, keyconf->key,
 	       keyconf->keylen);
 
-	priv->stations[sta_id].sta.key.key_offset =
-				iwl_get_free_ucode_key_index(priv);
+	if ((priv->stations[sta_id].sta.key.key_flags & STA_KEY_FLG_ENCRYPT_MSK)
+			== STA_KEY_FLG_NO_ENC)
+		priv->stations[sta_id].sta.key.key_offset =
+				 iwl_get_free_ucode_key_index(priv);
+	/* else, we are overriding an existing key => no need to allocated room
+	 * in uCode. */
+
 	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;
@@ -223,8 +671,7 @@ static int iwl_set_ccmp_dynamic_key_info(struct iwl_priv *priv,
 	spin_unlock_irqrestore(&priv->sta_lock, flags);
 
 	IWL_DEBUG_INFO("hwcrypto: modify ucode station key info\n");
-	return iwl4965_send_add_station(priv,
-				&priv->stations[sta_id].sta, CMD_ASYNC);
+	return iwl_send_add_sta(priv, &priv->stations[sta_id].sta, CMD_ASYNC);
 }
 
 static int iwl_set_tkip_dynamic_key_info(struct iwl_priv *priv,
@@ -236,15 +683,18 @@ static int iwl_set_tkip_dynamic_key_info(struct iwl_priv *priv,
 
 	keyconf->flags |= IEEE80211_KEY_FLAG_GENERATE_IV;
 	keyconf->flags |= IEEE80211_KEY_FLAG_GENERATE_MMIC;
-	keyconf->hw_key_idx = keyconf->keyidx;
 
 	spin_lock_irqsave(&priv->sta_lock, flags);
 
 	priv->stations[sta_id].keyinfo.alg = keyconf->alg;
-	priv->stations[sta_id].keyinfo.conf = keyconf;
 	priv->stations[sta_id].keyinfo.keylen = 16;
-	priv->stations[sta_id].sta.key.key_offset =
+
+	if ((priv->stations[sta_id].sta.key.key_flags & STA_KEY_FLG_ENCRYPT_MSK)
+			== STA_KEY_FLG_NO_ENC)
+		priv->stations[sta_id].sta.key.key_offset =
 				 iwl_get_free_ucode_key_index(priv);
+	/* else, we are overriding an existing key => no need to allocated room
+	 * in uCode. */
 
 	/* This copy is acutally not needed: we get the key with each TX */
 	memcpy(priv->stations[sta_id].keyinfo.key, keyconf->key, 16);
@@ -256,54 +706,84 @@ static int iwl_set_tkip_dynamic_key_info(struct iwl_priv *priv,
 	return ret;
 }
 
-int iwl_remove_dynamic_key(struct iwl_priv *priv, u8 sta_id)
+int iwl_remove_dynamic_key(struct iwl_priv *priv,
+				struct ieee80211_key_conf *keyconf,
+				u8 sta_id)
 {
 	unsigned long flags;
+	int ret = 0;
+	u16 key_flags;
+	u8 keyidx;
 
-	priv->key_mapping_key = 0;
+	priv->key_mapping_key--;
 
 	spin_lock_irqsave(&priv->sta_lock, flags);
+	key_flags = le16_to_cpu(priv->stations[sta_id].sta.key.key_flags);
+	keyidx = (key_flags >> STA_KEY_FLG_KEYID_POS) & 0x3;
+
+	IWL_DEBUG_WEP("Remove dynamic key: idx=%d sta=%d\n",
+		      keyconf->keyidx, sta_id);
+
+	if (keyconf->keyidx != keyidx) {
+		/* We need to remove a key with index different that the one
+		 * in the uCode. This means that the key we need to remove has
+		 * been replaced by another one with different index.
+		 * Don't do anything and return ok
+		 */
+		spin_unlock_irqrestore(&priv->sta_lock, flags);
+		return 0;
+	}
+
 	if (!test_and_clear_bit(priv->stations[sta_id].sta.key.key_offset,
 		&priv->ucode_key_table))
 		IWL_ERROR("index %d not used in uCode key table.\n",
 			priv->stations[sta_id].sta.key.key_offset);
 	memset(&priv->stations[sta_id].keyinfo, 0,
-					sizeof(struct iwl4965_hw_key));
+					sizeof(struct iwl_hw_key));
 	memset(&priv->stations[sta_id].sta.key, 0,
 					sizeof(struct iwl4965_keyinfo));
-	priv->stations[sta_id].sta.key.key_flags = STA_KEY_FLG_NO_ENC;
+	priv->stations[sta_id].sta.key.key_flags =
+			STA_KEY_FLG_NO_ENC | STA_KEY_FLG_INVALID;
+	priv->stations[sta_id].sta.key.key_offset = WEP_INVALID_OFFSET;
 	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");
-	return iwl4965_send_add_station(priv, &priv->stations[sta_id].sta, 0);
+	ret =  iwl_send_add_sta(priv, &priv->stations[sta_id].sta, CMD_ASYNC);
+	spin_unlock_irqrestore(&priv->sta_lock, flags);
+	return ret;
 }
+EXPORT_SYMBOL(iwl_remove_dynamic_key);
 
 int iwl_set_dynamic_key(struct iwl_priv *priv,
-				struct ieee80211_key_conf *key, u8 sta_id)
+				struct ieee80211_key_conf *keyconf, u8 sta_id)
 {
 	int ret;
 
-	priv->key_mapping_key = 1;
+	priv->key_mapping_key++;
+	keyconf->hw_key_idx = HW_KEY_DYNAMIC;
 
-	switch (key->alg) {
+	switch (keyconf->alg) {
 	case ALG_CCMP:
-		ret = iwl_set_ccmp_dynamic_key_info(priv, key, sta_id);
+		ret = iwl_set_ccmp_dynamic_key_info(priv, keyconf, sta_id);
 		break;
 	case ALG_TKIP:
-		ret = iwl_set_tkip_dynamic_key_info(priv, key, sta_id);
+		ret = iwl_set_tkip_dynamic_key_info(priv, keyconf, sta_id);
 		break;
 	case ALG_WEP:
-		ret = iwl_set_wep_dynamic_key_info(priv, key, sta_id);
+		ret = iwl_set_wep_dynamic_key_info(priv, keyconf, sta_id);
 		break;
 	default:
-		IWL_ERROR("Unknown alg: %s alg = %d\n", __func__, key->alg);
+		IWL_ERROR("Unknown alg: %s alg = %d\n", __func__, keyconf->alg);
 		ret = -EINVAL;
 	}
 
+	IWL_DEBUG_WEP("Set dynamic key: alg= %d len=%d idx=%d sta=%d ret=%d\n",
+		      keyconf->alg, keyconf->keylen, keyconf->keyidx,
+		      sta_id, ret);
+
 	return ret;
 }
+EXPORT_SYMBOL(iwl_set_dynamic_key);
 
 #ifdef CONFIG_IWLWIFI_DEBUG
 static void iwl_dump_lq_cmd(struct iwl_priv *priv,
@@ -345,11 +825,171 @@ int iwl_send_lq_cmd(struct iwl_priv *priv,
 
 	iwl_dump_lq_cmd(priv,lq);
 
-	if (iwl_is_associated(priv) && priv->assoc_station_added &&
-	    priv->lq_mngr.lq_ready)
+	if (iwl_is_associated(priv) && priv->assoc_station_added)
 		return  iwl_send_cmd(priv, &cmd);
 
 	return 0;
 }
 EXPORT_SYMBOL(iwl_send_lq_cmd);
 
+/**
+ * iwl_sta_init_lq - Initialize a station's hardware rate table
+ *
+ * The uCode's station table contains a table of fallback rates
+ * for automatic fallback during transmission.
+ *
+ * NOTE: This sets up a default set of values.  These will be replaced later
+ *       if the driver's iwl-4965-rs rate scaling algorithm is used, instead of
+ *       rc80211_simple.
+ *
+ * NOTE: Run REPLY_ADD_STA command to set up station table entry, before
+ *       calling this function (which runs REPLY_TX_LINK_QUALITY_CMD,
+ *       which requires station table entry to exist).
+ */
+static void iwl_sta_init_lq(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 selected rate, fall back
+	 * all the way down to 1M in IEEE order, and then spin on 1M */
+	if (is_ap)
+		r = IWL_RATE_54M_INDEX;
+	else if (priv->band == IEEE80211_BAND_5GHZ)
+		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;
+
+		/* Use Tx antenna B only */
+		rate_flags |= RATE_MCS_ANT_B_MSK; /*FIXME:RS*/
+
+		link_cmd.rs_table[i].rate_n_flags =
+			iwl_hw_set_rate_n_flags(iwl_rates[r].plcp, rate_flags);
+		r = iwl4965_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 : priv->hw_params.bcast_sta_id;
+
+	iwl_send_cmd_pdu_async(priv, REPLY_TX_LINK_QUALITY_CMD,
+			       sizeof(link_cmd), &link_cmd, NULL);
+}
+
+/**
+ * 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 this fnction
+ */
+int iwl_rxon_add_station(struct iwl_priv *priv, const u8 *addr, int is_ap)
+{
+	u8 sta_id;
+
+	/* Add station to device's station table */
+	struct ieee80211_conf *conf = &priv->hw->conf;
+	struct ieee80211_ht_info *cur_ht_config = &conf->ht_conf;
+
+	if ((is_ap) &&
+	    (conf->flags & IEEE80211_CONF_SUPPORT_HT_MODE) &&
+	    (priv->iw_mode == IEEE80211_IF_TYPE_STA))
+		sta_id = iwl_add_station_flags(priv, addr, is_ap,
+						   0, cur_ht_config);
+	else
+		sta_id = iwl_add_station_flags(priv, addr, is_ap,
+						   0, NULL);
+
+	/* Set up default rate scaling table in device's station table */
+	iwl_sta_init_lq(priv, addr, is_ap);
+
+	return sta_id;
+}
+EXPORT_SYMBOL(iwl_rxon_add_station);
+
+/**
+ * iwl_get_sta_id - Find station's index within station table
+ *
+ * If new IBSS station, create new entry in station table
+ */
+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 management, use broadcast station id */
+	if (((fc & IEEE80211_FCTL_FTYPE) != IEEE80211_FTYPE_DATA) ||
+	    is_multicast_ether_addr(hdr->addr1))
+		return priv->hw_params.bcast_sta_id;
+
+	switch (priv->iw_mode) {
+
+	/* If we are a client station in a BSS network, use the special
+	 * AP station entry (that's the only station we communicate with) */
+	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_find_station(priv, hdr->addr1);
+		if (sta_id != IWL_INVALID_STATION)
+			return sta_id;
+		return priv->hw_params.bcast_sta_id;
+
+	/* If this frame is going out to an IBSS network, find the station,
+	 * or create a new station table entry */
+	case IEEE80211_IF_TYPE_IBSS:
+		sta_id = iwl_find_station(priv, hdr->addr1);
+		if (sta_id != IWL_INVALID_STATION)
+			return sta_id;
+
+		/* Create new station table entry */
+		sta_id = iwl_add_station_flags(priv, hdr->addr1,
+						   0, CMD_ASYNC, NULL);
+
+		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(priv, IWL_DL_DROP, (u8 *) hdr, sizeof(*hdr));
+		return priv->hw_params.bcast_sta_id;
+
+	default:
+		IWL_WARNING("Unknown mode of operation: %d", priv->iw_mode);
+		return priv->hw_params.bcast_sta_id;
+	}
+}
+EXPORT_SYMBOL(iwl_get_sta_id);
+
+/**
+ * iwl_sta_modify_enable_tid_tx - Enable Tx for this TID in station table
+ */
+void iwl_sta_modify_enable_tid_tx(struct iwl_priv *priv, int sta_id, int tid)
+{
+	unsigned long flags;
+
+	/* Remove "disable" flag, to enable Tx for this TID */
+	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_sta(priv, &priv->stations[sta_id].sta, CMD_ASYNC);
+}
+EXPORT_SYMBOL(iwl_sta_modify_enable_tid_tx);
+
diff --git a/drivers/net/wireless/iwlwifi/iwl-sta.h b/drivers/net/wireless/iwlwifi/iwl-sta.h
index 44f272ecc827..221b93e670a6 100644
--- a/drivers/net/wireless/iwlwifi/iwl-sta.h
+++ b/drivers/net/wireless/iwlwifi/iwl-sta.h
@@ -29,21 +29,27 @@
 #ifndef __iwl_sta_h__
 #define __iwl_sta_h__
 
-#include <net/mac80211.h>
+#define HW_KEY_DYNAMIC 0
+#define HW_KEY_DEFAULT 1
 
-#include "iwl-eeprom.h"
-#include "iwl-core.h"
-#include "iwl-4965.h"
-#include "iwl-io.h"
-#include "iwl-helpers.h"
+/**
+ * iwl_find_station - Find station id for a given BSSID
+ * @bssid: MAC address of station ID to find
+ */
+u8 iwl_find_station(struct iwl_priv *priv, const u8 *bssid);
 
-int iwl_get_free_ucode_key_index(struct iwl_priv *priv);
 int iwl_send_static_wepkey_cmd(struct iwl_priv *priv, u8 send_if_empty);
 int iwl_remove_default_wep_key(struct iwl_priv *priv,
-				struct ieee80211_key_conf *key);
+			       struct ieee80211_key_conf *key);
 int iwl_set_default_wep_key(struct iwl_priv *priv,
-				struct ieee80211_key_conf *key);
-int iwl_remove_dynamic_key(struct iwl_priv *priv, u8 sta_id);
+			    struct ieee80211_key_conf *key);
 int iwl_set_dynamic_key(struct iwl_priv *priv,
-				struct ieee80211_key_conf *key, u8 sta_id);
+			struct ieee80211_key_conf *key, u8 sta_id);
+int iwl_remove_dynamic_key(struct iwl_priv *priv,
+			   struct ieee80211_key_conf *key, u8 sta_id);
+int iwl_rxon_add_station(struct iwl_priv *priv, const u8 *addr, int is_ap);
+int iwl_remove_station(struct iwl_priv *priv, const u8 *addr, int is_ap);
+int iwl_get_sta_id(struct iwl_priv *priv, struct ieee80211_hdr *hdr);
+void iwl_sta_modify_enable_tid_tx(struct iwl_priv *priv, int sta_id, int tid);
+int iwl_get_ra_sta_id(struct iwl_priv *priv, struct ieee80211_hdr *hdr);
 #endif /* __iwl_sta_h__ */
diff --git a/drivers/net/wireless/iwlwifi/iwl-tx.c b/drivers/net/wireless/iwlwifi/iwl-tx.c
new file mode 100644
index 000000000000..9b50b1052b09
--- /dev/null
+++ b/drivers/net/wireless/iwlwifi/iwl-tx.c
@@ -0,0 +1,1519 @@
+/******************************************************************************
+ *
+ * Copyright(c) 2003 - 2008 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
+ *
+ *****************************************************************************/
+
+#include <linux/etherdevice.h>
+#include <net/mac80211.h>
+#include "iwl-eeprom.h"
+#include "iwl-dev.h"
+#include "iwl-core.h"
+#include "iwl-sta.h"
+#include "iwl-io.h"
+#include "iwl-helpers.h"
+
+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
+};
+
+
+/**
+ * iwl_hw_txq_free_tfd - Free all chunks referenced by TFD [txq->q.read_ptr]
+ *
+ * Does NOT advance any TFD circular buffer read/write indexes
+ * Does NOT free the TFD itself (which is within circular buffer)
+ */
+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.read_ptr];
+	struct pci_dev *dev = priv->pci_dev;
+	int i;
+	int counter = 0;
+	int index, is_odd;
+
+	/* Host command buffers stay mapped in memory, nothing to clean */
+	if (txq->q.id == IWL_CMD_QUEUE_NUM)
+		return 0;
+
+	/* Sanity check on number of chunks */
+	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.
+	 * TFD info for odd chunks is different format than for even chunks. */
+	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);
+
+		/* Free SKB, if any, for this chunk */
+		if (txq->txb[txq->q.read_ptr].skb[i]) {
+			struct sk_buff *skb = txq->txb[txq->q.read_ptr].skb[i];
+
+			dev_kfree_skb(skb);
+			txq->txb[txq->q.read_ptr].skb[i] = NULL;
+		}
+	}
+	return 0;
+}
+EXPORT_SYMBOL(iwl_hw_txq_free_tfd);
+
+
+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);
+
+	/* Each TFD can point to a maximum 20 Tx buffers */
+	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,
+			     iwl_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;
+}
+EXPORT_SYMBOL(iwl_hw_txq_attach_buf_to_tfd);
+
+/**
+ * iwl_txq_update_write_ptr - Send new write index to hardware
+ */
+int iwl_txq_update_write_ptr(struct iwl_priv *priv, struct iwl_tx_queue *txq)
+{
+	u32 reg = 0;
+	int ret = 0;
+	int txq_id = txq->q.id;
+
+	if (txq->need_update == 0)
+		return ret;
+
+	/* 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 ret;
+		}
+
+		/* restore this queue's parameters in nic hardware. */
+		ret = iwl_grab_nic_access(priv);
+		if (ret)
+			return ret;
+		iwl_write_direct32(priv, HBUS_TARG_WRPTR,
+				     txq->q.write_ptr | (txq_id << 8));
+		iwl_release_nic_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.write_ptr | (txq_id << 8));
+
+	txq->need_update = 0;
+
+	return ret;
+}
+EXPORT_SYMBOL(iwl_txq_update_write_ptr);
+
+
+/**
+ * iwl_tx_queue_free - Deallocate DMA queue.
+ * @txq: Transmit queue to deallocate.
+ *
+ * Empty queue by removing and destroying all BD's.
+ * Free all buffers.
+ * 0-fill, but do not free "txq" descriptor structure.
+ */
+static 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->write_ptr != q->read_ptr;
+	     q->read_ptr = iwl_queue_inc_wrap(q->read_ptr, 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;
+
+	/* De-alloc array of command/tx buffers */
+	pci_free_consistent(dev, len, txq->cmd, txq->dma_addr_cmd);
+
+	/* De-alloc circular buffer of TFDs */
+	if (txq->q.n_bd)
+		pci_free_consistent(dev, sizeof(struct iwl_tfd_frame) *
+				    txq->q.n_bd, txq->bd, txq->q.dma_addr);
+
+	/* De-alloc array of per-TFD driver data */
+	kfree(txq->txb);
+	txq->txb = NULL;
+
+	/* 0-fill queue descriptor structure */
+	memset(txq, 0, sizeof(*txq));
+}
+
+/*************** DMA-QUEUE-GENERAL-FUNCTIONS  *****
+ * DMA services
+ *
+ * Theory of operation
+ *
+ * A Tx or Rx queue resides in host DRAM, and is comprised of a circular buffer
+ * of buffer descriptors, each of which points to one or more data buffers for
+ * the device to read from or fill.  Driver and device exchange status of each
+ * queue via "read" and "write" pointers.  Driver keeps minimum of 2 empty
+ * entries in each circular buffer, to protect against confusing empty and full
+ * queue states.
+ *
+ * The device reads or writes the data in the queues via the device's several
+ * DMA/FIFO channels.  Each queue is mapped to a single DMA channel.
+ *
+ * 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.
+ *
+ * See more detailed info in iwl-4965-hw.h.
+ ***************************************************/
+
+int iwl_queue_space(const struct iwl_queue *q)
+{
+	int s = q->read_ptr - q->write_ptr;
+
+	if (q->read_ptr > q->write_ptr)
+		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;
+}
+EXPORT_SYMBOL(iwl_queue_space);
+
+
+/**
+ * iwl_queue_init - Initialize queue's high/low-water and read/write indexes
+ */
+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->write_ptr = q->read_ptr = 0;
+
+	return 0;
+}
+
+/**
+ * iwl_tx_queue_alloc - Alloc driver data and TFD CB for one Tx/cmd queue
+ */
+static int iwl_tx_queue_alloc(struct iwl_priv *priv,
+			      struct iwl_tx_queue *txq, u32 id)
+{
+	struct pci_dev *dev = priv->pci_dev;
+
+	/* Driver private data, only for Tx (not command) queues,
+	 * not shared with device. */
+	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 auxiliary BD "
+				  "structures failed\n");
+			goto error;
+		}
+	} else
+		txq->txb = NULL;
+
+	/* Circular buffer of transmit frame descriptors (TFDs),
+	 * shared with device */
+	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:
+	kfree(txq->txb);
+	txq->txb = NULL;
+
+	return -ENOMEM;
+}
+
+/*
+ * Tell nic where to find circular buffer of Tx Frame Descriptors for
+ * given Tx queue, and enable the DMA channel used for that queue.
+ *
+ * 4965 supports up to 16 Tx queues in DRAM, mapped to up to 8 Tx DMA
+ * channels supported in hardware.
+ */
+static 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_nic_access(priv);
+	if (rc) {
+		spin_unlock_irqrestore(&priv->lock, flags);
+		return rc;
+	}
+
+	/* Circular buffer (TFD queue in DRAM) physical base address */
+	iwl_write_direct32(priv, FH_MEM_CBBC_QUEUE(txq_id),
+			     txq->q.dma_addr >> 8);
+
+	/* Enable DMA channel, using same id as for TFD queue */
+	iwl_write_direct32(
+		priv, FH_TCSR_CHNL_TX_CONFIG_REG(txq_id),
+		FH_TCSR_TX_CONFIG_REG_VAL_DMA_CHNL_ENABLE |
+		FH_TCSR_TX_CONFIG_REG_VAL_DMA_CREDIT_ENABLE_VAL);
+	iwl_release_nic_access(priv);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	return 0;
+}
+
+/**
+ * iwl_tx_queue_init - Allocate and initialize one tx/cmd queue
+ */
+static 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;
+
+	/*
+	 * Alloc buffer array for commands (Tx or other types of commands).
+	 * For the command queue (#4), allocate command space + one big
+	 * command for scan, since scan command is very huge; the system will
+	 * not have two scans at the same time, so only one is needed.
+	 * For normal Tx queues (all other queues), no super-size command
+	 * space is needed.
+	 */
+	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;
+
+	/* Alloc driver data array and TFD circular buffer */
+	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));
+
+	/* Initialize queue's high/low-water marks, and head/tail indexes */
+	iwl_queue_init(priv, &txq->q, TFD_QUEUE_SIZE_MAX, slots_num, txq_id);
+
+	/* Tell device where to find queue */
+	iwl_hw_tx_queue_init(priv, txq);
+
+	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_params.max_txq_num; txq_id++)
+		iwl_tx_queue_free(priv, &priv->txq[txq_id]);
+
+	/* Keep-warm buffer */
+	iwl_kw_free(priv);
+}
+EXPORT_SYMBOL(iwl_hw_txq_ctx_free);
+
+
+/**
+ * iwl_txq_ctx_reset - Reset TX queue context
+ * Destroys all DMA structures and initialise them again
+ *
+ * @param priv
+ * @return error code
+ */
+int iwl_txq_ctx_reset(struct iwl_priv *priv)
+{
+	int ret = 0;
+	int txq_id, slots_num;
+	unsigned long flags;
+
+	iwl_kw_free(priv);
+
+	/* Free all tx/cmd queues and keep-warm buffer */
+	iwl_hw_txq_ctx_free(priv);
+
+	/* Alloc keep-warm buffer */
+	ret = iwl_kw_alloc(priv);
+	if (ret) {
+		IWL_ERROR("Keep Warm allocation failed");
+		goto error_kw;
+	}
+	spin_lock_irqsave(&priv->lock, flags);
+	ret = iwl_grab_nic_access(priv);
+	if (unlikely(ret)) {
+		spin_unlock_irqrestore(&priv->lock, flags);
+		goto error_reset;
+	}
+
+	/* Turn off all Tx DMA fifos */
+	priv->cfg->ops->lib->txq_set_sched(priv, 0);
+
+	iwl_release_nic_access(priv);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+
+	/* Tell nic where to find the keep-warm buffer */
+	ret = iwl_kw_init(priv);
+	if (ret) {
+		IWL_ERROR("kw_init failed\n");
+		goto error_reset;
+	}
+
+	/* Alloc and init all Tx queues, including the command queue (#4) */
+	for (txq_id = 0; txq_id < priv->hw_params.max_txq_num; txq_id++) {
+		slots_num = (txq_id == IWL_CMD_QUEUE_NUM) ?
+					TFD_CMD_SLOTS : TFD_TX_CMD_SLOTS;
+		ret = iwl_tx_queue_init(priv, &priv->txq[txq_id], slots_num,
+				       txq_id);
+		if (ret) {
+			IWL_ERROR("Tx %d queue init failed\n", txq_id);
+			goto error;
+		}
+	}
+
+	return ret;
+
+ error:
+	iwl_hw_txq_ctx_free(priv);
+ error_reset:
+	iwl_kw_free(priv);
+ error_kw:
+	return ret;
+}
+/**
+ * iwl_txq_ctx_stop - Stop all Tx DMA channels, free Tx queue memory
+ */
+void iwl_txq_ctx_stop(struct iwl_priv *priv)
+{
+
+	int txq_id;
+	unsigned long flags;
+
+
+	/* Turn off all Tx DMA fifos */
+	spin_lock_irqsave(&priv->lock, flags);
+	if (iwl_grab_nic_access(priv)) {
+		spin_unlock_irqrestore(&priv->lock, flags);
+		return;
+	}
+
+	priv->cfg->ops->lib->txq_set_sched(priv, 0);
+
+	/* Stop each Tx DMA channel, and wait for it to be idle */
+	for (txq_id = 0; txq_id < priv->hw_params.max_txq_num; txq_id++) {
+		iwl_write_direct32(priv,
+				   FH_TCSR_CHNL_TX_CONFIG_REG(txq_id), 0x0);
+		iwl_poll_direct_bit(priv, FH_TSSR_TX_STATUS_REG,
+				    FH_TSSR_TX_STATUS_REG_MSK_CHNL_IDLE
+				    (txq_id), 200);
+	}
+	iwl_release_nic_access(priv);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	/* Deallocate memory for all Tx queues */
+	iwl_hw_txq_ctx_free(priv);
+}
+EXPORT_SYMBOL(iwl_txq_ctx_stop);
+
+/*
+ * handle build REPLY_TX command notification.
+ */
+static void iwl_tx_cmd_build_basic(struct iwl_priv *priv,
+				  struct iwl_tx_cmd *tx_cmd,
+				  struct ieee80211_tx_info *info,
+				  struct ieee80211_hdr *hdr,
+				  int is_unicast, u8 std_id)
+{
+	__le16 fc = hdr->frame_control;
+	__le32 tx_flags = tx_cmd->tx_flags;
+
+	tx_cmd->stop_time.life_time = TX_CMD_LIFE_TIME_INFINITE;
+	if (!(info->flags & IEEE80211_TX_CTL_NO_ACK)) {
+		tx_flags |= TX_CMD_FLG_ACK_MSK;
+		if (ieee80211_is_mgmt(fc))
+			tx_flags |= TX_CMD_FLG_SEQ_CTL_MSK;
+		if (ieee80211_is_probe_resp(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;
+	}
+
+	if (ieee80211_is_back_req(fc))
+		tx_flags |= TX_CMD_FLG_ACK_MSK | TX_CMD_FLG_IMM_BA_RSP_MASK;
+
+
+	tx_cmd->sta_id = std_id;
+	if (ieee80211_has_morefrags(fc))
+		tx_flags |= TX_CMD_FLG_MORE_FRAG_MSK;
+
+	if (ieee80211_is_data_qos(fc)) {
+		u8 *qc = ieee80211_get_qos_ctl(hdr);
+		tx_cmd->tid_tspec = qc[0] & 0xf;
+		tx_flags &= ~TX_CMD_FLG_SEQ_CTL_MSK;
+	} else {
+		tx_flags |= TX_CMD_FLG_SEQ_CTL_MSK;
+	}
+
+	priv->cfg->ops->utils->rts_tx_cmd_flag(info, &tx_flags);
+
+	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 (ieee80211_is_mgmt(fc)) {
+		if (ieee80211_is_assoc_req(fc) || ieee80211_is_reassoc_req(fc))
+			tx_cmd->timeout.pm_frame_timeout = cpu_to_le16(3);
+		else
+			tx_cmd->timeout.pm_frame_timeout = cpu_to_le16(2);
+	} else {
+		tx_cmd->timeout.pm_frame_timeout = 0;
+	}
+
+	tx_cmd->driver_txop = 0;
+	tx_cmd->tx_flags = tx_flags;
+	tx_cmd->next_frame_len = 0;
+}
+
+#define RTS_HCCA_RETRY_LIMIT		3
+#define RTS_DFAULT_RETRY_LIMIT		60
+
+static void iwl_tx_cmd_build_rate(struct iwl_priv *priv,
+			      struct iwl_tx_cmd *tx_cmd,
+			      struct ieee80211_tx_info *info,
+			      __le16 fc, int sta_id,
+			      int is_hcca)
+{
+	u8 rts_retry_limit = 0;
+	u8 data_retry_limit = 0;
+	u8 rate_plcp;
+	u16 rate_flags = 0;
+	int rate_idx;
+
+	rate_idx = min(ieee80211_get_tx_rate(priv->hw, info)->hw_value & 0xffff,
+			IWL_RATE_COUNT - 1);
+
+	rate_plcp = iwl_rates[rate_idx].plcp;
+
+	rts_retry_limit = (is_hcca) ?
+	    RTS_HCCA_RETRY_LIMIT : RTS_DFAULT_RETRY_LIMIT;
+
+	if ((rate_idx >= IWL_FIRST_CCK_RATE) && (rate_idx <= IWL_LAST_CCK_RATE))
+		rate_flags |= RATE_MCS_CCK_MSK;
+
+
+	if (ieee80211_is_probe_resp(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 (ieee80211_is_data(fc)) {
+		tx_cmd->initial_rate_index = 0;
+		tx_cmd->tx_flags |= TX_CMD_FLG_STA_RATE_MSK;
+	} else {
+		switch (fc & cpu_to_le16(IEEE80211_FCTL_STYPE)) {
+		case cpu_to_le16(IEEE80211_STYPE_AUTH):
+		case cpu_to_le16(IEEE80211_STYPE_DEAUTH):
+		case cpu_to_le16(IEEE80211_STYPE_ASSOC_REQ):
+		case cpu_to_le16(IEEE80211_STYPE_REASSOC_REQ):
+			if (tx_cmd->tx_flags & TX_CMD_FLG_RTS_MSK) {
+				tx_cmd->tx_flags &= ~TX_CMD_FLG_RTS_MSK;
+				tx_cmd->tx_flags |= TX_CMD_FLG_CTS_MSK;
+			}
+			break;
+		default:
+			break;
+		}
+
+		/* 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;
+		}
+	}
+
+	tx_cmd->rts_retry_limit = rts_retry_limit;
+	tx_cmd->data_retry_limit = data_retry_limit;
+	tx_cmd->rate_n_flags = iwl_hw_set_rate_n_flags(rate_plcp, rate_flags);
+}
+
+static void iwl_tx_cmd_build_hwcrypto(struct iwl_priv *priv,
+				      struct ieee80211_tx_info *info,
+				      struct iwl_tx_cmd *tx_cmd,
+				      struct sk_buff *skb_frag,
+				      int sta_id)
+{
+	struct ieee80211_key_conf *keyconf = info->control.hw_key;
+
+	switch (keyconf->alg) {
+	case ALG_CCMP:
+		tx_cmd->sec_ctl = TX_CMD_SEC_CCM;
+		memcpy(tx_cmd->key, keyconf->key, keyconf->keylen);
+		if (info->flags & IEEE80211_TX_CTL_AMPDU)
+			tx_cmd->tx_flags |= TX_CMD_FLG_AGG_CCMP_MSK;
+		IWL_DEBUG_TX("tx_cmd with aes hwcrypto\n");
+		break;
+
+	case ALG_TKIP:
+		tx_cmd->sec_ctl = TX_CMD_SEC_TKIP;
+		ieee80211_get_tkip_key(keyconf, skb_frag,
+			IEEE80211_TKIP_P2_KEY, tx_cmd->key);
+		IWL_DEBUG_TX("tx_cmd with tkip hwcrypto\n");
+		break;
+
+	case ALG_WEP:
+		tx_cmd->sec_ctl |= (TX_CMD_SEC_WEP |
+			(keyconf->keyidx & TX_CMD_SEC_MSK) << TX_CMD_SEC_SHIFT);
+
+		if (keyconf->keylen == WEP_KEY_LEN_128)
+			tx_cmd->sec_ctl |= TX_CMD_SEC_KEY128;
+
+		memcpy(&tx_cmd->key[3], keyconf->key, keyconf->keylen);
+
+		IWL_DEBUG_TX("Configuring packet for WEP encryption "
+			     "with key %d\n", keyconf->keyidx);
+		break;
+
+	default:
+		printk(KERN_ERR "Unknown encode alg %d\n", keyconf->alg);
+		break;
+	}
+}
+
+static void iwl_update_tx_stats(struct iwl_priv *priv, u16 fc, u16 len)
+{
+	/* 0 - mgmt, 1 - cnt, 2 - data */
+	int idx = (fc & IEEE80211_FCTL_FTYPE) >> 2;
+	priv->tx_stats[idx].cnt++;
+	priv->tx_stats[idx].bytes += len;
+}
+
+/*
+ * start REPLY_TX command process
+ */
+int iwl_tx_skb(struct iwl_priv *priv, struct sk_buff *skb)
+{
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+	struct iwl_tfd_frame *tfd;
+	u32 *control_flags;
+	int txq_id = skb_get_queue_mapping(skb);
+	struct iwl_tx_queue *txq = NULL;
+	struct iwl_queue *q = NULL;
+	dma_addr_t phys_addr;
+	dma_addr_t txcmd_phys;
+	dma_addr_t scratch_phys;
+	struct iwl_cmd *out_cmd = NULL;
+	struct iwl_tx_cmd *tx_cmd;
+	u16 len, idx, len_org;
+	u16 seq_number = 0;
+	u8 id, hdr_len, unicast;
+	u8 sta_id;
+	__le16 fc;
+	u8 wait_write_ptr = 0;
+	u8 tid = 0;
+	u8 *qc = NULL;
+	unsigned long flags;
+	int ret;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	if (iwl_is_rfkill(priv)) {
+		IWL_DEBUG_DROP("Dropping - RF KILL\n");
+		goto drop_unlock;
+	}
+
+	if (!priv->vif) {
+		IWL_DEBUG_DROP("Dropping - !priv->vif\n");
+		goto drop_unlock;
+	}
+
+	if ((ieee80211_get_tx_rate(priv->hw, info)->hw_value & 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 = hdr->frame_control;
+
+#ifdef CONFIG_IWLWIFI_DEBUG
+	if (ieee80211_is_auth(fc))
+		IWL_DEBUG_TX("Sending AUTH frame\n");
+	else if (ieee80211_is_assoc_req(fc))
+		IWL_DEBUG_TX("Sending ASSOC frame\n");
+	else if (ieee80211_is_reassoc_req(fc))
+		IWL_DEBUG_TX("Sending REASSOC frame\n");
+#endif
+
+	/* drop all data frame if we are not associated */
+	if (ieee80211_is_data(fc) &&
+	   (!iwl_is_associated(priv) ||
+	    ((priv->iw_mode == IEEE80211_IF_TYPE_STA) && !priv->assoc_id) ||
+	    !priv->assoc_station_added)) {
+		IWL_DEBUG_DROP("Dropping - !iwl_is_associated\n");
+		goto drop_unlock;
+	}
+
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	hdr_len = ieee80211_get_hdrlen(le16_to_cpu(fc));
+
+	/* Find (or create) index into station table for destination station */
+	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_TX("station Id %d\n", sta_id);
+
+	if (ieee80211_is_data_qos(fc)) {
+		qc = ieee80211_get_qos_ctl(hdr);
+		tid = qc[0] & 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;
+		/* aggregation is on for this <sta,tid> */
+		if (info->flags & IEEE80211_TX_CTL_AMPDU)
+			txq_id = priv->stations[sta_id].tid[tid].agg.txq_id;
+		priv->stations[sta_id].tid[tid].tfds_in_queue++;
+	}
+
+	/* Descriptor for chosen Tx queue */
+	txq = &priv->txq[txq_id];
+	q = &txq->q;
+
+	spin_lock_irqsave(&priv->lock, flags);
+
+	/* Set up first empty TFD within this queue's circular TFD buffer */
+	tfd = &txq->bd[q->write_ptr];
+	memset(tfd, 0, sizeof(*tfd));
+	control_flags = (u32 *) tfd;
+	idx = get_cmd_index(q, q->write_ptr, 0);
+
+	/* Set up driver data for this TFD */
+	memset(&(txq->txb[q->write_ptr]), 0, sizeof(struct iwl_tx_info));
+	txq->txb[q->write_ptr].skb[0] = skb;
+
+	/* Set up first empty entry in queue's array of Tx/cmd buffers */
+	out_cmd = &txq->cmd[idx];
+	tx_cmd = &out_cmd->cmd.tx;
+	memset(&out_cmd->hdr, 0, sizeof(out_cmd->hdr));
+	memset(tx_cmd, 0, sizeof(struct iwl_tx_cmd));
+
+	/*
+	 * Set up the Tx-command (not MAC!) header.
+	 * Store the chosen Tx queue and TFD index within the sequence field;
+	 * after Tx, uCode's Tx response will return this value so driver can
+	 * locate the frame within the tx queue and do post-tx processing.
+	 */
+	out_cmd->hdr.cmd = REPLY_TX;
+	out_cmd->hdr.sequence = cpu_to_le16((u16)(QUEUE_TO_SEQ(txq_id) |
+				INDEX_TO_SEQ(q->write_ptr)));
+
+	/* Copy MAC header from skb into command buffer */
+	memcpy(tx_cmd->hdr, hdr, hdr_len);
+
+	/*
+	 * Use the first empty entry in this queue's command buffer array
+	 * to contain the Tx command and MAC header concatenated together
+	 * (payload data will be in another buffer).
+	 * Size of this varies, due to varying MAC header length.
+	 * If end is not dword aligned, we'll have 2 extra bytes at the end
+	 * of the MAC header (device reads on dword boundaries).
+	 * We'll tell device about this padding later.
+	 */
+	len = sizeof(struct iwl_tx_cmd) +
+		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;
+
+	/* Physical address of this Tx command's header (not MAC header!),
+	 * within command buffer array. */
+	txcmd_phys = txq->dma_addr_cmd + sizeof(struct iwl_cmd) * idx +
+		     offsetof(struct iwl_cmd, hdr);
+
+	/* Add buffer containing Tx command and MAC(!) header to TFD's
+	 * first entry */
+	iwl_hw_txq_attach_buf_to_tfd(priv, tfd, txcmd_phys, len);
+
+	if (!(info->flags & IEEE80211_TX_CTL_DO_NOT_ENCRYPT))
+		iwl_tx_cmd_build_hwcrypto(priv, info, tx_cmd, skb, sta_id);
+
+	/* Set up TFD's 2nd entry to point directly to remainder of skb,
+	 * if any (802.11 null frames 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);
+	}
+
+	/* Tell NIC about any 2-byte padding after MAC header */
+	if (len_org)
+		tx_cmd->tx_flags |= TX_CMD_FLG_MH_PAD_MSK;
+
+	/* Total # bytes to be transmitted */
+	len = (u16)skb->len;
+	tx_cmd->len = cpu_to_le16(len);
+	/* TODO need this for burst mode later on */
+	iwl_tx_cmd_build_basic(priv, tx_cmd, info, hdr, unicast, sta_id);
+
+	/* set is_hcca to 0; it probably will never be implemented */
+	iwl_tx_cmd_build_rate(priv, tx_cmd, info, fc, sta_id, 0);
+
+	iwl_update_tx_stats(priv, le16_to_cpu(fc), len);
+
+	scratch_phys = txcmd_phys + sizeof(struct iwl_cmd_header) +
+		offsetof(struct iwl_tx_cmd, scratch);
+	tx_cmd->dram_lsb_ptr = cpu_to_le32(scratch_phys);
+	tx_cmd->dram_msb_ptr = iwl_get_dma_hi_address(scratch_phys);
+
+	if (!ieee80211_has_morefrags(hdr->frame_control)) {
+		txq->need_update = 1;
+		if (qc)
+			priv->stations[sta_id].tid[tid].seq_number = seq_number;
+	} else {
+		wait_write_ptr = 1;
+		txq->need_update = 0;
+	}
+
+	iwl_print_hex_dump(priv, IWL_DL_TX, (u8 *)tx_cmd, sizeof(*tx_cmd));
+
+	iwl_print_hex_dump(priv, IWL_DL_TX, (u8 *)tx_cmd->hdr, hdr_len);
+
+	/* Set up entry for this TFD in Tx byte-count array */
+	priv->cfg->ops->lib->txq_update_byte_cnt_tbl(priv, txq, len);
+
+	/* Tell device the write index *just past* this latest filled TFD */
+	q->write_ptr = iwl_queue_inc_wrap(q->write_ptr, q->n_bd);
+	ret = iwl_txq_update_write_ptr(priv, txq);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	if (ret)
+		return ret;
+
+	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_txq_update_write_ptr(priv, txq);
+			spin_unlock_irqrestore(&priv->lock, flags);
+		}
+
+		ieee80211_stop_queue(priv->hw, skb_get_queue_mapping(skb));
+	}
+
+	return 0;
+
+drop_unlock:
+	spin_unlock_irqrestore(&priv->lock, flags);
+drop:
+	return -1;
+}
+EXPORT_SYMBOL(iwl_tx_skb);
+
+/*************** HOST COMMAND QUEUE FUNCTIONS   *****/
+
+/**
+ * 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.
+ */
+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;
+	dma_addr_t phys_addr;
+	int ret;
+	unsigned long flags;
+
+	cmd->len = priv->cfg->ops->utils->get_hcmd_size(cmd->id, cmd->len);
+	fix_size = (u16)(cmd->len + sizeof(out_cmd->hdr));
+
+	/* 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_is_rfkill(priv)) {
+		IWL_DEBUG_INFO("Not sending command - RF KILL");
+		return -EIO;
+	}
+
+	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->write_ptr];
+	memset(tfd, 0, sizeof(*tfd));
+
+	control_flags = (u32 *) tfd;
+
+	idx = get_cmd_index(q, q->write_ptr, 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->write_ptr));
+	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->write_ptr, idx, IWL_CMD_QUEUE_NUM);
+
+	txq->need_update = 1;
+
+	/* Set up entry in queue's byte count circular buffer */
+	priv->cfg->ops->lib->txq_update_byte_cnt_tbl(priv, txq, 0);
+
+	/* Increment and update queue's write index */
+	q->write_ptr = iwl_queue_inc_wrap(q->write_ptr, q->n_bd);
+	ret = iwl_txq_update_write_ptr(priv, txq);
+
+	spin_unlock_irqrestore(&priv->hcmd_lock, flags);
+	return ret ? ret : idx;
+}
+
+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;
+	struct iwl_tx_info *tx_info;
+	int nfreed = 0;
+
+	if ((index >= q->n_bd) || (iwl_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->write_ptr, q->read_ptr);
+		return 0;
+	}
+
+	for (index = iwl_queue_inc_wrap(index, q->n_bd); q->read_ptr != index;
+		q->read_ptr = iwl_queue_inc_wrap(q->read_ptr, q->n_bd)) {
+
+		tx_info = &txq->txb[txq->q.read_ptr];
+		ieee80211_tx_status_irqsafe(priv->hw, tx_info->skb[0]);
+		tx_info->skb[0] = NULL;
+
+		if (priv->cfg->ops->lib->txq_inval_byte_cnt_tbl)
+			priv->cfg->ops->lib->txq_inval_byte_cnt_tbl(priv, txq);
+
+		iwl_hw_txq_free_tfd(priv, txq);
+		nfreed++;
+	}
+	return nfreed;
+}
+EXPORT_SYMBOL(iwl_tx_queue_reclaim);
+
+
+/**
+ * iwl_hcmd_queue_reclaim - Reclaim TX command queue entries already Tx'd
+ *
+ * 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 the stack that feeds us.
+ */
+static void iwl_hcmd_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) || (iwl_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->write_ptr, q->read_ptr);
+		return;
+	}
+
+	for (index = iwl_queue_inc_wrap(index, q->n_bd); q->read_ptr != index;
+		q->read_ptr = iwl_queue_inc_wrap(q->read_ptr, q->n_bd)) {
+
+		if (nfreed > 1) {
+			IWL_ERROR("HCMD skipped: index (%d) %d %d\n", index,
+					q->write_ptr, q->read_ptr);
+			queue_work(priv->workqueue, &priv->restart);
+		}
+		nfreed++;
+	}
+}
+
+/**
+ * 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
+ */
+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_hcmd_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);
+	}
+}
+EXPORT_SYMBOL(iwl_tx_cmd_complete);
+
+/*
+ * Find first available (lowest unused) Tx Queue, mark it "active".
+ * Called only when finding queue for aggregation.
+ * Should never return anything < 7, because they should already
+ * be in use as EDCA AC (0-3), Command (4), HCCA (5, 6).
+ */
+static int iwl_txq_ctx_activate_free(struct iwl_priv *priv)
+{
+	int txq_id;
+
+	for (txq_id = 0; txq_id < priv->hw_params.max_txq_num; txq_id++)
+		if (!test_and_set_bit(txq_id, &priv->txq_ctx_active_msk))
+			return txq_id;
+	return -1;
+}
+
+int iwl_tx_agg_start(struct iwl_priv *priv, const u8 *ra, u16 tid, u16 *ssn)
+{
+	int sta_id;
+	int tx_fifo;
+	int txq_id;
+	int ret;
+	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("%s on ra = %s tid = %d\n",
+			__func__, print_mac(mac, ra), tid);
+
+	sta_id = iwl_find_station(priv, ra);
+	if (sta_id == IWL_INVALID_STATION)
+		return -ENXIO;
+
+	if (priv->stations[sta_id].tid[tid].agg.state != IWL_AGG_OFF) {
+		IWL_ERROR("Start AGG when state is not IWL_AGG_OFF !\n");
+		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);
+
+	ret = priv->cfg->ops->lib->txq_agg_enable(priv, txq_id, tx_fifo,
+						  sta_id, tid, *ssn);
+	if (ret)
+		return ret;
+
+	if (tid_data->tfds_in_queue == 0) {
+		printk(KERN_ERR "HW queue is empty\n");
+		tid_data->agg.state = IWL_AGG_ON;
+		ieee80211_start_tx_ba_cb_irqsafe(priv->hw, ra, tid);
+	} else {
+		IWL_DEBUG_HT("HW queue is NOT empty: %d packets in HW queue\n",
+			     tid_data->tfds_in_queue);
+		tid_data->agg.state = IWL_EMPTYING_HW_QUEUE_ADDBA;
+	}
+	return ret;
+}
+EXPORT_SYMBOL(iwl_tx_agg_start);
+
+int iwl_tx_agg_stop(struct iwl_priv *priv , const u8 *ra, u16 tid)
+{
+	int tx_fifo_id, txq_id, sta_id, ssn = -1;
+	struct iwl_tid_data *tid_data;
+	int ret, write_ptr, read_ptr;
+	unsigned long flags;
+	DECLARE_MAC_BUF(mac);
+
+	if (!ra) {
+		IWL_ERROR("ra = NULL\n");
+		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_find_station(priv, ra);
+
+	if (sta_id == IWL_INVALID_STATION)
+		return -ENXIO;
+
+	if (priv->stations[sta_id].tid[tid].agg.state != IWL_AGG_ON)
+		IWL_WARNING("Stopping AGG while state not IWL_AGG_ON\n");
+
+	tid_data = &priv->stations[sta_id].tid[tid];
+	ssn = (tid_data->seq_number & IEEE80211_SCTL_SEQ) >> 4;
+	txq_id = tid_data->agg.txq_id;
+	write_ptr = priv->txq[txq_id].q.write_ptr;
+	read_ptr = priv->txq[txq_id].q.read_ptr;
+
+	/* The queue is not empty */
+	if (write_ptr != read_ptr) {
+		IWL_DEBUG_HT("Stopping a non empty AGG HW QUEUE\n");
+		priv->stations[sta_id].tid[tid].agg.state =
+				IWL_EMPTYING_HW_QUEUE_DELBA;
+		return 0;
+	}
+
+	IWL_DEBUG_HT("HW queue is empty\n");
+	priv->stations[sta_id].tid[tid].agg.state = IWL_AGG_OFF;
+
+	spin_lock_irqsave(&priv->lock, flags);
+	ret = priv->cfg->ops->lib->txq_agg_disable(priv, txq_id, ssn,
+						   tx_fifo_id);
+	spin_unlock_irqrestore(&priv->lock, flags);
+
+	if (ret)
+		return ret;
+
+	ieee80211_stop_tx_ba_cb_irqsafe(priv->hw, ra, tid);
+
+	return 0;
+}
+EXPORT_SYMBOL(iwl_tx_agg_stop);
+
+int iwl_txq_check_empty(struct iwl_priv *priv, int sta_id, u8 tid, int txq_id)
+{
+	struct iwl_queue *q = &priv->txq[txq_id].q;
+	u8 *addr = priv->stations[sta_id].sta.sta.addr;
+	struct iwl_tid_data *tid_data = &priv->stations[sta_id].tid[tid];
+
+	switch (priv->stations[sta_id].tid[tid].agg.state) {
+	case IWL_EMPTYING_HW_QUEUE_DELBA:
+		/* We are reclaiming the last packet of the */
+		/* aggregated HW queue */
+		if (txq_id  == tid_data->agg.txq_id &&
+		    q->read_ptr == q->write_ptr) {
+			u16 ssn = SEQ_TO_SN(tid_data->seq_number);
+			int tx_fifo = default_tid_to_tx_fifo[tid];
+			IWL_DEBUG_HT("HW queue empty: continue DELBA flow\n");
+			priv->cfg->ops->lib->txq_agg_disable(priv, txq_id,
+							     ssn, tx_fifo);
+			tid_data->agg.state = IWL_AGG_OFF;
+			ieee80211_stop_tx_ba_cb_irqsafe(priv->hw, addr, tid);
+		}
+		break;
+	case IWL_EMPTYING_HW_QUEUE_ADDBA:
+		/* We are reclaiming the last packet of the queue */
+		if (tid_data->tfds_in_queue == 0) {
+			IWL_DEBUG_HT("HW queue empty: continue ADDBA flow\n");
+			tid_data->agg.state = IWL_AGG_ON;
+			ieee80211_start_tx_ba_cb_irqsafe(priv->hw, addr, tid);
+		}
+		break;
+	}
+	return 0;
+}
+EXPORT_SYMBOL(iwl_txq_check_empty);
+
+/**
+ * iwl_tx_status_reply_compressed_ba - Update tx status from block-ack
+ *
+ * Go through block-ack's bitmap of ACK'd frames, update driver's record of
+ * ACK vs. not.  This gets sent to mac80211, then to rate scaling algo.
+ */
+static int iwl_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 seq_ctl = le16_to_cpu(ba_resp->seq_ctl);
+	u16 scd_flow = le16_to_cpu(ba_resp->scd_flow);
+	u64 bitmap;
+	int successes = 0;
+	struct ieee80211_tx_info *info;
+
+	if (unlikely(!agg->wait_for_ba))  {
+		IWL_ERROR("Received BA when not expected\n");
+		return -EINVAL;
+	}
+
+	/* Mark that the expected block-ack response arrived */
+	agg->wait_for_ba = 0;
+	IWL_DEBUG_TX_REPLY("BA %d %d\n", agg->start_idx, ba_resp->seq_ctl);
+
+	/* Calculate shift to align block-ack bits with our Tx window bits */
+	sh = agg->start_idx - SEQ_TO_INDEX(seq_ctl>>4);
+	if (sh < 0) /* tbw something is wrong with indices */
+		sh += 0x100;
+
+	/* don't use 64-bit values for now */
+	bitmap = le64_to_cpu(ba_resp->bitmap) >> 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 block-ack bitmap */
+	bitmap &= agg->bitmap;
+
+	/* For each frame attempted in aggregation,
+	 * update driver's record of tx frame's status. */
+	for (i = 0; i < agg->frame_count ; i++) {
+		ack = bitmap & (1 << i);
+		successes += !!ack;
+		IWL_DEBUG_TX_REPLY("%s ON i=%d idx=%d raw=%d\n",
+			ack? "ACK":"NACK", i, (agg->start_idx + i) & 0xff,
+			agg->start_idx + i);
+	}
+
+	info = IEEE80211_SKB_CB(priv->txq[scd_flow].txb[agg->start_idx].skb[0]);
+	memset(&info->status, 0, sizeof(info->status));
+	info->flags = IEEE80211_TX_STAT_ACK;
+	info->flags |= IEEE80211_TX_STAT_AMPDU;
+	info->status.ampdu_ack_map = successes;
+	info->status.ampdu_ack_len = agg->frame_count;
+	iwl_hwrate_to_tx_control(priv, agg->rate_n_flags, info);
+
+	IWL_DEBUG_TX_REPLY("Bitmap %llx\n", (unsigned long long)bitmap);
+
+	return 0;
+}
+
+/**
+ * iwl_rx_reply_compressed_ba - Handler for REPLY_COMPRESSED_BA
+ *
+ * Handles block-acknowledge notification from device, which reports success
+ * of frames sent via aggregation.
+ */
+void iwl_rx_reply_compressed_ba(struct iwl_priv *priv,
+					   struct iwl_rx_mem_buffer *rxb)
+{
+	struct iwl_rx_packet *pkt = (struct iwl_rx_packet *)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;
+	DECLARE_MAC_BUF(mac);
+
+	/* "flow" corresponds to Tx queue */
+	u16 scd_flow = le16_to_cpu(ba_resp->scd_flow);
+
+	/* "ssn" is start of block-ack Tx window, corresponds to index
+	 * (in Tx queue's circular buffer) of first TFD/frame in window */
+	u16 ba_resp_scd_ssn = le16_to_cpu(ba_resp->scd_ssn);
+
+	if (scd_flow >= priv->hw_params.max_txq_num) {
+		IWL_ERROR("BUG_ON scd_flow is bigger than number of queues");
+		return;
+	}
+
+	txq = &priv->txq[scd_flow];
+	agg = &priv->stations[ba_resp->sta_id].tid[ba_resp->tid].agg;
+
+	/* Find index just before block-ack window */
+	index = iwl_queue_dec_wrap(ba_resp_scd_ssn & 0xff, txq->q.n_bd);
+
+	/* TODO: Need to get this copy more safely - now good for debug */
+
+	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%llx, scd_flow = "
+			   "%d, scd_ssn = %d\n",
+			   ba_resp->tid,
+			   ba_resp->seq_ctl,
+			   (unsigned long long)le64_to_cpu(ba_resp->bitmap),
+			   ba_resp->scd_flow,
+			   ba_resp->scd_ssn);
+	IWL_DEBUG_TX_REPLY("DAT start_idx = %d, bitmap = 0x%llx \n",
+			   agg->start_idx,
+			   (unsigned long long)agg->bitmap);
+
+	/* Update driver's record of ACK vs. not for each frame in window */
+	iwl_tx_status_reply_compressed_ba(priv, agg, ba_resp);
+
+	/* Release all TFDs before the SSN, i.e. all TFDs in front of
+	 * block-ack window (we assume that they've been successfully
+	 * transmitted ... if not, it's too late anyway). */
+	if (txq->q.read_ptr != (ba_resp_scd_ssn & 0xff)) {
+		/* calculate mac80211 ampdu sw queue to wake */
+		int ampdu_q =
+		   scd_flow - priv->hw_params.first_ampdu_q + priv->hw->queues;
+		int freed = iwl_tx_queue_reclaim(priv, scd_flow, index);
+		priv->stations[ba_resp->sta_id].
+			tid[ba_resp->tid].tfds_in_queue -= freed;
+		if (iwl_queue_space(&txq->q) > txq->q.low_mark &&
+			priv->mac80211_registered &&
+			agg->state != IWL_EMPTYING_HW_QUEUE_DELBA)
+			ieee80211_wake_queue(priv->hw, ampdu_q);
+
+		iwl_txq_check_empty(priv, ba_resp->sta_id,
+				    ba_resp->tid, scd_flow);
+	}
+}
+EXPORT_SYMBOL(iwl_rx_reply_compressed_ba);
+
+#ifdef CONFIG_IWLWIFI_DEBUG
+#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";
+}
+EXPORT_SYMBOL(iwl_get_tx_fail_reason);
+#endif /* CONFIG_IWLWIFI_DEBUG */
diff --git a/drivers/net/wireless/iwlwifi/iwl3945-base.c b/drivers/net/wireless/iwlwifi/iwl3945-base.c
index 6027e1119c3f..4a22d3fba75b 100644
--- a/drivers/net/wireless/iwlwifi/iwl3945-base.c
+++ b/drivers/net/wireless/iwlwifi/iwl3945-base.c
@@ -102,16 +102,6 @@ MODULE_VERSION(DRV_VERSION);
 MODULE_AUTHOR(DRV_COPYRIGHT);
 MODULE_LICENSE("GPL");
 
-static __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_supported_band *iwl3945_get_band(
 		struct iwl3945_priv *priv, enum ieee80211_band band)
 {
@@ -547,10 +537,20 @@ static inline int iwl3945_is_init(struct iwl3945_priv *priv)
 	return test_bit(STATUS_INIT, &priv->status);
 }
 
+static inline int iwl3945_is_rfkill_sw(struct iwl3945_priv *priv)
+{
+	return test_bit(STATUS_RF_KILL_SW, &priv->status);
+}
+
+static inline int iwl3945_is_rfkill_hw(struct iwl3945_priv *priv)
+{
+	return test_bit(STATUS_RF_KILL_HW, &priv->status);
+}
+
 static inline int iwl3945_is_rfkill(struct iwl3945_priv *priv)
 {
-	return test_bit(STATUS_RF_KILL_HW, &priv->status) ||
-	       test_bit(STATUS_RF_KILL_SW, &priv->status);
+	return iwl3945_is_rfkill_hw(priv) ||
+		iwl3945_is_rfkill_sw(priv);
 }
 
 static inline int iwl3945_is_ready_rf(struct iwl3945_priv *priv)
@@ -980,7 +980,7 @@ static int iwl3945_full_rxon_required(struct iwl3945_priv *priv)
 {
 
 	/* These items are only settable from the full RXON command */
-	if (!(priv->active_rxon.filter_flags & RXON_FILTER_ASSOC_MSK) ||
+	if (!(iwl3945_is_associated(priv)) ||
 	    compare_ether_addr(priv->staging_rxon.bssid_addr,
 			       priv->active_rxon.bssid_addr) ||
 	    compare_ether_addr(priv->staging_rxon.node_addr,
@@ -2035,36 +2035,6 @@ static int iwl3945_send_power_mode(struct iwl3945_priv *priv, u32 mode)
 	return rc;
 }
 
-int iwl3945_is_network_packet(struct iwl3945_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);
-	default:
-		return 1;
-	}
-
-	return 1;
-}
-
 /**
  * iwl3945_scan_cancel - Cancel any currently executing HW scan
  *
@@ -2117,20 +2087,6 @@ static int iwl3945_scan_cancel_timeout(struct iwl3945_priv *priv, unsigned long
 	return ret;
 }
 
-static void iwl3945_sequence_reset(struct iwl3945_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;
-
-	iwl3945_scan_cancel(priv);
-}
-
 #define MAX_UCODE_BEACON_INTERVAL	1024
 #define INTEL_CONN_LISTEN_INTERVAL	__constant_cpu_to_le16(0xA)
 
@@ -2322,7 +2278,7 @@ static void iwl3945_connection_init_rx_config(struct iwl3945_priv *priv)
 #endif
 
 	ch_info = iwl3945_get_channel_info(priv, priv->band,
-				       le16_to_cpu(priv->staging_rxon.channel));
+				       le16_to_cpu(priv->active_rxon.channel));
 
 	if (!ch_info)
 		ch_info = &priv->channel_info[0];
@@ -2389,12 +2345,13 @@ static int iwl3945_set_mode(struct iwl3945_priv *priv, int mode)
 }
 
 static void iwl3945_build_tx_cmd_hwcrypto(struct iwl3945_priv *priv,
-				      struct ieee80211_tx_control *ctl,
+				      struct ieee80211_tx_info *info,
 				      struct iwl3945_cmd *cmd,
 				      struct sk_buff *skb_frag,
 				      int last_frag)
 {
-	struct iwl3945_hw_key *keyinfo = &priv->stations[ctl->key_idx].keyinfo;
+	struct iwl3945_hw_key *keyinfo =
+	    &priv->stations[info->control.hw_key->hw_key_idx].keyinfo;
 
 	switch (keyinfo->alg) {
 	case ALG_CCMP:
@@ -2417,7 +2374,7 @@ static void iwl3945_build_tx_cmd_hwcrypto(struct iwl3945_priv *priv,
 
 	case ALG_WEP:
 		cmd->cmd.tx.sec_ctl = TX_CMD_SEC_WEP |
-		    (ctl->key_idx & TX_CMD_SEC_MSK) << TX_CMD_SEC_SHIFT;
+		    (info->control.hw_key->hw_key_idx & TX_CMD_SEC_MSK) << TX_CMD_SEC_SHIFT;
 
 		if (keyinfo->keylen == 13)
 			cmd->cmd.tx.sec_ctl |= TX_CMD_SEC_KEY128;
@@ -2425,7 +2382,7 @@ static void iwl3945_build_tx_cmd_hwcrypto(struct iwl3945_priv *priv,
 		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);
+			     "with key %d\n", info->control.hw_key->hw_key_idx);
 		break;
 
 	default:
@@ -2439,20 +2396,19 @@ static void iwl3945_build_tx_cmd_hwcrypto(struct iwl3945_priv *priv,
  */
 static void iwl3945_build_tx_cmd_basic(struct iwl3945_priv *priv,
 				  struct iwl3945_cmd *cmd,
-				  struct ieee80211_tx_control *ctrl,
+				  struct ieee80211_tx_info *info,
 				  struct ieee80211_hdr *hdr,
 				  int is_unicast, u8 std_id)
 {
-	__le16 *qc;
-	u16 fc = le16_to_cpu(hdr->frame_control);
+	__le16 fc = 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)) {
+	if (!(info->flags & IEEE80211_TX_CTL_NO_ACK)) {
 		tx_flags |= TX_CMD_FLG_ACK_MSK;
-		if ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT)
+		if (ieee80211_is_mgmt(fc))
 			tx_flags |= TX_CMD_FLG_SEQ_CTL_MSK;
-		if (ieee80211_is_probe_response(fc) &&
+		if (ieee80211_is_probe_resp(fc) &&
 		    !(le16_to_cpu(hdr->seq_ctrl) & 0xf))
 			tx_flags |= TX_CMD_FLG_TSF_MSK;
 	} else {
@@ -2461,20 +2417,21 @@ static void iwl3945_build_tx_cmd_basic(struct iwl3945_priv *priv,
 	}
 
 	cmd->cmd.tx.sta_id = std_id;
-	if (ieee80211_get_morefrag(hdr))
+	if (ieee80211_has_morefrags(fc))
 		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);
+	if (ieee80211_is_data_qos(fc)) {
+		u8 *qc = ieee80211_get_qos_ctl(hdr);
+		cmd->cmd.tx.tid_tspec = qc[0] & 0xf;
 		tx_flags &= ~TX_CMD_FLG_SEQ_CTL_MSK;
-	} else
+	} else {
 		tx_flags |= TX_CMD_FLG_SEQ_CTL_MSK;
+	}
 
-	if (ctrl->flags & IEEE80211_TXCTL_USE_RTS_CTS) {
+	if (info->flags & IEEE80211_TX_CTL_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) {
+	} else if (info->flags & IEEE80211_TX_CTL_USE_CTS_PROTECT) {
 		tx_flags &= ~TX_CMD_FLG_RTS_MSK;
 		tx_flags |= TX_CMD_FLG_CTS_MSK;
 	}
@@ -2483,9 +2440,8 @@ static void iwl3945_build_tx_cmd_basic(struct iwl3945_priv *priv,
 		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)
+	if (ieee80211_is_mgmt(fc)) {
+		if (ieee80211_is_assoc_req(fc) || ieee80211_is_reassoc_req(fc))
 			cmd->cmd.tx.timeout.pm_frame_timeout = cpu_to_le16(3);
 		else
 			cmd->cmd.tx.timeout.pm_frame_timeout = cpu_to_le16(2);
@@ -2549,6 +2505,11 @@ static int iwl3945_get_sta_id(struct iwl3945_priv *priv, struct ieee80211_hdr *h
 		iwl3945_print_hex_dump(IWL_DL_DROP, (u8 *) hdr, sizeof(*hdr));
 		return priv->hw_setting.bcast_sta_id;
 	}
+	/* If we are in monitor mode, use BCAST. This is required for
+	 * packet injection. */
+	case IEEE80211_IF_TYPE_MNTR:
+		return priv->hw_setting.bcast_sta_id;
+
 	default:
 		IWL_WARNING("Unknown mode of operation: %d", priv->iw_mode);
 		return priv->hw_setting.bcast_sta_id;
@@ -2558,25 +2519,27 @@ static int iwl3945_get_sta_id(struct iwl3945_priv *priv, struct ieee80211_hdr *h
 /*
  * start REPLY_TX command process
  */
-static int iwl3945_tx_skb(struct iwl3945_priv *priv,
-		      struct sk_buff *skb, struct ieee80211_tx_control *ctl)
+static int iwl3945_tx_skb(struct iwl3945_priv *priv, struct sk_buff *skb)
 {
 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 	struct iwl3945_tfd_frame *tfd;
 	u32 *control_flags;
-	int txq_id = ctl->queue;
+	int txq_id = skb_get_queue_mapping(skb);
 	struct iwl3945_tx_queue *txq = NULL;
 	struct iwl3945_queue *q = NULL;
 	dma_addr_t phys_addr;
 	dma_addr_t txcmd_phys;
 	struct iwl3945_cmd *out_cmd = NULL;
-	u16 len, idx, len_org;
-	u8 id, hdr_len, unicast;
+	u16 len, idx, len_org, hdr_len;
+	u8 id;
+	u8 unicast;
 	u8 sta_id;
+	u8 tid = 0;
 	u16 seq_number = 0;
-	u16 fc;
-	__le16 *qc;
+	__le16 fc;
 	u8 wait_write_ptr = 0;
+	u8 *qc = NULL;
 	unsigned long flags;
 	int rc;
 
@@ -2586,12 +2549,7 @@ static int iwl3945_tx_skb(struct iwl3945_priv *priv,
 		goto drop_unlock;
 	}
 
-	if (!priv->vif) {
-		IWL_DEBUG_DROP("Dropping - !priv->vif\n");
-		goto drop_unlock;
-	}
-
-	if ((ctl->tx_rate->hw_value & 0xFF) == IWL_INVALID_RATE) {
+	if ((ieee80211_get_tx_rate(priv->hw, info)->hw_value & 0xFF) == IWL_INVALID_RATE) {
 		IWL_ERROR("ERROR: No TX rate available.\n");
 		goto drop_unlock;
 	}
@@ -2599,28 +2557,29 @@ static int iwl3945_tx_skb(struct iwl3945_priv *priv,
 	unicast = !is_multicast_ether_addr(hdr->addr1);
 	id = 0;
 
-	fc = le16_to_cpu(hdr->frame_control);
+	fc = hdr->frame_control;
 
 #ifdef CONFIG_IWL3945_DEBUG
 	if (ieee80211_is_auth(fc))
 		IWL_DEBUG_TX("Sending AUTH frame\n");
-	else if (ieee80211_is_assoc_request(fc))
+	else if (ieee80211_is_assoc_req(fc))
 		IWL_DEBUG_TX("Sending ASSOC frame\n");
-	else if (ieee80211_is_reassoc_request(fc))
+	else if (ieee80211_is_reassoc_req(fc))
 		IWL_DEBUG_TX("Sending REASSOC frame\n");
 #endif
 
 	/* drop all data frame if we are not associated */
-	if ((!iwl3945_is_associated(priv) ||
-	     ((priv->iw_mode == IEEE80211_IF_TYPE_STA) && !priv->assoc_id)) &&
-	    ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_DATA)) {
+	if (ieee80211_is_data(fc) &&
+	    (priv->iw_mode != IEEE80211_IF_TYPE_MNTR) && /* packet injection */
+	    (!iwl3945_is_associated(priv) ||
+	     ((priv->iw_mode == IEEE80211_IF_TYPE_STA) && !priv->assoc_id))) {
 		IWL_DEBUG_DROP("Dropping - !iwl3945_is_associated\n");
 		goto drop_unlock;
 	}
 
 	spin_unlock_irqrestore(&priv->lock, flags);
 
-	hdr_len = ieee80211_get_hdrlen(fc);
+	hdr_len = ieee80211_get_hdrlen(le16_to_cpu(fc));
 
 	/* Find (or create) index into station table for destination station */
 	sta_id = iwl3945_get_sta_id(priv, hdr);
@@ -2634,9 +2593,9 @@ static int iwl3945_tx_skb(struct iwl3945_priv *priv,
 
 	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);
+	if (ieee80211_is_data_qos(fc)) {
+		qc = ieee80211_get_qos_ctl(hdr);
+		tid = qc[0] & 0xf;
 		seq_number = priv->stations[sta_id].tid[tid].seq_number &
 				IEEE80211_SCTL_SEQ;
 		hdr->seq_ctrl = cpu_to_le16(seq_number) |
@@ -2660,8 +2619,6 @@ static int iwl3945_tx_skb(struct iwl3945_priv *priv,
 	/* Set up driver data for this TFD */
 	memset(&(txq->txb[q->write_ptr]), 0, sizeof(struct iwl3945_tx_info));
 	txq->txb[q->write_ptr].skb[0] = skb;
-	memcpy(&(txq->txb[q->write_ptr].status.control),
-	       ctl, sizeof(struct ieee80211_tx_control));
 
 	/* Init first empty entry in queue's array of Tx/cmd buffers */
 	out_cmd = &txq->cmd[idx];
@@ -2710,8 +2667,8 @@ static int iwl3945_tx_skb(struct iwl3945_priv *priv,
 	 * first entry */
 	iwl3945_hw_txq_attach_buf_to_tfd(priv, tfd, txcmd_phys, len);
 
-	if (!(ctl->flags & IEEE80211_TXCTL_DO_NOT_ENCRYPT))
-		iwl3945_build_tx_cmd_hwcrypto(priv, ctl, out_cmd, skb, 0);
+	if (!(info->flags & IEEE80211_TX_CTL_DO_NOT_ENCRYPT))
+		iwl3945_build_tx_cmd_hwcrypto(priv, info, out_cmd, skb, 0);
 
 	/* Set up TFD's 2nd entry to point directly to remainder of skb,
 	 * if any (802.11 null frames have no payload). */
@@ -2736,18 +2693,17 @@ static int iwl3945_tx_skb(struct iwl3945_priv *priv,
 	out_cmd->cmd.tx.len = cpu_to_le16(len);
 
 	/* TODO need this for burst mode later on */
-	iwl3945_build_tx_cmd_basic(priv, out_cmd, ctl, hdr, unicast, sta_id);
+	iwl3945_build_tx_cmd_basic(priv, out_cmd, info, hdr, unicast, sta_id);
 
 	/* set is_hcca to 0; it probably will never be implemented */
-	iwl3945_hw_build_tx_cmd_rate(priv, out_cmd, ctl, hdr, sta_id, 0);
+	iwl3945_hw_build_tx_cmd_rate(priv, out_cmd, info, 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)) {
+	if (!ieee80211_has_morefrags(hdr->frame_control)) {
 		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 {
@@ -2759,7 +2715,7 @@ static int iwl3945_tx_skb(struct iwl3945_priv *priv,
 			   sizeof(out_cmd->cmd.tx));
 
 	iwl3945_print_hex_dump(IWL_DL_TX, (u8 *)out_cmd->cmd.tx.hdr,
-			   ieee80211_get_hdrlen(fc));
+			   ieee80211_get_hdrlen(le16_to_cpu(fc)));
 
 	/* Tell device the write index *just past* this latest filled TFD */
 	q->write_ptr = iwl_queue_inc_wrap(q->write_ptr, q->n_bd);
@@ -2778,7 +2734,7 @@ static int iwl3945_tx_skb(struct iwl3945_priv *priv,
 			spin_unlock_irqrestore(&priv->lock, flags);
 		}
 
-		ieee80211_stop_queue(priv->hw, ctl->queue);
+		ieee80211_stop_queue(priv->hw, skb_get_queue_mapping(skb));
 	}
 
 	return 0;
@@ -2888,7 +2844,8 @@ static void iwl3945_radio_kill_sw(struct iwl3945_priv *priv, int disable_radio)
 		return;
 	}
 
-	queue_work(priv->workqueue, &priv->restart);
+	if (priv->is_open)
+		queue_work(priv->workqueue, &priv->restart);
 	return;
 }
 
@@ -2924,72 +2881,6 @@ void iwl3945_set_decrypted_flag(struct iwl3945_priv *priv, struct sk_buff *skb,
 	}
 }
 
-#define IWL_PACKET_RETRY_TIME HZ
-
-int iwl3945_is_duplicate_packet(struct iwl3945_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 iwl3945_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 iwl3945_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_IWL3945_SPECTRUM_MEASUREMENT
 
 #include "iwl-spectrum.h"
@@ -3241,7 +3132,7 @@ static void iwl3945_bg_beacon_update(struct work_struct *work)
 	struct sk_buff *beacon;
 
 	/* Pull updated AP beacon from mac80211. will fail if not in AP mode */
-	beacon = ieee80211_beacon_get(priv->hw, priv->vif, NULL);
+	beacon = ieee80211_beacon_get(priv->hw, priv->vif);
 
 	if (!beacon) {
 		IWL_ERROR("update beacon failed\n");
@@ -4848,7 +4739,7 @@ static int iwl3945_init_channel_map(struct iwl3945_priv *priv)
 			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%s(0x%02x"
+			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) ?
@@ -4858,7 +4749,6 @@ static int iwl3945_init_channel_map(struct iwl3945_priv *priv)
 				       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,
@@ -4994,9 +4884,6 @@ static int iwl3945_get_channels_for_scan(struct iwl3945_priv *priv,
 		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);
 
@@ -5843,7 +5730,7 @@ static void iwl3945_alive_start(struct iwl3945_priv *priv)
 	if (iwl3945_is_rfkill(priv))
 		return;
 
-	ieee80211_start_queues(priv->hw);
+	ieee80211_wake_queues(priv->hw);
 
 	priv->active_rate = priv->rates_mask;
 	priv->active_rate_basic = priv->rates_mask & IWL_BASIC_RATES_MASK;
@@ -5869,9 +5756,6 @@ static void iwl3945_alive_start(struct iwl3945_priv *priv)
 	/* Configure the adapter for unassociated operation */
 	iwl3945_commit_rxon(priv);
 
-	/* At this point, the NIC is initialized and operational */
-	priv->notif_missed_beacons = 0;
-
 	iwl3945_reg_txpower_periodic(priv);
 
 	iwl3945_led_register(priv);
@@ -5938,7 +5822,9 @@ static void __iwl3945_down(struct iwl3945_priv *priv)
 			       test_bit(STATUS_GEO_CONFIGURED, &priv->status) <<
 					STATUS_GEO_CONFIGURED |
 			       test_bit(STATUS_IN_SUSPEND, &priv->status) <<
-					STATUS_IN_SUSPEND;
+					STATUS_IN_SUSPEND |
+				test_bit(STATUS_EXIT_PENDING, &priv->status) <<
+					STATUS_EXIT_PENDING;
 		goto exit;
 	}
 
@@ -5953,7 +5839,9 @@ static void __iwl3945_down(struct iwl3945_priv *priv)
 			test_bit(STATUS_IN_SUSPEND, &priv->status) <<
 				STATUS_IN_SUSPEND |
 			test_bit(STATUS_FW_ERROR, &priv->status) <<
-				STATUS_FW_ERROR;
+				STATUS_FW_ERROR |
+			test_bit(STATUS_EXIT_PENDING, &priv->status) <<
+				STATUS_EXIT_PENDING;
 
 	spin_lock_irqsave(&priv->lock, flags);
 	iwl3945_clear_bit(priv, CSR_GP_CNTRL, CSR_GP_CNTRL_REG_FLAG_MAC_ACCESS_REQ);
@@ -6085,6 +5973,7 @@ static int __iwl3945_up(struct iwl3945_priv *priv)
 
 	set_bit(STATUS_EXIT_PENDING, &priv->status);
 	__iwl3945_down(priv);
+	clear_bit(STATUS_EXIT_PENDING, &priv->status);
 
 	/* tried to restart and config the device for as long as our
 	 * patience could withstand */
@@ -6152,6 +6041,26 @@ static void iwl3945_bg_rf_kill(struct work_struct *work)
 				    "Kill switch must be turned off for "
 				    "wireless networking to work.\n");
 	}
+
+	mutex_unlock(&priv->mutex);
+	iwl3945_rfkill_set_hw_state(priv);
+}
+
+static void iwl3945_bg_set_monitor(struct work_struct *work)
+{
+	struct iwl3945_priv *priv = container_of(work,
+				struct iwl3945_priv, set_monitor);
+
+	IWL_DEBUG(IWL_DL_STATE, "setting monitor mode\n");
+
+	mutex_lock(&priv->mutex);
+
+	if (!iwl3945_is_ready(priv))
+		IWL_DEBUG(IWL_DL_STATE, "leave - not ready\n");
+	else
+		if (iwl3945_set_mode(priv, IEEE80211_IF_TYPE_MNTR) != 0)
+			IWL_ERROR("iwl3945_set_mode() failed\n");
+
 	mutex_unlock(&priv->mutex);
 }
 
@@ -6388,6 +6297,7 @@ static void iwl3945_bg_up(struct work_struct *data)
 	mutex_lock(&priv->mutex);
 	__iwl3945_up(priv);
 	mutex_unlock(&priv->mutex);
+	iwl3945_rfkill_set_hw_state(priv);
 }
 
 static void iwl3945_bg_restart(struct work_struct *data)
@@ -6511,8 +6421,6 @@ static void iwl3945_bg_post_associate(struct work_struct *data)
 		break;
 	}
 
-	iwl3945_sequence_reset(priv);
-
 	iwl3945_activate_qos(priv, 0);
 
 	/* we have just associated, don't start scan too early */
@@ -6608,6 +6516,8 @@ static int iwl3945_mac_start(struct ieee80211_hw *hw)
 
 	mutex_unlock(&priv->mutex);
 
+	iwl3945_rfkill_set_hw_state(priv);
+
 	if (ret)
 		goto out_release_irq;
 
@@ -6678,8 +6588,7 @@ static void iwl3945_mac_stop(struct ieee80211_hw *hw)
 	IWL_DEBUG_MAC80211("leave\n");
 }
 
-static int iwl3945_mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb,
-		      struct ieee80211_tx_control *ctl)
+static int iwl3945_mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
 	struct iwl3945_priv *priv = hw->priv;
 
@@ -6692,9 +6601,9 @@ static int iwl3945_mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb,
 	}
 
 	IWL_DEBUG_TX("dev->xmit(%d bytes) at rate 0x%02x\n", skb->len,
-		     ctl->tx_rate->bitrate);
+		     ieee80211_get_tx_rate(hw, IEEE80211_SKB_CB(skb))->bitrate);
 
-	if (iwl3945_tx_skb(priv, skb, ctl))
+	if (iwl3945_tx_skb(priv, skb))
 		dev_kfree_skb_any(skb);
 
 	IWL_DEBUG_MAC80211("leave\n");
@@ -6837,7 +6746,7 @@ static void iwl3945_config_ap(struct iwl3945_priv *priv)
 		return;
 
 	/* The following should be done only at AP bring up */
-	if ((priv->active_rxon.filter_flags & RXON_FILTER_ASSOC_MSK) == 0) {
+	if (!(iwl3945_is_associated(priv))) {
 
 		/* RXON - unassoc (to set timing command) */
 		priv->staging_rxon.filter_flags &= ~RXON_FILTER_ASSOC_MSK;
@@ -6886,6 +6795,9 @@ static void iwl3945_config_ap(struct iwl3945_priv *priv)
 	 * clear sta table, add BCAST sta... */
 }
 
+/* temporary */
+static int iwl3945_mac_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb);
+
 static int iwl3945_mac_config_interface(struct ieee80211_hw *hw,
 					struct ieee80211_vif *vif,
 				    struct ieee80211_if_conf *conf)
@@ -6903,10 +6815,21 @@ static int iwl3945_mac_config_interface(struct ieee80211_hw *hw,
 		return 0;
 	}
 
+	/* handle this temporarily here */
+	if (priv->iw_mode == IEEE80211_IF_TYPE_IBSS &&
+	    conf->changed & IEEE80211_IFCC_BEACON) {
+		struct sk_buff *beacon = ieee80211_beacon_get(hw, vif);
+		if (!beacon)
+			return -ENOMEM;
+		rc = iwl3945_mac_beacon_update(hw, beacon);
+		if (rc)
+			return rc;
+	}
+
 	/* XXX: this MUST use conf->mac_addr */
 
 	if ((priv->iw_mode == IEEE80211_IF_TYPE_AP) &&
-	    (!conf->beacon || !conf->ssid_len)) {
+	    (!conf->ssid_len)) {
 		IWL_DEBUG_MAC80211
 		    ("Leaving in AP mode because HostAPD is not ready.\n");
 		return 0;
@@ -6938,7 +6861,7 @@ static int iwl3945_mac_config_interface(struct ieee80211_hw *hw,
 		if (priv->ibss_beacon)
 			dev_kfree_skb(priv->ibss_beacon);
 
-		priv->ibss_beacon = conf->beacon;
+		priv->ibss_beacon = ieee80211_beacon_get(hw, vif);
 	}
 
 	if (iwl3945_is_rfkill(priv))
@@ -6999,11 +6922,18 @@ static void iwl3945_configure_filter(struct ieee80211_hw *hw,
 				 unsigned int *total_flags,
 				 int mc_count, struct dev_addr_list *mc_list)
 {
-	/*
-	 * XXX: dummy
-	 * see also iwl3945_connection_init_rx_config
-	 */
-	*total_flags = 0;
+	struct iwl3945_priv *priv = hw->priv;
+
+	if (changed_flags & (*total_flags) & FIF_OTHER_BSS) {
+		IWL_DEBUG_MAC80211("Enter: type %d (0x%x, 0x%x)\n",
+				   IEEE80211_IF_TYPE_MNTR,
+				   changed_flags, *total_flags);
+		/* queue work 'cuz mac80211 is holding a lock which
+		 * prevents us from issuing (synchronous) f/w cmds */
+		queue_work(priv->workqueue, &priv->set_monitor);
+	}
+	*total_flags &= FIF_OTHER_BSS | FIF_ALLMULTI |
+			FIF_BCN_PRBRESP_PROMISC | FIF_CONTROL;
 }
 
 static void iwl3945_mac_remove_interface(struct ieee80211_hw *hw,
@@ -7061,9 +6991,10 @@ static int iwl3945_mac_hw_scan(struct ieee80211_hw *hw, u8 *ssid, size_t len)
 		rc = -EAGAIN;
 		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)) {
+	/* if we just finished scan ask for delay for a broadcast scan */
+	if ((len == 0) && priv->last_scan_jiffies &&
+	    time_after(priv->last_scan_jiffies + IWL_DELAY_NEXT_SCAN,
+		       jiffies)) {
 		rc = -EAGAIN;
 		goto out_unlock;
 	}
@@ -7150,7 +7081,7 @@ static int iwl3945_mac_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 	return rc;
 }
 
-static int iwl3945_mac_conf_tx(struct ieee80211_hw *hw, int queue,
+static int iwl3945_mac_conf_tx(struct ieee80211_hw *hw, u16 queue,
 			   const struct ieee80211_tx_queue_params *params)
 {
 	struct iwl3945_priv *priv = hw->priv;
@@ -7224,9 +7155,9 @@ static int iwl3945_mac_get_tx_stats(struct ieee80211_hw *hw,
 		q = &txq->q;
 		avail = iwl3945_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;
+		stats[i].len = q->n_window - avail;
+		stats[i].limit = q->n_window - q->high_mark;
+		stats[i].count = q->n_window;
 
 	}
 	spin_unlock_irqrestore(&priv->lock, flags);
@@ -7315,8 +7246,7 @@ static void iwl3945_mac_reset_tsf(struct ieee80211_hw *hw)
 
 }
 
-static int iwl3945_mac_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb,
-				 struct ieee80211_tx_control *control)
+static int iwl3945_mac_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
 	struct iwl3945_priv *priv = hw->priv;
 	unsigned long flags;
@@ -7398,37 +7328,6 @@ static DRIVER_ATTR(debug_level, S_IWUSR | S_IRUGO,
 
 #endif /* CONFIG_IWL3945_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 iwl3945_priv *priv = (struct iwl3945_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 iwl3945_priv *priv = (struct iwl3945_priv *)d->driver_data;
-
-	mutex_lock(&priv->mutex);
-	iwl3945_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)
 {
@@ -7879,6 +7778,7 @@ static void iwl3945_setup_deferred_work(struct iwl3945_priv *priv)
 	INIT_WORK(&priv->abort_scan, iwl3945_bg_abort_scan);
 	INIT_WORK(&priv->rf_kill, iwl3945_bg_rf_kill);
 	INIT_WORK(&priv->beacon_update, iwl3945_bg_beacon_update);
+	INIT_WORK(&priv->set_monitor, iwl3945_bg_set_monitor);
 	INIT_DELAYED_WORK(&priv->post_associate, iwl3945_bg_post_associate);
 	INIT_DELAYED_WORK(&priv->init_alive_start, iwl3945_bg_init_alive_start);
 	INIT_DELAYED_WORK(&priv->alive_start, iwl3945_bg_alive_start);
@@ -7913,7 +7813,6 @@ static struct attribute *iwl3945_sysfs_entries[] = {
 #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,
@@ -7943,7 +7842,6 @@ static struct ieee80211_ops iwl3945_hw_ops = {
 	.conf_tx = iwl3945_mac_conf_tx,
 	.get_tsf = iwl3945_mac_get_tsf,
 	.reset_tsf = iwl3945_mac_reset_tsf,
-	.beacon_update = iwl3945_mac_beacon_update,
 	.hw_scan = iwl3945_mac_hw_scan
 };
 
@@ -7953,7 +7851,6 @@ static int iwl3945_pci_probe(struct pci_dev *pdev, const struct pci_device_id *e
 	struct iwl3945_priv *priv;
 	struct ieee80211_hw *hw;
 	struct iwl_3945_cfg *cfg = (struct iwl_3945_cfg *)(ent->driver_data);
-	int i;
 	unsigned long flags;
 	DECLARE_MAC_BUF(mac);
 
@@ -8001,17 +7898,10 @@ static int iwl3945_pci_probe(struct pci_dev *pdev, const struct pci_device_id *e
 
 	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;
+	/* Tell mac80211 our characteristics */
+	hw->flags = IEEE80211_HW_HOST_GEN_BEACON_TEMPLATE |
+		    IEEE80211_HW_SIGNAL_DBM |
+		    IEEE80211_HW_NOISE_DBM;
 
 	/* 4 EDCA QOS priorities */
 	hw->queues = 4;
@@ -8021,9 +7911,6 @@ static int iwl3945_pci_probe(struct pci_dev *pdev, const struct pci_device_id *e
 	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);
@@ -8161,6 +8048,11 @@ static int iwl3945_pci_probe(struct pci_dev *pdev, const struct pci_device_id *e
 	pci_save_state(pdev);
 	pci_disable_device(pdev);
 
+	err = iwl3945_rfkill_init(priv);
+	if (err)
+		IWL_ERROR("Unable to initialize RFKILL system. "
+				  "Ignoring error: %d\n", err);
+
 	return 0;
 
  out_free_geos:
@@ -8191,8 +8083,6 @@ static int iwl3945_pci_probe(struct pci_dev *pdev, const struct pci_device_id *e
 static void __devexit iwl3945_pci_remove(struct pci_dev *pdev)
 {
 	struct iwl3945_priv *priv = pci_get_drvdata(pdev);
-	struct list_head *p, *q;
-	int i;
 	unsigned long flags;
 
 	if (!priv)
@@ -8213,16 +8103,9 @@ static void __devexit iwl3945_pci_remove(struct pci_dev *pdev)
 
 	iwl_synchronize_irq(priv);
 
-	/* 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 iwl3945_ibss_seq, list));
-		}
-	}
-
 	sysfs_remove_group(&pdev->dev.kobj, &iwl3945_attribute_group);
 
+	iwl3945_rfkill_unregister(priv);
 	iwl3945_dealloc_ucode_pci(priv);
 
 	if (priv->rxq.bd)
@@ -8252,7 +8135,7 @@ static void __devexit iwl3945_pci_remove(struct pci_dev *pdev)
 
 	iwl3945_free_channel_map(priv);
 	iwl3945_free_geos(priv);
-
+	kfree(priv->scan);
 	if (priv->ibss_beacon)
 		dev_kfree_skb(priv->ibss_beacon);
 
@@ -8291,6 +8174,114 @@ static int iwl3945_pci_resume(struct pci_dev *pdev)
 
 #endif /* CONFIG_PM */
 
+/*************** RFKILL FUNCTIONS **********/
+#ifdef CONFIG_IWL3945_RFKILL
+/* software rf-kill from user */
+static int iwl3945_rfkill_soft_rf_kill(void *data, enum rfkill_state state)
+{
+	struct iwl3945_priv *priv = data;
+	int err = 0;
+
+	if (!priv->rfkill)
+	return 0;
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+		return 0;
+
+	IWL_DEBUG_RF_KILL("we recieved soft RFKILL set to state %d\n", state);
+	mutex_lock(&priv->mutex);
+
+	switch (state) {
+	case RFKILL_STATE_UNBLOCKED:
+		if (iwl3945_is_rfkill_hw(priv)) {
+			err = -EBUSY;
+			goto out_unlock;
+		}
+		iwl3945_radio_kill_sw(priv, 0);
+		break;
+	case RFKILL_STATE_SOFT_BLOCKED:
+		iwl3945_radio_kill_sw(priv, 1);
+		break;
+	default:
+		IWL_WARNING("we recieved unexpected RFKILL state %d\n", state);
+		break;
+	}
+out_unlock:
+	mutex_unlock(&priv->mutex);
+
+	return err;
+}
+
+int iwl3945_rfkill_init(struct iwl3945_priv *priv)
+{
+	struct device *device = wiphy_dev(priv->hw->wiphy);
+	int ret = 0;
+
+	BUG_ON(device == NULL);
+
+	IWL_DEBUG_RF_KILL("Initializing RFKILL.\n");
+	priv->rfkill = rfkill_allocate(device, RFKILL_TYPE_WLAN);
+	if (!priv->rfkill) {
+		IWL_ERROR("Unable to allocate rfkill device.\n");
+		ret = -ENOMEM;
+		goto error;
+	}
+
+	priv->rfkill->name = priv->cfg->name;
+	priv->rfkill->data = priv;
+	priv->rfkill->state = RFKILL_STATE_UNBLOCKED;
+	priv->rfkill->toggle_radio = iwl3945_rfkill_soft_rf_kill;
+	priv->rfkill->user_claim_unsupported = 1;
+
+	priv->rfkill->dev.class->suspend = NULL;
+	priv->rfkill->dev.class->resume = NULL;
+
+	ret = rfkill_register(priv->rfkill);
+	if (ret) {
+		IWL_ERROR("Unable to register rfkill: %d\n", ret);
+		goto freed_rfkill;
+	}
+
+	IWL_DEBUG_RF_KILL("RFKILL initialization complete.\n");
+	return ret;
+
+freed_rfkill:
+	if (priv->rfkill != NULL)
+		rfkill_free(priv->rfkill);
+	priv->rfkill = NULL;
+
+error:
+	IWL_DEBUG_RF_KILL("RFKILL initialization complete.\n");
+	return ret;
+}
+
+void iwl3945_rfkill_unregister(struct iwl3945_priv *priv)
+{
+	if (priv->rfkill)
+		rfkill_unregister(priv->rfkill);
+
+	priv->rfkill = NULL;
+}
+
+/* set rf-kill to the right state. */
+void iwl3945_rfkill_set_hw_state(struct iwl3945_priv *priv)
+{
+
+	if (!priv->rfkill)
+		return;
+
+	if (iwl3945_is_rfkill_hw(priv)) {
+		rfkill_force_state(priv->rfkill, RFKILL_STATE_HARD_BLOCKED);
+		return;
+	}
+
+	if (!iwl3945_is_rfkill_sw(priv))
+		rfkill_force_state(priv->rfkill, RFKILL_STATE_UNBLOCKED);
+	else
+		rfkill_force_state(priv->rfkill, RFKILL_STATE_SOFT_BLOCKED);
+}
+#endif
+
 /*****************************************************************************
  *
  * driver and module entry point
diff --git a/drivers/net/wireless/iwlwifi/iwl4965-base.c b/drivers/net/wireless/iwlwifi/iwl4965-base.c
index 0bd55bb19739..71f5da3fe5c4 100644
--- a/drivers/net/wireless/iwlwifi/iwl4965-base.c
+++ b/drivers/net/wireless/iwlwifi/iwl4965-base.c
@@ -46,14 +46,13 @@
 #include <asm/div64.h>
 
 #include "iwl-eeprom.h"
-#include "iwl-4965.h"
+#include "iwl-dev.h"
 #include "iwl-core.h"
 #include "iwl-io.h"
 #include "iwl-helpers.h"
 #include "iwl-sta.h"
+#include "iwl-calib.h"
 
-static int iwl4965_tx_queue_update_write_ptr(struct iwl_priv *priv,
-				  struct iwl4965_tx_queue *txq);
 
 /******************************************************************************
  *
@@ -88,292 +87,6 @@ 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_supported_band *iwl4965_get_hw_mode(
-		struct iwl_priv *priv, enum ieee80211_band band)
-{
-	return priv->hw->wiphy->bands[band];
-}
-
-static int iwl4965_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 *iwl4965_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 (iwl4965_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;
-}
-
-/*************** DMA-QUEUE-GENERAL-FUNCTIONS  *****
- * DMA services
- *
- * Theory of operation
- *
- * A Tx or Rx queue resides in host DRAM, and is comprised of a circular buffer
- * of buffer descriptors, each of which points to one or more data buffers for
- * the device to read from or fill.  Driver and device exchange status of each
- * queue via "read" and "write" pointers.  Driver keeps minimum of 2 empty
- * entries in each circular buffer, to protect against confusing empty and full
- * queue states.
- *
- * The device reads or writes the data in the queues via the device's several
- * DMA/FIFO channels.  Each queue is mapped to a single DMA channel.
- *
- * 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 4965 operates with up to 17 queues:  One receive queue, one transmit
- * queue (#4) for sending commands to the device firmware, and 15 other
- * Tx queues that may be mapped to prioritized Tx DMA/FIFO channels.
- *
- * See more detailed info in iwl-4965-hw.h.
- ***************************************************/
-
-int iwl4965_queue_space(const struct iwl4965_queue *q)
-{
-	int s = q->read_ptr - q->write_ptr;
-
-	if (q->read_ptr > q->write_ptr)
-		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;
-}
-
-
-static inline int x2_queue_used(const struct iwl4965_queue *q, int i)
-{
-	return q->write_ptr > q->read_ptr ?
-		(i >= q->read_ptr && i < q->write_ptr) :
-		!(i < q->read_ptr && i >= q->write_ptr);
-}
-
-static inline u8 get_cmd_index(struct iwl4965_queue *q, u32 index, int is_huge)
-{
-	/* This is for scan command, the big buffer at end of command array */
-	if (is_huge)
-		return q->n_window;	/* must be power of 2 */
-
-	/* Otherwise, use normal size buffers */
-	return index & (q->n_window - 1);
-}
-
-/**
- * iwl4965_queue_init - Initialize queue's high/low-water and read/write indexes
- */
-static int iwl4965_queue_init(struct iwl_priv *priv, struct iwl4965_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->write_ptr = q->read_ptr = 0;
-
-	return 0;
-}
-
-/**
- * iwl4965_tx_queue_alloc - Alloc driver data and TFD CB for one Tx/cmd queue
- */
-static int iwl4965_tx_queue_alloc(struct iwl_priv *priv,
-			      struct iwl4965_tx_queue *txq, u32 id)
-{
-	struct pci_dev *dev = priv->pci_dev;
-
-	/* Driver private data, only for Tx (not command) queues,
-	 * not shared with device. */
-	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 auxiliary BD "
-				  "structures failed\n");
-			goto error;
-		}
-	} else
-		txq->txb = NULL;
-
-	/* Circular buffer of transmit frame descriptors (TFDs),
-	 * shared with device */
-	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;
-}
-
-/**
- * iwl4965_tx_queue_init - Allocate and initialize one tx/cmd queue
- */
-int iwl4965_tx_queue_init(struct iwl_priv *priv,
-		      struct iwl4965_tx_queue *txq, int slots_num, u32 txq_id)
-{
-	struct pci_dev *dev = priv->pci_dev;
-	int len;
-	int rc = 0;
-
-	/*
-	 * Alloc buffer array for commands (Tx or other types of commands).
-	 * For the command queue (#4), allocate command space + one big
-	 * command for scan, since scan command is very huge; the system will
-	 * not have two scans at the same time, so only one is needed.
-	 * For normal Tx queues (all other queues), no super-size command
-	 * space is needed.
-	 */
-	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;
-
-	/* Alloc driver data array and TFD circular buffer */
-	rc = iwl4965_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));
-
-	/* Initialize queue's high/low-water marks, and head/tail indexes */
-	iwl4965_queue_init(priv, &txq->q, TFD_QUEUE_SIZE_MAX, slots_num, txq_id);
-
-	/* Tell device where to find queue */
-	iwl4965_hw_tx_queue_init(priv, txq);
-
-	return 0;
-}
-
-/**
- * iwl4965_tx_queue_free - Deallocate DMA queue.
- * @txq: Transmit queue to deallocate.
- *
- * Empty queue by removing and destroying all BD's.
- * Free all buffers.
- * 0-fill, but do not free "txq" descriptor structure.
- */
-void iwl4965_tx_queue_free(struct iwl_priv *priv, struct iwl4965_tx_queue *txq)
-{
-	struct iwl4965_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->write_ptr != q->read_ptr;
-	     q->read_ptr = iwl_queue_inc_wrap(q->read_ptr, q->n_bd))
-		iwl4965_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;
-
-	/* De-alloc array of command/tx buffers */
-	pci_free_consistent(dev, len, txq->cmd, txq->dma_addr_cmd);
-
-	/* De-alloc circular buffer of TFDs */
-	if (txq->q.n_bd)
-		pci_free_consistent(dev, sizeof(struct iwl4965_tfd_frame) *
-				    txq->q.n_bd, txq->bd, txq->q.dma_addr);
-
-	/* De-alloc array of per-TFD driver data */
-	if (txq->txb) {
-		kfree(txq->txb);
-		txq->txb = NULL;
-	}
-
-	/* 0-fill queue descriptor structure */
-	memset(txq, 0, sizeof(*txq));
-}
-
-const u8 iwl4965_broadcast_addr[ETH_ALEN] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF };
-
 /*************** STATION TABLE MANAGEMENT ****
  * mac80211 should be examined to determine if sta_info is duplicating
  * the functionality provided here
@@ -381,213 +94,11 @@ const u8 iwl4965_broadcast_addr[ETH_ALEN] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF
 
 /**************************************************************/
 
-#if 0 /* temporary disable till we add real remove station */
-/**
- * iwl4965_remove_station - Remove driver's knowledge of station.
- *
- * NOTE:  This does not remove station from device's station table.
- */
-static u8 iwl4965_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_params.bcast_sta_id;
-	else
-		for (i = IWL_STA_ID; i < priv->hw_params.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
-
-/**
- * iwl4965_add_station_flags - Add station to tables in driver and device
- */
-u8 iwl4965_add_station_flags(struct iwl_priv *priv, const u8 *addr,
-				int is_ap, u8 flags, void *ht_data)
-{
-	int i;
-	int index = IWL_INVALID_STATION;
-	struct iwl4965_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_params.bcast_sta_id;
-	else
-		for (i = IWL_STA_ID; i < priv->hw_params.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 two conditions have the same outcome, but keep them separate
-	  since they have different meanings */
-	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++;
-
-	/* Set up the REPLY_ADD_STA command to send to device */
-	memset(&station->sta, 0, sizeof(struct iwl4965_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_IWL4965_HT
-	/* BCAST station and IBSS stations do not work in HT mode */
-	if (index != priv->hw_params.bcast_sta_id &&
-	    priv->iw_mode != IEEE80211_IF_TYPE_IBSS)
-		iwl4965_set_ht_add_station(priv, index,
-				 (struct ieee80211_ht_info *) ht_data);
-#endif /*CONFIG_IWL4965_HT*/
-
-	spin_unlock_irqrestore(&priv->sta_lock, flags_spin);
-
-	/* Add station to device's station table */
-	iwl4965_send_add_station(priv, &station->sta, flags);
-	return index;
-
-}
-
-
-
-/*************** HOST COMMAND QUEUE FUNCTIONS   *****/
-
-/**
- * iwl4965_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.
- */
-int iwl4965_enqueue_hcmd(struct iwl_priv *priv, struct iwl_host_cmd *cmd)
-{
-	struct iwl4965_tx_queue *txq = &priv->txq[IWL_CMD_QUEUE_NUM];
-	struct iwl4965_queue *q = &txq->q;
-	struct iwl4965_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_is_rfkill(priv)) {
-		IWL_DEBUG_INFO("Not sending command - RF KILL");
-		return -EIO;
-	}
-
-	if (iwl4965_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->write_ptr];
-	memset(tfd, 0, sizeof(*tfd));
-
-	control_flags = (u32 *) tfd;
 
-	idx = get_cmd_index(q, q->write_ptr, 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->write_ptr));
-	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);
-	iwl4965_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->write_ptr, idx, IWL_CMD_QUEUE_NUM);
-
-	txq->need_update = 1;
-
-	/* Set up entry in queue's byte count circular buffer */
-	priv->cfg->ops->lib->txq_update_byte_cnt_tbl(priv, txq, 0);
-
-	/* Increment and update queue's write index */
-	q->write_ptr = iwl_queue_inc_wrap(q->write_ptr, q->n_bd);
-	ret = iwl4965_tx_queue_update_write_ptr(priv, txq);
-
-	spin_unlock_irqrestore(&priv->hcmd_lock, flags);
-	return ret ? ret : idx;
-}
 
 static void iwl4965_set_rxon_hwcrypto(struct iwl_priv *priv, int hw_decrypt)
 {
-	struct iwl4965_rxon_cmd *rxon = &priv->staging_rxon;
+	struct iwl_rxon_cmd *rxon = &priv->staging_rxon;
 
 	if (hw_decrypt)
 		rxon->filter_flags &= ~RXON_FILTER_DIS_DECRYPT_MSK;
@@ -597,45 +108,13 @@ static void iwl4965_set_rxon_hwcrypto(struct iwl_priv *priv, int hw_decrypt)
 }
 
 /**
- * iwl4965_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 this fnction
- */
-static int iwl4965_rxon_add_station(struct iwl_priv *priv,
-				const u8 *addr, int is_ap)
-{
-	u8 sta_id;
-
-	/* Add station to device's station table */
-#ifdef CONFIG_IWL4965_HT
-	struct ieee80211_conf *conf = &priv->hw->conf;
-	struct ieee80211_ht_info *cur_ht_config = &conf->ht_conf;
-
-	if ((is_ap) &&
-	    (conf->flags & IEEE80211_CONF_SUPPORT_HT_MODE) &&
-	    (priv->iw_mode == IEEE80211_IF_TYPE_STA))
-		sta_id = iwl4965_add_station_flags(priv, addr, is_ap,
-						   0, cur_ht_config);
-	else
-#endif /* CONFIG_IWL4965_HT */
-		sta_id = iwl4965_add_station_flags(priv, addr, is_ap,
-						   0, NULL);
-
-	/* Set up default rate scaling table in device's station table */
-	iwl4965_add_station(priv, addr, is_ap);
-
-	return sta_id;
-}
-
-/**
  * iwl4965_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 iwl4965_check_rxon_cmd(struct iwl4965_rxon_cmd *rxon)
+static int iwl4965_check_rxon_cmd(struct iwl_rxon_cmd *rxon)
 {
 	int error = 0;
 	int counter = 1;
@@ -713,7 +192,7 @@ static int iwl4965_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) ||
+	if (!(iwl_is_associated(priv)) ||
 	    compare_ether_addr(priv->staging_rxon.bssid_addr,
 			       priv->active_rxon.bssid_addr) ||
 	    compare_ether_addr(priv->staging_rxon.node_addr,
@@ -760,18 +239,23 @@ static int iwl4965_full_rxon_required(struct iwl_priv *priv)
 static int iwl4965_commit_rxon(struct iwl_priv *priv)
 {
 	/* cast away the const for active_rxon in this function */
-	struct iwl4965_rxon_cmd *active_rxon = (void *)&priv->active_rxon;
+	struct iwl_rxon_cmd *active_rxon = (void *)&priv->active_rxon;
 	DECLARE_MAC_BUF(mac);
-	int rc = 0;
+	int ret;
+	bool new_assoc =
+		!!(priv->staging_rxon.filter_flags & RXON_FILTER_ASSOC_MSK);
 
 	if (!iwl_is_alive(priv))
-		return -1;
+		return -EBUSY;
 
 	/* always get timestamp with Rx frame */
 	priv->staging_rxon.flags |= RXON_FLG_TSF2HOST_MSK;
+	/* allow CTS-to-self if possible. this is relevant only for
+	 * 5000, but will not damage 4965 */
+	priv->staging_rxon.flags |= RXON_FLG_SELF_CTS_EN;
 
-	rc = iwl4965_check_rxon_cmd(&priv->staging_rxon);
-	if (rc) {
+	ret = iwl4965_check_rxon_cmd(&priv->staging_rxon);
+	if (ret) {
 		IWL_ERROR("Invalid RXON configuration.  Not committing.\n");
 		return -EINVAL;
 	}
@@ -780,49 +264,37 @@ static int iwl4965_commit_rxon(struct iwl_priv *priv)
 	 * iwl4965_rxon_assoc_cmd which is used to reconfigure filter
 	 * and other flags for the current radio configuration. */
 	if (!iwl4965_full_rxon_required(priv)) {
-		rc = iwl_send_rxon_assoc(priv);
-		if (rc) {
-			IWL_ERROR("Error setting RXON_ASSOC "
-				  "configuration (%d).\n", rc);
-			return rc;
+		ret = iwl_send_rxon_assoc(priv);
+		if (ret) {
+			IWL_ERROR("Error setting RXON_ASSOC (%d)\n", ret);
+			return ret;
 		}
 
 		memcpy(active_rxon, &priv->staging_rxon, sizeof(*active_rxon));
-
 		return 0;
 	}
 
 	/* station table will be cleared */
 	priv->assoc_station_added = 0;
 
-#ifdef CONFIG_IWL4965_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_IWL4965_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)) {
+	if (iwl_is_associated(priv) && new_assoc) {
 		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 iwl4965_rxon_cmd),
+		ret = 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) {
+		if (ret) {
 			active_rxon->filter_flags |= RXON_FILTER_ASSOC_MSK;
-			IWL_ERROR("Error clearing ASSOC_MSK on current "
-				  "configuration (%d).\n", rc);
-			return rc;
+			IWL_ERROR("Error clearing ASSOC_MSK (%d)\n", ret);
+			return ret;
 		}
 	}
 
@@ -830,65 +302,87 @@ static int iwl4965_commit_rxon(struct iwl_priv *priv)
 		       "* with%s RXON_FILTER_ASSOC_MSK\n"
 		       "* channel = %d\n"
 		       "* bssid = %s\n",
-		       ((priv->staging_rxon.filter_flags &
-			 RXON_FILTER_ASSOC_MSK) ? "" : "out"),
+		       (new_assoc ? "" : "out"),
 		       le16_to_cpu(priv->staging_rxon.channel),
 		       print_mac(mac, priv->staging_rxon.bssid_addr));
 
-	iwl4965_set_rxon_hwcrypto(priv, !priv->cfg->mod_params->sw_crypto);
-	/* Apply the new configuration */
-	rc = iwl_send_cmd_pdu(priv, REPLY_RXON,
-			      sizeof(struct iwl4965_rxon_cmd), &priv->staging_rxon);
-	if (rc) {
-		IWL_ERROR("Error setting new configuration (%d).\n", rc);
-		return rc;
+	iwl4965_set_rxon_hwcrypto(priv, !priv->hw_params.sw_crypto);
+
+	/* Apply the new configuration
+	 * RXON unassoc clears the station table in uCode, send it before
+	 * we add the bcast station. If assoc bit is set, we will send RXON
+	 * after having added the bcast and bssid station.
+	 */
+	if (!new_assoc) {
+		ret = iwl_send_cmd_pdu(priv, REPLY_RXON,
+			      sizeof(struct iwl_rxon_cmd), &priv->staging_rxon);
+		if (ret) {
+			IWL_ERROR("Error setting new RXON (%d)\n", ret);
+			return ret;
+		}
+		memcpy(active_rxon, &priv->staging_rxon, sizeof(*active_rxon));
 	}
 
-	iwlcore_clear_stations_table(priv);
+	iwl_clear_stations_table(priv);
 
-#ifdef CONFIG_IWL4965_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_IWL4965_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 = iwl4965_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 (iwl4965_rxon_add_station(priv, iwl4965_broadcast_addr, 0) ==
-	    IWL_INVALID_STATION) {
+	if (iwl_rxon_add_station(priv, iwl_bcast_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 (iwl4965_rxon_add_station(priv, priv->active_rxon.bssid_addr, 1)
-		    == IWL_INVALID_STATION) {
-			IWL_ERROR("Error adding AP address for transmit.\n");
-			return -EIO;
+	if (new_assoc) {
+		if (priv->iw_mode == IEEE80211_IF_TYPE_STA) {
+			ret = iwl_rxon_add_station(priv,
+					   priv->active_rxon.bssid_addr, 1);
+			if (ret == IWL_INVALID_STATION) {
+				IWL_ERROR("Error adding AP address for TX.\n");
+				return -EIO;
+			}
+			priv->assoc_station_added = 1;
+			if (priv->default_wep_key &&
+			    iwl_send_static_wepkey_cmd(priv, 0))
+				IWL_ERROR("Could not send WEP static key.\n");
 		}
-		priv->assoc_station_added = 1;
-		if (priv->default_wep_key &&
-		    iwl_send_static_wepkey_cmd(priv, 0))
-			IWL_ERROR("Could not send WEP static key.\n");
+
+		/* Apply the new configuration
+		 * RXON assoc doesn't clear the station table in uCode,
+		 */
+		ret = iwl_send_cmd_pdu(priv, REPLY_RXON,
+			      sizeof(struct iwl_rxon_cmd), &priv->staging_rxon);
+		if (ret) {
+			IWL_ERROR("Error setting new RXON (%d)\n", ret);
+			return ret;
+		}
+		memcpy(active_rxon, &priv->staging_rxon, sizeof(*active_rxon));
+	}
+
+	iwl_init_sensitivity(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 */
+	ret = iwl_set_tx_power(priv, priv->tx_power_user_lmt, true);
+	if (ret) {
+		IWL_ERROR("Error sending TX power (%d)\n", ret);
+		return ret;
 	}
 
 	return 0;
 }
 
+void iwl4965_update_chain_flags(struct iwl_priv *priv)
+{
+
+	iwl_set_rxon_chain(priv);
+	iwl4965_commit_rxon(priv);
+}
+
 static int iwl4965_send_bt_config(struct iwl_priv *priv)
 {
 	struct iwl4965_bt_cmd bt_cmd = {
@@ -903,155 +397,7 @@ static int iwl4965_send_bt_config(struct iwl_priv *priv)
 				sizeof(struct iwl4965_bt_cmd), &bt_cmd);
 }
 
-static int iwl4965_send_scan_abort(struct iwl_priv *priv)
-{
-	int rc = 0;
-	struct iwl4965_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 iwl4965_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 iwl4965_card_state_sync_callback(struct iwl_priv *priv,
-					struct iwl_cmd *cmd,
-					struct sk_buff *skb)
-{
-	return 1;
-}
-
-/*
- * CARD_STATE_CMD
- *
- * Use: Sets the device's 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 iwl4965_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 = iwl4965_card_state_sync_callback;
-
-	return iwl_send_cmd(priv, &cmd);
-}
-
-static int iwl4965_add_sta_sync_callback(struct iwl_priv *priv,
-				     struct iwl_cmd *cmd, struct sk_buff *skb)
-{
-	struct iwl4965_rx_packet *res = NULL;
-
-	if (!skb) {
-		IWL_ERROR("Error: Response NULL in REPLY_ADD_STA.\n");
-		return 1;
-	}
-
-	res = (struct iwl4965_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 iwl4965_send_add_station(struct iwl_priv *priv,
-			 struct iwl4965_addsta_cmd *sta, u8 flags)
-{
-	struct iwl4965_rx_packet *res = NULL;
-	int rc = 0;
-	struct iwl_host_cmd cmd = {
-		.id = REPLY_ADD_STA,
-		.len = sizeof(struct iwl4965_addsta_cmd),
-		.meta.flags = flags,
-		.data = sta,
-	};
-
-	if (flags & CMD_ASYNC)
-		cmd.meta.u.callback = iwl4965_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 iwl4965_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 void iwl4965_clear_free_frames(struct iwl_priv *priv)
+static void iwl_clear_free_frames(struct iwl_priv *priv)
 {
 	struct list_head *element;
 
@@ -1061,7 +407,7 @@ static void iwl4965_clear_free_frames(struct iwl_priv *priv)
 	while (!list_empty(&priv->free_frames)) {
 		element = priv->free_frames.next;
 		list_del(element);
-		kfree(list_entry(element, struct iwl4965_frame, list));
+		kfree(list_entry(element, struct iwl_frame, list));
 		priv->frames_count--;
 	}
 
@@ -1072,9 +418,9 @@ static void iwl4965_clear_free_frames(struct iwl_priv *priv)
 	}
 }
 
-static struct iwl4965_frame *iwl4965_get_free_frame(struct iwl_priv *priv)
+static struct iwl_frame *iwl_get_free_frame(struct iwl_priv *priv)
 {
-	struct iwl4965_frame *frame;
+	struct iwl_frame *frame;
 	struct list_head *element;
 	if (list_empty(&priv->free_frames)) {
 		frame = kzalloc(sizeof(*frame), GFP_KERNEL);
@@ -1089,10 +435,10 @@ static struct iwl4965_frame *iwl4965_get_free_frame(struct iwl_priv *priv)
 
 	element = priv->free_frames.next;
 	list_del(element);
-	return list_entry(element, struct iwl4965_frame, list);
+	return list_entry(element, struct iwl_frame, list);
 }
 
-static void iwl4965_free_frame(struct iwl_priv *priv, struct iwl4965_frame *frame)
+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);
@@ -1116,27 +462,39 @@ unsigned int iwl4965_fill_beacon_frame(struct iwl_priv *priv,
 	return priv->ibss_beacon->len;
 }
 
-static u8 iwl4965_rate_get_lowest_plcp(int rate_mask)
+static u8 iwl4965_rate_get_lowest_plcp(struct iwl_priv *priv)
 {
-	u8 i;
+	int i;
+	int rate_mask;
+
+	/* Set rate mask*/
+	if (priv->staging_rxon.flags & RXON_FLG_BAND_24G_MSK)
+		rate_mask = priv->active_rate_basic & 0xF;
+	else
+		rate_mask = priv->active_rate_basic & 0xFF0;
 
+	/* Find lowest valid rate */
 	for (i = IWL_RATE_1M_INDEX; i != IWL_RATE_INVALID;
-	     i = iwl4965_rates[i].next_ieee) {
+					i = iwl_rates[i].next_ieee) {
 		if (rate_mask & (1 << i))
-			return iwl4965_rates[i].plcp;
+			return iwl_rates[i].plcp;
 	}
 
-	return IWL_RATE_INVALID;
+	/* No valid rate was found. Assign the lowest one */
+	if (priv->staging_rxon.flags & RXON_FLG_BAND_24G_MSK)
+		return IWL_RATE_1M_PLCP;
+	else
+		return IWL_RATE_6M_PLCP;
 }
 
 static int iwl4965_send_beacon_cmd(struct iwl_priv *priv)
 {
-	struct iwl4965_frame *frame;
+	struct iwl_frame *frame;
 	unsigned int frame_size;
 	int rc;
 	u8 rate;
 
-	frame = iwl4965_get_free_frame(priv);
+	frame = iwl_get_free_frame(priv);
 
 	if (!frame) {
 		IWL_ERROR("Could not obtain free frame buffer for beacon "
@@ -1144,23 +502,14 @@ static int iwl4965_send_beacon_cmd(struct iwl_priv *priv)
 		return -ENOMEM;
 	}
 
-	if (!(priv->staging_rxon.flags & RXON_FLG_BAND_24G_MSK)) {
-		rate = iwl4965_rate_get_lowest_plcp(priv->active_rate_basic &
-						0xFF0);
-		if (rate == IWL_INVALID_RATE)
-			rate = IWL_RATE_6M_PLCP;
-	} else {
-		rate = iwl4965_rate_get_lowest_plcp(priv->active_rate_basic & 0xF);
-		if (rate == IWL_INVALID_RATE)
-			rate = IWL_RATE_1M_PLCP;
-	}
+	rate = iwl4965_rate_get_lowest_plcp(priv);
 
 	frame_size = iwl4965_hw_get_beacon_cmd(priv, frame, rate);
 
 	rc = iwl_send_cmd_pdu(priv, REPLY_TX_BEACON, frame_size,
 			      &frame->u.cmd[0]);
 
-	iwl4965_free_frame(priv, frame);
+	iwl_free_frame(priv, frame);
 
 	return rc;
 }
@@ -1171,184 +520,69 @@ static int iwl4965_send_beacon_cmd(struct iwl_priv *priv)
  *
  ******************************************************************************/
 
-static void iwl4965_unset_hw_params(struct iwl_priv *priv)
-{
-	if (priv->shared_virt)
-		pci_free_consistent(priv->pci_dev,
-				    sizeof(struct iwl4965_shared),
-				    priv->shared_virt,
-				    priv->shared_phys);
-}
-
-/**
- * iwl4965_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 iwl4965_supported_rate_to_ie(u8 *ie, u16 supported_rate,
-				    u16 basic_rate, int *left)
-{
-	u16 ret_rates = 0, bit;
-	int i;
-	u8 *cnt = ie;
-	u8 *rates = ie + 1;
-
-	for (bit = 1, i = 0; i < IWL_RATE_COUNT; i++, bit <<= 1) {
-		if (bit & supported_rate) {
-			ret_rates |= bit;
-			rates[*cnt] = iwl4965_rates[i].ieee |
-				((bit & basic_rate) ? 0x80 : 0x00);
-			(*cnt)++;
-			(*left)--;
-			if ((*left <= 0) ||
-			    (*cnt >= IWL_SUPPORTED_RATES_IE_LEN))
-				break;
-		}
-	}
-
-	return ret_rates;
-}
-
-/**
- * iwl4965_fill_probe_req - fill in all required fields and IE for probe request
- */
-static u16 iwl4965_fill_probe_req(struct iwl_priv *priv,
-				  enum ieee80211_band band,
-				  struct ieee80211_mgmt *frame,
-				  int left, int is_direct)
+static void iwl4965_ht_conf(struct iwl_priv *priv,
+			    struct ieee80211_bss_conf *bss_conf)
 {
-	int len = 0;
-	u8 *pos = NULL;
-	u16 active_rates, ret_rates, cck_rates, active_rate_basic;
-#ifdef CONFIG_IWL4965_HT
-	const struct ieee80211_supported_band *sband =
-						iwl4965_get_hw_mode(priv, band);
-#endif /* CONFIG_IWL4965_HT */
-
-	/* 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;
+	struct ieee80211_ht_info *ht_conf = bss_conf->ht_conf;
+	struct ieee80211_ht_bss_info *ht_bss_conf = bss_conf->ht_bss_conf;
+	struct iwl_ht_info *iwl_conf = &priv->current_ht_config;
 
-	frame->frame_control = cpu_to_le16(IEEE80211_STYPE_PROBE_REQ);
-	memcpy(frame->da, iwl4965_broadcast_addr, ETH_ALEN);
-	memcpy(frame->sa, priv->mac_addr, ETH_ALEN);
-	memcpy(frame->bssid, iwl4965_broadcast_addr, ETH_ALEN);
-	frame->seq_ctrl = 0;
+	IWL_DEBUG_MAC80211("enter: \n");
 
-	/* fill in our indirect SSID IE */
-	/* ...next IE... */
+	iwl_conf->is_ht = bss_conf->assoc_ht;
 
-	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;
+	if (!iwl_conf->is_ht)
+		return;
 
-	/* ... fill it in... */
-	*pos++ = WLAN_EID_SUPP_RATES;
-	*pos = 0;
+	priv->ps_mode = (u8)((ht_conf->cap & IEEE80211_HT_CAP_MIMO_PS) >> 2);
 
-	/* exclude 60M rate */
-	active_rates = priv->rates_mask;
-	active_rates &= ~IWL_RATE_60M_MASK;
+	if (ht_conf->cap & IEEE80211_HT_CAP_SGI_20)
+		iwl_conf->sgf |= HT_SHORT_GI_20MHZ;
+	if (ht_conf->cap & IEEE80211_HT_CAP_SGI_40)
+		iwl_conf->sgf |= HT_SHORT_GI_40MHZ;
 
-	active_rate_basic = active_rates & IWL_BASIC_RATES_MASK;
+	iwl_conf->is_green_field = !!(ht_conf->cap & IEEE80211_HT_CAP_GRN_FLD);
+	iwl_conf->max_amsdu_size =
+		!!(ht_conf->cap & IEEE80211_HT_CAP_MAX_AMSDU);
 
-	cck_rates = IWL_CCK_RATES_MASK & active_rates;
-	ret_rates = iwl4965_supported_rate_to_ie(pos, cck_rates,
-			active_rate_basic, &left);
-	active_rates &= ~ret_rates;
+	iwl_conf->supported_chan_width =
+		!!(ht_conf->cap & IEEE80211_HT_CAP_SUP_WIDTH);
+	iwl_conf->extension_chan_offset =
+		ht_bss_conf->bss_cap & IEEE80211_HT_IE_CHA_SEC_OFFSET;
+	/* If no above or below channel supplied disable FAT channel */
+	if (iwl_conf->extension_chan_offset != IEEE80211_HT_IE_CHA_SEC_ABOVE &&
+	    iwl_conf->extension_chan_offset != IEEE80211_HT_IE_CHA_SEC_BELOW) {
+		iwl_conf->extension_chan_offset = IEEE80211_HT_IE_CHA_SEC_NONE;
+		iwl_conf->supported_chan_width = 0;
+	}
 
-	ret_rates = iwl4965_supported_rate_to_ie(pos, active_rates,
-				 active_rate_basic, &left);
-	active_rates &= ~ret_rates;
+	iwl_conf->tx_mimo_ps_mode =
+		(u8)((ht_conf->cap & IEEE80211_HT_CAP_MIMO_PS) >> 2);
+	memcpy(iwl_conf->supp_mcs_set, ht_conf->supp_mcs_set, 16);
 
-	len += 2 + *pos;
-	pos += (*pos) + 1;
-	if (active_rates == 0)
-		goto fill_end;
+	iwl_conf->control_channel = ht_bss_conf->primary_channel;
+	iwl_conf->tx_chan_width =
+		!!(ht_bss_conf->bss_cap & IEEE80211_HT_IE_CHA_WIDTH);
+	iwl_conf->ht_protection =
+		ht_bss_conf->bss_op_mode & IEEE80211_HT_IE_HT_PROTECTION;
+	iwl_conf->non_GF_STA_present =
+		!!(ht_bss_conf->bss_op_mode & IEEE80211_HT_IE_NON_GF_STA_PRSNT);
 
-	/* 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;
-	iwl4965_supported_rate_to_ie(pos, active_rates,
-				 active_rate_basic, &left);
-	if (*pos > 0)
-		len += 2 + *pos;
-
-#ifdef CONFIG_IWL4965_HT
-	if (sband && sband->ht_info.ht_supported) {
-		struct ieee80211_ht_cap *ht_cap;
-		pos += (*pos) + 1;
-		*pos++ = WLAN_EID_HT_CAPABILITY;
-		*pos++ = sizeof(struct ieee80211_ht_cap);
-		ht_cap = (struct ieee80211_ht_cap *)pos;
-		ht_cap->cap_info = cpu_to_le16(sband->ht_info.cap);
-		memcpy(ht_cap->supp_mcs_set, sband->ht_info.supp_mcs_set, 16);
-		ht_cap->ampdu_params_info =(sband->ht_info.ampdu_factor &
-					    IEEE80211_HT_CAP_AMPDU_FACTOR) |
-					    ((sband->ht_info.ampdu_density << 2) &
-					    IEEE80211_HT_CAP_AMPDU_DENSITY);
-		len += 2 + sizeof(struct ieee80211_ht_cap);
-	}
-#endif  /*CONFIG_IWL4965_HT */
-
- fill_end:
-	return (u16)len;
+	IWL_DEBUG_MAC80211("control channel %d\n", iwl_conf->control_channel);
+	IWL_DEBUG_MAC80211("leave\n");
 }
 
 /*
  * QoS  support
 */
-static int iwl4965_send_qos_params_command(struct iwl_priv *priv,
-				       struct iwl4965_qosparam_cmd *qos)
-{
-
-	return iwl_send_cmd_pdu(priv, REPLY_QOS_PARAM,
-				sizeof(struct iwl4965_qosparam_cmd), qos);
-}
-
-static void iwl4965_activate_qos(struct iwl_priv *priv, u8 force)
+static void iwl_activate_qos(struct iwl_priv *priv, u8 force)
 {
-	unsigned long flags;
-
 	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 &&
@@ -1359,323 +593,18 @@ static void iwl4965_activate_qos(struct iwl_priv *priv, u8 force)
 		priv->qos_data.def_qos_parm.qos_flags |=
 			QOS_PARAM_FLG_UPDATE_EDCA_MSK;
 
-#ifdef CONFIG_IWL4965_HT
 	if (priv->current_ht_config.is_ht)
 		priv->qos_data.def_qos_parm.qos_flags |= QOS_PARAM_FLG_TGN_MSK;
-#endif /* CONFIG_IWL4965_HT */
-
-	spin_unlock_irqrestore(&priv->lock, flags);
 
 	if (force || iwl_is_associated(priv)) {
 		IWL_DEBUG_QOS("send QoS cmd with Qos active=%d FLAGS=0x%X\n",
 				priv->qos_data.qos_active,
 				priv->qos_data.def_qos_parm.qos_flags);
 
-		iwl4965_send_qos_params_command(priv,
-				&(priv->qos_data.def_qos_parm));
-	}
-}
-
-/*
- * 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 iwl4965_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 iwl4965_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 iwl4965_power_init_handle(struct iwl_priv *priv)
-{
-	int rc = 0, i;
-	struct iwl4965_power_mgr *pow_data;
-	int size = sizeof(struct iwl4965_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 iwl4965_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;
-		}
+		iwl_send_cmd_pdu_async(priv, REPLY_QOS_PARAM,
+				       sizeof(struct iwl_qosparam_cmd),
+				       &priv->qos_data.def_qos_parm, NULL);
 	}
-	return rc;
-}
-
-static int iwl4965_update_power_cmd(struct iwl_priv *priv,
-				struct iwl4965_powertable_cmd *cmd, u32 mode)
-{
-	int rc = 0, i;
-	u8 skip;
-	u32 max_sleep = 0;
-	struct iwl4965_power_vec_entry *range;
-	u8 period = 0;
-	struct iwl4965_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 iwl4965_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 iwl4965_send_power_mode(struct iwl_priv *priv, u32 mode)
-{
-	u32 uninitialized_var(final_mode);
-	int rc;
-	struct iwl4965_powertable_cmd cmd;
-
-	/* If on battery, set to 3,
-	 * if plugged into AC power, set to CAM ("continuously 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;
-
-	iwl4965_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 iwl4965_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);
-	default:
-		break;
-	}
-
-	return 1;
-}
-
-#define TX_STATUS_ENTRY(x) case TX_STATUS_FAIL_ ## x: return #x
-
-static const char *iwl4965_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";
-}
-
-/**
- * iwl4965_scan_cancel - Cancel any currently executing HW scan
- *
- * NOTE: priv->mutex is not required before calling this function
- */
-static int iwl4965_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;
-}
-
-/**
- * iwl4965_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 iwl4965_scan_cancel_timeout(struct iwl_priv *priv, unsigned long ms)
-{
-	unsigned long now = jiffies;
-	int ret;
-
-	ret = iwl4965_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 iwl4965_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;
-
-	iwl4965_scan_cancel(priv);
 }
 
 #define MAX_UCODE_BEACON_INTERVAL	4096
@@ -1750,46 +679,8 @@ static void iwl4965_setup_rxon_timing(struct iwl_priv *priv)
 		le16_to_cpu(priv->rxon_timing.atim_window));
 }
 
-static int iwl4965_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");
-	if (priv->cfg->sku & IWL_SKU_G)
-		priv->scan_bands |= BIT(IEEE80211_BAND_2GHZ);
-	if (priv->cfg->sku & IWL_SKU_A)
-		priv->scan_bands |= BIT(IEEE80211_BAND_5GHZ);
-	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 void iwl4965_set_flags_for_phymode(struct iwl_priv *priv,
-					  enum ieee80211_band band)
+static void iwl_set_flags_for_band(struct iwl_priv *priv,
+				   enum ieee80211_band band)
 {
 	if (band == IEEE80211_BAND_5GHZ) {
 		priv->staging_rxon.flags &=
@@ -1858,7 +749,7 @@ static void iwl4965_connection_init_rx_config(struct iwl_priv *priv)
 #endif
 
 	ch_info = iwl_get_channel_info(priv, priv->band,
-				       le16_to_cpu(priv->staging_rxon.channel));
+				       le16_to_cpu(priv->active_rxon.channel));
 
 	if (!ch_info)
 		ch_info = &priv->channel_info[0];
@@ -1874,7 +765,7 @@ static void iwl4965_connection_init_rx_config(struct iwl_priv *priv)
 	priv->staging_rxon.channel = cpu_to_le16(ch_info->channel);
 	priv->band = ch_info->band;
 
-	iwl4965_set_flags_for_phymode(priv, priv->band);
+	iwl_set_flags_for_band(priv, priv->band);
 
 	priv->staging_rxon.ofdm_basic_rates =
 	    (IWL_OFDM_RATES_MASK >> IWL_FIRST_OFDM_RATE) & 0xFF;
@@ -1887,38 +778,24 @@ static void iwl4965_connection_init_rx_config(struct iwl_priv *priv)
 	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);
+	iwl_set_rxon_chain(priv);
 }
 
 static int iwl4965_set_mode(struct iwl_priv *priv, int mode)
 {
-	if (mode == IEEE80211_IF_TYPE_IBSS) {
-		const struct iwl_channel_info *ch_info;
-
-		ch_info = iwl_get_channel_info(priv,
-			priv->band,
-			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;
-		}
-	}
-
 	priv->iw_mode = mode;
 
 	iwl4965_connection_init_rx_config(priv);
 	memcpy(priv->staging_rxon.node_addr, priv->mac_addr, ETH_ALEN);
 
-	iwlcore_clear_stations_table(priv);
+	iwl_clear_stations_table(priv);
 
 	/* dont commit rxon if rf-kill is on*/
 	if (!iwl_is_ready_rf(priv))
 		return -EAGAIN;
 
 	cancel_delayed_work(&priv->scan_check);
-	if (iwl4965_scan_cancel_timeout(priv, 100)) {
+	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;
@@ -1929,448 +806,13 @@ static int iwl4965_set_mode(struct iwl_priv *priv, int mode)
 	return 0;
 }
 
-static void iwl4965_build_tx_cmd_hwcrypto(struct iwl_priv *priv,
-				      struct ieee80211_tx_control *ctl,
-				      struct iwl_cmd *cmd,
-				      struct sk_buff *skb_frag,
-				      int sta_id)
-{
-	struct iwl4965_hw_key *keyinfo = &priv->stations[sta_id].keyinfo;
-	struct iwl_wep_key *wepkey;
-	int keyidx = 0;
-
-	BUG_ON(ctl->key_idx > 3);
-
-	switch (keyinfo->alg) {
-	case ALG_CCMP:
-		cmd->cmd.tx.sec_ctl = TX_CMD_SEC_CCM;
-		memcpy(cmd->cmd.tx.key, keyinfo->key, keyinfo->keylen);
-		if (ctl->flags & IEEE80211_TXCTL_AMPDU)
-			cmd->cmd.tx.tx_flags |= TX_CMD_FLG_AGG_CCMP_MSK;
-		IWL_DEBUG_TX("tx_cmd with aes hwcrypto\n");
-		break;
-
-	case ALG_TKIP:
-		cmd->cmd.tx.sec_ctl = TX_CMD_SEC_TKIP;
-		ieee80211_get_tkip_key(keyinfo->conf, skb_frag,
-			IEEE80211_TKIP_P2_KEY, cmd->cmd.tx.key);
-		IWL_DEBUG_TX("tx_cmd with tkip hwcrypto\n");
-		break;
-
-	case ALG_WEP:
-		wepkey = &priv->wep_keys[ctl->key_idx];
-		cmd->cmd.tx.sec_ctl = 0;
-		if (priv->default_wep_key) {
-			/* the WEP key was sent as static */
-			keyidx = ctl->key_idx;
-			memcpy(&cmd->cmd.tx.key[3], wepkey->key,
-							wepkey->key_size);
-			if (wepkey->key_size == WEP_KEY_LEN_128)
-				cmd->cmd.tx.sec_ctl |= TX_CMD_SEC_KEY128;
-		} else {
-			/* the WEP key was sent as dynamic */
-			keyidx = keyinfo->keyidx;
-			memcpy(&cmd->cmd.tx.key[3], keyinfo->key,
-							keyinfo->keylen);
-			if (keyinfo->keylen == WEP_KEY_LEN_128)
-				cmd->cmd.tx.sec_ctl |= TX_CMD_SEC_KEY128;
-		}
-
-		cmd->cmd.tx.sec_ctl |= (TX_CMD_SEC_WEP |
-			(keyidx & TX_CMD_SEC_MSK) << TX_CMD_SEC_SHIFT);
-
-		IWL_DEBUG_TX("Configuring packet for WEP encryption "
-			     "with key %d\n", keyidx);
-		break;
-
-	default:
-		printk(KERN_ERR "Unknown encode alg %d\n", keyinfo->alg);
-		break;
-	}
-}
-
-/*
- * handle build REPLY_TX command notification.
- */
-static void iwl4965_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;
-	}
-
-	if (ieee80211_is_back_request(fc))
-		tx_flags |= TX_CMD_FLG_ACK_MSK | TX_CMD_FLG_IMM_BA_RSP_MASK;
-
-
-	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 void iwl_update_tx_stats(struct iwl_priv *priv, u16 fc, u16 len)
-{
-	/* 0 - mgmt, 1 - cnt, 2 - data */
-	int idx = (fc & IEEE80211_FCTL_FTYPE) >> 2;
-	priv->tx_stats[idx].cnt++;
-	priv->tx_stats[idx].bytes += len;
-}
-/**
- * iwl4965_get_sta_id - Find station's index within station table
- *
- * If new IBSS station, create new entry in station table
- */
-static int iwl4965_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 management, use broadcast station id */
-	if (((fc & IEEE80211_FCTL_FTYPE) != IEEE80211_FTYPE_DATA) ||
-	    is_multicast_ether_addr(hdr->addr1))
-		return priv->hw_params.bcast_sta_id;
-
-	switch (priv->iw_mode) {
-
-	/* If we are a client station in a BSS network, use the special
-	 * AP station entry (that's the only station we communicate with) */
-	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 = iwl4965_hw_find_station(priv, hdr->addr1);
-		if (sta_id != IWL_INVALID_STATION)
-			return sta_id;
-		return priv->hw_params.bcast_sta_id;
-
-	/* If this frame is going out to an IBSS network, find the station,
-	 * or create a new station table entry */
-	case IEEE80211_IF_TYPE_IBSS:
-		sta_id = iwl4965_hw_find_station(priv, hdr->addr1);
-		if (sta_id != IWL_INVALID_STATION)
-			return sta_id;
-
-		/* Create new station table entry */
-		sta_id = iwl4965_add_station_flags(priv, hdr->addr1,
-						   0, CMD_ASYNC, NULL);
-
-		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_params.bcast_sta_id;
-
-	default:
-		IWL_WARNING("Unknown mode of operation: %d", priv->iw_mode);
-		return priv->hw_params.bcast_sta_id;
-	}
-}
-
-/*
- * start REPLY_TX command process
- */
-static int iwl4965_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 iwl4965_tfd_frame *tfd;
-	u32 *control_flags;
-	int txq_id = ctl->queue;
-	struct iwl4965_tx_queue *txq = NULL;
-	struct iwl4965_queue *q = NULL;
-	dma_addr_t phys_addr;
-	dma_addr_t txcmd_phys;
-	dma_addr_t scratch_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->vif) {
-		IWL_DEBUG_DROP("Dropping - !priv->vif\n");
-		goto drop_unlock;
-	}
-
-	if ((ctl->tx_rate->hw_value & 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
-
-	/* drop all data frame if we are not associated */
-	if (((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_DATA) &&
-	   (!iwl_is_associated(priv) ||
-	    ((priv->iw_mode == IEEE80211_IF_TYPE_STA) && !priv->assoc_id) ||
-	    !priv->assoc_station_added)) {
-		IWL_DEBUG_DROP("Dropping - !iwl_is_associated\n");
-		goto drop_unlock;
-	}
-
-	spin_unlock_irqrestore(&priv->lock, flags);
-
-	hdr_len = ieee80211_get_hdrlen(fc);
-
-	/* Find (or create) index into station table for destination station */
-	sta_id = iwl4965_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_IWL4965_HT
-		/* aggregation is on for this <sta,tid> */
-		if (ctl->flags & IEEE80211_TXCTL_AMPDU)
-			txq_id = priv->stations[sta_id].tid[tid].agg.txq_id;
-		priv->stations[sta_id].tid[tid].tfds_in_queue++;
-#endif /* CONFIG_IWL4965_HT */
-	}
-
-	/* Descriptor for chosen Tx queue */
-	txq = &priv->txq[txq_id];
-	q = &txq->q;
-
-	spin_lock_irqsave(&priv->lock, flags);
-
-	/* Set up first empty TFD within this queue's circular TFD buffer */
-	tfd = &txq->bd[q->write_ptr];
-	memset(tfd, 0, sizeof(*tfd));
-	control_flags = (u32 *) tfd;
-	idx = get_cmd_index(q, q->write_ptr, 0);
-
-	/* Set up driver data for this TFD */
-	memset(&(txq->txb[q->write_ptr]), 0, sizeof(struct iwl4965_tx_info));
-	txq->txb[q->write_ptr].skb[0] = skb;
-	memcpy(&(txq->txb[q->write_ptr].status.control),
-	       ctl, sizeof(struct ieee80211_tx_control));
-
-	/* Set up first empty entry in queue's array of Tx/cmd buffers */
-	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));
-
-	/*
-	 * Set up the Tx-command (not MAC!) header.
-	 * Store the chosen Tx queue and TFD index within the sequence field;
-	 * after Tx, uCode's Tx response will return this value so driver can
-	 * locate the frame within the tx queue and do post-tx processing.
-	 */
-	out_cmd->hdr.cmd = REPLY_TX;
-	out_cmd->hdr.sequence = cpu_to_le16((u16)(QUEUE_TO_SEQ(txq_id) |
-				INDEX_TO_SEQ(q->write_ptr)));
-
-	/* Copy MAC header from skb into command buffer */
-	memcpy(out_cmd->cmd.tx.hdr, hdr, hdr_len);
-
-	/*
-	 * Use the first empty entry in this queue's command buffer array
-	 * to contain the Tx command and MAC header concatenated together
-	 * (payload data will be in another buffer).
-	 * Size of this varies, due to varying MAC header length.
-	 * If end is not dword aligned, we'll have 2 extra bytes at the end
-	 * of the MAC header (device reads on dword boundaries).
-	 * We'll tell device about this padding later.
-	 */
-	len = priv->hw_params.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;
-
-	/* Physical address of this Tx command's header (not MAC header!),
-	 * within command buffer array. */
-	txcmd_phys = txq->dma_addr_cmd + sizeof(struct iwl_cmd) * idx +
-		     offsetof(struct iwl_cmd, hdr);
-
-	/* Add buffer containing Tx command and MAC(!) header to TFD's
-	 * first entry */
-	iwl4965_hw_txq_attach_buf_to_tfd(priv, tfd, txcmd_phys, len);
-
-	if (!(ctl->flags & IEEE80211_TXCTL_DO_NOT_ENCRYPT))
-		iwl4965_build_tx_cmd_hwcrypto(priv, ctl, out_cmd, skb, sta_id);
-
-	/* Set up TFD's 2nd entry to point directly to remainder of skb,
-	 * if any (802.11 null frames 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);
-		iwl4965_hw_txq_attach_buf_to_tfd(priv, tfd, phys_addr, len);
-	}
-
-	/* Tell 4965 about any 2-byte padding after MAC header */
-	if (len_org)
-		out_cmd->cmd.tx.tx_flags |= TX_CMD_FLG_MH_PAD_MSK;
-
-	/* Total # bytes to be transmitted */
-	len = (u16)skb->len;
-	out_cmd->cmd.tx.len = cpu_to_le16(len);
-
-	/* TODO need this for burst mode later on */
-	iwl4965_build_tx_cmd_basic(priv, out_cmd, ctl, hdr, unicast, sta_id);
-
-	/* set is_hcca to 0; it probably will never be implemented */
-	iwl4965_hw_build_tx_cmd_rate(priv, out_cmd, ctl, hdr, sta_id, 0);
-
-	iwl_update_tx_stats(priv, fc, len);
-
-	scratch_phys = txcmd_phys + sizeof(struct iwl_cmd_header) +
-		offsetof(struct iwl4965_tx_cmd, scratch);
-	out_cmd->cmd.tx.dram_lsb_ptr = cpu_to_le32(scratch_phys);
-	out_cmd->cmd.tx.dram_msb_ptr = iwl_get_dma_hi_address(scratch_phys);
-
-	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));
-
-	/* Set up entry for this TFD in Tx byte-count array */
-	priv->cfg->ops->lib->txq_update_byte_cnt_tbl(priv, txq, len);
-
-	/* Tell device the write index *just past* this latest filled TFD */
-	q->write_ptr = iwl_queue_inc_wrap(q->write_ptr, q->n_bd);
-	rc = iwl4965_tx_queue_update_write_ptr(priv, txq);
-	spin_unlock_irqrestore(&priv->lock, flags);
-
-	if (rc)
-		return rc;
-
-	if ((iwl4965_queue_space(q) < q->high_mark)
-	    && priv->mac80211_registered) {
-		if (wait_write_ptr) {
-			spin_lock_irqsave(&priv->lock, flags);
-			txq->need_update = 1;
-			iwl4965_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 iwl4965_set_rate(struct iwl_priv *priv)
 {
 	const struct ieee80211_supported_band *hw = NULL;
 	struct ieee80211_rate *rate;
 	int i;
 
-	hw = iwl4965_get_hw_mode(priv, priv->band);
+	hw = iwl_get_hw_mode(priv, priv->band);
 	if (!hw) {
 		IWL_ERROR("Failed to set rate: unable to get hw mode\n");
 		return;
@@ -2411,169 +853,6 @@ static void iwl4965_set_rate(struct iwl_priv *priv)
 		   (IWL_OFDM_BASIC_RATES_MASK >> IWL_FIRST_OFDM_RATE) & 0xFF;
 }
 
-void iwl4965_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) {
-		iwl4965_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);
-			/* call the host command only if no hw rf-kill set */
-			if (!test_bit(STATUS_RF_KILL_HW, &priv->status) &&
-			    iwl_is_ready(priv))
-				iwl4965_send_card_state(priv,
-							CARD_STATE_CMD_DISABLE,
-							0);
-			set_bit(STATUS_RF_KILL_SW, &priv->status);
-
-			/* make sure mac80211 stop sending Tx frame */
-			if (priv->mac80211_registered)
-				ieee80211_stop_queues(priv->hw);
-		}
-		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_nic_access(priv))
-		iwl_release_nic_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 iwl4965_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:
-		/* The uCode has got a bad phase 1 Key, pushes the packet.
-		 * Decryption will be done in SW. */
-		if ((decrypt_res & RX_RES_STATUS_DECRYPT_TYPE_MSK) ==
-		    RX_RES_STATUS_BAD_KEY_TTAK)
-			break;
-
-		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;
-	}
-}
-
-
-#define IWL_PACKET_RETRY_TIME HZ
-
-int iwl4965_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 iwl4965_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 iwl4965_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_IWL4965_SPECTRUM_MEASUREMENT
 
 #include "iwl-spectrum.h"
@@ -2632,7 +911,7 @@ static int iwl4965_get_measurement(struct iwl_priv *priv,
 			       u8 type)
 {
 	struct iwl4965_spectrum_cmd spectrum;
-	struct iwl4965_rx_packet *res;
+	struct iwl_rx_packet *res;
 	struct iwl_host_cmd cmd = {
 		.id = REPLY_SPECTRUM_MEASUREMENT_CMD,
 		.data = (void *)&spectrum,
@@ -2677,7 +956,7 @@ static int iwl4965_get_measurement(struct iwl_priv *priv,
 	if (rc)
 		return rc;
 
-	res = (struct iwl4965_rx_packet *)cmd.meta.u.skb->data;
+	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;
@@ -2707,352 +986,16 @@ static int iwl4965_get_measurement(struct iwl_priv *priv,
 }
 #endif
 
-static void iwl4965_txstatus_to_ieee(struct iwl_priv *priv,
-				 struct iwl4965_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;
-}
-
-/**
- * iwl4965_tx_queue_reclaim - Reclaim Tx queue entries already Tx'd
- *
- * 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 the stack that feeds us.
- */
-int iwl4965_tx_queue_reclaim(struct iwl_priv *priv, int txq_id, int index)
-{
-	struct iwl4965_tx_queue *txq = &priv->txq[txq_id];
-	struct iwl4965_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->write_ptr, q->read_ptr);
-		return 0;
-	}
-
-	for (index = iwl_queue_inc_wrap(index, q->n_bd);
-		q->read_ptr != index;
-		q->read_ptr = iwl_queue_inc_wrap(q->read_ptr, q->n_bd)) {
-		if (txq_id != IWL_CMD_QUEUE_NUM) {
-			iwl4965_txstatus_to_ieee(priv,
-					&(txq->txb[txq->q.read_ptr]));
-			iwl4965_hw_txq_free_tfd(priv, txq);
-		} else if (nfreed > 1) {
-			IWL_ERROR("HCMD skipped: index (%d) %d %d\n", index,
-					q->write_ptr, q->read_ptr);
-			queue_work(priv->workqueue, &priv->restart);
-		}
-		nfreed++;
-	}
-
-/*	if (iwl4965_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 iwl4965_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_IWL4965_HT
-
-static inline int iwl4965_get_ra_sta_id(struct iwl_priv *priv,
-				    struct ieee80211_hdr *hdr)
+static void iwl_rx_reply_alive(struct iwl_priv *priv,
+				struct iwl_rx_mem_buffer *rxb)
 {
-	if (priv->iw_mode == IEEE80211_IF_TYPE_STA)
-		return IWL_AP_ID;
-	else {
-		u8 *da = ieee80211_get_DA(hdr);
-		return iwl4965_hw_find_station(priv, da);
-	}
-}
-
-static struct ieee80211_hdr *iwl4965_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 iwl4965_get_scd_ssn(struct iwl4965_tx_resp *tx_resp)
-{
-	__le32 *scd_ssn = (__le32 *)((u32 *)&tx_resp->status +
-				tx_resp->frame_count);
-	return le32_to_cpu(*scd_ssn) & MAX_SN;
-
-}
-
-/**
- * iwl4965_tx_status_reply_tx - Handle Tx rspnse for frames in aggregation queue
- */
-static int iwl4965_tx_status_reply_tx(struct iwl_priv *priv,
-				      struct iwl4965_ht_agg *agg,
-				      struct iwl4965_tx_resp_agg *tx_resp,
-				      u16 start_idx)
-{
-	u16 status;
-	struct agg_tx_status *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 response w/o block-ack\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->bitmap = 0;
-
-	/* # frames attempted by Tx command */
-	if (agg->frame_count == 1) {
-		/* Only one frame was attempted; no block-ack will arrive */
-		status = le16_to_cpu(frame_status[0].status);
-		seq  = le16_to_cpu(frame_status[0].sequence);
-		idx = SEQ_TO_INDEX(seq);
-		txq_id = SEQ_TO_QUEUE(seq);
-
-		/* FIXME: code repetition */
-		IWL_DEBUG_TX_REPLY("FrameCnt = %d, StartIdx=%d idx=%d\n",
-				   agg->frame_count, agg->start_idx, idx);
-
-		tx_status = &(priv->txq[txq_id].txb[idx].status);
-		tx_status->retry_count = tx_resp->failure_frame;
-		tx_status->queue_number = status & 0xff;
-		tx_status->queue_length = tx_resp->failure_rts;
-		tx_status->control.flags &= ~IEEE80211_TXCTL_AMPDU;
-		tx_status->flags = iwl4965_is_tx_success(status)?
-			IEEE80211_TX_STATUS_ACK : 0;
-		iwl4965_hwrate_to_tx_control(priv,
-					     le32_to_cpu(tx_resp->rate_n_flags),
-					     &tx_status->control);
-		/* 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",
-				iwl4965_hw_get_rate_n_flags(tx_resp->rate_n_flags));
-
-		agg->wait_for_ba = 0;
-	} else {
-		/* Two or more frames were attempted; expect block-ack */
-		u64 bitmap = 0;
-		int start = agg->start_idx;
-
-		/* Construct bit-map of pending frames within Tx window */
-		for (i = 0; i < agg->frame_count; i++) {
-			u16 sc;
-			status = le16_to_cpu(frame_status[i].status);
-			seq  = le16_to_cpu(frame_status[i].sequence);
-			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 = iwl4965_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->bitmap = bitmap;
-		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%llx\n",
-				   agg->frame_count, agg->start_idx,
-				   (unsigned long long)agg->bitmap);
-
-		if (bitmap)
-			agg->wait_for_ba = 1;
-	}
-	return 0;
-}
-#endif
-
-/**
- * iwl4965_rx_reply_tx - Handle standard (non-aggregation) Tx response
- */
-static void iwl4965_rx_reply_tx(struct iwl_priv *priv,
-			    struct iwl4965_rx_mem_buffer *rxb)
-{
-	struct iwl4965_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 iwl4965_tx_queue *txq = &priv->txq[txq_id];
-	struct ieee80211_tx_status *tx_status;
-	struct iwl4965_tx_resp *tx_resp = (void *)&pkt->u.raw[0];
-	u32  status = le32_to_cpu(tx_resp->status);
-#ifdef CONFIG_IWL4965_HT
-	int tid = MAX_TID_COUNT, sta_id = IWL_INVALID_STATION;
-	struct ieee80211_hdr *hdr;
-	__le16 *qc;
-#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.write_ptr,
-			  txq->q.read_ptr);
-		return;
-	}
-
-#ifdef CONFIG_IWL4965_HT
-	hdr = iwl4965_tx_queue_get_hdr(priv, txq_id, index);
-	qc = ieee80211_get_qos_ctrl(hdr);
-
-	if (qc)
-		tid = le16_to_cpu(*qc) & 0xf;
-
-	sta_id = iwl4965_get_ra_sta_id(priv, hdr);
-	if (txq->sched_retry && unlikely(sta_id == IWL_INVALID_STATION)) {
-		IWL_ERROR("Station not known\n");
-		return;
-	}
-
-	if (txq->sched_retry) {
-		const u32 scd_ssn = iwl4965_get_scd_ssn(tx_resp);
-		struct iwl4965_ht_agg *agg = NULL;
-
-		if (!qc)
-			return;
-
-		agg = &priv->stations[sta_id].tid[tid].agg;
-
-		iwl4965_tx_status_reply_tx(priv, agg,
-				(struct iwl4965_tx_resp_agg *)tx_resp, index);
-
-		if ((tx_resp->frame_count == 1) &&
-		    !iwl4965_is_tx_success(status)) {
-			/* TODO: send BAR */
-		}
-
-		if (txq->q.read_ptr != (scd_ssn & 0xff)) {
-			int freed;
-			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);
-			freed = iwl4965_tx_queue_reclaim(priv, txq_id, index);
-			priv->stations[sta_id].tid[tid].tfds_in_queue -= freed;
-
-			if (iwl4965_queue_space(&txq->q) > txq->q.low_mark &&
-			    txq_id >= 0 && priv->mac80211_registered &&
-			    agg->state != IWL_EMPTYING_HW_QUEUE_DELBA)
-				ieee80211_wake_queue(priv->hw, txq_id);
-
-			iwl4965_check_empty_hw_queue(priv, sta_id, tid, txq_id);
-		}
-	} else {
-#endif /* CONFIG_IWL4965_HT */
-	tx_status = &(txq->txb[txq->q.read_ptr].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 =
-	    iwl4965_is_tx_success(status) ? IEEE80211_TX_STATUS_ACK : 0;
-	iwl4965_hwrate_to_tx_control(priv, le32_to_cpu(tx_resp->rate_n_flags),
-				     &tx_status->control);
-
-	IWL_DEBUG_TX("Tx queue %d Status %s (0x%08x) rate_n_flags 0x%x "
-		     "retries %d\n", txq_id, iwl4965_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) {
-#ifdef CONFIG_IWL4965_HT
-		int freed = iwl4965_tx_queue_reclaim(priv, txq_id, index);
-
-		if (tid != MAX_TID_COUNT)
-			priv->stations[sta_id].tid[tid].tfds_in_queue -= freed;
-		if (iwl4965_queue_space(&txq->q) > txq->q.low_mark &&
-			(txq_id >= 0) &&
-			priv->mac80211_registered)
-			ieee80211_wake_queue(priv->hw, txq_id);
-		if (tid != MAX_TID_COUNT)
-			iwl4965_check_empty_hw_queue(priv, sta_id, tid, txq_id);
-#endif
-	}
-#ifdef CONFIG_IWL4965_HT
-	}
-#endif /* CONFIG_IWL4965_HT */
-
-	if (iwl_check_bits(status, TX_ABORT_REQUIRED_MSK))
-		IWL_ERROR("TODO:  Implement Tx ABORT REQUIRED!!!\n");
-}
-
-
-static void iwl4965_rx_reply_alive(struct iwl_priv *priv,
-			       struct iwl4965_rx_mem_buffer *rxb)
-{
-	struct iwl4965_rx_packet *pkt = (void *)rxb->skb->data;
-	struct iwl4965_alive_resp *palive;
+	struct iwl_rx_packet *pkt = (struct iwl_rx_packet *)rxb->skb->data;
+	struct iwl_alive_resp *palive;
 	struct delayed_work *pwork;
 
 	palive = &pkt->u.alive_frame;
@@ -3066,12 +1009,12 @@ static void iwl4965_rx_reply_alive(struct iwl_priv *priv,
 		IWL_DEBUG_INFO("Initialization Alive received.\n");
 		memcpy(&priv->card_alive_init,
 		       &pkt->u.alive_frame,
-		       sizeof(struct iwl4965_init_alive_resp));
+		       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 iwl4965_alive_resp));
+		       sizeof(struct iwl_alive_resp));
 		pwork = &priv->alive_start;
 	}
 
@@ -3084,19 +1027,10 @@ static void iwl4965_rx_reply_alive(struct iwl_priv *priv,
 		IWL_WARNING("uCode did not respond OK.\n");
 }
 
-static void iwl4965_rx_reply_add_sta(struct iwl_priv *priv,
-				 struct iwl4965_rx_mem_buffer *rxb)
-{
-	struct iwl4965_rx_packet *pkt = (void *)rxb->skb->data;
-
-	IWL_DEBUG_RX("Received REPLY_ADD_STA: 0x%02X\n", pkt->u.status);
-	return;
-}
-
 static void iwl4965_rx_reply_error(struct iwl_priv *priv,
-			       struct iwl4965_rx_mem_buffer *rxb)
+				   struct iwl_rx_mem_buffer *rxb)
 {
-	struct iwl4965_rx_packet *pkt = (void *)rxb->skb->data;
+	struct iwl_rx_packet *pkt = (struct iwl_rx_packet *)rxb->skb->data;
 
 	IWL_ERROR("Error Reply type 0x%08X cmd %s (0x%02X) "
 		"seq 0x%04X ser 0x%08X\n",
@@ -3109,10 +1043,10 @@ static void iwl4965_rx_reply_error(struct iwl_priv *priv,
 
 #define TX_STATUS_ENTRY(x) case TX_STATUS_FAIL_ ## x: return #x
 
-static void iwl4965_rx_csa(struct iwl_priv *priv, struct iwl4965_rx_mem_buffer *rxb)
+static void iwl4965_rx_csa(struct iwl_priv *priv, struct iwl_rx_mem_buffer *rxb)
 {
-	struct iwl4965_rx_packet *pkt = (void *)rxb->skb->data;
-	struct iwl4965_rxon_cmd *rxon = (void *)&priv->active_rxon;
+	struct iwl_rx_packet *pkt = (struct iwl_rx_packet *)rxb->skb->data;
+	struct iwl_rxon_cmd *rxon = (void *)&priv->active_rxon;
 	struct iwl4965_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));
@@ -3121,15 +1055,15 @@ static void iwl4965_rx_csa(struct iwl_priv *priv, struct iwl4965_rx_mem_buffer *
 }
 
 static void iwl4965_rx_spectrum_measure_notif(struct iwl_priv *priv,
-					  struct iwl4965_rx_mem_buffer *rxb)
+					  struct iwl_rx_mem_buffer *rxb)
 {
 #ifdef CONFIG_IWL4965_SPECTRUM_MEASUREMENT
-	struct iwl4965_rx_packet *pkt = (void *)rxb->skb->data;
+	struct iwl_rx_packet *pkt = (struct iwl_rx_packet *)rxb->skb->data;
 	struct iwl4965_spectrum_notification *report = &(pkt->u.spectrum_notif);
 
 	if (!report->state) {
-		IWL_DEBUG(IWL_DL_11H | IWL_DL_INFO,
-			  "Spectrum Measure Notification: Start\n");
+		IWL_DEBUG(IWL_DL_11H,
+			"Spectrum Measure Notification: Start\n");
 		return;
 	}
 
@@ -3139,10 +1073,10 @@ static void iwl4965_rx_spectrum_measure_notif(struct iwl_priv *priv,
 }
 
 static void iwl4965_rx_pm_sleep_notif(struct iwl_priv *priv,
-				  struct iwl4965_rx_mem_buffer *rxb)
+				      struct iwl_rx_mem_buffer *rxb)
 {
 #ifdef CONFIG_IWLWIFI_DEBUG
-	struct iwl4965_rx_packet *pkt = (void *)rxb->skb->data;
+	struct iwl_rx_packet *pkt = (struct iwl_rx_packet *)rxb->skb->data;
 	struct iwl4965_sleep_notification *sleep = &(pkt->u.sleep_notif);
 	IWL_DEBUG_RX("sleep mode: %d, src: %d\n",
 		     sleep->pm_sleep_mode, sleep->pm_wakeup_src);
@@ -3150,13 +1084,13 @@ static void iwl4965_rx_pm_sleep_notif(struct iwl_priv *priv,
 }
 
 static void iwl4965_rx_pm_debug_statistics_notif(struct iwl_priv *priv,
-					     struct iwl4965_rx_mem_buffer *rxb)
+					     struct iwl_rx_mem_buffer *rxb)
 {
-	struct iwl4965_rx_packet *pkt = (void *)rxb->skb->data;
+	struct iwl_rx_packet *pkt = (struct iwl_rx_packet *)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));
+	iwl_print_hex_dump(priv, IWL_DL_RADIO, pkt->u.raw, le32_to_cpu(pkt->len));
 }
 
 static void iwl4965_bg_beacon_update(struct work_struct *work)
@@ -3166,7 +1100,7 @@ static void iwl4965_bg_beacon_update(struct work_struct *work)
 	struct sk_buff *beacon;
 
 	/* Pull updated AP beacon from mac80211. will fail if not in AP mode */
-	beacon = ieee80211_beacon_get(priv->hw, priv->vif, NULL);
+	beacon = ieee80211_beacon_get(priv->hw, priv->vif);
 
 	if (!beacon) {
 		IWL_ERROR("update beacon failed\n");
@@ -3184,17 +1118,37 @@ static void iwl4965_bg_beacon_update(struct work_struct *work)
 	iwl4965_send_beacon_cmd(priv);
 }
 
+/**
+ * iwl4965_bg_statistics_periodic - Timer callback to queue statistics
+ *
+ * This callback is provided in order to send a statistics request.
+ *
+ * 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.
+ */
+static void iwl4965_bg_statistics_periodic(unsigned long data)
+{
+	struct iwl_priv *priv = (struct iwl_priv *)data;
+
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
+		return;
+
+	iwl_send_statistics_request(priv, CMD_ASYNC);
+}
+
 static void iwl4965_rx_beacon_notif(struct iwl_priv *priv,
-				struct iwl4965_rx_mem_buffer *rxb)
+				struct iwl_rx_mem_buffer *rxb)
 {
 #ifdef CONFIG_IWLWIFI_DEBUG
-	struct iwl4965_rx_packet *pkt = (void *)rxb->skb->data;
+	struct iwl_rx_packet *pkt = (struct iwl_rx_packet *)rxb->skb->data;
 	struct iwl4965_beacon_notif *beacon = &(pkt->u.beacon_status);
-	u8 rate = iwl4965_hw_get_rate(beacon->beacon_notify_hdr.rate_n_flags);
+	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,
+		le32_to_cpu(beacon->beacon_notify_hdr.u.status) & TX_STATUS_MSK,
 		beacon->beacon_notify_hdr.failure_frame,
 		le32_to_cpu(beacon->ibss_mgr_status),
 		le32_to_cpu(beacon->high_tsf),
@@ -3206,129 +1160,12 @@ static void iwl4965_rx_beacon_notif(struct iwl_priv *priv,
 		queue_work(priv->workqueue, &priv->beacon_update);
 }
 
-/* Service response to REPLY_SCAN_CMD (0x80) */
-static void iwl4965_rx_reply_scan(struct iwl_priv *priv,
-			      struct iwl4965_rx_mem_buffer *rxb)
-{
-#ifdef CONFIG_IWLWIFI_DEBUG
-	struct iwl4965_rx_packet *pkt = (void *)rxb->skb->data;
-	struct iwl4965_scanreq_notification *notif =
-	    (struct iwl4965_scanreq_notification *)pkt->u.raw;
-
-	IWL_DEBUG_RX("Scan request status = 0x%x\n", notif->status);
-#endif
-}
-
-/* Service SCAN_START_NOTIFICATION (0x82) */
-static void iwl4965_rx_scan_start_notif(struct iwl_priv *priv,
-				    struct iwl4965_rx_mem_buffer *rxb)
-{
-	struct iwl4965_rx_packet *pkt = (void *)rxb->skb->data;
-	struct iwl4965_scanstart_notification *notif =
-	    (struct iwl4965_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 iwl4965_rx_scan_results_notif(struct iwl_priv *priv,
-				      struct iwl4965_rx_mem_buffer *rxb)
-{
-	struct iwl4965_rx_packet *pkt = (void *)rxb->skb->data;
-	struct iwl4965_scanresults_notification *notif =
-	    (struct iwl4965_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;
-	priv->next_scan_jiffies = 0;
-}
-
-/* Service SCAN_COMPLETE_NOTIFICATION (0x84) */
-static void iwl4965_rx_scan_complete_notif(struct iwl_priv *priv,
-				       struct iwl4965_rx_mem_buffer *rxb)
-{
-	struct iwl4965_rx_packet *pkt = (void *)rxb->skb->data;
-	struct iwl4965_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 & BIT(IEEE80211_BAND_2GHZ)) ?
-						"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, band G precedes A in order of scanning
-	 * as seen in iwl_bg_request_scan */
-	if (priv->scan_bands & BIT(IEEE80211_BAND_2GHZ))
-		priv->scan_bands &= ~BIT(IEEE80211_BAND_2GHZ);
-	else if (priv->scan_bands &  BIT(IEEE80211_BAND_5GHZ))
-		priv->scan_bands &= ~BIT(IEEE80211_BAND_5GHZ);
-
-	/* 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)
-			goto reschedule;
-	}
-
-	priv->last_scan_jiffies = jiffies;
-	priv->next_scan_jiffies = 0;
-	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 iwl4965_rx_card_state_notif(struct iwl_priv *priv,
-				    struct iwl4965_rx_mem_buffer *rxb)
+				    struct iwl_rx_mem_buffer *rxb)
 {
-	struct iwl4965_rx_packet *pkt = (void *)rxb->skb->data;
+	struct iwl_rx_packet *pkt = (struct iwl_rx_packet *)rxb->skb->data;
 	u32 flags = le32_to_cpu(pkt->u.card_state_notif.flags);
 	unsigned long status = priv->status;
 
@@ -3383,7 +1220,7 @@ static void iwl4965_rx_card_state_notif(struct iwl_priv *priv,
 		clear_bit(STATUS_RF_KILL_SW, &priv->status);
 
 	if (!(flags & RXON_CARD_DISABLED))
-		iwl4965_scan_cancel(priv);
+		iwl_scan_cancel(priv);
 
 	if ((test_bit(STATUS_RF_KILL_HW, &status) !=
 	     test_bit(STATUS_RF_KILL_HW, &priv->status)) ||
@@ -3403,10 +1240,9 @@ static void iwl4965_rx_card_state_notif(struct iwl_priv *priv,
  * This function chains into the hardware specific files for them to setup
  * any hardware specific handlers as well.
  */
-static void iwl4965_setup_rx_handlers(struct iwl_priv *priv)
+static void iwl_setup_rx_handlers(struct iwl_priv *priv)
 {
-	priv->rx_handlers[REPLY_ALIVE] = iwl4965_rx_reply_alive;
-	priv->rx_handlers[REPLY_ADD_STA] = iwl4965_rx_reply_add_sta;
+	priv->rx_handlers[REPLY_ALIVE] = iwl_rx_reply_alive;
 	priv->rx_handlers[REPLY_ERROR] = iwl4965_rx_reply_error;
 	priv->rx_handlers[CHANNEL_SWITCH_NOTIFICATION] = iwl4965_rx_csa;
 	priv->rx_handlers[SPECTRUM_MEASURE_NOTIFICATION] =
@@ -3421,500 +1257,47 @@ static void iwl4965_setup_rx_handlers(struct iwl_priv *priv)
 	 * statistics request from the host as well as for the periodic
 	 * statistics notifications (after received beacons) from the uCode.
 	 */
-	priv->rx_handlers[REPLY_STATISTICS_CMD] = iwl4965_hw_rx_statistics;
-	priv->rx_handlers[STATISTICS_NOTIFICATION] = iwl4965_hw_rx_statistics;
-
-	priv->rx_handlers[REPLY_SCAN_CMD] = iwl4965_rx_reply_scan;
-	priv->rx_handlers[SCAN_START_NOTIFICATION] = iwl4965_rx_scan_start_notif;
-	priv->rx_handlers[SCAN_RESULTS_NOTIFICATION] =
-	    iwl4965_rx_scan_results_notif;
-	priv->rx_handlers[SCAN_COMPLETE_NOTIFICATION] =
-	    iwl4965_rx_scan_complete_notif;
-	priv->rx_handlers[CARD_STATE_NOTIFICATION] = iwl4965_rx_card_state_notif;
-	priv->rx_handlers[REPLY_TX] = iwl4965_rx_reply_tx;
-
-	/* Set up hardware specific Rx handlers */
-	iwl4965_hw_rx_handler_setup(priv);
-}
-
-/**
- * iwl4965_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 iwl4965_tx_cmd_complete(struct iwl_priv *priv,
-				struct iwl4965_rx_mem_buffer *rxb)
-{
-	struct iwl4965_rx_packet *pkt = (struct iwl4965_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;
-
-	iwl4965_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
- *
- * Driver allocates a circular buffer of Receive Buffer Descriptors (RBDs),
- * each of which point to Receive Buffers to be filled by 4965.  These get
- * used not only for Rx frames, but for any command response or notification
- * from the 4965.  The driver and 4965 manage the Rx buffers by means
- * of indexes into the circular buffer.
- *
- * 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 replenish the iwl->rxq->rx_free.
- * + In iwl4965_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:
- *
- * iwl4965_rx_queue_alloc()   Allocates rx_free
- * iwl4965_rx_replenish()     Replenishes rx_free list from rx_used, and calls
- *                            iwl4965_rx_queue_restock
- * iwl4965_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 iwl4965_rx_replenish
- *
- * -- enable interrupts --
- * ISR - iwl4965_rx()         Detach iwl4965_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 iwl4965_rx_queue_restock to refill any empty
- *                            slots.
- * ...
- *
- */
-
-/**
- * iwl4965_rx_queue_space - Return number of free slots available in queue.
- */
-static int iwl4965_rx_queue_space(const struct iwl4965_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;
-}
-
-/**
- * iwl4965_rx_queue_update_write_ptr - Update the write pointer for the RX queue
- */
-int iwl4965_rx_queue_update_write_ptr(struct iwl_priv *priv, struct iwl4965_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 power-saving is in use, make sure device is awake */
-	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_nic_access(priv);
-		if (rc)
-			goto exit_unlock;
-
-		/* Device expects a multiple of 8 */
-		iwl_write_direct32(priv, FH_RSCSR_CHNL0_WPTR,
-				     q->write & ~0x7);
-		iwl_release_nic_access(priv);
-
-	/* Else device is assumed to be awake */
-	} else
-		/* Device expects a multiple of 8 */
-		iwl_write32(priv, FH_RSCSR_CHNL0_WPTR, q->write & ~0x7);
+	priv->rx_handlers[REPLY_STATISTICS_CMD] = iwl_rx_statistics;
+	priv->rx_handlers[STATISTICS_NOTIFICATION] = iwl_rx_statistics;
 
+	iwl_setup_rx_scan_handlers(priv);
 
-	q->need_update = 0;
-
- exit_unlock:
-	spin_unlock_irqrestore(&q->lock, flags);
-	return rc;
-}
-
-/**
- * iwl4965_dma_addr2rbd_ptr - convert a DMA address to a uCode read buffer ptr
- */
-static inline __le32 iwl4965_dma_addr2rbd_ptr(struct iwl_priv *priv,
-					  dma_addr_t dma_addr)
-{
-	return cpu_to_le32((u32)(dma_addr >> 8));
-}
-
-
-/**
- * iwl4965_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.
- */
-static int iwl4965_rx_queue_restock(struct iwl_priv *priv)
-{
-	struct iwl4965_rx_queue *rxq = &priv->rxq;
-	struct list_head *element;
-	struct iwl4965_rx_mem_buffer *rxb;
-	unsigned long flags;
-	int write, rc;
-
-	spin_lock_irqsave(&rxq->lock, flags);
-	write = rxq->write & ~0x7;
-	while ((iwl4965_rx_queue_space(rxq) > 0) && (rxq->free_count)) {
-		/* Get next free Rx buffer, remove from free list */
-		element = rxq->rx_free.next;
-		rxb = list_entry(element, struct iwl4965_rx_mem_buffer, list);
-		list_del(element);
-
-		/* Point to Rx buffer via next RBD in circular buffer */
-		rxq->bd[rxq->write] = iwl4965_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.
-	 * Increment device's write pointer in multiples of 8. */
-	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 = iwl4965_rx_queue_update_write_ptr(priv, rxq);
-		if (rc)
-			return rc;
-	}
-
-	return 0;
-}
-
-/**
- * iwl4965_rx_replenish - 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 iwl4965_rx_queue_restock.
- * This is called as a scheduled work item (except for during initialization)
- */
-static void iwl4965_rx_allocate(struct iwl_priv *priv)
-{
-	struct iwl4965_rx_queue *rxq = &priv->rxq;
-	struct list_head *element;
-	struct iwl4965_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 iwl4965_rx_mem_buffer, list);
-
-		/* Alloc a new receive buffer */
-		rxb->skb =
-		    alloc_skb(priv->hw_params.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);
+	/* status change handler */
+	priv->rx_handlers[CARD_STATE_NOTIFICATION] = iwl4965_rx_card_state_notif;
 
-		/* Get physical address of RB/SKB */
-		rxb->dma_addr =
-		    pci_map_single(priv->pci_dev, rxb->skb->data,
-			   priv->hw_params.rx_buf_size, PCI_DMA_FROMDEVICE);
-		list_add_tail(&rxb->list, &rxq->rx_free);
-		rxq->free_count++;
-	}
-	spin_unlock_irqrestore(&rxq->lock, flags);
+	priv->rx_handlers[MISSED_BEACONS_NOTIFICATION] =
+	    iwl_rx_missed_beacon_notif;
+	/* Rx handlers */
+	priv->rx_handlers[REPLY_RX_PHY_CMD] = iwl_rx_reply_rx_phy;
+	priv->rx_handlers[REPLY_RX_MPDU_CMD] = iwl_rx_reply_rx;
+	/* block ack */
+	priv->rx_handlers[REPLY_COMPRESSED_BA] = iwl_rx_reply_compressed_ba;
+	/* Set up hardware specific Rx handlers */
+	priv->cfg->ops->lib->rx_handler_setup(priv);
 }
 
 /*
  * this should be called while priv->lock is locked
 */
-static void __iwl4965_rx_replenish(void *data)
+static void __iwl_rx_replenish(struct iwl_priv *priv)
 {
-	struct iwl_priv *priv = data;
-
-	iwl4965_rx_allocate(priv);
-	iwl4965_rx_queue_restock(priv);
+	iwl_rx_allocate(priv);
+	iwl_rx_queue_restock(priv);
 }
 
 
-void iwl4965_rx_replenish(void *data)
-{
-	struct iwl_priv *priv = data;
-	unsigned long flags;
-
-	iwl4965_rx_allocate(priv);
-
-	spin_lock_irqsave(&priv->lock, flags);
-	iwl4965_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 its 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
- */
-static void iwl4965_rx_queue_free(struct iwl_priv *priv, struct iwl4965_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,
-					 priv->hw_params.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 iwl4965_rx_queue_alloc(struct iwl_priv *priv)
-{
-	struct iwl4965_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);
-
-	/* Alloc the circular buffer of Read Buffer Descriptors (RBDs) */
-	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 iwl4965_rx_queue_reset(struct iwl_priv *priv, struct iwl4965_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,
-					 priv->hw_params.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 iwl4965_calc_db_from_ratio(int sig_ratio)
-{
-	/* 1000:1 or higher just report as 60 dB */
-	if (sig_ratio >= 1000)
-		return 60;
-
-	/* 100:1 or higher, 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 iwl4965_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;
-}
-
 /**
- * iwl4965_rx_handle - Main entry function for receiving responses from uCode
+ * iwl_rx_handle - Main entry function for receiving responses from 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 iwl4965_rx_handle(struct iwl_priv *priv)
+void iwl_rx_handle(struct iwl_priv *priv)
 {
-	struct iwl4965_rx_mem_buffer *rxb;
-	struct iwl4965_rx_packet *pkt;
-	struct iwl4965_rx_queue *rxq = &priv->rxq;
+	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;
@@ -3923,14 +1306,14 @@ static void iwl4965_rx_handle(struct iwl_priv *priv)
 
 	/* uCode's read index (stored in shared DRAM) indicates the last Rx
 	 * buffer that the driver may process (last buffer filled by ucode). */
-	r = iwl4965_hw_get_rx_read(priv);
+	r = priv->cfg->ops->lib->shared_mem_rx_idx(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);
+		IWL_DEBUG(IWL_DL_RX, "r = %d, i = %d\n", r, i);
 
-	if (iwl4965_rx_queue_space(rxq) > (RX_QUEUE_SIZE / 2))
+	if (iwl_rx_queue_space(rxq) > (RX_QUEUE_SIZE / 2))
 		fill_rx = 1;
 
 	while (i != r) {
@@ -3946,7 +1329,7 @@ static void iwl4965_rx_handle(struct iwl_priv *priv)
 		pci_dma_sync_single_for_cpu(priv->pci_dev, rxb->dma_addr,
 					    priv->hw_params.rx_buf_size,
 					    PCI_DMA_FROMDEVICE);
-		pkt = (struct iwl4965_rx_packet *)rxb->skb->data;
+		pkt = (struct iwl_rx_packet *)rxb->skb->data;
 
 		/* Reclaim a command buffer only if this packet is a response
 		 *   to a (driver-originated) command.
@@ -3965,13 +1348,12 @@ static void iwl4965_rx_handle(struct iwl_priv *priv)
 		 *   handle those that need handling via function in
 		 *   rx_handlers table.  See iwl4965_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);
+			IWL_DEBUG(IWL_DL_RX, "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,
+			IWL_DEBUG(IWL_DL_RX,
 				"r %d i %d No handler needed for %s, 0x%02x\n",
 				r, i, get_cmd_string(pkt->hdr.cmd),
 				pkt->hdr.cmd);
@@ -3982,7 +1364,7 @@ static void iwl4965_rx_handle(struct iwl_priv *priv)
 			 * fire off the (possibly) blocking iwl_send_cmd()
 			 * as we reclaim the driver command queue */
 			if (rxb && rxb->skb)
-				iwl4965_tx_cmd_complete(priv, rxb);
+				iwl_tx_cmd_complete(priv, rxb);
 			else
 				IWL_WARNING("Claim null rxb?\n");
 		}
@@ -4009,7 +1391,7 @@ static void iwl4965_rx_handle(struct iwl_priv *priv)
 			count++;
 			if (count >= 8) {
 				priv->rxq.read = i;
-				__iwl4965_rx_replenish(priv);
+				__iwl_rx_replenish(priv);
 				count = 0;
 			}
 		}
@@ -4017,62 +1399,17 @@ static void iwl4965_rx_handle(struct iwl_priv *priv)
 
 	/* Backtrack one entry */
 	priv->rxq.read = i;
-	iwl4965_rx_queue_restock(priv);
-}
-
-/**
- * iwl4965_tx_queue_update_write_ptr - Send new write index to hardware
- */
-static int iwl4965_tx_queue_update_write_ptr(struct iwl_priv *priv,
-				  struct iwl4965_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_nic_access(priv);
-		if (rc)
-			return rc;
-		iwl_write_direct32(priv, HBUS_TARG_WRPTR,
-				     txq->q.write_ptr | (txq_id << 8));
-		iwl_release_nic_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.write_ptr | (txq_id << 8));
-
-	txq->need_update = 0;
-
-	return rc;
+	iwl_rx_queue_restock(priv);
 }
 
 #ifdef CONFIG_IWLWIFI_DEBUG
-static void iwl4965_print_rx_config_cmd(struct iwl4965_rxon_cmd *rxon)
+static void iwl4965_print_rx_config_cmd(struct iwl_priv *priv)
 {
+	struct iwl_rxon_cmd *rxon = &priv->staging_rxon;
 	DECLARE_MAC_BUF(mac);
 
 	IWL_DEBUG_RADIO("RX CONFIG:\n");
-	iwl_print_hex_dump(IWL_DL_RADIO, (u8 *) rxon, sizeof(*rxon));
+	iwl_print_hex_dump(priv, 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",
@@ -4118,173 +1455,6 @@ static inline void iwl4965_disable_interrupts(struct iwl_priv *priv)
 	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 iwl4965_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 (!priv->cfg->ops->lib->is_valid_rtc_data_addr(base)) {
-		IWL_ERROR("Not valid error log pointer 0x%08X\n", base);
-		return;
-	}
-
-	rc = iwl_grab_nic_access(priv);
-	if (rc) {
-		IWL_WARNING("Can not read from adapter at this time.\n");
-		return;
-	}
-
-	count = iwl_read_targ_mem(priv, base);
-
-	if (ERROR_START_OFFSET <= count * ERROR_ELEM_SIZE) {
-		IWL_ERROR("Start IWL Error Log Dump:\n");
-		IWL_ERROR("Status: 0x%08lX, count: %d\n", priv->status, count);
-	}
-
-	desc = iwl_read_targ_mem(priv, base + 1 * sizeof(u32));
-	blink1 = iwl_read_targ_mem(priv, base + 3 * sizeof(u32));
-	blink2 = iwl_read_targ_mem(priv, base + 4 * sizeof(u32));
-	ilink1 = iwl_read_targ_mem(priv, base + 5 * sizeof(u32));
-	ilink2 = iwl_read_targ_mem(priv, base + 6 * sizeof(u32));
-	data1 = iwl_read_targ_mem(priv, base + 7 * sizeof(u32));
-	data2 = iwl_read_targ_mem(priv, base + 8 * sizeof(u32));
-	line = iwl_read_targ_mem(priv, base + 9 * sizeof(u32));
-	time = iwl_read_targ_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_nic_access(priv);
-}
-
-#define EVENT_START_OFFSET  (4 * sizeof(u32))
-
-/**
- * iwl4965_print_event_log - Dump error event log to syslog
- *
- * NOTE: Must be called with iwl_grab_nic_access() already obtained!
- */
-static void iwl4965_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_targ_mem(priv, ptr);
-		ptr += sizeof(u32);
-		time = iwl_read_targ_mem(priv, ptr);
-		ptr += sizeof(u32);
-		if (mode == 0)
-			IWL_ERROR("0x%08x\t%04u\n", time, ev); /* data, ev */
-		else {
-			data = iwl_read_targ_mem(priv, ptr);
-			ptr += sizeof(u32);
-			IWL_ERROR("%010u\t0x%08x\t%04u\n", time, data, ev);
-		}
-	}
-}
-
-static void iwl4965_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 (!priv->cfg->ops->lib->is_valid_rtc_data_addr(base)) {
-		IWL_ERROR("Invalid event log pointer 0x%08X\n", base);
-		return;
-	}
-
-	rc = iwl_grab_nic_access(priv);
-	if (rc) {
-		IWL_WARNING("Can not read from adapter at this time.\n");
-		return;
-	}
-
-	/* event log header */
-	capacity = iwl_read_targ_mem(priv, base);
-	mode = iwl_read_targ_mem(priv, base + (1 * sizeof(u32)));
-	num_wraps = iwl_read_targ_mem(priv, base + (2 * sizeof(u32)));
-	next_entry = iwl_read_targ_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_nic_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)
-		iwl4965_print_event_log(priv, next_entry,
-				    capacity - next_entry, mode);
-
-	/* (then/else) start at top of log */
-	iwl4965_print_event_log(priv, 0, next_entry, mode);
-
-	iwl_release_nic_access(priv);
-}
 
 /**
  * iwl4965_irq_handle_error - called for HW or SW error interrupt from card
@@ -4298,10 +1468,10 @@ static void iwl4965_irq_handle_error(struct iwl_priv *priv)
 	clear_bit(STATUS_HCMD_ACTIVE, &priv->status);
 
 #ifdef CONFIG_IWLWIFI_DEBUG
-	if (iwl_debug_level & IWL_DL_FW_ERRORS) {
-		iwl4965_dump_nic_error_log(priv);
-		iwl4965_dump_nic_event_log(priv);
-		iwl4965_print_rx_config_cmd(&priv->staging_rxon);
+	if (priv->debug_level & IWL_DL_FW_ERRORS) {
+		iwl_dump_nic_error_log(priv);
+		iwl_dump_nic_event_log(priv);
+		iwl4965_print_rx_config_cmd(priv);
 	}
 #endif
 
@@ -4312,7 +1482,7 @@ static void iwl4965_irq_handle_error(struct iwl_priv *priv)
 	clear_bit(STATUS_READY, &priv->status);
 
 	if (!test_bit(STATUS_EXIT_PENDING, &priv->status)) {
-		IWL_DEBUG(IWL_DL_INFO | IWL_DL_FW_ERRORS,
+		IWL_DEBUG(IWL_DL_FW_ERRORS,
 			  "Restarting adapter due to uCode error.\n");
 
 		if (iwl_is_associated(priv)) {
@@ -4320,7 +1490,8 @@ static void iwl4965_irq_handle_error(struct iwl_priv *priv)
 			       sizeof(priv->recovery_rxon));
 			priv->error_recovering = 1;
 		}
-		queue_work(priv->workqueue, &priv->restart);
+		if (priv->cfg->mod_params->restart_fw)
+			queue_work(priv->workqueue, &priv->restart);
 	}
 }
 
@@ -4333,7 +1504,7 @@ static void iwl4965_error_recovery(struct iwl_priv *priv)
 	priv->staging_rxon.filter_flags &= ~RXON_FILTER_ASSOC_MSK;
 	iwl4965_commit_rxon(priv);
 
-	iwl4965_rxon_add_station(priv, priv->bssid, 1);
+	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);
@@ -4365,7 +1536,7 @@ static void iwl4965_irq_tasklet(struct iwl_priv *priv)
 	iwl_write32(priv, CSR_FH_INT_STATUS, inta_fh);
 
 #ifdef CONFIG_IWLWIFI_DEBUG
-	if (iwl_debug_level & IWL_DL_ISR) {
+	if (priv->debug_level & IWL_DL_ISR) {
 		/* just for debug */
 		inta_mask = iwl_read32(priv, CSR_INT_MASK);
 		IWL_DEBUG_ISR("inta 0x%08x, enabled 0x%08x, fh 0x%08x\n",
@@ -4399,7 +1570,7 @@ static void iwl4965_irq_tasklet(struct iwl_priv *priv)
 	}
 
 #ifdef CONFIG_IWLWIFI_DEBUG
-	if (iwl_debug_level & (IWL_DL_ISR)) {
+	if (priv->debug_level & (IWL_DL_ISR)) {
 		/* NIC fires this, but we don't use it, redundant with WAKEUP */
 		if (inta & CSR_INT_BIT_SCD)
 			IWL_DEBUG_ISR("Scheduler finished to transmit "
@@ -4420,18 +1591,15 @@ static void iwl4965_irq_tasklet(struct iwl_priv *priv)
 				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",
+		IWL_DEBUG(IWL_DL_RF_KILL, "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
-		 *   iwl4965_rx_card_state_notif() */
-		if (!hw_rf_kill && !test_bit(STATUS_ALIVE, &priv->status)) {
+		/* driver only loads ucode once setting the interface up.
+		 * the driver as well won't allow loading if RFKILL is set
+		 * therefore no need to restart the driver from this handler
+		 */
+		if (!hw_rf_kill && !test_bit(STATUS_ALIVE, &priv->status))
 			clear_bit(STATUS_RF_KILL_HW, &priv->status);
-			queue_work(priv->workqueue, &priv->restart);
-		}
 
 		handled |= CSR_INT_BIT_RF_KILL;
 	}
@@ -4453,13 +1621,13 @@ static void iwl4965_irq_tasklet(struct iwl_priv *priv)
 	/* uCode wakes up after power-down sleep */
 	if (inta & CSR_INT_BIT_WAKEUP) {
 		IWL_DEBUG_ISR("Wakeup interrupt\n");
-		iwl4965_rx_queue_update_write_ptr(priv, &priv->rxq);
-		iwl4965_tx_queue_update_write_ptr(priv, &priv->txq[0]);
-		iwl4965_tx_queue_update_write_ptr(priv, &priv->txq[1]);
-		iwl4965_tx_queue_update_write_ptr(priv, &priv->txq[2]);
-		iwl4965_tx_queue_update_write_ptr(priv, &priv->txq[3]);
-		iwl4965_tx_queue_update_write_ptr(priv, &priv->txq[4]);
-		iwl4965_tx_queue_update_write_ptr(priv, &priv->txq[5]);
+		iwl_rx_queue_update_write_ptr(priv, &priv->rxq);
+		iwl_txq_update_write_ptr(priv, &priv->txq[0]);
+		iwl_txq_update_write_ptr(priv, &priv->txq[1]);
+		iwl_txq_update_write_ptr(priv, &priv->txq[2]);
+		iwl_txq_update_write_ptr(priv, &priv->txq[3]);
+		iwl_txq_update_write_ptr(priv, &priv->txq[4]);
+		iwl_txq_update_write_ptr(priv, &priv->txq[5]);
 
 		handled |= CSR_INT_BIT_WAKEUP;
 	}
@@ -4468,13 +1636,16 @@ static void iwl4965_irq_tasklet(struct iwl_priv *priv)
 	 * 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)) {
-		iwl4965_rx_handle(priv);
+		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;
+		/* FH finished to write, send event */
+		priv->ucode_write_complete = 1;
+		wake_up_interruptible(&priv->wait_command_queue);
 	}
 
 	if (inta & ~handled)
@@ -4492,7 +1663,7 @@ static void iwl4965_irq_tasklet(struct iwl_priv *priv)
 		iwl4965_enable_interrupts(priv);
 
 #ifdef CONFIG_IWLWIFI_DEBUG
-	if (iwl_debug_level & (IWL_DL_ISR)) {
+	if (priv->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);
@@ -4561,297 +1732,6 @@ static irqreturn_t iwl4965_isr(int irq, void *data)
 	return IRQ_NONE;
 }
 
-/* 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 iwl4965_get_active_dwell_time(struct iwl_priv *priv,
-						enum ieee80211_band band)
-{
-	if (band == IEEE80211_BAND_5GHZ)
-		return IWL_ACTIVE_DWELL_TIME_52;
-	else
-		return IWL_ACTIVE_DWELL_TIME_24;
-}
-
-static u16 iwl4965_get_passive_dwell_time(struct iwl_priv *priv,
-					  enum ieee80211_band band)
-{
-	u16 active = iwl4965_get_active_dwell_time(priv, band);
-	u16 passive = (band != IEEE80211_BAND_5GHZ) ?
-	    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 iwl4965_get_channels_for_scan(struct iwl_priv *priv,
-					 enum ieee80211_band band,
-				     u8 is_active, u8 direct_mask,
-				     struct iwl4965_scan_channel *scan_ch)
-{
-	const struct ieee80211_channel *channels = NULL;
-	const struct ieee80211_supported_band *sband;
-	const struct iwl_channel_info *ch_info;
-	u16 passive_dwell = 0;
-	u16 active_dwell = 0;
-	int added, i;
-
-	sband = iwl4965_get_hw_mode(priv, band);
-	if (!sband)
-		return 0;
-
-	channels = sband->channels;
-
-	active_dwell = iwl4965_get_active_dwell_time(priv, band);
-	passive_dwell = iwl4965_get_passive_dwell_time(priv, band);
-
-	for (i = 0, added = 0; i < sband->n_channels; i++) {
-		if (channels[i].flags & IEEE80211_CHAN_DISABLED)
-			continue;
-
-		scan_ch->channel = ieee80211_frequency_to_channel(channels[i].center_freq);
-
-		ch_info = iwl_get_channel_info(priv, band, scan_ch->channel);
-		if (!is_channel_valid(ch_info)) {
-			IWL_DEBUG_SCAN("Channel %d is INVALID for this band.\n",
-				       scan_ch->channel);
-			continue;
-		}
-
-		if (!is_active || is_channel_passive(ch_info) ||
-		    (channels[i].flags & IEEE80211_CHAN_PASSIVE_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 txpower levels to defaults */
-		scan_ch->tpc.dsp_atten = 110;
-		/* scan_pwr_info->tpc.dsp_atten; */
-
-		/*scan_pwr_info->tpc.tx_gain; */
-		if (band == IEEE80211_BAND_5GHZ)
-			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 iwl4965_init_hw_rates(struct iwl_priv *priv,
-			      struct ieee80211_rate *rates)
-{
-	int i;
-
-	for (i = 0; i < IWL_RATE_COUNT; i++) {
-		rates[i].bitrate = iwl4965_rates[i].ieee * 5;
-		rates[i].hw_value = i; /* Rate scaling will work on indexes */
-		rates[i].hw_value_short = i;
-		rates[i].flags = 0;
-		if ((i > IWL_LAST_OFDM_RATE) || (i < IWL_FIRST_OFDM_RATE)) {
-			/*
-			 * If CCK != 1M then set short preamble rate flag.
-			 */
-			rates[i].flags |=
-				(iwl4965_rates[i].plcp == IWL_RATE_1M_PLCP) ?
-					0 : IEEE80211_RATE_SHORT_PREAMBLE;
-		}
-	}
-}
-
-/**
- * iwl4965_init_geos - Initialize mac80211's geo/channel info based from eeprom
- */
-int iwl4965_init_geos(struct iwl_priv *priv)
-{
-	struct iwl_channel_info *ch;
-	struct ieee80211_supported_band *sband;
-	struct ieee80211_channel *channels;
-	struct ieee80211_channel *geo_ch;
-	struct ieee80211_rate *rates;
-	int i = 0;
-
-	if (priv->bands[IEEE80211_BAND_2GHZ].n_bitrates ||
-	    priv->bands[IEEE80211_BAND_5GHZ].n_bitrates) {
-		IWL_DEBUG_INFO("Geography modes already initialized.\n");
-		set_bit(STATUS_GEO_CONFIGURED, &priv->status);
-		return 0;
-	}
-
-	channels = kzalloc(sizeof(struct ieee80211_channel) *
-			   priv->channel_count, GFP_KERNEL);
-	if (!channels)
-		return -ENOMEM;
-
-	rates = kzalloc((sizeof(struct ieee80211_rate) * (IWL_RATE_COUNT + 1)),
-			GFP_KERNEL);
-	if (!rates) {
-		kfree(channels);
-		return -ENOMEM;
-	}
-
-	/* 5.2GHz channels start after the 2.4GHz channels */
-	sband = &priv->bands[IEEE80211_BAND_5GHZ];
-	sband->channels = &channels[ARRAY_SIZE(iwl_eeprom_band_1)];
-	/* just OFDM */
-	sband->bitrates = &rates[IWL_FIRST_OFDM_RATE];
-	sband->n_bitrates = IWL_RATE_COUNT - IWL_FIRST_OFDM_RATE;
-
-	iwl4965_init_ht_hw_capab(priv, &sband->ht_info, IEEE80211_BAND_5GHZ);
-
-	sband = &priv->bands[IEEE80211_BAND_2GHZ];
-	sband->channels = channels;
-	/* OFDM & CCK */
-	sband->bitrates = rates;
-	sband->n_bitrates = IWL_RATE_COUNT;
-
-	iwl4965_init_ht_hw_capab(priv, &sband->ht_info, IEEE80211_BAND_2GHZ);
-
-	priv->ieee_channels = channels;
-	priv->ieee_rates = rates;
-
-	iwl4965_init_hw_rates(priv, rates);
-
-	for (i = 0;  i < priv->channel_count; i++) {
-		ch = &priv->channel_info[i];
-
-		/* FIXME: might be removed if scan is OK */
-		if (!is_channel_valid(ch))
-			continue;
-
-		if (is_channel_a_band(ch))
-			sband =  &priv->bands[IEEE80211_BAND_5GHZ];
-		else
-			sband =  &priv->bands[IEEE80211_BAND_2GHZ];
-
-		geo_ch = &sband->channels[sband->n_channels++];
-
-		geo_ch->center_freq = ieee80211_channel_to_frequency(ch->channel);
-		geo_ch->max_power = ch->max_power_avg;
-		geo_ch->max_antenna_gain = 0xff;
-		geo_ch->hw_value = ch->channel;
-
-		if (is_channel_valid(ch)) {
-			if (!(ch->flags & EEPROM_CHANNEL_IBSS))
-				geo_ch->flags |= IEEE80211_CHAN_NO_IBSS;
-
-			if (!(ch->flags & EEPROM_CHANNEL_ACTIVE))
-				geo_ch->flags |= IEEE80211_CHAN_PASSIVE_SCAN;
-
-			if (ch->flags & EEPROM_CHANNEL_RADAR)
-				geo_ch->flags |= IEEE80211_CHAN_RADAR;
-
-			if (ch->max_power_avg > priv->max_channel_txpower_limit)
-				priv->max_channel_txpower_limit =
-				    ch->max_power_avg;
-		} else {
-			geo_ch->flags |= IEEE80211_CHAN_DISABLED;
-		}
-
-		/* Save flags for reg domain usage */
-		geo_ch->orig_flags = geo_ch->flags;
-
-		IWL_DEBUG_INFO("Channel %d Freq=%d[%sGHz] %s flag=0%X\n",
-				ch->channel, geo_ch->center_freq,
-				is_channel_a_band(ch) ?  "5.2" : "2.4",
-				geo_ch->flags & IEEE80211_CHAN_DISABLED ?
-				"restricted" : "valid",
-				 geo_ch->flags);
-	}
-
-	if ((priv->bands[IEEE80211_BAND_5GHZ].n_channels == 0) &&
-	     priv->cfg->sku & IWL_SKU_A) {
-		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->cfg->sku &= ~IWL_SKU_A;
-	}
-
-	printk(KERN_INFO DRV_NAME
-	       ": Tunable channels: %d 802.11bg, %d 802.11a channels\n",
-	       priv->bands[IEEE80211_BAND_2GHZ].n_channels,
-	       priv->bands[IEEE80211_BAND_5GHZ].n_channels);
-
-	if (priv->bands[IEEE80211_BAND_2GHZ].n_channels)
-		priv->hw->wiphy->bands[IEEE80211_BAND_2GHZ] =
-			&priv->bands[IEEE80211_BAND_2GHZ];
-	if (priv->bands[IEEE80211_BAND_5GHZ].n_channels)
-		priv->hw->wiphy->bands[IEEE80211_BAND_5GHZ] =
-			&priv->bands[IEEE80211_BAND_5GHZ];
-
-	set_bit(STATUS_GEO_CONFIGURED, &priv->status);
-
-	return 0;
-}
-
-/*
- * iwl4965_free_geos - undo allocations in iwl4965_init_geos
- */
-void iwl4965_free_geos(struct iwl_priv *priv)
-{
-	kfree(priv->ieee_channels);
-	kfree(priv->ieee_rates);
-	clear_bit(STATUS_GEO_CONFIGURED, &priv->status);
-}
-
 /******************************************************************************
  *
  * uCode download functions
@@ -4868,146 +1748,6 @@ static void iwl4965_dealloc_ucode_pci(struct iwl_priv *priv)
 	iwl_free_fw_desc(priv->pci_dev, &priv->ucode_boot);
 }
 
-/**
- * iwl4965_verify_inst_full - verify runtime uCode image in card vs. host,
- *     looking at all data.
- */
-static int iwl4965_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_nic_access(priv);
-	if (rc)
-		return rc;
-
-	iwl_write_direct32(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_direct32(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_nic_access(priv);
-
-	if (!errcnt)
-		IWL_DEBUG_INFO
-		    ("ucode image in INSTRUCTION memory is good\n");
-
-	return rc;
-}
-
-
-/**
- * iwl4965_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 iwl4965_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_nic_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_direct32(priv, HBUS_TARG_MEM_RADDR,
-			i + RTC_INST_LOWER_BOUND);
-		val = _iwl_read_direct32(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_nic_access(priv);
-
-	return rc;
-}
-
-
-/**
- * iwl4965_verify_ucode - determine which instruction image is in SRAM,
- *    and verify its contents
- */
-static int iwl4965_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 = iwl4965_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 = iwl4965_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 = iwl4965_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");
-
-	/* Since nothing seems to match, show first several data entries in
-	 * instruction SRAM, so maybe visual inspection will give a clue.
-	 * Selection of bootstrap image (vs. other images) is arbitrary. */
-	image = (__le32 *)priv->ucode_boot.v_addr;
-	len = priv->ucode_boot.len;
-	rc = iwl4965_verify_inst_full(priv, image, len);
-
-	return rc;
-}
-
 static void iwl4965_nic_start(struct iwl_priv *priv)
 {
 	/* Remove all resets to allow NIC to operate */
@@ -5022,7 +1762,7 @@ static void iwl4965_nic_start(struct iwl_priv *priv)
  */
 static int iwl4965_read_ucode(struct iwl_priv *priv)
 {
-	struct iwl4965_ucode *ucode;
+	struct iwl_ucode *ucode;
 	int ret;
 	const struct firmware *ucode_raw;
 	const char *name = priv->cfg->fw_name;
@@ -5083,34 +1823,34 @@ static int iwl4965_read_ucode(struct iwl_priv *priv)
 	}
 
 	/* Verify that uCode images will fit in card's SRAM */
-	if (inst_size > IWL_MAX_INST_SIZE) {
+	if (inst_size > priv->hw_params.max_inst_size) {
 		IWL_DEBUG_INFO("uCode instr len %d too large to fit in\n",
 			       inst_size);
 		ret = -EINVAL;
 		goto err_release;
 	}
 
-	if (data_size > IWL_MAX_DATA_SIZE) {
+	if (data_size > priv->hw_params.max_data_size) {
 		IWL_DEBUG_INFO("uCode data len %d too large to fit in\n",
 				data_size);
 		ret = -EINVAL;
 		goto err_release;
 	}
-	if (init_size > IWL_MAX_INST_SIZE) {
+	if (init_size > priv->hw_params.max_inst_size) {
 		IWL_DEBUG_INFO
 		    ("uCode init instr len %d too large to fit in\n",
 		      init_size);
 		ret = -EINVAL;
 		goto err_release;
 	}
-	if (init_data_size > IWL_MAX_DATA_SIZE) {
+	if (init_data_size > priv->hw_params.max_data_size) {
 		IWL_DEBUG_INFO
 		    ("uCode init data len %d too large to fit in\n",
 		      init_data_size);
 		ret = -EINVAL;
 		goto err_release;
 	}
-	if (boot_size > IWL_MAX_BSM_SIZE) {
+	if (boot_size > priv->hw_params.max_bsm_size) {
 		IWL_DEBUG_INFO
 		    ("uCode boot instr len %d too large to fit in\n",
 		      boot_size);
@@ -5211,111 +1951,12 @@ static int iwl4965_read_ucode(struct iwl_priv *priv)
 	return ret;
 }
 
-
-/**
- * iwl4965_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 iwl4965_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_nic_access(priv);
-	if (rc) {
-		spin_unlock_irqrestore(&priv->lock, flags);
-		return rc;
-	}
-
-	/* Tell bootstrap uCode where to find image to load */
-	iwl_write_prph(priv, BSM_DRAM_INST_PTR_REG, pinst);
-	iwl_write_prph(priv, BSM_DRAM_DATA_PTR_REG, pdata);
-	iwl_write_prph(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_prph(priv, BSM_DRAM_INST_BYTECOUNT_REG,
-				 priv->ucode_code.len | BSM_DRAM_INST_LOAD);
-
-	iwl_release_nic_access(priv);
-
-	spin_unlock_irqrestore(&priv->lock, flags);
-
-	IWL_DEBUG_INFO("Runtime uCode pointers are set.\n");
-
-	return rc;
-}
-
-/**
- * iwl4965_init_alive_start - Called after REPLY_ALIVE notification received
- *
- * 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 iwl4965_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 (iwl4965_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 (iwl4965_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);
-}
-
-
 /**
- * iwl4965_alive_start - called after REPLY_ALIVE notification received
+ * iwl_alive_start - called after REPLY_ALIVE notification received
  *                   from protocol/runtime uCode (initialization uCode's
- *                   Alive gets handled by iwl4965_init_alive_start()).
+ *                   Alive gets handled by iwl_init_alive_start()).
  */
-static void iwl4965_alive_start(struct iwl_priv *priv)
+static void iwl_alive_start(struct iwl_priv *priv)
 {
 	int ret = 0;
 
@@ -5331,15 +1972,14 @@ static void iwl4965_alive_start(struct iwl_priv *priv)
 	/* 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 (iwl4965_verify_ucode(priv)) {
+	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;
 	}
 
-	iwlcore_clear_stations_table(priv);
-
+	iwl_clear_stations_table(priv);
 	ret = priv->cfg->ops->lib->alive_notify(priv);
 	if (ret) {
 		IWL_WARNING("Could not complete ALIVE transition [ntf]: %d\n",
@@ -5350,22 +1990,17 @@ static void iwl4965_alive_start(struct iwl_priv *priv)
 	/* After the ALIVE response, we can send host commands to 4965 uCode */
 	set_bit(STATUS_ALIVE, &priv->status);
 
-	/* Clear out the uCode error bit if it is set */
-	clear_bit(STATUS_FW_ERROR, &priv->status);
-
 	if (iwl_is_rfkill(priv))
 		return;
 
-	ieee80211_start_queues(priv->hw);
+	ieee80211_wake_queues(priv->hw);
 
 	priv->active_rate = priv->rates_mask;
 	priv->active_rate_basic = priv->rates_mask & IWL_BASIC_RATES_MASK;
 
-	iwl4965_send_power_mode(priv, IWL_POWER_LEVEL(priv->power_mode));
-
 	if (iwl_is_associated(priv)) {
-		struct iwl4965_rxon_cmd *active_rxon =
-				(struct iwl4965_rxon_cmd *)(&priv->active_rxon);
+		struct iwl_rxon_cmd *active_rxon =
+				(struct iwl_rxon_cmd *)&priv->active_rxon;
 
 		memcpy(&priv->staging_rxon, &priv->active_rxon,
 		       sizeof(priv->staging_rxon));
@@ -5379,13 +2014,13 @@ static void iwl4965_alive_start(struct iwl_priv *priv)
 	/* Configure Bluetooth device coexistence support */
 	iwl4965_send_bt_config(priv);
 
+	iwl_reset_run_time_calib(priv);
+
 	/* Configure the adapter for unassociated operation */
 	iwl4965_commit_rxon(priv);
 
 	/* At this point, the NIC is initialized and operational */
-	priv->notif_missed_beacons = 0;
-
-	iwl4965_rf_kill_ct_config(priv);
+	iwl_rf_kill_ct_config(priv);
 
 	iwl_leds_register(priv);
 
@@ -5396,34 +2031,33 @@ static void iwl4965_alive_start(struct iwl_priv *priv)
 	if (priv->error_recovering)
 		iwl4965_error_recovery(priv);
 
-	iwlcore_low_level_notify(priv, IWLCORE_START_EVT);
+	iwl_power_update_mode(priv, 1);
 	ieee80211_notify_mac(priv->hw, IEEE80211_NOTIFY_RE_ASSOC);
+
+	if (test_and_clear_bit(STATUS_MODE_PENDING, &priv->status))
+		iwl4965_set_mode(priv, priv->iw_mode);
+
 	return;
 
  restart:
 	queue_work(priv->workqueue, &priv->restart);
 }
 
-static void iwl4965_cancel_deferred_work(struct iwl_priv *priv);
+static void iwl_cancel_deferred_work(struct iwl_priv *priv);
 
 static void __iwl4965_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_leds_unregister(priv);
 
-	iwlcore_low_level_notify(priv, IWLCORE_STOP_EVT);
-
-	iwlcore_clear_stations_table(priv);
+	iwl_clear_stations_table(priv);
 
 	/* Unblock any waiting calls */
 	wake_up_interruptible_all(&priv->wait_command_queue);
@@ -5455,7 +2089,9 @@ static void __iwl4965_down(struct iwl_priv *priv)
 			       test_bit(STATUS_GEO_CONFIGURED, &priv->status) <<
 					STATUS_GEO_CONFIGURED |
 			       test_bit(STATUS_IN_SUSPEND, &priv->status) <<
-					STATUS_IN_SUSPEND;
+					STATUS_IN_SUSPEND |
+			       test_bit(STATUS_EXIT_PENDING, &priv->status) <<
+					STATUS_EXIT_PENDING;
 		goto exit;
 	}
 
@@ -5470,15 +2106,17 @@ static void __iwl4965_down(struct iwl_priv *priv)
 			test_bit(STATUS_IN_SUSPEND, &priv->status) <<
 				STATUS_IN_SUSPEND |
 			test_bit(STATUS_FW_ERROR, &priv->status) <<
-				STATUS_FW_ERROR;
+				STATUS_FW_ERROR |
+		       test_bit(STATUS_EXIT_PENDING, &priv->status) <<
+				STATUS_EXIT_PENDING;
 
 	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);
 
-	iwl4965_hw_txq_ctx_stop(priv);
-	iwl4965_hw_rxq_stop(priv);
+	iwl_txq_ctx_stop(priv);
+	iwl_rxq_stop(priv);
 
 	spin_lock_irqsave(&priv->lock, flags);
 	if (!iwl_grab_nic_access(priv)) {
@@ -5490,19 +2128,19 @@ static void __iwl4965_down(struct iwl_priv *priv)
 
 	udelay(5);
 
-	iwl4965_hw_nic_stop_master(priv);
-	iwl_set_bit(priv, CSR_RESET, CSR_RESET_REG_FLAG_SW_RESET);
-	iwl4965_hw_nic_reset(priv);
+	/* FIXME: apm_ops.suspend(priv) */
+	priv->cfg->ops->lib->apm_ops.reset(priv);
+	priv->cfg->ops->lib->free_shared_mem(priv);
 
  exit:
-	memset(&priv->card_alive, 0, sizeof(struct iwl4965_alive_resp));
+	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 */
-	iwl4965_clear_free_frames(priv);
+	iwl_clear_free_frames(priv);
 }
 
 static void iwl4965_down(struct iwl_priv *priv)
@@ -5511,7 +2149,7 @@ static void iwl4965_down(struct iwl_priv *priv)
 	__iwl4965_down(priv);
 	mutex_unlock(&priv->mutex);
 
-	iwl4965_cancel_deferred_work(priv);
+	iwl_cancel_deferred_work(priv);
 }
 
 #define MAX_HW_RESTARTS 5
@@ -5526,13 +2164,6 @@ static int __iwl4965_up(struct iwl_priv *priv)
 		return -EIO;
 	}
 
-	if (test_bit(STATUS_RF_KILL_SW, &priv->status)) {
-		IWL_WARNING("Radio disabled by SW RF kill (module "
-			    "parameter)\n");
-		iwl_rfkill_set_hw_state(priv);
-		return -ENODEV;
-	}
-
 	if (!priv->ucode_data_backup.v_addr || !priv->ucode_data.v_addr) {
 		IWL_ERROR("ucode not available for device bringup\n");
 		return -EIO;
@@ -5542,19 +2173,25 @@ static int __iwl4965_up(struct iwl_priv *priv)
 	if (iwl_read32(priv, CSR_GP_CNTRL) &
 				CSR_GP_CNTRL_REG_FLAG_HW_RF_KILL_SW)
 		clear_bit(STATUS_RF_KILL_HW, &priv->status);
-	else {
+	else
 		set_bit(STATUS_RF_KILL_HW, &priv->status);
-		if (!test_bit(STATUS_IN_SUSPEND, &priv->status)) {
-			iwl_rfkill_set_hw_state(priv);
-			IWL_WARNING("Radio disabled by HW RF Kill switch\n");
-			return -ENODEV;
-		}
+
+	if (!test_bit(STATUS_IN_SUSPEND, &priv->status) &&
+	    iwl_is_rfkill(priv)) {
+		IWL_WARNING("Radio disabled by %s RF Kill switch\n",
+		    test_bit(STATUS_RF_KILL_HW, &priv->status) ? "HW" : "SW");
+		return -ENODEV;
 	}
 
-	iwl_rfkill_set_hw_state(priv);
 	iwl_write32(priv, CSR_INT, 0xFFFFFFFF);
 
-	ret = priv->cfg->ops->lib->hw_nic_init(priv);
+	ret = priv->cfg->ops->lib->alloc_shared_mem(priv);
+	if (ret) {
+		IWL_ERROR("Unable to allocate shared memory\n");
+		return ret;
+	}
+
+	ret = iwl_hw_nic_init(priv);
 	if (ret) {
 		IWL_ERROR("Unable to init nic\n");
 		return ret;
@@ -5580,12 +2217,13 @@ static int __iwl4965_up(struct iwl_priv *priv)
 	       priv->ucode_data.len);
 
 	/* We return success when we resume from suspend and rf_kill is on. */
-	if (test_bit(STATUS_RF_KILL_HW, &priv->status))
+	if (test_bit(STATUS_RF_KILL_HW, &priv->status) ||
+	    test_bit(STATUS_RF_KILL_SW, &priv->status))
 		return 0;
 
 	for (i = 0; i < MAX_HW_RESTARTS; i++) {
 
-		iwlcore_clear_stations_table(priv);
+		iwl_clear_stations_table(priv);
 
 		/* load bootstrap state machine,
 		 * load bootstrap program into processor's memory,
@@ -5597,6 +2235,9 @@ static int __iwl4965_up(struct iwl_priv *priv)
 			continue;
 		}
 
+		/* Clear out the uCode error bit if it is set */
+		clear_bit(STATUS_FW_ERROR, &priv->status);
+
 		/* start card; "initialize" will load runtime ucode */
 		iwl4965_nic_start(priv);
 
@@ -5607,6 +2248,7 @@ static int __iwl4965_up(struct iwl_priv *priv)
 
 	set_bit(STATUS_EXIT_PENDING, &priv->status);
 	__iwl4965_down(priv);
+	clear_bit(STATUS_EXIT_PENDING, &priv->status);
 
 	/* tried to restart and config the device for as long as our
 	 * patience could withstand */
@@ -5621,7 +2263,7 @@ static int __iwl4965_up(struct iwl_priv *priv)
  *
  *****************************************************************************/
 
-static void iwl4965_bg_init_alive_start(struct work_struct *data)
+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);
@@ -5630,11 +2272,11 @@ static void iwl4965_bg_init_alive_start(struct work_struct *data)
 		return;
 
 	mutex_lock(&priv->mutex);
-	iwl4965_init_alive_start(priv);
+	priv->cfg->ops->lib->init_alive_start(priv);
 	mutex_unlock(&priv->mutex);
 }
 
-static void iwl4965_bg_alive_start(struct work_struct *data)
+static void iwl_bg_alive_start(struct work_struct *data)
 {
 	struct iwl_priv *priv =
 	    container_of(data, struct iwl_priv, alive_start.work);
@@ -5643,7 +2285,7 @@ static void iwl4965_bg_alive_start(struct work_struct *data)
 		return;
 
 	mutex_lock(&priv->mutex);
-	iwl4965_alive_start(priv);
+	iwl_alive_start(priv);
 	mutex_unlock(&priv->mutex);
 }
 
@@ -5659,7 +2301,7 @@ static void iwl4965_bg_rf_kill(struct work_struct *work)
 	mutex_lock(&priv->mutex);
 
 	if (!iwl_is_rfkill(priv)) {
-		IWL_DEBUG(IWL_DL_INFO | IWL_DL_RF_KILL,
+		IWL_DEBUG(IWL_DL_RF_KILL,
 			  "HW and/or SW RF Kill no longer active, restarting "
 			  "device\n");
 		if (!test_bit(STATUS_EXIT_PENDING, &priv->status))
@@ -5677,239 +2319,53 @@ static void iwl4965_bg_rf_kill(struct work_struct *work)
 				    "Kill switch must be turned off for "
 				    "wireless networking to work.\n");
 	}
-	iwl_rfkill_set_hw_state(priv);
-
 	mutex_unlock(&priv->mutex);
+	iwl_rfkill_set_hw_state(priv);
 }
 
-#define IWL_SCAN_CHECK_WATCHDOG (7 * HZ)
-
-static void iwl4965_bg_scan_check(struct work_struct *data)
+static void iwl4965_bg_set_monitor(struct work_struct *work)
 {
-	struct iwl_priv *priv =
-	    container_of(data, struct iwl_priv, scan_check.work);
+	struct iwl_priv *priv = container_of(work,
+				struct iwl_priv, set_monitor);
+	int ret;
 
-	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
-		return;
+	IWL_DEBUG(IWL_DL_STATE, "setting monitor mode\n");
 
 	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))
-			iwl4965_send_scan_abort(priv);
+	ret = iwl4965_set_mode(priv, IEEE80211_IF_TYPE_MNTR);
+
+	if (ret) {
+		if (ret == -EAGAIN)
+			IWL_DEBUG(IWL_DL_STATE, "leave - not ready\n");
+		else
+			IWL_ERROR("iwl4965_set_mode() failed ret = %d\n", ret);
 	}
+
 	mutex_unlock(&priv->mutex);
 }
 
-static void iwl4965_bg_request_scan(struct work_struct *data)
+static void iwl_bg_run_time_calib_work(struct work_struct *work)
 {
-	struct iwl_priv *priv =
-	    container_of(data, struct iwl_priv, request_scan);
-	struct iwl_host_cmd cmd = {
-		.id = REPLY_SCAN_CMD,
-		.len = sizeof(struct iwl4965_scan_cmd),
-		.meta.flags = CMD_SIZE_HUGE,
-	};
-	struct iwl4965_scan_cmd *scan;
-	struct ieee80211_conf *conf = NULL;
-	u16 cmd_len;
-	enum ieee80211_band band;
-	u8 direct_mask;
-	int ret = 0;
-
-	conf = ieee80211_get_hw_conf(priv->hw);
+	struct iwl_priv *priv = container_of(work, struct iwl_priv,
+			run_time_calib_work);
 
 	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");
-		ret = -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 iwl4965_scan_cmd) +
-				     IWL_MAX_SCAN_SIZE, GFP_KERNEL);
-		if (!priv->scan) {
-			ret = -ENOMEM;
-			goto done;
-		}
+	if (test_bit(STATUS_EXIT_PENDING, &priv->status) ||
+	    test_bit(STATUS_SCANNING, &priv->status)) {
+		mutex_unlock(&priv->mutex);
+		return;
 	}
-	scan = priv->scan;
-	memset(scan, 0, sizeof(struct iwl4965_scan_cmd) + IWL_MAX_SCAN_SIZE);
 
-	scan->quiet_plcp_th = IWL_PLCP_QUIET_THRESH;
-	scan->quiet_time = IWL_ACTIVE_QUIET_TIME;
+	if (priv->start_calib) {
+		iwl_chain_noise_calibration(priv, &priv->statistics);
 
-	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(200 * 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",
-		     iwl4965_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) && priv->essid_len) {
-		IWL_DEBUG_SCAN
-		  ("Kicking off one direct scan for '%s' when not associated\n",
-		   iwl4965_escape_essid(priv->essid, priv->essid_len));
-		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 {
-		IWL_DEBUG_SCAN("Kicking off one indirect scan.\n");
-		direct_mask = 0;
+		iwl_sensitivity_calibration(priv, &priv->statistics);
 	}
 
-	scan->tx_cmd.tx_flags = TX_CMD_FLG_SEQ_CTL_MSK;
-	scan->tx_cmd.sta_id = priv->hw_params.bcast_sta_id;
-	scan->tx_cmd.stop_time.life_time = TX_CMD_LIFE_TIME_INFINITE;
-
-
-	if (priv->scan_bands & BIT(IEEE80211_BAND_2GHZ)) {
-		scan->flags = RXON_FLG_BAND_24G_MSK | RXON_FLG_AUTO_DETECT_MSK;
-		scan->tx_cmd.rate_n_flags =
-				iwl4965_hw_set_rate_n_flags(IWL_RATE_1M_PLCP,
-				RATE_MCS_ANT_B_MSK|RATE_MCS_CCK_MSK);
-
-		scan->good_CRC_th = 0;
-		band = IEEE80211_BAND_2GHZ;
-	} else if (priv->scan_bands & BIT(IEEE80211_BAND_5GHZ)) {
-		scan->tx_cmd.rate_n_flags =
-				iwl4965_hw_set_rate_n_flags(IWL_RATE_6M_PLCP,
-				RATE_MCS_ANT_B_MSK);
-		scan->good_CRC_th = IWL_GOOD_CRC_TH;
-		band = IEEE80211_BAND_5GHZ;
-	} else {
-		IWL_WARNING("Invalid scan band count\n");
-		goto done;
-	}
-
-	/* We don't build a direct scan probe request; the uCode will do
-	 * that based on the direct_mask added to each channel entry */
-	cmd_len = iwl4965_fill_probe_req(priv, band,
-					(struct ieee80211_mgmt *)scan->data,
-					IWL_MAX_SCAN_SIZE - sizeof(*scan), 0);
-
-	scan->tx_cmd.len = cpu_to_le16(cmd_len);
-	/* 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)
-		scan->channel_count =
-			iwl4965_get_channels_for_scan(
-				priv, band, 1, /* active */
-				direct_mask,
-				(void *)&scan->data[le16_to_cpu(scan->tx_cmd.len)]);
-	else
-		scan->channel_count =
-			iwl4965_get_channels_for_scan(
-				priv, band, 0, /* passive */
-				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 iwl4965_scan_channel);
-	cmd.data = scan;
-	scan->len = cpu_to_le16(cmd.len);
-
-	set_bit(STATUS_SCAN_HW, &priv->status);
-	ret = iwl_send_cmd_sync(priv, &cmd);
-	if (ret)
-		goto done;
-
-	queue_delayed_work(priv->workqueue, &priv->scan_check,
-			   IWL_SCAN_CHECK_WATCHDOG);
-
 	mutex_unlock(&priv->mutex);
 	return;
-
- done:
-	/* inform mac80211 scan aborted */
-	queue_work(priv->workqueue, &priv->scan_completed);
-	mutex_unlock(&priv->mutex);
 }
 
 static void iwl4965_bg_up(struct work_struct *data)
@@ -5922,6 +2378,7 @@ static void iwl4965_bg_up(struct work_struct *data)
 	mutex_lock(&priv->mutex);
 	__iwl4965_up(priv);
 	mutex_unlock(&priv->mutex);
+	iwl_rfkill_set_hw_state(priv);
 }
 
 static void iwl4965_bg_restart(struct work_struct *data)
@@ -5944,7 +2401,7 @@ static void iwl4965_bg_rx_replenish(struct work_struct *data)
 		return;
 
 	mutex_lock(&priv->mutex);
-	iwl4965_rx_replenish(priv);
+	iwl_rx_replenish(priv);
 	mutex_unlock(&priv->mutex);
 }
 
@@ -5955,6 +2412,7 @@ static void iwl4965_post_associate(struct iwl_priv *priv)
 	struct ieee80211_conf *conf = NULL;
 	int ret = 0;
 	DECLARE_MAC_BUF(mac);
+	unsigned long flags;
 
 	if (priv->iw_mode == IEEE80211_IF_TYPE_AP) {
 		IWL_ERROR("%s Should not be called in AP mode\n", __FUNCTION__);
@@ -5973,7 +2431,7 @@ static void iwl4965_post_associate(struct iwl_priv *priv)
 	if (!priv->vif || !priv->is_open)
 		return;
 
-	iwl4965_scan_cancel_timeout(priv, 200);
+	iwl_scan_cancel_timeout(priv, 200);
 
 	conf = ieee80211_get_hw_conf(priv->hw);
 
@@ -5990,11 +2448,10 @@ static void iwl4965_post_associate(struct iwl_priv *priv)
 
 	priv->staging_rxon.filter_flags |= RXON_FILTER_ASSOC_MSK;
 
-#ifdef CONFIG_IWL4965_HT
 	if (priv->current_ht_config.is_ht)
-		iwl4965_set_rxon_ht(priv, &priv->current_ht_config);
-#endif /* CONFIG_IWL4965_HT*/
-	iwl4965_set_rxon_chain(priv);
+		iwl_set_rxon_ht(priv, &priv->current_ht_config);
+
+	iwl_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",
@@ -6020,17 +2477,14 @@ static void iwl4965_post_associate(struct iwl_priv *priv)
 
 	switch (priv->iw_mode) {
 	case IEEE80211_IF_TYPE_STA:
-		iwl4965_rate_scale_init(priv->hw, IWL_AP_ID);
 		break;
 
 	case IEEE80211_IF_TYPE_IBSS:
 
-		/* clear out the station table */
-		iwlcore_clear_stations_table(priv);
+		/* assume default assoc id */
+		priv->assoc_id = 1;
 
-		iwl4965_rxon_add_station(priv, iwl4965_broadcast_addr, 0);
-		iwl4965_rxon_add_station(priv, priv->bssid, 0);
-		iwl4965_rate_scale_init(priv->hw, IWL_STA_ID);
+		iwl_rxon_add_station(priv, priv->bssid, 0);
 		iwl4965_send_beacon_cmd(priv);
 
 		break;
@@ -6041,58 +2495,30 @@ static void iwl4965_post_associate(struct iwl_priv *priv)
 		break;
 	}
 
-	iwl4965_sequence_reset(priv);
-
-#ifdef CONFIG_IWL4965_SENSITIVITY
 	/* Enable Rx differential gain and sensitivity calibrations */
-	iwl4965_chain_noise_reset(priv);
+	iwl_chain_noise_reset(priv);
 	priv->start_calib = 1;
-#endif /* CONFIG_IWL4965_SENSITIVITY */
 
 	if (priv->iw_mode == IEEE80211_IF_TYPE_IBSS)
 		priv->assoc_station_added = 1;
 
-	iwl4965_activate_qos(priv, 0);
+	spin_lock_irqsave(&priv->lock, flags);
+	iwl_activate_qos(priv, 0);
+	spin_unlock_irqrestore(&priv->lock, flags);
 
+	iwl_power_update_mode(priv, 0);
 	/* we have just associated, don't start scan too early */
 	priv->next_scan_jiffies = jiffies + IWL_DELAY_NEXT_SCAN;
 }
 
-
-static void iwl4965_bg_post_associate(struct work_struct *data)
-{
-	struct iwl_priv *priv = container_of(data, struct iwl_priv,
-					     post_associate.work);
-
-	mutex_lock(&priv->mutex);
-	iwl4965_post_associate(priv);
-	mutex_unlock(&priv->mutex);
-
-}
-
-static void iwl4965_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);
-	iwl4965_send_scan_abort(priv);
-
-	mutex_unlock(&priv->mutex);
-}
-
 static int iwl4965_mac_config(struct ieee80211_hw *hw, struct ieee80211_conf *conf);
 
-static void iwl4965_bg_scan_completed(struct work_struct *work)
+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");
+	IWL_DEBUG_SCAN("SCAN complete scan\n");
 
 	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
 		return;
@@ -6105,7 +2531,7 @@ static void iwl4965_bg_scan_completed(struct work_struct *work)
 	/* Since setting the TXPOWER may have been deferred while
 	 * performing the scan, fire one off */
 	mutex_lock(&priv->mutex);
-	iwl4965_hw_reg_send_txpower(priv);
+	iwl_set_tx_power(priv, priv->tx_power_user_lmt, true);
 	mutex_unlock(&priv->mutex);
 }
 
@@ -6115,7 +2541,7 @@ static void iwl4965_bg_scan_completed(struct work_struct *work)
  *
  *****************************************************************************/
 
-#define UCODE_READY_TIMEOUT	(2 * HZ)
+#define UCODE_READY_TIMEOUT	(4 * HZ)
 
 static int iwl4965_mac_start(struct ieee80211_hw *hw)
 {
@@ -6141,7 +2567,7 @@ static int iwl4965_mac_start(struct ieee80211_hw *hw)
 	/* we should be verifying the device is ready to be opened */
 	mutex_lock(&priv->mutex);
 
-	memset(&priv->staging_rxon, 0, sizeof(struct iwl4965_rxon_cmd));
+	memset(&priv->staging_rxon, 0, sizeof(struct iwl_rxon_cmd));
 	/* fetch ucode file from disk, alloc and copy to bus-master buffers ...
 	 * ucode filename and max sizes are card-specific. */
 
@@ -6158,6 +2584,8 @@ static int iwl4965_mac_start(struct ieee80211_hw *hw)
 
 	mutex_unlock(&priv->mutex);
 
+	iwl_rfkill_set_hw_state(priv);
+
 	if (ret)
 		goto out_release_irq;
 
@@ -6166,15 +2594,15 @@ static int iwl4965_mac_start(struct ieee80211_hw *hw)
 	if (test_bit(STATUS_IN_SUSPEND, &priv->status))
 		return 0;
 
-	/* Wait for START_ALIVE from ucode. Otherwise callbacks from
+	/* Wait for START_ALIVE from Run Time ucode. Otherwise callbacks from
 	 * mac80211 will not be run successfully. */
 	ret = wait_event_interruptible_timeout(priv->wait_command_queue,
 			test_bit(STATUS_READY, &priv->status),
 			UCODE_READY_TIMEOUT);
 	if (!ret) {
 		if (!test_bit(STATUS_READY, &priv->status)) {
-			IWL_ERROR("Wait for START_ALIVE timeout after %dms.\n",
-				  jiffies_to_msecs(UCODE_READY_TIMEOUT));
+			IWL_ERROR("START_ALIVE timeout after %dms.\n",
+				jiffies_to_msecs(UCODE_READY_TIMEOUT));
 			ret = -ETIMEDOUT;
 			goto out_release_irq;
 		}
@@ -6212,8 +2640,7 @@ static void iwl4965_mac_stop(struct ieee80211_hw *hw)
 		 * RXON_FILTER_ASSOC_MSK BIT
 		 */
 		mutex_lock(&priv->mutex);
-		iwl4965_scan_cancel_timeout(priv, 100);
-		cancel_delayed_work(&priv->post_associate);
+		iwl_scan_cancel_timeout(priv, 100);
 		mutex_unlock(&priv->mutex);
 	}
 
@@ -6228,8 +2655,7 @@ static void iwl4965_mac_stop(struct ieee80211_hw *hw)
 	IWL_DEBUG_MAC80211("leave\n");
 }
 
-static int iwl4965_mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb,
-		      struct ieee80211_tx_control *ctl)
+static int iwl4965_mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
 	struct iwl_priv *priv = hw->priv;
 
@@ -6242,9 +2668,9 @@ static int iwl4965_mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb,
 	}
 
 	IWL_DEBUG_TX("dev->xmit(%d bytes) at rate 0x%02x\n", skb->len,
-		     ctl->tx_rate->bitrate);
+		     ieee80211_get_tx_rate(hw, IEEE80211_SKB_CB(skb))->bitrate);
 
-	if (iwl4965_tx_skb(priv, skb, ctl))
+	if (iwl_tx_skb(priv, skb))
 		dev_kfree_skb_any(skb);
 
 	IWL_DEBUG_MAC80211("leave\n");
@@ -6277,8 +2703,9 @@ static int iwl4965_mac_add_interface(struct ieee80211_hw *hw,
 		memcpy(priv->mac_addr, conf->mac_addr, ETH_ALEN);
 	}
 
-	if (iwl_is_ready(priv))
-		iwl4965_set_mode(priv, conf->type);
+	if (iwl4965_set_mode(priv, conf->type) == -EAGAIN)
+		/* we are not ready, will run again when ready */
+		set_bit(STATUS_MODE_PENDING, &priv->status);
 
 	mutex_unlock(&priv->mutex);
 
@@ -6299,12 +2726,21 @@ static int iwl4965_mac_config(struct ieee80211_hw *hw, struct ieee80211_conf *co
 	const struct iwl_channel_info *ch_info;
 	unsigned long flags;
 	int ret = 0;
+	u16 channel;
 
 	mutex_lock(&priv->mutex);
 	IWL_DEBUG_MAC80211("enter to channel %d\n", conf->channel->hw_value);
 
 	priv->add_radiotap = !!(conf->flags & IEEE80211_CONF_RADIOTAP);
 
+	if (conf->radio_enabled && iwl_radio_kill_sw_enable_radio(priv)) {
+		IWL_DEBUG_MAC80211("leave - RF-KILL - waiting for uCode\n");
+		goto out;
+	}
+
+	if (!conf->radio_enabled)
+		iwl_radio_kill_sw_disable_radio(priv);
+
 	if (!iwl_is_ready(priv)) {
 		IWL_DEBUG_MAC80211("leave - not ready\n");
 		ret = -EIO;
@@ -6319,33 +2755,37 @@ static int iwl4965_mac_config(struct ieee80211_hw *hw, struct ieee80211_conf *co
 		return 0;
 	}
 
-	spin_lock_irqsave(&priv->lock, flags);
-
-	ch_info = iwl_get_channel_info(priv, conf->channel->band,
-			ieee80211_frequency_to_channel(conf->channel->center_freq));
+	channel = ieee80211_frequency_to_channel(conf->channel->center_freq);
+	ch_info = iwl_get_channel_info(priv, conf->channel->band, channel);
 	if (!is_channel_valid(ch_info)) {
 		IWL_DEBUG_MAC80211("leave - invalid channel\n");
-		spin_unlock_irqrestore(&priv->lock, flags);
 		ret = -EINVAL;
 		goto out;
 	}
 
-#ifdef CONFIG_IWL4965_HT
+	if (priv->iw_mode == IEEE80211_IF_TYPE_IBSS &&
+	    !is_channel_ibss(ch_info)) {
+		IWL_ERROR("channel %d in band %d not IBSS channel\n",
+			conf->channel->hw_value, conf->channel->band);
+		ret = -EINVAL;
+		goto out;
+	}
+
+	spin_lock_irqsave(&priv->lock, flags);
+
 	/* if we are switching from 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->hw_value)
+	if ((le16_to_cpu(priv->staging_rxon.channel) != channel)
 #ifdef IEEE80211_CONF_CHANNEL_SWITCH
 	    && !(conf->flags & IEEE80211_CONF_CHANNEL_SWITCH)
 #endif
 	)
 		priv->staging_rxon.flags = 0;
-#endif /* CONFIG_IWL4965_HT */
 
-	iwlcore_set_rxon_channel(priv, conf->channel->band,
-		ieee80211_frequency_to_channel(conf->channel->center_freq));
+	iwl_set_rxon_channel(priv, conf->channel->band, channel);
 
-	iwl4965_set_flags_for_phymode(priv, conf->channel->band);
+	iwl_set_flags_for_band(priv, conf->channel->band);
 
 	/* The list of supported rates and rate mask can be different
 	 * for each band; since the band may have changed, reset
@@ -6361,9 +2801,6 @@ static int iwl4965_mac_config(struct ieee80211_hw *hw, struct ieee80211_conf *co
 	}
 #endif
 
-	if (priv->cfg->ops->lib->radio_kill_sw)
-		priv->cfg->ops->lib->radio_kill_sw(priv, !conf->radio_enabled);
-
 	if (!conf->radio_enabled) {
 		IWL_DEBUG_MAC80211("leave - radio disabled\n");
 		goto out;
@@ -6375,6 +2812,11 @@ static int iwl4965_mac_config(struct ieee80211_hw *hw, struct ieee80211_conf *co
 		goto out;
 	}
 
+	IWL_DEBUG_MAC80211("TX Power old=%d new=%d\n",
+			   priv->tx_power_user_lmt, conf->power_level);
+
+	iwl_set_tx_power(priv, conf->power_level, false);
+
 	iwl4965_set_rate(priv);
 
 	if (memcmp(&priv->active_rxon,
@@ -6394,12 +2836,13 @@ out:
 static void iwl4965_config_ap(struct iwl_priv *priv)
 {
 	int ret = 0;
+	unsigned long flags;
 
 	if (test_bit(STATUS_EXIT_PENDING, &priv->status))
 		return;
 
 	/* The following should be done only at AP bring up */
-	if ((priv->active_rxon.filter_flags & RXON_FILTER_ASSOC_MSK) == 0) {
+	if (!(iwl_is_associated(priv))) {
 
 		/* RXON - unassoc (to set timing command) */
 		priv->staging_rxon.filter_flags &= ~RXON_FILTER_ASSOC_MSK;
@@ -6414,7 +2857,7 @@ static void iwl4965_config_ap(struct iwl_priv *priv)
 			IWL_WARNING("REPLY_RXON_TIMING failed - "
 					"Attempting to continue.\n");
 
-		iwl4965_set_rxon_chain(priv);
+		iwl_set_rxon_chain(priv);
 
 		/* FIXME: what should be the assoc_id for AP? */
 		priv->staging_rxon.assoc_id = cpu_to_le16(priv->assoc_id);
@@ -6441,8 +2884,10 @@ static void iwl4965_config_ap(struct iwl_priv *priv)
 		/* restore RXON assoc */
 		priv->staging_rxon.filter_flags |= RXON_FILTER_ASSOC_MSK;
 		iwl4965_commit_rxon(priv);
-		iwl4965_activate_qos(priv, 1);
-		iwl4965_rxon_add_station(priv, iwl4965_broadcast_addr, 0);
+		spin_lock_irqsave(&priv->lock, flags);
+		iwl_activate_qos(priv, 1);
+		spin_unlock_irqrestore(&priv->lock, flags);
+		iwl_rxon_add_station(priv, iwl_bcast_addr, 0);
 	}
 	iwl4965_send_beacon_cmd(priv);
 
@@ -6451,6 +2896,9 @@ static void iwl4965_config_ap(struct iwl_priv *priv)
 	 * clear sta table, add BCAST sta... */
 }
 
+/* temporary */
+static int iwl4965_mac_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb);
+
 static int iwl4965_mac_config_interface(struct ieee80211_hw *hw,
 					struct ieee80211_vif *vif,
 				    struct ieee80211_if_conf *conf)
@@ -6468,8 +2916,18 @@ static int iwl4965_mac_config_interface(struct ieee80211_hw *hw,
 		return 0;
 	}
 
+	if (priv->iw_mode == IEEE80211_IF_TYPE_IBSS &&
+	    conf->changed & IEEE80211_IFCC_BEACON) {
+		struct sk_buff *beacon = ieee80211_beacon_get(hw, vif);
+		if (!beacon)
+			return -ENOMEM;
+		rc = iwl4965_mac_beacon_update(hw, beacon);
+		if (rc)
+			return rc;
+	}
+
 	if ((priv->iw_mode == IEEE80211_IF_TYPE_AP) &&
-	    (!conf->beacon || !conf->ssid_len)) {
+	    (!conf->ssid_len)) {
 		IWL_DEBUG_MAC80211
 		    ("Leaving in AP mode because HostAPD is not ready.\n");
 		return 0;
@@ -6501,7 +2959,7 @@ static int iwl4965_mac_config_interface(struct ieee80211_hw *hw,
 		if (priv->ibss_beacon)
 			dev_kfree_skb(priv->ibss_beacon);
 
-		priv->ibss_beacon = conf->beacon;
+		priv->ibss_beacon = ieee80211_beacon_get(hw, vif);
 	}
 
 	if (iwl_is_rfkill(priv))
@@ -6511,7 +2969,7 @@ static int iwl4965_mac_config_interface(struct ieee80211_hw *hw,
 	    !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 (iwl4965_scan_cancel_timeout(priv, 100)) {
+		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");
@@ -6531,12 +2989,12 @@ static int iwl4965_mac_config_interface(struct ieee80211_hw *hw,
 		else {
 			rc = iwl4965_commit_rxon(priv);
 			if ((priv->iw_mode == IEEE80211_IF_TYPE_STA) && rc)
-				iwl4965_rxon_add_station(
+				iwl_rxon_add_station(
 					priv, priv->active_rxon.bssid_addr, 1);
 		}
 
 	} else {
-		iwl4965_scan_cancel_timeout(priv, 100);
+		iwl_scan_cancel_timeout(priv, 100);
 		priv->staging_rxon.filter_flags &= ~RXON_FILTER_ASSOC_MSK;
 		iwl4965_commit_rxon(priv);
 	}
@@ -6562,11 +3020,18 @@ static void iwl4965_configure_filter(struct ieee80211_hw *hw,
 				 unsigned int *total_flags,
 				 int mc_count, struct dev_addr_list *mc_list)
 {
-	/*
-	 * XXX: dummy
-	 * see also iwl4965_connection_init_rx_config
-	 */
-	*total_flags = 0;
+	struct iwl_priv *priv = hw->priv;
+
+	if (changed_flags & (*total_flags) & FIF_OTHER_BSS) {
+		IWL_DEBUG_MAC80211("Enter: type %d (0x%x, 0x%x)\n",
+				   IEEE80211_IF_TYPE_MNTR,
+				   changed_flags, *total_flags);
+		/* queue work 'cuz mac80211 is holding a lock which
+		 * prevents us from issuing (synchronous) f/w cmds */
+		queue_work(priv->workqueue, &priv->set_monitor);
+	}
+	*total_flags &= FIF_OTHER_BSS | FIF_ALLMULTI |
+			FIF_BCN_PRBRESP_PROMISC | FIF_CONTROL;
 }
 
 static void iwl4965_mac_remove_interface(struct ieee80211_hw *hw,
@@ -6579,8 +3044,7 @@ static void iwl4965_mac_remove_interface(struct ieee80211_hw *hw,
 	mutex_lock(&priv->mutex);
 
 	if (iwl_is_ready_rf(priv)) {
-		iwl4965_scan_cancel_timeout(priv, 100);
-		cancel_delayed_work(&priv->post_associate);
+		iwl_scan_cancel_timeout(priv, 100);
 		priv->staging_rxon.filter_flags &= ~RXON_FILTER_ASSOC_MSK;
 		iwl4965_commit_rxon(priv);
 	}
@@ -6596,64 +3060,6 @@ static void iwl4965_mac_remove_interface(struct ieee80211_hw *hw,
 
 }
 
-
-#ifdef CONFIG_IWL4965_HT
-static void iwl4965_ht_conf(struct iwl_priv *priv,
-			    struct ieee80211_bss_conf *bss_conf)
-{
-	struct ieee80211_ht_info *ht_conf = bss_conf->ht_conf;
-	struct ieee80211_ht_bss_info *ht_bss_conf = bss_conf->ht_bss_conf;
-	struct iwl_ht_info *iwl_conf = &priv->current_ht_config;
-
-	IWL_DEBUG_MAC80211("enter: \n");
-
-	iwl_conf->is_ht = bss_conf->assoc_ht;
-
-	if (!iwl_conf->is_ht)
-		return;
-
-	priv->ps_mode = (u8)((ht_conf->cap & IEEE80211_HT_CAP_MIMO_PS) >> 2);
-
-	if (ht_conf->cap & IEEE80211_HT_CAP_SGI_20)
-		iwl_conf->sgf |= 0x1;
-	if (ht_conf->cap & IEEE80211_HT_CAP_SGI_40)
-		iwl_conf->sgf |= 0x2;
-
-	iwl_conf->is_green_field = !!(ht_conf->cap & IEEE80211_HT_CAP_GRN_FLD);
-	iwl_conf->max_amsdu_size =
-		!!(ht_conf->cap & IEEE80211_HT_CAP_MAX_AMSDU);
-
-	iwl_conf->supported_chan_width =
-		!!(ht_conf->cap & IEEE80211_HT_CAP_SUP_WIDTH);
-	iwl_conf->extension_chan_offset =
-		ht_bss_conf->bss_cap & IEEE80211_HT_IE_CHA_SEC_OFFSET;
-	/* If no above or below channel supplied disable FAT channel */
-	if (iwl_conf->extension_chan_offset != IWL_EXT_CHANNEL_OFFSET_ABOVE &&
-	    iwl_conf->extension_chan_offset != IWL_EXT_CHANNEL_OFFSET_BELOW)
-		iwl_conf->supported_chan_width = 0;
-
-	iwl_conf->tx_mimo_ps_mode =
-		(u8)((ht_conf->cap & IEEE80211_HT_CAP_MIMO_PS) >> 2);
-	memcpy(iwl_conf->supp_mcs_set, ht_conf->supp_mcs_set, 16);
-
-	iwl_conf->control_channel = ht_bss_conf->primary_channel;
-	iwl_conf->tx_chan_width =
-		!!(ht_bss_conf->bss_cap & IEEE80211_HT_IE_CHA_WIDTH);
-	iwl_conf->ht_protection =
-		ht_bss_conf->bss_op_mode & IEEE80211_HT_IE_HT_PROTECTION;
-	iwl_conf->non_GF_STA_present =
-		!!(ht_bss_conf->bss_op_mode & IEEE80211_HT_IE_NON_GF_STA_PRSNT);
-
-	IWL_DEBUG_MAC80211("control channel %d\n", iwl_conf->control_channel);
-	IWL_DEBUG_MAC80211("leave\n");
-}
-#else
-static inline void iwl4965_ht_conf(struct iwl_priv *priv,
-				   struct ieee80211_bss_conf *bss_conf)
-{
-}
-#endif
-
 #define IWL_DELAY_NEXT_SCAN_AFTER_ASSOC (HZ*6)
 static void iwl4965_bss_info_changed(struct ieee80211_hw *hw,
 				     struct ieee80211_vif *vif,
@@ -6684,7 +3090,7 @@ static void iwl4965_bss_info_changed(struct ieee80211_hw *hw,
 	if (changes & BSS_CHANGED_HT) {
 		IWL_DEBUG_MAC80211("HT %d\n", bss_conf->assoc_ht);
 		iwl4965_ht_conf(priv, bss_conf);
-		iwl4965_set_rxon_chain(priv);
+		iwl_set_rxon_chain(priv);
 	}
 
 	if (changes & BSS_CHANGED_ASSOC) {
@@ -6751,7 +3157,7 @@ static int iwl4965_mac_hw_scan(struct ieee80211_hw *hw, u8 *ssid, size_t len)
 	}
 	if (len) {
 		IWL_DEBUG_SCAN("direct scan for %s [%d]\n ",
-			       iwl4965_escape_essid(ssid, len), (int)len);
+			       iwl_escape_essid(ssid, len), (int)len);
 
 		priv->one_direct_scan = 1;
 		priv->direct_ssid_len = (u8)
@@ -6760,7 +3166,7 @@ static int iwl4965_mac_hw_scan(struct ieee80211_hw *hw, u8 *ssid, size_t len)
 	} else
 		priv->one_direct_scan = 0;
 
-	rc = iwl4965_scan_initiate(priv);
+	rc = iwl_scan_initiate(priv);
 
 	IWL_DEBUG_MAC80211("leave\n");
 
@@ -6784,14 +3190,14 @@ static void iwl4965_mac_update_tkip_key(struct ieee80211_hw *hw,
 
 	IWL_DEBUG_MAC80211("enter\n");
 
-	sta_id = iwl4965_hw_find_station(priv, addr);
+	sta_id = iwl_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;
 	}
 
-	iwl4965_scan_cancel_timeout(priv, 100);
+	iwl_scan_cancel_timeout(priv, 100);
 
 	key_flags |= (STA_KEY_FLG_TKIP | STA_KEY_FLG_MAP_KEY_MSK);
 	key_flags |= cpu_to_le16(keyconf->keyidx << STA_KEY_FLG_KEYID_POS);
@@ -6812,7 +3218,7 @@ static void iwl4965_mac_update_tkip_key(struct ieee80211_hw *hw,
 	priv->stations[sta_id].sta.sta.modify_mask = STA_MODIFY_KEY_MASK;
 	priv->stations[sta_id].sta.mode = STA_CONTROL_MODIFY_MSK;
 
-	iwl4965_send_add_station(priv, &priv->stations[sta_id].sta, CMD_ASYNC);
+	iwl_send_add_sta(priv, &priv->stations[sta_id].sta, CMD_ASYNC);
 
 	spin_unlock_irqrestore(&priv->sta_lock, flags);
 
@@ -6831,7 +3237,7 @@ static int iwl4965_mac_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 
 	IWL_DEBUG_MAC80211("enter\n");
 
-	if (priv->cfg->mod_params->sw_crypto) {
+	if (priv->hw_params.sw_crypto) {
 		IWL_DEBUG_MAC80211("leave - hwcrypto disabled\n");
 		return -EOPNOTSUPP;
 	}
@@ -6840,7 +3246,7 @@ static int iwl4965_mac_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 		/* only support pairwise keys */
 		return -EOPNOTSUPP;
 
-	sta_id = iwl4965_hw_find_station(priv, addr);
+	sta_id = iwl_find_station(priv, addr);
 	if (sta_id == IWL_INVALID_STATION) {
 		IWL_DEBUG_MAC80211("leave - %s not in station map.\n",
 				   print_mac(mac, addr));
@@ -6849,7 +3255,7 @@ static int iwl4965_mac_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 	}
 
 	mutex_lock(&priv->mutex);
-	iwl4965_scan_cancel_timeout(priv, 100);
+	iwl_scan_cancel_timeout(priv, 100);
 	mutex_unlock(&priv->mutex);
 
 	/* If we are getting WEP group key and we didn't receive any key mapping
@@ -6861,7 +3267,8 @@ static int iwl4965_mac_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 		if (cmd == SET_KEY)
 			is_default_wep_key = !priv->key_mapping_key;
 		else
-			is_default_wep_key = priv->default_wep_key;
+			is_default_wep_key =
+					(key->hw_key_idx == HW_KEY_DEFAULT);
 	}
 
 	switch (cmd) {
@@ -6877,7 +3284,7 @@ static int iwl4965_mac_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 		if (is_default_wep_key)
 			ret = iwl_remove_default_wep_key(priv, key);
 		else
-			ret = iwl_remove_dynamic_key(priv, sta_id);
+			ret = iwl_remove_dynamic_key(priv, key, sta_id);
 
 		IWL_DEBUG_MAC80211("disable hwcrypto key\n");
 		break;
@@ -6890,7 +3297,7 @@ static int iwl4965_mac_set_key(struct ieee80211_hw *hw, enum set_key_cmd cmd,
 	return ret;
 }
 
-static int iwl4965_mac_conf_tx(struct ieee80211_hw *hw, int queue,
+static int iwl4965_mac_conf_tx(struct ieee80211_hw *hw, u16 queue,
 			   const struct ieee80211_tx_queue_params *params)
 {
 	struct iwl_priv *priv = hw->priv;
@@ -6927,15 +3334,12 @@ static int iwl4965_mac_conf_tx(struct ieee80211_hw *hw, int queue,
 	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)
-		iwl4965_activate_qos(priv, 1);
+		iwl_activate_qos(priv, 1);
 	else if (priv->assoc_id && iwl_is_associated(priv))
-		iwl4965_activate_qos(priv, 0);
+		iwl_activate_qos(priv, 0);
 
-	mutex_unlock(&priv->mutex);
+	spin_unlock_irqrestore(&priv->lock, flags);
 
 	IWL_DEBUG_MAC80211("leave\n");
 	return 0;
@@ -6946,8 +3350,8 @@ static int iwl4965_mac_get_tx_stats(struct ieee80211_hw *hw,
 {
 	struct iwl_priv *priv = hw->priv;
 	int i, avail;
-	struct iwl4965_tx_queue *txq;
-	struct iwl4965_queue *q;
+	struct iwl_tx_queue *txq;
+	struct iwl_queue *q;
 	unsigned long flags;
 
 	IWL_DEBUG_MAC80211("enter\n");
@@ -6962,11 +3366,11 @@ static int iwl4965_mac_get_tx_stats(struct ieee80211_hw *hw,
 	for (i = 0; i < AC_NUM; i++) {
 		txq = &priv->txq[i];
 		q = &txq->q;
-		avail = iwl4965_queue_space(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;
+		stats[i].len = q->n_window - avail;
+		stats[i].limit = q->n_window - q->high_mark;
+		stats[i].count = q->n_window;
 
 	}
 	spin_unlock_irqrestore(&priv->lock, flags);
@@ -6979,14 +3383,9 @@ static int iwl4965_mac_get_tx_stats(struct ieee80211_hw *hw,
 static int iwl4965_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;
-}
+	struct iwl_priv *priv = hw->priv;
 
-static u64 iwl4965_mac_get_tsf(struct ieee80211_hw *hw)
-{
+	priv = hw->priv;
 	IWL_DEBUG_MAC80211("enter\n");
 	IWL_DEBUG_MAC80211("leave\n");
 
@@ -7001,16 +3400,11 @@ static void iwl4965_mac_reset_tsf(struct ieee80211_hw *hw)
 	mutex_lock(&priv->mutex);
 	IWL_DEBUG_MAC80211("enter\n");
 
-	priv->lq_mngr.lq_ready = 0;
-#ifdef CONFIG_IWL4965_HT
 	spin_lock_irqsave(&priv->lock, flags);
 	memset(&priv->current_ht_config, 0, sizeof(struct iwl_ht_info));
 	spin_unlock_irqrestore(&priv->lock, flags);
-#endif /* CONFIG_IWL4965_HT */
-
-	iwlcore_reset_qos(priv);
 
-	cancel_delayed_work(&priv->post_associate);
+	iwl_reset_qos(priv);
 
 	spin_lock_irqsave(&priv->lock, flags);
 	priv->assoc_id = 0;
@@ -7040,11 +3434,13 @@ static void iwl4965_mac_reset_tsf(struct ieee80211_hw *hw)
 	 * clear RXON_FILTER_ASSOC_MSK bit
 	 */
 	if (priv->iw_mode != IEEE80211_IF_TYPE_AP) {
-		iwl4965_scan_cancel_timeout(priv, 100);
+		iwl_scan_cancel_timeout(priv, 100);
 		priv->staging_rxon.filter_flags &= ~RXON_FILTER_ASSOC_MSK;
 		iwl4965_commit_rxon(priv);
 	}
 
+	iwl_power_update_mode(priv, 0);
+
 	/* Per mac80211.h: This is only used in IBSS mode... */
 	if (priv->iw_mode != IEEE80211_IF_TYPE_IBSS) {
 
@@ -7060,11 +3456,11 @@ static void iwl4965_mac_reset_tsf(struct ieee80211_hw *hw)
 	IWL_DEBUG_MAC80211("leave\n");
 }
 
-static int iwl4965_mac_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb,
-				 struct ieee80211_tx_control *control)
+static int iwl4965_mac_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
 	struct iwl_priv *priv = hw->priv;
 	unsigned long flags;
+	__le64 timestamp;
 
 	mutex_lock(&priv->mutex);
 	IWL_DEBUG_MAC80211("enter\n");
@@ -7089,13 +3485,15 @@ static int iwl4965_mac_beacon_update(struct ieee80211_hw *hw, struct sk_buff *sk
 	priv->ibss_beacon = skb;
 
 	priv->assoc_id = 0;
+	timestamp = ((struct ieee80211_mgmt *)skb->data)->u.beacon.timestamp;
+	priv->timestamp = le64_to_cpu(timestamp) +  (priv->beacon_int * 1000);
 
 	IWL_DEBUG_MAC80211("leave\n");
 	spin_unlock_irqrestore(&priv->lock, flags);
 
-	iwlcore_reset_qos(priv);
+	iwl_reset_qos(priv);
 
-	queue_work(priv->workqueue, &priv->post_associate.work);
+	iwl4965_post_associate(priv);
 
 	mutex_unlock(&priv->mutex);
 
@@ -7118,13 +3516,18 @@ static int iwl4965_mac_beacon_update(struct ieee80211_hw *hw, struct sk_buff *sk
  * See the level definitions in iwl for details.
  */
 
-static ssize_t show_debug_level(struct device_driver *d, char *buf)
+static ssize_t show_debug_level(struct device *d,
+				struct device_attribute *attr, char *buf)
 {
-	return sprintf(buf, "0x%08X\n", iwl_debug_level);
+	struct iwl_priv *priv = d->driver_data;
+
+	return sprintf(buf, "0x%08X\n", priv->debug_level);
 }
-static ssize_t store_debug_level(struct device_driver *d,
+static ssize_t store_debug_level(struct device *d,
+				struct device_attribute *attr,
 				 const char *buf, size_t count)
 {
+	struct iwl_priv *priv = d->driver_data;
 	char *p = (char *)buf;
 	u32 val;
 
@@ -7133,17 +3536,49 @@ static ssize_t store_debug_level(struct device_driver *d,
 		printk(KERN_INFO DRV_NAME
 		       ": %s is not in hex or decimal form.\n", buf);
 	else
-		iwl_debug_level = val;
+		priv->debug_level = val;
 
 	return strnlen(buf, count);
 }
 
-static DRIVER_ATTR(debug_level, S_IWUSR | S_IRUGO,
-		   show_debug_level, store_debug_level);
+static DEVICE_ATTR(debug_level, S_IWUSR | S_IRUGO,
+			show_debug_level, store_debug_level);
+
 
 #endif /* CONFIG_IWLWIFI_DEBUG */
 
 
+static ssize_t show_version(struct device *d,
+				struct device_attribute *attr, char *buf)
+{
+	struct iwl_priv *priv = d->driver_data;
+	struct iwl_alive_resp *palive = &priv->card_alive;
+	ssize_t pos = 0;
+	u16 eeprom_ver;
+
+	if (palive->is_valid)
+		pos += sprintf(buf + pos,
+				"fw version: 0x%01X.0x%01X.0x%01X.0x%01X\n"
+				"fw type: 0x%01X 0x%01X\n",
+				palive->ucode_major, palive->ucode_minor,
+				palive->sw_rev[0], palive->sw_rev[1],
+				palive->ver_type, palive->ver_subtype);
+	else
+		pos += sprintf(buf + pos, "fw not loaded\n");
+
+	if (priv->eeprom) {
+		eeprom_ver = iwl_eeprom_query16(priv, EEPROM_VERSION);
+		pos += sprintf(buf + pos, "EEPROM version: 0x%x\n",
+				 eeprom_ver);
+	} else {
+		pos += sprintf(buf + pos, "EEPROM not initialzed\n");
+	}
+
+	return pos;
+}
+
+static DEVICE_ATTR(version, S_IWUSR | S_IRUGO, show_version, NULL);
+
 static ssize_t show_temperature(struct device *d,
 				struct device_attribute *attr, char *buf)
 {
@@ -7152,7 +3587,7 @@ static ssize_t show_temperature(struct device *d,
 	if (!iwl_is_alive(priv))
 		return -EAGAIN;
 
-	return sprintf(buf, "%d\n", iwl4965_hw_get_temperature(priv));
+	return sprintf(buf, "%d\n", priv->temperature);
 }
 
 static DEVICE_ATTR(temperature, S_IRUGO, show_temperature, NULL);
@@ -7170,7 +3605,7 @@ 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);
+	return sprintf(buf, "%d\n", priv->tx_power_user_lmt);
 }
 
 static ssize_t store_tx_power(struct device *d,
@@ -7186,7 +3621,7 @@ static ssize_t store_tx_power(struct device *d,
 		printk(KERN_INFO DRV_NAME
 		       ": %s is not in decimal form.\n", buf);
 	else
-		iwl4965_hw_reg_set_txpower(priv, val);
+		iwl_set_tx_power(priv, val, false);
 
 	return count;
 }
@@ -7211,7 +3646,7 @@ static ssize_t store_flags(struct device *d,
 	mutex_lock(&priv->mutex);
 	if (le32_to_cpu(priv->staging_rxon.flags) != flags) {
 		/* Cancel any currently running scans... */
-		if (iwl4965_scan_cancel_timeout(priv, 100))
+		if (iwl_scan_cancel_timeout(priv, 100))
 			IWL_WARNING("Could not cancel scan.\n");
 		else {
 			IWL_DEBUG_INFO("Committing rxon.flags = 0x%04X\n",
@@ -7246,7 +3681,7 @@ static ssize_t store_filter_flags(struct device *d,
 	mutex_lock(&priv->mutex);
 	if (le32_to_cpu(priv->staging_rxon.filter_flags) != filter_flags) {
 		/* Cancel any currently running scans... */
-		if (iwl4965_scan_cancel_timeout(priv, 100))
+		if (iwl_scan_cancel_timeout(priv, 100))
 			IWL_WARNING("Could not cancel scan.\n");
 		else {
 			IWL_DEBUG_INFO("Committing rxon.filter_flags = "
@@ -7376,20 +3811,11 @@ static ssize_t store_power_level(struct device *d,
 		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 = iwl4965_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 = iwl_power_set_user_mode(priv, mode);
+	if (rc) {
+		IWL_DEBUG_MAC80211("failed setting power mode.\n");
+		goto out;
 	}
-
 	rc = count;
 
  out:
@@ -7419,7 +3845,7 @@ 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);
+	int level = priv->power_data.power_mode;
 	char *p = buf;
 
 	p += sprintf(p, "%d ", level);
@@ -7437,14 +3863,14 @@ static ssize_t show_power_level(struct device *d,
 			     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");
-
+*/
+	p += sprintf(p, " \n");
 	return (p - buf + 1);
-
 }
 
 static DEVICE_ATTR(power_level, S_IWUSR | S_IRUSR, show_power_level,
@@ -7453,8 +3879,62 @@ static DEVICE_ATTR(power_level, S_IWUSR | S_IRUSR, show_power_level,
 static ssize_t show_channels(struct device *d,
 			     struct device_attribute *attr, char *buf)
 {
-	/* all this shit doesn't belong into sysfs anyway */
-	return 0;
+
+	struct iwl_priv *priv = dev_get_drvdata(d);
+	struct ieee80211_channel *channels = NULL;
+	const struct ieee80211_supported_band *supp_band = NULL;
+	int len = 0, i;
+	int count = 0;
+
+	if (!test_bit(STATUS_GEO_CONFIGURED, &priv->status))
+		return -EAGAIN;
+
+	supp_band = iwl_get_hw_mode(priv, IEEE80211_BAND_2GHZ);
+	channels = supp_band->channels;
+	count = supp_band->n_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",
+				ieee80211_frequency_to_channel(
+				channels[i].center_freq),
+				channels[i].max_power,
+				channels[i].flags & IEEE80211_CHAN_RADAR ?
+				" (IEEE 802.11h required)" : "",
+				(!(channels[i].flags & IEEE80211_CHAN_NO_IBSS)
+				|| (channels[i].flags &
+				IEEE80211_CHAN_RADAR)) ? "" :
+				", IBSS",
+				channels[i].flags &
+				IEEE80211_CHAN_PASSIVE_SCAN ?
+				"passive only" : "active/passive");
+
+	supp_band = iwl_get_hw_mode(priv, IEEE80211_BAND_5GHZ);
+	channels = supp_band->channels;
+	count = supp_band->n_channels;
+
+	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",
+				ieee80211_frequency_to_channel(
+				channels[i].center_freq),
+				channels[i].max_power,
+				channels[i].flags & IEEE80211_CHAN_RADAR ?
+				" (IEEE 802.11h required)" : "",
+				((channels[i].flags & IEEE80211_CHAN_NO_IBSS)
+				|| (channels[i].flags &
+				IEEE80211_CHAN_RADAR)) ? "" :
+				", IBSS",
+				channels[i].flags &
+				IEEE80211_CHAN_PASSIVE_SCAN ?
+				"passive only" : "active/passive");
+
+	return len;
 }
 
 static DEVICE_ATTR(channels, S_IRUSR, show_channels, NULL);
@@ -7463,7 +3943,7 @@ 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 iwl4965_notif_statistics);
+	u32 size = sizeof(struct iwl_notif_statistics);
 	u32 len = 0, ofs = 0;
 	u8 *data = (u8 *) & priv->statistics;
 	int rc = 0;
@@ -7497,44 +3977,6 @@ static ssize_t show_statistics(struct device *d,
 
 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 iwl4965_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)
 {
@@ -7546,41 +3988,13 @@ static ssize_t show_status(struct device *d,
 
 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')
-		iwl4965_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')
-		iwl4965_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 iwl4965_setup_deferred_work(struct iwl_priv *priv)
+static void iwl_setup_deferred_work(struct iwl_priv *priv)
 {
 	priv->workqueue = create_workqueue(DRV_NAME);
 
@@ -7589,38 +4003,42 @@ static void iwl4965_setup_deferred_work(struct iwl_priv *priv)
 	INIT_WORK(&priv->up, iwl4965_bg_up);
 	INIT_WORK(&priv->restart, iwl4965_bg_restart);
 	INIT_WORK(&priv->rx_replenish, iwl4965_bg_rx_replenish);
-	INIT_WORK(&priv->scan_completed, iwl4965_bg_scan_completed);
-	INIT_WORK(&priv->request_scan, iwl4965_bg_request_scan);
-	INIT_WORK(&priv->abort_scan, iwl4965_bg_abort_scan);
 	INIT_WORK(&priv->rf_kill, iwl4965_bg_rf_kill);
 	INIT_WORK(&priv->beacon_update, iwl4965_bg_beacon_update);
-	INIT_DELAYED_WORK(&priv->post_associate, iwl4965_bg_post_associate);
-	INIT_DELAYED_WORK(&priv->init_alive_start, iwl4965_bg_init_alive_start);
-	INIT_DELAYED_WORK(&priv->alive_start, iwl4965_bg_alive_start);
-	INIT_DELAYED_WORK(&priv->scan_check, iwl4965_bg_scan_check);
+	INIT_WORK(&priv->set_monitor, iwl4965_bg_set_monitor);
+	INIT_WORK(&priv->run_time_calib_work, iwl_bg_run_time_calib_work);
+	INIT_DELAYED_WORK(&priv->init_alive_start, iwl_bg_init_alive_start);
+	INIT_DELAYED_WORK(&priv->alive_start, iwl_bg_alive_start);
+
+	/* FIXME : remove when resolved PENDING */
+	INIT_WORK(&priv->scan_completed, iwl_bg_scan_completed);
+	iwl_setup_scan_deferred_work(priv);
+
+	if (priv->cfg->ops->lib->setup_deferred_work)
+		priv->cfg->ops->lib->setup_deferred_work(priv);
 
-	iwl4965_hw_setup_deferred_work(priv);
+	init_timer(&priv->statistics_periodic);
+	priv->statistics_periodic.data = (unsigned long)priv;
+	priv->statistics_periodic.function = iwl4965_bg_statistics_periodic;
 
 	tasklet_init(&priv->irq_tasklet, (void (*)(unsigned long))
 		     iwl4965_irq_tasklet, (unsigned long)priv);
 }
 
-static void iwl4965_cancel_deferred_work(struct iwl_priv *priv)
+static void iwl_cancel_deferred_work(struct iwl_priv *priv)
 {
-	iwl4965_hw_cancel_deferred_work(priv);
+	if (priv->cfg->ops->lib->cancel_deferred_work)
+		priv->cfg->ops->lib->cancel_deferred_work(priv);
 
 	cancel_delayed_work_sync(&priv->init_alive_start);
 	cancel_delayed_work(&priv->scan_check);
 	cancel_delayed_work(&priv->alive_start);
-	cancel_delayed_work(&priv->post_associate);
 	cancel_work_sync(&priv->beacon_update);
+	del_timer_sync(&priv->statistics_periodic);
 }
 
 static struct attribute *iwl4965_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_IWL4965_SPECTRUM_MEASUREMENT
@@ -7633,6 +4051,10 @@ static struct attribute *iwl4965_sysfs_entries[] = {
 	&dev_attr_status.attr,
 	&dev_attr_temperature.attr,
 	&dev_attr_tx_power.attr,
+#ifdef CONFIG_IWLWIFI_DEBUG
+	&dev_attr_debug_level.attr,
+#endif
+	&dev_attr_version.attr,
 
 	NULL
 };
@@ -7656,13 +4078,9 @@ static struct ieee80211_ops iwl4965_hw_ops = {
 	.get_stats = iwl4965_mac_get_stats,
 	.get_tx_stats = iwl4965_mac_get_tx_stats,
 	.conf_tx = iwl4965_mac_conf_tx,
-	.get_tsf = iwl4965_mac_get_tsf,
 	.reset_tsf = iwl4965_mac_reset_tsf,
-	.beacon_update = iwl4965_mac_beacon_update,
 	.bss_info_changed = iwl4965_bss_info_changed,
-#ifdef CONFIG_IWL4965_HT
 	.ampdu_action = iwl4965_mac_ampdu_action,
-#endif  /* CONFIG_IWL4965_HT */
 	.hw_scan = iwl4965_mac_hw_scan
 };
 
@@ -7682,7 +4100,9 @@ static int iwl4965_pci_probe(struct pci_dev *pdev, const struct pci_device_id *e
 	/* Disabling hardware scan means that mac80211 will perform scans
 	 * "the hard way", rather than using device's scan. */
 	if (cfg->mod_params->disable_hw_scan) {
-		IWL_DEBUG_INFO("Disabling hw_scan\n");
+		if (cfg->mod_params->debug & IWL_DL_INFO)
+			dev_printk(KERN_DEBUG, &(pdev->dev),
+				   "Disabling hw_scan\n");
 		iwl4965_hw_ops.hw_scan = NULL;
 	}
 
@@ -7701,7 +4121,7 @@ static int iwl4965_pci_probe(struct pci_dev *pdev, const struct pci_device_id *e
 	priv->pci_dev = pdev;
 
 #ifdef CONFIG_IWLWIFI_DEBUG
-	iwl_debug_level = priv->cfg->mod_params->debug;
+	priv->debug_level = priv->cfg->mod_params->debug;
 	atomic_set(&priv->restrict_refcnt, 0);
 #endif
 
@@ -7715,13 +4135,19 @@ static int iwl4965_pci_probe(struct pci_dev *pdev, const struct pci_device_id *e
 
 	pci_set_master(pdev);
 
-	err = pci_set_dma_mask(pdev, DMA_32BIT_MASK);
+	err = pci_set_dma_mask(pdev, DMA_64BIT_MASK);
 	if (!err)
-		err = pci_set_consistent_dma_mask(pdev, DMA_32BIT_MASK);
+		err = pci_set_consistent_dma_mask(pdev, DMA_64BIT_MASK);
+	if (err) {
+		err = pci_set_dma_mask(pdev, DMA_32BIT_MASK);
+		if (!err)
+			err = pci_set_consistent_dma_mask(pdev, DMA_32BIT_MASK);
+		/* both attempts failed: */
 		if (err) {
-			printk(KERN_WARNING DRV_NAME
-				": No suitable DMA available.\n");
+			printk(KERN_WARNING "%s: No suitable DMA available.\n",
+				DRV_NAME);
 			goto out_pci_disable_device;
+		}
 	}
 
 	err = pci_request_regions(pdev, DRV_NAME);
@@ -7747,31 +4173,31 @@ static int iwl4965_pci_probe(struct pci_dev *pdev, const struct pci_device_id *e
 		(unsigned long long) pci_resource_len(pdev, 0));
 	IWL_DEBUG_INFO("pci_resource_base = %p\n", priv->hw_base);
 
+	iwl_hw_detect(priv);
 	printk(KERN_INFO DRV_NAME
-		": Detected Intel Wireless WiFi Link %s\n", priv->cfg->name);
+		": Detected Intel Wireless WiFi Link %s REV=0x%X\n",
+		priv->cfg->name, priv->hw_rev);
 
-	/*****************
-	 * 4. Read EEPROM
-	 *****************/
-	/* nic init */
-	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);
-	err = iwl_poll_bit(priv, CSR_GP_CNTRL,
-		CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY,
-		CSR_GP_CNTRL_REG_FLAG_MAC_CLOCK_READY, 25000);
+	/* amp init */
+	err = priv->cfg->ops->lib->apm_ops.init(priv);
 	if (err < 0) {
-		IWL_DEBUG_INFO("Failed to init the card\n");
+		IWL_DEBUG_INFO("Failed to init APMG\n");
 		goto out_iounmap;
 	}
+	/*****************
+	 * 4. Read EEPROM
+	 *****************/
 	/* Read the EEPROM */
 	err = iwl_eeprom_init(priv);
 	if (err) {
 		IWL_ERROR("Unable to init EEPROM\n");
 		goto out_iounmap;
 	}
-	/* MAC Address location in EEPROM same for 3945/4965 */
+	err = iwl_eeprom_check_version(priv);
+	if (err)
+		goto out_iounmap;
+
+	/* extract MAC Address */
 	iwl_eeprom_get_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);
@@ -7779,19 +4205,18 @@ static int iwl4965_pci_probe(struct pci_dev *pdev, const struct pci_device_id *e
 	/************************
 	 * 5. Setup HW constants
 	 ************************/
-	/* Device-specific setup */
-	if (priv->cfg->ops->lib->set_hw_params(priv)) {
+	if (iwl_set_hw_params(priv)) {
 		IWL_ERROR("failed to set hw parameters\n");
-		goto out_iounmap;
+		goto out_free_eeprom;
 	}
 
 	/*******************
-	 * 6. Setup hw/priv
+	 * 6. Setup priv
 	 *******************/
 
-	err = iwl_setup(priv);
+	err = iwl_init_drv(priv);
 	if (err)
-		goto out_unset_hw_params;
+		goto out_free_eeprom;
 	/* At this point both hw and priv are initialized. */
 
 	/**********************************
@@ -7804,9 +4229,6 @@ static int iwl4965_pci_probe(struct pci_dev *pdev, const struct pci_device_id *e
 		IWL_DEBUG_INFO("Radio disabled.\n");
 	}
 
-	if (priv->cfg->mod_params->enable_qos)
-		priv->qos_data.qos_enable = 1;
-
 	/********************
 	 * 8. Setup services
 	 ********************/
@@ -7817,17 +4239,12 @@ static int iwl4965_pci_probe(struct pci_dev *pdev, const struct pci_device_id *e
 	err = sysfs_create_group(&pdev->dev.kobj, &iwl4965_attribute_group);
 	if (err) {
 		IWL_ERROR("failed to create sysfs device attributes\n");
-		goto out_unset_hw_params;
+		goto out_uninit_drv;
 	}
 
-	err = iwl_dbgfs_register(priv, DRV_NAME);
-	if (err) {
-		IWL_ERROR("failed to create debugfs files\n");
-		goto out_remove_sysfs;
-	}
 
-	iwl4965_setup_deferred_work(priv);
-	iwl4965_setup_rx_handlers(priv);
+	iwl_setup_deferred_work(priv);
+	iwl_setup_rx_handlers(priv);
 
 	/********************
 	 * 9. Conclude
@@ -7835,14 +4252,31 @@ static int iwl4965_pci_probe(struct pci_dev *pdev, const struct pci_device_id *e
 	pci_save_state(pdev);
 	pci_disable_device(pdev);
 
-	/* notify iwlcore to init */
-	iwlcore_low_level_notify(priv, IWLCORE_INIT_EVT);
+	/**********************************
+	 * 10. Setup and register mac80211
+	 **********************************/
+
+	err = iwl_setup_mac(priv);
+	if (err)
+		goto out_remove_sysfs;
+
+	err = iwl_dbgfs_register(priv, DRV_NAME);
+	if (err)
+		IWL_ERROR("failed to create debugfs files\n");
+
+	err = iwl_rfkill_init(priv);
+	if (err)
+		IWL_ERROR("Unable to initialize RFKILL system. "
+				  "Ignoring error: %d\n", err);
+	iwl_power_initialize(priv);
 	return 0;
 
  out_remove_sysfs:
 	sysfs_remove_group(&pdev->dev.kobj, &iwl4965_attribute_group);
- out_unset_hw_params:
-	iwl4965_unset_hw_params(priv);
+ out_uninit_drv:
+	iwl_uninit_drv(priv);
+ out_free_eeprom:
+	iwl_eeprom_free(priv);
  out_iounmap:
 	pci_iounmap(pdev, priv->hw_base);
  out_pci_release_regions:
@@ -7859,8 +4293,6 @@ static int iwl4965_pci_probe(struct pci_dev *pdev, const struct pci_device_id *e
 static void __devexit iwl4965_pci_remove(struct pci_dev *pdev)
 {
 	struct iwl_priv *priv = pci_get_drvdata(pdev);
-	struct list_head *p, *q;
-	int i;
 	unsigned long flags;
 
 	if (!priv)
@@ -7868,6 +4300,9 @@ static void __devexit iwl4965_pci_remove(struct pci_dev *pdev)
 
 	IWL_DEBUG_INFO("*** UNLOAD DRIVER ***\n");
 
+	iwl_dbgfs_unregister(priv);
+	sysfs_remove_group(&pdev->dev.kobj, &iwl4965_attribute_group);
+
 	if (priv->mac80211_registered) {
 		ieee80211_unregister_hw(priv->hw);
 		priv->mac80211_registered = 0;
@@ -7886,26 +4321,15 @@ static void __devexit iwl4965_pci_remove(struct pci_dev *pdev)
 
 	iwl_synchronize_irq(priv);
 
-	/* 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 iwl4965_ibss_seq, list));
-		}
-	}
-
-	iwlcore_low_level_notify(priv, IWLCORE_REMOVE_EVT);
-	iwl_dbgfs_unregister(priv);
-	sysfs_remove_group(&pdev->dev.kobj, &iwl4965_attribute_group);
-
+	iwl_rfkill_unregister(priv);
 	iwl4965_dealloc_ucode_pci(priv);
 
 	if (priv->rxq.bd)
-		iwl4965_rx_queue_free(priv, &priv->rxq);
-	iwl4965_hw_txq_ctx_free(priv);
+		iwl_rx_queue_free(priv, &priv->rxq);
+	iwl_hw_txq_ctx_free(priv);
 
-	iwl4965_unset_hw_params(priv);
-	iwlcore_clear_stations_table(priv);
+	iwl_clear_stations_table(priv);
+	iwl_eeprom_free(priv);
 
 
 	/*netif_stop_queue(dev); */
@@ -7922,8 +4346,7 @@ static void __devexit iwl4965_pci_remove(struct pci_dev *pdev)
 	pci_disable_device(pdev);
 	pci_set_drvdata(pdev, NULL);
 
-	iwl_free_channel_map(priv);
-	iwl4965_free_geos(priv);
+	iwl_uninit_drv(priv);
 
 	if (priv->ibss_beacon)
 		dev_kfree_skb(priv->ibss_beacon);
@@ -7973,6 +4396,19 @@ static int iwl4965_pci_resume(struct pci_dev *pdev)
 static struct pci_device_id iwl_hw_card_ids[] = {
 	{IWL_PCI_DEVICE(0x4229, PCI_ANY_ID, iwl4965_agn_cfg)},
 	{IWL_PCI_DEVICE(0x4230, PCI_ANY_ID, iwl4965_agn_cfg)},
+#ifdef CONFIG_IWL5000
+	{IWL_PCI_DEVICE(0x4232, 0x1205, iwl5100_bg_cfg)},
+	{IWL_PCI_DEVICE(0x4232, 0x1305, iwl5100_bg_cfg)},
+	{IWL_PCI_DEVICE(0x4232, 0x1206, iwl5100_abg_cfg)},
+	{IWL_PCI_DEVICE(0x4232, 0x1306, iwl5100_abg_cfg)},
+	{IWL_PCI_DEVICE(0x4232, 0x1326, iwl5100_abg_cfg)},
+	{IWL_PCI_DEVICE(0x4237, 0x1216, iwl5100_abg_cfg)},
+	{IWL_PCI_DEVICE(0x4232, PCI_ANY_ID, iwl5100_agn_cfg)},
+	{IWL_PCI_DEVICE(0x4235, PCI_ANY_ID, iwl5300_agn_cfg)},
+	{IWL_PCI_DEVICE(0x4236, PCI_ANY_ID, iwl5300_agn_cfg)},
+	{IWL_PCI_DEVICE(0x4237, PCI_ANY_ID, iwl5100_agn_cfg)},
+	{IWL_PCI_DEVICE(0x423A, PCI_ANY_ID, iwl5350_agn_cfg)},
+#endif /* CONFIG_IWL5000 */
 	{0}
 };
 MODULE_DEVICE_TABLE(pci, iwl_hw_card_ids);
@@ -8006,20 +4442,9 @@ static int __init iwl4965_init(void)
 		IWL_ERROR("Unable to initialize PCI module\n");
 		goto error_register;
 	}
-#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");
-		goto error_debug;
-	}
-#endif
 
 	return ret;
 
-#ifdef CONFIG_IWLWIFI_DEBUG
-error_debug:
-	pci_unregister_driver(&iwl_driver);
-#endif
 error_register:
 	iwl4965_rate_control_unregister();
 	return ret;
@@ -8027,9 +4452,6 @@ error_register:
 
 static void __exit iwl4965_exit(void)
 {
-#ifdef CONFIG_IWLWIFI_DEBUG
-	driver_remove_file(&iwl_driver.driver, &driver_attr_debug_level);
-#endif
 	pci_unregister_driver(&iwl_driver);
 	iwl4965_rate_control_unregister();
 }
diff --git a/drivers/net/wireless/libertas/Makefile b/drivers/net/wireless/libertas/Makefile
index f0724e31adfd..02080a3682a9 100644
--- a/drivers/net/wireless/libertas/Makefile
+++ b/drivers/net/wireless/libertas/Makefile
@@ -1,9 +1,5 @@
-libertas-objs := main.o wext.o \
-		rx.o tx.o cmd.o 	  \
-		cmdresp.o scan.o	  \
-		11d.o 		  \
-		debugfs.o	  \
-		ethtool.o assoc.o
+libertas-objs := main.o wext.o rx.o tx.o cmd.o cmdresp.o scan.o 11d.o	\
+		 debugfs.o persistcfg.o ethtool.o assoc.o
 
 usb8xxx-objs += if_usb.o
 libertas_cs-objs += if_cs.o
diff --git a/drivers/net/wireless/libertas/assoc.c b/drivers/net/wireless/libertas/assoc.c
index c9c3640ce9fb..a267d6e65f03 100644
--- a/drivers/net/wireless/libertas/assoc.c
+++ b/drivers/net/wireless/libertas/assoc.c
@@ -603,7 +603,8 @@ static int assoc_helper_channel(struct lbs_private *priv,
 		/* Change mesh channel first; 21.p21 firmware won't let
 		   you change channel otherwise (even though it'll return
 		   an error to this */
-		lbs_mesh_config(priv, 0, assoc_req->channel);
+		lbs_mesh_config(priv, CMD_ACT_MESH_CONFIG_STOP,
+				assoc_req->channel);
 	}
 
 	lbs_deb_assoc("ASSOC: channel: %d -> %d\n",
@@ -642,7 +643,8 @@ static int assoc_helper_channel(struct lbs_private *priv,
 
  restore_mesh:
 	if (priv->mesh_dev)
-		lbs_mesh_config(priv, 1, priv->curbssparams.channel);
+		lbs_mesh_config(priv, CMD_ACT_MESH_CONFIG_START,
+				priv->curbssparams.channel);
 
  done:
 	lbs_deb_leave_args(LBS_DEB_ASSOC, "ret %d", ret);
@@ -1248,7 +1250,7 @@ static int get_common_rates(struct lbs_private *priv,
 	lbs_deb_hex(LBS_DEB_JOIN, "common rates", tmp, tmp_size);
 	lbs_deb_join("TX data rate 0x%02x\n", priv->cur_rate);
 
-	if (!priv->auto_rate) {
+	if (!priv->enablehwauto) {
 		for (i = 0; i < tmp_size; i++) {
 			if (tmp[i] == priv->cur_rate)
 				goto done;
diff --git a/drivers/net/wireless/libertas/cmd.c b/drivers/net/wireless/libertas/cmd.c
index 8124fd9b1353..75427e61898d 100644
--- a/drivers/net/wireless/libertas/cmd.c
+++ b/drivers/net/wireless/libertas/cmd.c
@@ -4,6 +4,7 @@
   */
 
 #include <net/iw_handler.h>
+#include <net/ieee80211.h>
 #include <linux/kfifo.h>
 #include "host.h"
 #include "hostcmd.h"
@@ -109,7 +110,7 @@ int lbs_update_hw_spec(struct lbs_private *priv)
 	 * CF card    firmware 5.0.16p0:   cap 0x00000303
 	 * USB dongle firmware 5.110.17p2: cap 0x00000303
 	 */
-	printk("libertas: %s, fw %u.%u.%up%u, cap 0x%08x\n",
+	lbs_pr_info("%s, fw %u.%u.%up%u, cap 0x%08x\n",
 		print_mac(mac, cmd.permanentaddr),
 		priv->fwrelease >> 24 & 0xff,
 		priv->fwrelease >> 16 & 0xff,
@@ -675,58 +676,60 @@ static int lbs_cmd_802_11_monitor_mode(struct cmd_ds_command *cmd,
 	return 0;
 }
 
-static int lbs_cmd_802_11_rate_adapt_rateset(struct lbs_private *priv,
-					      struct cmd_ds_command *cmd,
-					      u16 cmd_action)
+static __le16 lbs_rate_to_fw_bitmap(int rate, int lower_rates_ok)
 {
-	struct cmd_ds_802_11_rate_adapt_rateset
-	*rateadapt = &cmd->params.rateset;
-
-	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);
-
-	rateadapt->action = cpu_to_le16(cmd_action);
-	rateadapt->enablehwauto = cpu_to_le16(priv->enablehwauto);
-	rateadapt->bitmap = cpu_to_le16(priv->ratebitmap);
-
-	lbs_deb_leave(LBS_DEB_CMD);
-	return 0;
+/*		Bit  	Rate
+*		15:13 Reserved
+*		12    54 Mbps
+*		11    48 Mbps
+*		10    36 Mbps
+*		9     24 Mbps
+*		8     18 Mbps
+*		7     12 Mbps
+*		6     9 Mbps
+*		5     6 Mbps
+*		4     Reserved
+*		3     11 Mbps
+*		2     5.5 Mbps
+*		1     2 Mbps
+*		0     1 Mbps
+**/
+
+	uint16_t ratemask;
+	int i = lbs_data_rate_to_fw_index(rate);
+	if (lower_rates_ok)
+		ratemask = (0x1fef >> (12 - i));
+	else
+		ratemask = (1 << i);
+	return cpu_to_le16(ratemask);
 }
 
-/**
- *  @brief Get the current data rate
- *
- *  @param priv    	A pointer to struct lbs_private structure
- *
- *  @return 	   	The data rate on success, error on failure
- */
-int lbs_get_data_rate(struct lbs_private *priv)
+int lbs_cmd_802_11_rate_adapt_rateset(struct lbs_private *priv,
+				      uint16_t cmd_action)
 {
-	struct cmd_ds_802_11_data_rate cmd;
-	int ret = -1;
+	struct cmd_ds_802_11_rate_adapt_rateset cmd;
+	int ret;
 
 	lbs_deb_enter(LBS_DEB_CMD);
 
-	memset(&cmd, 0, sizeof(cmd));
-	cmd.hdr.size = cpu_to_le16(sizeof(cmd));
-	cmd.action = cpu_to_le16(CMD_ACT_GET_TX_RATE);
-
-	ret = lbs_cmd_with_response(priv, CMD_802_11_DATA_RATE, &cmd);
-	if (ret)
-		goto out;
+	if (!priv->cur_rate && !priv->enablehwauto)
+		return -EINVAL;
 
-	lbs_deb_hex(LBS_DEB_CMD, "DATA_RATE_RESP", (u8 *) &cmd, sizeof (cmd));
+	cmd.hdr.size = cpu_to_le16(sizeof(cmd));
 
-	ret = (int) lbs_fw_index_to_data_rate(cmd.rates[0]);
-	lbs_deb_cmd("DATA_RATE: current rate 0x%02x\n", ret);
+	cmd.action = cpu_to_le16(cmd_action);
+	cmd.enablehwauto = cpu_to_le16(priv->enablehwauto);
+	cmd.bitmap = lbs_rate_to_fw_bitmap(priv->cur_rate, priv->enablehwauto);
+	ret = lbs_cmd_with_response(priv, CMD_802_11_RATE_ADAPT_RATESET, &cmd);
+	if (!ret && cmd_action == CMD_ACT_GET) {
+		priv->ratebitmap = le16_to_cpu(cmd.bitmap);
+		priv->enablehwauto = le16_to_cpu(cmd.enablehwauto);
+	}
 
-out:
 	lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
 	return ret;
 }
+EXPORT_SYMBOL_GPL(lbs_cmd_802_11_rate_adapt_rateset);
 
 /**
  *  @brief Set the data rate
@@ -778,28 +781,6 @@ out:
 	return ret;
 }
 
-static int lbs_cmd_mac_multicast_adr(struct lbs_private *priv,
-				      struct cmd_ds_command *cmd,
-				      u16 cmd_action)
-{
-	struct cmd_ds_mac_multicast_adr *pMCastAdr = &cmd->params.madr;
-
-	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);
-
-	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) priv->nr_of_multicastmacaddr);
-	memcpy(pMCastAdr->maclist, priv->multicastlist,
-	       priv->nr_of_multicastmacaddr * ETH_ALEN);
-
-	lbs_deb_leave(LBS_DEB_CMD);
-	return 0;
-}
-
 /**
  *  @brief Get the radio channel
  *
@@ -1052,24 +1033,69 @@ int lbs_mesh_access(struct lbs_private *priv, uint16_t cmd_action,
 	return ret;
 }
 
-int lbs_mesh_config(struct lbs_private *priv, uint16_t enable, uint16_t chan)
+int lbs_mesh_config_send(struct lbs_private *priv,
+			 struct cmd_ds_mesh_config *cmd,
+			 uint16_t action, uint16_t type)
+{
+	int ret;
+
+	lbs_deb_enter(LBS_DEB_CMD);
+
+	cmd->hdr.command = cpu_to_le16(CMD_MESH_CONFIG);
+	cmd->hdr.size = cpu_to_le16(sizeof(struct cmd_ds_mesh_config));
+	cmd->hdr.result = 0;
+
+	cmd->type = cpu_to_le16(type);
+	cmd->action = cpu_to_le16(action);
+
+	ret = lbs_cmd_with_response(priv, CMD_MESH_CONFIG, cmd);
+
+	lbs_deb_leave(LBS_DEB_CMD);
+	return ret;
+}
+
+/* This function is the CMD_MESH_CONFIG legacy function.  It only handles the
+ * START and STOP actions.  The extended actions supported by CMD_MESH_CONFIG
+ * are all handled by preparing a struct cmd_ds_mesh_config and passing it to
+ * lbs_mesh_config_send.
+ */
+int lbs_mesh_config(struct lbs_private *priv, uint16_t action, uint16_t chan)
 {
 	struct cmd_ds_mesh_config cmd;
+	struct mrvl_meshie *ie;
 
 	memset(&cmd, 0, sizeof(cmd));
-	cmd.action = cpu_to_le16(enable);
 	cmd.channel = cpu_to_le16(chan);
-	cmd.type = cpu_to_le16(priv->mesh_tlv);
-	cmd.hdr.size = cpu_to_le16(sizeof(cmd));
-
-	if (enable) {
-		cmd.length = cpu_to_le16(priv->mesh_ssid_len);
-		memcpy(cmd.data, priv->mesh_ssid, priv->mesh_ssid_len);
+	ie = (struct mrvl_meshie *)cmd.data;
+
+	switch (action) {
+	case CMD_ACT_MESH_CONFIG_START:
+		ie->hdr.id = MFIE_TYPE_GENERIC;
+		ie->val.oui[0] = 0x00;
+		ie->val.oui[1] = 0x50;
+		ie->val.oui[2] = 0x43;
+		ie->val.type = MARVELL_MESH_IE_TYPE;
+		ie->val.subtype = MARVELL_MESH_IE_SUBTYPE;
+		ie->val.version = MARVELL_MESH_IE_VERSION;
+		ie->val.active_protocol_id = MARVELL_MESH_PROTO_ID_HWMP;
+		ie->val.active_metric_id = MARVELL_MESH_METRIC_ID;
+		ie->val.mesh_capability = MARVELL_MESH_CAPABILITY;
+		ie->val.mesh_id_len = priv->mesh_ssid_len;
+		memcpy(ie->val.mesh_id, priv->mesh_ssid, priv->mesh_ssid_len);
+		ie->hdr.len = sizeof(struct mrvl_meshie_val) -
+			IW_ESSID_MAX_SIZE + priv->mesh_ssid_len;
+		cmd.length = cpu_to_le16(sizeof(struct mrvl_meshie_val));
+		break;
+	case CMD_ACT_MESH_CONFIG_STOP:
+		break;
+	default:
+		return -1;
 	}
-	lbs_deb_cmd("mesh config enable %d TLV %x channel %d SSID %s\n",
-		    enable, priv->mesh_tlv, chan,
+	lbs_deb_cmd("mesh config action %d type %x channel %d SSID %s\n",
+		    action, priv->mesh_tlv, chan,
 		    escape_essid(priv->mesh_ssid, priv->mesh_ssid_len));
-	return lbs_cmd_with_response(priv, CMD_MESH_CONFIG, &cmd);
+
+	return lbs_mesh_config_send(priv, &cmd, action, priv->mesh_tlv);
 }
 
 static int lbs_cmd_bcn_ctrl(struct lbs_private * priv,
@@ -1144,7 +1170,7 @@ static void lbs_submit_command(struct lbs_private *priv,
 	struct cmd_header *cmd;
 	uint16_t cmdsize;
 	uint16_t command;
-	int timeo = 5 * HZ;
+	int timeo = 3 * HZ;
 	int ret;
 
 	lbs_deb_enter(LBS_DEB_HOST);
@@ -1162,7 +1188,7 @@ static void lbs_submit_command(struct lbs_private *priv,
 	/* These commands take longer */
 	if (command == CMD_802_11_SCAN || command == CMD_802_11_ASSOCIATE ||
 	    command == CMD_802_11_AUTHENTICATE)
-		timeo = 10 * HZ;
+		timeo = 5 * HZ;
 
 	lbs_deb_cmd("DNLD_CMD: command 0x%04x, seq %d, size %d\n",
 		     command, le16_to_cpu(cmd->seqnum), cmdsize);
@@ -1174,7 +1200,7 @@ static void lbs_submit_command(struct lbs_private *priv,
 		lbs_pr_info("DNLD_CMD: hw_host_to_card failed: %d\n", ret);
 		/* Let the timer kick in and retry, and potentially reset
 		   the whole thing if the condition persists */
-		timeo = HZ;
+		timeo = HZ/4;
 	}
 
 	/* Setup the timer after transmit command */
@@ -1279,8 +1305,7 @@ void lbs_set_mac_control(struct lbs_private *priv)
 	cmd.action = cpu_to_le16(priv->mac_control);
 	cmd.reserved = 0;
 
-	lbs_cmd_async(priv, CMD_MAC_CONTROL,
-		&cmd.hdr, sizeof(cmd));
+	lbs_cmd_async(priv, CMD_MAC_CONTROL, &cmd.hdr, sizeof(cmd));
 
 	lbs_deb_leave(LBS_DEB_CMD);
 }
@@ -1387,15 +1412,6 @@ int lbs_prepare_and_send_command(struct lbs_private *priv,
 						 cmd_action, pdata_buf);
 		break;
 
-	case CMD_802_11_RATE_ADAPT_RATESET:
-		ret = lbs_cmd_802_11_rate_adapt_rateset(priv,
-							 cmdptr, cmd_action);
-		break;
-
-	case CMD_MAC_MULTICAST_ADR:
-		ret = lbs_cmd_mac_multicast_adr(priv, cmdptr, cmd_action);
-		break;
-
 	case CMD_802_11_MONITOR_MODE:
 		ret = lbs_cmd_802_11_monitor_mode(cmdptr,
 				          cmd_action, pdata_buf);
@@ -1484,7 +1500,7 @@ int lbs_prepare_and_send_command(struct lbs_private *priv,
 		ret = lbs_cmd_bcn_ctrl(priv, cmdptr, cmd_action);
 		break;
 	default:
-		lbs_deb_host("PREP_CMD: unknown command 0x%04x\n", cmd_no);
+		lbs_pr_err("PREP_CMD: unknown command 0x%04x\n", cmd_no);
 		ret = -1;
 		break;
 	}
diff --git a/drivers/net/wireless/libertas/cmd.h b/drivers/net/wireless/libertas/cmd.h
index 3dfc2d43c224..a53b51f8bdb4 100644
--- a/drivers/net/wireless/libertas/cmd.h
+++ b/drivers/net/wireless/libertas/cmd.h
@@ -34,18 +34,22 @@ int lbs_update_hw_spec(struct lbs_private *priv);
 int lbs_mesh_access(struct lbs_private *priv, uint16_t cmd_action,
 		    struct cmd_ds_mesh_access *cmd);
 
-int lbs_get_data_rate(struct lbs_private *priv);
 int lbs_set_data_rate(struct lbs_private *priv, u8 rate);
 
 int lbs_get_channel(struct lbs_private *priv);
 int lbs_set_channel(struct lbs_private *priv, u8 channel);
 
+int lbs_mesh_config_send(struct lbs_private *priv,
+			 struct cmd_ds_mesh_config *cmd,
+			 uint16_t action, uint16_t type);
 int lbs_mesh_config(struct lbs_private *priv, uint16_t enable, uint16_t chan);
 
 int lbs_host_sleep_cfg(struct lbs_private *priv, uint32_t criteria);
 int lbs_suspend(struct lbs_private *priv);
-int lbs_resume(struct lbs_private *priv);
+void lbs_resume(struct lbs_private *priv);
 
+int lbs_cmd_802_11_rate_adapt_rateset(struct lbs_private *priv,
+				      uint16_t cmd_action);
 int lbs_cmd_802_11_inactivity_timeout(struct lbs_private *priv,
 				      uint16_t cmd_action, uint16_t *timeout);
 int lbs_cmd_802_11_sleep_params(struct lbs_private *priv, uint16_t cmd_action,
diff --git a/drivers/net/wireless/libertas/cmdresp.c b/drivers/net/wireless/libertas/cmdresp.c
index 5abecb7673e6..24de3c3cf877 100644
--- a/drivers/net/wireless/libertas/cmdresp.c
+++ b/drivers/net/wireless/libertas/cmdresp.c
@@ -203,22 +203,6 @@ static int lbs_ret_802_11_rf_tx_power(struct lbs_private *priv,
 	return 0;
 }
 
-static int lbs_ret_802_11_rate_adapt_rateset(struct lbs_private *priv,
-					      struct cmd_ds_command *resp)
-{
-	struct cmd_ds_802_11_rate_adapt_rateset *rates = &resp->params.rateset;
-
-	lbs_deb_enter(LBS_DEB_CMD);
-
-	if (rates->action == CMD_ACT_GET) {
-		priv->enablehwauto = le16_to_cpu(rates->enablehwauto);
-		priv->ratebitmap = le16_to_cpu(rates->bitmap);
-	}
-
-	lbs_deb_leave(LBS_DEB_CMD);
-	return 0;
-}
-
 static int lbs_ret_802_11_rssi(struct lbs_private *priv,
 				struct cmd_ds_command *resp)
 {
@@ -316,16 +300,11 @@ static inline int handle_cmd_response(struct lbs_private *priv,
 
 		break;
 
-	case CMD_RET(CMD_MAC_MULTICAST_ADR):
 	case CMD_RET(CMD_802_11_RESET):
 	case CMD_RET(CMD_802_11_AUTHENTICATE):
 	case CMD_RET(CMD_802_11_BEACON_STOP):
 		break;
 
-	case CMD_RET(CMD_802_11_RATE_ADAPT_RATESET):
-		ret = lbs_ret_802_11_rate_adapt_rateset(priv, resp);
-		break;
-
 	case CMD_RET(CMD_802_11_RSSI):
 		ret = lbs_ret_802_11_rssi(priv, resp);
 		break;
@@ -376,8 +355,8 @@ static inline int handle_cmd_response(struct lbs_private *priv,
 		break;
 
 	default:
-		lbs_deb_host("CMD_RESP: unknown cmd response 0x%04x\n",
-			     le16_to_cpu(resp->command));
+		lbs_pr_err("CMD_RESP: unknown cmd response 0x%04x\n",
+			   le16_to_cpu(resp->command));
 		break;
 	}
 	lbs_deb_leave(LBS_DEB_HOST);
diff --git a/drivers/net/wireless/libertas/decl.h b/drivers/net/wireless/libertas/decl.h
index b652fa301e19..a8ac974dacac 100644
--- a/drivers/net/wireless/libertas/decl.h
+++ b/drivers/net/wireless/libertas/decl.h
@@ -60,13 +60,17 @@ void lbs_mac_event_disconnected(struct lbs_private *priv);
 
 void lbs_send_iwevcustom_event(struct lbs_private *priv, s8 *str);
 
+/* persistcfg.c */
+void lbs_persist_config_init(struct net_device *net);
+void lbs_persist_config_remove(struct net_device *net);
+
 /* main.c */
 struct chan_freq_power *lbs_get_region_cfp_table(u8 region,
 	int *cfp_no);
 struct lbs_private *lbs_add_card(void *card, struct device *dmdev);
-int lbs_remove_card(struct lbs_private *priv);
+void lbs_remove_card(struct lbs_private *priv);
 int lbs_start_card(struct lbs_private *priv);
-int lbs_stop_card(struct lbs_private *priv);
+void lbs_stop_card(struct lbs_private *priv);
 void lbs_host_to_card_done(struct lbs_private *priv);
 
 int lbs_update_channel(struct lbs_private *priv);
diff --git a/drivers/net/wireless/libertas/defs.h b/drivers/net/wireless/libertas/defs.h
index d39520111062..12e687550bce 100644
--- a/drivers/net/wireless/libertas/defs.h
+++ b/drivers/net/wireless/libertas/defs.h
@@ -40,6 +40,7 @@
 #define LBS_DEB_THREAD	0x00100000
 #define LBS_DEB_HEX	0x00200000
 #define LBS_DEB_SDIO	0x00400000
+#define LBS_DEB_SYSFS	0x00800000
 
 extern unsigned int lbs_debug;
 
@@ -81,7 +82,8 @@ do { if ((lbs_debug & (grp)) == (grp)) \
 #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_deb_sdio(fmt, args...)      LBS_DEB_LL(LBS_DEB_SDIO, " thread", fmt, ##args)
+#define lbs_deb_sdio(fmt, args...)      LBS_DEB_LL(LBS_DEB_SDIO, " sdio", fmt, ##args)
+#define lbs_deb_sysfs(fmt, args...)     LBS_DEB_LL(LBS_DEB_SYSFS, " sysfs", fmt, ##args)
 
 #define lbs_pr_info(format, args...) \
 	printk(KERN_INFO DRV_NAME": " format, ## args)
@@ -170,6 +172,16 @@ static inline void lbs_deb_hex(unsigned int grp, const char *prompt, u8 *buf, in
 
 #define MARVELL_MESH_IE_LENGTH		9
 
+/* Values used to populate the struct mrvl_mesh_ie.  The only time you need this
+ * is when enabling the mesh using CMD_MESH_CONFIG.
+ */
+#define MARVELL_MESH_IE_TYPE		4
+#define MARVELL_MESH_IE_SUBTYPE		0
+#define MARVELL_MESH_IE_VERSION		0
+#define MARVELL_MESH_PROTO_ID_HWMP	0
+#define MARVELL_MESH_METRIC_ID		0
+#define MARVELL_MESH_CAPABILITY		0
+
 /** INT status Bit Definition*/
 #define MRVDRV_TX_DNLD_RDY		0x0001
 #define MRVDRV_RX_UPLD_RDY		0x0002
diff --git a/drivers/net/wireless/libertas/dev.h b/drivers/net/wireless/libertas/dev.h
index 0d9edb9b11f5..f5bb40c54d85 100644
--- a/drivers/net/wireless/libertas/dev.h
+++ b/drivers/net/wireless/libertas/dev.h
@@ -140,6 +140,8 @@ struct lbs_private {
 	wait_queue_head_t waitq;
 	struct workqueue_struct *work_thread;
 
+	struct work_struct mcast_work;
+
 	/** Scanning */
 	struct delayed_work scan_work;
 	struct delayed_work assoc_work;
@@ -151,6 +153,7 @@ struct lbs_private {
 
 	/** Hardware access */
 	int (*hw_host_to_card) (struct lbs_private *priv, u8 type, u8 *payload, u16 nb);
+	void (*reset_card) (struct lbs_private *priv);
 
 	/* Wake On LAN */
 	uint32_t wol_criteria;
@@ -234,8 +237,8 @@ struct lbs_private {
 	/** 802.11 statistics */
 //	struct cmd_DS_802_11_GET_STAT wlan802_11Stat;
 
-	u16 enablehwauto;
-	u16 ratebitmap;
+	uint16_t enablehwauto;
+	uint16_t ratebitmap;
 
 	u32 fragthsd;
 	u32 rtsthsd;
@@ -293,7 +296,6 @@ struct lbs_private {
 
 	/** data rate stuff */
 	u8 cur_rate;
-	u8 auto_rate;
 
 	/** RF calibration data */
 
diff --git a/drivers/net/wireless/libertas/host.h b/drivers/net/wireless/libertas/host.h
index 3915c3144fad..c92e41b4faf4 100644
--- a/drivers/net/wireless/libertas/host.h
+++ b/drivers/net/wireless/libertas/host.h
@@ -256,6 +256,23 @@ enum cmd_mesh_access_opts {
 	CMD_ACT_MESH_GET_AUTOSTART_ENABLED,
 };
 
+/* Define actions and types for CMD_MESH_CONFIG */
+enum cmd_mesh_config_actions {
+	CMD_ACT_MESH_CONFIG_STOP = 0,
+	CMD_ACT_MESH_CONFIG_START,
+	CMD_ACT_MESH_CONFIG_SET,
+	CMD_ACT_MESH_CONFIG_GET,
+};
+
+enum cmd_mesh_config_types {
+	CMD_TYPE_MESH_SET_BOOTFLAG = 1,
+	CMD_TYPE_MESH_SET_BOOTTIME,
+	CMD_TYPE_MESH_SET_DEF_CHANNEL,
+	CMD_TYPE_MESH_SET_MESH_IE,
+	CMD_TYPE_MESH_GET_DEFAULTS,
+	CMD_TYPE_MESH_GET_MESH_IE, /* GET_DEFAULTS is superset of GET_MESHIE */
+};
+
 /** Card Event definition */
 #define MACREG_INT_CODE_TX_PPA_FREE		0
 #define MACREG_INT_CODE_TX_DMA_DONE		1
diff --git a/drivers/net/wireless/libertas/hostcmd.h b/drivers/net/wireless/libertas/hostcmd.h
index f29bc5bbda3e..913b480211a9 100644
--- a/drivers/net/wireless/libertas/hostcmd.h
+++ b/drivers/net/wireless/libertas/hostcmd.h
@@ -219,6 +219,7 @@ struct cmd_ds_mac_control {
 };
 
 struct cmd_ds_mac_multicast_adr {
+	struct cmd_header hdr;
 	__le16 action;
 	__le16 nr_of_adrs;
 	u8 maclist[ETH_ALEN * MRVDRV_MAX_MULTICAST_LIST_SIZE];
@@ -499,6 +500,7 @@ struct cmd_ds_802_11_data_rate {
 };
 
 struct cmd_ds_802_11_rate_adapt_rateset {
+	struct cmd_header hdr;
 	__le16 action;
 	__le16 enablehwauto;
 	__le16 bitmap;
@@ -702,8 +704,6 @@ struct cmd_ds_command {
 		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_rate_adapt_rateset rateset;
-		struct cmd_ds_mac_multicast_adr madr;
 		struct cmd_ds_802_11_ad_hoc_join adj;
 		struct cmd_ds_802_11_rssi rssi;
 		struct cmd_ds_802_11_rssi_rsp rssirsp;
diff --git a/drivers/net/wireless/libertas/if_cs.c b/drivers/net/wireless/libertas/if_cs.c
index 54280e292ea5..04d7a251e3f0 100644
--- a/drivers/net/wireless/libertas/if_cs.c
+++ b/drivers/net/wireless/libertas/if_cs.c
@@ -122,7 +122,7 @@ static inline void if_cs_write16(struct if_cs_card *card, uint reg, u16 val)
 static inline void if_cs_write16_rep(
 	struct if_cs_card *card,
 	uint reg,
-	void *buf,
+	const void *buf,
 	unsigned long count)
 {
 	if (debug_output)
@@ -148,76 +148,149 @@ static int if_cs_poll_while_fw_download(struct if_cs_card *card, uint addr, u8 r
 {
 	int i;
 
-	for (i = 0; i < 1000; i++) {
+	for (i = 0; i < 100000; i++) {
 		u8 val = if_cs_read8(card, addr);
 		if (val == reg)
 			return i;
-		udelay(500);
+		udelay(5);
 	}
 	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_POWER_DOWN		0x0008
-#define IF_CS_H_IC_HOST_EVENT		0x0010
-#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
+/*
+ * First the bitmasks for the host/card interrupt/status registers:
+ */
+#define IF_CS_BIT_TX			0x0001
+#define IF_CS_BIT_RX			0x0002
+#define IF_CS_BIT_COMMAND		0x0004
+#define IF_CS_BIT_RESP			0x0008
+#define IF_CS_BIT_EVENT			0x0010
+#define	IF_CS_BIT_MASK			0x001f
 
-#define IF_CS_H_CMD_LEN			0x00000018
 
-#define IF_CS_H_CMD			0x0000001A
 
-#define IF_CS_C_READ_LEN		0x00000024
+/*
+ * It's not really clear to me what the host status register is for. It
+ * needs to be set almost in union with "host int cause". The following
+ * bits from above are used:
+ *
+ *   IF_CS_BIT_TX         driver downloaded a data packet
+ *   IF_CS_BIT_RX         driver got a data packet
+ *   IF_CS_BIT_COMMAND    driver downloaded a command
+ *   IF_CS_BIT_RESP       not used (has some meaning with powerdown)
+ *   IF_CS_BIT_EVENT      driver read a host event
+ */
+#define IF_CS_HOST_STATUS		0x00000000
 
-#define IF_CS_H_READ			0x00000010
+/*
+ * With the host int cause register can the host (that is, Linux) cause
+ * an interrupt in the firmware, to tell the firmware about those events:
+ *
+ *   IF_CS_BIT_TX         a data packet has been downloaded
+ *   IF_CS_BIT_RX         a received data packet has retrieved
+ *   IF_CS_BIT_COMMAND    a firmware block or a command has been downloaded
+ *   IF_CS_BIT_RESP       not used (has some meaning with powerdown)
+ *   IF_CS_BIT_EVENT      a host event (link lost etc) has been retrieved
+ */
+#define IF_CS_HOST_INT_CAUSE		0x00000002
 
-/* Card control registers and their bit definitions */
+/*
+ * The host int mask register is used to enable/disable interrupt.  However,
+ * I have the suspicion that disabled interrupts are lost.
+ */
+#define IF_CS_HOST_INT_MASK		0x00000004
 
-#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
+/*
+ * Used to send or receive data packets:
+ */
+#define IF_CS_WRITE			0x00000016
+#define IF_CS_WRITE_LEN			0x00000014
+#define IF_CS_READ			0x00000010
+#define IF_CS_READ_LEN			0x00000024
 
-#define IF_CS_C_INT_CAUSE		0x00000022
-#define	IF_CS_C_IC_MASK			0x001f
+/*
+ * Used to send commands (and to send firmware block) and to
+ * receive command responses:
+ */
+#define IF_CS_CMD			0x0000001A
+#define IF_CS_CMD_LEN			0x00000018
+#define IF_CS_RESP			0x00000012
+#define IF_CS_RESP_LEN			0x00000030
 
-#define IF_CS_C_SQ_READ_LOW		0x00000028
-#define IF_CS_C_SQ_HELPER_OK		0x10
+/*
+ * The card status registers shows what the card/firmware actually
+ * accepts:
+ *
+ *   IF_CS_BIT_TX        you may send a data packet
+ *   IF_CS_BIT_RX        you may retrieve a data packet
+ *   IF_CS_BIT_COMMAND   you may send a command
+ *   IF_CS_BIT_RESP      you may retrieve a command response
+ *   IF_CS_BIT_EVENT     the card has a event for use (link lost, snr low etc)
+ *
+ * When reading this register several times, you will get back the same
+ * results --- with one exception: the IF_CS_BIT_EVENT clear itself
+ * automatically.
+ *
+ * Not that we don't rely on BIT_RX,_BIT_RESP or BIT_EVENT because
+ * we handle this via the card int cause register.
+ */
+#define IF_CS_CARD_STATUS		0x00000020
+#define IF_CS_CARD_STATUS_MASK		0x7f00
 
-#define IF_CS_C_CMD_LEN			0x00000030
+/*
+ * The card int cause register is used by the card/firmware to notify us
+ * about the following events:
+ *
+ *   IF_CS_BIT_TX        a data packet has successfully been sentx
+ *   IF_CS_BIT_RX        a data packet has been received and can be retrieved
+ *   IF_CS_BIT_COMMAND   not used
+ *   IF_CS_BIT_RESP      the firmware has a command response for us
+ *   IF_CS_BIT_EVENT     the card has a event for use (link lost, snr low etc)
+ */
+#define IF_CS_CARD_INT_CAUSE		0x00000022
 
-#define IF_CS_C_CMD			0x00000012
+/*
+ * This is used to for handshaking with the card's bootloader/helper image
+ * to synchronize downloading of firmware blocks.
+ */
+#define IF_CS_SQ_READ_LOW		0x00000028
+#define IF_CS_SQ_HELPER_OK		0x10
 
+/*
+ * The scratch register tells us ...
+ *
+ * IF_CS_SCRATCH_BOOT_OK     the bootloader runs
+ * IF_CS_SCRATCH_HELPER_OK   the helper firmware already runs
+ */
 #define IF_CS_SCRATCH			0x0000003F
+#define IF_CS_SCRATCH_BOOT_OK		0x00
+#define IF_CS_SCRATCH_HELPER_OK		0x5a
 
+/*
+ * Used to detect ancient chips:
+ */
+#define IF_CS_PRODUCT_ID		0x0000001C
+#define IF_CS_CF8385_B1_REV		0x12
 
 
 /********************************************************************/
-/* I/O                                                              */
+/* I/O and interrupt handling                                       */
 /********************************************************************/
 
+static inline void if_cs_enable_ints(struct if_cs_card *card)
+{
+	lbs_deb_enter(LBS_DEB_CS);
+	if_cs_write16(card, IF_CS_HOST_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_HOST_INT_MASK, IF_CS_BIT_MASK);
+}
+
 /*
  * Called from if_cs_host_to_card to send a command to the hardware
  */
@@ -228,11 +301,12 @@ static int if_cs_send_cmd(struct lbs_private *priv, u8 *buf, u16 nb)
 	int loops = 0;
 
 	lbs_deb_enter(LBS_DEB_CS);
+	if_cs_disable_ints(card);
 
 	/* Is hardware ready? */
 	while (1) {
-		u16 val = if_cs_read16(card, IF_CS_C_STATUS);
-		if (val & IF_CS_C_S_CMD_DNLD_RDY)
+		u16 status = if_cs_read16(card, IF_CS_CARD_STATUS);
+		if (status & IF_CS_BIT_COMMAND)
 			break;
 		if (++loops > 100) {
 			lbs_pr_err("card not ready for commands\n");
@@ -241,51 +315,56 @@ static int if_cs_send_cmd(struct lbs_private *priv, u8 *buf, u16 nb)
 		mdelay(1);
 	}
 
-	if_cs_write16(card, IF_CS_H_CMD_LEN, nb);
+	if_cs_write16(card, IF_CS_CMD_LEN, nb);
 
-	if_cs_write16_rep(card, IF_CS_H_CMD, buf, nb / 2);
+	if_cs_write16_rep(card, IF_CS_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]);
+		if_cs_write8(card, IF_CS_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);
+	if_cs_write16(card, IF_CS_HOST_STATUS, IF_CS_BIT_COMMAND);
 
 	/* "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);
+	if_cs_write16(card, IF_CS_HOST_INT_CAUSE, IF_CS_BIT_COMMAND);
 	ret = 0;
 
 done:
+	if_cs_enable_ints(card);
 	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(struct lbs_private *priv, u8 *buf, u16 nb)
 {
 	struct if_cs_card *card = (struct if_cs_card *)priv->card;
+	u16 status;
 
 	lbs_deb_enter(LBS_DEB_CS);
+	if_cs_disable_ints(card);
 
-	if_cs_write16(card, IF_CS_H_WRITE_LEN, nb);
+	status = if_cs_read16(card, IF_CS_CARD_STATUS);
+	BUG_ON((status & IF_CS_BIT_TX) == 0);
+
+	if_cs_write16(card, IF_CS_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_cs_write16_rep(card, IF_CS_WRITE, buf, nb / 2);
 	if (nb & 1)
-		if_cs_write8(card, IF_CS_H_WRITE, buf[nb-1]);
+		if_cs_write8(card, IF_CS_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);
+	if_cs_write16(card, IF_CS_HOST_STATUS, IF_CS_BIT_TX);
+	if_cs_write16(card, IF_CS_HOST_INT_CAUSE, IF_CS_BIT_TX);
+	if_cs_enable_ints(card);
 
 	lbs_deb_leave(LBS_DEB_CS);
 }
 
-
 /*
  * Get the command result out of the card.
  */
@@ -293,27 +372,28 @@ static int if_cs_receive_cmdres(struct lbs_private *priv, u8 *data, u32 *len)
 {
 	unsigned long flags;
 	int ret = -1;
-	u16 val;
+	u16 status;
 
 	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");
+	status = if_cs_read16(priv->card, IF_CS_CARD_STATUS);
+	if ((status & IF_CS_BIT_RESP) == 0) {
+		lbs_pr_err("no cmd response in card\n");
+		*len = 0;
 		goto out;
 	}
 
-	*len = if_cs_read16(priv->card, IF_CS_C_CMD_LEN);
+	*len = if_cs_read16(priv->card, IF_CS_RESP_LEN);
 	if ((*len == 0) || (*len > LBS_CMD_BUFFER_SIZE)) {
 		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_cs_read16_rep(priv->card, IF_CS_RESP, data, *len/sizeof(u16));
 	if (*len & 1)
-		data[*len-1] = if_cs_read8(priv->card, IF_CS_C_CMD);
+		data[*len-1] = if_cs_read8(priv->card, IF_CS_RESP);
 
 	/* This is a workaround for a firmware that reports too much
 	 * bytes */
@@ -330,7 +410,6 @@ out:
 	return ret;
 }
 
-
 static struct sk_buff *if_cs_receive_data(struct lbs_private *priv)
 {
 	struct sk_buff *skb = NULL;
@@ -339,7 +418,7 @@ static struct sk_buff *if_cs_receive_data(struct lbs_private *priv)
 
 	lbs_deb_enter(LBS_DEB_CS);
 
-	len = if_cs_read16(priv->card, IF_CS_C_READ_LEN);
+	len = if_cs_read16(priv->card, IF_CS_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++;
@@ -354,38 +433,19 @@ static struct sk_buff *if_cs_receive_data(struct lbs_private *priv)
 	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_cs_read16_rep(priv->card, IF_CS_READ, data, len/sizeof(u16));
 	if (len & 1)
-		data[len-1] = if_cs_read8(priv->card, IF_CS_H_READ);
+		data[len-1] = if_cs_read8(priv->card, IF_CS_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);
+	if_cs_write16(priv->card, IF_CS_HOST_STATUS, IF_CS_BIT_RX);
+	if_cs_write16(priv->card, IF_CS_HOST_INT_CAUSE, IF_CS_BIT_RX);
 
 out:
 	lbs_deb_leave_args(LBS_DEB_CS, "ret %p", skb);
 	return skb;
 }
 
-
-
-/********************************************************************/
-/* 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 = data;
@@ -394,10 +454,10 @@ static irqreturn_t if_cs_interrupt(int irq, void *data)
 
 	lbs_deb_enter(LBS_DEB_CS);
 
-	cause = if_cs_read16(card, IF_CS_C_INT_CAUSE);
-	if_cs_write16(card, IF_CS_C_INT_CAUSE, cause & IF_CS_C_IC_MASK);
-
+	/* Ask card interrupt cause register if there is something for us */
+	cause = if_cs_read16(card, IF_CS_CARD_INT_CAUSE);
 	lbs_deb_cs("cause 0x%04x\n", cause);
+
 	if (cause == 0) {
 		/* Not for us */
 		return IRQ_NONE;
@@ -409,11 +469,7 @@ static irqreturn_t if_cs_interrupt(int irq, void *data)
 		return IRQ_HANDLED;
 	}
 
-	/* TODO: I'm not sure what the best ordering is */
-
-	cause = if_cs_read16(card, IF_CS_C_STATUS) & IF_CS_C_S_MASK;
-
-	if (cause & IF_CS_C_S_RX_UPLD_RDY) {
+	if (cause & IF_CS_BIT_RX) {
 		struct sk_buff *skb;
 		lbs_deb_cs("rx packet\n");
 		skb = if_cs_receive_data(priv);
@@ -421,16 +477,16 @@ static irqreturn_t if_cs_interrupt(int irq, void *data)
 			lbs_process_rxed_packet(priv, skb);
 	}
 
-	if (cause & IF_CS_H_IC_TX_OVER) {
-		lbs_deb_cs("tx over\n");
+	if (cause & IF_CS_BIT_TX) {
+		lbs_deb_cs("tx done\n");
 		lbs_host_to_card_done(priv);
 	}
 
-	if (cause & IF_CS_C_S_CMD_UPLD_RDY) {
+	if (cause & IF_CS_BIT_RESP) {
 		unsigned long flags;
 		u8 i;
 
-		lbs_deb_cs("cmd upload ready\n");
+		lbs_deb_cs("cmd resp\n");
 		spin_lock_irqsave(&priv->driver_lock, flags);
 		i = (priv->resp_idx == 0) ? 1 : 0;
 		spin_unlock_irqrestore(&priv->driver_lock, flags);
@@ -444,15 +500,17 @@ static irqreturn_t if_cs_interrupt(int irq, void *data)
 		spin_unlock_irqrestore(&priv->driver_lock, flags);
 	}
 
-	if (cause & IF_CS_H_IC_HOST_EVENT) {
-		u16 event = if_cs_read16(priv->card, IF_CS_C_STATUS)
-			& IF_CS_C_S_STATUS_MASK;
-		if_cs_write16(priv->card, IF_CS_H_INT_CAUSE,
-			IF_CS_H_IC_HOST_EVENT);
-		lbs_deb_cs("eventcause 0x%04x\n", event);
-		lbs_queue_event(priv, event >> 8 & 0xff);
+	if (cause & IF_CS_BIT_EVENT) {
+		u16 status = if_cs_read16(priv->card, IF_CS_CARD_STATUS);
+		if_cs_write16(priv->card, IF_CS_HOST_INT_CAUSE,
+			IF_CS_BIT_EVENT);
+		lbs_queue_event(priv, (status & IF_CS_CARD_STATUS_MASK) >> 8);
 	}
 
+	/* Clear interrupt cause */
+	if_cs_write16(card, IF_CS_CARD_INT_CAUSE, cause & IF_CS_BIT_MASK);
+
+	lbs_deb_leave(LBS_DEB_CS);
 	return IRQ_HANDLED;
 }
 
@@ -482,11 +540,11 @@ static int if_cs_prog_helper(struct if_cs_card *card)
 	/* "If the value is 0x5a, the firmware is already
 	 * downloaded successfully"
 	 */
-	if (scratch == 0x5a)
+	if (scratch == IF_CS_SCRATCH_HELPER_OK)
 		goto done;
 
 	/* "If the value is != 00, it is invalid value of register */
-	if (scratch != 0x00) {
+	if (scratch != IF_CS_SCRATCH_BOOT_OK) {
 		ret = -ENODEV;
 		goto done;
 	}
@@ -514,26 +572,26 @@ static int if_cs_prog_helper(struct if_cs_card *card)
 
 		/* "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);
+		if_cs_write16(card, IF_CS_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,
+			if_cs_write16_rep(card, IF_CS_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);
+		if_cs_write8(card, IF_CS_HOST_STATUS, IF_CS_BIT_COMMAND);
 
 		/* "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);
+		if_cs_write16(card, IF_CS_HOST_INT_CAUSE, IF_CS_BIT_COMMAND);
 
 		/* "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);
+		ret = if_cs_poll_while_fw_download(card, IF_CS_CARD_STATUS,
+			IF_CS_BIT_COMMAND);
 		if (ret < 0) {
 			lbs_pr_err("can't download helper at 0x%x, ret %d\n",
 				sent, ret);
@@ -575,14 +633,15 @@ static int if_cs_prog_real(struct if_cs_card *card)
 	}
 	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);
+	ret = if_cs_poll_while_fw_download(card, IF_CS_SQ_READ_LOW,
+		IF_CS_SQ_HELPER_OK);
 	if (ret < 0) {
 		lbs_pr_err("helper firmware doesn't answer\n");
 		goto err_release;
 	}
 
 	for (sent = 0; sent < fw->size; sent += len) {
-		len = if_cs_read16(card, IF_CS_C_SQ_READ_LOW);
+		len = if_cs_read16(card, IF_CS_SQ_READ_LOW);
 		if (len & 1) {
 			retry++;
 			lbs_pr_info("odd, need to retry this firmware block\n");
@@ -600,16 +659,16 @@ static int if_cs_prog_real(struct if_cs_card *card)
 		}
 
 
-		if_cs_write16(card, IF_CS_H_CMD_LEN, len);
+		if_cs_write16(card, IF_CS_CMD_LEN, len);
 
-		if_cs_write16_rep(card, IF_CS_H_CMD,
+		if_cs_write16_rep(card, IF_CS_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);
+		if_cs_write8(card, IF_CS_HOST_STATUS, IF_CS_BIT_COMMAND);
+		if_cs_write16(card, IF_CS_HOST_INT_CAUSE, IF_CS_BIT_COMMAND);
 
-		ret = if_cs_poll_while_fw_download(card, IF_CS_C_STATUS,
-			IF_CS_C_S_CMD_DNLD_RDY);
+		ret = if_cs_poll_while_fw_download(card, IF_CS_CARD_STATUS,
+			IF_CS_BIT_COMMAND);
 		if (ret < 0) {
 			lbs_pr_err("can't download firmware at 0x%x\n", sent);
 			goto err_release;
@@ -806,6 +865,12 @@ static int if_cs_probe(struct pcmcia_device *p_dev)
 	       p_dev->irq.AssignedIRQ, p_dev->io.BasePort1,
 	       p_dev->io.BasePort1 + p_dev->io.NumPorts1 - 1);
 
+	/* Check if we have a current silicon */
+	if (if_cs_read8(card, IF_CS_PRODUCT_ID) < IF_CS_CF8385_B1_REV) {
+		lbs_pr_err("old chips like 8385 rev B1 aren't supported\n");
+		ret = -ENODEV;
+		goto out2;
+	}
 
 	/* Load the firmware early, before calling into libertas.ko */
 	ret = if_cs_prog_helper(card);
@@ -837,7 +902,7 @@ static int if_cs_probe(struct pcmcia_device *p_dev)
 
 	/* Clear any interrupt cause that happend while sending
 	 * firmware/initializing card */
-	if_cs_write16(card, IF_CS_C_INT_CAUSE, IF_CS_C_IC_MASK);
+	if_cs_write16(card, IF_CS_CARD_INT_CAUSE, IF_CS_BIT_MASK);
 	if_cs_enable_ints(card);
 
 	/* And finally bring the card up */
diff --git a/drivers/net/wireless/libertas/if_sdio.c b/drivers/net/wireless/libertas/if_sdio.c
index 51f664bbee9d..b54e2ea8346b 100644
--- a/drivers/net/wireless/libertas/if_sdio.c
+++ b/drivers/net/wireless/libertas/if_sdio.c
@@ -1,7 +1,7 @@
 /*
  *  linux/drivers/net/wireless/libertas/if_sdio.c
  *
- *  Copyright 2007 Pierre Ossman
+ *  Copyright 2007-2008 Pierre Ossman
  *
  * Inspired by if_cs.c, Copyright 2007 Holger Schurig
  *
@@ -266,13 +266,10 @@ static int if_sdio_card_to_host(struct if_sdio_card *card)
 
 	/*
 	 * The transfer must be in one transaction or the firmware
-	 * goes suicidal.
+	 * goes suicidal. There's no way to guarantee that for all
+	 * controllers, but we can at least try.
 	 */
-	chunk = size;
-	if ((chunk > card->func->cur_blksize) || (chunk > 512)) {
-		chunk = (chunk + card->func->cur_blksize - 1) /
-			card->func->cur_blksize * card->func->cur_blksize;
-	}
+	chunk = sdio_align_size(card->func, size);
 
 	ret = sdio_readsb(card->func, card->buffer, card->ioport, chunk);
 	if (ret)
@@ -392,7 +389,7 @@ static int if_sdio_prog_helper(struct if_sdio_card *card)
 	unsigned long timeout;
 	u8 *chunk_buffer;
 	u32 chunk_size;
-	u8 *firmware;
+	const u8 *firmware;
 	size_t size;
 
 	lbs_deb_enter(LBS_DEB_SDIO);
@@ -508,7 +505,7 @@ static int if_sdio_prog_real(struct if_sdio_card *card)
 	unsigned long timeout;
 	u8 *chunk_buffer;
 	u32 chunk_size;
-	u8 *firmware;
+	const u8 *firmware;
 	size_t size, req_size;
 
 	lbs_deb_enter(LBS_DEB_SDIO);
@@ -696,13 +693,10 @@ static int if_sdio_host_to_card(struct lbs_private *priv,
 
 	/*
 	 * The transfer must be in one transaction or the firmware
-	 * goes suicidal.
+	 * goes suicidal. There's no way to guarantee that for all
+	 * controllers, but we can at least try.
 	 */
-	size = nb + 4;
-	if ((size > card->func->cur_blksize) || (size > 512)) {
-		size = (size + card->func->cur_blksize - 1) /
-			card->func->cur_blksize * card->func->cur_blksize;
-	}
+	size = sdio_align_size(card->func, nb + 4);
 
 	packet = kzalloc(sizeof(struct if_sdio_packet) + size,
 			GFP_ATOMIC);
diff --git a/drivers/net/wireless/libertas/if_usb.c b/drivers/net/wireless/libertas/if_usb.c
index 36288b29abf7..632c291404ab 100644
--- a/drivers/net/wireless/libertas/if_usb.c
+++ b/drivers/net/wireless/libertas/if_usb.c
@@ -7,6 +7,10 @@
 #include <linux/netdevice.h>
 #include <linux/usb.h>
 
+#ifdef CONFIG_OLPC
+#include <asm/olpc.h>
+#endif
+
 #define DRV_NAME "usb8xxx"
 
 #include "host.h"
@@ -146,6 +150,14 @@ static void if_usb_fw_timeo(unsigned long priv)
 	wake_up(&cardp->fw_wq);
 }
 
+#ifdef CONFIG_OLPC
+static void if_usb_reset_olpc_card(struct lbs_private *priv)
+{
+	printk(KERN_CRIT "Resetting OLPC wireless via EC...\n");
+	olpc_ec_cmd(0x25, NULL, 0, NULL, 0);
+}
+#endif
+
 /**
  *  @brief sets the configuration values
  *  @param ifnum	interface number
@@ -231,6 +243,11 @@ static int if_usb_probe(struct usb_interface *intf,
 	cardp->priv->fw_ready = 1;
 
 	priv->hw_host_to_card = if_usb_host_to_card;
+#ifdef CONFIG_OLPC
+	if (machine_is_olpc())
+		priv->reset_card = if_usb_reset_olpc_card;
+#endif
+
 	cardp->boot2_version = udev->descriptor.bcdDevice;
 
 	if_usb_submit_rx_urb(cardp);
@@ -293,7 +310,7 @@ static void if_usb_disconnect(struct usb_interface *intf)
 static int if_usb_send_fw_pkt(struct if_usb_card *cardp)
 {
 	struct fwdata *fwdata = cardp->ep_out_buf;
-	uint8_t *firmware = cardp->fw->data;
+	const uint8_t *firmware = cardp->fw->data;
 
 	/* If we got a CRC failure on the last block, back
 	   up and retry it */
@@ -364,6 +381,11 @@ static int if_usb_reset_device(struct if_usb_card *cardp)
 	ret = usb_reset_device(cardp->udev);
 	msleep(100);
 
+#ifdef CONFIG_OLPC
+	if (ret && machine_is_olpc())
+		if_usb_reset_olpc_card(NULL);
+#endif
+
 	lbs_deb_leave_args(LBS_DEB_USB, "ret %d", ret);
 
 	return ret;
@@ -746,7 +768,7 @@ static int if_usb_issue_boot_command(struct if_usb_card *cardp, int ivalue)
  *         len               image length
  *  @return     0 or -1
  */
-static int check_fwfile_format(uint8_t *data, uint32_t totlen)
+static int check_fwfile_format(const uint8_t *data, uint32_t totlen)
 {
 	uint32_t bincmd, exit;
 	uint32_t blksize, offset, len;
diff --git a/drivers/net/wireless/libertas/main.c b/drivers/net/wireless/libertas/main.c
index acfc4bfcc262..14d5d61cec4c 100644
--- a/drivers/net/wireless/libertas/main.c
+++ b/drivers/net/wireless/libertas/main.c
@@ -11,6 +11,7 @@
 #include <linux/if_arp.h>
 #include <linux/kthread.h>
 #include <linux/kfifo.h>
+#include <linux/stddef.h>
 
 #include <net/iw_handler.h>
 #include <net/ieee80211.h>
@@ -343,14 +344,15 @@ static ssize_t lbs_mesh_set(struct device *dev,
 {
 	struct lbs_private *priv = to_net_dev(dev)->priv;
 	int enable;
-	int ret;
+	int ret, action = CMD_ACT_MESH_CONFIG_STOP;
 
 	sscanf(buf, "%x", &enable);
 	enable = !!enable;
 	if (enable == !!priv->mesh_dev)
 		return count;
-
-	ret = lbs_mesh_config(priv, enable, priv->curbssparams.channel);
+	if (enable)
+		action = CMD_ACT_MESH_CONFIG_START;
+	ret = lbs_mesh_config(priv, action, priv->curbssparams.channel);
 	if (ret)
 		return ret;
 
@@ -446,6 +448,8 @@ static int lbs_mesh_stop(struct net_device *dev)
 
 	spin_unlock_irq(&priv->driver_lock);
 
+	schedule_work(&priv->mcast_work);
+
 	lbs_deb_leave(LBS_DEB_MESH);
 	return 0;
 }
@@ -467,6 +471,8 @@ static int lbs_eth_stop(struct net_device *dev)
 	netif_stop_queue(dev);
 	spin_unlock_irq(&priv->driver_lock);
 
+	schedule_work(&priv->mcast_work);
+
 	lbs_deb_leave(LBS_DEB_NET);
 	return 0;
 }
@@ -563,89 +569,116 @@ done:
 	return ret;
 }
 
-static int lbs_copy_multicast_address(struct lbs_private *priv,
-				     struct net_device *dev)
+
+static inline int mac_in_list(unsigned char *list, int list_len,
+			      unsigned char *mac)
 {
-	int i = 0;
-	struct dev_mc_list *mcptr = dev->mc_list;
+	while (list_len) {
+		if (!memcmp(list, mac, ETH_ALEN))
+			return 1;
+		list += ETH_ALEN;
+		list_len--;
+	}
+	return 0;
+}
+
+
+static int lbs_add_mcast_addrs(struct cmd_ds_mac_multicast_adr *cmd,
+			       struct net_device *dev, int nr_addrs)
+{
+	int i = nr_addrs;
+	struct dev_mc_list *mc_list;
+	DECLARE_MAC_BUF(mac);
+
+	if ((dev->flags & (IFF_UP|IFF_MULTICAST)) != (IFF_UP|IFF_MULTICAST))
+		return nr_addrs;
+
+	netif_addr_lock_bh(dev);
+	for (mc_list = dev->mc_list; mc_list; mc_list = mc_list->next) {
+		if (mac_in_list(cmd->maclist, nr_addrs, mc_list->dmi_addr)) {
+			lbs_deb_net("mcast address %s:%s skipped\n", dev->name,
+				    print_mac(mac, mc_list->dmi_addr));
+			continue;
+		}
 
-	for (i = 0; i < dev->mc_count; i++) {
-		memcpy(&priv->multicastlist[i], mcptr->dmi_addr, ETH_ALEN);
-		mcptr = mcptr->next;
+		if (i == MRVDRV_MAX_MULTICAST_LIST_SIZE)
+			break;
+		memcpy(&cmd->maclist[6*i], mc_list->dmi_addr, ETH_ALEN);
+		lbs_deb_net("mcast address %s:%s added to filter\n", dev->name,
+			    print_mac(mac, mc_list->dmi_addr));
+		i++;
 	}
+	netif_addr_unlock_bh(dev);
+	if (mc_list)
+		return -EOVERFLOW;
+
 	return i;
 }
 
-static void lbs_set_multicast_list(struct net_device *dev)
+static void lbs_set_mcast_worker(struct work_struct *work)
 {
-	struct lbs_private *priv = dev->priv;
-	int old_mac_control;
-	DECLARE_MAC_BUF(mac);
+	struct lbs_private *priv = container_of(work, struct lbs_private, mcast_work);
+	struct cmd_ds_mac_multicast_adr mcast_cmd;
+	int dev_flags;
+	int nr_addrs;
+	int old_mac_control = priv->mac_control;
 
 	lbs_deb_enter(LBS_DEB_NET);
 
-	old_mac_control = priv->mac_control;
-
-	if (dev->flags & IFF_PROMISC) {
-		lbs_deb_net("enable promiscuous mode\n");
-		priv->mac_control |=
-		    CMD_ACT_MAC_PROMISCUOUS_ENABLE;
-		priv->mac_control &=
-		    ~(CMD_ACT_MAC_ALL_MULTICAST_ENABLE |
-		      CMD_ACT_MAC_MULTICAST_ENABLE);
-	} else {
-		/* Multicast */
-		priv->mac_control &=
-		    ~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");
-			priv->mac_control |=
-			    CMD_ACT_MAC_ALL_MULTICAST_ENABLE;
-			priv->mac_control &=
-			    ~CMD_ACT_MAC_MULTICAST_ENABLE;
-		} else {
-			priv->mac_control &=
-			    ~CMD_ACT_MAC_ALL_MULTICAST_ENABLE;
-
-			if (!dev->mc_count) {
-				lbs_deb_net("no multicast addresses, "
-				       "disabling multicast\n");
-				priv->mac_control &=
-				    ~CMD_ACT_MAC_MULTICAST_ENABLE;
-			} else {
-				int i;
-
-				priv->mac_control |=
-				    CMD_ACT_MAC_MULTICAST_ENABLE;
-
-				priv->nr_of_multicastmacaddr =
-				    lbs_copy_multicast_address(priv, 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: %s\n",
-					       i, print_mac(mac,
-					       priv->multicastlist[i]));
-				}
-				/* send multicast addresses to firmware */
-				lbs_prepare_and_send_command(priv,
-						      CMD_MAC_MULTICAST_ADR,
-						      CMD_ACT_SET, 0, 0,
-						      NULL);
-			}
-		}
+	dev_flags = priv->dev->flags;
+	if (priv->mesh_dev)
+		dev_flags |= priv->mesh_dev->flags;
+
+	if (dev_flags & IFF_PROMISC) {
+		priv->mac_control |= CMD_ACT_MAC_PROMISCUOUS_ENABLE;
+		priv->mac_control &= ~(CMD_ACT_MAC_ALL_MULTICAST_ENABLE |
+				       CMD_ACT_MAC_MULTICAST_ENABLE);
+		goto out_set_mac_control;
+	} else if (dev_flags & IFF_ALLMULTI) {
+	do_allmulti:
+		priv->mac_control |= CMD_ACT_MAC_ALL_MULTICAST_ENABLE;
+		priv->mac_control &= ~(CMD_ACT_MAC_PROMISCUOUS_ENABLE |
+				       CMD_ACT_MAC_MULTICAST_ENABLE);
+		goto out_set_mac_control;
 	}
 
+	/* Once for priv->dev, again for priv->mesh_dev if it exists */
+	nr_addrs = lbs_add_mcast_addrs(&mcast_cmd, priv->dev, 0);
+	if (nr_addrs >= 0 && priv->mesh_dev)
+		nr_addrs = lbs_add_mcast_addrs(&mcast_cmd, priv->mesh_dev, nr_addrs);
+	if (nr_addrs < 0)
+		goto do_allmulti;
+
+	if (nr_addrs) {
+		int size = offsetof(struct cmd_ds_mac_multicast_adr,
+				    maclist[6*nr_addrs]);
+
+		mcast_cmd.action = cpu_to_le16(CMD_ACT_SET);
+		mcast_cmd.hdr.size = cpu_to_le16(size);
+		mcast_cmd.nr_of_adrs = cpu_to_le16(nr_addrs);
+
+		lbs_cmd_async(priv, CMD_MAC_MULTICAST_ADR, &mcast_cmd.hdr, size);
+
+		priv->mac_control |= CMD_ACT_MAC_MULTICAST_ENABLE;
+	} else
+		priv->mac_control &= ~CMD_ACT_MAC_MULTICAST_ENABLE;
+
+	priv->mac_control &= ~(CMD_ACT_MAC_PROMISCUOUS_ENABLE |
+			       CMD_ACT_MAC_ALL_MULTICAST_ENABLE);
+ out_set_mac_control:
 	if (priv->mac_control != old_mac_control)
 		lbs_set_mac_control(priv);
 
 	lbs_deb_leave(LBS_DEB_NET);
 }
 
+static void lbs_set_multicast_list(struct net_device *dev)
+{
+	struct lbs_private *priv = dev->priv;
+
+	schedule_work(&priv->mcast_work);
+}
+
 /**
  *  @brief This function handles the major jobs in the LBS driver.
  *  It handles all events generated by firmware, RX data received
@@ -689,20 +722,20 @@ static int lbs_thread(void *data)
 			shouldsleep = 1;	/* Something is en route to the device already */
 		else if (priv->tx_pending_len > 0)
 			shouldsleep = 0;	/* We've a packet to send */
+		else if (priv->resp_len[priv->resp_idx])
+			shouldsleep = 0;	/* We have a command response */
 		else if (priv->cur_cmd)
 			shouldsleep = 1;	/* Can't send a command; one already running */
 		else if (!list_empty(&priv->cmdpendingq))
 			shouldsleep = 0;	/* We have a command to send */
 		else if (__kfifo_len(priv->event_fifo))
 			shouldsleep = 0;	/* We have an event to process */
-		else if (priv->resp_len[priv->resp_idx])
-			shouldsleep = 0;	/* We have a command response */
 		else
 			shouldsleep = 1;	/* No command */
 
 		if (shouldsleep) {
 			lbs_deb_thread("sleeping, connect_status %d, "
-				"ps_mode %d, ps_state %d\n",
+				"psmode %d, psstate %d\n",
 				priv->connect_status,
 				priv->psmode, priv->psstate);
 			spin_unlock_irq(&priv->driver_lock);
@@ -749,16 +782,21 @@ static int lbs_thread(void *data)
 		if (priv->cmd_timed_out && priv->cur_cmd) {
 			struct cmd_ctrl_node *cmdnode = priv->cur_cmd;
 
-			if (++priv->nr_retries > 10) {
-				lbs_pr_info("Excessive timeouts submitting command %x\n",
-					    le16_to_cpu(cmdnode->cmdbuf->command));
+			if (++priv->nr_retries > 3) {
+				lbs_pr_info("Excessive timeouts submitting "
+					"command 0x%04x\n",
+					le16_to_cpu(cmdnode->cmdbuf->command));
 				lbs_complete_command(priv, cmdnode, -ETIMEDOUT);
 				priv->nr_retries = 0;
+				if (priv->reset_card)
+					priv->reset_card(priv);
 			} else {
 				priv->cur_cmd = NULL;
 				priv->dnld_sent = DNLD_RES_RECEIVED;
-				lbs_pr_info("requeueing command %x due to timeout (#%d)\n",
-					    le16_to_cpu(cmdnode->cmdbuf->command), priv->nr_retries);
+				lbs_pr_info("requeueing command 0x%04x due "
+					"to timeout (#%d)\n",
+					le16_to_cpu(cmdnode->cmdbuf->command),
+					priv->nr_retries);
 
 				/* Stick it back at the _top_ of the pending queue
 				   for immediate resubmission */
@@ -890,7 +928,7 @@ int lbs_suspend(struct lbs_private *priv)
 }
 EXPORT_SYMBOL_GPL(lbs_suspend);
 
-int lbs_resume(struct lbs_private *priv)
+void lbs_resume(struct lbs_private *priv)
 {
 	lbs_deb_enter(LBS_DEB_FW);
 
@@ -906,7 +944,6 @@ int lbs_resume(struct lbs_private *priv)
 		netif_device_attach(priv->mesh_dev);
 
 	lbs_deb_leave(LBS_DEB_FW);
-	return 0;
 }
 EXPORT_SYMBOL_GPL(lbs_resume);
 
@@ -929,20 +966,10 @@ static int lbs_setup_firmware(struct lbs_private *priv)
 	 */
 	memset(priv->current_addr, 0xff, ETH_ALEN);
 	ret = lbs_update_hw_spec(priv);
-	if (ret) {
-		ret = -1;
+	if (ret)
 		goto done;
-	}
 
 	lbs_set_mac_control(priv);
-
-	ret = lbs_get_data_rate(priv);
-	if (ret < 0) {
-		ret = -1;
-		goto done;
-	}
-
-	ret = 0;
 done:
 	lbs_deb_leave_args(LBS_DEB_FW, "ret %d", ret);
 	return ret;
@@ -960,12 +987,11 @@ static void command_timer_fn(unsigned long data)
 	lbs_deb_enter(LBS_DEB_CMD);
 	spin_lock_irqsave(&priv->driver_lock, flags);
 
-	if (!priv->cur_cmd) {
-		lbs_pr_info("Command timer expired; no pending command\n");
+	if (!priv->cur_cmd)
 		goto out;
-	}
 
-	lbs_pr_info("Command %x timed out\n", le16_to_cpu(priv->cur_cmd->cmdbuf->command));
+	lbs_pr_info("command 0x%04x timed out\n",
+		le16_to_cpu(priv->cur_cmd->cmdbuf->command));
 
 	priv->cmd_timed_out = 1;
 	wake_up_interruptible(&priv->waitq);
@@ -1019,7 +1045,7 @@ static int lbs_init_adapter(struct lbs_private *priv)
 	priv->curbssparams.channel = DEFAULT_AD_HOC_CHANNEL;
 	priv->mac_control = CMD_ACT_MAC_RX_ON | CMD_ACT_MAC_TX_ON;
 	priv->radioon = RADIO_ON;
-	priv->auto_rate = 1;
+	priv->enablehwauto = 1;
 	priv->capability = WLAN_CAPABILITY_SHORT_PREAMBLE;
 	priv->psmode = LBS802_11POWERMODECAM;
 	priv->psstate = PS_STATE_FULL_POWER;
@@ -1134,6 +1160,7 @@ struct lbs_private *lbs_add_card(void *card, struct device *dmdev)
 	priv->work_thread = create_singlethread_workqueue("lbs_worker");
 	INIT_DELAYED_WORK(&priv->assoc_work, lbs_association_worker);
 	INIT_DELAYED_WORK(&priv->scan_work, lbs_scan_worker);
+	INIT_WORK(&priv->mcast_work, lbs_set_mcast_worker);
 	INIT_WORK(&priv->sync_channel, lbs_sync_channel_worker);
 
 	sprintf(priv->mesh_ssid, "mesh");
@@ -1156,7 +1183,7 @@ done:
 EXPORT_SYMBOL_GPL(lbs_add_card);
 
 
-int lbs_remove_card(struct lbs_private *priv)
+void lbs_remove_card(struct lbs_private *priv)
 {
 	struct net_device *dev = priv->dev;
 	union iwreq_data wrqu;
@@ -1168,8 +1195,9 @@ int lbs_remove_card(struct lbs_private *priv)
 
 	dev = priv->dev;
 
-	cancel_delayed_work(&priv->scan_work);
-	cancel_delayed_work(&priv->assoc_work);
+	cancel_delayed_work_sync(&priv->scan_work);
+	cancel_delayed_work_sync(&priv->assoc_work);
+	cancel_work_sync(&priv->mcast_work);
 	destroy_workqueue(priv->work_thread);
 
 	if (priv->psmode == LBS802_11POWERMODEMAX_PSP) {
@@ -1191,7 +1219,6 @@ int lbs_remove_card(struct lbs_private *priv)
 	free_netdev(dev);
 
 	lbs_deb_leave(LBS_DEB_MAIN);
-	return 0;
 }
 EXPORT_SYMBOL_GPL(lbs_remove_card);
 
@@ -1236,9 +1263,11 @@ int lbs_start_card(struct lbs_private *priv)
 		   useful */
 
 		priv->mesh_tlv = 0x100 + 291;
-		if (lbs_mesh_config(priv, 1, priv->curbssparams.channel)) {
+		if (lbs_mesh_config(priv, CMD_ACT_MESH_CONFIG_START,
+				    priv->curbssparams.channel)) {
 			priv->mesh_tlv = 0x100 + 37;
-			if (lbs_mesh_config(priv, 1, priv->curbssparams.channel))
+			if (lbs_mesh_config(priv, CMD_ACT_MESH_CONFIG_START,
+					    priv->curbssparams.channel))
 				priv->mesh_tlv = 0;
 		}
 		if (priv->mesh_tlv) {
@@ -1262,24 +1291,28 @@ done:
 EXPORT_SYMBOL_GPL(lbs_start_card);
 
 
-int lbs_stop_card(struct lbs_private *priv)
+void lbs_stop_card(struct lbs_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);
 
+	if (!priv)
+		goto out;
+
 	netif_stop_queue(priv->dev);
 	netif_carrier_off(priv->dev);
 
 	lbs_debugfs_remove_one(priv);
 	device_remove_file(&dev->dev, &dev_attr_lbs_rtap);
-	if (priv->mesh_tlv)
+	if (priv->mesh_tlv) {
 		device_remove_file(&dev->dev, &dev_attr_lbs_mesh);
+	}
 
 	/* Flush pending command nodes */
+	del_timer_sync(&priv->command_timer);
 	spin_lock_irqsave(&priv->driver_lock, flags);
 	list_for_each_entry(cmdnode, &priv->cmdpendingq, list) {
 		cmdnode->result = -ENOENT;
@@ -1290,8 +1323,8 @@ int lbs_stop_card(struct lbs_private *priv)
 
 	unregister_netdev(dev);
 
-	lbs_deb_leave_args(LBS_DEB_MAIN, "ret %d", ret);
-	return ret;
+out:
+	lbs_deb_leave(LBS_DEB_MAIN);
 }
 EXPORT_SYMBOL_GPL(lbs_stop_card);
 
@@ -1332,6 +1365,8 @@ static int lbs_add_mesh(struct lbs_private *priv)
 #ifdef	WIRELESS_EXT
 	mesh_dev->wireless_handlers = (struct iw_handler_def *)&mesh_handler_def;
 #endif
+	mesh_dev->flags |= IFF_BROADCAST | IFF_MULTICAST;
+	mesh_dev->set_multicast_list = lbs_set_multicast_list;
 	/* Register virtual mesh interface */
 	ret = register_netdev(mesh_dev);
 	if (ret) {
@@ -1343,6 +1378,8 @@ static int lbs_add_mesh(struct lbs_private *priv)
 	if (ret)
 		goto err_unregister;
 
+	lbs_persist_config_init(mesh_dev);
+
 	/* Everything successful */
 	ret = 0;
 	goto done;
@@ -1369,8 +1406,9 @@ static void lbs_remove_mesh(struct lbs_private *priv)
 
 	lbs_deb_enter(LBS_DEB_MESH);
 	netif_stop_queue(mesh_dev);
-	netif_carrier_off(priv->mesh_dev);
+	netif_carrier_off(mesh_dev);
 	sysfs_remove_group(&(mesh_dev->dev.kobj), &lbs_mesh_attr_group);
+	lbs_persist_config_remove(mesh_dev);
 	unregister_netdev(mesh_dev);
 	priv->mesh_dev = NULL;
 	free_netdev(mesh_dev);
@@ -1533,10 +1571,11 @@ static void lbs_remove_rtap(struct lbs_private *priv)
 {
 	lbs_deb_enter(LBS_DEB_MAIN);
 	if (priv->rtap_net_dev == NULL)
-		return;
+		goto out;
 	unregister_netdev(priv->rtap_net_dev);
 	free_netdev(priv->rtap_net_dev);
 	priv->rtap_net_dev = NULL;
+out:
 	lbs_deb_leave(LBS_DEB_MAIN);
 }
 
@@ -1563,7 +1602,6 @@ static int lbs_add_rtap(struct lbs_private *priv)
 	rtap_dev->stop = lbs_rtap_stop;
 	rtap_dev->get_stats = lbs_rtap_get_stats;
 	rtap_dev->hard_start_xmit = lbs_rtap_hard_start_xmit;
-	rtap_dev->set_multicast_list = lbs_set_multicast_list;
 	rtap_dev->priv = priv;
 	SET_NETDEV_DEV(rtap_dev, priv->dev->dev.parent);
 
diff --git a/drivers/net/wireless/libertas/persistcfg.c b/drivers/net/wireless/libertas/persistcfg.c
new file mode 100644
index 000000000000..6d0ff8decaf7
--- /dev/null
+++ b/drivers/net/wireless/libertas/persistcfg.c
@@ -0,0 +1,453 @@
+#include <linux/moduleparam.h>
+#include <linux/delay.h>
+#include <linux/etherdevice.h>
+#include <linux/netdevice.h>
+#include <linux/if_arp.h>
+#include <linux/kthread.h>
+#include <linux/kfifo.h>
+
+#include "host.h"
+#include "decl.h"
+#include "dev.h"
+#include "wext.h"
+#include "debugfs.h"
+#include "scan.h"
+#include "assoc.h"
+#include "cmd.h"
+
+static int mesh_get_default_parameters(struct device *dev,
+				       struct mrvl_mesh_defaults *defs)
+{
+	struct lbs_private *priv = to_net_dev(dev)->priv;
+	struct cmd_ds_mesh_config cmd;
+	int ret;
+
+	memset(&cmd, 0, sizeof(struct cmd_ds_mesh_config));
+	ret = lbs_mesh_config_send(priv, &cmd, CMD_ACT_MESH_CONFIG_GET,
+				   CMD_TYPE_MESH_GET_DEFAULTS);
+
+	if (ret)
+		return -EOPNOTSUPP;
+
+	memcpy(defs, &cmd.data[0], sizeof(struct mrvl_mesh_defaults));
+
+	return 0;
+}
+
+/**
+ * @brief Get function for sysfs attribute bootflag
+ */
+static ssize_t bootflag_get(struct device *dev,
+			    struct device_attribute *attr, char *buf)
+{
+	struct mrvl_mesh_defaults defs;
+	int ret;
+
+	ret = mesh_get_default_parameters(dev, &defs);
+
+	if (ret)
+		return ret;
+
+	return snprintf(buf, 12, "0x%x\n", le32_to_cpu(defs.bootflag));
+}
+
+/**
+ * @brief Set function for sysfs attribute bootflag
+ */
+static ssize_t bootflag_set(struct device *dev, struct device_attribute *attr,
+			    const char *buf, size_t count)
+{
+	struct lbs_private *priv = to_net_dev(dev)->priv;
+	struct cmd_ds_mesh_config cmd;
+	uint32_t datum;
+	int ret;
+
+	memset(&cmd, 0, sizeof(cmd));
+	ret = sscanf(buf, "%x", &datum);
+	if (ret != 1)
+		return -EINVAL;
+
+	*((__le32 *)&cmd.data[0]) = cpu_to_le32(!!datum);
+	cmd.length = cpu_to_le16(sizeof(uint32_t));
+	ret = lbs_mesh_config_send(priv, &cmd, CMD_ACT_MESH_CONFIG_SET,
+				   CMD_TYPE_MESH_SET_BOOTFLAG);
+	if (ret)
+		return ret;
+
+	return strlen(buf);
+}
+
+/**
+ * @brief Get function for sysfs attribute boottime
+ */
+static ssize_t boottime_get(struct device *dev,
+			    struct device_attribute *attr, char *buf)
+{
+	struct mrvl_mesh_defaults defs;
+	int ret;
+
+	ret = mesh_get_default_parameters(dev, &defs);
+
+	if (ret)
+		return ret;
+
+	return snprintf(buf, 12, "0x%x\n", defs.boottime);
+}
+
+/**
+ * @brief Set function for sysfs attribute boottime
+ */
+static ssize_t boottime_set(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct lbs_private *priv = to_net_dev(dev)->priv;
+	struct cmd_ds_mesh_config cmd;
+	uint32_t datum;
+	int ret;
+
+	memset(&cmd, 0, sizeof(cmd));
+	ret = sscanf(buf, "%x", &datum);
+	if (ret != 1)
+		return -EINVAL;
+
+	/* A too small boot time will result in the device booting into
+	 * standalone (no-host) mode before the host can take control of it,
+	 * so the change will be hard to revert.  This may be a desired
+	 * feature (e.g to configure a very fast boot time for devices that
+	 * will not be attached to a host), but dangerous.  So I'm enforcing a
+	 * lower limit of 20 seconds:  remove and recompile the driver if this
+	 * does not work for you.
+	 */
+	datum = (datum < 20) ? 20 : datum;
+	cmd.data[0] = datum;
+	cmd.length = cpu_to_le16(sizeof(uint8_t));
+	ret = lbs_mesh_config_send(priv, &cmd, CMD_ACT_MESH_CONFIG_SET,
+				   CMD_TYPE_MESH_SET_BOOTTIME);
+	if (ret)
+		return ret;
+
+	return strlen(buf);
+}
+
+/**
+ * @brief Get function for sysfs attribute channel
+ */
+static ssize_t channel_get(struct device *dev,
+			   struct device_attribute *attr, char *buf)
+{
+	struct mrvl_mesh_defaults defs;
+	int ret;
+
+	ret = mesh_get_default_parameters(dev, &defs);
+
+	if (ret)
+		return ret;
+
+	return snprintf(buf, 12, "0x%x\n", le16_to_cpu(defs.channel));
+}
+
+/**
+ * @brief Set function for sysfs attribute channel
+ */
+static ssize_t channel_set(struct device *dev, struct device_attribute *attr,
+			   const char *buf, size_t count)
+{
+	struct lbs_private *priv = to_net_dev(dev)->priv;
+	struct cmd_ds_mesh_config cmd;
+	uint16_t datum;
+	int ret;
+
+	memset(&cmd, 0, sizeof(cmd));
+	ret = sscanf(buf, "%hx", &datum);
+	if (ret != 1 || datum < 1 || datum > 11)
+		return -EINVAL;
+
+	*((__le16 *)&cmd.data[0]) = cpu_to_le16(datum);
+	cmd.length = cpu_to_le16(sizeof(uint16_t));
+	ret = lbs_mesh_config_send(priv, &cmd, CMD_ACT_MESH_CONFIG_SET,
+				   CMD_TYPE_MESH_SET_DEF_CHANNEL);
+	if (ret)
+		return ret;
+
+	return strlen(buf);
+}
+
+/**
+ * @brief Get function for sysfs attribute mesh_id
+ */
+static ssize_t mesh_id_get(struct device *dev, struct device_attribute *attr,
+			   char *buf)
+{
+	struct mrvl_mesh_defaults defs;
+	int maxlen;
+	int ret;
+
+	ret = mesh_get_default_parameters(dev, &defs);
+
+	if (ret)
+		return ret;
+
+	if (defs.meshie.val.mesh_id_len > IW_ESSID_MAX_SIZE) {
+		lbs_pr_err("inconsistent mesh ID length");
+		defs.meshie.val.mesh_id_len = IW_ESSID_MAX_SIZE;
+	}
+
+	/* SSID not null terminated: reserve room for \0 + \n */
+	maxlen = defs.meshie.val.mesh_id_len + 2;
+	maxlen = (PAGE_SIZE > maxlen) ? maxlen : PAGE_SIZE;
+
+	defs.meshie.val.mesh_id[defs.meshie.val.mesh_id_len] = '\0';
+
+	return snprintf(buf, maxlen, "%s\n", defs.meshie.val.mesh_id);
+}
+
+/**
+ * @brief Set function for sysfs attribute mesh_id
+ */
+static ssize_t mesh_id_set(struct device *dev, struct device_attribute *attr,
+			   const char *buf, size_t count)
+{
+	struct cmd_ds_mesh_config cmd;
+	struct mrvl_mesh_defaults defs;
+	struct mrvl_meshie *ie;
+	struct lbs_private *priv = to_net_dev(dev)->priv;
+	int len;
+	int ret;
+
+	if (count < 2 || count > IW_ESSID_MAX_SIZE + 1)
+		return -EINVAL;
+
+	memset(&cmd, 0, sizeof(struct cmd_ds_mesh_config));
+	ie = (struct mrvl_meshie *) &cmd.data[0];
+
+	/* fetch all other Information Element parameters */
+	ret = mesh_get_default_parameters(dev, &defs);
+
+	cmd.length = cpu_to_le16(sizeof(struct mrvl_meshie));
+
+	/* transfer IE elements */
+	memcpy(ie, &defs.meshie, sizeof(struct mrvl_meshie));
+
+	len = count - 1;
+	memcpy(ie->val.mesh_id, buf, len);
+	/* SSID len */
+	ie->val.mesh_id_len = len;
+	/* IE len */
+	ie->hdr.len = sizeof(struct mrvl_meshie_val) - IW_ESSID_MAX_SIZE + len;
+
+	ret = lbs_mesh_config_send(priv, &cmd, CMD_ACT_MESH_CONFIG_SET,
+				   CMD_TYPE_MESH_SET_MESH_IE);
+	if (ret)
+		return ret;
+
+	return strlen(buf);
+}
+
+/**
+ * @brief Get function for sysfs attribute protocol_id
+ */
+static ssize_t protocol_id_get(struct device *dev,
+			       struct device_attribute *attr, char *buf)
+{
+	struct mrvl_mesh_defaults defs;
+	int ret;
+
+	ret = mesh_get_default_parameters(dev, &defs);
+
+	if (ret)
+		return ret;
+
+	return snprintf(buf, 5, "%d\n", defs.meshie.val.active_protocol_id);
+}
+
+/**
+ * @brief Set function for sysfs attribute protocol_id
+ */
+static ssize_t protocol_id_set(struct device *dev,
+		struct device_attribute *attr, const char *buf, size_t count)
+{
+	struct cmd_ds_mesh_config cmd;
+	struct mrvl_mesh_defaults defs;
+	struct mrvl_meshie *ie;
+	struct lbs_private *priv = to_net_dev(dev)->priv;
+	uint32_t datum;
+	int ret;
+
+	memset(&cmd, 0, sizeof(cmd));
+	ret = sscanf(buf, "%x", &datum);
+	if (ret != 1)
+		return -EINVAL;
+
+	/* fetch all other Information Element parameters */
+	ret = mesh_get_default_parameters(dev, &defs);
+
+	cmd.length = cpu_to_le16(sizeof(struct mrvl_meshie));
+
+	/* transfer IE elements */
+	ie = (struct mrvl_meshie *) &cmd.data[0];
+	memcpy(ie, &defs.meshie, sizeof(struct mrvl_meshie));
+	/* update protocol id */
+	ie->val.active_protocol_id = datum;
+
+	ret = lbs_mesh_config_send(priv, &cmd, CMD_ACT_MESH_CONFIG_SET,
+				   CMD_TYPE_MESH_SET_MESH_IE);
+	if (ret)
+		return ret;
+
+	return strlen(buf);
+}
+
+/**
+ * @brief Get function for sysfs attribute metric_id
+ */
+static ssize_t metric_id_get(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct mrvl_mesh_defaults defs;
+	int ret;
+
+	ret = mesh_get_default_parameters(dev, &defs);
+
+	if (ret)
+		return ret;
+
+	return snprintf(buf, 5, "%d\n", defs.meshie.val.active_metric_id);
+}
+
+/**
+ * @brief Set function for sysfs attribute metric_id
+ */
+static ssize_t metric_id_set(struct device *dev, struct device_attribute *attr,
+			     const char *buf, size_t count)
+{
+	struct cmd_ds_mesh_config cmd;
+	struct mrvl_mesh_defaults defs;
+	struct mrvl_meshie *ie;
+	struct lbs_private *priv = to_net_dev(dev)->priv;
+	uint32_t datum;
+	int ret;
+
+	memset(&cmd, 0, sizeof(cmd));
+	ret = sscanf(buf, "%x", &datum);
+	if (ret != 1)
+		return -EINVAL;
+
+	/* fetch all other Information Element parameters */
+	ret = mesh_get_default_parameters(dev, &defs);
+
+	cmd.length = cpu_to_le16(sizeof(struct mrvl_meshie));
+
+	/* transfer IE elements */
+	ie = (struct mrvl_meshie *) &cmd.data[0];
+	memcpy(ie, &defs.meshie, sizeof(struct mrvl_meshie));
+	/* update metric id */
+	ie->val.active_metric_id = datum;
+
+	ret = lbs_mesh_config_send(priv, &cmd, CMD_ACT_MESH_CONFIG_SET,
+				   CMD_TYPE_MESH_SET_MESH_IE);
+	if (ret)
+		return ret;
+
+	return strlen(buf);
+}
+
+/**
+ * @brief Get function for sysfs attribute capability
+ */
+static ssize_t capability_get(struct device *dev,
+		struct device_attribute *attr, char *buf)
+{
+	struct mrvl_mesh_defaults defs;
+	int ret;
+
+	ret = mesh_get_default_parameters(dev, &defs);
+
+	if (ret)
+		return ret;
+
+	return snprintf(buf, 5, "%d\n", defs.meshie.val.mesh_capability);
+}
+
+/**
+ * @brief Set function for sysfs attribute capability
+ */
+static ssize_t capability_set(struct device *dev, struct device_attribute *attr,
+			      const char *buf, size_t count)
+{
+	struct cmd_ds_mesh_config cmd;
+	struct mrvl_mesh_defaults defs;
+	struct mrvl_meshie *ie;
+	struct lbs_private *priv = to_net_dev(dev)->priv;
+	uint32_t datum;
+	int ret;
+
+	memset(&cmd, 0, sizeof(cmd));
+	ret = sscanf(buf, "%x", &datum);
+	if (ret != 1)
+		return -EINVAL;
+
+	/* fetch all other Information Element parameters */
+	ret = mesh_get_default_parameters(dev, &defs);
+
+	cmd.length = cpu_to_le16(sizeof(struct mrvl_meshie));
+
+	/* transfer IE elements */
+	ie = (struct mrvl_meshie *) &cmd.data[0];
+	memcpy(ie, &defs.meshie, sizeof(struct mrvl_meshie));
+	/* update value */
+	ie->val.mesh_capability = datum;
+
+	ret = lbs_mesh_config_send(priv, &cmd, CMD_ACT_MESH_CONFIG_SET,
+				   CMD_TYPE_MESH_SET_MESH_IE);
+	if (ret)
+		return ret;
+
+	return strlen(buf);
+}
+
+
+static DEVICE_ATTR(bootflag, 0644, bootflag_get, bootflag_set);
+static DEVICE_ATTR(boottime, 0644, boottime_get, boottime_set);
+static DEVICE_ATTR(channel, 0644, channel_get, channel_set);
+static DEVICE_ATTR(mesh_id, 0644, mesh_id_get, mesh_id_set);
+static DEVICE_ATTR(protocol_id, 0644, protocol_id_get, protocol_id_set);
+static DEVICE_ATTR(metric_id, 0644, metric_id_get, metric_id_set);
+static DEVICE_ATTR(capability, 0644, capability_get, capability_set);
+
+static struct attribute *boot_opts_attrs[] = {
+	&dev_attr_bootflag.attr,
+	&dev_attr_boottime.attr,
+	&dev_attr_channel.attr,
+	NULL
+};
+
+static struct attribute_group boot_opts_group = {
+	.name = "boot_options",
+	.attrs = boot_opts_attrs,
+};
+
+static struct attribute *mesh_ie_attrs[] = {
+	&dev_attr_mesh_id.attr,
+	&dev_attr_protocol_id.attr,
+	&dev_attr_metric_id.attr,
+	&dev_attr_capability.attr,
+	NULL
+};
+
+static struct attribute_group mesh_ie_group = {
+	.name = "mesh_ie",
+	.attrs = mesh_ie_attrs,
+};
+
+void lbs_persist_config_init(struct net_device *dev)
+{
+	int ret;
+	ret = sysfs_create_group(&(dev->dev.kobj), &boot_opts_group);
+	ret = sysfs_create_group(&(dev->dev.kobj), &mesh_ie_group);
+}
+
+void lbs_persist_config_remove(struct net_device *dev)
+{
+	sysfs_remove_group(&(dev->dev.kobj), &boot_opts_group);
+	sysfs_remove_group(&(dev->dev.kobj), &mesh_ie_group);
+}
diff --git a/drivers/net/wireless/libertas/rx.c b/drivers/net/wireless/libertas/rx.c
index 05af7316f698..5749f22b296f 100644
--- a/drivers/net/wireless/libertas/rx.c
+++ b/drivers/net/wireless/libertas/rx.c
@@ -237,7 +237,7 @@ int lbs_process_rxed_packet(struct lbs_private *priv, struct sk_buff *skb)
 	/* Take the data rate from the rxpd structure
 	 * only if the rate is auto
 	 */
-	if (priv->auto_rate)
+	if (priv->enablehwauto)
 		priv->cur_rate = lbs_fw_index_to_data_rate(p_rx_pd->rx_rate);
 
 	lbs_compute_rssi(priv, p_rx_pd);
@@ -383,7 +383,7 @@ static int process_rxed_802_11_packet(struct lbs_private *priv,
 	/* Take the data rate from the rxpd structure
 	 * only if the rate is auto
 	 */
-	if (priv->auto_rate)
+	if (priv->enablehwauto)
 		priv->cur_rate = lbs_fw_index_to_data_rate(prxpd->rx_rate);
 
 	lbs_compute_rssi(priv, prxpd);
diff --git a/drivers/net/wireless/libertas/scan.c b/drivers/net/wireless/libertas/scan.c
index 387d4878af2f..4b274562f965 100644
--- a/drivers/net/wireless/libertas/scan.c
+++ b/drivers/net/wireless/libertas/scan.c
@@ -776,8 +776,9 @@ out:
 #define MAX_CUSTOM_LEN 64
 
 static inline char *lbs_translate_scan(struct lbs_private *priv,
-				       char *start, char *stop,
-				       struct bss_descriptor *bss)
+					    struct iw_request_info *info,
+					    char *start, char *stop,
+					    struct bss_descriptor *bss)
 {
 	struct chan_freq_power *cfp;
 	char *current_val;	/* For rates */
@@ -801,24 +802,24 @@ static inline char *lbs_translate_scan(struct lbs_private *priv,
 	iwe.cmd = SIOCGIWAP;
 	iwe.u.ap_addr.sa_family = ARPHRD_ETHER;
 	memcpy(iwe.u.ap_addr.sa_data, &bss->bssid, ETH_ALEN);
-	start = iwe_stream_add_event(start, stop, &iwe, IW_EV_ADDR_LEN);
+	start = iwe_stream_add_event(info, start, stop, &iwe, IW_EV_ADDR_LEN);
 
 	/* SSID */
 	iwe.cmd = SIOCGIWESSID;
 	iwe.u.data.flags = 1;
 	iwe.u.data.length = min((uint32_t) bss->ssid_len, (uint32_t) IW_ESSID_MAX_SIZE);
-	start = iwe_stream_add_point(start, stop, &iwe, bss->ssid);
+	start = iwe_stream_add_point(info, start, stop, &iwe, bss->ssid);
 
 	/* Mode */
 	iwe.cmd = SIOCGIWMODE;
 	iwe.u.mode = bss->mode;
-	start = iwe_stream_add_event(start, stop, &iwe, IW_EV_UINT_LEN);
+	start = iwe_stream_add_event(info, start, stop, &iwe, IW_EV_UINT_LEN);
 
 	/* Frequency */
 	iwe.cmd = SIOCGIWFREQ;
 	iwe.u.freq.m = (long)cfp->freq * 100000;
 	iwe.u.freq.e = 1;
-	start = iwe_stream_add_event(start, stop, &iwe, IW_EV_FREQ_LEN);
+	start = iwe_stream_add_event(info, start, stop, &iwe, IW_EV_FREQ_LEN);
 
 	/* Add quality statistics */
 	iwe.cmd = IWEVQUAL;
@@ -852,7 +853,7 @@ static inline char *lbs_translate_scan(struct lbs_private *priv,
 		nf = priv->NF[TYPE_RXPD][TYPE_AVG] / AVG_SCALE;
 		iwe.u.qual.level = CAL_RSSI(snr, nf);
 	}
-	start = iwe_stream_add_event(start, stop, &iwe, IW_EV_QUAL_LEN);
+	start = iwe_stream_add_event(info, start, stop, &iwe, IW_EV_QUAL_LEN);
 
 	/* Add encryption capability */
 	iwe.cmd = SIOCGIWENCODE;
@@ -862,9 +863,9 @@ static inline char *lbs_translate_scan(struct lbs_private *priv,
 		iwe.u.data.flags = IW_ENCODE_DISABLED;
 	}
 	iwe.u.data.length = 0;
-	start = iwe_stream_add_point(start, stop, &iwe, bss->ssid);
+	start = iwe_stream_add_point(info, start, stop, &iwe, bss->ssid);
 
-	current_val = start + IW_EV_LCP_LEN;
+	current_val = start + iwe_stream_lcp_len(info);
 
 	iwe.cmd = SIOCGIWRATE;
 	iwe.u.bitrate.fixed = 0;
@@ -874,19 +875,19 @@ static inline char *lbs_translate_scan(struct lbs_private *priv,
 	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);
+		current_val = iwe_stream_add_value(info, start, current_val,
+						   stop, &iwe, IW_EV_PARAM_LEN);
 	}
 	if ((bss->mode == IW_MODE_ADHOC) && priv->adhoccreate
 	    && !lbs_ssid_cmp(priv->curbssparams.ssid,
 			     priv->curbssparams.ssid_len,
 			     bss->ssid, bss->ssid_len)) {
 		iwe.u.bitrate.value = 22 * 500000;
-		current_val = iwe_stream_add_value(start, current_val,
+		current_val = iwe_stream_add_value(info, start, current_val,
 						   stop, &iwe, IW_EV_PARAM_LEN);
 	}
 	/* Check if we added any event */
-	if((current_val - start) > IW_EV_LCP_LEN)
+	if ((current_val - start) > iwe_stream_lcp_len(info))
 		start = current_val;
 
 	memset(&iwe, 0, sizeof(iwe));
@@ -895,7 +896,7 @@ static inline char *lbs_translate_scan(struct lbs_private *priv,
 		memcpy(buf, bss->wpa_ie, bss->wpa_ie_len);
 		iwe.cmd = IWEVGENIE;
 		iwe.u.data.length = bss->wpa_ie_len;
-		start = iwe_stream_add_point(start, stop, &iwe, buf);
+		start = iwe_stream_add_point(info, start, stop, &iwe, buf);
 	}
 
 	memset(&iwe, 0, sizeof(iwe));
@@ -904,7 +905,7 @@ static inline char *lbs_translate_scan(struct lbs_private *priv,
 		memcpy(buf, bss->rsn_ie, bss->rsn_ie_len);
 		iwe.cmd = IWEVGENIE;
 		iwe.u.data.length = bss->rsn_ie_len;
-		start = iwe_stream_add_point(start, stop, &iwe, buf);
+		start = iwe_stream_add_point(info, start, stop, &iwe, buf);
 	}
 
 	if (bss->mesh) {
@@ -915,7 +916,8 @@ static inline char *lbs_translate_scan(struct lbs_private *priv,
 		p += snprintf(p, MAX_CUSTOM_LEN, "mesh-type: olpc");
 		iwe.u.data.length = p - custom;
 		if (iwe.u.data.length)
-			start = iwe_stream_add_point(start, stop, &iwe, custom);
+			start = iwe_stream_add_point(info, start, stop,
+						     &iwe, custom);
 	}
 
 out:
@@ -1036,7 +1038,7 @@ int lbs_get_scan(struct net_device *dev, struct iw_request_info *info,
 		}
 
 		/* Translate to WE format this entry */
-		next_ev = lbs_translate_scan(priv, ev, stop, iter_bss);
+		next_ev = lbs_translate_scan(priv, info, ev, stop, iter_bss);
 		if (next_ev == NULL)
 			continue;
 		ev = next_ev;
diff --git a/drivers/net/wireless/libertas/types.h b/drivers/net/wireless/libertas/types.h
index 4031be420862..e0c2599da92f 100644
--- a/drivers/net/wireless/libertas/types.h
+++ b/drivers/net/wireless/libertas/types.h
@@ -6,6 +6,8 @@
 
 #include <linux/if_ether.h>
 #include <asm/byteorder.h>
+#include <linux/wireless.h>
+#include <net/ieee80211.h>
 
 struct ieeetypes_cfparamset {
 	u8 elementid;
@@ -252,4 +254,32 @@ struct mrvlietypes_ledbhv {
 	struct led_bhv ledbhv[1];
 } __attribute__ ((packed));
 
+/* Meant to be packed as the value member of a struct ieee80211_info_element.
+ * Note that the len member of the ieee80211_info_element varies depending on
+ * the mesh_id_len */
+struct mrvl_meshie_val {
+	uint8_t oui[P80211_OUI_LEN];
+	uint8_t type;
+	uint8_t subtype;
+	uint8_t version;
+	uint8_t active_protocol_id;
+	uint8_t active_metric_id;
+	uint8_t mesh_capability;
+	uint8_t mesh_id_len;
+	uint8_t mesh_id[IW_ESSID_MAX_SIZE];
+} __attribute__ ((packed));
+
+struct mrvl_meshie {
+	struct ieee80211_info_element hdr;
+	struct mrvl_meshie_val val;
+} __attribute__ ((packed));
+
+struct mrvl_mesh_defaults {
+	__le32 bootflag;
+	uint8_t boottime;
+	uint8_t reserved;
+	__le16 channel;
+	struct mrvl_meshie meshie;
+} __attribute__ ((packed));
+
 #endif
diff --git a/drivers/net/wireless/libertas/wext.c b/drivers/net/wireless/libertas/wext.c
index 0973d015a520..8b3ed77860b3 100644
--- a/drivers/net/wireless/libertas/wext.c
+++ b/drivers/net/wireless/libertas/wext.c
@@ -1002,7 +1002,7 @@ static int lbs_mesh_set_freq(struct net_device *dev,
 		else if (priv->mode == IW_MODE_ADHOC)
 			lbs_stop_adhoc_network(priv);
 	}
-	lbs_mesh_config(priv, 1, fwrq->m);
+	lbs_mesh_config(priv, CMD_ACT_MESH_CONFIG_START, fwrq->m);
 	lbs_update_channel(priv);
 	ret = 0;
 
@@ -1021,29 +1021,38 @@ static int lbs_set_rate(struct net_device *dev, struct iw_request_info *info,
 
 	lbs_deb_enter(LBS_DEB_WEXT);
 	lbs_deb_wext("vwrq->value %d\n", vwrq->value);
+	lbs_deb_wext("vwrq->fixed %d\n", vwrq->fixed);
+
+	if (vwrq->fixed && vwrq->value == -1)
+		goto out;
 
 	/* Auto rate? */
-	if (vwrq->value == -1) {
-		priv->auto_rate = 1;
+	priv->enablehwauto = !vwrq->fixed;
+
+	if (vwrq->value == -1)
 		priv->cur_rate = 0;
-	} else {
+	else {
 		if (vwrq->value % 100000)
 			goto out;
 
+		new_rate = vwrq->value / 500000;
+		priv->cur_rate = new_rate;
+		/* the rest is only needed for lbs_set_data_rate() */
 		memset(rates, 0, sizeof(rates));
 		copy_active_data_rates(priv, 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;
 		}
-
-		priv->cur_rate = new_rate;
-		priv->auto_rate = 0;
 	}
 
-	ret = lbs_set_data_rate(priv, new_rate);
+	/* Try the newer command first (Firmware Spec 5.1 and above) */
+	ret = lbs_cmd_802_11_rate_adapt_rateset(priv, CMD_ACT_SET);
+
+	/* Fallback to older version */
+	if (ret)
+		ret = lbs_set_data_rate(priv, new_rate);
 
 out:
 	lbs_deb_leave_args(LBS_DEB_WEXT, "ret %d", ret);
@@ -1060,7 +1069,7 @@ static int lbs_get_rate(struct net_device *dev, struct iw_request_info *info,
 	if (priv->connect_status == LBS_CONNECTED) {
 		vwrq->value = priv->cur_rate * 500000;
 
-		if (priv->auto_rate)
+		if (priv->enablehwauto)
 			vwrq->fixed = 0;
 		else
 			vwrq->fixed = 1;
@@ -2011,7 +2020,8 @@ static int lbs_mesh_set_essid(struct net_device *dev,
 		priv->mesh_ssid_len = dwrq->length;
 	}
 
-	lbs_mesh_config(priv, 1, priv->curbssparams.channel);
+	lbs_mesh_config(priv, CMD_ACT_MESH_CONFIG_START,
+			priv->curbssparams.channel);
  out:
 	lbs_deb_leave_args(LBS_DEB_WEXT, "ret %d", ret);
 	return ret;
diff --git a/drivers/net/wireless/mac80211_hwsim.c b/drivers/net/wireless/mac80211_hwsim.c
new file mode 100644
index 000000000000..5816230d58f8
--- /dev/null
+++ b/drivers/net/wireless/mac80211_hwsim.c
@@ -0,0 +1,521 @@
+/*
+ * mac80211_hwsim - software simulator of 802.11 radio(s) for mac80211
+ * Copyright (c) 2008, Jouni Malinen <j@w1.fi>
+ *
+ * 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.
+ */
+
+/*
+ * TODO:
+ * - IBSS mode simulation (Beacon transmission with competition for "air time")
+ * - IEEE 802.11a and 802.11n modes
+ * - RX filtering based on filter configuration (data->rx_filter)
+ */
+
+#include <net/mac80211.h>
+#include <net/ieee80211_radiotap.h>
+#include <linux/if_arp.h>
+#include <linux/rtnetlink.h>
+#include <linux/etherdevice.h>
+
+MODULE_AUTHOR("Jouni Malinen");
+MODULE_DESCRIPTION("Software simulator of 802.11 radio(s) for mac80211");
+MODULE_LICENSE("GPL");
+
+static int radios = 2;
+module_param(radios, int, 0444);
+MODULE_PARM_DESC(radios, "Number of simulated radios");
+
+
+static struct class *hwsim_class;
+
+static struct ieee80211_hw **hwsim_radios;
+static int hwsim_radio_count;
+static struct net_device *hwsim_mon; /* global monitor netdev */
+
+
+static const struct ieee80211_channel hwsim_channels[] = {
+	{ .center_freq = 2412 },
+	{ .center_freq = 2417 },
+	{ .center_freq = 2422 },
+	{ .center_freq = 2427 },
+	{ .center_freq = 2432 },
+	{ .center_freq = 2437 },
+	{ .center_freq = 2442 },
+	{ .center_freq = 2447 },
+	{ .center_freq = 2452 },
+	{ .center_freq = 2457 },
+	{ .center_freq = 2462 },
+	{ .center_freq = 2467 },
+	{ .center_freq = 2472 },
+	{ .center_freq = 2484 },
+};
+
+static const struct ieee80211_rate hwsim_rates[] = {
+	{ .bitrate = 10 },
+	{ .bitrate = 20, .flags = IEEE80211_RATE_SHORT_PREAMBLE },
+	{ .bitrate = 55, .flags = IEEE80211_RATE_SHORT_PREAMBLE },
+	{ .bitrate = 110, .flags = IEEE80211_RATE_SHORT_PREAMBLE },
+	{ .bitrate = 60 },
+	{ .bitrate = 90 },
+	{ .bitrate = 120 },
+	{ .bitrate = 180 },
+	{ .bitrate = 240 },
+	{ .bitrate = 360 },
+	{ .bitrate = 480 },
+	{ .bitrate = 540 }
+};
+
+struct mac80211_hwsim_data {
+	struct device *dev;
+	struct ieee80211_supported_band band;
+	struct ieee80211_channel channels[ARRAY_SIZE(hwsim_channels)];
+	struct ieee80211_rate rates[ARRAY_SIZE(hwsim_rates)];
+
+	struct ieee80211_channel *channel;
+	int radio_enabled;
+	unsigned long beacon_int; /* in jiffies unit */
+	unsigned int rx_filter;
+	int started;
+	struct timer_list beacon_timer;
+};
+
+
+struct hwsim_radiotap_hdr {
+	struct ieee80211_radiotap_header hdr;
+	u8 rt_flags;
+	u8 rt_rate;
+	__le16 rt_channel;
+	__le16 rt_chbitmask;
+} __attribute__ ((packed));
+
+
+static int hwsim_mon_xmit(struct sk_buff *skb, struct net_device *dev)
+{
+	/* TODO: allow packet injection */
+	dev_kfree_skb(skb);
+	return 0;
+}
+
+
+static void mac80211_hwsim_monitor_rx(struct ieee80211_hw *hw,
+				      struct sk_buff *tx_skb)
+{
+	struct mac80211_hwsim_data *data = hw->priv;
+	struct sk_buff *skb;
+	struct hwsim_radiotap_hdr *hdr;
+	u16 flags;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(tx_skb);
+	struct ieee80211_rate *txrate = ieee80211_get_tx_rate(hw, info);
+
+	if (!netif_running(hwsim_mon))
+		return;
+
+	skb = skb_copy_expand(tx_skb, sizeof(*hdr), 0, GFP_ATOMIC);
+	if (skb == NULL)
+		return;
+
+	hdr = (struct hwsim_radiotap_hdr *) skb_push(skb, sizeof(*hdr));
+	hdr->hdr.it_version = PKTHDR_RADIOTAP_VERSION;
+	hdr->hdr.it_pad = 0;
+	hdr->hdr.it_len = cpu_to_le16(sizeof(*hdr));
+	hdr->hdr.it_present = cpu_to_le32((1 << IEEE80211_RADIOTAP_FLAGS) |
+					  (1 << IEEE80211_RADIOTAP_RATE) |
+					  (1 << IEEE80211_RADIOTAP_CHANNEL));
+	hdr->rt_flags = 0;
+	hdr->rt_rate = txrate->bitrate / 5;
+	hdr->rt_channel = cpu_to_le16(data->channel->center_freq);
+	flags = IEEE80211_CHAN_2GHZ;
+	if (txrate->flags & IEEE80211_RATE_ERP_G)
+		flags |= IEEE80211_CHAN_OFDM;
+	else
+		flags |= IEEE80211_CHAN_CCK;
+	hdr->rt_chbitmask = cpu_to_le16(flags);
+
+	skb->dev = hwsim_mon;
+	skb_set_mac_header(skb, 0);
+	skb->ip_summed = CHECKSUM_UNNECESSARY;
+	skb->pkt_type = PACKET_OTHERHOST;
+	skb->protocol = htons(ETH_P_802_2);
+	memset(skb->cb, 0, sizeof(skb->cb));
+	netif_rx(skb);
+}
+
+
+static int mac80211_hwsim_tx_frame(struct ieee80211_hw *hw,
+				   struct sk_buff *skb)
+{
+	struct mac80211_hwsim_data *data = hw->priv;
+	int i, ack = 0;
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+	struct ieee80211_rx_status rx_status;
+
+	memset(&rx_status, 0, sizeof(rx_status));
+	/* TODO: set mactime */
+	rx_status.freq = data->channel->center_freq;
+	rx_status.band = data->channel->band;
+	rx_status.rate_idx = info->tx_rate_idx;
+	/* TODO: simulate signal strength (and optional packet drop) */
+
+	/* Copy skb to all enabled radios that are on the current frequency */
+	for (i = 0; i < hwsim_radio_count; i++) {
+		struct mac80211_hwsim_data *data2;
+		struct sk_buff *nskb;
+
+		if (hwsim_radios[i] == NULL || hwsim_radios[i] == hw)
+			continue;
+		data2 = hwsim_radios[i]->priv;
+		if (!data2->started || !data2->radio_enabled ||
+		    data->channel->center_freq != data2->channel->center_freq)
+			continue;
+
+		nskb = skb_copy(skb, GFP_ATOMIC);
+		if (nskb == NULL)
+			continue;
+
+		if (memcmp(hdr->addr1, hwsim_radios[i]->wiphy->perm_addr,
+			   ETH_ALEN) == 0)
+			ack = 1;
+		ieee80211_rx_irqsafe(hwsim_radios[i], nskb, &rx_status);
+	}
+
+	return ack;
+}
+
+
+static int mac80211_hwsim_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
+{
+	struct mac80211_hwsim_data *data = hw->priv;
+	int ack;
+	struct ieee80211_tx_info *txi;
+
+	mac80211_hwsim_monitor_rx(hw, skb);
+
+	if (skb->len < 10) {
+		/* Should not happen; just a sanity check for addr1 use */
+		dev_kfree_skb(skb);
+		return NETDEV_TX_OK;
+	}
+
+	if (!data->radio_enabled) {
+		printk(KERN_DEBUG "%s: dropped TX frame since radio "
+		       "disabled\n", wiphy_name(hw->wiphy));
+		dev_kfree_skb(skb);
+		return NETDEV_TX_OK;
+	}
+
+	ack = mac80211_hwsim_tx_frame(hw, skb);
+
+	txi = IEEE80211_SKB_CB(skb);
+	memset(&txi->status, 0, sizeof(txi->status));
+	if (!(txi->flags & IEEE80211_TX_CTL_NO_ACK)) {
+		if (ack)
+			txi->flags |= IEEE80211_TX_STAT_ACK;
+		else
+			txi->status.excessive_retries = 1;
+	}
+	ieee80211_tx_status_irqsafe(hw, skb);
+	return NETDEV_TX_OK;
+}
+
+
+static int mac80211_hwsim_start(struct ieee80211_hw *hw)
+{
+	struct mac80211_hwsim_data *data = hw->priv;
+	printk(KERN_DEBUG "%s:%s\n", wiphy_name(hw->wiphy), __func__);
+	data->started = 1;
+	return 0;
+}
+
+
+static void mac80211_hwsim_stop(struct ieee80211_hw *hw)
+{
+	struct mac80211_hwsim_data *data = hw->priv;
+	data->started = 0;
+	printk(KERN_DEBUG "%s:%s\n", wiphy_name(hw->wiphy), __func__);
+}
+
+
+static int mac80211_hwsim_add_interface(struct ieee80211_hw *hw,
+					struct ieee80211_if_init_conf *conf)
+{
+	DECLARE_MAC_BUF(mac);
+	printk(KERN_DEBUG "%s:%s (type=%d mac_addr=%s)\n",
+	       wiphy_name(hw->wiphy), __func__, conf->type,
+	       print_mac(mac, conf->mac_addr));
+	return 0;
+}
+
+
+static void mac80211_hwsim_remove_interface(
+	struct ieee80211_hw *hw, struct ieee80211_if_init_conf *conf)
+{
+	DECLARE_MAC_BUF(mac);
+	printk(KERN_DEBUG "%s:%s (type=%d mac_addr=%s)\n",
+	       wiphy_name(hw->wiphy), __func__, conf->type,
+	       print_mac(mac, conf->mac_addr));
+}
+
+
+static void mac80211_hwsim_beacon_tx(void *arg, u8 *mac,
+				     struct ieee80211_vif *vif)
+{
+	struct ieee80211_hw *hw = arg;
+	struct sk_buff *skb;
+	struct ieee80211_tx_info *info;
+
+	if (vif->type != IEEE80211_IF_TYPE_AP)
+		return;
+
+	skb = ieee80211_beacon_get(hw, vif);
+	if (skb == NULL)
+		return;
+	info = IEEE80211_SKB_CB(skb);
+
+	mac80211_hwsim_monitor_rx(hw, skb);
+	mac80211_hwsim_tx_frame(hw, skb);
+	dev_kfree_skb(skb);
+}
+
+
+static void mac80211_hwsim_beacon(unsigned long arg)
+{
+	struct ieee80211_hw *hw = (struct ieee80211_hw *) arg;
+	struct mac80211_hwsim_data *data = hw->priv;
+
+	if (!data->started || !data->radio_enabled)
+		return;
+
+	ieee80211_iterate_active_interfaces_atomic(
+		hw, mac80211_hwsim_beacon_tx, hw);
+
+	data->beacon_timer.expires = jiffies + data->beacon_int;
+	add_timer(&data->beacon_timer);
+}
+
+
+static int mac80211_hwsim_config(struct ieee80211_hw *hw,
+				 struct ieee80211_conf *conf)
+{
+	struct mac80211_hwsim_data *data = hw->priv;
+
+	printk(KERN_DEBUG "%s:%s (freq=%d radio_enabled=%d beacon_int=%d)\n",
+	       wiphy_name(hw->wiphy), __func__,
+	       conf->channel->center_freq, conf->radio_enabled,
+	       conf->beacon_int);
+
+	data->channel = conf->channel;
+	data->radio_enabled = conf->radio_enabled;
+	data->beacon_int = 1024 * conf->beacon_int / 1000 * HZ / 1000;
+	if (data->beacon_int < 1)
+		data->beacon_int = 1;
+
+	if (!data->started || !data->radio_enabled)
+		del_timer(&data->beacon_timer);
+	else
+		mod_timer(&data->beacon_timer, jiffies + data->beacon_int);
+
+	return 0;
+}
+
+
+static void mac80211_hwsim_configure_filter(struct ieee80211_hw *hw,
+					    unsigned int changed_flags,
+					    unsigned int *total_flags,
+					    int mc_count,
+					    struct dev_addr_list *mc_list)
+{
+	struct mac80211_hwsim_data *data = hw->priv;
+
+	printk(KERN_DEBUG "%s:%s\n", wiphy_name(hw->wiphy), __func__);
+
+	data->rx_filter = 0;
+	if (*total_flags & FIF_PROMISC_IN_BSS)
+		data->rx_filter |= FIF_PROMISC_IN_BSS;
+	if (*total_flags & FIF_ALLMULTI)
+		data->rx_filter |= FIF_ALLMULTI;
+
+	*total_flags = data->rx_filter;
+}
+
+
+
+static const struct ieee80211_ops mac80211_hwsim_ops =
+{
+	.tx = mac80211_hwsim_tx,
+	.start = mac80211_hwsim_start,
+	.stop = mac80211_hwsim_stop,
+	.add_interface = mac80211_hwsim_add_interface,
+	.remove_interface = mac80211_hwsim_remove_interface,
+	.config = mac80211_hwsim_config,
+	.configure_filter = mac80211_hwsim_configure_filter,
+};
+
+
+static void mac80211_hwsim_free(void)
+{
+	int i;
+
+	for (i = 0; i < hwsim_radio_count; i++) {
+		if (hwsim_radios[i]) {
+			struct mac80211_hwsim_data *data;
+			data = hwsim_radios[i]->priv;
+			ieee80211_unregister_hw(hwsim_radios[i]);
+			device_unregister(data->dev);
+			ieee80211_free_hw(hwsim_radios[i]);
+		}
+	}
+	kfree(hwsim_radios);
+	class_destroy(hwsim_class);
+}
+
+
+static struct device_driver mac80211_hwsim_driver = {
+	.name = "mac80211_hwsim"
+};
+
+
+static void hwsim_mon_setup(struct net_device *dev)
+{
+	dev->hard_start_xmit = hwsim_mon_xmit;
+	dev->destructor = free_netdev;
+	ether_setup(dev);
+	dev->tx_queue_len = 0;
+	dev->type = ARPHRD_IEEE80211_RADIOTAP;
+	memset(dev->dev_addr, 0, ETH_ALEN);
+	dev->dev_addr[0] = 0x12;
+}
+
+
+static int __init init_mac80211_hwsim(void)
+{
+	int i, err = 0;
+	u8 addr[ETH_ALEN];
+	struct mac80211_hwsim_data *data;
+	struct ieee80211_hw *hw;
+	DECLARE_MAC_BUF(mac);
+
+	if (radios < 1 || radios > 65535)
+		return -EINVAL;
+
+	hwsim_radio_count = radios;
+	hwsim_radios = kcalloc(hwsim_radio_count,
+			       sizeof(struct ieee80211_hw *), GFP_KERNEL);
+	if (hwsim_radios == NULL)
+		return -ENOMEM;
+
+	hwsim_class = class_create(THIS_MODULE, "mac80211_hwsim");
+	if (IS_ERR(hwsim_class)) {
+		kfree(hwsim_radios);
+		return PTR_ERR(hwsim_class);
+	}
+
+	memset(addr, 0, ETH_ALEN);
+	addr[0] = 0x02;
+
+	for (i = 0; i < hwsim_radio_count; i++) {
+		printk(KERN_DEBUG "mac80211_hwsim: Initializing radio %d\n",
+		       i);
+		hw = ieee80211_alloc_hw(sizeof(*data), &mac80211_hwsim_ops);
+		if (hw == NULL) {
+			printk(KERN_DEBUG "mac80211_hwsim: ieee80211_alloc_hw "
+			       "failed\n");
+			err = -ENOMEM;
+			goto failed;
+		}
+		hwsim_radios[i] = hw;
+
+		data = hw->priv;
+		data->dev = device_create_drvdata(hwsim_class, NULL, 0, hw,
+						"hwsim%d", i);
+		if (IS_ERR(data->dev)) {
+			printk(KERN_DEBUG
+			       "mac80211_hwsim: device_create_drvdata "
+			       "failed (%ld)\n", PTR_ERR(data->dev));
+			err = -ENOMEM;
+			goto failed_drvdata;
+		}
+		data->dev->driver = &mac80211_hwsim_driver;
+
+		SET_IEEE80211_DEV(hw, data->dev);
+		addr[3] = i >> 8;
+		addr[4] = i;
+		SET_IEEE80211_PERM_ADDR(hw, addr);
+
+		hw->channel_change_time = 1;
+		hw->queues = 1;
+
+		memcpy(data->channels, hwsim_channels, sizeof(hwsim_channels));
+		memcpy(data->rates, hwsim_rates, sizeof(hwsim_rates));
+		data->band.channels = data->channels;
+		data->band.n_channels = ARRAY_SIZE(hwsim_channels);
+		data->band.bitrates = data->rates;
+		data->band.n_bitrates = ARRAY_SIZE(hwsim_rates);
+		hw->wiphy->bands[IEEE80211_BAND_2GHZ] = &data->band;
+
+		err = ieee80211_register_hw(hw);
+		if (err < 0) {
+			printk(KERN_DEBUG "mac80211_hwsim: "
+			       "ieee80211_register_hw failed (%d)\n", err);
+			goto failed_hw;
+		}
+
+		printk(KERN_DEBUG "%s: hwaddr %s registered\n",
+		       wiphy_name(hw->wiphy),
+		       print_mac(mac, hw->wiphy->perm_addr));
+
+		setup_timer(&data->beacon_timer, mac80211_hwsim_beacon,
+			    (unsigned long) hw);
+	}
+
+	hwsim_mon = alloc_netdev(0, "hwsim%d", hwsim_mon_setup);
+	if (hwsim_mon == NULL)
+		goto failed;
+
+	rtnl_lock();
+
+	err = dev_alloc_name(hwsim_mon, hwsim_mon->name);
+	if (err < 0)
+		goto failed_mon;
+
+
+	err = register_netdevice(hwsim_mon);
+	if (err < 0)
+		goto failed_mon;
+
+	rtnl_unlock();
+
+	return 0;
+
+failed_mon:
+	rtnl_unlock();
+	free_netdev(hwsim_mon);
+	mac80211_hwsim_free();
+	return err;
+
+failed_hw:
+	device_unregister(data->dev);
+failed_drvdata:
+	ieee80211_free_hw(hw);
+	hwsim_radios[i] = 0;
+failed:
+	mac80211_hwsim_free();
+	return err;
+}
+
+
+static void __exit exit_mac80211_hwsim(void)
+{
+	printk(KERN_DEBUG "mac80211_hwsim: unregister %d radios\n",
+	       hwsim_radio_count);
+
+	unregister_netdev(hwsim_mon);
+	mac80211_hwsim_free();
+}
+
+
+module_init(init_mac80211_hwsim);
+module_exit(exit_mac80211_hwsim);
diff --git a/drivers/net/wireless/orinoco.c b/drivers/net/wireless/orinoco.c
index 6d13a0d15a0c..b047306bf386 100644
--- a/drivers/net/wireless/orinoco.c
+++ b/drivers/net/wireless/orinoco.c
@@ -4046,6 +4046,7 @@ static int orinoco_ioctl_setscan(struct net_device *dev,
  * format that the Wireless Tools will understand - Jean II
  * Return message length or -errno for fatal errors */
 static inline char *orinoco_translate_scan(struct net_device *dev,
+					   struct iw_request_info *info,
 					   char *current_ev,
 					   char *end_buf,
 					   union hermes_scan_info *bss,
@@ -4062,7 +4063,8 @@ static inline char *orinoco_translate_scan(struct net_device *dev,
 	iwe.cmd = SIOCGIWAP;
 	iwe.u.ap_addr.sa_family = ARPHRD_ETHER;
 	memcpy(iwe.u.ap_addr.sa_data, bss->a.bssid, ETH_ALEN);
-	current_ev = iwe_stream_add_event(current_ev, end_buf, &iwe, IW_EV_ADDR_LEN);
+	current_ev = iwe_stream_add_event(info, current_ev, end_buf,
+					  &iwe, IW_EV_ADDR_LEN);
 
 	/* Other entries will be displayed in the order we give them */
 
@@ -4072,7 +4074,8 @@ static inline char *orinoco_translate_scan(struct net_device *dev,
 		iwe.u.data.length = 32;
 	iwe.cmd = SIOCGIWESSID;
 	iwe.u.data.flags = 1;
-	current_ev = iwe_stream_add_point(current_ev, end_buf, &iwe, bss->a.essid);
+	current_ev = iwe_stream_add_point(info, current_ev, end_buf,
+					  &iwe, bss->a.essid);
 
 	/* Add mode */
 	iwe.cmd = SIOCGIWMODE;
@@ -4082,7 +4085,8 @@ static inline char *orinoco_translate_scan(struct net_device *dev,
 			iwe.u.mode = IW_MODE_MASTER;
 		else
 			iwe.u.mode = IW_MODE_ADHOC;
-		current_ev = iwe_stream_add_event(current_ev, end_buf, &iwe, IW_EV_UINT_LEN);
+		current_ev = iwe_stream_add_event(info, current_ev, end_buf,
+						  &iwe, IW_EV_UINT_LEN);
 	}
 
 	channel = bss->s.channel;
@@ -4091,7 +4095,7 @@ static inline char *orinoco_translate_scan(struct net_device *dev,
 		iwe.cmd = SIOCGIWFREQ;
 		iwe.u.freq.m = channel_frequency[channel-1] * 100000;
 		iwe.u.freq.e = 1;
-		current_ev = iwe_stream_add_event(current_ev, end_buf,
+		current_ev = iwe_stream_add_event(info, current_ev, end_buf,
 						  &iwe, IW_EV_FREQ_LEN);
 	}
 
@@ -4106,7 +4110,8 @@ static inline char *orinoco_translate_scan(struct net_device *dev,
 		iwe.u.qual.qual = iwe.u.qual.level - iwe.u.qual.noise;
 	else
 		iwe.u.qual.qual = 0;
-	current_ev = iwe_stream_add_event(current_ev, end_buf, &iwe, IW_EV_QUAL_LEN);
+	current_ev = iwe_stream_add_event(info, current_ev, end_buf,
+					  &iwe, IW_EV_QUAL_LEN);
 
 	/* Add encryption capability */
 	iwe.cmd = SIOCGIWENCODE;
@@ -4115,7 +4120,8 @@ static inline char *orinoco_translate_scan(struct net_device *dev,
 	else
 		iwe.u.data.flags = IW_ENCODE_DISABLED;
 	iwe.u.data.length = 0;
-	current_ev = iwe_stream_add_point(current_ev, end_buf, &iwe, bss->a.essid);
+	current_ev = iwe_stream_add_point(info, current_ev, end_buf,
+					  &iwe, bss->a.essid);
 
 	/* Add EXTRA: Age to display seconds since last beacon/probe response
 	 * for given network. */
@@ -4126,11 +4132,12 @@ static inline char *orinoco_translate_scan(struct net_device *dev,
 		      jiffies_to_msecs(jiffies - last_scanned));
 	iwe.u.data.length = p - custom;
 	if (iwe.u.data.length)
-		current_ev = iwe_stream_add_point(current_ev, end_buf, &iwe, custom);
+		current_ev = iwe_stream_add_point(info, current_ev, end_buf,
+						  &iwe, custom);
 
 	/* Bit rate is not available in Lucent/Agere firmwares */
 	if (priv->firmware_type != FIRMWARE_TYPE_AGERE) {
-		char *current_val = current_ev + IW_EV_LCP_LEN;
+		char *current_val = current_ev + iwe_stream_lcp_len(info);
 		int i;
 		int step;
 
@@ -4149,12 +4156,13 @@ static inline char *orinoco_translate_scan(struct net_device *dev,
 				break;
 			/* Bit rate given in 500 kb/s units (+ 0x80) */
 			iwe.u.bitrate.value = ((bss->p.rates[i] & 0x7f) * 500000);
-			current_val = iwe_stream_add_value(current_ev, current_val,
+			current_val = iwe_stream_add_value(info, current_ev,
+							   current_val,
 							   end_buf, &iwe,
 							   IW_EV_PARAM_LEN);
 		}
 		/* Check if we added any event */
-		if ((current_val - current_ev) > IW_EV_LCP_LEN)
+		if ((current_val - current_ev) > iwe_stream_lcp_len(info))
 			current_ev = current_val;
 	}
 
@@ -4190,7 +4198,7 @@ static int orinoco_ioctl_getscan(struct net_device *dev,
 
 	list_for_each_entry(bss, &priv->bss_list, list) {
 		/* Translate to WE format this entry */
-		current_ev = orinoco_translate_scan(dev, current_ev,
+		current_ev = orinoco_translate_scan(dev, info, current_ev,
 						    extra + srq->length,
 						    &bss->bss,
 						    bss->last_scanned);
diff --git a/drivers/net/wireless/p54/p54.h b/drivers/net/wireless/p54/p54.h
index 06d2c67f4c81..c6f27b9022f9 100644
--- a/drivers/net/wireless/p54/p54.h
+++ b/drivers/net/wireless/p54/p54.h
@@ -64,7 +64,7 @@ struct p54_common {
 	unsigned int tx_hdr_len;
 	void *cached_vdcf;
 	unsigned int fw_var;
-	struct ieee80211_tx_queue_stats tx_stats;
+	struct ieee80211_tx_queue_stats tx_stats[4];
 };
 
 int p54_rx(struct ieee80211_hw *dev, struct sk_buff *skb);
diff --git a/drivers/net/wireless/p54/p54common.c b/drivers/net/wireless/p54/p54common.c
index 63f9badf3f52..ffaf7a6b6810 100644
--- a/drivers/net/wireless/p54/p54common.c
+++ b/drivers/net/wireless/p54/p54common.c
@@ -146,10 +146,10 @@ void p54_parse_firmware(struct ieee80211_hw *dev, const struct firmware *fw)
 
 	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;
+		priv->tx_stats[0].limit = 3;
+		priv->tx_stats[1].limit = 4;
+		priv->tx_stats[2].limit = 3;
+		priv->tx_stats[3].limit = 1;
 		dev->queues = 4;
 	}
 }
@@ -355,8 +355,9 @@ static void p54_rx_data(struct ieee80211_hw *dev, struct sk_buff *skb)
 	struct ieee80211_rx_status rx_status = {0};
 	u16 freq = le16_to_cpu(hdr->freq);
 
-	rx_status.ssi = hdr->rssi;
+	rx_status.signal = hdr->rssi;
 	/* XX correct? */
+	rx_status.qual = (100 * hdr->rssi) / 127;
 	rx_status.rate_idx = hdr->rate & 0xf;
 	rx_status.freq = freq;
 	rx_status.band = IEEE80211_BAND_2GHZ;
@@ -375,11 +376,8 @@ 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)
+		if (priv->tx_stats[i].len < priv->tx_stats[i].limit)
 			ieee80211_wake_queue(dev, i);
 }
 
@@ -395,45 +393,42 @@ static void p54_rx_frame_sent(struct ieee80211_hw *dev, struct sk_buff *skb)
 	u32 last_addr = priv->rx_start;
 
 	while (entry != (struct sk_buff *)&priv->tx_queue) {
-		range = (struct memrecord *)&entry->cb;
+		struct ieee80211_tx_info *info = IEEE80211_SKB_CB(entry);
+		range = (void *)info->driver_data;
 		if (range->start_addr == addr) {
-			struct ieee80211_tx_status status;
 			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
+			if (entry->next != (struct sk_buff *)&priv->tx_queue) {
+				struct ieee80211_tx_info *ni;
+				struct memrecord *mr;
+
+				ni = IEEE80211_SKB_CB(entry->next);
+				mr = (struct memrecord *)ni->driver_data;
+				freed = mr->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;
-			}
-			memset(&status, 0, sizeof(status));
-			memcpy(&status.control, range->control,
-			       sizeof(status.control));
-			kfree(range->control);
-			priv->tx_stats.data[status.control.queue].len--;
-
+			memset(&info->status, 0, sizeof(info->status));
+			priv->tx_stats[skb_get_queue_mapping(skb)].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 (!(info->flags & IEEE80211_TX_CTL_NO_ACK)) {
 				if (!(payload->status & 0x01))
-					status.flags |= IEEE80211_TX_STATUS_ACK;
+					info->flags |= IEEE80211_TX_STAT_ACK;
 				else
-					status.excessive_retries = 1;
+					info->status.excessive_retries = 1;
 			}
-			status.retry_count = payload->retries - 1;
-			status.ack_signal = le16_to_cpu(payload->ack_rssi);
+			info->status.retry_count = payload->retries - 1;
+			info->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);
+			ieee80211_tx_status_irqsafe(dev, entry);
 			break;
 		} else
 			last_addr = range->end_addr;
@@ -498,13 +493,11 @@ EXPORT_SYMBOL_GPL(p54_rx);
  * 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_control_hdr *data, u32 len)
 {
 	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;
@@ -516,7 +509,8 @@ static void p54_assign_address(struct ieee80211_hw *dev, struct sk_buff *skb,
 	left = skb_queue_len(&priv->tx_queue);
 	while (left--) {
 		u32 hole_size;
-		range = (struct memrecord *)&entry->cb;
+		struct ieee80211_tx_info *info = IEEE80211_SKB_CB(entry);
+		struct memrecord *range = (void *)info->driver_data;
 		hole_size = range->start_addr - last_addr;
 		if (!target_skb && hole_size >= len) {
 			target_skb = entry->prev;
@@ -531,17 +525,18 @@ static void p54_assign_address(struct ieee80211_hw *dev, struct sk_buff *skb,
 		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;
+			struct ieee80211_tx_info *info = IEEE80211_SKB_CB(target_skb);
+			struct memrecord *range = (void *)info->driver_data;
 			target_addr = range->end_addr;
 		}
 	} else
 		largest_hole = max(largest_hole, priv->rx_end - last_addr);
 
 	if (skb) {
-		range = (struct memrecord *)&skb->cb;
+		struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+		struct memrecord *range = (void *)info->driver_data;
 		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))
@@ -552,32 +547,27 @@ static void p54_assign_address(struct ieee80211_hw *dev, struct sk_buff *skb,
 	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)
+static int p54_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
 {
-	struct ieee80211_tx_queue_stats_data *current_queue;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+	struct ieee80211_tx_queue_stats *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];
+	current_queue = &priv->tx_stats[skb_get_queue_mapping(skb)];
 	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);
+		ieee80211_stop_queue(dev, skb_get_queue_mapping(skb));
 
 	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));
@@ -587,35 +577,37 @@ static int p54_tx(struct ieee80211_hw *dev, struct sk_buff *skb,
 	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);
+	hdr->type = (info->flags & IEEE80211_TX_CTL_NO_ACK) ? 0 : cpu_to_le16(1);
+	hdr->retry1 = hdr->retry2 = info->control.retry_limit;
 
 	memset(txhdr->wep_key, 0x0, 16);
 	txhdr->padding = 0;
 	txhdr->padding2 = 0;
 
 	/* TODO: add support for alternate retry TX rates */
-	rate = control->tx_rate->hw_value;
-	if (control->flags & IEEE80211_TXCTL_SHORT_PREAMBLE)
+	rate = ieee80211_get_tx_rate(dev, info)->hw_value;
+	if (info->flags & IEEE80211_TX_CTL_SHORT_PREAMBLE)
 		rate |= 0x10;
-	if (control->flags & IEEE80211_TXCTL_USE_RTS_CTS)
+	if (info->flags & IEEE80211_TX_CTL_USE_RTS_CTS)
 		rate |= 0x40;
-	else if (control->flags & IEEE80211_TXCTL_USE_CTS_PROTECT)
+	else if (info->flags & IEEE80211_TX_CTL_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->frame_type = cpu_to_le32(skb_get_queue_mapping(skb) + 4);
 	txhdr->magic4 = 0;
-	txhdr->antenna = (control->antenna_sel_tx == 0) ?
-		2 : control->antenna_sel_tx - 1;
+	txhdr->antenna = (info->antenna_sel_tx == 0) ?
+		2 : info->antenna_sel_tx - 1;
 	txhdr->output_power = 0x7f; // HW Maximum
-	txhdr->magic5 = (control->flags & IEEE80211_TXCTL_NO_ACK) ?
+	txhdr->magic5 = (info->flags & IEEE80211_TX_CTL_NO_ACK) ?
 		0 : ((rate > 0x3) ? cpu_to_le32(0x33) : cpu_to_le32(0x23));
 	if (padding)
 		txhdr->align[0] = padding;
 
+	/* modifies skb->cb and with it info, so must be last! */
+	p54_assign_address(dev, skb, hdr, skb->len);
+
 	priv->tx(dev, hdr, skb->len, 0);
 	return 0;
 }
@@ -638,7 +630,7 @@ static int p54_set_filter(struct ieee80211_hw *dev, u16 filter_type,
 	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);
+	p54_assign_address(dev, NULL, hdr, sizeof(*hdr) + sizeof(*filter));
 	hdr->type = cpu_to_le16(P54_CONTROL_TYPE_FILTER_SET);
 
 	filter->filter_type = cpu_to_le16(filter_type);
@@ -682,7 +674,7 @@ static int p54_set_freq(struct ieee80211_hw *dev, __le16 freq)
 	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);
+	p54_assign_address(dev, NULL, hdr, sizeof(*hdr) + payload_len);
 
 	chan->magic1 = cpu_to_le16(0x1);
 	chan->magic2 = cpu_to_le16(0x0);
@@ -755,7 +747,7 @@ static int p54_set_leds(struct ieee80211_hw *dev, int mode, int link, int act)
 	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);
+	p54_assign_address(dev, NULL, hdr, sizeof(*hdr) + sizeof(*led));
 
 	led = (struct p54_tx_control_led *) hdr->data;
 	led->mode = cpu_to_le16(mode);
@@ -805,7 +797,7 @@ static void p54_set_vdcf(struct ieee80211_hw *dev)
 
 	hdr = (void *)priv->cached_vdcf + priv->tx_hdr_len;
 
-	p54_assign_address(dev, NULL, hdr, sizeof(*hdr) + sizeof(*vdcf), NULL);
+	p54_assign_address(dev, NULL, hdr, sizeof(*hdr) + sizeof(*vdcf));
 
 	vdcf = (struct p54_tx_control_vdcf *) hdr->data;
 
@@ -841,12 +833,8 @@ 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);
+	while ((skb = skb_dequeue(&priv->tx_queue)))
 		kfree_skb(skb);
-	}
 	priv->stop(dev);
 	priv->mode = IEEE80211_IF_TYPE_INVALID;
 }
@@ -936,7 +924,7 @@ static void p54_configure_filter(struct ieee80211_hw *dev,
 	}
 }
 
-static int p54_conf_tx(struct ieee80211_hw *dev, int queue,
+static int p54_conf_tx(struct ieee80211_hw *dev, u16 queue,
 		       const struct ieee80211_tx_queue_params *params)
 {
 	struct p54_common *priv = dev->priv;
@@ -945,7 +933,7 @@ static int p54_conf_tx(struct ieee80211_hw *dev, int queue,
 	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))) {
+	if ((params) && !(queue > 4)) {
 		P54_SET_QUEUE(vdcf->queue[queue], params->aifs,
 			params->cw_min, params->cw_max, params->txop);
 	} else
@@ -967,11 +955,8 @@ 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]));
+	memcpy(stats, &priv->tx_stats, sizeof(stats[0]) * dev->queues);
 
 	return 0;
 }
@@ -1004,11 +989,12 @@ struct ieee80211_hw *p54_init_common(size_t priv_data_len)
 	skb_queue_head_init(&priv->tx_queue);
 	dev->wiphy->bands[IEEE80211_BAND_2GHZ] = &band_2GHz;
 	dev->flags = IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING | /* not sure */
-		    IEEE80211_HW_RX_INCLUDES_FCS;
+		     IEEE80211_HW_RX_INCLUDES_FCS |
+		     IEEE80211_HW_SIGNAL_UNSPEC;
 	dev->channel_change_time = 1000;	/* TODO: find actual value */
-	dev->max_rssi = 127;
+	dev->max_signal = 127;
 
-	priv->tx_stats.data[0].limit = 5;
+	priv->tx_stats[0].limit = 5;
 	dev->queues = 1;
 
 	dev->extra_tx_headroom = sizeof(struct p54_control_hdr) + 4 +
diff --git a/drivers/net/wireless/p54/p54common.h b/drivers/net/wireless/p54/p54common.h
index c15b56e1d75e..2245fcce92dc 100644
--- a/drivers/net/wireless/p54/p54common.h
+++ b/drivers/net/wireless/p54/p54common.h
@@ -152,7 +152,6 @@ struct pda_pa_curve_data {
 struct memrecord {
 	u32 start_addr;
 	u32 end_addr;
-	struct ieee80211_tx_control *control;
 };
 
 struct p54_eeprom_lm86 {
diff --git a/drivers/net/wireless/p54/p54pci.c b/drivers/net/wireless/p54/p54pci.c
index fa527723fbe0..7dd4add4bf4e 100644
--- a/drivers/net/wireless/p54/p54pci.c
+++ b/drivers/net/wireless/p54/p54pci.c
@@ -665,7 +665,7 @@ static int p54p_resume(struct pci_dev *pdev)
 
 	if (priv->common.mode != IEEE80211_IF_TYPE_INVALID) {
 		p54p_open(dev);
-		ieee80211_start_queues(dev);
+		ieee80211_wake_queues(dev);
 	}
 
 	return 0;
diff --git a/drivers/net/wireless/p54/p54usb.c b/drivers/net/wireless/p54/p54usb.c
index 1610a7308c1d..815c095ef797 100644
--- a/drivers/net/wireless/p54/p54usb.c
+++ b/drivers/net/wireless/p54/p54usb.c
@@ -376,7 +376,8 @@ static int p54u_upload_firmware_3887(struct ieee80211_hw *dev)
 	const struct firmware *fw_entry = NULL;
 	int err, alen;
 	u8 carry = 0;
-	u8 *buf, *tmp, *data;
+	u8 *buf, *tmp;
+	const u8 *data;
 	unsigned int left, remains, block_size;
 	struct x2_header *hdr;
 	unsigned long timeout;
@@ -523,7 +524,7 @@ static int p54u_upload_firmware_net2280(struct ieee80211_hw *dev)
 	void *buf;
 	__le32 reg;
 	unsigned int remains, offset;
-	u8 *data;
+	const u8 *data;
 
 	buf = kmalloc(512, GFP_KERNEL);
 	if (!buf) {
diff --git a/drivers/net/wireless/prism54/isl_ioctl.c b/drivers/net/wireless/prism54/isl_ioctl.c
index 5b375b289036..97fa14e0a479 100644
--- a/drivers/net/wireless/prism54/isl_ioctl.c
+++ b/drivers/net/wireless/prism54/isl_ioctl.c
@@ -571,8 +571,9 @@ prism54_set_scan(struct net_device *dev, struct iw_request_info *info,
  */
 
 static char *
-prism54_translate_bss(struct net_device *ndev, char *current_ev,
-		      char *end_buf, struct obj_bss *bss, char noise)
+prism54_translate_bss(struct net_device *ndev, struct iw_request_info *info,
+		      char *current_ev, char *end_buf, struct obj_bss *bss,
+		      char noise)
 {
 	struct iw_event iwe;	/* Temporary buffer */
 	short cap;
@@ -584,8 +585,8 @@ prism54_translate_bss(struct net_device *ndev, char *current_ev,
 	memcpy(iwe.u.ap_addr.sa_data, bss->address, 6);
 	iwe.u.ap_addr.sa_family = ARPHRD_ETHER;
 	iwe.cmd = SIOCGIWAP;
-	current_ev =
-	    iwe_stream_add_event(current_ev, end_buf, &iwe, IW_EV_ADDR_LEN);
+	current_ev = iwe_stream_add_event(info, current_ev, end_buf,
+					  &iwe, IW_EV_ADDR_LEN);
 
 	/* The following entries will be displayed in the same order we give them */
 
@@ -593,7 +594,7 @@ prism54_translate_bss(struct net_device *ndev, char *current_ev,
 	iwe.u.data.length = bss->ssid.length;
 	iwe.u.data.flags = 1;
 	iwe.cmd = SIOCGIWESSID;
-	current_ev = iwe_stream_add_point(current_ev, end_buf,
+	current_ev = iwe_stream_add_point(info, current_ev, end_buf,
 					  &iwe, bss->ssid.octets);
 
 	/* Capabilities */
@@ -610,9 +611,8 @@ prism54_translate_bss(struct net_device *ndev, char *current_ev,
 		iwe.u.mode = IW_MODE_ADHOC;
 	iwe.cmd = SIOCGIWMODE;
 	if (iwe.u.mode)
-		current_ev =
-		    iwe_stream_add_event(current_ev, end_buf, &iwe,
-					 IW_EV_UINT_LEN);
+		current_ev = iwe_stream_add_event(info, current_ev, end_buf,
+						  &iwe, IW_EV_UINT_LEN);
 
 	/* Encryption capability */
 	if (cap & CAP_CRYPT)
@@ -621,14 +621,15 @@ prism54_translate_bss(struct net_device *ndev, char *current_ev,
 		iwe.u.data.flags = IW_ENCODE_DISABLED;
 	iwe.u.data.length = 0;
 	iwe.cmd = SIOCGIWENCODE;
-	current_ev = iwe_stream_add_point(current_ev, end_buf, &iwe, NULL);
+	current_ev = iwe_stream_add_point(info, current_ev, end_buf,
+					  &iwe, NULL);
 
 	/* Add frequency. (short) bss->channel is the frequency in MHz */
 	iwe.u.freq.m = bss->channel;
 	iwe.u.freq.e = 6;
 	iwe.cmd = SIOCGIWFREQ;
-	current_ev =
-	    iwe_stream_add_event(current_ev, end_buf, &iwe, IW_EV_FREQ_LEN);
+	current_ev = iwe_stream_add_event(info, current_ev, end_buf,
+					  &iwe, IW_EV_FREQ_LEN);
 
 	/* Add quality statistics */
 	iwe.u.qual.level = bss->rssi;
@@ -636,20 +637,20 @@ prism54_translate_bss(struct net_device *ndev, char *current_ev,
 	/* do a simple SNR for quality */
 	iwe.u.qual.qual = bss->rssi - noise;
 	iwe.cmd = IWEVQUAL;
-	current_ev =
-	    iwe_stream_add_event(current_ev, end_buf, &iwe, IW_EV_QUAL_LEN);
+	current_ev = iwe_stream_add_event(info, current_ev, end_buf,
+					  &iwe, IW_EV_QUAL_LEN);
 
 	/* Add WPA/RSN Information Element, if any */
 	wpa_ie_len = prism54_wpa_bss_ie_get(priv, bss->address, wpa_ie);
 	if (wpa_ie_len > 0) {
 		iwe.cmd = IWEVGENIE;
 		iwe.u.data.length = min(wpa_ie_len, (size_t)MAX_WPA_IE_LEN);
-		current_ev = iwe_stream_add_point(current_ev, end_buf,
-				&iwe, wpa_ie);
+		current_ev = iwe_stream_add_point(info, current_ev, end_buf,
+						  &iwe, wpa_ie);
 	}
 	/* Do the bitrates */
 	{
-		char *	current_val = current_ev + IW_EV_LCP_LEN;
+		char *current_val = current_ev + iwe_stream_lcp_len(info);
 		int i;
 		int mask;
 
@@ -662,14 +663,14 @@ prism54_translate_bss(struct net_device *ndev, char *current_ev,
 		for(i = 0; i < sizeof(scan_rate_list); i++) {
 			if(bss->rates & mask) {
 				iwe.u.bitrate.value = (scan_rate_list[i] * 500000);
-				current_val = iwe_stream_add_value(current_ev, current_val,
-								   end_buf, &iwe,
-								   IW_EV_PARAM_LEN);
+				current_val = iwe_stream_add_value(
+					info, current_ev, current_val,
+					end_buf, &iwe, IW_EV_PARAM_LEN);
 			}
 			mask <<= 1;
 		}
 		/* Check if we added any event */
-		if ((current_val - current_ev) > IW_EV_LCP_LEN)
+		if ((current_val - current_ev) > iwe_stream_lcp_len(info))
 			current_ev = current_val;
 	}
 
@@ -710,7 +711,7 @@ prism54_get_scan(struct net_device *ndev, struct iw_request_info *info,
 
 	/* ok now, scan the list and translate its info */
 	for (i = 0; i < (int) bsslist->nr; i++) {
-		current_ev = prism54_translate_bss(ndev, current_ev,
+		current_ev = prism54_translate_bss(ndev, info, current_ev,
 						   extra + dwrq->length,
 						   &(bsslist->bsslist[i]),
 						   noise);
@@ -2704,6 +2705,7 @@ prism2_ioctl_scan_req(struct net_device *ndev,
                      struct prism2_hostapd_param *param)
 {
 	islpci_private *priv = netdev_priv(ndev);
+	struct iw_request_info info;
 	int i, rvalue;
 	struct obj_bsslist *bsslist;
 	u32 noise = 0;
@@ -2727,9 +2729,12 @@ prism2_ioctl_scan_req(struct net_device *ndev,
 	rvalue |= mgt_get_request(priv, DOT11_OID_BSSLIST, 0, NULL, &r);
 	bsslist = r.ptr;
 
+	info.cmd = PRISM54_HOSTAPD;
+	info.flags = 0;
+
 	/* ok now, scan the list and translate its info */
 	for (i = 0; i < min(IW_MAX_AP, (int) bsslist->nr); i++)
-		current_ev = prism54_translate_bss(ndev, current_ev,
+		current_ev = prism54_translate_bss(ndev, &info, current_ev,
 						   extra + IW_SCAN_MAX_DATA,
 						   &(bsslist->bsslist[i]),
 						   noise);
diff --git a/drivers/net/wireless/rndis_wlan.c b/drivers/net/wireless/rndis_wlan.c
index 18c9931e3267..00e965b9da75 100644
--- a/drivers/net/wireless/rndis_wlan.c
+++ b/drivers/net/wireless/rndis_wlan.c
@@ -310,8 +310,11 @@ enum wpa_key_mgmt { KEY_MGMT_802_1X, KEY_MGMT_PSK, KEY_MGMT_NONE,
 #define CAP_MODE_MASK		7
 #define CAP_SUPPORT_TXPOWER	8
 
-#define WORK_CONNECTION_EVENT	(1<<0)
-#define WORK_SET_MULTICAST_LIST	(1<<1)
+#define WORK_LINK_UP		(1<<0)
+#define WORK_LINK_DOWN		(1<<1)
+#define WORK_SET_MULTICAST_LIST	(1<<2)
+
+#define COMMAND_BUFFER_SIZE	(CONTROL_BUFFER_SIZE + sizeof(struct rndis_set))
 
 /* RNDIS device private data */
 struct rndis_wext_private {
@@ -361,6 +364,8 @@ struct rndis_wext_private {
 	u8  *wpa_ie;
 	int  wpa_cipher_pair;
 	int  wpa_cipher_group;
+
+	u8 command_buffer[COMMAND_BUFFER_SIZE];
 };
 
 
@@ -427,18 +432,23 @@ static int rndis_query_oid(struct usbnet *dev, __le32 oid, void *data, int *len)
 	buflen = *len + sizeof(*u.get);
 	if (buflen < CONTROL_BUFFER_SIZE)
 		buflen = CONTROL_BUFFER_SIZE;
-	u.buf = kmalloc(buflen, GFP_KERNEL);
-	if (!u.buf)
-		return -ENOMEM;
+
+	if (buflen > COMMAND_BUFFER_SIZE) {
+		u.buf = kmalloc(buflen, GFP_KERNEL);
+		if (!u.buf)
+			return -ENOMEM;
+	} else {
+		u.buf = priv->command_buffer;
+	}
+
+	mutex_lock(&priv->command_lock);
+
 	memset(u.get, 0, sizeof *u.get);
 	u.get->msg_type = RNDIS_MSG_QUERY;
 	u.get->msg_len = ccpu2(sizeof *u.get);
 	u.get->oid = oid;
 
-	mutex_lock(&priv->command_lock);
-	ret = rndis_command(dev, u.header);
-	mutex_unlock(&priv->command_lock);
-
+	ret = rndis_command(dev, u.header, buflen);
 	if (ret == 0) {
 		ret = le32_to_cpu(u.get_c->len);
 		*len = (*len > ret) ? ret : *len;
@@ -446,7 +456,10 @@ static int rndis_query_oid(struct usbnet *dev, __le32 oid, void *data, int *len)
 		ret = rndis_error_status(u.get_c->status);
 	}
 
-	kfree(u.buf);
+	mutex_unlock(&priv->command_lock);
+
+	if (u.buf != priv->command_buffer)
+		kfree(u.buf);
 	return ret;
 }
 
@@ -465,9 +478,16 @@ static int rndis_set_oid(struct usbnet *dev, __le32 oid, void *data, int len)
 	buflen = len + sizeof(*u.set);
 	if (buflen < CONTROL_BUFFER_SIZE)
 		buflen = CONTROL_BUFFER_SIZE;
-	u.buf = kmalloc(buflen, GFP_KERNEL);
-	if (!u.buf)
-		return -ENOMEM;
+
+	if (buflen > COMMAND_BUFFER_SIZE) {
+		u.buf = kmalloc(buflen, GFP_KERNEL);
+		if (!u.buf)
+			return -ENOMEM;
+	} else {
+		u.buf = priv->command_buffer;
+	}
+
+	mutex_lock(&priv->command_lock);
 
 	memset(u.set, 0, sizeof *u.set);
 	u.set->msg_type = RNDIS_MSG_SET;
@@ -478,14 +498,14 @@ static int rndis_set_oid(struct usbnet *dev, __le32 oid, void *data, int len)
 	u.set->handle = ccpu2(0);
 	memcpy(u.buf + sizeof(*u.set), data, len);
 
-	mutex_lock(&priv->command_lock);
-	ret = rndis_command(dev, u.header);
-	mutex_unlock(&priv->command_lock);
-
+	ret = rndis_command(dev, u.header, buflen);
 	if (ret == 0)
 		ret = rndis_error_status(u.set_c->status);
 
-	kfree(u.buf);
+	mutex_unlock(&priv->command_lock);
+
+	if (u.buf != priv->command_buffer)
+		kfree(u.buf);
 	return ret;
 }
 
@@ -620,8 +640,7 @@ static void dsconfig_to_freq(unsigned int dsconfig, struct iw_freq *freq)
 static int freq_to_dsconfig(struct iw_freq *freq, unsigned int *dsconfig)
 {
 	if (freq->m < 1000 && freq->e == 0) {
-		if (freq->m >= 1 &&
-			freq->m <= (sizeof(freq_chan) / sizeof(freq_chan[0])))
+		if (freq->m >= 1 && freq->m <= ARRAY_SIZE(freq_chan))
 			*dsconfig = freq_chan[freq->m - 1] * 1000;
 		else
 			return -1;
@@ -1135,7 +1154,7 @@ static int rndis_iw_get_range(struct net_device *dev,
 	/* fill in 802.11g rates */
 	if (has_80211g_rates) {
 		num = range->num_bitrates;
-		for (i = 0; i < sizeof(rates_80211g); i++) {
+		for (i = 0; i < ARRAY_SIZE(rates_80211g); i++) {
 			for (j = 0; j < num; j++) {
 				if (range->bitrate[j] ==
 					rates_80211g[i] * 1000000)
@@ -1159,10 +1178,9 @@ static int rndis_iw_get_range(struct net_device *dev,
 		range->throughput = 11 * 1000 * 1000 / 2;
 	}
 
-	range->num_channels = (sizeof(freq_chan)/sizeof(freq_chan[0]));
+	range->num_channels = ARRAY_SIZE(freq_chan);
 
-	for (i = 0; i < (sizeof(freq_chan)/sizeof(freq_chan[0])) &&
-			i < IW_MAX_FREQUENCIES; i++) {
+	for (i = 0; i < ARRAY_SIZE(freq_chan) && i < IW_MAX_FREQUENCIES; i++) {
 		range->freq[i].i = i + 1;
 		range->freq[i].m = freq_chan[i] * 100000;
 		range->freq[i].e = 1;
@@ -1630,7 +1648,9 @@ static int rndis_iw_set_scan(struct net_device *dev,
 
 
 static char *rndis_translate_scan(struct net_device *dev,
-    char *cev, char *end_buf, struct ndis_80211_bssid_ex *bssid)
+				  struct iw_request_info *info, char *cev,
+				  char *end_buf,
+				  struct ndis_80211_bssid_ex *bssid)
 {
 #ifdef DEBUG
 	struct usbnet *usbdev = dev->priv;
@@ -1649,14 +1669,14 @@ static char *rndis_translate_scan(struct net_device *dev,
 	iwe.cmd = SIOCGIWAP;
 	iwe.u.ap_addr.sa_family = ARPHRD_ETHER;
 	memcpy(iwe.u.ap_addr.sa_data, bssid->mac, ETH_ALEN);
-	cev = iwe_stream_add_event(cev, end_buf, &iwe, IW_EV_ADDR_LEN);
+	cev = iwe_stream_add_event(info, cev, end_buf, &iwe, IW_EV_ADDR_LEN);
 
 	devdbg(usbdev, "SSID(%d) %s", le32_to_cpu(bssid->ssid.length),
 						bssid->ssid.essid);
 	iwe.cmd = SIOCGIWESSID;
 	iwe.u.essid.length = le32_to_cpu(bssid->ssid.length);
 	iwe.u.essid.flags = 1;
-	cev = iwe_stream_add_point(cev, end_buf, &iwe, bssid->ssid.essid);
+	cev = iwe_stream_add_point(info, cev, end_buf, &iwe, bssid->ssid.essid);
 
 	devdbg(usbdev, "MODE %d", le32_to_cpu(bssid->net_infra));
 	iwe.cmd = SIOCGIWMODE;
@@ -1672,12 +1692,12 @@ static char *rndis_translate_scan(struct net_device *dev,
 		iwe.u.mode = IW_MODE_AUTO;
 		break;
 	}
-	cev = iwe_stream_add_event(cev, end_buf, &iwe, IW_EV_UINT_LEN);
+	cev = iwe_stream_add_event(info, cev, end_buf, &iwe, IW_EV_UINT_LEN);
 
 	devdbg(usbdev, "FREQ %d kHz", le32_to_cpu(bssid->config.ds_config));
 	iwe.cmd = SIOCGIWFREQ;
 	dsconfig_to_freq(le32_to_cpu(bssid->config.ds_config), &iwe.u.freq);
-	cev = iwe_stream_add_event(cev, end_buf, &iwe, IW_EV_FREQ_LEN);
+	cev = iwe_stream_add_event(info, cev, end_buf, &iwe, IW_EV_FREQ_LEN);
 
 	devdbg(usbdev, "QUAL %d", le32_to_cpu(bssid->rssi));
 	iwe.cmd = IWEVQUAL;
@@ -1686,7 +1706,7 @@ static char *rndis_translate_scan(struct net_device *dev,
 	iwe.u.qual.updated = IW_QUAL_QUAL_UPDATED
 			| IW_QUAL_LEVEL_UPDATED
 			| IW_QUAL_NOISE_INVALID;
-	cev = iwe_stream_add_event(cev, end_buf, &iwe, IW_EV_QUAL_LEN);
+	cev = iwe_stream_add_event(info, cev, end_buf, &iwe, IW_EV_QUAL_LEN);
 
 	devdbg(usbdev, "ENCODE %d", le32_to_cpu(bssid->privacy));
 	iwe.cmd = SIOCGIWENCODE;
@@ -1696,10 +1716,10 @@ static char *rndis_translate_scan(struct net_device *dev,
 	else
 		iwe.u.data.flags = IW_ENCODE_ENABLED | IW_ENCODE_NOKEY;
 
-	cev = iwe_stream_add_point(cev, end_buf, &iwe, NULL);
+	cev = iwe_stream_add_point(info, cev, end_buf, &iwe, NULL);
 
 	devdbg(usbdev, "RATES:");
-	current_val = cev + IW_EV_LCP_LEN;
+	current_val = cev + iwe_stream_lcp_len(info);
 	iwe.cmd = SIOCGIWRATE;
 	for (i = 0; i < sizeof(bssid->rates); i++) {
 		if (bssid->rates[i] & 0x7f) {
@@ -1707,13 +1727,13 @@ static char *rndis_translate_scan(struct net_device *dev,
 				((bssid->rates[i] & 0x7f) *
 				500000);
 			devdbg(usbdev, " %d", iwe.u.bitrate.value);
-			current_val = iwe_stream_add_value(cev,
+			current_val = iwe_stream_add_value(info, cev,
 				current_val, end_buf, &iwe,
 				IW_EV_PARAM_LEN);
 		}
 	}
 
-	if ((current_val - cev) > IW_EV_LCP_LEN)
+	if ((current_val - cev) > iwe_stream_lcp_len(info))
 		cev = current_val;
 
 	beacon = le32_to_cpu(bssid->config.beacon_period);
@@ -1721,14 +1741,14 @@ static char *rndis_translate_scan(struct net_device *dev,
 	iwe.cmd = IWEVCUSTOM;
 	snprintf(sbuf, sizeof(sbuf), "bcn_int=%d", beacon);
 	iwe.u.data.length = strlen(sbuf);
-	cev = iwe_stream_add_point(cev, end_buf, &iwe, sbuf);
+	cev = iwe_stream_add_point(info, cev, end_buf, &iwe, sbuf);
 
 	atim = le32_to_cpu(bssid->config.atim_window);
 	devdbg(usbdev, "ATIM %d", atim);
 	iwe.cmd = IWEVCUSTOM;
 	snprintf(sbuf, sizeof(sbuf), "atim=%u", atim);
 	iwe.u.data.length = strlen(sbuf);
-	cev = iwe_stream_add_point(cev, end_buf, &iwe, sbuf);
+	cev = iwe_stream_add_point(info, cev, end_buf, &iwe, sbuf);
 
 	ie = (void *)(bssid->ies + sizeof(struct ndis_80211_fixed_ies));
 	ie_len = min(bssid_len - (int)sizeof(*bssid),
@@ -1742,7 +1762,7 @@ static char *rndis_translate_scan(struct net_device *dev,
 					(ie->id == MFIE_TYPE_RSN) ? 2 : 1);
 			iwe.cmd = IWEVGENIE;
 			iwe.u.data.length = min(ie->len + 2, MAX_WPA_IE_LEN);
-			cev = iwe_stream_add_point(cev, end_buf, &iwe,
+			cev = iwe_stream_add_point(info, cev, end_buf, &iwe,
 								(u8 *)ie);
 		}
 
@@ -1785,8 +1805,8 @@ static int rndis_iw_get_scan(struct net_device *dev,
 	devdbg(usbdev, "SIOCGIWSCAN: %d BSSIDs found", count);
 
 	while (count && ((void *)bssid + bssid_len) <= (buf + len)) {
-		cev = rndis_translate_scan(dev, cev, extra + IW_SCAN_MAX_DATA,
-									bssid);
+		cev = rndis_translate_scan(dev, info, cev,
+					   extra + IW_SCAN_MAX_DATA, bssid);
 		bssid = (void *)bssid + bssid_len;
 		bssid_len = le32_to_cpu(bssid->length);
 		count--;
@@ -2213,7 +2233,9 @@ static void rndis_wext_worker(struct work_struct *work)
 	int assoc_size = sizeof(*info) + IW_CUSTOM_MAX + 32;
 	int ret, offset;
 
-	if (test_and_clear_bit(WORK_CONNECTION_EVENT, &priv->work_pending)) {
+	if (test_and_clear_bit(WORK_LINK_UP, &priv->work_pending)) {
+		netif_carrier_on(usbdev->net);
+
 		info = kzalloc(assoc_size, GFP_KERNEL);
 		if (!info)
 			goto get_bssid;
@@ -2251,6 +2273,15 @@ get_bssid:
 		}
 	}
 
+	if (test_and_clear_bit(WORK_LINK_DOWN, &priv->work_pending)) {
+		netif_carrier_off(usbdev->net);
+
+		evt.data.flags = 0;
+		evt.data.length = 0;
+		memset(evt.ap_addr.sa_data, 0, ETH_ALEN);
+		wireless_send_event(usbdev->net, SIOCGIWAP, &evt, NULL);
+	}
+
 	if (test_and_clear_bit(WORK_SET_MULTICAST_LIST, &priv->work_pending))
 		set_multicast_list(usbdev);
 }
@@ -2260,29 +2291,24 @@ static void rndis_wext_set_multicast_list(struct net_device *dev)
 	struct usbnet *usbdev = dev->priv;
 	struct rndis_wext_private *priv = get_rndis_wext_priv(usbdev);
 
+	if (test_bit(WORK_SET_MULTICAST_LIST, &priv->work_pending))
+		return;
+
 	set_bit(WORK_SET_MULTICAST_LIST, &priv->work_pending);
 	queue_work(priv->workqueue, &priv->work);
 }
 
-static void rndis_wext_link_change(struct usbnet *dev, int state)
+static void rndis_wext_link_change(struct usbnet *usbdev, int state)
 {
-	struct rndis_wext_private *priv = get_rndis_wext_priv(dev);
-	union iwreq_data evt;
+	struct rndis_wext_private *priv = get_rndis_wext_priv(usbdev);
 
-	if (state) {
-		/* queue work to avoid recursive calls into rndis_command */
-		set_bit(WORK_CONNECTION_EVENT, &priv->work_pending);
-		queue_work(priv->workqueue, &priv->work);
-	} else {
-		evt.data.flags = 0;
-		evt.data.length = 0;
-		memset(evt.ap_addr.sa_data, 0, ETH_ALEN);
-		wireless_send_event(dev->net, SIOCGIWAP, &evt, NULL);
-	}
+	/* queue work to avoid recursive calls into rndis_command */
+	set_bit(state ? WORK_LINK_UP : WORK_LINK_DOWN, &priv->work_pending);
+	queue_work(priv->workqueue, &priv->work);
 }
 
 
-static int rndis_wext_get_caps(struct usbnet *dev)
+static int rndis_wext_get_caps(struct usbnet *usbdev)
 {
 	struct {
 		__le32	num_items;
@@ -2290,18 +2316,18 @@ static int rndis_wext_get_caps(struct usbnet *dev)
 	} networks_supported;
 	int len, retval, i, n;
 	__le32 tx_power;
-	struct rndis_wext_private *priv = get_rndis_wext_priv(dev);
+	struct rndis_wext_private *priv = get_rndis_wext_priv(usbdev);
 
 	/* determine if supports setting txpower */
 	len = sizeof(tx_power);
-	retval = rndis_query_oid(dev, OID_802_11_TX_POWER_LEVEL, &tx_power,
-								&len);
+	retval = rndis_query_oid(usbdev, OID_802_11_TX_POWER_LEVEL, &tx_power,
+									&len);
 	if (retval == 0 && le32_to_cpu(tx_power) != 0xFF)
 		priv->caps |= CAP_SUPPORT_TXPOWER;
 
 	/* determine supported modes */
 	len = sizeof(networks_supported);
-	retval = rndis_query_oid(dev, OID_802_11_NETWORK_TYPES_SUPPORTED,
+	retval = rndis_query_oid(usbdev, OID_802_11_NETWORK_TYPES_SUPPORTED,
 						&networks_supported, &len);
 	if (retval >= 0) {
 		n = le32_to_cpu(networks_supported.num_items);
@@ -2440,9 +2466,9 @@ end:
 }
 
 
-static int bcm4320_early_init(struct usbnet *dev)
+static int bcm4320_early_init(struct usbnet *usbdev)
 {
-	struct rndis_wext_private *priv = get_rndis_wext_priv(dev);
+	struct rndis_wext_private *priv = get_rndis_wext_priv(usbdev);
 	char buf[8];
 
 	/* Early initialization settings, setting these won't have effect
@@ -2490,51 +2516,48 @@ static int bcm4320_early_init(struct usbnet *dev)
 	else
 		priv->param_workaround_interval = modparam_workaround_interval;
 
-	rndis_set_config_parameter_str(dev, "Country", priv->param_country);
-	rndis_set_config_parameter_str(dev, "FrameBursting",
+	rndis_set_config_parameter_str(usbdev, "Country", priv->param_country);
+	rndis_set_config_parameter_str(usbdev, "FrameBursting",
 					priv->param_frameburst ? "1" : "0");
-	rndis_set_config_parameter_str(dev, "Afterburner",
+	rndis_set_config_parameter_str(usbdev, "Afterburner",
 					priv->param_afterburner ? "1" : "0");
 	sprintf(buf, "%d", priv->param_power_save);
-	rndis_set_config_parameter_str(dev, "PowerSaveMode", buf);
+	rndis_set_config_parameter_str(usbdev, "PowerSaveMode", buf);
 	sprintf(buf, "%d", priv->param_power_output);
-	rndis_set_config_parameter_str(dev, "PwrOut", buf);
+	rndis_set_config_parameter_str(usbdev, "PwrOut", buf);
 	sprintf(buf, "%d", priv->param_roamtrigger);
-	rndis_set_config_parameter_str(dev, "RoamTrigger", buf);
+	rndis_set_config_parameter_str(usbdev, "RoamTrigger", buf);
 	sprintf(buf, "%d", priv->param_roamdelta);
-	rndis_set_config_parameter_str(dev, "RoamDelta", buf);
+	rndis_set_config_parameter_str(usbdev, "RoamDelta", buf);
 
 	return 0;
 }
 
 
-static int rndis_wext_bind(struct usbnet *dev, struct usb_interface *intf)
+static int rndis_wext_bind(struct usbnet *usbdev, struct usb_interface *intf)
 {
-	struct net_device *net = dev->net;
 	struct rndis_wext_private *priv;
 	int retval, len;
 	__le32 tmp;
 
 	/* allocate rndis private data */
-	priv = kmalloc(sizeof(struct rndis_wext_private), GFP_KERNEL);
+	priv = kzalloc(sizeof(struct rndis_wext_private), GFP_KERNEL);
 	if (!priv)
 		return -ENOMEM;
 
 	/* These have to be initialized before calling generic_rndis_bind().
 	 * Otherwise we'll be in big trouble in rndis_wext_early_init().
 	 */
-	dev->driver_priv = priv;
-	memset(priv, 0, sizeof(*priv));
-	memset(priv->name, 0, sizeof(priv->name));
+	usbdev->driver_priv = priv;
 	strcpy(priv->name, "IEEE802.11");
-	net->wireless_handlers = &rndis_iw_handlers;
-	priv->usbdev = dev;
+	usbdev->net->wireless_handlers = &rndis_iw_handlers;
+	priv->usbdev = usbdev;
 
 	mutex_init(&priv->command_lock);
 	spin_lock_init(&priv->stats_lock);
 
 	/* try bind rndis_host */
-	retval = generic_rndis_bind(dev, intf, FLAG_RNDIS_PHYM_WIRELESS);
+	retval = generic_rndis_bind(usbdev, intf, FLAG_RNDIS_PHYM_WIRELESS);
 	if (retval < 0)
 		goto fail;
 
@@ -2545,20 +2568,21 @@ static int rndis_wext_bind(struct usbnet *dev, struct usb_interface *intf)
 	 * rndis_host wants to avoid all OID as much as possible
 	 * so do promisc/multicast handling in rndis_wext.
 	 */
-	dev->net->set_multicast_list = rndis_wext_set_multicast_list;
+	usbdev->net->set_multicast_list = rndis_wext_set_multicast_list;
 	tmp = RNDIS_PACKET_TYPE_DIRECTED | RNDIS_PACKET_TYPE_BROADCAST;
-	retval = rndis_set_oid(dev, OID_GEN_CURRENT_PACKET_FILTER, &tmp,
+	retval = rndis_set_oid(usbdev, OID_GEN_CURRENT_PACKET_FILTER, &tmp,
 								sizeof(tmp));
 
 	len = sizeof(tmp);
-	retval = rndis_query_oid(dev, OID_802_3_MAXIMUM_LIST_SIZE, &tmp, &len);
+	retval = rndis_query_oid(usbdev, OID_802_3_MAXIMUM_LIST_SIZE, &tmp,
+								&len);
 	priv->multicast_size = le32_to_cpu(tmp);
 	if (retval < 0 || priv->multicast_size < 0)
 		priv->multicast_size = 0;
 	if (priv->multicast_size > 0)
-		dev->net->flags |= IFF_MULTICAST;
+		usbdev->net->flags |= IFF_MULTICAST;
 	else
-		dev->net->flags &= ~IFF_MULTICAST;
+		usbdev->net->flags &= ~IFF_MULTICAST;
 
 	priv->iwstats.qual.qual = 0;
 	priv->iwstats.qual.level = 0;
@@ -2568,12 +2592,13 @@ static int rndis_wext_bind(struct usbnet *dev, struct usb_interface *intf)
 					| IW_QUAL_QUAL_INVALID
 					| IW_QUAL_LEVEL_INVALID;
 
-	rndis_wext_get_caps(dev);
-	set_default_iw_params(dev);
+	rndis_wext_get_caps(usbdev);
+	set_default_iw_params(usbdev);
 
 	/* turn radio on */
 	priv->radio_on = 1;
-	disassociate(dev, 1);
+	disassociate(usbdev, 1);
+	netif_carrier_off(usbdev->net);
 
 	/* because rndis_command() sleeps we need to use workqueue */
 	priv->workqueue = create_singlethread_workqueue("rndis_wlan");
@@ -2590,12 +2615,12 @@ fail:
 }
 
 
-static void rndis_wext_unbind(struct usbnet *dev, struct usb_interface *intf)
+static void rndis_wext_unbind(struct usbnet *usbdev, struct usb_interface *intf)
 {
-	struct rndis_wext_private *priv = get_rndis_wext_priv(dev);
+	struct rndis_wext_private *priv = get_rndis_wext_priv(usbdev);
 
 	/* turn radio off */
-	disassociate(dev, 0);
+	disassociate(usbdev, 0);
 
 	cancel_delayed_work_sync(&priv->stats_work);
 	cancel_work_sync(&priv->work);
@@ -2606,13 +2631,13 @@ static void rndis_wext_unbind(struct usbnet *dev, struct usb_interface *intf)
 		kfree(priv->wpa_ie);
 	kfree(priv);
 
-	rndis_unbind(dev, intf);
+	rndis_unbind(usbdev, intf);
 }
 
 
-static int rndis_wext_reset(struct usbnet *dev)
+static int rndis_wext_reset(struct usbnet *usbdev)
 {
-	return deauthenticate(dev);
+	return deauthenticate(usbdev);
 }
 
 
diff --git a/drivers/net/wireless/rt2x00/Kconfig b/drivers/net/wireless/rt2x00/Kconfig
index 2d611876bbe0..d485a86bba75 100644
--- a/drivers/net/wireless/rt2x00/Kconfig
+++ b/drivers/net/wireless/rt2x00/Kconfig
@@ -5,12 +5,16 @@ config RT2X00
 	  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 mac80211 stack.
+	  These drivers make use of the mac80211 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".
 
+	  Additionally PCI and USB libraries will also be build depending
+	  on the types of drivers being selected, these libraries will be
+	  called "rt2x00pci.ko" and "rt2x00usb.ko".
+
 if RT2X00
 
 config RT2X00_LIB
@@ -32,35 +36,34 @@ config RT2X00_LIB_FIRMWARE
 config RT2X00_LIB_RFKILL
 	boolean
 	depends on RT2X00_LIB
-	depends on INPUT
 	select RFKILL
-	select INPUT_POLLDEV
 
 config RT2X00_LIB_LEDS
 	boolean
 	depends on RT2X00_LIB && NEW_LEDS
 
 config RT2400PCI
-	tristate "Ralink rt2400 pci/pcmcia support"
+	tristate "Ralink rt2400 (PCI/PCMCIA) support"
 	depends on PCI
 	select RT2X00_LIB_PCI
 	select EEPROM_93CX6
 	---help---
-	  This is an experimental driver for the Ralink rt2400 wireless chip.
+	  This adds support for rt2400 wireless chipset family.
+	  Supported chips: RT2460.
 
 	  When compiled as a module, this driver will be called "rt2400pci.ko".
 
 config RT2400PCI_RFKILL
-	bool "RT2400 rfkill support"
-	depends on RT2400PCI && INPUT
+	bool "Ralink rt2400 rfkill support"
+	depends on RT2400PCI
 	select RT2X00_LIB_RFKILL
 	---help---
-	  This adds support for integrated rt2400 devices that feature a
+	  This adds support for integrated rt2400 hardware that features a
 	  hardware button to control the radio state.
 	  This feature depends on the RF switch subsystem rfkill.
 
 config RT2400PCI_LEDS
-	bool "RT2400 leds support"
+	bool "Ralink rt2400 leds support"
 	depends on RT2400PCI && NEW_LEDS
 	select LEDS_CLASS
 	select RT2X00_LIB_LEDS
@@ -68,26 +71,27 @@ config RT2400PCI_LEDS
 	  This adds support for led triggers provided my mac80211.
 
 config RT2500PCI
-	tristate "Ralink rt2500 pci/pcmcia support"
+	tristate "Ralink rt2500 (PCI/PCMCIA) support"
 	depends on PCI
 	select RT2X00_LIB_PCI
 	select EEPROM_93CX6
 	---help---
-	  This is an experimental driver for the Ralink rt2500 wireless chip.
+	  This adds support for rt2500 wireless chipset family.
+	  Supported chips: RT2560.
 
 	  When compiled as a module, this driver will be called "rt2500pci.ko".
 
 config RT2500PCI_RFKILL
-	bool "RT2500 rfkill support"
-	depends on RT2500PCI && INPUT
+	bool "Ralink rt2500 rfkill support"
+	depends on RT2500PCI
 	select RT2X00_LIB_RFKILL
 	---help---
-	  This adds support for integrated rt2500 devices that feature a
+	  This adds support for integrated rt2500 hardware that features a
 	  hardware button to control the radio state.
 	  This feature depends on the RF switch subsystem rfkill.
 
 config RT2500PCI_LEDS
-	bool "RT2500 leds support"
+	bool "Ralink rt2500 leds support"
 	depends on RT2500PCI && NEW_LEDS
 	select LEDS_CLASS
 	select RT2X00_LIB_LEDS
@@ -95,28 +99,29 @@ config RT2500PCI_LEDS
 	  This adds support for led triggers provided my mac80211.
 
 config RT61PCI
-	tristate "Ralink rt61 pci/pcmcia support"
+	tristate "Ralink rt2501/rt61 (PCI/PCMCIA) support"
 	depends on PCI
 	select RT2X00_LIB_PCI
 	select RT2X00_LIB_FIRMWARE
 	select CRC_ITU_T
 	select EEPROM_93CX6
 	---help---
-	  This is an experimental driver for the Ralink rt61 wireless chip.
+	  This adds support for rt2501 wireless chipset family.
+	  Supported chips: RT2561, RT2561S & RT2661.
 
 	  When compiled as a module, this driver will be called "rt61pci.ko".
 
 config RT61PCI_RFKILL
-	bool "RT61 rfkill support"
-	depends on RT61PCI && INPUT
+	bool "Ralink rt2501/rt61 rfkill support"
+	depends on RT61PCI
 	select RT2X00_LIB_RFKILL
 	---help---
-	  This adds support for integrated rt61 devices that feature a
+	  This adds support for integrated rt61 hardware that features a
 	  hardware button to control the radio state.
 	  This feature depends on the RF switch subsystem rfkill.
 
 config RT61PCI_LEDS
-	bool "RT61 leds support"
+	bool "Ralink rt2501/rt61 leds support"
 	depends on RT61PCI && NEW_LEDS
 	select LEDS_CLASS
 	select RT2X00_LIB_LEDS
@@ -124,16 +129,17 @@ config RT61PCI_LEDS
 	  This adds support for led triggers provided my mac80211.
 
 config RT2500USB
-	tristate "Ralink rt2500 usb support"
+	tristate "Ralink rt2500 (USB) support"
 	depends on USB
 	select RT2X00_LIB_USB
 	---help---
-	  This is an experimental driver for the Ralink rt2500 wireless chip.
+	  This adds support for rt2500 wireless chipset family.
+	  Supported chips: RT2571 & RT2572.
 
 	  When compiled as a module, this driver will be called "rt2500usb.ko".
 
 config RT2500USB_LEDS
-	bool "RT2500 leds support"
+	bool "Ralink rt2500 leds support"
 	depends on RT2500USB && NEW_LEDS
 	select LEDS_CLASS
 	select RT2X00_LIB_LEDS
@@ -141,18 +147,19 @@ config RT2500USB_LEDS
 	  This adds support for led triggers provided my mac80211.
 
 config RT73USB
-	tristate "Ralink rt73 usb support"
+	tristate "Ralink rt2501/rt73 (USB) support"
 	depends on USB
 	select RT2X00_LIB_USB
 	select RT2X00_LIB_FIRMWARE
 	select CRC_ITU_T
 	---help---
-	  This is an experimental driver for the Ralink rt73 wireless chip.
+	  This adds support for rt2501 wireless chipset family.
+	  Supported chips: RT2571W, RT2573 & RT2671.
 
 	  When compiled as a module, this driver will be called "rt73usb.ko".
 
 config RT73USB_LEDS
-	bool "RT73 leds support"
+	bool "Ralink rt2501/rt73 leds support"
 	depends on RT73USB && NEW_LEDS
 	select LEDS_CLASS
 	select RT2X00_LIB_LEDS
@@ -165,7 +172,7 @@ config RT2X00_LIB_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.
+	  most important register types of the rt2x00 hardware.
 
 config RT2X00_DEBUG
 	bool "Ralink debug output"
diff --git a/drivers/net/wireless/rt2x00/rt2400pci.c b/drivers/net/wireless/rt2x00/rt2400pci.c
index b36ed1c6c746..4c0538d6099b 100644
--- a/drivers/net/wireless/rt2x00/rt2400pci.c
+++ b/drivers/net/wireless/rt2x00/rt2400pci.c
@@ -277,6 +277,17 @@ static int rt2400pci_blink_set(struct led_classdev *led_cdev,
 
 	return 0;
 }
+
+static void rt2400pci_init_led(struct rt2x00_dev *rt2x00dev,
+			       struct rt2x00_led *led,
+			       enum led_type type)
+{
+	led->rt2x00dev = rt2x00dev;
+	led->type = type;
+	led->led_dev.brightness_set = rt2400pci_brightness_set;
+	led->led_dev.blink_set = rt2400pci_blink_set;
+	led->flags = LED_INITIALIZED;
+}
 #endif /* CONFIG_RT2400PCI_LEDS */
 
 /*
@@ -620,48 +631,38 @@ static void rt2400pci_link_tuner(struct rt2x00_dev *rt2x00dev)
 static void rt2400pci_init_rxentry(struct rt2x00_dev *rt2x00dev,
 				   struct queue_entry *entry)
 {
-	struct queue_entry_priv_pci_rx *priv_rx = entry->priv_data;
+	struct queue_entry_priv_pci *entry_priv = entry->priv_data;
+	struct skb_frame_desc *skbdesc = get_skb_frame_desc(entry->skb);
 	u32 word;
 
-	rt2x00_desc_read(priv_rx->desc, 2, &word);
-	rt2x00_set_field32(&word, RXD_W2_BUFFER_LENGTH,
-			   entry->queue->data_size);
-	rt2x00_desc_write(priv_rx->desc, 2, word);
+	rt2x00_desc_read(entry_priv->desc, 2, &word);
+	rt2x00_set_field32(&word, RXD_W2_BUFFER_LENGTH, entry->skb->len);
+	rt2x00_desc_write(entry_priv->desc, 2, word);
 
-	rt2x00_desc_read(priv_rx->desc, 1, &word);
-	rt2x00_set_field32(&word, RXD_W1_BUFFER_ADDRESS, priv_rx->data_dma);
-	rt2x00_desc_write(priv_rx->desc, 1, word);
+	rt2x00_desc_read(entry_priv->desc, 1, &word);
+	rt2x00_set_field32(&word, RXD_W1_BUFFER_ADDRESS, skbdesc->skb_dma);
+	rt2x00_desc_write(entry_priv->desc, 1, word);
 
-	rt2x00_desc_read(priv_rx->desc, 0, &word);
+	rt2x00_desc_read(entry_priv->desc, 0, &word);
 	rt2x00_set_field32(&word, RXD_W0_OWNER_NIC, 1);
-	rt2x00_desc_write(priv_rx->desc, 0, word);
+	rt2x00_desc_write(entry_priv->desc, 0, word);
 }
 
 static void rt2400pci_init_txentry(struct rt2x00_dev *rt2x00dev,
 				   struct queue_entry *entry)
 {
-	struct queue_entry_priv_pci_tx *priv_tx = entry->priv_data;
+	struct queue_entry_priv_pci *entry_priv = entry->priv_data;
 	u32 word;
 
-	rt2x00_desc_read(priv_tx->desc, 1, &word);
-	rt2x00_set_field32(&word, TXD_W1_BUFFER_ADDRESS, priv_tx->data_dma);
-	rt2x00_desc_write(priv_tx->desc, 1, word);
-
-	rt2x00_desc_read(priv_tx->desc, 2, &word);
-	rt2x00_set_field32(&word, TXD_W2_BUFFER_LENGTH,
-			   entry->queue->data_size);
-	rt2x00_desc_write(priv_tx->desc, 2, word);
-
-	rt2x00_desc_read(priv_tx->desc, 0, &word);
+	rt2x00_desc_read(entry_priv->desc, 0, &word);
 	rt2x00_set_field32(&word, TXD_W0_VALID, 0);
 	rt2x00_set_field32(&word, TXD_W0_OWNER_NIC, 0);
-	rt2x00_desc_write(priv_tx->desc, 0, word);
+	rt2x00_desc_write(entry_priv->desc, 0, word);
 }
 
 static int rt2400pci_init_queues(struct rt2x00_dev *rt2x00dev)
 {
-	struct queue_entry_priv_pci_rx *priv_rx;
-	struct queue_entry_priv_pci_tx *priv_tx;
+	struct queue_entry_priv_pci *entry_priv;
 	u32 reg;
 
 	/*
@@ -674,28 +675,28 @@ static int rt2400pci_init_queues(struct rt2x00_dev *rt2x00dev)
 	rt2x00_set_field32(&reg, TXCSR2_NUM_PRIO, rt2x00dev->tx[0].limit);
 	rt2x00pci_register_write(rt2x00dev, TXCSR2, reg);
 
-	priv_tx = rt2x00dev->tx[1].entries[0].priv_data;
+	entry_priv = rt2x00dev->tx[1].entries[0].priv_data;
 	rt2x00pci_register_read(rt2x00dev, TXCSR3, &reg);
 	rt2x00_set_field32(&reg, TXCSR3_TX_RING_REGISTER,
-			   priv_tx->desc_dma);
+			   entry_priv->desc_dma);
 	rt2x00pci_register_write(rt2x00dev, TXCSR3, reg);
 
-	priv_tx = rt2x00dev->tx[0].entries[0].priv_data;
+	entry_priv = rt2x00dev->tx[0].entries[0].priv_data;
 	rt2x00pci_register_read(rt2x00dev, TXCSR5, &reg);
 	rt2x00_set_field32(&reg, TXCSR5_PRIO_RING_REGISTER,
-			   priv_tx->desc_dma);
+			   entry_priv->desc_dma);
 	rt2x00pci_register_write(rt2x00dev, TXCSR5, reg);
 
-	priv_tx = rt2x00dev->bcn[1].entries[0].priv_data;
+	entry_priv = rt2x00dev->bcn[1].entries[0].priv_data;
 	rt2x00pci_register_read(rt2x00dev, TXCSR4, &reg);
 	rt2x00_set_field32(&reg, TXCSR4_ATIM_RING_REGISTER,
-			   priv_tx->desc_dma);
+			   entry_priv->desc_dma);
 	rt2x00pci_register_write(rt2x00dev, TXCSR4, reg);
 
-	priv_tx = rt2x00dev->bcn[0].entries[0].priv_data;
+	entry_priv = rt2x00dev->bcn[0].entries[0].priv_data;
 	rt2x00pci_register_read(rt2x00dev, TXCSR6, &reg);
 	rt2x00_set_field32(&reg, TXCSR6_BEACON_RING_REGISTER,
-			   priv_tx->desc_dma);
+			   entry_priv->desc_dma);
 	rt2x00pci_register_write(rt2x00dev, TXCSR6, reg);
 
 	rt2x00pci_register_read(rt2x00dev, RXCSR1, &reg);
@@ -703,9 +704,10 @@ static int rt2400pci_init_queues(struct rt2x00_dev *rt2x00dev)
 	rt2x00_set_field32(&reg, RXCSR1_NUM_RXD, rt2x00dev->rx->limit);
 	rt2x00pci_register_write(rt2x00dev, RXCSR1, reg);
 
-	priv_rx = rt2x00dev->rx->entries[0].priv_data;
+	entry_priv = rt2x00dev->rx->entries[0].priv_data;
 	rt2x00pci_register_read(rt2x00dev, RXCSR2, &reg);
-	rt2x00_set_field32(&reg, RXCSR2_RX_RING_REGISTER, priv_rx->desc_dma);
+	rt2x00_set_field32(&reg, RXCSR2_RX_RING_REGISTER,
+			   entry_priv->desc_dma);
 	rt2x00pci_register_write(rt2x00dev, RXCSR2, reg);
 
 	return 0;
@@ -801,25 +803,32 @@ static int rt2400pci_init_registers(struct rt2x00_dev *rt2x00dev)
 	return 0;
 }
 
-static int rt2400pci_init_bbp(struct rt2x00_dev *rt2x00dev)
+static int rt2400pci_wait_bbp_ready(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");
+			return 0;
 		udelay(REGISTER_BUSY_DELAY);
 	}
 
 	ERROR(rt2x00dev, "BBP register access failed, aborting.\n");
 	return -EACCES;
+}
+
+static int rt2400pci_init_bbp(struct rt2x00_dev *rt2x00dev)
+{
+	unsigned int i;
+	u16 eeprom;
+	u8 reg_id;
+	u8 value;
+
+	if (unlikely(rt2400pci_wait_bbp_ready(rt2x00dev)))
+		return -EACCES;
 
-continue_csr_init:
 	rt2400pci_bbp_write(rt2x00dev, 1, 0x00);
 	rt2400pci_bbp_write(rt2x00dev, 3, 0x27);
 	rt2400pci_bbp_write(rt2x00dev, 4, 0x08);
@@ -858,7 +867,8 @@ static void rt2400pci_toggle_rx(struct rt2x00_dev *rt2x00dev,
 
 	rt2x00pci_register_read(rt2x00dev, RXCSR0, &reg);
 	rt2x00_set_field32(&reg, RXCSR0_DISABLE_RX,
-			   state == STATE_RADIO_RX_OFF);
+			   (state == STATE_RADIO_RX_OFF) ||
+			   (state == STATE_RADIO_RX_OFF_LINK));
 	rt2x00pci_register_write(rt2x00dev, RXCSR0, reg);
 }
 
@@ -895,17 +905,10 @@ static int rt2400pci_enable_radio(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Initialize all registers.
 	 */
-	if (rt2400pci_init_queues(rt2x00dev) ||
-	    rt2400pci_init_registers(rt2x00dev) ||
-	    rt2400pci_init_bbp(rt2x00dev)) {
-		ERROR(rt2x00dev, "Register initialization failed.\n");
+	if (unlikely(rt2400pci_init_queues(rt2x00dev) ||
+		     rt2400pci_init_registers(rt2x00dev) ||
+		     rt2400pci_init_bbp(rt2x00dev)))
 		return -EIO;
-	}
-
-	/*
-	 * Enable interrupts.
-	 */
-	rt2400pci_toggle_irq(rt2x00dev, STATE_RADIO_IRQ_ON);
 
 	return 0;
 }
@@ -927,11 +930,6 @@ static void rt2400pci_disable_radio(struct rt2x00_dev *rt2x00dev)
 	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,
@@ -966,10 +964,6 @@ static int rt2400pci_set_state(struct rt2x00_dev *rt2x00dev,
 		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;
 }
 
@@ -987,11 +981,13 @@ static int rt2400pci_set_device_state(struct rt2x00_dev *rt2x00dev,
 		break;
 	case STATE_RADIO_RX_ON:
 	case STATE_RADIO_RX_ON_LINK:
-		rt2400pci_toggle_rx(rt2x00dev, STATE_RADIO_RX_ON);
-		break;
 	case STATE_RADIO_RX_OFF:
 	case STATE_RADIO_RX_OFF_LINK:
-		rt2400pci_toggle_rx(rt2x00dev, STATE_RADIO_RX_OFF);
+		rt2400pci_toggle_rx(rt2x00dev, state);
+		break;
+	case STATE_RADIO_IRQ_ON:
+	case STATE_RADIO_IRQ_OFF:
+		rt2400pci_toggle_irq(rt2x00dev, state);
 		break;
 	case STATE_DEEP_SLEEP:
 	case STATE_SLEEP:
@@ -1004,6 +1000,10 @@ static int rt2400pci_set_device_state(struct rt2x00_dev *rt2x00dev,
 		break;
 	}
 
+	if (unlikely(retval))
+		ERROR(rt2x00dev, "Device failed to enter state %d (%d).\n",
+		      state, retval);
+
 	return retval;
 }
 
@@ -1012,18 +1012,23 @@ static int rt2400pci_set_device_state(struct rt2x00_dev *rt2x00dev,
  */
 static void rt2400pci_write_tx_desc(struct rt2x00_dev *rt2x00dev,
 				    struct sk_buff *skb,
-				    struct txentry_desc *txdesc,
-				    struct ieee80211_tx_control *control)
+				    struct txentry_desc *txdesc)
 {
 	struct skb_frame_desc *skbdesc = get_skb_frame_desc(skb);
+	struct queue_entry_priv_pci *entry_priv = skbdesc->entry->priv_data;
 	__le32 *txd = skbdesc->desc;
 	u32 word;
 
 	/*
 	 * Start writing the descriptor words.
 	 */
+	rt2x00_desc_read(entry_priv->desc, 1, &word);
+	rt2x00_set_field32(&word, TXD_W1_BUFFER_ADDRESS, skbdesc->skb_dma);
+	rt2x00_desc_write(entry_priv->desc, 1, word);
+
 	rt2x00_desc_read(txd, 2, &word);
-	rt2x00_set_field32(&word, TXD_W2_DATABYTE_COUNT, skbdesc->data_len);
+	rt2x00_set_field32(&word, TXD_W2_BUFFER_LENGTH, skb->len);
+	rt2x00_set_field32(&word, TXD_W2_DATABYTE_COUNT, skb->len);
 	rt2x00_desc_write(txd, 2, word);
 
 	rt2x00_desc_read(txd, 3, &word);
@@ -1057,20 +1062,53 @@ static void rt2400pci_write_tx_desc(struct rt2x00_dev *rt2x00dev,
 			   test_bit(ENTRY_TXD_RTS_FRAME, &txdesc->flags));
 	rt2x00_set_field32(&word, TXD_W0_IFS, txdesc->ifs);
 	rt2x00_set_field32(&word, TXD_W0_RETRY_MODE,
-			   !!(control->flags &
-			      IEEE80211_TXCTL_LONG_RETRY_LIMIT));
+			   test_bit(ENTRY_TXD_RETRY_MODE, &txdesc->flags));
 	rt2x00_desc_write(txd, 0, word);
 }
 
 /*
  * TX data initialization
  */
+static void rt2400pci_write_beacon(struct queue_entry *entry)
+{
+	struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
+	struct queue_entry_priv_pci *entry_priv = entry->priv_data;
+	struct skb_frame_desc *skbdesc = get_skb_frame_desc(entry->skb);
+	u32 word;
+	u32 reg;
+
+	/*
+	 * Disable beaconing while we are reloading the beacon data,
+	 * otherwise we might be sending out invalid data.
+	 */
+	rt2x00pci_register_read(rt2x00dev, CSR14, &reg);
+	rt2x00_set_field32(&reg, CSR14_TSF_COUNT, 0);
+	rt2x00_set_field32(&reg, CSR14_TBCN, 0);
+	rt2x00_set_field32(&reg, CSR14_BEACON_GEN, 0);
+	rt2x00pci_register_write(rt2x00dev, CSR14, reg);
+
+	/*
+	 * Replace rt2x00lib allocated descriptor with the
+	 * pointer to the _real_ hardware descriptor.
+	 * After that, map the beacon to DMA and update the
+	 * descriptor.
+	 */
+	memcpy(entry_priv->desc, skbdesc->desc, skbdesc->desc_len);
+	skbdesc->desc = entry_priv->desc;
+
+	rt2x00queue_map_txskb(rt2x00dev, entry->skb);
+
+	rt2x00_desc_read(entry_priv->desc, 1, &word);
+	rt2x00_set_field32(&word, TXD_W1_BUFFER_ADDRESS, skbdesc->skb_dma);
+	rt2x00_desc_write(entry_priv->desc, 1, word);
+}
+
 static void rt2400pci_kick_tx_queue(struct rt2x00_dev *rt2x00dev,
-				    const unsigned int queue)
+				    const enum data_queue_qid queue)
 {
 	u32 reg;
 
-	if (queue == RT2X00_BCN_QUEUE_BEACON) {
+	if (queue == QID_BEACON) {
 		rt2x00pci_register_read(rt2x00dev, CSR14, &reg);
 		if (!rt2x00_get_field32(reg, CSR14_BEACON_GEN)) {
 			rt2x00_set_field32(&reg, CSR14_TSF_COUNT, 1);
@@ -1082,12 +1120,9 @@ static void rt2400pci_kick_tx_queue(struct rt2x00_dev *rt2x00dev,
 	}
 
 	rt2x00pci_register_read(rt2x00dev, TXCSR0, &reg);
-	rt2x00_set_field32(&reg, TXCSR0_KICK_PRIO,
-			   (queue == IEEE80211_TX_QUEUE_DATA0));
-	rt2x00_set_field32(&reg, TXCSR0_KICK_TX,
-			   (queue == IEEE80211_TX_QUEUE_DATA1));
-	rt2x00_set_field32(&reg, TXCSR0_KICK_ATIM,
-			   (queue == RT2X00_BCN_QUEUE_ATIM));
+	rt2x00_set_field32(&reg, TXCSR0_KICK_PRIO, (queue == QID_AC_BE));
+	rt2x00_set_field32(&reg, TXCSR0_KICK_TX, (queue == QID_AC_BK));
+	rt2x00_set_field32(&reg, TXCSR0_KICK_ATIM, (queue == QID_ATIM));
 	rt2x00pci_register_write(rt2x00dev, TXCSR0, reg);
 }
 
@@ -1097,32 +1132,54 @@ static void rt2400pci_kick_tx_queue(struct rt2x00_dev *rt2x00dev,
 static void rt2400pci_fill_rxdone(struct queue_entry *entry,
 				  struct rxdone_entry_desc *rxdesc)
 {
-	struct queue_entry_priv_pci_rx *priv_rx = entry->priv_data;
+	struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
+	struct queue_entry_priv_pci *entry_priv = entry->priv_data;
 	u32 word0;
 	u32 word2;
 	u32 word3;
+	u32 word4;
+	u64 tsf;
+	u32 rx_low;
+	u32 rx_high;
 
-	rt2x00_desc_read(priv_rx->desc, 0, &word0);
-	rt2x00_desc_read(priv_rx->desc, 2, &word2);
-	rt2x00_desc_read(priv_rx->desc, 3, &word3);
+	rt2x00_desc_read(entry_priv->desc, 0, &word0);
+	rt2x00_desc_read(entry_priv->desc, 2, &word2);
+	rt2x00_desc_read(entry_priv->desc, 3, &word3);
+	rt2x00_desc_read(entry_priv->desc, 4, &word4);
 
-	rxdesc->flags = 0;
 	if (rt2x00_get_field32(word0, RXD_W0_CRC_ERROR))
 		rxdesc->flags |= RX_FLAG_FAILED_FCS_CRC;
 	if (rt2x00_get_field32(word0, RXD_W0_PHYSICAL_ERROR))
 		rxdesc->flags |= RX_FLAG_FAILED_PLCP_CRC;
 
 	/*
+	 * We only get the lower 32bits from the timestamp,
+	 * to get the full 64bits we must complement it with
+	 * the timestamp from get_tsf().
+	 * Note that when a wraparound of the lower 32bits
+	 * has occurred between the frame arrival and the get_tsf()
+	 * call, we must decrease the higher 32bits with 1 to get
+	 * to correct value.
+	 */
+	tsf = rt2x00dev->ops->hw->get_tsf(rt2x00dev->hw);
+	rx_low = rt2x00_get_field32(word4, RXD_W4_RX_END_TIME);
+	rx_high = upper_32_bits(tsf);
+
+	if ((u32)tsf <= rx_low)
+		rx_high--;
+
+	/*
 	 * Obtain the status about this packet.
 	 * The signal is the PLCP value, and needs to be stripped
 	 * of the preamble bit (0x08).
 	 */
+	rxdesc->timestamp = ((u64)rx_high << 32) | rx_low;
 	rxdesc->signal = rt2x00_get_field32(word2, RXD_W2_SIGNAL) & ~0x08;
 	rxdesc->rssi = rt2x00_get_field32(word2, RXD_W3_RSSI) -
 	    entry->queue->rt2x00dev->rssi_offset;
 	rxdesc->size = rt2x00_get_field32(word0, RXD_W0_DATABYTE_COUNT);
 
-	rxdesc->dev_flags = RXDONE_SIGNAL_PLCP;
+	rxdesc->dev_flags |= RXDONE_SIGNAL_PLCP;
 	if (rt2x00_get_field32(word0, RXD_W0_MY_BSS))
 		rxdesc->dev_flags |= RXDONE_MY_BSS;
 }
@@ -1131,18 +1188,18 @@ static void rt2400pci_fill_rxdone(struct queue_entry *entry,
  * Interrupt functions.
  */
 static void rt2400pci_txdone(struct rt2x00_dev *rt2x00dev,
-			     const enum ieee80211_tx_queue queue_idx)
+			     const enum data_queue_qid queue_idx)
 {
 	struct data_queue *queue = rt2x00queue_get_queue(rt2x00dev, queue_idx);
-	struct queue_entry_priv_pci_tx *priv_tx;
+	struct queue_entry_priv_pci *entry_priv;
 	struct queue_entry *entry;
 	struct txdone_entry_desc txdesc;
 	u32 word;
 
 	while (!rt2x00queue_empty(queue)) {
 		entry = rt2x00queue_get_entry(queue, Q_INDEX_DONE);
-		priv_tx = entry->priv_data;
-		rt2x00_desc_read(priv_tx->desc, 0, &word);
+		entry_priv = entry->priv_data;
+		rt2x00_desc_read(entry_priv->desc, 0, &word);
 
 		if (rt2x00_get_field32(word, TXD_W0_OWNER_NIC) ||
 		    !rt2x00_get_field32(word, TXD_W0_VALID))
@@ -1151,10 +1208,21 @@ static void rt2400pci_txdone(struct rt2x00_dev *rt2x00dev,
 		/*
 		 * Obtain the status about this packet.
 		 */
-		txdesc.status = rt2x00_get_field32(word, TXD_W0_RESULT);
+		txdesc.flags = 0;
+		switch (rt2x00_get_field32(word, TXD_W0_RESULT)) {
+		case 0: /* Success */
+		case 1: /* Success with retry */
+			__set_bit(TXDONE_SUCCESS, &txdesc.flags);
+			break;
+		case 2: /* Failure, excessive retries */
+			__set_bit(TXDONE_EXCESSIVE_RETRY, &txdesc.flags);
+			/* Don't break, this is a failed frame! */
+		default: /* Failure */
+			__set_bit(TXDONE_FAILURE, &txdesc.flags);
+		}
 		txdesc.retry = rt2x00_get_field32(word, TXD_W0_RETRY_COUNT);
 
-		rt2x00pci_txdone(rt2x00dev, entry, &txdesc);
+		rt2x00lib_txdone(entry, &txdesc);
 	}
 }
 
@@ -1198,19 +1266,19 @@ static irqreturn_t rt2400pci_interrupt(int irq, void *dev_instance)
 	 * 3 - Atim ring transmit done interrupt.
 	 */
 	if (rt2x00_get_field32(reg, CSR7_TXDONE_ATIMRING))
-		rt2400pci_txdone(rt2x00dev, RT2X00_BCN_QUEUE_ATIM);
+		rt2400pci_txdone(rt2x00dev, QID_ATIM);
 
 	/*
 	 * 4 - Priority ring transmit done interrupt.
 	 */
 	if (rt2x00_get_field32(reg, CSR7_TXDONE_PRIORING))
-		rt2400pci_txdone(rt2x00dev, IEEE80211_TX_QUEUE_DATA0);
+		rt2400pci_txdone(rt2x00dev, QID_AC_BE);
 
 	/*
 	 * 5 - Tx ring transmit done interrupt.
 	 */
 	if (rt2x00_get_field32(reg, CSR7_TXDONE_TXRING))
-		rt2400pci_txdone(rt2x00dev, IEEE80211_TX_QUEUE_DATA1);
+		rt2400pci_txdone(rt2x00dev, QID_AC_BK);
 
 	return IRQ_HANDLED;
 }
@@ -1309,23 +1377,10 @@ static int rt2400pci_init_eeprom(struct rt2x00_dev *rt2x00dev)
 #ifdef CONFIG_RT2400PCI_LEDS
 	value = rt2x00_get_field16(eeprom, EEPROM_ANTENNA_LED_MODE);
 
-	rt2x00dev->led_radio.rt2x00dev = rt2x00dev;
-	rt2x00dev->led_radio.type = LED_TYPE_RADIO;
-	rt2x00dev->led_radio.led_dev.brightness_set =
-	    rt2400pci_brightness_set;
-	rt2x00dev->led_radio.led_dev.blink_set =
-	    rt2400pci_blink_set;
-	rt2x00dev->led_radio.flags = LED_INITIALIZED;
-
-	if (value == LED_MODE_TXRX_ACTIVITY) {
-		rt2x00dev->led_qual.rt2x00dev = rt2x00dev;
-		rt2x00dev->led_qual.type = LED_TYPE_ACTIVITY;
-		rt2x00dev->led_qual.led_dev.brightness_set =
-		    rt2400pci_brightness_set;
-		rt2x00dev->led_qual.led_dev.blink_set =
-		    rt2400pci_blink_set;
-		rt2x00dev->led_qual.flags = LED_INITIALIZED;
-	}
+	rt2400pci_init_led(rt2x00dev, &rt2x00dev->led_radio, LED_TYPE_RADIO);
+	if (value == LED_MODE_TXRX_ACTIVITY)
+		rt2400pci_init_led(rt2x00dev, &rt2x00dev->led_qual,
+				   LED_TYPE_ACTIVITY);
 #endif /* CONFIG_RT2400PCI_LEDS */
 
 	/*
@@ -1375,13 +1430,11 @@ static void rt2400pci_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Initialize all hw fields.
 	 */
-	rt2x00dev->hw->flags = IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING;
+	rt2x00dev->hw->flags = IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING |
+			       IEEE80211_HW_SIGNAL_DBM;
 	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_DEV(rt2x00dev->hw, rt2x00dev->dev);
 	SET_IEEE80211_PERM_ADDR(rt2x00dev->hw,
 				rt2x00_eeprom_addr(rt2x00dev,
 						   EEPROM_MAC_ADDR_0));
@@ -1427,9 +1480,10 @@ static int rt2400pci_probe_hw(struct rt2x00_dev *rt2x00dev)
 	rt2400pci_probe_hw_mode(rt2x00dev);
 
 	/*
-	 * This device requires the atim queue
+	 * This device requires the atim queue and DMA-mapped skbs.
 	 */
 	__set_bit(DRIVER_REQUIRE_ATIM_QUEUE, &rt2x00dev->flags);
+	__set_bit(DRIVER_REQUIRE_DMA, &rt2x00dev->flags);
 
 	/*
 	 * Set the rssi offset.
@@ -1456,8 +1510,7 @@ static int rt2400pci_set_retry_limit(struct ieee80211_hw *hw,
 	return 0;
 }
 
-static int rt2400pci_conf_tx(struct ieee80211_hw *hw,
-			     int queue,
+static int rt2400pci_conf_tx(struct ieee80211_hw *hw, u16 queue,
 			     const struct ieee80211_tx_queue_params *params)
 {
 	struct rt2x00_dev *rt2x00dev = hw->priv;
@@ -1467,7 +1520,7 @@ static int rt2400pci_conf_tx(struct ieee80211_hw *hw,
 	 * per queue. So by default we only configure the TX queue,
 	 * and ignore all other configurations.
 	 */
-	if (queue != IEEE80211_TX_QUEUE_DATA0)
+	if (queue != 0)
 		return -EINVAL;
 
 	if (rt2x00mac_conf_tx(hw, queue, params))
@@ -1496,60 +1549,6 @@ static u64 rt2400pci_get_tsf(struct ieee80211_hw *hw)
 	return tsf;
 }
 
-static int rt2400pci_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb,
-				   struct ieee80211_tx_control *control)
-{
-	struct rt2x00_dev *rt2x00dev = hw->priv;
-	struct rt2x00_intf *intf = vif_to_intf(control->vif);
-	struct queue_entry_priv_pci_tx *priv_tx;
-	struct skb_frame_desc *skbdesc;
-	u32 reg;
-
-	if (unlikely(!intf->beacon))
-		return -ENOBUFS;
-	priv_tx = intf->beacon->priv_data;
-
-	/*
-	 * Fill in skb descriptor
-	 */
-	skbdesc = get_skb_frame_desc(skb);
-	memset(skbdesc, 0, sizeof(*skbdesc));
-	skbdesc->flags |= FRAME_DESC_DRIVER_GENERATED;
-	skbdesc->data = skb->data;
-	skbdesc->data_len = skb->len;
-	skbdesc->desc = priv_tx->desc;
-	skbdesc->desc_len = intf->beacon->queue->desc_size;
-	skbdesc->entry = intf->beacon;
-
-	/*
-	 * Disable beaconing while we are reloading the beacon data,
-	 * otherwise we might be sending out invalid data.
-	 */
-	rt2x00pci_register_read(rt2x00dev, CSR14, &reg);
-	rt2x00_set_field32(&reg, CSR14_TSF_COUNT, 0);
-	rt2x00_set_field32(&reg, CSR14_TBCN, 0);
-	rt2x00_set_field32(&reg, CSR14_BEACON_GEN, 0);
-	rt2x00pci_register_write(rt2x00dev, CSR14, reg);
-
-	/*
-	 * mac80211 doesn't provide the control->queue variable
-	 * for beacons. Set our own queue identification so
-	 * it can be used during descriptor initialization.
-	 */
-	control->queue = RT2X00_BCN_QUEUE_BEACON;
-	rt2x00lib_write_tx_desc(rt2x00dev, skb, control);
-
-	/*
-	 * Enable beacon generation.
-	 * Write entire beacon with descriptor to register,
-	 * and kick the beacon generator.
-	 */
-	memcpy(priv_tx->data, skb->data, skb->len);
-	rt2x00dev->ops->lib->kick_tx_queue(rt2x00dev, control->queue);
-
-	return 0;
-}
-
 static int rt2400pci_tx_last_beacon(struct ieee80211_hw *hw)
 {
 	struct rt2x00_dev *rt2x00dev = hw->priv;
@@ -1574,7 +1573,6 @@ static const struct ieee80211_ops rt2400pci_mac80211_ops = {
 	.conf_tx		= rt2400pci_conf_tx,
 	.get_tx_stats		= rt2x00mac_get_tx_stats,
 	.get_tsf		= rt2400pci_get_tsf,
-	.beacon_update		= rt2400pci_beacon_update,
 	.tx_last_beacon		= rt2400pci_tx_last_beacon,
 };
 
@@ -1592,6 +1590,7 @@ static const struct rt2x00lib_ops rt2400pci_rt2x00_ops = {
 	.link_tuner		= rt2400pci_link_tuner,
 	.write_tx_desc		= rt2400pci_write_tx_desc,
 	.write_tx_data		= rt2x00pci_write_tx_data,
+	.write_beacon		= rt2400pci_write_beacon,
 	.kick_tx_queue		= rt2400pci_kick_tx_queue,
 	.fill_rxdone		= rt2400pci_fill_rxdone,
 	.config_filter		= rt2400pci_config_filter,
@@ -1604,28 +1603,28 @@ static const struct data_queue_desc rt2400pci_queue_rx = {
 	.entry_num		= RX_ENTRIES,
 	.data_size		= DATA_FRAME_SIZE,
 	.desc_size		= RXD_DESC_SIZE,
-	.priv_size		= sizeof(struct queue_entry_priv_pci_rx),
+	.priv_size		= sizeof(struct queue_entry_priv_pci),
 };
 
 static const struct data_queue_desc rt2400pci_queue_tx = {
 	.entry_num		= TX_ENTRIES,
 	.data_size		= DATA_FRAME_SIZE,
 	.desc_size		= TXD_DESC_SIZE,
-	.priv_size		= sizeof(struct queue_entry_priv_pci_tx),
+	.priv_size		= sizeof(struct queue_entry_priv_pci),
 };
 
 static const struct data_queue_desc rt2400pci_queue_bcn = {
 	.entry_num		= BEACON_ENTRIES,
 	.data_size		= MGMT_FRAME_SIZE,
 	.desc_size		= TXD_DESC_SIZE,
-	.priv_size		= sizeof(struct queue_entry_priv_pci_tx),
+	.priv_size		= sizeof(struct queue_entry_priv_pci),
 };
 
 static const struct data_queue_desc rt2400pci_queue_atim = {
 	.entry_num		= ATIM_ENTRIES,
 	.data_size		= DATA_FRAME_SIZE,
 	.desc_size		= TXD_DESC_SIZE,
-	.priv_size		= sizeof(struct queue_entry_priv_pci_tx),
+	.priv_size		= sizeof(struct queue_entry_priv_pci),
 };
 
 static const struct rt2x00_ops rt2400pci_ops = {
@@ -1634,6 +1633,7 @@ static const struct rt2x00_ops rt2400pci_ops = {
 	.max_ap_intf	= 1,
 	.eeprom_size	= EEPROM_SIZE,
 	.rf_size	= RF_SIZE,
+	.tx_queues	= NUM_TX_QUEUES,
 	.rx		= &rt2400pci_queue_rx,
 	.tx		= &rt2400pci_queue_tx,
 	.bcn		= &rt2400pci_queue_bcn,
diff --git a/drivers/net/wireless/rt2x00/rt2400pci.h b/drivers/net/wireless/rt2x00/rt2400pci.h
index a5210f9a3360..bc5564258228 100644
--- a/drivers/net/wireless/rt2x00/rt2400pci.h
+++ b/drivers/net/wireless/rt2x00/rt2400pci.h
@@ -37,8 +37,6 @@
  * Signal information.
  * Defaul offset is required for RSSI <-> dBm conversion.
  */
-#define MAX_SIGNAL			100
-#define MAX_RX_SSI			-1
 #define DEFAULT_RSSI_OFFSET		100
 
 /*
@@ -52,6 +50,11 @@
 #define RF_SIZE				0x0010
 
 /*
+ * Number of TX queues.
+ */
+#define NUM_TX_QUEUES			2
+
+/*
  * Control/Status Registers(CSR).
  * Some values are set in TU, whereas 1 TU == 1024 us.
  */
diff --git a/drivers/net/wireless/rt2x00/rt2500pci.c b/drivers/net/wireless/rt2x00/rt2500pci.c
index f7731fb82555..aa6dfb811c71 100644
--- a/drivers/net/wireless/rt2x00/rt2500pci.c
+++ b/drivers/net/wireless/rt2x00/rt2500pci.c
@@ -277,6 +277,17 @@ static int rt2500pci_blink_set(struct led_classdev *led_cdev,
 
 	return 0;
 }
+
+static void rt2500pci_init_led(struct rt2x00_dev *rt2x00dev,
+			       struct rt2x00_led *led,
+			       enum led_type type)
+{
+	led->rt2x00dev = rt2x00dev;
+	led->type = type;
+	led->led_dev.brightness_set = rt2500pci_brightness_set;
+	led->led_dev.blink_set = rt2500pci_blink_set;
+	led->flags = LED_INITIALIZED;
+}
 #endif /* CONFIG_RT2500PCI_LEDS */
 
 /*
@@ -317,8 +328,7 @@ static void rt2500pci_config_intf(struct rt2x00_dev *rt2x00dev,
 				  struct rt2x00intf_conf *conf,
 				  const unsigned int flags)
 {
-	struct data_queue *queue =
-	    rt2x00queue_get_queue(rt2x00dev, RT2X00_BCN_QUEUE_BEACON);
+	struct data_queue *queue = rt2x00queue_get_queue(rt2x00dev, QID_BEACON);
 	unsigned int bcn_preload;
 	u32 reg;
 
@@ -716,38 +726,34 @@ dynamic_cca_tune:
 static void rt2500pci_init_rxentry(struct rt2x00_dev *rt2x00dev,
 				   struct queue_entry *entry)
 {
-	struct queue_entry_priv_pci_rx *priv_rx = entry->priv_data;
+	struct queue_entry_priv_pci *entry_priv = entry->priv_data;
+	struct skb_frame_desc *skbdesc = get_skb_frame_desc(entry->skb);
 	u32 word;
 
-	rt2x00_desc_read(priv_rx->desc, 1, &word);
-	rt2x00_set_field32(&word, RXD_W1_BUFFER_ADDRESS, priv_rx->data_dma);
-	rt2x00_desc_write(priv_rx->desc, 1, word);
+	rt2x00_desc_read(entry_priv->desc, 1, &word);
+	rt2x00_set_field32(&word, RXD_W1_BUFFER_ADDRESS, skbdesc->skb_dma);
+	rt2x00_desc_write(entry_priv->desc, 1, word);
 
-	rt2x00_desc_read(priv_rx->desc, 0, &word);
+	rt2x00_desc_read(entry_priv->desc, 0, &word);
 	rt2x00_set_field32(&word, RXD_W0_OWNER_NIC, 1);
-	rt2x00_desc_write(priv_rx->desc, 0, word);
+	rt2x00_desc_write(entry_priv->desc, 0, word);
 }
 
 static void rt2500pci_init_txentry(struct rt2x00_dev *rt2x00dev,
 				   struct queue_entry *entry)
 {
-	struct queue_entry_priv_pci_tx *priv_tx = entry->priv_data;
+	struct queue_entry_priv_pci *entry_priv = entry->priv_data;
 	u32 word;
 
-	rt2x00_desc_read(priv_tx->desc, 1, &word);
-	rt2x00_set_field32(&word, TXD_W1_BUFFER_ADDRESS, priv_tx->data_dma);
-	rt2x00_desc_write(priv_tx->desc, 1, word);
-
-	rt2x00_desc_read(priv_tx->desc, 0, &word);
+	rt2x00_desc_read(entry_priv->desc, 0, &word);
 	rt2x00_set_field32(&word, TXD_W0_VALID, 0);
 	rt2x00_set_field32(&word, TXD_W0_OWNER_NIC, 0);
-	rt2x00_desc_write(priv_tx->desc, 0, word);
+	rt2x00_desc_write(entry_priv->desc, 0, word);
 }
 
 static int rt2500pci_init_queues(struct rt2x00_dev *rt2x00dev)
 {
-	struct queue_entry_priv_pci_rx *priv_rx;
-	struct queue_entry_priv_pci_tx *priv_tx;
+	struct queue_entry_priv_pci *entry_priv;
 	u32 reg;
 
 	/*
@@ -760,28 +766,28 @@ static int rt2500pci_init_queues(struct rt2x00_dev *rt2x00dev)
 	rt2x00_set_field32(&reg, TXCSR2_NUM_PRIO, rt2x00dev->tx[0].limit);
 	rt2x00pci_register_write(rt2x00dev, TXCSR2, reg);
 
-	priv_tx = rt2x00dev->tx[1].entries[0].priv_data;
+	entry_priv = rt2x00dev->tx[1].entries[0].priv_data;
 	rt2x00pci_register_read(rt2x00dev, TXCSR3, &reg);
 	rt2x00_set_field32(&reg, TXCSR3_TX_RING_REGISTER,
-			   priv_tx->desc_dma);
+			   entry_priv->desc_dma);
 	rt2x00pci_register_write(rt2x00dev, TXCSR3, reg);
 
-	priv_tx = rt2x00dev->tx[0].entries[0].priv_data;
+	entry_priv = rt2x00dev->tx[0].entries[0].priv_data;
 	rt2x00pci_register_read(rt2x00dev, TXCSR5, &reg);
 	rt2x00_set_field32(&reg, TXCSR5_PRIO_RING_REGISTER,
-			   priv_tx->desc_dma);
+			   entry_priv->desc_dma);
 	rt2x00pci_register_write(rt2x00dev, TXCSR5, reg);
 
-	priv_tx = rt2x00dev->bcn[1].entries[0].priv_data;
+	entry_priv = rt2x00dev->bcn[1].entries[0].priv_data;
 	rt2x00pci_register_read(rt2x00dev, TXCSR4, &reg);
 	rt2x00_set_field32(&reg, TXCSR4_ATIM_RING_REGISTER,
-			   priv_tx->desc_dma);
+			   entry_priv->desc_dma);
 	rt2x00pci_register_write(rt2x00dev, TXCSR4, reg);
 
-	priv_tx = rt2x00dev->bcn[0].entries[0].priv_data;
+	entry_priv = rt2x00dev->bcn[0].entries[0].priv_data;
 	rt2x00pci_register_read(rt2x00dev, TXCSR6, &reg);
 	rt2x00_set_field32(&reg, TXCSR6_BEACON_RING_REGISTER,
-			   priv_tx->desc_dma);
+			   entry_priv->desc_dma);
 	rt2x00pci_register_write(rt2x00dev, TXCSR6, reg);
 
 	rt2x00pci_register_read(rt2x00dev, RXCSR1, &reg);
@@ -789,9 +795,10 @@ static int rt2500pci_init_queues(struct rt2x00_dev *rt2x00dev)
 	rt2x00_set_field32(&reg, RXCSR1_NUM_RXD, rt2x00dev->rx->limit);
 	rt2x00pci_register_write(rt2x00dev, RXCSR1, reg);
 
-	priv_rx = rt2x00dev->rx->entries[0].priv_data;
+	entry_priv = rt2x00dev->rx->entries[0].priv_data;
 	rt2x00pci_register_read(rt2x00dev, RXCSR2, &reg);
-	rt2x00_set_field32(&reg, RXCSR2_RX_RING_REGISTER, priv_rx->desc_dma);
+	rt2x00_set_field32(&reg, RXCSR2_RX_RING_REGISTER,
+			   entry_priv->desc_dma);
 	rt2x00pci_register_write(rt2x00dev, RXCSR2, reg);
 
 	return 0;
@@ -940,25 +947,32 @@ static int rt2500pci_init_registers(struct rt2x00_dev *rt2x00dev)
 	return 0;
 }
 
-static int rt2500pci_init_bbp(struct rt2x00_dev *rt2x00dev)
+static int rt2500pci_wait_bbp_ready(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");
+			return 0;
 		udelay(REGISTER_BUSY_DELAY);
 	}
 
 	ERROR(rt2x00dev, "BBP register access failed, aborting.\n");
 	return -EACCES;
+}
+
+static int rt2500pci_init_bbp(struct rt2x00_dev *rt2x00dev)
+{
+	unsigned int i;
+	u16 eeprom;
+	u8 reg_id;
+	u8 value;
+
+	if (unlikely(rt2500pci_wait_bbp_ready(rt2x00dev)))
+		return -EACCES;
 
-continue_csr_init:
 	rt2500pci_bbp_write(rt2x00dev, 3, 0x02);
 	rt2500pci_bbp_write(rt2x00dev, 4, 0x19);
 	rt2500pci_bbp_write(rt2x00dev, 14, 0x1c);
@@ -1013,7 +1027,8 @@ static void rt2500pci_toggle_rx(struct rt2x00_dev *rt2x00dev,
 
 	rt2x00pci_register_read(rt2x00dev, RXCSR0, &reg);
 	rt2x00_set_field32(&reg, RXCSR0_DISABLE_RX,
-			   state == STATE_RADIO_RX_OFF);
+			   (state == STATE_RADIO_RX_OFF) ||
+			   (state == STATE_RADIO_RX_OFF_LINK));
 	rt2x00pci_register_write(rt2x00dev, RXCSR0, reg);
 }
 
@@ -1050,17 +1065,10 @@ static int rt2500pci_enable_radio(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Initialize all registers.
 	 */
-	if (rt2500pci_init_queues(rt2x00dev) ||
-	    rt2500pci_init_registers(rt2x00dev) ||
-	    rt2500pci_init_bbp(rt2x00dev)) {
-		ERROR(rt2x00dev, "Register initialization failed.\n");
+	if (unlikely(rt2500pci_init_queues(rt2x00dev) ||
+		     rt2500pci_init_registers(rt2x00dev) ||
+		     rt2500pci_init_bbp(rt2x00dev)))
 		return -EIO;
-	}
-
-	/*
-	 * Enable interrupts.
-	 */
-	rt2500pci_toggle_irq(rt2x00dev, STATE_RADIO_IRQ_ON);
 
 	return 0;
 }
@@ -1082,11 +1090,6 @@ static void rt2500pci_disable_radio(struct rt2x00_dev *rt2x00dev)
 	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,
@@ -1121,10 +1124,6 @@ static int rt2500pci_set_state(struct rt2x00_dev *rt2x00dev,
 		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;
 }
 
@@ -1142,11 +1141,13 @@ static int rt2500pci_set_device_state(struct rt2x00_dev *rt2x00dev,
 		break;
 	case STATE_RADIO_RX_ON:
 	case STATE_RADIO_RX_ON_LINK:
-		rt2500pci_toggle_rx(rt2x00dev, STATE_RADIO_RX_ON);
-		break;
 	case STATE_RADIO_RX_OFF:
 	case STATE_RADIO_RX_OFF_LINK:
-		rt2500pci_toggle_rx(rt2x00dev, STATE_RADIO_RX_OFF);
+		rt2500pci_toggle_rx(rt2x00dev, state);
+		break;
+	case STATE_RADIO_IRQ_ON:
+	case STATE_RADIO_IRQ_OFF:
+		rt2500pci_toggle_irq(rt2x00dev, state);
 		break;
 	case STATE_DEEP_SLEEP:
 	case STATE_SLEEP:
@@ -1159,6 +1160,10 @@ static int rt2500pci_set_device_state(struct rt2x00_dev *rt2x00dev,
 		break;
 	}
 
+	if (unlikely(retval))
+		ERROR(rt2x00dev, "Device failed to enter state %d (%d).\n",
+		      state, retval);
+
 	return retval;
 }
 
@@ -1167,16 +1172,20 @@ static int rt2500pci_set_device_state(struct rt2x00_dev *rt2x00dev,
  */
 static void rt2500pci_write_tx_desc(struct rt2x00_dev *rt2x00dev,
 				    struct sk_buff *skb,
-				    struct txentry_desc *txdesc,
-				    struct ieee80211_tx_control *control)
+				    struct txentry_desc *txdesc)
 {
 	struct skb_frame_desc *skbdesc = get_skb_frame_desc(skb);
+	struct queue_entry_priv_pci *entry_priv = skbdesc->entry->priv_data;
 	__le32 *txd = skbdesc->desc;
 	u32 word;
 
 	/*
 	 * Start writing the descriptor words.
 	 */
+	rt2x00_desc_read(entry_priv->desc, 1, &word);
+	rt2x00_set_field32(&word, TXD_W1_BUFFER_ADDRESS, skbdesc->skb_dma);
+	rt2x00_desc_write(entry_priv->desc, 1, word);
+
 	rt2x00_desc_read(txd, 2, &word);
 	rt2x00_set_field32(&word, TXD_W2_IV_OFFSET, IEEE80211_HEADER);
 	rt2x00_set_field32(&word, TXD_W2_AIFS, txdesc->aifs);
@@ -1210,9 +1219,7 @@ static void rt2500pci_write_tx_desc(struct rt2x00_dev *rt2x00dev,
 	rt2x00_set_field32(&word, TXD_W0_CIPHER_OWNER, 1);
 	rt2x00_set_field32(&word, TXD_W0_IFS, txdesc->ifs);
 	rt2x00_set_field32(&word, TXD_W0_RETRY_MODE,
-			   !!(control->flags &
-			      IEEE80211_TXCTL_LONG_RETRY_LIMIT));
-	rt2x00_set_field32(&word, TXD_W0_DATABYTE_COUNT, skbdesc->data_len);
+			   test_bit(ENTRY_TXD_RETRY_MODE, &txdesc->flags));
 	rt2x00_set_field32(&word, TXD_W0_CIPHER_ALG, CIPHER_NONE);
 	rt2x00_desc_write(txd, 0, word);
 }
@@ -1220,12 +1227,46 @@ static void rt2500pci_write_tx_desc(struct rt2x00_dev *rt2x00dev,
 /*
  * TX data initialization
  */
+static void rt2500pci_write_beacon(struct queue_entry *entry)
+{
+	struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
+	struct queue_entry_priv_pci *entry_priv = entry->priv_data;
+	struct skb_frame_desc *skbdesc = get_skb_frame_desc(entry->skb);
+	u32 word;
+	u32 reg;
+
+	/*
+	 * Disable beaconing while we are reloading the beacon data,
+	 * otherwise we might be sending out invalid data.
+	 */
+	rt2x00pci_register_read(rt2x00dev, CSR14, &reg);
+	rt2x00_set_field32(&reg, CSR14_TSF_COUNT, 0);
+	rt2x00_set_field32(&reg, CSR14_TBCN, 0);
+	rt2x00_set_field32(&reg, CSR14_BEACON_GEN, 0);
+	rt2x00pci_register_write(rt2x00dev, CSR14, reg);
+
+	/*
+	 * Replace rt2x00lib allocated descriptor with the
+	 * pointer to the _real_ hardware descriptor.
+	 * After that, map the beacon to DMA and update the
+	 * descriptor.
+	 */
+	memcpy(entry_priv->desc, skbdesc->desc, skbdesc->desc_len);
+	skbdesc->desc = entry_priv->desc;
+
+	rt2x00queue_map_txskb(rt2x00dev, entry->skb);
+
+	rt2x00_desc_read(entry_priv->desc, 1, &word);
+	rt2x00_set_field32(&word, TXD_W1_BUFFER_ADDRESS, skbdesc->skb_dma);
+	rt2x00_desc_write(entry_priv->desc, 1, word);
+}
+
 static void rt2500pci_kick_tx_queue(struct rt2x00_dev *rt2x00dev,
-				    const unsigned int queue)
+				    const enum data_queue_qid queue)
 {
 	u32 reg;
 
-	if (queue == RT2X00_BCN_QUEUE_BEACON) {
+	if (queue == QID_BEACON) {
 		rt2x00pci_register_read(rt2x00dev, CSR14, &reg);
 		if (!rt2x00_get_field32(reg, CSR14_BEACON_GEN)) {
 			rt2x00_set_field32(&reg, CSR14_TSF_COUNT, 1);
@@ -1237,12 +1278,9 @@ static void rt2500pci_kick_tx_queue(struct rt2x00_dev *rt2x00dev,
 	}
 
 	rt2x00pci_register_read(rt2x00dev, TXCSR0, &reg);
-	rt2x00_set_field32(&reg, TXCSR0_KICK_PRIO,
-			   (queue == IEEE80211_TX_QUEUE_DATA0));
-	rt2x00_set_field32(&reg, TXCSR0_KICK_TX,
-			   (queue == IEEE80211_TX_QUEUE_DATA1));
-	rt2x00_set_field32(&reg, TXCSR0_KICK_ATIM,
-			   (queue == RT2X00_BCN_QUEUE_ATIM));
+	rt2x00_set_field32(&reg, TXCSR0_KICK_PRIO, (queue == QID_AC_BE));
+	rt2x00_set_field32(&reg, TXCSR0_KICK_TX, (queue == QID_AC_BK));
+	rt2x00_set_field32(&reg, TXCSR0_KICK_ATIM, (queue == QID_ATIM));
 	rt2x00pci_register_write(rt2x00dev, TXCSR0, reg);
 }
 
@@ -1252,14 +1290,13 @@ static void rt2500pci_kick_tx_queue(struct rt2x00_dev *rt2x00dev,
 static void rt2500pci_fill_rxdone(struct queue_entry *entry,
 				  struct rxdone_entry_desc *rxdesc)
 {
-	struct queue_entry_priv_pci_rx *priv_rx = entry->priv_data;
+	struct queue_entry_priv_pci *entry_priv = entry->priv_data;
 	u32 word0;
 	u32 word2;
 
-	rt2x00_desc_read(priv_rx->desc, 0, &word0);
-	rt2x00_desc_read(priv_rx->desc, 2, &word2);
+	rt2x00_desc_read(entry_priv->desc, 0, &word0);
+	rt2x00_desc_read(entry_priv->desc, 2, &word2);
 
-	rxdesc->flags = 0;
 	if (rt2x00_get_field32(word0, RXD_W0_CRC_ERROR))
 		rxdesc->flags |= RX_FLAG_FAILED_FCS_CRC;
 	if (rt2x00_get_field32(word0, RXD_W0_PHYSICAL_ERROR))
@@ -1276,7 +1313,6 @@ static void rt2500pci_fill_rxdone(struct queue_entry *entry,
 	    entry->queue->rt2x00dev->rssi_offset;
 	rxdesc->size = rt2x00_get_field32(word0, RXD_W0_DATABYTE_COUNT);
 
-	rxdesc->dev_flags = 0;
 	if (rt2x00_get_field32(word0, RXD_W0_OFDM))
 		rxdesc->dev_flags |= RXDONE_SIGNAL_PLCP;
 	if (rt2x00_get_field32(word0, RXD_W0_MY_BSS))
@@ -1287,18 +1323,18 @@ static void rt2500pci_fill_rxdone(struct queue_entry *entry,
  * Interrupt functions.
  */
 static void rt2500pci_txdone(struct rt2x00_dev *rt2x00dev,
-			     const enum ieee80211_tx_queue queue_idx)
+			     const enum data_queue_qid queue_idx)
 {
 	struct data_queue *queue = rt2x00queue_get_queue(rt2x00dev, queue_idx);
-	struct queue_entry_priv_pci_tx *priv_tx;
+	struct queue_entry_priv_pci *entry_priv;
 	struct queue_entry *entry;
 	struct txdone_entry_desc txdesc;
 	u32 word;
 
 	while (!rt2x00queue_empty(queue)) {
 		entry = rt2x00queue_get_entry(queue, Q_INDEX_DONE);
-		priv_tx = entry->priv_data;
-		rt2x00_desc_read(priv_tx->desc, 0, &word);
+		entry_priv = entry->priv_data;
+		rt2x00_desc_read(entry_priv->desc, 0, &word);
 
 		if (rt2x00_get_field32(word, TXD_W0_OWNER_NIC) ||
 		    !rt2x00_get_field32(word, TXD_W0_VALID))
@@ -1307,10 +1343,21 @@ static void rt2500pci_txdone(struct rt2x00_dev *rt2x00dev,
 		/*
 		 * Obtain the status about this packet.
 		 */
-		txdesc.status = rt2x00_get_field32(word, TXD_W0_RESULT);
+		txdesc.flags = 0;
+		switch (rt2x00_get_field32(word, TXD_W0_RESULT)) {
+		case 0: /* Success */
+		case 1: /* Success with retry */
+			__set_bit(TXDONE_SUCCESS, &txdesc.flags);
+			break;
+		case 2: /* Failure, excessive retries */
+			__set_bit(TXDONE_EXCESSIVE_RETRY, &txdesc.flags);
+			/* Don't break, this is a failed frame! */
+		default: /* Failure */
+			__set_bit(TXDONE_FAILURE, &txdesc.flags);
+		}
 		txdesc.retry = rt2x00_get_field32(word, TXD_W0_RETRY_COUNT);
 
-		rt2x00pci_txdone(rt2x00dev, entry, &txdesc);
+		rt2x00lib_txdone(entry, &txdesc);
 	}
 }
 
@@ -1354,19 +1401,19 @@ static irqreturn_t rt2500pci_interrupt(int irq, void *dev_instance)
 	 * 3 - Atim ring transmit done interrupt.
 	 */
 	if (rt2x00_get_field32(reg, CSR7_TXDONE_ATIMRING))
-		rt2500pci_txdone(rt2x00dev, RT2X00_BCN_QUEUE_ATIM);
+		rt2500pci_txdone(rt2x00dev, QID_ATIM);
 
 	/*
 	 * 4 - Priority ring transmit done interrupt.
 	 */
 	if (rt2x00_get_field32(reg, CSR7_TXDONE_PRIORING))
-		rt2500pci_txdone(rt2x00dev, IEEE80211_TX_QUEUE_DATA0);
+		rt2500pci_txdone(rt2x00dev, QID_AC_BE);
 
 	/*
 	 * 5 - Tx ring transmit done interrupt.
 	 */
 	if (rt2x00_get_field32(reg, CSR7_TXDONE_TXRING))
-		rt2500pci_txdone(rt2x00dev, IEEE80211_TX_QUEUE_DATA1);
+		rt2500pci_txdone(rt2x00dev, QID_AC_BK);
 
 	return IRQ_HANDLED;
 }
@@ -1486,23 +1533,10 @@ static int rt2500pci_init_eeprom(struct rt2x00_dev *rt2x00dev)
 #ifdef CONFIG_RT2500PCI_LEDS
 	value = rt2x00_get_field16(eeprom, EEPROM_ANTENNA_LED_MODE);
 
-	rt2x00dev->led_radio.rt2x00dev = rt2x00dev;
-	rt2x00dev->led_radio.type = LED_TYPE_RADIO;
-	rt2x00dev->led_radio.led_dev.brightness_set =
-	    rt2500pci_brightness_set;
-	rt2x00dev->led_radio.led_dev.blink_set =
-	    rt2500pci_blink_set;
-	rt2x00dev->led_radio.flags = LED_INITIALIZED;
-
-	if (value == LED_MODE_TXRX_ACTIVITY) {
-		rt2x00dev->led_qual.rt2x00dev = rt2x00dev;
-		rt2x00dev->led_qual.type = LED_TYPE_ACTIVITY;
-		rt2x00dev->led_qual.led_dev.brightness_set =
-		    rt2500pci_brightness_set;
-		rt2x00dev->led_qual.led_dev.blink_set =
-		    rt2500pci_blink_set;
-		rt2x00dev->led_qual.flags = LED_INITIALIZED;
-	}
+	rt2500pci_init_led(rt2x00dev, &rt2x00dev->led_radio, LED_TYPE_RADIO);
+	if (value == LED_MODE_TXRX_ACTIVITY)
+		rt2500pci_init_led(rt2x00dev, &rt2x00dev->led_qual,
+				   LED_TYPE_ACTIVITY);
 #endif /* CONFIG_RT2500PCI_LEDS */
 
 	/*
@@ -1695,13 +1729,12 @@ static void rt2500pci_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Initialize all hw fields.
 	 */
-	rt2x00dev->hw->flags = IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING;
+	rt2x00dev->hw->flags = IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING |
+			       IEEE80211_HW_SIGNAL_DBM;
+
 	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_DEV(rt2x00dev->hw, rt2x00dev->dev);
 	SET_IEEE80211_PERM_ADDR(rt2x00dev->hw,
 				rt2x00_eeprom_addr(rt2x00dev,
 						   EEPROM_MAC_ADDR_0));
@@ -1765,9 +1798,10 @@ static int rt2500pci_probe_hw(struct rt2x00_dev *rt2x00dev)
 	rt2500pci_probe_hw_mode(rt2x00dev);
 
 	/*
-	 * This device requires the atim queue
+	 * This device requires the atim queue and DMA-mapped skbs.
 	 */
 	__set_bit(DRIVER_REQUIRE_ATIM_QUEUE, &rt2x00dev->flags);
+	__set_bit(DRIVER_REQUIRE_DMA, &rt2x00dev->flags);
 
 	/*
 	 * Set the rssi offset.
@@ -1808,61 +1842,6 @@ static u64 rt2500pci_get_tsf(struct ieee80211_hw *hw)
 	return tsf;
 }
 
-static int rt2500pci_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb,
-				   struct ieee80211_tx_control *control)
-{
-	struct rt2x00_dev *rt2x00dev = hw->priv;
-	struct rt2x00_intf *intf = vif_to_intf(control->vif);
-	struct queue_entry_priv_pci_tx *priv_tx;
-	struct skb_frame_desc *skbdesc;
-	u32 reg;
-
-	if (unlikely(!intf->beacon))
-		return -ENOBUFS;
-
-	priv_tx = intf->beacon->priv_data;
-
-	/*
-	 * Fill in skb descriptor
-	 */
-	skbdesc = get_skb_frame_desc(skb);
-	memset(skbdesc, 0, sizeof(*skbdesc));
-	skbdesc->flags |= FRAME_DESC_DRIVER_GENERATED;
-	skbdesc->data = skb->data;
-	skbdesc->data_len = skb->len;
-	skbdesc->desc = priv_tx->desc;
-	skbdesc->desc_len = intf->beacon->queue->desc_size;
-	skbdesc->entry = intf->beacon;
-
-	/*
-	 * Disable beaconing while we are reloading the beacon data,
-	 * otherwise we might be sending out invalid data.
-	 */
-	rt2x00pci_register_read(rt2x00dev, CSR14, &reg);
-	rt2x00_set_field32(&reg, CSR14_TSF_COUNT, 0);
-	rt2x00_set_field32(&reg, CSR14_TBCN, 0);
-	rt2x00_set_field32(&reg, CSR14_BEACON_GEN, 0);
-	rt2x00pci_register_write(rt2x00dev, CSR14, reg);
-
-	/*
-	 * mac80211 doesn't provide the control->queue variable
-	 * for beacons. Set our own queue identification so
-	 * it can be used during descriptor initialization.
-	 */
-	control->queue = RT2X00_BCN_QUEUE_BEACON;
-	rt2x00lib_write_tx_desc(rt2x00dev, skb, control);
-
-	/*
-	 * Enable beacon generation.
-	 * Write entire beacon with descriptor to register,
-	 * and kick the beacon generator.
-	 */
-	memcpy(priv_tx->data, skb->data, skb->len);
-	rt2x00dev->ops->lib->kick_tx_queue(rt2x00dev, control->queue);
-
-	return 0;
-}
-
 static int rt2500pci_tx_last_beacon(struct ieee80211_hw *hw)
 {
 	struct rt2x00_dev *rt2x00dev = hw->priv;
@@ -1887,7 +1866,6 @@ static const struct ieee80211_ops rt2500pci_mac80211_ops = {
 	.conf_tx		= rt2x00mac_conf_tx,
 	.get_tx_stats		= rt2x00mac_get_tx_stats,
 	.get_tsf		= rt2500pci_get_tsf,
-	.beacon_update		= rt2500pci_beacon_update,
 	.tx_last_beacon		= rt2500pci_tx_last_beacon,
 };
 
@@ -1905,6 +1883,7 @@ static const struct rt2x00lib_ops rt2500pci_rt2x00_ops = {
 	.link_tuner		= rt2500pci_link_tuner,
 	.write_tx_desc		= rt2500pci_write_tx_desc,
 	.write_tx_data		= rt2x00pci_write_tx_data,
+	.write_beacon		= rt2500pci_write_beacon,
 	.kick_tx_queue		= rt2500pci_kick_tx_queue,
 	.fill_rxdone		= rt2500pci_fill_rxdone,
 	.config_filter		= rt2500pci_config_filter,
@@ -1917,28 +1896,28 @@ static const struct data_queue_desc rt2500pci_queue_rx = {
 	.entry_num		= RX_ENTRIES,
 	.data_size		= DATA_FRAME_SIZE,
 	.desc_size		= RXD_DESC_SIZE,
-	.priv_size		= sizeof(struct queue_entry_priv_pci_rx),
+	.priv_size		= sizeof(struct queue_entry_priv_pci),
 };
 
 static const struct data_queue_desc rt2500pci_queue_tx = {
 	.entry_num		= TX_ENTRIES,
 	.data_size		= DATA_FRAME_SIZE,
 	.desc_size		= TXD_DESC_SIZE,
-	.priv_size		= sizeof(struct queue_entry_priv_pci_tx),
+	.priv_size		= sizeof(struct queue_entry_priv_pci),
 };
 
 static const struct data_queue_desc rt2500pci_queue_bcn = {
 	.entry_num		= BEACON_ENTRIES,
 	.data_size		= MGMT_FRAME_SIZE,
 	.desc_size		= TXD_DESC_SIZE,
-	.priv_size		= sizeof(struct queue_entry_priv_pci_tx),
+	.priv_size		= sizeof(struct queue_entry_priv_pci),
 };
 
 static const struct data_queue_desc rt2500pci_queue_atim = {
 	.entry_num		= ATIM_ENTRIES,
 	.data_size		= DATA_FRAME_SIZE,
 	.desc_size		= TXD_DESC_SIZE,
-	.priv_size		= sizeof(struct queue_entry_priv_pci_tx),
+	.priv_size		= sizeof(struct queue_entry_priv_pci),
 };
 
 static const struct rt2x00_ops rt2500pci_ops = {
@@ -1947,6 +1926,7 @@ static const struct rt2x00_ops rt2500pci_ops = {
 	.max_ap_intf	= 1,
 	.eeprom_size	= EEPROM_SIZE,
 	.rf_size	= RF_SIZE,
+	.tx_queues	= NUM_TX_QUEUES,
 	.rx		= &rt2500pci_queue_rx,
 	.tx		= &rt2500pci_queue_tx,
 	.bcn		= &rt2500pci_queue_bcn,
diff --git a/drivers/net/wireless/rt2x00/rt2500pci.h b/drivers/net/wireless/rt2x00/rt2500pci.h
index 13899550465a..42f376929ea9 100644
--- a/drivers/net/wireless/rt2x00/rt2500pci.h
+++ b/drivers/net/wireless/rt2x00/rt2500pci.h
@@ -48,8 +48,6 @@
  * Signal information.
  * Defaul offset is required for RSSI <-> dBm conversion.
  */
-#define MAX_SIGNAL			100
-#define MAX_RX_SSI			-1
 #define DEFAULT_RSSI_OFFSET		121
 
 /*
@@ -63,6 +61,11 @@
 #define RF_SIZE				0x0014
 
 /*
+ * Number of TX queues.
+ */
+#define NUM_TX_QUEUES			2
+
+/*
  * Control/Status Registers(CSR).
  * Some values are set in TU, whereas 1 TU == 1024 us.
  */
@@ -748,7 +751,7 @@
 #define LEDCSR_LED_DEFAULT		FIELD32(0x00100000)
 
 /*
- * AES control register.
+ * SECCSR3: AES control register.
  */
 #define SECCSR3				0x00fc
 
@@ -892,7 +895,7 @@
 #define ARTCSR2_ACK_CTS_54MBS		FIELD32(0xff000000)
 
 /*
- * SECCSR1_RT2509: WEP control register.
+ * SECCSR1: 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.
diff --git a/drivers/net/wireless/rt2x00/rt2500usb.c b/drivers/net/wireless/rt2x00/rt2500usb.c
index d90512f97b39..3558cb210747 100644
--- a/drivers/net/wireless/rt2x00/rt2500usb.c
+++ b/drivers/net/wireless/rt2x00/rt2500usb.c
@@ -76,10 +76,10 @@ static inline void rt2500usb_register_multiread(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);
+				      value, length,
+				      REGISTER_TIMEOUT16(length));
 }
 
 static inline void rt2500usb_register_write(struct rt2x00_dev *rt2x00dev,
@@ -106,10 +106,10 @@ static inline void rt2500usb_register_multiwrite(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);
+				      value, length,
+				      REGISTER_TIMEOUT16(length));
 }
 
 static u16 rt2500usb_bbp_check(struct rt2x00_dev *rt2x00dev)
@@ -322,6 +322,17 @@ static int rt2500usb_blink_set(struct led_classdev *led_cdev,
 
 	return 0;
 }
+
+static void rt2500usb_init_led(struct rt2x00_dev *rt2x00dev,
+			       struct rt2x00_led *led,
+			       enum led_type type)
+{
+	led->rt2x00dev = rt2x00dev;
+	led->type = type;
+	led->led_dev.brightness_set = rt2500usb_brightness_set;
+	led->led_dev.blink_set = rt2500usb_blink_set;
+	led->flags = LED_INITIALIZED;
+}
 #endif /* CONFIG_RT2500USB_LEDS */
 
 /*
@@ -860,25 +871,32 @@ static int rt2500usb_init_registers(struct rt2x00_dev *rt2x00dev)
 	return 0;
 }
 
-static int rt2500usb_init_bbp(struct rt2x00_dev *rt2x00dev)
+static int rt2500usb_wait_bbp_ready(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");
+			return 0;
 		udelay(REGISTER_BUSY_DELAY);
 	}
 
 	ERROR(rt2x00dev, "BBP register access failed, aborting.\n");
 	return -EACCES;
+}
+
+static int rt2500usb_init_bbp(struct rt2x00_dev *rt2x00dev)
+{
+	unsigned int i;
+	u16 eeprom;
+	u8 value;
+	u8 reg_id;
+
+	if (unlikely(rt2500usb_wait_bbp_ready(rt2x00dev)))
+		return -EACCES;
 
-continue_csr_init:
 	rt2500usb_bbp_write(rt2x00dev, 3, 0x02);
 	rt2500usb_bbp_write(rt2x00dev, 4, 0x19);
 	rt2500usb_bbp_write(rt2x00dev, 14, 0x1c);
@@ -934,7 +952,8 @@ static void rt2500usb_toggle_rx(struct rt2x00_dev *rt2x00dev,
 
 	rt2500usb_register_read(rt2x00dev, TXRX_CSR2, &reg);
 	rt2x00_set_field16(&reg, TXRX_CSR2_DISABLE_RX,
-			   state == STATE_RADIO_RX_OFF);
+			   (state == STATE_RADIO_RX_OFF) ||
+			   (state == STATE_RADIO_RX_OFF_LINK));
 	rt2500usb_register_write(rt2x00dev, TXRX_CSR2, reg);
 }
 
@@ -943,11 +962,9 @@ 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");
+	if (unlikely(rt2500usb_init_registers(rt2x00dev) ||
+		     rt2500usb_init_bbp(rt2x00dev)))
 		return -EIO;
-	}
 
 	return 0;
 }
@@ -1000,10 +1017,6 @@ static int rt2500usb_set_state(struct rt2x00_dev *rt2x00dev,
 		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;
 }
 
@@ -1021,11 +1034,13 @@ static int rt2500usb_set_device_state(struct rt2x00_dev *rt2x00dev,
 		break;
 	case STATE_RADIO_RX_ON:
 	case STATE_RADIO_RX_ON_LINK:
-		rt2500usb_toggle_rx(rt2x00dev, STATE_RADIO_RX_ON);
-		break;
 	case STATE_RADIO_RX_OFF:
 	case STATE_RADIO_RX_OFF_LINK:
-		rt2500usb_toggle_rx(rt2x00dev, STATE_RADIO_RX_OFF);
+		rt2500usb_toggle_rx(rt2x00dev, state);
+		break;
+	case STATE_RADIO_IRQ_ON:
+	case STATE_RADIO_IRQ_OFF:
+		/* No support, but no error either */
 		break;
 	case STATE_DEEP_SLEEP:
 	case STATE_SLEEP:
@@ -1038,6 +1053,10 @@ static int rt2500usb_set_device_state(struct rt2x00_dev *rt2x00dev,
 		break;
 	}
 
+	if (unlikely(retval))
+		ERROR(rt2x00dev, "Device failed to enter state %d (%d).\n",
+		      state, retval);
+
 	return retval;
 }
 
@@ -1046,8 +1065,7 @@ static int rt2500usb_set_device_state(struct rt2x00_dev *rt2x00dev,
  */
 static void rt2500usb_write_tx_desc(struct rt2x00_dev *rt2x00dev,
 				    struct sk_buff *skb,
-				    struct txentry_desc *txdesc,
-				    struct ieee80211_tx_control *control)
+				    struct txentry_desc *txdesc)
 {
 	struct skb_frame_desc *skbdesc = get_skb_frame_desc(skb);
 	__le32 *txd = skbdesc->desc;
@@ -1071,7 +1089,7 @@ static void rt2500usb_write_tx_desc(struct rt2x00_dev *rt2x00dev,
 	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_RETRY_LIMIT, txdesc->retry_limit);
 	rt2x00_set_field32(&word, TXD_W0_MORE_FRAG,
 			   test_bit(ENTRY_TXD_MORE_FRAG, &txdesc->flags));
 	rt2x00_set_field32(&word, TXD_W0_ACK,
@@ -1081,13 +1099,73 @@ static void rt2500usb_write_tx_desc(struct rt2x00_dev *rt2x00dev,
 	rt2x00_set_field32(&word, TXD_W0_OFDM,
 			   test_bit(ENTRY_TXD_OFDM_RATE, &txdesc->flags));
 	rt2x00_set_field32(&word, TXD_W0_NEW_SEQ,
-			   !!(control->flags & IEEE80211_TXCTL_FIRST_FRAGMENT));
+			   test_bit(ENTRY_TXD_FIRST_FRAGMENT, &txdesc->flags));
 	rt2x00_set_field32(&word, TXD_W0_IFS, txdesc->ifs);
-	rt2x00_set_field32(&word, TXD_W0_DATABYTE_COUNT, skbdesc->data_len);
+	rt2x00_set_field32(&word, TXD_W0_DATABYTE_COUNT,
+			   skb->len - skbdesc->desc_len);
 	rt2x00_set_field32(&word, TXD_W0_CIPHER, CIPHER_NONE);
 	rt2x00_desc_write(txd, 0, word);
 }
 
+/*
+ * TX data initialization
+ */
+static void rt2500usb_beacondone(struct urb *urb);
+
+static void rt2500usb_write_beacon(struct queue_entry *entry)
+{
+	struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
+	struct usb_device *usb_dev = to_usb_device_intf(rt2x00dev->dev);
+	struct queue_entry_priv_usb_bcn *bcn_priv = entry->priv_data;
+	struct skb_frame_desc *skbdesc = get_skb_frame_desc(entry->skb);
+	int pipe = usb_sndbulkpipe(usb_dev, 1);
+	int length;
+	u16 reg;
+
+	/*
+	 * Add the descriptor in front of the skb.
+	 */
+	skb_push(entry->skb, entry->queue->desc_size);
+	memcpy(entry->skb->data, skbdesc->desc, skbdesc->desc_len);
+	skbdesc->desc = entry->skb->data;
+
+	/*
+	 * Disable beaconing while we are reloading the beacon data,
+	 * otherwise we might be sending out invalid data.
+	 */
+	rt2500usb_register_read(rt2x00dev, TXRX_CSR19, &reg);
+	rt2x00_set_field16(&reg, TXRX_CSR19_TSF_COUNT, 0);
+	rt2x00_set_field16(&reg, TXRX_CSR19_TBCN, 0);
+	rt2x00_set_field16(&reg, TXRX_CSR19_BEACON_GEN, 0);
+	rt2500usb_register_write(rt2x00dev, TXRX_CSR19, reg);
+
+	/*
+	 * 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, entry->skb);
+
+	usb_fill_bulk_urb(bcn_priv->urb, usb_dev, pipe,
+			  entry->skb->data, length, rt2500usb_beacondone,
+			  entry);
+
+	/*
+	 * 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.
+	 */
+	bcn_priv->guardian_data = 0;
+	usb_fill_bulk_urb(bcn_priv->guardian_urb, usb_dev, pipe,
+			  &bcn_priv->guardian_data, 1, rt2500usb_beacondone,
+			  entry);
+
+	/*
+	 * Send out the guardian byte.
+	 */
+	usb_submit_urb(bcn_priv->guardian_urb, GFP_ATOMIC);
+}
+
 static int rt2500usb_get_tx_data_len(struct rt2x00_dev *rt2x00dev,
 				     struct sk_buff *skb)
 {
@@ -1103,16 +1181,15 @@ static int rt2500usb_get_tx_data_len(struct rt2x00_dev *rt2x00dev,
 	return length;
 }
 
-/*
- * TX data initialization
- */
 static void rt2500usb_kick_tx_queue(struct rt2x00_dev *rt2x00dev,
-				    const unsigned int queue)
+				    const enum data_queue_qid queue)
 {
 	u16 reg;
 
-	if (queue != RT2X00_BCN_QUEUE_BEACON)
+	if (queue != QID_BEACON) {
+		rt2x00usb_kick_tx_queue(rt2x00dev, queue);
 		return;
+	}
 
 	rt2500usb_register_read(rt2x00dev, TXRX_CSR19, &reg);
 	if (!rt2x00_get_field16(reg, TXRX_CSR19_BEACON_GEN)) {
@@ -1138,30 +1215,28 @@ static void rt2500usb_kick_tx_queue(struct rt2x00_dev *rt2x00dev,
 static void rt2500usb_fill_rxdone(struct queue_entry *entry,
 				  struct rxdone_entry_desc *rxdesc)
 {
-	struct queue_entry_priv_usb_rx *priv_rx = entry->priv_data;
+	struct queue_entry_priv_usb *entry_priv = entry->priv_data;
 	struct skb_frame_desc *skbdesc = get_skb_frame_desc(entry->skb);
 	__le32 *rxd =
 	    (__le32 *)(entry->skb->data +
-		       (priv_rx->urb->actual_length - entry->queue->desc_size));
-	unsigned int offset = entry->queue->desc_size + 2;
+		       (entry_priv->urb->actual_length -
+			entry->queue->desc_size));
 	u32 word0;
 	u32 word1;
 
 	/*
-	 * Copy descriptor to the available headroom inside the skbuffer.
+	 * Copy descriptor to the skbdesc->desc buffer, making it safe from moving of
+	 * frame data in rt2x00usb.
 	 */
-	skb_push(entry->skb, offset);
-	memcpy(entry->skb->data, rxd, entry->queue->desc_size);
-	rxd = (__le32 *)entry->skb->data;
+	memcpy(skbdesc->desc, rxd, skbdesc->desc_len);
+	rxd = (__le32 *)skbdesc->desc;
 
 	/*
-	 * The descriptor is now aligned to 4 bytes and thus it is
-	 * now safe to read it on all architectures.
+	 * It is now safe to read the descriptor on all architectures.
 	 */
 	rt2x00_desc_read(rxd, 0, &word0);
 	rt2x00_desc_read(rxd, 1, &word1);
 
-	rxdesc->flags = 0;
 	if (rt2x00_get_field32(word0, RXD_W0_CRC_ERROR))
 		rxdesc->flags |= RX_FLAG_FAILED_FCS_CRC;
 	if (rt2x00_get_field32(word0, RXD_W0_PHYSICAL_ERROR))
@@ -1178,7 +1253,6 @@ static void rt2500usb_fill_rxdone(struct queue_entry *entry,
 	    entry->queue->rt2x00dev->rssi_offset;
 	rxdesc->size = rt2x00_get_field32(word0, RXD_W0_DATABYTE_COUNT);
 
-	rxdesc->dev_flags = 0;
 	if (rt2x00_get_field32(word0, RXD_W0_OFDM))
 		rxdesc->dev_flags |= RXDONE_SIGNAL_PLCP;
 	if (rt2x00_get_field32(word0, RXD_W0_MY_BSS))
@@ -1187,16 +1261,7 @@ static void rt2500usb_fill_rxdone(struct queue_entry *entry,
 	/*
 	 * Adjust the skb memory window to the frame boundaries.
 	 */
-	skb_pull(entry->skb, offset);
 	skb_trim(entry->skb, rxdesc->size);
-
-	/*
-	 * Set descriptor and data pointer.
-	 */
-	skbdesc->data = entry->skb->data;
-	skbdesc->data_len = rxdesc->size;
-	skbdesc->desc = rxd;
-	skbdesc->desc_len = entry->queue->desc_size;
 }
 
 /*
@@ -1205,7 +1270,7 @@ static void rt2500usb_fill_rxdone(struct queue_entry *entry,
 static void rt2500usb_beacondone(struct urb *urb)
 {
 	struct queue_entry *entry = (struct queue_entry *)urb->context;
-	struct queue_entry_priv_usb_bcn *priv_bcn = entry->priv_data;
+	struct queue_entry_priv_usb_bcn *bcn_priv = entry->priv_data;
 
 	if (!test_bit(DEVICE_ENABLED_RADIO, &entry->queue->rt2x00dev->flags))
 		return;
@@ -1216,9 +1281,9 @@ static void rt2500usb_beacondone(struct urb *urb)
 	 * Otherwise we should free the sk_buffer, the device
 	 * should be doing the rest of the work now.
 	 */
-	if (priv_bcn->guardian_urb == urb) {
-		usb_submit_urb(priv_bcn->urb, GFP_ATOMIC);
-	} else if (priv_bcn->urb == urb) {
+	if (bcn_priv->guardian_urb == urb) {
+		usb_submit_urb(bcn_priv->urb, GFP_ATOMIC);
+	} else if (bcn_priv->urb == urb) {
 		dev_kfree_skb(entry->skb);
 		entry->skb = NULL;
 	}
@@ -1397,23 +1462,10 @@ static int rt2500usb_init_eeprom(struct rt2x00_dev *rt2x00dev)
 #ifdef CONFIG_RT2500USB_LEDS
 	value = rt2x00_get_field16(eeprom, EEPROM_ANTENNA_LED_MODE);
 
-	rt2x00dev->led_radio.rt2x00dev = rt2x00dev;
-	rt2x00dev->led_radio.type = LED_TYPE_RADIO;
-	rt2x00dev->led_radio.led_dev.brightness_set =
-	    rt2500usb_brightness_set;
-	rt2x00dev->led_radio.led_dev.blink_set =
-	    rt2500usb_blink_set;
-	rt2x00dev->led_radio.flags = LED_INITIALIZED;
-
-	if (value == LED_MODE_TXRX_ACTIVITY) {
-		rt2x00dev->led_qual.rt2x00dev = rt2x00dev;
-		rt2x00dev->led_qual.type = LED_TYPE_ACTIVITY;
-		rt2x00dev->led_qual.led_dev.brightness_set =
-		    rt2500usb_brightness_set;
-		rt2x00dev->led_qual.led_dev.blink_set =
-		    rt2500usb_blink_set;
-		rt2x00dev->led_qual.flags = LED_INITIALIZED;
-	}
+	rt2500usb_init_led(rt2x00dev, &rt2x00dev->led_radio, LED_TYPE_RADIO);
+	if (value == LED_MODE_TXRX_ACTIVITY)
+		rt2500usb_init_led(rt2x00dev, &rt2x00dev->led_qual,
+				   LED_TYPE_ACTIVITY);
 #endif /* CONFIG_RT2500USB_LEDS */
 
 	/*
@@ -1600,13 +1652,12 @@ static void rt2500usb_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
 	rt2x00dev->hw->flags =
 	    IEEE80211_HW_HOST_GEN_BEACON_TEMPLATE |
 	    IEEE80211_HW_RX_INCLUDES_FCS |
-	    IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING;
+	    IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING |
+	    IEEE80211_HW_SIGNAL_DBM;
+
 	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_DEV(rt2x00dev->hw, rt2x00dev->dev);
 	SET_IEEE80211_PERM_ADDR(rt2x00dev->hw,
 				rt2x00_eeprom_addr(rt2x00dev,
 						   EEPROM_MAC_ADDR_0));
@@ -1684,97 +1735,6 @@ static int rt2500usb_probe_hw(struct rt2x00_dev *rt2x00dev)
 	return 0;
 }
 
-/*
- * IEEE80211 stack callback functions.
- */
-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 = rt2x00dev_usb_dev(rt2x00dev);
-	struct rt2x00_intf *intf = vif_to_intf(control->vif);
-	struct queue_entry_priv_usb_bcn *priv_bcn;
-	struct skb_frame_desc *skbdesc;
-	int pipe = usb_sndbulkpipe(usb_dev, 1);
-	int length;
-	u16 reg;
-
-	if (unlikely(!intf->beacon))
-		return -ENOBUFS;
-
-	priv_bcn = intf->beacon->priv_data;
-
-	/*
-	 * Add the descriptor in front of the skb.
-	 */
-	skb_push(skb, intf->beacon->queue->desc_size);
-	memset(skb->data, 0, intf->beacon->queue->desc_size);
-
-	/*
-	 * Fill in skb descriptor
-	 */
-	skbdesc = get_skb_frame_desc(skb);
-	memset(skbdesc, 0, sizeof(*skbdesc));
-	skbdesc->flags |= FRAME_DESC_DRIVER_GENERATED;
-	skbdesc->data = skb->data + intf->beacon->queue->desc_size;
-	skbdesc->data_len = skb->len - intf->beacon->queue->desc_size;
-	skbdesc->desc = skb->data;
-	skbdesc->desc_len = intf->beacon->queue->desc_size;
-	skbdesc->entry = intf->beacon;
-
-	/*
-	 * Disable beaconing while we are reloading the beacon data,
-	 * otherwise we might be sending out invalid data.
-	 */
-	rt2500usb_register_read(rt2x00dev, TXRX_CSR19, &reg);
-	rt2x00_set_field16(&reg, TXRX_CSR19_TSF_COUNT, 0);
-	rt2x00_set_field16(&reg, TXRX_CSR19_TBCN, 0);
-	rt2x00_set_field16(&reg, TXRX_CSR19_BEACON_GEN, 0);
-	rt2500usb_register_write(rt2x00dev, TXRX_CSR19, reg);
-
-	/*
-	 * mac80211 doesn't provide the control->queue variable
-	 * for beacons. Set our own queue identification so
-	 * it can be used during descriptor initialization.
-	 */
-	control->queue = RT2X00_BCN_QUEUE_BEACON;
-	rt2x00lib_write_tx_desc(rt2x00dev, skb, control);
-
-	/*
-	 * 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 = rt2500usb_get_tx_data_len(rt2x00dev, skb);
-
-	usb_fill_bulk_urb(priv_bcn->urb, usb_dev, pipe,
-			  skb->data, length, rt2500usb_beacondone,
-			  intf->beacon);
-
-	/*
-	 * 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.
-	 */
-	priv_bcn->guardian_data = 0;
-	usb_fill_bulk_urb(priv_bcn->guardian_urb, usb_dev, pipe,
-			  &priv_bcn->guardian_data, 1, rt2500usb_beacondone,
-			  intf->beacon);
-
-	/*
-	 * Send out the guardian byte.
-	 */
-	usb_submit_urb(priv_bcn->guardian_urb, GFP_ATOMIC);
-
-	/*
-	 * Enable beacon generation.
-	 */
-	rt2500usb_kick_tx_queue(rt2x00dev, control->queue);
-
-	return 0;
-}
-
 static const struct ieee80211_ops rt2500usb_mac80211_ops = {
 	.tx			= rt2x00mac_tx,
 	.start			= rt2x00mac_start,
@@ -1788,7 +1748,6 @@ static const struct ieee80211_ops rt2500usb_mac80211_ops = {
 	.bss_info_changed	= rt2x00mac_bss_info_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 = {
@@ -1803,6 +1762,7 @@ static const struct rt2x00lib_ops rt2500usb_rt2x00_ops = {
 	.link_tuner		= rt2500usb_link_tuner,
 	.write_tx_desc		= rt2500usb_write_tx_desc,
 	.write_tx_data		= rt2x00usb_write_tx_data,
+	.write_beacon		= rt2500usb_write_beacon,
 	.get_tx_data_len	= rt2500usb_get_tx_data_len,
 	.kick_tx_queue		= rt2500usb_kick_tx_queue,
 	.fill_rxdone		= rt2500usb_fill_rxdone,
@@ -1816,14 +1776,14 @@ static const struct data_queue_desc rt2500usb_queue_rx = {
 	.entry_num		= RX_ENTRIES,
 	.data_size		= DATA_FRAME_SIZE,
 	.desc_size		= RXD_DESC_SIZE,
-	.priv_size		= sizeof(struct queue_entry_priv_usb_rx),
+	.priv_size		= sizeof(struct queue_entry_priv_usb),
 };
 
 static const struct data_queue_desc rt2500usb_queue_tx = {
 	.entry_num		= TX_ENTRIES,
 	.data_size		= DATA_FRAME_SIZE,
 	.desc_size		= TXD_DESC_SIZE,
-	.priv_size		= sizeof(struct queue_entry_priv_usb_tx),
+	.priv_size		= sizeof(struct queue_entry_priv_usb),
 };
 
 static const struct data_queue_desc rt2500usb_queue_bcn = {
@@ -1837,7 +1797,7 @@ static const struct data_queue_desc rt2500usb_queue_atim = {
 	.entry_num		= ATIM_ENTRIES,
 	.data_size		= DATA_FRAME_SIZE,
 	.desc_size		= TXD_DESC_SIZE,
-	.priv_size		= sizeof(struct queue_entry_priv_usb_tx),
+	.priv_size		= sizeof(struct queue_entry_priv_usb),
 };
 
 static const struct rt2x00_ops rt2500usb_ops = {
@@ -1846,6 +1806,7 @@ static const struct rt2x00_ops rt2500usb_ops = {
 	.max_ap_intf	= 1,
 	.eeprom_size	= EEPROM_SIZE,
 	.rf_size	= RF_SIZE,
+	.tx_queues	= NUM_TX_QUEUES,
 	.rx		= &rt2500usb_queue_rx,
 	.tx		= &rt2500usb_queue_tx,
 	.bcn		= &rt2500usb_queue_bcn,
diff --git a/drivers/net/wireless/rt2x00/rt2500usb.h b/drivers/net/wireless/rt2x00/rt2500usb.h
index a37a068d0c71..4769ffeb4cc6 100644
--- a/drivers/net/wireless/rt2x00/rt2500usb.h
+++ b/drivers/net/wireless/rt2x00/rt2500usb.h
@@ -48,8 +48,6 @@
  * Signal information.
  * Defaul offset is required for RSSI <-> dBm conversion.
  */
-#define MAX_SIGNAL			100
-#define MAX_RX_SSI			-1
 #define DEFAULT_RSSI_OFFSET		120
 
 /*
@@ -63,6 +61,11 @@
 #define RF_SIZE				0x0014
 
 /*
+ * Number of TX queues.
+ */
+#define NUM_TX_QUEUES			2
+
+/*
  * Control/Status Registers(CSR).
  * Some values are set in TU, whereas 1 TU == 1024 us.
  */
@@ -206,7 +209,7 @@
 #define MAC_CSR21_OFF_PERIOD		FIELD16(0xff00)
 
 /*
- * Collision window control register.
+ * MAC_CSR22: Collision window control register.
  */
 #define MAC_CSR22			0x042c
 
@@ -293,7 +296,7 @@
 #define TXRX_CSR7_BBP_ID1_VALID		FIELD16(0x8000)
 
 /*
- * TXRX_CSR5: OFDM TX BBP ID1.
+ * TXRX_CSR8: OFDM TX BBP ID1.
  */
 #define TXRX_CSR8			0x0450
 #define TXRX_CSR8_BBP_ID0		FIELD16(0x007f)
@@ -367,7 +370,14 @@
  */
 
 /*
- * SEC_CSR0-SEC_CSR7: Shared key 0, word 0-7
+ * SEC_CSR0: Shared key 0, word 0
+ * SEC_CSR1: Shared key 0, word 1
+ * SEC_CSR2: Shared key 0, word 2
+ * SEC_CSR3: Shared key 0, word 3
+ * SEC_CSR4: Shared key 0, word 4
+ * SEC_CSR5: Shared key 0, word 5
+ * SEC_CSR6: Shared key 0, word 6
+ * SEC_CSR7: Shared key 0, word 7
  */
 #define SEC_CSR0			0x0480
 #define SEC_CSR1			0x0482
@@ -379,7 +389,14 @@
 #define SEC_CSR7			0x048e
 
 /*
- * SEC_CSR8-SEC_CSR15: Shared key 1, word 0-7
+ * SEC_CSR8: Shared key 1, word 0
+ * SEC_CSR9: Shared key 1, word 1
+ * SEC_CSR10: Shared key 1, word 2
+ * SEC_CSR11: Shared key 1, word 3
+ * SEC_CSR12: Shared key 1, word 4
+ * SEC_CSR13: Shared key 1, word 5
+ * SEC_CSR14: Shared key 1, word 6
+ * SEC_CSR15: Shared key 1, word 7
  */
 #define SEC_CSR8			0x0490
 #define SEC_CSR9			0x0492
@@ -391,7 +408,14 @@
 #define SEC_CSR15			0x049e
 
 /*
- * SEC_CSR16-SEC_CSR23: Shared key 2, word 0-7
+ * SEC_CSR16: Shared key 2, word 0
+ * SEC_CSR17: Shared key 2, word 1
+ * SEC_CSR18: Shared key 2, word 2
+ * SEC_CSR19: Shared key 2, word 3
+ * SEC_CSR20: Shared key 2, word 4
+ * SEC_CSR21: Shared key 2, word 5
+ * SEC_CSR22: Shared key 2, word 6
+ * SEC_CSR23: Shared key 2, word 7
  */
 #define SEC_CSR16			0x04a0
 #define SEC_CSR17			0x04a2
@@ -403,7 +427,14 @@
 #define SEC_CSR23			0x04ae
 
 /*
- * SEC_CSR24-SEC_CSR31: Shared key 3, word 0-7
+ * SEC_CSR24: Shared key 3, word 0
+ * SEC_CSR25: Shared key 3, word 1
+ * SEC_CSR26: Shared key 3, word 2
+ * SEC_CSR27: Shared key 3, word 3
+ * SEC_CSR28: Shared key 3, word 4
+ * SEC_CSR29: Shared key 3, word 5
+ * SEC_CSR30: Shared key 3, word 6
+ * SEC_CSR31: Shared key 3, word 7
  */
 #define SEC_CSR24			0x04b0
 #define SEC_CSR25			0x04b2
diff --git a/drivers/net/wireless/rt2x00/rt2x00.h b/drivers/net/wireless/rt2x00/rt2x00.h
index b4bf1e09cf9a..07b03b3c7ef1 100644
--- a/drivers/net/wireless/rt2x00/rt2x00.h
+++ b/drivers/net/wireless/rt2x00/rt2x00.h
@@ -44,7 +44,7 @@
 /*
  * Module information.
  */
-#define DRV_VERSION	"2.1.4"
+#define DRV_VERSION	"2.1.8"
 #define DRV_PROJECT	"http://rt2x00.serialmonkey.com"
 
 /*
@@ -111,33 +111,6 @@
 #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));
-}
-
-static inline int is_beacon(u16 fc)
-{
-	return (((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT) &&
-		((fc & IEEE80211_FCTL_STYPE) == IEEE80211_STYPE_BEACON));
-}
-
-/*
  * Chipset identification
  * The chipset on the device is composed of a RT and RF chip.
  * The chipset combination is important for determining device capabilities.
@@ -391,6 +364,8 @@ struct rt2x00_intf {
 #define DELAYED_UPDATE_BEACON		0x00000001
 #define DELAYED_CONFIG_ERP		0x00000002
 #define DELAYED_LED_ASSOC		0x00000004
+
+	u16 seqno;
 };
 
 static inline struct rt2x00_intf* vif_to_intf(struct ieee80211_vif *vif)
@@ -409,7 +384,7 @@ static inline struct rt2x00_intf* vif_to_intf(struct ieee80211_vif *vif)
  * @supported_rates: Rate types which are supported (CCK, OFDM).
  * @num_channels: Number of supported channels. This is used as array size
  *	for @tx_power_a, @tx_power_bg and @channels.
- * channels: Device/chipset specific channel values (See &struct rf_channel).
+ * @channels: Device/chipset specific channel values (See &struct rf_channel).
  * @tx_power_a: TX power values for all 5.2GHz channels (may be NULL).
  * @tx_power_bg: TX power values for all 2.4GHz channels (may be NULL).
  * @tx_power_default: Default TX power value to use when either
@@ -461,6 +436,7 @@ struct rt2x00lib_conf {
  */
 struct rt2x00lib_erp {
 	int short_preamble;
+	int cts_protection;
 
 	int ack_timeout;
 	int ack_consume_time;
@@ -511,8 +487,8 @@ struct rt2x00lib_ops {
 	 */
 	int (*probe_hw) (struct rt2x00_dev *rt2x00dev);
 	char *(*get_firmware_name) (struct rt2x00_dev *rt2x00dev);
-	u16 (*get_firmware_crc) (void *data, const size_t len);
-	int (*load_firmware) (struct rt2x00_dev *rt2x00dev, void *data,
+	u16 (*get_firmware_crc) (const void *data, const size_t len);
+	int (*load_firmware) (struct rt2x00_dev *rt2x00dev, const void *data,
 			      const size_t len);
 
 	/*
@@ -545,15 +521,13 @@ struct rt2x00lib_ops {
 	 */
 	void (*write_tx_desc) (struct rt2x00_dev *rt2x00dev,
 			       struct sk_buff *skb,
-			       struct txentry_desc *txdesc,
-			       struct ieee80211_tx_control *control);
-	int (*write_tx_data) (struct rt2x00_dev *rt2x00dev,
-			      struct data_queue *queue, struct sk_buff *skb,
-			      struct ieee80211_tx_control *control);
+			       struct txentry_desc *txdesc);
+	int (*write_tx_data) (struct queue_entry *entry);
+	void (*write_beacon) (struct queue_entry *entry);
 	int (*get_tx_data_len) (struct rt2x00_dev *rt2x00dev,
 				struct sk_buff *skb);
 	void (*kick_tx_queue) (struct rt2x00_dev *rt2x00dev,
-			       const unsigned int queue);
+			       const enum data_queue_qid queue);
 
 	/*
 	 * RX control handlers
@@ -597,6 +571,7 @@ struct rt2x00_ops {
 	const unsigned int max_ap_intf;
 	const unsigned int eeprom_size;
 	const unsigned int rf_size;
+	const unsigned int tx_queues;
 	const struct data_queue_desc *rx;
 	const struct data_queue_desc *tx;
 	const struct data_queue_desc *bcn;
@@ -626,11 +601,11 @@ enum rt2x00_flags {
 	/*
 	 * Driver features
 	 */
-	DRIVER_SUPPORT_MIXED_INTERFACES,
 	DRIVER_REQUIRE_FIRMWARE,
 	DRIVER_REQUIRE_BEACON_GUARD,
 	DRIVER_REQUIRE_ATIM_QUEUE,
 	DRIVER_REQUIRE_SCHEDULED,
+	DRIVER_REQUIRE_DMA,
 
 	/*
 	 * Driver configuration
@@ -655,11 +630,7 @@ struct rt2x00_dev {
 	 * 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 )
-#define rt2x00dev_usb_dev(__dev)\
-	( (struct usb_device *)interface_to_usbdev(rt2x00dev_usb(__dev)) )
+	struct device *dev;
 
 	/*
 	 * Callback functions.
@@ -682,7 +653,7 @@ struct rt2x00_dev {
 #define RFKILL_STATE_ALLOCATED		1
 #define RFKILL_STATE_REGISTERED		2
 	struct rfkill *rfkill;
-	struct input_polled_dev *poll_dev;
+	struct delayed_work rfkill_work;
 #endif /* CONFIG_RT2X00_LIB_RFKILL */
 
 	/*
@@ -820,8 +791,10 @@ struct rt2x00_dev {
 
 	/*
 	 * Scheduled work.
+	 * NOTE: intf_work will use ieee80211_iterate_active_interfaces()
+	 * which means it cannot be placed on the hw->workqueue
+	 * due to RTNL locking requirements.
 	 */
-	struct workqueue_struct *workqueue;
 	struct work_struct intf_work;
 	struct work_struct filter_work;
 
@@ -830,7 +803,7 @@ struct rt2x00_dev {
 	 * The Beacon array also contains the Atim queue
 	 * if that is supported by the device.
 	 */
-	int data_queues;
+	unsigned int data_queues;
 	struct data_queue *rx;
 	struct data_queue *tx;
 	struct data_queue *bcn;
@@ -934,55 +907,41 @@ static inline u16 get_duration_res(const unsigned int size, const u8 rate)
 }
 
 /**
- * rt2x00queue_get_queue - Convert mac80211 queue index to rt2x00 queue
+ * rt2x00queue_map_txskb - Map a skb into DMA for TX purposes.
  * @rt2x00dev: Pointer to &struct rt2x00_dev.
- * @queue: mac80211/rt2x00 queue index
- *	(see &enum ieee80211_tx_queue and &enum rt2x00_bcn_queue).
+ * @skb: The skb to map.
+ */
+void rt2x00queue_map_txskb(struct rt2x00_dev *rt2x00dev, struct sk_buff *skb);
+
+/**
+ * rt2x00queue_get_queue - Convert queue index to queue pointer
+ * @rt2x00dev: Pointer to &struct rt2x00_dev.
+ * @queue: rt2x00 queue index (see &enum data_queue_qid).
  */
 struct data_queue *rt2x00queue_get_queue(struct rt2x00_dev *rt2x00dev,
-					 const unsigned int queue);
+					 const enum data_queue_qid queue);
 
 /**
  * rt2x00queue_get_entry - Get queue entry where the given index points to.
- * @rt2x00dev: Pointer to &struct rt2x00_dev.
+ * @queue: Pointer to &struct data_queue from where we obtain the entry.
  * @index: Index identifier for obtaining the correct index.
  */
 struct queue_entry *rt2x00queue_get_entry(struct data_queue *queue,
 					  enum queue_index index);
 
-/**
- * rt2x00queue_index_inc - Index incrementation function
- * @queue: Queue (&struct data_queue) to perform the action on.
- * @action: Index type (&enum queue_index) to perform the action on.
- *
- * This function will increase the requested index on the queue,
- * it will grab the appropriate locks and handle queue overflow events by
- * resetting the index to the start of the queue.
- */
-void rt2x00queue_index_inc(struct data_queue *queue, enum queue_index index);
-
-
 /*
  * Interrupt context handlers.
  */
 void rt2x00lib_beacondone(struct rt2x00_dev *rt2x00dev);
 void rt2x00lib_txdone(struct queue_entry *entry,
 		      struct txdone_entry_desc *txdesc);
-void rt2x00lib_rxdone(struct queue_entry *entry,
-		      struct rxdone_entry_desc *rxdesc);
-
-/*
- * TX descriptor initializer
- */
-void rt2x00lib_write_tx_desc(struct rt2x00_dev *rt2x00dev,
-			     struct sk_buff *skb,
-			     struct ieee80211_tx_control *control);
+void rt2x00lib_rxdone(struct rt2x00_dev *rt2x00dev,
+		      struct queue_entry *entry);
 
 /*
  * mac80211 handlers.
  */
-int rt2x00mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb,
-		 struct ieee80211_tx_control *control);
+int rt2x00mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb);
 int rt2x00mac_start(struct ieee80211_hw *hw);
 void rt2x00mac_stop(struct ieee80211_hw *hw);
 int rt2x00mac_add_interface(struct ieee80211_hw *hw,
@@ -1005,7 +964,7 @@ void rt2x00mac_bss_info_changed(struct ieee80211_hw *hw,
 				struct ieee80211_vif *vif,
 				struct ieee80211_bss_conf *bss_conf,
 				u32 changes);
-int rt2x00mac_conf_tx(struct ieee80211_hw *hw, int queue,
+int rt2x00mac_conf_tx(struct ieee80211_hw *hw, u16 queue,
 		      const struct ieee80211_tx_queue_params *params);
 
 /*
diff --git a/drivers/net/wireless/rt2x00/rt2x00config.c b/drivers/net/wireless/rt2x00/rt2x00config.c
index 48608e8cc8b4..f20ca712504f 100644
--- a/drivers/net/wireless/rt2x00/rt2x00config.c
+++ b/drivers/net/wireless/rt2x00/rt2x00config.c
@@ -84,6 +84,8 @@ void rt2x00lib_config_erp(struct rt2x00_dev *rt2x00dev,
 	memset(&erp, 0, sizeof(erp));
 
 	erp.short_preamble = bss_conf->use_short_preamble;
+	erp.cts_protection = bss_conf->use_cts_prot;
+
 	erp.ack_timeout = PLCP + get_duration(ACK_SIZE, 10);
 	erp.ack_consume_time = SIFS + PLCP + get_duration(ACK_SIZE, 10);
 
diff --git a/drivers/net/wireless/rt2x00/rt2x00debug.c b/drivers/net/wireless/rt2x00/rt2x00debug.c
index bfab3b8780d6..300cf061035f 100644
--- a/drivers/net/wireless/rt2x00/rt2x00debug.c
+++ b/drivers/net/wireless/rt2x00/rt2x00debug.c
@@ -115,7 +115,7 @@ struct rt2x00debug_intf {
 };
 
 void rt2x00debug_dump_frame(struct rt2x00_dev *rt2x00dev,
-			    struct sk_buff *skb)
+			    enum rt2x00_dump_type type, struct sk_buff *skb)
 {
 	struct rt2x00debug_intf *intf = rt2x00dev->debugfs_intf;
 	struct skb_frame_desc *desc = get_skb_frame_desc(skb);
@@ -133,7 +133,7 @@ void rt2x00debug_dump_frame(struct rt2x00_dev *rt2x00dev,
 		return;
 	}
 
-	skbcopy = alloc_skb(sizeof(*dump_hdr) + desc->desc_len + desc->data_len,
+	skbcopy = alloc_skb(sizeof(*dump_hdr) + desc->desc_len + skb->len,
 			    GFP_ATOMIC);
 	if (!skbcopy) {
 		DEBUG(rt2x00dev, "Failed to copy skb for dump.\n");
@@ -144,18 +144,18 @@ void rt2x00debug_dump_frame(struct rt2x00_dev *rt2x00dev,
 	dump_hdr->version = cpu_to_le32(DUMP_HEADER_VERSION);
 	dump_hdr->header_length = cpu_to_le32(sizeof(*dump_hdr));
 	dump_hdr->desc_length = cpu_to_le32(desc->desc_len);
-	dump_hdr->data_length = cpu_to_le32(desc->data_len);
+	dump_hdr->data_length = cpu_to_le32(skb->len);
 	dump_hdr->chip_rt = cpu_to_le16(rt2x00dev->chip.rt);
 	dump_hdr->chip_rf = cpu_to_le16(rt2x00dev->chip.rf);
 	dump_hdr->chip_rev = cpu_to_le32(rt2x00dev->chip.rev);
-	dump_hdr->type = cpu_to_le16(desc->frame_type);
+	dump_hdr->type = cpu_to_le16(type);
 	dump_hdr->queue_index = desc->entry->queue->qid;
 	dump_hdr->entry_index = desc->entry->entry_idx;
 	dump_hdr->timestamp_sec = cpu_to_le32(timestamp.tv_sec);
 	dump_hdr->timestamp_usec = cpu_to_le32(timestamp.tv_usec);
 
 	memcpy(skb_put(skbcopy, desc->desc_len), desc->desc, desc->desc_len);
-	memcpy(skb_put(skbcopy, desc->data_len), desc->data, desc->data_len);
+	memcpy(skb_put(skbcopy, skb->len), skb->data, skb->len);
 
 	skb_queue_tail(&intf->frame_dump_skbqueue, skbcopy);
 	wake_up_interruptible(&intf->frame_dump_waitqueue);
diff --git a/drivers/net/wireless/rt2x00/rt2x00dev.c b/drivers/net/wireless/rt2x00/rt2x00dev.c
index c997d4f28ab3..8c93eb8353b0 100644
--- a/drivers/net/wireless/rt2x00/rt2x00dev.c
+++ b/drivers/net/wireless/rt2x00/rt2x00dev.c
@@ -28,7 +28,6 @@
 
 #include "rt2x00.h"
 #include "rt2x00lib.h"
-#include "rt2x00dump.h"
 
 /*
  * Link tuning handlers
@@ -75,7 +74,7 @@ static void rt2x00lib_start_link_tuner(struct rt2x00_dev *rt2x00dev)
 
 	rt2x00lib_reset_link_tuner(rt2x00dev);
 
-	queue_delayed_work(rt2x00dev->workqueue,
+	queue_delayed_work(rt2x00dev->hw->workqueue,
 			   &rt2x00dev->link.work, LINK_TUNE_INTERVAL);
 }
 
@@ -113,6 +112,8 @@ int rt2x00lib_enable_radio(struct rt2x00_dev *rt2x00dev)
 	if (status)
 		return status;
 
+	rt2x00dev->ops->lib->set_device_state(rt2x00dev, STATE_RADIO_IRQ_ON);
+
 	rt2x00leds_led_radio(rt2x00dev, true);
 	rt2x00led_led_activity(rt2x00dev, true);
 
@@ -126,7 +127,7 @@ int rt2x00lib_enable_radio(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Start the TX queues.
 	 */
-	ieee80211_start_queues(rt2x00dev->hw);
+	ieee80211_wake_queues(rt2x00dev->hw);
 
 	return 0;
 }
@@ -150,6 +151,7 @@ void rt2x00lib_disable_radio(struct rt2x00_dev *rt2x00dev)
 	 * Disable radio.
 	 */
 	rt2x00dev->ops->lib->set_device_state(rt2x00dev, STATE_RADIO_OFF);
+	rt2x00dev->ops->lib->set_device_state(rt2x00dev, STATE_RADIO_IRQ_OFF);
 	rt2x00led_led_activity(rt2x00dev, false);
 	rt2x00leds_led_radio(rt2x00dev, false);
 }
@@ -390,7 +392,7 @@ static void rt2x00lib_link_tuner(struct work_struct *work)
 	 * Increase tuner counter, and reschedule the next link tuner run.
 	 */
 	rt2x00dev->link.count++;
-	queue_delayed_work(rt2x00dev->workqueue,
+	queue_delayed_work(rt2x00dev->hw->workqueue,
 			   &rt2x00dev->link.work, LINK_TUNE_INTERVAL);
 }
 
@@ -407,8 +409,6 @@ static void rt2x00lib_intf_scheduled_iter(void *data, u8 *mac,
 {
 	struct rt2x00_dev *rt2x00dev = data;
 	struct rt2x00_intf *intf = vif_to_intf(vif);
-	struct sk_buff *skb;
-	struct ieee80211_tx_control control;
 	struct ieee80211_bss_conf conf;
 	int delayed_flags;
 
@@ -434,12 +434,8 @@ static void rt2x00lib_intf_scheduled_iter(void *data, u8 *mac,
 	if (!test_bit(DEVICE_ENABLED_RADIO, &rt2x00dev->flags))
 		return;
 
-	if (delayed_flags & DELAYED_UPDATE_BEACON) {
-		skb = ieee80211_beacon_get(rt2x00dev->hw, vif, &control);
-		if (skb && rt2x00dev->ops->hw->beacon_update(rt2x00dev->hw,
-							     skb, &control))
-			dev_kfree_skb(skb);
-	}
+	if (delayed_flags & DELAYED_UPDATE_BEACON)
+		rt2x00queue_update_beacon(rt2x00dev, vif);
 
 	if (delayed_flags & DELAYED_CONFIG_ERP)
 		rt2x00lib_config_erp(rt2x00dev, intf, &conf);
@@ -468,12 +464,19 @@ static void rt2x00lib_intf_scheduled(struct work_struct *work)
 static void rt2x00lib_beacondone_iter(void *data, u8 *mac,
 				      struct ieee80211_vif *vif)
 {
+	struct rt2x00_dev *rt2x00dev = data;
 	struct rt2x00_intf *intf = vif_to_intf(vif);
 
 	if (vif->type != IEEE80211_IF_TYPE_AP &&
 	    vif->type != IEEE80211_IF_TYPE_IBSS)
 		return;
 
+	/*
+	 * Clean up the beacon skb.
+	 */
+	rt2x00queue_free_skb(rt2x00dev, intf->beacon->skb);
+	intf->beacon->skb = NULL;
+
 	spin_lock(&intf->lock);
 	intf->delayed_flags |= DELAYED_UPDATE_BEACON;
 	spin_unlock(&intf->lock);
@@ -488,7 +491,7 @@ void rt2x00lib_beacondone(struct rt2x00_dev *rt2x00dev)
 						   rt2x00lib_beacondone_iter,
 						   rt2x00dev);
 
-	queue_work(rt2x00dev->workqueue, &rt2x00dev->intf_work);
+	schedule_work(&rt2x00dev->intf_work);
 }
 EXPORT_SYMBOL_GPL(rt2x00lib_beacondone);
 
@@ -496,79 +499,132 @@ void rt2x00lib_txdone(struct queue_entry *entry,
 		      struct txdone_entry_desc *txdesc)
 {
 	struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
-	struct skb_frame_desc *skbdesc;
-	struct ieee80211_tx_status tx_status;
-	int success = !!(txdesc->status == TX_SUCCESS ||
-			 txdesc->status == TX_SUCCESS_RETRY);
-	int fail = !!(txdesc->status == TX_FAIL_RETRY ||
-		      txdesc->status == TX_FAIL_INVALID ||
-		      txdesc->status == TX_FAIL_OTHER);
+	struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(entry->skb);
+	enum data_queue_qid qid = skb_get_queue_mapping(entry->skb);
+
+	/*
+	 * Unmap the skb.
+	 */
+	rt2x00queue_unmap_skb(rt2x00dev, entry->skb);
+
+	/*
+	 * Send frame to debugfs immediately, after this call is completed
+	 * we are going to overwrite the skb->cb array.
+	 */
+	rt2x00debug_dump_frame(rt2x00dev, DUMP_FRAME_TXDONE, entry->skb);
 
 	/*
 	 * Update TX statistics.
 	 */
-	rt2x00dev->link.qual.tx_success += success;
-	rt2x00dev->link.qual.tx_failed += fail;
+	rt2x00dev->link.qual.tx_success +=
+	    test_bit(TXDONE_SUCCESS, &txdesc->flags);
+	rt2x00dev->link.qual.tx_failed +=
+	    test_bit(TXDONE_FAILURE, &txdesc->flags);
 
 	/*
 	 * Initialize TX status
 	 */
-	tx_status.flags = 0;
-	tx_status.ack_signal = 0;
-	tx_status.excessive_retries = (txdesc->status == TX_FAIL_RETRY);
-	tx_status.retry_count = txdesc->retry;
-	memcpy(&tx_status.control, txdesc->control, sizeof(*txdesc->control));
+	memset(&tx_info->status, 0, sizeof(tx_info->status));
+	tx_info->status.ack_signal = 0;
+	tx_info->status.excessive_retries =
+	    test_bit(TXDONE_EXCESSIVE_RETRY, &txdesc->flags);
+	tx_info->status.retry_count = txdesc->retry;
 
-	if (!(tx_status.control.flags & IEEE80211_TXCTL_NO_ACK)) {
-		if (success)
-			tx_status.flags |= IEEE80211_TX_STATUS_ACK;
-		else
+	if (!(tx_info->flags & IEEE80211_TX_CTL_NO_ACK)) {
+		if (test_bit(TXDONE_SUCCESS, &txdesc->flags))
+			tx_info->flags |= IEEE80211_TX_STAT_ACK;
+		else if (test_bit(TXDONE_FAILURE, &txdesc->flags))
 			rt2x00dev->low_level_stats.dot11ACKFailureCount++;
 	}
 
-	tx_status.queue_length = entry->queue->limit;
-	tx_status.queue_number = tx_status.control.queue;
-
-	if (tx_status.control.flags & IEEE80211_TXCTL_USE_RTS_CTS) {
-		if (success)
+	if (tx_info->flags & IEEE80211_TX_CTL_USE_RTS_CTS) {
+		if (test_bit(TXDONE_SUCCESS, &txdesc->flags))
 			rt2x00dev->low_level_stats.dot11RTSSuccessCount++;
-		else
+		else if (test_bit(TXDONE_FAILURE, &txdesc->flags))
 			rt2x00dev->low_level_stats.dot11RTSFailureCount++;
 	}
 
 	/*
-	 * Send the tx_status to debugfs. Only send the status report
-	 * to mac80211 when the frame originated from there. If this was
-	 * a extra frame coming through a mac80211 library call (RTS/CTS)
-	 * then we should not send the status report back.
-	 * If send to mac80211, mac80211 will clean up the skb structure,
-	 * otherwise we have to do it ourself.
+	 * Only send the status report to mac80211 when TX status was
+	 * requested by it. If this was a extra frame coming through
+	 * a mac80211 library call (RTS/CTS) then we should not send the
+	 * status report back.
 	 */
-	skbdesc = get_skb_frame_desc(entry->skb);
-	skbdesc->frame_type = DUMP_FRAME_TXDONE;
-
-	rt2x00debug_dump_frame(rt2x00dev, entry->skb);
-
-	if (!(skbdesc->flags & FRAME_DESC_DRIVER_GENERATED))
-		ieee80211_tx_status_irqsafe(rt2x00dev->hw,
-					    entry->skb, &tx_status);
+	if (tx_info->flags & IEEE80211_TX_CTL_REQ_TX_STATUS)
+		ieee80211_tx_status_irqsafe(rt2x00dev->hw, entry->skb);
 	else
-		dev_kfree_skb(entry->skb);
+		dev_kfree_skb_irq(entry->skb);
+
+	/*
+	 * Make this entry available for reuse.
+	 */
 	entry->skb = NULL;
+	entry->flags = 0;
+
+	rt2x00dev->ops->lib->init_txentry(rt2x00dev, entry);
+
+	__clear_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags);
+	rt2x00queue_index_inc(entry->queue, Q_INDEX_DONE);
+
+	/*
+	 * If the data queue was below the threshold before the txdone
+	 * handler we must make sure the packet queue in the mac80211 stack
+	 * is reenabled when the txdone handler has finished.
+	 */
+	if (!rt2x00queue_threshold(entry->queue))
+		ieee80211_wake_queue(rt2x00dev->hw, qid);
 }
 EXPORT_SYMBOL_GPL(rt2x00lib_txdone);
 
-void rt2x00lib_rxdone(struct queue_entry *entry,
-		      struct rxdone_entry_desc *rxdesc)
+void rt2x00lib_rxdone(struct rt2x00_dev *rt2x00dev,
+		      struct queue_entry *entry)
 {
-	struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
+	struct rxdone_entry_desc rxdesc;
+	struct sk_buff *skb;
 	struct ieee80211_rx_status *rx_status = &rt2x00dev->rx_status;
 	struct ieee80211_supported_band *sband;
 	struct ieee80211_hdr *hdr;
 	const struct rt2x00_rate *rate;
+	unsigned int header_size;
+	unsigned int align;
 	unsigned int i;
 	int idx = -1;
-	u16 fc;
+
+	/*
+	 * Allocate a new sk_buffer. If no new buffer available, drop the
+	 * received frame and reuse the existing buffer.
+	 */
+	skb = rt2x00queue_alloc_rxskb(rt2x00dev, entry);
+	if (!skb)
+		return;
+
+	/*
+	 * Unmap the skb.
+	 */
+	rt2x00queue_unmap_skb(rt2x00dev, entry->skb);
+
+	/*
+	 * Extract the RXD details.
+	 */
+	memset(&rxdesc, 0, sizeof(rxdesc));
+	rt2x00dev->ops->lib->fill_rxdone(entry, &rxdesc);
+
+	/*
+	 * The data behind the ieee80211 header must be
+	 * aligned on a 4 byte boundary.
+	 */
+	header_size = ieee80211_get_hdrlen_from_skb(entry->skb);
+	align = ((unsigned long)(entry->skb->data + header_size)) & 3;
+
+	if (align) {
+		skb_push(entry->skb, align);
+		/* Move entire frame in 1 command */
+		memmove(entry->skb->data, entry->skb->data + align,
+			rxdesc.size);
+	}
+
+	/* Update data pointers, trim buffer to correct size */
+	skb_trim(entry->skb, rxdesc.size);
 
 	/*
 	 * Update RX statistics.
@@ -577,10 +633,10 @@ void rt2x00lib_rxdone(struct queue_entry *entry,
 	for (i = 0; i < sband->n_bitrates; i++) {
 		rate = rt2x00_get_rate(sband->bitrates[i].hw_value);
 
-		if (((rxdesc->dev_flags & RXDONE_SIGNAL_PLCP) &&
-		     (rate->plcp == rxdesc->signal)) ||
-		    (!(rxdesc->dev_flags & RXDONE_SIGNAL_PLCP) &&
-		      (rate->bitrate == rxdesc->signal))) {
+		if (((rxdesc.dev_flags & RXDONE_SIGNAL_PLCP) &&
+		     (rate->plcp == rxdesc.signal)) ||
+		    (!(rxdesc.dev_flags & RXDONE_SIGNAL_PLCP) &&
+		      (rate->bitrate == rxdesc.signal))) {
 			idx = i;
 			break;
 		}
@@ -588,8 +644,8 @@ void rt2x00lib_rxdone(struct queue_entry *entry,
 
 	if (idx < 0) {
 		WARNING(rt2x00dev, "Frame received with unrecognized signal,"
-			"signal=0x%.2x, plcp=%d.\n", rxdesc->signal,
-			!!(rxdesc->dev_flags & RXDONE_SIGNAL_PLCP));
+			"signal=0x%.2x, plcp=%d.\n", rxdesc.signal,
+			!!(rxdesc.dev_flags & RXDONE_SIGNAL_PLCP));
 		idx = 0;
 	}
 
@@ -597,170 +653,38 @@ void rt2x00lib_rxdone(struct queue_entry *entry,
 	 * Only update link status if this is a beacon frame carrying our bssid.
 	 */
 	hdr = (struct ieee80211_hdr *)entry->skb->data;
-	fc = le16_to_cpu(hdr->frame_control);
-	if (is_beacon(fc) && (rxdesc->dev_flags & RXDONE_MY_BSS))
-		rt2x00lib_update_link_stats(&rt2x00dev->link, rxdesc->rssi);
+	if (ieee80211_is_beacon(hdr->frame_control) &&
+	    (rxdesc.dev_flags & RXDONE_MY_BSS))
+		rt2x00lib_update_link_stats(&rt2x00dev->link, rxdesc.rssi);
 
 	rt2x00dev->link.qual.rx_success++;
 
+	rx_status->mactime = rxdesc.timestamp;
 	rx_status->rate_idx = idx;
-	rx_status->signal =
-	    rt2x00lib_calculate_link_signal(rt2x00dev, rxdesc->rssi);
-	rx_status->ssi = rxdesc->rssi;
-	rx_status->flag = rxdesc->flags;
+	rx_status->qual =
+	    rt2x00lib_calculate_link_signal(rt2x00dev, rxdesc.rssi);
+	rx_status->signal = rxdesc.rssi;
+	rx_status->flag = rxdesc.flags;
 	rx_status->antenna = rt2x00dev->link.ant.active.rx;
 
 	/*
 	 * Send frame to mac80211 & debugfs.
 	 * mac80211 will clean up the skb structure.
 	 */
-	get_skb_frame_desc(entry->skb)->frame_type = DUMP_FRAME_RXDONE;
-	rt2x00debug_dump_frame(rt2x00dev, entry->skb);
+	rt2x00debug_dump_frame(rt2x00dev, DUMP_FRAME_RXDONE, entry->skb);
 	ieee80211_rx_irqsafe(rt2x00dev->hw, entry->skb, rx_status);
-	entry->skb = NULL;
-}
-EXPORT_SYMBOL_GPL(rt2x00lib_rxdone);
-
-/*
- * TX descriptor initializer
- */
-void rt2x00lib_write_tx_desc(struct rt2x00_dev *rt2x00dev,
-			     struct sk_buff *skb,
-			     struct ieee80211_tx_control *control)
-{
-	struct txentry_desc txdesc;
-	struct skb_frame_desc *skbdesc = get_skb_frame_desc(skb);
-	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skbdesc->data;
-	const struct rt2x00_rate *rate;
-	int tx_rate;
-	int length;
-	int duration;
-	int residual;
-	u16 frame_control;
-	u16 seq_ctrl;
-
-	memset(&txdesc, 0, sizeof(txdesc));
-
-	txdesc.queue = skbdesc->entry->queue->qid;
-	txdesc.cw_min = skbdesc->entry->queue->cw_min;
-	txdesc.cw_max = skbdesc->entry->queue->cw_max;
-	txdesc.aifs = skbdesc->entry->queue->aifs;
-
-	/*
-	 * Read required fields from ieee80211 header.
-	 */
-	frame_control = le16_to_cpu(hdr->frame_control);
-	seq_ctrl = le16_to_cpu(hdr->seq_ctrl);
-
-	tx_rate = control->tx_rate->hw_value;
 
 	/*
-	 * Check whether this frame is to be acked
+	 * Replace the skb with the freshly allocated one.
 	 */
-	if (!(control->flags & IEEE80211_TXCTL_NO_ACK))
-		__set_bit(ENTRY_TXD_ACK, &txdesc.flags);
-
-	/*
-	 * Check if this is a RTS/CTS frame
-	 */
-	if (is_rts_frame(frame_control) || is_cts_frame(frame_control)) {
-		__set_bit(ENTRY_TXD_BURST, &txdesc.flags);
-		if (is_rts_frame(frame_control)) {
-			__set_bit(ENTRY_TXD_RTS_FRAME, &txdesc.flags);
-			__set_bit(ENTRY_TXD_ACK, &txdesc.flags);
-		} else
-			__clear_bit(ENTRY_TXD_ACK, &txdesc.flags);
-		if (control->rts_cts_rate)
-			tx_rate = control->rts_cts_rate->hw_value;
-	}
-
-	rate = rt2x00_get_rate(tx_rate);
-
-	/*
-	 * Check if more fragments are pending
-	 */
-	if (ieee80211_get_morefrag(hdr)) {
-		__set_bit(ENTRY_TXD_BURST, &txdesc.flags);
-		__set_bit(ENTRY_TXD_MORE_FRAG, &txdesc.flags);
-	}
-
-	/*
-	 * Beacons and probe responses require the tsf timestamp
-	 * to be inserted into the frame.
-	 */
-	if (control->queue == RT2X00_BCN_QUEUE_BEACON ||
-	    is_probe_resp(frame_control))
-		__set_bit(ENTRY_TXD_REQ_TIMESTAMP, &txdesc.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, &txdesc.flags))
-		txdesc.ifs = IFS_SIFS;
-	else
-		txdesc.ifs = IFS_BACKOFF;
-
-	/*
-	 * PLCP setup
-	 * Length calculation depends on OFDM/CCK rate.
-	 */
-	txdesc.signal = rate->plcp;
-	txdesc.service = 0x04;
-
-	length = skbdesc->data_len + FCS_LEN;
-	if (rate->flags & DEV_RATE_OFDM) {
-		__set_bit(ENTRY_TXD_OFDM_RATE, &txdesc.flags);
-
-		txdesc.length_high = (length >> 6) & 0x3f;
-		txdesc.length_low = length & 0x3f;
-	} else {
-		/*
-		 * Convert length to microseconds.
-		 */
-		residual = get_duration_res(length, rate->bitrate);
-		duration = get_duration(length, rate->bitrate);
-
-		if (residual != 0) {
-			duration++;
-
-			/*
-			 * Check if we need to set the Length Extension
-			 */
-			if (rate->bitrate == 110 && residual <= 30)
-				txdesc.service |= 0x80;
-		}
-
-		txdesc.length_high = (duration >> 8) & 0xff;
-		txdesc.length_low = duration & 0xff;
-
-		/*
-		 * When preamble is enabled we should set the
-		 * preamble bit for the signal.
-		 */
-		if (rt2x00_get_rate_preamble(tx_rate))
-			txdesc.signal |= 0x08;
-	}
-
-	rt2x00dev->ops->lib->write_tx_desc(rt2x00dev, skb, &txdesc, control);
+	entry->skb = skb;
+	entry->flags = 0;
 
-	/*
-	 * Update queue entry.
-	 */
-	skbdesc->entry->skb = skb;
+	rt2x00dev->ops->lib->init_rxentry(rt2x00dev, entry);
 
-	/*
-	 * The frame has been completely initialized and ready
-	 * for sending to the device. The caller will push the
-	 * frame to the device, but we are going to push the
-	 * frame to debugfs here.
-	 */
-	skbdesc->frame_type = DUMP_FRAME_TX;
-	rt2x00debug_dump_frame(rt2x00dev, skb);
+	rt2x00queue_index_inc(entry->queue, Q_INDEX);
 }
-EXPORT_SYMBOL_GPL(rt2x00lib_write_tx_desc);
+EXPORT_SYMBOL_GPL(rt2x00lib_rxdone);
 
 /*
  * Driver initialization handlers.
@@ -978,6 +902,11 @@ static int rt2x00lib_probe_hw(struct rt2x00_dev *rt2x00dev)
 		return status;
 
 	/*
+	 * Initialize HW fields.
+	 */
+	rt2x00dev->hw->queues = rt2x00dev->ops->tx_queues;
+
+	/*
 	 * Register HW.
 	 */
 	status = ieee80211_register_hw(rt2x00dev->hw);
@@ -1131,10 +1060,6 @@ int rt2x00lib_probe_dev(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Initialize configuration work.
 	 */
-	rt2x00dev->workqueue = create_singlethread_workqueue("rt2x00lib");
-	if (!rt2x00dev->workqueue)
-		goto exit;
-
 	INIT_WORK(&rt2x00dev->intf_work, rt2x00lib_intf_scheduled);
 	INIT_WORK(&rt2x00dev->filter_work, rt2x00lib_packetfilter_scheduled);
 	INIT_DELAYED_WORK(&rt2x00dev->link.work, rt2x00lib_link_tuner);
@@ -1195,13 +1120,6 @@ void rt2x00lib_remove_dev(struct rt2x00_dev *rt2x00dev)
 	rt2x00leds_unregister(rt2x00dev);
 
 	/*
-	 * Stop all queued work. Note that most tasks will already be halted
-	 * during rt2x00lib_disable_radio() and rt2x00lib_uninitialize().
-	 */
-	flush_workqueue(rt2x00dev->workqueue);
-	destroy_workqueue(rt2x00dev->workqueue);
-
-	/*
 	 * Free ieee80211_hw memory.
 	 */
 	rt2x00lib_remove_hw(rt2x00dev);
@@ -1246,7 +1164,6 @@ int rt2x00lib_suspend(struct rt2x00_dev *rt2x00dev, pm_message_t state)
 	 * Suspend/disable extra components.
 	 */
 	rt2x00leds_suspend(rt2x00dev);
-	rt2x00rfkill_suspend(rt2x00dev);
 	rt2x00debug_deregister(rt2x00dev);
 
 exit:
@@ -1302,7 +1219,6 @@ int rt2x00lib_resume(struct rt2x00_dev *rt2x00dev)
 	 * Restore/enable extra components.
 	 */
 	rt2x00debug_register(rt2x00dev);
-	rt2x00rfkill_resume(rt2x00dev);
 	rt2x00leds_resume(rt2x00dev);
 
 	/*
@@ -1343,7 +1259,7 @@ int rt2x00lib_resume(struct rt2x00_dev *rt2x00dev)
 	 * In that case we have disabled the TX queue and should
 	 * now enable it again
 	 */
-	ieee80211_start_queues(rt2x00dev->hw);
+	ieee80211_wake_queues(rt2x00dev->hw);
 
 	/*
 	 * During interface iteration we might have changed the
diff --git a/drivers/net/wireless/rt2x00/rt2x00firmware.c b/drivers/net/wireless/rt2x00/rt2x00firmware.c
index b971bc6e7ee2..bab05a56e7a0 100644
--- a/drivers/net/wireless/rt2x00/rt2x00firmware.c
+++ b/drivers/net/wireless/rt2x00/rt2x00firmware.c
@@ -100,6 +100,14 @@ int rt2x00lib_load_firmware(struct rt2x00_dev *rt2x00dev)
 	retval = rt2x00dev->ops->lib->load_firmware(rt2x00dev,
 						    rt2x00dev->fw->data,
 						    rt2x00dev->fw->size);
+
+	/*
+	 * When the firmware is uploaded to the hardware the LED
+	 * association status might have been triggered, for correct
+	 * LED handling it should now be reset.
+	 */
+	rt2x00leds_led_assoc(rt2x00dev, false);
+
 	return retval;
 }
 
diff --git a/drivers/net/wireless/rt2x00/rt2x00lib.h b/drivers/net/wireless/rt2x00/rt2x00lib.h
index 41ee02cd2825..f2c9b0e79b5f 100644
--- a/drivers/net/wireless/rt2x00/rt2x00lib.h
+++ b/drivers/net/wireless/rt2x00/rt2x00lib.h
@@ -26,12 +26,14 @@
 #ifndef RT2X00LIB_H
 #define RT2X00LIB_H
 
+#include "rt2x00dump.h"
+
 /*
  * Interval defines
  * Both the link tuner as the rfkill will be called once per second.
  */
 #define LINK_TUNE_INTERVAL	( round_jiffies_relative(HZ) )
-#define RFKILL_POLL_INTERVAL	( 1000 )
+#define RFKILL_POLL_INTERVAL	( round_jiffies_relative(HZ) )
 
 /*
  * rt2x00_rate: Per rate device information
@@ -96,9 +98,65 @@ void rt2x00lib_config_antenna(struct rt2x00_dev *rt2x00dev,
 void rt2x00lib_config(struct rt2x00_dev *rt2x00dev,
 		      struct ieee80211_conf *conf, const int force_config);
 
-/*
- * Queue handlers.
+/**
+ * DOC: Queue handlers
+ */
+
+/**
+ * rt2x00queue_alloc_rxskb - allocate a skb for RX purposes.
+ * @rt2x00dev: Pointer to &struct rt2x00_dev.
+ * @queue: The queue for which the skb will be applicable.
+ */
+struct sk_buff *rt2x00queue_alloc_rxskb(struct rt2x00_dev *rt2x00dev,
+					struct queue_entry *entry);
+
+/**
+ * rt2x00queue_unmap_skb - Unmap a skb from DMA.
+ * @rt2x00dev: Pointer to &struct rt2x00_dev.
+ * @skb: The skb to unmap.
+ */
+void rt2x00queue_unmap_skb(struct rt2x00_dev *rt2x00dev, struct sk_buff *skb);
+
+/**
+ * rt2x00queue_free_skb - free a skb
+ * @rt2x00dev: Pointer to &struct rt2x00_dev.
+ * @skb: The skb to free.
+ */
+void rt2x00queue_free_skb(struct rt2x00_dev *rt2x00dev, struct sk_buff *skb);
+
+/**
+ * rt2x00queue_free_skb - free a skb
+ * @rt2x00dev: Pointer to &struct rt2x00_dev.
+ * @skb: The skb to free.
+ */
+void rt2x00queue_free_skb(struct rt2x00_dev *rt2x00dev, struct sk_buff *skb);
+
+/**
+ * rt2x00queue_write_tx_frame - Write TX frame to hardware
+ * @queue: Queue over which the frame should be send
+ * @skb: The skb to send
+ */
+int rt2x00queue_write_tx_frame(struct data_queue *queue, struct sk_buff *skb);
+
+/**
+ * rt2x00queue_update_beacon - Send new beacon from mac80211 to hardware
+ * @rt2x00dev: Pointer to &struct rt2x00_dev.
+ * @vif: Interface for which the beacon should be updated.
  */
+int rt2x00queue_update_beacon(struct rt2x00_dev *rt2x00dev,
+			      struct ieee80211_vif *vif);
+
+/**
+ * rt2x00queue_index_inc - Index incrementation function
+ * @queue: Queue (&struct data_queue) to perform the action on.
+ * @index: Index type (&enum queue_index) to perform the action on.
+ *
+ * This function will increase the requested index on the queue,
+ * it will grab the appropriate locks and handle queue overflow events by
+ * resetting the index to the start of the queue.
+ */
+void rt2x00queue_index_inc(struct data_queue *queue, enum queue_index index);
+
 void rt2x00queue_init_rx(struct rt2x00_dev *rt2x00dev);
 void rt2x00queue_init_tx(struct rt2x00_dev *rt2x00dev);
 int rt2x00queue_initialize(struct rt2x00_dev *rt2x00dev);
@@ -128,7 +186,8 @@ static inline void rt2x00lib_free_firmware(struct rt2x00_dev *rt2x00dev)
 #ifdef CONFIG_RT2X00_LIB_DEBUGFS
 void rt2x00debug_register(struct rt2x00_dev *rt2x00dev);
 void rt2x00debug_deregister(struct rt2x00_dev *rt2x00dev);
-void rt2x00debug_dump_frame(struct rt2x00_dev *rt2x00dev, struct sk_buff *skb);
+void rt2x00debug_dump_frame(struct rt2x00_dev *rt2x00dev,
+			    enum rt2x00_dump_type type, struct sk_buff *skb);
 #else
 static inline void rt2x00debug_register(struct rt2x00_dev *rt2x00dev)
 {
@@ -139,6 +198,7 @@ static inline void rt2x00debug_deregister(struct rt2x00_dev *rt2x00dev)
 }
 
 static inline void rt2x00debug_dump_frame(struct rt2x00_dev *rt2x00dev,
+					  enum rt2x00_dump_type type,
 					  struct sk_buff *skb)
 {
 }
@@ -152,8 +212,6 @@ void rt2x00rfkill_register(struct rt2x00_dev *rt2x00dev);
 void rt2x00rfkill_unregister(struct rt2x00_dev *rt2x00dev);
 void rt2x00rfkill_allocate(struct rt2x00_dev *rt2x00dev);
 void rt2x00rfkill_free(struct rt2x00_dev *rt2x00dev);
-void rt2x00rfkill_suspend(struct rt2x00_dev *rt2x00dev);
-void rt2x00rfkill_resume(struct rt2x00_dev *rt2x00dev);
 #else
 static inline void rt2x00rfkill_register(struct rt2x00_dev *rt2x00dev)
 {
@@ -170,14 +228,6 @@ static inline void rt2x00rfkill_allocate(struct rt2x00_dev *rt2x00dev)
 static inline void rt2x00rfkill_free(struct rt2x00_dev *rt2x00dev)
 {
 }
-
-static inline void rt2x00rfkill_suspend(struct rt2x00_dev *rt2x00dev)
-{
-}
-
-static inline void rt2x00rfkill_resume(struct rt2x00_dev *rt2x00dev)
-{
-}
 #endif /* CONFIG_RT2X00_LIB_RFKILL */
 
 /*
diff --git a/drivers/net/wireless/rt2x00/rt2x00mac.c b/drivers/net/wireless/rt2x00/rt2x00mac.c
index 9cb023edd2e9..f1dcbaa80c3c 100644
--- a/drivers/net/wireless/rt2x00/rt2x00mac.c
+++ b/drivers/net/wireless/rt2x00/rt2x00mac.c
@@ -31,14 +31,14 @@
 
 static int rt2x00mac_tx_rts_cts(struct rt2x00_dev *rt2x00dev,
 				struct data_queue *queue,
-				struct sk_buff *frag_skb,
-				struct ieee80211_tx_control *control)
+				struct sk_buff *frag_skb)
 {
-	struct skb_frame_desc *skbdesc;
+	struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(frag_skb);
+	struct ieee80211_tx_info *rts_info;
 	struct sk_buff *skb;
 	int size;
 
-	if (control->flags & IEEE80211_TXCTL_USE_CTS_PROTECT)
+	if (tx_info->flags & IEEE80211_TX_CTL_USE_CTS_PROTECT)
 		size = sizeof(struct ieee80211_cts);
 	else
 		size = sizeof(struct ieee80211_rts);
@@ -52,23 +52,37 @@ static int rt2x00mac_tx_rts_cts(struct rt2x00_dev *rt2x00dev,
 	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, control->vif,
-					frag_skb->data, frag_skb->len, control,
+	/*
+	 * Copy TX information over from original frame to
+	 * RTS/CTS frame. Note that we set the no encryption flag
+	 * since we don't want this frame to be encrypted.
+	 * RTS frames should be acked, while CTS-to-self frames
+	 * should not. The ready for TX flag is cleared to prevent
+	 * it being automatically send when the descriptor is
+	 * written to the hardware.
+	 */
+	memcpy(skb->cb, frag_skb->cb, sizeof(skb->cb));
+	rts_info = IEEE80211_SKB_CB(skb);
+	rts_info->flags |= IEEE80211_TX_CTL_DO_NOT_ENCRYPT;
+	rts_info->flags &= ~IEEE80211_TX_CTL_USE_RTS_CTS;
+	rts_info->flags &= ~IEEE80211_TX_CTL_USE_CTS_PROTECT;
+	rts_info->flags &= ~IEEE80211_TX_CTL_REQ_TX_STATUS;
+
+	if (tx_info->flags & IEEE80211_TX_CTL_USE_CTS_PROTECT)
+		rts_info->flags |= IEEE80211_TX_CTL_NO_ACK;
+	else
+		rts_info->flags &= ~IEEE80211_TX_CTL_NO_ACK;
+
+	if (tx_info->flags & IEEE80211_TX_CTL_USE_CTS_PROTECT)
+		ieee80211_ctstoself_get(rt2x00dev->hw, tx_info->control.vif,
+					frag_skb->data, size, tx_info,
 					(struct ieee80211_cts *)(skb->data));
 	else
-		ieee80211_rts_get(rt2x00dev->hw, control->vif,
-				  frag_skb->data, frag_skb->len, control,
+		ieee80211_rts_get(rt2x00dev->hw, tx_info->control.vif,
+				  frag_skb->data, size, tx_info,
 				  (struct ieee80211_rts *)(skb->data));
 
-	/*
-	 * Initialize skb descriptor
-	 */
-	skbdesc = get_skb_frame_desc(skb);
-	memset(skbdesc, 0, sizeof(*skbdesc));
-	skbdesc->flags |= FRAME_DESC_DRIVER_GENERATED;
-
-	if (rt2x00dev->ops->lib->write_tx_data(rt2x00dev, queue, skb, control)) {
+	if (rt2x00queue_write_tx_frame(queue, skb)) {
 		WARNING(rt2x00dev, "Failed to send RTS/CTS frame.\n");
 		return NETDEV_TX_BUSY;
 	}
@@ -76,13 +90,14 @@ static int rt2x00mac_tx_rts_cts(struct rt2x00_dev *rt2x00dev,
 	return NETDEV_TX_OK;
 }
 
-int rt2x00mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb,
-		 struct ieee80211_tx_control *control)
+int rt2x00mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
 	struct rt2x00_dev *rt2x00dev = hw->priv;
+	struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(skb);
 	struct ieee80211_hdr *ieee80211hdr = (struct ieee80211_hdr *)skb->data;
+	enum data_queue_qid qid = skb_get_queue_mapping(skb);
+	struct rt2x00_intf *intf = vif_to_intf(tx_info->control.vif);
 	struct data_queue *queue;
-	struct skb_frame_desc *skbdesc;
 	u16 frame_control;
 
 	/*
@@ -100,57 +115,62 @@ int rt2x00mac_tx(struct ieee80211_hw *hw, struct sk_buff *skb,
 	/*
 	 * Determine which queue to put packet on.
 	 */
-	if (control->flags & IEEE80211_TXCTL_SEND_AFTER_DTIM &&
+	if (tx_info->flags & IEEE80211_TX_CTL_SEND_AFTER_DTIM &&
 	    test_bit(DRIVER_REQUIRE_ATIM_QUEUE, &rt2x00dev->flags))
-		queue = rt2x00queue_get_queue(rt2x00dev, RT2X00_BCN_QUEUE_ATIM);
+		queue = rt2x00queue_get_queue(rt2x00dev, QID_ATIM);
 	else
-		queue = rt2x00queue_get_queue(rt2x00dev, control->queue);
+		queue = rt2x00queue_get_queue(rt2x00dev, qid);
 	if (unlikely(!queue)) {
 		ERROR(rt2x00dev,
 		      "Attempt to send packet over invalid queue %d.\n"
-		      "Please file bug report to %s.\n",
-		      control->queue, DRV_PROJECT);
+		      "Please file bug report to %s.\n", qid, 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.
+	 * If CTS/RTS is required. create and queue that frame first.
+	 * Make sure we have at least enough entries available to send
+	 * this CTS/RTS frame as well as the data frame.
+	 * Note that when the driver has set the set_rts_threshold()
+	 * callback function it doesn't need software generation of
+	 * either RTS or CTS-to-self frame and handles everything
+	 * inside the hardware.
 	 */
 	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 ((tx_info->flags & (IEEE80211_TX_CTL_USE_RTS_CTS |
+			       IEEE80211_TX_CTL_USE_CTS_PROTECT)) &&
+	    !rt2x00dev->ops->hw->set_rts_threshold) {
 		if (rt2x00queue_available(queue) <= 1) {
-			ieee80211_stop_queue(rt2x00dev->hw, control->queue);
+			ieee80211_stop_queue(rt2x00dev->hw, qid);
 			return NETDEV_TX_BUSY;
 		}
 
-		if (rt2x00mac_tx_rts_cts(rt2x00dev, queue, skb, control)) {
-			ieee80211_stop_queue(rt2x00dev->hw, control->queue);
+		if (rt2x00mac_tx_rts_cts(rt2x00dev, queue, skb)) {
+			ieee80211_stop_queue(rt2x00dev->hw, qid);
 			return NETDEV_TX_BUSY;
 		}
 	}
 
 	/*
-	 * Initialize skb descriptor
+	 * XXX: This is as wrong as the old mac80211 code was,
+	 *	due to beacons not getting sequence numbers assigned
+	 *	properly.
 	 */
-	skbdesc = get_skb_frame_desc(skb);
-	memset(skbdesc, 0, sizeof(*skbdesc));
+	if (tx_info->flags & IEEE80211_TX_CTL_ASSIGN_SEQ) {
+		if (tx_info->flags & IEEE80211_TX_CTL_FIRST_FRAGMENT)
+			intf->seqno += 0x10;
+		ieee80211hdr->seq_ctrl &= cpu_to_le16(IEEE80211_SCTL_FRAG);
+		ieee80211hdr->seq_ctrl |= cpu_to_le16(intf->seqno);
+	}
 
-	if (rt2x00dev->ops->lib->write_tx_data(rt2x00dev, queue, skb, control)) {
-		ieee80211_stop_queue(rt2x00dev->hw, control->queue);
+	if (rt2x00queue_write_tx_frame(queue, skb)) {
+		ieee80211_stop_queue(rt2x00dev->hw, qid);
 		return NETDEV_TX_BUSY;
 	}
 
-	if (rt2x00queue_full(queue))
-		ieee80211_stop_queue(rt2x00dev->hw, control->queue);
-
-	if (rt2x00dev->ops->lib->kick_tx_queue)
-		rt2x00dev->ops->lib->kick_tx_queue(rt2x00dev, control->queue);
+	if (rt2x00queue_threshold(queue))
+		ieee80211_stop_queue(rt2x00dev->hw, qid);
 
 	return NETDEV_TX_OK;
 }
@@ -183,8 +203,7 @@ int rt2x00mac_add_interface(struct ieee80211_hw *hw,
 {
 	struct rt2x00_dev *rt2x00dev = hw->priv;
 	struct rt2x00_intf *intf = vif_to_intf(conf->vif);
-	struct data_queue *queue =
-	    rt2x00queue_get_queue(rt2x00dev, RT2X00_BCN_QUEUE_BEACON);
+	struct data_queue *queue = rt2x00queue_get_queue(rt2x00dev, QID_BEACON);
 	struct queue_entry *entry = NULL;
 	unsigned int i;
 
@@ -197,13 +216,12 @@ int rt2x00mac_add_interface(struct ieee80211_hw *hw,
 		return -ENODEV;
 
 	/*
-	 * When we don't support mixed interfaces (a combination
-	 * of sta and ap virtual interfaces) then we can only
-	 * add this interface when the rival interface count is 0.
+	 * We don't support mixed combinations of sta and ap virtual
+	 * interfaces. We can only add this interface when the rival
+	 * interface count is 0.
 	 */
-	if (!test_bit(DRIVER_SUPPORT_MIXED_INTERFACES, &rt2x00dev->flags) &&
-	    ((conf->type == IEEE80211_IF_TYPE_AP && rt2x00dev->intf_sta_count) ||
-	     (conf->type != IEEE80211_IF_TYPE_AP && rt2x00dev->intf_ap_count)))
+	if ((conf->type == IEEE80211_IF_TYPE_AP && rt2x00dev->intf_sta_count) ||
+	    (conf->type != IEEE80211_IF_TYPE_AP && rt2x00dev->intf_ap_count))
 		return -ENOBUFS;
 
 	/*
@@ -343,7 +361,8 @@ int rt2x00mac_config_interface(struct ieee80211_hw *hw,
 {
 	struct rt2x00_dev *rt2x00dev = hw->priv;
 	struct rt2x00_intf *intf = vif_to_intf(vif);
-	int status;
+	int update_bssid = 0;
+	int status = 0;
 
 	/*
 	 * Mac80211 might be calling this function while we are trying
@@ -355,12 +374,13 @@ int rt2x00mac_config_interface(struct ieee80211_hw *hw,
 	spin_lock(&intf->lock);
 
 	/*
-	 * If the interface does not work in master mode,
-	 * then the bssid value in the interface structure
-	 * should now be set.
+	 * conf->bssid can be NULL if coming from the internal
+	 * beacon update routine.
 	 */
-	if (conf->type != IEEE80211_IF_TYPE_AP)
+	if (conf->changed & IEEE80211_IFCC_BSSID && conf->bssid) {
+		update_bssid = 1;
 		memcpy(&intf->bssid, conf->bssid, ETH_ALEN);
+	}
 
 	spin_unlock(&intf->lock);
 
@@ -370,19 +390,14 @@ int rt2x00mac_config_interface(struct ieee80211_hw *hw,
 	 * values as arguments we make keep access to rt2x00_intf thread safe
 	 * even without the lock.
 	 */
-	rt2x00lib_config_intf(rt2x00dev, intf, conf->type, NULL, conf->bssid);
+	rt2x00lib_config_intf(rt2x00dev, intf, vif->type, NULL,
+			      update_bssid ? conf->bssid : NULL);
 
 	/*
-	 * We only need to initialize the beacon when master mode is enabled.
+	 * Update the beacon.
 	 */
-	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);
+	if (conf->changed & IEEE80211_IFCC_BEACON)
+		status = rt2x00queue_update_beacon(rt2x00dev, vif);
 
 	return status;
 }
@@ -428,7 +443,7 @@ void rt2x00mac_configure_filter(struct ieee80211_hw *hw,
 	if (!test_bit(DRIVER_REQUIRE_SCHEDULED, &rt2x00dev->flags))
 		rt2x00dev->ops->lib->config_filter(rt2x00dev, *total_flags);
 	else
-		queue_work(rt2x00dev->workqueue, &rt2x00dev->filter_work);
+		queue_work(rt2x00dev->hw->workqueue, &rt2x00dev->filter_work);
 }
 EXPORT_SYMBOL_GPL(rt2x00mac_configure_filter);
 
@@ -454,10 +469,10 @@ int rt2x00mac_get_tx_stats(struct ieee80211_hw *hw,
 	struct rt2x00_dev *rt2x00dev = hw->priv;
 	unsigned int i;
 
-	for (i = 0; i < hw->queues; i++) {
-		stats->data[i].len = rt2x00dev->tx[i].length;
-		stats->data[i].limit = rt2x00dev->tx[i].limit;
-		stats->data[i].count = rt2x00dev->tx[i].count;
+	for (i = 0; i < rt2x00dev->ops->tx_queues; i++) {
+		stats[i].len = rt2x00dev->tx[i].length;
+		stats[i].limit = rt2x00dev->tx[i].limit;
+		stats[i].count = rt2x00dev->tx[i].count;
 	}
 
 	return 0;
@@ -498,7 +513,7 @@ void rt2x00mac_bss_info_changed(struct ieee80211_hw *hw,
 	 * When the erp information has changed, we should perform
 	 * additional configuration steps. For all other changes we are done.
 	 */
-	if (changes & BSS_CHANGED_ERP_PREAMBLE) {
+	if (changes & (BSS_CHANGED_ERP_PREAMBLE | BSS_CHANGED_ERP_CTS_PROT)) {
 		if (!test_bit(DRIVER_REQUIRE_SCHEDULED, &rt2x00dev->flags))
 			rt2x00lib_config_erp(rt2x00dev, intf, bss_conf);
 		else
@@ -509,13 +524,13 @@ void rt2x00mac_bss_info_changed(struct ieee80211_hw *hw,
 	memcpy(&intf->conf, bss_conf, sizeof(*bss_conf));
 	if (delayed) {
 		intf->delayed_flags |= delayed;
-		queue_work(rt2x00dev->workqueue, &rt2x00dev->intf_work);
+		schedule_work(&rt2x00dev->intf_work);
 	}
 	spin_unlock(&intf->lock);
 }
 EXPORT_SYMBOL_GPL(rt2x00mac_bss_info_changed);
 
-int rt2x00mac_conf_tx(struct ieee80211_hw *hw, int queue_idx,
+int rt2x00mac_conf_tx(struct ieee80211_hw *hw, u16 queue_idx,
 		      const struct ieee80211_tx_queue_params *params)
 {
 	struct rt2x00_dev *rt2x00dev = hw->priv;
@@ -539,10 +554,7 @@ int rt2x00mac_conf_tx(struct ieee80211_hw *hw, int queue_idx,
 	else
 		queue->cw_max = 10; /* cw_min: 2^10 = 1024. */
 
-	if (params->aifs >= 0)
-		queue->aifs = params->aifs;
-	else
-		queue->aifs = 2;
+	queue->aifs = params->aifs;
 
 	INFO(rt2x00dev,
 	     "Configured TX queue %d - CWmin: %d, CWmax: %d, Aifs: %d.\n",
diff --git a/drivers/net/wireless/rt2x00/rt2x00pci.c b/drivers/net/wireless/rt2x00/rt2x00pci.c
index 60893de3bf8f..adf2876ed8ab 100644
--- a/drivers/net/wireless/rt2x00/rt2x00pci.c
+++ b/drivers/net/wireless/rt2x00/rt2x00pci.c
@@ -34,44 +34,34 @@
 /*
  * TX data handlers.
  */
-int rt2x00pci_write_tx_data(struct rt2x00_dev *rt2x00dev,
-			    struct data_queue *queue, struct sk_buff *skb,
-			    struct ieee80211_tx_control *control)
+int rt2x00pci_write_tx_data(struct queue_entry *entry)
 {
-	struct queue_entry *entry = rt2x00queue_get_entry(queue, Q_INDEX);
-	struct queue_entry_priv_pci_tx *priv_tx = entry->priv_data;
+	struct queue_entry_priv_pci *entry_priv = entry->priv_data;
 	struct skb_frame_desc *skbdesc;
 	u32 word;
 
-	if (rt2x00queue_full(queue))
-		return -EINVAL;
-
-	rt2x00_desc_read(priv_tx->desc, 0, &word);
+	rt2x00_desc_read(entry_priv->desc, 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"
+	/*
+	 * This should not happen, we already checked the entry
+	 * was ours. When the hardware disagrees there has been
+	 * a queue corruption!
+	 */
+	if (unlikely(rt2x00_get_field32(word, TXD_ENTRY_OWNER_NIC) ||
+		     rt2x00_get_field32(word, TXD_ENTRY_VALID))) {
+		ERROR(entry->queue->rt2x00dev,
+		      "Corrupt queue %d, accessing entry which is not ours.\n"
 		      "Please file bug report to %s.\n",
-		      control->queue, DRV_PROJECT);
+		      entry->queue->qid, DRV_PROJECT);
 		return -EINVAL;
 	}
 
 	/*
 	 * Fill in skb descriptor
 	 */
-	skbdesc = get_skb_frame_desc(skb);
-	skbdesc->data = skb->data;
-	skbdesc->data_len = skb->len;
-	skbdesc->desc = priv_tx->desc;
-	skbdesc->desc_len = queue->desc_size;
-	skbdesc->entry = entry;
-
-	memcpy(&priv_tx->control, control, sizeof(priv_tx->control));
-	memcpy(priv_tx->data, skb->data, skb->len);
-	rt2x00lib_write_tx_desc(rt2x00dev, skb, control);
-
-	rt2x00queue_index_inc(queue, Q_INDEX);
+	skbdesc = get_skb_frame_desc(entry->skb);
+	skbdesc->desc = entry_priv->desc;
+	skbdesc->desc_len = entry->queue->desc_size;
 
 	return 0;
 }
@@ -84,180 +74,62 @@ void rt2x00pci_rxdone(struct rt2x00_dev *rt2x00dev)
 {
 	struct data_queue *queue = rt2x00dev->rx;
 	struct queue_entry *entry;
-	struct queue_entry_priv_pci_rx *priv_rx;
-	struct ieee80211_hdr *hdr;
+	struct queue_entry_priv_pci *entry_priv;
 	struct skb_frame_desc *skbdesc;
-	struct rxdone_entry_desc rxdesc;
-	int header_size;
-	int align;
 	u32 word;
 
 	while (1) {
 		entry = rt2x00queue_get_entry(queue, Q_INDEX);
-		priv_rx = entry->priv_data;
-		rt2x00_desc_read(priv_rx->desc, 0, &word);
+		entry_priv = entry->priv_data;
+		rt2x00_desc_read(entry_priv->desc, 0, &word);
 
 		if (rt2x00_get_field32(word, RXD_ENTRY_OWNER_NIC))
 			break;
 
-		memset(&rxdesc, 0, sizeof(rxdesc));
-		rt2x00dev->ops->lib->fill_rxdone(entry, &rxdesc);
-
-		hdr = (struct ieee80211_hdr *)priv_rx->data;
-		header_size =
-		    ieee80211_get_hdrlen(le16_to_cpu(hdr->frame_control));
-
 		/*
-		 * The data behind the ieee80211 header must be
-		 * aligned on a 4 byte boundary.
-		 */
-		align = header_size % 4;
-
-		/*
-		 * Allocate the sk_buffer, initialize it and copy
-		 * all data into it.
-		 */
-		entry->skb = dev_alloc_skb(rxdesc.size + align);
-		if (!entry->skb)
-			return;
-
-		skb_reserve(entry->skb, align);
-		memcpy(skb_put(entry->skb, rxdesc.size),
-		       priv_rx->data, rxdesc.size);
-
-		/*
-		 * Fill in skb descriptor
+		 * Fill in desc fields of the skb descriptor
 		 */
 		skbdesc = get_skb_frame_desc(entry->skb);
-		memset(skbdesc, 0, sizeof(*skbdesc));
-		skbdesc->data = entry->skb->data;
-		skbdesc->data_len = entry->skb->len;
-		skbdesc->desc = priv_rx->desc;
-		skbdesc->desc_len = queue->desc_size;
-		skbdesc->entry = entry;
+		skbdesc->desc = entry_priv->desc;
+		skbdesc->desc_len = entry->queue->desc_size;
 
 		/*
 		 * Send the frame to rt2x00lib for further processing.
 		 */
-		rt2x00lib_rxdone(entry, &rxdesc);
-
-		if (test_bit(DEVICE_ENABLED_RADIO, &queue->rt2x00dev->flags)) {
-			rt2x00_set_field32(&word, RXD_ENTRY_OWNER_NIC, 1);
-			rt2x00_desc_write(priv_rx->desc, 0, word);
-		}
-
-		rt2x00queue_index_inc(queue, Q_INDEX);
+		rt2x00lib_rxdone(rt2x00dev, entry);
 	}
 }
 EXPORT_SYMBOL_GPL(rt2x00pci_rxdone);
 
-void rt2x00pci_txdone(struct rt2x00_dev *rt2x00dev, struct queue_entry *entry,
-		      struct txdone_entry_desc *txdesc)
-{
-	struct queue_entry_priv_pci_tx *priv_tx = entry->priv_data;
-	u32 word;
-
-	txdesc->control = &priv_tx->control;
-	rt2x00lib_txdone(entry, txdesc);
-
-	/*
-	 * Make this entry available for reuse.
-	 */
-	entry->flags = 0;
-
-	rt2x00_desc_read(priv_tx->desc, 0, &word);
-	rt2x00_set_field32(&word, TXD_ENTRY_OWNER_NIC, 0);
-	rt2x00_set_field32(&word, TXD_ENTRY_VALID, 0);
-	rt2x00_desc_write(priv_tx->desc, 0, word);
-
-	rt2x00queue_index_inc(entry->queue, Q_INDEX_DONE);
-
-	/*
-	 * If the data queue 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 (!rt2x00queue_full(entry->queue))
-		ieee80211_wake_queue(rt2x00dev->hw, priv_tx->control.queue);
-
-}
-EXPORT_SYMBOL_GPL(rt2x00pci_txdone);
-
 /*
  * Device initialization handlers.
  */
-#define desc_size(__queue)			\
-({						\
-	 ((__queue)->limit * (__queue)->desc_size);\
-})
-
-#define data_size(__queue)			\
-({						\
-	 ((__queue)->limit * (__queue)->data_size);\
-})
-
-#define dma_size(__queue)			\
-({						\
-	data_size(__queue) + desc_size(__queue);\
-})
-
-#define desc_offset(__queue, __base, __i)	\
-({						\
-	(__base) + data_size(__queue) + 	\
-	    ((__i) * (__queue)->desc_size);	\
-})
-
-#define data_offset(__queue, __base, __i)	\
-({						\
-	(__base) +				\
-	    ((__i) * (__queue)->data_size);	\
-})
-
 static int rt2x00pci_alloc_queue_dma(struct rt2x00_dev *rt2x00dev,
 				     struct data_queue *queue)
 {
-	struct pci_dev *pci_dev = rt2x00dev_pci(rt2x00dev);
-	struct queue_entry_priv_pci_rx *priv_rx;
-	struct queue_entry_priv_pci_tx *priv_tx;
+	struct queue_entry_priv_pci *entry_priv;
 	void *addr;
 	dma_addr_t dma;
-	void *desc_addr;
-	dma_addr_t desc_dma;
-	void *data_addr;
-	dma_addr_t data_dma;
 	unsigned int i;
 
 	/*
 	 * Allocate DMA memory for descriptor and buffer.
 	 */
-	addr = pci_alloc_consistent(pci_dev, dma_size(queue), &dma);
+	addr = dma_alloc_coherent(rt2x00dev->dev,
+				  queue->limit * queue->desc_size,
+				  &dma, GFP_KERNEL | GFP_DMA);
 	if (!addr)
 		return -ENOMEM;
 
-	memset(addr, 0, dma_size(queue));
+	memset(addr, 0, queue->limit * queue->desc_size);
 
 	/*
 	 * Initialize all queue entries to contain valid addresses.
 	 */
 	for (i = 0; i < queue->limit; i++) {
-		desc_addr = desc_offset(queue, addr, i);
-		desc_dma = desc_offset(queue, dma, i);
-		data_addr = data_offset(queue, addr, i);
-		data_dma = data_offset(queue, dma, i);
-
-		if (queue->qid == QID_RX) {
-			priv_rx = queue->entries[i].priv_data;
-			priv_rx->desc = desc_addr;
-			priv_rx->desc_dma = desc_dma;
-			priv_rx->data = data_addr;
-			priv_rx->data_dma = data_dma;
-		} else {
-			priv_tx = queue->entries[i].priv_data;
-			priv_tx->desc = desc_addr;
-			priv_tx->desc_dma = desc_dma;
-			priv_tx->data = data_addr;
-			priv_tx->data_dma = data_dma;
-		}
+		entry_priv = queue->entries[i].priv_data;
+		entry_priv->desc = addr + i * queue->desc_size;
+		entry_priv->desc_dma = dma + i * queue->desc_size;
 	}
 
 	return 0;
@@ -266,34 +138,19 @@ static int rt2x00pci_alloc_queue_dma(struct rt2x00_dev *rt2x00dev,
 static void rt2x00pci_free_queue_dma(struct rt2x00_dev *rt2x00dev,
 				     struct data_queue *queue)
 {
-	struct pci_dev *pci_dev = rt2x00dev_pci(rt2x00dev);
-	struct queue_entry_priv_pci_rx *priv_rx;
-	struct queue_entry_priv_pci_tx *priv_tx;
-	void *data_addr;
-	dma_addr_t data_dma;
-
-	if (queue->qid == QID_RX) {
-		priv_rx = queue->entries[0].priv_data;
-		data_addr = priv_rx->data;
-		data_dma = priv_rx->data_dma;
-
-		priv_rx->data = NULL;
-	} else {
-		priv_tx = queue->entries[0].priv_data;
-		data_addr = priv_tx->data;
-		data_dma = priv_tx->data_dma;
-
-		priv_tx->data = NULL;
-	}
-
-	if (data_addr)
-		pci_free_consistent(pci_dev, dma_size(queue),
-				    data_addr, data_dma);
+	struct queue_entry_priv_pci *entry_priv =
+	    queue->entries[0].priv_data;
+
+	if (entry_priv->desc)
+		dma_free_coherent(rt2x00dev->dev,
+				  queue->limit * queue->desc_size,
+				  entry_priv->desc, entry_priv->desc_dma);
+	entry_priv->desc = NULL;
 }
 
 int rt2x00pci_initialize(struct rt2x00_dev *rt2x00dev)
 {
-	struct pci_dev *pci_dev = rt2x00dev_pci(rt2x00dev);
+	struct pci_dev *pci_dev = to_pci_dev(rt2x00dev->dev);
 	struct data_queue *queue;
 	int status;
 
@@ -334,7 +191,7 @@ void rt2x00pci_uninitialize(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Free irq line.
 	 */
-	free_irq(rt2x00dev_pci(rt2x00dev)->irq, rt2x00dev);
+	free_irq(to_pci_dev(rt2x00dev->dev)->irq, rt2x00dev);
 
 	/*
 	 * Free DMA
@@ -363,7 +220,7 @@ static void rt2x00pci_free_reg(struct rt2x00_dev *rt2x00dev)
 
 static int rt2x00pci_alloc_reg(struct rt2x00_dev *rt2x00dev)
 {
-	struct pci_dev *pci_dev = rt2x00dev_pci(rt2x00dev);
+	struct pci_dev *pci_dev = to_pci_dev(rt2x00dev->dev);
 
 	rt2x00dev->csr.base = ioremap(pci_resource_start(pci_dev, 0),
 				      pci_resource_len(pci_dev, 0));
@@ -412,7 +269,7 @@ int rt2x00pci_probe(struct pci_dev *pci_dev, const struct pci_device_id *id)
 	if (pci_set_mwi(pci_dev))
 		ERROR_PROBE("MWI not available.\n");
 
-	if (pci_set_dma_mask(pci_dev, DMA_32BIT_MASK)) {
+	if (dma_set_mask(&pci_dev->dev, DMA_32BIT_MASK)) {
 		ERROR_PROBE("PCI DMA not supported.\n");
 		retval = -EIO;
 		goto exit_disable_device;
@@ -428,7 +285,7 @@ int rt2x00pci_probe(struct pci_dev *pci_dev, const struct pci_device_id *id)
 	pci_set_drvdata(pci_dev, hw);
 
 	rt2x00dev = hw->priv;
-	rt2x00dev->dev = pci_dev;
+	rt2x00dev->dev = &pci_dev->dev;
 	rt2x00dev->ops = ops;
 	rt2x00dev->hw = hw;
 
diff --git a/drivers/net/wireless/rt2x00/rt2x00pci.h b/drivers/net/wireless/rt2x00/rt2x00pci.h
index 9d1cdb99431c..80bf97c03e2d 100644
--- a/drivers/net/wireless/rt2x00/rt2x00pci.h
+++ b/drivers/net/wireless/rt2x00/rt2x00pci.h
@@ -82,49 +82,31 @@ static inline void rt2x00pci_register_write(struct rt2x00_dev *rt2x00dev,
 static inline void
 rt2x00pci_register_multiwrite(struct rt2x00_dev *rt2x00dev,
 			      const unsigned long offset,
-			      void *value, const u16 length)
+			      const void *value, const u16 length)
 {
 	memcpy_toio(rt2x00dev->csr.base + offset, value, length);
 }
 
-/*
- * TX data handlers.
- */
-int rt2x00pci_write_tx_data(struct rt2x00_dev *rt2x00dev,
-			    struct data_queue *queue, struct sk_buff *skb,
-			    struct ieee80211_tx_control *control);
-
 /**
- * struct queue_entry_priv_pci_rx: Per RX entry PCI specific information
+ * rt2x00pci_write_tx_data - Initialize data for TX operation
+ * @entry: The entry where the frame is located
  *
- * @desc: Pointer to device descriptor.
- * @data: Pointer to device's entry memory.
- * @dma: DMA pointer to &data.
+ * This function will initialize the DMA and skb descriptor
+ * to prepare the entry for the actual TX operation.
  */
-struct queue_entry_priv_pci_rx {
-	__le32 *desc;
-	dma_addr_t desc_dma;
-
-	void *data;
-	dma_addr_t data_dma;
-};
+int rt2x00pci_write_tx_data(struct queue_entry *entry);
 
 /**
- * struct queue_entry_priv_pci_tx: Per TX entry PCI specific information
+ * struct queue_entry_priv_pci: Per entry PCI specific information
  *
  * @desc: Pointer to device descriptor
+ * @desc_dma: DMA pointer to &desc.
  * @data: Pointer to device's entry memory.
- * @dma: DMA pointer to &data.
- * @control: mac80211 control structure used to transmit data.
+ * @data_dma: DMA pointer to &data.
  */
-struct queue_entry_priv_pci_tx {
+struct queue_entry_priv_pci {
 	__le32 *desc;
 	dma_addr_t desc_dma;
-
-	void *data;
-	dma_addr_t data_dma;
-
-	struct ieee80211_tx_control control;
 };
 
 /**
@@ -133,15 +115,6 @@ struct queue_entry_priv_pci_tx {
  */
 void rt2x00pci_rxdone(struct rt2x00_dev *rt2x00dev);
 
-/**
- * rt2x00pci_txdone - Handle TX done events
- * @rt2x00dev: Device pointer, see &struct rt2x00_dev.
- * @entry: Entry which has completed the transmission of a frame.
- * @desc: TX done descriptor
- */
-void rt2x00pci_txdone(struct rt2x00_dev *rt2x00dev, struct queue_entry *entry,
-		      struct txdone_entry_desc *desc);
-
 /*
  * Device initialization handlers.
  */
diff --git a/drivers/net/wireless/rt2x00/rt2x00queue.c b/drivers/net/wireless/rt2x00/rt2x00queue.c
index 659e9f44c40c..7f442030f5ad 100644
--- a/drivers/net/wireless/rt2x00/rt2x00queue.c
+++ b/drivers/net/wireless/rt2x00/rt2x00queue.c
@@ -25,24 +25,370 @@
 
 #include <linux/kernel.h>
 #include <linux/module.h>
+#include <linux/dma-mapping.h>
 
 #include "rt2x00.h"
 #include "rt2x00lib.h"
 
+struct sk_buff *rt2x00queue_alloc_rxskb(struct rt2x00_dev *rt2x00dev,
+					struct queue_entry *entry)
+{
+	unsigned int frame_size;
+	unsigned int reserved_size;
+	struct sk_buff *skb;
+	struct skb_frame_desc *skbdesc;
+
+	/*
+	 * The frame size includes descriptor size, because the
+	 * hardware directly receive the frame into the skbuffer.
+	 */
+	frame_size = entry->queue->data_size + entry->queue->desc_size;
+
+	/*
+	 * The payload should be aligned to a 4-byte boundary,
+	 * this means we need at least 3 bytes for moving the frame
+	 * into the correct offset.
+	 */
+	reserved_size = 4;
+
+	/*
+	 * Allocate skbuffer.
+	 */
+	skb = dev_alloc_skb(frame_size + reserved_size);
+	if (!skb)
+		return NULL;
+
+	skb_reserve(skb, reserved_size);
+	skb_put(skb, frame_size);
+
+	/*
+	 * Populate skbdesc.
+	 */
+	skbdesc = get_skb_frame_desc(skb);
+	memset(skbdesc, 0, sizeof(*skbdesc));
+	skbdesc->entry = entry;
+
+	if (test_bit(DRIVER_REQUIRE_DMA, &rt2x00dev->flags)) {
+		skbdesc->skb_dma = dma_map_single(rt2x00dev->dev,
+						  skb->data,
+						  skb->len,
+						  DMA_FROM_DEVICE);
+		skbdesc->flags |= SKBDESC_DMA_MAPPED_RX;
+	}
+
+	return skb;
+}
+
+void rt2x00queue_map_txskb(struct rt2x00_dev *rt2x00dev, struct sk_buff *skb)
+{
+	struct skb_frame_desc *skbdesc = get_skb_frame_desc(skb);
+
+	skbdesc->skb_dma = dma_map_single(rt2x00dev->dev, skb->data, skb->len,
+					  DMA_TO_DEVICE);
+	skbdesc->flags |= SKBDESC_DMA_MAPPED_TX;
+}
+EXPORT_SYMBOL_GPL(rt2x00queue_map_txskb);
+
+void rt2x00queue_unmap_skb(struct rt2x00_dev *rt2x00dev, struct sk_buff *skb)
+{
+	struct skb_frame_desc *skbdesc = get_skb_frame_desc(skb);
+
+	if (skbdesc->flags & SKBDESC_DMA_MAPPED_RX) {
+		dma_unmap_single(rt2x00dev->dev, skbdesc->skb_dma, skb->len,
+				 DMA_FROM_DEVICE);
+		skbdesc->flags &= ~SKBDESC_DMA_MAPPED_RX;
+	}
+
+	if (skbdesc->flags & SKBDESC_DMA_MAPPED_TX) {
+		dma_unmap_single(rt2x00dev->dev, skbdesc->skb_dma, skb->len,
+				 DMA_TO_DEVICE);
+		skbdesc->flags &= ~SKBDESC_DMA_MAPPED_TX;
+	}
+}
+
+void rt2x00queue_free_skb(struct rt2x00_dev *rt2x00dev, struct sk_buff *skb)
+{
+	if (!skb)
+		return;
+
+	rt2x00queue_unmap_skb(rt2x00dev, skb);
+	dev_kfree_skb_any(skb);
+}
+
+static void rt2x00queue_create_tx_descriptor(struct queue_entry *entry,
+					     struct txentry_desc *txdesc)
+{
+	struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
+	struct ieee80211_tx_info *tx_info = IEEE80211_SKB_CB(entry->skb);
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)entry->skb->data;
+	struct ieee80211_rate *rate =
+	    ieee80211_get_tx_rate(rt2x00dev->hw, tx_info);
+	const struct rt2x00_rate *hwrate;
+	unsigned int data_length;
+	unsigned int duration;
+	unsigned int residual;
+
+	memset(txdesc, 0, sizeof(*txdesc));
+
+	/*
+	 * Initialize information from queue
+	 */
+	txdesc->queue = entry->queue->qid;
+	txdesc->cw_min = entry->queue->cw_min;
+	txdesc->cw_max = entry->queue->cw_max;
+	txdesc->aifs = entry->queue->aifs;
+
+	/* Data length should be extended with 4 bytes for CRC */
+	data_length = entry->skb->len + 4;
+
+	/*
+	 * Check whether this frame is to be acked.
+	 */
+	if (!(tx_info->flags & IEEE80211_TX_CTL_NO_ACK))
+		__set_bit(ENTRY_TXD_ACK, &txdesc->flags);
+
+	/*
+	 * Check if this is a RTS/CTS frame
+	 */
+	if (ieee80211_is_rts(hdr->frame_control) ||
+	    ieee80211_is_cts(hdr->frame_control)) {
+		__set_bit(ENTRY_TXD_BURST, &txdesc->flags);
+		if (ieee80211_is_rts(hdr->frame_control))
+			__set_bit(ENTRY_TXD_RTS_FRAME, &txdesc->flags);
+		else
+			__set_bit(ENTRY_TXD_CTS_FRAME, &txdesc->flags);
+		if (tx_info->control.rts_cts_rate_idx >= 0)
+			rate =
+			    ieee80211_get_rts_cts_rate(rt2x00dev->hw, tx_info);
+	}
+
+	/*
+	 * Determine retry information.
+	 */
+	txdesc->retry_limit = tx_info->control.retry_limit;
+	if (tx_info->flags & IEEE80211_TX_CTL_LONG_RETRY_LIMIT)
+		__set_bit(ENTRY_TXD_RETRY_MODE, &txdesc->flags);
+
+	/*
+	 * Check if more fragments are pending
+	 */
+	if (ieee80211_has_morefrags(hdr->frame_control)) {
+		__set_bit(ENTRY_TXD_BURST, &txdesc->flags);
+		__set_bit(ENTRY_TXD_MORE_FRAG, &txdesc->flags);
+	}
+
+	/*
+	 * Beacons and probe responses require the tsf timestamp
+	 * to be inserted into the frame.
+	 */
+	if (ieee80211_is_beacon(hdr->frame_control) ||
+	    ieee80211_is_probe_resp(hdr->frame_control))
+		__set_bit(ENTRY_TXD_REQ_TIMESTAMP, &txdesc->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 (test_bit(ENTRY_TXD_RTS_FRAME, &txdesc->flags)) {
+		txdesc->ifs = IFS_SIFS;
+	} else if (tx_info->flags & IEEE80211_TX_CTL_FIRST_FRAGMENT) {
+		__set_bit(ENTRY_TXD_FIRST_FRAGMENT, &txdesc->flags);
+		txdesc->ifs = IFS_BACKOFF;
+	} else {
+		txdesc->ifs = IFS_SIFS;
+	}
+
+	/*
+	 * PLCP setup
+	 * Length calculation depends on OFDM/CCK rate.
+	 */
+	hwrate = rt2x00_get_rate(rate->hw_value);
+	txdesc->signal = hwrate->plcp;
+	txdesc->service = 0x04;
+
+	if (hwrate->flags & DEV_RATE_OFDM) {
+		__set_bit(ENTRY_TXD_OFDM_RATE, &txdesc->flags);
+
+		txdesc->length_high = (data_length >> 6) & 0x3f;
+		txdesc->length_low = data_length & 0x3f;
+	} else {
+		/*
+		 * Convert length to microseconds.
+		 */
+		residual = get_duration_res(data_length, hwrate->bitrate);
+		duration = get_duration(data_length, hwrate->bitrate);
+
+		if (residual != 0) {
+			duration++;
+
+			/*
+			 * Check if we need to set the Length Extension
+			 */
+			if (hwrate->bitrate == 110 && residual <= 30)
+				txdesc->service |= 0x80;
+		}
+
+		txdesc->length_high = (duration >> 8) & 0xff;
+		txdesc->length_low = duration & 0xff;
+
+		/*
+		 * When preamble is enabled we should set the
+		 * preamble bit for the signal.
+		 */
+		if (rt2x00_get_rate_preamble(rate->hw_value))
+			txdesc->signal |= 0x08;
+	}
+}
+
+static void rt2x00queue_write_tx_descriptor(struct queue_entry *entry,
+					    struct txentry_desc *txdesc)
+{
+	struct data_queue *queue = entry->queue;
+	struct rt2x00_dev *rt2x00dev = queue->rt2x00dev;
+
+	rt2x00dev->ops->lib->write_tx_desc(rt2x00dev, entry->skb, txdesc);
+
+	/*
+	 * All processing on the frame has been completed, this means
+	 * it is now ready to be dumped to userspace through debugfs.
+	 */
+	rt2x00debug_dump_frame(rt2x00dev, DUMP_FRAME_TX, entry->skb);
+
+	/*
+	 * Check if we need to kick the queue, there are however a few rules
+	 *	1) Don't kick beacon queue
+	 *	2) Don't kick unless this is the last in frame in a burst.
+	 *	   When the burst flag is set, this frame is always followed
+	 *	   by another frame which in some way are related to eachother.
+	 *	   This is true for fragments, RTS or CTS-to-self frames.
+	 *	3) Rule 2 can be broken when the available entries
+	 *	   in the queue are less then a certain threshold.
+	 */
+	if (entry->queue->qid == QID_BEACON)
+		return;
+
+	if (rt2x00queue_threshold(queue) ||
+	    !test_bit(ENTRY_TXD_BURST, &txdesc->flags))
+		rt2x00dev->ops->lib->kick_tx_queue(rt2x00dev, queue->qid);
+}
+
+int rt2x00queue_write_tx_frame(struct data_queue *queue, struct sk_buff *skb)
+{
+	struct queue_entry *entry = rt2x00queue_get_entry(queue, Q_INDEX);
+	struct txentry_desc txdesc;
+	struct skb_frame_desc *skbdesc;
+
+	if (unlikely(rt2x00queue_full(queue)))
+		return -EINVAL;
+
+	if (__test_and_set_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags)) {
+		ERROR(queue->rt2x00dev,
+		      "Arrived at non-free entry in the non-full queue %d.\n"
+		      "Please file bug report to %s.\n",
+		      queue->qid, DRV_PROJECT);
+		return -EINVAL;
+	}
+
+	/*
+	 * Copy all TX descriptor information into txdesc,
+	 * after that we are free to use the skb->cb array
+	 * for our information.
+	 */
+	entry->skb = skb;
+	rt2x00queue_create_tx_descriptor(entry, &txdesc);
+
+	/*
+	 * skb->cb array is now ours and we are free to use it.
+	 */
+	skbdesc = get_skb_frame_desc(entry->skb);
+	memset(skbdesc, 0, sizeof(*skbdesc));
+	skbdesc->entry = entry;
+
+	if (unlikely(queue->rt2x00dev->ops->lib->write_tx_data(entry))) {
+		__clear_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags);
+		return -EIO;
+	}
+
+	if (test_bit(DRIVER_REQUIRE_DMA, &queue->rt2x00dev->flags))
+		rt2x00queue_map_txskb(queue->rt2x00dev, skb);
+
+	__set_bit(ENTRY_DATA_PENDING, &entry->flags);
+
+	rt2x00queue_index_inc(queue, Q_INDEX);
+	rt2x00queue_write_tx_descriptor(entry, &txdesc);
+
+	return 0;
+}
+
+int rt2x00queue_update_beacon(struct rt2x00_dev *rt2x00dev,
+			      struct ieee80211_vif *vif)
+{
+	struct rt2x00_intf *intf = vif_to_intf(vif);
+	struct skb_frame_desc *skbdesc;
+	struct txentry_desc txdesc;
+	__le32 desc[16];
+
+	if (unlikely(!intf->beacon))
+		return -ENOBUFS;
+
+	intf->beacon->skb = ieee80211_beacon_get(rt2x00dev->hw, vif);
+	if (!intf->beacon->skb)
+		return -ENOMEM;
+
+	/*
+	 * Copy all TX descriptor information into txdesc,
+	 * after that we are free to use the skb->cb array
+	 * for our information.
+	 */
+	rt2x00queue_create_tx_descriptor(intf->beacon, &txdesc);
+
+	/*
+	 * For the descriptor we use a local array from where the
+	 * driver can move it to the correct location required for
+	 * the hardware.
+	 */
+	memset(desc, 0, sizeof(desc));
+
+	/*
+	 * Fill in skb descriptor
+	 */
+	skbdesc = get_skb_frame_desc(intf->beacon->skb);
+	memset(skbdesc, 0, sizeof(*skbdesc));
+	skbdesc->desc = desc;
+	skbdesc->desc_len = intf->beacon->queue->desc_size;
+	skbdesc->entry = intf->beacon;
+
+	/*
+	 * Write TX descriptor into reserved room in front of the beacon.
+	 */
+	rt2x00queue_write_tx_descriptor(intf->beacon, &txdesc);
+
+	/*
+	 * Send beacon to hardware.
+	 * Also enable beacon generation, which might have been disabled
+	 * by the driver during the config_beacon() callback function.
+	 */
+	rt2x00dev->ops->lib->write_beacon(intf->beacon);
+	rt2x00dev->ops->lib->kick_tx_queue(rt2x00dev, QID_BEACON);
+
+	return 0;
+}
+
 struct data_queue *rt2x00queue_get_queue(struct rt2x00_dev *rt2x00dev,
-					 const unsigned int queue)
+					 const enum data_queue_qid queue)
 {
 	int atim = test_bit(DRIVER_REQUIRE_ATIM_QUEUE, &rt2x00dev->flags);
 
-	if (queue < rt2x00dev->hw->queues && rt2x00dev->tx)
+	if (queue < rt2x00dev->ops->tx_queues && rt2x00dev->tx)
 		return &rt2x00dev->tx[queue];
 
 	if (!rt2x00dev->bcn)
 		return NULL;
 
-	if (queue == RT2X00_BCN_QUEUE_BEACON)
+	if (queue == QID_BEACON)
 		return &rt2x00dev->bcn[0];
-	else if (queue == RT2X00_BCN_QUEUE_ATIM && atim)
+	else if (queue == QID_ATIM && atim)
 		return &rt2x00dev->bcn[1];
 
 	return NULL;
@@ -96,7 +442,6 @@ void rt2x00queue_index_inc(struct data_queue *queue, enum queue_index index)
 
 	spin_unlock_irqrestore(&queue->lock, irqflags);
 }
-EXPORT_SYMBOL_GPL(rt2x00queue_index_inc);
 
 static void rt2x00queue_reset(struct data_queue *queue)
 {
@@ -153,6 +498,7 @@ static int rt2x00queue_alloc_entries(struct data_queue *queue,
 	rt2x00queue_reset(queue);
 
 	queue->limit = qdesc->entry_num;
+	queue->threshold = DIV_ROUND_UP(qdesc->entry_num, 10);
 	queue->data_size = qdesc->data_size;
 	queue->desc_size = qdesc->desc_size;
 
@@ -185,12 +531,41 @@ static int rt2x00queue_alloc_entries(struct data_queue *queue,
 	return 0;
 }
 
+static void rt2x00queue_free_skbs(struct rt2x00_dev *rt2x00dev,
+				  struct data_queue *queue)
+{
+	unsigned int i;
+
+	if (!queue->entries)
+		return;
+
+	for (i = 0; i < queue->limit; i++) {
+		if (queue->entries[i].skb)
+			rt2x00queue_free_skb(rt2x00dev, queue->entries[i].skb);
+	}
+}
+
+static int rt2x00queue_alloc_rxskbs(struct rt2x00_dev *rt2x00dev,
+				    struct data_queue *queue)
+{
+	unsigned int i;
+	struct sk_buff *skb;
+
+	for (i = 0; i < queue->limit; i++) {
+		skb = rt2x00queue_alloc_rxskb(rt2x00dev, &queue->entries[i]);
+		if (!skb)
+			return -ENOMEM;
+		queue->entries[i].skb = skb;
+	}
+
+	return 0;
+}
+
 int rt2x00queue_initialize(struct rt2x00_dev *rt2x00dev)
 {
 	struct data_queue *queue;
 	int status;
 
-
 	status = rt2x00queue_alloc_entries(rt2x00dev->rx, rt2x00dev->ops->rx);
 	if (status)
 		goto exit;
@@ -205,11 +580,14 @@ int rt2x00queue_initialize(struct rt2x00_dev *rt2x00dev)
 	if (status)
 		goto exit;
 
-	if (!test_bit(DRIVER_REQUIRE_ATIM_QUEUE, &rt2x00dev->flags))
-		return 0;
+	if (test_bit(DRIVER_REQUIRE_ATIM_QUEUE, &rt2x00dev->flags)) {
+		status = rt2x00queue_alloc_entries(&rt2x00dev->bcn[1],
+						   rt2x00dev->ops->atim);
+		if (status)
+			goto exit;
+	}
 
-	status = rt2x00queue_alloc_entries(&rt2x00dev->bcn[1],
-					   rt2x00dev->ops->atim);
+	status = rt2x00queue_alloc_rxskbs(rt2x00dev, rt2x00dev->rx);
 	if (status)
 		goto exit;
 
@@ -227,6 +605,8 @@ void rt2x00queue_uninitialize(struct rt2x00_dev *rt2x00dev)
 {
 	struct data_queue *queue;
 
+	rt2x00queue_free_skbs(rt2x00dev, rt2x00dev->rx);
+
 	queue_for_each(rt2x00dev, queue) {
 		kfree(queue->entries);
 		queue->entries = NULL;
@@ -255,11 +635,11 @@ int rt2x00queue_allocate(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * We need the following queues:
 	 * RX: 1
-	 * TX: hw->queues
+	 * TX: ops->tx_queues
 	 * Beacon: 1
 	 * Atim: 1 (if required)
 	 */
-	rt2x00dev->data_queues = 2 + rt2x00dev->hw->queues + req_atim;
+	rt2x00dev->data_queues = 2 + rt2x00dev->ops->tx_queues + req_atim;
 
 	queue = kzalloc(rt2x00dev->data_queues * sizeof(*queue), GFP_KERNEL);
 	if (!queue) {
@@ -272,7 +652,7 @@ int rt2x00queue_allocate(struct rt2x00_dev *rt2x00dev)
 	 */
 	rt2x00dev->rx = queue;
 	rt2x00dev->tx = &queue[1];
-	rt2x00dev->bcn = &queue[1 + rt2x00dev->hw->queues];
+	rt2x00dev->bcn = &queue[1 + rt2x00dev->ops->tx_queues];
 
 	/*
 	 * Initialize queue parameters.
@@ -280,7 +660,8 @@ int rt2x00queue_allocate(struct rt2x00_dev *rt2x00dev)
 	 * TX: qid = QID_AC_BE + index
 	 * TX: cw_min: 2^5 = 32.
 	 * TX: cw_max: 2^10 = 1024.
-	 * BCN & Atim: qid = QID_MGMT
+	 * BCN: qid = QID_BEACON
+	 * ATIM: qid = QID_ATIM
 	 */
 	rt2x00queue_init(rt2x00dev, rt2x00dev->rx, QID_RX);
 
@@ -288,9 +669,9 @@ int rt2x00queue_allocate(struct rt2x00_dev *rt2x00dev)
 	tx_queue_for_each(rt2x00dev, queue)
 		rt2x00queue_init(rt2x00dev, queue, qid++);
 
-	rt2x00queue_init(rt2x00dev, &rt2x00dev->bcn[0], QID_MGMT);
+	rt2x00queue_init(rt2x00dev, &rt2x00dev->bcn[0], QID_BEACON);
 	if (req_atim)
-		rt2x00queue_init(rt2x00dev, &rt2x00dev->bcn[1], QID_MGMT);
+		rt2x00queue_init(rt2x00dev, &rt2x00dev->bcn[1], QID_ATIM);
 
 	return 0;
 }
diff --git a/drivers/net/wireless/rt2x00/rt2x00queue.h b/drivers/net/wireless/rt2x00/rt2x00queue.h
index 7027c9f47d3f..8945945c892e 100644
--- a/drivers/net/wireless/rt2x00/rt2x00queue.h
+++ b/drivers/net/wireless/rt2x00/rt2x00queue.h
@@ -42,18 +42,32 @@
 /**
  * DOC: Number of entries per queue
  *
- * After research it was concluded that 12 entries in a RX and TX
- * queue would be sufficient. Although this is almost one third of
- * the amount the legacy driver allocated, the queues aren't getting
- * filled to the maximum even when working with the maximum rate.
+ * Under normal load without fragmentation 12 entries are sufficient
+ * without the queue being filled up to the maximum. When using fragmentation
+ * and the queue threshold code we need to add some additional margins to
+ * make sure the queue will never (or only under extreme load) fill up
+ * completely.
+ * Since we don't use preallocated DMA having a large number of queue entries
+ * will have only minimal impact on the memory requirements for the queue.
  */
-#define RX_ENTRIES	12
-#define TX_ENTRIES	12
+#define RX_ENTRIES	24
+#define TX_ENTRIES	24
 #define BEACON_ENTRIES	1
-#define ATIM_ENTRIES	1
+#define ATIM_ENTRIES	8
 
 /**
  * enum data_queue_qid: Queue identification
+ *
+ * @QID_AC_BE: AC BE queue
+ * @QID_AC_BK: AC BK queue
+ * @QID_AC_VI: AC VI queue
+ * @QID_AC_VO: AC VO queue
+ * @QID_HCCA: HCCA queue
+ * @QID_MGMT: MGMT queue (prio queue)
+ * @QID_RX: RX queue
+ * @QID_OTHER: None of the above (don't use, only present for completeness)
+ * @QID_BEACON: Beacon queue (value unspecified, don't send it to device)
+ * @QID_ATIM: Atim queue (value unspeficied, don't send it to device)
  */
 enum data_queue_qid {
 	QID_AC_BE = 0,
@@ -64,68 +78,55 @@ enum data_queue_qid {
 	QID_MGMT = 13,
 	QID_RX = 14,
 	QID_OTHER = 15,
-};
-
-/**
- * enum rt2x00_bcn_queue: Beacon queue index
- *
- * Start counting with a high offset, this because this enumeration
- * supplements &enum ieee80211_tx_queue and we should prevent value
- * conflicts.
- *
- * @RT2X00_BCN_QUEUE_BEACON: Beacon queue
- * @RT2X00_BCN_QUEUE_ATIM: Atim queue (sends frame after beacon)
- */
-enum rt2x00_bcn_queue {
-	RT2X00_BCN_QUEUE_BEACON = 100,
-	RT2X00_BCN_QUEUE_ATIM = 101,
+	QID_BEACON,
+	QID_ATIM,
 };
 
 /**
  * enum skb_frame_desc_flags: Flags for &struct skb_frame_desc
  *
- * @FRAME_DESC_DRIVER_GENERATED: Frame was generated inside driver
- *	and should not be reported back to mac80211 during txdone.
+ * @SKBDESC_DMA_MAPPED_RX: &skb_dma field has been mapped for RX
+ * @SKBDESC_DMA_MAPPED_TX: &skb_dma field has been mapped for TX
  */
 enum skb_frame_desc_flags {
-	FRAME_DESC_DRIVER_GENERATED = 1 << 0,
+	SKBDESC_DMA_MAPPED_RX = (1 << 0),
+	SKBDESC_DMA_MAPPED_TX = (1 << 1),
 };
 
 /**
  * struct skb_frame_desc: Descriptor information for the skb buffer
  *
- * This structure is placed over the skb->cb array, this means that
- * this structure should not exceed the size of that array (48 bytes).
+ * This structure is placed over the driver_data array, this means that
+ * this structure should not exceed the size of that array (40 bytes).
  *
  * @flags: Frame flags, see &enum skb_frame_desc_flags.
- * @frame_type: Frame type, see &enum rt2x00_dump_type.
- * @data: Pointer to data part of frame (Start of ieee80211 header).
+ * @desc_len: Length of the frame descriptor.
  * @desc: Pointer to descriptor part of the frame.
  *	Note that this pointer could point to something outside
  *	of the scope of the skb->data pointer.
- * @data_len: Length of the frame data.
- * @desc_len: Length of the frame descriptor.
-
+ * @skb_dma: (PCI-only) the DMA address associated with the sk buffer.
  * @entry: The entry to which this sk buffer belongs.
  */
 struct skb_frame_desc {
 	unsigned int flags;
 
-	unsigned int frame_type;
-
-	void *data;
+	unsigned int desc_len;
 	void *desc;
 
-	unsigned int data_len;
-	unsigned int desc_len;
+	dma_addr_t skb_dma;
 
 	struct queue_entry *entry;
 };
 
+/**
+ * get_skb_frame_desc - Obtain the rt2x00 frame descriptor from a sk_buff.
+ * @skb: &struct sk_buff from where we obtain the &struct skb_frame_desc
+ */
 static inline struct skb_frame_desc* get_skb_frame_desc(struct sk_buff *skb)
 {
-	BUILD_BUG_ON(sizeof(struct skb_frame_desc) > sizeof(skb->cb));
-	return (struct skb_frame_desc *)&skb->cb[0];
+	BUILD_BUG_ON(sizeof(struct skb_frame_desc) >
+		     IEEE80211_TX_INFO_DRIVER_DATA_SIZE);
+	return (struct skb_frame_desc *)&IEEE80211_SKB_CB(skb)->driver_data;
 }
 
 /**
@@ -145,6 +146,7 @@ enum rxdone_entry_desc_flags {
  *
  * Summary of information that has been read from the RX frame descriptor.
  *
+ * @timestamp: RX Timestamp
  * @signal: Signal of the received frame.
  * @rssi: RSSI of the received frame.
  * @size: Data size of the received frame.
@@ -153,6 +155,7 @@ enum rxdone_entry_desc_flags {
 
  */
 struct rxdone_entry_desc {
+	u64 timestamp;
 	int signal;
 	int rssi;
 	int size;
@@ -161,18 +164,32 @@ struct rxdone_entry_desc {
 };
 
 /**
+ * enum txdone_entry_desc_flags: Flags for &struct txdone_entry_desc
+ *
+ * @TXDONE_UNKNOWN: Hardware could not determine success of transmission.
+ * @TXDONE_SUCCESS: Frame was successfully send
+ * @TXDONE_FAILURE: Frame was not successfully send
+ * @TXDONE_EXCESSIVE_RETRY: In addition to &TXDONE_FAILURE, the
+ *	frame transmission failed due to excessive retries.
+ */
+enum txdone_entry_desc_flags {
+	TXDONE_UNKNOWN = 1 << 0,
+	TXDONE_SUCCESS = 1 << 1,
+	TXDONE_FAILURE = 1 << 2,
+	TXDONE_EXCESSIVE_RETRY = 1 << 3,
+};
+
+/**
  * struct txdone_entry_desc: TX done entry descriptor
  *
  * Summary of information that has been read from the TX frame descriptor
  * after the device is done with transmission.
  *
- * @control: Control structure which was used to transmit the frame.
- * @status: TX status (See &enum tx_status).
+ * @flags: TX done flags (See &enum txdone_entry_desc_flags).
  * @retry: Retry count.
  */
 struct txdone_entry_desc {
-	struct ieee80211_tx_control *control;
-	int status;
+	unsigned long flags;
 	int retry;
 };
 
@@ -180,19 +197,25 @@ struct txdone_entry_desc {
  * enum txentry_desc_flags: Status flags for TX entry descriptor
  *
  * @ENTRY_TXD_RTS_FRAME: This frame is a RTS frame.
+ * @ENTRY_TXD_CTS_FRAME: This frame is a CTS-to-self frame.
  * @ENTRY_TXD_OFDM_RATE: This frame is send out with an OFDM rate.
+ * @ENTRY_TXD_FIRST_FRAGMENT: This is the first frame.
  * @ENTRY_TXD_MORE_FRAG: This frame is followed by another fragment.
  * @ENTRY_TXD_REQ_TIMESTAMP: Require timestamp to be inserted.
  * @ENTRY_TXD_BURST: This frame belongs to the same burst event.
  * @ENTRY_TXD_ACK: An ACK is required for this frame.
+ * @ENTRY_TXD_RETRY_MODE: When set, the long retry count is used.
  */
 enum txentry_desc_flags {
 	ENTRY_TXD_RTS_FRAME,
+	ENTRY_TXD_CTS_FRAME,
 	ENTRY_TXD_OFDM_RATE,
+	ENTRY_TXD_FIRST_FRAGMENT,
 	ENTRY_TXD_MORE_FRAG,
 	ENTRY_TXD_REQ_TIMESTAMP,
 	ENTRY_TXD_BURST,
 	ENTRY_TXD_ACK,
+	ENTRY_TXD_RETRY_MODE,
 };
 
 /**
@@ -206,6 +229,7 @@ enum txentry_desc_flags {
  * @length_low: PLCP length low word.
  * @signal: PLCP signal.
  * @service: PLCP service.
+ * @retry_limit: Max number of retries.
  * @aifs: AIFS value.
  * @ifs: IFS value.
  * @cw_min: cwmin value.
@@ -221,10 +245,11 @@ struct txentry_desc {
 	u16 signal;
 	u16 service;
 
-	int aifs;
-	int ifs;
-	int cw_min;
-	int cw_max;
+	short retry_limit;
+	short aifs;
+	short ifs;
+	short cw_min;
+	short cw_max;
 };
 
 /**
@@ -239,12 +264,14 @@ struct txentry_desc {
  * @ENTRY_OWNER_DEVICE_CRYPTO: This entry is owned by the device for data
  *	encryption or decryption. The entry should only be touched after
  *	the device has signaled it is done with it.
+ * @ENTRY_DATA_PENDING: This entry contains a valid frame and is waiting
+ *	for the signal to start sending.
  */
-
 enum queue_entry_flags {
 	ENTRY_BCN_ASSIGNED,
 	ENTRY_OWNER_DEVICE_DATA,
 	ENTRY_OWNER_DEVICE_CRYPTO,
+	ENTRY_DATA_PENDING,
 };
 
 /**
@@ -302,6 +329,7 @@ enum queue_index {
  *	index corruption due to concurrency.
  * @count: Number of frames handled in the queue.
  * @limit: Maximum number of entries in the queue.
+ * @threshold: Minimum number of free entries before queue is kicked by force.
  * @length: Number of frames in queue.
  * @index: Index pointers to entry positions in the queue,
  *	use &enum queue_index to get a specific index field.
@@ -320,6 +348,7 @@ struct data_queue {
 	spinlock_t lock;
 	unsigned int count;
 	unsigned short limit;
+	unsigned short threshold;
 	unsigned short length;
 	unsigned short index[Q_INDEX_MAX];
 
@@ -369,7 +398,7 @@ struct data_queue_desc {
  * the end of the TX queue array.
  */
 #define tx_queue_end(__dev) \
-	&(__dev)->tx[(__dev)->hw->queues]
+	&(__dev)->tx[(__dev)->ops->tx_queues]
 
 /**
  * queue_loop - Loop through the queues within a specific range (HELPER MACRO).
@@ -444,6 +473,15 @@ static inline int rt2x00queue_available(struct data_queue *queue)
 }
 
 /**
+ * rt2x00queue_threshold - Check if the queue is below threshold
+ * @queue: Queue to check.
+ */
+static inline int rt2x00queue_threshold(struct data_queue *queue)
+{
+	return rt2x00queue_available(queue) < queue->threshold;
+}
+
+/**
  * rt2x00_desc_read - Read a word from the hardware descriptor.
  * @desc: Base descriptor address
  * @word: Word index from where the descriptor should be read.
diff --git a/drivers/net/wireless/rt2x00/rt2x00reg.h b/drivers/net/wireless/rt2x00/rt2x00reg.h
index 0325bed2fbf5..7e88ce5651b9 100644
--- a/drivers/net/wireless/rt2x00/rt2x00reg.h
+++ b/drivers/net/wireless/rt2x00/rt2x00reg.h
@@ -27,17 +27,6 @@
 #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 {
@@ -141,83 +130,107 @@ struct rt2x00_field32 {
 
 /*
  * Power of two check, this will check
- * if the mask that has been given contains
- * and contiguous set of bits.
+ * if the mask that has been given contains and contiguous set of bits.
+ * Note that we cannot use the is_power_of_2() function since this
+ * check must be done at compile-time.
  */
 #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))
 
+/*
+ * Macro's to find first set bit in a variable.
+ * These macro's behaves the same as the __ffs() function with
+ * the most important difference that this is done during
+ * compile-time rather then run-time.
+ */
+#define compile_ffs2(__x) \
+	__builtin_choose_expr(((__x) & 0x1), 0, 1)
+
+#define compile_ffs4(__x) \
+	__builtin_choose_expr(((__x) & 0x3), \
+			      (compile_ffs2((__x))), \
+			      (compile_ffs2((__x) >> 2) + 2))
+
+#define compile_ffs8(__x) \
+	__builtin_choose_expr(((__x) & 0xf), \
+			      (compile_ffs4((__x))), \
+			      (compile_ffs4((__x) >> 4) + 4))
+
+#define compile_ffs16(__x) \
+	__builtin_choose_expr(((__x) & 0xff), \
+			      (compile_ffs8((__x))), \
+			      (compile_ffs8((__x) >> 8) + 8))
+
+#define compile_ffs32(__x) \
+	__builtin_choose_expr(((__x) & 0xffff), \
+			      (compile_ffs16((__x))), \
+			      (compile_ffs16((__x) >> 16) + 16))
+
+/*
+ * This macro will check the requirements for the FIELD{8,16,32} macros
+ * The mask should be a constant non-zero contiguous set of bits which
+ * does not exceed the given typelimit.
+ */
+#define FIELD_CHECK(__mask, __type)			\
+	BUILD_BUG_ON(!__builtin_constant_p(__mask) ||	\
+		     !(__mask) ||			\
+		     !is_valid_mask(__mask) ||		\
+		     (__mask) != (__type)(__mask))	\
+
 #define FIELD8(__mask)				\
 ({						\
-	BUILD_BUG_ON(!(__mask) ||		\
-		     !is_valid_mask(__mask) ||	\
-		     (__mask) != (u8)(__mask));	\
+	FIELD_CHECK(__mask, u8);		\
 	(struct rt2x00_field8) {		\
-		__ffs(__mask), (__mask)		\
+		compile_ffs8(__mask), (__mask)	\
 	};					\
 })
 
 #define FIELD16(__mask)				\
 ({						\
-	BUILD_BUG_ON(!(__mask) ||		\
-		     !is_valid_mask(__mask) ||	\
-		     (__mask) != (u16)(__mask));\
+	FIELD_CHECK(__mask, u16);		\
 	(struct rt2x00_field16) {		\
-		__ffs(__mask), (__mask)		\
+		compile_ffs16(__mask), (__mask)	\
 	};					\
 })
 
 #define FIELD32(__mask)				\
 ({						\
-	BUILD_BUG_ON(!(__mask) ||		\
-		     !is_valid_mask(__mask) ||	\
-		     (__mask) != (u32)(__mask));\
+	FIELD_CHECK(__mask, u32);		\
 	(struct rt2x00_field32) {		\
-		__ffs(__mask), (__mask)		\
+		compile_ffs32(__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;
-}
+#define SET_FIELD(__reg, __type, __field, __value)\
+({						\
+	typecheck(__type, __field);		\
+	*(__reg) &= ~((__field).bit_mask);	\
+	*(__reg) |= ((__value) <<		\
+	    ((__field).bit_offset)) &		\
+	    ((__field).bit_mask);		\
+})
+
+#define GET_FIELD(__reg, __type, __field)	\
+({						\
+	typecheck(__type, __field);		\
+	((__reg) & ((__field).bit_mask)) >>	\
+	    ((__field).bit_offset);		\
+})
+
+#define rt2x00_set_field32(__reg, __field, __value) \
+	SET_FIELD(__reg, struct rt2x00_field32, __field, __value)
+#define rt2x00_get_field32(__reg, __field) \
+	GET_FIELD(__reg, struct rt2x00_field32, __field)
+
+#define rt2x00_set_field16(__reg, __field, __value) \
+	SET_FIELD(__reg, struct rt2x00_field16, __field, __value)
+#define rt2x00_get_field16(__reg, __field) \
+	GET_FIELD(__reg, struct rt2x00_field16, __field)
+
+#define rt2x00_set_field8(__reg, __field, __value) \
+	SET_FIELD(__reg, struct rt2x00_field8, __field, __value)
+#define rt2x00_get_field8(__reg, __field) \
+	GET_FIELD(__reg, struct rt2x00_field8, __field)
 
 #endif /* RT2X00REG_H */
diff --git a/drivers/net/wireless/rt2x00/rt2x00rfkill.c b/drivers/net/wireless/rt2x00/rt2x00rfkill.c
index fcef9885ab5e..04b29716d356 100644
--- a/drivers/net/wireless/rt2x00/rt2x00rfkill.c
+++ b/drivers/net/wireless/rt2x00/rt2x00rfkill.c
@@ -23,7 +23,6 @@
 	Abstract: rt2x00 rfkill routines.
  */
 
-#include <linux/input-polldev.h>
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/rfkill.h>
@@ -45,28 +44,51 @@ static int rt2x00rfkill_toggle_radio(void *data, enum rfkill_state state)
 	if (!test_bit(DEVICE_STARTED, &rt2x00dev->flags))
 		return 0;
 
-	if (state == RFKILL_STATE_ON) {
+	if (state == RFKILL_STATE_UNBLOCKED) {
 		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) {
+	} else if (state == RFKILL_STATE_SOFT_BLOCKED) {
 		INFO(rt2x00dev, "Hardware button pressed, disabling radio.\n");
 		__set_bit(DEVICE_DISABLED_RADIO_HW, &rt2x00dev->flags);
 		rt2x00lib_disable_radio(rt2x00dev);
+	} else {
+		WARNING(rt2x00dev, "Received unexpected rfkill state %d.\n",
+			state);
 	}
 
 	return retval;
 }
 
-static void rt2x00rfkill_poll(struct input_polled_dev *poll_dev)
+static int rt2x00rfkill_get_state(void *data, enum rfkill_state *state)
 {
-	struct rt2x00_dev *rt2x00dev = poll_dev->private;
-	int state = rt2x00dev->ops->lib->rfkill_poll(rt2x00dev);
+	struct rt2x00_dev *rt2x00dev = data;
 
-	if (rt2x00dev->rfkill->state != state) {
-		input_report_key(poll_dev->input, KEY_WLAN, 1);
-		input_report_key(poll_dev->input, KEY_WLAN, 0);
-	}
+	*state = rt2x00dev->rfkill->state;
+
+	return 0;
+}
+
+static void rt2x00rfkill_poll(struct work_struct *work)
+{
+	struct rt2x00_dev *rt2x00dev =
+	    container_of(work, struct rt2x00_dev, rfkill_work.work);
+	int state;
+
+	if (!test_bit(RFKILL_STATE_REGISTERED, &rt2x00dev->rfkill_state))
+		return;
+
+	/*
+	 * rfkill_poll reports 1 when the key has been pressed and the
+	 * radio should be blocked.
+	 */
+	state = !rt2x00dev->ops->lib->rfkill_poll(rt2x00dev) ?
+	    RFKILL_STATE_UNBLOCKED : RFKILL_STATE_SOFT_BLOCKED;
+
+	rfkill_force_state(rt2x00dev->rfkill, state);
+
+	queue_delayed_work(rt2x00dev->hw->workqueue,
+			   &rt2x00dev->rfkill_work, RFKILL_POLL_INTERVAL);
 }
 
 void rt2x00rfkill_register(struct rt2x00_dev *rt2x00dev)
@@ -80,12 +102,6 @@ void rt2x00rfkill_register(struct rt2x00_dev *rt2x00dev)
 		return;
 	}
 
-	if (input_register_polled_device(rt2x00dev->poll_dev)) {
-		ERROR(rt2x00dev, "Failed to register polled device.\n");
-		rfkill_unregister(rt2x00dev->rfkill);
-		return;
-	}
-
 	__set_bit(RFKILL_STATE_REGISTERED, &rt2x00dev->rfkill_state);
 
 	/*
@@ -93,7 +109,7 @@ void rt2x00rfkill_register(struct rt2x00_dev *rt2x00dev)
 	 * and correctly sends the signal to the rfkill layer about this
 	 * state.
 	 */
-	rt2x00rfkill_poll(rt2x00dev->poll_dev);
+	rt2x00rfkill_poll(&rt2x00dev->rfkill_work.work);
 }
 
 void rt2x00rfkill_unregister(struct rt2x00_dev *rt2x00dev)
@@ -102,38 +118,13 @@ void rt2x00rfkill_unregister(struct rt2x00_dev *rt2x00dev)
 	    !test_bit(RFKILL_STATE_REGISTERED, &rt2x00dev->rfkill_state))
 		return;
 
-	input_unregister_polled_device(rt2x00dev->poll_dev);
+	cancel_delayed_work_sync(&rt2x00dev->rfkill_work);
+
 	rfkill_unregister(rt2x00dev->rfkill);
 
 	__clear_bit(RFKILL_STATE_REGISTERED, &rt2x00dev->rfkill_state);
 }
 
-static struct input_polled_dev *
-rt2x00rfkill_allocate_polldev(struct rt2x00_dev *rt2x00dev)
-{
-	struct input_polled_dev *poll_dev;
-
-	poll_dev = input_allocate_polled_device();
-	if (!poll_dev)
-		return NULL;
-
-	poll_dev->private = rt2x00dev;
-	poll_dev->poll = rt2x00rfkill_poll;
-	poll_dev->poll_interval = RFKILL_POLL_INTERVAL;
-
-	poll_dev->input->name = rt2x00dev->ops->name;
-	poll_dev->input->phys = wiphy_name(rt2x00dev->hw->wiphy);
-	poll_dev->input->id.bustype = BUS_HOST;
-	poll_dev->input->id.vendor = 0x1814;
-	poll_dev->input->id.product = rt2x00dev->chip.rt;
-	poll_dev->input->id.version = rt2x00dev->chip.rev;
-	poll_dev->input->dev.parent = wiphy_dev(rt2x00dev->hw->wiphy);
-	poll_dev->input->evbit[0] = BIT(EV_KEY);
-	set_bit(KEY_WLAN, poll_dev->input->keybit);
-
-	return poll_dev;
-}
-
 void rt2x00rfkill_allocate(struct rt2x00_dev *rt2x00dev)
 {
 	if (!test_bit(CONFIG_SUPPORT_HW_BUTTON, &rt2x00dev->flags))
@@ -150,14 +141,9 @@ void rt2x00rfkill_allocate(struct rt2x00_dev *rt2x00dev)
 	rt2x00dev->rfkill->data = rt2x00dev;
 	rt2x00dev->rfkill->state = -1;
 	rt2x00dev->rfkill->toggle_radio = rt2x00rfkill_toggle_radio;
+	rt2x00dev->rfkill->get_state = rt2x00rfkill_get_state;
 
-	rt2x00dev->poll_dev = rt2x00rfkill_allocate_polldev(rt2x00dev);
-	if (!rt2x00dev->poll_dev) {
-		ERROR(rt2x00dev, "Failed to allocate polled device.\n");
-		rfkill_free(rt2x00dev->rfkill);
-		rt2x00dev->rfkill = NULL;
-		return;
-	}
+	INIT_DELAYED_WORK(&rt2x00dev->rfkill_work, rt2x00rfkill_poll);
 
 	return;
 }
@@ -168,32 +154,8 @@ void rt2x00rfkill_free(struct rt2x00_dev *rt2x00dev)
 	    !test_bit(RFKILL_STATE_ALLOCATED, &rt2x00dev->rfkill_state))
 		return;
 
-	input_free_polled_device(rt2x00dev->poll_dev);
-	rt2x00dev->poll_dev = NULL;
+	cancel_delayed_work_sync(&rt2x00dev->rfkill_work);
 
 	rfkill_free(rt2x00dev->rfkill);
 	rt2x00dev->rfkill = NULL;
 }
-
-void rt2x00rfkill_suspend(struct rt2x00_dev *rt2x00dev)
-{
-	if (!test_bit(CONFIG_SUPPORT_HW_BUTTON, &rt2x00dev->flags) ||
-	    !test_bit(RFKILL_STATE_ALLOCATED, &rt2x00dev->rfkill_state))
-		return;
-
-	input_free_polled_device(rt2x00dev->poll_dev);
-	rt2x00dev->poll_dev = NULL;
-}
-
-void rt2x00rfkill_resume(struct rt2x00_dev *rt2x00dev)
-{
-	if (!test_bit(CONFIG_SUPPORT_HW_BUTTON, &rt2x00dev->flags) ||
-	    !test_bit(RFKILL_STATE_ALLOCATED, &rt2x00dev->rfkill_state))
-		return;
-
-	rt2x00dev->poll_dev = rt2x00rfkill_allocate_polldev(rt2x00dev);
-	if (!rt2x00dev->poll_dev) {
-		ERROR(rt2x00dev, "Failed to allocate polled device.\n");
-		return;
-	}
-}
diff --git a/drivers/net/wireless/rt2x00/rt2x00usb.c b/drivers/net/wireless/rt2x00/rt2x00usb.c
index e5ceae805b57..83862e7f7aec 100644
--- a/drivers/net/wireless/rt2x00/rt2x00usb.c
+++ b/drivers/net/wireless/rt2x00/rt2x00usb.c
@@ -40,7 +40,7 @@ int rt2x00usb_vendor_request(struct rt2x00_dev *rt2x00dev,
 			     void *buffer, const u16 buffer_length,
 			     const int timeout)
 {
-	struct usb_device *usb_dev = rt2x00dev_usb_dev(rt2x00dev);
+	struct usb_device *usb_dev = to_usb_device_intf(rt2x00dev->dev);
 	int status;
 	unsigned int i;
 	unsigned int pipe =
@@ -129,17 +129,12 @@ static void rt2x00usb_interrupt_txdone(struct urb *urb)
 {
 	struct queue_entry *entry = (struct queue_entry *)urb->context;
 	struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
-	struct queue_entry_priv_usb_tx *priv_tx = entry->priv_data;
 	struct txdone_entry_desc txdesc;
-	__le32 *txd = (__le32 *)entry->skb->data;
-	u32 word;
 
 	if (!test_bit(DEVICE_ENABLED_RADIO, &rt2x00dev->flags) ||
-	    !__test_and_clear_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags))
+	    !test_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags))
 		return;
 
-	rt2x00_desc_read(txd, 0, &word);
-
 	/*
 	 * Remove the descriptor data from the buffer.
 	 */
@@ -147,128 +142,116 @@ static void rt2x00usb_interrupt_txdone(struct urb *urb)
 
 	/*
 	 * Obtain the status about this packet.
+	 * Note that when the status is 0 it does not mean the
+	 * frame was send out correctly. It only means the frame
+	 * was succesfully pushed to the hardware, we have no
+	 * way to determine the transmission status right now.
+	 * (Only indirectly by looking at the failed TX counters
+	 * in the register).
 	 */
-	txdesc.status = !urb->status ? TX_SUCCESS : TX_FAIL_RETRY;
+	if (!urb->status)
+		__set_bit(TXDONE_UNKNOWN, &txdesc.flags);
+	else
+		__set_bit(TXDONE_FAILURE, &txdesc.flags);
 	txdesc.retry = 0;
-	txdesc.control = &priv_tx->control;
 
 	rt2x00lib_txdone(entry, &txdesc);
-
-	/*
-	 * Make this entry available for reuse.
-	 */
-	entry->flags = 0;
-	rt2x00queue_index_inc(entry->queue, Q_INDEX_DONE);
-
-	/*
-	 * If the data queue 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 (!rt2x00queue_full(entry->queue))
-		ieee80211_wake_queue(rt2x00dev->hw, priv_tx->control.queue);
 }
 
-int rt2x00usb_write_tx_data(struct rt2x00_dev *rt2x00dev,
-			    struct data_queue *queue, struct sk_buff *skb,
-			    struct ieee80211_tx_control *control)
+int rt2x00usb_write_tx_data(struct queue_entry *entry)
 {
-	struct usb_device *usb_dev = rt2x00dev_usb_dev(rt2x00dev);
-	struct queue_entry *entry = rt2x00queue_get_entry(queue, Q_INDEX);
-	struct queue_entry_priv_usb_tx *priv_tx = entry->priv_data;
+	struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
+	struct usb_device *usb_dev = to_usb_device_intf(rt2x00dev->dev);
+	struct queue_entry_priv_usb *entry_priv = entry->priv_data;
 	struct skb_frame_desc *skbdesc;
 	u32 length;
 
-	if (rt2x00queue_full(queue))
-		return -EINVAL;
-
-	if (test_bit(ENTRY_OWNER_DEVICE_DATA, &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);
-		return -EINVAL;
-	}
-
 	/*
 	 * Add the descriptor in front of the skb.
 	 */
-	skb_push(skb, queue->desc_size);
-	memset(skb->data, 0, queue->desc_size);
+	skb_push(entry->skb, entry->queue->desc_size);
+	memset(entry->skb->data, 0, entry->queue->desc_size);
 
 	/*
 	 * Fill in skb descriptor
 	 */
-	skbdesc = get_skb_frame_desc(skb);
-	skbdesc->data = skb->data + queue->desc_size;
-	skbdesc->data_len = skb->len - queue->desc_size;
-	skbdesc->desc = skb->data;
-	skbdesc->desc_len = queue->desc_size;
-	skbdesc->entry = entry;
-
-	memcpy(&priv_tx->control, control, sizeof(priv_tx->control));
-	rt2x00lib_write_tx_desc(rt2x00dev, skb, control);
+	skbdesc = get_skb_frame_desc(entry->skb);
+	skbdesc->desc = entry->skb->data;
+	skbdesc->desc_len = entry->queue->desc_size;
 
 	/*
 	 * 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, skb);
-
-	/*
-	 * Initialize URB and send the frame to the device.
-	 */
-	__set_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags);
-	usb_fill_bulk_urb(priv_tx->urb, usb_dev, usb_sndbulkpipe(usb_dev, 1),
-			  skb->data, length, rt2x00usb_interrupt_txdone, entry);
-	usb_submit_urb(priv_tx->urb, GFP_ATOMIC);
+	length = rt2x00dev->ops->lib->get_tx_data_len(rt2x00dev, entry->skb);
 
-	rt2x00queue_index_inc(queue, Q_INDEX);
+	usb_fill_bulk_urb(entry_priv->urb, usb_dev,
+			  usb_sndbulkpipe(usb_dev, 1),
+			  entry->skb->data, length,
+			  rt2x00usb_interrupt_txdone, entry);
 
 	return 0;
 }
 EXPORT_SYMBOL_GPL(rt2x00usb_write_tx_data);
 
-/*
- * RX data handlers.
- */
-static struct sk_buff* rt2x00usb_alloc_rxskb(struct data_queue *queue)
+static inline void rt2x00usb_kick_tx_entry(struct queue_entry *entry)
 {
-	struct sk_buff *skb;
-	unsigned int frame_size;
+	struct queue_entry_priv_usb *entry_priv = entry->priv_data;
+
+	if (__test_and_clear_bit(ENTRY_DATA_PENDING, &entry->flags))
+		usb_submit_urb(entry_priv->urb, GFP_ATOMIC);
+}
+
+void rt2x00usb_kick_tx_queue(struct rt2x00_dev *rt2x00dev,
+			     const enum data_queue_qid qid)
+{
+	struct data_queue *queue = rt2x00queue_get_queue(rt2x00dev, qid);
+	unsigned long irqflags;
+	unsigned int index;
+	unsigned int index_done;
+	unsigned int i;
 
 	/*
-	 * As alignment we use 2 and not NET_IP_ALIGN because we need
-	 * to be sure we have 2 bytes room in the head. (NET_IP_ALIGN
-	 * can be 0 on some hardware). We use these 2 bytes for frame
-	 * alignment later, we assume that the chance that
-	 * header_size % 4 == 2 is bigger then header_size % 2 == 0
-	 * and thus optimize alignment by reserving the 2 bytes in
-	 * advance.
+	 * Only protect the range we are going to loop over,
+	 * if during our loop a extra entry is set to pending
+	 * it should not be kicked during this run, since it
+	 * is part of another TX operation.
 	 */
-	frame_size = queue->data_size + queue->desc_size;
-	skb = dev_alloc_skb(queue->desc_size + frame_size + 2);
-	if (!skb)
-		return NULL;
-
-	skb_reserve(skb, queue->desc_size + 2);
-	skb_put(skb, frame_size);
+	spin_lock_irqsave(&queue->lock, irqflags);
+	index = queue->index[Q_INDEX];
+	index_done = queue->index[Q_INDEX_DONE];
+	spin_unlock_irqrestore(&queue->lock, irqflags);
 
-	return skb;
+	/*
+	 * Start from the TX done pointer, this guarentees that we will
+	 * send out all frames in the correct order.
+	 */
+	if (index_done < index) {
+		for (i = index_done; i < index; i++)
+			rt2x00usb_kick_tx_entry(&queue->entries[i]);
+	} else {
+		for (i = index_done; i < queue->limit; i++)
+			rt2x00usb_kick_tx_entry(&queue->entries[i]);
+
+		for (i = 0; i < index; i++)
+			rt2x00usb_kick_tx_entry(&queue->entries[i]);
+	}
 }
+EXPORT_SYMBOL_GPL(rt2x00usb_kick_tx_queue);
 
+/*
+ * RX data handlers.
+ */
 static void rt2x00usb_interrupt_rxdone(struct urb *urb)
 {
 	struct queue_entry *entry = (struct queue_entry *)urb->context;
 	struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
-	struct sk_buff *skb;
-	struct skb_frame_desc *skbdesc;
-	struct rxdone_entry_desc rxdesc;
-	int header_size;
+	struct skb_frame_desc *skbdesc = get_skb_frame_desc(entry->skb);
+	u8 rxd[32];
 
 	if (!test_bit(DEVICE_ENABLED_RADIO, &rt2x00dev->flags) ||
-	    !test_and_clear_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags))
+	    !test_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags))
 		return;
 
 	/*
@@ -276,61 +259,22 @@ static void rt2x00usb_interrupt_rxdone(struct urb *urb)
 	 * to be actually valid, or if the urb is signaling
 	 * a problem.
 	 */
-	if (urb->actual_length < entry->queue->desc_size || urb->status)
-		goto skip_entry;
-
-	/*
-	 * Fill in skb descriptor
-	 */
-	skbdesc = get_skb_frame_desc(entry->skb);
-	memset(skbdesc, 0, sizeof(*skbdesc));
-	skbdesc->entry = entry;
-
-	memset(&rxdesc, 0, sizeof(rxdesc));
-	rt2x00dev->ops->lib->fill_rxdone(entry, &rxdesc);
-
-	/*
-	 * The data behind the ieee80211 header must be
-	 * aligned on a 4 byte boundary.
-	 */
-	header_size = ieee80211_get_hdrlen_from_skb(entry->skb);
-	if (header_size % 4 == 0) {
-		skb_push(entry->skb, 2);
-		memmove(entry->skb->data, entry->skb->data + 2,
-			entry->skb->len - 2);
-		skbdesc->data = entry->skb->data;
-		skb_trim(entry->skb,entry->skb->len - 2);
+	if (urb->actual_length < entry->queue->desc_size || urb->status) {
+		__set_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags);
+		usb_submit_urb(urb, GFP_ATOMIC);
+		return;
 	}
 
 	/*
-	 * 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.
+	 * Fill in desc fields of the skb descriptor
 	 */
-	skb = rt2x00usb_alloc_rxskb(entry->queue);
-	if (!skb)
-		goto skip_entry;
+	skbdesc->desc = rxd;
+	skbdesc->desc_len = entry->queue->desc_size;
 
 	/*
 	 * Send the frame to rt2x00lib for further processing.
 	 */
-	rt2x00lib_rxdone(entry, &rxdesc);
-
-	/*
-	 * 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, &entry->queue->rt2x00dev->flags)) {
-		__set_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags);
-		usb_submit_urb(urb, GFP_ATOMIC);
-	}
-
-	rt2x00queue_index_inc(entry->queue, Q_INDEX);
+	rt2x00lib_rxdone(rt2x00dev, entry);
 }
 
 /*
@@ -338,27 +282,21 @@ skip_entry:
  */
 void rt2x00usb_disable_radio(struct rt2x00_dev *rt2x00dev)
 {
-	struct queue_entry_priv_usb_rx *priv_rx;
-	struct queue_entry_priv_usb_tx *priv_tx;
-	struct queue_entry_priv_usb_bcn *priv_bcn;
+	struct queue_entry_priv_usb *entry_priv;
+	struct queue_entry_priv_usb_bcn *bcn_priv;
 	struct data_queue *queue;
 	unsigned int i;
 
-	rt2x00usb_vendor_request_sw(rt2x00dev, USB_RX_CONTROL, 0x0000, 0x0000,
+	rt2x00usb_vendor_request_sw(rt2x00dev, USB_RX_CONTROL, 0, 0,
 				    REGISTER_TIMEOUT);
 
 	/*
 	 * Cancel all queues.
 	 */
-	for (i = 0; i < rt2x00dev->rx->limit; i++) {
-		priv_rx = rt2x00dev->rx->entries[i].priv_data;
-		usb_kill_urb(priv_rx->urb);
-	}
-
-	tx_queue_for_each(rt2x00dev, queue) {
+	queue_for_each(rt2x00dev, queue) {
 		for (i = 0; i < queue->limit; i++) {
-			priv_tx = queue->entries[i].priv_data;
-			usb_kill_urb(priv_tx->urb);
+			entry_priv = queue->entries[i].priv_data;
+			usb_kill_urb(entry_priv->urb);
 		}
 	}
 
@@ -369,19 +307,9 @@ void rt2x00usb_disable_radio(struct rt2x00_dev *rt2x00dev)
 		return;
 
 	for (i = 0; i < rt2x00dev->bcn->limit; i++) {
-		priv_bcn = rt2x00dev->bcn->entries[i].priv_data;
-		usb_kill_urb(priv_bcn->urb);
-
-		if (priv_bcn->guardian_urb)
-			usb_kill_urb(priv_bcn->guardian_urb);
-	}
-
-	if (!test_bit(DRIVER_REQUIRE_ATIM_QUEUE, &rt2x00dev->flags))
-		return;
-
-	for (i = 0; i < rt2x00dev->bcn[1].limit; i++) {
-		priv_tx = rt2x00dev->bcn[1].entries[i].priv_data;
-		usb_kill_urb(priv_tx->urb);
+		bcn_priv = rt2x00dev->bcn->entries[i].priv_data;
+		if (bcn_priv->guardian_urb)
+			usb_kill_urb(bcn_priv->guardian_urb);
 	}
 }
 EXPORT_SYMBOL_GPL(rt2x00usb_disable_radio);
@@ -392,16 +320,16 @@ EXPORT_SYMBOL_GPL(rt2x00usb_disable_radio);
 void rt2x00usb_init_rxentry(struct rt2x00_dev *rt2x00dev,
 			    struct queue_entry *entry)
 {
-	struct usb_device *usb_dev = rt2x00dev_usb_dev(rt2x00dev);
-	struct queue_entry_priv_usb_rx *priv_rx = entry->priv_data;
+	struct usb_device *usb_dev = to_usb_device_intf(rt2x00dev->dev);
+	struct queue_entry_priv_usb *entry_priv = entry->priv_data;
 
-	usb_fill_bulk_urb(priv_rx->urb, usb_dev,
+	usb_fill_bulk_urb(entry_priv->urb, usb_dev,
 			  usb_rcvbulkpipe(usb_dev, 1),
 			  entry->skb->data, entry->skb->len,
 			  rt2x00usb_interrupt_rxdone, entry);
 
 	__set_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags);
-	usb_submit_urb(priv_rx->urb, GFP_ATOMIC);
+	usb_submit_urb(entry_priv->urb, GFP_ATOMIC);
 }
 EXPORT_SYMBOL_GPL(rt2x00usb_init_rxentry);
 
@@ -415,38 +343,31 @@ EXPORT_SYMBOL_GPL(rt2x00usb_init_txentry);
 static int rt2x00usb_alloc_urb(struct rt2x00_dev *rt2x00dev,
 			       struct data_queue *queue)
 {
-	struct queue_entry_priv_usb_rx *priv_rx;
-	struct queue_entry_priv_usb_tx *priv_tx;
-	struct queue_entry_priv_usb_bcn *priv_bcn;
-	struct urb *urb;
-	unsigned int guardian =
-	    test_bit(DRIVER_REQUIRE_BEACON_GUARD, &rt2x00dev->flags);
+	struct queue_entry_priv_usb *entry_priv;
+	struct queue_entry_priv_usb_bcn *bcn_priv;
 	unsigned int i;
 
+	for (i = 0; i < queue->limit; i++) {
+		entry_priv = queue->entries[i].priv_data;
+		entry_priv->urb = usb_alloc_urb(0, GFP_KERNEL);
+		if (!entry_priv->urb)
+			return -ENOMEM;
+	}
+
 	/*
-	 * Allocate the URB's
+	 * If this is not the beacon queue or
+	 * no guardian byte was required for the beacon,
+	 * then we are done.
 	 */
+	if (rt2x00dev->bcn != queue ||
+	    !test_bit(DRIVER_REQUIRE_BEACON_GUARD, &rt2x00dev->flags))
+		return 0;
+
 	for (i = 0; i < queue->limit; i++) {
-		urb = usb_alloc_urb(0, GFP_KERNEL);
-		if (!urb)
+		bcn_priv = queue->entries[i].priv_data;
+		bcn_priv->guardian_urb = usb_alloc_urb(0, GFP_KERNEL);
+		if (!bcn_priv->guardian_urb)
 			return -ENOMEM;
-
-		if (queue->qid == QID_RX) {
-			priv_rx = queue->entries[i].priv_data;
-			priv_rx->urb = urb;
-		} else if (queue->qid == QID_MGMT && guardian) {
-			priv_bcn = queue->entries[i].priv_data;
-			priv_bcn->urb = urb;
-
-			urb = usb_alloc_urb(0, GFP_KERNEL);
-			if (!urb)
-				return -ENOMEM;
-
-			priv_bcn->guardian_urb = urb;
-		} else {
-			priv_tx = queue->entries[i].priv_data;
-			priv_tx->urb = urb;
-		}
 	}
 
 	return 0;
@@ -455,47 +376,39 @@ static int rt2x00usb_alloc_urb(struct rt2x00_dev *rt2x00dev,
 static void rt2x00usb_free_urb(struct rt2x00_dev *rt2x00dev,
 			       struct data_queue *queue)
 {
-	struct queue_entry_priv_usb_rx *priv_rx;
-	struct queue_entry_priv_usb_tx *priv_tx;
-	struct queue_entry_priv_usb_bcn *priv_bcn;
-	struct urb *urb;
-	unsigned int guardian =
-	    test_bit(DRIVER_REQUIRE_BEACON_GUARD, &rt2x00dev->flags);
+	struct queue_entry_priv_usb *entry_priv;
+	struct queue_entry_priv_usb_bcn *bcn_priv;
 	unsigned int i;
 
 	if (!queue->entries)
 		return;
 
 	for (i = 0; i < queue->limit; i++) {
-		if (queue->qid == QID_RX) {
-			priv_rx = queue->entries[i].priv_data;
-			urb = priv_rx->urb;
-		} else if (queue->qid == QID_MGMT && guardian) {
-			priv_bcn = queue->entries[i].priv_data;
-
-			usb_kill_urb(priv_bcn->guardian_urb);
-			usb_free_urb(priv_bcn->guardian_urb);
-
-			urb = priv_bcn->urb;
-		} else {
-			priv_tx = queue->entries[i].priv_data;
-			urb = priv_tx->urb;
-		}
+		entry_priv = queue->entries[i].priv_data;
+		usb_kill_urb(entry_priv->urb);
+		usb_free_urb(entry_priv->urb);
+	}
 
-		usb_kill_urb(urb);
-		usb_free_urb(urb);
-		if (queue->entries[i].skb)
-			kfree_skb(queue->entries[i].skb);
+	/*
+	 * If this is not the beacon queue or
+	 * no guardian byte was required for the beacon,
+	 * then we are done.
+	 */
+	if (rt2x00dev->bcn != queue ||
+	    !test_bit(DRIVER_REQUIRE_BEACON_GUARD, &rt2x00dev->flags))
+		return;
+
+	for (i = 0; i < queue->limit; i++) {
+		bcn_priv = queue->entries[i].priv_data;
+		usb_kill_urb(bcn_priv->guardian_urb);
+		usb_free_urb(bcn_priv->guardian_urb);
 	}
 }
 
 int rt2x00usb_initialize(struct rt2x00_dev *rt2x00dev)
 {
 	struct data_queue *queue;
-	struct sk_buff *skb;
-	unsigned int entry_size;
-	unsigned int i;
-	int uninitialized_var(status);
+	int status;
 
 	/*
 	 * Allocate DMA
@@ -506,18 +419,6 @@ int rt2x00usb_initialize(struct rt2x00_dev *rt2x00dev)
 			goto exit;
 	}
 
-	/*
-	 * For the RX queue, skb's should be allocated.
-	 */
-	entry_size = rt2x00dev->rx->data_size + rt2x00dev->rx->desc_size;
-	for (i = 0; i < rt2x00dev->rx->limit; i++) {
-		skb = rt2x00usb_alloc_rxskb(rt2x00dev->rx);
-		if (!skb)
-			goto exit;
-
-		rt2x00dev->rx->entries[i].skb = skb;
-	}
-
 	return 0;
 
 exit:
@@ -596,7 +497,7 @@ int rt2x00usb_probe(struct usb_interface *usb_intf,
 	usb_set_intfdata(usb_intf, hw);
 
 	rt2x00dev = hw->priv;
-	rt2x00dev->dev = usb_intf;
+	rt2x00dev->dev = &usb_intf->dev;
 	rt2x00dev->ops = ops;
 	rt2x00dev->hw = hw;
 	mutex_init(&rt2x00dev->usb_cache_mutex);
diff --git a/drivers/net/wireless/rt2x00/rt2x00usb.h b/drivers/net/wireless/rt2x00/rt2x00usb.h
index 11e55180cbaf..aad794adf52c 100644
--- a/drivers/net/wireless/rt2x00/rt2x00usb.h
+++ b/drivers/net/wireless/rt2x00/rt2x00usb.h
@@ -26,6 +26,12 @@
 #ifndef RT2X00USB_H
 #define RT2X00USB_H
 
+#define to_usb_device_intf(d) \
+({ \
+	struct usb_interface *intf = to_usb_interface(d); \
+	interface_to_usbdev(intf); \
+})
+
 /*
  * This variable should be used with the
  * usb_driver structure initialization.
@@ -47,6 +53,20 @@
 #define REGISTER_TIMEOUT		500
 #define REGISTER_TIMEOUT_FIRMWARE	1000
 
+/**
+ * REGISTER_TIMEOUT16 - Determine the timeout for 16bit register access
+ * @__datalen: Data length
+ */
+#define REGISTER_TIMEOUT16(__datalen)	\
+	( REGISTER_TIMEOUT * ((__datalen) / sizeof(u16)) )
+
+/**
+ * REGISTER_TIMEOUT32 - Determine the timeout for 32bit register access
+ * @__datalen: Data length
+ */
+#define REGISTER_TIMEOUT32(__datalen)	\
+	( REGISTER_TIMEOUT * ((__datalen) / sizeof(u32)) )
+
 /*
  * Cache size
  */
@@ -185,13 +205,12 @@ static inline int rt2x00usb_vendor_request_sw(struct rt2x00_dev *rt2x00dev,
  * kmalloc for correct handling inside the kernel USB layer.
  */
 static inline int rt2x00usb_eeprom_read(struct rt2x00_dev *rt2x00dev,
-					__le16 *eeprom, const u16 lenght)
+					__le16 *eeprom, const u16 length)
 {
-	int timeout = REGISTER_TIMEOUT * (lenght / sizeof(u16));
-
 	return rt2x00usb_vendor_request(rt2x00dev, USB_EEPROM_READ,
 					USB_VENDOR_REQUEST_IN, 0, 0,
-					eeprom, lenght, timeout);
+					eeprom, length,
+					REGISTER_TIMEOUT16(length));
 }
 
 /*
@@ -199,55 +218,53 @@ static inline int rt2x00usb_eeprom_read(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_queue *queue, struct sk_buff *skb,
-			    struct ieee80211_tx_control *control);
-
 /**
- * struct queue_entry_priv_usb_rx: Per RX entry USB specific information
+ * rt2x00usb_write_tx_data - Initialize URB for TX operation
+ * @entry: The entry where the frame is located
  *
- * @urb: Urb structure used for device communication.
+ * This function will initialize the URB and skb descriptor
+ * to prepare the entry for the actual TX operation.
  */
-struct queue_entry_priv_usb_rx {
-	struct urb *urb;
-};
+int rt2x00usb_write_tx_data(struct queue_entry *entry);
 
 /**
- * struct queue_entry_priv_usb_tx: Per TX entry USB specific information
+ * struct queue_entry_priv_usb: Per entry USB specific information
  *
  * @urb: Urb structure used for device communication.
- * @control: mac80211 control structure used to transmit data.
  */
-struct queue_entry_priv_usb_tx {
+struct queue_entry_priv_usb {
 	struct urb *urb;
-
-	struct ieee80211_tx_control control;
 };
 
 /**
- * struct queue_entry_priv_usb_tx: Per TX entry USB specific information
+ * struct queue_entry_priv_usb_bcn: Per TX entry USB specific information
  *
- * The first section should match &struct queue_entry_priv_usb_tx exactly.
+ * The first section should match &struct queue_entry_priv_usb exactly.
  * rt2500usb can use this structure to send a guardian byte when working
  * with beacons.
  *
  * @urb: Urb structure used for device communication.
- * @control: mac80211 control structure used to transmit data.
  * @guardian_data: Set to 0, used for sending the guardian data.
  * @guardian_urb: Urb structure used to send the guardian data.
  */
 struct queue_entry_priv_usb_bcn {
 	struct urb *urb;
 
-	struct ieee80211_tx_control control;
-
 	unsigned int guardian_data;
 	struct urb *guardian_urb;
 };
 
+/**
+ * rt2x00usb_kick_tx_queue - Kick data queue
+ * @rt2x00dev: Pointer to &struct rt2x00_dev
+ * @qid: Data queue to kick
+ *
+ * This will walk through all entries of the queue and push all pending
+ * frames to the hardware as a single burst.
+ */
+void rt2x00usb_kick_tx_queue(struct rt2x00_dev *rt2x00dev,
+			     const enum data_queue_qid qid);
+
 /*
  * Device initialization handlers.
  */
diff --git a/drivers/net/wireless/rt2x00/rt61pci.c b/drivers/net/wireless/rt2x00/rt61pci.c
index c3afb5cbe807..f7c1f92c1448 100644
--- a/drivers/net/wireless/rt2x00/rt61pci.c
+++ b/drivers/net/wireless/rt2x00/rt61pci.c
@@ -330,6 +330,17 @@ static int rt61pci_blink_set(struct led_classdev *led_cdev,
 
 	return 0;
 }
+
+static void rt61pci_init_led(struct rt2x00_dev *rt2x00dev,
+			     struct rt2x00_led *led,
+			     enum led_type type)
+{
+	led->rt2x00dev = rt2x00dev;
+	led->type = type;
+	led->led_dev.brightness_set = rt61pci_brightness_set;
+	led->led_dev.blink_set = rt61pci_blink_set;
+	led->flags = LED_INITIALIZED;
+}
 #endif /* CONFIG_RT61PCI_LEDS */
 
 /*
@@ -915,7 +926,7 @@ static char *rt61pci_get_firmware_name(struct rt2x00_dev *rt2x00dev)
 	return fw_name;
 }
 
-static u16 rt61pci_get_firmware_crc(void *data, const size_t len)
+static u16 rt61pci_get_firmware_crc(const void *data, const size_t len)
 {
 	u16 crc;
 
@@ -932,7 +943,7 @@ static u16 rt61pci_get_firmware_crc(void *data, const size_t len)
 	return crc;
 }
 
-static int rt61pci_load_firmware(struct rt2x00_dev *rt2x00dev, void *data,
+static int rt61pci_load_firmware(struct rt2x00_dev *rt2x00dev, const void *data,
 				 const size_t len)
 {
 	int i;
@@ -1018,49 +1029,35 @@ static int rt61pci_load_firmware(struct rt2x00_dev *rt2x00dev, void *data,
 static void rt61pci_init_rxentry(struct rt2x00_dev *rt2x00dev,
 				 struct queue_entry *entry)
 {
-	struct queue_entry_priv_pci_rx *priv_rx = entry->priv_data;
+	struct queue_entry_priv_pci *entry_priv = entry->priv_data;
+	struct skb_frame_desc *skbdesc = get_skb_frame_desc(entry->skb);
 	u32 word;
 
-	rt2x00_desc_read(priv_rx->desc, 5, &word);
+	rt2x00_desc_read(entry_priv->desc, 5, &word);
 	rt2x00_set_field32(&word, RXD_W5_BUFFER_PHYSICAL_ADDRESS,
-			   priv_rx->data_dma);
-	rt2x00_desc_write(priv_rx->desc, 5, word);
+			   skbdesc->skb_dma);
+	rt2x00_desc_write(entry_priv->desc, 5, word);
 
-	rt2x00_desc_read(priv_rx->desc, 0, &word);
+	rt2x00_desc_read(entry_priv->desc, 0, &word);
 	rt2x00_set_field32(&word, RXD_W0_OWNER_NIC, 1);
-	rt2x00_desc_write(priv_rx->desc, 0, word);
+	rt2x00_desc_write(entry_priv->desc, 0, word);
 }
 
 static void rt61pci_init_txentry(struct rt2x00_dev *rt2x00dev,
 				 struct queue_entry *entry)
 {
-	struct queue_entry_priv_pci_tx *priv_tx = entry->priv_data;
+	struct queue_entry_priv_pci *entry_priv = entry->priv_data;
 	u32 word;
 
-	rt2x00_desc_read(priv_tx->desc, 1, &word);
-	rt2x00_set_field32(&word, TXD_W1_BUFFER_COUNT, 1);
-	rt2x00_desc_write(priv_tx->desc, 1, word);
-
-	rt2x00_desc_read(priv_tx->desc, 5, &word);
-	rt2x00_set_field32(&word, TXD_W5_PID_TYPE, entry->queue->qid);
-	rt2x00_set_field32(&word, TXD_W5_PID_SUBTYPE, entry->entry_idx);
-	rt2x00_desc_write(priv_tx->desc, 5, word);
-
-	rt2x00_desc_read(priv_tx->desc, 6, &word);
-	rt2x00_set_field32(&word, TXD_W6_BUFFER_PHYSICAL_ADDRESS,
-			   priv_tx->data_dma);
-	rt2x00_desc_write(priv_tx->desc, 6, word);
-
-	rt2x00_desc_read(priv_tx->desc, 0, &word);
+	rt2x00_desc_read(entry_priv->desc, 0, &word);
 	rt2x00_set_field32(&word, TXD_W0_VALID, 0);
 	rt2x00_set_field32(&word, TXD_W0_OWNER_NIC, 0);
-	rt2x00_desc_write(priv_tx->desc, 0, word);
+	rt2x00_desc_write(entry_priv->desc, 0, word);
 }
 
 static int rt61pci_init_queues(struct rt2x00_dev *rt2x00dev)
 {
-	struct queue_entry_priv_pci_rx *priv_rx;
-	struct queue_entry_priv_pci_tx *priv_tx;
+	struct queue_entry_priv_pci *entry_priv;
 	u32 reg;
 
 	/*
@@ -1082,28 +1079,28 @@ static int rt61pci_init_queues(struct rt2x00_dev *rt2x00dev)
 			   rt2x00dev->tx[0].desc_size / 4);
 	rt2x00pci_register_write(rt2x00dev, TX_RING_CSR1, reg);
 
-	priv_tx = rt2x00dev->tx[0].entries[0].priv_data;
+	entry_priv = rt2x00dev->tx[0].entries[0].priv_data;
 	rt2x00pci_register_read(rt2x00dev, AC0_BASE_CSR, &reg);
 	rt2x00_set_field32(&reg, AC0_BASE_CSR_RING_REGISTER,
-			   priv_tx->desc_dma);
+			   entry_priv->desc_dma);
 	rt2x00pci_register_write(rt2x00dev, AC0_BASE_CSR, reg);
 
-	priv_tx = rt2x00dev->tx[1].entries[0].priv_data;
+	entry_priv = rt2x00dev->tx[1].entries[0].priv_data;
 	rt2x00pci_register_read(rt2x00dev, AC1_BASE_CSR, &reg);
 	rt2x00_set_field32(&reg, AC1_BASE_CSR_RING_REGISTER,
-			   priv_tx->desc_dma);
+			   entry_priv->desc_dma);
 	rt2x00pci_register_write(rt2x00dev, AC1_BASE_CSR, reg);
 
-	priv_tx = rt2x00dev->tx[2].entries[0].priv_data;
+	entry_priv = rt2x00dev->tx[2].entries[0].priv_data;
 	rt2x00pci_register_read(rt2x00dev, AC2_BASE_CSR, &reg);
 	rt2x00_set_field32(&reg, AC2_BASE_CSR_RING_REGISTER,
-			   priv_tx->desc_dma);
+			   entry_priv->desc_dma);
 	rt2x00pci_register_write(rt2x00dev, AC2_BASE_CSR, reg);
 
-	priv_tx = rt2x00dev->tx[3].entries[0].priv_data;
+	entry_priv = rt2x00dev->tx[3].entries[0].priv_data;
 	rt2x00pci_register_read(rt2x00dev, AC3_BASE_CSR, &reg);
 	rt2x00_set_field32(&reg, AC3_BASE_CSR_RING_REGISTER,
-			   priv_tx->desc_dma);
+			   entry_priv->desc_dma);
 	rt2x00pci_register_write(rt2x00dev, AC3_BASE_CSR, reg);
 
 	rt2x00pci_register_read(rt2x00dev, RX_RING_CSR, &reg);
@@ -1113,10 +1110,10 @@ static int rt61pci_init_queues(struct rt2x00_dev *rt2x00dev)
 	rt2x00_set_field32(&reg, RX_RING_CSR_RXD_WRITEBACK_SIZE, 4);
 	rt2x00pci_register_write(rt2x00dev, RX_RING_CSR, reg);
 
-	priv_rx = rt2x00dev->rx->entries[0].priv_data;
+	entry_priv = rt2x00dev->rx->entries[0].priv_data;
 	rt2x00pci_register_read(rt2x00dev, RX_BASE_CSR, &reg);
 	rt2x00_set_field32(&reg, RX_BASE_CSR_RING_REGISTER,
-			   priv_rx->desc_dma);
+			   entry_priv->desc_dma);
 	rt2x00pci_register_write(rt2x00dev, RX_BASE_CSR, reg);
 
 	rt2x00pci_register_read(rt2x00dev, TX_DMA_DST_CSR, &reg);
@@ -1294,25 +1291,32 @@ static int rt61pci_init_registers(struct rt2x00_dev *rt2x00dev)
 	return 0;
 }
 
-static int rt61pci_init_bbp(struct rt2x00_dev *rt2x00dev)
+static int rt61pci_wait_bbp_ready(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");
+			return 0;
 		udelay(REGISTER_BUSY_DELAY);
 	}
 
 	ERROR(rt2x00dev, "BBP register access failed, aborting.\n");
 	return -EACCES;
+}
+
+static int rt61pci_init_bbp(struct rt2x00_dev *rt2x00dev)
+{
+	unsigned int i;
+	u16 eeprom;
+	u8 reg_id;
+	u8 value;
+
+	if (unlikely(rt61pci_wait_bbp_ready(rt2x00dev)))
+		return -EACCES;
 
-continue_csr_init:
 	rt61pci_bbp_write(rt2x00dev, 3, 0x00);
 	rt61pci_bbp_write(rt2x00dev, 15, 0x30);
 	rt61pci_bbp_write(rt2x00dev, 21, 0xc8);
@@ -1361,7 +1365,8 @@ static void rt61pci_toggle_rx(struct rt2x00_dev *rt2x00dev,
 
 	rt2x00pci_register_read(rt2x00dev, TXRX_CSR0, &reg);
 	rt2x00_set_field32(&reg, TXRX_CSR0_DISABLE_RX,
-			   state == STATE_RADIO_RX_OFF);
+			   (state == STATE_RADIO_RX_OFF) ||
+			   (state == STATE_RADIO_RX_OFF_LINK));
 	rt2x00pci_register_write(rt2x00dev, TXRX_CSR0, reg);
 }
 
@@ -1413,17 +1418,10 @@ static int rt61pci_enable_radio(struct rt2x00_dev *rt2x00dev)
 	/*
 	 * Initialize all registers.
 	 */
-	if (rt61pci_init_queues(rt2x00dev) ||
-	    rt61pci_init_registers(rt2x00dev) ||
-	    rt61pci_init_bbp(rt2x00dev)) {
-		ERROR(rt2x00dev, "Register initialization failed.\n");
+	if (unlikely(rt61pci_init_queues(rt2x00dev) ||
+		     rt61pci_init_registers(rt2x00dev) ||
+		     rt61pci_init_bbp(rt2x00dev)))
 		return -EIO;
-	}
-
-	/*
-	 * Enable interrupts.
-	 */
-	rt61pci_toggle_irq(rt2x00dev, STATE_RADIO_IRQ_ON);
 
 	/*
 	 * Enable RX.
@@ -1455,11 +1453,6 @@ static void rt61pci_disable_radio(struct rt2x00_dev *rt2x00dev)
 	rt2x00_set_field32(&reg, TX_CNTL_CSR_ABORT_TX_AC2, 1);
 	rt2x00_set_field32(&reg, TX_CNTL_CSR_ABORT_TX_AC3, 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)
@@ -1467,7 +1460,6 @@ 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);
 
@@ -1483,16 +1475,12 @@ static int rt61pci_set_state(struct rt2x00_dev *rt2x00dev, enum dev_state 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)
+		state = rt2x00_get_field32(reg, MAC_CSR12_BBP_CURRENT_STATE);
+		if (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;
 }
 
@@ -1510,11 +1498,13 @@ static int rt61pci_set_device_state(struct rt2x00_dev *rt2x00dev,
 		break;
 	case STATE_RADIO_RX_ON:
 	case STATE_RADIO_RX_ON_LINK:
-		rt61pci_toggle_rx(rt2x00dev, STATE_RADIO_RX_ON);
-		break;
 	case STATE_RADIO_RX_OFF:
 	case STATE_RADIO_RX_OFF_LINK:
-		rt61pci_toggle_rx(rt2x00dev, STATE_RADIO_RX_OFF);
+		rt61pci_toggle_rx(rt2x00dev, state);
+		break;
+	case STATE_RADIO_IRQ_ON:
+	case STATE_RADIO_IRQ_OFF:
+		rt61pci_toggle_irq(rt2x00dev, state);
 		break;
 	case STATE_DEEP_SLEEP:
 	case STATE_SLEEP:
@@ -1527,6 +1517,10 @@ static int rt61pci_set_device_state(struct rt2x00_dev *rt2x00dev,
 		break;
 	}
 
+	if (unlikely(retval))
+		ERROR(rt2x00dev, "Device failed to enter state %d (%d).\n",
+		      state, retval);
+
 	return retval;
 }
 
@@ -1535,8 +1529,7 @@ static int rt61pci_set_device_state(struct rt2x00_dev *rt2x00dev,
  */
 static void rt61pci_write_tx_desc(struct rt2x00_dev *rt2x00dev,
 				    struct sk_buff *skb,
-				    struct txentry_desc *txdesc,
-				    struct ieee80211_tx_control *control)
+				    struct txentry_desc *txdesc)
 {
 	struct skb_frame_desc *skbdesc = get_skb_frame_desc(skb);
 	__le32 *txd = skbdesc->desc;
@@ -1552,6 +1545,7 @@ static void rt61pci_write_tx_desc(struct rt2x00_dev *rt2x00dev,
 	rt2x00_set_field32(&word, TXD_W1_CWMAX, txdesc->cw_max);
 	rt2x00_set_field32(&word, TXD_W1_IV_OFFSET, IEEE80211_HEADER);
 	rt2x00_set_field32(&word, TXD_W1_HW_SEQUENCE, 1);
+	rt2x00_set_field32(&word, TXD_W1_BUFFER_COUNT, 1);
 	rt2x00_desc_write(txd, 1, word);
 
 	rt2x00_desc_read(txd, 2, &word);
@@ -1562,14 +1556,22 @@ static void rt61pci_write_tx_desc(struct rt2x00_dev *rt2x00dev,
 	rt2x00_desc_write(txd, 2, word);
 
 	rt2x00_desc_read(txd, 5, &word);
+	rt2x00_set_field32(&word, TXD_W5_PID_TYPE, skbdesc->entry->queue->qid);
+	rt2x00_set_field32(&word, TXD_W5_PID_SUBTYPE,
+			   skbdesc->entry->entry_idx);
 	rt2x00_set_field32(&word, TXD_W5_TX_POWER,
 			   TXPOWER_TO_DEV(rt2x00dev->tx_power));
 	rt2x00_set_field32(&word, TXD_W5_WAITING_DMA_DONE_INT, 1);
 	rt2x00_desc_write(txd, 5, word);
 
+	rt2x00_desc_read(txd, 6, &word);
+	rt2x00_set_field32(&word, TXD_W6_BUFFER_PHYSICAL_ADDRESS,
+			   skbdesc->skb_dma);
+	rt2x00_desc_write(txd, 6, word);
+
 	if (skbdesc->desc_len > TXINFO_SIZE) {
 		rt2x00_desc_read(txd, 11, &word);
-		rt2x00_set_field32(&word, TXD_W11_BUFFER_LENGTH0, skbdesc->data_len);
+		rt2x00_set_field32(&word, TXD_W11_BUFFER_LENGTH0, skb->len);
 		rt2x00_desc_write(txd, 11, word);
 	}
 
@@ -1586,10 +1588,9 @@ static void rt61pci_write_tx_desc(struct rt2x00_dev *rt2x00dev,
 			   test_bit(ENTRY_TXD_OFDM_RATE, &txdesc->flags));
 	rt2x00_set_field32(&word, TXD_W0_IFS, txdesc->ifs);
 	rt2x00_set_field32(&word, TXD_W0_RETRY_MODE,
-			   !!(control->flags &
-			      IEEE80211_TXCTL_LONG_RETRY_LIMIT));
+			   test_bit(ENTRY_TXD_RETRY_MODE, &txdesc->flags));
 	rt2x00_set_field32(&word, TXD_W0_TKIP_MIC, 0);
-	rt2x00_set_field32(&word, TXD_W0_DATABYTE_COUNT, skbdesc->data_len);
+	rt2x00_set_field32(&word, TXD_W0_DATABYTE_COUNT, skb->len);
 	rt2x00_set_field32(&word, TXD_W0_BURST,
 			   test_bit(ENTRY_TXD_BURST, &txdesc->flags));
 	rt2x00_set_field32(&word, TXD_W0_CIPHER_ALG, CIPHER_NONE);
@@ -1599,12 +1600,47 @@ static void rt61pci_write_tx_desc(struct rt2x00_dev *rt2x00dev,
 /*
  * TX data initialization
  */
+static void rt61pci_write_beacon(struct queue_entry *entry)
+{
+	struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
+	struct skb_frame_desc *skbdesc = get_skb_frame_desc(entry->skb);
+	unsigned int beacon_base;
+	u32 reg;
+
+	/*
+	 * Disable beaconing while we are reloading the beacon data,
+	 * otherwise we might be sending out invalid data.
+	 */
+	rt2x00pci_register_read(rt2x00dev, TXRX_CSR9, &reg);
+	rt2x00_set_field32(&reg, TXRX_CSR9_TSF_TICKING, 0);
+	rt2x00_set_field32(&reg, TXRX_CSR9_TBTT_ENABLE, 0);
+	rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_GEN, 0);
+	rt2x00pci_register_write(rt2x00dev, TXRX_CSR9, reg);
+
+	/*
+	 * Write entire beacon with descriptor to register.
+	 */
+	beacon_base = HW_BEACON_OFFSET(entry->entry_idx);
+	rt2x00pci_register_multiwrite(rt2x00dev,
+				      beacon_base,
+				      skbdesc->desc, skbdesc->desc_len);
+	rt2x00pci_register_multiwrite(rt2x00dev,
+				      beacon_base + skbdesc->desc_len,
+				      entry->skb->data, entry->skb->len);
+
+	/*
+	 * Clean up beacon skb.
+	 */
+	dev_kfree_skb_any(entry->skb);
+	entry->skb = NULL;
+}
+
 static void rt61pci_kick_tx_queue(struct rt2x00_dev *rt2x00dev,
-				  const unsigned int queue)
+				  const enum data_queue_qid queue)
 {
 	u32 reg;
 
-	if (queue == RT2X00_BCN_QUEUE_BEACON) {
+	if (queue == QID_BEACON) {
 		/*
 		 * For Wi-Fi faily generated beacons between participating
 		 * stations. Set TBTT phase adaptive adjustment step to 8us.
@@ -1622,14 +1658,10 @@ static void rt61pci_kick_tx_queue(struct rt2x00_dev *rt2x00dev,
 	}
 
 	rt2x00pci_register_read(rt2x00dev, TX_CNTL_CSR, &reg);
-	rt2x00_set_field32(&reg, TX_CNTL_CSR_KICK_TX_AC0,
-			   (queue == IEEE80211_TX_QUEUE_DATA0));
-	rt2x00_set_field32(&reg, TX_CNTL_CSR_KICK_TX_AC1,
-			   (queue == IEEE80211_TX_QUEUE_DATA1));
-	rt2x00_set_field32(&reg, TX_CNTL_CSR_KICK_TX_AC2,
-			   (queue == IEEE80211_TX_QUEUE_DATA2));
-	rt2x00_set_field32(&reg, TX_CNTL_CSR_KICK_TX_AC3,
-			   (queue == IEEE80211_TX_QUEUE_DATA3));
+	rt2x00_set_field32(&reg, TX_CNTL_CSR_KICK_TX_AC0, (queue == QID_AC_BE));
+	rt2x00_set_field32(&reg, TX_CNTL_CSR_KICK_TX_AC1, (queue == QID_AC_BK));
+	rt2x00_set_field32(&reg, TX_CNTL_CSR_KICK_TX_AC2, (queue == QID_AC_VI));
+	rt2x00_set_field32(&reg, TX_CNTL_CSR_KICK_TX_AC3, (queue == QID_AC_VO));
 	rt2x00pci_register_write(rt2x00dev, TX_CNTL_CSR, reg);
 }
 
@@ -1680,14 +1712,13 @@ static int rt61pci_agc_to_rssi(struct rt2x00_dev *rt2x00dev, int rxd_w1)
 static void rt61pci_fill_rxdone(struct queue_entry *entry,
 			        struct rxdone_entry_desc *rxdesc)
 {
-	struct queue_entry_priv_pci_rx *priv_rx = entry->priv_data;
+	struct queue_entry_priv_pci *entry_priv = entry->priv_data;
 	u32 word0;
 	u32 word1;
 
-	rt2x00_desc_read(priv_rx->desc, 0, &word0);
-	rt2x00_desc_read(priv_rx->desc, 1, &word1);
+	rt2x00_desc_read(entry_priv->desc, 0, &word0);
+	rt2x00_desc_read(entry_priv->desc, 1, &word1);
 
-	rxdesc->flags = 0;
 	if (rt2x00_get_field32(word0, RXD_W0_CRC_ERROR))
 		rxdesc->flags |= RX_FLAG_FAILED_FCS_CRC;
 
@@ -1701,7 +1732,6 @@ static void rt61pci_fill_rxdone(struct queue_entry *entry,
 	rxdesc->rssi = rt61pci_agc_to_rssi(entry->queue->rt2x00dev, word1);
 	rxdesc->size = rt2x00_get_field32(word0, RXD_W0_DATABYTE_COUNT);
 
-	rxdesc->dev_flags = 0;
 	if (rt2x00_get_field32(word0, RXD_W0_OFDM))
 		rxdesc->dev_flags |= RXDONE_SIGNAL_PLCP;
 	if (rt2x00_get_field32(word0, RXD_W0_MY_BSS))
@@ -1716,7 +1746,7 @@ static void rt61pci_txdone(struct rt2x00_dev *rt2x00dev)
 	struct data_queue *queue;
 	struct queue_entry *entry;
 	struct queue_entry *entry_done;
-	struct queue_entry_priv_pci_tx *priv_tx;
+	struct queue_entry_priv_pci *entry_priv;
 	struct txdone_entry_desc txdesc;
 	u32 word;
 	u32 reg;
@@ -1761,8 +1791,8 @@ static void rt61pci_txdone(struct rt2x00_dev *rt2x00dev)
 			continue;
 
 		entry = &queue->entries[index];
-		priv_tx = entry->priv_data;
-		rt2x00_desc_read(priv_tx->desc, 0, &word);
+		entry_priv = entry->priv_data;
+		rt2x00_desc_read(entry_priv->desc, 0, &word);
 
 		if (rt2x00_get_field32(word, TXD_W0_OWNER_NIC) ||
 		    !rt2x00_get_field32(word, TXD_W0_VALID))
@@ -1777,20 +1807,31 @@ static void rt61pci_txdone(struct rt2x00_dev *rt2x00dev)
 				"TX status report missed for entry %d\n",
 				entry_done->entry_idx);
 
-			txdesc.status = TX_FAIL_OTHER;
+			txdesc.flags = 0;
+			__set_bit(TXDONE_UNKNOWN, &txdesc.flags);
 			txdesc.retry = 0;
 
-			rt2x00pci_txdone(rt2x00dev, entry_done, &txdesc);
+			rt2x00lib_txdone(entry_done, &txdesc);
 			entry_done = rt2x00queue_get_entry(queue, Q_INDEX_DONE);
 		}
 
 		/*
 		 * Obtain the status about this packet.
 		 */
-		txdesc.status = rt2x00_get_field32(reg, STA_CSR4_TX_RESULT);
+		txdesc.flags = 0;
+		switch (rt2x00_get_field32(reg, STA_CSR4_TX_RESULT)) {
+		case 0: /* Success, maybe with retry */
+			__set_bit(TXDONE_SUCCESS, &txdesc.flags);
+			break;
+		case 6: /* Failure, excessive retries */
+			__set_bit(TXDONE_EXCESSIVE_RETRY, &txdesc.flags);
+			/* Don't break, this is a failed frame! */
+		default: /* Failure */
+			__set_bit(TXDONE_FAILURE, &txdesc.flags);
+		}
 		txdesc.retry = rt2x00_get_field32(reg, STA_CSR4_RETRY_COUNT);
 
-		rt2x00pci_txdone(rt2x00dev, entry, &txdesc);
+		rt2x00lib_txdone(entry, &txdesc);
 	}
 }
 
@@ -1976,7 +2017,7 @@ static int rt61pci_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	 * To determine the RT chip we have to read the
 	 * PCI header of the device.
 	 */
-	pci_read_config_word(rt2x00dev_pci(rt2x00dev),
+	pci_read_config_word(to_pci_dev(rt2x00dev->dev),
 			     PCI_CONFIG_HEADER_DEVICE, &device);
 	value = rt2x00_get_field16(eeprom, EEPROM_ANTENNA_RF_TYPE);
 	rt2x00pci_register_read(rt2x00dev, MAC_CSR0, &reg);
@@ -2078,31 +2119,11 @@ static int rt61pci_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	rt2x00_eeprom_read(rt2x00dev, EEPROM_LED, &eeprom);
 	value = rt2x00_get_field16(eeprom, EEPROM_LED_LED_MODE);
 
-	rt2x00dev->led_radio.rt2x00dev = rt2x00dev;
-	rt2x00dev->led_radio.type = LED_TYPE_RADIO;
-	rt2x00dev->led_radio.led_dev.brightness_set =
-	    rt61pci_brightness_set;
-	rt2x00dev->led_radio.led_dev.blink_set =
-	    rt61pci_blink_set;
-	rt2x00dev->led_radio.flags = LED_INITIALIZED;
-
-	rt2x00dev->led_assoc.rt2x00dev = rt2x00dev;
-	rt2x00dev->led_assoc.type = LED_TYPE_ASSOC;
-	rt2x00dev->led_assoc.led_dev.brightness_set =
-	    rt61pci_brightness_set;
-	rt2x00dev->led_assoc.led_dev.blink_set =
-	    rt61pci_blink_set;
-	rt2x00dev->led_assoc.flags = LED_INITIALIZED;
-
-	if (value == LED_MODE_SIGNAL_STRENGTH) {
-		rt2x00dev->led_qual.rt2x00dev = rt2x00dev;
-		rt2x00dev->led_qual.type = LED_TYPE_QUALITY;
-		rt2x00dev->led_qual.led_dev.brightness_set =
-		    rt61pci_brightness_set;
-		rt2x00dev->led_qual.led_dev.blink_set =
-		    rt61pci_blink_set;
-		rt2x00dev->led_qual.flags = LED_INITIALIZED;
-	}
+	rt61pci_init_led(rt2x00dev, &rt2x00dev->led_radio, LED_TYPE_RADIO);
+	rt61pci_init_led(rt2x00dev, &rt2x00dev->led_assoc, LED_TYPE_ASSOC);
+	if (value == LED_MODE_SIGNAL_STRENGTH)
+		rt61pci_init_led(rt2x00dev, &rt2x00dev->led_qual,
+				 LED_TYPE_QUALITY);
 
 	rt2x00_set_field16(&rt2x00dev->led_mcu_reg, MCU_LEDCS_LED_MODE, value);
 	rt2x00_set_field16(&rt2x00dev->led_mcu_reg, MCU_LEDCS_POLARITY_GPIO_0,
@@ -2258,13 +2279,11 @@ static void rt61pci_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
 	 */
 	rt2x00dev->hw->flags =
 	    IEEE80211_HW_HOST_GEN_BEACON_TEMPLATE |
-	    IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING;
+	    IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING |
+	    IEEE80211_HW_SIGNAL_DBM;
 	rt2x00dev->hw->extra_tx_headroom = 0;
-	rt2x00dev->hw->max_signal = MAX_SIGNAL;
-	rt2x00dev->hw->max_rssi = MAX_RX_SSI;
-	rt2x00dev->hw->queues = 4;
 
-	SET_IEEE80211_DEV(rt2x00dev->hw, &rt2x00dev_pci(rt2x00dev)->dev);
+	SET_IEEE80211_DEV(rt2x00dev->hw, rt2x00dev->dev);
 	SET_IEEE80211_PERM_ADDR(rt2x00dev->hw,
 				rt2x00_eeprom_addr(rt2x00dev,
 						   EEPROM_MAC_ADDR_0));
@@ -2327,9 +2346,10 @@ static int rt61pci_probe_hw(struct rt2x00_dev *rt2x00dev)
 	rt61pci_probe_hw_mode(rt2x00dev);
 
 	/*
-	 * This device requires firmware.
+	 * This device requires firmware and DMA mapped skbs.
 	 */
 	__set_bit(DRIVER_REQUIRE_FIRMWARE, &rt2x00dev->flags);
+	__set_bit(DRIVER_REQUIRE_DMA, &rt2x00dev->flags);
 
 	/*
 	 * Set the rssi offset.
@@ -2370,67 +2390,6 @@ static u64 rt61pci_get_tsf(struct ieee80211_hw *hw)
 	return tsf;
 }
 
-static int rt61pci_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb,
-			  struct ieee80211_tx_control *control)
-{
-	struct rt2x00_dev *rt2x00dev = hw->priv;
-	struct rt2x00_intf *intf = vif_to_intf(control->vif);
-	struct queue_entry_priv_pci_tx *priv_tx;
-	struct skb_frame_desc *skbdesc;
-	unsigned int beacon_base;
-	u32 reg;
-
-	if (unlikely(!intf->beacon))
-		return -ENOBUFS;
-
-	priv_tx = intf->beacon->priv_data;
-	memset(priv_tx->desc, 0, intf->beacon->queue->desc_size);
-
-	/*
-	 * Fill in skb descriptor
-	 */
-	skbdesc = get_skb_frame_desc(skb);
-	memset(skbdesc, 0, sizeof(*skbdesc));
-	skbdesc->flags |= FRAME_DESC_DRIVER_GENERATED;
-	skbdesc->data = skb->data;
-	skbdesc->data_len = skb->len;
-	skbdesc->desc = priv_tx->desc;
-	skbdesc->desc_len = intf->beacon->queue->desc_size;
-	skbdesc->entry = intf->beacon;
-
-	/*
-	 * Disable beaconing while we are reloading the beacon data,
-	 * otherwise we might be sending out invalid data.
-	 */
-	rt2x00pci_register_read(rt2x00dev, TXRX_CSR9, &reg);
-	rt2x00_set_field32(&reg, TXRX_CSR9_TSF_TICKING, 0);
-	rt2x00_set_field32(&reg, TXRX_CSR9_TBTT_ENABLE, 0);
-	rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_GEN, 0);
-	rt2x00pci_register_write(rt2x00dev, TXRX_CSR9, reg);
-
-	/*
-	 * mac80211 doesn't provide the control->queue variable
-	 * for beacons. Set our own queue identification so
-	 * it can be used during descriptor initialization.
-	 */
-	control->queue = RT2X00_BCN_QUEUE_BEACON;
-	rt2x00lib_write_tx_desc(rt2x00dev, skb, control);
-
-	/*
-	 * Write entire beacon with descriptor to register,
-	 * and kick the beacon generator.
-	 */
-	beacon_base = HW_BEACON_OFFSET(intf->beacon->entry_idx);
-	rt2x00pci_register_multiwrite(rt2x00dev, beacon_base,
-				      skbdesc->desc, skbdesc->desc_len);
-	rt2x00pci_register_multiwrite(rt2x00dev,
-				      beacon_base + skbdesc->desc_len,
-				      skbdesc->data, skbdesc->data_len);
-	rt61pci_kick_tx_queue(rt2x00dev, control->queue);
-
-	return 0;
-}
-
 static const struct ieee80211_ops rt61pci_mac80211_ops = {
 	.tx			= rt2x00mac_tx,
 	.start			= rt2x00mac_start,
@@ -2446,7 +2405,6 @@ static const struct ieee80211_ops rt61pci_mac80211_ops = {
 	.conf_tx		= rt2x00mac_conf_tx,
 	.get_tx_stats		= rt2x00mac_get_tx_stats,
 	.get_tsf		= rt61pci_get_tsf,
-	.beacon_update		= rt61pci_beacon_update,
 };
 
 static const struct rt2x00lib_ops rt61pci_rt2x00_ops = {
@@ -2466,6 +2424,7 @@ static const struct rt2x00lib_ops rt61pci_rt2x00_ops = {
 	.link_tuner		= rt61pci_link_tuner,
 	.write_tx_desc		= rt61pci_write_tx_desc,
 	.write_tx_data		= rt2x00pci_write_tx_data,
+	.write_beacon		= rt61pci_write_beacon,
 	.kick_tx_queue		= rt61pci_kick_tx_queue,
 	.fill_rxdone		= rt61pci_fill_rxdone,
 	.config_filter		= rt61pci_config_filter,
@@ -2478,21 +2437,21 @@ static const struct data_queue_desc rt61pci_queue_rx = {
 	.entry_num		= RX_ENTRIES,
 	.data_size		= DATA_FRAME_SIZE,
 	.desc_size		= RXD_DESC_SIZE,
-	.priv_size		= sizeof(struct queue_entry_priv_pci_rx),
+	.priv_size		= sizeof(struct queue_entry_priv_pci),
 };
 
 static const struct data_queue_desc rt61pci_queue_tx = {
 	.entry_num		= TX_ENTRIES,
 	.data_size		= DATA_FRAME_SIZE,
 	.desc_size		= TXD_DESC_SIZE,
-	.priv_size		= sizeof(struct queue_entry_priv_pci_tx),
+	.priv_size		= sizeof(struct queue_entry_priv_pci),
 };
 
 static const struct data_queue_desc rt61pci_queue_bcn = {
 	.entry_num		= 4 * BEACON_ENTRIES,
 	.data_size		= 0, /* No DMA required for beacons */
 	.desc_size		= TXINFO_SIZE,
-	.priv_size		= sizeof(struct queue_entry_priv_pci_tx),
+	.priv_size		= sizeof(struct queue_entry_priv_pci),
 };
 
 static const struct rt2x00_ops rt61pci_ops = {
@@ -2501,6 +2460,7 @@ static const struct rt2x00_ops rt61pci_ops = {
 	.max_ap_intf	= 4,
 	.eeprom_size	= EEPROM_SIZE,
 	.rf_size	= RF_SIZE,
+	.tx_queues	= NUM_TX_QUEUES,
 	.rx		= &rt61pci_queue_rx,
 	.tx		= &rt61pci_queue_tx,
 	.bcn		= &rt61pci_queue_bcn,
diff --git a/drivers/net/wireless/rt2x00/rt61pci.h b/drivers/net/wireless/rt2x00/rt61pci.h
index 3511bba7ff65..1004d5b899e6 100644
--- a/drivers/net/wireless/rt2x00/rt61pci.h
+++ b/drivers/net/wireless/rt2x00/rt61pci.h
@@ -39,8 +39,6 @@
  * Signal information.
  * Defaul offset is required for RSSI <-> dBm conversion.
  */
-#define MAX_SIGNAL			100
-#define MAX_RX_SSI			-1
 #define DEFAULT_RSSI_OFFSET		120
 
 /*
@@ -54,6 +52,11 @@
 #define RF_SIZE				0x0014
 
 /*
+ * Number of TX queues.
+ */
+#define NUM_TX_QUEUES			4
+
+/*
  * PCI registers.
  */
 
diff --git a/drivers/net/wireless/rt2x00/rt73usb.c b/drivers/net/wireless/rt2x00/rt73usb.c
index 46e9e081fbf1..d383735ab8f2 100644
--- a/drivers/net/wireless/rt2x00/rt73usb.c
+++ b/drivers/net/wireless/rt2x00/rt73usb.c
@@ -74,10 +74,10 @@ static inline void rt73usb_register_multiread(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);
+				      value, length,
+				      REGISTER_TIMEOUT32(length));
 }
 
 static inline void rt73usb_register_write(struct rt2x00_dev *rt2x00dev,
@@ -102,10 +102,10 @@ static inline void rt73usb_register_multiwrite(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);
+				      value, length,
+				      REGISTER_TIMEOUT32(length));
 }
 
 static u32 rt73usb_bbp_check(struct rt2x00_dev *rt2x00dev)
@@ -341,6 +341,17 @@ static int rt73usb_blink_set(struct led_classdev *led_cdev,
 
 	return 0;
 }
+
+static void rt73usb_init_led(struct rt2x00_dev *rt2x00dev,
+			     struct rt2x00_led *led,
+			     enum led_type type)
+{
+	led->rt2x00dev = rt2x00dev;
+	led->type = type;
+	led->led_dev.brightness_set = rt73usb_brightness_set;
+	led->led_dev.blink_set = rt73usb_blink_set;
+	led->flags = LED_INITIALIZED;
+}
 #endif /* CONFIG_RT73USB_LEDS */
 
 /*
@@ -856,7 +867,7 @@ static char *rt73usb_get_firmware_name(struct rt2x00_dev *rt2x00dev)
 	return FIRMWARE_RT2571;
 }
 
-static u16 rt73usb_get_firmware_crc(void *data, const size_t len)
+static u16 rt73usb_get_firmware_crc(const void *data, const size_t len)
 {
 	u16 crc;
 
@@ -873,16 +884,15 @@ static u16 rt73usb_get_firmware_crc(void *data, const size_t len)
 	return crc;
 }
 
-static int rt73usb_load_firmware(struct rt2x00_dev *rt2x00dev, void *data,
+static int rt73usb_load_firmware(struct rt2x00_dev *rt2x00dev, const void *data,
 				 const size_t len)
 {
 	unsigned int i;
 	int status;
 	u32 reg;
-	char *ptr = data;
+	const char *ptr = data;
 	char *cache;
 	int buflen;
-	int timeout;
 
 	/*
 	 * Wait for stable hardware.
@@ -913,14 +923,14 @@ static int rt73usb_load_firmware(struct rt2x00_dev *rt2x00dev, void *data,
 
 	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, 0,
-					 cache, buflen, timeout);
+					 cache, buflen,
+					 REGISTER_TIMEOUT32(buflen));
 
 		ptr += buflen;
 	}
@@ -1100,25 +1110,32 @@ static int rt73usb_init_registers(struct rt2x00_dev *rt2x00dev)
 	return 0;
 }
 
-static int rt73usb_init_bbp(struct rt2x00_dev *rt2x00dev)
+static int rt73usb_wait_bbp_ready(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");
+			return 0;
 		udelay(REGISTER_BUSY_DELAY);
 	}
 
 	ERROR(rt2x00dev, "BBP register access failed, aborting.\n");
 	return -EACCES;
+}
+
+static int rt73usb_init_bbp(struct rt2x00_dev *rt2x00dev)
+{
+	unsigned int i;
+	u16 eeprom;
+	u8 reg_id;
+	u8 value;
+
+	if (unlikely(rt73usb_wait_bbp_ready(rt2x00dev)))
+		return -EACCES;
 
-continue_csr_init:
 	rt73usb_bbp_write(rt2x00dev, 3, 0x80);
 	rt73usb_bbp_write(rt2x00dev, 15, 0x30);
 	rt73usb_bbp_write(rt2x00dev, 21, 0xc8);
@@ -1168,7 +1185,8 @@ static void rt73usb_toggle_rx(struct rt2x00_dev *rt2x00dev,
 
 	rt73usb_register_read(rt2x00dev, TXRX_CSR0, &reg);
 	rt2x00_set_field32(&reg, TXRX_CSR0_DISABLE_RX,
-			   state == STATE_RADIO_RX_OFF);
+			   (state == STATE_RADIO_RX_OFF) ||
+			   (state == STATE_RADIO_RX_OFF_LINK));
 	rt73usb_register_write(rt2x00dev, TXRX_CSR0, reg);
 }
 
@@ -1177,11 +1195,9 @@ 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");
+	if (unlikely(rt73usb_init_registers(rt2x00dev) ||
+		     rt73usb_init_bbp(rt2x00dev)))
 		return -EIO;
-	}
 
 	return 0;
 }
@@ -1203,7 +1219,6 @@ 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);
 
@@ -1219,16 +1234,12 @@ static int rt73usb_set_state(struct rt2x00_dev *rt2x00dev, enum dev_state 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)
+		state = rt2x00_get_field32(reg, MAC_CSR12_BBP_CURRENT_STATE);
+		if (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;
 }
 
@@ -1246,11 +1257,13 @@ static int rt73usb_set_device_state(struct rt2x00_dev *rt2x00dev,
 		break;
 	case STATE_RADIO_RX_ON:
 	case STATE_RADIO_RX_ON_LINK:
-		rt73usb_toggle_rx(rt2x00dev, STATE_RADIO_RX_ON);
-		break;
 	case STATE_RADIO_RX_OFF:
 	case STATE_RADIO_RX_OFF_LINK:
-		rt73usb_toggle_rx(rt2x00dev, STATE_RADIO_RX_OFF);
+		rt73usb_toggle_rx(rt2x00dev, state);
+		break;
+	case STATE_RADIO_IRQ_ON:
+	case STATE_RADIO_IRQ_OFF:
+		/* No support, but no error either */
 		break;
 	case STATE_DEEP_SLEEP:
 	case STATE_SLEEP:
@@ -1263,6 +1276,10 @@ static int rt73usb_set_device_state(struct rt2x00_dev *rt2x00dev,
 		break;
 	}
 
+	if (unlikely(retval))
+		ERROR(rt2x00dev, "Device failed to enter state %d (%d).\n",
+		      state, retval);
+
 	return retval;
 }
 
@@ -1271,8 +1288,7 @@ static int rt73usb_set_device_state(struct rt2x00_dev *rt2x00dev,
  */
 static void rt73usb_write_tx_desc(struct rt2x00_dev *rt2x00dev,
 				    struct sk_buff *skb,
-				    struct txentry_desc *txdesc,
-				    struct ieee80211_tx_control *control)
+				    struct txentry_desc *txdesc)
 {
 	struct skb_frame_desc *skbdesc = get_skb_frame_desc(skb);
 	__le32 *txd = skbdesc->desc;
@@ -1317,16 +1333,59 @@ static void rt73usb_write_tx_desc(struct rt2x00_dev *rt2x00dev,
 			   test_bit(ENTRY_TXD_OFDM_RATE, &txdesc->flags));
 	rt2x00_set_field32(&word, TXD_W0_IFS, txdesc->ifs);
 	rt2x00_set_field32(&word, TXD_W0_RETRY_MODE,
-			   !!(control->flags &
-			      IEEE80211_TXCTL_LONG_RETRY_LIMIT));
+			   test_bit(ENTRY_TXD_RETRY_MODE, &txdesc->flags));
 	rt2x00_set_field32(&word, TXD_W0_TKIP_MIC, 0);
-	rt2x00_set_field32(&word, TXD_W0_DATABYTE_COUNT, skbdesc->data_len);
+	rt2x00_set_field32(&word, TXD_W0_DATABYTE_COUNT,
+			   skb->len - skbdesc->desc_len);
 	rt2x00_set_field32(&word, TXD_W0_BURST2,
 			   test_bit(ENTRY_TXD_BURST, &txdesc->flags));
 	rt2x00_set_field32(&word, TXD_W0_CIPHER_ALG, CIPHER_NONE);
 	rt2x00_desc_write(txd, 0, word);
 }
 
+/*
+ * TX data initialization
+ */
+static void rt73usb_write_beacon(struct queue_entry *entry)
+{
+	struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
+	struct skb_frame_desc *skbdesc = get_skb_frame_desc(entry->skb);
+	unsigned int beacon_base;
+	u32 reg;
+
+	/*
+	 * Add the descriptor in front of the skb.
+	 */
+	skb_push(entry->skb, entry->queue->desc_size);
+	memcpy(entry->skb->data, skbdesc->desc, skbdesc->desc_len);
+	skbdesc->desc = entry->skb->data;
+
+	/*
+	 * Disable beaconing while we are reloading the beacon data,
+	 * otherwise we might be sending out invalid data.
+	 */
+	rt73usb_register_read(rt2x00dev, TXRX_CSR9, &reg);
+	rt2x00_set_field32(&reg, TXRX_CSR9_TSF_TICKING, 0);
+	rt2x00_set_field32(&reg, TXRX_CSR9_TBTT_ENABLE, 0);
+	rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_GEN, 0);
+	rt73usb_register_write(rt2x00dev, TXRX_CSR9, reg);
+
+	/*
+	 * Write entire beacon with descriptor to register.
+	 */
+	beacon_base = HW_BEACON_OFFSET(entry->entry_idx);
+	rt2x00usb_vendor_request(rt2x00dev, USB_MULTI_WRITE,
+				 USB_VENDOR_REQUEST_OUT, beacon_base, 0,
+				 entry->skb->data, entry->skb->len,
+				 REGISTER_TIMEOUT32(entry->skb->len));
+
+	/*
+	 * Clean up the beacon skb.
+	 */
+	dev_kfree_skb(entry->skb);
+	entry->skb = NULL;
+}
+
 static int rt73usb_get_tx_data_len(struct rt2x00_dev *rt2x00dev,
 				   struct sk_buff *skb)
 {
@@ -1342,16 +1401,15 @@ static int rt73usb_get_tx_data_len(struct rt2x00_dev *rt2x00dev,
 	return length;
 }
 
-/*
- * TX data initialization
- */
 static void rt73usb_kick_tx_queue(struct rt2x00_dev *rt2x00dev,
-				  const unsigned int queue)
+				  const enum data_queue_qid queue)
 {
 	u32 reg;
 
-	if (queue != RT2X00_BCN_QUEUE_BEACON)
+	if (queue != QID_BEACON) {
+		rt2x00usb_kick_tx_queue(rt2x00dev, queue);
 		return;
+	}
 
 	/*
 	 * For Wi-Fi faily generated beacons between participating stations.
@@ -1421,25 +1479,22 @@ static void rt73usb_fill_rxdone(struct queue_entry *entry,
 {
 	struct skb_frame_desc *skbdesc = get_skb_frame_desc(entry->skb);
 	__le32 *rxd = (__le32 *)entry->skb->data;
-	unsigned int offset = entry->queue->desc_size + 2;
 	u32 word0;
 	u32 word1;
 
 	/*
-	 * Copy descriptor to the available headroom inside the skbuffer.
+	 * Copy descriptor to the skbdesc->desc buffer, making it safe from moving of
+	 * frame data in rt2x00usb.
 	 */
-	skb_push(entry->skb, offset);
-	memcpy(entry->skb->data, rxd, entry->queue->desc_size);
-	rxd = (__le32 *)entry->skb->data;
+	memcpy(skbdesc->desc, rxd, skbdesc->desc_len);
+	rxd = (__le32 *)skbdesc->desc;
 
 	/*
-	 * The descriptor is now aligned to 4 bytes and thus it is
-	 * now safe to read it on all architectures.
+	 * It is now safe to read the descriptor on all architectures.
 	 */
 	rt2x00_desc_read(rxd, 0, &word0);
 	rt2x00_desc_read(rxd, 1, &word1);
 
-	rxdesc->flags = 0;
 	if (rt2x00_get_field32(word0, RXD_W0_CRC_ERROR))
 		rxdesc->flags |= RX_FLAG_FAILED_FCS_CRC;
 
@@ -1453,25 +1508,16 @@ static void rt73usb_fill_rxdone(struct queue_entry *entry,
 	rxdesc->rssi = rt73usb_agc_to_rssi(entry->queue->rt2x00dev, word1);
 	rxdesc->size = rt2x00_get_field32(word0, RXD_W0_DATABYTE_COUNT);
 
-	rxdesc->dev_flags = 0;
 	if (rt2x00_get_field32(word0, RXD_W0_OFDM))
 		rxdesc->dev_flags |= RXDONE_SIGNAL_PLCP;
 	if (rt2x00_get_field32(word0, RXD_W0_MY_BSS))
 		rxdesc->dev_flags |= RXDONE_MY_BSS;
 
 	/*
-	 * Adjust the skb memory window to the frame boundaries.
+	 * Set skb pointers, and update frame information.
 	 */
-	skb_pull(entry->skb, offset + entry->queue->desc_size);
+	skb_pull(entry->skb, entry->queue->desc_size);
 	skb_trim(entry->skb, rxdesc->size);
-
-	/*
-	 * Set descriptor and data pointer.
-	 */
-	skbdesc->data = entry->skb->data;
-	skbdesc->data_len = rxdesc->size;
-	skbdesc->desc = rxd;
-	skbdesc->desc_len = entry->queue->desc_size;
 }
 
 /*
@@ -1644,31 +1690,11 @@ static int rt73usb_init_eeprom(struct rt2x00_dev *rt2x00dev)
 #ifdef CONFIG_RT73USB_LEDS
 	rt2x00_eeprom_read(rt2x00dev, EEPROM_LED, &eeprom);
 
-	rt2x00dev->led_radio.rt2x00dev = rt2x00dev;
-	rt2x00dev->led_radio.type = LED_TYPE_RADIO;
-	rt2x00dev->led_radio.led_dev.brightness_set =
-	    rt73usb_brightness_set;
-	rt2x00dev->led_radio.led_dev.blink_set =
-	    rt73usb_blink_set;
-	rt2x00dev->led_radio.flags = LED_INITIALIZED;
-
-	rt2x00dev->led_assoc.rt2x00dev = rt2x00dev;
-	rt2x00dev->led_assoc.type = LED_TYPE_ASSOC;
-	rt2x00dev->led_assoc.led_dev.brightness_set =
-	    rt73usb_brightness_set;
-	rt2x00dev->led_assoc.led_dev.blink_set =
-	    rt73usb_blink_set;
-	rt2x00dev->led_assoc.flags = LED_INITIALIZED;
-
-	if (value == LED_MODE_SIGNAL_STRENGTH) {
-		rt2x00dev->led_qual.rt2x00dev = rt2x00dev;
-		rt2x00dev->led_qual.type = LED_TYPE_QUALITY;
-		rt2x00dev->led_qual.led_dev.brightness_set =
-		    rt73usb_brightness_set;
-		rt2x00dev->led_qual.led_dev.blink_set =
-		    rt73usb_blink_set;
-		rt2x00dev->led_qual.flags = LED_INITIALIZED;
-	}
+	rt73usb_init_led(rt2x00dev, &rt2x00dev->led_radio, LED_TYPE_RADIO);
+	rt73usb_init_led(rt2x00dev, &rt2x00dev->led_assoc, LED_TYPE_ASSOC);
+	if (value == LED_MODE_SIGNAL_STRENGTH)
+		rt73usb_init_led(rt2x00dev, &rt2x00dev->led_qual,
+				 LED_TYPE_QUALITY);
 
 	rt2x00_set_field16(&rt2x00dev->led_mcu_reg, MCU_LEDCS_LED_MODE, value);
 	rt2x00_set_field16(&rt2x00dev->led_mcu_reg, MCU_LEDCS_POLARITY_GPIO_0,
@@ -1846,13 +1872,11 @@ static void rt73usb_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
 	 */
 	rt2x00dev->hw->flags =
 	    IEEE80211_HW_HOST_GEN_BEACON_TEMPLATE |
-	    IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING;
+	    IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING |
+	    IEEE80211_HW_SIGNAL_DBM;
 	rt2x00dev->hw->extra_tx_headroom = TXD_DESC_SIZE;
-	rt2x00dev->hw->max_signal = MAX_SIGNAL;
-	rt2x00dev->hw->max_rssi = MAX_RX_SSI;
-	rt2x00dev->hw->queues = 4;
 
-	SET_IEEE80211_DEV(rt2x00dev->hw, &rt2x00dev_usb(rt2x00dev)->dev);
+	SET_IEEE80211_DEV(rt2x00dev->hw, rt2x00dev->dev);
 	SET_IEEE80211_PERM_ADDR(rt2x00dev->hw,
 				rt2x00_eeprom_addr(rt2x00dev,
 						   EEPROM_MAC_ADDR_0));
@@ -1974,69 +1998,6 @@ static u64 rt73usb_get_tsf(struct ieee80211_hw *hw)
 #define rt73usb_get_tsf	NULL
 #endif
 
-static int rt73usb_beacon_update(struct ieee80211_hw *hw, struct sk_buff *skb,
-				 struct ieee80211_tx_control *control)
-{
-	struct rt2x00_dev *rt2x00dev = hw->priv;
-	struct rt2x00_intf *intf = vif_to_intf(control->vif);
-	struct skb_frame_desc *skbdesc;
-	unsigned int beacon_base;
-	unsigned int timeout;
-	u32 reg;
-
-	if (unlikely(!intf->beacon))
-		return -ENOBUFS;
-
-	/*
-	 * Add the descriptor in front of the skb.
-	 */
-	skb_push(skb, intf->beacon->queue->desc_size);
-	memset(skb->data, 0, intf->beacon->queue->desc_size);
-
-	/*
-	 * Fill in skb descriptor
-	 */
-	skbdesc = get_skb_frame_desc(skb);
-	memset(skbdesc, 0, sizeof(*skbdesc));
-	skbdesc->flags |= FRAME_DESC_DRIVER_GENERATED;
-	skbdesc->data = skb->data + intf->beacon->queue->desc_size;
-	skbdesc->data_len = skb->len - intf->beacon->queue->desc_size;
-	skbdesc->desc = skb->data;
-	skbdesc->desc_len = intf->beacon->queue->desc_size;
-	skbdesc->entry = intf->beacon;
-
-	/*
-	 * Disable beaconing while we are reloading the beacon data,
-	 * otherwise we might be sending out invalid data.
-	 */
-	rt73usb_register_read(rt2x00dev, TXRX_CSR9, &reg);
-	rt2x00_set_field32(&reg, TXRX_CSR9_TSF_TICKING, 0);
-	rt2x00_set_field32(&reg, TXRX_CSR9_TBTT_ENABLE, 0);
-	rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_GEN, 0);
-	rt73usb_register_write(rt2x00dev, TXRX_CSR9, reg);
-
-	/*
-	 * mac80211 doesn't provide the control->queue variable
-	 * for beacons. Set our own queue identification so
-	 * it can be used during descriptor initialization.
-	 */
-	control->queue = RT2X00_BCN_QUEUE_BEACON;
-	rt2x00lib_write_tx_desc(rt2x00dev, skb, control);
-
-	/*
-	 * Write entire beacon with descriptor to register,
-	 * and kick the beacon generator.
-	 */
-	beacon_base = HW_BEACON_OFFSET(intf->beacon->entry_idx);
-	timeout = REGISTER_TIMEOUT * (skb->len / sizeof(u32));
-	rt2x00usb_vendor_request(rt2x00dev, USB_MULTI_WRITE,
-				 USB_VENDOR_REQUEST_OUT, beacon_base, 0,
-				 skb->data, skb->len, timeout);
-	rt73usb_kick_tx_queue(rt2x00dev, control->queue);
-
-	return 0;
-}
-
 static const struct ieee80211_ops rt73usb_mac80211_ops = {
 	.tx			= rt2x00mac_tx,
 	.start			= rt2x00mac_start,
@@ -2052,7 +2013,6 @@ static const struct ieee80211_ops rt73usb_mac80211_ops = {
 	.conf_tx		= rt2x00mac_conf_tx,
 	.get_tx_stats		= rt2x00mac_get_tx_stats,
 	.get_tsf		= rt73usb_get_tsf,
-	.beacon_update		= rt73usb_beacon_update,
 };
 
 static const struct rt2x00lib_ops rt73usb_rt2x00_ops = {
@@ -2070,6 +2030,7 @@ static const struct rt2x00lib_ops rt73usb_rt2x00_ops = {
 	.link_tuner		= rt73usb_link_tuner,
 	.write_tx_desc		= rt73usb_write_tx_desc,
 	.write_tx_data		= rt2x00usb_write_tx_data,
+	.write_beacon		= rt73usb_write_beacon,
 	.get_tx_data_len	= rt73usb_get_tx_data_len,
 	.kick_tx_queue		= rt73usb_kick_tx_queue,
 	.fill_rxdone		= rt73usb_fill_rxdone,
@@ -2083,21 +2044,21 @@ static const struct data_queue_desc rt73usb_queue_rx = {
 	.entry_num		= RX_ENTRIES,
 	.data_size		= DATA_FRAME_SIZE,
 	.desc_size		= RXD_DESC_SIZE,
-	.priv_size		= sizeof(struct queue_entry_priv_usb_rx),
+	.priv_size		= sizeof(struct queue_entry_priv_usb),
 };
 
 static const struct data_queue_desc rt73usb_queue_tx = {
 	.entry_num		= TX_ENTRIES,
 	.data_size		= DATA_FRAME_SIZE,
 	.desc_size		= TXD_DESC_SIZE,
-	.priv_size		= sizeof(struct queue_entry_priv_usb_tx),
+	.priv_size		= sizeof(struct queue_entry_priv_usb),
 };
 
 static const struct data_queue_desc rt73usb_queue_bcn = {
 	.entry_num		= 4 * BEACON_ENTRIES,
 	.data_size		= MGMT_FRAME_SIZE,
 	.desc_size		= TXINFO_SIZE,
-	.priv_size		= sizeof(struct queue_entry_priv_usb_tx),
+	.priv_size		= sizeof(struct queue_entry_priv_usb),
 };
 
 static const struct rt2x00_ops rt73usb_ops = {
@@ -2106,6 +2067,7 @@ static const struct rt2x00_ops rt73usb_ops = {
 	.max_ap_intf	= 4,
 	.eeprom_size	= EEPROM_SIZE,
 	.rf_size	= RF_SIZE,
+	.tx_queues	= NUM_TX_QUEUES,
 	.rx		= &rt73usb_queue_rx,
 	.tx		= &rt73usb_queue_tx,
 	.bcn		= &rt73usb_queue_bcn,
diff --git a/drivers/net/wireless/rt2x00/rt73usb.h b/drivers/net/wireless/rt2x00/rt73usb.h
index 06d687425fef..148493501011 100644
--- a/drivers/net/wireless/rt2x00/rt73usb.h
+++ b/drivers/net/wireless/rt2x00/rt73usb.h
@@ -39,8 +39,6 @@
  * Signal information.
  * Defaul offset is required for RSSI <-> dBm conversion.
  */
-#define MAX_SIGNAL			100
-#define MAX_RX_SSI			-1
 #define DEFAULT_RSSI_OFFSET		120
 
 /*
@@ -54,6 +52,11 @@
 #define RF_SIZE				0x0014
 
 /*
+ * Number of TX queues.
+ */
+#define NUM_TX_QUEUES			4
+
+/*
  * USB registers.
  */
 
diff --git a/drivers/net/wireless/rtl8180_dev.c b/drivers/net/wireless/rtl8180_dev.c
index c181f23e930d..b7172a12c057 100644
--- a/drivers/net/wireless/rtl8180_dev.c
+++ b/drivers/net/wireless/rtl8180_dev.c
@@ -132,8 +132,8 @@ static void rtl8180_handle_rx(struct ieee80211_hw *dev)
 
 			rx_status.antenna = (flags2 >> 15) & 1;
 			/* TODO: improve signal/rssi reporting */
-			rx_status.signal = flags2 & 0xFF;
-			rx_status.ssi = (flags2 >> 8) & 0x7F;
+			rx_status.qual = flags2 & 0xFF;
+			rx_status.signal = (flags2 >> 8) & 0x7F;
 			/* XXX: is this correct? */
 			rx_status.rate_idx = (flags >> 20) & 0xF;
 			rx_status.freq = dev->conf.channel->center_freq;
@@ -170,34 +170,29 @@ static void rtl8180_handle_tx(struct ieee80211_hw *dev, unsigned int prio)
 	while (skb_queue_len(&ring->queue)) {
 		struct rtl8180_tx_desc *entry = &ring->desc[ring->idx];
 		struct sk_buff *skb;
-		struct ieee80211_tx_status status;
-		struct ieee80211_tx_control *control;
+		struct ieee80211_tx_info *info;
 		u32 flags = le32_to_cpu(entry->flags);
 
 		if (flags & RTL8180_TX_DESC_FLAG_OWN)
 			return;
 
-		memset(&status, 0, sizeof(status));
-
 		ring->idx = (ring->idx + 1) % ring->entries;
 		skb = __skb_dequeue(&ring->queue);
 		pci_unmap_single(priv->pdev, le32_to_cpu(entry->tx_buf),
 				 skb->len, PCI_DMA_TODEVICE);
 
-		control = *((struct ieee80211_tx_control **)skb->cb);
-		if (control)
-			memcpy(&status.control, control, sizeof(*control));
-		kfree(control);
+		info = IEEE80211_SKB_CB(skb);
+		memset(&info->status, 0, sizeof(info->status));
 
-		if (!(status.control.flags & IEEE80211_TXCTL_NO_ACK)) {
+		if (!(info->flags & IEEE80211_TX_CTL_NO_ACK)) {
 			if (flags & RTL8180_TX_DESC_FLAG_TX_OK)
-				status.flags = IEEE80211_TX_STATUS_ACK;
+				info->flags |= IEEE80211_TX_STAT_ACK;
 			else
-				status.excessive_retries = 1;
+				info->status.excessive_retries = 1;
 		}
-		status.retry_count = flags & 0xFF;
+		info->status.retry_count = flags & 0xFF;
 
-		ieee80211_tx_status_irqsafe(dev, skb, &status);
+		ieee80211_tx_status_irqsafe(dev, skb);
 		if (ring->entries - skb_queue_len(&ring->queue) == 2)
 			ieee80211_wake_queue(dev, prio);
 	}
@@ -238,9 +233,9 @@ static irqreturn_t rtl8180_interrupt(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
-static int rtl8180_tx(struct ieee80211_hw *dev, struct sk_buff *skb,
-		      struct ieee80211_tx_control *control)
+static int rtl8180_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
 {
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 	struct rtl8180_priv *priv = dev->priv;
 	struct rtl8180_tx_ring *ring;
 	struct rtl8180_tx_desc *entry;
@@ -251,46 +246,40 @@ static int rtl8180_tx(struct ieee80211_hw *dev, struct sk_buff *skb,
 	u16 plcp_len = 0;
 	__le16 rts_duration = 0;
 
-	prio = control->queue;
+	prio = skb_get_queue_mapping(skb);
 	ring = &priv->tx_ring[prio];
 
 	mapping = pci_map_single(priv->pdev, skb->data,
 				 skb->len, PCI_DMA_TODEVICE);
 
-	BUG_ON(!control->tx_rate);
-
 	tx_flags = RTL8180_TX_DESC_FLAG_OWN | RTL8180_TX_DESC_FLAG_FS |
 		   RTL8180_TX_DESC_FLAG_LS |
-		   (control->tx_rate->hw_value << 24) | skb->len;
+		   (ieee80211_get_tx_rate(dev, info)->hw_value << 24) |
+		   skb->len;
 
 	if (priv->r8185)
 		tx_flags |= RTL8180_TX_DESC_FLAG_DMA |
 			    RTL8180_TX_DESC_FLAG_NO_ENC;
 
-	if (control->flags & IEEE80211_TXCTL_USE_RTS_CTS) {
-		BUG_ON(!control->rts_cts_rate);
+	if (info->flags & IEEE80211_TX_CTL_USE_RTS_CTS) {
 		tx_flags |= RTL8180_TX_DESC_FLAG_RTS;
-		tx_flags |= control->rts_cts_rate->hw_value << 19;
-	} else if (control->flags & IEEE80211_TXCTL_USE_CTS_PROTECT) {
-		BUG_ON(!control->rts_cts_rate);
+		tx_flags |= ieee80211_get_rts_cts_rate(dev, info)->hw_value << 19;
+	} else if (info->flags & IEEE80211_TX_CTL_USE_CTS_PROTECT) {
 		tx_flags |= RTL8180_TX_DESC_FLAG_CTS;
-		tx_flags |= control->rts_cts_rate->hw_value << 19;
+		tx_flags |= ieee80211_get_rts_cts_rate(dev, info)->hw_value << 19;
 	}
 
-	*((struct ieee80211_tx_control **) skb->cb) =
-		kmemdup(control, sizeof(*control), GFP_ATOMIC);
-
-	if (control->flags & IEEE80211_TXCTL_USE_RTS_CTS)
+	if (info->flags & IEEE80211_TX_CTL_USE_RTS_CTS)
 		rts_duration = ieee80211_rts_duration(dev, priv->vif, skb->len,
-						      control);
+						      info);
 
 	if (!priv->r8185) {
 		unsigned int remainder;
 
 		plcp_len = DIV_ROUND_UP(16 * (skb->len + 4),
-					(control->tx_rate->bitrate * 2) / 10);
+				(ieee80211_get_tx_rate(dev, info)->bitrate * 2) / 10);
 		remainder = (16 * (skb->len + 4)) %
-			    ((control->tx_rate->bitrate * 2) / 10);
+			    ((ieee80211_get_tx_rate(dev, info)->bitrate * 2) / 10);
 		if (remainder > 0 && remainder <= 6)
 			plcp_len |= 1 << 15;
 	}
@@ -303,13 +292,13 @@ static int rtl8180_tx(struct ieee80211_hw *dev, struct sk_buff *skb,
 	entry->plcp_len = cpu_to_le16(plcp_len);
 	entry->tx_buf = cpu_to_le32(mapping);
 	entry->frame_len = cpu_to_le32(skb->len);
-	entry->flags2 = control->alt_retry_rate != NULL ?
-			control->alt_retry_rate->bitrate << 4 : 0;
-	entry->retry_limit = control->retry_limit;
+	entry->flags2 = info->control.alt_retry_rate_idx >= 0 ?
+		ieee80211_get_alt_retry_rate(dev, info)->bitrate << 4 : 0;
+	entry->retry_limit = info->control.retry_limit;
 	entry->flags = cpu_to_le32(tx_flags);
 	__skb_queue_tail(&ring->queue, skb);
 	if (ring->entries - skb_queue_len(&ring->queue) < 2)
-		ieee80211_stop_queue(dev, control->queue);
+		ieee80211_stop_queue(dev, skb_get_queue_mapping(skb));
 	spin_unlock_irqrestore(&priv->lock, flags);
 
 	rtl818x_iowrite8(priv, &priv->map->TX_DMA_POLLING, (1 << (prio + 4)));
@@ -525,7 +514,6 @@ static void rtl8180_free_tx_ring(struct ieee80211_hw *dev, unsigned int prio)
 
 		pci_unmap_single(priv->pdev, le32_to_cpu(entry->tx_buf),
 				 skb->len, PCI_DMA_TODEVICE);
-		kfree(*((struct ieee80211_tx_control **) skb->cb));
 		kfree_skb(skb);
 		ring->idx = (ring->idx + 1) % ring->entries;
 	}
@@ -894,9 +882,10 @@ static int __devinit rtl8180_probe(struct pci_dev *pdev,
 	dev->wiphy->bands[IEEE80211_BAND_2GHZ] = &priv->band;
 
 	dev->flags = IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING |
-		     IEEE80211_HW_RX_INCLUDES_FCS;
+		     IEEE80211_HW_RX_INCLUDES_FCS |
+		     IEEE80211_HW_SIGNAL_UNSPEC;
 	dev->queues = 1;
-	dev->max_rssi = 65;
+	dev->max_signal = 65;
 
 	reg = rtl818x_ioread32(priv, &priv->map->TX_CONF);
 	reg &= RTL818X_TX_CONF_HWVER_MASK;
diff --git a/drivers/net/wireless/rtl8187.h b/drivers/net/wireless/rtl8187.h
index 076d88b6db0e..3afb49f8866a 100644
--- a/drivers/net/wireless/rtl8187.h
+++ b/drivers/net/wireless/rtl8187.h
@@ -44,23 +44,48 @@ struct rtl8187_rx_hdr {
 	__le64 mac_time;
 } __attribute__((packed));
 
-struct rtl8187_tx_info {
-	struct ieee80211_tx_control *control;
-	struct urb *urb;
-	struct ieee80211_hw *dev;
-};
-
-struct rtl8187_tx_hdr {
+struct rtl8187b_rx_hdr {
 	__le32 flags;
+	__le64 mac_time;
+	u8 noise;
+	u8 signal;
+	u8 agc;
+	u8 reserved;
+	__le32 unused;
+} __attribute__((packed));
+
+/* {rtl8187,rtl8187b}_tx_info is in skb */
+
+/* Tx flags are common between rtl8187 and rtl8187b */
 #define RTL8187_TX_FLAG_NO_ENCRYPT	(1 << 15)
 #define RTL8187_TX_FLAG_MORE_FRAG	(1 << 17)
 #define RTL8187_TX_FLAG_CTS		(1 << 18)
 #define RTL8187_TX_FLAG_RTS		(1 << 23)
+
+struct rtl8187_tx_hdr {
+	__le32 flags;
+	__le16 rts_duration;
+	__le16 len;
+	__le32 retry;
+} __attribute__((packed));
+
+struct rtl8187b_tx_hdr {
+	__le32 flags;
 	__le16 rts_duration;
 	__le16 len;
+	__le32 unused_1;
+	__le16 unused_2;
+	__le16 tx_duration;
+	__le32 unused_3;
 	__le32 retry;
+	__le32 unused_4[2];
 } __attribute__((packed));
 
+enum {
+	DEVICE_RTL8187,
+	DEVICE_RTL8187B
+};
+
 struct rtl8187_priv {
 	/* common between rtl818x drivers */
 	struct rtl818x_csr *map;
@@ -76,70 +101,120 @@ struct rtl8187_priv {
 	u32 rx_conf;
 	u16 txpwr_base;
 	u8 asic_rev;
+	u8 is_rtl8187b;
+	enum {
+		RTL8187BvB,
+		RTL8187BvD,
+		RTL8187BvE
+	} hw_rev;
 	struct sk_buff_head rx_queue;
+	u8 signal;
+	u8 quality;
+	u8 noise;
 };
 
 void rtl8187_write_phy(struct ieee80211_hw *dev, u8 addr, u32 data);
 
-static inline u8 rtl818x_ioread8(struct rtl8187_priv *priv, u8 *addr)
+static inline u8 rtl818x_ioread8_idx(struct rtl8187_priv *priv,
+				     u8 *addr, u8 idx)
 {
 	u8 val;
 
 	usb_control_msg(priv->udev, usb_rcvctrlpipe(priv->udev, 0),
 			RTL8187_REQ_GET_REG, RTL8187_REQT_READ,
-			(unsigned long)addr, 0, &val, sizeof(val), HZ / 2);
+			(unsigned long)addr, idx & 0x03, &val,
+			sizeof(val), HZ / 2);
 
 	return val;
 }
 
-static inline u16 rtl818x_ioread16(struct rtl8187_priv *priv, __le16 *addr)
+static inline u8 rtl818x_ioread8(struct rtl8187_priv *priv, u8 *addr)
+{
+	return rtl818x_ioread8_idx(priv, addr, 0);
+}
+
+static inline u16 rtl818x_ioread16_idx(struct rtl8187_priv *priv,
+				       __le16 *addr, u8 idx)
 {
 	__le16 val;
 
 	usb_control_msg(priv->udev, usb_rcvctrlpipe(priv->udev, 0),
 			RTL8187_REQ_GET_REG, RTL8187_REQT_READ,
-			(unsigned long)addr, 0, &val, sizeof(val), HZ / 2);
+			(unsigned long)addr, idx & 0x03, &val,
+			sizeof(val), HZ / 2);
 
 	return le16_to_cpu(val);
 }
 
-static inline u32 rtl818x_ioread32(struct rtl8187_priv *priv, __le32 *addr)
+static inline u16 rtl818x_ioread16(struct rtl8187_priv *priv, __le16 *addr)
+{
+	return rtl818x_ioread16_idx(priv, addr, 0);
+}
+
+static inline u32 rtl818x_ioread32_idx(struct rtl8187_priv *priv,
+				       __le32 *addr, u8 idx)
 {
 	__le32 val;
 
 	usb_control_msg(priv->udev, usb_rcvctrlpipe(priv->udev, 0),
 			RTL8187_REQ_GET_REG, RTL8187_REQT_READ,
-			(unsigned long)addr, 0, &val, sizeof(val), HZ / 2);
+			(unsigned long)addr, idx & 0x03, &val,
+			sizeof(val), HZ / 2);
 
 	return le32_to_cpu(val);
 }
 
-static inline void rtl818x_iowrite8(struct rtl8187_priv *priv,
-				    u8 *addr, u8 val)
+static inline u32 rtl818x_ioread32(struct rtl8187_priv *priv, __le32 *addr)
+{
+	return rtl818x_ioread32_idx(priv, addr, 0);
+}
+
+static inline void rtl818x_iowrite8_idx(struct rtl8187_priv *priv,
+					u8 *addr, u8 val, u8 idx)
 {
 	usb_control_msg(priv->udev, usb_sndctrlpipe(priv->udev, 0),
 			RTL8187_REQ_SET_REG, RTL8187_REQT_WRITE,
-			(unsigned long)addr, 0, &val, sizeof(val), HZ / 2);
+			(unsigned long)addr, idx & 0x03, &val,
+			sizeof(val), HZ / 2);
+}
+
+static inline void rtl818x_iowrite8(struct rtl8187_priv *priv, u8 *addr, u8 val)
+{
+	rtl818x_iowrite8_idx(priv, addr, val, 0);
 }
 
-static inline void rtl818x_iowrite16(struct rtl8187_priv *priv,
-				     __le16 *addr, u16 val)
+static inline void rtl818x_iowrite16_idx(struct rtl8187_priv *priv,
+					 __le16 *addr, u16 val, u8 idx)
 {
 	__le16 buf = cpu_to_le16(val);
 
 	usb_control_msg(priv->udev, usb_sndctrlpipe(priv->udev, 0),
 			RTL8187_REQ_SET_REG, RTL8187_REQT_WRITE,
-			(unsigned long)addr, 0, &buf, sizeof(buf), HZ / 2);
+			(unsigned long)addr, idx & 0x03, &buf, sizeof(buf),
+			HZ / 2);
 }
 
-static inline void rtl818x_iowrite32(struct rtl8187_priv *priv,
-				     __le32 *addr, u32 val)
+static inline void rtl818x_iowrite16(struct rtl8187_priv *priv, __le16 *addr,
+				     u16 val)
+{
+	rtl818x_iowrite16_idx(priv, addr, val, 0);
+}
+
+static inline void rtl818x_iowrite32_idx(struct rtl8187_priv *priv,
+					 __le32 *addr, u32 val, u8 idx)
 {
 	__le32 buf = cpu_to_le32(val);
 
 	usb_control_msg(priv->udev, usb_sndctrlpipe(priv->udev, 0),
 			RTL8187_REQ_SET_REG, RTL8187_REQT_WRITE,
-			(unsigned long)addr, 0, &buf, sizeof(buf), HZ / 2);
+			(unsigned long)addr, idx & 0x03, &buf, sizeof(buf),
+			HZ / 2);
+}
+
+static inline void rtl818x_iowrite32(struct rtl8187_priv *priv, __le32 *addr,
+				     u32 val)
+{
+	rtl818x_iowrite32_idx(priv, addr, val, 0);
 }
 
 #endif /* RTL8187_H */
diff --git a/drivers/net/wireless/rtl8187_dev.c b/drivers/net/wireless/rtl8187_dev.c
index 9223ada5f00e..d3067b1216ca 100644
--- a/drivers/net/wireless/rtl8187_dev.c
+++ b/drivers/net/wireless/rtl8187_dev.c
@@ -27,19 +27,21 @@
 
 MODULE_AUTHOR("Michael Wu <flamingice@sourmilk.net>");
 MODULE_AUTHOR("Andrea Merello <andreamrl@tiscali.it>");
-MODULE_DESCRIPTION("RTL8187 USB wireless driver");
+MODULE_DESCRIPTION("RTL8187/RTL8187B USB wireless driver");
 MODULE_LICENSE("GPL");
 
 static struct usb_device_id rtl8187_table[] __devinitdata = {
 	/* Realtek */
-	{USB_DEVICE(0x0bda, 0x8187)},
+	{USB_DEVICE(0x0bda, 0x8187), .driver_info = DEVICE_RTL8187},
+	{USB_DEVICE(0x0bda, 0x8189), .driver_info = DEVICE_RTL8187B},
+	{USB_DEVICE(0x0bda, 0x8197), .driver_info = DEVICE_RTL8187B},
 	/* Netgear */
-	{USB_DEVICE(0x0846, 0x6100)},
-	{USB_DEVICE(0x0846, 0x6a00)},
+	{USB_DEVICE(0x0846, 0x6100), .driver_info = DEVICE_RTL8187},
+	{USB_DEVICE(0x0846, 0x6a00), .driver_info = DEVICE_RTL8187},
 	/* HP */
-	{USB_DEVICE(0x03f0, 0xca02)},
+	{USB_DEVICE(0x03f0, 0xca02), .driver_info = DEVICE_RTL8187},
 	/* Sitecom */
-	{USB_DEVICE(0x0df6, 0x000d)},
+	{USB_DEVICE(0x0df6, 0x000d), .driver_info = DEVICE_RTL8187},
 	{}
 };
 
@@ -150,27 +152,25 @@ void rtl8187_write_phy(struct ieee80211_hw *dev, u8 addr, u32 data)
 
 static void rtl8187_tx_cb(struct urb *urb)
 {
-	struct ieee80211_tx_status status;
 	struct sk_buff *skb = (struct sk_buff *)urb->context;
-	struct rtl8187_tx_info *info = (struct rtl8187_tx_info *)skb->cb;
-
-	memset(&status, 0, sizeof(status));
-
-	usb_free_urb(info->urb);
-	if (info->control)
-		memcpy(&status.control, info->control, sizeof(status.control));
-	kfree(info->control);
-	skb_pull(skb, sizeof(struct rtl8187_tx_hdr));
-	status.flags |= IEEE80211_TX_STATUS_ACK;
-	ieee80211_tx_status_irqsafe(info->dev, skb, &status);
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+	struct ieee80211_hw *hw = info->driver_data[0];
+	struct rtl8187_priv *priv = hw->priv;
+
+	usb_free_urb(info->driver_data[1]);
+	skb_pull(skb, priv->is_rtl8187b ? sizeof(struct rtl8187b_tx_hdr) :
+					  sizeof(struct rtl8187_tx_hdr));
+	memset(&info->status, 0, sizeof(info->status));
+	info->flags |= IEEE80211_TX_STAT_ACK;
+	ieee80211_tx_status_irqsafe(hw, skb);
 }
 
-static int rtl8187_tx(struct ieee80211_hw *dev, struct sk_buff *skb,
-		      struct ieee80211_tx_control *control)
+static int rtl8187_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
 {
 	struct rtl8187_priv *priv = dev->priv;
-	struct rtl8187_tx_hdr *hdr;
-	struct rtl8187_tx_info *info;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+	unsigned int ep;
+	void *buf;
 	struct urb *urb;
 	__le16 rts_dur = 0;
 	u32 flags;
@@ -185,35 +185,60 @@ static int rtl8187_tx(struct ieee80211_hw *dev, struct sk_buff *skb,
 	flags = skb->len;
 	flags |= RTL8187_TX_FLAG_NO_ENCRYPT;
 
-	BUG_ON(!control->tx_rate);
-
-	flags |= control->tx_rate->hw_value << 24;
-	if (ieee80211_get_morefrag((struct ieee80211_hdr *)skb->data))
+	flags |= ieee80211_get_tx_rate(dev, info)->hw_value << 24;
+	if (ieee80211_has_morefrags(((struct ieee80211_hdr *)skb->data)->frame_control))
 		flags |= RTL8187_TX_FLAG_MORE_FRAG;
-	if (control->flags & IEEE80211_TXCTL_USE_RTS_CTS) {
-		BUG_ON(!control->rts_cts_rate);
+	if (info->flags & IEEE80211_TX_CTL_USE_RTS_CTS) {
 		flags |= RTL8187_TX_FLAG_RTS;
-		flags |= control->rts_cts_rate->hw_value << 19;
+		flags |= ieee80211_get_rts_cts_rate(dev, info)->hw_value << 19;
 		rts_dur = ieee80211_rts_duration(dev, priv->vif,
-						 skb->len, control);
-	} else if (control->flags & IEEE80211_TXCTL_USE_CTS_PROTECT) {
-		BUG_ON(!control->rts_cts_rate);
+						 skb->len, info);
+	} else if (info->flags & IEEE80211_TX_CTL_USE_CTS_PROTECT) {
 		flags |= RTL8187_TX_FLAG_CTS;
-		flags |= control->rts_cts_rate->hw_value << 19;
+		flags |= ieee80211_get_rts_cts_rate(dev, info)->hw_value << 19;
 	}
 
-	hdr = (struct rtl8187_tx_hdr *)skb_push(skb, sizeof(*hdr));
-	hdr->flags = cpu_to_le32(flags);
-	hdr->len = 0;
-	hdr->rts_duration = rts_dur;
-	hdr->retry = cpu_to_le32(control->retry_limit << 8);
+	if (!priv->is_rtl8187b) {
+		struct rtl8187_tx_hdr *hdr =
+			(struct rtl8187_tx_hdr *)skb_push(skb, sizeof(*hdr));
+		hdr->flags = cpu_to_le32(flags);
+		hdr->len = 0;
+		hdr->rts_duration = rts_dur;
+		hdr->retry = cpu_to_le32(info->control.retry_limit << 8);
+		buf = hdr;
 
-	info = (struct rtl8187_tx_info *)skb->cb;
-	info->control = kmemdup(control, sizeof(*control), GFP_ATOMIC);
-	info->urb = urb;
-	info->dev = dev;
-	usb_fill_bulk_urb(urb, priv->udev, usb_sndbulkpipe(priv->udev, 2),
-			  hdr, skb->len, rtl8187_tx_cb, skb);
+		ep = 2;
+	} else {
+		/* fc needs to be calculated before skb_push() */
+		unsigned int epmap[4] = { 6, 7, 5, 4 };
+		struct ieee80211_hdr *tx_hdr =
+			(struct ieee80211_hdr *)(skb->data);
+		u16 fc = le16_to_cpu(tx_hdr->frame_control);
+
+		struct rtl8187b_tx_hdr *hdr =
+			(struct rtl8187b_tx_hdr *)skb_push(skb, sizeof(*hdr));
+		struct ieee80211_rate *txrate =
+			ieee80211_get_tx_rate(dev, info);
+		memset(hdr, 0, sizeof(*hdr));
+		hdr->flags = cpu_to_le32(flags);
+		hdr->rts_duration = rts_dur;
+		hdr->retry = cpu_to_le32(info->control.retry_limit << 8);
+		hdr->tx_duration =
+			ieee80211_generic_frame_duration(dev, priv->vif,
+							 skb->len, txrate);
+		buf = hdr;
+
+		if ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_MGMT)
+			ep = 12;
+		else
+			ep = epmap[skb_get_queue_mapping(skb)];
+	}
+
+	info->driver_data[0] = dev;
+	info->driver_data[1] = urb;
+
+	usb_fill_bulk_urb(urb, priv->udev, usb_sndbulkpipe(priv->udev, ep),
+			  buf, skb->len, rtl8187_tx_cb, skb);
 	rc = usb_submit_urb(urb, GFP_ATOMIC);
 	if (rc < 0) {
 		usb_free_urb(urb);
@@ -229,7 +254,6 @@ static void rtl8187_rx_cb(struct urb *urb)
 	struct rtl8187_rx_info *info = (struct rtl8187_rx_info *)skb->cb;
 	struct ieee80211_hw *dev = info->dev;
 	struct rtl8187_priv *priv = dev->priv;
-	struct rtl8187_rx_hdr *hdr;
 	struct ieee80211_rx_status rx_status = { 0 };
 	int rate, signal;
 	u32 flags;
@@ -250,11 +274,33 @@ 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));
-	flags = le32_to_cpu(hdr->flags);
-	skb_trim(skb, flags & 0x0FFF);
+	if (!priv->is_rtl8187b) {
+		struct rtl8187_rx_hdr *hdr =
+			(typeof(hdr))(skb_tail_pointer(skb) - sizeof(*hdr));
+		flags = le32_to_cpu(hdr->flags);
+		signal = hdr->signal & 0x7f;
+		rx_status.antenna = (hdr->signal >> 7) & 1;
+		rx_status.signal = signal;
+		rx_status.noise = hdr->noise;
+		rx_status.mactime = le64_to_cpu(hdr->mac_time);
+		priv->signal = signal;
+		priv->quality = signal;
+		priv->noise = hdr->noise;
+	} else {
+		struct rtl8187b_rx_hdr *hdr =
+			(typeof(hdr))(skb_tail_pointer(skb) - sizeof(*hdr));
+		flags = le32_to_cpu(hdr->flags);
+		signal = hdr->agc >> 1;
+		rx_status.antenna = (hdr->signal >> 7) & 1;
+		rx_status.signal = 64 - min(hdr->noise, (u8)64);
+		rx_status.noise = hdr->noise;
+		rx_status.mactime = le64_to_cpu(hdr->mac_time);
+		priv->signal = hdr->signal;
+		priv->quality = hdr->agc >> 1;
+		priv->noise = hdr->noise;
+	}
 
-	signal = hdr->agc >> 1;
+	skb_trim(skb, flags & 0x0FFF);
 	rate = (flags >> 20) & 0xF;
 	if (rate > 3) {	/* OFDM rate */
 		if (signal > 90)
@@ -270,13 +316,11 @@ static void rtl8187_rx_cb(struct urb *urb)
 		signal = 95 - signal;
 	}
 
-	rx_status.antenna = (hdr->signal >> 7) & 1;
-	rx_status.signal = 64 - min(hdr->noise, (u8)64);
-	rx_status.ssi = signal;
+	rx_status.qual = priv->quality;
+	rx_status.signal = signal;
 	rx_status.rate_idx = rate;
 	rx_status.freq = dev->conf.channel->center_freq;
 	rx_status.band = dev->conf.channel->band;
-	rx_status.mactime = le64_to_cpu(hdr->mac_time);
 	rx_status.flag |= RX_FLAG_TSFT;
 	if (flags & (1 << 13))
 		rx_status.flag |= RX_FLAG_FAILED_FCS_CRC;
@@ -316,7 +360,8 @@ static int rtl8187_init_urbs(struct ieee80211_hw *dev)
 			break;
 		}
 		usb_fill_bulk_urb(entry, priv->udev,
-				  usb_rcvbulkpipe(priv->udev, 1),
+				  usb_rcvbulkpipe(priv->udev,
+				  priv->is_rtl8187b ? 3 : 1),
 				  skb_tail_pointer(skb),
 				  RTL8187_MAX_RX, rtl8187_rx_cb, skb);
 		info = (struct rtl8187_rx_info *)skb->cb;
@@ -329,29 +374,12 @@ static int rtl8187_init_urbs(struct ieee80211_hw *dev)
 	return 0;
 }
 
-static int rtl8187_init_hw(struct ieee80211_hw *dev)
+static int rtl8187_cmd_reset(struct ieee80211_hw *dev)
 {
 	struct rtl8187_priv *priv = dev->priv;
 	u8 reg;
 	int i;
 
-	/* reset */
-	rtl818x_iowrite8(priv, &priv->map->EEPROM_CMD, RTL818X_EEPROM_CMD_CONFIG);
-	reg = rtl818x_ioread8(priv, &priv->map->CONFIG3);
-	rtl818x_iowrite8(priv, &priv->map->CONFIG3, reg | RTL818X_CONFIG3_ANAPARAM_WRITE);
-	rtl818x_iowrite32(priv, &priv->map->ANAPARAM, RTL8225_ANAPARAM_ON);
-	rtl818x_iowrite32(priv, &priv->map->ANAPARAM2, RTL8225_ANAPARAM2_ON);
-	rtl818x_iowrite8(priv, &priv->map->CONFIG3, reg & ~RTL818X_CONFIG3_ANAPARAM_WRITE);
-	rtl818x_iowrite8(priv, &priv->map->EEPROM_CMD, RTL818X_EEPROM_CMD_NORMAL);
-
-	rtl818x_iowrite16(priv, &priv->map->INT_MASK, 0);
-
-	msleep(200);
-	rtl818x_iowrite8(priv, (u8 *)0xFE18, 0x10);
-	rtl818x_iowrite8(priv, (u8 *)0xFE18, 0x11);
-	rtl818x_iowrite8(priv, (u8 *)0xFE18, 0x00);
-	msleep(200);
-
 	reg = rtl818x_ioread8(priv, &priv->map->CMD);
 	reg &= (1 << 1);
 	reg |= RTL818X_CMD_RESET;
@@ -387,12 +415,52 @@ static int rtl8187_init_hw(struct ieee80211_hw *dev)
 		return -ETIMEDOUT;
 	}
 
+	return 0;
+}
+
+static int rtl8187_init_hw(struct ieee80211_hw *dev)
+{
+	struct rtl8187_priv *priv = dev->priv;
+	u8 reg;
+	int res;
+
+	/* reset */
+	rtl818x_iowrite8(priv, &priv->map->EEPROM_CMD,
+			 RTL818X_EEPROM_CMD_CONFIG);
+	reg = rtl818x_ioread8(priv, &priv->map->CONFIG3);
+	rtl818x_iowrite8(priv, &priv->map->CONFIG3, reg |
+			 RTL818X_CONFIG3_ANAPARAM_WRITE);
+	rtl818x_iowrite32(priv, &priv->map->ANAPARAM,
+			  RTL8187_RTL8225_ANAPARAM_ON);
+	rtl818x_iowrite32(priv, &priv->map->ANAPARAM2,
+			  RTL8187_RTL8225_ANAPARAM2_ON);
+	rtl818x_iowrite8(priv, &priv->map->CONFIG3, reg &
+			 ~RTL818X_CONFIG3_ANAPARAM_WRITE);
+	rtl818x_iowrite8(priv, &priv->map->EEPROM_CMD,
+			 RTL818X_EEPROM_CMD_NORMAL);
+
+	rtl818x_iowrite16(priv, &priv->map->INT_MASK, 0);
+
+	msleep(200);
+	rtl818x_iowrite8(priv, (u8 *)0xFE18, 0x10);
+	rtl818x_iowrite8(priv, (u8 *)0xFE18, 0x11);
+	rtl818x_iowrite8(priv, (u8 *)0xFE18, 0x00);
+	msleep(200);
+
+	res = rtl8187_cmd_reset(dev);
+	if (res)
+		return res;
+
 	rtl818x_iowrite8(priv, &priv->map->EEPROM_CMD, RTL818X_EEPROM_CMD_CONFIG);
 	reg = rtl818x_ioread8(priv, &priv->map->CONFIG3);
-	rtl818x_iowrite8(priv, &priv->map->CONFIG3, reg | RTL818X_CONFIG3_ANAPARAM_WRITE);
-	rtl818x_iowrite32(priv, &priv->map->ANAPARAM, RTL8225_ANAPARAM_ON);
-	rtl818x_iowrite32(priv, &priv->map->ANAPARAM2, RTL8225_ANAPARAM2_ON);
-	rtl818x_iowrite8(priv, &priv->map->CONFIG3, reg & ~RTL818X_CONFIG3_ANAPARAM_WRITE);
+	rtl818x_iowrite8(priv, &priv->map->CONFIG3,
+			reg | RTL818X_CONFIG3_ANAPARAM_WRITE);
+	rtl818x_iowrite32(priv, &priv->map->ANAPARAM,
+			  RTL8187_RTL8225_ANAPARAM_ON);
+	rtl818x_iowrite32(priv, &priv->map->ANAPARAM2,
+			  RTL8187_RTL8225_ANAPARAM2_ON);
+	rtl818x_iowrite8(priv, &priv->map->CONFIG3,
+			reg & ~RTL818X_CONFIG3_ANAPARAM_WRITE);
 	rtl818x_iowrite8(priv, &priv->map->EEPROM_CMD, RTL818X_EEPROM_CMD_NORMAL);
 
 	/* setup card */
@@ -437,9 +505,11 @@ static int rtl8187_init_hw(struct ieee80211_hw *dev)
 	rtl818x_iowrite32(priv, &priv->map->RF_TIMING, 0x000a8008);
 	rtl818x_iowrite16(priv, &priv->map->BRSR, 0xFFFF);
 	rtl818x_iowrite32(priv, &priv->map->RF_PARA, 0x00100044);
-	rtl818x_iowrite8(priv, &priv->map->EEPROM_CMD, RTL818X_EEPROM_CMD_CONFIG);
+	rtl818x_iowrite8(priv, &priv->map->EEPROM_CMD,
+			 RTL818X_EEPROM_CMD_CONFIG);
 	rtl818x_iowrite8(priv, &priv->map->CONFIG3, 0x44);
-	rtl818x_iowrite8(priv, &priv->map->EEPROM_CMD, RTL818X_EEPROM_CMD_NORMAL);
+	rtl818x_iowrite8(priv, &priv->map->EEPROM_CMD,
+			 RTL818X_EEPROM_CMD_NORMAL);
 	rtl818x_iowrite16(priv, &priv->map->RFPinsEnable, 0x1FF7);
 	msleep(100);
 
@@ -456,16 +526,201 @@ static int rtl8187_init_hw(struct ieee80211_hw *dev)
 	return 0;
 }
 
+static const u8 rtl8187b_reg_table[][3] = {
+	{0xF0, 0x32, 0}, {0xF1, 0x32, 0}, {0xF2, 0x00, 0}, {0xF3, 0x00, 0},
+	{0xF4, 0x32, 0}, {0xF5, 0x43, 0}, {0xF6, 0x00, 0}, {0xF7, 0x00, 0},
+	{0xF8, 0x46, 0}, {0xF9, 0xA4, 0}, {0xFA, 0x00, 0}, {0xFB, 0x00, 0},
+	{0xFC, 0x96, 0}, {0xFD, 0xA4, 0}, {0xFE, 0x00, 0}, {0xFF, 0x00, 0},
+
+	{0x58, 0x4B, 1}, {0x59, 0x00, 1}, {0x5A, 0x4B, 1}, {0x5B, 0x00, 1},
+	{0x60, 0x4B, 1}, {0x61, 0x09, 1}, {0x62, 0x4B, 1}, {0x63, 0x09, 1},
+	{0xCE, 0x0F, 1}, {0xCF, 0x00, 1}, {0xE0, 0xFF, 1}, {0xE1, 0x0F, 1},
+	{0xE2, 0x00, 1}, {0xF0, 0x4E, 1}, {0xF1, 0x01, 1}, {0xF2, 0x02, 1},
+	{0xF3, 0x03, 1}, {0xF4, 0x04, 1}, {0xF5, 0x05, 1}, {0xF6, 0x06, 1},
+	{0xF7, 0x07, 1}, {0xF8, 0x08, 1},
+
+	{0x4E, 0x00, 2}, {0x0C, 0x04, 2}, {0x21, 0x61, 2}, {0x22, 0x68, 2},
+	{0x23, 0x6F, 2}, {0x24, 0x76, 2}, {0x25, 0x7D, 2}, {0x26, 0x84, 2},
+	{0x27, 0x8D, 2}, {0x4D, 0x08, 2}, {0x50, 0x05, 2}, {0x51, 0xF5, 2},
+	{0x52, 0x04, 2}, {0x53, 0xA0, 2}, {0x54, 0x1F, 2}, {0x55, 0x23, 2},
+	{0x56, 0x45, 2}, {0x57, 0x67, 2}, {0x58, 0x08, 2}, {0x59, 0x08, 2},
+	{0x5A, 0x08, 2}, {0x5B, 0x08, 2}, {0x60, 0x08, 2}, {0x61, 0x08, 2},
+	{0x62, 0x08, 2}, {0x63, 0x08, 2}, {0x64, 0xCF, 2}, {0x72, 0x56, 2},
+	{0x73, 0x9A, 2},
+
+	{0x34, 0xF0, 0}, {0x35, 0x0F, 0}, {0x5B, 0x40, 0}, {0x84, 0x88, 0},
+	{0x85, 0x24, 0}, {0x88, 0x54, 0}, {0x8B, 0xB8, 0}, {0x8C, 0x07, 0},
+	{0x8D, 0x00, 0}, {0x94, 0x1B, 0}, {0x95, 0x12, 0}, {0x96, 0x00, 0},
+	{0x97, 0x06, 0}, {0x9D, 0x1A, 0}, {0x9F, 0x10, 0}, {0xB4, 0x22, 0},
+	{0xBE, 0x80, 0}, {0xDB, 0x00, 0}, {0xEE, 0x00, 0}, {0x91, 0x03, 0},
+
+	{0x4C, 0x00, 2}, {0x9F, 0x00, 3}, {0x8C, 0x01, 0}, {0x8D, 0x10, 0},
+	{0x8E, 0x08, 0}, {0x8F, 0x00, 0}
+};
+
+static int rtl8187b_init_hw(struct ieee80211_hw *dev)
+{
+	struct rtl8187_priv *priv = dev->priv;
+	int res, i;
+	u8 reg;
+
+	rtl818x_iowrite8(priv, &priv->map->EEPROM_CMD,
+			 RTL818X_EEPROM_CMD_CONFIG);
+
+	reg = rtl818x_ioread8(priv, &priv->map->CONFIG3);
+	reg |= RTL818X_CONFIG3_ANAPARAM_WRITE | RTL818X_CONFIG3_GNT_SELECT;
+	rtl818x_iowrite8(priv, &priv->map->CONFIG3, reg);
+	rtl818x_iowrite32(priv, &priv->map->ANAPARAM2,
+			  RTL8187B_RTL8225_ANAPARAM2_ON);
+	rtl818x_iowrite32(priv, &priv->map->ANAPARAM,
+			  RTL8187B_RTL8225_ANAPARAM_ON);
+	rtl818x_iowrite8(priv, &priv->map->ANAPARAM3,
+			 RTL8187B_RTL8225_ANAPARAM3_ON);
+
+	rtl818x_iowrite8(priv, (u8 *)0xFF61, 0x10);
+	reg = rtl818x_ioread8(priv, (u8 *)0xFF62);
+	rtl818x_iowrite8(priv, (u8 *)0xFF62, reg & ~(1 << 5));
+	rtl818x_iowrite8(priv, (u8 *)0xFF62, reg | (1 << 5));
+
+	reg = rtl818x_ioread8(priv, &priv->map->CONFIG3);
+	reg &= ~RTL818X_CONFIG3_ANAPARAM_WRITE;
+	rtl818x_iowrite8(priv, &priv->map->CONFIG3, reg);
+
+	rtl818x_iowrite8(priv, &priv->map->EEPROM_CMD,
+			 RTL818X_EEPROM_CMD_NORMAL);
+
+	res = rtl8187_cmd_reset(dev);
+	if (res)
+		return res;
+
+	rtl818x_iowrite16(priv, (__le16 *)0xFF2D, 0x0FFF);
+	reg = rtl818x_ioread8(priv, &priv->map->CW_CONF);
+	reg |= RTL818X_CW_CONF_PERPACKET_RETRY_SHIFT;
+	rtl818x_iowrite8(priv, &priv->map->CW_CONF, reg);
+	reg = rtl818x_ioread8(priv, &priv->map->TX_AGC_CTL);
+	reg |= RTL818X_TX_AGC_CTL_PERPACKET_GAIN_SHIFT |
+	       RTL818X_TX_AGC_CTL_PERPACKET_ANTSEL_SHIFT;
+	rtl818x_iowrite8(priv, &priv->map->TX_AGC_CTL, reg);
+
+	rtl818x_iowrite16_idx(priv, (__le16 *)0xFFE0, 0x0FFF, 1);
+	reg = rtl818x_ioread8(priv, &priv->map->RATE_FALLBACK);
+	reg |= RTL818X_RATE_FALLBACK_ENABLE;
+	rtl818x_iowrite8(priv, &priv->map->RATE_FALLBACK, reg);
+
+	rtl818x_iowrite16(priv, &priv->map->BEACON_INTERVAL, 100);
+	rtl818x_iowrite16(priv, &priv->map->ATIM_WND, 2);
+	rtl818x_iowrite16_idx(priv, (__le16 *)0xFFD4, 0xFFFF, 1);
+
+	rtl818x_iowrite8(priv, &priv->map->EEPROM_CMD,
+			 RTL818X_EEPROM_CMD_CONFIG);
+	reg = rtl818x_ioread8(priv, &priv->map->CONFIG1);
+	rtl818x_iowrite8(priv, &priv->map->CONFIG1, (reg & 0x3F) | 0x80);
+	rtl818x_iowrite8(priv, &priv->map->EEPROM_CMD,
+			 RTL818X_EEPROM_CMD_NORMAL);
+
+	rtl818x_iowrite8(priv, &priv->map->WPA_CONF, 0);
+	for (i = 0; i < ARRAY_SIZE(rtl8187b_reg_table); i++) {
+		rtl818x_iowrite8_idx(priv,
+				     (u8 *)(uintptr_t)
+				     (rtl8187b_reg_table[i][0] | 0xFF00),
+				     rtl8187b_reg_table[i][1],
+				     rtl8187b_reg_table[i][2]);
+	}
+
+	rtl818x_iowrite16(priv, &priv->map->TID_AC_MAP, 0xFA50);
+	rtl818x_iowrite16(priv, &priv->map->INT_MIG, 0);
+
+	rtl818x_iowrite32_idx(priv, (__le32 *)0xFFF0, 0, 1);
+	rtl818x_iowrite32_idx(priv, (__le32 *)0xFFF4, 0, 1);
+	rtl818x_iowrite8_idx(priv, (u8 *)0xFFF8, 0, 1);
+
+	rtl818x_iowrite32(priv, &priv->map->RF_TIMING, 0x00004001);
+
+	rtl818x_iowrite16_idx(priv, (__le16 *)0xFF72, 0x569A, 2);
+
+	rtl818x_iowrite8(priv, &priv->map->EEPROM_CMD,
+			 RTL818X_EEPROM_CMD_CONFIG);
+	reg = rtl818x_ioread8(priv, &priv->map->CONFIG3);
+	reg |= RTL818X_CONFIG3_ANAPARAM_WRITE;
+	rtl818x_iowrite8(priv, &priv->map->CONFIG3, reg);
+	rtl818x_iowrite8(priv, &priv->map->EEPROM_CMD,
+			 RTL818X_EEPROM_CMD_NORMAL);
+
+	rtl818x_iowrite16(priv, &priv->map->RFPinsOutput, 0x0480);
+	rtl818x_iowrite16(priv, &priv->map->RFPinsSelect, 0x2488);
+	rtl818x_iowrite16(priv, &priv->map->RFPinsEnable, 0x1FFF);
+	msleep(1100);
+
+	priv->rf->init(dev);
+
+	reg = RTL818X_CMD_TX_ENABLE | RTL818X_CMD_RX_ENABLE;
+	rtl818x_iowrite8(priv, &priv->map->CMD, reg);
+	rtl818x_iowrite16(priv, &priv->map->INT_MASK, 0xFFFF);
+
+	rtl818x_iowrite8(priv, (u8 *)0xFE41, 0xF4);
+	rtl818x_iowrite8(priv, (u8 *)0xFE40, 0x00);
+	rtl818x_iowrite8(priv, (u8 *)0xFE42, 0x00);
+	rtl818x_iowrite8(priv, (u8 *)0xFE42, 0x01);
+	rtl818x_iowrite8(priv, (u8 *)0xFE40, 0x0F);
+	rtl818x_iowrite8(priv, (u8 *)0xFE42, 0x00);
+	rtl818x_iowrite8(priv, (u8 *)0xFE42, 0x01);
+
+	reg = rtl818x_ioread8(priv, (u8 *)0xFFDB);
+	rtl818x_iowrite8(priv, (u8 *)0xFFDB, reg | (1 << 2));
+	rtl818x_iowrite16_idx(priv, (__le16 *)0xFF72, 0x59FA, 3);
+	rtl818x_iowrite16_idx(priv, (__le16 *)0xFF74, 0x59D2, 3);
+	rtl818x_iowrite16_idx(priv, (__le16 *)0xFF76, 0x59D2, 3);
+	rtl818x_iowrite16_idx(priv, (__le16 *)0xFF78, 0x19FA, 3);
+	rtl818x_iowrite16_idx(priv, (__le16 *)0xFF7A, 0x19FA, 3);
+	rtl818x_iowrite16_idx(priv, (__le16 *)0xFF7C, 0x00D0, 3);
+	rtl818x_iowrite8(priv, (u8 *)0xFF61, 0);
+	rtl818x_iowrite8_idx(priv, (u8 *)0xFF80, 0x0F, 1);
+	rtl818x_iowrite8_idx(priv, (u8 *)0xFF83, 0x03, 1);
+	rtl818x_iowrite8(priv, (u8 *)0xFFDA, 0x10);
+	rtl818x_iowrite8_idx(priv, (u8 *)0xFF4D, 0x08, 2);
+
+	rtl818x_iowrite32(priv, &priv->map->HSSI_PARA, 0x0600321B);
+
+	rtl818x_iowrite16_idx(priv, (__le16 *)0xFFEC, 0x0800, 1);
+
+	return 0;
+}
+
 static int rtl8187_start(struct ieee80211_hw *dev)
 {
 	struct rtl8187_priv *priv = dev->priv;
 	u32 reg;
 	int ret;
 
-	ret = rtl8187_init_hw(dev);
+	ret = (!priv->is_rtl8187b) ? rtl8187_init_hw(dev) :
+				     rtl8187b_init_hw(dev);
 	if (ret)
 		return ret;
 
+	if (priv->is_rtl8187b) {
+		reg = RTL818X_RX_CONF_MGMT |
+		      RTL818X_RX_CONF_DATA |
+		      RTL818X_RX_CONF_BROADCAST |
+		      RTL818X_RX_CONF_NICMAC |
+		      RTL818X_RX_CONF_BSSID |
+		      (7 << 13 /* RX FIFO threshold NONE */) |
+		      (7 << 10 /* MAX RX DMA */) |
+		      RTL818X_RX_CONF_RX_AUTORESETPHY |
+		      RTL818X_RX_CONF_ONLYERLPKT |
+		      RTL818X_RX_CONF_MULTICAST;
+		priv->rx_conf = reg;
+		rtl818x_iowrite32(priv, &priv->map->RX_CONF, reg);
+
+		rtl818x_iowrite32(priv, &priv->map->TX_CONF,
+				  RTL818X_TX_CONF_HW_SEQNUM |
+				  RTL818X_TX_CONF_DISREQQSIZE |
+				  (7 << 8  /* short retry limit */) |
+				  (7 << 0  /* long retry limit */) |
+				  (7 << 21 /* MAX TX DMA */));
+		rtl8187_init_urbs(dev);
+		return 0;
+	}
+
 	rtl818x_iowrite16(priv, &priv->map->INT_MASK, 0xFFFF);
 
 	rtl818x_iowrite32(priv, &priv->map->MAR[0], ~0);
@@ -592,18 +847,20 @@ static int rtl8187_config(struct ieee80211_hw *dev, struct ieee80211_conf *conf)
 	msleep(10);
 	rtl818x_iowrite32(priv, &priv->map->TX_CONF, reg);
 
-	rtl818x_iowrite8(priv, &priv->map->SIFS, 0x22);
-
-	if (conf->flags & IEEE80211_CONF_SHORT_SLOT_TIME) {
-		rtl818x_iowrite8(priv, &priv->map->SLOT, 0x9);
-		rtl818x_iowrite8(priv, &priv->map->DIFS, 0x14);
-		rtl818x_iowrite8(priv, &priv->map->EIFS, 91 - 0x14);
-		rtl818x_iowrite8(priv, &priv->map->CW_VAL, 0x73);
-	} else {
-		rtl818x_iowrite8(priv, &priv->map->SLOT, 0x14);
-		rtl818x_iowrite8(priv, &priv->map->DIFS, 0x24);
-		rtl818x_iowrite8(priv, &priv->map->EIFS, 91 - 0x24);
-		rtl818x_iowrite8(priv, &priv->map->CW_VAL, 0xa5);
+	if (!priv->is_rtl8187b) {
+		rtl818x_iowrite8(priv, &priv->map->SIFS, 0x22);
+
+		if (conf->flags & IEEE80211_CONF_SHORT_SLOT_TIME) {
+			rtl818x_iowrite8(priv, &priv->map->SLOT, 0x9);
+			rtl818x_iowrite8(priv, &priv->map->DIFS, 0x14);
+			rtl818x_iowrite8(priv, &priv->map->EIFS, 91 - 0x14);
+			rtl818x_iowrite8(priv, &priv->map->CW_VAL, 0x73);
+		} else {
+			rtl818x_iowrite8(priv, &priv->map->SLOT, 0x14);
+			rtl818x_iowrite8(priv, &priv->map->DIFS, 0x24);
+			rtl818x_iowrite8(priv, &priv->map->EIFS, 91 - 0x24);
+			rtl818x_iowrite8(priv, &priv->map->CW_VAL, 0xa5);
+		}
 	}
 
 	rtl818x_iowrite16(priv, &priv->map->ATIM_WND, 2);
@@ -619,14 +876,20 @@ static int rtl8187_config_interface(struct ieee80211_hw *dev,
 {
 	struct rtl8187_priv *priv = dev->priv;
 	int i;
+	u8 reg;
 
 	for (i = 0; i < ETH_ALEN; i++)
 		rtl818x_iowrite8(priv, &priv->map->BSSID[i], conf->bssid[i]);
 
-	if (is_valid_ether_addr(conf->bssid))
-		rtl818x_iowrite8(priv, &priv->map->MSR, RTL818X_MSR_INFRA);
-	else
-		rtl818x_iowrite8(priv, &priv->map->MSR, RTL818X_MSR_NO_LINK);
+	if (is_valid_ether_addr(conf->bssid)) {
+		reg = RTL818X_MSR_INFRA;
+		if (priv->is_rtl8187b)
+			reg |= RTL818X_MSR_ENEDCA;
+		rtl818x_iowrite8(priv, &priv->map->MSR, reg);
+	} else {
+		reg = RTL818X_MSR_NO_LINK;
+		rtl818x_iowrite8(priv, &priv->map->MSR, reg);
+	}
 
 	return 0;
 }
@@ -713,6 +976,7 @@ static int __devinit rtl8187_probe(struct usb_interface *intf,
 	struct rtl8187_priv *priv;
 	struct eeprom_93cx6 eeprom;
 	struct ieee80211_channel *channel;
+	const char *chip_name;
 	u16 txpwr, reg;
 	int err, i;
 	DECLARE_MAC_BUF(mac);
@@ -724,6 +988,7 @@ static int __devinit rtl8187_probe(struct usb_interface *intf,
 	}
 
 	priv = dev->priv;
+	priv->is_rtl8187b = (id->driver_info == DEVICE_RTL8187B);
 
 	SET_IEEE80211_DEV(dev, &intf->dev);
 	usb_set_intfdata(intf, dev);
@@ -750,11 +1015,9 @@ static int __devinit rtl8187_probe(struct usb_interface *intf,
 
 	priv->mode = IEEE80211_IF_TYPE_MNTR;
 	dev->flags = IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING |
-		     IEEE80211_HW_RX_INCLUDES_FCS;
-	dev->extra_tx_headroom = sizeof(struct rtl8187_tx_hdr);
-	dev->queues = 1;
-	dev->max_rssi = 65;
-	dev->max_signal = 64;
+		     IEEE80211_HW_RX_INCLUDES_FCS |
+		     IEEE80211_HW_SIGNAL_UNSPEC;
+	dev->max_signal = 65;
 
 	eeprom.data = dev;
 	eeprom.register_read = rtl8187_eeprom_register_read;
@@ -788,12 +1051,6 @@ static int __devinit rtl8187_probe(struct usb_interface *intf,
 		(*channel++).hw_value = txpwr & 0xFF;
 		(*channel++).hw_value = txpwr >> 8;
 	}
-	for (i = 0; i < 2; i++) {
-		eeprom_93cx6_read(&eeprom, RTL8187_EEPROM_TXPWR_CHAN_6 + i,
-				  &txpwr);
-		(*channel++).hw_value = txpwr & 0xFF;
-		(*channel++).hw_value = txpwr >> 8;
-	}
 
 	eeprom_93cx6_read(&eeprom, RTL8187_EEPROM_TXPWR_BASE,
 			  &priv->txpwr_base);
@@ -807,7 +1064,90 @@ static int __devinit rtl8187_probe(struct usb_interface *intf,
 	rtl818x_iowrite8(priv, &priv->map->PGSELECT, reg);
 	rtl818x_iowrite8(priv, &priv->map->EEPROM_CMD, RTL818X_EEPROM_CMD_NORMAL);
 
+	if (!priv->is_rtl8187b) {
+		u32 reg32;
+		reg32 = rtl818x_ioread32(priv, &priv->map->TX_CONF);
+		reg32 &= RTL818X_TX_CONF_HWVER_MASK;
+		switch (reg32) {
+		case RTL818X_TX_CONF_R8187vD_B:
+			/* Some RTL8187B devices have a USB ID of 0x8187
+			 * detect them here */
+			chip_name = "RTL8187BvB(early)";
+			priv->is_rtl8187b = 1;
+			priv->hw_rev = RTL8187BvB;
+			break;
+		case RTL818X_TX_CONF_R8187vD:
+			chip_name = "RTL8187vD";
+			break;
+		default:
+			chip_name = "RTL8187vB (default)";
+		}
+       } else {
+		/*
+		 * Force USB request to write radio registers for 8187B, Realtek
+		 * only uses it in their sources
+		 */
+		/*if (priv->asic_rev == 0) {
+			printk(KERN_WARNING "rtl8187: Forcing use of USB "
+			       "requests to write to radio registers\n");
+			priv->asic_rev = 1;
+		}*/
+		switch (rtl818x_ioread8(priv, (u8 *)0xFFE1)) {
+		case RTL818X_R8187B_B:
+			chip_name = "RTL8187BvB";
+			priv->hw_rev = RTL8187BvB;
+			break;
+		case RTL818X_R8187B_D:
+			chip_name = "RTL8187BvD";
+			priv->hw_rev = RTL8187BvD;
+			break;
+		case RTL818X_R8187B_E:
+			chip_name = "RTL8187BvE";
+			priv->hw_rev = RTL8187BvE;
+			break;
+		default:
+			chip_name = "RTL8187BvB (default)";
+			priv->hw_rev = RTL8187BvB;
+		}
+	}
+
+	if (!priv->is_rtl8187b) {
+		for (i = 0; i < 2; i++) {
+			eeprom_93cx6_read(&eeprom,
+					  RTL8187_EEPROM_TXPWR_CHAN_6 + i,
+					  &txpwr);
+			(*channel++).hw_value = txpwr & 0xFF;
+			(*channel++).hw_value = txpwr >> 8;
+		}
+	} else {
+		eeprom_93cx6_read(&eeprom, RTL8187_EEPROM_TXPWR_CHAN_6,
+				  &txpwr);
+		(*channel++).hw_value = txpwr & 0xFF;
+
+		eeprom_93cx6_read(&eeprom, 0x0A, &txpwr);
+		(*channel++).hw_value = txpwr & 0xFF;
+
+		eeprom_93cx6_read(&eeprom, 0x1C, &txpwr);
+		(*channel++).hw_value = txpwr & 0xFF;
+		(*channel++).hw_value = txpwr >> 8;
+	}
+
+	if (priv->is_rtl8187b)
+		printk(KERN_WARNING "rtl8187: 8187B chip detected. Support "
+			"is EXPERIMENTAL, and could damage your\n"
+			"         hardware, use at your own risk\n");
+	if ((id->driver_info == DEVICE_RTL8187) && priv->is_rtl8187b)
+		printk(KERN_INFO "rtl8187: inconsistency between id with OEM"
+		       " info!\n");
+
 	priv->rf = rtl8187_detect_rf(dev);
+	dev->extra_tx_headroom = (!priv->is_rtl8187b) ?
+				  sizeof(struct rtl8187_tx_hdr) :
+				  sizeof(struct rtl8187b_tx_hdr);
+	if (!priv->is_rtl8187b)
+		dev->queues = 1;
+	else
+		dev->queues = 4;
 
 	err = ieee80211_register_hw(dev);
 	if (err) {
@@ -815,9 +1155,9 @@ static int __devinit rtl8187_probe(struct usb_interface *intf,
 		goto err_free_dev;
 	}
 
-	printk(KERN_INFO "%s: hwaddr %s, rtl8187 V%d + %s\n",
+	printk(KERN_INFO "%s: hwaddr %s, %s V%d + %s\n",
 	       wiphy_name(dev->wiphy), print_mac(mac, dev->wiphy->perm_addr),
-	       priv->asic_rev, priv->rf->name);
+	       chip_name, priv->asic_rev, priv->rf->name);
 
 	return 0;
 
@@ -847,7 +1187,7 @@ static struct usb_driver rtl8187_driver = {
 	.name		= KBUILD_MODNAME,
 	.id_table	= rtl8187_table,
 	.probe		= rtl8187_probe,
-	.disconnect	= rtl8187_disconnect,
+	.disconnect	= __devexit_p(rtl8187_disconnect),
 };
 
 static int __init rtl8187_init(void)
diff --git a/drivers/net/wireless/rtl8187_rtl8225.c b/drivers/net/wireless/rtl8187_rtl8225.c
index 9146387b4c5e..1bae89903410 100644
--- a/drivers/net/wireless/rtl8187_rtl8225.c
+++ b/drivers/net/wireless/rtl8187_rtl8225.c
@@ -305,9 +305,12 @@ static void rtl8225_rf_set_tx_power(struct ieee80211_hw *dev, int channel)
 	/* anaparam2 on */
 	rtl818x_iowrite8(priv, &priv->map->EEPROM_CMD, RTL818X_EEPROM_CMD_CONFIG);
 	reg = rtl818x_ioread8(priv, &priv->map->CONFIG3);
-	rtl818x_iowrite8(priv, &priv->map->CONFIG3, reg | RTL818X_CONFIG3_ANAPARAM_WRITE);
-	rtl818x_iowrite32(priv, &priv->map->ANAPARAM2, RTL8225_ANAPARAM2_ON);
-	rtl818x_iowrite8(priv, &priv->map->CONFIG3, reg & ~RTL818X_CONFIG3_ANAPARAM_WRITE);
+	rtl818x_iowrite8(priv, &priv->map->CONFIG3,
+			reg | RTL818X_CONFIG3_ANAPARAM_WRITE);
+	rtl818x_iowrite32(priv, &priv->map->ANAPARAM2,
+			  RTL8187_RTL8225_ANAPARAM2_ON);
+	rtl818x_iowrite8(priv, &priv->map->CONFIG3,
+			reg & ~RTL818X_CONFIG3_ANAPARAM_WRITE);
 	rtl818x_iowrite8(priv, &priv->map->EEPROM_CMD, RTL818X_EEPROM_CMD_NORMAL);
 
 	rtl8225_write_phy_ofdm(dev, 2, 0x42);
@@ -471,12 +474,42 @@ static void rtl8225_rf_init(struct ieee80211_hw *dev)
 	rtl8225_write_phy_cck(dev, 0x41, rtl8225_threshold[2]);
 }
 
+static const u8 rtl8225z2_agc[] = {
+	0x5e, 0x5e, 0x5e, 0x5e, 0x5d, 0x5b, 0x59, 0x57, 0x55, 0x53, 0x51, 0x4f,
+	0x4d, 0x4b, 0x49, 0x47, 0x45, 0x43, 0x41, 0x3f, 0x3d, 0x3b, 0x39, 0x37,
+	0x35, 0x33, 0x31, 0x2f, 0x2d, 0x2b, 0x29, 0x27, 0x25, 0x23, 0x21, 0x1f,
+	0x1d, 0x1b, 0x19, 0x17, 0x15, 0x13, 0x11, 0x0f, 0x0d, 0x0b, 0x09, 0x07,
+	0x05, 0x03, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01,
+	0x01, 0x01, 0x01, 0x01, 0x19, 0x19, 0x19, 0x19, 0x19, 0x19, 0x19, 0x19,
+	0x19, 0x20, 0x21, 0x22, 0x23, 0x24, 0x25, 0x26, 0x26, 0x27, 0x27, 0x28,
+	0x28, 0x29, 0x2a, 0x2a, 0x2a, 0x2b, 0x2b, 0x2b, 0x2c, 0x2c, 0x2c, 0x2d,
+	0x2d, 0x2d, 0x2d, 0x2e, 0x2e, 0x2e, 0x2e, 0x2f, 0x2f, 0x2f, 0x30, 0x30,
+	0x31, 0x31, 0x31, 0x31, 0x31, 0x31, 0x31, 0x31, 0x31, 0x31, 0x31, 0x31,
+	0x31, 0x31, 0x31, 0x31, 0x31, 0x31, 0x31, 0x31
+};
+static const u8 rtl8225z2_ofdm[] = {
+	0x10, 0x0d, 0x01, 0x00, 0x14, 0xfb, 0xfb, 0x60,
+	0x00, 0x60, 0x00, 0x00, 0x00, 0x5c, 0x00, 0x00,
+	0x40, 0x00, 0x40, 0x00, 0x00, 0x00, 0xa8, 0x26,
+	0x32, 0x33, 0x07, 0xa5, 0x6f, 0x55, 0xc8, 0xb3,
+	0x0a, 0xe1, 0x2C, 0x8a, 0x86, 0x83, 0x34, 0x0f,
+	0x4f, 0x24, 0x6f, 0xc2, 0x6b, 0x40, 0x80, 0x00,
+	0xc0, 0xc1, 0x58, 0xf1, 0x00, 0xe4, 0x90, 0x3e,
+	0x6d, 0x3c, 0xfb, 0x07
+};
+
 static const u8 rtl8225z2_tx_power_cck_ch14[] = {
-	0x36, 0x35, 0x2e, 0x1b, 0x00, 0x00, 0x00, 0x00
+	0x36, 0x35, 0x2e, 0x1b, 0x00, 0x00, 0x00, 0x00,
+	0x30, 0x2f, 0x29, 0x15, 0x00, 0x00, 0x00, 0x00,
+	0x30, 0x2f, 0x29, 0x15, 0x00, 0x00, 0x00, 0x00,
+	0x30, 0x2f, 0x29, 0x15, 0x00, 0x00, 0x00, 0x00
 };
 
 static const u8 rtl8225z2_tx_power_cck[] = {
-	0x36, 0x35, 0x2e, 0x25, 0x1c, 0x12, 0x09, 0x04
+	0x36, 0x35, 0x2e, 0x25, 0x1c, 0x12, 0x09, 0x04,
+	0x30, 0x2f, 0x29, 0x21, 0x19, 0x10, 0x08, 0x03,
+	0x2b, 0x2a, 0x25, 0x1e, 0x16, 0x0e, 0x07, 0x03,
+	0x26, 0x25, 0x21, 0x1b, 0x14, 0x0d, 0x06, 0x03
 };
 
 static const u8 rtl8225z2_tx_power_ofdm[] = {
@@ -526,9 +559,12 @@ static void rtl8225z2_rf_set_tx_power(struct ieee80211_hw *dev, int channel)
 	/* anaparam2 on */
 	rtl818x_iowrite8(priv, &priv->map->EEPROM_CMD, RTL818X_EEPROM_CMD_CONFIG);
 	reg = rtl818x_ioread8(priv, &priv->map->CONFIG3);
-	rtl818x_iowrite8(priv, &priv->map->CONFIG3, reg | RTL818X_CONFIG3_ANAPARAM_WRITE);
-	rtl818x_iowrite32(priv, &priv->map->ANAPARAM2, RTL8225_ANAPARAM2_ON);
-	rtl818x_iowrite8(priv, &priv->map->CONFIG3, reg & ~RTL818X_CONFIG3_ANAPARAM_WRITE);
+	rtl818x_iowrite8(priv, &priv->map->CONFIG3,
+			reg | RTL818X_CONFIG3_ANAPARAM_WRITE);
+	rtl818x_iowrite32(priv, &priv->map->ANAPARAM2,
+			  RTL8187_RTL8225_ANAPARAM2_ON);
+	rtl818x_iowrite8(priv, &priv->map->CONFIG3,
+			reg & ~RTL818X_CONFIG3_ANAPARAM_WRITE);
 	rtl818x_iowrite8(priv, &priv->map->EEPROM_CMD, RTL818X_EEPROM_CMD_NORMAL);
 
 	rtl8225_write_phy_ofdm(dev, 2, 0x42);
@@ -542,6 +578,85 @@ static void rtl8225z2_rf_set_tx_power(struct ieee80211_hw *dev, int channel)
 	msleep(1);
 }
 
+static void rtl8225z2_b_rf_set_tx_power(struct ieee80211_hw *dev, int channel)
+{
+	struct rtl8187_priv *priv = dev->priv;
+	u8 cck_power, ofdm_power;
+	const u8 *tmp;
+	int i;
+
+	cck_power = priv->channels[channel - 1].hw_value & 0xF;
+	ofdm_power = priv->channels[channel - 1].hw_value >> 4;
+
+	if (cck_power > 15)
+		cck_power = (priv->hw_rev == RTL8187BvB) ? 15 : 22;
+	else
+		cck_power += (priv->hw_rev == RTL8187BvB) ? 0 : 7;
+	cck_power += priv->txpwr_base & 0xF;
+	cck_power = min(cck_power, (u8)35);
+
+	if (ofdm_power > 15)
+		ofdm_power = (priv->hw_rev == RTL8187BvB) ? 17 : 25;
+	else
+		ofdm_power += (priv->hw_rev == RTL8187BvB) ? 2 : 10;
+	ofdm_power += (priv->txpwr_base >> 4) & 0xF;
+	ofdm_power = min(ofdm_power, (u8)35);
+
+	if (channel == 14)
+		tmp = rtl8225z2_tx_power_cck_ch14;
+	else
+		tmp = rtl8225z2_tx_power_cck;
+
+	if (priv->hw_rev == RTL8187BvB) {
+		if (cck_power <= 6)
+			; /* do nothing */
+		else if (cck_power <= 11)
+			tmp += 8;
+		else
+			tmp += 16;
+	} else {
+		if (cck_power <= 5)
+			; /* do nothing */
+		else if (cck_power <= 11)
+			tmp += 8;
+		else if (cck_power <= 17)
+			tmp += 16;
+		else
+			tmp += 24;
+	}
+
+	for (i = 0; i < 8; i++)
+		rtl8225_write_phy_cck(dev, 0x44 + i, *tmp++);
+
+	rtl818x_iowrite8(priv, &priv->map->TX_GAIN_CCK,
+			 rtl8225z2_tx_gain_cck_ofdm[cck_power]);
+	msleep(1);
+
+	rtl818x_iowrite8(priv, &priv->map->TX_GAIN_OFDM,
+			 rtl8225z2_tx_gain_cck_ofdm[ofdm_power] << 1);
+	if (priv->hw_rev == RTL8187BvB) {
+		if (ofdm_power <= 11) {
+			rtl8225_write_phy_ofdm(dev, 0x87, 0x60);
+			rtl8225_write_phy_ofdm(dev, 0x89, 0x60);
+		} else {
+			rtl8225_write_phy_ofdm(dev, 0x87, 0x5c);
+			rtl8225_write_phy_ofdm(dev, 0x89, 0x5c);
+		}
+	} else {
+		if (ofdm_power <= 11) {
+			rtl8225_write_phy_ofdm(dev, 0x87, 0x5c);
+			rtl8225_write_phy_ofdm(dev, 0x89, 0x5c);
+		} else if (ofdm_power <= 17) {
+			rtl8225_write_phy_ofdm(dev, 0x87, 0x54);
+			rtl8225_write_phy_ofdm(dev, 0x89, 0x54);
+		} else {
+			rtl8225_write_phy_ofdm(dev, 0x87, 0x50);
+			rtl8225_write_phy_ofdm(dev, 0x89, 0x50);
+		}
+	}
+	msleep(1);
+}
+
 static const u16 rtl8225z2_rxgain[] = {
 	0x0400, 0x0401, 0x0402, 0x0403, 0x0404, 0x0405, 0x0408, 0x0409,
 	0x040a, 0x040b, 0x0502, 0x0503, 0x0504, 0x0505, 0x0540, 0x0541,
@@ -715,6 +830,81 @@ static void rtl8225z2_rf_init(struct ieee80211_hw *dev)
 	rtl818x_iowrite32(priv, (__le32 *)0xFF94, 0x3dc00002);
 }
 
+static void rtl8225z2_b_rf_init(struct ieee80211_hw *dev)
+{
+	struct rtl8187_priv *priv = dev->priv;
+	int i;
+
+	rtl8225_write(dev, 0x0, 0x0B7); msleep(1);
+	rtl8225_write(dev, 0x1, 0xEE0); msleep(1);
+	rtl8225_write(dev, 0x2, 0x44D); msleep(1);
+	rtl8225_write(dev, 0x3, 0x441); msleep(1);
+	rtl8225_write(dev, 0x4, 0x8C3); msleep(1);
+	rtl8225_write(dev, 0x5, 0xC72); msleep(1);
+	rtl8225_write(dev, 0x6, 0x0E6); msleep(1);
+	rtl8225_write(dev, 0x7, 0x82A); msleep(1);
+	rtl8225_write(dev, 0x8, 0x03F); msleep(1);
+	rtl8225_write(dev, 0x9, 0x335); msleep(1);
+	rtl8225_write(dev, 0xa, 0x9D4); msleep(1);
+	rtl8225_write(dev, 0xb, 0x7BB); msleep(1);
+	rtl8225_write(dev, 0xc, 0x850); msleep(1);
+	rtl8225_write(dev, 0xd, 0xCDF); msleep(1);
+	rtl8225_write(dev, 0xe, 0x02B); msleep(1);
+	rtl8225_write(dev, 0xf, 0x114); msleep(1);
+
+	rtl8225_write(dev, 0x0, 0x1B7); msleep(1);
+
+	for (i = 0; i < ARRAY_SIZE(rtl8225z2_rxgain); i++) {
+		rtl8225_write(dev, 0x1, i + 1); msleep(1);
+		rtl8225_write(dev, 0x2, rtl8225z2_rxgain[i]); msleep(1);
+	}
+
+	rtl8225_write(dev, 0x3, 0x080); msleep(1);
+	rtl8225_write(dev, 0x5, 0x004); msleep(1);
+	rtl8225_write(dev, 0x0, 0x0B7); msleep(1);
+	msleep(3000);
+
+	rtl8225_write(dev, 0x2, 0xC4D); msleep(1);
+	msleep(2000);
+
+	rtl8225_write(dev, 0x2, 0x44D); msleep(1);
+	rtl8225_write(dev, 0x0, 0x2BF); msleep(1);
+
+	rtl818x_iowrite8(priv, &priv->map->TX_GAIN_CCK, 0x03);
+	rtl818x_iowrite8(priv, &priv->map->TX_GAIN_OFDM, 0x07);
+	rtl818x_iowrite8(priv, &priv->map->TX_ANTENNA, 0x03);
+
+	rtl8225_write_phy_ofdm(dev, 0x80, 0x12);
+	for (i = 0; i < ARRAY_SIZE(rtl8225z2_agc); i++) {
+		rtl8225_write_phy_ofdm(dev, 0xF, rtl8225z2_agc[i]);
+		rtl8225_write_phy_ofdm(dev, 0xE, 0x80 + i);
+		rtl8225_write_phy_ofdm(dev, 0xE, 0);
+	}
+	rtl8225_write_phy_ofdm(dev, 0x80, 0x10);
+
+	for (i = 0; i < ARRAY_SIZE(rtl8225z2_ofdm); i++)
+		rtl8225_write_phy_ofdm(dev, i, rtl8225z2_ofdm[i]);
+
+	rtl818x_iowrite8(priv, &priv->map->SIFS, 0x22);
+	rtl818x_iowrite8(priv, &priv->map->SLOT, 9);
+	rtl818x_iowrite8(priv, (u8 *)0xFFF0, 28);
+	rtl818x_iowrite8(priv, (u8 *)0xFFF4, 28);
+	rtl818x_iowrite8(priv, (u8 *)0xFFF8, 28);
+	rtl818x_iowrite8(priv, (u8 *)0xFFFC, 28);
+	rtl818x_iowrite8(priv, (u8 *)0xFF2D, 0x5B);
+	rtl818x_iowrite8(priv, (u8 *)0xFF79, 0x5B);
+	rtl818x_iowrite32(priv, (__le32 *)0xFFF0, (7 << 12) | (3 << 8) | 28);
+	rtl818x_iowrite32(priv, (__le32 *)0xFFF4, (7 << 12) | (3 << 8) | 28);
+	rtl818x_iowrite32(priv, (__le32 *)0xFFF8, (7 << 12) | (3 << 8) | 28);
+	rtl818x_iowrite32(priv, (__le32 *)0xFFFC, (7 << 12) | (3 << 8) | 28);
+	rtl818x_iowrite8(priv, &priv->map->ACM_CONTROL, 0);
+
+	rtl8225_write_phy_ofdm(dev, 0x97, 0x46); msleep(1);
+	rtl8225_write_phy_ofdm(dev, 0xa4, 0xb6); msleep(1);
+	rtl8225_write_phy_ofdm(dev, 0x85, 0xfc); msleep(1);
+	rtl8225_write_phy_cck(dev, 0xc1, 0x88); msleep(1);
+}
+
 static void rtl8225_rf_stop(struct ieee80211_hw *dev)
 {
 	u8 reg;
@@ -725,8 +915,19 @@ static void rtl8225_rf_stop(struct ieee80211_hw *dev)
 	rtl818x_iowrite8(priv, &priv->map->EEPROM_CMD, RTL818X_EEPROM_CMD_CONFIG);
 	reg = rtl818x_ioread8(priv, &priv->map->CONFIG3);
 	rtl818x_iowrite8(priv, &priv->map->CONFIG3, reg | RTL818X_CONFIG3_ANAPARAM_WRITE);
-	rtl818x_iowrite32(priv, &priv->map->ANAPARAM2, RTL8225_ANAPARAM2_OFF);
-	rtl818x_iowrite32(priv, &priv->map->ANAPARAM, RTL8225_ANAPARAM_OFF);
+	if (!priv->is_rtl8187b) {
+		rtl818x_iowrite32(priv, &priv->map->ANAPARAM2,
+				  RTL8187_RTL8225_ANAPARAM2_OFF);
+		rtl818x_iowrite32(priv, &priv->map->ANAPARAM,
+				  RTL8187_RTL8225_ANAPARAM_OFF);
+	} else {
+		rtl818x_iowrite32(priv, &priv->map->ANAPARAM2,
+				  RTL8187B_RTL8225_ANAPARAM2_OFF);
+		rtl818x_iowrite32(priv, &priv->map->ANAPARAM,
+				  RTL8187B_RTL8225_ANAPARAM_OFF);
+		rtl818x_iowrite8(priv, &priv->map->ANAPARAM3,
+				  RTL8187B_RTL8225_ANAPARAM3_OFF);
+	}
 	rtl818x_iowrite8(priv, &priv->map->CONFIG3, reg & ~RTL818X_CONFIG3_ANAPARAM_WRITE);
 	rtl818x_iowrite8(priv, &priv->map->EEPROM_CMD, RTL818X_EEPROM_CMD_NORMAL);
 }
@@ -739,8 +940,10 @@ static void rtl8225_rf_set_channel(struct ieee80211_hw *dev,
 
 	if (priv->rf->init == rtl8225_rf_init)
 		rtl8225_rf_set_tx_power(dev, chan);
-	else
+	else if (priv->rf->init == rtl8225z2_rf_init)
 		rtl8225z2_rf_set_tx_power(dev, chan);
+	else
+		rtl8225z2_b_rf_set_tx_power(dev, chan);
 
 	rtl8225_write(dev, 0x7, rtl8225_chan[chan - 1]);
 	msleep(10);
@@ -760,19 +963,30 @@ static const struct rtl818x_rf_ops rtl8225z2_ops = {
 	.set_chan	= rtl8225_rf_set_channel
 };
 
+static const struct rtl818x_rf_ops rtl8225z2_b_ops = {
+	.name		= "rtl8225z2",
+	.init		= rtl8225z2_b_rf_init,
+	.stop		= rtl8225_rf_stop,
+	.set_chan	= rtl8225_rf_set_channel
+};
+
 const struct rtl818x_rf_ops * rtl8187_detect_rf(struct ieee80211_hw *dev)
 {
 	u16 reg8, reg9;
+	struct rtl8187_priv *priv = dev->priv;
 
-	rtl8225_write(dev, 0, 0x1B7);
+	if (!priv->is_rtl8187b) {
+		rtl8225_write(dev, 0, 0x1B7);
 
-	reg8 = rtl8225_read(dev, 8);
-	reg9 = rtl8225_read(dev, 9);
+		reg8 = rtl8225_read(dev, 8);
+		reg9 = rtl8225_read(dev, 9);
 
-	rtl8225_write(dev, 0, 0x0B7);
+		rtl8225_write(dev, 0, 0x0B7);
 
-	if (reg8 != 0x588 || reg9 != 0x700)
-		return &rtl8225_ops;
+		if (reg8 != 0x588 || reg9 != 0x700)
+			return &rtl8225_ops;
 
-	return &rtl8225z2_ops;
+		return &rtl8225z2_ops;
+	} else
+		return &rtl8225z2_b_ops;
 }
diff --git a/drivers/net/wireless/rtl8187_rtl8225.h b/drivers/net/wireless/rtl8187_rtl8225.h
index d39ed0295b6e..20c5b6ead0f6 100644
--- a/drivers/net/wireless/rtl8187_rtl8225.h
+++ b/drivers/net/wireless/rtl8187_rtl8225.h
@@ -15,10 +15,17 @@
 #ifndef RTL8187_RTL8225_H
 #define RTL8187_RTL8225_H
 
-#define RTL8225_ANAPARAM_ON	0xa0000a59
-#define RTL8225_ANAPARAM2_ON	0x860c7312
-#define RTL8225_ANAPARAM_OFF	0xa00beb59
-#define RTL8225_ANAPARAM2_OFF	0x840dec11
+#define RTL8187_RTL8225_ANAPARAM_ON	0xa0000a59
+#define RTL8187_RTL8225_ANAPARAM2_ON	0x860c7312
+#define RTL8187_RTL8225_ANAPARAM_OFF	0xa00beb59
+#define RTL8187_RTL8225_ANAPARAM2_OFF	0x840dec11
+
+#define RTL8187B_RTL8225_ANAPARAM_ON	0x45090658
+#define RTL8187B_RTL8225_ANAPARAM2_ON	0x727f3f52
+#define RTL8187B_RTL8225_ANAPARAM3_ON	0x00
+#define RTL8187B_RTL8225_ANAPARAM_OFF	0x55480658
+#define RTL8187B_RTL8225_ANAPARAM2_OFF	0x72003f50
+#define RTL8187B_RTL8225_ANAPARAM3_OFF	0x00
 
 const struct rtl818x_rf_ops * rtl8187_detect_rf(struct ieee80211_hw *);
 
diff --git a/drivers/net/wireless/rtl818x.h b/drivers/net/wireless/rtl818x.h
index 4f7d38f506eb..00900fe16fce 100644
--- a/drivers/net/wireless/rtl818x.h
+++ b/drivers/net/wireless/rtl818x.h
@@ -66,7 +66,10 @@ struct rtl818x_csr {
 #define RTL818X_TX_CONF_R8180_F		(3 << 25)
 #define RTL818X_TX_CONF_R8185_ABC	(4 << 25)
 #define RTL818X_TX_CONF_R8185_D		(5 << 25)
+#define RTL818X_TX_CONF_R8187vD		(5 << 25)
+#define RTL818X_TX_CONF_R8187vD_B	(6 << 25)
 #define RTL818X_TX_CONF_HWVER_MASK	(7 << 25)
+#define RTL818X_TX_CONF_DISREQQSIZE	(1 << 28)
 #define RTL818X_TX_CONF_PROBE_DTS	(1 << 29)
 #define RTL818X_TX_CONF_HW_SEQNUM	(1 << 30)
 #define RTL818X_TX_CONF_CW_MIN		(1 << 31)
@@ -106,8 +109,11 @@ struct rtl818x_csr {
 #define RTL818X_MSR_NO_LINK		(0 << 2)
 #define RTL818X_MSR_ADHOC		(1 << 2)
 #define RTL818X_MSR_INFRA		(2 << 2)
+#define RTL818X_MSR_MASTER		(3 << 2)
+#define RTL818X_MSR_ENEDCA		(4 << 2)
 	u8	CONFIG3;
 #define RTL818X_CONFIG3_ANAPARAM_WRITE	(1 << 6)
+#define RTL818X_CONFIG3_GNT_SELECT	(1 << 7)
 	u8	CONFIG4;
 #define RTL818X_CONFIG4_POWEROFF	(1 << 6)
 #define RTL818X_CONFIG4_VCOOFF		(1 << 7)
@@ -133,7 +139,9 @@ struct rtl818x_csr {
 	__le32	RF_TIMING;
 	u8	GP_ENABLE;
 	u8	GPIO;
-	u8	reserved_12[10];
+	u8	reserved_12[2];
+	__le32	HSSI_PARA;
+	u8	reserved_13[4];
 	u8	TX_AGC_CTL;
 #define RTL818X_TX_AGC_CTL_PERPACKET_GAIN_SHIFT		(1 << 0)
 #define RTL818X_TX_AGC_CTL_PERPACKET_ANTSEL_SHIFT	(1 << 1)
@@ -141,29 +149,39 @@ struct rtl818x_csr {
 	u8	TX_GAIN_CCK;
 	u8	TX_GAIN_OFDM;
 	u8	TX_ANTENNA;
-	u8	reserved_13[16];
+	u8	reserved_14[16];
 	u8	WPA_CONF;
-	u8	reserved_14[3];
+	u8	reserved_15[3];
 	u8	SIFS;
 	u8	DIFS;
 	u8	SLOT;
-	u8	reserved_15[5];
+	u8	reserved_16[5];
 	u8	CW_CONF;
 #define RTL818X_CW_CONF_PERPACKET_CW_SHIFT	(1 << 0)
 #define RTL818X_CW_CONF_PERPACKET_RETRY_SHIFT	(1 << 1)
 	u8	CW_VAL;
 	u8	RATE_FALLBACK;
-	u8	reserved_16[25];
+#define RTL818X_RATE_FALLBACK_ENABLE	(1 << 7)
+	u8	ACM_CONTROL;
+	u8	reserved_17[24];
 	u8	CONFIG5;
 	u8	TX_DMA_POLLING;
-	u8	reserved_17[2];
+	u8	reserved_18[2];
 	__le16	CWR;
 	u8	RETRY_CTR;
-	u8	reserved_18[5];
+	u8	reserved_19[3];
+	__le16	INT_MIG;
+/* RTL818X_R8187B_*: magic numbers from ioregisters */
+#define RTL818X_R8187B_B	0
+#define RTL818X_R8187B_D	1
+#define RTL818X_R8187B_E	2
 	__le32	RDSAR;
-	u8	reserved_19[12];
-	__le16	FEMR;
+	__le16	TID_AC_MAP;
 	u8	reserved_20[4];
+	u8	ANAPARAM3;
+	u8	reserved_21[5];
+	__le16	FEMR;
+	u8	reserved_22[4];
 	__le16	TALLY_CNT;
 	u8	TALLY_SEL;
 } __attribute__((packed));
diff --git a/drivers/net/wireless/strip.c b/drivers/net/wireless/strip.c
index 883af891ebfb..417e9e675fac 100644
--- a/drivers/net/wireless/strip.c
+++ b/drivers/net/wireless/strip.c
@@ -2728,7 +2728,7 @@ static int strip_ioctl(struct tty_struct *tty, struct file *file,
 /************************************************************************/
 /* Initialization							*/
 
-static struct tty_ldisc strip_ldisc = {
+static struct tty_ldisc_ops strip_ldisc = {
 	.magic = TTY_LDISC_MAGIC,
 	.name = "strip",
 	.owner = THIS_MODULE,
diff --git a/drivers/net/wireless/wl3501_cs.c b/drivers/net/wireless/wl3501_cs.c
index 42a36b3f3ff7..377141995e36 100644
--- a/drivers/net/wireless/wl3501_cs.c
+++ b/drivers/net/wireless/wl3501_cs.c
@@ -1624,25 +1624,25 @@ static int wl3501_get_scan(struct net_device *dev, struct iw_request_info *info,
 		iwe.cmd			= SIOCGIWAP;
 		iwe.u.ap_addr.sa_family = ARPHRD_ETHER;
 		memcpy(iwe.u.ap_addr.sa_data, this->bss_set[i].bssid, ETH_ALEN);
-		current_ev = iwe_stream_add_event(current_ev,
+		current_ev = iwe_stream_add_event(info, current_ev,
 						  extra + IW_SCAN_MAX_DATA,
 						  &iwe, IW_EV_ADDR_LEN);
 		iwe.cmd		  = SIOCGIWESSID;
 		iwe.u.data.flags  = 1;
 		iwe.u.data.length = this->bss_set[i].ssid.el.len;
-		current_ev = iwe_stream_add_point(current_ev,
+		current_ev = iwe_stream_add_point(info, current_ev,
 						  extra + IW_SCAN_MAX_DATA,
 						  &iwe,
 						  this->bss_set[i].ssid.essid);
 		iwe.cmd	   = SIOCGIWMODE;
 		iwe.u.mode = this->bss_set[i].bss_type;
-		current_ev = iwe_stream_add_event(current_ev,
+		current_ev = iwe_stream_add_event(info, current_ev,
 						  extra + IW_SCAN_MAX_DATA,
 						  &iwe, IW_EV_UINT_LEN);
 		iwe.cmd = SIOCGIWFREQ;
 		iwe.u.freq.m = this->bss_set[i].ds_pset.chan;
 		iwe.u.freq.e = 0;
-		current_ev = iwe_stream_add_event(current_ev,
+		current_ev = iwe_stream_add_event(info, current_ev,
 						  extra + IW_SCAN_MAX_DATA,
 						  &iwe, IW_EV_FREQ_LEN);
 		iwe.cmd = SIOCGIWENCODE;
@@ -1651,7 +1651,7 @@ static int wl3501_get_scan(struct net_device *dev, struct iw_request_info *info,
 		else
 			iwe.u.data.flags = IW_ENCODE_DISABLED;
 		iwe.u.data.length = 0;
-		current_ev = iwe_stream_add_point(current_ev,
+		current_ev = iwe_stream_add_point(info, current_ev,
 						  extra + IW_SCAN_MAX_DATA,
 						  &iwe, NULL);
 	}
diff --git a/drivers/net/wireless/zd1201.c b/drivers/net/wireless/zd1201.c
index d5c0c66188ca..b16ec6e5f0e3 100644
--- a/drivers/net/wireless/zd1201.c
+++ b/drivers/net/wireless/zd1201.c
@@ -49,7 +49,7 @@ MODULE_DEVICE_TABLE(usb, zd1201_table);
 static int zd1201_fw_upload(struct usb_device *dev, int apfw)
 {
 	const struct firmware *fw_entry;
-	char *data;
+	const char *data;
 	unsigned long len;
 	int err;
 	unsigned char ret;
@@ -1152,32 +1152,36 @@ static int zd1201_get_scan(struct net_device *dev,
 		iwe.cmd = SIOCGIWAP;
 		iwe.u.ap_addr.sa_family = ARPHRD_ETHER;
 		memcpy(iwe.u.ap_addr.sa_data, zd->rxdata+i+6, 6);
-		cev = iwe_stream_add_event(cev, end_buf, &iwe, IW_EV_ADDR_LEN);
+		cev = iwe_stream_add_event(info, cev, end_buf,
+					   &iwe, IW_EV_ADDR_LEN);
 
 		iwe.cmd = SIOCGIWESSID;
 		iwe.u.data.length = zd->rxdata[i+16];
 		iwe.u.data.flags = 1;
-		cev = iwe_stream_add_point(cev, end_buf, &iwe, zd->rxdata+i+18);
+		cev = iwe_stream_add_point(info, cev, end_buf,
+					   &iwe, zd->rxdata+i+18);
 
 		iwe.cmd = SIOCGIWMODE;
 		if (zd->rxdata[i+14]&0x01)
 			iwe.u.mode = IW_MODE_MASTER;
 		else
 			iwe.u.mode = IW_MODE_ADHOC;
-		cev = iwe_stream_add_event(cev, end_buf, &iwe, IW_EV_UINT_LEN);
+		cev = iwe_stream_add_event(info, cev, end_buf,
+					   &iwe, IW_EV_UINT_LEN);
 		
 		iwe.cmd = SIOCGIWFREQ;
 		iwe.u.freq.m = zd->rxdata[i+0];
 		iwe.u.freq.e = 0;
-		cev = iwe_stream_add_event(cev, end_buf, &iwe, IW_EV_FREQ_LEN);
+		cev = iwe_stream_add_event(info, cev, end_buf,
+					   &iwe, IW_EV_FREQ_LEN);
 		
 		iwe.cmd = SIOCGIWRATE;
 		iwe.u.bitrate.fixed = 0;
 		iwe.u.bitrate.disabled = 0;
 		for (j=0; j<10; j++) if (zd->rxdata[i+50+j]) {
 			iwe.u.bitrate.value = (zd->rxdata[i+50+j]&0x7f)*500000;
-			cev=iwe_stream_add_event(cev, end_buf, &iwe,
-			    IW_EV_PARAM_LEN);
+			cev = iwe_stream_add_event(info, cev, end_buf,
+						   &iwe, IW_EV_PARAM_LEN);
 		}
 		
 		iwe.cmd = SIOCGIWENCODE;
@@ -1186,14 +1190,15 @@ static int zd1201_get_scan(struct net_device *dev,
 			iwe.u.data.flags = IW_ENCODE_ENABLED;
 		else
 			iwe.u.data.flags = IW_ENCODE_DISABLED;
-		cev = iwe_stream_add_point(cev, end_buf, &iwe, NULL);
+		cev = iwe_stream_add_point(info, cev, end_buf, &iwe, NULL);
 		
 		iwe.cmd = IWEVQUAL;
 		iwe.u.qual.qual = zd->rxdata[i+4];
 		iwe.u.qual.noise= zd->rxdata[i+2]/10-100;
 		iwe.u.qual.level = (256+zd->rxdata[i+4]*100)/255-100;
 		iwe.u.qual.updated = 7;
-		cev = iwe_stream_add_event(cev, end_buf, &iwe, IW_EV_QUAL_LEN);
+		cev = iwe_stream_add_event(info, cev, end_buf,
+					   &iwe, IW_EV_QUAL_LEN);
 	}
 
 	if (!enabled_save)
diff --git a/drivers/net/wireless/zd1211rw/zd_mac.c b/drivers/net/wireless/zd1211rw/zd_mac.c
index 694e95d35fd4..fcc532bb6a7e 100644
--- a/drivers/net/wireless/zd1211rw/zd_mac.c
+++ b/drivers/net/wireless/zd1211rw/zd_mac.c
@@ -224,36 +224,6 @@ out:
 	return r;
 }
 
-/**
- * clear_tx_skb_control_block - clears the control block of tx skbuffs
- * @skb: a &struct sk_buff pointer
- *
- * This clears the control block of skbuff buffers, which were transmitted to
- * the device. Notify that the function is not thread-safe, so prevent
- * multiple calls.
- */
-static void clear_tx_skb_control_block(struct sk_buff *skb)
-{
-	struct zd_tx_skb_control_block *cb =
-		(struct zd_tx_skb_control_block *)skb->cb;
-
-	kfree(cb->control);
-	cb->control = NULL;
-}
-
-/**
- * kfree_tx_skb - frees a tx skbuff
- * @skb: a &struct sk_buff pointer
- *
- * Frees the tx skbuff. Frees also the allocated control structure in the
- * control block if necessary.
- */
-static void kfree_tx_skb(struct sk_buff *skb)
-{
-	clear_tx_skb_control_block(skb);
-	dev_kfree_skb_any(skb);
-}
-
 static void zd_op_stop(struct ieee80211_hw *hw)
 {
 	struct zd_mac *mac = zd_hw_mac(hw);
@@ -276,40 +246,15 @@ static void zd_op_stop(struct ieee80211_hw *hw)
 
 
 	while ((skb = skb_dequeue(ack_wait_queue)))
-		kfree_tx_skb(skb);
-}
-
-/**
- * init_tx_skb_control_block - initializes skb control block
- * @skb: a &sk_buff pointer
- * @dev: pointer to the mac80221 device
- * @control: mac80211 tx control applying for the frame in @skb
- *
- * Initializes the control block of the skbuff to be transmitted.
- */
-static int init_tx_skb_control_block(struct sk_buff *skb,
-				     struct ieee80211_hw *hw,
-	                             struct ieee80211_tx_control *control)
-{
-	struct zd_tx_skb_control_block *cb =
-		(struct zd_tx_skb_control_block *)skb->cb;
-
-	ZD_ASSERT(sizeof(*cb) <= sizeof(skb->cb));
-	memset(cb, 0, sizeof(*cb));
-	cb->hw= hw;
-	cb->control = kmalloc(sizeof(*control), GFP_ATOMIC);
-	if (cb->control == NULL)
-		return -ENOMEM;
-	memcpy(cb->control, control, sizeof(*control));
-
-	return 0;
+		dev_kfree_skb_any(skb);
 }
 
 /**
  * tx_status - reports tx status of a packet if required
  * @hw - a &struct ieee80211_hw pointer
  * @skb - a sk-buffer
- * @status - the tx status of the packet without control information
+ * @flags: extra flags to set in the TX status info
+ * @ackssi: ACK signal strength
  * @success - True for successfull transmission of the frame
  *
  * This information calls ieee80211_tx_status_irqsafe() if required by the
@@ -319,18 +264,17 @@ static int init_tx_skb_control_block(struct sk_buff *skb,
  * If no status information has been requested, the skb is freed.
  */
 static void tx_status(struct ieee80211_hw *hw, struct sk_buff *skb,
-	              struct ieee80211_tx_status *status,
-		      bool success)
+		      u32 flags, int ackssi, bool success)
 {
-	struct zd_tx_skb_control_block *cb = (struct zd_tx_skb_control_block *)
-		skb->cb;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+
+	memset(&info->status, 0, sizeof(info->status));
 
-	ZD_ASSERT(cb->control != NULL);
-	memcpy(&status->control, cb->control, sizeof(status->control));
 	if (!success)
-		status->excessive_retries = 1;
-	clear_tx_skb_control_block(skb);
-	ieee80211_tx_status_irqsafe(hw, skb, status);
+		info->status.excessive_retries = 1;
+	info->flags |= flags;
+	info->status.ack_signal = ackssi;
+	ieee80211_tx_status_irqsafe(hw, skb);
 }
 
 /**
@@ -345,15 +289,12 @@ void zd_mac_tx_failed(struct ieee80211_hw *hw)
 {
 	struct sk_buff_head *q = &zd_hw_mac(hw)->ack_wait_queue;
 	struct sk_buff *skb;
-	struct ieee80211_tx_status status;
 
 	skb = skb_dequeue(q);
 	if (skb == NULL)
 		return;
 
-	memset(&status, 0, sizeof(status));
-
-	tx_status(hw, skb, &status, 0);
+	tx_status(hw, skb, 0, 0, 0);
 }
 
 /**
@@ -368,28 +309,20 @@ void zd_mac_tx_failed(struct ieee80211_hw *hw)
  */
 void zd_mac_tx_to_dev(struct sk_buff *skb, int error)
 {
-	struct zd_tx_skb_control_block *cb =
-		(struct zd_tx_skb_control_block *)skb->cb;
-	struct ieee80211_hw *hw = cb->hw;
-
-	if (likely(cb->control)) {
-		skb_pull(skb, sizeof(struct zd_ctrlset));
-		if (unlikely(error ||
-		    (cb->control->flags & IEEE80211_TXCTL_NO_ACK)))
-		{
-			struct ieee80211_tx_status status;
-			memset(&status, 0, sizeof(status));
-			tx_status(hw, skb, &status, !error);
-		} else {
-			struct sk_buff_head *q =
-				&zd_hw_mac(hw)->ack_wait_queue;
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
+	struct ieee80211_hw *hw = info->driver_data[0];
 
-			skb_queue_tail(q, skb);
-			while (skb_queue_len(q) > ZD_MAC_MAX_ACK_WAITERS)
-				zd_mac_tx_failed(hw);
-		}
+	skb_pull(skb, sizeof(struct zd_ctrlset));
+	if (unlikely(error ||
+	    (info->flags & IEEE80211_TX_CTL_NO_ACK))) {
+		tx_status(hw, skb, 0, 0, !error);
 	} else {
-		kfree_tx_skb(skb);
+		struct sk_buff_head *q =
+			&zd_hw_mac(hw)->ack_wait_queue;
+
+		skb_queue_tail(q, skb);
+		while (skb_queue_len(q) > ZD_MAC_MAX_ACK_WAITERS)
+			zd_mac_tx_failed(hw);
 	}
 }
 
@@ -443,8 +376,6 @@ static int zd_calc_tx_length_us(u8 *service, u8 zd_rate, u16 tx_length)
 static void cs_set_control(struct zd_mac *mac, struct zd_ctrlset *cs,
 	                   struct ieee80211_hdr *header, u32 flags)
 {
-	u16 fctl = le16_to_cpu(header->frame_control);
-
 	/*
 	 * CONTROL TODO:
 	 * - if backoff needed, enable bit 0
@@ -454,7 +385,7 @@ static void cs_set_control(struct zd_mac *mac, struct zd_ctrlset *cs,
 	cs->control = 0;
 
 	/* First fragment */
-	if (flags & IEEE80211_TXCTL_FIRST_FRAGMENT)
+	if (flags & IEEE80211_TX_CTL_FIRST_FRAGMENT)
 		cs->control |= ZD_CS_NEED_RANDOM_BACKOFF;
 
 	/* Multicast */
@@ -462,79 +393,104 @@ static void cs_set_control(struct zd_mac *mac, struct zd_ctrlset *cs,
 		cs->control |= ZD_CS_MULTICAST;
 
 	/* PS-POLL */
-	if ((fctl & (IEEE80211_FCTL_FTYPE|IEEE80211_FCTL_STYPE)) ==
-	    (IEEE80211_FTYPE_CTL|IEEE80211_STYPE_PSPOLL))
+	if (ieee80211_is_pspoll(header->frame_control))
 		cs->control |= ZD_CS_PS_POLL_FRAME;
 
-	if (flags & IEEE80211_TXCTL_USE_RTS_CTS)
+	if (flags & IEEE80211_TX_CTL_USE_RTS_CTS)
 		cs->control |= ZD_CS_RTS;
 
-	if (flags & IEEE80211_TXCTL_USE_CTS_PROTECT)
+	if (flags & IEEE80211_TX_CTL_USE_CTS_PROTECT)
 		cs->control |= ZD_CS_SELF_CTS;
 
 	/* FIXME: Management frame? */
 }
 
-void zd_mac_config_beacon(struct ieee80211_hw *hw, struct sk_buff *beacon)
+static int zd_mac_config_beacon(struct ieee80211_hw *hw, struct sk_buff *beacon)
 {
 	struct zd_mac *mac = zd_hw_mac(hw);
+	int r;
 	u32 tmp, j = 0;
 	/* 4 more bytes for tail CRC */
 	u32 full_len = beacon->len + 4;
-	zd_iowrite32(&mac->chip, CR_BCN_FIFO_SEMAPHORE, 0);
-	zd_ioread32(&mac->chip, CR_BCN_FIFO_SEMAPHORE, &tmp);
+
+	r = zd_iowrite32(&mac->chip, CR_BCN_FIFO_SEMAPHORE, 0);
+	if (r < 0)
+		return r;
+	r = zd_ioread32(&mac->chip, CR_BCN_FIFO_SEMAPHORE, &tmp);
+	if (r < 0)
+		return r;
+
 	while (tmp & 0x2) {
-		zd_ioread32(&mac->chip, CR_BCN_FIFO_SEMAPHORE, &tmp);
+		r = zd_ioread32(&mac->chip, CR_BCN_FIFO_SEMAPHORE, &tmp);
+		if (r < 0)
+			return r;
 		if ((++j % 100) == 0) {
 			printk(KERN_ERR "CR_BCN_FIFO_SEMAPHORE not ready\n");
 			if (j >= 500)  {
 				printk(KERN_ERR "Giving up beacon config.\n");
-				return;
+				return -ETIMEDOUT;
 			}
 		}
 		msleep(1);
 	}
 
-	zd_iowrite32(&mac->chip, CR_BCN_FIFO, full_len - 1);
-	if (zd_chip_is_zd1211b(&mac->chip))
-		zd_iowrite32(&mac->chip, CR_BCN_LENGTH, full_len - 1);
+	r = zd_iowrite32(&mac->chip, CR_BCN_FIFO, full_len - 1);
+	if (r < 0)
+		return r;
+	if (zd_chip_is_zd1211b(&mac->chip)) {
+		r = zd_iowrite32(&mac->chip, CR_BCN_LENGTH, full_len - 1);
+		if (r < 0)
+			return r;
+	}
 
-	for (j = 0 ; j < beacon->len; j++)
-		zd_iowrite32(&mac->chip, CR_BCN_FIFO,
+	for (j = 0 ; j < beacon->len; j++) {
+		r = zd_iowrite32(&mac->chip, CR_BCN_FIFO,
 				*((u8 *)(beacon->data + j)));
+		if (r < 0)
+			return r;
+	}
+
+	for (j = 0; j < 4; j++) {
+		r = zd_iowrite32(&mac->chip, CR_BCN_FIFO, 0x0);
+		if (r < 0)
+			return r;
+	}
 
-	for (j = 0; j < 4; j++)
-		zd_iowrite32(&mac->chip, CR_BCN_FIFO, 0x0);
+	r = zd_iowrite32(&mac->chip, CR_BCN_FIFO_SEMAPHORE, 1);
+	if (r < 0)
+		return r;
 
-	zd_iowrite32(&mac->chip, CR_BCN_FIFO_SEMAPHORE, 1);
 	/* 802.11b/g 2.4G CCK 1Mb
 	 * 802.11a, not yet implemented, uses different values (see GPL vendor
 	 * driver)
 	 */
-	zd_iowrite32(&mac->chip, CR_BCN_PLCP_CFG, 0x00000400 |
+	return zd_iowrite32(&mac->chip, CR_BCN_PLCP_CFG, 0x00000400 |
 			(full_len << 19));
 }
 
 static int fill_ctrlset(struct zd_mac *mac,
-			struct sk_buff *skb,
-			struct ieee80211_tx_control *control)
+			struct sk_buff *skb)
 {
 	int r;
 	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *) skb->data;
 	unsigned int frag_len = skb->len + FCS_LEN;
 	unsigned int packet_length;
+	struct ieee80211_rate *txrate;
 	struct zd_ctrlset *cs = (struct zd_ctrlset *)
 		skb_push(skb, sizeof(struct zd_ctrlset));
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 
 	ZD_ASSERT(frag_len <= 0xffff);
 
-	cs->modulation = control->tx_rate->hw_value;
-	if (control->flags & IEEE80211_TXCTL_SHORT_PREAMBLE)
-		cs->modulation = control->tx_rate->hw_value_short;
+	txrate = ieee80211_get_tx_rate(mac->hw, info);
+
+	cs->modulation = txrate->hw_value;
+	if (info->flags & IEEE80211_TX_CTL_SHORT_PREAMBLE)
+		cs->modulation = txrate->hw_value_short;
 
 	cs->tx_length = cpu_to_le16(frag_len);
 
-	cs_set_control(mac, cs, hdr, control->flags);
+	cs_set_control(mac, cs, hdr, info->flags);
 
 	packet_length = frag_len + sizeof(struct zd_ctrlset) + 10;
 	ZD_ASSERT(packet_length <= 0xffff);
@@ -579,24 +535,21 @@ static int fill_ctrlset(struct zd_mac *mac,
  * control block of the skbuff will be initialized. If necessary the incoming
  * mac80211 queues will be stopped.
  */
-static int zd_op_tx(struct ieee80211_hw *hw, struct sk_buff *skb,
-		     struct ieee80211_tx_control *control)
+static int zd_op_tx(struct ieee80211_hw *hw, struct sk_buff *skb)
 {
 	struct zd_mac *mac = zd_hw_mac(hw);
+	struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
 	int r;
 
-	r = fill_ctrlset(mac, skb, control);
+	r = fill_ctrlset(mac, skb);
 	if (r)
 		return r;
 
-	r = init_tx_skb_control_block(skb, hw, control);
-	if (r)
-		return r;
+	info->driver_data[0] = hw;
+
 	r = zd_usb_tx(&mac->chip.usb, skb);
-	if (r) {
-		clear_tx_skb_control_block(skb);
+	if (r)
 		return r;
-	}
 	return 0;
 }
 
@@ -617,13 +570,11 @@ static int zd_op_tx(struct ieee80211_hw *hw, struct sk_buff *skb,
 static int filter_ack(struct ieee80211_hw *hw, struct ieee80211_hdr *rx_hdr,
 		      struct ieee80211_rx_status *stats)
 {
-	u16 fc = le16_to_cpu(rx_hdr->frame_control);
 	struct sk_buff *skb;
 	struct sk_buff_head *q;
 	unsigned long flags;
 
-	if ((fc & (IEEE80211_FCTL_FTYPE | IEEE80211_FCTL_STYPE)) !=
-	    (IEEE80211_FTYPE_CTL | IEEE80211_STYPE_ACK))
+	if (!ieee80211_is_ack(rx_hdr->frame_control))
 		return 0;
 
 	q = &zd_hw_mac(hw)->ack_wait_queue;
@@ -634,13 +585,8 @@ static int filter_ack(struct ieee80211_hw *hw, struct ieee80211_hdr *rx_hdr,
 		tx_hdr = (struct ieee80211_hdr *)skb->data;
 		if (likely(!compare_ether_addr(tx_hdr->addr2, rx_hdr->addr1)))
 		{
-			struct ieee80211_tx_status status;
-
-			memset(&status, 0, sizeof(status));
-			status.flags = IEEE80211_TX_STATUS_ACK;
-			status.ack_signal = stats->ssi;
 			__skb_unlink(skb, q);
-			tx_status(hw, skb, &status, 1);
+			tx_status(hw, skb, IEEE80211_TX_STAT_ACK, stats->signal, 1);
 			goto out;
 		}
 	}
@@ -656,8 +602,8 @@ int zd_mac_rx(struct ieee80211_hw *hw, const u8 *buffer, unsigned int length)
 	const struct rx_status *status;
 	struct sk_buff *skb;
 	int bad_frame = 0;
-	u16 fc;
-	bool is_qos, is_4addr, need_padding;
+	__le16 fc;
+	int need_padding;
 	int i;
 	u8 rate;
 
@@ -691,8 +637,8 @@ int zd_mac_rx(struct ieee80211_hw *hw, const u8 *buffer, unsigned int length)
 
 	stats.freq = zd_channels[_zd_chip_get_channel(&mac->chip) - 1].center_freq;
 	stats.band = IEEE80211_BAND_2GHZ;
-	stats.ssi = status->signal_strength;
-	stats.signal = zd_rx_qual_percent(buffer,
+	stats.signal = status->signal_strength;
+	stats.qual = zd_rx_qual_percent(buffer,
 		                          length - sizeof(struct rx_status),
 		                          status);
 
@@ -716,13 +662,8 @@ int zd_mac_rx(struct ieee80211_hw *hw, const u8 *buffer, unsigned int length)
 			&& !mac->pass_ctrl)
 		return 0;
 
-	fc = le16_to_cpu(*((__le16 *) buffer));
-
-	is_qos = ((fc & IEEE80211_FCTL_FTYPE) == IEEE80211_FTYPE_DATA) &&
-		 (fc & IEEE80211_STYPE_QOS_DATA);
-	is_4addr = (fc & (IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS)) ==
-		   (IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS);
-	need_padding = is_qos ^ is_4addr;
+	fc = *(__le16 *)buffer;
+	need_padding = ieee80211_is_data_qos(fc) ^ ieee80211_has_a4(fc);
 
 	skb = dev_alloc_skb(length + (need_padding ? 2 : 0));
 	if (skb == NULL)
@@ -751,6 +692,7 @@ static int zd_op_add_interface(struct ieee80211_hw *hw,
 	case IEEE80211_IF_TYPE_MNTR:
 	case IEEE80211_IF_TYPE_MESH_POINT:
 	case IEEE80211_IF_TYPE_STA:
+	case IEEE80211_IF_TYPE_IBSS:
 		mac->type = conf->type;
 		break;
 	default:
@@ -781,14 +723,24 @@ static int zd_op_config_interface(struct ieee80211_hw *hw,
 {
 	struct zd_mac *mac = zd_hw_mac(hw);
 	int associated;
+	int r;
 
-	if (mac->type == IEEE80211_IF_TYPE_MESH_POINT) {
+	if (mac->type == IEEE80211_IF_TYPE_MESH_POINT ||
+	    mac->type == IEEE80211_IF_TYPE_IBSS) {
 		associated = true;
-		if (conf->beacon) {
-			zd_mac_config_beacon(hw, conf->beacon);
-			kfree_skb(conf->beacon);
-			zd_set_beacon_interval(&mac->chip, BCN_MODE_IBSS |
+		if (conf->changed & IEEE80211_IFCC_BEACON) {
+			struct sk_buff *beacon = ieee80211_beacon_get(hw, vif);
+
+			if (!beacon)
+				return -ENOMEM;
+			r = zd_mac_config_beacon(hw, beacon);
+			if (r < 0)
+				return r;
+			r = zd_set_beacon_interval(&mac->chip, BCN_MODE_IBSS |
 					hw->conf.beacon_int);
+			if (r < 0)
+				return r;
+			kfree_skb(beacon);
 		}
 	} else
 		associated = is_valid_ether_addr(conf->bssid);
@@ -983,10 +935,10 @@ struct ieee80211_hw *zd_mac_alloc_hw(struct usb_interface *intf)
 	hw->wiphy->bands[IEEE80211_BAND_2GHZ] = &mac->band;
 
 	hw->flags = IEEE80211_HW_RX_INCLUDES_FCS |
-		    IEEE80211_HW_HOST_GEN_BEACON_TEMPLATE;
-	hw->max_rssi = 100;
-	hw->max_signal = 100;
+		    IEEE80211_HW_HOST_GEN_BEACON_TEMPLATE |
+		    IEEE80211_HW_SIGNAL_DB;
 
+	hw->max_signal = 100;
 	hw->queues = 1;
 	hw->extra_tx_headroom = sizeof(struct zd_ctrlset);
 
diff --git a/drivers/net/wireless/zd1211rw/zd_mac.h b/drivers/net/wireless/zd1211rw/zd_mac.h
index 71170244d2c9..18c1d56d3dd7 100644
--- a/drivers/net/wireless/zd1211rw/zd_mac.h
+++ b/drivers/net/wireless/zd1211rw/zd_mac.h
@@ -149,22 +149,6 @@ struct housekeeping {
 	struct delayed_work link_led_work;
 };
 
-/**
- * struct zd_tx_skb_control_block - control block for tx skbuffs
- * @control: &struct ieee80211_tx_control pointer
- * @context: context pointer
- *
- * This structure is used to fill the cb field in an &sk_buff to transmit.
- * The control field is NULL, if there is no requirement from the mac80211
- * stack to report about the packet ACK. This is the case if the flag
- * IEEE80211_TXCTL_NO_ACK is not set in &struct ieee80211_tx_control.
- */
-struct zd_tx_skb_control_block {
-	struct ieee80211_tx_control *control;
-	struct ieee80211_hw *hw;
-	void *context;
-};
-
 #define ZD_MAC_STATS_BUFFER_SIZE 16
 
 #define ZD_MAC_MAX_ACK_WAITERS 10
diff --git a/drivers/net/wireless/zd1211rw/zd_usb.c b/drivers/net/wireless/zd1211rw/zd_usb.c
index 6cdad9764604..da8b7433e3a6 100644
--- a/drivers/net/wireless/zd1211rw/zd_usb.c
+++ b/drivers/net/wireless/zd1211rw/zd_usb.c
@@ -170,10 +170,11 @@ static int upload_code(struct usb_device *udev,
 	if (flags & REBOOT) {
 		u8 ret;
 
+		/* Use "DMA-aware" buffer. */
 		r = usb_control_msg(udev, usb_rcvctrlpipe(udev, 0),
 			USB_REQ_FIRMWARE_CONFIRM,
 			USB_DIR_IN | USB_TYPE_VENDOR,
-			0, 0, &ret, sizeof(ret), 5000 /* ms */);
+			0, 0, p, sizeof(ret), 5000 /* ms */);
 		if (r != sizeof(ret)) {
 			dev_err(&udev->dev,
 				"control request firmeware confirmation failed."
@@ -182,6 +183,7 @@ static int upload_code(struct usb_device *udev,
 				r = -ENODEV;
 			goto error;
 		}
+		ret = p[0];
 		if (ret & 0x80) {
 			dev_err(&udev->dev,
 				"Internal error while downloading."
@@ -313,22 +315,31 @@ int zd_usb_read_fw(struct zd_usb *usb, zd_addr_t addr, u8 *data, u16 len)
 {
 	int r;
 	struct usb_device *udev = zd_usb_to_usbdev(usb);
+	u8 *buf;
 
+	/* Use "DMA-aware" buffer. */
+	buf = kmalloc(len, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
 	r = usb_control_msg(udev, usb_rcvctrlpipe(udev, 0),
 		USB_REQ_FIRMWARE_READ_DATA, USB_DIR_IN | 0x40, addr, 0,
-		data, len, 5000);
+		buf, len, 5000);
 	if (r < 0) {
 		dev_err(&udev->dev,
 			"read over firmware interface failed: %d\n", r);
-		return r;
+		goto exit;
 	} else if (r != len) {
 		dev_err(&udev->dev,
 			"incomplete read over firmware interface: %d/%d\n",
 			r, len);
-		return -EIO;
+		r = -EIO;
+		goto exit;
 	}
-
-	return 0;
+	r = 0;
+	memcpy(data, buf, len);
+exit:
+	kfree(buf);
+	return r;
 }
 
 #define urb_dev(urb) (&(urb)->dev->dev)
@@ -870,7 +881,7 @@ static void tx_urb_complete(struct urb *urb)
 {
 	int r;
 	struct sk_buff *skb;
-	struct zd_tx_skb_control_block *cb;
+	struct ieee80211_tx_info *info;
 	struct zd_usb *usb;
 
 	switch (urb->status) {
@@ -894,8 +905,8 @@ free_urb:
 	 * grab 'usb' pointer before handing off the skb (since
 	 * it might be freed by zd_mac_tx_to_dev or mac80211)
 	 */
-	cb = (struct zd_tx_skb_control_block *)skb->cb;
-	usb = &zd_hw_mac(cb->hw)->chip.usb;
+	info = IEEE80211_SKB_CB(skb);
+	usb = &zd_hw_mac(info->driver_data[0])->chip.usb;
 	zd_mac_tx_to_dev(skb, urb->status);
 	free_tx_urb(usb, urb);
 	tx_dec_submitted_urbs(usb);