krackattacks/src/ap/mbo_ap.c

/*
 * hostapd - MBO
 * Copyright (c) 2016, Qualcomm Atheros, 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 "common/ieee802_11_defs.h"
#include "common/ieee802_11_common.h"
#include "hostapd.h"
#include "sta_info.h"
#include "mbo_ap.h"


void mbo_ap_sta_free(struct sta_info *sta)
{
	struct mbo_non_pref_chan_info *info, *prev;

	info = sta->non_pref_chan;
	sta->non_pref_chan = NULL;
	while (info) {
		prev = info;
		info = info->next;
		os_free(prev);
	}
}


static void mbo_ap_parse_non_pref_chan(struct sta_info *sta,
				       const u8 *buf, size_t len)
{
	struct mbo_non_pref_chan_info *info, *tmp;
	char channels[200], *pos, *end;
	size_t num_chan, i;
	int ret;

	if (len <= 4)
		return; /* Not enough room for any channels */

	num_chan = len - 4;
	info = os_zalloc(sizeof(*info) + num_chan);
	if (!info)
		return;
	info->op_class = buf[0];
	info->pref = buf[len - 3];
	info->reason_code = buf[len - 2];
	info->reason_detail = buf[len - 1];
	info->num_channels = num_chan;
	buf++;
	os_memcpy(info->channels, buf, num_chan);
	if (!sta->non_pref_chan) {
		sta->non_pref_chan = info;
	} else {
		tmp = sta->non_pref_chan;
		while (tmp->next)
			tmp = tmp->next;
		tmp->next = info;
	}

	pos = channels;
	end = pos + sizeof(channels);
	*pos = '\0';
	for (i = 0; i < num_chan; i++) {
		ret = os_snprintf(pos, end - pos, "%s%u",
				  i == 0 ? "" : " ", buf[i]);
		if (os_snprintf_error(end - pos, ret)) {
			*pos = '\0';
			break;
		}
		pos += ret;
	}

	wpa_printf(MSG_DEBUG, "MBO: STA " MACSTR
		   " non-preferred channel list (op class %u, pref %u, reason code %u, reason detail %u, channels %s)",
		   MAC2STR(sta->addr), info->op_class, info->pref,
		   info->reason_code, info->reason_detail, channels);
}


void mbo_ap_check_sta_assoc(struct hostapd_data *hapd, struct sta_info *sta,
			    struct ieee802_11_elems *elems)
{
	const u8 *pos, *attr, *end;
	size_t len;

	if (!hapd->conf->mbo_enabled || !elems->mbo)
		return;

	pos = elems->mbo + 4;
	len = elems->mbo_len - 4;
	wpa_hexdump(MSG_DEBUG, "MBO: Association Request attributes", pos, len);

	attr = get_ie(pos, len, MBO_ATTR_ID_CELL_DATA_CAPA);
	if (attr && attr[1] >= 1)
		sta->cell_capa = attr[2];

	mbo_ap_sta_free(sta);
	end = pos + len;
	while (end - pos > 1) {
		u8 ie_len = pos[1];

		if (2 + ie_len > end - pos)
			break;

		if (pos[0] == MBO_ATTR_ID_NON_PREF_CHAN_REPORT)
			mbo_ap_parse_non_pref_chan(sta, pos + 2, ie_len);
		pos += 2 + pos[1];
	}
}


int mbo_ap_get_info(struct sta_info *sta, char *buf, size_t buflen)
{
	char *pos = buf, *end = buf + buflen;
	int ret;
	struct mbo_non_pref_chan_info *info;
	u8 i;
	unsigned int count = 0;

	if (!sta->cell_capa)
		return 0;

	ret = os_snprintf(pos, end - pos, "mbo_cell_capa=%u\n", sta->cell_capa);
	if (os_snprintf_error(end - pos, ret))
		return pos - buf;
	pos += ret;

	for (info = sta->non_pref_chan; info; info = info->next) {
		char *pos2 = pos;

		ret = os_snprintf(pos2, end - pos2,
				  "non_pref_chan[%u]=%u:%u:%u:%u:",
				  count, info->op_class, info->pref,
				  info->reason_code, info->reason_detail);
		count++;
		if (os_snprintf_error(end - pos2, ret))
			break;
		pos2 += ret;

		for (i = 0; i < info->num_channels; i++) {
			ret = os_snprintf(pos2, end - pos2, "%u%s",
					  info->channels[i],
					  i + 1 < info->num_channels ?
					  "," : "");
			if (os_snprintf_error(end - pos2, ret)) {
				pos2 = NULL;
				break;
			}
			pos2 += ret;
		}

		if (!pos2)
			break;
		ret = os_snprintf(pos2, end - pos2, "\n");
		if (os_snprintf_error(end - pos2, ret))
			break;
		pos2 += ret;
		pos = pos2;
	}

	return pos - buf;
}


static void mbo_ap_wnm_notif_req_cell_capa(struct sta_info *sta,
					   const u8 *buf, size_t len)
{
	if (len < 1)
		return;
	wpa_printf(MSG_DEBUG, "MBO: STA " MACSTR
		   " updated cellular data capability: %u",
		   MAC2STR(sta->addr), buf[0]);
	sta->cell_capa = buf[0];
}


static void mbo_ap_wnm_notif_req_elem(struct sta_info *sta, u8 type,
				      const u8 *buf, size_t len,
				      int *first_non_pref_chan)
{
	switch (type) {
	case WFA_WNM_NOTIF_SUBELEM_NON_PREF_CHAN_REPORT:
		if (*first_non_pref_chan) {
			/*
			 * Need to free the previously stored entries now to
			 * allow the update to replace all entries.
			 */
			*first_non_pref_chan = 0;
			mbo_ap_sta_free(sta);
		}
		mbo_ap_parse_non_pref_chan(sta, buf, len);
		break;
	case WFA_WNM_NOTIF_SUBELEM_CELL_DATA_CAPA:
		mbo_ap_wnm_notif_req_cell_capa(sta, buf, len);
		break;
	default:
		wpa_printf(MSG_DEBUG,
			   "MBO: Ignore unknown WNM Notification WFA subelement %u",
			   type);
		break;
	}
}


void mbo_ap_wnm_notification_req(struct hostapd_data *hapd, const u8 *addr,
				 const u8 *buf, size_t len)
{
	const u8 *pos, *end;
	u8 ie_len;
	struct sta_info *sta;
	int first_non_pref_chan = 1;

	if (!hapd->conf->mbo_enabled)
		return;

	sta = ap_get_sta(hapd, addr);
	if (!sta)
		return;

	pos = buf;
	end = buf + len;

	while (end - pos > 1) {
		ie_len = pos[1];

		if (2 + ie_len > end - pos)
			break;

		if (pos[0] == WLAN_EID_VENDOR_SPECIFIC &&
		    ie_len >= 4 && WPA_GET_BE24(pos + 2) == OUI_WFA)
			mbo_ap_wnm_notif_req_elem(sta, pos[5],
						  pos + 6, ie_len - 4,
						  &first_non_pref_chan);
		else
			wpa_printf(MSG_DEBUG,
				   "MBO: Ignore unknown WNM Notification element %u (len=%u)",
				   pos[0], pos[1]);

		pos += 2 + pos[1];
	}
}