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
22struct 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
33struct 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
44struct 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
52static void gas_server_free_response(struct gas_server_response *response);
53
54
55static 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
73static 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
84static void
85gas_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));
100	if (!response) {
101		wpabuf_free(query_resp);
102		return;
103	}
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) {
124		wpabuf_free(query_resp);
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
158static int
159gas_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
237static void
238gas_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) {
263		dl_list_del(&response->list);
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
288static int
289gas_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 */
321int 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
359static 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
383void 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
418struct 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
437void 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
461int 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}
488