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

/*
 * wlantest frame injection
 * 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 "common/defs.h"
#include "common/ieee802_11_defs.h"
#include "crypto/aes_wrap.h"
#include "wlantest.h"


static int inject_frame(int s, const void *data, size_t len)
{
#define	IEEE80211_RADIOTAP_F_FRAG	0x08
	unsigned char rtap_hdr[] = {
		0x00, 0x00, /* radiotap version */
		0x0e, 0x00, /* radiotap length */
		0x02, 0xc0, 0x00, 0x00, /* bmap: flags, tx and rx flags */
		IEEE80211_RADIOTAP_F_FRAG, /* F_FRAG (fragment if required) */
		0x00,       /* padding */
		0x00, 0x00, /* RX and TX flags to indicate that */
		0x00, 0x00, /* this is the injected frame directly */
	};
	struct iovec iov[2] = {
		{
			.iov_base = &rtap_hdr,
			.iov_len = sizeof(rtap_hdr),
		},
		{
			.iov_base = (void *) data,
			.iov_len = len,
		}
	};
	struct msghdr msg = {
		.msg_name = NULL,
		.msg_namelen = 0,
		.msg_iov = iov,
		.msg_iovlen = 2,
		.msg_control = NULL,
		.msg_controllen = 0,
		.msg_flags = 0,
	};
	int ret;

	ret = sendmsg(s, &msg, 0);
	if (ret < 0)
		perror("sendmsg");
	return ret;
}


static int is_robust_mgmt(u8 *frame, size_t len)
{
	struct ieee80211_mgmt *mgmt;
	u16 fc, stype;
	if (len < 24)
		return 0;
	mgmt = (struct ieee80211_mgmt *) frame;
	fc = le_to_host16(mgmt->frame_control);
	if (WLAN_FC_GET_TYPE(fc) != WLAN_FC_TYPE_MGMT)
		return 0;
	stype = WLAN_FC_GET_STYPE(fc);
	if (stype == WLAN_FC_STYPE_DEAUTH || stype == WLAN_FC_STYPE_DISASSOC)
		return 1;
	if (stype == WLAN_FC_STYPE_ACTION) {
		if (len < 25)
			return 0;
		if (mgmt->u.action.category != WLAN_ACTION_PUBLIC)
			return 1;
	}
	return 0;
}


static int wlantest_inject_bip(struct wlantest *wt, struct wlantest_bss *bss,
			       u8 *frame, size_t len, int incorrect_key)
{
	u8 *prot, *pos, *buf;
	u8 mic[16];
	u8 dummy[16];
	int ret;
	u16 fc;
	struct ieee80211_hdr *hdr;
	size_t plen;

	if (!bss->igtk_set[bss->igtk_idx])
		return -1;

	plen = len + 18;
	prot = os_malloc(plen);
	if (prot == NULL)
		return -1;
	os_memcpy(prot, frame, len);
	pos = prot + len;
	*pos++ = WLAN_EID_MMIE;
	*pos++ = 16;
	WPA_PUT_LE16(pos, bss->igtk_idx);
	pos += 2;
	inc_byte_array(bss->ipn[bss->igtk_idx], 6);
	os_memcpy(pos, bss->ipn[bss->igtk_idx], 6);
	pos += 6;
	os_memset(pos, 0, 8); /* MIC */

	buf = os_malloc(plen + 20 - 24);
	if (buf == NULL) {
		os_free(prot);
		return -1;
	}

	/* BIP AAD: FC(masked) A1 A2 A3 */
	hdr = (struct ieee80211_hdr *) frame;
	fc = le_to_host16(hdr->frame_control);
	fc &= ~(WLAN_FC_RETRY | WLAN_FC_PWRMGT | WLAN_FC_MOREDATA);
	WPA_PUT_LE16(buf, fc);
	os_memcpy(buf + 2, hdr->addr1, 3 * ETH_ALEN);
	os_memcpy(buf + 20, prot + 24, plen - 24);
	wpa_hexdump(MSG_MSGDUMP, "BIP: AAD|Body(masked)", buf, plen + 20 - 24);
	/* MIC = L(AES-128-CMAC(AAD || Frame Body(masked)), 0, 64) */
	os_memset(dummy, 0x11, sizeof(dummy));
	if (omac1_aes_128(incorrect_key ? dummy : bss->igtk[bss->igtk_idx],
			  buf, plen + 20 - 24, mic) < 0) {
		os_free(prot);
		os_free(buf);
		return -1;
	}
	os_free(buf);

	os_memcpy(pos, mic, 8);
	wpa_hexdump(MSG_DEBUG, "BIP MMIE MIC", pos, 8);

	ret = inject_frame(wt->monitor_sock, prot, plen);
	os_free(prot);

	return (ret < 0) ? -1 : 0;
}


static int wlantest_inject_prot_bc(struct wlantest *wt,
				   struct wlantest_bss *bss,
				   u8 *frame, size_t len, int incorrect_key)
{
	u8 *crypt;
	size_t crypt_len;
	int ret;
	u8 dummy[64];
	u8 *pn;
	struct ieee80211_hdr *hdr;
	u16 fc;
	int hdrlen;

	hdr = (struct ieee80211_hdr *) frame;
	hdrlen = 24;
	fc = le_to_host16(hdr->frame_control);

	if (!bss->gtk_len[bss->gtk_idx])
		return -1;

	if ((fc & (WLAN_FC_TODS | WLAN_FC_FROMDS)) ==
	    (WLAN_FC_TODS | WLAN_FC_FROMDS))
		hdrlen += ETH_ALEN;
	pn = bss->rsc[bss->gtk_idx];
	inc_byte_array(pn, 6);

	os_memset(dummy, 0x11, sizeof(dummy));
	if (bss->group_cipher == WPA_CIPHER_TKIP)
		crypt = tkip_encrypt(incorrect_key ? dummy :
				     bss->gtk[bss->gtk_idx],
				     frame, len, hdrlen, NULL, pn,
				     bss->gtk_idx, &crypt_len);
	else
		crypt = ccmp_encrypt(incorrect_key ? dummy :
				     bss->gtk[bss->gtk_idx],
				     frame, len, hdrlen, NULL, pn,
				     bss->gtk_idx, &crypt_len);

	if (crypt == NULL)
		return -1;

	ret = inject_frame(wt->monitor_sock, crypt, crypt_len);
	os_free(crypt);

	return (ret < 0) ? -1 : 0;
}


static int wlantest_inject_prot(struct wlantest *wt, struct wlantest_bss *bss,
				struct wlantest_sta *sta, u8 *frame,
				size_t len, int incorrect_key)
{
	u8 *crypt;
	size_t crypt_len;
	int ret;
	u8 dummy[64];
	u8 *pn;
	struct ieee80211_hdr *hdr;
	u16 fc;
	int tid = 0;
	u8 *qos = NULL;
	int hdrlen;
	struct wlantest_tdls *tdls = NULL;
	const u8 *tk = NULL;

	hdr = (struct ieee80211_hdr *) frame;
	hdrlen = 24;
	fc = le_to_host16(hdr->frame_control);

	if (WLAN_FC_GET_TYPE(fc) == WLAN_FC_TYPE_DATA &&
	    (fc & (WLAN_FC_TODS | WLAN_FC_FROMDS)) == 0) {
		struct wlantest_sta *sta2;
		bss = bss_get(wt, hdr->addr3);
		if (bss == NULL) {
			wpa_printf(MSG_DEBUG, "No BSS found for TDLS "
				   "injection");
			return -1;
		}
		sta = sta_find(bss, hdr->addr2);
		sta2 = sta_find(bss, hdr->addr1);
		if (sta == NULL || sta2 == NULL) {
			wpa_printf(MSG_DEBUG, "No stations found for TDLS "
				   "injection");
			return -1;
		}
		dl_list_for_each(tdls, &bss->tdls, struct wlantest_tdls, list)
		{
			if ((tdls->init == sta && tdls->resp == sta2) ||
			    (tdls->init == sta2 && tdls->resp == sta)) {
				if (!tdls->link_up)
					wpa_printf(MSG_DEBUG, "TDLS: Link not "
						   "up, but injecting Data "
						   "frame on direct link");
				tk = tdls->tpk.tk;
				break;
			}
		}
	}

	if (tk == NULL && sta == NULL) {
		if (WLAN_FC_GET_TYPE(fc) == WLAN_FC_TYPE_MGMT)
			return wlantest_inject_bip(wt, bss, frame, len,
						   incorrect_key);
		return wlantest_inject_prot_bc(wt, bss, frame, len,
					       incorrect_key);
	}

	if (tk == NULL && !sta->ptk_set) {
		wpa_printf(MSG_DEBUG, "No key known for injection");
		return -1;
	}

	if (WLAN_FC_GET_TYPE(fc) == WLAN_FC_TYPE_MGMT)
		tid = 16;
	else if (WLAN_FC_GET_TYPE(fc) == WLAN_FC_TYPE_DATA) {
		if ((fc & (WLAN_FC_TODS | WLAN_FC_FROMDS)) ==
		    (WLAN_FC_TODS | WLAN_FC_FROMDS))
			hdrlen += ETH_ALEN;
		if (WLAN_FC_GET_STYPE(fc) & 0x08) {
			qos = frame + hdrlen;
			hdrlen += 2;
			tid = qos[0] & 0x0f;
		}
	}
	if (tk) {
		if (os_memcmp(hdr->addr2, tdls->init->addr, ETH_ALEN) == 0)
			pn = tdls->rsc_init[tid];
		else
			pn = tdls->rsc_resp[tid];
	} else if (os_memcmp(hdr->addr2, bss->bssid, ETH_ALEN) == 0)
		pn = sta->rsc_fromds[tid];
	else
		pn = sta->rsc_tods[tid];
	inc_byte_array(pn, 6);

	os_memset(dummy, 0x11, sizeof(dummy));
	if (tk) 
		crypt = ccmp_encrypt(incorrect_key ? dummy : tk,
				     frame, len, hdrlen, qos, pn, 0,
				     &crypt_len);
	else if (sta->pairwise_cipher == WPA_CIPHER_TKIP)
		crypt = tkip_encrypt(incorrect_key ? dummy : sta->ptk.tk1,
				     frame, len, hdrlen, qos, pn, 0,
				     &crypt_len);
	else
		crypt = ccmp_encrypt(incorrect_key ? dummy : sta->ptk.tk1,
				     frame, len, hdrlen, qos, pn, 0,
				     &crypt_len);

	if (crypt == NULL) {
		wpa_printf(MSG_DEBUG, "Frame encryption failed");
		return -1;
	}

	wpa_hexdump(MSG_DEBUG, "Inject frame (encrypted)", crypt, crypt_len);
	ret = inject_frame(wt->monitor_sock, crypt, crypt_len);
	os_free(crypt);
	wpa_printf(MSG_DEBUG, "inject_frame for protected frame: %d", ret);

	return (ret < 0) ? -1 : 0;
}


int wlantest_inject(struct wlantest *wt, struct wlantest_bss *bss,
		    struct wlantest_sta *sta, u8 *frame, size_t len,
		    enum wlantest_inject_protection prot)
{
	int ret;
	struct ieee80211_hdr *hdr;
	u16 fc;
	int protectable, protect = 0;

	wpa_hexdump(MSG_DEBUG, "Inject frame", frame, len);
	if (wt->monitor_sock < 0) {
		wpa_printf(MSG_INFO, "Cannot inject frames when monitor "
			   "interface is not in use");
		return -1;
	}

	hdr = (struct ieee80211_hdr *) frame;
	fc = le_to_host16(hdr->frame_control);
	protectable = WLAN_FC_GET_TYPE(fc) == WLAN_FC_TYPE_DATA ||
		is_robust_mgmt(frame, len);

	if (prot == WLANTEST_INJECT_PROTECTED ||
	    prot == WLANTEST_INJECT_INCORRECT_KEY) {
		if (!sta &&
		    ((WLAN_FC_GET_TYPE(fc) == WLAN_FC_TYPE_MGMT &&
		      !bss->igtk_set[bss->igtk_idx]) ||
		     (WLAN_FC_GET_TYPE(fc) == WLAN_FC_TYPE_DATA &&
		      !bss->gtk_len[bss->gtk_idx]))) {
			wpa_printf(MSG_INFO, "No GTK/IGTK known for "
				   MACSTR " to protect the injected "
				   "frame", MAC2STR(bss->bssid));
			return -1;
		}
		if (sta && !sta->ptk_set) {
			wpa_printf(MSG_INFO, "No PTK known for the STA " MACSTR
				   " to encrypt the injected frame",
				   MAC2STR(sta->addr));
			return -1;
		}
		protect = 1;
	} else if (protectable && prot != WLANTEST_INJECT_UNPROTECTED) {
		if (sta && sta->ptk_set)
			protect = 1;
		else if (!sta) {
			if (WLAN_FC_GET_TYPE(fc) == WLAN_FC_TYPE_DATA &&
			    bss->gtk_len[bss->gtk_idx])
				protect = 1;
			if (WLAN_FC_GET_TYPE(fc) == WLAN_FC_TYPE_MGMT &&
			    bss->igtk_set[bss->igtk_idx])
				protect = 1;
		}
	}

	if (protect)
		return wlantest_inject_prot(
			wt, bss, sta, frame, len,
			prot == WLANTEST_INJECT_INCORRECT_KEY);

	ret = inject_frame(wt->monitor_sock, frame, len);
	wpa_printf(MSG_DEBUG, "inject_frame for unprotected frame: %d", ret);
	return (ret < 0) ? -1 : 0;
}
Commit	Line	Data
7d23e971 JM	1	/*
	2	* wlantest frame injection
	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"
571ab37b JM	18	#include "common/defs.h"
571ab37b JM	19	#include "common/ieee802_11_defs.h"
2e4c3469	20	#include "crypto/aes_wrap.h"
7d23e971 JM	21	#include "wlantest.h"
	22
	23
	24	static int inject_frame(int s, const void *data, size_t len)
	25	{
	26	#define IEEE80211_RADIOTAP_F_FRAG 0x08
	27	unsigned char rtap_hdr[] = {
	28	0x00, 0x00, /* radiotap version */
	29	0x0e, 0x00, /* radiotap length */
	30	0x02, 0xc0, 0x00, 0x00, /* bmap: flags, tx and rx flags */
	31	IEEE80211_RADIOTAP_F_FRAG, /* F_FRAG (fragment if required) */
	32	0x00, /* padding */
	33	0x00, 0x00, /* RX and TX flags to indicate that */
	34	0x00, 0x00, /* this is the injected frame directly */
	35	};
	36	struct iovec iov[2] = {
	37	{
	38	.iov_base = &rtap_hdr,
	39	.iov_len = sizeof(rtap_hdr),
	40	},
	41	{
	42	.iov_base = (void *) data,
	43	.iov_len = len,
	44	}
	45	};
	46	struct msghdr msg = {
	47	.msg_name = NULL,
	48	.msg_namelen = 0,
	49	.msg_iov = iov,
	50	.msg_iovlen = 2,
	51	.msg_control = NULL,
	52	.msg_controllen = 0,
	53	.msg_flags = 0,
	54	};
	55	int ret;
	56
	57	ret = sendmsg(s, &msg, 0);
	58	if (ret < 0)
	59	perror("sendmsg");
	60	return ret;
	61	}
	62
	63
571ab37b JM	64	static int is_robust_mgmt(u8 *frame, size_t len)
	65	{
	66	struct ieee80211_mgmt *mgmt;
	67	u16 fc, stype;
	68	if (len < 24)
	69	return 0;
	70	mgmt = (struct ieee80211_mgmt *) frame;
	71	fc = le_to_host16(mgmt->frame_control);
	72	if (WLAN_FC_GET_TYPE(fc) != WLAN_FC_TYPE_MGMT)
	73	return 0;
	74	stype = WLAN_FC_GET_STYPE(fc);
	75	if (stype == WLAN_FC_STYPE_DEAUTH \|\| stype == WLAN_FC_STYPE_DISASSOC)
	76	return 1;
	77	if (stype == WLAN_FC_STYPE_ACTION) {
	78	if (len < 25)
	79	return 0;
	80	if (mgmt->u.action.category != WLAN_ACTION_PUBLIC)
	81	return 1;
	82	}
	83	return 0;
	84	}
	85
	86
2e4c3469 JM	87	static int wlantest_inject_bip(struct wlantest wt, struct wlantest_bss bss,
	88	u8 *frame, size_t len, int incorrect_key)
	89	{
	90	u8 prot, pos, *buf;
	91	u8 mic[16];
	92	u8 dummy[16];
	93	int ret;
	94	u16 fc;
	95	struct ieee80211_hdr *hdr;
	96	size_t plen;
	97
	98	if (!bss->igtk_set[bss->igtk_idx])
	99	return -1;
	100
	101	plen = len + 18;
	102	prot = os_malloc(plen);
	103	if (prot == NULL)
	104	return -1;
	105	os_memcpy(prot, frame, len);
	106	pos = prot + len;
	107	*pos++ = WLAN_EID_MMIE;
	108	*pos++ = 16;
	109	WPA_PUT_LE16(pos, bss->igtk_idx);
	110	pos += 2;
	111	inc_byte_array(bss->ipn[bss->igtk_idx], 6);
	112	os_memcpy(pos, bss->ipn[bss->igtk_idx], 6);
	113	pos += 6;
	114	os_memset(pos, 0, 8); /* MIC */
	115
	116	buf = os_malloc(plen + 20 - 24);
	117	if (buf == NULL) {
	118	os_free(prot);
	119	return -1;
	120	}
	121
	122	/* BIP AAD: FC(masked) A1 A2 A3 */
	123	hdr = (struct ieee80211_hdr *) frame;
	124	fc = le_to_host16(hdr->frame_control);
	125	fc &= ~(WLAN_FC_RETRY \| WLAN_FC_PWRMGT \| WLAN_FC_MOREDATA);
	126	WPA_PUT_LE16(buf, fc);
	127	os_memcpy(buf + 2, hdr->addr1, 3 * ETH_ALEN);
	128	os_memcpy(buf + 20, prot + 24, plen - 24);
	129	wpa_hexdump(MSG_MSGDUMP, "BIP: AAD\|Body(masked)", buf, plen + 20 - 24);
	130	/* MIC = L(AES-128-CMAC(AAD \|\| Frame Body(masked)), 0, 64) */
	131	os_memset(dummy, 0x11, sizeof(dummy));
	132	if (omac1_aes_128(incorrect_key ? dummy : bss->igtk[bss->igtk_idx],
	133	buf, plen + 20 - 24, mic) < 0) {
	134	os_free(prot);
	135	os_free(buf);
	136	return -1;
	137	}
	138	os_free(buf);
	139
	140	os_memcpy(pos, mic, 8);
	141	wpa_hexdump(MSG_DEBUG, "BIP MMIE MIC", pos, 8);
	142
	143	ret = inject_frame(wt->monitor_sock, prot, plen);
	144	os_free(prot);
	145
	146	return (ret < 0) ? -1 : 0;
	147	}
	148
	149
	150	static int wlantest_inject_prot_bc(struct wlantest *wt,
151	struct wlantest_bss *bss,
152	u8 *frame, size_t len, int incorrect_key)
153	{
154	u8 *crypt;
155	size_t crypt_len;
156	int ret;
157	u8 dummy[64];
158	u8 *pn;
159	struct ieee80211_hdr *hdr;
160	u16 fc;
161	int hdrlen;
162
163	hdr = (struct ieee80211_hdr *) frame;
164	hdrlen = 24;
165	fc = le_to_host16(hdr->frame_control);
166
167	if (!bss->gtk_len[bss->gtk_idx])
168	return -1;
169
170	if ((fc & (WLAN_FC_TODS \| WLAN_FC_FROMDS)) ==
171	(WLAN_FC_TODS \| WLAN_FC_FROMDS))
172	hdrlen += ETH_ALEN;
173	pn = bss->rsc[bss->gtk_idx];
174	inc_byte_array(pn, 6);
175
176	os_memset(dummy, 0x11, sizeof(dummy));
177	if (bss->group_cipher == WPA_CIPHER_TKIP)
178	crypt = tkip_encrypt(incorrect_key ? dummy :
179	bss->gtk[bss->gtk_idx],
180	frame, len, hdrlen, NULL, pn,
181	bss->gtk_idx, &crypt_len);
182	else
183	crypt = ccmp_encrypt(incorrect_key ? dummy :
184	bss->gtk[bss->gtk_idx],
185	frame, len, hdrlen, NULL, pn,
186	bss->gtk_idx, &crypt_len);
187
188	if (crypt == NULL)
189	return -1;
190
191	ret = inject_frame(wt->monitor_sock, crypt, crypt_len);
192	os_free(crypt);
193
194	return (ret < 0) ? -1 : 0;
195	}
196
197
571ab37b JM	198	static int wlantest_inject_prot(struct wlantest wt, struct wlantest_bss bss,
	199	struct wlantest_sta sta, u8 frame,
	200	size_t len, int incorrect_key)
	201	{
	202	u8 *crypt;
	203	size_t crypt_len;
	204	int ret;
	205	u8 dummy[64];
	206	u8 *pn;
	207	struct ieee80211_hdr *hdr;
	208	u16 fc;
	209	int tid = 0;
	210	u8 *qos = NULL;
	211	int hdrlen;
b3a6d9d4 JM	212	struct wlantest_tdls *tdls = NULL;
b3a6d9d4 JM	213	const u8 *tk = NULL;
571ab37b	214
2e4c3469 JM	215	hdr = (struct ieee80211_hdr *) frame;
	216	hdrlen = 24;
	217	fc = le_to_host16(hdr->frame_control);
	218
880a97dc JM	219	if (WLAN_FC_GET_TYPE(fc) == WLAN_FC_TYPE_DATA &&
880a97dc JM	220	(fc & (WLAN_FC_TODS \| WLAN_FC_FROMDS)) == 0) {
b3a6d9d4 JM	221	struct wlantest_sta *sta2;
b3a6d9d4 JM	222	bss = bss_get(wt, hdr->addr3);
a8401116 JM	223	if (bss == NULL) {
	224	wpa_printf(MSG_DEBUG, "No BSS found for TDLS "
	225	"injection");
b3a6d9d4	226	return -1;
a8401116	227	}
b3a6d9d4 JM	228	sta = sta_find(bss, hdr->addr2);
b3a6d9d4 JM	229	sta2 = sta_find(bss, hdr->addr1);
a8401116 JM	230	if (sta == NULL \|\| sta2 == NULL) {
	231	wpa_printf(MSG_DEBUG, "No stations found for TDLS "
	232	"injection");
b3a6d9d4	233	return -1;
a8401116	234	}
b3a6d9d4 JM	235	dl_list_for_each(tdls, &bss->tdls, struct wlantest_tdls, list)
	236	{
	237	if ((tdls->init == sta && tdls->resp == sta2) \|\|
	238	(tdls->init == sta2 && tdls->resp == sta)) {
	239	if (!tdls->link_up)
	240	wpa_printf(MSG_DEBUG, "TDLS: Link not "
	241	"up, but injecting Data "
	242	"frame on direct link");
	243	tk = tdls->tpk.tk;
	244	break;
	245	}
	246	}
	247	}
	248
	249	if (tk == NULL && sta == NULL) {
2e4c3469 JM	250	if (WLAN_FC_GET_TYPE(fc) == WLAN_FC_TYPE_MGMT)
	251	return wlantest_inject_bip(wt, bss, frame, len,
	252	incorrect_key);
	253	return wlantest_inject_prot_bc(wt, bss, frame, len,
	254	incorrect_key);
	255	}
571ab37b	256
a8401116 JM	257	if (tk == NULL && !sta->ptk_set) {
a8401116 JM	258	wpa_printf(MSG_DEBUG, "No key known for injection");
571ab37b	259	return -1;
a8401116	260	}
571ab37b	261
571ab37b JM	262	if (WLAN_FC_GET_TYPE(fc) == WLAN_FC_TYPE_MGMT)
	263	tid = 16;
	264	else if (WLAN_FC_GET_TYPE(fc) == WLAN_FC_TYPE_DATA) {
	265	if ((fc & (WLAN_FC_TODS \| WLAN_FC_FROMDS)) ==
	266	(WLAN_FC_TODS \| WLAN_FC_FROMDS))
	267	hdrlen += ETH_ALEN;
	268	if (WLAN_FC_GET_STYPE(fc) & 0x08) {
	269	qos = frame + hdrlen;
	270	hdrlen += 2;
	271	tid = qos[0] & 0x0f;
	272	}
	273	}
b3a6d9d4 JM	274	if (tk) {
	275	if (os_memcmp(hdr->addr2, tdls->init->addr, ETH_ALEN) == 0)
	276	pn = tdls->rsc_init[tid];
	277	else
	278	pn = tdls->rsc_resp[tid];
	279	} else if (os_memcmp(hdr->addr2, bss->bssid, ETH_ALEN) == 0)
571ab37b JM	280	pn = sta->rsc_fromds[tid];
	281	else
	282	pn = sta->rsc_tods[tid];
	283	inc_byte_array(pn, 6);
	284
	285	os_memset(dummy, 0x11, sizeof(dummy));
b3a6d9d4 JM	286	if (tk)
	287	crypt = ccmp_encrypt(incorrect_key ? dummy : tk,
	288	frame, len, hdrlen, qos, pn, 0,
	289	&crypt_len);
	290	else if (sta->pairwise_cipher == WPA_CIPHER_TKIP)
571ab37b JM	291	crypt = tkip_encrypt(incorrect_key ? dummy : sta->ptk.tk1,
	292	frame, len, hdrlen, qos, pn, 0,
	293	&crypt_len);
	294	else
	295	crypt = ccmp_encrypt(incorrect_key ? dummy : sta->ptk.tk1,
	296	frame, len, hdrlen, qos, pn, 0,
	297	&crypt_len);
	298
a8401116 JM	299	if (crypt == NULL) {
a8401116 JM	300	wpa_printf(MSG_DEBUG, "Frame encryption failed");
571ab37b	301	return -1;
a8401116	302	}
571ab37b	303
902621e2	304	wpa_hexdump(MSG_DEBUG, "Inject frame (encrypted)", crypt, crypt_len);
571ab37b JM	305	ret = inject_frame(wt->monitor_sock, crypt, crypt_len);
571ab37b JM	306	os_free(crypt);
a8401116	307	wpa_printf(MSG_DEBUG, "inject_frame for protected frame: %d", ret);
571ab37b JM	308
	309	return (ret < 0) ? -1 : 0;
	310	}
	311
	312
7d23e971 JM	313	int wlantest_inject(struct wlantest wt, struct wlantest_bss bss,
	314	struct wlantest_sta sta, u8 frame, size_t len,
	315	enum wlantest_inject_protection prot)
	316	{
	317	int ret;
571ab37b JM	318	struct ieee80211_hdr *hdr;
	319	u16 fc;
	320	int protectable, protect = 0;
7d23e971 JM	321
	322	wpa_hexdump(MSG_DEBUG, "Inject frame", frame, len);
	323	if (wt->monitor_sock < 0) {
	324	wpa_printf(MSG_INFO, "Cannot inject frames when monitor "
	325	"interface is not in use");
	326	return -1;
	327	}
	328
571ab37b JM	329	hdr = (struct ieee80211_hdr *) frame;
	330	fc = le_to_host16(hdr->frame_control);
	331	protectable = WLAN_FC_GET_TYPE(fc) == WLAN_FC_TYPE_DATA \|\|
	332	is_robust_mgmt(frame, len);
	333
	334	if (prot == WLANTEST_INJECT_PROTECTED \|\|
	335	prot == WLANTEST_INJECT_INCORRECT_KEY) {
2e4c3469 JM	336	if (!sta &&
	337	((WLAN_FC_GET_TYPE(fc) == WLAN_FC_TYPE_MGMT &&
	338	!bss->igtk_set[bss->igtk_idx]) \|\|
	339	(WLAN_FC_GET_TYPE(fc) == WLAN_FC_TYPE_DATA &&
	340	!bss->gtk_len[bss->gtk_idx]))) {
	341	wpa_printf(MSG_INFO, "No GTK/IGTK known for "
	342	MACSTR " to protect the injected "
	343	"frame", MAC2STR(bss->bssid));
571ab37b JM	344	return -1;
	345	}
	346	if (sta && !sta->ptk_set) {
	347	wpa_printf(MSG_INFO, "No PTK known for the STA " MACSTR
	348	" to encrypt the injected frame",
	349	MAC2STR(sta->addr));
	350	return -1;
	351	}
	352	protect = 1;
	353	} else if (protectable && prot != WLANTEST_INJECT_UNPROTECTED) {
	354	if (sta && sta->ptk_set)
	355	protect = 1;
	356	else if (!sta) {
	357	if (WLAN_FC_GET_TYPE(fc) == WLAN_FC_TYPE_DATA &&
2e4c3469	358	bss->gtk_len[bss->gtk_idx])
571ab37b JM	359	protect = 1;
571ab37b JM	360	if (WLAN_FC_GET_TYPE(fc) == WLAN_FC_TYPE_MGMT &&
2e4c3469	361	bss->igtk_set[bss->igtk_idx])
571ab37b JM	362	protect = 1;
	363	}
	364	}
	365
571ab37b JM	366	if (protect)
	367	return wlantest_inject_prot(
	368	wt, bss, sta, frame, len,
	369	prot == WLANTEST_INJECT_INCORRECT_KEY);
7d23e971 JM	370
7d23e971 JM	371	ret = inject_frame(wt->monitor_sock, frame, len);
a8401116	372	wpa_printf(MSG_DEBUG, "inject_frame for unprotected frame: %d", ret);
7d23e971 JM	373	return (ret < 0) ? -1 : 0;
7d23e971 JM	374	}