Update hostapd to 0.6.4

[openwrt.git] / package / b43 / src / pio.c

diff --git a/package/b43/src/pio.c b/package/b43/src/pio.c
index 67752a2..8b1555d 100644 (file)
--- a/package/b43/src/pio.c
+++ b/package/b43/src/pio.c
@@ -2,9 +2,9 @@
 
   Broadcom B43 wireless driver
 
 
   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
 
   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,628 +25,818 @@
 
 #include "b43.h"
 #include "pio.h"
 
 #include "b43.h"
 #include "pio.h"

+#include "dma.h"

 #include "main.h"
 #include "xmit.h"
 
 #include <linux/delay.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(*((__le16 *)(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;
-       u16 packetindex;
-
-       /* We use the upper 4 bits for the PIO
-        * controller ID and the lower 12 bits
-        * for the packet index (in the cache).
+       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 = packet->index;
-       B43_WARN_ON(packetindex & ~0x0FFF);
-       cookie |= (u16) packetindex;
+       cookie = (((u16)q->index + 1) << 12);
+       cookie |= pack->index;

 
        return cookie;
 }
 
 static
 
        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_pio *pio = &dev->pio;

-       struct b43_pioqueue *queue = NULL;
-       int packetindex;
+       struct b43_pio_txqueue *q = NULL;
+       unsigned int pack_index;

 
        switch (cookie & 0xF000) {
 
        switch (cookie & 0xF000) {

-       case 0x0000:
-               queue = pio->queue0;
-               break;

        case 0x1000:
        case 0x1000:

-               queue = pio->queue1;
+               q = pio->tx_queue_AC_BK;

                break;
        case 0x2000:
                break;
        case 0x2000:

-               queue = pio->queue2;
+               q = pio->tx_queue_AC_BE;

                break;
        case 0x3000:
                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;
                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);
-               packet->index = i;
+#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_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));
+       }
+
+       spin_lock_irqsave(&q->lock, flags);

 
 

-       B43_WARN_ON(queue->tx_suspended);
-       B43_WARN_ON(list_empty(&queue->txfree));
+       hdrlen = b43_txhdr_size(dev);
+       total_len = roundup(skb->len + hdrlen, 4);

 
 

-       packet = list_entry(queue->txfree.next, struct b43_pio_txpacket, list);
-       packet->skb = skb;
+       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);

 
 

-       memset(&packet->txstat, 0, sizeof(packet->txstat));
-       memcpy(&packet->txstat.control, ctl, sizeof(*ctl));
+       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)
 {
 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;
                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)
 {
 }
 
 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;
 
        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)

 {
 {

-       __le16 preamble[21] = { 0 };
-       struct b43_rxhdr_fw4 *rxhdr;
-       u16 tmp, len;
+       struct b43_wldev *dev = q->dev;
+       struct b43_rxhdr_fw4 rxhdr;
+       u16 len;

        u32 macstat;
        u32 macstat;

-       int i, preamble_readwords;
+       unsigned int i, padding;

        struct sk_buff *skb;
        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);
+       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);
+               }

        }
        }

-       b43dbg(queue->dev->wl, "PIO RX timed out\n");
-       return;
+       b43dbg(q->dev->wl, "PIO RX timed out\n");
+       return 1;

 data_ready:
 
 data_ready:
 

-       len = b43_pio_read(queue, B43_PIO_RXDATA);
-       if (unlikely(len > 0x700)) {
-               pio_rx_error(queue, 0, "len > 0x700");
-               return;
-       }
-       if (unlikely(len == 0 && queue->mmio_base != B43_MMIO_PIO4_BASE)) {
-               pio_rx_error(queue, 0, "len == 0");
-               return;
+       /* 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 = le32_to_cpu(rxhdr->mac_status);
-       if (macstat & B43_RX_MAC_FCSERR) {
-               pio_rx_error(queue,
-                            (queue->mmio_base == B43_MMIO_PIO1_BASE),
-                            "Frame FCS error");
-               return;
+       if (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)) {
        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);
-               *((__le16 *)(skb->data + i)) = cpu_to_le16(tmp);
-       }
-       if (len % 2) {
-               tmp = b43_pio_read(queue, B43_PIO_RXDATA);
-               skb->data[len - 1] = (tmp & 0x00FF);
-/* The specs say the following is required, but
- * it is wrong and corrupts the PLCP. If we don't do
- * this, the PLCP seems to be correct. So ifdef it out for now.
- */
-#if 0
-               if (rxflags2 & B43_RXHDR_FLAGS2_TYPE2FRAME)
-                       skb->data[2] = (tmp & 0xFF00) >> 8;
-               else
-                       skb->data[0] = (tmp & 0xFF00) >> 8;
-#endif
+       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);

 }
 }