[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 "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_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;

	if (sta == NULL)
		return -1; /* TODO: add support for group Data and BIP */

	if (!sta->ptk_set)
		return -1;

	hdr = (struct ieee80211_hdr *) frame;
	hdrlen = 24;
	fc = le_to_host16(hdr->frame_control);
	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 (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 (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)
		return -1;

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

	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) {
			wpa_printf(MSG_INFO, "Broadcast protection not yet "
				   "implemented");
			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[1] || bss->gtk_len[2]))
				protect = 1;
			if (WLAN_FC_GET_TYPE(fc) == WLAN_FC_TYPE_MGMT &&
			    (bss->igtk_set[4] || bss->igtk_set[5]))
				protect = 1;
		}
	}

	if ((prot == WLANTEST_INJECT_PROTECTED ||
	     prot == WLANTEST_INJECT_INCORRECT_KEY) && !protect) {
		wpa_printf(MSG_INFO, "Cannot protect injected frame");
		return -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);
	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"
7d23e971 JM	20	#include "wlantest.h"
	21
	22
	23	static int inject_frame(int s, const void *data, size_t len)
	24	{
	25	#define IEEE80211_RADIOTAP_F_FRAG 0x08
	26	unsigned char rtap_hdr[] = {
	27	0x00, 0x00, /* radiotap version */
	28	0x0e, 0x00, /* radiotap length */
	29	0x02, 0xc0, 0x00, 0x00, /* bmap: flags, tx and rx flags */
	30	IEEE80211_RADIOTAP_F_FRAG, /* F_FRAG (fragment if required) */
	31	0x00, /* padding */
	32	0x00, 0x00, /* RX and TX flags to indicate that */
	33	0x00, 0x00, /* this is the injected frame directly */
	34	};
	35	struct iovec iov[2] = {
	36	{
	37	.iov_base = &rtap_hdr,
	38	.iov_len = sizeof(rtap_hdr),
	39	},
	40	{
	41	.iov_base = (void *) data,
	42	.iov_len = len,
	43	}
	44	};
	45	struct msghdr msg = {
	46	.msg_name = NULL,
	47	.msg_namelen = 0,
	48	.msg_iov = iov,
	49	.msg_iovlen = 2,
	50	.msg_control = NULL,
	51	.msg_controllen = 0,
	52	.msg_flags = 0,
	53	};
	54	int ret;
	55
	56	ret = sendmsg(s, &msg, 0);
	57	if (ret < 0)
	58	perror("sendmsg");
	59	return ret;
	60	}
	61
	62
571ab37b JM	63	static int is_robust_mgmt(u8 *frame, size_t len)
	64	{
	65	struct ieee80211_mgmt *mgmt;
	66	u16 fc, stype;
	67	if (len < 24)
	68	return 0;
	69	mgmt = (struct ieee80211_mgmt *) frame;
	70	fc = le_to_host16(mgmt->frame_control);
	71	if (WLAN_FC_GET_TYPE(fc) != WLAN_FC_TYPE_MGMT)
	72	return 0;
	73	stype = WLAN_FC_GET_STYPE(fc);
	74	if (stype == WLAN_FC_STYPE_DEAUTH \|\| stype == WLAN_FC_STYPE_DISASSOC)
	75	return 1;
	76	if (stype == WLAN_FC_STYPE_ACTION) {
	77	if (len < 25)
	78	return 0;
	79	if (mgmt->u.action.category != WLAN_ACTION_PUBLIC)
	80	return 1;
	81	}
	82	return 0;
	83	}
	84
	85
	86	static int wlantest_inject_prot(struct wlantest wt, struct wlantest_bss bss,
	87	struct wlantest_sta sta, u8 frame,
	88	size_t len, int incorrect_key)
	89	{
	90	u8 *crypt;
	91	size_t crypt_len;
	92	int ret;
	93	u8 dummy[64];
	94	u8 *pn;
	95	struct ieee80211_hdr *hdr;
	96	u16 fc;
	97	int tid = 0;
	98	u8 *qos = NULL;
	99	int hdrlen;
	100
	101	if (sta == NULL)
	102	return -1; /* TODO: add support for group Data and BIP */
	103
	104	if (!sta->ptk_set)
	105	return -1;
	106
	107	hdr = (struct ieee80211_hdr *) frame;
	108	hdrlen = 24;
	109	fc = le_to_host16(hdr->frame_control);
	110	if (WLAN_FC_GET_TYPE(fc) == WLAN_FC_TYPE_MGMT)
	111	tid = 16;
	112	else if (WLAN_FC_GET_TYPE(fc) == WLAN_FC_TYPE_DATA) {
	113	if ((fc & (WLAN_FC_TODS \| WLAN_FC_FROMDS)) ==
	114	(WLAN_FC_TODS \| WLAN_FC_FROMDS))
	115	hdrlen += ETH_ALEN;
	116	if (WLAN_FC_GET_STYPE(fc) & 0x08) {
	117	qos = frame + hdrlen;
	118	hdrlen += 2;
	119	tid = qos[0] & 0x0f;
	120	}
	121	}
	122	if (os_memcmp(hdr->addr2, bss->bssid, ETH_ALEN) == 0)
	123	pn = sta->rsc_fromds[tid];
	124	else
	125	pn = sta->rsc_tods[tid];
	126	inc_byte_array(pn, 6);
127
128	os_memset(dummy, 0x11, sizeof(dummy));
129	if (sta->pairwise_cipher == WPA_CIPHER_TKIP)
130	crypt = tkip_encrypt(incorrect_key ? dummy : sta->ptk.tk1,
131	frame, len, hdrlen, qos, pn, 0,
132	&crypt_len);
133	else
134	crypt = ccmp_encrypt(incorrect_key ? dummy : sta->ptk.tk1,
135	frame, len, hdrlen, qos, pn, 0,
136	&crypt_len);
137
138	if (crypt == NULL)
139	return -1;
140
141	ret = inject_frame(wt->monitor_sock, crypt, crypt_len);
142	os_free(crypt);
143
144	return (ret < 0) ? -1 : 0;
145	}
146
147
7d23e971 JM	148	int wlantest_inject(struct wlantest wt, struct wlantest_bss bss,
	149	struct wlantest_sta sta, u8 frame, size_t len,
	150	enum wlantest_inject_protection prot)
	151	{
	152	int ret;
571ab37b JM	153	struct ieee80211_hdr *hdr;
	154	u16 fc;
	155	int protectable, protect = 0;
7d23e971 JM	156
	157	wpa_hexdump(MSG_DEBUG, "Inject frame", frame, len);
	158	if (wt->monitor_sock < 0) {
	159	wpa_printf(MSG_INFO, "Cannot inject frames when monitor "
	160	"interface is not in use");
	161	return -1;
	162	}
	163
571ab37b JM	164	hdr = (struct ieee80211_hdr *) frame;
	165	fc = le_to_host16(hdr->frame_control);
	166	protectable = WLAN_FC_GET_TYPE(fc) == WLAN_FC_TYPE_DATA \|\|
	167	is_robust_mgmt(frame, len);
	168
	169	if (prot == WLANTEST_INJECT_PROTECTED \|\|
	170	prot == WLANTEST_INJECT_INCORRECT_KEY) {
	171	if (!sta) {
	172	wpa_printf(MSG_INFO, "Broadcast protection not yet "
	173	"implemented");
	174	return -1;
	175	}
	176	if (sta && !sta->ptk_set) {
	177	wpa_printf(MSG_INFO, "No PTK known for the STA " MACSTR
	178	" to encrypt the injected frame",
	179	MAC2STR(sta->addr));
	180	return -1;
	181	}
	182	protect = 1;
	183	} else if (protectable && prot != WLANTEST_INJECT_UNPROTECTED) {
	184	if (sta && sta->ptk_set)
	185	protect = 1;
	186	else if (!sta) {
	187	if (WLAN_FC_GET_TYPE(fc) == WLAN_FC_TYPE_DATA &&
	188	(bss->gtk_len[1] \|\| bss->gtk_len[2]))
	189	protect = 1;
	190	if (WLAN_FC_GET_TYPE(fc) == WLAN_FC_TYPE_MGMT &&
	191	(bss->igtk_set[4] \|\| bss->igtk_set[5]))
	192	protect = 1;
	193	}
	194	}
	195
	196	if ((prot == WLANTEST_INJECT_PROTECTED \|\|
	197	prot == WLANTEST_INJECT_INCORRECT_KEY) && !protect) {
	198	wpa_printf(MSG_INFO, "Cannot protect injected frame");
7d23e971	199	return -1;
571ab37b JM	200	}
	201
	202	if (protect)
	203	return wlantest_inject_prot(
	204	wt, bss, sta, frame, len,
	205	prot == WLANTEST_INJECT_INCORRECT_KEY);
7d23e971 JM	206
	207	ret = inject_frame(wt->monitor_sock, frame, len);
	208	return (ret < 0) ? -1 : 0;
	209	}