From d8378c5a0f9d89bdd3c157507a4af88d73401df5 Mon Sep 17 00:00:00 2001 From: Andrew Zaborowski Date: Fri, 22 Sep 2017 05:06:37 +0200 Subject: [PATCH] ap: Handle Probe Request frames Parse probe requests and send probe responses to show up in active scans. --- src/ap.c | 162 +++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 162 insertions(+) diff --git a/src/ap.c b/src/ap.c index 2f5e5ac9..773bef83 100644 --- a/src/ap.c +++ b/src/ap.c @@ -34,10 +34,12 @@ #include "src/iwd.h" #include "src/scan.h" #include "src/device.h" +#include "src/netdev.h" #include "src/wiphy.h" #include "src/crypto.h" #include "src/ie.h" #include "src/mpdu.h" +#include "src/util.h" #include "src/ap.h" struct ap_state { @@ -49,6 +51,7 @@ struct ap_state { unsigned int ciphers; uint32_t beacon_interval; struct l_uintset *rates; + struct l_queue *frame_watch_ids; uint32_t start_stop_cmd_id; }; @@ -56,14 +59,26 @@ static struct l_genl_family *nl80211 = NULL; static struct l_queue *ap_list = NULL; +static void ap_frame_watch_remove(void *data, void *user_data) +{ + struct netdev *netdev = user_data; + + if (L_PTR_TO_UINT(data)) + netdev_frame_watch_remove(netdev, L_PTR_TO_UINT(data)); +} + static void ap_free(void *data) { struct ap_state *ap = data; + struct netdev *netdev = device_get_netdev(ap->device); l_free(ap->ssid); memset(ap->psk, 0, strlen(ap->psk)); l_free(ap->psk); + l_queue_foreach(ap->frame_watch_ids, ap_frame_watch_remove, netdev); + l_queue_destroy(ap->frame_watch_ids, NULL); + if (ap->start_stop_cmd_id) l_genl_family_cancel(nl80211, ap->start_stop_cmd_id); @@ -165,6 +180,139 @@ static size_t ap_build_beacon_pr_tail(struct ap_state *ap, uint8_t *out_buf) return len; } +static uint32_t ap_send_mgmt_frame(struct ap_state *ap, + const struct mmpdu_header *frame, + size_t frame_len, bool wait_ack, + l_genl_msg_func_t callback, + void *user_data) +{ + struct l_genl_msg *msg; + uint32_t ifindex = device_get_ifindex(ap->device); + uint32_t id; + uint32_t ch_freq = scan_channel_to_freq(ap->channel, SCAN_BAND_2_4_GHZ); + + msg = l_genl_msg_new_sized(NL80211_CMD_FRAME, 128 + frame_len); + + l_genl_msg_append_attr(msg, NL80211_ATTR_IFINDEX, 4, &ifindex); + l_genl_msg_append_attr(msg, NL80211_ATTR_WIPHY_FREQ, 4, &ch_freq); + l_genl_msg_append_attr(msg, NL80211_ATTR_FRAME, frame_len, frame); + if (!wait_ack) + l_genl_msg_append_attr(msg, NL80211_ATTR_DONT_WAIT_FOR_ACK, + 0, NULL); + + id = l_genl_family_send(nl80211, msg, callback, user_data, NULL); + + if (!id) + l_genl_msg_unref(msg); + + return id; +} + +static void ap_probe_resp_cb(struct l_genl_msg *msg, void *user_data) +{ + if (l_genl_msg_get_error(msg) < 0) + l_error("AP Probe Response not sent: %i", + l_genl_msg_get_error(msg)); + else + l_info("AP Probe Response sent OK"); +} + +/* + * Parse Probe Request according to 802.11-2016 9.3.3.10 and act according + * to 802.11-2016 11.1.4.3 + */ +static void ap_probe_req_cb(struct netdev *netdev, + const struct mmpdu_header *hdr, + const void *body, size_t body_len, + void *user_data) +{ + struct ap_state *ap = user_data; + const struct mmpdu_probe_request *req = body; + const char *ssid = NULL; + const uint8_t *ssid_list = NULL; + size_t ssid_len = 0, ssid_list_len = 0, len; + int dsss_channel = -1; + struct ie_tlv_iter iter; + const uint8_t *bssid = device_get_address(ap->device); + bool match = false; + uint8_t resp[512]; + + l_info("AP Probe Request from %s", + util_address_to_string(hdr->address_2)); + + ie_tlv_iter_init(&iter, req->ies, body_len - sizeof(*req)); + + while (ie_tlv_iter_next(&iter)) + switch (ie_tlv_iter_get_tag(&iter)) { + case IE_TYPE_SSID: + ssid = (const char *) ie_tlv_iter_get_data(&iter); + ssid_len = ie_tlv_iter_get_length(&iter); + break; + + case IE_TYPE_SSID_LIST: + ssid_list = ie_tlv_iter_get_data(&iter); + ssid_list_len = ie_tlv_iter_get_length(&iter); + break; + + case IE_TYPE_DSSS_PARAMETER_SET: + if (ie_tlv_iter_get_length(&iter) != 1) + return; + + dsss_channel = ie_tlv_iter_get_data(&iter)[0]; + break; + } + + /* + * Check if we should reply to this Probe Request according to + * 802.11-2016 section 11.1.4.3.2. + */ + + if (memcmp(hdr->address_1, bssid, 6) && + !util_is_broadcast_address(hdr->address_1)) + match = false; + + if (memcmp(hdr->address_3, bssid, 6) && + !util_is_broadcast_address(hdr->address_3)) + match = false; + + if (!ssid || ssid_len == 0) /* Wildcard SSID */ + match = true; + else if (ssid && ssid_len == strlen(ap->ssid) && /* Specific SSID */ + !memcmp(ssid, ap->ssid, ssid_len)) + match = true; + else if (ssid_list) { /* SSID List */ + ie_tlv_iter_init(&iter, ssid_list, ssid_list_len); + + while (ie_tlv_iter_next(&iter)) { + if (ie_tlv_iter_get_tag(&iter) != IE_TYPE_SSID) + return; + + ssid = (const char *) ie_tlv_iter_get_data(&iter); + ssid_len = ie_tlv_iter_get_length(&iter); + + if (ssid_len == strlen(ap->ssid) && + !memcmp(ssid, ap->ssid, ssid_len)) { + match = true; + break; + } + } + } + + if (dsss_channel != -1 && dsss_channel != ap->channel) + match = false; + + if (!match) + return; + + len = ap_build_beacon_pr_head(ap, + MPDU_MANAGEMENT_SUBTYPE_PROBE_RESPONSE, + hdr->address_2, resp); + len += ap_build_beacon_pr_tail(ap, resp + len); + + ap_send_mgmt_frame(ap, (struct mmpdu_header *) resp, len, false, + ap_probe_resp_cb, NULL); +} + static void ap_stopped(struct ap_state *ap) { ap->event_cb(ap->device, AP_EVENT_STOPPED); @@ -259,9 +407,12 @@ static struct l_genl_msg *ap_build_cmd_start_ap(struct ap_state *ap) int ap_start(struct device *device, const char *ssid, const char *psk, ap_event_cb_t event_cb) { + struct netdev *netdev = device_get_netdev(device); struct wiphy *wiphy = device_get_wiphy(device); struct ap_state *ap; struct l_genl_msg *cmd; + const struct l_queue_entry *entry; + uint32_t id; if (l_queue_find(ap_list, ap_match_device, device)) return -EEXIST; @@ -281,6 +432,17 @@ int ap_start(struct device *device, const char *ssid, const char *psk, l_uintset_put(ap->rates, 2); /* 1 Mbps*/ l_uintset_put(ap->rates, 11); /* 5.5 Mbps*/ l_uintset_put(ap->rates, 22); /* 11 Mbps*/ + ap->frame_watch_ids = l_queue_new(); + + id = netdev_frame_watch_add(netdev, 0x0000 | + (MPDU_MANAGEMENT_SUBTYPE_PROBE_REQUEST << 4), + NULL, 0, ap_probe_req_cb, ap); + l_queue_push_tail(ap->frame_watch_ids, L_UINT_TO_PTR(id)); + + for (entry = l_queue_get_entries(ap->frame_watch_ids); entry; + entry = entry->next) + if (!L_PTR_TO_UINT(entry->data)) + goto error; cmd = ap_build_cmd_start_ap(ap); if (!cmd)