From 6981ffdc2f5d59aac75c8446363c474e33f18b31 Mon Sep 17 00:00:00 2001
From: Kevin Fang <kevin.fang@qca.qualcomm.com>
Date: Fri, 7 Oct 2011 08:51:19 +0800
Subject: [PATCH] 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 | 53 ++++++++++++++++++++++
 1 file changed, 53 insertions(+)

diff --git a/drivers/net/wireless/ath/ath6kl/cfg80211.c b/drivers/net/wireless/ath/ath6kl/cfg80211.c
index 2acfa7fadd07..40a2d7a8b76a 100644
--- a/drivers/net/wireless/ath/ath6kl/cfg80211.c
+++ b/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)) {