From e434254946c64bb27d7b202c0eeac03e49c166aa Mon Sep 17 00:00:00 2001
From: Johannes Berg <johannes.berg@intel.com>
Date: Thu, 7 Jul 2022 15:28:14 +0200
Subject: [PATCH] wifi: mac80211: add a helper to fragment an element

The way this works is that you add all the element data,
keeping a pointer to the length field of the element.
Then call this helper function, which will fragment the
element if there was more than 255 bytes in the element,
memmove()ing the data back if needed.

Signed-off-by: Johannes Berg <johannes.berg@intel.com>
---
 net/mac80211/ieee80211_i.h |  1 +
 net/mac80211/util.c        | 28 ++++++++++++++++++++++++++++
 2 files changed, 29 insertions(+)

diff --git a/net/mac80211/ieee80211_i.h b/net/mac80211/ieee80211_i.h
index 74d5fc5889bb..d17d73e8d19f 100644
--- a/net/mac80211/ieee80211_i.h
+++ b/net/mac80211/ieee80211_i.h
@@ -2193,6 +2193,7 @@ ieee802_11_parse_elems(const u8 *start, size_t len, bool action,
 	return ieee802_11_parse_elems_crc(start, len, action, 0, 0, bss);
 }
 
+void ieee80211_fragment_element(struct sk_buff *skb, u8 *len_pos);
 
 extern const int ieee802_1d_to_ac[8];
 
diff --git a/net/mac80211/util.c b/net/mac80211/util.c
index 6d6ba23aa074..2ff8d1ec564c 100644
--- a/net/mac80211/util.c
+++ b/net/mac80211/util.c
@@ -4780,3 +4780,31 @@ u8 *ieee80211_ie_build_eht_cap(u8 *pos,
 
 	return pos;
 }
+
+void ieee80211_fragment_element(struct sk_buff *skb, u8 *len_pos)
+{
+	unsigned int elem_len;
+
+	if (!len_pos)
+		return;
+
+	elem_len = skb->data + skb->len - len_pos - 1;
+
+	while (elem_len > 255) {
+		/* this one is 255 */
+		*len_pos = 255;
+		/* remaining data gets smaller */
+		elem_len -= 255;
+		/* make space for the fragment ID/len in SKB */
+		skb_put(skb, 2);
+		/* shift back the remaining data to place fragment ID/len */
+		memmove(len_pos + 255 + 3, len_pos + 255 + 1, elem_len);
+		/* place the fragment ID */
+		len_pos += 255 + 1;
+		*len_pos = WLAN_EID_FRAGMENT;
+		/* and point to fragment length to update later */
+		len_pos++;
+	}
+
+	*len_pos = elem_len;
+}