src/common/nan_de.c

/*
 * 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 };

enum nan_de_service_type {
	NAN_DE_PUBLISH,
	NAN_DE_SUBSCRIBE,
};

static const u8 p2p_network_id[ETH_ALEN] =
{ 0x51, 0x6f, 0x9a, 0x02, 0x00, 0x00 };

static const u8 wildcard_bssid[ETH_ALEN] =
{ 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };

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;
	bool needs_fsd;
	unsigned int freq;
	unsigned int default_freq;
	int *freq_list;
	u8 a3[ETH_ALEN];
	bool a3_set;

	/* 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;
	bool is_p2p;
};

struct nan_de {
	u8 nmi[ETH_ALEN];
	bool offload;
	bool ap;
	unsigned int max_listen;
	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;
};


bool nan_de_is_nan_network_id(const u8 *addr)
{
	return ether_addr_equal(addr, nan_network_id);
}


bool nan_de_is_p2p_network_id(const u8 *addr)
{
	return ether_addr_equal(addr, p2p_network_id);
}


struct nan_de * nan_de_init(const u8 *nmi, bool offload, bool ap,
			    unsigned int max_listen,
			    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->offload = offload;
	de->ap = ap;
	de->max_listen = max_listen ? max_listen : 1000;
	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;
	if (os_reltime_initialized(&srv->end_time) &&
	    os_reltime_before(&srv->end_time, &srv->pause_state_end))
		srv->pause_state_end = srv->end_time;
	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, const u8 *a3, 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);
	}

	nan_de_tx(de, srv->freq, wait_time, dst, de->nmi, a3, 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;
	const u8 *network_id;
	const u8 *bssid;

	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;
	}

	if (srv->is_p2p) {
		network_id = p2p_network_id;
		bssid = wildcard_bssid;
	} else {
		network_id = nan_network_id;
		bssid = nan_network_id;
	}

	nan_de_tx_sdf(de, srv, wait_time, type, network_id, bssid,
		      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 (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;
	}

	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;
	}

	if (srv->type == NAN_DE_PUBLISH &&
	    srv->publish.fsd &&
	    os_reltime_initialized(&srv->pause_state_end)) {
		os_reltime_sub(&srv->pause_state_end, now, &diff);
		tmp = os_reltime_in_ms(&diff);
		if (next == -1 || tmp < next)
			next = tmp;
		return next;
	}

	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)))
			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->type == NAN_DE_PUBLISH &&
		    srv->publish.fsd &&
		    os_reltime_initialized(&srv->pause_state_end) &&
		    de->tx_wait_end_freq == 0 &&
		    de->listen_freq == 0 && de->ext_listen_freq == 0) {
			struct os_reltime diff;
			int duration;

			os_reltime_sub(&srv->pause_state_end, &now, &diff);
			duration = os_reltime_in_ms(&diff);
			if (duration < 0)
				continue;
			if ((unsigned int) duration > de->max_listen)
				duration = de->max_listen;
			if (de->cb.listen(de->cb.ctx, srv->freq, duration) ==
			    0) {
				wpa_printf(MSG_DEBUG,
					   "NAN: Publisher in pauseState - started listen on %u MHz",
					   srv->freq);
				de->listen_freq = srv->freq;
				return;
			}
		}

		if (srv_next == 0 && !started && !de->offload &&
		    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->offload && 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;

	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)
{
	if (de->tx_wait_end_freq)
		wpa_printf(MSG_DEBUG,
			   "NAN: TX wait for response ended (freq=%u)",
			   de->tx_wait_end_freq);
	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_process_elem_container(struct nan_de *de, const u8 *buf,
					  size_t len, const u8 *peer_addr,
					  unsigned int freq, bool p2p)
{
	const u8 *elem;
	u16 elem_len;

	elem = nan_de_get_attr(buf, len, NAN_ATTR_ELEM_CONTAINER, 0);
	if (!elem)
		return;

	elem++;
	elem_len = WPA_GET_LE16(elem);
	elem += 2;
	/* Skip the attribute if there is not enough froom for an element. */
	if (elem_len < 1 + 2)
		return;

	/* Skip Map ID */
	elem++;
	elem_len--;

	if (p2p && de->cb.process_p2p_usd_elems)
		de->cb.process_p2p_usd_elems(de->cb.ctx, elem, elem_len,
					     peer_addr, freq);
}


static void nan_de_rx_publish(struct nan_de *de, struct nan_de_service *srv,
			      const u8 *peer_addr, const u8 *a3, 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 (!de->offload && 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 (!de->offload && !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
	 * <length,value> 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, const u8 *a3,
				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;
	const u8 *network_id;

	/* 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;
	}

	if (de->offload)
		goto offload;

	/* 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);
	}

	if (srv->is_p2p)
		network_id = p2p_network_id;
	else
		network_id = nan_network_id;

	if (srv->publish.solicited_multicast || !a3)
		a3 = network_id;
	else if (srv->is_p2p)
		a3 = de->nmi;

	nan_de_tx(de, srv->freq, 100,
		  srv->publish.solicited_multicast ? network_id : peer_addr,
		  de->nmi, a3, buf);
	wpabuf_free(buf);

	if (!srv->is_p2p)
		nan_de_pause_state(srv, peer_addr, instance_id);

offload:
	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, const u8 *a3,
				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;
	}

	if (srv->type == NAN_DE_PUBLISH && !ssi)
		nan_de_pause_state(srv, peer_addr, instance_id);

	os_memcpy(srv->a3, a3, ETH_ALEN);
	srv->a3_set = true;

	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, const u8 *a3,
			  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);
			}
			nan_de_process_elem_container(de, buf, len, peer_addr,
						      freq, srv->is_p2p);
		}

		switch (type) {
		case NAN_SRV_CTRL_PUBLISH:
			nan_de_rx_publish(de, srv, peer_addr, a3, 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, a3, 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, a3, instance_id,
					    ssi, ssi_len);
			break;
		}
	}
}


void nan_de_rx_sdf(struct nan_de *de, const u8 *peer_addr, const u8 *a3,
		   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, a3, 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;
}


const u8 * nan_de_get_service_id(struct nan_de *de, int id)
{
	struct nan_de_service *srv;

	if (id < 1 || id > NAN_DE_MAX_SERVICE)
		return NULL;
	srv = de->service[id - 1];
	if (!srv)
		return NULL;
	return srv->service_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, bool p2p)
{
	int publish_id;
	struct nan_de_service *srv;

	if (!service_name) {
		wpa_printf(MSG_DEBUG, "NAN: Publish() - no service_name");
		return -1;
	}

	if (!params->unsolicited && !params->solicited) {
		wpa_printf(MSG_INFO,
			   "NAN: Publish() - both unsolicited and solicited disabled is invalid");
		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;
	srv->is_p2p = p2p;
	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, bool p2p)
{
	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));

	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->subscribe.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;
	}

	wpa_printf(MSG_DEBUG, "NAN: Assigned new subscribe handle %d for %s",
		   subscribe_id, service_name);
	srv->id = subscribe_id;
	srv->is_p2p = p2p;
	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;
	const u8 *a3;
	const u8 *network_id;

	if (handle < 1 || handle > NAN_DE_MAX_SERVICE)
		return -1;

	srv = de->service[handle - 1];
	if (!srv)
		return -1;

	if (srv->is_p2p)
		network_id = p2p_network_id;
	else
		network_id = nan_network_id;

	if (srv->a3_set)
		a3 = srv->a3;
	else
		a3 = network_id;
	nan_de_tx_sdf(de, srv, 100, NAN_SRV_CTRL_FOLLOW_UP,
		      peer_addr, a3, req_instance_id, ssi);

	return 0;
}