4 files changed, 159 insertions, 6 deletions

diff --git a/drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.c b/drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.c
index 0e4457c83e4b..e113fb6f3241 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.c
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2010 Broadcom Corporation
+ * Copyright (c) 2013 Hauke Mehrtens <hauke@hauke-m.de>
  *
  * Permission to use, copy, modify, and/or distribute this software for any
  * purpose with or without fee is hereby granted, provided that the above
@@ -520,9 +521,17 @@ brcms_ops_bss_info_changed(struct ieee80211_hw *hw,
                 brcms_c_set_addrmatch(wl->wlc, RCM_BSSID_OFFSET, info->bssid);
                 spin_unlock_bh(&wl->lock);
         }
-        if (changed & BSS_CHANGED_BEACON)
+        if (changed & BSS_CHANGED_BEACON) {
                 /* Beacon data changed, retrieve new beacon (beaconing modes) */
-                brcms_err(core, "%s: beacon changed\n", __func__);
+                struct sk_buff *beacon;
+                u16 tim_offset = 0;
+
+                spin_lock_bh(&wl->lock);
+                beacon = ieee80211_beacon_get_tim(hw, vif, &tim_offset, NULL);
+                brcms_c_set_new_beacon(wl->wlc, beacon, tim_offset,
+                                       info->dtim_period);
+                spin_unlock_bh(&wl->lock);
+        }
 
         if (changed & BSS_CHANGED_BEACON_ENABLED) {
                 /* Beaconing should be enabled/disabled (beaconing modes) */
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/main.c b/drivers/net/wireless/brcm80211/brcmsmac/main.c
index 4ffb0c6d1a0c..60dc2c4bb2f4 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/main.c
+++ b/drivers/net/wireless/brcm80211/brcmsmac/main.c
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2010 Broadcom Corporation
+ * Copyright (c) 2013 Hauke Mehrtens <hauke@hauke-m.de>
  *
  * Permission to use, copy, modify, and/or distribute this software for any
  * purpose with or without fee is hereby granted, provided that the above
@@ -448,6 +449,8 @@ static void brcms_c_detach_mfree(struct brcms_c_info *wlc)
         kfree(wlc->corestate);
         kfree(wlc->hw->bandstate[0]);
         kfree(wlc->hw);
+        if (wlc->beacon)
+                dev_kfree_skb_any(wlc->beacon);
 
         /* free the wlc */
         kfree(wlc);
@@ -4084,10 +4087,14 @@ void brcms_c_wme_setparams(struct brcms_c_info *wlc, u16 aci,
                                           *shm_entry++);
         }
 
-        if (suspend) {
+        if (suspend)
                 brcms_c_suspend_mac_and_wait(wlc);
+
+        brcms_c_update_beacon(wlc);
+        brcms_c_update_probe_resp(wlc, false);
+
+        if (suspend)
                 brcms_c_enable_mac(wlc);
-        }
 }
 
 static void brcms_c_edcf_setparams(struct brcms_c_info *wlc, bool suspend)
@@ -7375,6 +7382,107 @@ int brcms_c_get_header_len(void)
         return TXOFF;
 }
 
+static void brcms_c_beacon_write(struct brcms_c_info *wlc,
+                                 struct sk_buff *beacon, u16 tim_offset,
+                                 u16 dtim_period, bool bcn0, bool bcn1)
+{
+        size_t len;
+        struct ieee80211_tx_info *tx_info;
+        struct brcms_hardware *wlc_hw = wlc->hw;
+        struct ieee80211_hw *ieee_hw = brcms_c_pub(wlc)->ieee_hw;
+
+        /* Get tx_info */
+        tx_info = IEEE80211_SKB_CB(beacon);
+
+        len = min_t(size_t, beacon->len, BCN_TMPL_LEN);
+        wlc->bcn_rspec = ieee80211_get_tx_rate(ieee_hw, tx_info)->hw_value;
+
+        brcms_c_compute_plcp(wlc, wlc->bcn_rspec,
+                             len + FCS_LEN - D11_PHY_HDR_LEN, beacon->data);
+
+        /* "Regular" and 16 MBSS but not for 4 MBSS */
+        /* Update the phytxctl for the beacon based on the rspec */
+        brcms_c_beacon_phytxctl_txant_upd(wlc, wlc->bcn_rspec);
+
+        if (bcn0) {
+                /* write the probe response into the template region */
+                brcms_b_write_template_ram(wlc_hw, T_BCN0_TPL_BASE,
+                                            (len + 3) & ~3, beacon->data);
+
+                /* write beacon length to SCR */
+                brcms_b_write_shm(wlc_hw, M_BCN0_FRM_BYTESZ, (u16) len);
+        }
+        if (bcn1) {
+                /* write the probe response into the template region */
+                brcms_b_write_template_ram(wlc_hw, T_BCN1_TPL_BASE,
+                                            (len + 3) & ~3, beacon->data);
+
+                /* write beacon length to SCR */
+                brcms_b_write_shm(wlc_hw, M_BCN1_FRM_BYTESZ, (u16) len);
+        }
+
+        if (tim_offset != 0) {
+                brcms_b_write_shm(wlc_hw, M_TIMBPOS_INBEACON,
+                                  tim_offset + D11B_PHY_HDR_LEN);
+                brcms_b_write_shm(wlc_hw, M_DOT11_DTIMPERIOD, dtim_period);
+        } else {
+                brcms_b_write_shm(wlc_hw, M_TIMBPOS_INBEACON,
+                                  len + D11B_PHY_HDR_LEN);
+                brcms_b_write_shm(wlc_hw, M_DOT11_DTIMPERIOD, 0);
+        }
+}
+
+static void brcms_c_update_beacon_hw(struct brcms_c_info *wlc,
+                                     struct sk_buff *beacon, u16 tim_offset,
+                                     u16 dtim_period)
+{
+        struct brcms_hardware *wlc_hw = wlc->hw;
+        struct bcma_device *core = wlc_hw->d11core;
+
+        /* Hardware beaconing for this config */
+        u32 both_valid = MCMD_BCN0VLD | MCMD_BCN1VLD;
+
+        /* Check if both templates are in use, if so sched. an interrupt
+         *      that will call back into this routine
+         */
+        if ((bcma_read32(core, D11REGOFFS(maccommand)) & both_valid) == both_valid)
+                /* clear any previous status */
+                bcma_write32(core, D11REGOFFS(macintstatus), MI_BCNTPL);
+
+        if (wlc->beacon_template_virgin) {
+                wlc->beacon_template_virgin = false;
+                brcms_c_beacon_write(wlc, beacon, tim_offset, dtim_period, true,
+                                     true);
+                /* mark beacon0 valid */
+                bcma_set32(core, D11REGOFFS(maccommand), MCMD_BCN0VLD);
+                return;
+        }
+
+        /* Check that after scheduling the interrupt both of the
+         *      templates are still busy. if not clear the int. & remask
+         */
+        if ((bcma_read32(core, D11REGOFFS(maccommand)) & both_valid) == both_valid) {
+                wlc->defmacintmask |= MI_BCNTPL;
+                return;
+        }
+
+        if (!(bcma_read32(core, D11REGOFFS(maccommand)) & MCMD_BCN0VLD)) {
+                brcms_c_beacon_write(wlc, beacon, tim_offset, dtim_period, true,
+                                     false);
+                /* mark beacon0 valid */
+                bcma_set32(core, D11REGOFFS(maccommand), MCMD_BCN0VLD);
+                return;
+        }
+        if (!(bcma_read32(core, D11REGOFFS(maccommand)) & MCMD_BCN1VLD)) {
+                brcms_c_beacon_write(wlc, beacon, tim_offset, dtim_period,
+                                     false, true);
+                /* mark beacon0 valid */
+                bcma_set32(core, D11REGOFFS(maccommand), MCMD_BCN1VLD);
+                return;
+        }
+        return;
+}
+
 /*
  * Update all beacons for the system.
  */
@@ -7383,9 +7491,31 @@ void brcms_c_update_beacon(struct brcms_c_info *wlc)
         struct brcms_bss_cfg *bsscfg = wlc->bsscfg;
 
         if (wlc->pub->up && (bsscfg->type == BRCMS_TYPE_AP ||
-                             bsscfg->type == BRCMS_TYPE_ADHOC))
+                             bsscfg->type == BRCMS_TYPE_ADHOC)) {
                 /* Clear the soft intmask */
                 wlc->defmacintmask &= ~MI_BCNTPL;
+                if (!wlc->beacon)
+                        return;
+                brcms_c_update_beacon_hw(wlc, wlc->beacon,
+                                         wlc->beacon_tim_offset,
+                                         wlc->beacon_dtim_period);
+        }
+}
+
+void brcms_c_set_new_beacon(struct brcms_c_info *wlc, struct sk_buff *beacon,
+                            u16 tim_offset, u16 dtim_period)
+{
+        if (!beacon)
+                return;
+        if (wlc->beacon)
+                dev_kfree_skb_any(wlc->beacon);
+        wlc->beacon = beacon;
+
+        /* add PLCP */
+        skb_push(wlc->beacon, D11_PHY_HDR_LEN);
+        wlc->beacon_tim_offset = tim_offset;
+        wlc->beacon_dtim_period = dtim_period;
+        brcms_c_update_beacon(wlc);
 }
 
 /* Write ssid into shared memory */
@@ -7784,6 +7914,10 @@ bool brcms_c_dpc(struct brcms_c_info *wlc, bool bounded)
                 brcms_rfkill_set_hw_state(wlc->wl);
         }
 
+        /* BCN template is available */
+        if (macintstatus & MI_BCNTPL)
+                brcms_c_update_beacon(wlc);
+
         /* it isn't done and needs to be resched if macintstatus is non-zero */
         return wlc->macintstatus != 0;
 
@@ -7920,6 +8054,7 @@ brcms_c_attach(struct brcms_info *wl, struct bcma_device *core, uint unit,
         pub->unit = unit;
         pub->_piomode = piomode;
         wlc->bandinit_pending = false;
+        wlc->beacon_template_virgin = true;
 
         /* populate struct brcms_c_info with default values  */
         brcms_c_info_init(wlc, unit);
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/main.h b/drivers/net/wireless/brcm80211/brcmsmac/main.h
index 397cff3cea57..82382da9a493 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/main.h
+++ b/drivers/net/wireless/brcm80211/brcmsmac/main.h
@@ -492,6 +492,8 @@ struct brcms_c_info {
         bool radio_monitor;
         bool going_down;
 
+        bool beacon_template_virgin;
+
         struct brcms_timer *wdtimer;
         struct brcms_timer *radio_timer;
 
@@ -561,6 +563,10 @@ struct brcms_c_info {
 
         struct wiphy *wiphy;
         struct scb pri_scb;
+
+        struct sk_buff *beacon;
+        u16 beacon_tim_offset;
+        u16 beacon_dtim_period;
 };
 
 /* antsel module specific state */
@@ -630,7 +636,6 @@ extern u16 brcms_c_compute_rtscts_dur(struct brcms_c_info *wlc, bool cts_only,
 extern void brcms_c_inval_dma_pkts(struct brcms_hardware *hw,
                                struct ieee80211_sta *sta,
                                void (*dma_callback_fn));
-extern void brcms_c_update_beacon(struct brcms_c_info *wlc);
 extern void brcms_c_update_probe_resp(struct brcms_c_info *wlc, bool suspend);
 extern int brcms_c_set_nmode(struct brcms_c_info *wlc);
 extern void brcms_c_beacon_phytxctl_txant_upd(struct brcms_c_info *wlc,
diff --git a/drivers/net/wireless/brcm80211/brcmsmac/pub.h b/drivers/net/wireless/brcm80211/brcmsmac/pub.h
index 9dae59c1c13a..8a3071f8f511 100644
--- a/drivers/net/wireless/brcm80211/brcmsmac/pub.h
+++ b/drivers/net/wireless/brcm80211/brcmsmac/pub.h
@@ -332,5 +332,9 @@ extern bool brcms_c_check_radio_disabled(struct brcms_c_info *wlc);
 extern void brcms_c_mute(struct brcms_c_info *wlc, bool on);
 extern bool brcms_c_tx_flush_completed(struct brcms_c_info *wlc);
 extern void brcms_c_start_station(struct brcms_c_info *wlc, u8 *addr);
+extern void brcms_c_update_beacon(struct brcms_c_info *wlc);
+extern void brcms_c_set_new_beacon(struct brcms_c_info *wlc,
+                                   struct sk_buff *beacon, u16 tim_offset,
+                                   u16 dtim_period);
 
 #endif                          /* _BRCM_PUB_H_ */