wscutil: Implement wsc_build_probe_response

2 files changed, 96 insertions, 0 deletions

diff --git a/src/wscutil.c b/src/wscutil.c
index 99a74d1e6..4f22bd83b 100644
--- a/src/wscutil.c
+++ b/src/wscutil.c
@@ -31,6 +31,7 @@
 
 #include <ell/ell.h>
 
+#include "src/util.h"
 #include "src/wscutil.h"
 
 const unsigned char wsc_wfa_oui[3] = { 0x00, 0x37, 0x2a };
@@ -1759,6 +1760,12 @@ static uint8_t *wsc_attr_builder_free(struct wsc_attr_builder *builder,
 	return ret;
 }
 
+static void build_ap_setup_locked(struct wsc_attr_builder *builder, bool locked)
+{
+	wsc_attr_builder_start_attr(builder, WSC_ATTR_AP_SETUP_LOCKED);
+	wsc_attr_builder_put_u8(builder, locked ? 0x01 : 0x00);
+}
+
 static void build_association_state(struct wsc_attr_builder *builder,
 					enum wsc_association_state state)
 {
@@ -2014,6 +2021,22 @@ static void build_r_snonce2(struct wsc_attr_builder *builder,
 	wsc_attr_builder_put_bytes(builder, nonce, 16);
 }
 
+static void build_selected_registrar(struct wsc_attr_builder *builder,
+							bool selected)
+{
+	wsc_attr_builder_start_attr(builder, WSC_ATTR_SELECTED_REGISTRAR);
+	wsc_attr_builder_put_u8(builder, selected ? 0x01 : 0x00);
+}
+
+static void build_selected_registrar_configuration_methods(
+					struct wsc_attr_builder *builder,
+					uint16_t config_methods)
+{
+	wsc_attr_builder_start_attr(builder,
+			WSC_ATTR_SELECTED_REGISTRAR_CONFIGURATION_METHODS);
+	wsc_attr_builder_put_u16(builder, config_methods);
+}
+
 static void build_ssid(struct wsc_attr_builder *builder, const uint8_t *ssid,
 							size_t ssid_len)
 {
@@ -2129,6 +2152,76 @@ done:
 	return ret;
 }
 
+uint8_t *wsc_build_probe_response(
+		const struct wsc_probe_response *probe_response,
+		size_t *out_len)
+{
+	struct wsc_attr_builder *builder;
+	uint8_t *ret;
+
+	builder = wsc_attr_builder_new(512);
+	build_version(builder, 0x10);
+	build_wsc_state(builder, probe_response->state);
+
+	if (probe_response->ap_setup_locked)
+		build_ap_setup_locked(builder, true);
+
+	if (probe_response->selected_registrar) {
+		build_selected_registrar(builder, true);
+		build_device_password_id(builder,
+				probe_response->device_password_id);
+		build_selected_registrar_configuration_methods(builder,
+				probe_response->selected_reg_config_methods);
+	}
+
+	build_response_type(builder, probe_response->response_type);
+	build_uuid_e(builder, probe_response->uuid_e);
+	build_manufacturer(builder, probe_response->manufacturer);
+	build_model_name(builder, probe_response->model_name);
+	build_model_number(builder, probe_response->model_number);
+	build_serial_number(builder, probe_response->serial_number);
+	build_primary_device_type(builder,
+					&probe_response->primary_device_type);
+	build_device_name(builder, probe_response->device_name);
+	build_configuration_methods(builder, probe_response->config_methods);
+
+	if (probe_response->rf_bands & (probe_response->rf_bands - 1))
+		build_rf_bands(builder, probe_response->rf_bands);
+
+	if (!probe_response->version2)
+		goto done;
+
+	START_WFA_VENDOR_EXTENSION();
+
+	if (!util_mem_is_zero(probe_response->authorized_macs, 30)) {
+		int count;
+
+		for (count = 1; count < 5; count++)
+			if (util_mem_is_zero(probe_response->authorized_macs +
+						count * 6, 30 - count * 6))
+				break;
+
+		wsc_attr_builder_put_u8(builder,
+					WSC_WFA_EXTENSION_AUTHORIZED_MACS);
+		wsc_attr_builder_put_u8(builder, count * 6);
+		wsc_attr_builder_put_bytes(builder,
+					probe_response->authorized_macs,
+					count * 6);
+	}
+
+	if (probe_response->reg_config_methods) {
+		wsc_attr_builder_put_u8(builder,
+			WSC_WFA_EXTENSION_REGISTRAR_CONFIGRATION_METHODS);
+		wsc_attr_builder_put_u8(builder, 2);
+		wsc_attr_builder_put_u16(builder,
+					probe_response->reg_config_methods);
+	}
+
+done:
+	ret = wsc_attr_builder_free(builder, false, out_len);
+	return ret;
+}
+
 uint8_t *wsc_build_association_request(
 		const struct wsc_association_request *association_request,
 		size_t *out_len)
diff --git a/src/wscutil.h b/src/wscutil.h
index 0d06a60f8..fc28a01e2 100644
--- a/src/wscutil.h
+++ b/src/wscutil.h
@@ -606,6 +606,9 @@ uint8_t *wsc_build_credential(const struct wsc_credential *in, size_t *out_len);
 
 uint8_t *wsc_build_probe_request(const struct wsc_probe_request *probe_request,
 				size_t *out_len);
+uint8_t *wsc_build_probe_response(
+		const struct wsc_probe_response *probe_response,
+		size_t *out_len);
 uint8_t *wsc_build_association_request(
 		const struct wsc_association_request *association_request,
 		size_t *out_len);