7 files changed, 427 insertions, 262 deletions

diff --git a/drivers/net/wireless/b43/b43.h b/drivers/net/wireless/b43/b43.h
index 33459d61a717..d40be1568517 100644
--- a/drivers/net/wireless/b43/b43.h
+++ b/drivers/net/wireless/b43/b43.h
@@ -99,6 +99,8 @@
 #define B43_MMIO_TSF_2                  0x636   /* core rev < 3 only */
 #define B43_MMIO_TSF_3                  0x638   /* core rev < 3 only */
 #define B43_MMIO_RNG                    0x65A
+#define B43_MMIO_IFSCTL                 0x688 /* Interframe space control */
+#define  B43_MMIO_IFSCTL_USE_EDCF       0x0004
 #define B43_MMIO_POWERUP_DELAY          0x6A8
 
 /* SPROM boardflags_lo values */
@@ -587,15 +589,13 @@ struct b43_phy {
 
 /* Data structures for DMA transmission, per 80211 core. */
 struct b43_dma {
-        struct b43_dmaring *tx_ring0;
-        struct b43_dmaring *tx_ring1;
-        struct b43_dmaring *tx_ring2;
-        struct b43_dmaring *tx_ring3;
-        struct b43_dmaring *tx_ring4;
-        struct b43_dmaring *tx_ring5;
-
-        struct b43_dmaring *rx_ring0;
-        struct b43_dmaring *rx_ring3;   /* only available on core.rev < 5 */
+        struct b43_dmaring *tx_ring_AC_BK; /* Background */
+        struct b43_dmaring *tx_ring_AC_BE; /* Best Effort */
+        struct b43_dmaring *tx_ring_AC_VI; /* Video */
+        struct b43_dmaring *tx_ring_AC_VO; /* Voice */
+        struct b43_dmaring *tx_ring_mcast; /* Multicast */
+
+        struct b43_dmaring *rx_ring;
 };
 
 /* Context information for a noise calculation (Link Quality). */
@@ -621,6 +621,35 @@ struct b43_key {
         u8 algorithm;
 };
 
+/* SHM offsets to the QOS data structures for the 4 different queues. */
+#define B43_QOS_PARAMS(queue)   (B43_SHM_SH_EDCFQ + \
+                                 (B43_NR_QOSPARAMS * sizeof(u16) * (queue)))
+#define B43_QOS_BACKGROUND      B43_QOS_PARAMS(0)
+#define B43_QOS_BESTEFFORT      B43_QOS_PARAMS(1)
+#define B43_QOS_VIDEO           B43_QOS_PARAMS(2)
+#define B43_QOS_VOICE           B43_QOS_PARAMS(3)
+
+/* QOS parameter hardware data structure offsets. */
+#define B43_NR_QOSPARAMS        22
+enum {
+        B43_QOSPARAM_TXOP = 0,
+        B43_QOSPARAM_CWMIN,
+        B43_QOSPARAM_CWMAX,
+        B43_QOSPARAM_CWCUR,
+        B43_QOSPARAM_AIFS,
+        B43_QOSPARAM_BSLOTS,
+        B43_QOSPARAM_REGGAP,
+        B43_QOSPARAM_STATUS,
+};
+
+/* QOS parameters for a queue. */
+struct b43_qos_params {
+        /* The QOS parameters */
+        struct ieee80211_tx_queue_params p;
+        /* Does this need to get uploaded to hardware? */
+        bool need_hw_update;
+};
+
 struct b43_wldev;
 
 /* Data structure for the WLAN parts (802.11 cores) of the b43 chip. */
@@ -673,6 +702,12 @@ struct b43_wl {
         struct sk_buff *current_beacon;
         bool beacon0_uploaded;
         bool beacon1_uploaded;
+
+        /* The current QOS parameters for the 4 queues.
+         * This is protected by the irq_lock. */
+        struct b43_qos_params qos_params[4];
+        /* Workqueue for updating QOS parameters in hardware. */
+        struct work_struct qos_update_work;
 };
 
 /* In-memory representation of a cached microcode file. */
diff --git a/drivers/net/wireless/b43/dma.c b/drivers/net/wireless/b43/dma.c
index 3dfb28a34be9..663aed4e9e05 100644
--- a/drivers/net/wireless/b43/dma.c
+++ b/drivers/net/wireless/b43/dma.c
@@ -38,6 +38,7 @@
 #include <linux/delay.h>
 #include <linux/skbuff.h>
 #include <linux/etherdevice.h>
+#include <asm/div64.h>
 
 
 /* 32bit DMA ops. */
@@ -291,52 +292,6 @@ static inline int request_slot(struct b43_dmaring *ring)
         return slot;
 }
 
-/* Mac80211-queue to b43-ring mapping */
-static struct b43_dmaring *priority_to_txring(struct b43_wldev *dev,
-                                              int queue_priority)
-{
-        struct b43_dmaring *ring;
-
-/*FIXME: For now we always run on TX-ring-1 */
-        return dev->dma.tx_ring1;
-
-        /* 0 = highest priority */
-        switch (queue_priority) {
-        default:
-                B43_WARN_ON(1);
-                /* fallthrough */
-        case 0:
-                ring = dev->dma.tx_ring3;
-                break;
-        case 1:
-                ring = dev->dma.tx_ring2;
-                break;
-        case 2:
-                ring = dev->dma.tx_ring1;
-                break;
-        case 3:
-                ring = dev->dma.tx_ring0;
-                break;
-        }
-
-        return ring;
-}
-
-/* b43-ring to mac80211-queue mapping */
-static inline int txring_to_priority(struct b43_dmaring *ring)
-{
-        static const u8 idx_to_prio[] = { 3, 2, 1, 0, };
-        unsigned int index;
-
-/*FIXME: have only one queue, for now */
-        return 0;
-
-        index = ring->index;
-        if (B43_WARN_ON(index >= ARRAY_SIZE(idx_to_prio)))
-                index = 0;
-        return idx_to_prio[index];
-}
-
 static u16 b43_dmacontroller_base(enum b43_dmatype type, int controller_idx)
 {
         static const u16 map64[] = {
@@ -924,16 +879,52 @@ struct b43_dmaring *b43_setup_dmaring(struct b43_wldev *dev,
         goto out;
 }
 
+#define divide(a, b)    ({      \
+        typeof(a) __a = a;      \
+        do_div(__a, b);         \
+        __a;                    \
+  })
+
+#define modulo(a, b)    ({      \
+        typeof(a) __a = a;      \
+        do_div(__a, b);         \
+  })
+
 /* Main cleanup function. */
-static void b43_destroy_dmaring(struct b43_dmaring *ring)
+static void b43_destroy_dmaring(struct b43_dmaring *ring,
+                                const char *ringname)
 {
         if (!ring)
                 return;
 
-        b43dbg(ring->dev->wl, "DMA-%u 0x%04X (%s) max used slots: %d/%d\n",
-               (unsigned int)(ring->type),
-               ring->mmio_base,
-               (ring->tx) ? "TX" : "RX", ring->max_used_slots, ring->nr_slots);
+#ifdef CONFIG_B43_DEBUG
+        {
+                /* Print some statistics. */
+                u64 failed_packets = ring->nr_failed_tx_packets;
+                u64 succeed_packets = ring->nr_succeed_tx_packets;
+                u64 nr_packets = failed_packets + succeed_packets;
+                u64 permille_failed = 0, average_tries = 0;
+
+                if (nr_packets)
+                        permille_failed = divide(failed_packets * 1000, nr_packets);
+                if (nr_packets)
+                        average_tries = divide(ring->nr_total_packet_tries * 100, nr_packets);
+
+                b43dbg(ring->dev->wl, "DMA-%u %s: "
+                       "Used slots %d/%d, Failed frames %llu/%llu = %llu.%01llu%%, "
+                       "Average tries %llu.%02llu\n",
+                       (unsigned int)(ring->type), ringname,
+                       ring->max_used_slots,
+                       ring->nr_slots,
+                       (unsigned long long)failed_packets,
+                       (unsigned long long)nr_packets,
+                       (unsigned long long)divide(permille_failed, 10),
+                       (unsigned long long)modulo(permille_failed, 10),
+                       (unsigned long long)divide(average_tries, 100),
+                       (unsigned long long)modulo(average_tries, 100));
+        }
+#endif /* DEBUG */
+
         /* Device IRQs are disabled prior entering this function,
          * so no need to take care of concurrency with rx handler stuff.
          */
@@ -946,33 +937,26 @@ static void b43_destroy_dmaring(struct b43_dmaring *ring)
         kfree(ring);
 }
 
+#define destroy_ring(dma, ring) do {                            \
+        b43_destroy_dmaring((dma)->ring, __stringify(ring));    \
+        (dma)->ring = NULL;                                     \
+    } while (0)
+
 void b43_dma_free(struct b43_wldev *dev)
 {
         struct b43_dma *dma = &dev->dma;
 
-        b43_destroy_dmaring(dma->rx_ring3);
-        dma->rx_ring3 = NULL;
-        b43_destroy_dmaring(dma->rx_ring0);
-        dma->rx_ring0 = NULL;
-
-        b43_destroy_dmaring(dma->tx_ring5);
-        dma->tx_ring5 = NULL;
-        b43_destroy_dmaring(dma->tx_ring4);
-        dma->tx_ring4 = NULL;
-        b43_destroy_dmaring(dma->tx_ring3);
-        dma->tx_ring3 = NULL;
-        b43_destroy_dmaring(dma->tx_ring2);
-        dma->tx_ring2 = NULL;
-        b43_destroy_dmaring(dma->tx_ring1);
-        dma->tx_ring1 = NULL;
-        b43_destroy_dmaring(dma->tx_ring0);
-        dma->tx_ring0 = NULL;
+        destroy_ring(dma, rx_ring);
+        destroy_ring(dma, tx_ring_AC_BK);
+        destroy_ring(dma, tx_ring_AC_BE);
+        destroy_ring(dma, tx_ring_AC_VI);
+        destroy_ring(dma, tx_ring_AC_VO);
+        destroy_ring(dma, tx_ring_mcast);
 }
 
 int b43_dma_init(struct b43_wldev *dev)
 {
         struct b43_dma *dma = &dev->dma;
-        struct b43_dmaring *ring;
         int err;
         u64 dmamask;
         enum b43_dmatype type;
@@ -1002,83 +986,57 @@ int b43_dma_init(struct b43_wldev *dev)
 
         err = -ENOMEM;
         /* setup TX DMA channels. */
-        ring = b43_setup_dmaring(dev, 0, 1, type);
-        if (!ring)
+        dma->tx_ring_AC_BK = b43_setup_dmaring(dev, 0, 1, type);
+        if (!dma->tx_ring_AC_BK)
                 goto out;
-        dma->tx_ring0 = ring;
 
-        ring = b43_setup_dmaring(dev, 1, 1, type);
-        if (!ring)
-                goto err_destroy_tx0;
-        dma->tx_ring1 = ring;
+        dma->tx_ring_AC_BE = b43_setup_dmaring(dev, 1, 1, type);
+        if (!dma->tx_ring_AC_BE)
+                goto err_destroy_bk;
 
-        ring = b43_setup_dmaring(dev, 2, 1, type);
-        if (!ring)
-                goto err_destroy_tx1;
-        dma->tx_ring2 = ring;
+        dma->tx_ring_AC_VI = b43_setup_dmaring(dev, 2, 1, type);
+        if (!dma->tx_ring_AC_VI)
+                goto err_destroy_be;
 
-        ring = b43_setup_dmaring(dev, 3, 1, type);
-        if (!ring)
-                goto err_destroy_tx2;
-        dma->tx_ring3 = ring;
+        dma->tx_ring_AC_VO = b43_setup_dmaring(dev, 3, 1, type);
+        if (!dma->tx_ring_AC_VO)
+                goto err_destroy_vi;
 
-        ring = b43_setup_dmaring(dev, 4, 1, type);
-        if (!ring)
-                goto err_destroy_tx3;
-        dma->tx_ring4 = ring;
+        dma->tx_ring_mcast = b43_setup_dmaring(dev, 4, 1, type);
+        if (!dma->tx_ring_mcast)
+                goto err_destroy_vo;
 
-        ring = b43_setup_dmaring(dev, 5, 1, type);
-        if (!ring)
-                goto err_destroy_tx4;
-        dma->tx_ring5 = ring;
+        /* setup RX DMA channel. */
+        dma->rx_ring = b43_setup_dmaring(dev, 0, 0, type);
+        if (!dma->rx_ring)
+                goto err_destroy_mcast;
 
-        /* setup RX DMA channels. */
-        ring = b43_setup_dmaring(dev, 0, 0, type);
-        if (!ring)
-                goto err_destroy_tx5;
-        dma->rx_ring0 = ring;
-
-        if (dev->dev->id.revision < 5) {
-                ring = b43_setup_dmaring(dev, 3, 0, type);
-                if (!ring)
-                        goto err_destroy_rx0;
-                dma->rx_ring3 = ring;
-        }
+        /* No support for the TX status DMA ring. */
+        B43_WARN_ON(dev->dev->id.revision < 5);
 
         b43dbg(dev->wl, "%u-bit DMA initialized\n",
                (unsigned int)type);
         err = 0;
-      out:
+out:
         return err;
 
-      err_destroy_rx0:
-        b43_destroy_dmaring(dma->rx_ring0);
-        dma->rx_ring0 = NULL;
-      err_destroy_tx5:
-        b43_destroy_dmaring(dma->tx_ring5);
-        dma->tx_ring5 = NULL;
-      err_destroy_tx4:
-        b43_destroy_dmaring(dma->tx_ring4);
-        dma->tx_ring4 = NULL;
-      err_destroy_tx3:
-        b43_destroy_dmaring(dma->tx_ring3);
-        dma->tx_ring3 = NULL;
-      err_destroy_tx2:
-        b43_destroy_dmaring(dma->tx_ring2);
-        dma->tx_ring2 = NULL;
-      err_destroy_tx1:
-        b43_destroy_dmaring(dma->tx_ring1);
-        dma->tx_ring1 = NULL;
-      err_destroy_tx0:
-        b43_destroy_dmaring(dma->tx_ring0);
-        dma->tx_ring0 = NULL;
-        goto out;
+err_destroy_mcast:
+        destroy_ring(dma, tx_ring_mcast);
+err_destroy_vo:
+        destroy_ring(dma, tx_ring_AC_VO);
+err_destroy_vi:
+        destroy_ring(dma, tx_ring_AC_VI);
+err_destroy_be:
+        destroy_ring(dma, tx_ring_AC_BE);
+err_destroy_bk:
+        destroy_ring(dma, tx_ring_AC_BK);
+        return err;
 }
 
 /* Generate a cookie for the TX header. */
 static u16 generate_cookie(struct b43_dmaring *ring, int slot)
 {
-        u16 cookie = 0x1000;
+        u16 cookie;
 
         /* Use the upper 4 bits of the cookie as
          * DMA controller ID and store the slot number
@@ -1088,30 +1046,9 @@ static u16 generate_cookie(struct b43_dmaring *ring, int slot)
          * It can also not be 0xFFFF because that is special
          * for multicast frames.
          */
-        switch (ring->index) {
-        case 0:
-                cookie = 0x1000;
-                break;
-        case 1:
-                cookie = 0x2000;
-                break;
-        case 2:
-                cookie = 0x3000;
-                break;
-        case 3:
-                cookie = 0x4000;
-                break;
-        case 4:
-                cookie = 0x5000;
-                break;
-        case 5:
-                cookie = 0x6000;
-                break;
-        default:
-                B43_WARN_ON(1);
-        }
+        cookie = (((u16)ring->index + 1) << 12);
         B43_WARN_ON(slot & ~0x0FFF);
-        cookie |= (u16) slot;
+        cookie |= (u16)slot;
 
         return cookie;
 }
@@ -1125,22 +1062,19 @@ struct b43_dmaring *parse_cookie(struct b43_wldev *dev, u16 cookie, int *slot)
 
         switch (cookie & 0xF000) {
         case 0x1000:
-                ring = dma->tx_ring0;
+                ring = dma->tx_ring_AC_BK;
                 break;
         case 0x2000:
-                ring = dma->tx_ring1;
+                ring = dma->tx_ring_AC_BE;
                 break;
         case 0x3000:
-                ring = dma->tx_ring2;
+                ring = dma->tx_ring_AC_VI;
                 break;
         case 0x4000:
-                ring = dma->tx_ring3;
+                ring = dma->tx_ring_AC_VO;
                 break;
         case 0x5000:
-                ring = dma->tx_ring4;
-                break;
-        case 0x6000:
-                ring = dma->tx_ring5;
+                ring = dma->tx_ring_mcast;
                 break;
         default:
                 B43_WARN_ON(1);
@@ -1272,6 +1206,37 @@ static inline int should_inject_overflow(struct b43_dmaring *ring)
         return 0;
 }
 
+/* Static mapping of mac80211's queues (priorities) to b43 DMA rings. */
+static struct b43_dmaring * select_ring_by_priority(struct b43_wldev *dev,
+                                                    u8 queue_prio)
+{
+        struct b43_dmaring *ring;
+
+        if (b43_modparam_qos) {
+                /* 0 = highest priority */
+                switch (queue_prio) {
+                default:
+                        B43_WARN_ON(1);
+                        /* fallthrough */
+                case 0:
+                        ring = dev->dma.tx_ring_AC_VO;
+                        break;
+                case 1:
+                        ring = dev->dma.tx_ring_AC_VI;
+                        break;
+                case 2:
+                        ring = dev->dma.tx_ring_AC_BE;
+                        break;
+                case 3:
+                        ring = dev->dma.tx_ring_AC_BK;
+                        break;
+                }
+        } else
+                ring = dev->dma.tx_ring_AC_BE;
+
+        return ring;
+}
+
 int b43_dma_tx(struct b43_wldev *dev,
                struct sk_buff *skb, struct ieee80211_tx_control *ctl)
 {
@@ -1288,13 +1253,13 @@ int b43_dma_tx(struct b43_wldev *dev,
         hdr = (struct ieee80211_hdr *)skb->data;
         if (ctl->flags & IEEE80211_TXCTL_SEND_AFTER_DTIM) {
                 /* The multicast ring will be sent after the DTIM */
-                ring = dev->dma.tx_ring4;
+                ring = dev->dma.tx_ring_mcast;
                 /* Set the more-data bit. Ucode will clear it on
                  * the last frame for us. */
                 hdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_MOREDATA);
         } else {
                 /* Decide by priority where to put this frame. */
-                ring = priority_to_txring(dev, ctl->queue);
+                ring = select_ring_by_priority(dev, ctl->queue);
         }
 
         spin_lock_irqsave(&ring->lock, flags);
@@ -1309,6 +1274,11 @@ int b43_dma_tx(struct b43_wldev *dev,
          * That would be a mac80211 bug. */
         B43_WARN_ON(ring->stopped);
 
+        /* 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;
+
         err = dma_tx_fragment(ring, skb, ctl);
         if (unlikely(err == -ENOKEY)) {
                 /* Drop this packet, as we don't have the encryption key
@@ -1325,7 +1295,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, txring_to_priority(ring));
+                ieee80211_stop_queue(dev->wl->hw, ctl->queue);
                 ring->stopped = 1;
                 if (b43_debug(dev, B43_DBG_DMAVERBOSE)) {
                         b43dbg(dev->wl, "Stopped TX ring %d\n", ring->index);
@@ -1337,6 +1307,38 @@ out_unlock:
         return err;
 }
 
+static void b43_fill_txstatus_report(struct b43_dmaring *ring,
+                                    struct ieee80211_tx_status *report,
+                                    const struct b43_txstatus *status)
+{
+        bool frame_failed = 0;
+
+        if (status->acked) {
+                /* The frame was ACKed. */
+                report->flags |= IEEE80211_TX_STATUS_ACK;
+        } else {
+                /* The frame was not ACKed... */
+                if (!(report->control.flags & IEEE80211_TXCTL_NO_ACK)) {
+                        /* ...but we expected an ACK. */
+                        frame_failed = 1;
+                        report->excessive_retries = 1;
+                }
+        }
+        if (status->frame_count == 0) {
+                /* The frame was not transmitted at all. */
+                report->retry_count = 0;
+        } else {
+                report->retry_count = status->frame_count - 1;
+#ifdef CONFIG_B43_DEBUG
+                if (frame_failed)
+                        ring->nr_failed_tx_packets++;
+                else
+                        ring->nr_succeed_tx_packets++;
+                ring->nr_total_packet_tries += status->frame_count;
+#endif /* DEBUG */
+        }
+}
+
 void b43_dma_handle_txstatus(struct b43_wldev *dev,
                              const struct b43_txstatus *status)
 {
@@ -1371,18 +1373,7 @@ void b43_dma_handle_txstatus(struct b43_wldev *dev,
                          * status of the transmission.
                          * Some fields of txstat are already filled in dma_tx().
                          */
-                        if (status->acked) {
-                                meta->txstat.flags |= IEEE80211_TX_STATUS_ACK;
-                        } else {
-                                if (!(meta->txstat.control.flags
-                                      & IEEE80211_TXCTL_NO_ACK))
-                                        meta->txstat.excessive_retries = 1;
-                        }
-                        if (status->frame_count == 0) {
-                                /* The frame was not transmitted at all. */
-                                meta->txstat.retry_count = 0;
-                        } else
-                                meta->txstat.retry_count = status->frame_count - 1;
+                        b43_fill_txstatus_report(ring, &(meta->txstat), status);
                         ieee80211_tx_status_irqsafe(dev->wl->hw, meta->skb,
                                                     &(meta->txstat));
                         /* skb is freed by ieee80211_tx_status_irqsafe() */
@@ -1404,7 +1395,7 @@ void b43_dma_handle_txstatus(struct b43_wldev *dev,
         dev->stats.last_tx = jiffies;
         if (ring->stopped) {
                 B43_WARN_ON(free_slots(ring) < SLOTS_PER_PACKET);
-                ieee80211_wake_queue(dev->wl->hw, txring_to_priority(ring));
+                ieee80211_wake_queue(dev->wl->hw, ring->queue_prio);
                 ring->stopped = 0;
                 if (b43_debug(dev, B43_DBG_DMAVERBOSE)) {
                         b43dbg(dev->wl, "Woke up TX ring %d\n", ring->index);
@@ -1425,7 +1416,7 @@ void b43_dma_get_tx_stats(struct b43_wldev *dev,
 
         for (i = 0; i < nr_queues; i++) {
                 data = &(stats->data[i]);
-                ring = priority_to_txring(dev, i);
+                ring = select_ring_by_priority(dev, i);
 
                 spin_lock_irqsave(&ring->lock, flags);
                 data->len = ring->used_slots / SLOTS_PER_PACKET;
@@ -1451,25 +1442,6 @@ static void dma_rx(struct b43_dmaring *ring, int *slot)
         sync_descbuffer_for_cpu(ring, meta->dmaaddr, ring->rx_buffersize);
         skb = meta->skb;
 
-        if (ring->index == 3) {
-                /* We received an xmit status. */
-                struct b43_hwtxstatus *hw = (struct b43_hwtxstatus *)skb->data;
-                int i = 0;
-
-                while (hw->cookie == 0) {
-                        if (i > 100)
-                                break;
-                        i++;
-                        udelay(2);
-                        barrier();
-                }
-                b43_handle_hwtxstatus(ring->dev, hw);
-                /* recycle the descriptor buffer. */
-                sync_descbuffer_for_device(ring, meta->dmaaddr,
-                                           ring->rx_buffersize);
-
-                return;
-        }
         rxhdr = (struct b43_rxhdr_fw4 *)skb->data;
         len = le16_to_cpu(rxhdr->frame_len);
         if (len == 0) {
@@ -1526,7 +1498,7 @@ static void dma_rx(struct b43_dmaring *ring, int *slot)
         skb_pull(skb, ring->frameoffset);
 
         b43_rx(ring->dev, skb, rxhdr);
-      drop:
+drop:
         return;
 }
 
@@ -1572,21 +1544,19 @@ static void b43_dma_tx_resume_ring(struct b43_dmaring *ring)
 void b43_dma_tx_suspend(struct b43_wldev *dev)
 {
         b43_power_saving_ctl_bits(dev, B43_PS_AWAKE);
-        b43_dma_tx_suspend_ring(dev->dma.tx_ring0);
-        b43_dma_tx_suspend_ring(dev->dma.tx_ring1);
-        b43_dma_tx_suspend_ring(dev->dma.tx_ring2);
-        b43_dma_tx_suspend_ring(dev->dma.tx_ring3);
-        b43_dma_tx_suspend_ring(dev->dma.tx_ring4);
-        b43_dma_tx_suspend_ring(dev->dma.tx_ring5);
+        b43_dma_tx_suspend_ring(dev->dma.tx_ring_AC_BK);
+        b43_dma_tx_suspend_ring(dev->dma.tx_ring_AC_BE);
+        b43_dma_tx_suspend_ring(dev->dma.tx_ring_AC_VI);
+        b43_dma_tx_suspend_ring(dev->dma.tx_ring_AC_VO);
+        b43_dma_tx_suspend_ring(dev->dma.tx_ring_mcast);
 }
 
 void b43_dma_tx_resume(struct b43_wldev *dev)
 {
-        b43_dma_tx_resume_ring(dev->dma.tx_ring5);
-        b43_dma_tx_resume_ring(dev->dma.tx_ring4);
-        b43_dma_tx_resume_ring(dev->dma.tx_ring3);
-        b43_dma_tx_resume_ring(dev->dma.tx_ring2);
-        b43_dma_tx_resume_ring(dev->dma.tx_ring1);
-        b43_dma_tx_resume_ring(dev->dma.tx_ring0);
+        b43_dma_tx_resume_ring(dev->dma.tx_ring_mcast);
+        b43_dma_tx_resume_ring(dev->dma.tx_ring_AC_VO);
+        b43_dma_tx_resume_ring(dev->dma.tx_ring_AC_VI);
+        b43_dma_tx_resume_ring(dev->dma.tx_ring_AC_BE);
+        b43_dma_tx_resume_ring(dev->dma.tx_ring_AC_BK);
         b43_power_saving_ctl_bits(dev, 0);
 }
diff --git a/drivers/net/wireless/b43/dma.h b/drivers/net/wireless/b43/dma.h
index c0d6b69e6501..ea27085dec0e 100644
--- a/drivers/net/wireless/b43/dma.h
+++ b/drivers/net/wireless/b43/dma.h
@@ -245,6 +245,9 @@ struct b43_dmaring {
         enum b43_dmatype type;
         /* Boolean. Is this ring stopped at ieee80211 level? */
         bool stopped;
+        /* The QOS priority assigned to this ring. Only used for TX rings.
+         * This is the mac80211 "queue" value. */
+        u8 queue_prio;
         /* Lock, only used for TX. */
         spinlock_t lock;
         struct b43_wldev *dev;
@@ -253,7 +256,13 @@ struct b43_dmaring {
         int max_used_slots;
         /* Last time we injected a ring overflow. */
         unsigned long last_injected_overflow;
-#endif                          /* CONFIG_B43_DEBUG */
+        /* Statistics: Number of successfully transmitted packets */
+        u64 nr_succeed_tx_packets;
+        /* Statistics: Number of failed TX packets */
+        u64 nr_failed_tx_packets;
+        /* Statistics: Total number of TX plus all retries. */
+        u64 nr_total_packet_tries;
+#endif /* CONFIG_B43_DEBUG */
 };
 
 static inline u32 b43_dma_read(struct b43_dmaring *ring, u16 offset)
diff --git a/drivers/net/wireless/b43/main.c b/drivers/net/wireless/b43/main.c
index f745308faaad..694e29570e5d 100644
--- a/drivers/net/wireless/b43/main.c
+++ b/drivers/net/wireless/b43/main.c
@@ -78,6 +78,11 @@ static int modparam_nohwcrypt;
 module_param_named(nohwcrypt, modparam_nohwcrypt, int, 0444);
 MODULE_PARM_DESC(nohwcrypt, "Disable hardware encryption.");
 
+int b43_modparam_qos = 1;
+module_param_named(qos, b43_modparam_qos, int, 0444);
+MODULE_PARM_DESC(qos, "Enable QOS support (default on)");
+
+
 static const struct ssb_device_id b43_ssb_tbl[] = {
         SSB_DEVICE(SSB_VENDOR_BROADCOM, SSB_DEV_80211, 5),
         SSB_DEVICE(SSB_VENDOR_BROADCOM, SSB_DEV_80211, 6),
@@ -1589,11 +1594,10 @@ static void b43_interrupt_tasklet(struct b43_wldev *dev)
 
         /* Check the DMA reason registers for received data. */
         if (dma_reason[0] & B43_DMAIRQ_RX_DONE)
-                b43_dma_rx(dev->dma.rx_ring0);
-        if (dma_reason[3] & B43_DMAIRQ_RX_DONE)
-                b43_dma_rx(dev->dma.rx_ring3);
+                b43_dma_rx(dev->dma.rx_ring);
         B43_WARN_ON(dma_reason[1] & B43_DMAIRQ_RX_DONE);
         B43_WARN_ON(dma_reason[2] & B43_DMAIRQ_RX_DONE);
+        B43_WARN_ON(dma_reason[3] & B43_DMAIRQ_RX_DONE);
         B43_WARN_ON(dma_reason[4] & B43_DMAIRQ_RX_DONE);
         B43_WARN_ON(dma_reason[5] & B43_DMAIRQ_RX_DONE);
 
@@ -2708,10 +2712,178 @@ out:
         return NETDEV_TX_OK;
 }
 
+/* Locking: wl->irq_lock */
+static void b43_qos_params_upload(struct b43_wldev *dev,
+                                  const struct ieee80211_tx_queue_params *p,
+                                  u16 shm_offset)
+{
+        u16 params[B43_NR_QOSPARAMS];
+        int cw_min, cw_max, aifs, 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;
+
+        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_BSLOTS] = bslots;
+        params[B43_QOSPARAM_REGGAP] = bslots + aifs;
+
+        for (i = 0; i < ARRAY_SIZE(params); i++) {
+                if (i == B43_QOSPARAM_STATUS) {
+                        tmp = b43_shm_read16(dev, B43_SHM_SHARED,
+                                             shm_offset + (i * 2));
+                        /* Mark the parameters as updated. */
+                        tmp |= 0x100;
+                        b43_shm_write16(dev, B43_SHM_SHARED,
+                                        shm_offset + (i * 2),
+                                        tmp);
+                } else {
+                        b43_shm_write16(dev, B43_SHM_SHARED,
+                                        shm_offset + (i * 2),
+                                        params[i]);
+                }
+        }
+}
+
+/* Update the QOS parameters in hardware. */
+static void b43_qos_update(struct b43_wldev *dev)
+{
+        struct b43_wl *wl = dev->wl;
+        struct b43_qos_params *params;
+        unsigned long flags;
+        unsigned int i;
+
+        /* Mapping of mac80211 queues to b43 SHM offsets. */
+        static const u16 qos_shm_offsets[] = {
+                [0] = B43_QOS_VOICE,
+                [1] = B43_QOS_VIDEO,
+                [2] = B43_QOS_BESTEFFORT,
+                [3] = B43_QOS_BACKGROUND,
+        };
+        BUILD_BUG_ON(ARRAY_SIZE(qos_shm_offsets) != ARRAY_SIZE(wl->qos_params));
+
+        b43_mac_suspend(dev);
+        spin_lock_irqsave(&wl->irq_lock, flags);
+
+        for (i = 0; i < ARRAY_SIZE(wl->qos_params); i++) {
+                params = &(wl->qos_params[i]);
+                if (params->need_hw_update) {
+                        b43_qos_params_upload(dev, &(params->p),
+                                              qos_shm_offsets[i]);
+                        params->need_hw_update = 0;
+                }
+        }
+
+        spin_unlock_irqrestore(&wl->irq_lock, flags);
+        b43_mac_enable(dev);
+}
+
+static void b43_qos_clear(struct b43_wl *wl)
+{
+        struct b43_qos_params *params;
+        unsigned int i;
+
+        for (i = 0; i < ARRAY_SIZE(wl->qos_params); i++) {
+                params = &(wl->qos_params[i]);
+
+                memset(&(params->p), 0, sizeof(params->p));
+                params->p.aifs = -1;
+                params->need_hw_update = 1;
+        }
+}
+
+/* Initialize the core's QOS capabilities */
+static void b43_qos_init(struct b43_wldev *dev)
+{
+        struct b43_wl *wl = dev->wl;
+        unsigned int i;
+
+        /* Upload the current QOS parameters. */
+        for (i = 0; i < ARRAY_SIZE(wl->qos_params); i++)
+                wl->qos_params[i].need_hw_update = 1;
+        b43_qos_update(dev);
+
+        /* Enable QOS support. */
+        b43_hf_write(dev, b43_hf_read(dev) | B43_HF_EDCF);
+        b43_write16(dev, B43_MMIO_IFSCTL,
+                    b43_read16(dev, B43_MMIO_IFSCTL)
+                    | B43_MMIO_IFSCTL_USE_EDCF);
+}
+
+static void b43_qos_update_work(struct work_struct *work)
+{
+        struct b43_wl *wl = container_of(work, struct b43_wl, qos_update_work);
+        struct b43_wldev *dev;
+
+        mutex_lock(&wl->mutex);
+        dev = wl->current_dev;
+        if (likely(dev && (b43_status(dev) >= B43_STAT_INITIALIZED)))
+                b43_qos_update(dev);
+        mutex_unlock(&wl->mutex);
+}
+
 static int b43_op_conf_tx(struct ieee80211_hw *hw,
-                          int queue,
+                          int _queue,
                           const struct ieee80211_tx_queue_params *params)
 {
+        struct b43_wl *wl = hw_to_b43_wl(hw);
+        unsigned long flags;
+        unsigned int queue = (unsigned int)_queue;
+        struct b43_qos_params *p;
+
+        if (queue >= ARRAY_SIZE(wl->qos_params)) {
+                /* Queue not available or don't support setting
+                 * params on this queue. Return success to not
+                 * confuse mac80211. */
+                return 0;
+        }
+
+        spin_lock_irqsave(&wl->irq_lock, flags);
+        p = &(wl->qos_params[queue]);
+        memcpy(&(p->p), params, sizeof(p->p));
+        p->need_hw_update = 1;
+        spin_unlock_irqrestore(&wl->irq_lock, flags);
+
+        queue_work(hw->workqueue, &wl->qos_update_work);
+
         return 0;
 }
 
@@ -3732,6 +3904,7 @@ static int b43_op_start(struct ieee80211_hw *hw)
         memset(wl->mac_addr, 0, ETH_ALEN);
         wl->filter_flags = 0;
         wl->radiotap_enabled = 0;
+        b43_qos_clear(wl);
 
         /* First register RFkill.
          * LEDs that are registered later depend on it. */
@@ -3773,6 +3946,7 @@ static void b43_op_stop(struct ieee80211_hw *hw)
         struct b43_wldev *dev = wl->current_dev;
 
         b43_rfkill_exit(dev);
+        cancel_work_sync(&(wl->qos_update_work));
 
         mutex_lock(&wl->mutex);
         if (b43_status(dev) >= B43_STAT_STARTED)
@@ -3835,6 +4009,16 @@ static int b43_op_ibss_beacon_update(struct ieee80211_hw *hw,
         return 0;
 }
 
+static void b43_op_sta_notify(struct ieee80211_hw *hw,
+                              struct ieee80211_vif *vif,
+                              enum sta_notify_cmd notify_cmd,
+                              const u8 *addr)
+{
+        struct b43_wl *wl = hw_to_b43_wl(hw);
+
+        B43_WARN_ON(!vif || wl->vif != vif);
+}
+
 static const struct ieee80211_ops b43_hw_ops = {
         .tx                     = b43_op_tx,
         .conf_tx                = b43_op_conf_tx,
@@ -3851,6 +4035,7 @@ static const struct ieee80211_ops b43_hw_ops = {
         .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,
 };
 
 /* Hard-reset the chip. Do not call this directly.
@@ -4122,7 +4307,7 @@ static int b43_wireless_init(struct ssb_device *dev)
         hw->max_signal = 100;
         hw->max_rssi = -110;
         hw->max_noise = -110;
-        hw->queues = 1;         /* FIXME: hardware has more queues */
+        hw->queues = b43_modparam_qos ? 4 : 1;
         SET_IEEE80211_DEV(hw, dev->dev);
         if (is_valid_ether_addr(sprom->et1mac))
                 SET_IEEE80211_PERM_ADDR(hw, sprom->et1mac);
@@ -4138,6 +4323,7 @@ static int b43_wireless_init(struct ssb_device *dev)
         spin_lock_init(&wl->shm_lock);
         mutex_init(&wl->mutex);
         INIT_LIST_HEAD(&wl->devlist);
+        INIT_WORK(&wl->qos_update_work, b43_qos_update_work);
 
         ssb_set_devtypedata(dev, wl);
         b43info(wl, "Broadcom %04X WLAN found\n", dev->bus->chip_id);
diff --git a/drivers/net/wireless/b43/main.h b/drivers/net/wireless/b43/main.h
index 24a79f5d6ff5..1197975f9207 100644
--- a/drivers/net/wireless/b43/main.h
+++ b/drivers/net/wireless/b43/main.h
@@ -38,6 +38,10 @@
 /* Magic helper macro to pad structures. Ignore those above. It's magic. */
 #define PAD_BYTES(nr_bytes)             P4D_BYTES( __LINE__ , (nr_bytes))
 
+
+extern int b43_modparam_qos;
+
+
 /* Lightweight function to convert a frequency (in Mhz) to a channel number. */
 static inline u8 b43_freq_to_channel_5ghz(int freq)
 {
diff --git a/drivers/net/wireless/b43/xmit.c b/drivers/net/wireless/b43/xmit.c
index 187c11bee0f1..ec10a8e182f9 100644
--- a/drivers/net/wireless/b43/xmit.c
+++ b/drivers/net/wireless/b43/xmit.c
@@ -705,30 +705,3 @@ void b43_tx_resume(struct b43_wldev *dev)
 {
         b43_dma_tx_resume(dev);
 }
-
-#if 0
-static void upload_qos_parms(struct b43_wldev *dev,
-                             const u16 * parms, u16 offset)
-{
-        int i;
-
-        for (i = 0; i < B43_NR_QOSPARMS; i++) {
-                b43_shm_write16(dev, B43_SHM_SHARED,
-                                offset + (i * 2), parms[i]);
-        }
-}
-#endif
-
-/* Initialize the QoS parameters */
-void b43_qos_init(struct b43_wldev *dev)
-{
-        /* FIXME: This function must probably be called from the mac80211
-         * config callback. */
-        return;
-
-        b43_hf_write(dev, b43_hf_read(dev) | B43_HF_EDCF);
-        //FIXME kill magic
-        b43_write16(dev, 0x688, b43_read16(dev, 0x688) | 0x4);
-
-        /*TODO: We might need some stack support here to get the values. */
-}
diff --git a/drivers/net/wireless/b43/xmit.h b/drivers/net/wireless/b43/xmit.h
index 41765039552b..bf58a8a85258 100644
--- a/drivers/net/wireless/b43/xmit.h
+++ b/drivers/net/wireless/b43/xmit.h
@@ -302,18 +302,6 @@ void b43_handle_hwtxstatus(struct b43_wldev *dev,
 void b43_tx_suspend(struct b43_wldev *dev);
 void b43_tx_resume(struct b43_wldev *dev);
 
-#define B43_NR_QOSPARMS         22
-enum {
-        B43_QOSPARM_TXOP = 0,
-        B43_QOSPARM_CWMIN,
-        B43_QOSPARM_CWMAX,
-        B43_QOSPARM_CWCUR,
-        B43_QOSPARM_AIFS,
-        B43_QOSPARM_BSLOTS,
-        B43_QOSPARM_REGGAP,
-        B43_QOSPARM_STATUS,
-};
-void b43_qos_init(struct b43_wldev *dev);
 
 /* Helper functions for converting the key-table index from "firmware-format"
  * to "raw-format" and back. The firmware API changed for this at some revision.