[thirdparty/hostap.git] / src / common / gas_server.c

/*
 * Generic advertisement service (GAS) server
 * Copyright (c) 2017, Qualcomm Atheros, Inc.
 *
 * This software may be distributed under the terms of the BSD license.
 * See README for more details.
 */

#include "includes.h"

#include "utils/common.h"
#include "utils/list.h"
#include "utils/eloop.h"
#include "ieee802_11_defs.h"
#include "gas.h"
#include "gas_server.h"


#define MAX_ADV_PROTO_ID_LEN 10
#define GAS_QUERY_TIMEOUT 10

struct gas_server_handler {
	struct dl_list list;
	u8 adv_proto_id[MAX_ADV_PROTO_ID_LEN];
	u8 adv_proto_id_len;
	struct wpabuf * (*req_cb)(void *ctx, const u8 *sa,
				  const u8 *query, size_t query_len);
	void (*status_cb)(void *ctx, struct wpabuf *resp, int ok);
	void *ctx;
	struct gas_server *gas;
};

struct gas_server_response {
	struct dl_list list;
	size_t offset;
	u8 frag_id;
	struct wpabuf *resp;
	int freq;
	u8 dst[ETH_ALEN];
	u8 dialog_token;
	struct gas_server_handler *handler;
};

struct gas_server {
	struct dl_list handlers; /* struct gas_server_handler::list */
	struct dl_list responses; /* struct gas_server_response::list */
	void (*tx)(void *ctx, int freq, const u8 *da, struct wpabuf *resp,
		   unsigned int wait_time);
	void *ctx;
};

static void gas_server_free_response(struct gas_server_response *response);


static void gas_server_response_timeout(void *eloop_ctx, void *user_ctx)
{
	struct gas_server_response *response = eloop_ctx;

	wpa_printf(MSG_DEBUG, "GAS: Response @%p timeout for " MACSTR
		   " (dialog_token=%u freq=%d frag_id=%u sent=%lu/%lu) - drop pending data",
		   response, MAC2STR(response->dst), response->dialog_token,
		   response->freq, response->frag_id,
		   (unsigned long) response->offset,
		   (unsigned long) wpabuf_len(response->resp));
	response->handler->status_cb(response->handler->ctx,
				     response->resp, 0);
	response->resp = NULL;
	dl_list_del(&response->list);
	gas_server_free_response(response);
}


static void gas_server_free_response(struct gas_server_response *response)
{
	if (!response)
		return;
	wpa_printf(MSG_DEBUG, "DPP: Free GAS response @%p", response);
	eloop_cancel_timeout(gas_server_response_timeout, response, NULL);
	wpabuf_free(response->resp);
	os_free(response);
}


static void
gas_server_send_resp(struct gas_server *gas, struct gas_server_handler *handler,
		     const u8 *da, int freq, u8 dialog_token,
		     struct wpabuf *query_resp)
{
	size_t max_len = (freq > 56160) ? 928 : 1400;
	size_t hdr_len = 24 + 2 + 5 + 3 + handler->adv_proto_id_len + 2;
	size_t resp_frag_len;
	struct wpabuf *resp;
	u16 comeback_delay;
	struct gas_server_response *response;

	if (!query_resp)
		return;

	response = os_zalloc(sizeof(*response));
	if (!response) {
		wpabuf_free(query_resp);
		return;
	}
	wpa_printf(MSG_DEBUG, "DPP: Allocated GAS response @%p", response);
	response->freq = freq;
	response->handler = handler;
	os_memcpy(response->dst, da, ETH_ALEN);
	response->dialog_token = dialog_token;
	if (hdr_len + wpabuf_len(query_resp) > max_len) {
		/* Need to use comeback to initiate fragmentation */
		comeback_delay = 1;
		resp_frag_len = 0;
	} else {
		/* Full response fits into the initial response */
		comeback_delay = 0;
		resp_frag_len = wpabuf_len(query_resp);
	}

	resp = gas_build_initial_resp(dialog_token, WLAN_STATUS_SUCCESS,
				      comeback_delay,
				      handler->adv_proto_id_len +
				      resp_frag_len);
	if (!resp) {
		wpabuf_free(query_resp);
		gas_server_free_response(response);
		return;
	}

	/* Advertisement Protocol element */
	wpabuf_put_u8(resp, WLAN_EID_ADV_PROTO);
	wpabuf_put_u8(resp, 1 + handler->adv_proto_id_len); /* Length */
	wpabuf_put_u8(resp, 0x7f);
	/* Advertisement Protocol ID */
	wpabuf_put_data(resp, handler->adv_proto_id, handler->adv_proto_id_len);

	/* Query Response Length */
	wpabuf_put_le16(resp, resp_frag_len);
	if (!comeback_delay)
		wpabuf_put_buf(resp, query_resp);

	if (comeback_delay) {
		wpa_printf(MSG_DEBUG,
			   "GAS: Need to fragment query response");
	} else {
		wpa_printf(MSG_DEBUG,
			   "GAS: Full query response fits in the GAS Initial Response frame");
	}
	response->offset = resp_frag_len;
	response->resp = query_resp;
	dl_list_add(&gas->responses, &response->list);
	gas->tx(gas->ctx, freq, da, resp, comeback_delay ? 2000 : 0);
	wpabuf_free(resp);
	eloop_register_timeout(GAS_QUERY_TIMEOUT, 0,
			       gas_server_response_timeout, response, NULL);
}


static int
gas_server_rx_initial_req(struct gas_server *gas, const u8 *da, const u8 *sa,
			  const u8 *bssid, int freq, u8 dialog_token,
			  const u8 *data, size_t len)
{
	const u8 *pos, *end, *adv_proto, *query_req;
	u8 adv_proto_len;
	u16 query_req_len;
	struct gas_server_handler *handler;
	struct wpabuf *resp;

	wpa_hexdump(MSG_MSGDUMP, "GAS: Received GAS Initial Request frame",
		    data, len);
	pos = data;
	end = data + len;

	if (end - pos < 2 || pos[0] != WLAN_EID_ADV_PROTO) {
		wpa_printf(MSG_DEBUG,
			   "GAS: No Advertisement Protocol element found");
		return -1;
	}
	pos++;
	adv_proto_len = *pos++;
	if (end - pos < adv_proto_len || adv_proto_len < 2) {
		wpa_printf(MSG_DEBUG,
			   "GAS: Truncated Advertisement Protocol element");
		return -1;
	}

	adv_proto = pos;
	pos += adv_proto_len;
	wpa_hexdump(MSG_MSGDUMP, "GAS: Advertisement Protocol element",
		    adv_proto, adv_proto_len);

	if (end - pos < 2) {
		wpa_printf(MSG_DEBUG, "GAS: No Query Request Length field");
		return -1;
	}
	query_req_len = WPA_GET_LE16(pos);
	pos += 2;
	if (end - pos < query_req_len) {
		wpa_printf(MSG_DEBUG, "GAS: Truncated Query Request field");
		return -1;
	}
	query_req = pos;
	pos += query_req_len;
	wpa_hexdump(MSG_MSGDUMP, "GAS: Query Request",
		    query_req, query_req_len);

	if (pos < end) {
		wpa_hexdump(MSG_MSGDUMP,
			    "GAS: Ignored extra data after Query Request field",
			    pos, end - pos);
	}

	dl_list_for_each(handler, &gas->handlers, struct gas_server_handler,
			 list) {
		if (adv_proto_len < 1 + handler->adv_proto_id_len ||
		    os_memcmp(adv_proto + 1, handler->adv_proto_id,
			      handler->adv_proto_id_len) != 0)
			continue;

		wpa_printf(MSG_DEBUG,
			   "GAS: Calling handler for the requested Advertisement Protocol ID");
		resp = handler->req_cb(handler->ctx, sa, query_req,
				       query_req_len);
		wpa_hexdump_buf(MSG_MSGDUMP, "GAS: Response from the handler",
				resp);
		gas_server_send_resp(gas, handler, sa, freq, dialog_token,
				     resp);
		return 0;
	}

	wpa_printf(MSG_DEBUG,
		   "GAS: No registered handler for the requested Advertisement Protocol ID");
	return -1;
}


static void
gas_server_handle_rx_comeback_req(struct gas_server_response *response)
{
	struct gas_server_handler *handler = response->handler;
	struct gas_server *gas = handler->gas;
	size_t max_len = (response->freq > 56160) ? 928 : 1400;
	size_t hdr_len = 24 + 2 + 6 + 3 + handler->adv_proto_id_len + 2;
	size_t remaining, resp_frag_len;
	struct wpabuf *resp;

	remaining = wpabuf_len(response->resp) - response->offset;
	if (hdr_len + remaining > max_len)
		resp_frag_len = max_len - hdr_len;
	else
		resp_frag_len = remaining;
	wpa_printf(MSG_DEBUG,
		   "GAS: Sending out %u/%u remaining Query Response octets",
		   (unsigned int) resp_frag_len, (unsigned int) remaining);

	resp = gas_build_comeback_resp(response->dialog_token,
				       WLAN_STATUS_SUCCESS,
				       response->frag_id++,
				       resp_frag_len < remaining, 0,
				       handler->adv_proto_id_len +
				       resp_frag_len);
	if (!resp) {
		dl_list_del(&response->list);
		gas_server_free_response(response);
		return;
	}

	/* Advertisement Protocol element */
	wpabuf_put_u8(resp, WLAN_EID_ADV_PROTO);
	wpabuf_put_u8(resp, 1 + handler->adv_proto_id_len); /* Length */
	wpabuf_put_u8(resp, 0x7f);
	/* Advertisement Protocol ID */
	wpabuf_put_data(resp, handler->adv_proto_id, handler->adv_proto_id_len);

	/* Query Response Length */
	wpabuf_put_le16(resp, resp_frag_len);
	wpabuf_put_data(resp, wpabuf_head_u8(response->resp) + response->offset,
			resp_frag_len);

	response->offset += resp_frag_len;

	gas->tx(gas->ctx, response->freq, response->dst, resp,
		remaining > resp_frag_len ? 2000 : 0);
	wpabuf_free(resp);
}


static int
gas_server_rx_comeback_req(struct gas_server *gas, const u8 *da, const u8 *sa,
			   const u8 *bssid, int freq, u8 dialog_token)
{
	struct gas_server_response *response;

	dl_list_for_each(response, &gas->responses, struct gas_server_response,
			 list) {
		if (response->dialog_token != dialog_token ||
		    os_memcmp(sa, response->dst, ETH_ALEN) != 0)
			continue;
		gas_server_handle_rx_comeback_req(response);
		return 0;
	}

	wpa_printf(MSG_DEBUG, "GAS: No pending GAS response for " MACSTR
		   " (dialog token %u)", MAC2STR(sa), dialog_token);
	return -1;
}


/**
 * gas_query_rx - Indicate reception of a Public Action or Protected Dual frame
 * @gas: GAS query data from gas_server_init()
 * @da: Destination MAC address of the Action frame
 * @sa: Source MAC address of the Action frame
 * @bssid: BSSID of the Action frame
 * @categ: Category of the Action frame
 * @data: Payload of the Action frame
 * @len: Length of @data
 * @freq: Frequency (in MHz) on which the frame was received
 * Returns: 0 if the Public Action frame was a GAS request frame or -1 if not
 */
int gas_server_rx(struct gas_server *gas, const u8 *da, const u8 *sa,
		  const u8 *bssid, u8 categ, const u8 *data, size_t len,
		  int freq)
{
	u8 action, dialog_token;
	const u8 *pos, *end;

	if (!gas || len < 2)
		return -1;

	if (categ == WLAN_ACTION_PROTECTED_DUAL)
		return -1; /* Not supported for now */

	pos = data;
	end = data + len;
	action = *pos++;
	dialog_token = *pos++;

	if (action != WLAN_PA_GAS_INITIAL_REQ &&
	    action != WLAN_PA_GAS_COMEBACK_REQ)
		return -1; /* Not a GAS request */

	wpa_printf(MSG_DEBUG, "GAS: Received GAS %s Request frame DA=" MACSTR
		   " SA=" MACSTR " BSSID=" MACSTR
		   " freq=%d dialog_token=%u len=%u",
		   action == WLAN_PA_GAS_INITIAL_REQ ? "Initial" : "Comeback",
		   MAC2STR(da), MAC2STR(sa), MAC2STR(bssid), freq, dialog_token,
		   (unsigned int) len);

	if (action == WLAN_PA_GAS_INITIAL_REQ)
		return gas_server_rx_initial_req(gas, da, sa, bssid,
						 freq, dialog_token,
						 pos, end - pos);
	return gas_server_rx_comeback_req(gas, da, sa, bssid,
					  freq, dialog_token);
}


static void gas_server_handle_tx_status(struct gas_server_response *response,
					int ack)
{
	if (ack && response->offset < wpabuf_len(response->resp)) {
		wpa_printf(MSG_DEBUG,
			   "GAS: More fragments remaining - keep pending entry");
		return;
	}

	if (!ack)
		wpa_printf(MSG_DEBUG,
			   "GAS: No ACK received - drop pending entry");
	else
		wpa_printf(MSG_DEBUG,
			   "GAS: Last fragment of the response sent out - drop pending entry");

	response->handler->status_cb(response->handler->ctx,
				     response->resp, ack);
	response->resp = NULL;
	dl_list_del(&response->list);
	gas_server_free_response(response);
}


void gas_server_tx_status(struct gas_server *gas, const u8 *dst, const u8 *data,
			  size_t data_len, int ack)
{
	const u8 *pos;
	u8 action, code, dialog_token;
	struct gas_server_response *response;

	if (data_len < 24 + 3)
		return;
	pos = data + 24;
	action = *pos++;
	code = *pos++;
	dialog_token = *pos++;
	if (action != WLAN_ACTION_PUBLIC ||
	    (code != WLAN_PA_GAS_INITIAL_RESP &&
	     code != WLAN_PA_GAS_COMEBACK_RESP))
		return;
	wpa_printf(MSG_DEBUG, "GAS: TX status dst=" MACSTR
		   " ack=%d %s dialog_token=%u",
		   MAC2STR(dst), ack,
		   code == WLAN_PA_GAS_INITIAL_RESP ? "initial" : "comeback",
		   dialog_token);
	dl_list_for_each(response, &gas->responses, struct gas_server_response,
			 list) {
		if (response->dialog_token != dialog_token ||
		    os_memcmp(dst, response->dst, ETH_ALEN) != 0)
			continue;
		gas_server_handle_tx_status(response, ack);
		return;
	}

	wpa_printf(MSG_DEBUG, "GAS: No pending response matches TX status");
}


struct gas_server * gas_server_init(void *ctx,
				    void (*tx)(void *ctx, int freq,
					       const u8 *da,
					       struct wpabuf *buf,
					       unsigned int wait_time))
{
	struct gas_server *gas;

	gas = os_zalloc(sizeof(*gas));
	if (!gas)
		return NULL;
	gas->ctx = ctx;
	gas->tx = tx;
	dl_list_init(&gas->handlers);
	dl_list_init(&gas->responses);
	return gas;
}


void gas_server_deinit(struct gas_server *gas)
{
	struct gas_server_handler *handler, *tmp;
	struct gas_server_response *response, *tmp_r;

	if (!gas)
		return;

	dl_list_for_each_safe(handler, tmp, &gas->handlers,
			      struct gas_server_handler, list) {
		dl_list_del(&handler->list);
		os_free(handler);
	}

	dl_list_for_each_safe(response, tmp_r, &gas->responses,
			      struct gas_server_response, list) {
		dl_list_del(&response->list);
		gas_server_free_response(response);
	}

	os_free(gas);
}


int gas_server_register(struct gas_server *gas,
			const u8 *adv_proto_id, u8 adv_proto_id_len,
			struct wpabuf *
			(*req_cb)(void *ctx, const u8 *sa,
				  const u8 *query, size_t query_len),
			void (*status_cb)(void *ctx, struct wpabuf *resp,
					  int ok),
			void *ctx)
{
	struct gas_server_handler *handler;

	if (!gas || adv_proto_id_len > MAX_ADV_PROTO_ID_LEN)
		return -1;
	handler = os_zalloc(sizeof(*handler));
	if (!handler)
		return -1;

	os_memcpy(handler->adv_proto_id, adv_proto_id, adv_proto_id_len);
	handler->adv_proto_id_len = adv_proto_id_len;
	handler->req_cb = req_cb;
	handler->status_cb = status_cb;
	handler->ctx = ctx;
	handler->gas = gas;
	dl_list_add(&gas->handlers, &handler->list);

	return 0;
}
Commit	Line	Data
461d39af JM	1	/*
	2	* Generic advertisement service (GAS) server
	3	* Copyright (c) 2017, Qualcomm Atheros, Inc.
	4	*
	5	* This software may be distributed under the terms of the BSD license.
	6	* See README for more details.
	7	*/
	8
	9	#include "includes.h"
	10
	11	#include "utils/common.h"
	12	#include "utils/list.h"
	13	#include "utils/eloop.h"
	14	#include "ieee802_11_defs.h"
	15	#include "gas.h"
	16	#include "gas_server.h"
	17
	18
	19	#define MAX_ADV_PROTO_ID_LEN 10
	20	#define GAS_QUERY_TIMEOUT 10
	21
	22	struct gas_server_handler {
	23	struct dl_list list;
	24	u8 adv_proto_id[MAX_ADV_PROTO_ID_LEN];
	25	u8 adv_proto_id_len;
	26	struct wpabuf * (req_cb)(void ctx, const u8 *sa,
	27	const u8 *query, size_t query_len);
	28	void (status_cb)(void ctx, struct wpabuf *resp, int ok);
	29	void *ctx;
	30	struct gas_server *gas;
	31	};
	32
	33	struct gas_server_response {
	34	struct dl_list list;
	35	size_t offset;
	36	u8 frag_id;
	37	struct wpabuf *resp;
	38	int freq;
	39	u8 dst[ETH_ALEN];
	40	u8 dialog_token;
	41	struct gas_server_handler *handler;
	42	};
	43
	44	struct gas_server {
	45	struct dl_list handlers; /* struct gas_server_handler::list */
	46	struct dl_list responses; /* struct gas_server_response::list */
	47	void (tx)(void ctx, int freq, const u8 da, struct wpabuf resp,
	48	unsigned int wait_time);
	49	void *ctx;
	50	};
	51
	52	static void gas_server_free_response(struct gas_server_response *response);
	53
	54
	55	static void gas_server_response_timeout(void eloop_ctx, void user_ctx)
	56	{
	57	struct gas_server_response *response = eloop_ctx;
	58
	59	wpa_printf(MSG_DEBUG, "GAS: Response @%p timeout for " MACSTR
	60	" (dialog_token=%u freq=%d frag_id=%u sent=%lu/%lu) - drop pending data",
	61	response, MAC2STR(response->dst), response->dialog_token,
	62	response->freq, response->frag_id,
	63	(unsigned long) response->offset,
	64	(unsigned long) wpabuf_len(response->resp));
65	response->handler->status_cb(response->handler->ctx,
66	response->resp, 0);
67	response->resp = NULL;
68	dl_list_del(&response->list);
69	gas_server_free_response(response);
70	}
71
72
73	static void gas_server_free_response(struct gas_server_response *response)
74	{
75	if (!response)
76	return;
77	wpa_printf(MSG_DEBUG, "DPP: Free GAS response @%p", response);
78	eloop_cancel_timeout(gas_server_response_timeout, response, NULL);
79	wpabuf_free(response->resp);
80	os_free(response);
81	}
82
83
84	static void
85	gas_server_send_resp(struct gas_server gas, struct gas_server_handler handler,
86	const u8 *da, int freq, u8 dialog_token,
87	struct wpabuf *query_resp)
88	{
89	size_t max_len = (freq > 56160) ? 928 : 1400;
90	size_t hdr_len = 24 + 2 + 5 + 3 + handler->adv_proto_id_len + 2;
91	size_t resp_frag_len;
92	struct wpabuf *resp;
93	u16 comeback_delay;
94	struct gas_server_response *response;
95
96	if (!query_resp)
97	return;
98
99	response = os_zalloc(sizeof(*response));
c7896ef9 JM	100	if (!response) {
c7896ef9 JM	101	wpabuf_free(query_resp);
461d39af	102	return;
c7896ef9	103	}
461d39af JM	104	wpa_printf(MSG_DEBUG, "DPP: Allocated GAS response @%p", response);
	105	response->freq = freq;
	106	response->handler = handler;
	107	os_memcpy(response->dst, da, ETH_ALEN);
	108	response->dialog_token = dialog_token;
	109	if (hdr_len + wpabuf_len(query_resp) > max_len) {
	110	/* Need to use comeback to initiate fragmentation */
	111	comeback_delay = 1;
	112	resp_frag_len = 0;
	113	} else {
	114	/* Full response fits into the initial response */
	115	comeback_delay = 0;
	116	resp_frag_len = wpabuf_len(query_resp);
	117	}
	118
	119	resp = gas_build_initial_resp(dialog_token, WLAN_STATUS_SUCCESS,
	120	comeback_delay,
	121	handler->adv_proto_id_len +
	122	resp_frag_len);
	123	if (!resp) {
c7896ef9	124	wpabuf_free(query_resp);
461d39af JM	125	gas_server_free_response(response);
	126	return;
	127	}
	128
	129	/* Advertisement Protocol element */
	130	wpabuf_put_u8(resp, WLAN_EID_ADV_PROTO);
	131	wpabuf_put_u8(resp, 1 + handler->adv_proto_id_len); /* Length */
	132	wpabuf_put_u8(resp, 0x7f);
	133	/* Advertisement Protocol ID */
	134	wpabuf_put_data(resp, handler->adv_proto_id, handler->adv_proto_id_len);
	135
	136	/* Query Response Length */
	137	wpabuf_put_le16(resp, resp_frag_len);
	138	if (!comeback_delay)
	139	wpabuf_put_buf(resp, query_resp);
	140
	141	if (comeback_delay) {
	142	wpa_printf(MSG_DEBUG,
	143	"GAS: Need to fragment query response");
	144	} else {
	145	wpa_printf(MSG_DEBUG,
	146	"GAS: Full query response fits in the GAS Initial Response frame");
	147	}
	148	response->offset = resp_frag_len;
	149	response->resp = query_resp;
	150	dl_list_add(&gas->responses, &response->list);
	151	gas->tx(gas->ctx, freq, da, resp, comeback_delay ? 2000 : 0);
	152	wpabuf_free(resp);
	153	eloop_register_timeout(GAS_QUERY_TIMEOUT, 0,
	154	gas_server_response_timeout, response, NULL);
	155	}
	156
	157
	158	static int
	159	gas_server_rx_initial_req(struct gas_server gas, const u8 da, const u8 *sa,
	160	const u8 *bssid, int freq, u8 dialog_token,
	161	const u8 *data, size_t len)
	162	{
	163	const u8 pos, end, adv_proto, query_req;
	164	u8 adv_proto_len;
	165	u16 query_req_len;
	166	struct gas_server_handler *handler;
	167	struct wpabuf *resp;
	168
	169	wpa_hexdump(MSG_MSGDUMP, "GAS: Received GAS Initial Request frame",
	170	data, len);
	171	pos = data;
	172	end = data + len;
	173
	174	if (end - pos < 2 \|\| pos[0] != WLAN_EID_ADV_PROTO) {
	175	wpa_printf(MSG_DEBUG,
	176	"GAS: No Advertisement Protocol element found");
	177	return -1;
	178	}
	179	pos++;
	180	adv_proto_len = *pos++;
	181	if (end - pos < adv_proto_len \|\| adv_proto_len < 2) {
	182	wpa_printf(MSG_DEBUG,
	183	"GAS: Truncated Advertisement Protocol element");
	184	return -1;
	185	}
	186
	187	adv_proto = pos;
	188	pos += adv_proto_len;
189	wpa_hexdump(MSG_MSGDUMP, "GAS: Advertisement Protocol element",
190	adv_proto, adv_proto_len);
191
192	if (end - pos < 2) {
193	wpa_printf(MSG_DEBUG, "GAS: No Query Request Length field");
194	return -1;
195	}
196	query_req_len = WPA_GET_LE16(pos);
197	pos += 2;
198	if (end - pos < query_req_len) {
199	wpa_printf(MSG_DEBUG, "GAS: Truncated Query Request field");
200	return -1;
201	}
202	query_req = pos;
203	pos += query_req_len;
204	wpa_hexdump(MSG_MSGDUMP, "GAS: Query Request",
205	query_req, query_req_len);
206
207	if (pos < end) {
208	wpa_hexdump(MSG_MSGDUMP,
209	"GAS: Ignored extra data after Query Request field",
210	pos, end - pos);
211	}
212
213	dl_list_for_each(handler, &gas->handlers, struct gas_server_handler,
214	list) {
215	if (adv_proto_len < 1 + handler->adv_proto_id_len \|\|
216	os_memcmp(adv_proto + 1, handler->adv_proto_id,
217	handler->adv_proto_id_len) != 0)
218	continue;
219
220	wpa_printf(MSG_DEBUG,
221	"GAS: Calling handler for the requested Advertisement Protocol ID");
222	resp = handler->req_cb(handler->ctx, sa, query_req,
223	query_req_len);
224	wpa_hexdump_buf(MSG_MSGDUMP, "GAS: Response from the handler",
225	resp);
226	gas_server_send_resp(gas, handler, sa, freq, dialog_token,
227	resp);
228	return 0;
229	}
230
231	wpa_printf(MSG_DEBUG,
232	"GAS: No registered handler for the requested Advertisement Protocol ID");
233	return -1;
234	}
235
236
237	static void
238	gas_server_handle_rx_comeback_req(struct gas_server_response *response)
239	{
240	struct gas_server_handler *handler = response->handler;
241	struct gas_server *gas = handler->gas;
242	size_t max_len = (response->freq > 56160) ? 928 : 1400;
243	size_t hdr_len = 24 + 2 + 6 + 3 + handler->adv_proto_id_len + 2;
244	size_t remaining, resp_frag_len;
245	struct wpabuf *resp;
246
247	remaining = wpabuf_len(response->resp) - response->offset;
248	if (hdr_len + remaining > max_len)
249	resp_frag_len = max_len - hdr_len;
250	else
251	resp_frag_len = remaining;
252	wpa_printf(MSG_DEBUG,
253	"GAS: Sending out %u/%u remaining Query Response octets",
254	(unsigned int) resp_frag_len, (unsigned int) remaining);
255
256	resp = gas_build_comeback_resp(response->dialog_token,
257	WLAN_STATUS_SUCCESS,
258	response->frag_id++,
259	resp_frag_len < remaining, 0,
260	handler->adv_proto_id_len +
261	resp_frag_len);
262	if (!resp) {
11f07f02	263	dl_list_del(&response->list);
461d39af JM	264	gas_server_free_response(response);
	265	return;
	266	}
	267
	268	/* Advertisement Protocol element */
	269	wpabuf_put_u8(resp, WLAN_EID_ADV_PROTO);
	270	wpabuf_put_u8(resp, 1 + handler->adv_proto_id_len); /* Length */
	271	wpabuf_put_u8(resp, 0x7f);
	272	/* Advertisement Protocol ID */
	273	wpabuf_put_data(resp, handler->adv_proto_id, handler->adv_proto_id_len);
	274
	275	/* Query Response Length */
	276	wpabuf_put_le16(resp, resp_frag_len);
	277	wpabuf_put_data(resp, wpabuf_head_u8(response->resp) + response->offset,
	278	resp_frag_len);
	279
	280	response->offset += resp_frag_len;
	281
	282	gas->tx(gas->ctx, response->freq, response->dst, resp,
	283	remaining > resp_frag_len ? 2000 : 0);
	284	wpabuf_free(resp);
	285	}
	286
	287
	288	static int
	289	gas_server_rx_comeback_req(struct gas_server gas, const u8 da, const u8 *sa,
	290	const u8 *bssid, int freq, u8 dialog_token)
	291	{
	292	struct gas_server_response *response;
	293
	294	dl_list_for_each(response, &gas->responses, struct gas_server_response,
	295	list) {
	296	if (response->dialog_token != dialog_token \|\|
	297	os_memcmp(sa, response->dst, ETH_ALEN) != 0)
	298	continue;
	299	gas_server_handle_rx_comeback_req(response);
	300	return 0;
	301	}
	302
	303	wpa_printf(MSG_DEBUG, "GAS: No pending GAS response for " MACSTR
	304	" (dialog token %u)", MAC2STR(sa), dialog_token);
	305	return -1;
	306	}
	307
	308
	309	/**
	310	* gas_query_rx - Indicate reception of a Public Action or Protected Dual frame
	311	* @gas: GAS query data from gas_server_init()
	312	* @da: Destination MAC address of the Action frame
	313	* @sa: Source MAC address of the Action frame
	314	* @bssid: BSSID of the Action frame
	315	* @categ: Category of the Action frame
	316	* @data: Payload of the Action frame
	317	* @len: Length of @data
	318	* @freq: Frequency (in MHz) on which the frame was received
	319	* Returns: 0 if the Public Action frame was a GAS request frame or -1 if not
	320	*/
	321	int gas_server_rx(struct gas_server gas, const u8 da, const u8 *sa,
	322	const u8 bssid, u8 categ, const u8 data, size_t len,
	323	int freq)
	324	{
	325	u8 action, dialog_token;
	326	const u8 pos, end;
	327
328	if (!gas \|\| len < 2)
329	return -1;
330
331	if (categ == WLAN_ACTION_PROTECTED_DUAL)
332	return -1; /* Not supported for now */
333
334	pos = data;
335	end = data + len;
336	action = *pos++;
337	dialog_token = *pos++;
338
339	if (action != WLAN_PA_GAS_INITIAL_REQ &&
340	action != WLAN_PA_GAS_COMEBACK_REQ)
341	return -1; /* Not a GAS request */
342
343	wpa_printf(MSG_DEBUG, "GAS: Received GAS %s Request frame DA=" MACSTR
344	" SA=" MACSTR " BSSID=" MACSTR
345	" freq=%d dialog_token=%u len=%u",
346	action == WLAN_PA_GAS_INITIAL_REQ ? "Initial" : "Comeback",
347	MAC2STR(da), MAC2STR(sa), MAC2STR(bssid), freq, dialog_token,
348	(unsigned int) len);
349
350	if (action == WLAN_PA_GAS_INITIAL_REQ)
351	return gas_server_rx_initial_req(gas, da, sa, bssid,
352	freq, dialog_token,
353	pos, end - pos);
354	return gas_server_rx_comeback_req(gas, da, sa, bssid,
355	freq, dialog_token);
356	}
357
358
359	static void gas_server_handle_tx_status(struct gas_server_response *response,
360	int ack)
361	{
362	if (ack && response->offset < wpabuf_len(response->resp)) {
363	wpa_printf(MSG_DEBUG,
364	"GAS: More fragments remaining - keep pending entry");
365	return;
366	}
367
368	if (!ack)
369	wpa_printf(MSG_DEBUG,
370	"GAS: No ACK received - drop pending entry");
371	else
372	wpa_printf(MSG_DEBUG,
373	"GAS: Last fragment of the response sent out - drop pending entry");
374
375	response->handler->status_cb(response->handler->ctx,
376	response->resp, ack);
377	response->resp = NULL;
378	dl_list_del(&response->list);
379	gas_server_free_response(response);
380	}
381
382
383	void gas_server_tx_status(struct gas_server gas, const u8 dst, const u8 *data,
384	size_t data_len, int ack)
385	{
386	const u8 *pos;
387	u8 action, code, dialog_token;
388	struct gas_server_response *response;
389
390	if (data_len < 24 + 3)
391	return;
392	pos = data + 24;
393	action = *pos++;
394	code = *pos++;
395	dialog_token = *pos++;
396	if (action != WLAN_ACTION_PUBLIC \|\|
397	(code != WLAN_PA_GAS_INITIAL_RESP &&
398	code != WLAN_PA_GAS_COMEBACK_RESP))
399	return;
400	wpa_printf(MSG_DEBUG, "GAS: TX status dst=" MACSTR
401	" ack=%d %s dialog_token=%u",
402	MAC2STR(dst), ack,
403	code == WLAN_PA_GAS_INITIAL_RESP ? "initial" : "comeback",
404	dialog_token);
405	dl_list_for_each(response, &gas->responses, struct gas_server_response,
406	list) {
407	if (response->dialog_token != dialog_token \|\|
408	os_memcmp(dst, response->dst, ETH_ALEN) != 0)
409	continue;
410	gas_server_handle_tx_status(response, ack);
411	return;
412	}
413
414	wpa_printf(MSG_DEBUG, "GAS: No pending response matches TX status");
415	}
416
417
418	struct gas_server * gas_server_init(void *ctx,
419	void (tx)(void ctx, int freq,
420	const u8 *da,
421	struct wpabuf *buf,
422	unsigned int wait_time))
423	{
424	struct gas_server *gas;
425
426	gas = os_zalloc(sizeof(*gas));
427	if (!gas)
428	return NULL;
429	gas->ctx = ctx;
430	gas->tx = tx;
431	dl_list_init(&gas->handlers);
432	dl_list_init(&gas->responses);
433	return gas;
434	}
435
436
437	void gas_server_deinit(struct gas_server *gas)
438	{
439	struct gas_server_handler handler, tmp;
440	struct gas_server_response response, tmp_r;
441
442	if (!gas)
443	return;
444
445	dl_list_for_each_safe(handler, tmp, &gas->handlers,
446	struct gas_server_handler, list) {
447	dl_list_del(&handler->list);
448	os_free(handler);
449	}
450
451	dl_list_for_each_safe(response, tmp_r, &gas->responses,
452	struct gas_server_response, list) {
453	dl_list_del(&response->list);
454	gas_server_free_response(response);
455	}
456
457	os_free(gas);
458	}
459
460
461	int gas_server_register(struct gas_server *gas,
462	const u8 *adv_proto_id, u8 adv_proto_id_len,
463	struct wpabuf *
464	(req_cb)(void ctx, const u8 *sa,
465	const u8 *query, size_t query_len),
466	void (status_cb)(void ctx, struct wpabuf *resp,
467	int ok),
468	void *ctx)
469	{
470	struct gas_server_handler *handler;
471
472	if (!gas \|\| adv_proto_id_len > MAX_ADV_PROTO_ID_LEN)
473	return -1;
474	handler = os_zalloc(sizeof(*handler));
475	if (!handler)
476	return -1;
477
478	os_memcpy(handler->adv_proto_id, adv_proto_id, adv_proto_id_len);
479	handler->adv_proto_id_len = adv_proto_id_len;
480	handler->req_cb = req_cb;
481	handler->status_cb = status_cb;
482	handler->ctx = ctx;
483	handler->gas = gas;
484	dl_list_add(&gas->handlers, &handler->list);
485
486	return 0;
487	}