[PATCH 13/13] mbssid: add nl80211 support

[Aloka Dixit]

From: John Crispin <john at phrozen.org>

Send MBSSID and EMA configuration parameters to kernel.

Signed-off-by: John Crispin <john at phrozen.org>
Co-developed-by: Aloka Dixit <quic_alokad at quicinc.com>
Signed-off-by: Aloka Dixit <quic_alokad at quicinc.com>
---
 src/drivers/driver_nl80211.c      | 53 ++++++++++++++++++++++++++++++-
 src/drivers/driver_nl80211_capa.c | 26 +++++++++++++++
 2 files changed, 78 insertions(+), 1 deletion(-)

diff --git a/src/drivers/driver_nl80211.c b/src/drivers/driver_nl80211.c
index 0b3c79411099..8dfb3224e06e 100644
--- a/src/drivers/driver_nl80211.c
+++ b/src/drivers/driver_nl80211.c
@@ -4533,6 +4533,55 @@ static int nl80211_unsol_bcast_probe_resp(struct i802_bss *bss,
 #endif /* CONFIG_IEEE80211AX */
 
 
+static int nl80211_mbssid(struct nl_msg *msg,
+			 struct wpa_driver_ap_params *params)
+{
+	struct nlattr *config, *elems;
+	int ifidx;
+
+	config = nla_nest_start(msg, NL80211_ATTR_MBSSID_CONFIG);
+	if (!config)
+		goto fail;
+
+	nla_put_u8(msg, NL80211_MBSSID_CONFIG_ATTR_INDEX, params->mbssid_index);
+	if (params->mbssid_tx_iface) {
+		ifidx = if_nametoindex(params->mbssid_tx_iface);
+		if (ifidx <= 0)
+			goto fail;
+		nla_put_u32(msg,
+			    NL80211_MBSSID_CONFIG_ATTR_TX_IFINDEX,
+			    ifidx);
+	}
+
+	if (params->ema)
+		nla_put_flag(msg, NL80211_MBSSID_CONFIG_ATTR_EMA);
+
+	nla_nest_end(msg, config);
+
+	if (params->mbssid_elem_count && params->mbssid_elem_len &&
+	    params->mbssid_elem_offset && *params->mbssid_elem_offset) {
+		u8 i, **offs = params->mbssid_elem_offset;
+
+		elems = nla_nest_start(msg, NL80211_ATTR_MBSSID_ELEMS);
+		if (!elems)
+			goto fail;
+
+		for (i = 0; i < params->mbssid_elem_count - 1; i++)
+			nla_put(msg, i + 1, offs[i + 1] - offs[i], offs[i]);
+
+		nla_put(msg, i + 1,
+			*offs + params->mbssid_elem_len - offs[i],
+			offs[i]);
+
+		nla_nest_end(msg, elems);
+	}
+	return 0;
+
+fail:
+	return -1;
+}
+
+
 static int wpa_driver_nl80211_set_ap(void *priv,
 				     struct wpa_driver_ap_params *params)
 {
@@ -4841,6 +4890,9 @@ static int wpa_driver_nl80211_set_ap(void *priv,
 		goto fail;
 #endif /* CONFIG_FILS */
 
+	if (params->mbssid_count && nl80211_mbssid(msg, params) < 0)
+		goto fail;
+
 	ret = send_and_recv_msgs_connect_handle(drv, msg, bss, 1);
 	if (ret) {
 		wpa_printf(MSG_DEBUG, "nl80211: Beacon set failed: %d (%s)",
@@ -5439,7 +5491,6 @@ const char * nl80211_iftype_str(enum nl80211_iftype mode)
 	}
 }
 
-
 static int nl80211_create_iface_once(struct wpa_driver_nl80211_data *drv,
 				     const char *ifname,
 				     enum nl80211_iftype iftype,
diff --git a/src/drivers/driver_nl80211_capa.c b/src/drivers/driver_nl80211_capa.c
index 200c545c2b70..5a0760a4a5ef 100644
--- a/src/drivers/driver_nl80211_capa.c
+++ b/src/drivers/driver_nl80211_capa.c
@@ -846,6 +846,29 @@ err:
 }
 
 
+static void wiphy_info_mbssid(struct wpa_driver_capa *cap, struct nlattr *attr)
+{
+	struct nlattr *config[NL80211_MBSSID_CONFIG_ATTR_MAX + 1];
+
+	if (nla_parse_nested(config, NL80211_MBSSID_CONFIG_ATTR_MAX, attr,
+			     NULL))
+		return;
+
+	if (!config[NL80211_MBSSID_CONFIG_ATTR_MAX_INTERFACES])
+		return;
+
+	cap->mbssid_max_interfaces =
+		nla_get_u8(config[NL80211_MBSSID_CONFIG_ATTR_MAX_INTERFACES]);
+
+	if (config[NL80211_MBSSID_CONFIG_ATTR_MAX_EMA_PROFILE_PERIODICITY])
+		cap->ema_max_periodicity =
+		nla_get_u8(config[NL80211_MBSSID_CONFIG_ATTR_MAX_EMA_PROFILE_PERIODICITY]);
+
+	wpa_printf(MSG_DEBUG,
+		   "multiple_bssid: max interfaces %u, max profile periodicity %u\n",
+		   cap->mbssid_max_interfaces, cap->ema_max_periodicity);
+}
+
 static int wiphy_info_handler(struct nl_msg *msg, void *arg)
 {
 	struct nlattr *tb[NL80211_ATTR_MAX + 1];
@@ -1077,6 +1100,9 @@ static int wiphy_info_handler(struct nl_msg *msg, void *arg)
 	if (tb[NL80211_ATTR_WIPHY_SELF_MANAGED_REG])
 		capa->flags |= WPA_DRIVER_FLAGS_SELF_MANAGED_REGULATORY;
 
+	if (tb[NL80211_ATTR_MBSSID_CONFIG])
+		wiphy_info_mbssid(capa, tb[NL80211_ATTR_MBSSID_CONFIG]);
+
 	return NL_SKIP;
 }
 
-- 
2.31.1