b43: Upload both beacon templates on initial load

This updates the beacon template code to upload both templates,
if we never uploaded one before.

Signed-off-by: Michael Buesch <mb@bu3sch.de>
Signed-off-by: John W. Linville <linville@tuxdriver.com>

diff --git a/drivers/net/wireless/b43/b43.h b/drivers/net/wireless/b43/b43.h
index 37783cd..dfa4bdd 100644 (file)
--- 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 8fdba94..b8e7775 100644 (file)
--- 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. */