From a4aae9f9b8483cee92e78ec72dc337017a99e690 Mon Sep 17 00:00:00 2001
From: Veerendranath Jakkam <vjakkam@codeaurora.org>
Date: Tue, 31 Aug 2021 09:37:09 +0530
Subject: [PATCH] DSCP: Indicate DSCP Policy support in (Re)Association Request
 frame

Indicate DSCP Policy capability by including a WFA Capabilities element
containing the relevant bit set to 1 in the (Re)Association Request
frames when enabled by user.

Signed-off-by: Veerendranath Jakkam <vjakkam@codeaurora.org>
---
 src/common/ieee802_11_defs.h    |  6 ++++
 wpa_supplicant/wpa_supplicant.c | 51 +++++++++++++++++++++++++++++++++
 2 files changed, 57 insertions(+)

diff --git a/src/common/ieee802_11_defs.h b/src/common/ieee802_11_defs.h
index 971e046e0..24dbfa8bd 100644
--- a/src/common/ieee802_11_defs.h
+++ b/src/common/ieee802_11_defs.h
@@ -1363,6 +1363,8 @@ struct ieee80211_ampe_ie {
 #define SAE_PK_OUI_TYPE 0x1f
 #define QM_IE_VENDOR_TYPE 0x506f9a22
 #define QM_IE_OUI_TYPE 0x22
+#define WFA_CAPA_IE_VENDOR_TYPE 0x506f9a23
+#define WFA_CAPA_OUI_TYPE 0x23
 
 #define MULTI_AP_SUB_ELEM_TYPE 0x06
 #define MULTI_AP_TEAR_DOWN BIT(4)
@@ -2475,4 +2477,8 @@ enum dscp_policy_request_type {
 #define DSCP_POLICY_CTRL_MORE	BIT(0)
 #define DSCP_POLICY_CTRL_RESET	BIT(1)
 
+/* Wi-Fi Alliance Capabilities element - Capabilities field */
+#define WFA_CAPA_QM_DSCP_POLICY BIT(0)
+#define WFA_CAPA_QM_UNSOLIC_DSCP BIT(1)
+
 #endif /* IEEE802_11_DEFS_H */
diff --git a/wpa_supplicant/wpa_supplicant.c b/wpa_supplicant/wpa_supplicant.c
index e0723a2b3..1e5f6bcc7 100644
--- a/wpa_supplicant/wpa_supplicant.c
+++ b/wpa_supplicant/wpa_supplicant.c
@@ -2803,6 +2803,54 @@ int wpa_is_fils_sk_pfs_supported(struct wpa_supplicant *wpa_s)
 #endif /* CONFIG_FILS */
 
 
+static int wpas_populate_wfa_capa(struct wpa_supplicant *wpa_s,
+				  struct wpa_bss *bss,
+				  u8 *wpa_ie, size_t wpa_ie_len,
+				  size_t max_wpa_ie_len)
+{
+	struct wpabuf *wfa_ie = NULL;
+	u8 wfa_capa[1];
+	size_t wfa_ie_len, buf_len;
+
+	os_memset(wfa_capa, 0, sizeof(wfa_capa));
+	if (wpa_s->enable_dscp_policy_capa)
+		wfa_capa[0] |= WFA_CAPA_QM_DSCP_POLICY;
+
+	if (!wfa_capa[0])
+		return wpa_ie_len;
+
+	/* Wi-Fi Alliance element */
+	buf_len = 1 +	/* Element ID */
+		  1 +	/* Length */
+		  3 +	/* OUI */
+		  1 +	/* OUI Type */
+		  1 +	/* Capabilities Length */
+		  sizeof(wfa_capa);	/* Capabilities */
+	wfa_ie = wpabuf_alloc(buf_len);
+	if (!wfa_ie)
+		return wpa_ie_len;
+
+	wpabuf_put_u8(wfa_ie, WLAN_EID_VENDOR_SPECIFIC);
+	wpabuf_put_u8(wfa_ie, buf_len - 2);
+	wpabuf_put_be24(wfa_ie, OUI_WFA);
+	wpabuf_put_u8(wfa_ie, WFA_CAPA_OUI_TYPE);
+	wpabuf_put_u8(wfa_ie, sizeof(wfa_capa));
+	wpabuf_put_data(wfa_ie, wfa_capa, sizeof(wfa_capa));
+
+	wfa_ie_len = wpabuf_len(wfa_ie);
+	if (wpa_ie_len + wfa_ie_len <= max_wpa_ie_len) {
+		wpa_hexdump_buf(MSG_MSGDUMP, "WFA Capabilities element",
+				wfa_ie);
+		os_memcpy(wpa_ie + wpa_ie_len, wpabuf_head(wfa_ie),
+			  wfa_ie_len);
+		wpa_ie_len += wfa_ie_len;
+	}
+
+	wpabuf_free(wfa_ie);
+	return wpa_ie_len;
+}
+
+
 static u8 * wpas_populate_assoc_ies(
 	struct wpa_supplicant *wpa_s,
 	struct wpa_bss *bss, struct wpa_ssid *ssid,
@@ -3280,6 +3328,9 @@ pfs_fail:
 	}
 mscs_end:
 
+	wpa_ie_len = wpas_populate_wfa_capa(wpa_s, bss, wpa_ie, wpa_ie_len,
+					    max_wpa_ie_len);
+
 	if (ssid->multi_ap_backhaul_sta) {
 		size_t multi_ap_ie_len;