X-Git-Url: http://git.rohieb.name/openwrt.git/blobdiff_plain/900cb9fb858b29ba749ff243755ccb5f073f2b54..0e23a76f3ddaef6b7a71c9ed7d6b1adff256bf1a:/package/b43/src/pio.c?ds=sidebyside

diff --git a/package/b43/src/pio.c b/package/b43/src/pio.c
index 4ac91fdd3..8b1555d95 100644
--- a/package/b43/src/pio.c
+++ b/package/b43/src/pio.c
@@ -2,9 +2,9 @@
 
   Broadcom B43 wireless driver
 
-  PIO Transmission
+  PIO data transfer
 
-  Copyright (c) 2005 Michael Buesch <mb@bu3sch.de>
+  Copyright (c) 2005-2008 Michael Buesch <mb@bu3sch.de>
 
   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
@@ -25,626 +25,818 @@
 
 #include "b43.h"
 #include "pio.h"
+#include "dma.h"
 #include "main.h"
 #include "xmit.h"
 
 #include <linux/delay.h>
 
-static void tx_start(struct b43_pioqueue *queue)
-{
-	b43_pio_write(queue, B43_PIO_TXCTL, B43_PIO_TXCTL_INIT);
-}
 
-static void tx_octet(struct b43_pioqueue *queue, u8 octet)
-{
-	if (queue->need_workarounds) {
-		b43_pio_write(queue, B43_PIO_TXDATA, octet);
-		b43_pio_write(queue, B43_PIO_TXCTL, B43_PIO_TXCTL_WRITELO);
-	} else {
-		b43_pio_write(queue, B43_PIO_TXCTL, B43_PIO_TXCTL_WRITELO);
-		b43_pio_write(queue, B43_PIO_TXDATA, octet);
-	}
-}
+static void b43_pio_rx_work(struct work_struct *work);
 
-static u16 tx_get_next_word(const u8 * txhdr,
-			    const u8 * packet,
-			    size_t txhdr_size, unsigned int *pos)
-{
-	const u8 *source;
-	unsigned int i = *pos;
-	u16 ret;
 
-	if (i < txhdr_size) {
-		source = txhdr;
-	} else {
-		source = packet;
-		i -= txhdr_size;
-	}
-	ret = le16_to_cpu(*((u16 *) (source + i)));
-	*pos += 2;
-
-	return ret;
-}
-
-static void tx_data(struct b43_pioqueue *queue,
-		    u8 * txhdr, const u8 * packet, unsigned int octets)
+static u16 generate_cookie(struct b43_pio_txqueue *q,
+			   struct b43_pio_txpacket *pack)
 {
-	u16 data;
-	unsigned int i = 0;
-
-	if (queue->need_workarounds) {
-		data = tx_get_next_word(txhdr, packet,
-					sizeof(struct b43_txhdr_fw4), &i);
-		b43_pio_write(queue, B43_PIO_TXDATA, data);
-	}
-	b43_pio_write(queue, B43_PIO_TXCTL,
-		      B43_PIO_TXCTL_WRITELO | B43_PIO_TXCTL_WRITEHI);
-	while (i < octets - 1) {
-		data = tx_get_next_word(txhdr, packet,
-					sizeof(struct b43_txhdr_fw4), &i);
-		b43_pio_write(queue, B43_PIO_TXDATA, data);
-	}
-	if (octets % 2)
-		tx_octet(queue,
-			 packet[octets - sizeof(struct b43_txhdr_fw4) - 1]);
-}
-
-static void tx_complete(struct b43_pioqueue *queue, struct sk_buff *skb)
-{
-	if (queue->need_workarounds) {
-		b43_pio_write(queue, B43_PIO_TXDATA, skb->data[skb->len - 1]);
-		b43_pio_write(queue, B43_PIO_TXCTL,
-			      B43_PIO_TXCTL_WRITELO | B43_PIO_TXCTL_COMPLETE);
-	} else {
-		b43_pio_write(queue, B43_PIO_TXCTL, B43_PIO_TXCTL_COMPLETE);
-	}
-}
-
-static u16 generate_cookie(struct b43_pioqueue *queue,
-			   struct b43_pio_txpacket *packet)
-{
-	u16 cookie = 0x0000;
-	int packetindex;
-
-	/* We use the upper 4 bits for the PIO
-	 * controller ID and the lower 12 bits
-	 * for the packet index (in the cache).
+	u16 cookie;
+
+	/* Use the upper 4 bits of the cookie as
+	 * PIO controller ID and store the packet index number
+	 * in the lower 12 bits.
+	 * Note that the cookie must never be 0, as this
+	 * is a special value used in RX path.
+	 * It can also not be 0xFFFF because that is special
+	 * for multicast frames.
 	 */
-	switch (queue->mmio_base) {
-	case B43_MMIO_PIO1_BASE:
-		break;
-	case B43_MMIO_PIO2_BASE:
-		cookie = 0x1000;
-		break;
-	case B43_MMIO_PIO3_BASE:
-		cookie = 0x2000;
-		break;
-	case B43_MMIO_PIO4_BASE:
-		cookie = 0x3000;
-		break;
-	default:
-		B43_WARN_ON(1);
-	}
-	packetindex = pio_txpacket_getindex(packet);
-	B43_WARN_ON(packetindex & ~0x0FFF);
-	cookie |= (u16) packetindex;
+	cookie = (((u16)q->index + 1) << 12);
+	cookie |= pack->index;
 
 	return cookie;
 }
 
 static
-struct b43_pioqueue *parse_cookie(struct b43_wldev *dev,
-				  u16 cookie, struct b43_pio_txpacket **packet)
+struct b43_pio_txqueue * parse_cookie(struct b43_wldev *dev,
+				      u16 cookie,
+				      struct b43_pio_txpacket **pack)
 {
 	struct b43_pio *pio = &dev->pio;
-	struct b43_pioqueue *queue = NULL;
-	int packetindex;
+	struct b43_pio_txqueue *q = NULL;
+	unsigned int pack_index;
 
 	switch (cookie & 0xF000) {
-	case 0x0000:
-		queue = pio->queue0;
-		break;
 	case 0x1000:
-		queue = pio->queue1;
+		q = pio->tx_queue_AC_BK;
 		break;
 	case 0x2000:
-		queue = pio->queue2;
+		q = pio->tx_queue_AC_BE;
 		break;
 	case 0x3000:
-		queue = pio->queue3;
+		q = pio->tx_queue_AC_VI;
+		break;
+	case 0x4000:
+		q = pio->tx_queue_AC_VO;
+		break;
+	case 0x5000:
+		q = pio->tx_queue_mcast;
 		break;
-	default:
-		B43_WARN_ON(1);
 	}
-	packetindex = (cookie & 0x0FFF);
-	B43_WARN_ON(!(packetindex >= 0 && packetindex < B43_PIO_MAXTXPACKETS));
-	*packet = &(queue->tx_packets_cache[packetindex]);
-
-	return queue;
+	if (B43_WARN_ON(!q))
+		return NULL;
+	pack_index = (cookie & 0x0FFF);
+	if (B43_WARN_ON(pack_index >= ARRAY_SIZE(q->packets)))
+		return NULL;
+	*pack = &q->packets[pack_index];
+
+	return q;
 }
 
-union txhdr_union {
-	struct b43_txhdr_fw4 txhdr_fw4;
-};
-
-static void pio_tx_write_fragment(struct b43_pioqueue *queue,
-				  struct sk_buff *skb,
-				  struct b43_pio_txpacket *packet,
-				  size_t txhdr_size)
+static u16 index_to_pioqueue_base(struct b43_wldev *dev,
+				  unsigned int index)
 {
-	union txhdr_union txhdr_data;
-	u8 *txhdr = NULL;
-	unsigned int octets;
-
-	txhdr = (u8 *) (&txhdr_data.txhdr_fw4);
-
-	B43_WARN_ON(skb_shinfo(skb)->nr_frags);
-	b43_generate_txhdr(queue->dev,
-			   txhdr, skb->data, skb->len,
-			   &packet->txstat.control,
-			   generate_cookie(queue, packet));
-
-	tx_start(queue);
-	octets = skb->len + txhdr_size;
-	if (queue->need_workarounds)
-		octets--;
-	tx_data(queue, txhdr, (u8 *) skb->data, octets);
-	tx_complete(queue, skb);
+	static const u16 bases[] = {
+		B43_MMIO_PIO_BASE0,
+		B43_MMIO_PIO_BASE1,
+		B43_MMIO_PIO_BASE2,
+		B43_MMIO_PIO_BASE3,
+		B43_MMIO_PIO_BASE4,
+		B43_MMIO_PIO_BASE5,
+		B43_MMIO_PIO_BASE6,
+		B43_MMIO_PIO_BASE7,
+	};
+	static const u16 bases_rev11[] = {
+		B43_MMIO_PIO11_BASE0,
+		B43_MMIO_PIO11_BASE1,
+		B43_MMIO_PIO11_BASE2,
+		B43_MMIO_PIO11_BASE3,
+		B43_MMIO_PIO11_BASE4,
+		B43_MMIO_PIO11_BASE5,
+	};
+
+	if (dev->dev->id.revision >= 11) {
+		B43_WARN_ON(index >= ARRAY_SIZE(bases_rev11));
+		return bases_rev11[index];
+	}
+	B43_WARN_ON(index >= ARRAY_SIZE(bases));
+	return bases[index];
 }
 
-static void free_txpacket(struct b43_pio_txpacket *packet)
+static u16 pio_txqueue_offset(struct b43_wldev *dev)
 {
-	struct b43_pioqueue *queue = packet->queue;
+	if (dev->dev->id.revision >= 11)
+		return 0x18;
+	return 0;
+}
 
-	if (packet->skb)
-		dev_kfree_skb_any(packet->skb);
-	list_move(&packet->list, &queue->txfree);
-	queue->nr_txfree++;
+static u16 pio_rxqueue_offset(struct b43_wldev *dev)
+{
+	if (dev->dev->id.revision >= 11)
+		return 0x38;
+	return 8;
 }
 
-static int pio_tx_packet(struct b43_pio_txpacket *packet)
+static struct b43_pio_txqueue * b43_setup_pioqueue_tx(struct b43_wldev *dev,
+						      unsigned int index)
 {
-	struct b43_pioqueue *queue = packet->queue;
-	struct sk_buff *skb = packet->skb;
-	u16 octets;
-
-	octets = (u16) skb->len + sizeof(struct b43_txhdr_fw4);
-	if (queue->tx_devq_size < octets) {
-		b43warn(queue->dev->wl, "PIO queue too small. "
-			"Dropping packet.\n");
-		/* Drop it silently (return success) */
-		free_txpacket(packet);
-		return 0;
+	struct b43_pio_txqueue *q;
+	struct b43_pio_txpacket *p;
+	unsigned int i;
+
+	q = kzalloc(sizeof(*q), GFP_KERNEL);
+	if (!q)
+		return NULL;
+	spin_lock_init(&q->lock);
+	q->dev = dev;
+	q->rev = dev->dev->id.revision;
+	q->mmio_base = index_to_pioqueue_base(dev, index) +
+		       pio_txqueue_offset(dev);
+	q->index = index;
+
+	q->free_packet_slots = B43_PIO_MAX_NR_TXPACKETS;
+	if (q->rev >= 8) {
+		q->buffer_size = 1920; //FIXME this constant is wrong.
+	} else {
+		q->buffer_size = b43_piotx_read16(q, B43_PIO_TXQBUFSIZE);
+		q->buffer_size -= 80;
 	}
-	B43_WARN_ON(queue->tx_devq_packets > B43_PIO_MAXTXDEVQPACKETS);
-	B43_WARN_ON(queue->tx_devq_used > queue->tx_devq_size);
-	/* Check if there is sufficient free space on the device
-	 * TX queue. If not, return and let the TX tasklet
-	 * retry later.
-	 */
-	if (queue->tx_devq_packets == B43_PIO_MAXTXDEVQPACKETS)
-		return -EBUSY;
-	if (queue->tx_devq_used + octets > queue->tx_devq_size)
-		return -EBUSY;
-	/* Now poke the device. */
-	pio_tx_write_fragment(queue, skb, packet, sizeof(struct b43_txhdr_fw4));
-
-	/* Account for the packet size.
-	 * (We must not overflow the device TX queue)
-	 */
-	queue->tx_devq_packets++;
-	queue->tx_devq_used += octets;
 
-	/* Transmission started, everything ok, move the
-	 * packet to the txrunning list.
-	 */
-	list_move_tail(&packet->list, &queue->txrunning);
+	INIT_LIST_HEAD(&q->packets_list);
+	for (i = 0; i < ARRAY_SIZE(q->packets); i++) {
+		p = &(q->packets[i]);
+		INIT_LIST_HEAD(&p->list);
+		p->index = i;
+		p->queue = q;
+		list_add(&p->list, &q->packets_list);
+	}
 
-	return 0;
+	return q;
 }
 
-static void tx_tasklet(unsigned long d)
+static struct b43_pio_rxqueue * b43_setup_pioqueue_rx(struct b43_wldev *dev,
+						      unsigned int index)
 {
-	struct b43_pioqueue *queue = (struct b43_pioqueue *)d;
-	struct b43_wldev *dev = queue->dev;
-	unsigned long flags;
-	struct b43_pio_txpacket *packet, *tmp_packet;
-	int err;
-	u16 txctl;
-
-	spin_lock_irqsave(&dev->wl->irq_lock, flags);
-	if (queue->tx_frozen)
-		goto out_unlock;
-	txctl = b43_pio_read(queue, B43_PIO_TXCTL);
-	if (txctl & B43_PIO_TXCTL_SUSPEND)
-		goto out_unlock;
+	struct b43_pio_rxqueue *q;
+
+	q = kzalloc(sizeof(*q), GFP_KERNEL);
+	if (!q)
+		return NULL;
+	spin_lock_init(&q->lock);
+	q->dev = dev;
+	q->rev = dev->dev->id.revision;
+	q->mmio_base = index_to_pioqueue_base(dev, index) +
+		       pio_rxqueue_offset(dev);
+	INIT_WORK(&q->rx_work, b43_pio_rx_work);
+
+	/* Enable Direct FIFO RX (PIO) on the engine. */
+	b43_dma_direct_fifo_rx(dev, index, 1);
+
+	return q;
+}
 
-	list_for_each_entry_safe(packet, tmp_packet, &queue->txqueue, list) {
-		/* Try to transmit the packet. This can fail, if
-		 * the device queue is full. In case of failure, the
-		 * packet is left in the txqueue.
-		 * If transmission succeed, the packet is moved to txrunning.
-		 * If it is impossible to transmit the packet, it
-		 * is dropped.
-		 */
-		err = pio_tx_packet(packet);
-		if (err)
-			break;
+static void b43_pio_cancel_tx_packets(struct b43_pio_txqueue *q)
+{
+	struct b43_pio_txpacket *pack;
+	unsigned int i;
+
+	for (i = 0; i < ARRAY_SIZE(q->packets); i++) {
+		pack = &(q->packets[i]);
+		if (pack->skb) {
+			dev_kfree_skb_any(pack->skb);
+			pack->skb = NULL;
+		}
 	}
-      out_unlock:
-	spin_unlock_irqrestore(&dev->wl->irq_lock, flags);
 }
 
-static void setup_txqueues(struct b43_pioqueue *queue)
+static void b43_destroy_pioqueue_tx(struct b43_pio_txqueue *q,
+				    const char *name)
 {
-	struct b43_pio_txpacket *packet;
-	int i;
+	if (!q)
+		return;
+	b43_pio_cancel_tx_packets(q);
+	kfree(q);
+}
 
-	queue->nr_txfree = B43_PIO_MAXTXPACKETS;
-	for (i = 0; i < B43_PIO_MAXTXPACKETS; i++) {
-		packet = &(queue->tx_packets_cache[i]);
+static void b43_destroy_pioqueue_rx(struct b43_pio_rxqueue *q,
+				    const char *name)
+{
+	if (!q)
+		return;
+	kfree(q);
+}
 
-		packet->queue = queue;
-		INIT_LIST_HEAD(&packet->list);
+#define destroy_queue_tx(pio, queue) do {				\
+	b43_destroy_pioqueue_tx((pio)->queue, __stringify(queue));	\
+	(pio)->queue = NULL;						\
+  } while (0)
 
-		list_add(&packet->list, &queue->txfree);
-	}
-}
+#define destroy_queue_rx(pio, queue) do {				\
+	b43_destroy_pioqueue_rx((pio)->queue, __stringify(queue));	\
+	(pio)->queue = NULL;						\
+  } while (0)
 
-static
-struct b43_pioqueue *b43_setup_pioqueue(struct b43_wldev *dev,
-					u16 pio_mmio_base)
+void b43_pio_free(struct b43_wldev *dev)
 {
-	struct b43_pioqueue *queue;
-	u16 qsize;
+	struct b43_pio *pio;
 
-	queue = kzalloc(sizeof(*queue), GFP_KERNEL);
-	if (!queue)
-		goto out;
+	if (!b43_using_pio_transfers(dev))
+		return;
+	pio = &dev->pio;
 
-	queue->dev = dev;
-	queue->mmio_base = pio_mmio_base;
-	queue->need_workarounds = (dev->dev->id.revision < 3);
+	destroy_queue_rx(pio, rx_queue);
+	destroy_queue_tx(pio, tx_queue_mcast);
+	destroy_queue_tx(pio, tx_queue_AC_VO);
+	destroy_queue_tx(pio, tx_queue_AC_VI);
+	destroy_queue_tx(pio, tx_queue_AC_BE);
+	destroy_queue_tx(pio, tx_queue_AC_BK);
+}
+
+void b43_pio_stop(struct b43_wldev *dev)
+{
+	if (!b43_using_pio_transfers(dev))
+		return;
+	cancel_work_sync(&dev->pio.rx_queue->rx_work);
+}
 
-	INIT_LIST_HEAD(&queue->txfree);
-	INIT_LIST_HEAD(&queue->txqueue);
-	INIT_LIST_HEAD(&queue->txrunning);
-	tasklet_init(&queue->txtask, tx_tasklet, (unsigned long)queue);
+int b43_pio_init(struct b43_wldev *dev)
+{
+	struct b43_pio *pio = &dev->pio;
+	int err = -ENOMEM;
 
 	b43_write32(dev, B43_MMIO_MACCTL, b43_read32(dev, B43_MMIO_MACCTL)
 		    & ~B43_MACCTL_BE);
+	b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_RXPADOFF, 0);
 
-	qsize = b43_read16(dev, queue->mmio_base + B43_PIO_TXQBUFSIZE);
-	if (qsize == 0) {
-		b43err(dev->wl, "This card does not support PIO "
-		       "operation mode. Please use DMA mode "
-		       "(module parameter pio=0).\n");
-		goto err_freequeue;
-	}
-	if (qsize <= B43_PIO_TXQADJUST) {
-		b43err(dev->wl, "PIO tx device-queue too small (%u)\n", qsize);
-		goto err_freequeue;
-	}
-	qsize -= B43_PIO_TXQADJUST;
-	queue->tx_devq_size = qsize;
+	pio->tx_queue_AC_BK = b43_setup_pioqueue_tx(dev, 0);
+	if (!pio->tx_queue_AC_BK)
+		goto out;
 
-	setup_txqueues(queue);
+	pio->tx_queue_AC_BE = b43_setup_pioqueue_tx(dev, 1);
+	if (!pio->tx_queue_AC_BE)
+		goto err_destroy_bk;
 
-      out:
-	return queue;
+	pio->tx_queue_AC_VI = b43_setup_pioqueue_tx(dev, 2);
+	if (!pio->tx_queue_AC_VI)
+		goto err_destroy_be;
 
-      err_freequeue:
-	kfree(queue);
-	queue = NULL;
-	goto out;
-}
+	pio->tx_queue_AC_VO = b43_setup_pioqueue_tx(dev, 3);
+	if (!pio->tx_queue_AC_VO)
+		goto err_destroy_vi;
 
-static void cancel_transfers(struct b43_pioqueue *queue)
-{
-	struct b43_pio_txpacket *packet, *tmp_packet;
+	pio->tx_queue_mcast = b43_setup_pioqueue_tx(dev, 4);
+	if (!pio->tx_queue_mcast)
+		goto err_destroy_vo;
+
+	pio->rx_queue = b43_setup_pioqueue_rx(dev, 0);
+	if (!pio->rx_queue)
+		goto err_destroy_mcast;
 
-	tasklet_disable(&queue->txtask);
+	b43dbg(dev->wl, "PIO initialized\n");
+	err = 0;
+out:
+	return err;
 
-	list_for_each_entry_safe(packet, tmp_packet, &queue->txrunning, list)
-	    free_txpacket(packet);
-	list_for_each_entry_safe(packet, tmp_packet, &queue->txqueue, list)
-	    free_txpacket(packet);
+err_destroy_mcast:
+	destroy_queue_tx(pio, tx_queue_mcast);
+err_destroy_vo:
+	destroy_queue_tx(pio, tx_queue_AC_VO);
+err_destroy_vi:
+	destroy_queue_tx(pio, tx_queue_AC_VI);
+err_destroy_be:
+	destroy_queue_tx(pio, tx_queue_AC_BE);
+err_destroy_bk:
+	destroy_queue_tx(pio, tx_queue_AC_BK);
+	return err;
 }
 
-static void b43_destroy_pioqueue(struct b43_pioqueue *queue)
+/* Static mapping of mac80211's queues (priorities) to b43 PIO queues. */
+static struct b43_pio_txqueue * select_queue_by_priority(struct b43_wldev *dev,
+							 u8 queue_prio)
 {
-	if (!queue)
-		return;
+	struct b43_pio_txqueue *q;
+
+	if (b43_modparam_qos) {
+		/* 0 = highest priority */
+		switch (queue_prio) {
+		default:
+			B43_WARN_ON(1);
+			/* fallthrough */
+		case 0:
+			q = dev->pio.tx_queue_AC_VO;
+			break;
+		case 1:
+			q = dev->pio.tx_queue_AC_VI;
+			break;
+		case 2:
+			q = dev->pio.tx_queue_AC_BE;
+			break;
+		case 3:
+			q = dev->pio.tx_queue_AC_BK;
+			break;
+		}
+	} else
+		q = dev->pio.tx_queue_AC_BE;
 
-	cancel_transfers(queue);
-	kfree(queue);
+	return q;
 }
 
-void b43_pio_free(struct b43_wldev *dev)
+static u16 tx_write_2byte_queue(struct b43_pio_txqueue *q,
+				u16 ctl,
+				const void *_data,
+				unsigned int data_len)
 {
-	struct b43_pio *pio;
+	struct b43_wldev *dev = q->dev;
+	const u8 *data = _data;
+
+	ctl |= B43_PIO_TXCTL_WRITELO | B43_PIO_TXCTL_WRITEHI;
+	b43_piotx_write16(q, B43_PIO_TXCTL, ctl);
+
+	ssb_block_write(dev->dev, data, (data_len & ~1),
+			q->mmio_base + B43_PIO_TXDATA,
+			sizeof(u16));
+	if (data_len & 1) {
+		/* Write the last byte. */
+		ctl &= ~B43_PIO_TXCTL_WRITEHI;
+		b43_piotx_write16(q, B43_PIO_TXCTL, ctl);
+		b43_piotx_write16(q, B43_PIO_TXDATA, data[data_len - 1]);
+	}
 
-	if (!b43_using_pio(dev))
-		return;
-	pio = &dev->pio;
+	return ctl;
+}
 
-	b43_destroy_pioqueue(pio->queue3);
-	pio->queue3 = NULL;
-	b43_destroy_pioqueue(pio->queue2);
-	pio->queue2 = NULL;
-	b43_destroy_pioqueue(pio->queue1);
-	pio->queue1 = NULL;
-	b43_destroy_pioqueue(pio->queue0);
-	pio->queue0 = NULL;
+static void pio_tx_frame_2byte_queue(struct b43_pio_txpacket *pack,
+				     const u8 *hdr, unsigned int hdrlen)
+{
+	struct b43_pio_txqueue *q = pack->queue;
+	const char *frame = pack->skb->data;
+	unsigned int frame_len = pack->skb->len;
+	u16 ctl;
+
+	ctl = b43_piotx_read16(q, B43_PIO_TXCTL);
+	ctl |= B43_PIO_TXCTL_FREADY;
+	ctl &= ~B43_PIO_TXCTL_EOF;
+
+	/* Transfer the header data. */
+	ctl = tx_write_2byte_queue(q, ctl, hdr, hdrlen);
+	/* Transfer the frame data. */
+	ctl = tx_write_2byte_queue(q, ctl, frame, frame_len);
+
+	ctl |= B43_PIO_TXCTL_EOF;
+	b43_piotx_write16(q, B43_PIO_TXCTL, ctl);
 }
 
-int b43_pio_init(struct b43_wldev *dev)
+static u32 tx_write_4byte_queue(struct b43_pio_txqueue *q,
+				u32 ctl,
+				const void *_data,
+				unsigned int data_len)
 {
-	struct b43_pio *pio = &dev->pio;
-	struct b43_pioqueue *queue;
-	int err = -ENOMEM;
+	struct b43_wldev *dev = q->dev;
+	const u8 *data = _data;
+
+	ctl |= B43_PIO8_TXCTL_0_7 | B43_PIO8_TXCTL_8_15 |
+	       B43_PIO8_TXCTL_16_23 | B43_PIO8_TXCTL_24_31;
+	b43_piotx_write32(q, B43_PIO8_TXCTL, ctl);
+
+	ssb_block_write(dev->dev, data, (data_len & ~3),
+			q->mmio_base + B43_PIO8_TXDATA,
+			sizeof(u32));
+	if (data_len & 3) {
+		u32 value = 0;
+
+		/* Write the last few bytes. */
+		ctl &= ~(B43_PIO8_TXCTL_8_15 | B43_PIO8_TXCTL_16_23 |
+			 B43_PIO8_TXCTL_24_31);
+		data = &(data[data_len - 1]);
+		switch (data_len & 3) {
+		case 3:
+			ctl |= B43_PIO8_TXCTL_16_23;
+			value |= (u32)(*data) << 16;
+			data--;
+		case 2:
+			ctl |= B43_PIO8_TXCTL_8_15;
+			value |= (u32)(*data) << 8;
+			data--;
+		case 1:
+			value |= (u32)(*data);
+		}
+		b43_piotx_write32(q, B43_PIO8_TXCTL, ctl);
+		b43_piotx_write32(q, B43_PIO8_TXDATA, value);
+	}
 
-	queue = b43_setup_pioqueue(dev, B43_MMIO_PIO1_BASE);
-	if (!queue)
-		goto out;
-	pio->queue0 = queue;
+	return ctl;
+}
 
-	queue = b43_setup_pioqueue(dev, B43_MMIO_PIO2_BASE);
-	if (!queue)
-		goto err_destroy0;
-	pio->queue1 = queue;
+static void pio_tx_frame_4byte_queue(struct b43_pio_txpacket *pack,
+				     const u8 *hdr, unsigned int hdrlen)
+{
+	struct b43_pio_txqueue *q = pack->queue;
+	const char *frame = pack->skb->data;
+	unsigned int frame_len = pack->skb->len;
+	u32 ctl;
+
+	ctl = b43_piotx_read32(q, B43_PIO8_TXCTL);
+	ctl |= B43_PIO8_TXCTL_FREADY;
+	ctl &= ~B43_PIO8_TXCTL_EOF;
+
+	/* Transfer the header data. */
+	ctl = tx_write_4byte_queue(q, ctl, hdr, hdrlen);
+	/* Transfer the frame data. */
+	ctl = tx_write_4byte_queue(q, ctl, frame, frame_len);
+
+	ctl |= B43_PIO8_TXCTL_EOF;
+	b43_piotx_write32(q, B43_PIO_TXCTL, ctl);
+}
 
-	queue = b43_setup_pioqueue(dev, B43_MMIO_PIO3_BASE);
-	if (!queue)
-		goto err_destroy1;
-	pio->queue2 = queue;
+static int pio_tx_frame(struct b43_pio_txqueue *q,
+			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);
+
+	cookie = generate_cookie(q, pack);
+	hdrlen = b43_txhdr_size(q->dev);
+	err = b43_generate_txhdr(q->dev, (u8 *)&txhdr, skb->data,
+				 skb->len, info, cookie);
+	if (err)
+		return err;
+
+	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,
+				B43_SHM_SH_MCASTCOOKIE, cookie);
+	}
 
-	queue = b43_setup_pioqueue(dev, B43_MMIO_PIO4_BASE);
-	if (!queue)
-		goto err_destroy2;
-	pio->queue3 = queue;
+	pack->skb = skb;
+	if (q->rev >= 8)
+		pio_tx_frame_4byte_queue(pack, (const u8 *)&txhdr, hdrlen);
+	else
+		pio_tx_frame_2byte_queue(pack, (const u8 *)&txhdr, hdrlen);
 
-	if (dev->dev->id.revision < 3)
-		dev->irq_savedstate |= B43_IRQ_PIO_WORKAROUND;
+	/* Remove it from the list of available packet slots.
+	 * It will be put back when we receive the status report. */
+	list_del(&pack->list);
 
-	b43dbg(dev->wl, "PIO initialized\n");
-	err = 0;
-      out:
-	return err;
+	/* Update the queue statistics. */
+	q->buffer_used += roundup(skb->len + hdrlen, 4);
+	q->free_packet_slots -= 1;
 
-      err_destroy2:
-	b43_destroy_pioqueue(pio->queue2);
-	pio->queue2 = NULL;
-      err_destroy1:
-	b43_destroy_pioqueue(pio->queue1);
-	pio->queue1 = NULL;
-      err_destroy0:
-	b43_destroy_pioqueue(pio->queue0);
-	pio->queue0 = NULL;
-	goto out;
+	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_pioqueue *queue = dev->pio.queue1;
-	struct b43_pio_txpacket *packet;
+	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 (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
+		 * for us on the last frame. */
+		hdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_MOREDATA);
+	} else {
+		/* Decide by priority where to put this frame. */
+		q = select_queue_by_priority(dev, skb_get_queue_mapping(skb));
+	}
 
-	B43_WARN_ON(queue->tx_suspended);
-	B43_WARN_ON(list_empty(&queue->txfree));
+	spin_lock_irqsave(&q->lock, flags);
 
-	packet = list_entry(queue->txfree.next, struct b43_pio_txpacket, list);
-	packet->skb = skb;
+	hdrlen = b43_txhdr_size(dev);
+	total_len = roundup(skb->len + hdrlen, 4);
 
-	memset(&packet->txstat, 0, sizeof(packet->txstat));
-	memcpy(&packet->txstat.control, ctl, sizeof(*ctl));
+	if (unlikely(total_len > q->buffer_size)) {
+		err = -ENOBUFS;
+		b43dbg(dev->wl, "PIO: TX packet longer than queue.\n");
+		goto out_unlock;
+	}
+	if (unlikely(q->free_packet_slots == 0)) {
+		err = -ENOBUFS;
+		b43warn(dev->wl, "PIO: TX packet overflow.\n");
+		goto out_unlock;
+	}
+	B43_WARN_ON(q->buffer_used > q->buffer_size);
+
+	if (total_len > (q->buffer_size - q->buffer_used)) {
+		/* Not enough memory on the queue. */
+		err = -EBUSY;
+		ieee80211_stop_queue(dev->wl->hw, skb_get_queue_mapping(skb));
+		q->stopped = 1;
+		goto out_unlock;
+	}
 
-	list_move_tail(&packet->list, &queue->txqueue);
-	queue->nr_txfree--;
-	queue->nr_tx_packets++;
-	B43_WARN_ON(queue->nr_txfree >= B43_PIO_MAXTXPACKETS);
+	/* 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 = skb_get_queue_mapping(skb);
+
+	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. */
+		dev_kfree_skb_any(skb);
+		err = 0;
+		goto out_unlock;
+	}
+	if (unlikely(err)) {
+		b43err(dev->wl, "PIO transmission failure\n");
+		goto out_unlock;
+	}
+	q->nr_tx_packets++;
+
+	B43_WARN_ON(q->buffer_used > q->buffer_size);
+	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, skb_get_queue_mapping(skb));
+		q->stopped = 1;
+	}
 
-	tasklet_schedule(&queue->txtask);
+out_unlock:
+	spin_unlock_irqrestore(&q->lock, flags);
 
-	return 0;
+	return err;
 }
 
+/* Called with IRQs disabled. */
 void b43_pio_handle_txstatus(struct b43_wldev *dev,
 			     const struct b43_txstatus *status)
 {
-	struct b43_pioqueue *queue;
-	struct b43_pio_txpacket *packet;
+	struct b43_pio_txqueue *q;
+	struct b43_pio_txpacket *pack = NULL;
+	unsigned int total_len;
+	struct ieee80211_tx_info *info;
 
-	queue = parse_cookie(dev, status->cookie, &packet);
-	if (B43_WARN_ON(!queue))
+	q = parse_cookie(dev, status->cookie, &pack);
+	if (unlikely(!q))
 		return;
+	B43_WARN_ON(!pack);
 
-	queue->tx_devq_packets--;
-	queue->tx_devq_used -=
-	    (packet->skb->len + sizeof(struct b43_txhdr_fw4));
+	spin_lock(&q->lock); /* IRQs are already disabled. */
 
-	if (status->acked) {
-		packet->txstat.flags |= IEEE80211_TX_STATUS_ACK;
-	} else {
-		if (!(packet->txstat.control.flags & IEEE80211_TXCTL_NO_ACK))
-			packet->txstat.excessive_retries = 1;
+	info = (void *)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->skb = NULL;
+	list_add(&pack->list, &q->packets_list);
+
+	if (q->stopped) {
+		ieee80211_wake_queue(dev->wl->hw, q->queue_prio);
+		q->stopped = 0;
 	}
-	if (status->frame_count == 0) {
-		/* The frame was not transmitted at all. */
-		packet->txstat.retry_count = 0;
-	} else
-		packet->txstat.retry_count = status->frame_count - 1;
-	ieee80211_tx_status_irqsafe(dev->wl->hw, packet->skb,
-				    &(packet->txstat));
-	packet->skb = NULL;
-
-	free_txpacket(packet);
-	/* If there are packets on the txqueue, poke the tasklet
-	 * to transmit them.
-	 */
-	if (!list_empty(&queue->txqueue))
-		tasklet_schedule(&queue->txtask);
+
+	spin_unlock(&q->lock);
 }
 
 void b43_pio_get_tx_stats(struct b43_wldev *dev,
 			  struct ieee80211_tx_queue_stats *stats)
 {
-	struct b43_pio *pio = &dev->pio;
-	struct b43_pioqueue *queue;
-	struct ieee80211_tx_queue_stats_data *data;
-
-	queue = pio->queue1;
-	data = &(stats->data[0]);
-	data->len = B43_PIO_MAXTXPACKETS - queue->nr_txfree;
-	data->limit = B43_PIO_MAXTXPACKETS;
-	data->count = queue->nr_tx_packets;
-}
-
-static void pio_rx_error(struct b43_pioqueue *queue,
-			 int clear_buffers, const char *error)
-{
+	const int nr_queues = dev->wl->hw->queues;
+	struct b43_pio_txqueue *q;
+	unsigned long flags;
 	int i;
 
-	b43err(queue->dev->wl, "PIO RX error: %s\n", error);
-	b43_pio_write(queue, B43_PIO_RXCTL, B43_PIO_RXCTL_READY);
-	if (clear_buffers) {
-		B43_WARN_ON(queue->mmio_base != B43_MMIO_PIO1_BASE);
-		for (i = 0; i < 15; i++) {
-			/* Dummy read. */
-			b43_pio_read(queue, B43_PIO_RXDATA);
-		}
+	for (i = 0; i < nr_queues; i++) {
+		q = select_queue_by_priority(dev, i);
+
+		spin_lock_irqsave(&q->lock, flags);
+		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);
 	}
 }
 
-void b43_pio_rx(struct b43_pioqueue *queue)
+/* Returns whether we should fetch another frame. */
+static bool pio_rx_frame(struct b43_pio_rxqueue *q)
 {
-	u16 preamble[21] = { 0 };
-	struct b43_rxhdr_fw4 *rxhdr;
-	u16 tmp, len, macstat;
-	int i, preamble_readwords;
+	struct b43_wldev *dev = q->dev;
+	struct b43_rxhdr_fw4 rxhdr;
+	u16 len;
+	u32 macstat;
+	unsigned int i, padding;
 	struct sk_buff *skb;
-
-	tmp = b43_pio_read(queue, B43_PIO_RXCTL);
-	if (!(tmp & B43_PIO_RXCTL_DATAAVAILABLE))
-		return;
-	b43_pio_write(queue, B43_PIO_RXCTL, B43_PIO_RXCTL_DATAAVAILABLE);
-
-	for (i = 0; i < 10; i++) {
-		tmp = b43_pio_read(queue, B43_PIO_RXCTL);
-		if (tmp & B43_PIO_RXCTL_READY)
-			goto data_ready;
-		udelay(10);
-	}
-	b43dbg(queue->dev->wl, "PIO RX timed out\n");
-	return;
-      data_ready:
-
-	len = b43_pio_read(queue, B43_PIO_RXDATA);
-	if (unlikely(len > 0x700)) {
-		pio_rx_error(queue, 0, "len > 0x700");
-		return;
+	const char *err_msg = NULL;
+
+	memset(&rxhdr, 0, sizeof(rxhdr));
+
+	/* Check if we have data and wait for it to get ready. */
+	if (q->rev >= 8) {
+		u32 ctl;
+
+		ctl = b43_piorx_read32(q, B43_PIO8_RXCTL);
+		if (!(ctl & B43_PIO8_RXCTL_FRAMERDY))
+			return 0;
+		b43_piorx_write32(q, B43_PIO8_RXCTL,
+				  B43_PIO8_RXCTL_FRAMERDY);
+		for (i = 0; i < 10; i++) {
+			ctl = b43_piorx_read32(q, B43_PIO8_RXCTL);
+			if (ctl & B43_PIO8_RXCTL_DATARDY)
+				goto data_ready;
+			udelay(10);
+		}
+	} else {
+		u16 ctl;
+
+		ctl = b43_piorx_read16(q, B43_PIO_RXCTL);
+		if (!(ctl & B43_PIO_RXCTL_FRAMERDY))
+			return 0;
+		b43_piorx_write16(q, B43_PIO_RXCTL,
+				  B43_PIO_RXCTL_FRAMERDY);
+		for (i = 0; i < 10; i++) {
+			ctl = b43_piorx_read16(q, B43_PIO_RXCTL);
+			if (ctl & B43_PIO_RXCTL_DATARDY)
+				goto data_ready;
+			udelay(10);
+		}
 	}
-	if (unlikely(len == 0 && queue->mmio_base != B43_MMIO_PIO4_BASE)) {
-		pio_rx_error(queue, 0, "len == 0");
-		return;
+	b43dbg(q->dev->wl, "PIO RX timed out\n");
+	return 1;
+data_ready:
+
+	/* Get the preamble (RX header) */
+	if (q->rev >= 8) {
+		ssb_block_read(dev->dev, &rxhdr, sizeof(rxhdr),
+			       q->mmio_base + B43_PIO8_RXDATA,
+			       sizeof(u32));
+	} else {
+		ssb_block_read(dev->dev, &rxhdr, sizeof(rxhdr),
+			       q->mmio_base + B43_PIO_RXDATA,
+			       sizeof(u16));
 	}
-	preamble[0] = cpu_to_le16(len);
-	if (queue->mmio_base == B43_MMIO_PIO4_BASE)
-		preamble_readwords = 14 / sizeof(u16);
-	else
-		preamble_readwords = 18 / sizeof(u16);
-	for (i = 0; i < preamble_readwords; i++) {
-		tmp = b43_pio_read(queue, B43_PIO_RXDATA);
-		preamble[i + 1] = cpu_to_le16(tmp);
+	/* Sanity checks. */
+	len = le16_to_cpu(rxhdr.frame_len);
+	if (unlikely(len > 0x700)) {
+		err_msg = "len > 0x700";
+		goto rx_error;
 	}
-	rxhdr = (struct b43_rxhdr_fw4 *)preamble;
-	macstat = le16_to_cpu(rxhdr->mac_status);
-	if (macstat & B43_RX_MAC_FCSERR) {
-		pio_rx_error(queue,
-			     (queue->mmio_base == B43_MMIO_PIO1_BASE),
-			     "Frame FCS error");
-		return;
+	if (unlikely(len == 0)) {
+		err_msg = "len == 0";
+		goto rx_error;
 	}
-	if (queue->mmio_base == B43_MMIO_PIO4_BASE) {
-		/* We received an xmit status. */
-		struct b43_hwtxstatus *hw;
-
-		hw = (struct b43_hwtxstatus *)(preamble + 1);
-		b43_handle_hwtxstatus(queue->dev, hw);
 
-		return;
+	macstat = le32_to_cpu(rxhdr.mac_status);
+	if (macstat & B43_RX_MAC_FCSERR) {
+		if (!(q->dev->wl->filter_flags & FIF_FCSFAIL)) {
+			/* Drop frames with failed FCS. */
+			err_msg = "Frame FCS error";
+			goto rx_error;
+		}
 	}
 
-	skb = dev_alloc_skb(len);
+	/* We always pad 2 bytes, as that's what upstream code expects
+	 * due to the RX-header being 30 bytes. In case the frame is
+	 * unaligned, we pad another 2 bytes. */
+	padding = (macstat & B43_RX_MAC_PADDING) ? 2 : 0;
+	skb = dev_alloc_skb(len + padding + 2);
 	if (unlikely(!skb)) {
-		pio_rx_error(queue, 1, "OOM");
-		return;
+		err_msg = "Out of memory";
+		goto rx_error;
 	}
-	skb_put(skb, len);
-	for (i = 0; i < len - 1; i += 2) {
-		tmp = b43_pio_read(queue, B43_PIO_RXDATA);
-		*((u16 *) (skb->data + i)) = cpu_to_le16(tmp);
-	}
-	if (len % 2) {
-		tmp = b43_pio_read(queue, B43_PIO_RXDATA);
-		skb->data[len - 1] = (tmp & 0x00FF);
-/* The specs say the following is required, but
- * it is wrong and corrupts the PLCP. If we don't do
- * this, the PLCP seems to be correct. So ifdef it out for now.
- */
-#if 0
-		if (rxflags2 & B43_RXHDR_FLAGS2_TYPE2FRAME)
-			skb->data[2] = (tmp & 0xFF00) >> 8;
-		else
-			skb->data[0] = (tmp & 0xFF00) >> 8;
-#endif
+	skb_reserve(skb, 2);
+	skb_put(skb, len + padding);
+	if (q->rev >= 8) {
+		ssb_block_read(dev->dev, skb->data + padding, (len & ~3),
+			       q->mmio_base + B43_PIO8_RXDATA,
+			       sizeof(u32));
+		if (len & 3) {
+			u32 value;
+			char *data;
+
+			/* Read the last few bytes. */
+			value = b43_piorx_read32(q, B43_PIO8_RXDATA);
+			data = &(skb->data[len + padding - 1]);
+			switch (len & 3) {
+			case 3:
+				*data = (value >> 16);
+				data--;
+			case 2:
+				*data = (value >> 8);
+				data--;
+			case 1:
+				*data = value;
+			}
+		}
+	} else {
+		ssb_block_read(dev->dev, skb->data + padding, (len & ~1),
+			       q->mmio_base + B43_PIO_RXDATA,
+			       sizeof(u16));
+		if (len & 1) {
+			u16 value;
+
+			/* Read the last byte. */
+			value = b43_piorx_read16(q, B43_PIO_RXDATA);
+			skb->data[len + padding - 1] = value;
+		}
 	}
-	b43_rx(queue->dev, skb, rxhdr);
+
+	b43_rx(q->dev, skb, &rxhdr);
+
+	return 1;
+
+rx_error:
+	if (err_msg)
+		b43dbg(q->dev->wl, "PIO RX error: %s\n", err_msg);
+	b43_piorx_write16(q, B43_PIO_RXCTL, B43_PIO_RXCTL_DATARDY);
+	return 1;
 }
 
-void b43_pio_tx_suspend(struct b43_pioqueue *queue)
+/* RX workqueue. We can sleep, yay! */
+static void b43_pio_rx_work(struct work_struct *work)
 {
-	b43_power_saving_ctl_bits(queue->dev, B43_PS_AWAKE);
-	b43_pio_write(queue, B43_PIO_TXCTL, b43_pio_read(queue, B43_PIO_TXCTL)
-		      | B43_PIO_TXCTL_SUSPEND);
+	struct b43_pio_rxqueue *q = container_of(work, struct b43_pio_rxqueue,
+						 rx_work);
+	unsigned int budget = 50;
+	bool stop;
+
+	do {
+		spin_lock_irq(&q->lock);
+		stop = (pio_rx_frame(q) == 0);
+		spin_unlock_irq(&q->lock);
+		cond_resched();
+		if (stop)
+			break;
+	} while (--budget);
 }
 
-void b43_pio_tx_resume(struct b43_pioqueue *queue)
+/* Called with IRQs disabled. */
+void b43_pio_rx(struct b43_pio_rxqueue *q)
 {
-	b43_pio_write(queue, B43_PIO_TXCTL, b43_pio_read(queue, B43_PIO_TXCTL)
-		      & ~B43_PIO_TXCTL_SUSPEND);
-	b43_power_saving_ctl_bits(queue->dev, 0);
-	tasklet_schedule(&queue->txtask);
+	/* Due to latency issues we must run the RX path in
+	 * a workqueue to be able to schedule between packets. */
+	queue_work(q->dev->wl->hw->workqueue, &q->rx_work);
 }
 
-void b43_pio_freeze_txqueues(struct b43_wldev *dev)
+static void b43_pio_tx_suspend_queue(struct b43_pio_txqueue *q)
 {
-	struct b43_pio *pio;
+	unsigned long flags;
 
-	B43_WARN_ON(!b43_using_pio(dev));
-	pio = &dev->pio;
-	pio->queue0->tx_frozen = 1;
-	pio->queue1->tx_frozen = 1;
-	pio->queue2->tx_frozen = 1;
-	pio->queue3->tx_frozen = 1;
+	spin_lock_irqsave(&q->lock, flags);
+	if (q->rev >= 8) {
+		b43_piotx_write32(q, B43_PIO8_TXCTL,
+				  b43_piotx_read32(q, B43_PIO8_TXCTL)
+				  | B43_PIO8_TXCTL_SUSPREQ);
+	} else {
+		b43_piotx_write16(q, B43_PIO_TXCTL,
+				  b43_piotx_read16(q, B43_PIO_TXCTL)
+				  | B43_PIO_TXCTL_SUSPREQ);
+	}
+	spin_unlock_irqrestore(&q->lock, flags);
 }
 
-void b43_pio_thaw_txqueues(struct b43_wldev *dev)
+static void b43_pio_tx_resume_queue(struct b43_pio_txqueue *q)
 {
-	struct b43_pio *pio;
+	unsigned long flags;
 
-	B43_WARN_ON(!b43_using_pio(dev));
-	pio = &dev->pio;
-	pio->queue0->tx_frozen = 0;
-	pio->queue1->tx_frozen = 0;
-	pio->queue2->tx_frozen = 0;
-	pio->queue3->tx_frozen = 0;
-	if (!list_empty(&pio->queue0->txqueue))
-		tasklet_schedule(&pio->queue0->txtask);
-	if (!list_empty(&pio->queue1->txqueue))
-		tasklet_schedule(&pio->queue1->txtask);
-	if (!list_empty(&pio->queue2->txqueue))
-		tasklet_schedule(&pio->queue2->txtask);
-	if (!list_empty(&pio->queue3->txqueue))
-		tasklet_schedule(&pio->queue3->txtask);
+	spin_lock_irqsave(&q->lock, flags);
+	if (q->rev >= 8) {
+		b43_piotx_write32(q, B43_PIO8_TXCTL,
+				  b43_piotx_read32(q, B43_PIO8_TXCTL)
+				  & ~B43_PIO8_TXCTL_SUSPREQ);
+	} else {
+		b43_piotx_write16(q, B43_PIO_TXCTL,
+				  b43_piotx_read16(q, B43_PIO_TXCTL)
+				  & ~B43_PIO_TXCTL_SUSPREQ);
+	}
+	spin_unlock_irqrestore(&q->lock, flags);
+}
+
+void b43_pio_tx_suspend(struct b43_wldev *dev)
+{
+	b43_power_saving_ctl_bits(dev, B43_PS_AWAKE);
+	b43_pio_tx_suspend_queue(dev->pio.tx_queue_AC_BK);
+	b43_pio_tx_suspend_queue(dev->pio.tx_queue_AC_BE);
+	b43_pio_tx_suspend_queue(dev->pio.tx_queue_AC_VI);
+	b43_pio_tx_suspend_queue(dev->pio.tx_queue_AC_VO);
+	b43_pio_tx_suspend_queue(dev->pio.tx_queue_mcast);
+}
+
+void b43_pio_tx_resume(struct b43_wldev *dev)
+{
+	b43_pio_tx_resume_queue(dev->pio.tx_queue_mcast);
+	b43_pio_tx_resume_queue(dev->pio.tx_queue_AC_VO);
+	b43_pio_tx_resume_queue(dev->pio.tx_queue_AC_VI);
+	b43_pio_tx_resume_queue(dev->pio.tx_queue_AC_BE);
+	b43_pio_tx_resume_queue(dev->pio.tx_queue_AC_BK);
+	b43_power_saving_ctl_bits(dev, 0);
 }