Index: madwifi-ng-r2568-20070710/ath/if_ath.c
===================================================================
--- madwifi-ng-r2568-20070710.orig/ath/if_ath.c	2007-07-11 22:59:41.856233797 +0200
+++ madwifi-ng-r2568-20070710/ath/if_ath.c	2007-07-11 22:59:42.416265710 +0200
@@ -4426,16 +4426,31 @@
 	struct ieee80211com *ic = &sc->sc_ic;
 	struct ath_hal *ah = sc->sc_ah;
 	struct ieee80211_node *ni;
-	u_int32_t nexttbtt, intval;
+	u_int32_t nexttbtt = 0;
+	u_int32_t intval;
+	u_int64_t tsf, hw_tsf;
+	u_int32_t tsftu, hw_tsftu;
+	int should_reset_tsf = 0;
 
 	if (vap == NULL)
 		vap = TAILQ_FIRST(&ic->ic_vaps);   /* XXX */
 
 	ni = vap->iv_bss;
 
-	/* extract tstamp from last beacon and convert to TU */
-	nexttbtt = TSF_TO_TU(LE_READ_4(ni->ni_tstamp.data + 4),
-			     LE_READ_4(ni->ni_tstamp.data));
+	hw_tsf = ath_hal_gettsf64(ah);
+	tsf = le64_to_cpu(ni->ni_tstamp.tsf);
+	hw_tsftu = hw_tsf >> 10;
+	tsftu = tsf >> 10;
+
+	/* we should reset hw TSF only once, so we increment
+	   ni_tstamp.tsf to avoid resetting the hw TSF multiple
+	   times */
+
+	if (tsf == 0) {
+		should_reset_tsf = 1;
+		ni->ni_tstamp.tsf = cpu_to_le64(1);
+	}
+
 	/* XXX conditionalize multi-bss support? */
 	if (ic->ic_opmode == IEEE80211_M_HOSTAP) {
 		/*
@@ -4449,20 +4464,61 @@
 		if (sc->sc_stagbeacons)
 			intval /= ATH_BCBUF;	/* for staggered beacons */
 		if ((sc->sc_nostabeacons) &&
-		    (vap->iv_opmode == IEEE80211_M_HOSTAP))
-			nexttbtt = 0;
+			(vap->iv_opmode == IEEE80211_M_HOSTAP))
+			should_reset_tsf = 1;
 	} else
 		intval = ni->ni_intval & HAL_BEACON_PERIOD;
-	if (nexttbtt == 0)		/* e.g. for ap mode */
+
+#define	FUDGE	2
+	sc->sc_syncbeacon = 0;
+	if (should_reset_tsf) {
+
+		/* We just created the interface and TSF will be reset to
+		   zero, so next beacon will be sent at the next intval
+		   time */
+
 		nexttbtt = intval;
-	else if (intval)		/* NB: can be 0 for monitor mode */
-		nexttbtt = roundup(nexttbtt, intval);
-	DPRINTF(sc, ATH_DEBUG_BEACON, "%s: nexttbtt %u intval %u (%u)\n",
-		__func__, nexttbtt, intval, ni->ni_intval);
+	} else if (intval) {	/* NB: can be 0 for monitor mode */
+		if (tsf == 1) {
+ 	
+			/* We do not receive any beacons or probe response. Since
+			   a beacon should be sent every 'intval' ms, we compute
+			   the next beacon timestamp using the hardware TSF. We
+			   ensure that it is at least FUDGE ms ahead of the
+			   current TSF. Otherwise, we use the next beacon
+			   timestamp again */
+
+			nexttbtt = roundup(hw_tsftu +1, intval);
+			while (nexttbtt <= hw_tsftu + FUDGE) {
+				nexttbtt += intval;
+			}
+		} else {
+			if (tsf > hw_tsf) {
+
+			/* We do receive a beacon from someone else in the past,
+			   but the hw TSF has not been updated (otherwise we
+			   would have tsf >= hw_tsf). Since we cannot use the
+			   hardware TSF, we will do nothing and wait for the
+			   next beacon. In order to do so, we set sc->syncbeacon
+			   again */
+
+				sc->sc_syncbeacon = 1;
+				goto ath_beacon_config_debug;
+			} else {
+				/* We do receive a beacon in the past, normal case. We
+				   make sure that the timestamp is at least FUDGE ms
+				   ahead of the hardware TSF */
+
+				nexttbtt = tsftu + intval;
+				while (nexttbtt <= hw_tsftu + FUDGE) {
+					nexttbtt += intval;
+				}
+			}
+		}
+	}
+
 	if (ic->ic_opmode == IEEE80211_M_STA &&	!(sc->sc_nostabeacons)) {
 		HAL_BEACON_STATE bs;
-		u_int64_t tsf;
-		u_int32_t tsftu;
 		int dtimperiod, dtimcount;
 		int cfpperiod, cfpcount;
 
@@ -4478,13 +4534,13 @@
 			dtimcount = 0;		/* XXX? */
 		cfpperiod = 1;			/* NB: no PCF support yet */
 		cfpcount = 0;
-#define	FUDGE	2
 		/*
 		 * Pull nexttbtt forward to reflect the current
 		 * TSF and calculate dtim+cfp state for the result.
 		 */
-		tsf = ath_hal_gettsf64(ah);
-		tsftu = TSF_TO_TU(tsf>>32, tsf) + FUDGE;
+		nexttbtt = tsftu;
+		if (nexttbtt == 0)		/* e.g. for ap mode */
+			nexttbtt = intval;
 		do {
 			nexttbtt += intval;
 			if (--dtimcount < 0) {
@@ -4492,7 +4548,7 @@
 				if (--cfpcount < 0)
 					cfpcount = cfpperiod - 1;
 			}
-		} while (nexttbtt < tsftu);
+		} while (nexttbtt < hw_tsftu + FUDGE);
 #undef FUDGE
 		memset(&bs, 0, sizeof(bs));
 		bs.bs_intval = intval;
@@ -4544,7 +4600,7 @@
 		DPRINTF(sc, ATH_DEBUG_BEACON,
 			"%s: tsf %llu tsf:tu %u intval %u nexttbtt %u dtim %u nextdtim %u bmiss %u sleep %u cfp:period %u maxdur %u next %u timoffset %u\n",
 			__func__,
-			(unsigned long long) tsf, tsftu,
+			(unsigned long long) hw_tsf, hw_tsftu,
 			bs.bs_intval,
 			bs.bs_nexttbtt,
 			bs.bs_dtimperiod,
@@ -4566,7 +4622,7 @@
 		ath_hal_intrset(ah, sc->sc_imask);
 	} else {
 		ath_hal_intrset(ah, 0);
-		if (nexttbtt == intval)
+		if (should_reset_tsf)
 			intval |= HAL_BEACON_RESET_TSF;
 		if (ic->ic_opmode == IEEE80211_M_IBSS) {
 			/*
@@ -4603,8 +4659,40 @@
 		if (ic->ic_opmode == IEEE80211_M_IBSS && sc->sc_hasveol)
 			ath_beacon_start_adhoc(sc, vap);
 	}
-	sc->sc_syncbeacon = 0;
 #undef TSF_TO_TU
+
+	ath_beacon_config_debug:
+
+	/* we print all debug messages here, in order to preserve the
+	   time critical aspect of this function */
+
+	DPRINTF(sc, ATH_DEBUG_BEACON,
+		"%s: ni=%p tsf=%llu hw_tsf=%llu tsftu=%u hw_tsftu=%u\n",
+		__func__, ni, tsf, hw_tsf, tsftu, hw_tsftu);
+
+	if (should_reset_tsf) {
+		/* we just created the interface */
+		DPRINTF(sc, ATH_DEBUG_BEACON, "%s: first beacon\n",__func__);
+	} else {
+		if (tsf == 1) {
+			/* we do not receive any beacons or probe response */
+			DPRINTF(sc, ATH_DEBUG_BEACON,
+				"%s: no beacon received...\n",__func__);
+		} else {
+			if (tsf > hw_tsf) {
+				/* we do receive a beacon and the hw TSF has not been updated */
+				DPRINTF(sc, ATH_DEBUG_BEACON,
+					"%s: beacon received, but TSF is incorrect\n",__func__);
+			} else {
+				/* we do receive a beacon in the past, normal case */
+				DPRINTF(sc, ATH_DEBUG_BEACON,
+					"%s: beacon received, TSF is correct\n",__func__);
+			}
+		}
+	}
+
+	DPRINTF(sc, ATH_DEBUG_BEACON, "%s: nexttbtt=%u intval=%u\n",
+		__func__,nexttbtt, intval & HAL_BEACON_PERIOD);
 }
 
 static int