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

/*
 * CTR with CBC-MAC Protocol (CCMP)
 * Copyright (c) 2010-2012, 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 "common/ieee802_11_defs.h"
#include "crypto/aes.h"
#include "crypto/aes_wrap.h"
#include "wlantest.h"


static void ccmp_aad_nonce(const struct ieee80211_hdr *hdr, const u8 *data,
			   u8 *aad, size_t *aad_len, u8 *nonce)
{
	u16 fc, stype, seq;
	int qos = 0, addr4 = 0;
	u8 *pos;

	nonce[0] = 0;

	fc = le_to_host16(hdr->frame_control);
	stype = WLAN_FC_GET_STYPE(fc);
	if ((fc & (WLAN_FC_TODS | WLAN_FC_FROMDS)) ==
	    (WLAN_FC_TODS | WLAN_FC_FROMDS))
		addr4 = 1;

	if (WLAN_FC_GET_TYPE(fc) == WLAN_FC_TYPE_DATA) {
		fc &= ~0x0070; /* Mask subtype bits */
		if (stype & 0x08) {
			const u8 *qc;
			qos = 1;
			fc &= ~WLAN_FC_ORDER;
			qc = (const u8 *) (hdr + 1);
			if (addr4)
				qc += ETH_ALEN;
			nonce[0] = qc[0] & 0x0f;
		}
	} else if (WLAN_FC_GET_TYPE(fc) == WLAN_FC_TYPE_MGMT)
		nonce[0] |= 0x10; /* Management */

	fc &= ~(WLAN_FC_RETRY | WLAN_FC_PWRMGT | WLAN_FC_MOREDATA);
	fc |= WLAN_FC_ISWEP;
	WPA_PUT_LE16(aad, fc);
	pos = aad + 2;
	os_memcpy(pos, hdr->addr1, 3 * ETH_ALEN);
	pos += 3 * ETH_ALEN;
	seq = le_to_host16(hdr->seq_ctrl);
	seq &= ~0xfff0; /* Mask Seq#; do not modify Frag# */
	WPA_PUT_LE16(pos, seq);
	pos += 2;

	os_memcpy(pos, hdr + 1, addr4 * ETH_ALEN + qos * 2);
	pos += addr4 * ETH_ALEN;
	if (qos) {
		pos[0] &= ~0x70;
		if (1 /* FIX: either device has SPP A-MSDU Capab = 0 */)
			pos[0] &= ~0x80;
		pos++;
		*pos++ = 0x00;
	}

	*aad_len = pos - aad;

	os_memcpy(nonce + 1, hdr->addr2, ETH_ALEN);
	nonce[7] = data[7]; /* PN5 */
	nonce[8] = data[6]; /* PN4 */
	nonce[9] = data[5]; /* PN3 */
	nonce[10] = data[4]; /* PN2 */
	nonce[11] = data[1]; /* PN1 */
	nonce[12] = data[0]; /* PN0 */
}


u8 * ccmp_decrypt(const u8 *tk, const struct ieee80211_hdr *hdr,
		  const u8 *data, size_t data_len, size_t *decrypted_len)
{
	u8 aad[30], nonce[13];
	size_t aad_len;
	size_t mlen;
	u8 *plain;

	if (data_len < 8 + 8)
		return NULL;

	plain = os_malloc(data_len + AES_BLOCK_SIZE);
	if (plain == NULL)
		return NULL;

	mlen = data_len - 8 - 8;

	os_memset(aad, 0, sizeof(aad));
	ccmp_aad_nonce(hdr, data, aad, &aad_len, nonce);
	wpa_hexdump(MSG_EXCESSIVE, "CCMP AAD", aad, aad_len);
	wpa_hexdump(MSG_EXCESSIVE, "CCMP nonce", nonce, 13);

	if (aes_ccm_ad(tk, 16, nonce, 8, data + 8, mlen, aad, aad_len,
		       data + 8 + mlen, plain) < 0) {
		u16 seq_ctrl = le_to_host16(hdr->seq_ctrl);
		wpa_printf(MSG_INFO, "Invalid CCMP MIC in frame: A1=" MACSTR
			   " A2=" MACSTR " A3=" MACSTR " seq=%u frag=%u",
			   MAC2STR(hdr->addr1), MAC2STR(hdr->addr2),
			   MAC2STR(hdr->addr3),
			   WLAN_GET_SEQ_SEQ(seq_ctrl),
			   WLAN_GET_SEQ_FRAG(seq_ctrl));
		os_free(plain);
		return NULL;
	}
	wpa_hexdump(MSG_EXCESSIVE, "CCMP decrypted", plain, mlen);

	*decrypted_len = mlen;
	return plain;
}


void ccmp_get_pn(u8 *pn, const u8 *data)
{
	pn[0] = data[7]; /* PN5 */
	pn[1] = data[6]; /* PN4 */
	pn[2] = data[5]; /* PN3 */
	pn[3] = data[4]; /* PN2 */
	pn[4] = data[1]; /* PN1 */
	pn[5] = data[0]; /* PN0 */
}


u8 * ccmp_encrypt(const u8 *tk, u8 *frame, size_t len, size_t hdrlen, u8 *qos,
		  u8 *pn, int keyid, size_t *encrypted_len)
{
	u8 aad[30], nonce[13];
	size_t aad_len, plen;
	u8 *crypt, *pos;
	struct ieee80211_hdr *hdr;

	if (len < hdrlen || hdrlen < 24)
		return NULL;
	plen = len - hdrlen;

	crypt = os_malloc(hdrlen + 8 + plen + 8 + AES_BLOCK_SIZE);
	if (crypt == NULL)
		return NULL;

	os_memcpy(crypt, frame, hdrlen);
	hdr = (struct ieee80211_hdr *) crypt;
	hdr->frame_control |= host_to_le16(WLAN_FC_ISWEP);
	pos = crypt + hdrlen;
	*pos++ = pn[5]; /* PN0 */
	*pos++ = pn[4]; /* PN1 */
	*pos++ = 0x00; /* Rsvd */
	*pos++ = 0x20 | (keyid << 6);
	*pos++ = pn[3]; /* PN2 */
	*pos++ = pn[2]; /* PN3 */
	*pos++ = pn[1]; /* PN4 */
	*pos++ = pn[0]; /* PN5 */

	os_memset(aad, 0, sizeof(aad));
	ccmp_aad_nonce(hdr, crypt + hdrlen, aad, &aad_len, nonce);
	wpa_hexdump(MSG_EXCESSIVE, "CCMP AAD", aad, aad_len);
	wpa_hexdump(MSG_EXCESSIVE, "CCMP nonce", nonce, 13);

	if (aes_ccm_ae(tk, 16, nonce, 8, frame + hdrlen, plen, aad, aad_len,
		       pos, pos + plen) < 0) {
		os_free(crypt);
		return NULL;
	}

	wpa_hexdump(MSG_EXCESSIVE, "CCMP encrypted", crypt + hdrlen + 8, plen);

	*encrypted_len = hdrlen + 8 + plen + 8;

	return crypt;
}


u8 * ccmp_256_decrypt(const u8 *tk, const struct ieee80211_hdr *hdr,
		      const u8 *data, size_t data_len, size_t *decrypted_len)
{
	u8 aad[30], nonce[13];
	size_t aad_len;
	size_t mlen;
	u8 *plain;

	if (data_len < 8 + 16)
		return NULL;

	plain = os_malloc(data_len + AES_BLOCK_SIZE);
	if (plain == NULL)
		return NULL;

	mlen = data_len - 8 - 16;

	os_memset(aad, 0, sizeof(aad));
	ccmp_aad_nonce(hdr, data, aad, &aad_len, nonce);
	wpa_hexdump(MSG_EXCESSIVE, "CCMP-256 AAD", aad, aad_len);
	wpa_hexdump(MSG_EXCESSIVE, "CCMP-256 nonce", nonce, 13);

	if (aes_ccm_ad(tk, 16, nonce, 16, data + 8, mlen, aad, aad_len,
		       data + 8 + mlen, plain) < 0) {
		u16 seq_ctrl = le_to_host16(hdr->seq_ctrl);
		wpa_printf(MSG_INFO, "Invalid CCMP-256 MIC in frame: A1=" MACSTR
			   " A2=" MACSTR " A3=" MACSTR " seq=%u frag=%u",
			   MAC2STR(hdr->addr1), MAC2STR(hdr->addr2),
			   MAC2STR(hdr->addr3),
			   WLAN_GET_SEQ_SEQ(seq_ctrl),
			   WLAN_GET_SEQ_FRAG(seq_ctrl));
		os_free(plain);
		return NULL;
	}
	wpa_hexdump(MSG_EXCESSIVE, "CCMP-256 decrypted", plain, mlen);

	*decrypted_len = mlen;
	return plain;
}


u8 * ccmp_256_encrypt(const u8 *tk, u8 *frame, size_t len, size_t hdrlen,
		      u8 *qos, u8 *pn, int keyid, size_t *encrypted_len)
{
	u8 aad[30], nonce[13];
	size_t aad_len, plen;
	u8 *crypt, *pos;
	struct ieee80211_hdr *hdr;

	if (len < hdrlen || hdrlen < 24)
		return NULL;
	plen = len - hdrlen;

	crypt = os_malloc(hdrlen + 8 + plen + 16 + AES_BLOCK_SIZE);
	if (crypt == NULL)
		return NULL;

	os_memcpy(crypt, frame, hdrlen);
	hdr = (struct ieee80211_hdr *) crypt;
	hdr->frame_control |= host_to_le16(WLAN_FC_ISWEP);
	pos = crypt + hdrlen;
	*pos++ = pn[5]; /* PN0 */
	*pos++ = pn[4]; /* PN1 */
	*pos++ = 0x00; /* Rsvd */
	*pos++ = 0x20 | (keyid << 6);
	*pos++ = pn[3]; /* PN2 */
	*pos++ = pn[2]; /* PN3 */
	*pos++ = pn[1]; /* PN4 */
	*pos++ = pn[0]; /* PN5 */

	os_memset(aad, 0, sizeof(aad));
	ccmp_aad_nonce(hdr, crypt + hdrlen, aad, &aad_len, nonce);
	wpa_hexdump(MSG_EXCESSIVE, "CCMP-256 AAD", aad, aad_len);
	wpa_hexdump(MSG_EXCESSIVE, "CCMP-256 nonce", nonce, 13);

	if (aes_ccm_ae(tk, 16, nonce, 16, frame + hdrlen, plen, aad, aad_len,
		       pos, pos + plen) < 0) {
		os_free(crypt);
		return NULL;
	}

	wpa_hexdump(MSG_EXCESSIVE, "CCMP-256 encrypted", crypt + hdrlen + 8,
		    plen);

	*encrypted_len = hdrlen + 8 + plen + 16;

	return crypt;
}
Commit	Line	Data
d318c534 JM	1	/*
d318c534 JM	2	* CTR with CBC-MAC Protocol (CCMP)
05d6054c	3	* Copyright (c) 2010-2012, Jouni Malinen <j@w1.fi>
d318c534	4	*
0f3d578e JM	5	* This software may be distributed under the terms of the BSD license.
0f3d578e JM	6	* See README for more details.
d318c534 JM	7	*/
	8
	9	#include "utils/includes.h"
	10
	11	#include "utils/common.h"
	12	#include "common/ieee802_11_defs.h"
	13	#include "crypto/aes.h"
4bf39105	14	#include "crypto/aes_wrap.h"
d318c534 JM	15	#include "wlantest.h"
	16
	17
	18	static void ccmp_aad_nonce(const struct ieee80211_hdr hdr, const u8 data,
	19	u8 aad, size_t aad_len, u8 *nonce)
	20	{
	21	u16 fc, stype, seq;
	22	int qos = 0, addr4 = 0;
	23	u8 *pos;
	24
	25	nonce[0] = 0;
	26
	27	fc = le_to_host16(hdr->frame_control);
	28	stype = WLAN_FC_GET_STYPE(fc);
	29	if ((fc & (WLAN_FC_TODS \| WLAN_FC_FROMDS)) ==
	30	(WLAN_FC_TODS \| WLAN_FC_FROMDS))
	31	addr4 = 1;
	32
	33	if (WLAN_FC_GET_TYPE(fc) == WLAN_FC_TYPE_DATA) {
	34	fc &= ~0x0070; /* Mask subtype bits */
	35	if (stype & 0x08) {
	36	const u8 *qc;
	37	qos = 1;
	38	fc &= ~WLAN_FC_ORDER;
	39	qc = (const u8 *) (hdr + 1);
	40	if (addr4)
	41	qc += ETH_ALEN;
	42	nonce[0] = qc[0] & 0x0f;
	43	}
	44	} else if (WLAN_FC_GET_TYPE(fc) == WLAN_FC_TYPE_MGMT)
	45	nonce[0] \|= 0x10; /* Management */
	46
	47	fc &= ~(WLAN_FC_RETRY \| WLAN_FC_PWRMGT \| WLAN_FC_MOREDATA);
	48	fc \|= WLAN_FC_ISWEP;
	49	WPA_PUT_LE16(aad, fc);
	50	pos = aad + 2;
	51	os_memcpy(pos, hdr->addr1, 3 * ETH_ALEN);
	52	pos += 3 * ETH_ALEN;
	53	seq = le_to_host16(hdr->seq_ctrl);
	54	seq &= ~0xfff0; /* Mask Seq#; do not modify Frag# */
	55	WPA_PUT_LE16(pos, seq);
	56	pos += 2;
	57
	58	os_memcpy(pos, hdr + 1, addr4 * ETH_ALEN + qos * 2);
	59	pos += addr4 * ETH_ALEN;
	60	if (qos) {
ef44a08b	61	pos[0] &= ~0x70;
d318c534	62	if (1 /* FIX: either device has SPP A-MSDU Capab = 0 */)
ef44a08b	63	pos[0] &= ~0x80;
d318c534 JM	64	pos++;
	65	*pos++ = 0x00;
	66	}
	67
	68	*aad_len = pos - aad;
	69
	70	os_memcpy(nonce + 1, hdr->addr2, ETH_ALEN);
	71	nonce[7] = data[7]; /* PN5 */
	72	nonce[8] = data[6]; /* PN4 */
	73	nonce[9] = data[5]; /* PN3 */
	74	nonce[10] = data[4]; /* PN2 */
	75	nonce[11] = data[1]; /* PN1 */
	76	nonce[12] = data[0]; /* PN0 */
	77	}
	78
	79
d318c534 JM	80	u8 * ccmp_decrypt(const u8 tk, const struct ieee80211_hdr hdr,
	81	const u8 data, size_t data_len, size_t decrypted_len)
	82	{
05d6054c	83	u8 aad[30], nonce[13];
d318c534	84	size_t aad_len;
05d6054c JM	85	size_t mlen;
05d6054c JM	86	u8 *plain;
d318c534 JM	87
	88	if (data_len < 8 + 8)
	89	return NULL;
	90
71a7e936	91	plain = os_malloc(data_len + AES_BLOCK_SIZE);
d318c534 JM	92	if (plain == NULL)
	93	return NULL;
	94
d318c534	95	mlen = data_len - 8 - 8;
d318c534 JM	96
d318c534 JM	97	os_memset(aad, 0, sizeof(aad));
05d6054c JM	98	ccmp_aad_nonce(hdr, data, aad, &aad_len, nonce);
05d6054c JM	99	wpa_hexdump(MSG_EXCESSIVE, "CCMP AAD", aad, aad_len);
d318c534 JM	100	wpa_hexdump(MSG_EXCESSIVE, "CCMP nonce", nonce, 13);
d318c534 JM	101
05d6054c JM	102	if (aes_ccm_ad(tk, 16, nonce, 8, data + 8, mlen, aad, aad_len,
05d6054c JM	103	data + 8 + mlen, plain) < 0) {
3558c41e JM	104	u16 seq_ctrl = le_to_host16(hdr->seq_ctrl);
	105	wpa_printf(MSG_INFO, "Invalid CCMP MIC in frame: A1=" MACSTR
	106	" A2=" MACSTR " A3=" MACSTR " seq=%u frag=%u",
	107	MAC2STR(hdr->addr1), MAC2STR(hdr->addr2),
	108	MAC2STR(hdr->addr3),
	109	WLAN_GET_SEQ_SEQ(seq_ctrl),
	110	WLAN_GET_SEQ_FRAG(seq_ctrl));
d318c534 JM	111	os_free(plain);
	112	return NULL;
	113	}
05d6054c	114	wpa_hexdump(MSG_EXCESSIVE, "CCMP decrypted", plain, mlen);
d318c534 JM	115
	116	*decrypted_len = mlen;
	117	return plain;
	118	}
2edd5c23 JM	119
	120
	121	void ccmp_get_pn(u8 pn, const u8 data)
	122	{
	123	pn[0] = data[7]; /* PN5 */
	124	pn[1] = data[6]; /* PN4 */
	125	pn[2] = data[5]; /* PN3 */
	126	pn[3] = data[4]; /* PN2 */
	127	pn[4] = data[1]; /* PN1 */
	128	pn[5] = data[0]; /* PN0 */
	129	}
571ab37b JM	130
	131
	132	u8 * ccmp_encrypt(const u8 tk, u8 frame, size_t len, size_t hdrlen, u8 *qos,
	133	u8 pn, int keyid, size_t encrypted_len)
	134	{
05d6054c JM	135	u8 aad[30], nonce[13];
	136	size_t aad_len, plen;
	137	u8 crypt, pos;
571ab37b	138	struct ieee80211_hdr *hdr;
571ab37b JM	139
	140	if (len < hdrlen \|\| hdrlen < 24)
	141	return NULL;
	142	plen = len - hdrlen;
571ab37b	143
71a7e936	144	crypt = os_malloc(hdrlen + 8 + plen + 8 + AES_BLOCK_SIZE);
571ab37b JM	145	if (crypt == NULL)
	146	return NULL;
	147
	148	os_memcpy(crypt, frame, hdrlen);
	149	hdr = (struct ieee80211_hdr *) crypt;
	150	hdr->frame_control \|= host_to_le16(WLAN_FC_ISWEP);
	151	pos = crypt + hdrlen;
	152	pos++ = pn[5]; / PN0 */
	153	pos++ = pn[4]; / PN1 */
81d59891	154	pos++ = 0x00; / Rsvd */
571ab37b JM	155	*pos++ = 0x20 \| (keyid << 6);
	156	pos++ = pn[3]; / PN2 */
	157	pos++ = pn[2]; / PN3 */
	158	pos++ = pn[1]; / PN4 */
	159	pos++ = pn[0]; / PN5 */
	160
571ab37b	161	os_memset(aad, 0, sizeof(aad));
05d6054c JM	162	ccmp_aad_nonce(hdr, crypt + hdrlen, aad, &aad_len, nonce);
05d6054c JM	163	wpa_hexdump(MSG_EXCESSIVE, "CCMP AAD", aad, aad_len);
571ab37b JM	164	wpa_hexdump(MSG_EXCESSIVE, "CCMP nonce", nonce, 13);
571ab37b JM	165
05d6054c JM	166	if (aes_ccm_ae(tk, 16, nonce, 8, frame + hdrlen, plen, aad, aad_len,
	167	pos, pos + plen) < 0) {
	168	os_free(crypt);
	169	return NULL;
571ab37b JM	170	}
571ab37b JM	171
571ab37b JM	172	wpa_hexdump(MSG_EXCESSIVE, "CCMP encrypted", crypt + hdrlen + 8, plen);
571ab37b JM	173
571ab37b JM	174	*encrypted_len = hdrlen + 8 + plen + 8;
	175
	176	return crypt;
	177	}
7d19d3e9 JM	178
	179
	180	u8 * ccmp_256_decrypt(const u8 tk, const struct ieee80211_hdr hdr,
	181	const u8 data, size_t data_len, size_t decrypted_len)
	182	{
	183	u8 aad[30], nonce[13];
	184	size_t aad_len;
	185	size_t mlen;
	186	u8 *plain;
	187
	188	if (data_len < 8 + 16)
	189	return NULL;
	190
	191	plain = os_malloc(data_len + AES_BLOCK_SIZE);
	192	if (plain == NULL)
	193	return NULL;
	194
	195	mlen = data_len - 8 - 16;
	196
	197	os_memset(aad, 0, sizeof(aad));
	198	ccmp_aad_nonce(hdr, data, aad, &aad_len, nonce);
	199	wpa_hexdump(MSG_EXCESSIVE, "CCMP-256 AAD", aad, aad_len);
	200	wpa_hexdump(MSG_EXCESSIVE, "CCMP-256 nonce", nonce, 13);
	201
	202	if (aes_ccm_ad(tk, 16, nonce, 16, data + 8, mlen, aad, aad_len,
	203	data + 8 + mlen, plain) < 0) {
	204	u16 seq_ctrl = le_to_host16(hdr->seq_ctrl);
	205	wpa_printf(MSG_INFO, "Invalid CCMP-256 MIC in frame: A1=" MACSTR
	206	" A2=" MACSTR " A3=" MACSTR " seq=%u frag=%u",
	207	MAC2STR(hdr->addr1), MAC2STR(hdr->addr2),
	208	MAC2STR(hdr->addr3),
	209	WLAN_GET_SEQ_SEQ(seq_ctrl),
	210	WLAN_GET_SEQ_FRAG(seq_ctrl));
	211	os_free(plain);
	212	return NULL;
	213	}
	214	wpa_hexdump(MSG_EXCESSIVE, "CCMP-256 decrypted", plain, mlen);
	215
	216	*decrypted_len = mlen;
	217	return plain;
	218	}
	219
	220
	221	u8 * ccmp_256_encrypt(const u8 tk, u8 frame, size_t len, size_t hdrlen,
	222	u8 qos, u8 pn, int keyid, size_t *encrypted_len)
	223	{
	224	u8 aad[30], nonce[13];
	225	size_t aad_len, plen;
	226	u8 crypt, pos;
	227	struct ieee80211_hdr *hdr;
	228
	229	if (len < hdrlen \|\| hdrlen < 24)
	230	return NULL;
	231	plen = len - hdrlen;
	232
	233	crypt = os_malloc(hdrlen + 8 + plen + 16 + AES_BLOCK_SIZE);
	234	if (crypt == NULL)
	235	return NULL;
	236
	237	os_memcpy(crypt, frame, hdrlen);
	238	hdr = (struct ieee80211_hdr *) crypt;
	239	hdr->frame_control \|= host_to_le16(WLAN_FC_ISWEP);
	240	pos = crypt + hdrlen;
	241	pos++ = pn[5]; / PN0 */
242	pos++ = pn[4]; / PN1 */
243	pos++ = 0x00; / Rsvd */
244	*pos++ = 0x20 \| (keyid << 6);
245	pos++ = pn[3]; / PN2 */
246	pos++ = pn[2]; / PN3 */
247	pos++ = pn[1]; / PN4 */
248	pos++ = pn[0]; / PN5 */
249
250	os_memset(aad, 0, sizeof(aad));
251	ccmp_aad_nonce(hdr, crypt + hdrlen, aad, &aad_len, nonce);
252	wpa_hexdump(MSG_EXCESSIVE, "CCMP-256 AAD", aad, aad_len);
253	wpa_hexdump(MSG_EXCESSIVE, "CCMP-256 nonce", nonce, 13);
254
255	if (aes_ccm_ae(tk, 16, nonce, 16, frame + hdrlen, plen, aad, aad_len,
256	pos, pos + plen) < 0) {
257	os_free(crypt);
258	return NULL;
259	}
260
261	wpa_hexdump(MSG_EXCESSIVE, "CCMP-256 encrypted", crypt + hdrlen + 8,
262	plen);
263
264	*encrypted_len = hdrlen + 8 + plen + 16;
265
266	return crypt;
267	}