1 files changed, 275 insertions, 170 deletions

diff --git a/drivers/net/wireless/ath/ath9k/beacon.c b/drivers/net/wireless/ath/ath9k/beacon.c
index 4d4b22d52dfd..d4d8ceced89b 100644
--- a/drivers/net/wireless/ath/ath9k/beacon.c
+++ b/drivers/net/wireless/ath/ath9k/beacon.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2008-2009 Atheros Communications Inc.
+ * Copyright (c) 2008-2011 Atheros Communications Inc.
  *
  * Permission to use, copy, modify, and/or distribute this software for any
  * purpose with or without fee is hereby granted, provided that the above
@@ -18,6 +18,12 @@
 
 #define FUDGE 2
 
+static void ath9k_reset_beacon_status(struct ath_softc *sc)
+{
+        sc->beacon.tx_processed = false;
+        sc->beacon.tx_last = false;
+}
+
 /*
  *  This function will modify certain transmit queue properties depending on
  *  the operating mode of the station (AP or AdHoc).  Parameters are AIFS
@@ -28,7 +34,7 @@ int ath_beaconq_config(struct ath_softc *sc)
         struct ath_hw *ah = sc->sc_ah;
         struct ath_common *common = ath9k_hw_common(ah);
         struct ath9k_tx_queue_info qi, qi_be;
-        int qnum;
+        struct ath_txq *txq;
 
         ath9k_hw_get_txq_props(ah, sc->beacon.beaconq, &qi);
         if (sc->sc_ah->opmode == NL80211_IFTYPE_AP) {
@@ -38,16 +44,16 @@ int ath_beaconq_config(struct ath_softc *sc)
                 qi.tqi_cwmax = 0;
         } else {
                 /* Adhoc mode; important thing is to use 2x cwmin. */
-                qnum = sc->tx.hwq_map[WME_AC_BE];
-                ath9k_hw_get_txq_props(ah, qnum, &qi_be);
+                txq = sc->tx.txq_map[WME_AC_BE];
+                ath9k_hw_get_txq_props(ah, txq->axq_qnum, &qi_be);
                 qi.tqi_aifs = qi_be.tqi_aifs;
                 qi.tqi_cwmin = 4*qi_be.tqi_cwmin;
                 qi.tqi_cwmax = qi_be.tqi_cwmax;
         }
 
         if (!ath9k_hw_set_txq_props(ah, sc->beacon.beaconq, &qi)) {
-                ath_print(common, ATH_DBG_FATAL,
-                          "Unable to update h/w beacon queue parameters\n");
+                ath_err(common,
+                        "Unable to update h/w beacon queue parameters\n");
                 return 0;
         } else {
                 ath9k_hw_resettxqueue(ah, sc->beacon.beaconq);
@@ -57,8 +63,8 @@ int ath_beaconq_config(struct ath_softc *sc)
 
 /*
  *  Associates the beacon frame buffer with a transmit descriptor.  Will set
- *  up all required antenna switch parameters, rate codes, and channel flags.
- *  Beacons are always sent out at the lowest rate, and are not retried.
+ *  up rate codes, and channel flags. Beacons are always sent out at the
+ *  lowest rate, and are not retried.
 */
 static void ath_beacon_setup(struct ath_softc *sc, struct ath_vif *avp,
                              struct ath_buf *bf, int rateidx)
@@ -68,20 +74,16 @@ static void ath_beacon_setup(struct ath_softc *sc, struct ath_vif *avp,
         struct ath_common *common = ath9k_hw_common(ah);
         struct ath_desc *ds;
         struct ath9k_11n_rate_series series[4];
-        int flags, antenna, ctsrate = 0, ctsduration = 0;
+        int flags, ctsrate = 0, ctsduration = 0;
         struct ieee80211_supported_band *sband;
         u8 rate = 0;
 
+        ath9k_reset_beacon_status(sc);
+
         ds = bf->bf_desc;
         flags = ATH9K_TXDESC_NOACK;
 
         ds->ds_link = 0;
-        /*
-         * Switch antenna every beacon.
-         * Should only switch every beacon period, not for every SWBA
-         * XXX assumes two antennae
-         */
-        antenna = ((sc->beacon.ast_be_xmit / sc->nbcnvifs) & 1 ? 2 : 1);
 
         sband = &sc->sbands[common->hw->conf.channel->band];
         rate = sband->bitrates[rateidx].hw_value;
@@ -103,17 +105,35 @@ static void ath_beacon_setup(struct ath_softc *sc, struct ath_vif *avp,
         memset(series, 0, sizeof(struct ath9k_11n_rate_series) * 4);
         series[0].Tries = 1;
         series[0].Rate = rate;
-        series[0].ChSel = common->tx_chainmask;
+        series[0].ChSel = ath_txchainmask_reduction(sc,
+                        common->tx_chainmask, series[0].Rate);
         series[0].RateFlags = (ctsrate) ? ATH9K_RATESERIES_RTS_CTS : 0;
         ath9k_hw_set11n_ratescenario(ah, ds, ds, 0, ctsrate, ctsduration,
                                      series, 4, 0);
 }
 
+static void ath_tx_cabq(struct ieee80211_hw *hw, struct sk_buff *skb)
+{
+        struct ath_softc *sc = hw->priv;
+        struct ath_common *common = ath9k_hw_common(sc->sc_ah);
+        struct ath_tx_control txctl;
+
+        memset(&txctl, 0, sizeof(struct ath_tx_control));
+        txctl.txq = sc->beacon.cabq;
+
+        ath_dbg(common, ATH_DBG_XMIT,
+                "transmitting CABQ packet, skb: %p\n", skb);
+
+        if (ath_tx_start(hw, skb, &txctl) != 0) {
+                ath_dbg(common, ATH_DBG_XMIT, "CABQ TX failed\n");
+                dev_kfree_skb_any(skb);
+        }
+}
+
 static struct ath_buf *ath_beacon_generate(struct ieee80211_hw *hw,
                                            struct ieee80211_vif *vif)
 {
-        struct ath_wiphy *aphy = hw->priv;
-        struct ath_softc *sc = aphy->sc;
+        struct ath_softc *sc = hw->priv;
         struct ath_common *common = ath9k_hw_common(sc->sc_ah);
         struct ath_buf *bf;
         struct ath_vif *avp;
@@ -122,13 +142,12 @@ static struct ath_buf *ath_beacon_generate(struct ieee80211_hw *hw,
         struct ieee80211_tx_info *info;
         int cabq_depth;
 
-        if (aphy->state != ATH_WIPHY_ACTIVE)
-                return NULL;
+        ath9k_reset_beacon_status(sc);
 
         avp = (void *)vif->drv_priv;
         cabq = sc->beacon.cabq;
 
-        if (avp->av_bcbuf == NULL)
+        if ((avp->av_bcbuf == NULL) || !avp->is_bslot_active)
                 return NULL;
 
         /* Release the old beacon first */
@@ -136,9 +155,10 @@ static struct ath_buf *ath_beacon_generate(struct ieee80211_hw *hw,
         bf = avp->av_bcbuf;
         skb = bf->bf_mpdu;
         if (skb) {
-                dma_unmap_single(sc->dev, bf->bf_dmacontext,
+                dma_unmap_single(sc->dev, bf->bf_buf_addr,
                                  skb->len, DMA_TO_DEVICE);
                 dev_kfree_skb_any(skb);
+                bf->bf_buf_addr = 0;
         }
 
         /* Get a new beacon from mac80211 */
@@ -162,14 +182,13 @@ static struct ath_buf *ath_beacon_generate(struct ieee80211_hw *hw,
                 hdr->seq_ctrl |= cpu_to_le16(sc->tx.seq_no);
         }
 
-        bf->bf_buf_addr = bf->bf_dmacontext =
-                dma_map_single(sc->dev, skb->data,
-                               skb->len, DMA_TO_DEVICE);
+        bf->bf_buf_addr = dma_map_single(sc->dev, skb->data,
+                                         skb->len, DMA_TO_DEVICE);
         if (unlikely(dma_mapping_error(sc->dev, bf->bf_buf_addr))) {
                 dev_kfree_skb_any(skb);
                 bf->bf_mpdu = NULL;
-                ath_print(common, ATH_DBG_FATAL,
-                          "dma_mapping_error on beaconing\n");
+                bf->bf_buf_addr = 0;
+                ath_err(common, "dma_mapping_error on beaconing\n");
                 return NULL;
         }
 
@@ -189,8 +208,8 @@ static struct ath_buf *ath_beacon_generate(struct ieee80211_hw *hw,
 
         if (skb && cabq_depth) {
                 if (sc->nvifs > 1) {
-                        ath_print(common, ATH_DBG_BEACON,
-                                  "Flushing previous cabq traffic\n");
+                        ath_dbg(common, ATH_DBG_BEACON,
+                                "Flushing previous cabq traffic\n");
                         ath_draintxq(sc, cabq, false);
                 }
         }
@@ -205,13 +224,13 @@ static struct ath_buf *ath_beacon_generate(struct ieee80211_hw *hw,
         return bf;
 }
 
-int ath_beacon_alloc(struct ath_wiphy *aphy, struct ieee80211_vif *vif)
+int ath_beacon_alloc(struct ath_softc *sc, struct ieee80211_vif *vif)
 {
-        struct ath_softc *sc = aphy->sc;
         struct ath_common *common = ath9k_hw_common(sc->sc_ah);
         struct ath_vif *avp;
         struct ath_buf *bf;
         struct sk_buff *skb;
+        struct ath_beacon_config *cur_conf = &sc->cur_beacon_conf;
         __le64 tstamp;
 
         avp = (void *)vif->drv_priv;
@@ -224,9 +243,7 @@ int ath_beacon_alloc(struct ath_wiphy *aphy, struct ieee80211_vif *vif)
                                                  struct ath_buf, list);
                 list_del(&avp->av_bcbuf->list);
 
-                if (sc->sc_ah->opmode == NL80211_IFTYPE_AP ||
-                    sc->sc_ah->opmode == NL80211_IFTYPE_ADHOC ||
-                    sc->sc_ah->opmode == NL80211_IFTYPE_MESH_POINT) {
+                if (ath9k_uses_beacons(vif->type)) {
                         int slot;
                         /*
                          * Assign the vif to a beacon xmit slot. As
@@ -236,6 +253,7 @@ int ath_beacon_alloc(struct ath_wiphy *aphy, struct ieee80211_vif *vif)
                         for (slot = 0; slot < ATH_BCBUF; slot++)
                                 if (sc->beacon.bslot[slot] == NULL) {
                                         avp->av_bslot = slot;
+                                        avp->is_bslot_active = false;
 
                                         /* NB: keep looking for a double slot */
                                         if (slot == 0 || !sc->beacon.bslot[slot-1])
@@ -243,7 +261,6 @@ int ath_beacon_alloc(struct ath_wiphy *aphy, struct ieee80211_vif *vif)
                                 }
                         BUG_ON(sc->beacon.bslot[avp->av_bslot] != NULL);
                         sc->beacon.bslot[avp->av_bslot] = vif;
-                        sc->beacon.bslot_aphy[avp->av_bslot] = aphy;
                         sc->nbcnvifs++;
                 }
         }
@@ -252,27 +269,26 @@ int ath_beacon_alloc(struct ath_wiphy *aphy, struct ieee80211_vif *vif)
         bf = avp->av_bcbuf;
         if (bf->bf_mpdu != NULL) {
                 skb = bf->bf_mpdu;
-                dma_unmap_single(sc->dev, bf->bf_dmacontext,
+                dma_unmap_single(sc->dev, bf->bf_buf_addr,
                                  skb->len, DMA_TO_DEVICE);
                 dev_kfree_skb_any(skb);
                 bf->bf_mpdu = NULL;
+                bf->bf_buf_addr = 0;
         }
 
         /* NB: the beacon data buffer must be 32-bit aligned. */
         skb = ieee80211_beacon_get(sc->hw, vif);
-        if (skb == NULL) {
-                ath_print(common, ATH_DBG_BEACON, "cannot get skb\n");
+        if (skb == NULL)
                 return -ENOMEM;
-        }
 
         tstamp = ((struct ieee80211_mgmt *)skb->data)->u.beacon.timestamp;
-        sc->beacon.bc_tstamp = le64_to_cpu(tstamp);
+        sc->beacon.bc_tstamp = (u32) le64_to_cpu(tstamp);
         /* Calculate a TSF adjustment factor required for staggered beacons. */
         if (avp->av_bslot > 0) {
                 u64 tsfadjust;
                 int intval;
 
-                intval = sc->beacon_interval ? : ATH_DEFAULT_BINTVAL;
+                intval = cur_conf->beacon_interval ? : ATH_DEFAULT_BINTVAL;
 
                 /*
                  * Calculate the TSF offset for this beacon slot, i.e., the
@@ -282,13 +298,12 @@ int ath_beacon_alloc(struct ath_wiphy *aphy, struct ieee80211_vif *vif)
                  * adjustment. Other slots are adjusted to get the timestamp
                  * close to the TBTT for the BSS.
                  */
-                tsfadjust = intval * avp->av_bslot / ATH_BCBUF;
-                avp->tsf_adjust = cpu_to_le64(TU_TO_USEC(tsfadjust));
+                tsfadjust = TU_TO_USEC(intval * avp->av_bslot) / ATH_BCBUF;
+                avp->tsf_adjust = cpu_to_le64(tsfadjust);
 
-                ath_print(common, ATH_DBG_BEACON,
-                          "stagger beacons, bslot %d intval "
-                          "%u tsfadjust %llu\n",
-                          avp->av_bslot, intval, (unsigned long long)tsfadjust);
+                ath_dbg(common, ATH_DBG_BEACON,
+                        "stagger beacons, bslot %d intval %u tsfadjust %llu\n",
+                        avp->av_bslot, intval, (unsigned long long)tsfadjust);
 
                 ((struct ieee80211_mgmt *)skb->data)->u.beacon.timestamp =
                         avp->tsf_adjust;
@@ -296,16 +311,16 @@ int ath_beacon_alloc(struct ath_wiphy *aphy, struct ieee80211_vif *vif)
                 avp->tsf_adjust = cpu_to_le64(0);
 
         bf->bf_mpdu = skb;
-        bf->bf_buf_addr = bf->bf_dmacontext =
-                dma_map_single(sc->dev, skb->data,
-                               skb->len, DMA_TO_DEVICE);
+        bf->bf_buf_addr = dma_map_single(sc->dev, skb->data,
+                                         skb->len, DMA_TO_DEVICE);
         if (unlikely(dma_mapping_error(sc->dev, bf->bf_buf_addr))) {
                 dev_kfree_skb_any(skb);
                 bf->bf_mpdu = NULL;
-                ath_print(common, ATH_DBG_FATAL,
-                          "dma_mapping_error on beacon alloc\n");
+                bf->bf_buf_addr = 0;
+                ath_err(common, "dma_mapping_error on beacon alloc\n");
                 return -ENOMEM;
         }
+        avp->is_bslot_active = true;
 
         return 0;
 }
@@ -315,19 +330,21 @@ void ath_beacon_return(struct ath_softc *sc, struct ath_vif *avp)
         if (avp->av_bcbuf != NULL) {
                 struct ath_buf *bf;
 
+                avp->is_bslot_active = false;
                 if (avp->av_bslot != -1) {
                         sc->beacon.bslot[avp->av_bslot] = NULL;
-                        sc->beacon.bslot_aphy[avp->av_bslot] = NULL;
                         sc->nbcnvifs--;
+                        avp->av_bslot = -1;
                 }
 
                 bf = avp->av_bcbuf;
                 if (bf->bf_mpdu != NULL) {
                         struct sk_buff *skb = bf->bf_mpdu;
-                        dma_unmap_single(sc->dev, bf->bf_dmacontext,
+                        dma_unmap_single(sc->dev, bf->bf_buf_addr,
                                          skb->len, DMA_TO_DEVICE);
                         dev_kfree_skb_any(skb);
                         bf->bf_mpdu = NULL;
+                        bf->bf_buf_addr = 0;
                 }
                 list_add_tail(&bf->list, &sc->beacon.bbuf);
 
@@ -338,15 +355,13 @@ void ath_beacon_return(struct ath_softc *sc, struct ath_vif *avp)
 void ath_beacon_tasklet(unsigned long data)
 {
         struct ath_softc *sc = (struct ath_softc *)data;
+        struct ath_beacon_config *cur_conf = &sc->cur_beacon_conf;
         struct ath_hw *ah = sc->sc_ah;
         struct ath_common *common = ath9k_hw_common(ah);
         struct ath_buf *bf = NULL;
         struct ieee80211_vif *vif;
-        struct ath_wiphy *aphy;
         int slot;
-        u32 bfaddr, bc = 0, tsftu;
-        u64 tsf;
-        u16 intval;
+        u32 bfaddr, bc = 0;
 
         /*
          * Check if the previous beacon has gone out.  If
@@ -358,60 +373,65 @@ void ath_beacon_tasklet(unsigned long data)
         if (ath9k_hw_numtxpending(ah, sc->beacon.beaconq) != 0) {
                 sc->beacon.bmisscnt++;
 
-                if (sc->beacon.bmisscnt < BSTUCK_THRESH) {
-                        ath_print(common, ATH_DBG_BEACON,
-                                  "missed %u consecutive beacons\n",
-                                  sc->beacon.bmisscnt);
+                if (sc->beacon.bmisscnt < BSTUCK_THRESH * sc->nbcnvifs) {
+                        ath_dbg(common, ATH_DBG_BSTUCK,
+                                "missed %u consecutive beacons\n",
+                                sc->beacon.bmisscnt);
+                        ath9k_hw_stop_dma_queue(ah, sc->beacon.beaconq);
+                        if (sc->beacon.bmisscnt > 3)
+                                ath9k_hw_bstuck_nfcal(ah);
                 } else if (sc->beacon.bmisscnt >= BSTUCK_THRESH) {
-                        ath_print(common, ATH_DBG_BEACON,
-                                  "beacon is officially stuck\n");
+                        ath_dbg(common, ATH_DBG_BSTUCK,
+                                "beacon is officially stuck\n");
                         sc->sc_flags |= SC_OP_TSF_RESET;
-                        ath_reset(sc, false);
+                        ath_reset(sc, true);
                 }
 
                 return;
         }
 
-        if (sc->beacon.bmisscnt != 0) {
-                ath_print(common, ATH_DBG_BEACON,
-                          "resume beacon xmit after %u misses\n",
-                          sc->beacon.bmisscnt);
-                sc->beacon.bmisscnt = 0;
-        }
-
         /*
          * Generate beacon frames. we are sending frames
          * staggered so calculate the slot for this frame based
          * on the tsf to safeguard against missing an swba.
          */
 
-        intval = sc->beacon_interval ? : ATH_DEFAULT_BINTVAL;
 
-        tsf = ath9k_hw_gettsf64(ah);
-        tsftu = TSF_TO_TU(tsf>>32, tsf);
-        slot = ((tsftu % intval) * ATH_BCBUF) / intval;
-        /*
-         * Reverse the slot order to get slot 0 on the TBTT offset that does
-         * not require TSF adjustment and other slots adding
-         * slot/ATH_BCBUF * beacon_int to timestamp. For example, with
-         * ATH_BCBUF = 4, we process beacon slots as follows: 3 2 1 0 3 2 1 ..
-         * and slot 0 is at correct offset to TBTT.
-         */
-        slot = ATH_BCBUF - slot - 1;
-        vif = sc->beacon.bslot[slot];
-        aphy = sc->beacon.bslot_aphy[slot];
+        if (ah->opmode == NL80211_IFTYPE_AP) {
+                u16 intval;
+                u32 tsftu;
+                u64 tsf;
+
+                intval = cur_conf->beacon_interval ? : ATH_DEFAULT_BINTVAL;
+                tsf = ath9k_hw_gettsf64(ah);
+                tsf += TU_TO_USEC(ah->config.sw_beacon_response_time);
+                tsftu = TSF_TO_TU((tsf * ATH_BCBUF) >>32, tsf * ATH_BCBUF);
+                slot = (tsftu % (intval * ATH_BCBUF)) / intval;
+                vif = sc->beacon.bslot[slot];
+
+                ath_dbg(common, ATH_DBG_BEACON,
+                        "slot %d [tsf %llu tsftu %u intval %u] vif %p\n",
+                        slot, tsf, tsftu / ATH_BCBUF, intval, vif);
+        } else {
+                slot = 0;
+                vif = sc->beacon.bslot[slot];
+        }
 
-        ath_print(common, ATH_DBG_BEACON,
-                  "slot %d [tsf %llu tsftu %u intval %u] vif %p\n",
-                  slot, tsf, tsftu, intval, vif);
 
         bfaddr = 0;
         if (vif) {
-                bf = ath_beacon_generate(aphy->hw, vif);
+                bf = ath_beacon_generate(sc->hw, vif);
                 if (bf != NULL) {
                         bfaddr = bf->bf_daddr;
                         bc = 1;
                 }
+
+                if (sc->beacon.bmisscnt != 0) {
+                        ath_dbg(common, ATH_DBG_BSTUCK,
+                                "resume beacon xmit after %u misses\n",
+                                sc->beacon.bmisscnt);
+                        sc->beacon.bmisscnt = 0;
+                }
         }
 
         /*
@@ -439,16 +459,6 @@ void ath_beacon_tasklet(unsigned long data)
                 sc->beacon.updateslot = OK;
         }
         if (bfaddr != 0) {
-                /*
-                 * Stop any current dma and put the new frame(s) on the queue.
-                 * This should never fail since we check above that no frames
-                 * are still pending on the queue.
-                 */
-                if (!ath9k_hw_stoptxdma(ah, sc->beacon.beaconq)) {
-                        ath_print(common, ATH_DBG_FATAL,
-                                "beacon queue %u did not stop?\n", sc->beacon.beaconq);
-                }
-
                 /* NB: cabq traffic should already be queued and primed */
                 ath9k_hw_puttxbuf(ah, sc->beacon.beaconq, bfaddr);
                 ath9k_hw_txstart(ah, sc->beacon.beaconq);
@@ -461,13 +471,17 @@ static void ath9k_beacon_init(struct ath_softc *sc,
                               u32 next_beacon,
                               u32 beacon_period)
 {
-        if (beacon_period & ATH9K_BEACON_RESET_TSF)
+        if (sc->sc_flags & SC_OP_TSF_RESET) {
                 ath9k_ps_wakeup(sc);
+                ath9k_hw_reset_tsf(sc->sc_ah);
+        }
 
         ath9k_hw_beaconinit(sc->sc_ah, next_beacon, beacon_period);
 
-        if (beacon_period & ATH9K_BEACON_RESET_TSF)
+        if (sc->sc_flags & SC_OP_TSF_RESET) {
                 ath9k_ps_restore(sc);
+                sc->sc_flags &= ~SC_OP_TSF_RESET;
+        }
 }
 
 /*
@@ -482,32 +496,23 @@ static void ath_beacon_config_ap(struct ath_softc *sc,
         u32 nexttbtt, intval;
 
         /* NB: the beacon interval is kept internally in TU's */
-        intval = conf->beacon_interval & ATH9K_BEACON_PERIOD;
+        intval = TU_TO_USEC(conf->beacon_interval & ATH9K_BEACON_PERIOD);
         intval /= ATH_BCBUF;    /* for staggered beacons */
         nexttbtt = intval;
 
-        if (sc->sc_flags & SC_OP_TSF_RESET)
-                intval |= ATH9K_BEACON_RESET_TSF;
-
         /*
          * In AP mode we enable the beacon timers and SWBA interrupts to
          * prepare beacon frames.
          */
-        intval |= ATH9K_BEACON_ENA;
         ah->imask |= ATH9K_INT_SWBA;
         ath_beaconq_config(sc);
 
         /* Set the computed AP beacon timers */
 
-        ath9k_hw_set_interrupts(ah, 0);
+        ath9k_hw_disable_interrupts(ah);
         ath9k_beacon_init(sc, nexttbtt, intval);
         sc->beacon.bmisscnt = 0;
         ath9k_hw_set_interrupts(ah, ah->imask);
-
-        /* Clear the reset TSF flag, so that subsequent beacon updation
-           will not reset the HW TSF. */
-
-        sc->sc_flags &= ~SC_OP_TSF_RESET;
 }
 
 /*
@@ -532,8 +537,8 @@ static void ath_beacon_config_sta(struct ath_softc *sc,
 
         /* No need to configure beacon if we are not associated */
         if (!common->curaid) {
-                ath_print(common, ATH_DBG_BEACON,
-                         "STA is not yet associated..skipping beacon config\n");
+                ath_dbg(common, ATH_DBG_BEACON,
+                        "STA is not yet associated..skipping beacon config\n");
                 return;
         }
 
@@ -545,8 +550,6 @@ static void ath_beacon_config_sta(struct ath_softc *sc,
          * last beacon we received (which may be none).
          */
         dtimperiod = conf->dtim_period;
-        if (dtimperiod <= 0)            /* NB: 0 if not known */
-                dtimperiod = 1;
         dtimcount = conf->dtim_count;
         if (dtimcount >= dtimperiod)    /* NB: sanity check */
                 dtimcount = 0;
@@ -554,8 +557,6 @@ static void ath_beacon_config_sta(struct ath_softc *sc,
         cfpcount = 0;
 
         sleepduration = conf->listen_interval * intval;
-        if (sleepduration <= 0)
-                sleepduration = intval;
 
         /*
          * Pull nexttbtt forward to reflect the current
@@ -626,89 +627,135 @@ static void ath_beacon_config_sta(struct ath_softc *sc,
         /* TSF out of range threshold fixed at 1 second */
         bs.bs_tsfoor_threshold = ATH9K_TSFOOR_THRESHOLD;
 
-        ath_print(common, ATH_DBG_BEACON, "tsf: %llu tsftu: %u\n", tsf, tsftu);
-        ath_print(common, ATH_DBG_BEACON,
-                  "bmiss: %u sleep: %u cfp-period: %u maxdur: %u next: %u\n",
-                  bs.bs_bmissthreshold, bs.bs_sleepduration,
-                  bs.bs_cfpperiod, bs.bs_cfpmaxduration, bs.bs_cfpnext);
+        ath_dbg(common, ATH_DBG_BEACON, "tsf: %llu tsftu: %u\n", tsf, tsftu);
+        ath_dbg(common, ATH_DBG_BEACON,
+                "bmiss: %u sleep: %u cfp-period: %u maxdur: %u next: %u\n",
+                bs.bs_bmissthreshold, bs.bs_sleepduration,
+                bs.bs_cfpperiod, bs.bs_cfpmaxduration, bs.bs_cfpnext);
 
         /* Set the computed STA beacon timers */
 
-        ath9k_hw_set_interrupts(ah, 0);
+        ath9k_hw_disable_interrupts(ah);
         ath9k_hw_set_sta_beacon_timers(ah, &bs);
         ah->imask |= ATH9K_INT_BMISS;
-        ath9k_hw_set_interrupts(ah, ah->imask);
+
+        /*
+         * If the beacon config is called beacause of TSFOOR,
+         * Interrupts will be enabled back at the end of ath9k_tasklet
+         */
+        if (!(sc->ps_flags & PS_TSFOOR_SYNC))
+                ath9k_hw_set_interrupts(ah, ah->imask);
 }
 
 static void ath_beacon_config_adhoc(struct ath_softc *sc,
-                                    struct ath_beacon_config *conf,
-                                    struct ieee80211_vif *vif)
+                                    struct ath_beacon_config *conf)
 {
         struct ath_hw *ah = sc->sc_ah;
         struct ath_common *common = ath9k_hw_common(ah);
-        u64 tsf;
-        u32 tsftu, intval, nexttbtt;
-
-        intval = conf->beacon_interval & ATH9K_BEACON_PERIOD;
-
+        u32 tsf, delta, intval, nexttbtt;
 
-        /* Pull nexttbtt forward to reflect the current TSF */
+        ath9k_reset_beacon_status(sc);
 
-        nexttbtt = TSF_TO_TU(sc->beacon.bc_tstamp >> 32, sc->beacon.bc_tstamp);
-        if (nexttbtt == 0)
-                nexttbtt = intval;
-        else if (intval)
-                nexttbtt = roundup(nexttbtt, intval);
+        tsf = ath9k_hw_gettsf32(ah) + TU_TO_USEC(FUDGE);
+        intval = TU_TO_USEC(conf->beacon_interval & ATH9K_BEACON_PERIOD);
 
-        tsf = ath9k_hw_gettsf64(ah);
-        tsftu = TSF_TO_TU((u32)(tsf>>32), (u32)tsf) + FUDGE;
-        do {
-                nexttbtt += intval;
-        } while (nexttbtt < tsftu);
+        if (!sc->beacon.bc_tstamp)
+                nexttbtt = tsf + intval;
+        else {
+                if (tsf > sc->beacon.bc_tstamp)
+                        delta = (tsf - sc->beacon.bc_tstamp);
+                else
+                        delta = (tsf + 1 + (~0U - sc->beacon.bc_tstamp));
+                nexttbtt = tsf + intval - (delta % intval);
+        }
 
-        ath_print(common, ATH_DBG_BEACON,
-                  "IBSS nexttbtt %u intval %u (%u)\n",
-                  nexttbtt, intval, conf->beacon_interval);
+        ath_dbg(common, ATH_DBG_BEACON,
+                "IBSS nexttbtt %u intval %u (%u)\n",
+                nexttbtt, intval, conf->beacon_interval);
 
         /*
          * In IBSS mode enable the beacon timers but only enable SWBA interrupts
          * if we need to manually prepare beacon frames.  Otherwise we use a
          * self-linked tx descriptor and let the hardware deal with things.
          */
-        intval |= ATH9K_BEACON_ENA;
         ah->imask |= ATH9K_INT_SWBA;
 
         ath_beaconq_config(sc);
 
         /* Set the computed ADHOC beacon timers */
 
-        ath9k_hw_set_interrupts(ah, 0);
+        ath9k_hw_disable_interrupts(ah);
         ath9k_beacon_init(sc, nexttbtt, intval);
         sc->beacon.bmisscnt = 0;
-        ath9k_hw_set_interrupts(ah, ah->imask);
+        /*
+         * If the beacon config is called beacause of TSFOOR,
+         * Interrupts will be enabled back at the end of ath9k_tasklet
+         */
+        if (!(sc->ps_flags & PS_TSFOOR_SYNC))
+                ath9k_hw_set_interrupts(ah, ah->imask);
 }
 
-void ath_beacon_config(struct ath_softc *sc, struct ieee80211_vif *vif)
+static bool ath9k_allow_beacon_config(struct ath_softc *sc,
+                                      struct ieee80211_vif *vif)
 {
         struct ath_beacon_config *cur_conf = &sc->cur_beacon_conf;
         struct ath_common *common = ath9k_hw_common(sc->sc_ah);
-        enum nl80211_iftype iftype;
+        struct ieee80211_bss_conf *bss_conf = &vif->bss_conf;
+        struct ath_vif *avp = (void *)vif->drv_priv;
 
-        /* Setup the beacon configuration parameters */
-        if (vif) {
-                struct ieee80211_bss_conf *bss_conf = &vif->bss_conf;
+        /*
+         * Can not have different beacon interval on multiple
+         * AP interface case
+         */
+        if ((sc->sc_ah->opmode == NL80211_IFTYPE_AP) &&
+            (sc->nbcnvifs > 1) &&
+            (vif->type == NL80211_IFTYPE_AP) &&
+            (cur_conf->beacon_interval != bss_conf->beacon_int)) {
+                ath_dbg(common, ATH_DBG_CONFIG,
+                        "Changing beacon interval of multiple \
+                        AP interfaces !\n");
+                return false;
+        }
+        /*
+         * Can not configure station vif's beacon config
+         * while on AP opmode
+         */
+        if ((sc->sc_ah->opmode == NL80211_IFTYPE_AP) &&
+            (vif->type != NL80211_IFTYPE_AP)) {
+                ath_dbg(common, ATH_DBG_CONFIG,
+                        "STA vif's beacon not allowed on AP mode\n");
+                return false;
+        }
+        /*
+         * Do not allow beacon config if HW was already configured
+         * with another STA vif
+         */
+        if ((sc->sc_ah->opmode == NL80211_IFTYPE_STATION) &&
+            (vif->type == NL80211_IFTYPE_STATION) &&
+            (sc->sc_flags & SC_OP_BEACONS) &&
+            !avp->primary_sta_vif) {
+                ath_dbg(common, ATH_DBG_CONFIG,
+                        "Beacon already configured for a station interface\n");
+                return false;
+        }
+        return true;
+}
 
-                iftype = vif->type;
+void ath_beacon_config(struct ath_softc *sc, struct ieee80211_vif *vif)
+{
+        struct ath_beacon_config *cur_conf = &sc->cur_beacon_conf;
+        struct ieee80211_bss_conf *bss_conf = &vif->bss_conf;
 
-                cur_conf->beacon_interval = bss_conf->beacon_int;
-                cur_conf->dtim_period = bss_conf->dtim_period;
-                cur_conf->listen_interval = 1;
-                cur_conf->dtim_count = 1;
-                cur_conf->bmiss_timeout =
-                        ATH_DEFAULT_BMISS_LIMIT * cur_conf->beacon_interval;
-        } else {
-                iftype = sc->sc_ah->opmode;
-        }
+        if (!ath9k_allow_beacon_config(sc, vif))
+                return;
+
+        /* Setup the beacon configuration parameters */
+        cur_conf->beacon_interval = bss_conf->beacon_int;
+        cur_conf->dtim_period = bss_conf->dtim_period;
+        cur_conf->listen_interval = 1;
+        cur_conf->dtim_count = 1;
+        cur_conf->bmiss_timeout =
+                ATH_DEFAULT_BMISS_LIMIT * cur_conf->beacon_interval;
 
         /*
          * It looks like mac80211 may end up using beacon interval of zero in
@@ -719,22 +766,80 @@ void ath_beacon_config(struct ath_softc *sc, struct ieee80211_vif *vif)
         if (cur_conf->beacon_interval == 0)
                 cur_conf->beacon_interval = 100;
 
-        switch (iftype) {
+        /*
+         * We don't parse dtim period from mac80211 during the driver
+         * initialization as it breaks association with hidden-ssid
+         * AP and it causes latency in roaming
+         */
+        if (cur_conf->dtim_period == 0)
+                cur_conf->dtim_period = 1;
+
+        ath_set_beacon(sc);
+}
+
+static bool ath_has_valid_bslot(struct ath_softc *sc)
+{
+        struct ath_vif *avp;
+        int slot;
+        bool found = false;
+
+        for (slot = 0; slot < ATH_BCBUF; slot++) {
+                if (sc->beacon.bslot[slot]) {
+                        avp = (void *)sc->beacon.bslot[slot]->drv_priv;
+                        if (avp->is_bslot_active) {
+                                found = true;
+                                break;
+                        }
+                }
+        }
+        return found;
+}
+
+
+void ath_set_beacon(struct ath_softc *sc)
+{
+        struct ath_common *common = ath9k_hw_common(sc->sc_ah);
+        struct ath_beacon_config *cur_conf = &sc->cur_beacon_conf;
+
+        switch (sc->sc_ah->opmode) {
         case NL80211_IFTYPE_AP:
-                ath_beacon_config_ap(sc, cur_conf);
+                if (ath_has_valid_bslot(sc))
+                        ath_beacon_config_ap(sc, cur_conf);
                 break;
         case NL80211_IFTYPE_ADHOC:
         case NL80211_IFTYPE_MESH_POINT:
-                ath_beacon_config_adhoc(sc, cur_conf, vif);
+                ath_beacon_config_adhoc(sc, cur_conf);
                 break;
         case NL80211_IFTYPE_STATION:
                 ath_beacon_config_sta(sc, cur_conf);
                 break;
         default:
-                ath_print(common, ATH_DBG_CONFIG,
-                          "Unsupported beaconing mode\n");
+                ath_dbg(common, ATH_DBG_CONFIG,
+                        "Unsupported beaconing mode\n");
                 return;
         }
 
         sc->sc_flags |= SC_OP_BEACONS;
 }
+
+void ath9k_set_beaconing_status(struct ath_softc *sc, bool status)
+{
+        struct ath_hw *ah = sc->sc_ah;
+
+        if (!ath_has_valid_bslot(sc))
+                return;
+
+        ath9k_ps_wakeup(sc);
+        if (status) {
+                /* Re-enable beaconing */
+                ah->imask |= ATH9K_INT_SWBA;
+                ath9k_hw_set_interrupts(ah, ah->imask);
+        } else {
+                /* Disable SWBA interrupt */
+                ah->imask &= ~ATH9K_INT_SWBA;
+                ath9k_hw_set_interrupts(ah, ah->imask);
+                tasklet_kill(&sc->bcon_tasklet);
+                ath9k_hw_stop_dma_queue(ah, sc->beacon.beaconq);
+        }
+        ath9k_ps_restore(sc);
+}