diff --git a/src/wsc.c b/src/wsc.c index c10021bc..9db4b031 100644 --- a/src/wsc.c +++ b/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)