From: Benjamin Berg <benjamin.berg@open-mesh.com>
Date: Mon, 4 Jul 2016 14:37:25 +0200
Subject: [PATCH] ath9k: Fix beacon configuration for addition/removal of
 interfaces

This patch fixes some issues with interface reconfiguration. It could
for example happen that an AP interface in beacon slot 0 was removed
leaving an IBSS station in one of the other slots. When this happens
the driver never sends out the beacon as it only tries to send a beacon
from slot 0.

Appart from that the tracking of required changes to the beacon config is
relatively complicated and prone to errors.

The approach taken here is to solve reconfiguration issues is to
reconfigure the beacons when any interface changes. This means that
the complexity of deciding whether an interface change may modify the
beacon configuration is gone. It also means that the beacon config will
be reliably updated when an interface is removed.

The issue that a single non-AP interface might not be in beacon
slot 0 and wouldn't be send out is solved by moving it into the
first slot. The TSF value in hardware is adjusted accordingly so
that the timestamp of the beacons stay consistent.

Signed-off-by: Benjamin Berg <benjamin.berg@open-mesh.com>
---

--- a/drivers/net/wireless/ath/ath9k/ath9k.h
+++ b/drivers/net/wireless/ath/ath9k/ath9k.h
@@ -637,6 +637,8 @@ struct ath9k_vif_iter_data {
 	int nwds;      /* number of WDS vifs */
 	int nadhocs;   /* number of adhoc vifs */
 	int nocbs;     /* number of OCB vifs */
+	int nbcnvifs;  /* number of beaconing vifs */
+	struct ieee80211_vif *primary_beacon_vif;
 	struct ieee80211_vif *primary_sta;
 };
 
@@ -685,10 +687,11 @@ struct ath_beacon {
 };
 
 void ath9k_beacon_tasklet(unsigned long data);
-void ath9k_beacon_config(struct ath_softc *sc, struct ieee80211_vif *vif,
-			 u32 changed);
+void ath9k_beacon_config(struct ath_softc *sc, struct ieee80211_vif *main_vif,
+			 bool beacons);
 void ath9k_beacon_assign_slot(struct ath_softc *sc, struct ieee80211_vif *vif);
 void ath9k_beacon_remove_slot(struct ath_softc *sc, struct ieee80211_vif *vif);
+void ath9k_beacon_ensure_primary_slot(struct ath_softc *sc);
 void ath9k_set_beacon(struct ath_softc *sc);
 bool ath9k_csa_is_finished(struct ath_softc *sc, struct ieee80211_vif *vif);
 void ath9k_csa_update(struct ath_softc *sc);
--- a/drivers/net/wireless/ath/ath9k/beacon.c
+++ b/drivers/net/wireless/ath/ath9k/beacon.c
@@ -209,7 +209,6 @@ void ath9k_beacon_assign_slot(struct ath
 	}
 
 	sc->beacon.bslot[avp->av_bslot] = vif;
-	sc->nbcnvifs++;
 
 	ath_dbg(common, CONFIG, "Added interface at beacon slot: %d\n",
 		avp->av_bslot);
@@ -220,15 +219,12 @@ void ath9k_beacon_remove_slot(struct ath
 	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
 	struct ath_vif *avp = (void *)vif->drv_priv;
 	struct ath_buf *bf = avp->av_bcbuf;
-	struct ath_beacon_config *cur_conf = &sc->cur_chan->beacon;
 
 	ath_dbg(common, CONFIG, "Removing interface at beacon slot: %d\n",
 		avp->av_bslot);
 
 	tasklet_disable(&sc->bcon_tasklet);
 
-	cur_conf->enable_beacon &= ~BIT(avp->av_bslot);
-
 	if (bf && bf->bf_mpdu) {
 		struct sk_buff *skb = bf->bf_mpdu;
 		dma_unmap_single(sc->dev, bf->bf_buf_addr,
@@ -240,12 +236,73 @@ void ath9k_beacon_remove_slot(struct ath
 
 	avp->av_bcbuf = NULL;
 	sc->beacon.bslot[avp->av_bslot] = NULL;
-	sc->nbcnvifs--;
 	list_add_tail(&bf->list, &sc->beacon.bbuf);
 
 	tasklet_enable(&sc->bcon_tasklet);
 }
 
+void ath9k_beacon_ensure_primary_slot(struct ath_softc *sc)
+{
+	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
+	struct ieee80211_vif *vif;
+	struct ath_vif *avp;
+	s64 tsfadjust;
+	u32 offset;
+	int first_slot = ATH_BCBUF;
+	int slot;
+
+	tasklet_disable(&sc->bcon_tasklet);
+
+	/* Find first taken slot. */
+	for (slot = 0; slot < ATH_BCBUF; slot++) {
+		if (sc->beacon.bslot[slot]) {
+			first_slot = slot;
+			break;
+		}
+	}
+	if (first_slot == 0)
+		goto out;
+
+	/* Re-enumarate all slots, moving them forward. */
+	for (slot = 0; slot < ATH_BCBUF; slot++) {
+		if (slot + first_slot < ATH_BCBUF) {
+			vif = sc->beacon.bslot[slot + first_slot];
+			sc->beacon.bslot[slot] = vif;
+
+			if (vif) {
+				avp = (void *)vif->drv_priv;
+				avp->av_bslot = slot;
+			}
+		} else {
+			sc->beacon.bslot[slot] = NULL;
+		}
+	}
+
+	vif = sc->beacon.bslot[0];
+	if (WARN_ON(!vif))
+		goto out;
+
+	/* Get the tsf_adjust value for the new first slot. */
+	avp = (void *)vif->drv_priv;
+	tsfadjust = le64_to_cpu(avp->tsf_adjust);
+
+	ath_dbg(common, CONFIG,
+		"Adjusting global TSF after beacon slot reassignment: %lld\n",
+		(signed long long)tsfadjust);
+
+	/* Modify TSF as required and update the HW. */
+	avp->chanctx->tsf_val += tsfadjust;
+	if (sc->cur_chan == avp->chanctx) {
+		offset = ath9k_hw_get_tsf_offset(&avp->chanctx->tsf_ts, NULL);
+		ath9k_hw_settsf64(sc->sc_ah, avp->chanctx->tsf_val + offset);
+	}
+
+	/* The slots tsf_adjust will be updated by ath9k_beacon_config later. */
+
+out:
+	tasklet_enable(&sc->bcon_tasklet);
+}
+
 static int ath9k_beacon_choose_slot(struct ath_softc *sc)
 {
 	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
@@ -274,26 +331,33 @@ static int ath9k_beacon_choose_slot(stru
 	return slot;
 }
 
-static void ath9k_set_tsfadjust(struct ath_softc *sc, struct ieee80211_vif *vif)
+static void ath9k_set_tsfadjust(struct ath_softc *sc,
+				struct ath_beacon_config *cur_conf)
 {
 	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
-	struct ath_vif *avp = (void *)vif->drv_priv;
-	struct ath_beacon_config *cur_conf = &avp->chanctx->beacon;
 	s64 tsfadjust;
+	int slot;
 
-	if (avp->av_bslot == 0)
-		return;
+	for (slot = 0; slot < ATH_BCBUF; slot++) {
+		struct ath_vif *avp;
 
-	/* tsf_adjust is added to the TSF value. We send out the beacon late,
-	 * so need to adjust the TSF starting point to be later in time (i.e.
-	 * the theoretical first beacon has a TSF of 0 after correction).
-	 */
-	tsfadjust = cur_conf->beacon_interval * avp->av_bslot;
-	tsfadjust = -TU_TO_USEC(tsfadjust) / ATH_BCBUF;
-	avp->tsf_adjust = cpu_to_le64(tsfadjust);
+		if (!sc->beacon.bslot[slot])
+			continue;
 
-	ath_dbg(common, CONFIG, "tsfadjust is: %lld for bslot: %d\n",
-		(signed long long)tsfadjust, avp->av_bslot);
+		avp = (void *)sc->beacon.bslot[slot]->drv_priv;
+
+		/* tsf_adjust is added to the TSF value. We send out the
+		 * beacon late, so need to adjust the TSF starting point to be
+		 * later in time (i.e. the theoretical first beacon has a TSF
+		 * of 0 after correction).
+		 */
+		tsfadjust = cur_conf->beacon_interval * avp->av_bslot;
+		tsfadjust = -TU_TO_USEC(tsfadjust) / ATH_BCBUF;
+		avp->tsf_adjust = cpu_to_le64(tsfadjust);
+
+		ath_dbg(common, CONFIG, "tsfadjust is: %lld for bslot: %d\n",
+			(signed long long)tsfadjust, avp->av_bslot);
+	}
 }
 
 bool ath9k_csa_is_finished(struct ath_softc *sc, struct ieee80211_vif *vif)
@@ -447,20 +511,28 @@ void ath9k_beacon_tasklet(unsigned long
  * Both nexttbtt and intval have to be in usecs.
  */
 static void ath9k_beacon_init(struct ath_softc *sc, u32 nexttbtt,
-			      u32 intval, bool reset_tsf)
+			      u32 intval)
 {
 	struct ath_hw *ah = sc->sc_ah;
 
 	ath9k_hw_disable_interrupts(ah);
-	if (reset_tsf)
-		ath9k_hw_reset_tsf(ah);
 	ath9k_beaconq_config(sc);
 	ath9k_hw_beaconinit(ah, nexttbtt, intval);
+	ah->imask |= ATH9K_INT_SWBA;
 	sc->beacon.bmisscnt = 0;
 	ath9k_hw_set_interrupts(ah);
 	ath9k_hw_enable_interrupts(ah);
 }
 
+static void ath9k_beacon_stop(struct ath_softc *sc)
+{
+	ath9k_hw_disable_interrupts(sc->sc_ah);
+	sc->sc_ah->imask &= ~(ATH9K_INT_SWBA | ATH9K_INT_BMISS);
+	sc->beacon.bmisscnt = 0;
+	ath9k_hw_set_interrupts(sc->sc_ah);
+	ath9k_hw_enable_interrupts(sc->sc_ah);
+}
+
 /*
  * For multi-bss ap support beacons are either staggered evenly over N slots or
  * burst together.  For the former arrange for the SWBA to be delivered for each
@@ -472,7 +544,7 @@ static void ath9k_beacon_config_ap(struc
 	struct ath_hw *ah = sc->sc_ah;
 
 	ath9k_cmn_beacon_config_ap(ah, conf, ATH_BCBUF);
-	ath9k_beacon_init(sc, conf->nexttbtt, conf->intval, false);
+	ath9k_beacon_init(sc, conf->nexttbtt, conf->intval);
 }
 
 static void ath9k_beacon_config_sta(struct ath_hw *ah,
@@ -501,7 +573,7 @@ static void ath9k_beacon_config_adhoc(st
 
 	ath9k_cmn_beacon_config_adhoc(ah, conf);
 
-	ath9k_beacon_init(sc, conf->nexttbtt, conf->intval, conf->ibss_creator);
+	ath9k_beacon_init(sc, conf->nexttbtt, conf->intval);
 
 	/*
 	 * Set the global 'beacon has been configured' flag for the
@@ -511,44 +583,6 @@ static void ath9k_beacon_config_adhoc(st
 		set_bit(ATH_OP_BEACONS, &common->op_flags);
 }
 
-static bool ath9k_allow_beacon_config(struct ath_softc *sc,
-				      struct ieee80211_vif *vif)
-{
-	struct ath_common *common = ath9k_hw_common(sc->sc_ah);
-	struct ath_vif *avp = (void *)vif->drv_priv;
-
-	if (ath9k_is_chanctx_enabled()) {
-		/*
-		 * If the VIF is not present in the current channel context,
-		 * then we can't do the usual opmode checks. Allow the
-		 * beacon config for the VIF to be updated in this case and
-		 * return immediately.
-		 */
-		if (sc->cur_chan != avp->chanctx)
-			return true;
-	}
-
-	if (sc->sc_ah->opmode == NL80211_IFTYPE_AP) {
-		if (vif->type != NL80211_IFTYPE_AP) {
-			ath_dbg(common, CONFIG,
-				"An AP interface is already present !\n");
-			return false;
-		}
-	}
-
-	if (sc->sc_ah->opmode == NL80211_IFTYPE_STATION) {
-		if ((vif->type == NL80211_IFTYPE_STATION) &&
-		    test_bit(ATH_OP_BEACONS, &common->op_flags) &&
-		    vif != sc->cur_chan->primary_sta) {
-			ath_dbg(common, CONFIG,
-				"Beacon already configured for a station interface\n");
-			return false;
-		}
-	}
-
-	return true;
-}
-
 static void ath9k_cache_beacon_config(struct ath_softc *sc,
 				      struct ath_chanctx *ctx,
 				      struct ieee80211_bss_conf *bss_conf)
@@ -584,87 +618,79 @@ static void ath9k_cache_beacon_config(st
 	if (cur_conf->dtim_period == 0)
 		cur_conf->dtim_period = 1;
 
+	ath9k_set_tsfadjust(sc, cur_conf);
 }
 
-void ath9k_beacon_config(struct ath_softc *sc, struct ieee80211_vif *vif,
-			 u32 changed)
+void ath9k_beacon_config(struct ath_softc *sc, struct ieee80211_vif *main_vif,
+			 bool beacons)
 {
-	struct ieee80211_bss_conf *bss_conf = &vif->bss_conf;
-        struct ath_hw *ah = sc->sc_ah;
-        struct ath_common *common = ath9k_hw_common(ah);
-	struct ath_vif *avp = (void *)vif->drv_priv;
-	struct ath_chanctx *ctx = avp->chanctx;
+	struct ath_hw *ah = sc->sc_ah;
+	struct ath_common *common = ath9k_hw_common(ah);
+	struct ath_vif *avp;
+	struct ath_chanctx *ctx;
 	struct ath_beacon_config *cur_conf;
 	unsigned long flags;
+	bool enabled;
 	bool skip_beacon = false;
 
-	if (!ctx)
+	if (!beacons) {
+		clear_bit(ATH_OP_BEACONS, &common->op_flags);
+		ath9k_beacon_stop(sc);
 		return;
+	}
 
-	cur_conf = &avp->chanctx->beacon;
-	if (vif->type == NL80211_IFTYPE_AP)
-		ath9k_set_tsfadjust(sc, vif);
-
-	if (!ath9k_allow_beacon_config(sc, vif))
+	if (WARN_ON(!main_vif))
 		return;
 
-	if (vif->type == NL80211_IFTYPE_STATION) {
-		ath9k_cache_beacon_config(sc, ctx, bss_conf);
-		if (ctx != sc->cur_chan)
-			return;
+	avp = (void *)main_vif->drv_priv;
+	ctx = avp->chanctx;
+	cur_conf = &ctx->beacon;
+	enabled = cur_conf->enable_beacon;
+	cur_conf->enable_beacon = beacons;
+
+	if (sc->sc_ah->opmode == NL80211_IFTYPE_STATION) {
+		ath9k_cache_beacon_config(sc, ctx, &main_vif->bss_conf);
 
 		ath9k_set_beacon(sc);
 		set_bit(ATH_OP_BEACONS, &common->op_flags);
 		return;
 	}
 
-	/*
-	 * Take care of multiple interfaces when
-	 * enabling/disabling SWBA.
-	 */
-	if (changed & BSS_CHANGED_BEACON_ENABLED) {
-		bool enabled = cur_conf->enable_beacon;
-
-		if (!bss_conf->enable_beacon) {
-			cur_conf->enable_beacon &= ~BIT(avp->av_bslot);
-		} else {
-			cur_conf->enable_beacon |= BIT(avp->av_bslot);
-			if (!enabled)
-				ath9k_cache_beacon_config(sc, ctx, bss_conf);
-		}
-	}
-
-	if (ctx != sc->cur_chan)
-		return;
+	/* Update the beacon configuration. */
+	ath9k_cache_beacon_config(sc, ctx, &main_vif->bss_conf);
 
 	/*
 	 * Configure the HW beacon registers only when we have a valid
 	 * beacon interval.
 	 */
 	if (cur_conf->beacon_interval) {
-		/*
-		 * If we are joining an existing IBSS network, start beaconing
-		 * only after a TSF-sync has taken place. Ensure that this
-		 * happens by setting the appropriate flags.
+		/* Special case to sync the TSF when joining an existing IBSS.
+		 * This is only done if no AP interface is active.
+		 * Note that mac80211 always resets the TSF when creating a new
+		 * IBSS interface.
 		 */
-		if ((changed & BSS_CHANGED_IBSS) && !bss_conf->ibss_creator &&
-		    bss_conf->enable_beacon) {
+		if (sc->sc_ah->opmode == NL80211_IFTYPE_ADHOC &&
+		    !enabled && beacons && !main_vif->bss_conf.ibss_creator) {
 			spin_lock_irqsave(&sc->sc_pm_lock, flags);
 			sc->ps_flags |= PS_BEACON_SYNC | PS_WAIT_FOR_BEACON;
 			spin_unlock_irqrestore(&sc->sc_pm_lock, flags);
 			skip_beacon = true;
-		} else {
-			ath9k_set_beacon(sc);
 		}
 
 		/*
 		 * Do not set the ATH_OP_BEACONS flag for IBSS joiner mode
 		 * here, it is done in ath9k_beacon_config_adhoc().
 		 */
-		if (cur_conf->enable_beacon && !skip_beacon)
+		if (beacons && !skip_beacon) {
 			set_bit(ATH_OP_BEACONS, &common->op_flags);
-		else
+			ath9k_set_beacon(sc);
+		} else {
 			clear_bit(ATH_OP_BEACONS, &common->op_flags);
+			ath9k_beacon_stop(sc);
+		}
+	} else {
+		clear_bit(ATH_OP_BEACONS, &common->op_flags);
+		ath9k_beacon_stop(sc);
 	}
 }
 
--- a/drivers/net/wireless/ath/ath9k/common.h
+++ b/drivers/net/wireless/ath/ath9k/common.h
@@ -50,6 +50,7 @@
 #define IEEE80211_MS_TO_TU(x)   (((x) * 1000) / 1024)
 
 struct ath_beacon_config {
+	struct ieee80211_vif *main_vif;
 	int beacon_interval;
 	u16 dtim_period;
 	u16 bmiss_timeout;
--- a/drivers/net/wireless/ath/ath9k/main.c
+++ b/drivers/net/wireless/ath/ath9k/main.c
@@ -910,6 +910,22 @@ static bool ath9k_uses_beacons(int type)
 	}
 }
 
+static void ath9k_vif_iter_set_beacon(struct ath9k_vif_iter_data *iter_data,
+				      struct ieee80211_vif *vif)
+{
+	/* Use the first (configured) interface, but prefering AP interfaces. */
+	if (!iter_data->primary_beacon_vif) {
+		iter_data->primary_beacon_vif = vif;
+	} else {
+		if (iter_data->primary_beacon_vif->type != NL80211_IFTYPE_AP &&
+		    vif->type == NL80211_IFTYPE_AP)
+		iter_data->primary_beacon_vif = vif;
+	}
+
+	iter_data->beacons = true;
+	iter_data->nbcnvifs += 1;
+}
+
 static void ath9k_vif_iter(struct ath9k_vif_iter_data *iter_data,
 			   u8 *mac, struct ieee80211_vif *vif)
 {
@@ -931,6 +947,8 @@ static void ath9k_vif_iter(struct ath9k_
 	switch (vif->type) {
 	case NL80211_IFTYPE_AP:
 		iter_data->naps++;
+		if (vif->bss_conf.enable_beacon)
+			ath9k_vif_iter_set_beacon(iter_data, vif);
 		break;
 	case NL80211_IFTYPE_STATION:
 		iter_data->nstations++;
@@ -943,12 +961,12 @@ static void ath9k_vif_iter(struct ath9k_
 	case NL80211_IFTYPE_ADHOC:
 		iter_data->nadhocs++;
 		if (vif->bss_conf.enable_beacon)
-			iter_data->beacons = true;
+			ath9k_vif_iter_set_beacon(iter_data, vif);
 		break;
 	case NL80211_IFTYPE_MESH_POINT:
 		iter_data->nmeshes++;
 		if (vif->bss_conf.enable_beacon)
-			iter_data->beacons = true;
+			ath9k_vif_iter_set_beacon(iter_data, vif);
 		break;
 	case NL80211_IFTYPE_WDS:
 		iter_data->nwds++;
@@ -1081,7 +1099,6 @@ void ath9k_calculate_summary_state(struc
 	struct ath_hw *ah = sc->sc_ah;
 	struct ath_common *common = ath9k_hw_common(ah);
 	struct ath9k_vif_iter_data iter_data;
-	struct ath_beacon_config *cur_conf;
 
 	ath_chanctx_check_active(sc, ctx);
 
@@ -1103,13 +1120,12 @@ void ath9k_calculate_summary_state(struc
 	ath_hw_setbssidmask(common);
 
 	if (iter_data.naps > 0) {
-		cur_conf = &ctx->beacon;
 		ath9k_hw_set_tsfadjust(ah, true);
 		ah->opmode = NL80211_IFTYPE_AP;
-		if (cur_conf->enable_beacon)
-			iter_data.beacons = true;
 	} else {
 		ath9k_hw_set_tsfadjust(ah, false);
+		if (iter_data.beacons)
+			ath9k_beacon_ensure_primary_slot(sc);
 
 		if (iter_data.nmeshes)
 			ah->opmode = NL80211_IFTYPE_MESH_POINT;
@@ -1134,7 +1150,6 @@ void ath9k_calculate_summary_state(struc
 			ctx->switch_after_beacon = true;
 	}
 
-	ah->imask &= ~ATH9K_INT_SWBA;
 	if (ah->opmode == NL80211_IFTYPE_STATION) {
 		bool changed = (iter_data.primary_sta != ctx->primary_sta);
 
@@ -1151,16 +1166,12 @@ void ath9k_calculate_summary_state(struc
 			if (ath9k_hw_mci_is_enabled(sc->sc_ah))
 				ath9k_mci_update_wlan_channels(sc, true);
 		}
-	} else if (iter_data.beacons) {
-		ah->imask |= ATH9K_INT_SWBA;
 	}
+	sc->nbcnvifs = iter_data.nbcnvifs;
+	ath9k_beacon_config(sc, iter_data.primary_beacon_vif,
+			    iter_data.beacons);
 	ath9k_hw_set_interrupts(ah);
 
-	if (iter_data.beacons)
-		set_bit(ATH_OP_BEACONS, &common->op_flags);
-	else
-		clear_bit(ATH_OP_BEACONS, &common->op_flags);
-
 	if (ah->slottime != iter_data.slottime) {
 		ah->slottime = iter_data.slottime;
 		ath9k_hw_init_global_settings(ah);
@@ -1777,9 +1788,7 @@ static void ath9k_bss_info_changed(struc
 	if ((changed & BSS_CHANGED_BEACON_ENABLED) ||
 	    (changed & BSS_CHANGED_BEACON_INT) ||
 	    (changed & BSS_CHANGED_BEACON_INFO)) {
-		ath9k_beacon_config(sc, vif, changed);
-		if (changed & BSS_CHANGED_BEACON_ENABLED)
-			ath9k_calculate_summary_state(sc, avp->chanctx);
+		ath9k_calculate_summary_state(sc, avp->chanctx);
 	}
 
 	if ((avp->chanctx == sc->cur_chan) &&
