wifi: mac80211: add sta link addition/removal

Add the necessary infrastructure, including a new driver
method, to add/remove links to/from a station. To do this,
refactor the link alloc/free a bit, splitting that so we
can do it without linking them, to handle failures better.

Note that a station entry must be created representing an
MLD or a non-MLD STA, it cannot change between the two.
When representing an MLD, the 'deflink' is used for the
first link, which might be removed later, in which case
the memory isn't reused.

Signed-off-by: Johannes Berg <johannes.berg@intel.com>

9 files changed, 195 insertions, 33 deletions

diff --git a/net/mac80211/cfg.c b/net/mac80211/cfg.c
index e30659b1242b..abc3fa5a8d7e 100644
--- a/net/mac80211/cfg.c
+++ b/net/mac80211/cfg.c
@@ -1839,7 +1839,7 @@ static int ieee80211_add_station(struct wiphy *wiphy, struct net_device *dev,
 	    !sdata->u.mgd.associated)
 		return -EINVAL;
 
-	sta = sta_info_alloc(sdata, mac, GFP_KERNEL);
+	sta = sta_info_alloc(sdata, mac, -1, GFP_KERNEL);
 	if (!sta)
 		return -ENOMEM;
 
diff --git a/net/mac80211/driver-ops.h b/net/mac80211/driver-ops.h
index c8133c84d7d5..52be89f8f0bc 100644
--- a/net/mac80211/driver-ops.h
+++ b/net/mac80211/driver-ops.h
@@ -1554,4 +1554,25 @@ static inline int drv_change_vif_links(struct ieee80211_local *local,
 	return ret;
 }
 
+static inline int drv_change_sta_links(struct ieee80211_local *local,
+				       struct ieee80211_sub_if_data *sdata,
+				       struct ieee80211_sta *sta,
+				       u16 old_links, u16 new_links)
+{
+	int ret = -EOPNOTSUPP;
+
+	might_sleep();
+
+	if (!check_sdata_in_driver(sdata))
+		return -EIO;
+
+	trace_drv_change_sta_links(local, sdata, sta, old_links, new_links);
+	if (local->ops->change_sta_links)
+		ret = local->ops->change_sta_links(&local->hw, &sdata->vif, sta,
+						   old_links, new_links);
+	trace_drv_return_int(local, ret);
+
+	return ret;
+}
+
 #endif /* __MAC80211_DRIVER_OPS */
diff --git a/net/mac80211/ibss.c b/net/mac80211/ibss.c
index 09c11c067cbf..65a3808dc92a 100644
--- a/net/mac80211/ibss.c
+++ b/net/mac80211/ibss.c
@@ -629,7 +629,7 @@ ieee80211_ibss_add_sta(struct ieee80211_sub_if_data *sdata, const u8 *bssid,
 	scan_width = cfg80211_chandef_to_scan_width(&chanctx_conf->def);
 	rcu_read_unlock();
 
-	sta = sta_info_alloc(sdata, addr, GFP_KERNEL);
+	sta = sta_info_alloc(sdata, addr, -1, GFP_KERNEL);
 	if (!sta) {
 		rcu_read_lock();
 		return NULL;
@@ -1229,7 +1229,7 @@ void ieee80211_ibss_rx_no_sta(struct ieee80211_sub_if_data *sdata,
 	scan_width = cfg80211_chandef_to_scan_width(&chanctx_conf->def);
 	rcu_read_unlock();
 
-	sta = sta_info_alloc(sdata, addr, GFP_ATOMIC);
+	sta = sta_info_alloc(sdata, addr, -1, GFP_ATOMIC);
 	if (!sta)
 		return;
 
diff --git a/net/mac80211/mesh_plink.c b/net/mac80211/mesh_plink.c
index e24bd48bc915..fe54ac431202 100644
--- a/net/mac80211/mesh_plink.c
+++ b/net/mac80211/mesh_plink.c
@@ -510,7 +510,7 @@ __mesh_sta_info_alloc(struct ieee80211_sub_if_data *sdata, u8 *hw_addr)
 	if (aid < 0)
 		return NULL;
 
-	sta = sta_info_alloc(sdata, hw_addr, GFP_KERNEL);
+	sta = sta_info_alloc(sdata, hw_addr, -1, GFP_KERNEL);
 	if (!sta)
 		return NULL;
 
diff --git a/net/mac80211/mlme.c b/net/mac80211/mlme.c
index b6ee8da07663..c2c997086553 100644
--- a/net/mac80211/mlme.c
+++ b/net/mac80211/mlme.c
@@ -5579,7 +5579,7 @@ static int ieee80211_prep_connection(struct ieee80211_sub_if_data *sdata,
 	}
 
 	if (!have_sta) {
-		new_sta = sta_info_alloc(sdata, cbss->bssid, GFP_KERNEL);
+		new_sta = sta_info_alloc(sdata, cbss->bssid, -1, GFP_KERNEL);
 		if (!new_sta)
 			return -ENOMEM;
 	}
diff --git a/net/mac80211/ocb.c b/net/mac80211/ocb.c
index 468c741a9aeb..0fd29d9c496c 100644
--- a/net/mac80211/ocb.c
+++ b/net/mac80211/ocb.c
@@ -69,7 +69,7 @@ void ieee80211_ocb_rx_no_sta(struct ieee80211_sub_if_data *sdata,
 	scan_width = cfg80211_chandef_to_scan_width(&chanctx_conf->def);
 	rcu_read_unlock();
 
-	sta = sta_info_alloc(sdata, addr, GFP_ATOMIC);
+	sta = sta_info_alloc(sdata, addr, -1, GFP_ATOMIC);
 	if (!sta)
 		return;
 
diff --git a/net/mac80211/sta_info.c b/net/mac80211/sta_info.c
index f20ce7bbcb39..b1426a2459e8 100644
--- a/net/mac80211/sta_info.c
+++ b/net/mac80211/sta_info.c
@@ -64,6 +64,11 @@
  * freed before they are done using it.
  */
 
+struct sta_link_alloc {
+	struct link_sta_info info;
+	struct ieee80211_link_sta sta;
+};
+
 static const struct rhashtable_params sta_rht_params = {
 	.nelem_hint = 3, /* start small */
 	.automatic_shrinking = true,
@@ -245,17 +250,27 @@ struct sta_info *sta_info_get_by_idx(struct ieee80211_sub_if_data *sdata,
 	return NULL;
 }
 
-static void sta_info_free_links(struct sta_info *sta)
+static void sta_info_free_link(struct link_sta_info *link_sta)
 {
-	unsigned int link_id;
+	free_percpu(link_sta->pcpu_rx_stats);
+}
 
-	for (link_id = 0; link_id < ARRAY_SIZE(sta->link); link_id++) {
-		if (!sta->link[link_id])
-			continue;
-		free_percpu(sta->link[link_id]->pcpu_rx_stats);
+static void sta_remove_link(struct sta_info *sta, unsigned int link_id)
+{
+	struct sta_link_alloc *alloc = NULL;
 
-		if (sta->link[link_id] != &sta->deflink)
-			kfree(sta->link[link_id]);
+	if (WARN_ON(!sta->link[link_id]))
+		return;
+
+	if (sta->link[link_id] != &sta->deflink)
+		alloc = container_of(sta->link[link_id], typeof(*alloc), info);
+
+	sta->sta.valid_links &= ~BIT(link_id);
+	sta->link[link_id] = NULL;
+	sta->sta.link[link_id] = NULL;
+	if (alloc) {
+		sta_info_free_link(&alloc->info);
+		kfree(alloc);
 	}
 }
 
@@ -272,6 +287,15 @@ static void sta_info_free_links(struct sta_info *sta)
  */
 void sta_info_free(struct ieee80211_local *local, struct sta_info *sta)
 {
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(sta->link); i++) {
+		if (!(sta->sta.valid_links & BIT(i)))
+			continue;
+
+		sta_remove_link(sta, i);
+	}
+
 	/*
 	 * If we had used sta_info_pre_move_state() then we might not
 	 * have gone through the state transitions down again, so do
@@ -302,7 +326,7 @@ void sta_info_free(struct ieee80211_local *local, struct sta_info *sta)
 	kfree(sta->mesh);
 #endif
 
-	sta_info_free_links(sta);
+	sta_info_free_link(&sta->deflink);
 	kfree(sta);
 }
 
@@ -348,19 +372,13 @@ static int sta_prepare_rate_control(struct ieee80211_local *local,
 	return 0;
 }
 
-static int sta_info_init_link(struct sta_info *sta,
-			      unsigned int link_id,
-			      struct link_sta_info *link_info,
-			      struct ieee80211_link_sta *link_sta,
-			      gfp_t gfp)
+static int sta_info_alloc_link(struct ieee80211_local *local,
+			       struct link_sta_info *link_info,
+			       gfp_t gfp)
 {
-	struct ieee80211_local *local = sta->local;
 	struct ieee80211_hw *hw = &local->hw;
 	int i;
 
-	link_info->sta = sta;
-	link_info->link_id = link_id;
-
 	if (ieee80211_hw_check(hw, USES_RSS)) {
 		link_info->pcpu_rx_stats =
 			alloc_percpu_gfp(struct ieee80211_sta_rx_stats, gfp);
@@ -368,9 +386,6 @@ static int sta_info_init_link(struct sta_info *sta,
 			return -ENOMEM;
 	}
 
-	sta->link[link_id] = link_info;
-	sta->sta.link[link_id] = link_sta;
-
 	link_info->rx_stats.last_rx = jiffies;
 	u64_stats_init(&link_info->rx_stats.syncp);
 
@@ -382,8 +397,19 @@ static int sta_info_init_link(struct sta_info *sta,
 	return 0;
 }
 
+static void sta_info_add_link(struct sta_info *sta,
+			      unsigned int link_id,
+			      struct link_sta_info *link_info,
+			      struct ieee80211_link_sta *link_sta)
+{
+	link_info->sta = sta;
+	link_info->link_id = link_id;
+	sta->link[link_id] = link_info;
+	sta->sta.link[link_id] = link_sta;
+}
+
 struct sta_info *sta_info_alloc(struct ieee80211_sub_if_data *sdata,
-				const u8 *addr, gfp_t gfp)
+				const u8 *addr, int link_id, gfp_t gfp)
 {
 	struct ieee80211_local *local = sdata->local;
 	struct ieee80211_hw *hw = &local->hw;
@@ -397,9 +423,17 @@ struct sta_info *sta_info_alloc(struct ieee80211_sub_if_data *sdata,
 	sta->local = local;
 	sta->sdata = sdata;
 
-	if (sta_info_init_link(sta, 0, &sta->deflink, &sta->sta.deflink, gfp))
+	if (sta_info_alloc_link(local, &sta->deflink, gfp))
 		return NULL;
 
+	if (link_id >= 0) {
+		sta_info_add_link(sta, link_id, &sta->deflink,
+				  &sta->sta.deflink);
+		sta->sta.valid_links = BIT(link_id);
+	} else {
+		sta_info_add_link(sta, 0, &sta->deflink, &sta->sta.deflink);
+	}
+
 	spin_lock_init(&sta->lock);
 	spin_lock_init(&sta->ps_lock);
 	INIT_WORK(&sta->drv_deliver_wk, sta_deliver_ps_frames);
@@ -565,7 +599,7 @@ free_txq:
 	if (sta->sta.txq[0])
 		kfree(to_txq_info(sta->sta.txq[0]));
 free:
-	sta_info_free_links(sta);
+	sta_info_free_link(&sta->deflink);
 #ifdef CONFIG_MAC80211_MESH
 	kfree(sta->mesh);
 #endif
@@ -2613,3 +2647,77 @@ void ieee80211_sta_set_expected_throughput(struct ieee80211_sta *pubsta,
 
 	sta_update_codel_params(sta, thr);
 }
+
+int ieee80211_sta_allocate_link(struct sta_info *sta, unsigned int link_id)
+{
+	struct ieee80211_sub_if_data *sdata = sta->sdata;
+	struct sta_link_alloc *alloc;
+	int ret;
+
+	lockdep_assert_held(&sdata->local->sta_mtx);
+
+	/* must represent an MLD from the start */
+	if (WARN_ON(!sta->sta.valid_links))
+		return -EINVAL;
+
+	if (WARN_ON(sta->sta.valid_links & BIT(link_id) ||
+		    sta->link[link_id]))
+		return -EBUSY;
+
+	alloc = kzalloc(sizeof(*alloc), GFP_KERNEL);
+	if (!alloc)
+		return -ENOMEM;
+
+	ret = sta_info_alloc_link(sdata->local, &alloc->info, GFP_KERNEL);
+	if (ret) {
+		kfree(alloc);
+		return ret;
+	}
+
+	sta_info_add_link(sta, link_id, &alloc->info, &alloc->sta);
+
+	return 0;
+}
+
+int ieee80211_sta_activate_link(struct sta_info *sta, unsigned int link_id)
+{
+	struct ieee80211_sub_if_data *sdata = sta->sdata;
+	u16 old_links = sta->sta.valid_links;
+	u16 new_links = old_links | BIT(link_id);
+	int ret;
+
+	lockdep_assert_held(&sdata->local->sta_mtx);
+
+	if (WARN_ON(old_links == new_links || !sta->link[link_id]))
+		return -EINVAL;
+
+	sta->sta.valid_links = new_links;
+
+	if (!test_sta_flag(sta, WLAN_STA_INSERTED))
+		return 0;
+
+	ret = drv_change_sta_links(sdata->local, sdata, &sta->sta,
+				   old_links, new_links);
+	if (ret) {
+		sta->sta.valid_links = old_links;
+		sta_remove_link(sta, link_id);
+	}
+
+	return ret;
+}
+
+void ieee80211_sta_remove_link(struct sta_info *sta, unsigned int link_id)
+{
+	struct ieee80211_sub_if_data *sdata = sta->sdata;
+
+	lockdep_assert_held(&sdata->local->sta_mtx);
+
+	sta->sta.valid_links &= ~BIT(link_id);
+
+	if (test_sta_flag(sta, WLAN_STA_INSERTED))
+		drv_change_sta_links(sdata->local, sdata, &sta->sta,
+				     sta->sta.valid_links,
+				     sta->sta.valid_links & ~BIT(link_id));
+
+	sta_remove_link(sta, link_id);
+}
diff --git a/net/mac80211/sta_info.h b/net/mac80211/sta_info.h
index 2ff3d5fd0cbf..8ec65bb7d13e 100644
--- a/net/mac80211/sta_info.h
+++ b/net/mac80211/sta_info.h
@@ -623,7 +623,6 @@ struct link_sta_info {
  * @tdls_chandef: a TDLS peer can have a wider chandef that is compatible to
  *	the BSS one.
  * @frags: fragment cache
- * @multi_link_sta: Identifies if this sta is a MLD STA or regular STA
  * @deflink: This is the default link STA information, for non MLO STA all link
  *	specific STA information is accessed through @deflink or through
  *	link[0] which points to address of @deflink. For MLO Link STA
@@ -708,7 +707,6 @@ struct sta_info {
 
 	struct ieee80211_fragment_cache frags;
 
-	bool multi_link_sta;
 	struct link_sta_info deflink;
 	struct link_sta_info *link[IEEE80211_MLD_MAX_NUM_LINKS];
 
@@ -833,7 +831,7 @@ struct sta_info *sta_info_get_by_idx(struct ieee80211_sub_if_data *sdata,
  * until sta_info_insert().
  */
 struct sta_info *sta_info_alloc(struct ieee80211_sub_if_data *sdata,
-				const u8 *addr, gfp_t gfp);
+				const u8 *addr, int link_id, gfp_t gfp);
 
 void sta_info_free(struct ieee80211_local *local, struct sta_info *sta);
 
@@ -892,6 +890,10 @@ u32 sta_get_expected_throughput(struct sta_info *sta);
 void ieee80211_sta_expire(struct ieee80211_sub_if_data *sdata,
 			  unsigned long exp_time);
 
+int ieee80211_sta_allocate_link(struct sta_info *sta, unsigned int link_id);
+int ieee80211_sta_activate_link(struct sta_info *sta, unsigned int link_id);
+void ieee80211_sta_remove_link(struct sta_info *sta, unsigned int link_id);
+
 void ieee80211_sta_ps_deliver_wakeup(struct sta_info *sta);
 void ieee80211_sta_ps_deliver_poll_response(struct sta_info *sta);
 void ieee80211_sta_ps_deliver_uapsd(struct sta_info *sta);
diff --git a/net/mac80211/trace.h b/net/mac80211/trace.h
index 256ebab13cda..9804634e7d99 100644
--- a/net/mac80211/trace.h
+++ b/net/mac80211/trace.h
@@ -2482,6 +2482,37 @@ TRACE_EVENT(drv_change_vif_links,
 	)
 );
 
+TRACE_EVENT(drv_change_sta_links,
+	TP_PROTO(struct ieee80211_local *local,
+		 struct ieee80211_sub_if_data *sdata,
+		 struct ieee80211_sta *sta,
+		 u16 old_links, u16 new_links),
+
+	TP_ARGS(local, sdata, sta, old_links, new_links),
+
+	TP_STRUCT__entry(
+		LOCAL_ENTRY
+		VIF_ENTRY
+		STA_ENTRY
+		__field(u16, old_links)
+		__field(u16, new_links)
+	),
+
+	TP_fast_assign(
+		LOCAL_ASSIGN;
+		VIF_ASSIGN;
+		STA_ASSIGN;
+		__entry->old_links = old_links;
+		__entry->new_links = new_links;
+	),
+
+	TP_printk(
+		LOCAL_PR_FMT VIF_PR_FMT STA_PR_FMT " old_links:0x%04x, new_links:0x%04x\n",
+		LOCAL_PR_ARG, VIF_PR_ARG, STA_PR_ARG,
+		__entry->old_links, __entry->new_links
+	)
+);
+
 /*
  * Tracing for API calls that drivers call.
  */