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;