mac80211: backport EMA beacon support

Backport EMA beacon support from kernel 6.4.
It is required for MBSSID/EMA suport in ath11k that will follow.

Tested-by: Francisco G Luna <[email protected]>
Signed-off-by: Robert Marko <[email protected]>

package/kernel/mac80211/patches/subsys/336-v6.4-wifi-mac80211-generate-EMA-beacons-in-AP-mode.patch

@@ -0,0 +1,372 @@
+From bd54f3c29077f23dad92ef82a78061b40be30c65 Mon Sep 17 00:00:00 2001
+From: Aloka Dixit <[email protected]>
+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 <[email protected]>
+Co-developed-by: John Crispin <[email protected]>
+Signed-off-by: John Crispin <[email protected]>
+Link: https://lore.kernel.org/r/[email protected]
+Signed-off-by: Johannes Berg <[email protected]>
+---
+ 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;
+ 

package/kernel/mac80211/patches/subsys/500-mac80211_configure_antenna_gain.patch

@@ -77,7 +77,7 @@
  static void ieee80211_rfkill_poll(struct wiphy *wiphy)
  {
  	struct ieee80211_local *local = wiphy_priv(wiphy);
-@@ -4953,6 +4966,7 @@ const struct cfg80211_ops mac80211_confi
+@@ -4956,6 +4969,7 @@ const struct cfg80211_ops mac80211_confi
  	.set_wiphy_params = ieee80211_set_wiphy_params,
  	.set_tx_power = ieee80211_set_tx_power,
  	.get_tx_power = ieee80211_get_tx_power,
@@ -87,7 +87,7 @@
  	CFG80211_TESTMODE_DUMP(ieee80211_testmode_dump)
 --- a/net/mac80211/ieee80211_i.h
 +++ b/net/mac80211/ieee80211_i.h
-@@ -1538,6 +1538,7 @@ struct ieee80211_local {
+@@ -1542,6 +1542,7 @@ struct ieee80211_local {
  	int dynamic_ps_forced_timeout;
  
  	int user_power_level; /* in dBm, for all interfaces */