From 659a1605d82706a197dd7693aa2785c0c1fc0b72 Mon Sep 17 00:00:00 2001
From: Anton Nayshtut <qca_antonn@qca.qualcomm.com>
Date: Wed, 21 Jan 2015 15:30:48 +0200
Subject: [PATCH] Parsing of multi band element

Signed-off-by: Jouni Malinen <jouni@qca.qualcomm.com>
---
 src/common/ieee802_11_common.c | 74 ++++++++++++++++++++++++++++++++++
 src/common/ieee802_11_common.h | 14 +++++++
 2 files changed, 88 insertions(+)

diff --git a/src/common/ieee802_11_common.c b/src/common/ieee802_11_common.c
index 5385faf7d..884ae8d89 100644
--- a/src/common/ieee802_11_common.c
+++ b/src/common/ieee802_11_common.c
@@ -339,6 +339,18 @@ ParseRes ieee802_11_parse_elems(const u8 *start, size_t len,
 			/* after mic everything is encrypted, so stop. */
 			left = elen;
 			break;
+		case WLAN_EID_MULTI_BAND:
+			if (elems->mb_ies.nof_ies >= MAX_NOF_MB_IES_SUPPORTED) {
+				wpa_printf(MSG_MSGDUMP,
+					   "IEEE 802.11 element parse ignored MB IE (id=%d elen=%d)",
+					   id, elen);
+				break;
+			}
+
+			elems->mb_ies.ies[elems->mb_ies.nof_ies].ie = pos;
+			elems->mb_ies.ies[elems->mb_ies.nof_ies].ie_len = elen;
+			elems->mb_ies.nof_ies++;
+			break;
 		default:
 			unknown++;
 			if (!show_errors)
@@ -946,3 +958,65 @@ const char * fc2str(u16 fc)
 	return "WLAN_FC_TYPE_UNKNOWN";
 #undef C2S
 }
+
+
+#define IE_HEADER_SIZE               ((u8) (2 * sizeof(u8)))
+#define IE_BUFFER_LENGTH(ie_len_val) ((ie_len_val) + IE_HEADER_SIZE)
+
+int mb_ies_info_by_ies(struct mb_ies_info *info, const u8 *ies_buf,
+		       size_t ies_len)
+{
+	os_memset(info, 0, sizeof(*info));
+
+	while (ies_buf && ies_len >= IE_HEADER_SIZE &&
+	       info->nof_ies < MAX_NOF_MB_IES_SUPPORTED) {
+		size_t len = IE_BUFFER_LENGTH(ies_buf[1]);
+
+		if (len > ies_len) {
+			wpa_hexdump(MSG_DEBUG, "Truncated IEs",
+				    ies_buf, ies_len);
+			return -1;
+		}
+
+		if (ies_buf[0] == WLAN_EID_MULTI_BAND) {
+			wpa_printf(MSG_DEBUG, "MB IE of %zu bytes found", len);
+			info->ies[info->nof_ies].ie = ies_buf + IE_HEADER_SIZE;
+			info->ies[info->nof_ies].ie_len = ies_buf[1];
+			info->nof_ies++;
+		}
+
+		ies_len -= len;
+		ies_buf += len;
+	}
+
+	return 0;
+}
+
+
+struct wpabuf * mb_ies_by_info(struct mb_ies_info *info)
+{
+	struct wpabuf *mb_ies = NULL;
+
+	WPA_ASSERT(info != NULL);
+
+	if (info->nof_ies) {
+		u8 i;
+		size_t mb_ies_size = 0;
+
+		for (i = 0; i < info->nof_ies; i++)
+			mb_ies_size += IE_BUFFER_LENGTH(info->ies[i].ie_len);
+
+		mb_ies = wpabuf_alloc(mb_ies_size);
+		if (mb_ies) {
+			for (i = 0; i < info->nof_ies; i++) {
+				wpabuf_put_u8(mb_ies, WLAN_EID_MULTI_BAND);
+				wpabuf_put_u8(mb_ies, info->ies[i].ie_len);
+				wpabuf_put_data(mb_ies,
+						info->ies[i].ie,
+						info->ies[i].ie_len);
+			}
+		}
+	}
+
+	return mb_ies;
+}
diff --git a/src/common/ieee802_11_common.h b/src/common/ieee802_11_common.h
index c84d8a748..8d73980e0 100644
--- a/src/common/ieee802_11_common.h
+++ b/src/common/ieee802_11_common.h
@@ -9,6 +9,16 @@
 #ifndef IEEE802_11_COMMON_H
 #define IEEE802_11_COMMON_H
 
+#define MAX_NOF_MB_IES_SUPPORTED 5
+
+struct mb_ies_info {
+	struct {
+		const u8 *ie;
+		u8 ie_len;
+	} ies[MAX_NOF_MB_IES_SUPPORTED];
+	u8 nof_ies;
+};
+
 /* Parsed Information Elements */
 struct ieee802_11_elems {
 	const u8 *ssid;
@@ -76,6 +86,7 @@ struct ieee802_11_elems {
 	u8 osen_len;
 	u8 ampe_len;
 	u8 mic_len;
+	struct mb_ies_info mb_ies;
 };
 
 typedef enum { ParseOK = 0, ParseUnknown = 1, ParseFailed = -1 } ParseRes;
@@ -104,6 +115,9 @@ int ieee80211_chan_to_freq(const char *country, u8 op_class, u8 chan);
 int ieee80211_is_dfs(int freq);
 
 int supp_rates_11b_only(struct ieee802_11_elems *elems);
+int mb_ies_info_by_ies(struct mb_ies_info *info, const u8 *ies_buf,
+		       size_t ies_len);
+struct wpabuf * mb_ies_by_info(struct mb_ies_info *info);
 
 const char * fc2str(u16 fc);
 #endif /* IEEE802_11_COMMON_H */