wsc: Add initial scanning logic

src/wsc.c

@@ -34,6 +34,7 @@
 #include "mpdu.h"
 #include "ie.h"
 #include "wscutil.h"
+#include "util.h"
 #include "wsc.h"
 
 #define WALK_TIME 120
@@ -42,15 +43,121 @@ static struct l_genl_family *nl80211 = NULL;
 static uint32_t netdev_watch = 0;
 
 struct wsc_sm {
+	uint32_t ifindex;
 	uint8_t *wsc_ies;
 	size_t wsc_ies_size;
 	struct l_timeout *walk_timer;
+	uint32_t scan_id;
 };
 
 struct wsc {
 	struct netdev *netdev;
+	struct l_dbus_message *pending;
+	struct wsc_sm *sm;
 };
 
+static bool scan_results(uint32_t wiphy_id, uint32_t ifindex,
+				struct l_queue *bss_list, void *userdata)
+{
+	struct wsc_sm *sm = userdata;
+	struct scan_bss *bss_2g;
+	struct scan_bss *bss_5g;
+	struct scan_bss *target;
+	uint8_t uuid_2g[16];
+	uint8_t uuid_5g[16];
+	const struct l_queue_entry *bss_entry;
+	struct wsc_probe_response probe_response;
+
+	bss_2g = NULL;
+	bss_5g = NULL;
+
+	sm->scan_id = 0;
+
+	for (bss_entry = l_queue_get_entries(bss_list); bss_entry;
+				bss_entry = bss_entry->next) {
+		struct scan_bss *bss = bss_entry->data;
+		enum scan_band band;
+		int err;
+
+		l_debug("bss '%s' with SSID: %s, freq: %u",
+			scan_bss_address_to_string(bss),
+			util_ssid_to_utf8(bss->ssid_len, bss->ssid),
+			bss->frequency);
+
+		l_debug("bss->wsc: %p, %zu", bss->wsc, bss->wsc_size);
+
+		if (!bss->wsc)
+			continue;
+
+		err = wsc_parse_probe_response(bss->wsc, bss->wsc_size,
+						&probe_response);
+		if (err < 0) {
+			l_debug("ProbeResponse parse failed: %s",
+							strerror(-err));
+			continue;
+		}
+
+		l_debug("SelectedRegistar: %s",
+			probe_response.selected_registrar ? "true" : "false");
+
+		if (!probe_response.selected_registrar)
+			continue;
+
+		if (probe_response.device_password_id !=
+				WSC_DEVICE_PASSWORD_ID_PUSH_BUTTON)
+			continue;
+
+		scan_freq_to_channel(bss->frequency, &band);
+
+		switch (band) {
+		case SCAN_BAND_2_4_GHZ:
+			if (bss_2g) {
+				l_debug("2G Session overlap error");
+				return false;
+			}
+
+			bss_2g = bss;
+			memcpy(uuid_2g, probe_response.uuid_e, 16);
+			break;
+
+		case SCAN_BAND_5_GHZ:
+			if (bss_5g) {
+				l_debug("5G Session overlap error");
+				return false;
+			}
+
+			bss_5g = bss;
+			memcpy(uuid_5g, probe_response.uuid_e, 16);
+			break;
+
+		default:
+			return false;
+		}
+	}
+
+	if (bss_2g && bss_5g && memcmp(uuid_2g, uuid_5g, 16)) {
+		l_debug("Found two PBC APs on different bands");
+		return false;
+	}
+
+	if (bss_5g)
+		target = bss_5g;
+	else if (bss_2g)
+		target = bss_2g;
+	else {
+		l_debug("No PBC APs found, running the scan again");
+		sm->scan_id = scan_active(sm->ifindex,
+						sm->wsc_ies, sm->wsc_ies_size,
+						NULL, scan_results, sm, NULL);
+		return false;
+	}
+
+	l_debug("Found AP to connect to: %s",
+			scan_bss_address_to_string(target));
+
+	return false;
+}
+
 struct wsc_sm *wsc_sm_new_pushbutton(uint32_t ifindex, const uint8_t *addr,
 					uint32_t bands)
 {
@@ -97,16 +204,27 @@ struct wsc_sm *wsc_sm_new_pushbutton(uint32_t ifindex, const uint8_t *addr,
 							&sm->wsc_ies_size);
 	l_free(wsc_data);
 
-	if (sm->wsc_ies) {
+	if (!sm->wsc_ies) {
 		l_free(sm);
 		return NULL;
 	}
 
+	sm->ifindex = ifindex;
+	sm->scan_id = scan_active(ifindex, sm->wsc_ies, sm->wsc_ies_size,
+					NULL, scan_results, sm, NULL);
+
 	return sm;
 }
 
 void wsc_sm_free(struct wsc_sm *sm)
 {
+	l_free(sm->wsc_ies);
+
+	if (sm->scan_id > 0) {
+		scan_cancel(sm->ifindex, sm->scan_id);
+		sm->scan_id = 0;
+	}
+
 	l_free(sm);
 }
 
@@ -114,18 +232,43 @@ static struct l_dbus_message *wsc_push_button(struct l_dbus *dbus,
 						struct l_dbus_message *message,
 						void *user_data)
 {
+	struct wsc *wsc = user_data;
+
 	l_debug("");
 
-	return dbus_error_not_implemented(message);
+	if (wsc->pending)
+		return dbus_error_busy(message);
+
+	/* TODO: Parse wiphy bands to set the RF Bands properly below */
+	wsc->sm = wsc_sm_new_pushbutton(netdev_get_ifindex(wsc->netdev),
+				netdev_get_address(wsc->netdev),
+				SCAN_BAND_2_4_GHZ | SCAN_BAND_5_GHZ);
+
+	wsc->pending = l_dbus_message_ref(message);
+	return NULL;
 }
 
 static struct l_dbus_message *wsc_cancel(struct l_dbus *dbus,
 						struct l_dbus_message *message,
 						void *user_data)
 {
+	struct wsc *wsc = user_data;
+	struct l_dbus_message *reply;
+
 	l_debug("");
 
-	return dbus_error_not_implemented(message);
+	if (!wsc->pending)
+		return dbus_error_not_available(message);
+
+	reply = l_dbus_message_new_method_return(message);
+	l_dbus_message_set_arguments(reply, "");
+
+	dbus_pending_reply(&wsc->pending, dbus_error_aborted(wsc->pending));
+
+	wsc_sm_free(wsc->sm);
+	wsc->sm = NULL;
+
+	return reply;
 }
 
 static void setup_wsc_interface(struct l_dbus_interface *interface)