[v13,2/3] mac80211: MBSSID and EMA beacon handling in AP mode

Commit Message

Aloka Dixit Oct. 6, 2021, 4:09 a.m. UTC

From: John Crispin <[e-mail]>

Add new fields in struct beacon_data to store all MBSSID elements.

For a non-EMA AP, generate a single beacon template which includes all
MBSSID elements.
For an EMA AP, generate multiple beacon templates, each including
a single MBSSID element. EMA profile periodicity equals the count of
elements.

Move CSA offset to reflect the MBSSID element length.

Add new APIs for the drivers to retrieve MBSSID and EMA beacons:
- ieee80211_beacon_get_template_ema_index() - Generate the EMA beacon
  which includes the multiple BSSID element at the given index.
  Drivers can use this function in a loop as well NULL is returned.
- ieee80211_beacon_get_template_ema_list() - Generate all EMA templates.
  Drivers must call ieee80211_beacon_free_ema_list() to free the memory.
No change in the prototype for the existing API
ieee80211_beacon_get_template() which should be used for non-EMA cases,
both with and without multiple BSSID enabled.

Signed-off-by: John Crispin <[e-mail]>
Co-developed-by: Aloka Dixit <[e-mail]>
Signed-off-by: Aloka Dixit <[e-mail]>
---
v13:
- Reverted lock related changes around __ieee80211_beacon_get().
- Replaced the infinite loop to generate EMA beacons by a finite
  loop and also reordered code so that rcu_dereference(ap->beacon)
  happens only once.
- Removed ema_index from struct beacon_data and the function
  ieee80211_beacon_get_template_ema_next(), replaced by
  ieee80211_beacon_get_template_ema_index() as given in the description.

 include/net/mac80211.h     |  66 ++++++++++++++
 net/mac80211/cfg.c         | 163 +++++++++++++++++++++++++++-------
 net/mac80211/ieee80211_i.h |   1 +
 net/mac80211/tx.c          | 175 ++++++++++++++++++++++++++++++++++---
 4 files changed, 363 insertions(+), 42 deletions(-)

Comments

Johannes Berg Nov. 26, 2021, 11:23 a.m. UTC | #1

On Tue, 2021-10-05 at 21:09 -0700, Aloka Dixit wrote:

>  
> +static struct sk_buff *
> +ieee80211_beacon_get_ap_mbssid(struct ieee80211_hw *hw,
> +			       struct ieee80211_vif *vif,
> +			       struct ieee80211_mutable_offsets *offs,
> +			       bool is_template,
> +			       struct beacon_data *beacon,
> +			       struct ieee80211_chanctx_conf *chanctx_conf,
> +			       int ema_index,
> +			       struct list_head *ema_list)

This function is called from ieee80211_beacon_get_ap(). That's called
from __ieee80211_beacon_get(), under RCU read lock.

> +	for (i = 0; i < beacon->mbssid_ies->cnt; i++) {
> +		struct ieee80211_ema_bcns *bcn;
> +
> +		bcn = kzalloc(sizeof(*bcn), GFP_KERNEL);

Therefore, you really cannot GFP_KERNEL allocate anything. But I really
only saw this because I went back to my comments on v12 where this was
still more obvious.


Given that we're already in v13 of this patch, let's take a step back.
Much of what I've pointed out in the reviews really is automatically
testable.

Can you please add support for this to hwsim and hostapd and add a few
tests to the hostap hwsim tests? Given the complexity of this, being
able to run the tests for it myself would give me a lot more confidence
in it.

johannes

Aloka Dixit Jan. 14, 2022, 7:23 p.m. UTC | #2

On 2021-11-26 03:23, Johannes Berg wrote:
> On Tue, 2021-10-05 at 21:09 -0700, Aloka Dixit wrote:
> 
>> 
>> +static struct sk_buff *
>> +ieee80211_beacon_get_ap_mbssid(struct ieee80211_hw *hw,
>> +			       struct ieee80211_vif *vif,
>> +			       struct ieee80211_mutable_offsets *offs,
>> +			       bool is_template,
>> +			       struct beacon_data *beacon,
>> +			       struct ieee80211_chanctx_conf *chanctx_conf,
>> +			       int ema_index,
>> +			       struct list_head *ema_list)
> 
> This function is called from ieee80211_beacon_get_ap(). That's called
> from __ieee80211_beacon_get(), under RCU read lock.
> 
>> +	for (i = 0; i < beacon->mbssid_ies->cnt; i++) {
>> +		struct ieee80211_ema_bcns *bcn;
>> +
>> +		bcn = kzalloc(sizeof(*bcn), GFP_KERNEL);
> 
> Therefore, you really cannot GFP_KERNEL allocate anything. But I really
> only saw this because I went back to my comments on v12 where this was
> still more obvious.
> 

Okay, I understand now that it is illegal because GFP_KERNEL is 
blocking.

I thought of following:
lock rcu -> get mbssid count first -> unlock rcu -> allocate memory.
But in that case, will have again: lock -> dereference to get beacon 
snapshot.
Beacon can change in between so new count might be wrong. In general 
sounds complicated and wrong.

I read that GFP_ATOMIC should be used sparingly, mainly for interrupt 
handlers etc.
Do you think this code path warrants its use?
Or should I look for some other function split?

Will add hwsim test cases before the next version but I genuinely did 
not see any issue during testing with current code.
So can you tell me which debug flags should be enabled to make such 
errors become obvious to someone like me who is new to these details in 
kernel programming?

Thanks!!

Johannes Berg Jan. 14, 2022, 8:12 p.m. UTC | #3

Hi!

> 
> > This function is called from ieee80211_beacon_get_ap(). That's called
> > from __ieee80211_beacon_get(), under RCU read lock.
> > 
> > > +	for (i = 0; i < beacon->mbssid_ies->cnt; i++) {
> > > +		struct ieee80211_ema_bcns *bcn;
> > > +
> > > +		bcn = kzalloc(sizeof(*bcn), GFP_KERNEL);
> > 
> > Therefore, you really cannot GFP_KERNEL allocate anything. But I really
> > only saw this because I went back to my comments on v12 where this was
> > still more obvious.
> > 
> 
> Okay, I understand now that it is illegal because GFP_KERNEL is 
> blocking.

Right.

> I thought of following:
> lock rcu -> get mbssid count first -> unlock rcu -> allocate memory.
> But in that case, will have again: lock -> dereference to get beacon 
> snapshot.
> Beacon can change in between so new count might be wrong. In general 
> sounds complicated and wrong.

Indeed. You could make it work (and count changing is highly unlikely!)
by going back and checking if the count was correct in the critical
section, and then going back if necessary (i.e. if it was wrong). But if
you do this, you should do something like this pseudo-code:

rcu_read_lock();
repeat:
calculated_size = calculate_size();
rcu_read_unlock();

alloc = kzalloc(calculated_size, GFP_KERNEL);
// omitting error handling

rcu_read_lock();
calculated_size = calculate_size();
if (ksize(alloc) < calculated_size)
	goto repeat;
...


i.e. note the ksize(), since allocations are rounded up. Even if the
count increased, you might not need a new allocation.

Also maybe anyway it'd make sense to allocate all of them together as an
array, rather than individual pointers for each beacon?


> I read that GFP_ATOMIC should be used sparingly, mainly for interrupt 
> handlers etc.

I guess once every beacon is still fairly sparingly though :)


> Do you think this code path warrants its use?
> Or should I look for some other function split?
> 
> Will add hwsim test cases before the next version but I genuinely did 
> not see any issue during testing with current code.

Sounds great, thanks!

> So can you tell me which debug flags should be enabled to make such 
> errors become obvious to someone like me who is new to these details in 
> kernel programming?

Hmm. I guess you want at least CONFIG_DEBUG_ATOMIC_SLEEP for this case.
But probably best to (also) turn on lockdep (CONFIG_PROVE_LOCKING=y),
including all the RCU checks (CONFIG_PROVE_RCU=y).

With that, it really _should_ be obvious here once that code path
executes at all, regardless of whether the kzalloc(GFP_KERNEL) actually
sleeps or not.

johannes

Aloka Dixit Jan. 14, 2022, 8:34 p.m. UTC | #4

On 2022-01-14 12:12, Johannes Berg wrote:
> Hi!
> 
>> 
>> > This function is called from ieee80211_beacon_get_ap(). That's called
>> > from __ieee80211_beacon_get(), under RCU read lock.
>> >
>> > > +	for (i = 0; i < beacon->mbssid_ies->cnt; i++) {
>> > > +		struct ieee80211_ema_bcns *bcn;
>> > > +
>> > > +		bcn = kzalloc(sizeof(*bcn), GFP_KERNEL);
>> >
>> > Therefore, you really cannot GFP_KERNEL allocate anything. But I really
>> > only saw this because I went back to my comments on v12 where this was
>> > still more obvious.
>> >
>> 
>> Okay, I understand now that it is illegal because GFP_KERNEL is
>> blocking.
> 
> Right.
> 
>> I thought of following:
>> lock rcu -> get mbssid count first -> unlock rcu -> allocate memory.
>> But in that case, will have again: lock -> dereference to get beacon
>> snapshot.
>> Beacon can change in between so new count might be wrong. In general
>> sounds complicated and wrong.
> 
> Indeed. You could make it work (and count changing is highly unlikely!)
> by going back and checking if the count was correct in the critical
> section, and then going back if necessary (i.e. if it was wrong). But 
> if
> you do this, you should do something like this pseudo-code:
> 
> rcu_read_lock();
> repeat:
> calculated_size = calculate_size();
> rcu_read_unlock();
> 
> alloc = kzalloc(calculated_size, GFP_KERNEL);
> // omitting error handling
> 
> rcu_read_lock();
> calculated_size = calculate_size();
> if (ksize(alloc) < calculated_size)
> 	goto repeat;
> ...
> 
> 
> i.e. note the ksize(), since allocations are rounded up. Even if the
> count increased, you might not need a new allocation.
> 
> Also maybe anyway it'd make sense to allocate all of them together as 
> an
> array, rather than individual pointers for each beacon?
> 
> 
>> I read that GFP_ATOMIC should be used sparingly, mainly for interrupt
>> handlers etc.
> 
> I guess once every beacon is still fairly sparingly though :)
> 
> 

Thank you so much for the quick response, will enable the debug flags 
henceforth.

With that, what would be a better way:
(1) Making it work with pseudo code, still using GFP_KERNEL or
(2) Changing to GFP_ATOMIC but otherwise keep the code fairly similar to 
v13 (preferably allocating an array instead of separate pointers as you 
suggested)?

Johannes Berg Jan. 14, 2022, 8:50 p.m. UTC | #5

On Fri, 2022-01-14 at 12:34 -0800, Aloka Dixit wrote:
> 
> With that, what would be a better way:
> (1) Making it work with pseudo code, still using GFP_KERNEL or
> (2) Changing to GFP_ATOMIC but otherwise keep the code fairly similar to 
> v13 (preferably allocating an array instead of separate pointers as you 
> suggested)?
> 

It's a good question. I'm not really into the code right now, but I'd
say GFP_ATOMIC should be OK.

I'm not even sure the functions you're modifying are always guaranteed
to be called in a context where you can sleep? E.g. I believe the driver
can call ieee80211_beacon_get_tim() in atomic context itself, e.g. even
from timer/interrupt to get the next beacon?

johannes

Patch

diff --git a/include/net/mac80211.h b/include/net/mac80211.h
index 13cad5e9e6c0..dab66ada0ebd 100644
--- a/include/net/mac80211.h
+++ b/include/net/mac80211.h
@@ -4919,12 +4919,14 @@  void ieee80211_report_low_ack(struct ieee80211_sta *sta, u32 num_packets);
  * @cntdwn_counter_offs: array of IEEE80211_MAX_CNTDWN_COUNTERS_NUM offsets
  *	to countdown counters.  This array can contain zero values which
  *	should be ignored.
+ * @mbssid_off: position of the multiple bssid element
  */
 struct ieee80211_mutable_offsets {
 	u16 tim_offset;
 	u16 tim_length;
 
 	u16 cntdwn_counter_offs[IEEE80211_MAX_CNTDWN_COUNTERS_NUM];
+	u16 mbssid_off;
 };
 
 /**
@@ -4951,6 +4953,70 @@  ieee80211_beacon_get_template(struct ieee80211_hw *hw,
 			      struct ieee80211_vif *vif,
 			      struct ieee80211_mutable_offsets *offs);
 
+/**
+ * ieee80211_beacon_get_template_ema_index - EMA beacon template generation
+ *	function for drivers using the sw offload path.
+ * @hw: pointer obtained from ieee80211_alloc_hw().
+ * @vif: &struct ieee80211_vif pointer from the add_interface callback.
+ * @offs: &struct ieee80211_mutable_offsets pointer to struct that will
+ *	receive the offsets that may be updated by the driver.
+ * @ema_index: index of the beacon in the EMA set, must be more than or equal
+ *	to 0.
+ *
+ * This function follows the same rules as ieee80211_beacon_get_template()
+ * but returns a beacon template which includes multiple BSSID element at the
+ * requested index.
+ *
+ * Return: The beacon template. %NULL on error or if the given index is more
+ *	than or equal to the number of EMA beacons.
+ */
+struct sk_buff *
+ieee80211_beacon_get_template_ema_index(struct ieee80211_hw *hw,
+					struct ieee80211_vif *vif,
+					struct ieee80211_mutable_offsets *offs,
+					u8 ema_index);
+
+/**
+ * struct ieee80211_ema_bcns - list entry of an EMA beacon
+ * @list: the list pointer.
+ * @skb: the skb containing this specific beacon
+ * @offs: &struct ieee80211_mutable_offsets pointer to struct that will
+ *	receive the offsets that may be updated by the driver.
+ */
+struct ieee80211_ema_bcns {
+	struct list_head list;
+	struct sk_buff *skb;
+	struct ieee80211_mutable_offsets offs;
+};
+
+/**
+ * ieee80211_beacon_get_template_ema_list - EMA beacon template generation
+ *	function for drivers using the hw offload.
+ * @hw: pointer obtained from ieee80211_alloc_hw().
+ * @vif: &struct ieee80211_vif pointer from the add_interface callback.
+ * @head: linked list head that will get populated with
+ *	&struct ieee80211_ema_bcns pointers. Driver must call INIT_LIST_HEAD()
+ *	before calling this function to initialize the list.
+ *
+ * This function follows the same rules as ieee80211_beacon_get_template()
+ * but returns a linked list of all beacon templates required to cover all
+ * profiles in the multiple BSSID set. Each template includes only one multiple
+ * BSSID element. Driver must call ieee80211_beacon_free_ema_list() to free
+ * the allocated memory after done.
+ */
+void ieee80211_beacon_get_template_ema_list(struct ieee80211_hw *hw,
+					    struct ieee80211_vif *vif,
+					    struct list_head *head);
+
+/**
+ * ieee80211_beacon_free_ema_list - free an EMA beacon template list
+ * @head: linked list head containing &struct ieee80211_ema_bcns pointers.
+ *
+ * This function will free a list previously acquired by calling
+ * ieee80211_beacon_get_template_ema_list()
+ */
+void ieee80211_beacon_free_ema_list(struct list_head *head);
+
 /**
  * ieee80211_beacon_get_tim - beacon generation function
  * @hw: pointer obtained from ieee80211_alloc_hw().
diff --git a/net/mac80211/cfg.c b/net/mac80211/cfg.c
index e2b791c37591..cfe99de457cb 100644
--- a/net/mac80211/cfg.c
+++ b/net/mac80211/cfg.c
@@ -987,15 +987,49 @@  static int ieee80211_set_ftm_responder_params(
 	return 0;
 }
 
+static int ieee80211_get_mbssid_beacon_len(struct cfg80211_mbssid_elems *elems)
+{
+	int i, len = 0;
+
+	if (!elems)
+		return 0;
+
+	for (i = 0; i < elems->cnt; i++)
+		len += elems->elem[i].len;
+
+	return len;
+}
+
+static u8 *ieee80211_copy_mbssid_beacon(u8 *offset,
+					struct cfg80211_mbssid_elems *dest,
+					struct cfg80211_mbssid_elems *src)
+{
+	int i;
+
+	if (!dest || !src)
+		return offset;
+
+	dest->cnt = src->cnt;
+	for (i = 0; i < dest->cnt; i++) {
+		dest->elem[i].len = src->elem[i].len;
+		memcpy(offset, src->elem[i].data, dest->elem[i].len);
+		dest->elem[i].data = offset;
+		offset += dest->elem[i].len;
+	}
+
+	return offset;
+}
+
 static int ieee80211_assign_beacon(struct ieee80211_sub_if_data *sdata,
 				   struct cfg80211_beacon_data *params,
 				   const struct ieee80211_csa_settings *csa,
 				   const struct ieee80211_color_change_settings *cca)
 {
-	struct beacon_data *new, *old;
-	int new_head_len, new_tail_len;
+	struct beacon_data *new = NULL, *old;
+	int new_head_len, new_tail_len, new_mbssid_len = 0;
 	int size, err;
 	u32 changed = BSS_CHANGED_BEACON;
+	u8 *new_mbssid_offset;
 
 	old = sdata_dereference(sdata->u.ap.beacon, sdata);
 
@@ -1017,12 +1051,27 @@  static int ieee80211_assign_beacon(struct ieee80211_sub_if_data *sdata,
 	else
 		new_tail_len = old->tail_len;
 
-	size = sizeof(*new) + new_head_len + new_tail_len;
+	/* new or old multiple BSSID elements? */
+	if (params->mbssid_ies)
+		new_mbssid_len = ieee80211_get_mbssid_beacon_len(params->mbssid_ies);
+	else if (old && old->mbssid_ies)
+		new_mbssid_len = ieee80211_get_mbssid_beacon_len(old->mbssid_ies);
+
+	size = sizeof(*new) + new_head_len + new_tail_len + new_mbssid_len;
 
 	new = kzalloc(size, GFP_KERNEL);
 	if (!new)
 		return -ENOMEM;
 
+	if (new_mbssid_len) {
+		new->mbssid_ies = kzalloc(sizeof(*params->mbssid_ies),
+					  GFP_KERNEL);
+		if (!new->mbssid_ies) {
+			err = -ENOMEM;
+			goto error;
+		}
+	}
+
 	/* start filling the new info now */
 
 	/*
@@ -1034,6 +1083,15 @@  static int ieee80211_assign_beacon(struct ieee80211_sub_if_data *sdata,
 	new->head_len = new_head_len;
 	new->tail_len = new_tail_len;
 
+	/* copy in optional mbssid_ies */
+	new_mbssid_offset = new->tail + new_tail_len;
+	if (params->mbssid_ies)
+		ieee80211_copy_mbssid_beacon(new_mbssid_offset, new->mbssid_ies,
+					     params->mbssid_ies);
+	else if (old && old->mbssid_ies)
+		ieee80211_copy_mbssid_beacon(new_mbssid_offset, new->mbssid_ies,
+					     old->mbssid_ies);
+
 	if (csa) {
 		new->cntdwn_current_counter = csa->count;
 		memcpy(new->cntdwn_counter_offsets, csa->counter_offsets_beacon,
@@ -1059,10 +1117,8 @@  static int ieee80211_assign_beacon(struct ieee80211_sub_if_data *sdata,
 
 	err = ieee80211_set_probe_resp(sdata, params->probe_resp,
 				       params->probe_resp_len, csa, cca);
-	if (err < 0) {
-		kfree(new);
-		return err;
-	}
+	if (err < 0)
+		goto error;
 	if (err == 0)
 		changed |= BSS_CHANGED_AP_PROBE_RESP;
 
@@ -1074,20 +1130,26 @@  static int ieee80211_assign_beacon(struct ieee80211_sub_if_data *sdata,
 							 params->civicloc,
 							 params->civicloc_len);
 
-		if (err < 0) {
-			kfree(new);
-			return err;
-		}
+		if (err < 0)
+			goto error;
 
 		changed |= BSS_CHANGED_FTM_RESPONDER;
 	}
 
 	rcu_assign_pointer(sdata->u.ap.beacon, new);
 
-	if (old)
+	if (old) {
+		kfree(old->mbssid_ies);
 		kfree_rcu(old, rcu_head);
-
+	}
 	return changed;
+
+error:
+	if (new) {
+		kfree(new->mbssid_ies);
+		kfree(new);
+	}
+	return err;
 }
 
 static int ieee80211_start_ap(struct wiphy *wiphy, struct net_device *dev,
@@ -1246,8 +1308,10 @@  static int ieee80211_start_ap(struct wiphy *wiphy, struct net_device *dev,
 	if (err) {
 		old = sdata_dereference(sdata->u.ap.beacon, sdata);
 
-		if (old)
+		if (old) {
+			kfree(old->mbssid_ies);
 			kfree_rcu(old, rcu_head);
+		}
 		RCU_INIT_POINTER(sdata->u.ap.beacon, NULL);
 		goto error;
 	}
@@ -1327,8 +1391,11 @@  static int ieee80211_stop_ap(struct wiphy *wiphy, struct net_device *dev)
 
 	mutex_unlock(&local->mtx);
 
-	kfree(sdata->u.ap.next_beacon);
-	sdata->u.ap.next_beacon = NULL;
+	if (sdata->u.ap.next_beacon) {
+		kfree(sdata->u.ap.next_beacon->mbssid_ies);
+		kfree(sdata->u.ap.next_beacon);
+		sdata->u.ap.next_beacon = NULL;
+	}
 
 	/* turn off carrier for this interface and dependent VLANs */
 	list_for_each_entry(vlan, &sdata->u.ap.vlans, u.vlan.list)
@@ -1340,6 +1407,7 @@  static int ieee80211_stop_ap(struct wiphy *wiphy, struct net_device *dev)
 	RCU_INIT_POINTER(sdata->u.ap.probe_resp, NULL);
 	RCU_INIT_POINTER(sdata->u.ap.fils_discovery, NULL);
 	RCU_INIT_POINTER(sdata->u.ap.unsol_bcast_probe_resp, NULL);
+	kfree(old_beacon->mbssid_ies);
 	kfree_rcu(old_beacon, rcu_head);
 	if (old_probe_resp)
 		kfree_rcu(old_probe_resp, rcu_head);
@@ -3123,13 +3191,24 @@  cfg80211_beacon_dup(struct cfg80211_beacon_data *beacon)
 
 	len = beacon->head_len + beacon->tail_len + beacon->beacon_ies_len +
 	      beacon->proberesp_ies_len + beacon->assocresp_ies_len +
-	      beacon->probe_resp_len + beacon->lci_len + beacon->civicloc_len;
+	      beacon->probe_resp_len + beacon->lci_len + beacon->civicloc_len +
+	      ieee80211_get_mbssid_beacon_len(beacon->mbssid_ies);
 
 	new_beacon = kzalloc(sizeof(*new_beacon) + len, GFP_KERNEL);
 	if (!new_beacon)
 		return NULL;
 
+	if (beacon->mbssid_ies) {
+		new_beacon->mbssid_ies = kzalloc(sizeof(*beacon->mbssid_ies),
+						 GFP_KERNEL);
+		if (!new_beacon->mbssid_ies) {
+			kfree(new_beacon);
+			return NULL;
+		}
+	}
+
 	pos = (u8 *)(new_beacon + 1);
+
 	if (beacon->head_len) {
 		new_beacon->head_len = beacon->head_len;
 		new_beacon->head = pos;
@@ -3166,6 +3245,9 @@  cfg80211_beacon_dup(struct cfg80211_beacon_data *beacon)
 		memcpy(pos, beacon->probe_resp, beacon->probe_resp_len);
 		pos += beacon->probe_resp_len;
 	}
+	if (beacon->mbssid_ies && beacon->mbssid_ies->cnt)
+		pos = ieee80211_copy_mbssid_beacon(pos, new_beacon->mbssid_ies,
+						   beacon->mbssid_ies);
 
 	/* might copy -1, meaning no changes requested */
 	new_beacon->ftm_responder = beacon->ftm_responder;
@@ -3203,8 +3285,11 @@  static int ieee80211_set_after_csa_beacon(struct ieee80211_sub_if_data *sdata,
 	case NL80211_IFTYPE_AP:
 		err = ieee80211_assign_beacon(sdata, sdata->u.ap.next_beacon,
 					      NULL, NULL);
-		kfree(sdata->u.ap.next_beacon);
-		sdata->u.ap.next_beacon = NULL;
+		if (sdata->u.ap.next_beacon) {
+			kfree(sdata->u.ap.next_beacon->mbssid_ies);
+			kfree(sdata->u.ap.next_beacon);
+			sdata->u.ap.next_beacon = NULL;
+		}
 
 		if (err < 0)
 			return err;
@@ -3359,8 +3444,10 @@  static int ieee80211_set_csa_beacon(struct ieee80211_sub_if_data *sdata,
 		if ((params->n_counter_offsets_beacon >
 		     IEEE80211_MAX_CNTDWN_COUNTERS_NUM) ||
 		    (params->n_counter_offsets_presp >
-		     IEEE80211_MAX_CNTDWN_COUNTERS_NUM))
-			return -EINVAL;
+		     IEEE80211_MAX_CNTDWN_COUNTERS_NUM)) {
+			err = -EINVAL;
+			goto error;
+		}
 
 		csa.counter_offsets_beacon = params->counter_offsets_beacon;
 		csa.counter_offsets_presp = params->counter_offsets_presp;
@@ -3369,10 +3456,8 @@  static int ieee80211_set_csa_beacon(struct ieee80211_sub_if_data *sdata,
 		csa.count = params->count;
 
 		err = ieee80211_assign_beacon(sdata, &params->beacon_csa, &csa, NULL);
-		if (err < 0) {
-			kfree(sdata->u.ap.next_beacon);
-			return err;
-		}
+		if (err < 0)
+			goto error;
 		*changed |= err;
 
 		break;
@@ -3455,13 +3540,24 @@  static int ieee80211_set_csa_beacon(struct ieee80211_sub_if_data *sdata,
 	}
 
 	return 0;
+
+error:
+	if (sdata->vif.type == NL80211_IFTYPE_AP && sdata->u.ap.next_beacon) {
+		kfree(sdata->u.ap.next_beacon->mbssid_ies);
+		kfree(sdata->u.ap.next_beacon);
+		sdata->u.ap.next_beacon = NULL;
+	}
+	return err;
 }
 
 static void ieee80211_color_change_abort(struct ieee80211_sub_if_data  *sdata)
 {
 	sdata->vif.color_change_active = false;
-	kfree(sdata->u.ap.next_beacon);
-	sdata->u.ap.next_beacon = NULL;
+	if (sdata->u.ap.next_beacon) {
+		kfree(sdata->u.ap.next_beacon->mbssid_ies);
+		kfree(sdata->u.ap.next_beacon);
+		sdata->u.ap.next_beacon = NULL;
+	}
 
 	cfg80211_color_change_aborted_notify(sdata->dev);
 }
@@ -4199,8 +4295,11 @@  ieee80211_set_after_color_change_beacon(struct ieee80211_sub_if_data *sdata,
 
 		ret = ieee80211_assign_beacon(sdata, sdata->u.ap.next_beacon,
 					      NULL, NULL);
-		kfree(sdata->u.ap.next_beacon);
-		sdata->u.ap.next_beacon = NULL;
+		if (sdata->u.ap.next_beacon) {
+			kfree(sdata->u.ap.next_beacon->mbssid_ies);
+			kfree(sdata->u.ap.next_beacon);
+			sdata->u.ap.next_beacon = NULL;
+		}
 
 		if (ret < 0)
 			return ret;
@@ -4243,7 +4342,11 @@  ieee80211_set_color_change_beacon(struct ieee80211_sub_if_data *sdata,
 		err = ieee80211_assign_beacon(sdata, &params->beacon_color_change,
 					      NULL, &color_change);
 		if (err < 0) {
-			kfree(sdata->u.ap.next_beacon);
+			if (sdata->u.ap.next_beacon) {
+				kfree(sdata->u.ap.next_beacon->mbssid_ies);
+				kfree(sdata->u.ap.next_beacon);
+				sdata->u.ap.next_beacon = NULL;
+			}
 			return err;
 		}
 		*changed |= err;
diff --git a/net/mac80211/ieee80211_i.h b/net/mac80211/ieee80211_i.h
index c3b8590a7e90..cf98823beeba 100644
--- a/net/mac80211/ieee80211_i.h
+++ b/net/mac80211/ieee80211_i.h
@@ -257,6 +257,7 @@  struct beacon_data {
 	struct ieee80211_meshconf_ie *meshconf;
 	u16 cntdwn_counter_offsets[IEEE80211_MAX_CNTDWN_COUNTERS_NUM];
 	u8 cntdwn_current_counter;
+	struct cfg80211_mbssid_elems *mbssid_ies;
 	struct rcu_head rcu_head;
 };
 
diff --git a/net/mac80211/tx.c b/net/mac80211/tx.c
index ac9ab007dc6f..5e26427ff100 100644
--- a/net/mac80211/tx.c
+++ b/net/mac80211/tx.c
@@ -5033,19 +5033,62 @@  ieee80211_beacon_get_finish(struct ieee80211_hw *hw,
 		       IEEE80211_TX_CTL_FIRST_FRAGMENT;
 }
 
+static int ieee80211_beacon_mbssid_len(struct beacon_data *beacon, int index)
+{
+	int len = 0;
+
+	if (!beacon->mbssid_ies || !beacon->mbssid_ies->cnt)
+		return 0;
+	else if (index >= beacon->mbssid_ies->cnt)
+		return -1;
+
+	if (index >= 0) {
+		len = beacon->mbssid_ies->elem[index].len;
+	} else {
+		u8 i;
+
+		for (i = 0; i < beacon->mbssid_ies->cnt; i++)
+			len += beacon->mbssid_ies->elem[i].len;
+	}
+
+	return len;
+}
+
+static void ieee80211_beacon_add_mbssid(struct beacon_data *beacon, int index,
+					struct sk_buff *skb)
+{
+	if (!beacon->mbssid_ies || !beacon->mbssid_ies->cnt ||
+	    index >= beacon->mbssid_ies->cnt)
+		return;
+
+	if (index >= 0) {
+		skb_put_data(skb, beacon->mbssid_ies->elem[index].data,
+			     beacon->mbssid_ies->elem[index].len);
+	} else {
+		int i;
+
+		for (i = 0; i < beacon->mbssid_ies->cnt; i++)
+			skb_put_data(skb,
+				     beacon->mbssid_ies->elem[i].data,
+				     beacon->mbssid_ies->elem[i].len);
+	}
+}
+
 static struct sk_buff *
-ieee80211_beacon_get_ap(struct ieee80211_hw *hw,
-			struct ieee80211_vif *vif,
-			struct ieee80211_mutable_offsets *offs,
-			bool is_template,
-			struct beacon_data *beacon,
-			struct ieee80211_chanctx_conf *chanctx_conf)
+__ieee80211_beacon_get_ap(struct ieee80211_hw *hw,
+			  struct ieee80211_vif *vif,
+			  struct ieee80211_mutable_offsets *offs,
+			  bool is_template,
+			  struct beacon_data *beacon,
+			  struct ieee80211_chanctx_conf *chanctx_conf,
+			  int ema_index)
 {
 	struct ieee80211_local *local = hw_to_local(hw);
 	struct ieee80211_sub_if_data *sdata = vif_to_sdata(vif);
 	struct ieee80211_if_ap *ap = &sdata->u.ap;
 	struct sk_buff *skb = NULL;
 	u16 csa_off_base = 0;
+	size_t mbssid_elems_len = 0;
 
 	if (beacon->cntdwn_counter_offsets[0]) {
 		if (!is_template)
@@ -5054,12 +5097,16 @@  ieee80211_beacon_get_ap(struct ieee80211_hw *hw,
 		ieee80211_set_beacon_cntdwn(sdata, beacon);
 	}
 
+	mbssid_elems_len = ieee80211_beacon_mbssid_len(beacon, ema_index);
+	if (mbssid_elems_len == -1)
+		return NULL;
+
 	/* headroom, head length,
-	 * tail length and maximum TIM length
+	 * tail length, maximum TIM length and multiple BSSID length
 	 */
 	skb = dev_alloc_skb(local->tx_headroom + beacon->head_len +
 			    beacon->tail_len + 256 +
-			    local->hw.extra_beacon_tailroom);
+			    local->hw.extra_beacon_tailroom + mbssid_elems_len);
 	if (!skb)
 		return NULL;
 
@@ -5077,6 +5124,14 @@  ieee80211_beacon_get_ap(struct ieee80211_hw *hw,
 		csa_off_base = skb->len;
 	}
 
+	if (mbssid_elems_len) {
+		ieee80211_beacon_add_mbssid(beacon, ema_index, skb);
+		if (offs) {
+			offs->mbssid_off = skb->len - mbssid_elems_len;
+			csa_off_base = skb->len;
+		}
+	}
+
 	if (beacon->tail)
 		skb_put_data(skb, beacon->tail, beacon->tail_len);
 
@@ -5088,11 +5143,73 @@  ieee80211_beacon_get_ap(struct ieee80211_hw *hw,
 	return skb;
 }
 
+static struct sk_buff *
+ieee80211_beacon_get_ap_mbssid(struct ieee80211_hw *hw,
+			       struct ieee80211_vif *vif,
+			       struct ieee80211_mutable_offsets *offs,
+			       bool is_template,
+			       struct beacon_data *beacon,
+			       struct ieee80211_chanctx_conf *chanctx_conf,
+			       int ema_index,
+			       struct list_head *ema_list)
+{
+	u8 i;
+
+	if (!ema_list)
+		return __ieee80211_beacon_get_ap(hw, vif, offs, is_template,
+						 beacon, chanctx_conf,
+						 ema_index);
+
+	for (i = 0; i < beacon->mbssid_ies->cnt; i++) {
+		struct ieee80211_ema_bcns *bcn;
+
+		bcn = kzalloc(sizeof(*bcn), GFP_KERNEL);
+		if (!bcn)
+			break;
+
+		bcn->skb = __ieee80211_beacon_get_ap(hw, vif, &bcn->offs,
+						     is_template, beacon,
+						     chanctx_conf, i);
+		if (!bcn->skb) {
+			kfree(bcn);
+			break;
+		}
+		list_add_tail(&bcn->list, ema_list);
+	}
+
+	if (i < beacon->mbssid_ies->cnt)
+		ieee80211_beacon_free_ema_list(ema_list);
+
+	return NULL;
+}
+
+static struct sk_buff *
+ieee80211_beacon_get_ap(struct ieee80211_hw *hw,
+			struct ieee80211_vif *vif,
+			struct ieee80211_mutable_offsets *offs,
+			bool is_template,
+			struct beacon_data *beacon,
+			struct ieee80211_chanctx_conf *chanctx_conf,
+			int ema_index,
+			struct list_head *ema_list)
+{
+	if (beacon->mbssid_ies && beacon->mbssid_ies->cnt)
+		return ieee80211_beacon_get_ap_mbssid(hw, vif, offs,
+						      is_template, beacon,
+						      chanctx_conf, ema_index,
+						      ema_list);
+
+	return __ieee80211_beacon_get_ap(hw, vif, offs, is_template, beacon,
+					 chanctx_conf, -1);
+}
+
 static struct sk_buff *
 __ieee80211_beacon_get(struct ieee80211_hw *hw,
 		       struct ieee80211_vif *vif,
 		       struct ieee80211_mutable_offsets *offs,
-		       bool is_template)
+		       bool is_template,
+		       int ema_index,
+		       struct list_head *ema_list)
 {
 	struct ieee80211_local *local = hw_to_local(hw);
 	struct beacon_data *beacon = NULL;
@@ -5119,7 +5236,8 @@  __ieee80211_beacon_get(struct ieee80211_hw *hw,
 			goto out;
 
 		skb = ieee80211_beacon_get_ap(hw, vif, offs, is_template,
-					      beacon, chanctx_conf);
+					      beacon, chanctx_conf, ema_index,
+					      ema_list);
 	} else if (sdata->vif.type == NL80211_IFTYPE_ADHOC) {
 		struct ieee80211_if_ibss *ifibss = &sdata->u.ibss;
 		struct ieee80211_hdr *hdr;
@@ -5205,16 +5323,49 @@  ieee80211_beacon_get_template(struct ieee80211_hw *hw,
 			      struct ieee80211_vif *vif,
 			      struct ieee80211_mutable_offsets *offs)
 {
-	return __ieee80211_beacon_get(hw, vif, offs, true);
+	return __ieee80211_beacon_get(hw, vif, offs, true, -1, NULL);
 }
 EXPORT_SYMBOL(ieee80211_beacon_get_template);
 
+struct sk_buff *
+ieee80211_beacon_get_template_ema_index(struct ieee80211_hw *hw,
+					struct ieee80211_vif *vif,
+					struct ieee80211_mutable_offsets *offs,
+					u8 ema_index)
+{
+	return __ieee80211_beacon_get(hw, vif, offs, true, ema_index, NULL);
+}
+EXPORT_SYMBOL(ieee80211_beacon_get_template_ema_index);
+
+void ieee80211_beacon_free_ema_list(struct list_head *head)
+{
+	struct ieee80211_ema_bcns *ema, *tmp;
+
+	list_for_each_entry_safe(ema, tmp, head, list) {
+		kfree_skb(ema->skb);
+		kfree(ema);
+	}
+}
+EXPORT_SYMBOL(ieee80211_beacon_free_ema_list);
+
+void ieee80211_beacon_get_template_ema_list(struct ieee80211_hw *hw,
+					    struct ieee80211_vif *vif,
+					    struct list_head *head)
+{
+	if (!head)
+		return;
+
+	(void)__ieee80211_beacon_get(hw, vif, NULL, false, 0, head);
+}
+EXPORT_SYMBOL(ieee80211_beacon_get_template_ema_list);
+
 struct sk_buff *ieee80211_beacon_get_tim(struct ieee80211_hw *hw,
 					 struct ieee80211_vif *vif,
 					 u16 *tim_offset, u16 *tim_length)
 {
 	struct ieee80211_mutable_offsets offs = {};
-	struct sk_buff *bcn = __ieee80211_beacon_get(hw, vif, &offs, false);
+	struct sk_buff *bcn = __ieee80211_beacon_get(hw, vif, &offs, false, 0,
+						     NULL);
 	struct sk_buff *copy;
 	struct ieee80211_supported_band *sband;
 	int shift;