+From fb4fc65312a19630f7726e2bd992931349daebee Mon Sep 17 00:00:00 2001
+From: John Crispin <john@phrozen.org>
+Date: Tue, 6 Oct 2020 15:51:59 +0200
+Subject: [PATCH 3/4] mac80211: add multiple bssid/EMA support to beacon
+ handling
+
+With beacon_data now holding the additional information about the multiple
+bssid elements, we need to honour these in the various beacon handling
+code paths.
+
+Extend ieee80211_beacon_get_template() to allow generation of EMA beacons.
+The API provides support for HW that can offload the EMA beaconing aswell
+as HW that will require periodic updates of the beacon template.
+
+Signed-off-by: John Crispin <john@phrozen.org>
+---
+ include/net/mac80211.h | 92 +++++++++++++++++++++-
+ net/mac80211/cfg.c | 59 +++++++++++++-
+ net/mac80211/ieee80211_i.h | 2 +
+ net/mac80211/tx.c | 156 +++++++++++++++++++++++++++++++++----
+ 4 files changed, 288 insertions(+), 21 deletions(-)
+
+Index: backports-5.8-1/include/net/mac80211.h
+===================================================================
+--- backports-5.8-1.orig/include/net/mac80211.h
++++ backports-5.8-1/include/net/mac80211.h
+@@ -4834,6 +4834,8 @@ struct ieee80211_mutable_offsets {
+ u16 tim_length;
+
+ u16 csa_counter_offs[IEEE80211_MAX_CSA_COUNTERS_NUM];
++ u16 multiple_bssid_offset;
++ u16 multiple_bssid_length;
+ };
+
+ /**
+@@ -4861,6 +4863,91 @@ ieee80211_beacon_get_template(struct iee
+ struct ieee80211_mutable_offsets *offs);
+
+ /**
++ * enum ieee80211_bcn_tmpl_ema - EMA beacon generation type
++ * @IEEE80211_BCN_EMA_NONE: don't generate an EMA beacon.
++ * @IEEE80211_BCN_EMA_NEXT: generate the next periodicity beacon.
++ * @IEEE80211_BCN_EMA_INDEX: generate beacon by periodicity index
++ * if the value is >= this enum value.
++ */
++enum ieee80211_bcn_tmpl_ema {
++ IEEE80211_BCN_EMA_NONE = -2,
++ IEEE80211_BCN_EMA_NEXT = -1,
++ IEEE80211_BCN_EMA_INDEX = 0,
++};
++
++/**
++ * ieee80211_beacon_get_template_ema_next - 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.
++ *
++ * This function differs from ieee80211_beacon_get_template in the sense that
++ * it generates EMA VAP templates. When we use multiple_bssid, the beacons can
++ * get very large costing a lot of airtime. To work around this, we iterate
++ * over the multiple bssid elements and only send one inside the beacon for
++ * 1..n. Calling this function will auto-increment the periodicity counter.
++ *
++ * This function needs to follow the same rules as ieee80211_beacon_get_template
++ *
++ * Return: The beacon template. %NULL on error.
++ */
++
++struct sk_buff *
++ieee80211_beacon_get_template_ema_next(struct ieee80211_hw *hw,
++ struct ieee80211_vif *vif,
++ struct ieee80211_mutable_offsets *offs);
++
++/** struct ieee80211_ema_bcn_list - 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_bcn_list {
++ 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_bcn_list pointers.
++ *
++ * This function differs from ieee80211_beacon_get_template in the sense that
++ * it generates EMA VAP templates. When we use multiple_bssid, the beacons can
++ * get very large costing a lot of airtime. To work around this, we iterate
++ * over the multiple bssid elements and only send one inside the beacon for
++ * 1..n. This function will populate a linked list that the driver can pass
++ * to the HW.
++ *
++ * This function needs to follow the same rules as ieee80211_beacon_get_template
++ *
++ * Return: The nuber of entries in the list or 0 on error.
++ */
++
++int
++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_bcn_list 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().
+ * @vif: &struct ieee80211_vif pointer from the add_interface callback.
+Index: backports-5.8-1/net/mac80211/cfg.c
+===================================================================
+--- backports-5.8-1.orig/net/mac80211/cfg.c
++++ backports-5.8-1/net/mac80211/cfg.c
+@@ -929,12 +929,38 @@ static int ieee80211_set_ftm_responder_p
+ return 0;
+ }
+
++static int ieee80211_get_multiple_bssid_beacon_len(struct cfg80211_multiple_bssid_data *data)
++{
++ int i, len = 0;
++
++ for (i = 0; i < data->cnt; i++)
++ len += data->len[i];
++
++ return len;
++}
++
++static u8 *ieee80211_copy_multiple_bssid_beacon(u8 *offset,
++ struct cfg80211_multiple_bssid_data *new,
++ struct cfg80211_multiple_bssid_data *old)
++{
++ int i;
++
++ *new = *old;
++ for (i = 0; i < new->cnt; i++) {
++ new->ies[i] = offset;
++ memcpy(new->ies[i], old->ies[i], new->len[i]);
++ offset += new->len[i];
++ }
++ return offset;
++}
++
+ static int ieee80211_assign_beacon(struct ieee80211_sub_if_data *sdata,
+ struct cfg80211_beacon_data *params,
+ const struct ieee80211_csa_settings *csa)
+ {
+ struct beacon_data *new, *old;
+- int new_head_len, new_tail_len;
++ int new_head_len, new_tail_len, new_multiple_bssid_len;
++ u8 *new_multiple_bssid_offset;
+ int size, err;
+ u32 changed = BSS_CHANGED_BEACON;
+
+@@ -958,7 +984,15 @@ static int ieee80211_assign_beacon(struc
+ else
+ new_tail_len = old->tail_len;
+
+- size = sizeof(*new) + new_head_len + new_tail_len;
++ /* new or old multiple_bssid? */
++ if (params->multiple_bssid.cnt || !old)
++ new_multiple_bssid_len =
++ ieee80211_get_multiple_bssid_beacon_len(¶ms->multiple_bssid);
++ else
++ new_multiple_bssid_len =
++ ieee80211_get_multiple_bssid_beacon_len(&old->multiple_bssid);
++
++ size = sizeof(*new) + new_head_len + new_tail_len + new_multiple_bssid_len;
+
+ new = kzalloc(size, GFP_KERNEL);
+ if (!new)
+@@ -975,6 +1009,18 @@ static int ieee80211_assign_beacon(struc
+ new->head_len = new_head_len;
+ new->tail_len = new_tail_len;
+
++ new_multiple_bssid_offset = new->tail + new_tail_len;
++
++ /* copy in optional multiple_bssid_ies */
++ if (params->multiple_bssid.cnt)
++ ieee80211_copy_multiple_bssid_beacon(new_multiple_bssid_offset,
++ &new->multiple_bssid,
++ ¶ms->multiple_bssid);
++ else if (old && old->multiple_bssid.cnt)
++ ieee80211_copy_multiple_bssid_beacon(new_multiple_bssid_offset,
++ &new->multiple_bssid,
++ &old->multiple_bssid);
++
+ if (csa) {
+ new->csa_current_counter = csa->count;
+ memcpy(new->csa_counter_offsets, csa->counter_offsets_beacon,
+@@ -3011,7 +3057,8 @@ 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;
++ beacon->probe_resp_len + beacon->lci_len + beacon->civicloc_len +
++ ieee80211_get_multiple_bssid_beacon_len(&beacon->multiple_bssid);
+
+ new_beacon = kzalloc(sizeof(*new_beacon) + len, GFP_KERNEL);
+ if (!new_beacon)
+@@ -3054,6 +3101,10 @@ cfg80211_beacon_dup(struct cfg80211_beac
+ memcpy(pos, beacon->probe_resp, beacon->probe_resp_len);
+ pos += beacon->probe_resp_len;
+ }
++ if (beacon->multiple_bssid.cnt)
++ pos = ieee80211_copy_multiple_bssid_beacon(pos,
++ &new_beacon->multiple_bssid,
++ &beacon->multiple_bssid);
+
+ /* might copy -1, meaning no changes requested */
+ new_beacon->ftm_responder = beacon->ftm_responder;
+Index: backports-5.8-1/net/mac80211/ieee80211_i.h
+===================================================================
+--- backports-5.8-1.orig/net/mac80211/ieee80211_i.h
++++ backports-5.8-1/net/mac80211/ieee80211_i.h
+@@ -262,6 +262,8 @@ struct beacon_data {
+ struct ieee80211_meshconf_ie *meshconf;
+ u16 csa_counter_offsets[IEEE80211_MAX_CSA_COUNTERS_NUM];
+ u8 csa_current_counter;
++ struct cfg80211_multiple_bssid_data multiple_bssid;
++ u16 ema_index;
+ struct rcu_head rcu_head;
+ };
+
+Index: backports-5.8-1/net/mac80211/tx.c
+===================================================================
+--- backports-5.8-1.orig/net/mac80211/tx.c
++++ backports-5.8-1/net/mac80211/tx.c
+@@ -4719,11 +4719,26 @@ static int ieee80211_beacon_protect(stru
+ return 0;
+ }
+
++static void
++ieee80211_beacon_add_multiple_bssid_config(struct ieee80211_vif *vif, struct sk_buff *skb,
++ struct cfg80211_multiple_bssid_data *config)
++{
++ u8 *pos = skb_put(skb, 6);
++
++ *pos++ = WLAN_EID_EXTENSION;
++ *pos++ = 4;
++ *pos++ = WLAN_EID_EXT_MULTIPLE_BSSID_CONFIGURATION;
++ *pos++ = 2;
++ *pos++ = vif->bss_conf.multiple_bssid.count;
++ *pos++ = config->cnt;
++}
++
+ 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 ieee80211_local *local = hw_to_local(hw);
+ struct beacon_data *beacon = NULL;
+@@ -4735,13 +4750,11 @@ __ieee80211_beacon_get(struct ieee80211_
+ struct ieee80211_chanctx_conf *chanctx_conf;
+ int csa_off_base = 0;
+
+- rcu_read_lock();
+-
+ sdata = vif_to_sdata(vif);
+ chanctx_conf = rcu_dereference(sdata->vif.chanctx_conf);
+
+ if (!ieee80211_sdata_running(sdata) || !chanctx_conf)
+- goto out;
++ return NULL;
+
+ if (offs)
+ memset(offs, 0, sizeof(*offs));
+@@ -4751,6 +4764,8 @@ __ieee80211_beacon_get(struct ieee80211_
+
+ beacon = rcu_dereference(ap->beacon);
+ if (beacon) {
++ int multiple_bssid_len = 0;
++
+ if (beacon->csa_counter_offsets[0]) {
+ if (!is_template)
+ __ieee80211_csa_update_counter(beacon);
+@@ -4758,6 +4773,27 @@ __ieee80211_beacon_get(struct ieee80211_
+ ieee80211_set_csa(sdata, beacon);
+ }
+
++ if (ema_index == IEEE80211_BCN_EMA_NEXT) {
++ ema_index = beacon->ema_index;
++ beacon->ema_index++;
++ beacon->ema_index %= beacon->multiple_bssid.cnt;
++ }
++
++ if (ema_index != IEEE80211_BCN_EMA_NONE &&
++ ema_index >= beacon->multiple_bssid.cnt)
++ return NULL;
++
++ if (beacon->multiple_bssid.cnt) {
++ if (ema_index >= IEEE80211_BCN_EMA_INDEX) {
++ multiple_bssid_len = beacon->multiple_bssid.len[ema_index];
++ } else {
++ int i;
++
++ for (i = 0; i < beacon->multiple_bssid.cnt; i++)
++ multiple_bssid_len = beacon->multiple_bssid.len[i];
++ }
++ }
++
+ /*
+ * headroom, head length,
+ * tail length and maximum TIM length
+@@ -4765,9 +4801,10 @@ __ieee80211_beacon_get(struct ieee80211_
+ skb = dev_alloc_skb(local->tx_headroom +
+ beacon->head_len +
+ beacon->tail_len + 256 +
+- local->hw.extra_beacon_tailroom);
++ local->hw.extra_beacon_tailroom +
++ multiple_bssid_len);
+ if (!skb)
+- goto out;
++ return NULL;
+
+ skb_reserve(skb, local->tx_headroom);
+ skb_put_data(skb, beacon->head, beacon->head_len);
+@@ -4783,21 +4820,38 @@ __ieee80211_beacon_get(struct ieee80211_
+ csa_off_base = skb->len;
+ }
+
++ if (multiple_bssid_len) {
++ if (ema_index >= IEEE80211_BCN_EMA_INDEX) {
++ ieee80211_beacon_add_multiple_bssid_config(vif, skb,
++ &beacon->multiple_bssid);
++ skb_put_data(skb, beacon->multiple_bssid.ies[beacon->ema_index],
++ beacon->multiple_bssid.len[beacon->ema_index]);
++ } else {
++ int i;
++
++ for (i = 0; i < beacon->multiple_bssid.cnt; i++)
++ skb_put_data(skb, beacon->multiple_bssid.ies[i],
++ beacon->multiple_bssid.len[i]);
++ }
++ if (offs)
++ offs->multiple_bssid_offset = skb->len - multiple_bssid_len;
++ }
++
+ if (beacon->tail)
+ skb_put_data(skb, beacon->tail,
+ beacon->tail_len);
+
+ if (ieee80211_beacon_protect(skb, local, sdata) < 0)
+- goto out;
++ return NULL;
+ } else
+- goto out;
++ return NULL;
+ } else if (sdata->vif.type == NL80211_IFTYPE_ADHOC) {
+ struct ieee80211_if_ibss *ifibss = &sdata->u.ibss;
+ struct ieee80211_hdr *hdr;
+
+ beacon = rcu_dereference(ifibss->presp);
+ if (!beacon)
+- goto out;
++ return NULL;
+
+ if (beacon->csa_counter_offsets[0]) {
+ if (!is_template)
+@@ -4809,7 +4863,7 @@ __ieee80211_beacon_get(struct ieee80211_
+ skb = dev_alloc_skb(local->tx_headroom + beacon->head_len +
+ local->hw.extra_beacon_tailroom);
+ if (!skb)
+- goto out;
++ return NULL;
+ skb_reserve(skb, local->tx_headroom);
+ skb_put_data(skb, beacon->head, beacon->head_len);
+
+@@ -4821,7 +4875,7 @@ __ieee80211_beacon_get(struct ieee80211_
+
+ beacon = rcu_dereference(ifmsh->beacon);
+ if (!beacon)
+- goto out;
++ return NULL;
+
+ if (beacon->csa_counter_offsets[0]) {
+ if (!is_template)
+@@ -4844,7 +4898,7 @@ __ieee80211_beacon_get(struct ieee80211_
+ beacon->tail_len +
+ local->hw.extra_beacon_tailroom);
+ if (!skb)
+- goto out;
++ return NULL;
+ skb_reserve(skb, local->tx_headroom);
+ skb_put_data(skb, beacon->head, beacon->head_len);
+ ieee80211_beacon_add_tim(sdata, &ifmsh->ps, skb, is_template);
+@@ -4857,7 +4911,7 @@ __ieee80211_beacon_get(struct ieee80211_
+ skb_put_data(skb, beacon->tail, beacon->tail_len);
+ } else {
+ WARN_ON(1);
+- goto out;
++ return NULL;
+ }
+
+ /* CSA offsets */
+@@ -4900,8 +4954,6 @@ __ieee80211_beacon_get(struct ieee80211_
+ info->flags |= IEEE80211_TX_CTL_CLEAR_PS_FILT |
+ IEEE80211_TX_CTL_ASSIGN_SEQ |
+ IEEE80211_TX_CTL_FIRST_FRAGMENT;
+- out:
+- rcu_read_unlock();
+ return skb;
+
+ }
+@@ -4911,16 +4963,86 @@ ieee80211_beacon_get_template(struct iee
+ struct ieee80211_vif *vif,
+ struct ieee80211_mutable_offsets *offs)
+ {
+- return __ieee80211_beacon_get(hw, vif, offs, true);
++ struct sk_buff *bcn;
++
++ rcu_read_lock();
++ bcn = __ieee80211_beacon_get(hw, vif, offs, true,
++ IEEE80211_BCN_EMA_NONE);
++ rcu_read_unlock();
++
++ return bcn;
+ }
+ EXPORT_SYMBOL(ieee80211_beacon_get_template);
+
++struct sk_buff *
++ieee80211_beacon_get_template_ema_next(struct ieee80211_hw *hw,
++ struct ieee80211_vif *vif,
++ struct ieee80211_mutable_offsets *offs)
++{
++ struct sk_buff *bcn;
++
++ rcu_read_lock();
++ bcn = __ieee80211_beacon_get(hw, vif, offs, true,
++ IEEE80211_BCN_EMA_NEXT);
++ rcu_read_unlock();
++
++ return bcn;
++}
++EXPORT_SYMBOL(ieee80211_beacon_get_template_ema_next);
++
++void
++ieee80211_beacon_free_ema_list(struct list_head *head)
++{
++ struct ieee80211_ema_bcn_list *ema, *tmp;
++
++ list_for_each_entry_safe(ema, tmp, head, list) {
++ kfree_skb(ema->skb);
++ kfree(ema);
++ }
++}
++EXPORT_SYMBOL(ieee80211_beacon_free_ema_list);
++
++int
++ieee80211_beacon_get_template_ema_list(struct ieee80211_hw *hw,
++ struct ieee80211_vif *vif,
++ struct list_head *head)
++{
++ int cnt = 0;
++
++ rcu_read_lock();
++ while (true) {
++ struct ieee80211_ema_bcn_list *ema;
++
++ ema = kmalloc(sizeof(*ema), GFP_KERNEL);
++ if (!ema) {
++ ieee80211_beacon_free_ema_list(head);
++ cnt = 0;
++ goto out;
++ }
++
++ ema->skb = __ieee80211_beacon_get(hw, vif, &ema->offs, true,
++ cnt);
++ if (!ema->skb) {
++ kfree(ema);
++ break;
++ }
++ list_add_tail(&ema->list, head);
++ cnt++;
++ }
++out:
++ rcu_read_unlock();
++
++ return cnt;
++}
++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,
++ IEEE80211_BCN_EMA_NONE);
+ struct sk_buff *copy;
+ struct ieee80211_supported_band *sband;
+ int shift;