2 files changed, 55 insertions, 16 deletions

diff --git a/drivers/net/wireless/b43/b43.h b/drivers/net/wireless/b43/b43.h
index 37783cdd301a..dfa4bdd5597c 100644
--- a/drivers/net/wireless/b43/b43.h
+++ b/drivers/net/wireless/b43/b43.h
@@ -737,6 +737,7 @@ struct b43_wl {
         struct ieee80211_tx_control beacon_txctl;
         bool beacon0_uploaded;
         bool beacon1_uploaded;
+        bool beacon_templates_virgin; /* Never wrote the templates? */
         struct work_struct beacon_update_trigger;
 
         /* The current QOS parameters for the 4 queues.
diff --git a/drivers/net/wireless/b43/main.c b/drivers/net/wireless/b43/main.c
index 8fdba9415c04..6c3d9ea0a9f8 100644
--- a/drivers/net/wireless/b43/main.c
+++ b/drivers/net/wireless/b43/main.c
@@ -1544,6 +1544,30 @@ static void b43_write_probe_resp_template(struct b43_wldev *dev,
         kfree(probe_resp_data);
 }
 
+static void b43_upload_beacon0(struct b43_wldev *dev)
+{
+        struct b43_wl *wl = dev->wl;
+
+        if (wl->beacon0_uploaded)
+                return;
+        b43_write_beacon_template(dev, 0x68, 0x18);
+        /* FIXME: Probe resp upload doesn't really belong here,
+         *        but we don't use that feature anyway. */
+        b43_write_probe_resp_template(dev, 0x268, 0x4A,
+                                      &__b43_ratetable[3]);
+        wl->beacon0_uploaded = 1;
+}
+
+static void b43_upload_beacon1(struct b43_wldev *dev)
+{
+        struct b43_wl *wl = dev->wl;
+
+        if (wl->beacon1_uploaded)
+                return;
+        b43_write_beacon_template(dev, 0x468, 0x1A);
+        wl->beacon1_uploaded = 1;
+}
+
 static void handle_irq_beacon(struct b43_wldev *dev)
 {
         struct b43_wl *wl = dev->wl;
@@ -1568,24 +1592,27 @@ static void handle_irq_beacon(struct b43_wldev *dev)
                 return;
         }
 
-        if (!beacon0_valid) {
-                if (!wl->beacon0_uploaded) {
-                        b43_write_beacon_template(dev, 0x68, 0x18);
-                        b43_write_probe_resp_template(dev, 0x268, 0x4A,
-                                                      &__b43_ratetable[3]);
-                        wl->beacon0_uploaded = 1;
-                }
+        if (unlikely(wl->beacon_templates_virgin)) {
+                /* We never uploaded a beacon before.
+                 * Upload both templates now, but only mark one valid. */
+                wl->beacon_templates_virgin = 0;
+                b43_upload_beacon0(dev);
+                b43_upload_beacon1(dev);
                 cmd = b43_read32(dev, B43_MMIO_MACCMD);
                 cmd |= B43_MACCMD_BEACON0_VALID;
                 b43_write32(dev, B43_MMIO_MACCMD, cmd);
-        } else if (!beacon1_valid) {
-                if (!wl->beacon1_uploaded) {
-                        b43_write_beacon_template(dev, 0x468, 0x1A);
-                        wl->beacon1_uploaded = 1;
+        } else {
+                if (!beacon0_valid) {
+                        b43_upload_beacon0(dev);
+                        cmd = b43_read32(dev, B43_MMIO_MACCMD);
+                        cmd |= B43_MACCMD_BEACON0_VALID;
+                        b43_write32(dev, B43_MMIO_MACCMD, cmd);
+                } else if (!beacon1_valid) {
+                        b43_upload_beacon1(dev);
+                        cmd = b43_read32(dev, B43_MMIO_MACCMD);
+                        cmd |= B43_MACCMD_BEACON1_VALID;
+                        b43_write32(dev, B43_MMIO_MACCMD, cmd);
                 }
-                cmd = b43_read32(dev, B43_MMIO_MACCMD);
-                cmd |= B43_MACCMD_BEACON1_VALID;
-                b43_write32(dev, B43_MMIO_MACCMD, cmd);
         }
 }
 
@@ -4073,6 +4100,9 @@ static int b43_op_start(struct ieee80211_hw *hw)
         wl->filter_flags = 0;
         wl->radiotap_enabled = 0;
         b43_qos_clear(wl);
+        wl->beacon0_uploaded = 0;
+        wl->beacon1_uploaded = 0;
+        wl->beacon_templates_virgin = 1;
 
         /* First register RFkill.
          * LEDs that are registered later depend on it. */
@@ -4241,7 +4271,9 @@ static void b43_chip_reset(struct work_struct *work)
                         goto out;
                 }
         }
-      out:
+out:
+        if (err)
+                wl->current_dev = NULL; /* Failed to init the dev. */
         mutex_unlock(&wl->mutex);
         if (err)
                 b43err(wl, "Controller restart FAILED\n");
@@ -4382,9 +4414,11 @@ static void b43_one_core_detach(struct ssb_device *dev)
         struct b43_wldev *wldev;
         struct b43_wl *wl;
 
+        /* Do not cancel ieee80211-workqueue based work here.
+         * See comment in b43_remove(). */
+
         wldev = ssb_get_drvdata(dev);
         wl = wldev->wl;
-        cancel_work_sync(&wldev->restart_work);
         b43_debugfs_remove_device(wldev);
         b43_wireless_core_detach(wldev);
         list_del(&wldev->list);
@@ -4569,6 +4603,10 @@ static void b43_remove(struct ssb_device *dev)
         struct b43_wl *wl = ssb_get_devtypedata(dev);
         struct b43_wldev *wldev = ssb_get_drvdata(dev);
 
+        /* We must cancel any work here before unregistering from ieee80211,
+         * as the ieee80211 unreg will destroy the workqueue. */
+        cancel_work_sync(&wldev->restart_work);
+
         B43_WARN_ON(!wl);
         if (wl->current_dev == wldev)
                 ieee80211_unregister_hw(wl->hw);