1 files changed, 21 insertions, 7 deletions

diff --git a/drivers/net/wireless/ath/ath9k/beacon.c b/drivers/net/wireless/ath/ath9k/beacon.c
index 57f91a9ff0eb..a21b21339fbc 100644
--- a/drivers/net/wireless/ath/ath9k/beacon.c
+++ b/drivers/net/wireless/ath/ath9k/beacon.c
@@ -559,6 +559,7 @@ static void ath_beacon_config_sta(struct ath_softc *sc,
         int cfpperiod, cfpcount;
         u32 nexttbtt = 0, intval, tsftu;
         u64 tsf;
+        int num_beacons, offset, dtim_dec_count, cfp_dec_count;
 
         memset(&bs, 0, sizeof(bs));
         intval = conf->beacon_interval & ATH9K_BEACON_PERIOD;
@@ -586,14 +587,27 @@ static void ath_beacon_config_sta(struct ath_softc *sc,
          */
         tsf = ath9k_hw_gettsf64(sc->sc_ah);
         tsftu = TSF_TO_TU(tsf>>32, tsf) + FUDGE;
-        do {
+
+        num_beacons = tsftu / intval + 1;
+        offset = tsftu % intval;
+        nexttbtt = tsftu - offset;
+        if (offset)
                 nexttbtt += intval;
-                if (--dtimcount < 0) {
-                        dtimcount = dtimperiod - 1;
-                        if (--cfpcount < 0)
-                                cfpcount = cfpperiod - 1;
-                }
-        } while (nexttbtt < tsftu);
+
+        /* DTIM Beacon every dtimperiod Beacon */
+        dtim_dec_count = num_beacons % dtimperiod;
+        /* CFP every cfpperiod DTIM Beacon */
+        cfp_dec_count = (num_beacons / dtimperiod) % cfpperiod;
+        if (dtim_dec_count)
+                cfp_dec_count++;
+
+        dtimcount -= dtim_dec_count;
+        if (dtimcount < 0)
+                dtimcount += dtimperiod;
+
+        cfpcount -= cfp_dec_count;
+        if (cfpcount < 0)
+                cfpcount += cfpperiod;
 
         bs.bs_intval = intval;
         bs.bs_nexttbtt = nexttbtt;