ath6kl: Add WSC IE on the associate message

For some WPS test items, such as item "5.1.14"
STAUT must include the WSC IE in the 802.11 Association Request frame.
Therefore, add the corresponding IE in association message.

Signed-off-by: Kevin Fang <kevin.fang@qca.qualcomm.com>
Signed-off-by: Kalle Valo <kvalo@qca.qualcomm.com>

drivers/net/wireless/ath/ath6kl/cfg80211.c

@@ -237,6 +237,53 @@ static bool ath6kl_cfg80211_ready(struct ath6kl *ar)
 	return true;
 }
 
+static bool ath6kl_is_wpa_ie(const u8 *pos)
+{
+	return pos[0] == WLAN_EID_WPA && pos[1] >= 4 &&
+		pos[2] == 0x00 && pos[3] == 0x50 &&
+		pos[4] == 0xf2 && pos[5] == 0x01;
+}
+
+static bool ath6kl_is_rsn_ie(const u8 *pos)
+{
+	return pos[0] == WLAN_EID_RSN;
+}
+
+static int ath6kl_set_assoc_req_ies(struct ath6kl *ar, const u8 *ies,
+					size_t ies_len)
+{
+	const u8 *pos;
+	u8 *buf = NULL;
+	size_t len = 0;
+	int ret;
+
+	/*
+	 * Filter out RSN/WPA IE(s)
+	 */
+
+	if (ies && ies_len) {
+		buf = kmalloc(ies_len, GFP_KERNEL);
+		if (buf == NULL)
+			return -ENOMEM;
+		pos = ies;
+
+		while (pos + 1 < ies + ies_len) {
+			if (pos + 2 + pos[1] > ies + ies_len)
+				break;
+			if (!(ath6kl_is_wpa_ie(pos) || ath6kl_is_rsn_ie(pos))) {
+				memcpy(buf + len, pos, 2 + pos[1]);
+				len += 2 + pos[1];
+			}
+			pos += 2 + pos[1];
+		}
+	}
+
+	ret = ath6kl_wmi_set_appie_cmd(ar->wmi, WMI_FRAME_ASSOC_REQ,
+				       buf, len);
+	kfree(buf);
+	return ret;
+}
+
 static int ath6kl_cfg80211_connect(struct wiphy *wiphy, struct net_device *dev,
 				   struct cfg80211_connect_params *sme)
 {
@@ -285,6 +332,12 @@ static int ath6kl_cfg80211_connect(struct wiphy *wiphy, struct net_device *dev,
 		}
 	}
 
+	if (sme->ie && (sme->ie_len > 0)) {
+		status = ath6kl_set_assoc_req_ies(ar, sme->ie, sme->ie_len);
+		if (status)
+			return status;
+	}
+
 	if (test_bit(CONNECTED, &ar->flag) &&
 	    ar->ssid_len == sme->ssid_len &&
 	    !memcmp(ar->ssid, sme->ssid, ar->ssid_len)) {