2 files changed, 33 insertions, 18 deletions

diff --git a/drivers/net/wireless/bcm43xx/bcm43xx.h b/drivers/net/wireless/bcm43xx/bcm43xx.h
index ee6571ed706d..bee84b58060c 100644
--- a/drivers/net/wireless/bcm43xx/bcm43xx.h
+++ b/drivers/net/wireless/bcm43xx/bcm43xx.h
@@ -737,6 +737,8 @@ struct bcm43xx_private {
         u32 irq_savedstate;
         /* Link Quality calculation context. */
         struct bcm43xx_noise_calculation noisecalc;
+        /* if > 0 MAC is suspended. if == 0 MAC is enabled. */
+        int mac_suspended;
 
         /* Threshold values. */
         //TODO: The RTS thr has to be _used_. Currently, it is only set via WX.
diff --git a/drivers/net/wireless/bcm43xx/bcm43xx_main.c b/drivers/net/wireless/bcm43xx/bcm43xx_main.c
index ab3a0ee9fac8..1af330d496fc 100644
--- a/drivers/net/wireless/bcm43xx/bcm43xx_main.c
+++ b/drivers/net/wireless/bcm43xx/bcm43xx_main.c
@@ -2298,13 +2298,17 @@ static int bcm43xx_gpio_cleanup(struct bcm43xx_private *bcm)
 /* http://bcm-specs.sipsolutions.net/EnableMac */
 void bcm43xx_mac_enable(struct bcm43xx_private *bcm)
 {
-        bcm43xx_write32(bcm, BCM43xx_MMIO_STATUS_BITFIELD,
-                        bcm43xx_read32(bcm, BCM43xx_MMIO_STATUS_BITFIELD)
-                        | BCM43xx_SBF_MAC_ENABLED);
-        bcm43xx_write32(bcm, BCM43xx_MMIO_GEN_IRQ_REASON, BCM43xx_IRQ_READY);
-        bcm43xx_read32(bcm, BCM43xx_MMIO_STATUS_BITFIELD); /* dummy read */
-        bcm43xx_read32(bcm, BCM43xx_MMIO_GEN_IRQ_REASON); /* dummy read */
-        bcm43xx_power_saving_ctl_bits(bcm, -1, -1);
+        bcm->mac_suspended--;
+        assert(bcm->mac_suspended >= 0);
+        if (bcm->mac_suspended == 0) {
+                bcm43xx_write32(bcm, BCM43xx_MMIO_STATUS_BITFIELD,
+                                bcm43xx_read32(bcm, BCM43xx_MMIO_STATUS_BITFIELD)
+                                | BCM43xx_SBF_MAC_ENABLED);
+                bcm43xx_write32(bcm, BCM43xx_MMIO_GEN_IRQ_REASON, BCM43xx_IRQ_READY);
+                bcm43xx_read32(bcm, BCM43xx_MMIO_STATUS_BITFIELD); /* dummy read */
+                bcm43xx_read32(bcm, BCM43xx_MMIO_GEN_IRQ_REASON); /* dummy read */
+                bcm43xx_power_saving_ctl_bits(bcm, -1, -1);
+        }
 }
 
 /* http://bcm-specs.sipsolutions.net/SuspendMAC */
@@ -2313,18 +2317,23 @@ void bcm43xx_mac_suspend(struct bcm43xx_private *bcm)
         int i;
         u32 tmp;
 
-        bcm43xx_power_saving_ctl_bits(bcm, -1, 1);
-        bcm43xx_write32(bcm, BCM43xx_MMIO_STATUS_BITFIELD,
-                        bcm43xx_read32(bcm, BCM43xx_MMIO_STATUS_BITFIELD)
-                        & ~BCM43xx_SBF_MAC_ENABLED);
-        bcm43xx_read32(bcm, BCM43xx_MMIO_GEN_IRQ_REASON); /* dummy read */
-        for (i = 100000; i; i--) {
-                tmp = bcm43xx_read32(bcm, BCM43xx_MMIO_GEN_IRQ_REASON);
-                if (tmp & BCM43xx_IRQ_READY)
-                        return;
-                udelay(10);
+        assert(bcm->mac_suspended >= 0);
+        if (bcm->mac_suspended == 0) {
+                bcm43xx_power_saving_ctl_bits(bcm, -1, 1);
+                bcm43xx_write32(bcm, BCM43xx_MMIO_STATUS_BITFIELD,
+                                bcm43xx_read32(bcm, BCM43xx_MMIO_STATUS_BITFIELD)
+                                & ~BCM43xx_SBF_MAC_ENABLED);
+                bcm43xx_read32(bcm, BCM43xx_MMIO_GEN_IRQ_REASON); /* dummy read */
+                for (i = 100000; i; i--) {
+                        tmp = bcm43xx_read32(bcm, BCM43xx_MMIO_GEN_IRQ_REASON);
+                        if (tmp & BCM43xx_IRQ_READY)
+                                goto out;
+                        udelay(10);
+                }
+                printkl(KERN_ERR PFX "MAC suspend failed\n");
         }
-        printkl(KERN_ERR PFX "MAC suspend failed\n");
+out:
+        bcm->mac_suspended++;
 }
 
 void bcm43xx_set_iwmode(struct bcm43xx_private *bcm,
@@ -3183,7 +3192,9 @@ static void bcm43xx_periodic_work_handler(void *d)
                  * be preemtible.
                  */
                 netif_stop_queue(bcm->net_dev);
+                synchronize_net();
                 spin_lock_irqsave(&bcm->irq_lock, flags);
+                bcm43xx_mac_suspend(bcm);
                 if (bcm43xx_using_pio(bcm))
                         bcm43xx_pio_freeze_txqueues(bcm);
                 savedirqs = bcm43xx_interrupt_disable(bcm, BCM43xx_IRQ_ALL);
@@ -3207,6 +3218,7 @@ static void bcm43xx_periodic_work_handler(void *d)
                         bcm43xx_interrupt_enable(bcm, savedirqs);
                         if (bcm43xx_using_pio(bcm))
                                 bcm43xx_pio_thaw_txqueues(bcm);
+                        bcm43xx_mac_enable(bcm);
                 }
                 netif_wake_queue(bcm->net_dev);
         }
@@ -3820,6 +3832,7 @@ static int bcm43xx_init_private(struct bcm43xx_private *bcm,
         bcm->softmac->set_channel = bcm43xx_ieee80211_set_chan;
 
         bcm->irq_savedstate = BCM43xx_IRQ_INITIAL;
+        bcm->mac_suspended = 1;
         bcm->pci_dev = pci_dev;
         bcm->net_dev = net_dev;
         bcm->bad_frames_preempt = modparam_bad_frames_preempt;