mac80211_hwsim: fix beacon timing

A beacon period starts at TSF time 0. Spoof this by
rounding the starting beacon time to a multiple of the
beacon interval, and keep TBTT aligned on TSF adjustment.

Signed-off-by: Thomas Pedersen <thomas@cozybit.com>
Signed-off-by: Johannes Berg <johannes.berg@intel.com>

drivers/net/wireless/mac80211_hwsim.c

@@ -362,6 +362,7 @@ struct mac80211_hwsim_data {
 
 	/* difference between this hw's clock and the real clock, in usecs */
 	s64 tsf_offset;
+	s64 bcn_delta;
 };
 
 
@@ -428,9 +429,12 @@ static void mac80211_hwsim_set_tsf(struct ieee80211_hw *hw,
 {
 	struct mac80211_hwsim_data *data = hw->priv;
 	u64 now = mac80211_hwsim_get_tsf(hw, vif);
+	u32 bcn_int = data->beacon_int;
 	s64 delta = tsf - now;
 
 	data->tsf_offset += delta;
+	/* adjust after beaconing with new timestamp at old TBTT */
+	data->bcn_delta = do_div(delta, bcn_int);
 }
 
 static void mac80211_hwsim_monitor_rx(struct ieee80211_hw *hw,
@@ -1018,6 +1022,12 @@ mac80211_hwsim_beacon(struct hrtimer *timer)
 		hw, IEEE80211_IFACE_ITER_NORMAL,
 		mac80211_hwsim_beacon_tx, hw);
 
+	/* beacon at new TBTT + beacon interval */
+	if (data->bcn_delta) {
+		bcn_int -= data->bcn_delta;
+		data->bcn_delta = 0;
+	}
+
 	next_bcn = ktime_add(hrtimer_get_expires(timer),
 			     ns_to_ktime(bcn_int * 1000));
 	tasklet_hrtimer_start(&data->beacon_timer, next_bcn, HRTIMER_MODE_ABS);
@@ -1062,8 +1072,12 @@ static int mac80211_hwsim_config(struct ieee80211_hw *hw, u32 changed)
 	if (!data->started || !data->beacon_int)
 		tasklet_hrtimer_cancel(&data->beacon_timer);
 	else if (!hrtimer_is_queued(&data->beacon_timer.timer)) {
+		u64 tsf = mac80211_hwsim_get_tsf(hw, NULL);
+		u32 bcn_int = data->beacon_int;
+		u64 until_tbtt = bcn_int - do_div(tsf, bcn_int);
+
 		tasklet_hrtimer_start(&data->beacon_timer,
-				      ns_to_ktime(data->beacon_int * 1000),
+				      ns_to_ktime(until_tbtt * 1000),
 				      HRTIMER_MODE_REL);
 	}
 
@@ -1123,11 +1137,15 @@ static void mac80211_hwsim_bss_info_changed(struct ieee80211_hw *hw,
 		if (data->started &&
 		    !hrtimer_is_queued(&data->beacon_timer.timer) &&
 		    info->enable_beacon) {
+			u64 tsf, until_tbtt;
+			u32 bcn_int;
 			if (WARN_ON(!data->beacon_int))
 				data->beacon_int = 1000 * 1024;
+			tsf = mac80211_hwsim_get_tsf(hw, vif);
+			bcn_int = data->beacon_int;
+			until_tbtt = bcn_int - do_div(tsf, bcn_int);
 			tasklet_hrtimer_start(&data->beacon_timer,
-					      ns_to_ktime(data->beacon_int *
-							  1000),
+					      ns_to_ktime(until_tbtt * 1000),
 					      HRTIMER_MODE_REL);
 		} else if (!info->enable_beacon)
 			tasklet_hrtimer_cancel(&data->beacon_timer);