1 files changed, 71 insertions, 4 deletions

diff --git a/src/common/ieee802_11_common.c b/src/common/ieee802_11_common.c
index 242f933..96ef5b6 100644
--- a/src/common/ieee802_11_common.c
+++ b/src/common/ieee802_11_common.c
@@ -1,6 +1,6 @@
 /*
  * IEEE 802.11 Common routines
- * Copyright (c) 2002-2008, Jouni Malinen <j@w1.fi>
+ * Copyright (c) 2002-2009, Jouni Malinen <j@w1.fi>
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
@@ -19,7 +19,7 @@
 #include "ieee802_11_common.h"
 
 
-static int ieee802_11_parse_vendor_specific(u8 *pos, size_t elen,
+static int ieee802_11_parse_vendor_specific(const u8 *pos, size_t elen,
 					    struct ieee802_11_elems *elems,
 					    int show_errors)
 {
@@ -131,12 +131,12 @@ static int ieee802_11_parse_vendor_specific(u8 *pos, size_t elen,
  * @show_errors: Whether to show parsing errors in debug log
  * Returns: Parsing result
  */
-ParseRes ieee802_11_parse_elems(u8 *start, size_t len,
+ParseRes ieee802_11_parse_elems(const u8 *start, size_t len,
 				struct ieee802_11_elems *elems,
 				int show_errors)
 {
 	size_t left = len;
-	u8 *pos = start;
+	const u8 *pos = start;
 	int unknown = 0;
 
 	os_memset(elems, 0, sizeof(*elems));
@@ -257,3 +257,70 @@ ParseRes ieee802_11_parse_elems(u8 *start, size_t len,
 
 	return unknown ? ParseUnknown : ParseOK;
 }
+
+
+int ieee802_11_ie_count(const u8 *ies, size_t ies_len)
+{
+	int count = 0;
+	const u8 *pos, *end;
+
+	if (ies == NULL)
+		return 0;
+
+	pos = ies;
+	end = ies + ies_len;
+
+	while (pos + 2 <= end) {
+		if (pos + 2 + pos[1] > end)
+			break;
+		count++;
+		pos += 2 + pos[1];
+	}
+
+	return count;
+}
+
+
+struct wpabuf * ieee802_11_vendor_ie_concat(const u8 *ies, size_t ies_len,
+					    u32 oui_type)
+{
+	struct wpabuf *buf;
+	const u8 *end, *pos, *ie;
+
+	pos = ies;
+	end = ies + ies_len;
+	ie = NULL;
+
+	while (pos + 1 < end) {
+		if (pos + 2 + pos[1] > end)
+			return NULL;
+		if (pos[0] == WLAN_EID_VENDOR_SPECIFIC && pos[1] >= 4 &&
+		    WPA_GET_BE32(&pos[2]) == oui_type) {
+			ie = pos;
+			break;
+		}
+		pos += 2 + pos[1];
+	}
+
+	if (ie == NULL)
+		return NULL; /* No specified vendor IE found */
+
+	buf = wpabuf_alloc(ies_len);
+	if (buf == NULL)
+		return NULL;
+
+	/*
+	 * There may be multiple vendor IEs in the message, so need to
+	 * concatenate their data fields.
+	 */
+	while (pos + 1 < end) {
+		if (pos + 2 + pos[1] > end)
+			break;
+		if (pos[0] == WLAN_EID_VENDOR_SPECIFIC && pos[1] >= 4 &&
+		    WPA_GET_BE32(&pos[2]) == oui_type)
+			wpabuf_put_data(buf, pos + 6, pos[1] - 4);
+		pos += 2 + pos[1];
+	}
+
+	return buf;
+}