[PATCH v2 4/4] wifi: mac80211: support setting S1G short beacon period and tail

[Gilad Itzkovitch <gilad.itzkovitch@xxxxxxxxxxxxxx>]

From: Kieran Frewen <kieran.frewen@xxxxxxxxxxxxxx>

With the kernel able to send both short and long S1G beacons, include
the ability for setting the short beacon period.
It also adds the support for distinguish short beacon tail.

Signed-off-by: Kieran Frewen <kieran.frewen@xxxxxxxxxxxxxx>
Co-developed-by: Gilad Itzkovitch <gilad.itzkovitch@xxxxxxxxxxxxxx>
Signed-off-by: Gilad Itzkovitch <gilad.itzkovitch@xxxxxxxxxxxxxx>
---
 net/mac80211/cfg.c         | 22 +++++++++++++++++++---
 net/mac80211/ieee80211_i.h |  4 ++--
 net/mac80211/tx.c          |  4 +++-
 3 files changed, 24 insertions(+), 6 deletions(-)

diff --git a/net/mac80211/cfg.c b/net/mac80211/cfg.c
index a42abaa25273..09905f827b29 100644
--- a/net/mac80211/cfg.c
+++ b/net/mac80211/cfg.c
@@ -1091,7 +1091,7 @@ static int ieee80211_assign_beacon(struct ieee80211_sub_if_data *sdata,
 {
 	struct cfg80211_mbssid_elems *mbssid = NULL;
 	struct beacon_data *new, *old;
-	int new_head_len, new_tail_len;
+	int new_head_len, new_tail_len, new_short_tail_len;
 	int size, err;
 	u32 changed = BSS_CHANGED_BEACON;
 	struct ieee80211_bss_conf *link_conf = link->conf;
@@ -1115,7 +1115,13 @@ 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;
+	if (params->short_tail || !old)
+	       /* params->tail_len will be zero for !params->tail */
+		new_short_tail_len = params->short_tail_len;
+	else
+		new_short_tail_len = old->short_tail_len;
+
+	size = sizeof(*new) + new_head_len + new_tail_len + new_short_tail_len;
 
 	/* new or old multiple BSSID elements? */
 	if (params->mbssid_ies) {
@@ -1142,9 +1148,12 @@ static int ieee80211_assign_beacon(struct ieee80211_sub_if_data *sdata,
 	new->tail = new->head + new_head_len;
 	new->head_len = new_head_len;
 	new->tail_len = new_tail_len;
+	new->short_tail = new->tail + new->tail_len;
+	new->short_tail_len = new_short_tail_len;
+
 	/* copy in optional mbssid_ies */
 	if (mbssid) {
-		u8 *pos = new->tail + new->tail_len;
+		u8 *pos = new->short_tail + new->short_tail_len;
 
 		new->mbssid_ies = (void *)pos;
 		pos += struct_size(new->mbssid_ies, elem, mbssid->cnt);
@@ -1177,6 +1186,13 @@ static int ieee80211_assign_beacon(struct ieee80211_sub_if_data *sdata,
 		if (old)
 			memcpy(new->tail, old->tail, new_tail_len);
 
+	/* copy in optional short tail */
+	if (params->short_tail)
+		memcpy(new->short_tail, params->short_tail, new_short_tail_len);
+	else
+		if (old)
+			memcpy(new->short_tail, old->short_tail, new_short_tail_len);
+
 	err = ieee80211_set_probe_resp(sdata, params->probe_resp,
 				       params->probe_resp_len, csa, cca, link);
 	if (err < 0) {
diff --git a/net/mac80211/ieee80211_i.h b/net/mac80211/ieee80211_i.h
index 2e1d829c548a..ef58e6f74b52 100644
--- a/net/mac80211/ieee80211_i.h
+++ b/net/mac80211/ieee80211_i.h
@@ -263,8 +263,8 @@ struct ieee80211_color_change_settings {
 };
 
 struct beacon_data {
-	u8 *head, *tail;
-	int head_len, tail_len;
+	u8 *head, *tail, *short_tail;
+	int head_len, tail_len, short_tail_len;
 	struct ieee80211_meshconf_ie *meshconf;
 	u16 cntdwn_counter_offsets[IEEE80211_MAX_CNTDWN_COUNTERS_NUM];
 	u8 cntdwn_current_counter;
diff --git a/net/mac80211/tx.c b/net/mac80211/tx.c
index a12b32544a3a..b75817915def 100644
--- a/net/mac80211/tx.c
+++ b/net/mac80211/tx.c
@@ -5165,7 +5165,9 @@ ieee80211_beacon_get_ap(struct ieee80211_hw *hw,
 		csa_off_base = skb->len;
 	}
 
-	if (beacon->tail && !is_short)
+	if (beacon->short_tail && is_short)
+		skb_put_data(skb, beacon->short_tail, beacon->short_tail_len);
+	else if (beacon->tail && !is_short)
 		skb_put_data(skb, beacon->tail, beacon->tail_len);
 
 	if (ieee80211_beacon_protect(skb, local, sdata, link) < 0)
-- 
2.34.1