[linux-2.6-block.git] / drivers / staging / rtl8192u / ieee80211 / ieee80211_crypt_ccmp.c

/*
 * Host AP crypt: host-based CCMP encryption implementation for Host AP driver
 *
 * Copyright (c) 2003-2004, Jouni Malinen <jkmaline@cc.hut.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. See README and COPYING for
 * more details.
 */

#include <linux/module.h>
#include <linux/init.h>
#include <linux/slab.h>
#include <linux/random.h>
#include <linux/skbuff.h>
#include <linux/netdevice.h>
#include <linux/if_ether.h>
#include <linux/if_arp.h>
#include <linux/string.h>
#include <linux/wireless.h>

#include "ieee80211.h"

#include <linux/crypto.h>
    #include <linux/scatterlist.h>

MODULE_AUTHOR("Jouni Malinen");
MODULE_DESCRIPTION("Host AP crypt: CCMP");
MODULE_LICENSE("GPL");

#define AES_BLOCK_LEN 16
#define CCMP_HDR_LEN 8
#define CCMP_MIC_LEN 8
#define CCMP_TK_LEN 16
#define CCMP_PN_LEN 6

struct ieee80211_ccmp_data {
	u8 key[CCMP_TK_LEN];
	int key_set;

	u8 tx_pn[CCMP_PN_LEN];
	u8 rx_pn[CCMP_PN_LEN];

	u32 dot11RSNAStatsCCMPFormatErrors;
	u32 dot11RSNAStatsCCMPReplays;
	u32 dot11RSNAStatsCCMPDecryptErrors;

	int key_idx;

	struct crypto_tfm *tfm;

	/* scratch buffers for virt_to_page() (crypto API) */
	u8 tx_b0[AES_BLOCK_LEN], tx_b[AES_BLOCK_LEN],
		tx_e[AES_BLOCK_LEN], tx_s0[AES_BLOCK_LEN];
	u8 rx_b0[AES_BLOCK_LEN], rx_b[AES_BLOCK_LEN], rx_a[AES_BLOCK_LEN];
};

static void ieee80211_ccmp_aes_encrypt(struct crypto_tfm *tfm,
			     const u8 pt[16], u8 ct[16])
{
	crypto_cipher_encrypt_one((void *)tfm, ct, pt);
}

static void *ieee80211_ccmp_init(int key_idx)
{
	struct ieee80211_ccmp_data *priv;

	priv = kzalloc(sizeof(*priv), GFP_ATOMIC);
	if (!priv)
		goto fail;
	priv->key_idx = key_idx;

	priv->tfm = (void *)crypto_alloc_cipher("aes", 0, CRYPTO_ALG_ASYNC);
	if (IS_ERR(priv->tfm)) {
		printk(KERN_DEBUG "ieee80211_crypt_ccmp: could not allocate crypto API aes\n");
		priv->tfm = NULL;
		goto fail;
	}

	return priv;

fail:
	if (priv) {
		if (priv->tfm)
			crypto_free_cipher((void *)priv->tfm);
		kfree(priv);
	}

	return NULL;
}

static void ieee80211_ccmp_deinit(void *priv)
{
	struct ieee80211_ccmp_data *_priv = priv;

	if (_priv && _priv->tfm)
		crypto_free_cipher((void *)_priv->tfm);
	kfree(priv);
}

static inline void xor_block(u8 *b, u8 *a, size_t len)
{
	int i;

	for (i = 0; i < len; i++)
		b[i] ^= a[i];
}

static void ccmp_init_blocks(struct crypto_tfm *tfm,
			     struct rtl_80211_hdr_4addr *hdr,
			     u8 *pn, size_t dlen, u8 *b0, u8 *auth,
			     u8 *s0)
{
	u8 *pos, qc = 0;
	size_t aad_len;
	u16 fc;
	int a4_included, qc_included;
	u8 aad[2 * AES_BLOCK_LEN];

	fc = le16_to_cpu(hdr->frame_ctl);
	a4_included = ((fc & (IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS)) ==
		       (IEEE80211_FCTL_TODS | IEEE80211_FCTL_FROMDS));
	/* qc_included = ((WLAN_FC_GET_TYPE(fc) == IEEE80211_FTYPE_DATA) &&
	 *	       (WLAN_FC_GET_STYPE(fc) & 0x08));
	 */
	/* fixed by David :2006.9.6 */
	qc_included = (WLAN_FC_GET_TYPE(fc) == IEEE80211_FTYPE_DATA) &&
		       (WLAN_FC_GET_STYPE(fc) & 0x80);
	aad_len = 22;
	if (a4_included)
		aad_len += 6;
	if (qc_included) {
		pos = (u8 *)&hdr->addr4;
		if (a4_included)
			pos += 6;
		qc = *pos & 0x0f;
		aad_len += 2;
	}
	/* CCM Initial Block:
	 * Flag (Include authentication header, M=3 (8-octet MIC),
	 *       L=1 (2-octet Dlen))
	 * Nonce: 0x00 | A2 | PN
	 * Dlen
	 */
	b0[0] = 0x59;
	b0[1] = qc;
	memcpy(b0 + 2, hdr->addr2, ETH_ALEN);
	memcpy(b0 + 8, pn, CCMP_PN_LEN);
	b0[14] = (dlen >> 8) & 0xff;
	b0[15] = dlen & 0xff;

	/* AAD:
	 * FC with bits 4..6 and 11..13 masked to zero; 14 is always one
	 * A1 | A2 | A3
	 * SC with bits 4..15 (seq#) masked to zero
	 * A4 (if present)
	 * QC (if present)
	 */
	pos = (u8 *)hdr;
	aad[0] = 0; /* aad_len >> 8 */
	aad[1] = aad_len & 0xff;
	aad[2] = pos[0] & 0x8f;
	aad[3] = pos[1] & 0xc7;
	memcpy(aad + 4, hdr->addr1, 3 * ETH_ALEN);
	pos = (u8 *)&hdr->seq_ctl;
	aad[22] = pos[0] & 0x0f;
	aad[23] = 0; /* all bits masked */
	memset(aad + 24, 0, 8);
	if (a4_included)
		memcpy(aad + 24, hdr->addr4, ETH_ALEN);
	if (qc_included) {
		aad[a4_included ? 30 : 24] = qc;
		/* rest of QC masked */
	}

	/* Start with the first block and AAD */
	ieee80211_ccmp_aes_encrypt(tfm, b0, auth);
	xor_block(auth, aad, AES_BLOCK_LEN);
	ieee80211_ccmp_aes_encrypt(tfm, auth, auth);
	xor_block(auth, &aad[AES_BLOCK_LEN], AES_BLOCK_LEN);
	ieee80211_ccmp_aes_encrypt(tfm, auth, auth);
	b0[0] &= 0x07;
	b0[14] = 0;
	b0[15] = 0;
	ieee80211_ccmp_aes_encrypt(tfm, b0, s0);
}

static int ieee80211_ccmp_encrypt(struct sk_buff *skb, int hdr_len, void *priv)
{
	struct ieee80211_ccmp_data *key = priv;
	int data_len, i;
	u8 *pos;
	struct rtl_80211_hdr_4addr *hdr;
	struct cb_desc *tcb_desc = (struct cb_desc *)(skb->cb + MAX_DEV_ADDR_SIZE);

	if (skb_headroom(skb) < CCMP_HDR_LEN ||
	    skb_tailroom(skb) < CCMP_MIC_LEN ||
	    skb->len < hdr_len)
		return -1;

	data_len = skb->len - hdr_len;
	pos = skb_push(skb, CCMP_HDR_LEN);
	memmove(pos, pos + CCMP_HDR_LEN, hdr_len);
	pos += hdr_len;
	/* mic = skb_put(skb, CCMP_MIC_LEN); */

	i = CCMP_PN_LEN - 1;
	while (i >= 0) {
		key->tx_pn[i]++;
		if (key->tx_pn[i] != 0)
			break;
		i--;
	}

	*pos++ = key->tx_pn[5];
	*pos++ = key->tx_pn[4];
	*pos++ = 0;
	*pos++ = (key->key_idx << 6) | (1 << 5) /* Ext IV included */;
	*pos++ = key->tx_pn[3];
	*pos++ = key->tx_pn[2];
	*pos++ = key->tx_pn[1];
	*pos++ = key->tx_pn[0];

	hdr = (struct rtl_80211_hdr_4addr *)skb->data;
	if (!tcb_desc->bHwSec) {
		int blocks, last, len;
		u8 *mic;
		u8 *b0 = key->tx_b0;
		u8 *b = key->tx_b;
		u8 *e = key->tx_e;
		u8 *s0 = key->tx_s0;

		/* mic is moved to here by john */
		mic = skb_put(skb, CCMP_MIC_LEN);

		ccmp_init_blocks(key->tfm, hdr, key->tx_pn, data_len, b0, b, s0);

		blocks = DIV_ROUND_UP(data_len, AES_BLOCK_LEN);
		last = data_len % AES_BLOCK_LEN;

		for (i = 1; i <= blocks; i++) {
			len = (i == blocks && last) ? last : AES_BLOCK_LEN;
			/* Authentication */
			xor_block(b, pos, len);
			ieee80211_ccmp_aes_encrypt(key->tfm, b, b);
			/* Encryption, with counter */
			b0[14] = (i >> 8) & 0xff;
			b0[15] = i & 0xff;
			ieee80211_ccmp_aes_encrypt(key->tfm, b0, e);
			xor_block(pos, e, len);
			pos += len;
		}

		for (i = 0; i < CCMP_MIC_LEN; i++)
			mic[i] = b[i] ^ s0[i];
	}
	return 0;
}

static int ieee80211_ccmp_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
{
	struct ieee80211_ccmp_data *key = priv;
	u8 keyidx, *pos;
	struct rtl_80211_hdr_4addr *hdr;
	struct cb_desc *tcb_desc = (struct cb_desc *)(skb->cb + MAX_DEV_ADDR_SIZE);
	u8 pn[6];

	if (skb->len < hdr_len + CCMP_HDR_LEN + CCMP_MIC_LEN) {
		key->dot11RSNAStatsCCMPFormatErrors++;
		return -1;
	}

	hdr = (struct rtl_80211_hdr_4addr *)skb->data;
	pos = skb->data + hdr_len;
	keyidx = pos[3];
	if (!(keyidx & (1 << 5))) {
		if (net_ratelimit()) {
			printk(KERN_DEBUG "CCMP: received packet without ExtIV flag from %pM\n",
				hdr->addr2);
		}
		key->dot11RSNAStatsCCMPFormatErrors++;
		return -2;
	}
	keyidx >>= 6;
	if (key->key_idx != keyidx) {
		printk(KERN_DEBUG "CCMP: RX tkey->key_idx=%d frame keyidx=%d priv=%p\n",
			key->key_idx, keyidx, priv);
		return -6;
	}
	if (!key->key_set) {
		if (net_ratelimit()) {
			printk(KERN_DEBUG "CCMP: received packet from %pM with keyid=%d that does not have a configured key\n",
				hdr->addr2, keyidx);
		}
		return -3;
	}

	pn[0] = pos[7];
	pn[1] = pos[6];
	pn[2] = pos[5];
	pn[3] = pos[4];
	pn[4] = pos[1];
	pn[5] = pos[0];
	pos += 8;

	if (memcmp(pn, key->rx_pn, CCMP_PN_LEN) <= 0) {
		if (net_ratelimit()) {
			printk(KERN_DEBUG "CCMP: replay detected: STA=%pM previous PN %pm received PN %pm\n",
			       hdr->addr2, key->rx_pn, pn);
		}
		key->dot11RSNAStatsCCMPReplays++;
		return -4;
	}
	if (!tcb_desc->bHwSec) {
		size_t data_len = skb->len - hdr_len - CCMP_HDR_LEN - CCMP_MIC_LEN;
		u8 *mic = skb->data + skb->len - CCMP_MIC_LEN;
		u8 *b0 = key->rx_b0;
		u8 *b = key->rx_b;
		u8 *a = key->rx_a;
		int i, blocks, last, len;

		ccmp_init_blocks(key->tfm, hdr, pn, data_len, b0, a, b);
		xor_block(mic, b, CCMP_MIC_LEN);

		blocks = DIV_ROUND_UP(data_len, AES_BLOCK_LEN);
		last = data_len % AES_BLOCK_LEN;

		for (i = 1; i <= blocks; i++) {
			len = (i == blocks && last) ? last : AES_BLOCK_LEN;
			/* Decrypt, with counter */
			b0[14] = (i >> 8) & 0xff;
			b0[15] = i & 0xff;
			ieee80211_ccmp_aes_encrypt(key->tfm, b0, b);
			xor_block(pos, b, len);
			/* Authentication */
			xor_block(a, pos, len);
			ieee80211_ccmp_aes_encrypt(key->tfm, a, a);
			pos += len;
		}

		if (memcmp(mic, a, CCMP_MIC_LEN) != 0) {
			if (net_ratelimit()) {
				printk(KERN_DEBUG "CCMP: decrypt failed: STA=%pM\n",
					hdr->addr2);
			}
			key->dot11RSNAStatsCCMPDecryptErrors++;
			return -5;
		}

		memcpy(key->rx_pn, pn, CCMP_PN_LEN);
	}
	/* Remove hdr and MIC */
	memmove(skb->data + CCMP_HDR_LEN, skb->data, hdr_len);
	skb_pull(skb, CCMP_HDR_LEN);
	skb_trim(skb, skb->len - CCMP_MIC_LEN);

	return keyidx;
}

static int ieee80211_ccmp_set_key(void *key, int len, u8 *seq, void *priv)
{
	struct ieee80211_ccmp_data *data = priv;
	int keyidx;
	struct crypto_tfm *tfm = data->tfm;

	keyidx = data->key_idx;
	memset(data, 0, sizeof(*data));
	data->key_idx = keyidx;
	data->tfm = tfm;
	if (len == CCMP_TK_LEN) {
		memcpy(data->key, key, CCMP_TK_LEN);
		data->key_set = 1;
		if (seq) {
			data->rx_pn[0] = seq[5];
			data->rx_pn[1] = seq[4];
			data->rx_pn[2] = seq[3];
			data->rx_pn[3] = seq[2];
			data->rx_pn[4] = seq[1];
			data->rx_pn[5] = seq[0];
		}
		crypto_cipher_setkey((void *)data->tfm, data->key, CCMP_TK_LEN);
	} else if (len == 0) {
		data->key_set = 0;
	} else {
		return -1;
	}

	return 0;
}

static int ieee80211_ccmp_get_key(void *key, int len, u8 *seq, void *priv)
{
	struct ieee80211_ccmp_data *data = priv;

	if (len < CCMP_TK_LEN)
		return -1;

	if (!data->key_set)
		return 0;
	memcpy(key, data->key, CCMP_TK_LEN);

	if (seq) {
		seq[0] = data->tx_pn[5];
		seq[1] = data->tx_pn[4];
		seq[2] = data->tx_pn[3];
		seq[3] = data->tx_pn[2];
		seq[4] = data->tx_pn[1];
		seq[5] = data->tx_pn[0];
	}

	return CCMP_TK_LEN;
}

static char *ieee80211_ccmp_print_stats(char *p, void *priv)
{
	struct ieee80211_ccmp_data *ccmp = priv;

	p += sprintf(p, "key[%d] alg=CCMP key_set=%d tx_pn=%pm rx_pn=%pm format_errors=%d replays=%d decrypt_errors=%d\n",
		     ccmp->key_idx, ccmp->key_set,
		     ccmp->tx_pn, ccmp->rx_pn,
		     ccmp->dot11RSNAStatsCCMPFormatErrors,
		     ccmp->dot11RSNAStatsCCMPReplays,
		     ccmp->dot11RSNAStatsCCMPDecryptErrors);

	return p;
}

static struct ieee80211_crypto_ops ieee80211_crypt_ccmp = {
	.name			= "CCMP",
	.init			= ieee80211_ccmp_init,
	.deinit			= ieee80211_ccmp_deinit,
	.encrypt_mpdu		= ieee80211_ccmp_encrypt,
	.decrypt_mpdu		= ieee80211_ccmp_decrypt,
	.encrypt_msdu		= NULL,
	.decrypt_msdu		= NULL,
	.set_key		= ieee80211_ccmp_set_key,
	.get_key		= ieee80211_ccmp_get_key,
	.print_stats		= ieee80211_ccmp_print_stats,
	.extra_prefix_len	= CCMP_HDR_LEN,
	.extra_postfix_len	= CCMP_MIC_LEN,
	.owner			= THIS_MODULE,
};

int __init ieee80211_crypto_ccmp_init(void)
{
	return ieee80211_register_crypto_ops(&ieee80211_crypt_ccmp);
}

void __exit ieee80211_crypto_ccmp_exit(void)
{
	ieee80211_unregister_crypto_ops(&ieee80211_crypt_ccmp);
}
Commit	Line	Data
8fc8598e JC	1	/*
	2	* Host AP crypt: host-based CCMP encryption implementation for Host AP driver
	3	*
	4	* Copyright (c) 2003-2004, Jouni Malinen <jkmaline@cc.hut.fi>
	5	*
	6	* This program is free software; you can redistribute it and/or modify
	7	* it under the terms of the GNU General Public License version 2 as
	8	* published by the Free Software Foundation. See README and COPYING for
	9	* more details.
	10	*/
	11
8fc8598e JC	12	#include <linux/module.h>
	13	#include <linux/init.h>
	14	#include <linux/slab.h>
	15	#include <linux/random.h>
	16	#include <linux/skbuff.h>
	17	#include <linux/netdevice.h>
	18	#include <linux/if_ether.h>
	19	#include <linux/if_arp.h>
a6a93bc0	20	#include <linux/string.h>
8fc8598e JC	21	#include <linux/wireless.h>
	22
	23	#include "ieee80211.h"
	24
8fc8598e	25	#include <linux/crypto.h>
8fc8598e	26	#include <linux/scatterlist.h>
8fc8598e JC	27
	28	MODULE_AUTHOR("Jouni Malinen");
	29	MODULE_DESCRIPTION("Host AP crypt: CCMP");
	30	MODULE_LICENSE("GPL");
	31
8fc8598e JC	32	#define AES_BLOCK_LEN 16
	33	#define CCMP_HDR_LEN 8
	34	#define CCMP_MIC_LEN 8
	35	#define CCMP_TK_LEN 16
	36	#define CCMP_PN_LEN 6
	37
	38	struct ieee80211_ccmp_data {
	39	u8 key[CCMP_TK_LEN];
	40	int key_set;
	41
	42	u8 tx_pn[CCMP_PN_LEN];
	43	u8 rx_pn[CCMP_PN_LEN];
	44
	45	u32 dot11RSNAStatsCCMPFormatErrors;
	46	u32 dot11RSNAStatsCCMPReplays;
	47	u32 dot11RSNAStatsCCMPDecryptErrors;
	48
	49	int key_idx;
	50
	51	struct crypto_tfm *tfm;
	52
	53	/* scratch buffers for virt_to_page() (crypto API) */
	54	u8 tx_b0[AES_BLOCK_LEN], tx_b[AES_BLOCK_LEN],
	55	tx_e[AES_BLOCK_LEN], tx_s0[AES_BLOCK_LEN];
	56	u8 rx_b0[AES_BLOCK_LEN], rx_b[AES_BLOCK_LEN], rx_a[AES_BLOCK_LEN];
	57	};
	58
11889e81	59	static void ieee80211_ccmp_aes_encrypt(struct crypto_tfm *tfm,
8fc8598e JC	60	const u8 pt[16], u8 ct[16])
8fc8598e JC	61	{
6560d4b9	62	crypto_cipher_encrypt_one((void *)tfm, ct, pt);
8fc8598e JC	63	}
8fc8598e JC	64
6560d4b9	65	static void *ieee80211_ccmp_init(int key_idx)
8fc8598e JC	66	{
	67	struct ieee80211_ccmp_data *priv;
	68
7a6cb0d5	69	priv = kzalloc(sizeof(*priv), GFP_ATOMIC);
ce53f4fa	70	if (!priv)
8fc8598e	71	goto fail;
8fc8598e JC	72	priv->key_idx = key_idx;
8fc8598e JC	73
0c9fc85c	74	priv->tfm = (void *)crypto_alloc_cipher("aes", 0, CRYPTO_ALG_ASYNC);
8fc8598e	75	if (IS_ERR(priv->tfm)) {
20433972	76	printk(KERN_DEBUG "ieee80211_crypt_ccmp: could not allocate crypto API aes\n");
8fc8598e JC	77	priv->tfm = NULL;
	78	goto fail;
	79	}
f61fb935	80
8fc8598e JC	81	return priv;
	82
	83	fail:
	84	if (priv) {
	85	if (priv->tfm)
6560d4b9	86	crypto_free_cipher((void *)priv->tfm);
8fc8598e JC	87	kfree(priv);
	88	}
	89
	90	return NULL;
	91	}
	92
8fc8598e JC	93	static void ieee80211_ccmp_deinit(void *priv)
	94	{
	95	struct ieee80211_ccmp_data *_priv = priv;
f61fb935	96
8fc8598e	97	if (_priv && _priv->tfm)
6560d4b9	98	crypto_free_cipher((void *)_priv->tfm);
8fc8598e JC	99	kfree(priv);
	100	}
	101
8fc8598e JC	102	static inline void xor_block(u8 b, u8 a, size_t len)
	103	{
	104	int i;
28b70100	105
8fc8598e JC	106	for (i = 0; i < len; i++)
	107	b[i] ^= a[i];
	108	}
	109
8fc8598e	110	static void ccmp_init_blocks(struct crypto_tfm *tfm,
5c2918a5	111	struct rtl_80211_hdr_4addr *hdr,
8fc8598e JC	112	u8 pn, size_t dlen, u8 b0, u8 *auth,
	113	u8 *s0)
	114	{
	115	u8 *pos, qc = 0;
	116	size_t aad_len;
	117	u16 fc;
	118	int a4_included, qc_included;
	119	u8 aad[2 * AES_BLOCK_LEN];
	120
	121	fc = le16_to_cpu(hdr->frame_ctl);
	122	a4_included = ((fc & (IEEE80211_FCTL_TODS \| IEEE80211_FCTL_FROMDS)) ==
	123	(IEEE80211_FCTL_TODS \| IEEE80211_FCTL_FROMDS));
27a98a28	124	/* qc_included = ((WLAN_FC_GET_TYPE(fc) == IEEE80211_FTYPE_DATA) &&
	125	* (WLAN_FC_GET_STYPE(fc) & 0x08));
	126	*/
4d2c47a9	127	/* fixed by David :2006.9.6 */
2060f31a HM	128	qc_included = (WLAN_FC_GET_TYPE(fc) == IEEE80211_FTYPE_DATA) &&
2060f31a HM	129	(WLAN_FC_GET_STYPE(fc) & 0x80);
8fc8598e JC	130	aad_len = 22;
	131	if (a4_included)
	132	aad_len += 6;
	133	if (qc_included) {
5f7a9da0	134	pos = (u8 *)&hdr->addr4;
8fc8598e JC	135	if (a4_included)
	136	pos += 6;
	137	qc = *pos & 0x0f;
	138	aad_len += 2;
	139	}
	140	/* CCM Initial Block:
	141	* Flag (Include authentication header, M=3 (8-octet MIC),
	142	* L=1 (2-octet Dlen))
	143	* Nonce: 0x00 \| A2 \| PN
27a98a28	144	* Dlen
27a98a28	145	*/
8fc8598e JC	146	b0[0] = 0x59;
	147	b0[1] = qc;
	148	memcpy(b0 + 2, hdr->addr2, ETH_ALEN);
	149	memcpy(b0 + 8, pn, CCMP_PN_LEN);
	150	b0[14] = (dlen >> 8) & 0xff;
	151	b0[15] = dlen & 0xff;
	152
	153	/* AAD:
	154	* FC with bits 4..6 and 11..13 masked to zero; 14 is always one
	155	* A1 \| A2 \| A3
	156	* SC with bits 4..15 (seq#) masked to zero
	157	* A4 (if present)
	158	* QC (if present)
	159	*/
5f7a9da0	160	pos = (u8 *)hdr;
8fc8598e JC	161	aad[0] = 0; /* aad_len >> 8 */
	162	aad[1] = aad_len & 0xff;
	163	aad[2] = pos[0] & 0x8f;
	164	aad[3] = pos[1] & 0xc7;
	165	memcpy(aad + 4, hdr->addr1, 3 * ETH_ALEN);
5f7a9da0	166	pos = (u8 *)&hdr->seq_ctl;
8fc8598e JC	167	aad[22] = pos[0] & 0x0f;
	168	aad[23] = 0; /* all bits masked */
	169	memset(aad + 24, 0, 8);
	170	if (a4_included)
	171	memcpy(aad + 24, hdr->addr4, ETH_ALEN);
	172	if (qc_included) {
	173	aad[a4_included ? 30 : 24] = qc;
	174	/* rest of QC masked */
	175	}
	176
	177	/* Start with the first block and AAD */
	178	ieee80211_ccmp_aes_encrypt(tfm, b0, auth);
	179	xor_block(auth, aad, AES_BLOCK_LEN);
	180	ieee80211_ccmp_aes_encrypt(tfm, auth, auth);
	181	xor_block(auth, &aad[AES_BLOCK_LEN], AES_BLOCK_LEN);
	182	ieee80211_ccmp_aes_encrypt(tfm, auth, auth);
	183	b0[0] &= 0x07;
a090189c	184	b0[14] = 0;
a090189c	185	b0[15] = 0;
8fc8598e JC	186	ieee80211_ccmp_aes_encrypt(tfm, b0, s0);
	187	}
	188
8fc8598e JC	189	static int ieee80211_ccmp_encrypt(struct sk_buff skb, int hdr_len, void priv)
	190	{
	191	struct ieee80211_ccmp_data *key = priv;
	192	int data_len, i;
	193	u8 *pos;
5c2918a5	194	struct rtl_80211_hdr_4addr *hdr;
20f896c4	195	struct cb_desc tcb_desc = (struct cb_desc )(skb->cb + MAX_DEV_ADDR_SIZE);
8fc8598e JC	196
	197	if (skb_headroom(skb) < CCMP_HDR_LEN \|\|
	198	skb_tailroom(skb) < CCMP_MIC_LEN \|\|
	199	skb->len < hdr_len)
	200	return -1;
	201
	202	data_len = skb->len - hdr_len;
	203	pos = skb_push(skb, CCMP_HDR_LEN);
	204	memmove(pos, pos + CCMP_HDR_LEN, hdr_len);
	205	pos += hdr_len;
4d2c47a9	206	/* mic = skb_put(skb, CCMP_MIC_LEN); */
8fc8598e JC	207
	208	i = CCMP_PN_LEN - 1;
	209	while (i >= 0) {
	210	key->tx_pn[i]++;
	211	if (key->tx_pn[i] != 0)
	212	break;
	213	i--;
	214	}
	215
	216	*pos++ = key->tx_pn[5];
	217	*pos++ = key->tx_pn[4];
	218	*pos++ = 0;
	219	pos++ = (key->key_idx << 6) \| (1 << 5) / Ext IV included */;
	220	*pos++ = key->tx_pn[3];
	221	*pos++ = key->tx_pn[2];
	222	*pos++ = key->tx_pn[1];
	223	*pos++ = key->tx_pn[0];
	224
5f7a9da0	225	hdr = (struct rtl_80211_hdr_4addr *)skb->data;
d9098c55	226	if (!tcb_desc->bHwSec) {
8fc8598e JC	227	int blocks, last, len;
	228	u8 *mic;
	229	u8 *b0 = key->tx_b0;
	230	u8 *b = key->tx_b;
	231	u8 *e = key->tx_e;
	232	u8 *s0 = key->tx_s0;
	233
4d2c47a9	234	/* mic is moved to here by john */
8fc8598e JC	235	mic = skb_put(skb, CCMP_MIC_LEN);
	236
	237	ccmp_init_blocks(key->tfm, hdr, key->tx_pn, data_len, b0, b, s0);
	238
b1b20e71	239	blocks = DIV_ROUND_UP(data_len, AES_BLOCK_LEN);
8fc8598e JC	240	last = data_len % AES_BLOCK_LEN;
	241
	242	for (i = 1; i <= blocks; i++) {
	243	len = (i == blocks && last) ? last : AES_BLOCK_LEN;
	244	/* Authentication */
	245	xor_block(b, pos, len);
	246	ieee80211_ccmp_aes_encrypt(key->tfm, b, b);
	247	/* Encryption, with counter */
	248	b0[14] = (i >> 8) & 0xff;
	249	b0[15] = i & 0xff;
	250	ieee80211_ccmp_aes_encrypt(key->tfm, b0, e);
	251	xor_block(pos, e, len);
	252	pos += len;
	253	}
	254
	255	for (i = 0; i < CCMP_MIC_LEN; i++)
	256	mic[i] = b[i] ^ s0[i];
	257	}
	258	return 0;
	259	}
	260
8fc8598e JC	261	static int ieee80211_ccmp_decrypt(struct sk_buff skb, int hdr_len, void priv)
	262	{
	263	struct ieee80211_ccmp_data *key = priv;
	264	u8 keyidx, *pos;
5c2918a5	265	struct rtl_80211_hdr_4addr *hdr;
20f896c4	266	struct cb_desc tcb_desc = (struct cb_desc )(skb->cb + MAX_DEV_ADDR_SIZE);
8fc8598e JC	267	u8 pn[6];
	268
	269	if (skb->len < hdr_len + CCMP_HDR_LEN + CCMP_MIC_LEN) {
	270	key->dot11RSNAStatsCCMPFormatErrors++;
	271	return -1;
	272	}
	273
5f7a9da0	274	hdr = (struct rtl_80211_hdr_4addr *)skb->data;
8fc8598e JC	275	pos = skb->data + hdr_len;
	276	keyidx = pos[3];
	277	if (!(keyidx & (1 << 5))) {
	278	if (net_ratelimit()) {
20433972	279	printk(KERN_DEBUG "CCMP: received packet without ExtIV flag from %pM\n",
20433972	280	hdr->addr2);
8fc8598e JC	281	}
	282	key->dot11RSNAStatsCCMPFormatErrors++;
	283	return -2;
	284	}
	285	keyidx >>= 6;
	286	if (key->key_idx != keyidx) {
20433972	287	printk(KERN_DEBUG "CCMP: RX tkey->key_idx=%d frame keyidx=%d priv=%p\n",
20433972	288	key->key_idx, keyidx, priv);
8fc8598e JC	289	return -6;
	290	}
	291	if (!key->key_set) {
	292	if (net_ratelimit()) {
20433972	293	printk(KERN_DEBUG "CCMP: received packet from %pM with keyid=%d that does not have a configured key\n",
20433972	294	hdr->addr2, keyidx);
8fc8598e JC	295	}
	296	return -3;
	297	}
	298
	299	pn[0] = pos[7];
	300	pn[1] = pos[6];
	301	pn[2] = pos[5];
	302	pn[3] = pos[4];
	303	pn[4] = pos[1];
	304	pn[5] = pos[0];
	305	pos += 8;
	306
	307	if (memcmp(pn, key->rx_pn, CCMP_PN_LEN) <= 0) {
	308	if (net_ratelimit()) {
20433972	309	printk(KERN_DEBUG "CCMP: replay detected: STA=%pM previous PN %pm received PN %pm\n",
0ee9f67c	310	hdr->addr2, key->rx_pn, pn);
8fc8598e JC	311	}
	312	key->dot11RSNAStatsCCMPReplays++;
	313	return -4;
	314	}
d9098c55	315	if (!tcb_desc->bHwSec) {
8fc8598e JC	316	size_t data_len = skb->len - hdr_len - CCMP_HDR_LEN - CCMP_MIC_LEN;
	317	u8 *mic = skb->data + skb->len - CCMP_MIC_LEN;
	318	u8 *b0 = key->rx_b0;
	319	u8 *b = key->rx_b;
	320	u8 *a = key->rx_a;
	321	int i, blocks, last, len;
	322
8fc8598e JC	323	ccmp_init_blocks(key->tfm, hdr, pn, data_len, b0, a, b);
	324	xor_block(mic, b, CCMP_MIC_LEN);
	325
b1b20e71	326	blocks = DIV_ROUND_UP(data_len, AES_BLOCK_LEN);
8fc8598e JC	327	last = data_len % AES_BLOCK_LEN;
	328
	329	for (i = 1; i <= blocks; i++) {
	330	len = (i == blocks && last) ? last : AES_BLOCK_LEN;
	331	/* Decrypt, with counter */
	332	b0[14] = (i >> 8) & 0xff;
	333	b0[15] = i & 0xff;
	334	ieee80211_ccmp_aes_encrypt(key->tfm, b0, b);
	335	xor_block(pos, b, len);
	336	/* Authentication */
	337	xor_block(a, pos, len);
	338	ieee80211_ccmp_aes_encrypt(key->tfm, a, a);
	339	pos += len;
	340	}
	341
	342	if (memcmp(mic, a, CCMP_MIC_LEN) != 0) {
	343	if (net_ratelimit()) {
20433972	344	printk(KERN_DEBUG "CCMP: decrypt failed: STA=%pM\n",
20433972	345	hdr->addr2);
8fc8598e JC	346	}
	347	key->dot11RSNAStatsCCMPDecryptErrors++;
	348	return -5;
	349	}
	350
	351	memcpy(key->rx_pn, pn, CCMP_PN_LEN);
	352	}
	353	/* Remove hdr and MIC */
	354	memmove(skb->data + CCMP_HDR_LEN, skb->data, hdr_len);
	355	skb_pull(skb, CCMP_HDR_LEN);
	356	skb_trim(skb, skb->len - CCMP_MIC_LEN);
	357
	358	return keyidx;
	359	}
	360
8fc8598e JC	361	static int ieee80211_ccmp_set_key(void key, int len, u8 seq, void *priv)
	362	{
	363	struct ieee80211_ccmp_data *data = priv;
	364	int keyidx;
	365	struct crypto_tfm *tfm = data->tfm;
	366
	367	keyidx = data->key_idx;
	368	memset(data, 0, sizeof(*data));
	369	data->key_idx = keyidx;
	370	data->tfm = tfm;
	371	if (len == CCMP_TK_LEN) {
	372	memcpy(data->key, key, CCMP_TK_LEN);
	373	data->key_set = 1;
	374	if (seq) {
	375	data->rx_pn[0] = seq[5];
	376	data->rx_pn[1] = seq[4];
	377	data->rx_pn[2] = seq[3];
	378	data->rx_pn[3] = seq[2];
	379	data->rx_pn[4] = seq[1];
	380	data->rx_pn[5] = seq[0];
	381	}
6560d4b9	382	crypto_cipher_setkey((void *)data->tfm, data->key, CCMP_TK_LEN);
64ae44af	383	} else if (len == 0) {
8fc8598e	384	data->key_set = 0;
64ae44af	385	} else {
8fc8598e	386	return -1;
64ae44af	387	}
8fc8598e JC	388
	389	return 0;
	390	}
	391
8fc8598e JC	392	static int ieee80211_ccmp_get_key(void key, int len, u8 seq, void *priv)
	393	{
	394	struct ieee80211_ccmp_data *data = priv;
	395
	396	if (len < CCMP_TK_LEN)
	397	return -1;
	398
	399	if (!data->key_set)
	400	return 0;
	401	memcpy(key, data->key, CCMP_TK_LEN);
	402
	403	if (seq) {
	404	seq[0] = data->tx_pn[5];
	405	seq[1] = data->tx_pn[4];
	406	seq[2] = data->tx_pn[3];
	407	seq[3] = data->tx_pn[2];
	408	seq[4] = data->tx_pn[1];
	409	seq[5] = data->tx_pn[0];
	410	}
	411
	412	return CCMP_TK_LEN;
	413	}
	414
6560d4b9	415	static char ieee80211_ccmp_print_stats(char p, void *priv)
8fc8598e JC	416	{
8fc8598e JC	417	struct ieee80211_ccmp_data *ccmp = priv;
28b70100	418
20433972	419	p += sprintf(p, "key[%d] alg=CCMP key_set=%d tx_pn=%pm rx_pn=%pm format_errors=%d replays=%d decrypt_errors=%d\n",
8fc8598e	420	ccmp->key_idx, ccmp->key_set,
0ee9f67c	421	ccmp->tx_pn, ccmp->rx_pn,
8fc8598e JC	422	ccmp->dot11RSNAStatsCCMPFormatErrors,
	423	ccmp->dot11RSNAStatsCCMPReplays,
	424	ccmp->dot11RSNAStatsCCMPDecryptErrors);
	425
	426	return p;
	427	}
	428
8fc8598e JC	429	static struct ieee80211_crypto_ops ieee80211_crypt_ccmp = {
	430	.name = "CCMP",
	431	.init = ieee80211_ccmp_init,
	432	.deinit = ieee80211_ccmp_deinit,
	433	.encrypt_mpdu = ieee80211_ccmp_encrypt,
	434	.decrypt_mpdu = ieee80211_ccmp_decrypt,
	435	.encrypt_msdu = NULL,
	436	.decrypt_msdu = NULL,
	437	.set_key = ieee80211_ccmp_set_key,
	438	.get_key = ieee80211_ccmp_get_key,
	439	.print_stats = ieee80211_ccmp_print_stats,
	440	.extra_prefix_len = CCMP_HDR_LEN,
	441	.extra_postfix_len = CCMP_MIC_LEN,
	442	.owner = THIS_MODULE,
	443	};
	444
f61fb935	445	int __init ieee80211_crypto_ccmp_init(void)
8fc8598e JC	446	{
	447	return ieee80211_register_crypto_ops(&ieee80211_crypt_ccmp);
	448	}
	449
f61fb935	450	void __exit ieee80211_crypto_ccmp_exit(void)
8fc8598e JC	451	{
	452	ieee80211_unregister_crypto_ops(&ieee80211_crypt_ccmp);
	453	}