1 files changed, 17 insertions, 1 deletions

diff --git a/drivers/net/wireless/b43/main.c b/drivers/net/wireless/b43/main.c
index 0a4d0b3eceb0..d29323d50837 100644
--- a/drivers/net/wireless/b43/main.c
+++ b/drivers/net/wireless/b43/main.c
@@ -3093,7 +3093,6 @@ static int b43_op_tx(struct ieee80211_hw *hw,
         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)
@@ -3102,6 +3101,9 @@ static void b43_qos_params_upload(struct b43_wldev *dev,
         int bslots, tmp;
         unsigned int i;
 
+        if (!dev->qos_enabled)
+                return;
+
         bslots = b43_read16(dev, B43_MMIO_RNG) & p->cw_min;
 
         memset(&params, 0, sizeof(params));
@@ -3147,6 +3149,9 @@ static void b43_qos_upload_all(struct b43_wldev *dev)
         struct b43_qos_params *params;
         unsigned int i;
 
+        if (!dev->qos_enabled)
+                return;
+
         BUILD_BUG_ON(ARRAY_SIZE(b43_qos_shm_offsets) !=
                      ARRAY_SIZE(wl->qos_params));
 
@@ -3206,6 +3211,16 @@ static void b43_qos_clear(struct b43_wl *wl)
 /* Initialize the core's QOS capabilities */
 static void b43_qos_init(struct b43_wldev *dev)
 {
+        if (!dev->qos_enabled) {
+                /* Disable 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);
+                b43dbg(dev->wl, "QoS disabled\n");
+                return;
+        }
+
         /* Upload the current QOS parameters. */
         b43_qos_upload_all(dev);
 
@@ -3214,6 +3229,7 @@ static void b43_qos_init(struct b43_wldev *dev)
         b43_write16(dev, B43_MMIO_IFSCTL,
                     b43_read16(dev, B43_MMIO_IFSCTL)
                     | B43_MMIO_IFSCTL_USE_EDCF);
+        b43dbg(dev->wl, "QoS enabled\n");
 }
 
 static int b43_op_conf_tx(struct ieee80211_hw *hw, u16 _queue,