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

/*
 * Received Data frame processing for TDLS packets
 * Copyright (c) 2010, Jouni Malinen <j@w1.fi>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * Alternatively, this software may be distributed under the terms of BSD
 * license.
 *
 * See README and COPYING 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)
			wpa_printf(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;

	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_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)
		return -1;

	len = 2 * ETH_ALEN + 1 + 2 + 18 + 2 + elems->rsn_ie_len +
		2 + elems->timeout_int_len + 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 + elems->timeout_int_len);
	pos += 2 + elems->timeout_int_len;
	/* 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) {
		wpa_printf(MSG_DEBUG, "TDLS: Valid MIC");
		return 0;
	}
	wpa_printf(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;

	if (len < 3) {
		wpa_printf(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];
	}
}


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) {
		wpa_printf(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) {
				wpa_printf(MSG_DEBUG, "TDLS: Dialog token "
					   "mismatch in TDLS Setup Response "
					   "(failure)");
				break;
			}
			wpa_printf(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) {
		wpa_printf(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) {
		wpa_printf(MSG_INFO, "Too short TDLS Setup Response " MACSTR
			   " -> " MACSTR, MAC2STR(src), MAC2STR(dst));
		return;
	}

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


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) {
		wpa_printf(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)
		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;

	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(tdls, 3, &elems) == 0) {
		tdls->dialog_token = data[2];
		wpa_printf(MSG_DEBUG, "TDLS: Dialog Token for the link: %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) {
		wpa_printf(MSG_DEBUG, "TDLS: Remove reverse link entry");
		tdls_deinit(tdls);
	}
}


static int tdls_verify_mic_teardown(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)
		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) {
		wpa_printf(MSG_DEBUG, "TDLS: Valid MIC");
		return 0;
	}
	wpa_printf(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) {
		tdls->link_up = 0;
		tdls_verify_mic_teardown(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	*
	5	* This program is free software; you can redistribute it and/or modify
	6	* it under the terms of the GNU General Public License version 2 as
	7	* published by the Free Software Foundation.
	8	*
	9	* Alternatively, this software may be distributed under the terms of BSD
	10	* license.
	11	*
	12	* See README and COPYING for more details.
	13	*/
	14
	15	#include "utils/includes.h"
	16
	17	#include "utils/common.h"
	18	#include "crypto/sha256.h"
	19	#include "crypto/crypto.h"
	20	#include "crypto/aes_wrap.h"
	21	#include "common/ieee802_11_defs.h"
	22	#include "common/ieee802_11_common.h"
	23	#include "wlantest.h"
	24
	25
0d2e395d	26	static struct wlantest_tdls * get_tdls(struct wlantest wt, const u8 linkid,
44a04866	27	int create_new, const u8 *bssid)
b39f5834 JM	28	{
	29	struct wlantest_bss *bss;
	30	struct wlantest_sta init, resp;
	31	struct wlantest_tdls *tdls;
	32
	33	bss = bss_find(wt, linkid);
44a04866 JM	34	if (bss == NULL && bssid) {
	35	bss = bss_find(wt, bssid);
	36	if (bss)
	37	wpa_printf(MSG_INFO, "TDLS: Incorrect BSSID " MACSTR
	38	" in LinkId?! (init=" MACSTR " resp="
	39	MACSTR ")",
	40	MAC2STR(linkid), MAC2STR(linkid + ETH_ALEN),
	41	MAC2STR(linkid + 2 * ETH_ALEN));
	42	}
b39f5834 JM	43	if (bss == NULL)
	44	return NULL;
	45
	46	init = sta_find(bss, linkid + ETH_ALEN);
	47	if (init == NULL)
	48	return NULL;
	49
	50	resp = sta_find(bss, linkid + 2 * ETH_ALEN);
	51	if (resp == NULL)
	52	return NULL;
	53
	54	dl_list_for_each(tdls, &bss->tdls, struct wlantest_tdls, list) {
	55	if (tdls->init == init && tdls->resp == resp)
	56	return tdls;
	57	}
	58
0d2e395d JM	59	if (!create_new)
	60	return NULL;
	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
134	static int tdls_verify_mic(struct wlantest_tdls *tdls, u8 trans_seq,
135	struct ieee802_11_elems *elems)
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 \|\|
145	elems->timeout_int == NULL \|\| elems->ftie == NULL)
146	return -1;
147
148	len = 2 * ETH_ALEN + 1 + 2 + 18 + 2 + elems->rsn_ie_len +
149	2 + elems->timeout_int_len + 2 + elems->ftie_len;
150
151	buf = os_zalloc(len);
152	if (buf == NULL)
153	return -1;
154
155	pos = buf;
156	/* 1) TDLS initiator STA MAC address */
157	os_memcpy(pos, elems->link_id + ETH_ALEN, ETH_ALEN);
158	pos += ETH_ALEN;
159	/* 2) TDLS responder STA MAC address */
160	os_memcpy(pos, elems->link_id + 2 * ETH_ALEN, ETH_ALEN);
161	pos += ETH_ALEN;
162	/* 3) Transaction Sequence number */
163	*pos++ = trans_seq;
164	/* 4) Link Identifier IE */
165	os_memcpy(pos, elems->link_id - 2, 2 + 18);
166	pos += 2 + 18;
167	/* 5) RSN IE */
168	os_memcpy(pos, elems->rsn_ie - 2, 2 + elems->rsn_ie_len);
169	pos += 2 + elems->rsn_ie_len;
170	/* 6) Timeout Interval IE */
171	os_memcpy(pos, elems->timeout_int - 2, 2 + elems->timeout_int_len);
172	pos += 2 + elems->timeout_int_len;
173	/* 7) FTIE, with the MIC field of the FTIE set to 0 */
174	os_memcpy(pos, elems->ftie - 2, 2 + elems->ftie_len);
175	pos += 2;
176	tmp_ftie = (struct rsn_ftie *) pos;
177	os_memset(tmp_ftie->mic, 0, 16);
178	pos += elems->ftie_len;
179
180	wpa_hexdump(MSG_DEBUG, "TDLS: Data for FTIE MIC", buf, pos - buf);
181	wpa_hexdump_key(MSG_DEBUG, "TDLS: KCK", tdls->tpk.kck, 16);
182	ret = omac1_aes_128(tdls->tpk.kck, buf, pos - buf, mic);
183	os_free(buf);
184	if (ret)
185	return -1;
186	wpa_hexdump(MSG_DEBUG, "TDLS: FTIE MIC", mic, 16);
187	rx_ftie = (const struct rsn_ftie *) elems->ftie;
188
189	if (os_memcmp(mic, rx_ftie->mic, 16) == 0) {
190	wpa_printf(MSG_DEBUG, "TDLS: Valid MIC");
191	return 0;
192	}
193	wpa_printf(MSG_DEBUG, "TDLS: Invalid MIC");
194	return -1;
195	}
196
197
198	static void rx_data_tdls_setup_request(struct wlantest wt, const u8 bssid,
199	const u8 sta_addr, const u8 dst,
200	const u8 *src,
201	const u8 *data, size_t len)
202	{
203	struct ieee802_11_elems elems;
eb4923fd	204	struct wlantest_tdls *tdls;
b39f5834	205
44a04866 JM	206	if (len < 3) {
	207	wpa_printf(MSG_INFO, "Too short TDLS Setup Request " MACSTR
	208	" -> " MACSTR, MAC2STR(src), MAC2STR(dst));
b39f5834	209	return;
44a04866	210	}
b39f5834 JM	211	wpa_printf(MSG_DEBUG, "TDLS Setup Request " MACSTR " -> "
	212	MACSTR, MAC2STR(src), MAC2STR(dst));
	213
	214	if (ieee802_11_parse_elems(data + 3, len - 3, &elems, 1) ==
	215	ParseFailed \|\| elems.link_id == NULL)
	216	return;
	217	wpa_printf(MSG_DEBUG, "TDLS Link Identifier: BSSID " MACSTR
	218	" initiator STA " MACSTR " responder STA " MACSTR,
	219	MAC2STR(elems.link_id), MAC2STR(elems.link_id + ETH_ALEN),
	220	MAC2STR(elems.link_id + 2 * ETH_ALEN));
44a04866 JM	221	tdls = get_tdls(wt, elems.link_id, 1, bssid);
44a04866 JM	222	if (tdls) {
eb4923fd	223	tdls->counters[WLANTEST_TDLS_COUNTER_SETUP_REQ]++;
44a04866 JM	224	tdls->dialog_token = data[0];
	225	}
	226	}
	227
	228
	229	static void rx_data_tdls_setup_response_failure(struct wlantest *wt,
	230	const u8 *bssid,
	231	const u8 *sta_addr,
	232	u8 dialog_token, u16 status)
	233	{
	234	struct wlantest_bss *bss;
	235	struct wlantest_tdls *tdls;
	236	struct wlantest_sta *sta;
	237
	238	if (status == WLAN_STATUS_SUCCESS) {
	239	wpa_printf(MSG_INFO, "TDLS: Invalid TDLS Setup Response from "
	240	MACSTR, MAC2STR(sta_addr));
	241	return;
	242	}
	243
	244	bss = bss_find(wt, bssid);
	245	if (!bss)
	246	return;
	247	sta = sta_find(bss, sta_addr);
	248	if (!sta)
	249	return;
	250
	251	dl_list_for_each(tdls, &bss->tdls, struct wlantest_tdls, list) {
	252	if (tdls->resp == sta) {
	253	if (dialog_token != tdls->dialog_token) {
	254	wpa_printf(MSG_DEBUG, "TDLS: Dialog token "
	255	"mismatch in TDLS Setup Response "
	256	"(failure)");
	257	break;
	258	}
	259	wpa_printf(MSG_DEBUG, "TDLS: Found matching TDLS "
	260	"setup session based on dialog token");
	261	tdls->counters[
	262	WLANTEST_TDLS_COUNTER_SETUP_RESP_FAIL]++;
	263	break;
	264	}
	265	}
b39f5834 JM	266	}
	267
	268
	269	static void rx_data_tdls_setup_response(struct wlantest wt, const u8 bssid,
	270	const u8 sta_addr, const u8 dst,
	271	const u8 *src,
	272	const u8 *data, size_t len)
	273	{
	274	u16 status;
	275	struct ieee802_11_elems elems;
	276	struct wlantest_tdls *tdls;
	277
44a04866 JM	278	if (len < 3) {
	279	wpa_printf(MSG_INFO, "Too short TDLS Setup Response " MACSTR
	280	" -> " MACSTR, MAC2STR(src), MAC2STR(dst));
b39f5834	281	return;
44a04866	282	}
b39f5834 JM	283	status = WPA_GET_LE16(data);
	284	wpa_printf(MSG_DEBUG, "TDLS Setup Response " MACSTR " -> "
	285	MACSTR " (status %d)",
	286	MAC2STR(src), MAC2STR(dst), status);
44a04866 JM	287	if (len < 5) {
	288	wpa_printf(MSG_INFO, "Too short TDLS Setup Response " MACSTR
	289	" -> " MACSTR, MAC2STR(src), MAC2STR(dst));
b39f5834	290	return;
44a04866	291	}
b39f5834 JM	292
b39f5834 JM	293	if (ieee802_11_parse_elems(data + 5, len - 5, &elems, 1) ==
44a04866 JM	294	ParseFailed \|\| elems.link_id == NULL) {
	295	/* Need to match TDLS link based on Dialog Token */
	296	rx_data_tdls_setup_response_failure(wt, bssid, sta_addr,
	297	data[2], status);
b39f5834	298	return;
44a04866	299	}
b39f5834 JM	300	wpa_printf(MSG_DEBUG, "TDLS Link Identifier: BSSID " MACSTR
	301	" initiator STA " MACSTR " responder STA " MACSTR,
	302	MAC2STR(elems.link_id), MAC2STR(elems.link_id + ETH_ALEN),
	303	MAC2STR(elems.link_id + 2 * ETH_ALEN));
	304
44a04866	305	tdls = get_tdls(wt, elems.link_id, 1, bssid);
b39f5834 JM	306	if (!tdls)
b39f5834 JM	307	return;
eb4923fd JM	308	if (status)
	309	tdls->counters[WLANTEST_TDLS_COUNTER_SETUP_RESP_FAIL]++;
	310	else
	311	tdls->counters[WLANTEST_TDLS_COUNTER_SETUP_RESP_OK]++;
44a04866 JM	312
	313	if (status != WLAN_STATUS_SUCCESS)
	314	return;
	315
b39f5834 JM	316	if (tdls_derive_tpk(tdls, bssid, elems.ftie, elems.ftie_len) < 1)
b39f5834 JM	317	return;
29ec7457 JM	318	if (tdls_verify_mic(tdls, 2, &elems) == 0) {
	319	tdls->dialog_token = data[2];
	320	wpa_printf(MSG_DEBUG, "TDLS: Dialog Token for the link: %u",
	321	tdls->dialog_token);
	322	}
b39f5834 JM	323	}
	324
	325
	326	static void rx_data_tdls_setup_confirm(struct wlantest wt, const u8 bssid,
	327	const u8 sta_addr, const u8 dst,
	328	const u8 *src,
	329	const u8 *data, size_t len)
	330	{
	331	u16 status;
	332	struct ieee802_11_elems elems;
	333	struct wlantest_tdls *tdls;
0d2e395d	334	u8 link_id[3 * ETH_ALEN];
b39f5834	335
44a04866 JM	336	if (len < 3) {
	337	wpa_printf(MSG_INFO, "Too short TDLS Setup Confirm " MACSTR
	338	" -> " MACSTR, MAC2STR(src), MAC2STR(dst));
b39f5834	339	return;
44a04866	340	}
b39f5834 JM	341	status = WPA_GET_LE16(data);
	342	wpa_printf(MSG_DEBUG, "TDLS Setup Confirm " MACSTR " -> "
	343	MACSTR " (status %d)",
	344	MAC2STR(src), MAC2STR(dst), status);
b39f5834 JM	345
	346	if (ieee802_11_parse_elems(data + 3, len - 3, &elems, 1) ==
	347	ParseFailed \|\| elems.link_id == NULL)
	348	return;
	349	wpa_printf(MSG_DEBUG, "TDLS Link Identifier: BSSID " MACSTR
	350	" initiator STA " MACSTR " responder STA " MACSTR,
	351	MAC2STR(elems.link_id), MAC2STR(elems.link_id + ETH_ALEN),
	352	MAC2STR(elems.link_id + 2 * ETH_ALEN));
	353
44a04866	354	tdls = get_tdls(wt, elems.link_id, 1, bssid);
4d00fe48 JM	355	if (tdls == NULL)
4d00fe48 JM	356	return;
eb4923fd JM	357	if (status)
	358	tdls->counters[WLANTEST_TDLS_COUNTER_SETUP_CONF_FAIL]++;
	359	else
	360	tdls->counters[WLANTEST_TDLS_COUNTER_SETUP_CONF_OK]++;
	361
	362	if (status != WLAN_STATUS_SUCCESS)
	363	return;
4d00fe48 JM	364
4d00fe48 JM	365	tdls->link_up = 1;
0d2e395d JM	366	if (tdls_derive_tpk(tdls, bssid, elems.ftie, elems.ftie_len) < 1) {
	367	if (elems.ftie == NULL)
	368	goto remove_reverse;
b39f5834	369	return;
0d2e395d	370	}
29ec7457 JM	371	if (tdls_verify_mic(tdls, 3, &elems) == 0) {
	372	tdls->dialog_token = data[2];
	373	wpa_printf(MSG_DEBUG, "TDLS: Dialog Token for the link: %u",
	374	tdls->dialog_token);
	375	}
0d2e395d JM	376
	377	remove_reverse:
	378	/*
	379	* The TDLS link itself is bidirectional, but there is explicit
	380	* initiator/responder roles. Remove the other direction of the link
	381	* (if it exists) to make sure that the link counters are stored for
	382	* the current TDLS entery.
	383	*/
	384	os_memcpy(link_id, elems.link_id, ETH_ALEN);
	385	os_memcpy(link_id + ETH_ALEN, elems.link_id + 2 * ETH_ALEN, ETH_ALEN);
	386	os_memcpy(link_id + 2 * ETH_ALEN, elems.link_id + ETH_ALEN, ETH_ALEN);
44a04866	387	tdls = get_tdls(wt, link_id, 0, bssid);
0d2e395d JM	388	if (tdls) {
	389	wpa_printf(MSG_DEBUG, "TDLS: Remove reverse link entry");
	390	tdls_deinit(tdls);
	391	}
29ec7457 JM	392	}
	393
	394
	395	static int tdls_verify_mic_teardown(struct wlantest_tdls *tdls, u8 trans_seq,
	396	const u8 *reason_code,
	397	struct ieee802_11_elems *elems)
	398	{
	399	u8 buf, pos;
	400	int len;
	401	u8 mic[16];
	402	int ret;
	403	const struct rsn_ftie *rx_ftie;
	404	struct rsn_ftie *tmp_ftie;
	405
	406	if (elems->link_id == NULL \|\| elems->ftie == NULL)
	407	return -1;
	408
	409	len = 2 + 18 + 2 + 1 + 1 + 2 + elems->ftie_len;
	410
	411	buf = os_zalloc(len);
	412	if (buf == NULL)
	413	return -1;
	414
	415	pos = buf;
	416	/* 1) Link Identifier IE */
	417	os_memcpy(pos, elems->link_id - 2, 2 + 18);
	418	pos += 2 + 18;
	419	/* 2) Reason Code */
	420	os_memcpy(pos, reason_code, 2);
	421	pos += 2;
	422	/* 3) Dialog token */
	423	*pos++ = tdls->dialog_token;
	424	/* 4) Transaction Sequence number */
	425	*pos++ = trans_seq;
	426	/* 5) FTIE, with the MIC field of the FTIE set to 0 */
	427	os_memcpy(pos, elems->ftie - 2, 2 + elems->ftie_len);
	428	pos += 2;
	429	tmp_ftie = (struct rsn_ftie *) pos;
	430	os_memset(tmp_ftie->mic, 0, 16);
	431	pos += elems->ftie_len;
	432
	433	wpa_hexdump(MSG_DEBUG, "TDLS: Data for FTIE MIC", buf, pos - buf);
	434	wpa_hexdump_key(MSG_DEBUG, "TDLS: KCK", tdls->tpk.kck, 16);
	435	ret = omac1_aes_128(tdls->tpk.kck, buf, pos - buf, mic);
	436	os_free(buf);
	437	if (ret)
	438	return -1;
	439	wpa_hexdump(MSG_DEBUG, "TDLS: FTIE MIC", mic, 16);
	440	rx_ftie = (const struct rsn_ftie *) elems->ftie;
	441
	442	if (os_memcmp(mic, rx_ftie->mic, 16) == 0) {
	443	wpa_printf(MSG_DEBUG, "TDLS: Valid MIC");
	444	return 0;
	445	}
	446	wpa_printf(MSG_DEBUG, "TDLS: Invalid MIC");
	447	return -1;
b39f5834 JM	448	}
	449
	450
	451	static void rx_data_tdls_teardown(struct wlantest wt, const u8 bssid,
	452	const u8 sta_addr, const u8 dst,
	453	const u8 *src,
	454	const u8 *data, size_t len)
	455	{
	456	u16 reason;
	457	struct ieee802_11_elems elems;
	458	struct wlantest_tdls *tdls;
	459
	460	if (len < 2)
	461	return;
	462	reason = WPA_GET_LE16(data);
	463	wpa_printf(MSG_DEBUG, "TDLS Teardown " MACSTR " -> "
	464	MACSTR " (reason %d)",
	465	MAC2STR(src), MAC2STR(dst), reason);
	466
	467	if (ieee802_11_parse_elems(data + 2, len - 2, &elems, 1) ==
	468	ParseFailed \|\| elems.link_id == NULL)
	469	return;
	470	wpa_printf(MSG_DEBUG, "TDLS Link Identifier: BSSID " MACSTR
	471	" initiator STA " MACSTR " responder STA " MACSTR,
	472	MAC2STR(elems.link_id), MAC2STR(elems.link_id + ETH_ALEN),
	473	MAC2STR(elems.link_id + 2 * ETH_ALEN));
	474
44a04866	475	tdls = get_tdls(wt, elems.link_id, 1, bssid);
4d00fe48	476	if (tdls) {
b39f5834	477	tdls->link_up = 0;
4d00fe48 JM	478	tdls_verify_mic_teardown(tdls, 4, data, &elems);
4d00fe48 JM	479	}
b39f5834 JM	480	}
	481
	482
	483	static void rx_data_tdls(struct wlantest wt, const u8 bssid,
	484	const u8 sta_addr, const u8 dst, const u8 *src,
	485	const u8 *data, size_t len)
	486	{
	487	/* data contains the payload of a TDLS Action frame */
	488	if (len < 2 \|\| data[0] != WLAN_ACTION_TDLS) {
	489	wpa_hexdump(MSG_DEBUG, "Unrecognized encapsulated TDLS frame",
	490	data, len);
	491	return;
	492	}
	493
	494	switch (data[1]) {
	495	case WLAN_TDLS_SETUP_REQUEST:
	496	rx_data_tdls_setup_request(wt, bssid, sta_addr, dst, src,
	497	data + 2, len - 2);
	498	break;
	499	case WLAN_TDLS_SETUP_RESPONSE:
	500	rx_data_tdls_setup_response(wt, bssid, sta_addr, dst, src,
	501	data + 2, len - 2);
	502	break;
	503	case WLAN_TDLS_SETUP_CONFIRM:
	504	rx_data_tdls_setup_confirm(wt, bssid, sta_addr, dst, src,
	505	data + 2, len - 2);
	506	break;
	507	case WLAN_TDLS_TEARDOWN:
	508	rx_data_tdls_teardown(wt, bssid, sta_addr, dst, src, data + 2,
	509	len - 2);
	510	break;
	511	case WLAN_TDLS_DISCOVERY_REQUEST:
	512	wpa_printf(MSG_DEBUG, "TDLS Discovery Request " MACSTR " -> "
	513	MACSTR, MAC2STR(src), MAC2STR(dst));
	514	break;
	515	}
	516	}
	517
	518
	519	void rx_data_80211_encap(struct wlantest wt, const u8 bssid,
	520	const u8 sta_addr, const u8 dst, const u8 *src,
	521	const u8 *data, size_t len)
	522	{
	523	wpa_hexdump(MSG_EXCESSIVE, "802.11 data encap frame", data, len);
	524	if (len < 1)
	525	return;
	526	if (data[0] == 0x02)
	527	rx_data_tdls(wt, bssid, sta_addr, dst, src, data + 1, len - 1);
	528	}