From cd465852b451c7e33ce334c525c232816ce40c83 Mon Sep 17 00:00:00 2001
From: Denis Kenzior <denkenz@gmail.com>
Date: Fri, 17 Jul 2015 14:52:46 -0500
Subject: [PATCH] wsc: Add initial message parser framework

---
 src/wsc.c | 162 ++++++++++++++++++++++++++++++++++++++++++++++++++++++
 src/wsc.h |   1 +
 2 files changed, 163 insertions(+)

diff --git a/src/wsc.c b/src/wsc.c
index aeb3bba1..ac3923ad 100644
--- a/src/wsc.c
+++ b/src/wsc.c
@@ -25,6 +25,8 @@
 #endif
 
 #include <stdbool.h>
+#include <stdarg.h>
+#include <errno.h>
 
 #include <ell/ell.h>
 
@@ -121,3 +123,163 @@ bool wsc_attr_iter_recurse_wfa_ext(struct wsc_attr_iter *iter,
 
 	return true;
 }
+
+enum attr_flag {
+	ATTR_FLAG_REQUIRED,	/* Always required */
+	ATTR_FLAG_VERSION2,	/* Included if Version2 is present */
+	ATTR_FLAG_REGISTRAR,	/* Included if Selected Registrar is true */
+};
+
+typedef bool (*attr_handler)(struct wsc_attr_iter *, void *);
+
+static attr_handler handler_for_type(enum wsc_attr type)
+{
+	switch (type) {
+	default:
+		break;
+	}
+
+	return NULL;
+}
+
+struct attr_handler_entry {
+	enum wsc_attr type;
+	unsigned int flags;
+	void *data;
+	bool present;
+};
+
+static bool verify_version2(struct wsc_wfa_ext_iter *ext_iter)
+{
+	if (!wsc_wfa_ext_iter_next(ext_iter))
+		return false;
+
+	if (wsc_wfa_ext_iter_get_type(ext_iter) != WSC_WFA_EXTENSION_VERSION2)
+		return false;
+
+	if (wsc_wfa_ext_iter_get_length(ext_iter) != 1)
+		return false;
+
+	return true;
+}
+
+static int wsc_parse_attrs(const unsigned char *pdu, unsigned int len,
+				bool *out_version2,
+				struct wsc_wfa_ext_iter *ext_iter,
+				enum wsc_attr type, ...)
+{
+	struct wsc_attr_iter iter;
+	struct l_queue *entries;
+	const struct l_queue_entry *e;
+	va_list args;
+	bool version2 = false;
+	bool have_required = true;
+	bool parse_error = false;
+
+	wsc_attr_iter_init(&iter, pdu, len);
+
+	va_start(args, type);
+
+	entries = l_queue_new();
+
+	while (type != WSC_ATTR_INVALID) {
+		struct attr_handler_entry *entry;
+
+		entry = l_new(struct attr_handler_entry, 1);
+
+		entry->type = type;
+		entry->flags = va_arg(args, unsigned int);
+		entry->data = va_arg(args, void *);
+
+		type = va_arg(args, enum wsc_attr);
+		l_queue_push_tail(entries, entry);
+	}
+
+	va_end(args);
+	e = l_queue_get_entries(entries);
+
+	while (wsc_attr_iter_next(&iter)) {
+		attr_handler handler;
+		struct attr_handler_entry *entry;
+		const struct l_queue_entry *e2;
+
+		for (e2 = e; e2; e2 = e2->next) {
+			entry = e2->data;
+
+			if (wsc_attr_iter_get_type(&iter) == entry->type) {
+				entry->present = true;
+				break;
+			}
+
+			if (entry->flags & ATTR_FLAG_REQUIRED) {
+				have_required = false;
+				goto done;
+			}
+		}
+
+		if (e2 == NULL) {
+			if (wsc_attr_iter_get_type(&iter)
+					!= WSC_ATTR_VENDOR_EXTENSION)
+				break;
+
+			if (!wsc_attr_iter_recurse_wfa_ext(&iter, ext_iter))
+				break;
+
+			if (!verify_version2(ext_iter)) {
+				parse_error = true;
+				goto done;
+			}
+
+			version2 = true;
+			goto check;
+		}
+
+		handler = handler_for_type(entry->type);
+
+		if (!handler(&iter, entry->data)) {
+			parse_error = true;
+			goto done;
+		}
+
+		e = e2->next;
+	}
+
+	for (; e; e = e->next) {
+		struct attr_handler_entry *entry = e->data;
+
+		if (entry->flags & ATTR_FLAG_REQUIRED)
+			parse_error = true;
+	}
+
+check:
+	if (version2) {
+		struct attr_handler_entry *entry;
+
+		for (e = l_queue_get_entries(entries); e; e = e->next) {
+			entry = e->data;
+
+			if (!(entry->flags & ATTR_FLAG_VERSION2))
+				continue;
+
+			if (entry->present)
+				continue;
+
+			parse_error = true;
+			goto done;
+		}
+	}
+
+	/* TODO: Check Selected Registrar attributes */
+
+done:
+	l_queue_destroy(entries, l_free);
+
+	if (!have_required)
+		return -EINVAL;
+	if (parse_error)
+		return -EBADMSG;
+
+	*out_version2 = version2;
+
+	return 0;
+}
diff --git a/src/wsc.h b/src/wsc.h
index 27cdfdf2..0040cd2d 100644
--- a/src/wsc.h
+++ b/src/wsc.h
@@ -109,6 +109,7 @@ enum wsc_attr {
 	WSC_ATTR_WSC_STATE					= 0x1044,
 	WSC_ATTR_X509_CERTIFICATE_REQUEST			= 0x104B,
 	WSC_ATTR_X509_CERTIFICATE				= 0x104C,
+	WSC_ATTR_INVALID					= 0x0000,
 };
 
 /* Table 29 */