From b6e7b7a4d686faa96a1154fc8bee14341aaea6d5 Mon Sep 17 00:00:00 2001
From: Denis Kenzior <denkenz@gmail.com>
Date: Thu, 18 Aug 2016 17:48:04 -0500
Subject: [PATCH] wscutil: Add M3 parser

---
 src/wscutil.c | 26 ++++++++++++++++++++++++++
 src/wscutil.h |  9 +++++++++
 2 files changed, 35 insertions(+)

diff --git a/src/wscutil.c b/src/wscutil.c
index c796b145..2ace1dc6 100644
--- a/src/wscutil.c
+++ b/src/wscutil.c
@@ -1065,6 +1065,32 @@ int wsc_parse_m2(const uint8_t *pdu, uint32_t len, struct wsc_m2 *out)
 	return 0;
 }
 
+int wsc_parse_m3(const uint8_t *pdu, uint32_t len, struct wsc_m3 *out)
+{
+	int r;
+	struct wsc_wfa_ext_iter iter;
+	uint8_t version;
+	enum wsc_message_type msg_type;
+
+	memset(out, 0, sizeof(struct wsc_m3));
+
+	r = wsc_parse_attrs(pdu, len, &out->version2, &iter, out->authenticator,
+		REQUIRED(VERSION, &version),
+		REQUIRED(MESSAGE_TYPE, &msg_type),
+		REQUIRED(REGISTRAR_NONCE, &out->registrar_nonce),
+		REQUIRED(E_HASH1, &out->e_hash1),
+		REQUIRED(E_HASH2, &out->e_hash2),
+		WSC_ATTR_INVALID);
+
+	if (r < 0)
+		return r;
+
+	if (msg_type != WSC_MESSAGE_TYPE_M3)
+		return -EBADMSG;
+
+	return 0;
+}
+
 struct wsc_attr_builder {
 	size_t capacity;
 	uint8_t *buf;
diff --git a/src/wscutil.h b/src/wscutil.h
index 9338b974..74926394 100644
--- a/src/wscutil.h
+++ b/src/wscutil.h
@@ -429,6 +429,14 @@ struct wsc_m2 {
 	uint8_t authenticator[8];
 };
 
+struct wsc_m3 {
+	bool version2;
+	uint8_t registrar_nonce[16];
+	uint8_t e_hash1[32];
+	uint8_t e_hash2[32];
+	uint8_t authenticator[8];
+};
+
 int wsc_parse_beacon(const unsigned char *pdu, unsigned int len,
 				struct wsc_beacon *out);
 int wsc_parse_probe_response(const unsigned char *pdu, unsigned int len,
@@ -438,6 +446,7 @@ int wsc_parse_probe_request(const unsigned char *pdu, unsigned int len,
 
 int wsc_parse_m1(const uint8_t *pdu, uint32_t len, struct wsc_m1 *out);
 int wsc_parse_m2(const uint8_t *pdu, uint32_t len, struct wsc_m2 *out);
+int wsc_parse_m3(const uint8_t *pdu, uint32_t len, struct wsc_m3 *out);
 
 uint8_t *wsc_build_probe_request(const struct wsc_probe_request *probe_request,
 				size_t *out_len);