1 /*
2  * wpa_supplicant - Robust AV procedures
3  * Copyright (c) 2020, The Linux Foundation
4  *
5  * This software may be distributed under the terms of the BSD license.
6  * See README for more details.
7  */
8 
9 #include "utils/includes.h"
10 #include "utils/common.h"
11 #include "utils/eloop.h"
12 #include "common/wpa_ctrl.h"
13 #include "common/ieee802_11_common.h"
14 #include "wpa_supplicant_i.h"
15 #include "driver_i.h"
16 #include "bss.h"
17 
18 
19 #define SCS_RESP_TIMEOUT 1
20 #define DSCP_REQ_TIMEOUT 5
21 
22 
wpas_populate_mscs_descriptor_ie(struct robust_av_data * robust_av,struct wpabuf * buf)23 void wpas_populate_mscs_descriptor_ie(struct robust_av_data *robust_av,
24 				      struct wpabuf *buf)
25 {
26 	u8 *len, *len1;
27 
28 	/* MSCS descriptor element */
29 	wpabuf_put_u8(buf, WLAN_EID_EXTENSION);
30 	len = wpabuf_put(buf, 1);
31 	wpabuf_put_u8(buf, WLAN_EID_EXT_MSCS_DESCRIPTOR);
32 	wpabuf_put_u8(buf, robust_av->request_type);
33 	wpabuf_put_u8(buf, robust_av->up_bitmap);
34 	wpabuf_put_u8(buf, robust_av->up_limit);
35 	wpabuf_put_le32(buf, robust_av->stream_timeout);
36 
37 	if (robust_av->request_type != SCS_REQ_REMOVE) {
38 		/* TCLAS mask element */
39 		wpabuf_put_u8(buf, WLAN_EID_EXTENSION);
40 		len1 = wpabuf_put(buf, 1);
41 		wpabuf_put_u8(buf, WLAN_EID_EXT_TCLAS_MASK);
42 
43 		/* Frame classifier */
44 		wpabuf_put_data(buf, robust_av->frame_classifier,
45 				robust_av->frame_classifier_len);
46 		*len1 = (u8 *) wpabuf_put(buf, 0) - len1 - 1;
47 	}
48 
49 	*len = (u8 *) wpabuf_put(buf, 0) - len - 1;
50 }
51 
52 
wpas_populate_type4_classifier(struct type4_params * type4_param,struct wpabuf * buf)53 static int wpas_populate_type4_classifier(struct type4_params *type4_param,
54 					  struct wpabuf *buf)
55 {
56 	/* classifier parameters */
57 	wpabuf_put_u8(buf, type4_param->classifier_mask);
58 	if (type4_param->ip_version == IPV4) {
59 		wpabuf_put_u8(buf, IPV4); /* IP version */
60 		wpabuf_put_data(buf, &type4_param->ip_params.v4.src_ip.s_addr,
61 				4);
62 		wpabuf_put_data(buf, &type4_param->ip_params.v4.dst_ip.s_addr,
63 				4);
64 		wpabuf_put_be16(buf, type4_param->ip_params.v4.src_port);
65 		wpabuf_put_be16(buf, type4_param->ip_params.v4.dst_port);
66 		wpabuf_put_u8(buf, type4_param->ip_params.v4.dscp);
67 		wpabuf_put_u8(buf, type4_param->ip_params.v4.protocol);
68 		wpabuf_put_u8(buf, 0); /* Reserved octet */
69 	} else {
70 		wpabuf_put_u8(buf, IPV6);
71 		wpabuf_put_data(buf, &type4_param->ip_params.v6.src_ip.s6_addr,
72 				16);
73 		wpabuf_put_data(buf, &type4_param->ip_params.v6.dst_ip.s6_addr,
74 				16);
75 		wpabuf_put_be16(buf, type4_param->ip_params.v6.src_port);
76 		wpabuf_put_be16(buf, type4_param->ip_params.v6.dst_port);
77 		wpabuf_put_u8(buf, type4_param->ip_params.v6.dscp);
78 		wpabuf_put_u8(buf, type4_param->ip_params.v6.next_header);
79 		wpabuf_put_data(buf, type4_param->ip_params.v6.flow_label, 3);
80 	}
81 
82 	return 0;
83 }
84 
85 
wpas_populate_type10_classifier(struct type10_params * type10_param,struct wpabuf * buf)86 static int wpas_populate_type10_classifier(struct type10_params *type10_param,
87 					   struct wpabuf *buf)
88 {
89 	/* classifier parameters */
90 	wpabuf_put_u8(buf, type10_param->prot_instance);
91 	wpabuf_put_u8(buf, type10_param->prot_number);
92 	wpabuf_put_data(buf, type10_param->filter_value,
93 			type10_param->filter_len);
94 	wpabuf_put_data(buf, type10_param->filter_mask,
95 			type10_param->filter_len);
96 	return 0;
97 }
98 
99 
wpas_populate_scs_descriptor_ie(struct scs_desc_elem * desc_elem,struct wpabuf * buf)100 static int wpas_populate_scs_descriptor_ie(struct scs_desc_elem *desc_elem,
101 					   struct wpabuf *buf)
102 {
103 	u8 *len, *len1;
104 	struct tclas_element *tclas_elem;
105 	unsigned int i;
106 
107 	/* SCS Descriptor element */
108 	wpabuf_put_u8(buf, WLAN_EID_SCS_DESCRIPTOR);
109 	len = wpabuf_put(buf, 1);
110 	wpabuf_put_u8(buf, desc_elem->scs_id);
111 	wpabuf_put_u8(buf, desc_elem->request_type);
112 	if (desc_elem->request_type == SCS_REQ_REMOVE)
113 		goto end;
114 
115 	if (desc_elem->intra_access_priority || desc_elem->scs_up_avail) {
116 		wpabuf_put_u8(buf, WLAN_EID_INTRA_ACCESS_CATEGORY_PRIORITY);
117 		wpabuf_put_u8(buf, 1);
118 		wpabuf_put_u8(buf, desc_elem->intra_access_priority);
119 	}
120 
121 	tclas_elem = desc_elem->tclas_elems;
122 
123 	if (!tclas_elem)
124 		return -1;
125 
126 	for (i = 0; i < desc_elem->num_tclas_elem; i++, tclas_elem++) {
127 		int ret;
128 
129 		/* TCLAS element */
130 		wpabuf_put_u8(buf, WLAN_EID_TCLAS);
131 		len1 = wpabuf_put(buf, 1);
132 		wpabuf_put_u8(buf, 255); /* User Priority: not compared */
133 		/* Frame Classifier */
134 		wpabuf_put_u8(buf, tclas_elem->classifier_type);
135 		/* Frame classifier parameters */
136 		switch (tclas_elem->classifier_type) {
137 		case 4:
138 			ret = wpas_populate_type4_classifier(
139 				&tclas_elem->frame_classifier.type4_param,
140 				buf);
141 			break;
142 		case 10:
143 			ret = wpas_populate_type10_classifier(
144 				&tclas_elem->frame_classifier.type10_param,
145 				buf);
146 			break;
147 		default:
148 			return -1;
149 		}
150 
151 		if (ret == -1) {
152 			wpa_printf(MSG_ERROR,
153 				   "Failed to populate frame classifier");
154 			return -1;
155 		}
156 
157 		*len1 = (u8 *) wpabuf_put(buf, 0) - len1 - 1;
158 	}
159 
160 	if (desc_elem->num_tclas_elem > 1) {
161 		/* TCLAS Processing element */
162 		wpabuf_put_u8(buf, WLAN_EID_TCLAS_PROCESSING);
163 		wpabuf_put_u8(buf, 1);
164 		wpabuf_put_u8(buf, desc_elem->tclas_processing);
165 	}
166 
167 end:
168 	*len = (u8 *) wpabuf_put(buf, 0) - len - 1;
169 	return 0;
170 }
171 
172 
wpas_send_mscs_req(struct wpa_supplicant * wpa_s)173 int wpas_send_mscs_req(struct wpa_supplicant *wpa_s)
174 {
175 	struct wpabuf *buf;
176 	size_t buf_len;
177 	int ret;
178 
179 	if (wpa_s->wpa_state != WPA_COMPLETED || !wpa_s->current_ssid)
180 		return 0;
181 
182 	if (!wpa_bss_ext_capab(wpa_s->current_bss, WLAN_EXT_CAPAB_MSCS)) {
183 		wpa_dbg(wpa_s, MSG_INFO,
184 			"AP does not support MSCS - could not send MSCS Req");
185 		return -1;
186 	}
187 
188 	if (!wpa_s->mscs_setup_done &&
189 	    wpa_s->robust_av.request_type != SCS_REQ_ADD) {
190 		wpa_msg(wpa_s, MSG_INFO,
191 			"MSCS: Failed to send MSCS Request: request type invalid");
192 		return -1;
193 	}
194 
195 	buf_len = 3 +	/* Action frame header */
196 		  3 +	/* MSCS descriptor IE header */
197 		  1 +	/* Request type */
198 		  2 +	/* User priority control */
199 		  4 +	/* Stream timeout */
200 		  3 +	/* TCLAS Mask IE header */
201 		  wpa_s->robust_av.frame_classifier_len;
202 
203 	buf = wpabuf_alloc(buf_len);
204 	if (!buf) {
205 		wpa_printf(MSG_ERROR, "Failed to allocate MSCS req");
206 		return -1;
207 	}
208 
209 	wpabuf_put_u8(buf, WLAN_ACTION_ROBUST_AV_STREAMING);
210 	wpabuf_put_u8(buf, ROBUST_AV_MSCS_REQ);
211 	wpa_s->robust_av.dialog_token++;
212 	wpabuf_put_u8(buf, wpa_s->robust_av.dialog_token);
213 
214 	/* MSCS descriptor element */
215 	wpas_populate_mscs_descriptor_ie(&wpa_s->robust_av, buf);
216 
217 	wpa_hexdump_buf(MSG_MSGDUMP, "MSCS Request", buf);
218 	ret = wpa_drv_send_action(wpa_s, wpa_s->assoc_freq, 0, wpa_s->bssid,
219 				  wpa_s->own_addr, wpa_s->bssid,
220 				  wpabuf_head(buf), wpabuf_len(buf), 0);
221 	if (ret < 0)
222 		wpa_dbg(wpa_s, MSG_INFO, "MSCS: Failed to send MSCS Request");
223 
224 	wpabuf_free(buf);
225 	return ret;
226 }
227 
228 
tclas_elem_len(const struct tclas_element * elem)229 static size_t tclas_elem_len(const struct tclas_element *elem)
230 {
231 	size_t buf_len = 0;
232 
233 	buf_len += 2 +	/* TCLAS element header */
234 		1 +	/* User Priority */
235 		1 ;	/* Classifier Type */
236 
237 	if (elem->classifier_type == 4) {
238 		enum ip_version ip_ver;
239 
240 		buf_len += 1 +	/* Classifier mask */
241 			1 +	/* IP version */
242 			1 +	/* user priority */
243 			2 +	/* src_port */
244 			2 +	/* dst_port */
245 			1 ;	/* dscp */
246 		ip_ver = elem->frame_classifier.type4_param.ip_version;
247 		if (ip_ver == IPV4) {
248 			buf_len += 4 +  /* src_ip */
249 				4 +	/* dst_ip */
250 				1 +	/* protocol */
251 				1 ;  /* Reserved */
252 		} else if (ip_ver == IPV6) {
253 			buf_len += 16 +  /* src_ip */
254 				16 +  /* dst_ip */
255 				1  +  /* next_header */
256 				3  ;  /* flow_label */
257 		} else {
258 			wpa_printf(MSG_ERROR, "%s: Incorrect IP version %d",
259 				   __func__, ip_ver);
260 			return 0;
261 		}
262 	} else if (elem->classifier_type == 10) {
263 		buf_len += 1 +	/* protocol instance */
264 			1 +	/* protocol number */
265 			2 * elem->frame_classifier.type10_param.filter_len;
266 	} else {
267 		wpa_printf(MSG_ERROR, "%s: Incorrect classifier type %u",
268 			   __func__, elem->classifier_type);
269 		return 0;
270 	}
271 
272 	return buf_len;
273 }
274 
275 
allocate_scs_buf(struct scs_desc_elem * desc_elem,unsigned int num_scs_desc)276 static struct wpabuf * allocate_scs_buf(struct scs_desc_elem *desc_elem,
277 					unsigned int num_scs_desc)
278 {
279 	struct wpabuf *buf;
280 	size_t buf_len = 0;
281 	unsigned int i, j;
282 
283 	buf_len = 3; /* Action frame header */
284 
285 	for (i = 0; i < num_scs_desc; i++, desc_elem++) {
286 		struct tclas_element *tclas_elem;
287 
288 		buf_len += 2 +	/* SCS descriptor IE header */
289 			   1 +	/* SCSID */
290 			   1 ;	/* Request type */
291 
292 		if (desc_elem->request_type == SCS_REQ_REMOVE)
293 			continue;
294 
295 		if (desc_elem->intra_access_priority || desc_elem->scs_up_avail)
296 			buf_len += 3;
297 
298 		tclas_elem = desc_elem->tclas_elems;
299 		if (!tclas_elem) {
300 			wpa_printf(MSG_ERROR, "%s: TCLAS element null",
301 				   __func__);
302 			return NULL;
303 		}
304 
305 		for (j = 0; j < desc_elem->num_tclas_elem; j++, tclas_elem++) {
306 			size_t elen;
307 
308 			elen = tclas_elem_len(tclas_elem);
309 			if (elen == 0)
310 				return NULL;
311 			buf_len += elen;
312 		}
313 
314 		if (desc_elem->num_tclas_elem > 1) {
315 			buf_len += 1 +	/* TCLAS Processing eid */
316 				   1 +	/* length */
317 				   1 ;	/* processing */
318 		}
319 	}
320 
321 	buf = wpabuf_alloc(buf_len);
322 	if (!buf) {
323 		wpa_printf(MSG_ERROR, "Failed to allocate SCS req");
324 		return NULL;
325 	}
326 
327 	return buf;
328 }
329 
330 
scs_request_timer(void * eloop_ctx,void * timeout_ctx)331 static void scs_request_timer(void *eloop_ctx, void *timeout_ctx)
332 {
333 	struct wpa_supplicant *wpa_s = eloop_ctx;
334 	struct active_scs_elem *scs_desc, *prev;
335 
336 	if (wpa_s->wpa_state != WPA_COMPLETED || !wpa_s->current_ssid)
337 		return;
338 
339 	/* Once timeout is over, remove all SCS descriptors with no response */
340 	dl_list_for_each_safe(scs_desc, prev, &wpa_s->active_scs_ids,
341 			      struct active_scs_elem, list) {
342 		u8 bssid[ETH_ALEN] = { 0 };
343 		const u8 *src;
344 
345 		if (scs_desc->status == SCS_DESC_SUCCESS)
346 			continue;
347 
348 		if (wpa_s->current_bss)
349 			src = wpa_s->current_bss->bssid;
350 		else
351 			src = bssid;
352 
353 		wpa_msg(wpa_s, MSG_INFO, WPA_EVENT_SCS_RESULT "bssid=" MACSTR
354 			" SCSID=%u status_code=timedout", MAC2STR(src),
355 			scs_desc->scs_id);
356 
357 		dl_list_del(&scs_desc->list);
358 		wpa_printf(MSG_INFO, "%s: SCSID %d removed after timeout",
359 			   __func__, scs_desc->scs_id);
360 		os_free(scs_desc);
361 	}
362 
363 	eloop_cancel_timeout(scs_request_timer, wpa_s, NULL);
364 	wpa_s->ongoing_scs_req = false;
365 }
366 
367 
wpas_send_scs_req(struct wpa_supplicant * wpa_s)368 int wpas_send_scs_req(struct wpa_supplicant *wpa_s)
369 {
370 	struct wpabuf *buf = NULL;
371 	struct scs_desc_elem *desc_elem = NULL;
372 	int ret = -1;
373 	unsigned int i;
374 
375 	if (wpa_s->wpa_state != WPA_COMPLETED || !wpa_s->current_ssid)
376 		return -1;
377 
378 	if (!wpa_bss_ext_capab(wpa_s->current_bss, WLAN_EXT_CAPAB_SCS)) {
379 		wpa_dbg(wpa_s, MSG_INFO,
380 			"AP does not support SCS - could not send SCS Request");
381 		return -1;
382 	}
383 
384 	desc_elem = wpa_s->scs_robust_av_req.scs_desc_elems;
385 	if (!desc_elem)
386 		return -1;
387 
388 	buf = allocate_scs_buf(desc_elem,
389 			       wpa_s->scs_robust_av_req.num_scs_desc);
390 	if (!buf)
391 		return -1;
392 
393 	wpabuf_put_u8(buf, WLAN_ACTION_ROBUST_AV_STREAMING);
394 	wpabuf_put_u8(buf, ROBUST_AV_SCS_REQ);
395 	wpa_s->scs_dialog_token++;
396 	if (wpa_s->scs_dialog_token == 0)
397 		wpa_s->scs_dialog_token++;
398 	wpabuf_put_u8(buf, wpa_s->scs_dialog_token);
399 
400 	for (i = 0; i < wpa_s->scs_robust_av_req.num_scs_desc;
401 	     i++, desc_elem++) {
402 		/* SCS Descriptor element */
403 		if (wpas_populate_scs_descriptor_ie(desc_elem, buf) < 0)
404 			goto end;
405 	}
406 
407 	wpa_hexdump_buf(MSG_DEBUG, "SCS Request", buf);
408 	ret = wpa_drv_send_action(wpa_s, wpa_s->assoc_freq, 0, wpa_s->bssid,
409 				  wpa_s->own_addr, wpa_s->bssid,
410 				  wpabuf_head(buf), wpabuf_len(buf), 0);
411 	if (ret < 0) {
412 		wpa_dbg(wpa_s, MSG_ERROR, "SCS: Failed to send SCS Request");
413 		wpa_s->scs_dialog_token--;
414 		goto end;
415 	}
416 
417 	desc_elem = wpa_s->scs_robust_av_req.scs_desc_elems;
418 	for (i = 0; i < wpa_s->scs_robust_av_req.num_scs_desc;
419 	     i++, desc_elem++) {
420 		struct active_scs_elem *active_scs_elem;
421 
422 		if (desc_elem->request_type != SCS_REQ_ADD)
423 			continue;
424 
425 		active_scs_elem = os_malloc(sizeof(struct active_scs_elem));
426 		if (!active_scs_elem)
427 			break;
428 		active_scs_elem->scs_id = desc_elem->scs_id;
429 		active_scs_elem->status = SCS_DESC_SENT;
430 		dl_list_add(&wpa_s->active_scs_ids, &active_scs_elem->list);
431 	}
432 
433 	/*
434 	 * Register a timeout after which this request will be removed from
435 	 * the cache.
436 	 */
437 	eloop_register_timeout(SCS_RESP_TIMEOUT, 0, scs_request_timer, wpa_s,
438 			       NULL);
439 	wpa_s->ongoing_scs_req = true;
440 
441 end:
442 	wpabuf_free(buf);
443 	free_up_scs_desc(&wpa_s->scs_robust_av_req);
444 
445 	return ret;
446 }
447 
448 
free_up_tclas_elem(struct scs_desc_elem * elem)449 void free_up_tclas_elem(struct scs_desc_elem *elem)
450 {
451 	struct tclas_element *tclas_elems = elem->tclas_elems;
452 	unsigned int num_tclas_elem = elem->num_tclas_elem;
453 	struct tclas_element *tclas_data;
454 	unsigned int j;
455 
456 	elem->tclas_elems = NULL;
457 	elem->num_tclas_elem = 0;
458 
459 	if (!tclas_elems)
460 		return;
461 
462 	tclas_data = tclas_elems;
463 	for (j = 0; j < num_tclas_elem; j++, tclas_data++) {
464 		if (tclas_data->classifier_type != 10)
465 			continue;
466 
467 		os_free(tclas_data->frame_classifier.type10_param.filter_value);
468 		os_free(tclas_data->frame_classifier.type10_param.filter_mask);
469 	}
470 
471 	os_free(tclas_elems);
472 }
473 
474 
free_up_scs_desc(struct scs_robust_av_data * data)475 void free_up_scs_desc(struct scs_robust_av_data *data)
476 {
477 	struct scs_desc_elem *desc_elems = data->scs_desc_elems;
478 	unsigned int num_scs_desc = data->num_scs_desc;
479 	struct scs_desc_elem *desc_data;
480 	unsigned int i;
481 
482 	data->scs_desc_elems = NULL;
483 	data->num_scs_desc = 0;
484 
485 	if (!desc_elems)
486 		return;
487 
488 	desc_data = desc_elems;
489 	for (i = 0; i < num_scs_desc; i++, desc_data++) {
490 		if (desc_data->request_type == SCS_REQ_REMOVE ||
491 		    !desc_data->tclas_elems)
492 			continue;
493 
494 		free_up_tclas_elem(desc_data);
495 	}
496 	os_free(desc_elems);
497 }
498 
499 
wpas_handle_robust_av_recv_action(struct wpa_supplicant * wpa_s,const u8 * src,const u8 * buf,size_t len)500 void wpas_handle_robust_av_recv_action(struct wpa_supplicant *wpa_s,
501 				       const u8 *src, const u8 *buf, size_t len)
502 {
503 	u8 dialog_token;
504 	u16 status_code;
505 
506 	if (len < 3)
507 		return;
508 
509 	dialog_token = *buf++;
510 	if (dialog_token != wpa_s->robust_av.dialog_token) {
511 		wpa_printf(MSG_INFO,
512 			   "MSCS: Drop received frame due to dialog token mismatch: received:%u expected:%u",
513 			   dialog_token, wpa_s->robust_av.dialog_token);
514 		return;
515 	}
516 
517 	status_code = WPA_GET_LE16(buf);
518 	wpa_msg(wpa_s, MSG_INFO, WPA_EVENT_MSCS_RESULT "bssid=" MACSTR
519 		" status_code=%u", MAC2STR(src), status_code);
520 	wpa_s->mscs_setup_done = status_code == WLAN_STATUS_SUCCESS;
521 }
522 
523 
wpas_handle_assoc_resp_mscs(struct wpa_supplicant * wpa_s,const u8 * bssid,const u8 * ies,size_t ies_len)524 void wpas_handle_assoc_resp_mscs(struct wpa_supplicant *wpa_s, const u8 *bssid,
525 				 const u8 *ies, size_t ies_len)
526 {
527 	const u8 *mscs_desc_ie, *mscs_status;
528 	u16 status;
529 
530 	/* Process optional MSCS Status subelement when MSCS IE is in
531 	 * (Re)Association Response frame */
532 	if (!ies || ies_len == 0 || !wpa_s->robust_av.valid_config)
533 		return;
534 
535 	mscs_desc_ie = get_ie_ext(ies, ies_len, WLAN_EID_EXT_MSCS_DESCRIPTOR);
536 	if (!mscs_desc_ie || mscs_desc_ie[1] <= 8)
537 		return;
538 
539 	/* Subelements start after (ie_id(1) + ie_len(1) + ext_id(1) +
540 	 * request type(1) + upc(2) + stream timeout(4) =) 10.
541 	 */
542 	mscs_status = get_ie(&mscs_desc_ie[10], mscs_desc_ie[1] - 8,
543 			     MCSC_SUBELEM_STATUS);
544 	if (!mscs_status || mscs_status[1] < 2)
545 		return;
546 
547 	status = WPA_GET_LE16(mscs_status + 2);
548 	wpa_msg(wpa_s, MSG_INFO, WPA_EVENT_MSCS_RESULT "bssid=" MACSTR
549 		" status_code=%u", MAC2STR(bssid), status);
550 	wpa_s->mscs_setup_done = status == WLAN_STATUS_SUCCESS;
551 }
552 
553 
wpas_wait_for_dscp_req_timer(void * eloop_ctx,void * timeout_ctx)554 static void wpas_wait_for_dscp_req_timer(void *eloop_ctx, void *timeout_ctx)
555 {
556 	struct wpa_supplicant *wpa_s = eloop_ctx;
557 
558 	/* Once timeout is over, reset wait flag and allow sending DSCP query */
559 	wpa_printf(MSG_DEBUG,
560 		   "QM: Wait time over for sending DSCP request - allow DSCP query");
561 	wpa_s->wait_for_dscp_req = 0;
562 	wpa_msg(wpa_s, MSG_INFO, WPA_EVENT_DSCP_POLICY "request_wait end");
563 }
564 
565 
wpas_handle_assoc_resp_qos_mgmt(struct wpa_supplicant * wpa_s,const u8 * ies,size_t ies_len)566 void wpas_handle_assoc_resp_qos_mgmt(struct wpa_supplicant *wpa_s,
567 				     const u8 *ies, size_t ies_len)
568 {
569 	const u8 *wfa_capa;
570 
571 	wpa_s->connection_dscp = 0;
572 	if (wpa_s->wait_for_dscp_req)
573 		eloop_cancel_timeout(wpas_wait_for_dscp_req_timer, wpa_s, NULL);
574 
575 	if (!ies || ies_len == 0 || !wpa_s->enable_dscp_policy_capa)
576 		return;
577 
578 	wfa_capa = get_vendor_ie(ies, ies_len, WFA_CAPA_IE_VENDOR_TYPE);
579 	if (!wfa_capa || wfa_capa[1] < 6 || wfa_capa[6] < 1 ||
580 	    !(wfa_capa[7] & WFA_CAPA_QM_DSCP_POLICY))
581 		return; /* AP does not enable QM DSCP Policy */
582 
583 	wpa_s->connection_dscp = 1;
584 	wpa_s->wait_for_dscp_req = !!(wfa_capa[7] &
585 				      WFA_CAPA_QM_UNSOLIC_DSCP);
586 	if (!wpa_s->wait_for_dscp_req)
587 		return;
588 
589 	/* Register a timeout after which dscp query can be sent to AP. */
590 	wpa_msg(wpa_s, MSG_INFO, WPA_EVENT_DSCP_POLICY "request_wait start");
591 	eloop_register_timeout(DSCP_REQ_TIMEOUT, 0,
592 			       wpas_wait_for_dscp_req_timer, wpa_s, NULL);
593 }
594 
595 
wpas_handle_robust_av_scs_recv_action(struct wpa_supplicant * wpa_s,const u8 * src,const u8 * buf,size_t len)596 void wpas_handle_robust_av_scs_recv_action(struct wpa_supplicant *wpa_s,
597 					   const u8 *src, const u8 *buf,
598 					   size_t len)
599 {
600 	u8 dialog_token;
601 	unsigned int i, count;
602 	struct active_scs_elem *scs_desc, *prev;
603 
604 	if (len < 2)
605 		return;
606 	if (!wpa_s->ongoing_scs_req) {
607 		wpa_printf(MSG_INFO,
608 			   "SCS: Drop received response due to no ongoing request");
609 		return;
610 	}
611 
612 	dialog_token = *buf++;
613 	len--;
614 	if (dialog_token != wpa_s->scs_dialog_token) {
615 		wpa_printf(MSG_INFO,
616 			   "SCS: Drop received frame due to dialog token mismatch: received:%u expected:%u",
617 			   dialog_token, wpa_s->scs_dialog_token);
618 		return;
619 	}
620 
621 	/* This Count field does not exist in the IEEE Std 802.11-2020
622 	 * definition of the SCS Response frame. However, it was accepted to
623 	 * be added into REVme per REVme/D0.0 CC35 CID 49 (edits in document
624 	 * 11-21-0688-07). */
625 	count = *buf++;
626 	len--;
627 	if (count == 0 || count * 3 > len) {
628 		wpa_printf(MSG_INFO,
629 			   "SCS: Drop received frame due to invalid count: %u (remaining %zu octets)",
630 			   count, len);
631 		return;
632 	}
633 
634 	for (i = 0; i < count; i++) {
635 		u8 id;
636 		u16 status;
637 		bool scs_desc_found = false;
638 
639 		id = *buf++;
640 		status = WPA_GET_LE16(buf);
641 		buf += 2;
642 		len -= 3;
643 
644 		dl_list_for_each(scs_desc, &wpa_s->active_scs_ids,
645 				 struct active_scs_elem, list) {
646 			if (id == scs_desc->scs_id) {
647 				scs_desc_found = true;
648 				break;
649 			}
650 		}
651 
652 		if (!scs_desc_found) {
653 			wpa_printf(MSG_INFO, "SCS: SCS ID invalid %u", id);
654 			continue;
655 		}
656 
657 		if (status != WLAN_STATUS_SUCCESS) {
658 			dl_list_del(&scs_desc->list);
659 			os_free(scs_desc);
660 		} else if (status == WLAN_STATUS_SUCCESS) {
661 			scs_desc->status = SCS_DESC_SUCCESS;
662 		}
663 
664 		wpa_msg(wpa_s, MSG_INFO, WPA_EVENT_SCS_RESULT "bssid=" MACSTR
665 			" SCSID=%u status_code=%u", MAC2STR(src), id, status);
666 	}
667 
668 	eloop_cancel_timeout(scs_request_timer, wpa_s, NULL);
669 	wpa_s->ongoing_scs_req = false;
670 
671 	dl_list_for_each_safe(scs_desc, prev, &wpa_s->active_scs_ids,
672 			      struct active_scs_elem, list) {
673 		if (scs_desc->status != SCS_DESC_SUCCESS) {
674 			wpa_msg(wpa_s, MSG_INFO,
675 				WPA_EVENT_SCS_RESULT "bssid=" MACSTR
676 				" SCSID=%u status_code=response_not_received",
677 				MAC2STR(src), scs_desc->scs_id);
678 			dl_list_del(&scs_desc->list);
679 			os_free(scs_desc);
680 		}
681 	}
682 }
683 
684 
wpas_clear_active_scs_ids(struct wpa_supplicant * wpa_s)685 static void wpas_clear_active_scs_ids(struct wpa_supplicant *wpa_s)
686 {
687 	struct active_scs_elem *scs_elem;
688 
689 	while ((scs_elem = dl_list_first(&wpa_s->active_scs_ids,
690 					 struct active_scs_elem, list))) {
691 		dl_list_del(&scs_elem->list);
692 		os_free(scs_elem);
693 	}
694 }
695 
696 
wpas_scs_deinit(struct wpa_supplicant * wpa_s)697 void wpas_scs_deinit(struct wpa_supplicant *wpa_s)
698 {
699 	free_up_scs_desc(&wpa_s->scs_robust_av_req);
700 	wpa_s->scs_dialog_token = 0;
701 	wpas_clear_active_scs_ids(wpa_s);
702 	eloop_cancel_timeout(scs_request_timer, wpa_s, NULL);
703 	wpa_s->ongoing_scs_req = false;
704 }
705 
706 
write_ipv4_info(char * pos,int total_len,const struct ipv4_params * v4)707 static int write_ipv4_info(char *pos, int total_len,
708 			   const struct ipv4_params *v4)
709 {
710 	int res, rem_len;
711 	char addr[INET_ADDRSTRLEN];
712 
713 	rem_len = total_len;
714 
715 	if (v4->param_mask & BIT(1)) {
716 		if (!inet_ntop(AF_INET, &v4->src_ip, addr, INET_ADDRSTRLEN)) {
717 			wpa_printf(MSG_ERROR,
718 				   "QM: Failed to set IPv4 source address");
719 			return -1;
720 		}
721 
722 		res = os_snprintf(pos, rem_len, " src_ip=%s", addr);
723 		if (os_snprintf_error(rem_len, res))
724 			return -1;
725 
726 		pos += res;
727 		rem_len -= res;
728 	}
729 
730 	if (v4->param_mask & BIT(2)) {
731 		if (!inet_ntop(AF_INET, &v4->dst_ip, addr, INET_ADDRSTRLEN)) {
732 			wpa_printf(MSG_ERROR,
733 				   "QM: Failed to set IPv4 destination address");
734 			return -1;
735 		}
736 
737 		res = os_snprintf(pos, rem_len, " dst_ip=%s", addr);
738 		if (os_snprintf_error(rem_len, res))
739 			return -1;
740 
741 		pos += res;
742 		rem_len -= res;
743 	}
744 
745 	if (v4->param_mask & BIT(3)) {
746 		res = os_snprintf(pos, rem_len, " src_port=%d", v4->src_port);
747 		if (os_snprintf_error(rem_len, res))
748 			return -1;
749 
750 		pos += res;
751 		rem_len -= res;
752 	}
753 
754 	if (v4->param_mask & BIT(4)) {
755 		res = os_snprintf(pos, rem_len, " dst_port=%d", v4->dst_port);
756 		if (os_snprintf_error(rem_len, res))
757 			return -1;
758 
759 		pos += res;
760 		rem_len -= res;
761 	}
762 
763 	if (v4->param_mask & BIT(6)) {
764 		res = os_snprintf(pos, rem_len, " protocol=%d", v4->protocol);
765 		if (os_snprintf_error(rem_len, res))
766 			return -1;
767 
768 		pos += res;
769 		rem_len -= res;
770 	}
771 
772 	return total_len - rem_len;
773 }
774 
775 
write_ipv6_info(char * pos,int total_len,const struct ipv6_params * v6)776 static int write_ipv6_info(char *pos, int total_len,
777 			   const struct ipv6_params *v6)
778 {
779 	int res, rem_len;
780 	char addr[INET6_ADDRSTRLEN];
781 
782 	rem_len = total_len;
783 
784 	if (v6->param_mask & BIT(1)) {
785 		if (!inet_ntop(AF_INET6, &v6->src_ip, addr, INET6_ADDRSTRLEN)) {
786 			wpa_printf(MSG_ERROR,
787 				   "QM: Failed to set IPv6 source addr");
788 			return -1;
789 		}
790 
791 		res = os_snprintf(pos, rem_len, " src_ip=%s", addr);
792 		if (os_snprintf_error(rem_len, res))
793 			return -1;
794 
795 		pos += res;
796 		rem_len -= res;
797 	}
798 
799 	if (v6->param_mask & BIT(2)) {
800 		if (!inet_ntop(AF_INET6, &v6->dst_ip, addr, INET6_ADDRSTRLEN)) {
801 			wpa_printf(MSG_ERROR,
802 				   "QM: Failed to set IPv6 destination addr");
803 			return -1;
804 		}
805 
806 		res = os_snprintf(pos, rem_len, " dst_ip=%s", addr);
807 		if (os_snprintf_error(rem_len, res))
808 			return -1;
809 
810 		pos += res;
811 		rem_len -= res;
812 	}
813 
814 	if (v6->param_mask & BIT(3)) {
815 		res = os_snprintf(pos, rem_len, " src_port=%d", v6->src_port);
816 		if (os_snprintf_error(rem_len, res))
817 			return -1;
818 
819 		pos += res;
820 		rem_len -= res;
821 	}
822 
823 	if (v6->param_mask & BIT(4)) {
824 		res = os_snprintf(pos, rem_len, " dst_port=%d", v6->dst_port);
825 		if (os_snprintf_error(rem_len, res))
826 			return -1;
827 
828 		pos += res;
829 		rem_len -= res;
830 	}
831 
832 	if (v6->param_mask & BIT(6)) {
833 		res = os_snprintf(pos, rem_len, " protocol=%d",
834 				  v6->next_header);
835 		if (os_snprintf_error(rem_len, res))
836 			return -1;
837 
838 		pos += res;
839 		rem_len -= res;
840 	}
841 
842 	return total_len - rem_len;
843 }
844 
845 
846 struct dscp_policy_data {
847 	u8 policy_id;
848 	u8 req_type;
849 	u8 dscp;
850 	bool dscp_info;
851 	const u8 *frame_classifier;
852 	u8 frame_classifier_len;
853 	struct type4_params type4_param;
854 	const u8 *domain_name;
855 	u8 domain_name_len;
856 	u16 start_port;
857 	u16 end_port;
858 	bool port_range_info;
859 };
860 
861 
set_frame_classifier_type4_ipv4(struct dscp_policy_data * policy)862 static int set_frame_classifier_type4_ipv4(struct dscp_policy_data *policy)
863 {
864 	u8 classifier_mask;
865 	const u8 *frame_classifier = policy->frame_classifier;
866 	struct type4_params *type4_param = &policy->type4_param;
867 
868 	if (policy->frame_classifier_len < 18) {
869 		wpa_printf(MSG_ERROR,
870 			   "QM: Received IPv4 frame classifier with insufficient length %d",
871 			   policy->frame_classifier_len);
872 		return -1;
873 	}
874 
875 	classifier_mask = frame_classifier[1];
876 
877 	/* Classifier Mask - bit 1 = Source IP Address */
878 	if (classifier_mask & BIT(1)) {
879 		type4_param->ip_params.v4.param_mask |= BIT(1);
880 		os_memcpy(&type4_param->ip_params.v4.src_ip,
881 			  &frame_classifier[3], 4);
882 	}
883 
884 	/* Classifier Mask - bit 2 = Destination IP Address */
885 	if (classifier_mask & BIT(2)) {
886 		if (policy->domain_name) {
887 			wpa_printf(MSG_ERROR,
888 				   "QM: IPv4: Both domain name and destination IP address not expected");
889 			return -1;
890 		}
891 
892 		type4_param->ip_params.v4.param_mask |= BIT(2);
893 		os_memcpy(&type4_param->ip_params.v4.dst_ip,
894 			  &frame_classifier[7], 4);
895 	}
896 
897 	/* Classifier Mask - bit 3 = Source Port */
898 	if (classifier_mask & BIT(3)) {
899 		type4_param->ip_params.v4.param_mask |= BIT(3);
900 		type4_param->ip_params.v4.src_port =
901 			WPA_GET_BE16(&frame_classifier[11]);
902 	}
903 
904 	/* Classifier Mask - bit 4 = Destination Port */
905 	if (classifier_mask & BIT(4)) {
906 		if (policy->port_range_info) {
907 			wpa_printf(MSG_ERROR,
908 				   "QM: IPv4: Both port range and destination port not expected");
909 			return -1;
910 		}
911 
912 		type4_param->ip_params.v4.param_mask |= BIT(4);
913 		type4_param->ip_params.v4.dst_port =
914 			WPA_GET_BE16(&frame_classifier[13]);
915 	}
916 
917 	/* Classifier Mask - bit 5 = DSCP (ignored) */
918 
919 	/* Classifier Mask - bit 6 = Protocol */
920 	if (classifier_mask & BIT(6)) {
921 		type4_param->ip_params.v4.param_mask |= BIT(6);
922 		type4_param->ip_params.v4.protocol = frame_classifier[16];
923 	}
924 
925 	return 0;
926 }
927 
928 
set_frame_classifier_type4_ipv6(struct dscp_policy_data * policy)929 static int set_frame_classifier_type4_ipv6(struct dscp_policy_data *policy)
930 {
931 	u8 classifier_mask;
932 	const u8 *frame_classifier = policy->frame_classifier;
933 	struct type4_params *type4_param = &policy->type4_param;
934 
935 	if (policy->frame_classifier_len < 44) {
936 		wpa_printf(MSG_ERROR,
937 			   "QM: Received IPv6 frame classifier with insufficient length %d",
938 			   policy->frame_classifier_len);
939 		return -1;
940 	}
941 
942 	classifier_mask = frame_classifier[1];
943 
944 	/* Classifier Mask - bit 1 = Source IP Address */
945 	if (classifier_mask & BIT(1)) {
946 		type4_param->ip_params.v6.param_mask |= BIT(1);
947 		os_memcpy(&type4_param->ip_params.v6.src_ip,
948 			  &frame_classifier[3], 16);
949 	}
950 
951 	/* Classifier Mask - bit 2 = Destination IP Address */
952 	if (classifier_mask & BIT(2)) {
953 		if (policy->domain_name) {
954 			wpa_printf(MSG_ERROR,
955 				   "QM: IPv6: Both domain name and destination IP address not expected");
956 			return -1;
957 		}
958 		type4_param->ip_params.v6.param_mask |= BIT(2);
959 		os_memcpy(&type4_param->ip_params.v6.dst_ip,
960 			  &frame_classifier[19], 16);
961 	}
962 
963 	/* Classifier Mask - bit 3 = Source Port */
964 	if (classifier_mask & BIT(3)) {
965 		type4_param->ip_params.v6.param_mask |= BIT(3);
966 		type4_param->ip_params.v6.src_port =
967 				WPA_GET_BE16(&frame_classifier[35]);
968 	}
969 
970 	/* Classifier Mask - bit 4 = Destination Port */
971 	if (classifier_mask & BIT(4)) {
972 		if (policy->port_range_info) {
973 			wpa_printf(MSG_ERROR,
974 				   "IPv6: Both port range and destination port not expected");
975 			return -1;
976 		}
977 
978 		type4_param->ip_params.v6.param_mask |= BIT(4);
979 		type4_param->ip_params.v6.dst_port =
980 				WPA_GET_BE16(&frame_classifier[37]);
981 	}
982 
983 	/* Classifier Mask - bit 5 = DSCP (ignored) */
984 
985 	/* Classifier Mask - bit 6 = Next Header */
986 	if (classifier_mask & BIT(6)) {
987 		type4_param->ip_params.v6.param_mask |= BIT(6);
988 		type4_param->ip_params.v6.next_header = frame_classifier[40];
989 	}
990 
991 	return 0;
992 }
993 
994 
wpas_set_frame_classifier_params(struct dscp_policy_data * policy)995 static int wpas_set_frame_classifier_params(struct dscp_policy_data *policy)
996 {
997 	const u8 *frame_classifier = policy->frame_classifier;
998 	u8 frame_classifier_len = policy->frame_classifier_len;
999 
1000 	if (frame_classifier_len < 3) {
1001 		wpa_printf(MSG_ERROR,
1002 			   "QM: Received frame classifier with insufficient length %d",
1003 			   frame_classifier_len);
1004 		return -1;
1005 	}
1006 
1007 	/* Only allowed Classifier Type: IP and higher layer parameters (4) */
1008 	if (frame_classifier[0] != 4) {
1009 		wpa_printf(MSG_ERROR,
1010 			   "QM: Received frame classifier with invalid classifier type %d",
1011 			   frame_classifier[0]);
1012 		return -1;
1013 	}
1014 
1015 	/* Classifier Mask - bit 0 = Version */
1016 	if (!(frame_classifier[1] & BIT(0))) {
1017 		wpa_printf(MSG_ERROR,
1018 			   "QM: Received frame classifier without IP version");
1019 		return -1;
1020 	}
1021 
1022 	/* Version (4 or 6) */
1023 	if (frame_classifier[2] == 4) {
1024 		if (set_frame_classifier_type4_ipv4(policy)) {
1025 			wpa_printf(MSG_ERROR,
1026 				   "QM: Failed to set IPv4 parameters");
1027 			return -1;
1028 		}
1029 
1030 		policy->type4_param.ip_version = IPV4;
1031 	} else if (frame_classifier[2] == 6) {
1032 		if (set_frame_classifier_type4_ipv6(policy)) {
1033 			wpa_printf(MSG_ERROR,
1034 				   "QM: Failed to set IPv6 parameters");
1035 			return -1;
1036 		}
1037 
1038 		policy->type4_param.ip_version = IPV6;
1039 	} else {
1040 		wpa_printf(MSG_ERROR,
1041 			   "QM: Received unknown IP version %d",
1042 			   frame_classifier[2]);
1043 		return -1;
1044 	}
1045 
1046 	return 0;
1047 }
1048 
1049 
dscp_valid_domain_name(const char * str)1050 static bool dscp_valid_domain_name(const char *str)
1051 {
1052 	if (!str[0])
1053 		return false;
1054 
1055 	while (*str) {
1056 		if (is_ctrl_char(*str) || *str == ' ' || *str == '=')
1057 			return false;
1058 		str++;
1059 	}
1060 
1061 	return true;
1062 }
1063 
1064 
wpas_add_dscp_policy(struct wpa_supplicant * wpa_s,struct dscp_policy_data * policy)1065 static void wpas_add_dscp_policy(struct wpa_supplicant *wpa_s,
1066 				 struct dscp_policy_data *policy)
1067 {
1068 	int ip_ver = 0, res;
1069 	char policy_str[1000], *pos;
1070 	int len;
1071 
1072 	if (!policy->frame_classifier && !policy->domain_name &&
1073 	    !policy->port_range_info) {
1074 		wpa_printf(MSG_ERROR,
1075 			   "QM: Invalid DSCP policy - no attributes present");
1076 		goto fail;
1077 	}
1078 
1079 	policy_str[0] = '\0';
1080 	pos = policy_str;
1081 	len = sizeof(policy_str);
1082 
1083 	if (policy->frame_classifier) {
1084 		struct type4_params *type4 = &policy->type4_param;
1085 
1086 		if (wpas_set_frame_classifier_params(policy)) {
1087 			wpa_printf(MSG_ERROR,
1088 				   "QM: Failed to set frame classifier parameters");
1089 			goto fail;
1090 		}
1091 
1092 		if (type4->ip_version == IPV4)
1093 			res = write_ipv4_info(pos, len, &type4->ip_params.v4);
1094 		else
1095 			res = write_ipv6_info(pos, len, &type4->ip_params.v6);
1096 
1097 		if (res <= 0) {
1098 			wpa_printf(MSG_ERROR,
1099 				   "QM: Failed to write IP parameters");
1100 			goto fail;
1101 		}
1102 
1103 		ip_ver = type4->ip_version;
1104 
1105 		pos += res;
1106 		len -= res;
1107 	}
1108 
1109 	if (policy->port_range_info) {
1110 		res = os_snprintf(pos, len, " start_port=%u end_port=%u",
1111 				  policy->start_port, policy->end_port);
1112 		if (os_snprintf_error(len, res)) {
1113 			wpa_printf(MSG_ERROR,
1114 				   "QM: Failed to write port range attributes for policy id = %d",
1115 				   policy->policy_id);
1116 			goto fail;
1117 		}
1118 
1119 		pos += res;
1120 		len -= res;
1121 	}
1122 
1123 	if (policy->domain_name) {
1124 		char domain_name_str[250];
1125 
1126 		if (policy->domain_name_len >= sizeof(domain_name_str)) {
1127 			wpa_printf(MSG_ERROR,
1128 				   "QM: Domain name length higher than max expected");
1129 			goto fail;
1130 		}
1131 		os_memcpy(domain_name_str, policy->domain_name,
1132 			  policy->domain_name_len);
1133 		domain_name_str[policy->domain_name_len] = '\0';
1134 		if (!dscp_valid_domain_name(domain_name_str)) {
1135 			wpa_printf(MSG_ERROR, "QM: Invalid domain name string");
1136 			goto fail;
1137 		}
1138 		res = os_snprintf(pos, len, " domain_name=%s", domain_name_str);
1139 		if (os_snprintf_error(len, res)) {
1140 			wpa_printf(MSG_ERROR,
1141 				   "QM: Failed to write domain name attribute for policy id = %d",
1142 				   policy->policy_id);
1143 			goto fail;
1144 		}
1145 	}
1146 
1147 	wpa_msg(wpa_s, MSG_INFO, WPA_EVENT_DSCP_POLICY
1148 		"add policy_id=%u dscp=%u ip_version=%d%s",
1149 		policy->policy_id, policy->dscp, ip_ver, policy_str);
1150 	return;
1151 fail:
1152 	wpa_msg(wpa_s, MSG_INFO, WPA_EVENT_DSCP_POLICY "reject policy_id=%u",
1153 		policy->policy_id);
1154 }
1155 
1156 
wpas_dscp_deinit(struct wpa_supplicant * wpa_s)1157 void wpas_dscp_deinit(struct wpa_supplicant *wpa_s)
1158 {
1159 	wpa_printf(MSG_DEBUG, "QM: Clear all active DSCP policies");
1160 	wpa_msg(wpa_s, MSG_INFO, WPA_EVENT_DSCP_POLICY "clear_all");
1161 	wpa_s->dscp_req_dialog_token = 0;
1162 	wpa_s->dscp_query_dialog_token = 0;
1163 	wpa_s->connection_dscp = 0;
1164 	if (wpa_s->wait_for_dscp_req) {
1165 		wpa_s->wait_for_dscp_req = 0;
1166 		eloop_cancel_timeout(wpas_wait_for_dscp_req_timer, wpa_s, NULL);
1167 	}
1168 }
1169 
1170 
wpas_fill_dscp_policy(struct dscp_policy_data * policy,u8 attr_id,u8 attr_len,const u8 * attr_data)1171 static void wpas_fill_dscp_policy(struct dscp_policy_data *policy, u8 attr_id,
1172 				  u8 attr_len, const u8 *attr_data)
1173 {
1174 	switch (attr_id) {
1175 	case QM_ATTR_PORT_RANGE:
1176 		if (attr_len < 4) {
1177 			wpa_printf(MSG_ERROR,
1178 				   "QM: Received Port Range attribute with insufficient length %d",
1179 				    attr_len);
1180 			break;
1181 		}
1182 		policy->start_port = WPA_GET_BE16(attr_data);
1183 		policy->end_port = WPA_GET_BE16(attr_data + 2);
1184 		policy->port_range_info = true;
1185 		break;
1186 	case QM_ATTR_DSCP_POLICY:
1187 		if (attr_len < 3) {
1188 			wpa_printf(MSG_ERROR,
1189 				   "QM: Received DSCP Policy attribute with insufficient length %d",
1190 				   attr_len);
1191 			return;
1192 		}
1193 		policy->policy_id = attr_data[0];
1194 		policy->req_type = attr_data[1];
1195 		policy->dscp = attr_data[2];
1196 		policy->dscp_info = true;
1197 		break;
1198 	case QM_ATTR_TCLAS:
1199 		if (attr_len < 1) {
1200 			wpa_printf(MSG_ERROR,
1201 				   "QM: Received TCLAS attribute with insufficient length %d",
1202 				   attr_len);
1203 			return;
1204 		}
1205 		policy->frame_classifier = attr_data;
1206 		policy->frame_classifier_len = attr_len;
1207 		break;
1208 	case QM_ATTR_DOMAIN_NAME:
1209 		if (attr_len < 1) {
1210 			wpa_printf(MSG_ERROR,
1211 				   "QM: Received domain name attribute with insufficient length %d",
1212 				   attr_len);
1213 			return;
1214 		}
1215 		policy->domain_name = attr_data;
1216 		policy->domain_name_len = attr_len;
1217 		break;
1218 	default:
1219 		wpa_printf(MSG_ERROR, "QM: Received invalid QoS attribute %d",
1220 			   attr_id);
1221 		break;
1222 	}
1223 }
1224 
1225 
wpas_handle_qos_mgmt_recv_action(struct wpa_supplicant * wpa_s,const u8 * src,const u8 * buf,size_t len)1226 void wpas_handle_qos_mgmt_recv_action(struct wpa_supplicant *wpa_s,
1227 				      const u8 *src,
1228 				      const u8 *buf, size_t len)
1229 {
1230 	int rem_len;
1231 	const u8 *qos_ie, *attr;
1232 	int more, reset;
1233 
1234 	if (!wpa_s->enable_dscp_policy_capa) {
1235 		wpa_printf(MSG_ERROR,
1236 			   "QM: Ignore DSCP Policy frame since the capability is not enabled");
1237 		return;
1238 	}
1239 
1240 	if (!pmf_in_use(wpa_s, src)) {
1241 		wpa_printf(MSG_ERROR,
1242 			   "QM: Ignore DSCP Policy frame since PMF is not in use");
1243 		return;
1244 	}
1245 
1246 	if (!wpa_s->connection_dscp) {
1247 		 wpa_printf(MSG_DEBUG,
1248 			    "QM: DSCP Policy capability not enabled for the current association - ignore QoS Management Action frames");
1249 		return;
1250 	}
1251 
1252 	if (len < 1)
1253 		return;
1254 
1255 	/* Handle only DSCP Policy Request frame */
1256 	if (buf[0] != QM_DSCP_POLICY_REQ) {
1257 		wpa_printf(MSG_ERROR, "QM: Received unexpected QoS action frame %d",
1258 			   buf[0]);
1259 		return;
1260 	}
1261 
1262 	if (len < 3) {
1263 		wpa_printf(MSG_ERROR,
1264 			   "Received QoS Management DSCP Policy Request frame with invalid length %zu",
1265 			   len);
1266 		return;
1267 	}
1268 
1269 	/* Clear wait_for_dscp_req on receiving first DSCP request from AP */
1270 	if (wpa_s->wait_for_dscp_req) {
1271 		wpa_s->wait_for_dscp_req = 0;
1272 		eloop_cancel_timeout(wpas_wait_for_dscp_req_timer, wpa_s, NULL);
1273 	}
1274 
1275 	wpa_s->dscp_req_dialog_token = buf[1];
1276 	more = buf[2] & DSCP_POLICY_CTRL_MORE;
1277 	reset = buf[2] & DSCP_POLICY_CTRL_RESET;
1278 
1279 	wpa_msg(wpa_s, MSG_INFO, WPA_EVENT_DSCP_POLICY "request_start%s%s",
1280 		reset ? " clear_all" : "", more ? " more" : "");
1281 
1282 	qos_ie = buf + 3;
1283 	rem_len = len - 3;
1284 	while (rem_len > 2) {
1285 		struct dscp_policy_data policy;
1286 		int rem_attrs_len, ie_len;
1287 
1288 		ie_len = 2 + qos_ie[1];
1289 		if (rem_len < ie_len)
1290 			break;
1291 
1292 		if (rem_len < 6 || qos_ie[0] != WLAN_EID_VENDOR_SPECIFIC ||
1293 		    qos_ie[1] < 4 ||
1294 		    WPA_GET_BE32(&qos_ie[2]) != QM_IE_VENDOR_TYPE) {
1295 			rem_len -= ie_len;
1296 			qos_ie += ie_len;
1297 			continue;
1298 		}
1299 
1300 		os_memset(&policy, 0, sizeof(struct dscp_policy_data));
1301 		attr = qos_ie + 6;
1302 		rem_attrs_len = qos_ie[1] - 4;
1303 
1304 		while (rem_attrs_len > 2 && rem_attrs_len >= 2 + attr[1]) {
1305 			wpas_fill_dscp_policy(&policy, attr[0], attr[1],
1306 					      &attr[2]);
1307 			rem_attrs_len -= 2 + attr[1];
1308 			attr += 2 + attr[1];
1309 		}
1310 
1311 		rem_len -= ie_len;
1312 		qos_ie += ie_len;
1313 
1314 		if (!policy.dscp_info) {
1315 			wpa_printf(MSG_ERROR,
1316 				   "QM: Received QoS IE without DSCP Policy attribute");
1317 			continue;
1318 		}
1319 
1320 		if (policy.req_type == DSCP_POLICY_REQ_ADD)
1321 			wpas_add_dscp_policy(wpa_s, &policy);
1322 		else if (policy.req_type == DSCP_POLICY_REQ_REMOVE)
1323 			wpa_msg(wpa_s, MSG_INFO, WPA_EVENT_DSCP_POLICY
1324 				"remove policy_id=%u", policy.policy_id);
1325 		else
1326 			wpa_msg(wpa_s, MSG_INFO, WPA_EVENT_DSCP_POLICY
1327 				"reject policy_id=%u", policy.policy_id);
1328 	}
1329 
1330 	wpa_msg(wpa_s, MSG_INFO, WPA_EVENT_DSCP_POLICY "request_end");
1331 }
1332 
1333 
wpas_send_dscp_response(struct wpa_supplicant * wpa_s,struct dscp_resp_data * resp_data)1334 int wpas_send_dscp_response(struct wpa_supplicant *wpa_s,
1335 			    struct dscp_resp_data *resp_data)
1336 {
1337 	struct wpabuf *buf = NULL;
1338 	size_t buf_len;
1339 	int ret = -1, i;
1340 	u8 resp_control = 0;
1341 
1342 	if (wpa_s->wpa_state != WPA_COMPLETED || !wpa_s->current_ssid) {
1343 		wpa_printf(MSG_ERROR,
1344 			   "QM: Failed to send DSCP response - not connected to AP");
1345 		return -1;
1346 	}
1347 
1348 	if (resp_data->solicited && !wpa_s->dscp_req_dialog_token) {
1349 		wpa_printf(MSG_ERROR, "QM: No ongoing DSCP request");
1350 		return -1;
1351 	}
1352 
1353 	if (!wpa_s->connection_dscp) {
1354 		wpa_printf(MSG_ERROR,
1355 			   "QM: Failed to send DSCP response - DSCP capability not enabled for the current association");
1356 		return -1;
1357 
1358 	}
1359 
1360 	buf_len = 1 +	/* Category */
1361 		  3 +	/* OUI */
1362 		  1 +	/* OUI Type */
1363 		  1 +	/* OUI Subtype */
1364 		  1 +	/* Dialog Token */
1365 		  1 +	/* Response Control */
1366 		  1 +	/* Count */
1367 		  2 * resp_data->num_policies;  /* Status list */
1368 	buf = wpabuf_alloc(buf_len);
1369 	if (!buf) {
1370 		wpa_printf(MSG_ERROR,
1371 			   "QM: Failed to allocate DSCP policy response");
1372 		return -1;
1373 	}
1374 
1375 	wpabuf_put_u8(buf, WLAN_ACTION_VENDOR_SPECIFIC_PROTECTED);
1376 	wpabuf_put_be24(buf, OUI_WFA);
1377 	wpabuf_put_u8(buf, QM_ACTION_OUI_TYPE);
1378 	wpabuf_put_u8(buf, QM_DSCP_POLICY_RESP);
1379 
1380 	wpabuf_put_u8(buf, resp_data->solicited ?
1381 		      wpa_s->dscp_req_dialog_token : 0);
1382 
1383 	if (resp_data->more)
1384 		resp_control |= DSCP_POLICY_CTRL_MORE;
1385 	if (resp_data->reset)
1386 		resp_control |= DSCP_POLICY_CTRL_RESET;
1387 	wpabuf_put_u8(buf, resp_control);
1388 
1389 	wpabuf_put_u8(buf, resp_data->num_policies);
1390 	for (i = 0; i < resp_data->num_policies; i++) {
1391 		wpabuf_put_u8(buf, resp_data->policy[i].id);
1392 		wpabuf_put_u8(buf, resp_data->policy[i].status);
1393 	}
1394 
1395 	wpa_hexdump_buf(MSG_MSGDUMP, "DSCP response frame: ", buf);
1396 	ret = wpa_drv_send_action(wpa_s, wpa_s->assoc_freq, 0, wpa_s->bssid,
1397 				  wpa_s->own_addr, wpa_s->bssid,
1398 				  wpabuf_head(buf), wpabuf_len(buf), 0);
1399 	if (ret < 0) {
1400 		wpa_msg(wpa_s, MSG_INFO, "QM: Failed to send DSCP response");
1401 		goto fail;
1402 	}
1403 
1404 	/*
1405 	 * Mark DSCP request complete whether response sent is solicited or
1406 	 * unsolicited
1407 	 */
1408 	wpa_s->dscp_req_dialog_token = 0;
1409 
1410 fail:
1411 	wpabuf_free(buf);
1412 	return ret;
1413 }
1414 
1415 
wpas_send_dscp_query(struct wpa_supplicant * wpa_s,const char * domain_name,size_t domain_name_length)1416 int wpas_send_dscp_query(struct wpa_supplicant *wpa_s, const char *domain_name,
1417 			 size_t domain_name_length)
1418 {
1419 	struct wpabuf *buf = NULL;
1420 	int ret, dscp_query_size;
1421 
1422 	if (wpa_s->wpa_state != WPA_COMPLETED || !wpa_s->current_ssid)
1423 		return -1;
1424 
1425 	if (!wpa_s->connection_dscp) {
1426 		wpa_printf(MSG_ERROR,
1427 			   "QM: Failed to send DSCP query - DSCP capability not enabled for the current association");
1428 		return -1;
1429 	}
1430 
1431 	if (wpa_s->wait_for_dscp_req) {
1432 		wpa_printf(MSG_INFO, "QM: Wait until AP sends a DSCP request");
1433 		return -1;
1434 	}
1435 
1436 #define DOMAIN_NAME_OFFSET (4 /* OUI */ + 1 /* Attr Id */ + 1 /* Attr len */)
1437 
1438 	if (domain_name_length > 255 - DOMAIN_NAME_OFFSET) {
1439 		wpa_printf(MSG_ERROR, "QM: Too long domain name");
1440 		return -1;
1441 	}
1442 
1443 	dscp_query_size = 1 + /* Category */
1444 			  4 + /* OUI Type */
1445 			  1 + /* OUI subtype */
1446 			  1; /* Dialog Token */
1447 	if (domain_name && domain_name_length)
1448 		dscp_query_size += 1 + /* Element ID */
1449 			1 + /* IE Length */
1450 			DOMAIN_NAME_OFFSET + domain_name_length;
1451 
1452 	buf = wpabuf_alloc(dscp_query_size);
1453 	if (!buf) {
1454 		wpa_printf(MSG_ERROR, "QM: Failed to allocate DSCP query");
1455 		return -1;
1456 	}
1457 
1458 	wpabuf_put_u8(buf, WLAN_ACTION_VENDOR_SPECIFIC_PROTECTED);
1459 	wpabuf_put_be32(buf, QM_ACTION_VENDOR_TYPE);
1460 	wpabuf_put_u8(buf, QM_DSCP_POLICY_QUERY);
1461 	wpa_s->dscp_query_dialog_token++;
1462 	if (wpa_s->dscp_query_dialog_token == 0)
1463 		wpa_s->dscp_query_dialog_token++;
1464 	wpabuf_put_u8(buf, wpa_s->dscp_query_dialog_token);
1465 
1466 	if (domain_name && domain_name_length) {
1467 		/* Domain Name attribute */
1468 		wpabuf_put_u8(buf, WLAN_EID_VENDOR_SPECIFIC);
1469 		wpabuf_put_u8(buf, DOMAIN_NAME_OFFSET + domain_name_length);
1470 		wpabuf_put_be32(buf, QM_IE_VENDOR_TYPE);
1471 		wpabuf_put_u8(buf, QM_ATTR_DOMAIN_NAME);
1472 		wpabuf_put_u8(buf, domain_name_length);
1473 		wpabuf_put_data(buf, domain_name, domain_name_length);
1474 	}
1475 #undef DOMAIN_NAME_OFFSET
1476 
1477 	ret = wpa_drv_send_action(wpa_s, wpa_s->assoc_freq, 0, wpa_s->bssid,
1478 				  wpa_s->own_addr, wpa_s->bssid,
1479 				  wpabuf_head(buf), wpabuf_len(buf), 0);
1480 	if (ret < 0) {
1481 		wpa_dbg(wpa_s, MSG_ERROR, "QM: Failed to send DSCP query");
1482 		wpa_s->dscp_query_dialog_token--;
1483 	}
1484 
1485 	wpabuf_free(buf);
1486 	return ret;
1487 }
1488