diff --git a/src/common/nan_de.c b/src/common/nan_de.c new file mode 100644 index 000000000..e1999a06b --- /dev/null +++ b/src/common/nan_de.c @@ -0,0 +1,1389 @@ +/* + * NAN Discovery Engine + * Copyright (c) 2024, Qualcomm Innovation Center, Inc. + * + * This software may be distributed under the terms of the BSD license. + * See README for more details. + */ + +#include "utils/includes.h" + +#include "utils/common.h" +#include "utils/eloop.h" +#include "crypto/crypto.h" +#include "crypto/sha256.h" +#include "ieee802_11_defs.h" +#include "nan.h" +#include "nan_de.h" + +static const u8 nan_network_id[ETH_ALEN] = +{ 0x51, 0x6f, 0x9a, 0x01, 0x00, 0x00 }; +static const u8 wildcard_bssid[ETH_ALEN] = +{ 0xff, 0xff, 0xff, 0xff, 0xff, 0xff }; + +enum nan_de_service_type { + NAN_DE_PUBLISH, + NAN_DE_SUBSCRIBE, +}; + +struct nan_de_service { + int id; + enum nan_de_service_type type; + char *service_name; + u8 service_id[NAN_SERVICE_ID_LEN]; + struct nan_publish_params publish; + struct nan_subscribe_params subscribe; + enum nan_service_protocol_type srv_proto_type; + struct wpabuf *ssi; + struct wpabuf *elems; + struct os_reltime time_started; + struct os_reltime end_time; + struct os_reltime last_multicast; + struct os_reltime first_discovered; + struct os_reltime last_followup; + bool needs_fsd; + unsigned int freq; + unsigned int default_freq; + int *freq_list; + + /* pauseState information for Publish function */ + struct os_reltime pause_state_end; + u8 sel_peer_id; + u8 sel_peer_addr[ETH_ALEN]; + + /* Publish state - channel iteration */ + bool in_multi_chan; + bool first_multi_chan; + int multi_chan_idx; /* index to freq_list[] */ + struct os_reltime next_publish_state; + struct os_reltime next_publish_chan; + unsigned int next_publish_duration; +}; + +struct nan_de { + u8 nmi[ETH_ALEN]; + bool ap; + struct nan_callbacks cb; + + struct nan_de_service *service[NAN_DE_MAX_SERVICE]; + unsigned int num_service; + + int next_handle; + + unsigned int ext_listen_freq; + unsigned int listen_freq; + unsigned int tx_wait_status_freq; + unsigned int tx_wait_end_freq; +}; + + +struct nan_de * nan_de_init(const u8 *nmi, bool ap, + const struct nan_callbacks *cb) +{ + struct nan_de *de; + + de = os_zalloc(sizeof(*de)); + if (!de) + return NULL; + + os_memcpy(de->nmi, nmi, ETH_ALEN); + de->ap = ap; + os_memcpy(&de->cb, cb, sizeof(*cb)); + + return de; +} + + +static void nan_de_service_free(struct nan_de_service *srv) +{ + os_free(srv->service_name); + wpabuf_free(srv->ssi); + wpabuf_free(srv->elems); + os_free(srv->freq_list); + os_free(srv); +} + + +static void nan_de_service_deinit(struct nan_de *de, struct nan_de_service *srv, + enum nan_de_reason reason) +{ + if (!srv) + return; + if (srv->type == NAN_DE_PUBLISH && de->cb.publish_terminated) + de->cb.publish_terminated(de->cb.ctx, srv->id, reason); + if (srv->type == NAN_DE_SUBSCRIBE && de->cb.subscribe_terminated) + de->cb.subscribe_terminated(de->cb.ctx, srv->id, reason); + nan_de_service_free(srv); +} + + +static void nan_de_clear_pending(struct nan_de *de) +{ + de->listen_freq = 0; + de->tx_wait_status_freq = 0; + de->tx_wait_end_freq = 0; +} + + +void nan_de_flush(struct nan_de *de) +{ + unsigned int i; + + if (!de) + return; + + for (i = 0; i < NAN_DE_MAX_SERVICE; i++) { + nan_de_service_deinit(de, de->service[i], + NAN_DE_REASON_USER_REQUEST); + de->service[i] = NULL; + } + + de->num_service = 0; + nan_de_clear_pending(de); +} + + +static void nan_de_pause_state(struct nan_de_service *srv, const u8 *peer_addr, + u8 peer_id) +{ + wpa_printf(MSG_DEBUG, "NAN: Start pauseState"); + os_get_reltime(&srv->pause_state_end); + srv->pause_state_end.sec += 60; + os_memcpy(srv->sel_peer_addr, peer_addr, ETH_ALEN); + srv->sel_peer_id = peer_id; +} + + +static void nan_de_unpause_state(struct nan_de_service *srv) +{ + wpa_printf(MSG_DEBUG, "NAN: Stop pauseState"); + srv->pause_state_end.sec = 0; + srv->pause_state_end.usec = 0; + os_memset(srv->sel_peer_addr, 0, ETH_ALEN); + srv->sel_peer_id = 0; +} + + +static struct wpabuf * nan_de_alloc_sdf(size_t len) +{ + struct wpabuf *buf; + + buf = wpabuf_alloc(2 + 4 + len); + if (buf) { + wpabuf_put_u8(buf, WLAN_ACTION_PUBLIC); + wpabuf_put_u8(buf, WLAN_PA_VENDOR_SPECIFIC); + wpabuf_put_be32(buf, NAN_SDF_VENDOR_TYPE); + } + + return buf; +} + + +static int nan_de_tx(struct nan_de *de, unsigned int freq, + unsigned int wait_time, + const u8 *dst, const u8 *src, const u8 *bssid, + const struct wpabuf *buf) +{ + int res; + + if (!de->cb.tx) + return -1; + + res = de->cb.tx(de->cb.ctx, freq, wait_time, dst, src, bssid, buf); + if (res < 0) + return res; + + de->tx_wait_status_freq = freq; + de->tx_wait_end_freq = wait_time ? freq : 0; + + return res; +} + + +static void nan_de_tx_sdf(struct nan_de *de, struct nan_de_service *srv, + unsigned int wait_time, + enum nan_service_control_type type, + const u8 *dst, u8 req_instance_id, + const struct wpabuf *ssi) +{ + struct wpabuf *buf; + size_t len = 0, sda_len, sdea_len; + u8 ctrl = type; + u16 sdea_ctrl = 0; + + /* Service Descriptor attribute */ + sda_len = NAN_SERVICE_ID_LEN + 1 + 1 + 1; + len += NAN_ATTR_HDR_LEN + sda_len; + + /* Service Descriptor Extension attribute */ + sdea_len = 1 + 2; + if (ssi) + sdea_len += 2 + 4 + wpabuf_len(ssi); + len += NAN_ATTR_HDR_LEN + sdea_len; + + /* Element Container attribute */ + if (srv->elems) + len += NAN_ATTR_HDR_LEN + 1 + wpabuf_len(srv->elems); + + buf = nan_de_alloc_sdf(len); + if (!buf) + return; + + /* Service Descriptor attribute */ + wpabuf_put_u8(buf, NAN_ATTR_SDA); + wpabuf_put_le16(buf, sda_len); + wpabuf_put_data(buf, srv->service_id, NAN_SERVICE_ID_LEN); + wpabuf_put_u8(buf, srv->id); /* Instance ID */ + wpabuf_put_u8(buf, req_instance_id); /* Requestor Instance ID */ + wpabuf_put_u8(buf, ctrl); + + /* Service Descriptor Extension attribute */ + if (srv->type == NAN_DE_PUBLISH || ssi) { + wpabuf_put_u8(buf, NAN_ATTR_SDEA); + wpabuf_put_le16(buf, sdea_len); + wpabuf_put_u8(buf, srv->id); /* Instance ID */ + if (srv->type == NAN_DE_PUBLISH) { + if (srv->publish.fsd) + sdea_ctrl |= NAN_SDEA_CTRL_FSD_REQ; + if (srv->publish.fsd_gas) + sdea_ctrl |= NAN_SDEA_CTRL_FSD_GAS; + } + wpabuf_put_le16(buf, sdea_ctrl); + if (ssi) { + wpabuf_put_le16(buf, 4 + wpabuf_len(ssi)); + wpabuf_put_be24(buf, OUI_WFA); + wpabuf_put_u8(buf, srv->srv_proto_type); + wpabuf_put_buf(buf, ssi); + } + } + + /* Element Container attribute */ + if (srv->elems) { + wpabuf_put_u8(buf, NAN_ATTR_ELEM_CONTAINER); + wpabuf_put_le16(buf, 1 + wpabuf_len(srv->elems)); + wpabuf_put_u8(buf, 0); /* Map ID */ + wpabuf_put_buf(buf, srv->elems); + } + + /* Wi-Fi Aware specification v4.0 uses NAN Cluster ID as A3 for USD, + * but there is no synchronization in USD as as such, no NAN Cluster + * either. Use Wildcard BSSID instead. */ + nan_de_tx(de, srv->freq, wait_time, dst, de->nmi, wildcard_bssid, buf); + wpabuf_free(buf); +} + + +static int nan_de_time_to_next_chan_change(struct nan_de_service *srv) +{ + struct os_reltime tmp, diff, now; + + if (os_reltime_before(&srv->next_publish_state, + &srv->next_publish_chan)) + tmp = srv->next_publish_state; + else if (srv->in_multi_chan) + tmp = srv->next_publish_chan; + else + tmp = srv->next_publish_state; + + os_get_reltime(&now); + os_reltime_sub(&tmp, &now, &diff); + return os_reltime_in_ms(&diff); +} + + +static void nan_de_set_publish_times(struct nan_de_service *srv) +{ + os_get_reltime(&srv->next_publish_state); + srv->next_publish_chan = srv->next_publish_state; + /* Swap single/multi channel state in N * 100 TU */ + os_reltime_add_ms(&srv->next_publish_state, + srv->next_publish_duration * 1024 / 1000); + + /* Swap channel in multi channel state after 150 ms */ + os_reltime_add_ms(&srv->next_publish_chan, 150); +} + + +static void nan_de_check_chan_change(struct nan_de_service *srv) +{ + if (srv->next_publish_duration) { + /* Update end times for the first operation of the publish + * iteration */ + nan_de_set_publish_times(srv); + srv->next_publish_duration = 0; + } else if (srv->in_multi_chan) { + if (!os_reltime_initialized(&srv->pause_state_end)) { + srv->multi_chan_idx++; + if (srv->freq_list[srv->multi_chan_idx] == 0) + srv->multi_chan_idx = 0; + srv->freq = srv->freq_list[srv->multi_chan_idx]; + wpa_printf(MSG_DEBUG, + "NAN: Publish multi-channel change to %u MHz", + srv->freq); + } + os_get_reltime(&srv->next_publish_chan); + os_reltime_add_ms(&srv->next_publish_chan, 150); + } +} + + +static void nan_de_tx_multicast(struct nan_de *de, struct nan_de_service *srv, + u8 req_instance_id) +{ + enum nan_service_control_type type; + unsigned int wait_time = 100; + + if (srv->type == NAN_DE_PUBLISH) { + int ms; + + type = NAN_SRV_CTRL_PUBLISH; + + nan_de_check_chan_change(srv); + ms = nan_de_time_to_next_chan_change(srv); + if (ms < 100) + ms = 100; + wait_time = ms; + } else if (srv->type == NAN_DE_SUBSCRIBE) { + type = NAN_SRV_CTRL_SUBSCRIBE; + } else { + return; + } + + nan_de_tx_sdf(de, srv, wait_time, type, nan_network_id, + req_instance_id, srv->ssi); + os_get_reltime(&srv->last_multicast); +} + + +static void nan_de_add_srv(struct nan_de *de, struct nan_de_service *srv) +{ + int ttl; + + os_get_reltime(&srv->time_started); + ttl = srv->type == NAN_DE_PUBLISH ? srv->publish.ttl : + srv->subscribe.ttl; + if (ttl) { + srv->end_time = srv->time_started; + srv->end_time.sec += ttl; + } + + de->service[srv->id - 1] = srv; + de->num_service++; +} + + +static void nan_de_del_srv(struct nan_de *de, struct nan_de_service *srv, + enum nan_de_reason reason) +{ + de->service[srv->id - 1] = NULL; + nan_de_service_deinit(de, srv, reason); + de->num_service--; + if (de->num_service == 0) + nan_de_clear_pending(de); +} + + +static bool nan_de_srv_expired(struct nan_de_service *srv, + struct os_reltime *now) +{ + if (os_reltime_initialized(&srv->end_time)) + return os_reltime_before(&srv->end_time, now); + + if (srv->type == NAN_DE_PUBLISH) { + /* Time out after one transmission (and wait for FSD) */ + if (!os_reltime_initialized(&srv->last_multicast)) + return false; + if (!srv->publish.fsd) + return true; + if (os_reltime_initialized(&srv->last_followup) && + !os_reltime_expired(now, &srv->last_followup, 1)) + return false; + if (os_reltime_expired(now, &srv->last_multicast, 1)) + return true; + } + + if (srv->type == NAN_DE_SUBSCRIBE) { + /* Time out after first DiscoveryResult event (and wait for + * FSD) */ + if (!os_reltime_initialized(&srv->first_discovered)) + return false; + if (!srv->needs_fsd) + return true; + if (os_reltime_initialized(&srv->last_followup) && + !os_reltime_expired(now, &srv->last_followup, 1)) + return false; + if (os_reltime_expired(now, &srv->first_discovered, 1)) + return true; + } + + return false; +} + + +static int nan_de_next_multicast(struct nan_de *de, struct nan_de_service *srv, + struct os_reltime *now) +{ + unsigned int period; + struct os_reltime next, diff; + + if (srv->type == NAN_DE_PUBLISH && !srv->publish.unsolicited) + return -1; + if (srv->type == NAN_DE_SUBSCRIBE && !srv->subscribe.active) + return -1; + + if (!os_reltime_initialized(&srv->last_multicast)) + return 0; + + if (srv->type == NAN_DE_PUBLISH && srv->publish.ttl == 0) + return -1; + + if (srv->type == NAN_DE_PUBLISH && + os_reltime_initialized(&srv->pause_state_end)) + return -1; + + period = srv->type == NAN_DE_PUBLISH ? + srv->publish.announcement_period : + srv->subscribe.query_period; + if (period == 0) + period = 100; + next = srv->last_multicast; + os_reltime_add_ms(&next, period); + + if (srv->type == NAN_DE_PUBLISH) { + if (!de->tx_wait_end_freq && srv->publish.unsolicited && + os_reltime_before(&next, now)) + return 0; + next = srv->next_publish_state; + } + + if (os_reltime_before(&next, now)) + return 0; + + os_reltime_sub(&next, now, &diff); + return os_reltime_in_ms(&diff); +} + + +static int nan_de_srv_time_to_next(struct nan_de *de, + struct nan_de_service *srv, + struct os_reltime *now) +{ + struct os_reltime diff; + int next = -1, tmp; + + if (os_reltime_initialized(&srv->end_time)) { + os_reltime_sub(&srv->end_time, now, &diff); + tmp = os_reltime_in_ms(&diff); + if (next == -1 || tmp < next) + next = tmp; + } + + tmp = nan_de_next_multicast(de, srv, now); + if (tmp >= 0 && (next == -1 || tmp < next)) + next = tmp; + + if (srv->type == NAN_DE_PUBLISH && + os_reltime_initialized(&srv->last_multicast)) { + /* Time out after one transmission (and wait for FSD) */ + tmp = srv->publish.fsd ? 1000 : 100; + if (next == -1 || tmp < next) + next = tmp; + } + + if (srv->type == NAN_DE_SUBSCRIBE && + os_reltime_initialized(&srv->first_discovered)) { + /* Time out after first DiscoveryResult event (and wait for + * FSD) */ + tmp = srv->needs_fsd ? 1000 : 100; + if (next == -1 || tmp < next) + next = tmp; + } + + if (os_reltime_initialized(&srv->next_publish_state)) { + os_reltime_sub(&srv->next_publish_state, now, &diff); + if (diff.sec < 0 || (diff.sec == 0 && diff.usec < 0)) + tmp = 0; + else + tmp = os_reltime_in_ms(&diff); + if (next == -1 || tmp < next) + next = tmp; + } + + return next; +} + + +static void nan_de_start_new_publish_state(struct nan_de_service *srv, + bool force_single) +{ + unsigned int n; + + if (force_single || !srv->freq_list || srv->freq_list[0] == 0) + srv->in_multi_chan = false; + else + srv->in_multi_chan = !srv->in_multi_chan; + + /* Use hardcoded Nmin=5 and Nmax=10 and pick a random N from that range. + * Use same values for M. */ + n = 5 + os_random() % 5; + srv->next_publish_duration = n * 100; + + nan_de_set_publish_times(srv); + + if (os_reltime_initialized(&srv->pause_state_end)) + return; + + if (srv->in_multi_chan && srv->freq_list && srv->freq_list[0]) { + if (!srv->first_multi_chan) + srv->multi_chan_idx++; + if (srv->freq_list[srv->multi_chan_idx] == 0) + srv->multi_chan_idx = 0; + srv->first_multi_chan = false; + srv->freq = srv->freq_list[srv->multi_chan_idx]; + } else { + srv->freq = srv->default_freq; + } + + wpa_printf(MSG_DEBUG, + "NAN: Publish in %s channel state for %u TU; starting with %u MHz", + srv->in_multi_chan ? "multi" : "single", n * 100, srv->freq); +} + + +static void nan_de_timer(void *eloop_ctx, void *timeout_ctx) +{ + struct nan_de *de = eloop_ctx; + unsigned int i; + int next = -1; + bool started = false; + struct os_reltime now; + + os_get_reltime(&now); + + for (i = 0; i < NAN_DE_MAX_SERVICE; i++) { + struct nan_de_service *srv = de->service[i]; + int srv_next; + + if (!srv) + continue; + + if (nan_de_srv_expired(srv, &now)) { + wpa_printf(MSG_DEBUG, "NAN: Service id %d expired", + srv->id); + nan_de_del_srv(de, srv, NAN_DE_REASON_TIMEOUT); + continue; + } + + if (os_reltime_initialized(&srv->next_publish_state) && + os_reltime_before(&srv->next_publish_state, &now)) + nan_de_start_new_publish_state(srv, false); + + if (srv->type == NAN_DE_PUBLISH && + os_reltime_initialized(&srv->pause_state_end) && + (os_reltime_before(&srv->pause_state_end, &now) || + (srv->publish.fsd && + os_reltime_initialized(&srv->last_followup) && + os_reltime_expired(&now, &srv->last_followup, 1)))) + nan_de_unpause_state(srv); + + srv_next = nan_de_srv_time_to_next(de, srv, &now); + if (srv_next >= 0 && (next == -1 || srv_next < next)) + next = srv_next; + + if (srv_next == 0 && !started && + de->listen_freq == 0 && de->ext_listen_freq == 0 && + de->tx_wait_end_freq == 0 && + nan_de_next_multicast(de, srv, &now) == 0) { + started = true; + nan_de_tx_multicast(de, srv, 0); + } + + if (!started && de->cb.listen && + de->listen_freq == 0 && de->ext_listen_freq == 0 && + de->tx_wait_end_freq == 0 && + ((srv->type == NAN_DE_PUBLISH && + !srv->publish.unsolicited && srv->publish.solicited) || + (srv->type == NAN_DE_SUBSCRIBE && + !srv->subscribe.active))) { + int duration = 1000; + + if (srv->type == NAN_DE_PUBLISH) { + nan_de_check_chan_change(srv); + duration = nan_de_time_to_next_chan_change(srv); + if (duration < 150) + duration = 150; + } + + started = true; + if (de->cb.listen(de->cb.ctx, srv->freq, duration) == 0) + de->listen_freq = srv->freq; + } + + } + + if (next < 0) + return; + + if (next == 0) + next = 1; + wpa_printf(MSG_DEBUG, "NAN: Next timer in %u ms", next); + eloop_register_timeout(next / 1000, (next % 1000) * 1000, nan_de_timer, + de, NULL); +} + + +static void nan_de_run_timer(struct nan_de *de) +{ + eloop_cancel_timeout(nan_de_timer, de, NULL); + eloop_register_timeout(0, 0, nan_de_timer, de, NULL); +} + + +void nan_de_deinit(struct nan_de *de) +{ + eloop_cancel_timeout(nan_de_timer, de, NULL); + nan_de_flush(de); + os_free(de); +} + + +void nan_de_listen_started(struct nan_de *de, unsigned int freq, + unsigned int duration) +{ + if (freq != de->listen_freq) + de->ext_listen_freq = freq; +} + + +void nan_de_listen_ended(struct nan_de *de, unsigned int freq) +{ + if (freq == de->ext_listen_freq) + de->ext_listen_freq = 0; + + if (freq == de->listen_freq) { + de->listen_freq = 0; + nan_de_run_timer(de); + } +} + + +void nan_de_tx_status(struct nan_de *de, unsigned int freq, const u8 *dst) +{ + if (freq == de->tx_wait_status_freq) + de->tx_wait_status_freq = 0; +} + + +void nan_de_tx_wait_ended(struct nan_de *de) +{ + de->tx_wait_end_freq = 0; + nan_de_run_timer(de); +} + + +static const u8 * +nan_de_get_attr(const u8 *buf, size_t len, enum nan_attr_id id, + unsigned int skip) +{ + const u8 *pos = buf, *end = buf + len; + + while (end - pos >= NAN_ATTR_HDR_LEN) { + const u8 *attr = pos; + u8 attr_id; + u16 attr_len; + + attr_id = *pos++; + attr_len = WPA_GET_LE16(pos); + pos += 2; + if (attr_len > end - pos) { + wpa_printf(MSG_DEBUG, + "NAN: Truncated attribute %u (len %u; left %zu)", + attr_id, attr_len, end - pos); + break; + } + + if (attr_id == id) { + if (skip == 0) + return attr; + skip--; + } + + pos += attr_len; + } + + return NULL; +} + + +static void nan_de_get_sdea(const u8 *buf, size_t len, u8 instance_id, + u16 *sdea_control, + enum nan_service_protocol_type *srv_proto_type, + const u8 **ssi, size_t *ssi_len) +{ + unsigned int skip; + const u8 *sdea, *end; + u16 sdea_len; + + for (skip = 0; ; skip++) { + sdea = nan_de_get_attr(buf, len, NAN_ATTR_SDEA, skip); + if (!sdea) + break; + + sdea++; + sdea_len = WPA_GET_LE16(sdea); + sdea += 2; + if (sdea_len < 1 + 2) + continue; + end = sdea + sdea_len; + + if (instance_id != *sdea++) + continue; /* Mismatching Instance ID */ + + *sdea_control = WPA_GET_LE16(sdea); + sdea += 2; + + if (*sdea_control & NAN_SDEA_CTRL_RANGE_LIMIT) { + if (end - sdea < 4) + continue; + sdea += 4; + } + + if (*sdea_control & NAN_SDEA_CTRL_SRV_UPD_INDIC) { + if (end - sdea < 1) + continue; + sdea++; + } + + if (end - sdea >= 2) { + u16 srv_info_len; + + srv_info_len = WPA_GET_LE16(sdea); + sdea += 2; + + if (srv_info_len > end - sdea) + continue; + + if (srv_info_len >= 4 && + WPA_GET_BE24(sdea) == OUI_WFA) { + *srv_proto_type = sdea[3]; + *ssi = sdea + 4; + *ssi_len = srv_info_len - 4; + } + } + } +} + + +static void nan_de_rx_publish(struct nan_de *de, struct nan_de_service *srv, + const u8 *peer_addr, u8 instance_id, + u8 req_instance_id, u16 sdea_control, + enum nan_service_protocol_type srv_proto_type, + const u8 *ssi, size_t ssi_len) +{ + /* Subscribe function processing of a receive Publish message */ + if (!os_reltime_initialized(&srv->first_discovered)) { + os_get_reltime(&srv->first_discovered); + srv->needs_fsd = sdea_control & NAN_SDEA_CTRL_FSD_REQ; + nan_de_run_timer(de); + } + + if (srv->subscribe.active && req_instance_id == 0) { + /* Active subscriber replies with a Subscribe message if it + * received a matching unsolicited Publish message. */ + nan_de_tx_multicast(de, srv, instance_id); + } + + if (!srv->subscribe.active && req_instance_id == 0) { + /* Passive subscriber replies with a Follow-up message without + * Service Specific Info field if it received a matching + * unsolicited Publish message. */ + nan_de_transmit(de, srv->id, NULL, NULL, peer_addr, + instance_id); + } + + if (de->cb.discovery_result) + de->cb.discovery_result( + de->cb.ctx, srv->id, srv_proto_type, + ssi, ssi_len, instance_id, + peer_addr, + sdea_control & NAN_SDEA_CTRL_FSD_REQ, + sdea_control & NAN_SDEA_CTRL_FSD_GAS); +} + + +static bool nan_de_filter_match(struct nan_de_service *srv, + const u8 *matching_filter, + size_t matching_filter_len) +{ + const u8 *pos, *end; + + /* Since we do not currently support matching_filter_rx values for the + * local Publish function, any matching filter with at least one + * pair with length larger than zero implies a mismatch. + */ + + if (!matching_filter) + return true; + + pos = matching_filter; + end = matching_filter + matching_filter_len; + + while (pos < end) { + u8 len; + + len = *pos++; + if (len > end - pos) + break; + if (len) { + /* A non-empty Matching Filter entry: no match since + * there is no local matching_filter_rx. */ + return false; + } + } + + return true; +} + + +static void nan_de_rx_subscribe(struct nan_de *de, struct nan_de_service *srv, + const u8 *peer_addr, u8 instance_id, + const u8 *matching_filter, + size_t matching_filter_len, + enum nan_service_protocol_type srv_proto_type, + const u8 *ssi, size_t ssi_len) +{ + struct wpabuf *buf; + size_t len = 0, sda_len, sdea_len; + u8 ctrl = 0; + u16 sdea_ctrl = 0; + + /* Publish function processing of a receive Subscribe message */ + + if (!nan_de_filter_match(srv, matching_filter, matching_filter_len)) + return; + + if (!srv->publish.solicited) + return; + + if (os_reltime_initialized(&srv->pause_state_end) && + (!ether_addr_equal(peer_addr, srv->sel_peer_addr) || + instance_id != srv->sel_peer_id)) { + wpa_printf(MSG_DEBUG, + "NAN: In pauseState - ignore Subscribe message from another subscriber"); + return; + } + + /* Reply with a solicited Publish message */ + /* Service Descriptor attribute */ + sda_len = NAN_SERVICE_ID_LEN + 1 + 1 + 1; + len += NAN_ATTR_HDR_LEN + sda_len; + + /* Service Descriptor Extension attribute */ + sdea_len = 1 + 2; + if (srv->ssi) + sdea_len += 2 + 4 + wpabuf_len(srv->ssi); + len += NAN_ATTR_HDR_LEN + sdea_len; + + /* Element Container attribute */ + if (srv->elems) + len += NAN_ATTR_HDR_LEN + 1 + wpabuf_len(srv->elems); + + buf = nan_de_alloc_sdf(len); + if (!buf) + return; + + /* Service Descriptor attribute */ + wpabuf_put_u8(buf, NAN_ATTR_SDA); + wpabuf_put_le16(buf, sda_len); + wpabuf_put_data(buf, srv->service_id, NAN_SERVICE_ID_LEN); + wpabuf_put_u8(buf, srv->id); /* Instance ID */ + wpabuf_put_u8(buf, instance_id); /* Requestor Instance ID */ + ctrl |= NAN_SRV_CTRL_PUBLISH; + wpabuf_put_u8(buf, ctrl); + + /* Service Descriptor Extension attribute */ + if (srv->type == NAN_DE_PUBLISH || srv->ssi) { + wpabuf_put_u8(buf, NAN_ATTR_SDEA); + wpabuf_put_le16(buf, sdea_len); + wpabuf_put_u8(buf, srv->id); /* Instance ID */ + if (srv->type == NAN_DE_PUBLISH) { + if (srv->publish.fsd) + sdea_ctrl |= NAN_SDEA_CTRL_FSD_REQ; + if (srv->publish.fsd_gas) + sdea_ctrl |= NAN_SDEA_CTRL_FSD_GAS; + } + wpabuf_put_le16(buf, sdea_ctrl); + if (srv->ssi) { + wpabuf_put_le16(buf, 4 + wpabuf_len(srv->ssi)); + wpabuf_put_be24(buf, OUI_WFA); + wpabuf_put_u8(buf, srv->srv_proto_type); + wpabuf_put_buf(buf, srv->ssi); + } + } + + /* Element Container attribute */ + if (srv->elems) { + wpabuf_put_u8(buf, NAN_ATTR_ELEM_CONTAINER); + wpabuf_put_le16(buf, 1 + wpabuf_len(srv->elems)); + wpabuf_put_u8(buf, 0); /* Map ID */ + wpabuf_put_buf(buf, srv->elems); + } + + /* Wi-Fi Aware specification v4.0 uses NAN Cluster ID as A3 for USD, + * but there is no synchronization in USD as as such, no NAN Cluster + * either. Use Wildcard BSSID instead. */ + nan_de_tx(de, srv->freq, 100, + srv->publish.solicited_multicast ? nan_network_id : peer_addr, + de->nmi, wildcard_bssid, buf); + wpabuf_free(buf); + + nan_de_pause_state(srv, peer_addr, instance_id); + + if (!srv->publish.disable_events && de->cb.replied) + de->cb.replied(de->cb.ctx, srv->id, peer_addr, instance_id, + srv_proto_type, ssi, ssi_len); +} + + +static void nan_de_rx_follow_up(struct nan_de *de, struct nan_de_service *srv, + const u8 *peer_addr, u8 instance_id, + const u8 *ssi, size_t ssi_len) +{ + /* Follow-up function processing of a receive Follow-up message for a + * Subscribe or Publish instance */ + + if (srv->type == NAN_DE_PUBLISH && + os_reltime_initialized(&srv->pause_state_end) && + (!ether_addr_equal(peer_addr, srv->sel_peer_addr) || + instance_id != srv->sel_peer_id || + !ssi)) { + wpa_printf(MSG_DEBUG, + "NAN: In pauseState - ignore Follow-up message from another subscriber or without ssi"); + return; + } + + os_get_reltime(&srv->last_followup); + + if (srv->type == NAN_DE_PUBLISH && !ssi) + nan_de_pause_state(srv, peer_addr, instance_id); + + if (de->cb.receive) + de->cb.receive(de->cb.ctx, srv->id, instance_id, ssi, ssi_len, + peer_addr); +} + + +static void nan_de_rx_sda(struct nan_de *de, const u8 *peer_addr, + unsigned int freq, const u8 *buf, size_t len, + const u8 *sda, size_t sda_len) +{ + const u8 *service_id; + u8 instance_id, req_instance_id, ctrl; + u16 sdea_control = 0; + unsigned int i; + enum nan_service_control_type type = 0; + enum nan_service_protocol_type srv_proto_type = 0; + const u8 *ssi = NULL; + size_t ssi_len = 0; + bool first = true; + const u8 *end; + const u8 *matching_filter = NULL; + size_t matching_filter_len = 0; + + if (sda_len < NAN_SERVICE_ID_LEN + 1 + 1 + 1) + return; + end = sda + sda_len; + + service_id = sda; + sda += NAN_SERVICE_ID_LEN; + instance_id = *sda++; + req_instance_id = *sda++; + ctrl = *sda; + type = ctrl & NAN_SRV_CTRL_TYPE_MASK; + wpa_printf(MSG_DEBUG, + "NAN: SDA - Service ID %02x%02x%02x%02x%02x%02x Instance ID %u Requestor Instance ID %u Service Control 0x%x (Service Control Type %u)", + MAC2STR(service_id), instance_id, req_instance_id, + ctrl, type); + if (type != NAN_SRV_CTRL_PUBLISH && + type != NAN_SRV_CTRL_SUBSCRIBE && + type != NAN_SRV_CTRL_FOLLOW_UP) { + wpa_printf(MSG_DEBUG, + "NAN: Discard SDF with unknown Service Control Type %u", + type); + return; + } + + if (ctrl & NAN_SRV_CTRL_BINDING_BITMAP) { + if (end - sda < 2) + return; + sda += 2; + } + + if (ctrl & NAN_SRV_CTRL_MATCHING_FILTER) { + u8 flen; + + if (end - sda < 1) + return; + flen = *sda++; + if (end - sda < flen) + return; + matching_filter = sda; + matching_filter_len = flen; + sda += flen; + } + + if (ctrl & NAN_SRV_CTRL_RESP_FILTER) { + u8 flen; + + if (end - sda < 1) + return; + flen = *sda++; + if (end - sda < flen) + return; + sda += flen; + } + + if (ctrl & NAN_SRV_CTRL_SRV_INFO) { + u8 flen; + + if (end - sda < 1) + return; + flen = *sda++; + if (end - sda < flen) + return; + if (flen >= 4 && WPA_GET_BE24(sda) == OUI_WFA) { + srv_proto_type = sda[3]; + ssi = sda + 4; + ssi_len = flen - 4; + wpa_printf(MSG_DEBUG, "NAN: Service Protocol Type %d", + srv_proto_type); + wpa_hexdump(MSG_MSGDUMP, "NAN: ssi", ssi, ssi_len); + } + sda += flen; + } + + for (i = 0; i < NAN_DE_MAX_SERVICE; i++) { + struct nan_de_service *srv = de->service[i]; + + if (!srv) + continue; + if (os_memcmp(srv->service_id, service_id, + NAN_SERVICE_ID_LEN) != 0) + continue; + if (type == NAN_SRV_CTRL_PUBLISH) { + if (srv->type == NAN_DE_PUBLISH) + continue; + if (req_instance_id && srv->id != req_instance_id) + continue; + } + if (type == NAN_SRV_CTRL_SUBSCRIBE && + srv->type == NAN_DE_SUBSCRIBE) + continue; + wpa_printf(MSG_DEBUG, "NAN: Received SDF matches service ID %u", + i + 1); + + if (first) { + first = false; + nan_de_get_sdea(buf, len, instance_id, &sdea_control, + &srv_proto_type, &ssi, &ssi_len); + + if (ssi) { + wpa_printf(MSG_DEBUG, + "NAN: Service Protocol Type %d", + srv_proto_type); + wpa_hexdump(MSG_MSGDUMP, "NAN: ssi", + ssi, ssi_len); + } + } + + switch (type) { + case NAN_SRV_CTRL_PUBLISH: + nan_de_rx_publish(de, srv, peer_addr, instance_id, + req_instance_id, + sdea_control, srv_proto_type, + ssi, ssi_len); + break; + case NAN_SRV_CTRL_SUBSCRIBE: + nan_de_rx_subscribe(de, srv, peer_addr, instance_id, + matching_filter, + matching_filter_len, + srv_proto_type, + ssi, ssi_len); + break; + case NAN_SRV_CTRL_FOLLOW_UP: + nan_de_rx_follow_up(de, srv, peer_addr, instance_id, + ssi, ssi_len); + break; + } + } +} + + +void nan_de_rx_sdf(struct nan_de *de, const u8 *peer_addr, unsigned int freq, + const u8 *buf, size_t len) +{ + const u8 *sda; + u16 sda_len; + unsigned int skip; + + if (!de->num_service) + return; + + wpa_printf(MSG_DEBUG, "NAN: RX SDF from " MACSTR " freq=%u len=%zu", + MAC2STR(peer_addr), freq, len); + + wpa_hexdump(MSG_MSGDUMP, "NAN: SDF payload", buf, len); + + for (skip = 0; ; skip++) { + sda = nan_de_get_attr(buf, len, NAN_ATTR_SDA, skip); + if (!sda) + break; + + sda++; + sda_len = WPA_GET_LE16(sda); + sda += 2; + nan_de_rx_sda(de, peer_addr, freq, buf, len, sda, sda_len); + } +} + + +static int nan_de_get_handle(struct nan_de *de) +{ + int i = de->next_handle; + + if (de->num_service >= NAN_DE_MAX_SERVICE) + goto fail; + + do { + if (!de->service[i]) { + de->next_handle = (i + 1) % NAN_DE_MAX_SERVICE; + return i + 1; + } + i = (i + 1) % NAN_DE_MAX_SERVICE; + } while (i != de->next_handle); + +fail: + wpa_printf(MSG_DEBUG, "NAN: No more room for a new service"); + return -1; +} + + +static int nan_de_derive_service_id(struct nan_de_service *srv) +{ + u8 hash[SHA256_MAC_LEN]; + char *name, *pos; + int ret; + const u8 *addr[1]; + size_t len[1]; + + name = os_strdup(srv->service_name); + if (!name) + return -1; + pos = name; + while (*pos) { + *pos = tolower(*pos); + pos++; + } + + addr[0] = (u8 *) name; + len[0] = os_strlen(name); + ret = sha256_vector(1, addr, len, hash); + os_free(name); + if (ret == 0) + os_memcpy(srv->service_id, hash, NAN_SERVICE_ID_LEN); + + return ret; +} + + +int nan_de_publish(struct nan_de *de, const char *service_name, + enum nan_service_protocol_type srv_proto_type, + const struct wpabuf *ssi, const struct wpabuf *elems, + struct nan_publish_params *params) +{ + int publish_id; + struct nan_de_service *srv; + + if (!service_name) { + wpa_printf(MSG_DEBUG, "NAN: Publish() - no service_name"); + return -1; + } + + publish_id = nan_de_get_handle(de); + if (publish_id < 1) + return -1; + + srv = os_zalloc(sizeof(*srv)); + if (!srv) + return -1; + srv->type = NAN_DE_PUBLISH; + srv->freq = srv->default_freq = params->freq; + srv->service_name = os_strdup(service_name); + if (!srv->service_name) + goto fail; + if (nan_de_derive_service_id(srv) < 0) + goto fail; + os_memcpy(&srv->publish, params, sizeof(*params)); + + if (params->freq_list) { + size_t len; + + len = (int_array_len(params->freq_list) + 1) * sizeof(int); + srv->freq_list = os_memdup(params->freq_list, len); + if (!srv->freq_list) + goto fail; + } + srv->publish.freq_list = NULL; + + srv->srv_proto_type = srv_proto_type; + if (ssi) { + srv->ssi = wpabuf_dup(ssi); + if (!srv->ssi) + goto fail; + } + if (elems) { + srv->elems = wpabuf_dup(elems); + if (!srv->elems) + goto fail; + } + + /* Prepare for single and multi-channel states; starting with + * single channel */ + srv->first_multi_chan = true; + nan_de_start_new_publish_state(srv, true); + + wpa_printf(MSG_DEBUG, "NAN: Assigned new publish handle %d for %s", + publish_id, service_name); + srv->id = publish_id; + nan_de_add_srv(de, srv); + nan_de_run_timer(de); + return publish_id; +fail: + nan_de_service_free(srv); + return -1; +} + + +void nan_de_cancel_publish(struct nan_de *de, int publish_id) +{ + struct nan_de_service *srv; + + wpa_printf(MSG_DEBUG, "NAN: CancelPublish(publish_id=%d)", publish_id); + + if (publish_id < 1 || publish_id > NAN_DE_MAX_SERVICE) + return; + srv = de->service[publish_id - 1]; + if (!srv || srv->type != NAN_DE_PUBLISH) + return; + nan_de_del_srv(de, srv, NAN_DE_REASON_USER_REQUEST); +} + + +int nan_de_update_publish(struct nan_de *de, int publish_id, + const struct wpabuf *ssi) +{ + struct nan_de_service *srv; + + wpa_printf(MSG_DEBUG, "NAN: UpdatePublish(publish_id=%d)", publish_id); + + if (publish_id < 1 || publish_id > NAN_DE_MAX_SERVICE) + return -1; + srv = de->service[publish_id - 1]; + if (!srv || srv->type != NAN_DE_PUBLISH) + return -1; + + wpabuf_free(srv->ssi); + srv->ssi = NULL; + if (!ssi) + return 0; + srv->ssi = wpabuf_dup(ssi); + if (!srv->ssi) + return -1; + return 0; +} + + +int nan_de_subscribe(struct nan_de *de, const char *service_name, + enum nan_service_protocol_type srv_proto_type, + const struct wpabuf *ssi, const struct wpabuf *elems, + struct nan_subscribe_params *params) +{ + int subscribe_id; + struct nan_de_service *srv; + + if (!service_name) { + wpa_printf(MSG_DEBUG, "NAN: Subscribe() - no service_name"); + return -1; + } + + subscribe_id = nan_de_get_handle(de); + if (subscribe_id < 1) + return -1; + + srv = os_zalloc(sizeof(*srv)); + if (!srv) + return -1; + srv->type = NAN_DE_SUBSCRIBE; + srv->freq = params->freq; + srv->service_name = os_strdup(service_name); + if (!srv->service_name) + goto fail; + if (nan_de_derive_service_id(srv) < 0) + goto fail; + os_memcpy(&srv->subscribe, params, sizeof(*params)); + srv->srv_proto_type = srv_proto_type; + if (ssi) { + srv->ssi = wpabuf_dup(ssi); + if (!srv->ssi) + goto fail; + } + if (elems) { + srv->elems = wpabuf_dup(elems); + if (!srv->elems) + goto fail; + } + + wpa_printf(MSG_DEBUG, "NAN: Assigned new subscribe handle %d for %s", + subscribe_id, service_name); + srv->id = subscribe_id; + nan_de_add_srv(de, srv); + nan_de_run_timer(de); + return subscribe_id; +fail: + nan_de_service_free(srv); + return -1; +} + + +void nan_de_cancel_subscribe(struct nan_de *de, int subscribe_id) +{ + struct nan_de_service *srv; + + if (subscribe_id < 1 || subscribe_id > NAN_DE_MAX_SERVICE) + return; + srv = de->service[subscribe_id - 1]; + if (!srv || srv->type != NAN_DE_SUBSCRIBE) + return; + nan_de_del_srv(de, srv, NAN_DE_REASON_USER_REQUEST); +} + + +int nan_de_transmit(struct nan_de *de, int handle, + const struct wpabuf *ssi, const struct wpabuf *elems, + const u8 *peer_addr, u8 req_instance_id) +{ + struct nan_de_service *srv; + + if (handle < 1 || handle > NAN_DE_MAX_SERVICE) + return -1; + + srv = de->service[handle - 1]; + if (!srv) + return -1; + + nan_de_tx_sdf(de, srv, 100, NAN_SRV_CTRL_FOLLOW_UP, + peer_addr, req_instance_id, ssi); + + os_get_reltime(&srv->last_followup); + return 0; +} diff --git a/src/common/nan_de.h b/src/common/nan_de.h new file mode 100644 index 000000000..62235064b --- /dev/null +++ b/src/common/nan_de.h @@ -0,0 +1,145 @@ +/* + * NAN Discovery Engine + * Copyright (c) 2024, Qualcomm Innovation Center, Inc. + * + * This software may be distributed under the terms of the BSD license. + * See README for more details. + */ + +#ifndef NAN_DE_H +#define NAN_DE_H + +#include "nan.h" + +/* Maximum number of active local publish and subscribe instances */ +#ifndef NAN_DE_MAX_SERVICE +#define NAN_DE_MAX_SERVICE 20 +#endif /* NAN_DE_MAX_SERVICE */ + +struct nan_de; + +enum nan_de_reason { + NAN_DE_REASON_TIMEOUT, + NAN_DE_REASON_USER_REQUEST, + NAN_DE_REASON_FAILURE, +}; + +struct nan_callbacks { + void *ctx; + + int (*tx)(void *ctx, unsigned int freq, unsigned int wait_time, + const u8 *dst, const u8 *src, const u8 *bssid, + const struct wpabuf *buf); + int (*listen)(void *ctx, unsigned int freq, unsigned int duration); + + /* NAN DE Events */ + void (*discovery_result)(void *ctx, int subscribe_id, + enum nan_service_protocol_type srv_proto_type, + const u8 *ssi, size_t ssi_len, + int peer_publish_id, + const u8 *peer_addr, bool fsd, bool fsd_gas); + + void (*replied)(void *ctx, int publish_id, const u8 *peer_addr, + int peer_subscribe_id, + enum nan_service_protocol_type srv_proto_type, + const u8 *ssi, size_t ssi_len); + + void (*publish_terminated)(void *ctx, int publish_id, + enum nan_de_reason reason); + + void (*subscribe_terminated)(void *ctx, int subscribe_id, + enum nan_de_reason reason); + + void (*receive)(void *ctx, int id, int peer_instance_id, + const u8 *ssi, size_t ssi_len, + const u8 *peer_addr); +}; + +struct nan_de * nan_de_init(const u8 *nmi, bool ap, + const struct nan_callbacks *cb); +void nan_de_flush(struct nan_de *de); +void nan_de_deinit(struct nan_de *de); + +void nan_de_listen_started(struct nan_de *de, unsigned int freq, + unsigned int duration); +void nan_de_listen_ended(struct nan_de *de, unsigned int freq); +void nan_de_tx_status(struct nan_de *de, unsigned int freq, const u8 *dst); +void nan_de_tx_wait_ended(struct nan_de *de); + +void nan_de_rx_sdf(struct nan_de *de, const u8 *peer_addr, unsigned int freq, + const u8 *buf, size_t len); + +struct nan_publish_params { + /* configuration_parameters */ + + /* Publish type */ + bool unsolicited; + bool solicited; + + /* Solicited transmission type */ + bool solicited_multicast; + + /* Time to live (in seconds); 0 = one TX only */ + unsigned int ttl; + + /* Event conditions */ + bool disable_events; + + /* Further Service Discovery flag */ + bool fsd; + + /* Further Service Discovery function */ + bool fsd_gas; + + /* Default frequency (defaultPublishChannel) */ + unsigned int freq; + + /* Multi-channel frequencies (publishChannelList) */ + const int *freq_list; + + /* Announcement period in ms; 0 = use default */ + unsigned int announcement_period; +}; + +/* Returns -1 on failure or >0 publish_id */ +int nan_de_publish(struct nan_de *de, const char *service_name, + enum nan_service_protocol_type srv_proto_type, + const struct wpabuf *ssi, const struct wpabuf *elems, + struct nan_publish_params *params); + +void nan_de_cancel_publish(struct nan_de *de, int publish_id); + +int nan_de_update_publish(struct nan_de *de, int publish_id, + const struct wpabuf *ssi); + +struct nan_subscribe_params { + /* configuration_parameters */ + + /* Subscribe type */ + bool active; + + /* Time to live (in seconds); 0 = until first result */ + unsigned int ttl; + + /* Selected frequency */ + unsigned int freq; + + /* Query period in ms; 0 = use default */ + unsigned int query_period; +}; + +/* Returns -1 on failure or >0 subscribe_id */ +int nan_de_subscribe(struct nan_de *de, const char *service_name, + enum nan_service_protocol_type srv_proto_type, + const struct wpabuf *ssi, const struct wpabuf *elems, + struct nan_subscribe_params *params); + +void nan_de_cancel_subscribe(struct nan_de *de, int subscribe_id); + +/* handle = publish_id or subscribe_id + * req_instance_id = peer publish_id or subscribe_id */ +int nan_de_transmit(struct nan_de *de, int handle, + const struct wpabuf *ssi, const struct wpabuf *elems, + const u8 *peer_addr, u8 req_instance_id); + +#endif /* NAN_DE_H */