[thirdparty/hostap.git] / wlantest / rx_tdls.c

/*
 * Received Data frame processing for TDLS packets
 * Copyright (c) 2010, Jouni Malinen <j@w1.fi>
 *
 * 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 "crypto/sha256.h"
#include "crypto/crypto.h"
#include "crypto/aes_wrap.h"
#include "common/ieee802_11_defs.h"
#include "common/ieee802_11_common.h"
#include "wlantest.h"


static struct wlantest_tdls * get_tdls(struct wlantest *wt, const u8 *linkid,
				       int create_new, const u8 *bssid)
{
	struct wlantest_bss *bss;
	struct wlantest_sta *init, *resp;
	struct wlantest_tdls *tdls;

	bss = bss_find(wt, linkid);
	if (bss == NULL && bssid) {
		bss = bss_find(wt, bssid);
		if (bss)
			add_note(wt, MSG_INFO, "TDLS: Incorrect BSSID " MACSTR
				 " in LinkId?! (init=" MACSTR " resp="
				 MACSTR ")",
				 MAC2STR(linkid), MAC2STR(linkid + ETH_ALEN),
				 MAC2STR(linkid + 2 * ETH_ALEN));
	}
	if (bss == NULL)
		return NULL;

	init = sta_find(bss, linkid + ETH_ALEN);
	if (init == NULL)
		return NULL;

	resp = sta_find(bss, linkid + 2 * ETH_ALEN);
	if (resp == NULL)
		return NULL;

	dl_list_for_each(tdls, &bss->tdls, struct wlantest_tdls, list) {
		if (tdls->init == init && tdls->resp == resp)
			return tdls;
	}

	if (!create_new)
		return NULL;

	add_note(wt, MSG_DEBUG, "Add new TDLS link context: initiator " MACSTR
		 " responder " MACSTR " BSSID " MACSTR,
		 MAC2STR(linkid + ETH_ALEN),
		 MAC2STR(linkid + 2 * ETH_ALEN),
		 MAC2STR(bssid));

	tdls = os_zalloc(sizeof(*tdls));
	if (tdls == NULL)
		return NULL;
	tdls->init = init;
	tdls->resp = resp;
	dl_list_add(&bss->tdls, &tdls->list);
	return tdls;
}


static int tdls_derive_tpk(struct wlantest_tdls *tdls, const u8 *bssid,
			   const u8 *ftie, u8 ftie_len)
{
	const struct rsn_ftie *f;
	u8 key_input[SHA256_MAC_LEN];
	const u8 *nonce[2];
	size_t len[2];
	u8 data[3 * ETH_ALEN];

	if (ftie == NULL || ftie_len < sizeof(struct rsn_ftie))
		return 0;

	f = (const struct rsn_ftie *) ftie;
	wpa_hexdump(MSG_DEBUG, "TDLS ANonce", f->anonce, WPA_NONCE_LEN);
	wpa_hexdump(MSG_DEBUG, "TDLS SNonce", f->snonce, WPA_NONCE_LEN);

	/*
	 * IEEE Std 802.11z-2010 8.5.9.1:
	 * TPK-Key-Input = SHA-256(min(SNonce, ANonce) || max(SNonce, ANonce))
	 */
	len[0] = WPA_NONCE_LEN;
	len[1] = WPA_NONCE_LEN;
	if (os_memcmp(f->anonce, f->snonce, WPA_NONCE_LEN) < 0) {
		nonce[0] = f->anonce;
		nonce[1] = f->snonce;
	} else {
		nonce[0] = f->snonce;
		nonce[1] = f->anonce;
	}
	sha256_vector(2, nonce, len, key_input);
	wpa_hexdump_key(MSG_DEBUG, "TDLS: TPK-Key-Input",
			key_input, SHA256_MAC_LEN);

	/*
	 * TPK-Key-Data = KDF-N_KEY(TPK-Key-Input, "TDLS PMK",
	 *	min(MAC_I, MAC_R) || max(MAC_I, MAC_R) || BSSID || N_KEY)
	 * TODO: is N_KEY really included in KDF Context and if so, in which
	 * presentation format (little endian 16-bit?) is it used? It gets
	 * added by the KDF anyway..
	 */

	if (os_memcmp(tdls->init->addr, tdls->resp->addr, ETH_ALEN) < 0) {
		os_memcpy(data, tdls->init->addr, ETH_ALEN);
		os_memcpy(data + ETH_ALEN, tdls->resp->addr, ETH_ALEN);
	} else {
		os_memcpy(data, tdls->resp->addr, ETH_ALEN);
		os_memcpy(data + ETH_ALEN, tdls->init->addr, ETH_ALEN);
	}
	os_memcpy(data + 2 * ETH_ALEN, bssid, ETH_ALEN);
	wpa_hexdump(MSG_DEBUG, "TDLS: KDF Context", data, sizeof(data));

	sha256_prf(key_input, SHA256_MAC_LEN, "TDLS PMK", data, sizeof(data),
		   (u8 *) &tdls->tpk, sizeof(tdls->tpk));
	wpa_hexdump_key(MSG_DEBUG, "TDLS: TPK-KCK",
			tdls->tpk.kck, sizeof(tdls->tpk.kck));
	wpa_hexdump_key(MSG_DEBUG, "TDLS: TPK-TK",
			tdls->tpk.tk, sizeof(tdls->tpk.tk));

	return 1;
}


static int tdls_verify_mic(struct wlantest *wt, struct wlantest_tdls *tdls,
			   u8 trans_seq, struct ieee802_11_elems *elems)
{
	u8 *buf, *pos;
	int len;
	u8 mic[16];
	int ret;
	const struct rsn_ftie *rx_ftie;
	struct rsn_ftie *tmp_ftie;

	if (elems->link_id == NULL || elems->rsn_ie == NULL ||
	    elems->timeout_int == NULL || elems->ftie == NULL ||
	    elems->ftie_len < sizeof(struct rsn_ftie))
		return -1;

	len = 2 * ETH_ALEN + 1 + 2 + 18 + 2 + elems->rsn_ie_len +
		2 + 5 + 2 + elems->ftie_len;

	buf = os_zalloc(len);
	if (buf == NULL)
		return -1;

	pos = buf;
	/* 1) TDLS initiator STA MAC address */
	os_memcpy(pos, elems->link_id + ETH_ALEN, ETH_ALEN);
	pos += ETH_ALEN;
	/* 2) TDLS responder STA MAC address */
	os_memcpy(pos, elems->link_id + 2 * ETH_ALEN, ETH_ALEN);
	pos += ETH_ALEN;
	/* 3) Transaction Sequence number */
	*pos++ = trans_seq;
	/* 4) Link Identifier IE */
	os_memcpy(pos, elems->link_id - 2, 2 + 18);
	pos += 2 + 18;
	/* 5) RSN IE */
	os_memcpy(pos, elems->rsn_ie - 2, 2 + elems->rsn_ie_len);
	pos += 2 + elems->rsn_ie_len;
	/* 6) Timeout Interval IE */
	os_memcpy(pos, elems->timeout_int - 2, 2 + 5);
	pos += 2 + 5;
	/* 7) FTIE, with the MIC field of the FTIE set to 0 */
	os_memcpy(pos, elems->ftie - 2, 2 + elems->ftie_len);
	pos += 2;
	tmp_ftie = (struct rsn_ftie *) pos;
	os_memset(tmp_ftie->mic, 0, 16);
	pos += elems->ftie_len;

	wpa_hexdump(MSG_DEBUG, "TDLS: Data for FTIE MIC", buf, pos - buf);
	wpa_hexdump_key(MSG_DEBUG, "TDLS: KCK", tdls->tpk.kck, 16);
	ret = omac1_aes_128(tdls->tpk.kck, buf, pos - buf, mic);
	os_free(buf);
	if (ret)
		return -1;
	wpa_hexdump(MSG_DEBUG, "TDLS: FTIE MIC", mic, 16);
	rx_ftie = (const struct rsn_ftie *) elems->ftie;

	if (os_memcmp(mic, rx_ftie->mic, 16) == 0) {
		add_note(wt, MSG_DEBUG, "TDLS: Valid MIC");
		return 0;
	}
	add_note(wt, MSG_DEBUG, "TDLS: Invalid MIC");
	return -1;
}


static void rx_data_tdls_setup_request(struct wlantest *wt, const u8 *bssid,
				       const u8 *sta_addr, const u8 *dst,
				       const u8 *src,
				       const u8 *data, size_t len)
{
	struct ieee802_11_elems elems;
	struct wlantest_tdls *tdls;
	u8 linkid[3 * ETH_ALEN];

	if (len < 3) {
		add_note(wt, MSG_INFO, "Too short TDLS Setup Request " MACSTR
			 " -> " MACSTR, MAC2STR(src), MAC2STR(dst));
		return;
	}
	wpa_printf(MSG_DEBUG, "TDLS Setup Request " MACSTR " -> "
		   MACSTR, MAC2STR(src), MAC2STR(dst));

	if (ieee802_11_parse_elems(data + 3, len - 3, &elems, 1) ==
	    ParseFailed || elems.link_id == NULL)
		return;
	wpa_printf(MSG_DEBUG, "TDLS Link Identifier: BSSID " MACSTR
		   " initiator STA " MACSTR " responder STA " MACSTR,
		   MAC2STR(elems.link_id), MAC2STR(elems.link_id + ETH_ALEN),
		   MAC2STR(elems.link_id + 2 * ETH_ALEN));
	tdls = get_tdls(wt, elems.link_id, 1, bssid);
	if (tdls) {
		tdls->counters[WLANTEST_TDLS_COUNTER_SETUP_REQ]++;
		tdls->dialog_token = data[0];
		if (elems.ftie && elems.ftie_len >= sizeof(struct rsn_ftie)) {
			const struct rsn_ftie *f;
			f = (const struct rsn_ftie *) elems.ftie;
			os_memcpy(tdls->inonce, f->snonce, WPA_NONCE_LEN);
		}
	}

	/* Check whether reverse direction context exists already */
	os_memcpy(linkid, bssid, ETH_ALEN);
	os_memcpy(linkid + ETH_ALEN, dst, ETH_ALEN);
	os_memcpy(linkid + 2 * ETH_ALEN, src, ETH_ALEN);
	tdls = get_tdls(wt, linkid, 0, bssid);
	if (tdls)
		add_note(wt, MSG_INFO, "Reverse direction TDLS context exists");
}


static void rx_data_tdls_setup_response_failure(struct wlantest *wt,
						const u8 *bssid,
						const u8 *sta_addr,
						u8 dialog_token, u16 status)
{
	struct wlantest_bss *bss;
	struct wlantest_tdls *tdls;
	struct wlantest_sta *sta;

	if (status == WLAN_STATUS_SUCCESS) {
		add_note(wt, MSG_INFO, "TDLS: Invalid TDLS Setup Response from "
			 MACSTR, MAC2STR(sta_addr));
		return;
	}

	bss = bss_find(wt, bssid);
	if (!bss)
		return;
	sta = sta_find(bss, sta_addr);
	if (!sta)
		return;

	dl_list_for_each(tdls, &bss->tdls, struct wlantest_tdls, list) {
		if (tdls->resp == sta) {
			if (dialog_token != tdls->dialog_token) {
				add_note(wt, MSG_DEBUG, "TDLS: Dialog token "
					 "mismatch in TDLS Setup Response "
					 "(failure)");
				break;
			}
			add_note(wt, MSG_DEBUG, "TDLS: Found matching TDLS "
				 "setup session based on dialog token");
			tdls->counters[
				WLANTEST_TDLS_COUNTER_SETUP_RESP_FAIL]++;
			break;
		}
	}
}


static void rx_data_tdls_setup_response(struct wlantest *wt, const u8 *bssid,
					const u8 *sta_addr, const u8 *dst,
					const u8 *src,
					const u8 *data, size_t len)
{
	u16 status;
	struct ieee802_11_elems elems;
	struct wlantest_tdls *tdls;

	if (len < 3) {
		add_note(wt, MSG_INFO, "Too short TDLS Setup Response " MACSTR
			 " -> " MACSTR, MAC2STR(src), MAC2STR(dst));
		return;
	}
	status = WPA_GET_LE16(data);
	wpa_printf(MSG_DEBUG, "TDLS Setup Response " MACSTR " -> "
		   MACSTR " (status %d)",
		   MAC2STR(src), MAC2STR(dst), status);
	if (len < 5 && status == 0) {
		add_note(wt, MSG_INFO, "Too short TDLS Setup Response " MACSTR
			 " -> " MACSTR, MAC2STR(src), MAC2STR(dst));
		return;
	}

	if (len < 5 ||
	    ieee802_11_parse_elems(data + 5, len - 5, &elems, 1) ==
	    ParseFailed || elems.link_id == NULL) {
		/* Need to match TDLS link based on Dialog Token */
		rx_data_tdls_setup_response_failure(wt, bssid, sta_addr,
						    data[2], status);
		return;
	}
	wpa_printf(MSG_DEBUG, "TDLS Link Identifier: BSSID " MACSTR
		   " initiator STA " MACSTR " responder STA " MACSTR,
		   MAC2STR(elems.link_id), MAC2STR(elems.link_id + ETH_ALEN),
		   MAC2STR(elems.link_id + 2 * ETH_ALEN));

	tdls = get_tdls(wt, elems.link_id, 1, bssid);
	if (!tdls) {
		add_note(wt, MSG_INFO, "No match TDLS context found");
		return;
	}
	if (status)
		tdls->counters[WLANTEST_TDLS_COUNTER_SETUP_RESP_FAIL]++;
	else
		tdls->counters[WLANTEST_TDLS_COUNTER_SETUP_RESP_OK]++;

	if (status != WLAN_STATUS_SUCCESS)
		return;

	if (elems.ftie && elems.ftie_len >= sizeof(struct rsn_ftie)) {
		const struct rsn_ftie *f;
		f = (const struct rsn_ftie *) elems.ftie;
		if (os_memcmp(tdls->inonce, f->snonce, WPA_NONCE_LEN) != 0) {
			add_note(wt, MSG_INFO, "Mismatch in TDLS initiator "
				 "nonce");
		}
		os_memcpy(tdls->rnonce, f->anonce, WPA_NONCE_LEN);
	}

	if (tdls_derive_tpk(tdls, bssid, elems.ftie, elems.ftie_len) < 1)
		return;
	if (tdls_verify_mic(wt, tdls, 2, &elems) == 0) {
		tdls->dialog_token = data[2];
		add_note(wt, MSG_DEBUG, "TDLS: Dialog Token for the link: %u",
			 tdls->dialog_token);
	}
}


static void rx_data_tdls_setup_confirm_failure(struct wlantest *wt,
					       const u8 *bssid,
					       const u8 *src,
					       u8 dialog_token, u16 status)
{
	struct wlantest_bss *bss;
	struct wlantest_tdls *tdls;
	struct wlantest_sta *sta;

	if (status == WLAN_STATUS_SUCCESS) {
		add_note(wt, MSG_INFO, "TDLS: Invalid TDLS Setup Confirm from "
			 MACSTR, MAC2STR(src));
		return;
	}

	bss = bss_find(wt, bssid);
	if (!bss)
		return;
	sta = sta_find(bss, src);
	if (!sta)
		return;

	dl_list_for_each(tdls, &bss->tdls, struct wlantest_tdls, list) {
		if (tdls->init == sta) {
			if (dialog_token != tdls->dialog_token) {
				add_note(wt, MSG_DEBUG, "TDLS: Dialog token "
					 "mismatch in TDLS Setup Confirm "
					 "(failure)");
				break;
			}
			add_note(wt, MSG_DEBUG, "TDLS: Found matching TDLS "
				 "setup session based on dialog token");
			tdls->counters[
				WLANTEST_TDLS_COUNTER_SETUP_CONF_FAIL]++;
			break;
		}
	}
}


static void rx_data_tdls_setup_confirm(struct wlantest *wt, const u8 *bssid,
				       const u8 *sta_addr, const u8 *dst,
				       const u8 *src,
				       const u8 *data, size_t len)
{
	u16 status;
	struct ieee802_11_elems elems;
	struct wlantest_tdls *tdls;
	u8 link_id[3 * ETH_ALEN];

	if (len < 3) {
		add_note(wt, MSG_INFO, "Too short TDLS Setup Confirm " MACSTR
			 " -> " MACSTR, MAC2STR(src), MAC2STR(dst));
		return;
	}
	status = WPA_GET_LE16(data);
	wpa_printf(MSG_DEBUG, "TDLS Setup Confirm " MACSTR " -> "
		   MACSTR " (status %d)",
		   MAC2STR(src), MAC2STR(dst), status);

	if (ieee802_11_parse_elems(data + 3, len - 3, &elems, 1) ==
	    ParseFailed || elems.link_id == NULL) {
		/* Need to match TDLS link based on Dialog Token */
		rx_data_tdls_setup_confirm_failure(wt, bssid, src,
						   data[2], status);
		return;
	}
	wpa_printf(MSG_DEBUG, "TDLS Link Identifier: BSSID " MACSTR
		   " initiator STA " MACSTR " responder STA " MACSTR,
		   MAC2STR(elems.link_id), MAC2STR(elems.link_id + ETH_ALEN),
		   MAC2STR(elems.link_id + 2 * ETH_ALEN));

	tdls = get_tdls(wt, elems.link_id, 1, bssid);
	if (tdls == NULL)
		return;
	if (status)
		tdls->counters[WLANTEST_TDLS_COUNTER_SETUP_CONF_FAIL]++;
	else
		tdls->counters[WLANTEST_TDLS_COUNTER_SETUP_CONF_OK]++;

	if (status != WLAN_STATUS_SUCCESS)
		return;

	if (elems.ftie && elems.ftie_len >= sizeof(struct rsn_ftie)) {
		const struct rsn_ftie *f;
		f = (const struct rsn_ftie *) elems.ftie;
		if (os_memcmp(tdls->inonce, f->snonce, WPA_NONCE_LEN) != 0) {
			add_note(wt, MSG_INFO, "Mismatch in TDLS initiator "
				 "nonce");
		}
		if (os_memcmp(tdls->rnonce, f->anonce, WPA_NONCE_LEN) != 0) {
			add_note(wt, MSG_INFO, "Mismatch in TDLS responder "
				 "nonce");
		}
	}

	tdls->link_up = 1;
	if (tdls_derive_tpk(tdls, bssid, elems.ftie, elems.ftie_len) < 1) {
		if (elems.ftie == NULL)
			goto remove_reverse;
		return;
	}
	if (tdls_verify_mic(wt, tdls, 3, &elems) == 0) {
		tdls->dialog_token = data[2];
		add_note(wt, MSG_DEBUG, "TDLS: Link up - Dialog Token: %u",
			 tdls->dialog_token);
	}

remove_reverse:
	/*
	 * The TDLS link itself is bidirectional, but there is explicit
	 * initiator/responder roles. Remove the other direction of the link
	 * (if it exists) to make sure that the link counters are stored for
	 * the current TDLS entery.
	 */
	os_memcpy(link_id, elems.link_id, ETH_ALEN);
	os_memcpy(link_id + ETH_ALEN, elems.link_id + 2 * ETH_ALEN, ETH_ALEN);
	os_memcpy(link_id + 2 * ETH_ALEN, elems.link_id + ETH_ALEN, ETH_ALEN);
	tdls = get_tdls(wt, link_id, 0, bssid);
	if (tdls) {
		add_note(wt, MSG_DEBUG, "TDLS: Remove reverse link entry");
		tdls_deinit(tdls);
	}
}


static int tdls_verify_mic_teardown(struct wlantest *wt,
				    struct wlantest_tdls *tdls, u8 trans_seq,
				    const u8 *reason_code,
				    struct ieee802_11_elems *elems)
{
	u8 *buf, *pos;
	int len;
	u8 mic[16];
	int ret;
	const struct rsn_ftie *rx_ftie;
	struct rsn_ftie *tmp_ftie;

	if (elems->link_id == NULL || elems->ftie == NULL ||
	    elems->ftie_len < sizeof(struct rsn_ftie))
		return -1;

	len = 2 + 18 + 2 + 1 + 1 + 2 + elems->ftie_len;

	buf = os_zalloc(len);
	if (buf == NULL)
		return -1;

	pos = buf;
	/* 1) Link Identifier IE */
	os_memcpy(pos, elems->link_id - 2, 2 + 18);
	pos += 2 + 18;
	/* 2) Reason Code */
	os_memcpy(pos, reason_code, 2);
	pos += 2;
	/* 3) Dialog token */
	*pos++ = tdls->dialog_token;
	/* 4) Transaction Sequence number */
	*pos++ = trans_seq;
	/* 5) FTIE, with the MIC field of the FTIE set to 0 */
	os_memcpy(pos, elems->ftie - 2, 2 + elems->ftie_len);
	pos += 2;
	tmp_ftie = (struct rsn_ftie *) pos;
	os_memset(tmp_ftie->mic, 0, 16);
	pos += elems->ftie_len;

	wpa_hexdump(MSG_DEBUG, "TDLS: Data for FTIE MIC", buf, pos - buf);
	wpa_hexdump_key(MSG_DEBUG, "TDLS: KCK", tdls->tpk.kck, 16);
	ret = omac1_aes_128(tdls->tpk.kck, buf, pos - buf, mic);
	os_free(buf);
	if (ret)
		return -1;
	wpa_hexdump(MSG_DEBUG, "TDLS: FTIE MIC", mic, 16);
	rx_ftie = (const struct rsn_ftie *) elems->ftie;

	if (os_memcmp(mic, rx_ftie->mic, 16) == 0) {
		add_note(wt, MSG_DEBUG, "TDLS: Valid MIC");
		return 0;
	}
	add_note(wt, MSG_DEBUG, "TDLS: Invalid MIC");
	return -1;
}


static void rx_data_tdls_teardown(struct wlantest *wt, const u8 *bssid,
				  const u8 *sta_addr, const u8 *dst,
				  const u8 *src,
				  const u8 *data, size_t len)
{
	u16 reason;
	struct ieee802_11_elems elems;
	struct wlantest_tdls *tdls;

	if (len < 2)
		return;
	reason = WPA_GET_LE16(data);
	wpa_printf(MSG_DEBUG, "TDLS Teardown " MACSTR " -> "
		   MACSTR " (reason %d)",
		   MAC2STR(src), MAC2STR(dst), reason);

	if (ieee802_11_parse_elems(data + 2, len - 2, &elems, 1) ==
	    ParseFailed || elems.link_id == NULL)
		return;
	wpa_printf(MSG_DEBUG, "TDLS Link Identifier: BSSID " MACSTR
		   " initiator STA " MACSTR " responder STA " MACSTR,
		   MAC2STR(elems.link_id), MAC2STR(elems.link_id + ETH_ALEN),
		   MAC2STR(elems.link_id + 2 * ETH_ALEN));

	tdls = get_tdls(wt, elems.link_id, 1, bssid);
	if (tdls) {
		if (tdls->link_up)
			add_note(wt, MSG_DEBUG, "TDLS: Link down");
		tdls->link_up = 0;
		tdls->counters[WLANTEST_TDLS_COUNTER_TEARDOWN]++;
		tdls_verify_mic_teardown(wt, tdls, 4, data, &elems);
	}
}


static void rx_data_tdls(struct wlantest *wt, const u8 *bssid,
			 const u8 *sta_addr, const u8 *dst, const u8 *src,
			 const u8 *data, size_t len)
{
	/* data contains the payload of a TDLS Action frame */
	if (len < 2 || data[0] != WLAN_ACTION_TDLS) {
		wpa_hexdump(MSG_DEBUG, "Unrecognized encapsulated TDLS frame",
			    data, len);
		return;
	}

	switch (data[1]) {
	case WLAN_TDLS_SETUP_REQUEST:
		rx_data_tdls_setup_request(wt, bssid, sta_addr, dst, src,
					   data + 2, len - 2);
		break;
	case WLAN_TDLS_SETUP_RESPONSE:
		rx_data_tdls_setup_response(wt, bssid, sta_addr, dst, src,
					    data + 2, len - 2);
		break;
	case WLAN_TDLS_SETUP_CONFIRM:
		rx_data_tdls_setup_confirm(wt, bssid, sta_addr, dst, src,
					   data + 2, len - 2);
		break;
	case WLAN_TDLS_TEARDOWN:
		rx_data_tdls_teardown(wt, bssid, sta_addr, dst, src, data + 2,
				      len - 2);
		break;
	case WLAN_TDLS_DISCOVERY_REQUEST:
		wpa_printf(MSG_DEBUG, "TDLS Discovery Request " MACSTR " -> "
			   MACSTR, MAC2STR(src), MAC2STR(dst));
		break;
	}
}


void rx_data_80211_encap(struct wlantest *wt, const u8 *bssid,
			 const u8 *sta_addr, const u8 *dst, const u8 *src,
			 const u8 *data, size_t len)
{
	wpa_hexdump(MSG_EXCESSIVE, "802.11 data encap frame", data, len);
	if (len < 1)
		return;
	if (data[0] == 0x02)
		rx_data_tdls(wt, bssid, sta_addr, dst, src, data + 1, len - 1);
}
Commit	Line	Data
b39f5834 JM	1	/*
	2	* Received Data frame processing for TDLS packets
	3	* Copyright (c) 2010, Jouni Malinen <j@w1.fi>
	4	*
0f3d578e JM	5	* This software may be distributed under the terms of the BSD license.
0f3d578e JM	6	* See README for more details.
b39f5834 JM	7	*/
	8
	9	#include "utils/includes.h"
	10
	11	#include "utils/common.h"
	12	#include "crypto/sha256.h"
	13	#include "crypto/crypto.h"
	14	#include "crypto/aes_wrap.h"
	15	#include "common/ieee802_11_defs.h"
	16	#include "common/ieee802_11_common.h"
	17	#include "wlantest.h"
	18
	19
0d2e395d	20	static struct wlantest_tdls * get_tdls(struct wlantest wt, const u8 linkid,
44a04866	21	int create_new, const u8 *bssid)
b39f5834 JM	22	{
	23	struct wlantest_bss *bss;
	24	struct wlantest_sta init, resp;
	25	struct wlantest_tdls *tdls;
	26
	27	bss = bss_find(wt, linkid);
44a04866 JM	28	if (bss == NULL && bssid) {
	29	bss = bss_find(wt, bssid);
	30	if (bss)
e4d99217 JM	31	add_note(wt, MSG_INFO, "TDLS: Incorrect BSSID " MACSTR
	32	" in LinkId?! (init=" MACSTR " resp="
	33	MACSTR ")",
	34	MAC2STR(linkid), MAC2STR(linkid + ETH_ALEN),
	35	MAC2STR(linkid + 2 * ETH_ALEN));
44a04866	36	}
b39f5834 JM	37	if (bss == NULL)
	38	return NULL;
	39
	40	init = sta_find(bss, linkid + ETH_ALEN);
	41	if (init == NULL)
	42	return NULL;
	43
	44	resp = sta_find(bss, linkid + 2 * ETH_ALEN);
	45	if (resp == NULL)
	46	return NULL;
	47
	48	dl_list_for_each(tdls, &bss->tdls, struct wlantest_tdls, list) {
	49	if (tdls->init == init && tdls->resp == resp)
	50	return tdls;
	51	}
	52
0d2e395d JM	53	if (!create_new)
	54	return NULL;
	55
4ac800db JM	56	add_note(wt, MSG_DEBUG, "Add new TDLS link context: initiator " MACSTR
	57	" responder " MACSTR " BSSID " MACSTR,
	58	MAC2STR(linkid + ETH_ALEN),
	59	MAC2STR(linkid + 2 * ETH_ALEN),
	60	MAC2STR(bssid));
	61
b39f5834 JM	62	tdls = os_zalloc(sizeof(*tdls));
	63	if (tdls == NULL)
	64	return NULL;
	65	tdls->init = init;
	66	tdls->resp = resp;
	67	dl_list_add(&bss->tdls, &tdls->list);
	68	return tdls;
	69	}
	70
	71
	72	static int tdls_derive_tpk(struct wlantest_tdls tdls, const u8 bssid,
	73	const u8 *ftie, u8 ftie_len)
	74	{
	75	const struct rsn_ftie *f;
	76	u8 key_input[SHA256_MAC_LEN];
	77	const u8 *nonce[2];
	78	size_t len[2];
	79	u8 data[3 * ETH_ALEN];
	80
	81	if (ftie == NULL \|\| ftie_len < sizeof(struct rsn_ftie))
	82	return 0;
	83
	84	f = (const struct rsn_ftie *) ftie;
	85	wpa_hexdump(MSG_DEBUG, "TDLS ANonce", f->anonce, WPA_NONCE_LEN);
	86	wpa_hexdump(MSG_DEBUG, "TDLS SNonce", f->snonce, WPA_NONCE_LEN);
	87
	88	/*
	89	* IEEE Std 802.11z-2010 8.5.9.1:
	90	* TPK-Key-Input = SHA-256(min(SNonce, ANonce) \|\| max(SNonce, ANonce))
	91	*/
	92	len[0] = WPA_NONCE_LEN;
	93	len[1] = WPA_NONCE_LEN;
	94	if (os_memcmp(f->anonce, f->snonce, WPA_NONCE_LEN) < 0) {
	95	nonce[0] = f->anonce;
	96	nonce[1] = f->snonce;
	97	} else {
	98	nonce[0] = f->snonce;
	99	nonce[1] = f->anonce;
	100	}
	101	sha256_vector(2, nonce, len, key_input);
	102	wpa_hexdump_key(MSG_DEBUG, "TDLS: TPK-Key-Input",
	103	key_input, SHA256_MAC_LEN);
	104
	105	/*
	106	* TPK-Key-Data = KDF-N_KEY(TPK-Key-Input, "TDLS PMK",
	107	* min(MAC_I, MAC_R) \|\| max(MAC_I, MAC_R) \|\| BSSID \|\| N_KEY)
	108	* TODO: is N_KEY really included in KDF Context and if so, in which
	109	* presentation format (little endian 16-bit?) is it used? It gets
	110	* added by the KDF anyway..
	111	*/
	112
	113	if (os_memcmp(tdls->init->addr, tdls->resp->addr, ETH_ALEN) < 0) {
	114	os_memcpy(data, tdls->init->addr, ETH_ALEN);
	115	os_memcpy(data + ETH_ALEN, tdls->resp->addr, ETH_ALEN);
	116	} else {
	117	os_memcpy(data, tdls->resp->addr, ETH_ALEN);
	118	os_memcpy(data + ETH_ALEN, tdls->init->addr, ETH_ALEN);
	119	}
	120	os_memcpy(data + 2 * ETH_ALEN, bssid, ETH_ALEN);
	121	wpa_hexdump(MSG_DEBUG, "TDLS: KDF Context", data, sizeof(data));
	122
	123	sha256_prf(key_input, SHA256_MAC_LEN, "TDLS PMK", data, sizeof(data),
	124	(u8 *) &tdls->tpk, sizeof(tdls->tpk));
	125	wpa_hexdump_key(MSG_DEBUG, "TDLS: TPK-KCK",
126	tdls->tpk.kck, sizeof(tdls->tpk.kck));
127	wpa_hexdump_key(MSG_DEBUG, "TDLS: TPK-TK",
128	tdls->tpk.tk, sizeof(tdls->tpk.tk));
129
130	return 1;
131	}
132
133
e4d99217 JM	134	static int tdls_verify_mic(struct wlantest wt, struct wlantest_tdls tdls,
e4d99217 JM	135	u8 trans_seq, struct ieee802_11_elems *elems)
b39f5834 JM	136	{
	137	u8 buf, pos;
	138	int len;
	139	u8 mic[16];
	140	int ret;
	141	const struct rsn_ftie *rx_ftie;
	142	struct rsn_ftie *tmp_ftie;
	143
	144	if (elems->link_id == NULL \|\| elems->rsn_ie == NULL \|\|
d4c4ec92 JM	145	elems->timeout_int == NULL \|\| elems->ftie == NULL \|\|
d4c4ec92 JM	146	elems->ftie_len < sizeof(struct rsn_ftie))
b39f5834 JM	147	return -1;
	148
	149	len = 2 * ETH_ALEN + 1 + 2 + 18 + 2 + elems->rsn_ie_len +
b39a0591	150	2 + 5 + 2 + elems->ftie_len;
b39f5834 JM	151
	152	buf = os_zalloc(len);
	153	if (buf == NULL)
	154	return -1;
	155
	156	pos = buf;
	157	/* 1) TDLS initiator STA MAC address */
	158	os_memcpy(pos, elems->link_id + ETH_ALEN, ETH_ALEN);
	159	pos += ETH_ALEN;
	160	/* 2) TDLS responder STA MAC address */
	161	os_memcpy(pos, elems->link_id + 2 * ETH_ALEN, ETH_ALEN);
	162	pos += ETH_ALEN;
	163	/* 3) Transaction Sequence number */
	164	*pos++ = trans_seq;
	165	/* 4) Link Identifier IE */
	166	os_memcpy(pos, elems->link_id - 2, 2 + 18);
	167	pos += 2 + 18;
	168	/* 5) RSN IE */
	169	os_memcpy(pos, elems->rsn_ie - 2, 2 + elems->rsn_ie_len);
	170	pos += 2 + elems->rsn_ie_len;
	171	/* 6) Timeout Interval IE */
b39a0591 JM	172	os_memcpy(pos, elems->timeout_int - 2, 2 + 5);
b39a0591 JM	173	pos += 2 + 5;
b39f5834 JM	174	/* 7) FTIE, with the MIC field of the FTIE set to 0 */
	175	os_memcpy(pos, elems->ftie - 2, 2 + elems->ftie_len);
	176	pos += 2;
	177	tmp_ftie = (struct rsn_ftie *) pos;
	178	os_memset(tmp_ftie->mic, 0, 16);
	179	pos += elems->ftie_len;
	180
	181	wpa_hexdump(MSG_DEBUG, "TDLS: Data for FTIE MIC", buf, pos - buf);
	182	wpa_hexdump_key(MSG_DEBUG, "TDLS: KCK", tdls->tpk.kck, 16);
	183	ret = omac1_aes_128(tdls->tpk.kck, buf, pos - buf, mic);
	184	os_free(buf);
	185	if (ret)
	186	return -1;
	187	wpa_hexdump(MSG_DEBUG, "TDLS: FTIE MIC", mic, 16);
	188	rx_ftie = (const struct rsn_ftie *) elems->ftie;
	189
	190	if (os_memcmp(mic, rx_ftie->mic, 16) == 0) {
e4d99217	191	add_note(wt, MSG_DEBUG, "TDLS: Valid MIC");
b39f5834 JM	192	return 0;
b39f5834 JM	193	}
e4d99217	194	add_note(wt, MSG_DEBUG, "TDLS: Invalid MIC");
b39f5834 JM	195	return -1;
	196	}
	197
	198
	199	static void rx_data_tdls_setup_request(struct wlantest wt, const u8 bssid,
	200	const u8 sta_addr, const u8 dst,
	201	const u8 *src,
	202	const u8 *data, size_t len)
	203	{
	204	struct ieee802_11_elems elems;
eb4923fd	205	struct wlantest_tdls *tdls;
4ac800db	206	u8 linkid[3 * ETH_ALEN];
b39f5834	207
44a04866	208	if (len < 3) {
e4d99217 JM	209	add_note(wt, MSG_INFO, "Too short TDLS Setup Request " MACSTR
e4d99217 JM	210	" -> " MACSTR, MAC2STR(src), MAC2STR(dst));
b39f5834	211	return;
44a04866	212	}
b39f5834 JM	213	wpa_printf(MSG_DEBUG, "TDLS Setup Request " MACSTR " -> "
	214	MACSTR, MAC2STR(src), MAC2STR(dst));
	215
	216	if (ieee802_11_parse_elems(data + 3, len - 3, &elems, 1) ==
	217	ParseFailed \|\| elems.link_id == NULL)
	218	return;
	219	wpa_printf(MSG_DEBUG, "TDLS Link Identifier: BSSID " MACSTR
	220	" initiator STA " MACSTR " responder STA " MACSTR,
	221	MAC2STR(elems.link_id), MAC2STR(elems.link_id + ETH_ALEN),
	222	MAC2STR(elems.link_id + 2 * ETH_ALEN));
44a04866 JM	223	tdls = get_tdls(wt, elems.link_id, 1, bssid);
44a04866 JM	224	if (tdls) {
eb4923fd	225	tdls->counters[WLANTEST_TDLS_COUNTER_SETUP_REQ]++;
44a04866	226	tdls->dialog_token = data[0];
4ac800db JM	227	if (elems.ftie && elems.ftie_len >= sizeof(struct rsn_ftie)) {
	228	const struct rsn_ftie *f;
	229	f = (const struct rsn_ftie *) elems.ftie;
	230	os_memcpy(tdls->inonce, f->snonce, WPA_NONCE_LEN);
	231	}
44a04866	232	}
4ac800db JM	233
	234	/* Check whether reverse direction context exists already */
	235	os_memcpy(linkid, bssid, ETH_ALEN);
	236	os_memcpy(linkid + ETH_ALEN, dst, ETH_ALEN);
	237	os_memcpy(linkid + 2 * ETH_ALEN, src, ETH_ALEN);
	238	tdls = get_tdls(wt, linkid, 0, bssid);
	239	if (tdls)
	240	add_note(wt, MSG_INFO, "Reverse direction TDLS context exists");
44a04866 JM	241	}
	242
	243
	244	static void rx_data_tdls_setup_response_failure(struct wlantest *wt,
	245	const u8 *bssid,
	246	const u8 *sta_addr,
	247	u8 dialog_token, u16 status)
	248	{
	249	struct wlantest_bss *bss;
	250	struct wlantest_tdls *tdls;
	251	struct wlantest_sta *sta;
	252
	253	if (status == WLAN_STATUS_SUCCESS) {
e4d99217 JM	254	add_note(wt, MSG_INFO, "TDLS: Invalid TDLS Setup Response from "
e4d99217 JM	255	MACSTR, MAC2STR(sta_addr));
44a04866 JM	256	return;
	257	}
	258
	259	bss = bss_find(wt, bssid);
	260	if (!bss)
	261	return;
	262	sta = sta_find(bss, sta_addr);
	263	if (!sta)
	264	return;
	265
	266	dl_list_for_each(tdls, &bss->tdls, struct wlantest_tdls, list) {
	267	if (tdls->resp == sta) {
	268	if (dialog_token != tdls->dialog_token) {
e4d99217 JM	269	add_note(wt, MSG_DEBUG, "TDLS: Dialog token "
	270	"mismatch in TDLS Setup Response "
	271	"(failure)");
44a04866 JM	272	break;
44a04866 JM	273	}
e4d99217 JM	274	add_note(wt, MSG_DEBUG, "TDLS: Found matching TDLS "
e4d99217 JM	275	"setup session based on dialog token");
44a04866 JM	276	tdls->counters[
	277	WLANTEST_TDLS_COUNTER_SETUP_RESP_FAIL]++;
	278	break;
	279	}
	280	}
b39f5834 JM	281	}
	282
	283
	284	static void rx_data_tdls_setup_response(struct wlantest wt, const u8 bssid,
	285	const u8 sta_addr, const u8 dst,
	286	const u8 *src,
	287	const u8 *data, size_t len)
	288	{
	289	u16 status;
	290	struct ieee802_11_elems elems;
	291	struct wlantest_tdls *tdls;
	292
44a04866	293	if (len < 3) {
e4d99217 JM	294	add_note(wt, MSG_INFO, "Too short TDLS Setup Response " MACSTR
e4d99217 JM	295	" -> " MACSTR, MAC2STR(src), MAC2STR(dst));
b39f5834	296	return;
44a04866	297	}
b39f5834 JM	298	status = WPA_GET_LE16(data);
	299	wpa_printf(MSG_DEBUG, "TDLS Setup Response " MACSTR " -> "
	300	MACSTR " (status %d)",
	301	MAC2STR(src), MAC2STR(dst), status);
2878cae7	302	if (len < 5 && status == 0) {
e4d99217 JM	303	add_note(wt, MSG_INFO, "Too short TDLS Setup Response " MACSTR
e4d99217 JM	304	" -> " MACSTR, MAC2STR(src), MAC2STR(dst));
b39f5834	305	return;
44a04866	306	}
b39f5834	307
2878cae7 JM	308	if (len < 5 \|\|
2878cae7 JM	309	ieee802_11_parse_elems(data + 5, len - 5, &elems, 1) ==
44a04866 JM	310	ParseFailed \|\| elems.link_id == NULL) {
	311	/* Need to match TDLS link based on Dialog Token */
	312	rx_data_tdls_setup_response_failure(wt, bssid, sta_addr,
	313	data[2], status);
b39f5834	314	return;
44a04866	315	}
b39f5834 JM	316	wpa_printf(MSG_DEBUG, "TDLS Link Identifier: BSSID " MACSTR
	317	" initiator STA " MACSTR " responder STA " MACSTR,
	318	MAC2STR(elems.link_id), MAC2STR(elems.link_id + ETH_ALEN),
	319	MAC2STR(elems.link_id + 2 * ETH_ALEN));
	320
44a04866	321	tdls = get_tdls(wt, elems.link_id, 1, bssid);
4ac800db JM	322	if (!tdls) {
4ac800db JM	323	add_note(wt, MSG_INFO, "No match TDLS context found");
b39f5834	324	return;
4ac800db	325	}
eb4923fd JM	326	if (status)
	327	tdls->counters[WLANTEST_TDLS_COUNTER_SETUP_RESP_FAIL]++;
	328	else
	329	tdls->counters[WLANTEST_TDLS_COUNTER_SETUP_RESP_OK]++;
44a04866 JM	330
	331	if (status != WLAN_STATUS_SUCCESS)
	332	return;
	333
4ac800db JM	334	if (elems.ftie && elems.ftie_len >= sizeof(struct rsn_ftie)) {
	335	const struct rsn_ftie *f;
	336	f = (const struct rsn_ftie *) elems.ftie;
	337	if (os_memcmp(tdls->inonce, f->snonce, WPA_NONCE_LEN) != 0) {
	338	add_note(wt, MSG_INFO, "Mismatch in TDLS initiator "
	339	"nonce");
	340	}
	341	os_memcpy(tdls->rnonce, f->anonce, WPA_NONCE_LEN);
	342	}
	343
b39f5834 JM	344	if (tdls_derive_tpk(tdls, bssid, elems.ftie, elems.ftie_len) < 1)
b39f5834 JM	345	return;
e4d99217	346	if (tdls_verify_mic(wt, tdls, 2, &elems) == 0) {
29ec7457	347	tdls->dialog_token = data[2];
e4d99217 JM	348	add_note(wt, MSG_DEBUG, "TDLS: Dialog Token for the link: %u",
e4d99217 JM	349	tdls->dialog_token);
29ec7457	350	}
b39f5834 JM	351	}
	352
	353
4e062e35 JM	354	static void rx_data_tdls_setup_confirm_failure(struct wlantest *wt,
4e062e35 JM	355	const u8 *bssid,
6ca4da65	356	const u8 *src,
4e062e35 JM	357	u8 dialog_token, u16 status)
	358	{
	359	struct wlantest_bss *bss;
	360	struct wlantest_tdls *tdls;
	361	struct wlantest_sta *sta;
	362
	363	if (status == WLAN_STATUS_SUCCESS) {
e4d99217 JM	364	add_note(wt, MSG_INFO, "TDLS: Invalid TDLS Setup Confirm from "
e4d99217 JM	365	MACSTR, MAC2STR(src));
4e062e35 JM	366	return;
	367	}
	368
	369	bss = bss_find(wt, bssid);
	370	if (!bss)
	371	return;
6ca4da65	372	sta = sta_find(bss, src);
4e062e35 JM	373	if (!sta)
	374	return;
	375
	376	dl_list_for_each(tdls, &bss->tdls, struct wlantest_tdls, list) {
	377	if (tdls->init == sta) {
	378	if (dialog_token != tdls->dialog_token) {
e4d99217 JM	379	add_note(wt, MSG_DEBUG, "TDLS: Dialog token "
	380	"mismatch in TDLS Setup Confirm "
	381	"(failure)");
4e062e35 JM	382	break;
4e062e35 JM	383	}
e4d99217 JM	384	add_note(wt, MSG_DEBUG, "TDLS: Found matching TDLS "
e4d99217 JM	385	"setup session based on dialog token");
4e062e35 JM	386	tdls->counters[
	387	WLANTEST_TDLS_COUNTER_SETUP_CONF_FAIL]++;
	388	break;
	389	}
	390	}
	391	}
	392
	393
b39f5834 JM	394	static void rx_data_tdls_setup_confirm(struct wlantest wt, const u8 bssid,
	395	const u8 sta_addr, const u8 dst,
	396	const u8 *src,
	397	const u8 *data, size_t len)
	398	{
	399	u16 status;
	400	struct ieee802_11_elems elems;
	401	struct wlantest_tdls *tdls;
0d2e395d	402	u8 link_id[3 * ETH_ALEN];
b39f5834	403
44a04866	404	if (len < 3) {
e4d99217 JM	405	add_note(wt, MSG_INFO, "Too short TDLS Setup Confirm " MACSTR
e4d99217 JM	406	" -> " MACSTR, MAC2STR(src), MAC2STR(dst));
b39f5834	407	return;
44a04866	408	}
b39f5834 JM	409	status = WPA_GET_LE16(data);
	410	wpa_printf(MSG_DEBUG, "TDLS Setup Confirm " MACSTR " -> "
	411	MACSTR " (status %d)",
	412	MAC2STR(src), MAC2STR(dst), status);
b39f5834 JM	413
b39f5834 JM	414	if (ieee802_11_parse_elems(data + 3, len - 3, &elems, 1) ==
4e062e35 JM	415	ParseFailed \|\| elems.link_id == NULL) {
4e062e35 JM	416	/* Need to match TDLS link based on Dialog Token */
6ca4da65	417	rx_data_tdls_setup_confirm_failure(wt, bssid, src,
4e062e35	418	data[2], status);
b39f5834	419	return;
4e062e35	420	}
b39f5834 JM	421	wpa_printf(MSG_DEBUG, "TDLS Link Identifier: BSSID " MACSTR
	422	" initiator STA " MACSTR " responder STA " MACSTR,
	423	MAC2STR(elems.link_id), MAC2STR(elems.link_id + ETH_ALEN),
	424	MAC2STR(elems.link_id + 2 * ETH_ALEN));
	425
44a04866	426	tdls = get_tdls(wt, elems.link_id, 1, bssid);
4d00fe48 JM	427	if (tdls == NULL)
4d00fe48 JM	428	return;
eb4923fd JM	429	if (status)
	430	tdls->counters[WLANTEST_TDLS_COUNTER_SETUP_CONF_FAIL]++;
	431	else
	432	tdls->counters[WLANTEST_TDLS_COUNTER_SETUP_CONF_OK]++;
	433
	434	if (status != WLAN_STATUS_SUCCESS)
	435	return;
4d00fe48	436
4ac800db JM	437	if (elems.ftie && elems.ftie_len >= sizeof(struct rsn_ftie)) {
	438	const struct rsn_ftie *f;
	439	f = (const struct rsn_ftie *) elems.ftie;
	440	if (os_memcmp(tdls->inonce, f->snonce, WPA_NONCE_LEN) != 0) {
	441	add_note(wt, MSG_INFO, "Mismatch in TDLS initiator "
	442	"nonce");
	443	}
	444	if (os_memcmp(tdls->rnonce, f->anonce, WPA_NONCE_LEN) != 0) {
	445	add_note(wt, MSG_INFO, "Mismatch in TDLS responder "
	446	"nonce");
	447	}
	448	}
	449
4d00fe48	450	tdls->link_up = 1;
0d2e395d JM	451	if (tdls_derive_tpk(tdls, bssid, elems.ftie, elems.ftie_len) < 1) {
	452	if (elems.ftie == NULL)
	453	goto remove_reverse;
b39f5834	454	return;
0d2e395d	455	}
e4d99217	456	if (tdls_verify_mic(wt, tdls, 3, &elems) == 0) {
29ec7457	457	tdls->dialog_token = data[2];
4ac800db	458	add_note(wt, MSG_DEBUG, "TDLS: Link up - Dialog Token: %u",
e4d99217	459	tdls->dialog_token);
29ec7457	460	}
0d2e395d JM	461
	462	remove_reverse:
	463	/*
	464	* The TDLS link itself is bidirectional, but there is explicit
	465	* initiator/responder roles. Remove the other direction of the link
	466	* (if it exists) to make sure that the link counters are stored for
	467	* the current TDLS entery.
	468	*/
	469	os_memcpy(link_id, elems.link_id, ETH_ALEN);
	470	os_memcpy(link_id + ETH_ALEN, elems.link_id + 2 * ETH_ALEN, ETH_ALEN);
	471	os_memcpy(link_id + 2 * ETH_ALEN, elems.link_id + ETH_ALEN, ETH_ALEN);
44a04866	472	tdls = get_tdls(wt, link_id, 0, bssid);
0d2e395d	473	if (tdls) {
e4d99217	474	add_note(wt, MSG_DEBUG, "TDLS: Remove reverse link entry");
0d2e395d JM	475	tdls_deinit(tdls);
0d2e395d JM	476	}
29ec7457 JM	477	}
	478
	479
e4d99217 JM	480	static int tdls_verify_mic_teardown(struct wlantest *wt,
e4d99217 JM	481	struct wlantest_tdls *tdls, u8 trans_seq,
29ec7457 JM	482	const u8 *reason_code,
	483	struct ieee802_11_elems *elems)
	484	{
	485	u8 buf, pos;
	486	int len;
	487	u8 mic[16];
	488	int ret;
	489	const struct rsn_ftie *rx_ftie;
	490	struct rsn_ftie *tmp_ftie;
	491
d4c4ec92 JM	492	if (elems->link_id == NULL \|\| elems->ftie == NULL \|\|
d4c4ec92 JM	493	elems->ftie_len < sizeof(struct rsn_ftie))
29ec7457 JM	494	return -1;
	495
	496	len = 2 + 18 + 2 + 1 + 1 + 2 + elems->ftie_len;
	497
	498	buf = os_zalloc(len);
	499	if (buf == NULL)
	500	return -1;
	501
	502	pos = buf;
	503	/* 1) Link Identifier IE */
	504	os_memcpy(pos, elems->link_id - 2, 2 + 18);
	505	pos += 2 + 18;
	506	/* 2) Reason Code */
	507	os_memcpy(pos, reason_code, 2);
	508	pos += 2;
	509	/* 3) Dialog token */
	510	*pos++ = tdls->dialog_token;
	511	/* 4) Transaction Sequence number */
	512	*pos++ = trans_seq;
	513	/* 5) FTIE, with the MIC field of the FTIE set to 0 */
	514	os_memcpy(pos, elems->ftie - 2, 2 + elems->ftie_len);
	515	pos += 2;
	516	tmp_ftie = (struct rsn_ftie *) pos;
	517	os_memset(tmp_ftie->mic, 0, 16);
	518	pos += elems->ftie_len;
	519
	520	wpa_hexdump(MSG_DEBUG, "TDLS: Data for FTIE MIC", buf, pos - buf);
	521	wpa_hexdump_key(MSG_DEBUG, "TDLS: KCK", tdls->tpk.kck, 16);
	522	ret = omac1_aes_128(tdls->tpk.kck, buf, pos - buf, mic);
	523	os_free(buf);
	524	if (ret)
	525	return -1;
	526	wpa_hexdump(MSG_DEBUG, "TDLS: FTIE MIC", mic, 16);
	527	rx_ftie = (const struct rsn_ftie *) elems->ftie;
	528
	529	if (os_memcmp(mic, rx_ftie->mic, 16) == 0) {
e4d99217	530	add_note(wt, MSG_DEBUG, "TDLS: Valid MIC");
29ec7457 JM	531	return 0;
29ec7457 JM	532	}
e4d99217	533	add_note(wt, MSG_DEBUG, "TDLS: Invalid MIC");
29ec7457	534	return -1;
b39f5834 JM	535	}
	536
	537
	538	static void rx_data_tdls_teardown(struct wlantest wt, const u8 bssid,
	539	const u8 sta_addr, const u8 dst,
	540	const u8 *src,
	541	const u8 *data, size_t len)
	542	{
	543	u16 reason;
	544	struct ieee802_11_elems elems;
	545	struct wlantest_tdls *tdls;
	546
	547	if (len < 2)
	548	return;
	549	reason = WPA_GET_LE16(data);
	550	wpa_printf(MSG_DEBUG, "TDLS Teardown " MACSTR " -> "
	551	MACSTR " (reason %d)",
	552	MAC2STR(src), MAC2STR(dst), reason);
	553
	554	if (ieee802_11_parse_elems(data + 2, len - 2, &elems, 1) ==
	555	ParseFailed \|\| elems.link_id == NULL)
	556	return;
	557	wpa_printf(MSG_DEBUG, "TDLS Link Identifier: BSSID " MACSTR
	558	" initiator STA " MACSTR " responder STA " MACSTR,
	559	MAC2STR(elems.link_id), MAC2STR(elems.link_id + ETH_ALEN),
	560	MAC2STR(elems.link_id + 2 * ETH_ALEN));
	561
44a04866	562	tdls = get_tdls(wt, elems.link_id, 1, bssid);
4d00fe48	563	if (tdls) {
4ac800db JM	564	if (tdls->link_up)
4ac800db JM	565	add_note(wt, MSG_DEBUG, "TDLS: Link down");
b39f5834	566	tdls->link_up = 0;
5acf56f6	567	tdls->counters[WLANTEST_TDLS_COUNTER_TEARDOWN]++;
e4d99217	568	tdls_verify_mic_teardown(wt, tdls, 4, data, &elems);
4d00fe48	569	}
b39f5834 JM	570	}
	571
	572
	573	static void rx_data_tdls(struct wlantest wt, const u8 bssid,
	574	const u8 sta_addr, const u8 dst, const u8 *src,
	575	const u8 *data, size_t len)
	576	{
	577	/* data contains the payload of a TDLS Action frame */
	578	if (len < 2 \|\| data[0] != WLAN_ACTION_TDLS) {
	579	wpa_hexdump(MSG_DEBUG, "Unrecognized encapsulated TDLS frame",
	580	data, len);
	581	return;
	582	}
	583
	584	switch (data[1]) {
	585	case WLAN_TDLS_SETUP_REQUEST:
	586	rx_data_tdls_setup_request(wt, bssid, sta_addr, dst, src,
	587	data + 2, len - 2);
	588	break;
	589	case WLAN_TDLS_SETUP_RESPONSE:
	590	rx_data_tdls_setup_response(wt, bssid, sta_addr, dst, src,
	591	data + 2, len - 2);
	592	break;
	593	case WLAN_TDLS_SETUP_CONFIRM:
	594	rx_data_tdls_setup_confirm(wt, bssid, sta_addr, dst, src,
	595	data + 2, len - 2);
	596	break;
	597	case WLAN_TDLS_TEARDOWN:
	598	rx_data_tdls_teardown(wt, bssid, sta_addr, dst, src, data + 2,
	599	len - 2);
	600	break;
	601	case WLAN_TDLS_DISCOVERY_REQUEST:
	602	wpa_printf(MSG_DEBUG, "TDLS Discovery Request " MACSTR " -> "
	603	MACSTR, MAC2STR(src), MAC2STR(dst));
	604	break;
	605	}
	606	}
	607
	608
	609	void rx_data_80211_encap(struct wlantest wt, const u8 bssid,
	610	const u8 sta_addr, const u8 dst, const u8 *src,
	611	const u8 *data, size_t len)
	612	{
	613	wpa_hexdump(MSG_EXCESSIVE, "802.11 data encap frame", data, len);
	614	if (len < 1)
	615	return;
	616	if (data[0] == 0x02)
	617	rx_data_tdls(wt, bssid, sta_addr, dst, src, data + 1, len - 1);
	618	}