Received-SPF: pass (google.com: domain of linux-wireless-owner@vger.kernel.org designates 2620:137:e000::1:20 as permitted sender) client-ip=2620:137:e000::1:20;
From:   gregory.greenman@intel.com
To:     johannes@sipsolutions.net
Cc:     linux-wireless@vger.kernel.org,
        Benjamin Berg <benjamin.berg@intel.com>,
        Gregory Greenman <gregory.greenman@intel.com>
Subject: [PATCH 16/20] wifi: cfg80211: add element defragmentation helper
Date:   Fri, 16 Jun 2023 09:54:05 +0300
Message-Id: <20230616094949.29c3ebeed10d.I009c049289dd0162c2e858ed8b68d2875a672ed6@changeid>
In-Reply-To: <20230616065409.633290-1-gregory.greenman@intel.com>
References: <20230616065409.633290-1-gregory.greenman@intel.com>
MIME-Version: 1.0
Content-Transfer-Encoding: 8bit
Precedence: bulk

From: Benjamin Berg <benjamin.berg@intel.com>

This is already needed within mac80211 and support is also needed by
cfg80211 to parse ML elements.

Signed-off-by: Benjamin Berg <benjamin.berg@intel.com>
Signed-off-by: Gregory Greenman <gregory.greenman@intel.com>
---
 include/net/cfg80211.h | 22 ++++++++++++++++
 net/wireless/scan.c    | 60 ++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 82 insertions(+)

diff --git a/include/net/cfg80211.h b/include/net/cfg80211.h
index 94ca5cb340f6..cf14a93e61eb 100644
--- a/include/net/cfg80211.h
+++ b/include/net/cfg80211.h
@@ -6664,6 +6664,28 @@ cfg80211_find_vendor_ie(unsigned int oui, int oui_type,
 	return (const void *)cfg80211_find_vendor_elem(oui, oui_type, ies, len);
 }
 
+/**
+ * cfg80211_defragment_element - Defrag the given element data into a buffer
+ *
+ * @elem: the element to defragment
+ * @ies: elements where @elem is contained
+ * @ieslen: length of @ies
+ * @data: buffer to store element data
+ * @data_len: length of @data
+ * @frag_id: the element ID of fragments
+ *
+ * Return: length of @data, or -EINVAL on error
+ *
+ * Copy out all data from an element that may be fragmented into @data, while
+ * skipping all headers.
+ *
+ * The function uses memmove() internally. It is acceptable to defragment an
+ * element in-place.
+ */
+ssize_t cfg80211_defragment_element(const struct element *elem, const u8 *ies,
+				    size_t ieslen, u8 *data, size_t data_len,
+				    u8 frag_id);
+
 /**
  * cfg80211_send_layer2_update - send layer 2 update frame
  *
diff --git a/net/wireless/scan.c b/net/wireless/scan.c
index 75e6e032bb3a..dc71c6ac5bf5 100644
--- a/net/wireless/scan.c
+++ b/net/wireless/scan.c
@@ -2288,6 +2288,66 @@ static void cfg80211_parse_mbssid_data(struct wiphy *wiphy,
 	kfree(profile);
 }
 
+ssize_t cfg80211_defragment_element(const struct element *elem, const u8 *ies,
+				    size_t ieslen, u8 *data, size_t data_len,
+				    u8 frag_id)
+{
+	const struct element *next;
+	ssize_t copied;
+	u8 elem_datalen;
+
+	if (!elem)
+		return -EINVAL;
+
+	/* elem might be invalid after the memmove */
+	next = (void *)(elem->data + elem->datalen);
+
+	elem_datalen = elem->datalen;
+	if (elem->id == WLAN_EID_EXTENSION) {
+		copied = elem->datalen - 1;
+		if (copied > data_len)
+			return -ENOSPC;
+
+		memmove(data, elem->data + 1, copied);
+	} else {
+		copied = elem->datalen;
+		if (copied > data_len)
+			return -ENOSPC;
+
+		memmove(data, elem->data, copied);
+	}
+
+	/* Fragmented elements must have 255 bytes */
+	if (elem_datalen < 255)
+		return copied;
+
+	for (elem = next;
+	     elem->data < ies + ieslen &&
+		elem->data + elem->datalen < ies + ieslen;
+	     elem = next) {
+		/* elem might be invalid after the memmove */
+		next = (void *)(elem->data + elem->datalen);
+
+		if (elem->id != frag_id)
+			break;
+
+		elem_datalen = elem->datalen;
+
+		if (copied + elem_datalen > data_len)
+			return -ENOSPC;
+
+		memmove(data + copied, elem->data, elem_datalen);
+		copied += elem_datalen;
+
+		/* Only the last fragment may be short */
+		if (elem_datalen != 255)
+			break;
+	}
+
+	return copied;
+}
+EXPORT_SYMBOL(cfg80211_defragment_element);
+
 struct cfg80211_bss *
 cfg80211_inform_bss_data(struct wiphy *wiphy,
 			 struct cfg80211_inform_bss *data,
-- 
2.38.1