From ad08dfad336c9fb33cefa049a80ca7fb7b653066 Mon Sep 17 00:00:00 2001
From: Denis Kenzior <denkenz@gmail.com>
Date: Wed, 15 Jul 2015 14:44:28 -0500
Subject: [PATCH] wsc: Add WFA extension iterator + recurse

---
 src/wsc.c | 55 +++++++++++++++++++++++++++++++++++++++++++++++++++++++
 src/wsc.h | 33 +++++++++++++++++++++++++++++++++
 2 files changed, 88 insertions(+)

diff --git a/src/wsc.c b/src/wsc.c
index 9bcd7558..aeb3bba1 100644
--- a/src/wsc.c
+++ b/src/wsc.c
@@ -30,6 +30,42 @@
 
 #include "wsc.h"
 
+void wsc_wfa_ext_iter_init(struct wsc_wfa_ext_iter *iter,
+				const unsigned char *pdu, unsigned short len)
+{
+	iter->pdu = pdu;
+	iter->max = len;
+	iter->pos = 0;
+}
+
+bool wsc_wfa_ext_iter_next(struct wsc_wfa_ext_iter *iter)
+{
+	const unsigned char *start = iter->pdu + iter->pos;
+	const unsigned char *end = iter->pdu + iter->max;
+	unsigned char type;
+	unsigned char len;
+
+	if (iter->pos + 2 >= iter->max)
+		return false;
+
+	type = *start;
+	start += 1;
+
+	len = *start;
+	start += 1;
+
+	if (start + len > end)
+		return false;
+
+	iter->type = type;
+	iter->len = len;
+	iter->data = start;
+
+	iter->pos = start + len - iter->pdu;
+
+	return true;
+}
+
 void wsc_attr_iter_init(struct wsc_attr_iter *iter, const unsigned char *pdu,
 			unsigned int len)
 {
@@ -66,3 +102,22 @@ bool wsc_attr_iter_next(struct wsc_attr_iter *iter)
 
 	return true;
 }
+
+bool wsc_attr_iter_recurse_wfa_ext(struct wsc_attr_iter *iter,
+					struct wsc_wfa_ext_iter *wfa_iter)
+{
+	static const unsigned char wfa_ext[3] = { 0x00, 0x37, 0x2a };
+
+	if (iter->type != WSC_ATTR_VENDOR_EXTENSION)
+		return false;
+
+	if (iter->len < 3)
+		return false;
+
+	if (memcmp(iter->data, wfa_ext, sizeof(wfa_ext)))
+		return false;
+
+	wsc_wfa_ext_iter_init(wfa_iter, iter->data + 3, iter->len - 3);
+
+	return true;
+}
diff --git a/src/wsc.h b/src/wsc.h
index ddc170ea..27cdfdf2 100644
--- a/src/wsc.h
+++ b/src/wsc.h
@@ -257,6 +257,37 @@ enum wsc_config_state {
 	WSC_CONFIG_STATE_CONFIGURED		= 0x02,
 };
 
+struct wsc_wfa_ext_iter {
+	unsigned short max;
+	unsigned short pos;
+	const unsigned char *pdu;
+	unsigned char type;
+	unsigned char len;
+	const unsigned char *data;
+};
+
+void wsc_wfa_ext_iter_init(struct wsc_wfa_ext_iter *iter,
+				const unsigned char *pdu, unsigned short len);
+bool wsc_wfa_ext_iter_next(struct wsc_wfa_ext_iter *iter);
+
+static inline unsigned char wsc_wfa_ext_iter_get_type(
+						struct wsc_wfa_ext_iter *iter)
+{
+	return iter->type;
+}
+
+static inline unsigned char wsc_wfa_ext_iter_get_length(
+						struct wsc_wfa_ext_iter *iter)
+{
+	return iter->len;
+}
+
+static inline const unsigned char *wsc_wfa_ext_iter_get_data(
+						struct wsc_wfa_ext_iter *iter)
+{
+	return iter->data;
+}
+
 struct wsc_attr_iter {
 	unsigned int max;
 	unsigned int pos;
@@ -269,6 +300,8 @@ struct wsc_attr_iter {
 void wsc_attr_iter_init(struct wsc_attr_iter *iter, const unsigned char *pdu,
 			unsigned int len);
 bool wsc_attr_iter_next(struct wsc_attr_iter *iter);
+bool wsc_attr_iter_recurse_wfa_ext(struct wsc_attr_iter *iter,
+					struct wsc_wfa_ext_iter *wfa_iter);
 
 static inline unsigned int wsc_attr_iter_get_type(struct wsc_attr_iter *iter)
 {