From 2b489947c21743005b354596cd1f726b5353b865 Mon Sep 17 00:00:00 2001
From: Andreas Schultz <aschultz@tpip.net>
Date: Thu, 3 Mar 2016 16:44:15 +0100
Subject: [PATCH] add support for setting the STA WME fields from WMM

---
 src/common/binding/ieee80211/ieee80211.c | 13 ++++++++++++
 src/common/binding/ieee80211/ieee80211.h | 27 ++++++++++++++++++++++++
 src/wtp/binding/ieee80211/wifi_drivers.c |  5 +++++
 src/wtp/binding/ieee80211/wifi_drivers.h |  2 ++
 src/wtp/binding/ieee80211/wifi_nl80211.c | 16 ++++++++++++++
 src/wtp/binding/ieee80211/wifi_nl80211.h |  4 ++++
 6 files changed, 67 insertions(+)

diff --git a/src/common/binding/ieee80211/ieee80211.c b/src/common/binding/ieee80211/ieee80211.c
index aaff719..4380cf1 100644
--- a/src/common/binding/ieee80211/ieee80211.c
+++ b/src/common/binding/ieee80211/ieee80211.c
@@ -193,6 +193,19 @@ int ieee80211_retrieve_information_elements_position(struct ieee80211_ie_items*
 		case IEEE80211_IE_SSID_LIST:
 			items->ssid_list = (struct ieee80211_ie_ssid_list *)data;
 			break;
+
+		case IEEE80211_IE_VENDOR_SPECIFIC: {
+			struct ieee80211_ie_vendor_specific *vs =
+				(struct ieee80211_ie_vendor_specific *)data;
+			uint32_t oui = vs->oui[0] << 16 | vs->oui[1] << 8 | vs->oui[2];
+
+			if (oui == MICROSOFT_OUI &&
+			    vs->oui_type == WMM_TYPE &&
+			    vs->oui_subtype == WMM_INFORMATION_ELEMENT) {
+				items->wmm_ie = (struct ieee80211_ie_wmm_information_element *)data;
+				break;
+			}
+		}
 		}
 
 		/* Next Information Element */
diff --git a/src/common/binding/ieee80211/ieee80211.h b/src/common/binding/ieee80211/ieee80211.h
index df99697..4e6352a 100644
--- a/src/common/binding/ieee80211/ieee80211.h
+++ b/src/common/binding/ieee80211/ieee80211.h
@@ -474,6 +474,32 @@ struct ieee80211_ie_ssid_list {
 	uint8_t lists[0];
 } STRUCT_PACKED;
 
+/* 802.11 Vendor Specific */
+#define IEEE80211_IE_VENDOR_SPECIFIC	221
+
+#define MICROSOFT_OUI			0x0050F2
+struct ieee80211_ie_vendor_specific {
+	uint8_t id;
+	uint8_t len;
+	uint8_t oui[3];
+	uint8_t oui_type;
+	int8_t oui_subtype;
+} STRUCT_PACKED;
+
+#define WMM_TYPE			2
+#define WMM_INFORMATION_ELEMENT		0
+#define WMM_PARAMETER_ELEMENT		1
+
+struct ieee80211_ie_wmm_information_element {
+	uint8_t id;
+	uint8_t len;
+	uint8_t oui[3];
+	uint8_t oui_type;
+	uint8_t oui_subtype;
+	uint8_t version;
+	uint8_t qos_info;
+} STRUCT_PACKED;
+
 /* 802.11 All information elements */
 struct ieee80211_ie_items {
 	struct ieee80211_ie_ssid *ssid;
@@ -487,6 +513,7 @@ struct ieee80211_ie_items {
 	struct ieee80211_ie_qos_capability *qos_capability;
 	struct ieee80211_ie_power_constraint *power_constraint;
 	struct ieee80211_ie_ssid_list *ssid_list;
+	struct ieee80211_ie_wmm_information_element *wmm_ie;
 };
 
 /* IEEE 802.11 functions */
diff --git a/src/wtp/binding/ieee80211/wifi_drivers.c b/src/wtp/binding/ieee80211/wifi_drivers.c
index 1829de6..1ceead5 100644
--- a/src/wtp/binding/ieee80211/wifi_drivers.c
+++ b/src/wtp/binding/ieee80211/wifi_drivers.c
@@ -694,6 +694,11 @@ static void wifi_wlan_receive_station_mgmt_association_request(struct wifi_wlan*
 	/* */
 	capwap_logging_info("Receive IEEE802.11 Association Request from %s station", station->addrtext);
 
+	if (ieitems.wmm_ie != NULL && ieitems.wmm_ie->version == 1) {
+		station->flags |= WIFI_STATION_FLAGS_WMM;
+		station->qosinfo = ieitems.wmm_ie->qos_info;
+	}
+
 	/* */
 	if (wlan->macmode == CAPWAP_ADD_WLAN_MACMODE_LOCAL) {
 		/* Verify SSID */
diff --git a/src/wtp/binding/ieee80211/wifi_drivers.h b/src/wtp/binding/ieee80211/wifi_drivers.h
index 82ee720..6569bd4 100644
--- a/src/wtp/binding/ieee80211/wifi_drivers.h
+++ b/src/wtp/binding/ieee80211/wifi_drivers.h
@@ -360,6 +360,8 @@ struct wifi_station {
 
 	/* Authentication */
 	uint16_t authalgorithm;
+
+	uint8_t qosinfo;
 };
 
 /* */
diff --git a/src/wtp/binding/ieee80211/wifi_nl80211.c b/src/wtp/binding/ieee80211/wifi_nl80211.c
index 8d57d69..2434656 100644
--- a/src/wtp/binding/ieee80211/wifi_nl80211.c
+++ b/src/wtp/binding/ieee80211/wifi_nl80211.c
@@ -1045,8 +1045,24 @@ int nl80211_station_authorize(struct wifi_wlan* wlan, struct wifi_station* stati
 	memset(&flagstation, 0, sizeof(struct nl80211_sta_flag_update));
 	flagstation.mask = nl80211_station_get_flags(station);
 	flagstation.set = flagstation.mask;
+        log_printf(LOG_DEBUG, "  * flags set=0x%x mask=0x%x",
+                   flagstation.set, flagstation.mask);
 	nla_put(msg, NL80211_ATTR_STA_FLAGS2, sizeof(struct nl80211_sta_flag_update), &flagstation);
 
+	if (station->flags & WIFI_STATION_FLAGS_WMM) {
+		struct nlattr *wme;
+
+		log_printf(LOG_DEBUG, "  * qosinfo=0x%x", station->qosinfo);
+
+		wme = nla_nest_start(msg, NL80211_ATTR_STA_WME);
+		nla_put_u8(msg, NL80211_STA_WME_UAPSD_QUEUES,
+			   station->qosinfo & WMM_QOSINFO_STA_AC_MASK);
+		nla_put_u8(msg, NL80211_STA_WME_MAX_SP,
+			   (station->qosinfo >> WMM_QOSINFO_STA_SP_SHIFT) &
+			   WMM_QOSINFO_STA_SP_MASK);
+		nla_nest_end(msg, wme);
+	}
+
 	/* */
 	result = nl80211_send_and_recv_msg(wlanhandle->devicehandle->globalhandle, msg, NULL, NULL);
 	if (result) {
diff --git a/src/wtp/binding/ieee80211/wifi_nl80211.h b/src/wtp/binding/ieee80211/wifi_nl80211.h
index a681093..9687b91 100644
--- a/src/wtp/binding/ieee80211/wifi_nl80211.h
+++ b/src/wtp/binding/ieee80211/wifi_nl80211.h
@@ -9,6 +9,10 @@
 #define nl_sock nl_handle
 #endif
 
+#define WMM_QOSINFO_STA_AC_MASK 0x0f
+#define WMM_QOSINFO_STA_SP_MASK 0x03
+#define WMM_QOSINFO_STA_SP_SHIFT 5
+
 /* */
 typedef int (*nl_valid_cb)(struct nl_msg* msg, void* data);