From 907a2fd7b9e7a9f0931ad6b5b97365203d9136d2 Mon Sep 17 00:00:00 2001
From: James Prestwood <prestwoj@gmail.com>
Date: Wed, 3 Aug 2022 14:36:34 -0700
Subject: [PATCH] nl80211util: add nl80211_parse_supported_frequencies

A helper function to parse supported and disabled frequencies.
---
 src/nl80211util.c | 44 ++++++++++++++++++++++++++++++++++++++++++++
 src/nl80211util.h |  4 ++++
 2 files changed, 48 insertions(+)

diff --git a/src/nl80211util.c b/src/nl80211util.c
index 9092b204..8fe86ee6 100644
--- a/src/nl80211util.c
+++ b/src/nl80211util.c
@@ -32,6 +32,7 @@
 
 #include "src/nl80211util.h"
 #include "src/band.h"
+#include "src/util.h"
 
 typedef bool (*attr_handler)(const void *data, uint16_t len, void *o);
 
@@ -484,3 +485,46 @@ int nl80211_parse_chandef(struct l_genl_msg *msg, struct band_chandef *out)
 	memcpy(out, &t, sizeof(t));
 	return 0;
 }
+
+int nl80211_parse_supported_frequencies(struct l_genl_attr *band_freqs,
+					struct scan_freq_set *supported_list,
+					struct scan_freq_set *disabled_list)
+{
+	uint16_t type, len;
+	const void *data;
+	struct l_genl_attr attr;
+	struct l_genl_attr nested;
+
+	if (!l_genl_attr_recurse(band_freqs, &nested))
+		return -EBADMSG;
+
+	while (l_genl_attr_next(&nested, NULL, NULL, NULL)) {
+		uint32_t freq = 0;
+		bool disabled = false;
+
+		if (!l_genl_attr_recurse(&nested, &attr))
+			continue;
+
+		while (l_genl_attr_next(&attr, &type, &len, &data)) {
+			switch (type) {
+			case NL80211_FREQUENCY_ATTR_FREQ:
+				freq = *((uint32_t *) data);
+				break;
+			case NL80211_FREQUENCY_ATTR_DISABLED:
+				disabled = true;
+				break;
+			}
+		}
+
+		if (!freq)
+			continue;
+
+		if (supported_list)
+			scan_freq_set_add(supported_list, freq);
+
+		if (disabled && disabled_list)
+			scan_freq_set_add(disabled_list, freq);
+	}
+
+	return 0;
+}
diff --git a/src/nl80211util.h b/src/nl80211util.h
index 01777195..1d32b700 100644
--- a/src/nl80211util.h
+++ b/src/nl80211util.h
@@ -23,6 +23,7 @@
 #include <ell/ell.h>
 
 struct band_chandef;
+struct scan_freq_set;
 
 int nl80211_parse_attrs(struct l_genl_msg *msg, int tag, ...);
 
@@ -55,3 +56,6 @@ struct l_genl_msg *nl80211_build_cmd_frame(uint32_t ifindex,
 						size_t iov_len);
 
 int nl80211_parse_chandef(struct l_genl_msg *msg, struct band_chandef *out);
+int nl80211_parse_supported_frequencies(struct l_genl_attr *band_freqs,
+					struct scan_freq_set *supported,
+					struct scan_freq_set *disabled);