recipes-wifi/linux-mac80211/files/patches-6.x/subsys/336-v6.4-wifi-mac80211-generate-EMA-beacons-in-AP-mode.patch - filogic/rdk-b/meta-filogic - Gitiles

 From bd54f3c29077f23dad92ef82a78061b40be30c65 Mon Sep 17 00:00:00 2001
 From: Aloka Dixit <quic_alokad@quicinc.com>
 Date: Mon, 5 Dec 2022 16:50:37 -0800
 Subject: [PATCH] wifi: mac80211: generate EMA beacons in AP mode

 Add APIs to generate an array of beacons for an EMA AP (enhanced
 multiple BSSID advertisements), each including a single MBSSID element.
 EMA profile periodicity equals the count of elements.

 - ieee80211_beacon_get_template_ema_list() - Generate and return all
 EMA beacon 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 AP.

 - ieee80211_beacon_get_template_ema_index() - Generate a beacon which
 includes the multiple BSSID element at the given index. Drivers can use
 this function in a loop until NULL is returned which indicates end of
 available MBSSID elements.

 - ieee80211_beacon_free_ema_list() - free the memory allocated for the
 list of EMA beacon templates.

 Modify existing functions ieee80211_beacon_get_ap(),
 ieee80211_get_mbssid_beacon_len() and ieee80211_beacon_add_mbssid()
 to accept a new parameter for EMA index.

 Signed-off-by: Aloka Dixit <quic_alokad@quicinc.com>
 Co-developed-by: John Crispin <john@phrozen.org>
 Signed-off-by: John Crispin <john@phrozen.org>
 Link: https://lore.kernel.org/r/20221206005040.3177-2-quic_alokad@quicinc.com
 Signed-off-by: Johannes Berg <johannes.berg@intel.com>
 ---
  include/net/mac80211.h     |  68 +++++++++++++++++++
  net/mac80211/cfg.c         |  11 +--
  net/mac80211/ieee80211_i.h |  10 ++-
  net/mac80211/tx.c          | 134 ++++++++++++++++++++++++++++++++++---
  4 files changed, 205 insertions(+), 18 deletions(-)

 --- a/include/net/mac80211.h
 +++ b/include/net/mac80211.h
 @@ -5252,6 +5252,74 @@ ieee80211_beacon_get_template(struct iee
  			      unsigned int link_id);

  /**
 + * ieee80211_beacon_get_template_ema_index - EMA beacon template generation
 + * @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.
 + * @link_id: the link id to which the beacon belongs (or 0 for a non-MLD AP).
 + * @ema_index: index of the beacon in the EMA set.
 + *
 + * 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 indicates the end of EMA templates.
 + */
 +struct sk_buff *
 +ieee80211_beacon_get_template_ema_index(struct ieee80211_hw *hw,
 +					struct ieee80211_vif *vif,
 +					struct ieee80211_mutable_offsets *offs,
 +					unsigned int link_id, u8 ema_index);
 +
 +/**
 + * struct ieee80211_ema_beacons - List of EMA beacons
 + * @cnt: count of EMA beacons.
 + *
 + * @bcn: array of EMA beacons.
 + * @bcn.skb: the skb containing this specific beacon
 + * @bcn.offs: &struct ieee80211_mutable_offsets pointer to struct that will
 + *	receive the offsets that may be updated by the driver.
 + */
 +struct ieee80211_ema_beacons {
 +	u8 cnt;
 +	struct {
 +		struct sk_buff *skb;
 +		struct ieee80211_mutable_offsets offs;
 +	} bcn[];
 +};
 +
 +/**
 + * ieee80211_beacon_get_template_ema_list - EMA beacon template generation
 + * @hw: pointer obtained from ieee80211_alloc_hw().
 + * @vif: &struct ieee80211_vif pointer from the add_interface callback.
 + * @link_id: the link id to which the beacon belongs (or 0 for a non-MLD AP)
 + *
 + * This function follows the same rules as ieee80211_beacon_get_template()
 + * but allocates and returns a pointer to 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 memory.
 + *
 + * Return: EMA beacon templates of type struct ieee80211_ema_beacons *.
 + *	%NULL on error.
 + */
 +struct ieee80211_ema_beacons *
 +ieee80211_beacon_get_template_ema_list(struct ieee80211_hw *hw,
 +				       struct ieee80211_vif *vif,
 +				       unsigned int link_id);
 +
 +/**
 + * ieee80211_beacon_free_ema_list - free an EMA beacon template list
 + * @ema_beacons: list of EMA beacons of type &struct ieee80211_ema_beacons pointers.
 + *
 + * This function will free a list previously acquired by calling
 + * ieee80211_beacon_get_template_ema_list()
 + */
 +void ieee80211_beacon_free_ema_list(struct ieee80211_ema_beacons *ema_beacons);
 +
 +/**
   * ieee80211_beacon_get_tim - beacon generation function
   * @hw: pointer obtained from ieee80211_alloc_hw().
   * @vif: &struct ieee80211_vif pointer from the add_interface callback.
 --- a/net/mac80211/cfg.c
 +++ b/net/mac80211/cfg.c
 @@ -1122,11 +1122,11 @@ static int ieee80211_assign_beacon(struc
  	if (params->mbssid_ies) {
  		mbssid = params->mbssid_ies;
  		size += struct_size(new->mbssid_ies, elem, mbssid->cnt);
 -		size += ieee80211_get_mbssid_beacon_len(mbssid);
 +		size += ieee80211_get_mbssid_beacon_len(mbssid, mbssid->cnt);
  	} else if (old && old->mbssid_ies) {
  		mbssid = old->mbssid_ies;
  		size += struct_size(new->mbssid_ies, elem, mbssid->cnt);
 -		size += ieee80211_get_mbssid_beacon_len(mbssid);
 +		size += ieee80211_get_mbssid_beacon_len(mbssid, mbssid->cnt);
  	}

  	new = kzalloc(size, GFP_KERNEL);
 @@ -3384,8 +3384,11 @@ cfg80211_beacon_dup(struct cfg80211_beac

  	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 +
 -	      ieee80211_get_mbssid_beacon_len(beacon->mbssid_ies);
 +	      beacon->probe_resp_len + beacon->lci_len + beacon->civicloc_len;
 +
 +	if (beacon->mbssid_ies)
 +		len += ieee80211_get_mbssid_beacon_len(beacon->mbssid_ies,
 +						       beacon->mbssid_ies->cnt);

  	new_beacon = kzalloc(sizeof(*new_beacon) + len, GFP_KERNEL);
  	if (!new_beacon)
 --- a/net/mac80211/ieee80211_i.h
 +++ b/net/mac80211/ieee80211_i.h
 @@ -1182,13 +1182,17 @@ ieee80211_vif_get_shift(struct ieee80211
  }

  static inline int
 -ieee80211_get_mbssid_beacon_len(struct cfg80211_mbssid_elems *elems)
 +ieee80211_get_mbssid_beacon_len(struct cfg80211_mbssid_elems *elems, u8 i)
  {
 -	int i, len = 0;
 +	int len = 0;

 -	if (!elems)
 +	if (!elems || !elems->cnt || i > elems->cnt)
  		return 0;

 +	if (i < elems->cnt)
 +		return elems->elem[i].len;
 +
 +	/* i == elems->cnt, calculate total length of all MBSSID elements */
  	for (i = 0; i < elems->cnt; i++)
  		len += elems->elem[i].len;

 --- a/net/mac80211/tx.c
 +++ b/net/mac80211/tx.c
 @@ -5205,13 +5205,20 @@ ieee80211_beacon_get_finish(struct ieee8
  }

  static void
 -ieee80211_beacon_add_mbssid(struct sk_buff *skb, struct beacon_data *beacon)
 +ieee80211_beacon_add_mbssid(struct sk_buff *skb, struct beacon_data *beacon,
 +			    u8 i)
  {
 -	int i;
 +	if (!beacon->mbssid_ies || !beacon->mbssid_ies->cnt ||
 +	    i > beacon->mbssid_ies->cnt)
 +		return;

 -	if (!beacon->mbssid_ies)
 +	if (i < beacon->mbssid_ies->cnt) {
 +		skb_put_data(skb, beacon->mbssid_ies->elem[i].data,
 +			     beacon->mbssid_ies->elem[i].len);
  		return;
 +	}

 +	/* i == beacon->mbssid_ies->cnt, include all MBSSID elements */
  	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);
 @@ -5224,7 +5231,8 @@ ieee80211_beacon_get_ap(struct ieee80211
  			struct ieee80211_mutable_offsets *offs,
  			bool is_template,
  			struct beacon_data *beacon,
 -			struct ieee80211_chanctx_conf *chanctx_conf)
 +			struct ieee80211_chanctx_conf *chanctx_conf,
 +			u8 ema_index)
  {
  	struct ieee80211_local *local = hw_to_local(hw);
  	struct ieee80211_sub_if_data *sdata = vif_to_sdata(vif);
 @@ -5243,7 +5251,9 @@ ieee80211_beacon_get_ap(struct ieee80211
  	/* headroom, head length,
  	 * tail length, maximum TIM length and multiple BSSID length
  	 */
 -	mbssid_len = ieee80211_get_mbssid_beacon_len(beacon->mbssid_ies);
 +	mbssid_len = ieee80211_get_mbssid_beacon_len(beacon->mbssid_ies,
 +						     ema_index);
 +
  	skb = dev_alloc_skb(local->tx_headroom + beacon->head_len +
  			    beacon->tail_len + 256 +
  			    local->hw.extra_beacon_tailroom + mbssid_len);
 @@ -5261,7 +5271,7 @@ ieee80211_beacon_get_ap(struct ieee80211
  		offs->cntdwn_counter_offs[0] = beacon->cntdwn_counter_offsets[0];

  		if (mbssid_len) {
 -			ieee80211_beacon_add_mbssid(skb, beacon);
 +			ieee80211_beacon_add_mbssid(skb, beacon, ema_index);
  			offs->mbssid_off = skb->len - mbssid_len;
  		}

 @@ -5280,12 +5290,51 @@ ieee80211_beacon_get_ap(struct ieee80211
  	return skb;
  }

 +static struct ieee80211_ema_beacons *
 +ieee80211_beacon_get_ap_ema_list(struct ieee80211_hw *hw,
 +				 struct ieee80211_vif *vif,
 +				 struct ieee80211_link_data *link,
 +				 struct ieee80211_mutable_offsets *offs,
 +				 bool is_template, struct beacon_data *beacon,
 +				 struct ieee80211_chanctx_conf *chanctx_conf)
 +{
 +	struct ieee80211_ema_beacons *ema = NULL;
 +
 +	if (!beacon->mbssid_ies || !beacon->mbssid_ies->cnt)
 +		return NULL;
 +
 +	ema = kzalloc(struct_size(ema, bcn, beacon->mbssid_ies->cnt),
 +		      GFP_ATOMIC);
 +	if (!ema)
 +		return NULL;
 +
 +	for (ema->cnt = 0; ema->cnt < beacon->mbssid_ies->cnt; ema->cnt++) {
 +		ema->bcn[ema->cnt].skb =
 +			ieee80211_beacon_get_ap(hw, vif, link,
 +						&ema->bcn[ema->cnt].offs,
 +						is_template, beacon,
 +						chanctx_conf, ema->cnt);
 +		if (!ema->bcn[ema->cnt].skb)
 +			break;
 +	}
 +
 +	if (ema->cnt == beacon->mbssid_ies->cnt)
 +		return ema;
 +
 +	ieee80211_beacon_free_ema_list(ema);
 +	return NULL;
 +}
 +
 +#define IEEE80211_INCLUDE_ALL_MBSSID_ELEMS -1
 +
  static struct sk_buff *
  __ieee80211_beacon_get(struct ieee80211_hw *hw,
  		       struct ieee80211_vif *vif,
  		       struct ieee80211_mutable_offsets *offs,
  		       bool is_template,
 -		       unsigned int link_id)
 +		       unsigned int link_id,
 +		       int ema_index,
 +		       struct ieee80211_ema_beacons **ema_beacons)
  {
  	struct ieee80211_local *local = hw_to_local(hw);
  	struct beacon_data *beacon = NULL;
 @@ -5314,8 +5363,29 @@ __ieee80211_beacon_get(struct ieee80211_
  		if (!beacon)
  			goto out;

 -		skb = ieee80211_beacon_get_ap(hw, vif, link, offs, is_template,
 -					      beacon, chanctx_conf);
 +		if (ema_beacons) {
 +			*ema_beacons =
 +				ieee80211_beacon_get_ap_ema_list(hw, vif, link,
 +								 offs,
 +								 is_template,
 +								 beacon,
 +								 chanctx_conf);
 +		} else {
 +			if (beacon->mbssid_ies && beacon->mbssid_ies->cnt) {
 +				if (ema_index >= beacon->mbssid_ies->cnt)
 +					goto out; /* End of MBSSID elements */
 +
 +				if (ema_index <= IEEE80211_INCLUDE_ALL_MBSSID_ELEMS)
 +					ema_index = beacon->mbssid_ies->cnt;
 +			} else {
 +				ema_index = 0;
 +			}
 +
 +			skb = ieee80211_beacon_get_ap(hw, vif, link, offs,
 +						      is_template, beacon,
 +						      chanctx_conf,
 +						      ema_index);
 +		}
  	} else if (sdata->vif.type == NL80211_IFTYPE_ADHOC) {
  		struct ieee80211_if_ibss *ifibss = &sdata->u.ibss;
  		struct ieee80211_hdr *hdr;
 @@ -5403,10 +5473,50 @@ ieee80211_beacon_get_template(struct iee
  			      struct ieee80211_mutable_offsets *offs,
  			      unsigned int link_id)
  {
 -	return __ieee80211_beacon_get(hw, vif, offs, true, link_id);
 +	return __ieee80211_beacon_get(hw, vif, offs, true, link_id,
 +				      IEEE80211_INCLUDE_ALL_MBSSID_ELEMS, 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,
 +					unsigned int link_id, u8 ema_index)
 +{
 +	return __ieee80211_beacon_get(hw, vif, offs, true, link_id, ema_index,
 +				      NULL);
 +}
 +EXPORT_SYMBOL(ieee80211_beacon_get_template_ema_index);
 +
 +void ieee80211_beacon_free_ema_list(struct ieee80211_ema_beacons *ema_beacons)
 +{
 +	u8 i;
 +
 +	if (!ema_beacons)
 +		return;
 +
 +	for (i = 0; i < ema_beacons->cnt; i++)
 +		kfree_skb(ema_beacons->bcn[i].skb);
 +
 +	kfree(ema_beacons);
 +}
 +EXPORT_SYMBOL(ieee80211_beacon_free_ema_list);
 +
 +struct ieee80211_ema_beacons *
 +ieee80211_beacon_get_template_ema_list(struct ieee80211_hw *hw,
 +				       struct ieee80211_vif *vif,
 +				       unsigned int link_id)
 +{
 +	struct ieee80211_ema_beacons *ema_beacons = NULL;
 +
 +	WARN_ON(__ieee80211_beacon_get(hw, vif, NULL, false, link_id, 0,
 +				       &ema_beacons));
 +
 +	return ema_beacons;
 +}
 +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,
 @@ -5414,7 +5524,9 @@ struct sk_buff *ieee80211_beacon_get_tim
  {
  	struct ieee80211_mutable_offsets offs = {};
  	struct sk_buff *bcn = __ieee80211_beacon_get(hw, vif, &offs, false,
 -						     link_id);
 +						     link_id,
 +						     IEEE80211_INCLUDE_ALL_MBSSID_ELEMS,
 +						     NULL);
  	struct sk_buff *copy;
  	int shift;
	From bd54f3c29077f23dad92ef82a78061b40be30c65 Mon Sep 17 00:00:00 2001
	From: Aloka Dixit <quic_alokad@quicinc.com>
	Date: Mon, 5 Dec 2022 16:50:37 -0800
	Subject: [PATCH] wifi: mac80211: generate EMA beacons in AP mode

	Add APIs to generate an array of beacons for an EMA AP (enhanced
	multiple BSSID advertisements), each including a single MBSSID element.
	EMA profile periodicity equals the count of elements.

	- ieee80211_beacon_get_template_ema_list() - Generate and return all
	EMA beacon 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 AP.

	- ieee80211_beacon_get_template_ema_index() - Generate a beacon which
	includes the multiple BSSID element at the given index. Drivers can use
	this function in a loop until NULL is returned which indicates end of
	available MBSSID elements.

	- ieee80211_beacon_free_ema_list() - free the memory allocated for the
	list of EMA beacon templates.

	Modify existing functions ieee80211_beacon_get_ap(),
	ieee80211_get_mbssid_beacon_len() and ieee80211_beacon_add_mbssid()
	to accept a new parameter for EMA index.

	Signed-off-by: Aloka Dixit <quic_alokad@quicinc.com>
	Co-developed-by: John Crispin <john@phrozen.org>
	Signed-off-by: John Crispin <john@phrozen.org>
	Link: https://lore.kernel.org/r/20221206005040.3177-2-quic_alokad@quicinc.com
	Signed-off-by: Johannes Berg <johannes.berg@intel.com>
	---
	include/net/mac80211.h \| 68 +++++++++++++++++++
	net/mac80211/cfg.c \| 11 +--
	net/mac80211/ieee80211_i.h \| 10 ++-
	net/mac80211/tx.c \| 134 ++++++++++++++++++++++++++++++++++---
	4 files changed, 205 insertions(+), 18 deletions(-)

	--- a/include/net/mac80211.h
	+++ b/include/net/mac80211.h
	@@ -5252,6 +5252,74 @@ ieee80211_beacon_get_template(struct iee
	unsigned int link_id);

	/**
	+ * ieee80211_beacon_get_template_ema_index - EMA beacon template generation
	+ * @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.
	+ * @link_id: the link id to which the beacon belongs (or 0 for a non-MLD AP).
	+ * @ema_index: index of the beacon in the EMA set.
	+ *
	+ * 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 indicates the end of EMA templates.
	+ */
	+struct sk_buff *
	+ieee80211_beacon_get_template_ema_index(struct ieee80211_hw *hw,
	+ struct ieee80211_vif *vif,
	+ struct ieee80211_mutable_offsets *offs,
	+ unsigned int link_id, u8 ema_index);
	+
	+/**
	+ * struct ieee80211_ema_beacons - List of EMA beacons
	+ * @cnt: count of EMA beacons.
	+ *
	+ * @bcn: array of EMA beacons.
	+ * @bcn.skb: the skb containing this specific beacon
	+ * @bcn.offs: &struct ieee80211_mutable_offsets pointer to struct that will
	+ * receive the offsets that may be updated by the driver.
	+ */
	+struct ieee80211_ema_beacons {
	+ u8 cnt;
	+ struct {
	+ struct sk_buff *skb;
	+ struct ieee80211_mutable_offsets offs;
	+ } bcn[];
	+};
	+
	+/**
	+ * ieee80211_beacon_get_template_ema_list - EMA beacon template generation
	+ * @hw: pointer obtained from ieee80211_alloc_hw().
	+ * @vif: &struct ieee80211_vif pointer from the add_interface callback.
	+ * @link_id: the link id to which the beacon belongs (or 0 for a non-MLD AP)
	+ *
	+ * This function follows the same rules as ieee80211_beacon_get_template()
	+ * but allocates and returns a pointer to 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 memory.
	+ *
	+ * Return: EMA beacon templates of type struct ieee80211_ema_beacons *.
	+ * %NULL on error.
	+ */
	+struct ieee80211_ema_beacons *
	+ieee80211_beacon_get_template_ema_list(struct ieee80211_hw *hw,
	+ struct ieee80211_vif *vif,
	+ unsigned int link_id);
	+
	+/**
	+ * ieee80211_beacon_free_ema_list - free an EMA beacon template list
	+ * @ema_beacons: list of EMA beacons of type &struct ieee80211_ema_beacons pointers.
	+ *
	+ * This function will free a list previously acquired by calling
	+ * ieee80211_beacon_get_template_ema_list()
	+ */
	+void ieee80211_beacon_free_ema_list(struct ieee80211_ema_beacons *ema_beacons);
	+
	+/**
	* ieee80211_beacon_get_tim - beacon generation function
	* @hw: pointer obtained from ieee80211_alloc_hw().
	* @vif: &struct ieee80211_vif pointer from the add_interface callback.
	--- a/net/mac80211/cfg.c
	+++ b/net/mac80211/cfg.c
	@@ -1122,11 +1122,11 @@ static int ieee80211_assign_beacon(struc
	if (params->mbssid_ies) {
	mbssid = params->mbssid_ies;
	size += struct_size(new->mbssid_ies, elem, mbssid->cnt);
	- size += ieee80211_get_mbssid_beacon_len(mbssid);
	+ size += ieee80211_get_mbssid_beacon_len(mbssid, mbssid->cnt);
	} else if (old && old->mbssid_ies) {
	mbssid = old->mbssid_ies;
	size += struct_size(new->mbssid_ies, elem, mbssid->cnt);
	- size += ieee80211_get_mbssid_beacon_len(mbssid);
	+ size += ieee80211_get_mbssid_beacon_len(mbssid, mbssid->cnt);
	}

	new = kzalloc(size, GFP_KERNEL);
	@@ -3384,8 +3384,11 @@ cfg80211_beacon_dup(struct cfg80211_beac

	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 +
	- ieee80211_get_mbssid_beacon_len(beacon->mbssid_ies);
	+ beacon->probe_resp_len + beacon->lci_len + beacon->civicloc_len;
	+
	+ if (beacon->mbssid_ies)
	+ len += ieee80211_get_mbssid_beacon_len(beacon->mbssid_ies,
	+ beacon->mbssid_ies->cnt);

	new_beacon = kzalloc(sizeof(*new_beacon) + len, GFP_KERNEL);
	if (!new_beacon)
	--- a/net/mac80211/ieee80211_i.h
	+++ b/net/mac80211/ieee80211_i.h
	@@ -1182,13 +1182,17 @@ ieee80211_vif_get_shift(struct ieee80211
	}

	static inline int
	-ieee80211_get_mbssid_beacon_len(struct cfg80211_mbssid_elems *elems)
	+ieee80211_get_mbssid_beacon_len(struct cfg80211_mbssid_elems *elems, u8 i)
	{
	- int i, len = 0;
	+ int len = 0;

	- if (!elems)
	+ if (!elems \|\| !elems->cnt \|\| i > elems->cnt)
	return 0;

	+ if (i < elems->cnt)
	+ return elems->elem[i].len;
	+
	+ /* i == elems->cnt, calculate total length of all MBSSID elements */
	for (i = 0; i < elems->cnt; i++)
	len += elems->elem[i].len;

	--- a/net/mac80211/tx.c
	+++ b/net/mac80211/tx.c
	@@ -5205,13 +5205,20 @@ ieee80211_beacon_get_finish(struct ieee8
	}

	static void
	-ieee80211_beacon_add_mbssid(struct sk_buff skb, struct beacon_data beacon)
	+ieee80211_beacon_add_mbssid(struct sk_buff skb, struct beacon_data beacon,
	+ u8 i)
	{
	- int i;
	+ if (!beacon->mbssid_ies \|\| !beacon->mbssid_ies->cnt \|\|
	+ i > beacon->mbssid_ies->cnt)
	+ return;

	- if (!beacon->mbssid_ies)
	+ if (i < beacon->mbssid_ies->cnt) {
	+ skb_put_data(skb, beacon->mbssid_ies->elem[i].data,
	+ beacon->mbssid_ies->elem[i].len);
	return;
	+ }

	+ /* i == beacon->mbssid_ies->cnt, include all MBSSID elements */
	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);
	@@ -5224,7 +5231,8 @@ ieee80211_beacon_get_ap(struct ieee80211
	struct ieee80211_mutable_offsets *offs,
	bool is_template,
	struct beacon_data *beacon,
	- struct ieee80211_chanctx_conf *chanctx_conf)
	+ struct ieee80211_chanctx_conf *chanctx_conf,
	+ u8 ema_index)
	{
	struct ieee80211_local *local = hw_to_local(hw);
	struct ieee80211_sub_if_data *sdata = vif_to_sdata(vif);
	@@ -5243,7 +5251,9 @@ ieee80211_beacon_get_ap(struct ieee80211
	/* headroom, head length,
	* tail length, maximum TIM length and multiple BSSID length
	*/
	- mbssid_len = ieee80211_get_mbssid_beacon_len(beacon->mbssid_ies);
	+ mbssid_len = ieee80211_get_mbssid_beacon_len(beacon->mbssid_ies,
	+ ema_index);
	+
	skb = dev_alloc_skb(local->tx_headroom + beacon->head_len +
	beacon->tail_len + 256 +
	local->hw.extra_beacon_tailroom + mbssid_len);
	@@ -5261,7 +5271,7 @@ ieee80211_beacon_get_ap(struct ieee80211
	offs->cntdwn_counter_offs[0] = beacon->cntdwn_counter_offsets[0];

	if (mbssid_len) {
	- ieee80211_beacon_add_mbssid(skb, beacon);
	+ ieee80211_beacon_add_mbssid(skb, beacon, ema_index);
	offs->mbssid_off = skb->len - mbssid_len;
	}

	@@ -5280,12 +5290,51 @@ ieee80211_beacon_get_ap(struct ieee80211
	return skb;
	}

	+static struct ieee80211_ema_beacons *
	+ieee80211_beacon_get_ap_ema_list(struct ieee80211_hw *hw,
	+ struct ieee80211_vif *vif,
	+ struct ieee80211_link_data *link,
	+ struct ieee80211_mutable_offsets *offs,
	+ bool is_template, struct beacon_data *beacon,
	+ struct ieee80211_chanctx_conf *chanctx_conf)
	+{
	+ struct ieee80211_ema_beacons *ema = NULL;
	+
	+ if (!beacon->mbssid_ies \|\| !beacon->mbssid_ies->cnt)
	+ return NULL;
	+
	+ ema = kzalloc(struct_size(ema, bcn, beacon->mbssid_ies->cnt),
	+ GFP_ATOMIC);
	+ if (!ema)
	+ return NULL;
	+
	+ for (ema->cnt = 0; ema->cnt < beacon->mbssid_ies->cnt; ema->cnt++) {
	+ ema->bcn[ema->cnt].skb =
	+ ieee80211_beacon_get_ap(hw, vif, link,
	+ &ema->bcn[ema->cnt].offs,
	+ is_template, beacon,
	+ chanctx_conf, ema->cnt);
	+ if (!ema->bcn[ema->cnt].skb)
	+ break;
	+ }
	+
	+ if (ema->cnt == beacon->mbssid_ies->cnt)
	+ return ema;
	+
	+ ieee80211_beacon_free_ema_list(ema);
	+ return NULL;
	+}
	+
	+#define IEEE80211_INCLUDE_ALL_MBSSID_ELEMS -1
	+
	static struct sk_buff *
	__ieee80211_beacon_get(struct ieee80211_hw *hw,
	struct ieee80211_vif *vif,
	struct ieee80211_mutable_offsets *offs,
	bool is_template,
	- unsigned int link_id)
	+ unsigned int link_id,
	+ int ema_index,
	+ struct ieee80211_ema_beacons **ema_beacons)
	{
	struct ieee80211_local *local = hw_to_local(hw);
	struct beacon_data *beacon = NULL;
	@@ -5314,8 +5363,29 @@ __ieee80211_beacon_get(struct ieee80211_
	if (!beacon)
	goto out;

	- skb = ieee80211_beacon_get_ap(hw, vif, link, offs, is_template,
	- beacon, chanctx_conf);
	+ if (ema_beacons) {
	+ *ema_beacons =
	+ ieee80211_beacon_get_ap_ema_list(hw, vif, link,
	+ offs,
	+ is_template,
	+ beacon,
	+ chanctx_conf);
	+ } else {
	+ if (beacon->mbssid_ies && beacon->mbssid_ies->cnt) {
	+ if (ema_index >= beacon->mbssid_ies->cnt)
	+ goto out; /* End of MBSSID elements */
	+
	+ if (ema_index <= IEEE80211_INCLUDE_ALL_MBSSID_ELEMS)
	+ ema_index = beacon->mbssid_ies->cnt;
	+ } else {
	+ ema_index = 0;
	+ }
	+
	+ skb = ieee80211_beacon_get_ap(hw, vif, link, offs,
	+ is_template, beacon,
	+ chanctx_conf,
	+ ema_index);
	+ }
	} else if (sdata->vif.type == NL80211_IFTYPE_ADHOC) {
	struct ieee80211_if_ibss *ifibss = &sdata->u.ibss;
	struct ieee80211_hdr *hdr;
	@@ -5403,10 +5473,50 @@ ieee80211_beacon_get_template(struct iee
	struct ieee80211_mutable_offsets *offs,
	unsigned int link_id)
	{
	- return __ieee80211_beacon_get(hw, vif, offs, true, link_id);
	+ return __ieee80211_beacon_get(hw, vif, offs, true, link_id,
	+ IEEE80211_INCLUDE_ALL_MBSSID_ELEMS, 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,
	+ unsigned int link_id, u8 ema_index)
	+{
	+ return __ieee80211_beacon_get(hw, vif, offs, true, link_id, ema_index,
	+ NULL);
	+}
	+EXPORT_SYMBOL(ieee80211_beacon_get_template_ema_index);
	+
	+void ieee80211_beacon_free_ema_list(struct ieee80211_ema_beacons *ema_beacons)
	+{
	+ u8 i;
	+
	+ if (!ema_beacons)
	+ return;
	+
	+ for (i = 0; i < ema_beacons->cnt; i++)
	+ kfree_skb(ema_beacons->bcn[i].skb);
	+
	+ kfree(ema_beacons);
	+}
	+EXPORT_SYMBOL(ieee80211_beacon_free_ema_list);
	+
	+struct ieee80211_ema_beacons *
	+ieee80211_beacon_get_template_ema_list(struct ieee80211_hw *hw,
	+ struct ieee80211_vif *vif,
	+ unsigned int link_id)
	+{
	+ struct ieee80211_ema_beacons *ema_beacons = NULL;
	+
	+ WARN_ON(__ieee80211_beacon_get(hw, vif, NULL, false, link_id, 0,
	+ &ema_beacons));
	+
	+ return ema_beacons;
	+}
	+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,
	@@ -5414,7 +5524,9 @@ struct sk_buff *ieee80211_beacon_get_tim
	{
	struct ieee80211_mutable_offsets offs = {};
	struct sk_buff *bcn = __ieee80211_beacon_get(hw, vif, &offs, false,
	- link_id);
	+ link_id,
	+ IEEE80211_INCLUDE_ALL_MBSSID_ELEMS,
	+ NULL);
	struct sk_buff *copy;
	int shift;