ap: Handle Probe Request frames

Parse probe requests and send probe responses to show up in active
scans.

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)