[deliverable/linux.git] / drivers / staging / rtl8192e / rtllib_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 "rtllib.h"

#include <linux/crypto.h>

#include <linux/scatterlist.h>

#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 rtllib_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 rtllib_ccmp_aes_encrypt(struct crypto_tfm *tfm,
			     const u8 pt[16], u8 ct[16])
{
	crypto_cipher_encrypt_one((void *)tfm, ct, pt);
}

static void *rtllib_ccmp_init(int key_idx)
{
	struct rtllib_ccmp_data *priv;

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

	priv->tfm = (void *)crypto_alloc_cipher("aes", 0, CRYPTO_ALG_ASYNC);
	if (IS_ERR(priv->tfm)) {
		pr_debug("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 rtllib_ccmp_deinit(void *priv)
{
	struct rtllib_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 rtllib_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 & (RTLLIB_FCTL_TODS | RTLLIB_FCTL_FROMDS)) ==
		       (RTLLIB_FCTL_TODS | RTLLIB_FCTL_FROMDS));

	qc_included = ((WLAN_FC_GET_TYPE(fc) == RTLLIB_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 */
	rtllib_ccmp_aes_encrypt(tfm, b0, auth);
	xor_block(auth, aad, AES_BLOCK_LEN);
	rtllib_ccmp_aes_encrypt(tfm, auth, auth);
	xor_block(auth, &aad[AES_BLOCK_LEN], AES_BLOCK_LEN);
	rtllib_ccmp_aes_encrypt(tfm, auth, auth);
	b0[0] &= 0x07;
	b0[14] = b0[15] = 0;
	rtllib_ccmp_aes_encrypt(tfm, b0, s0);
}


static int rtllib_ccmp_encrypt(struct sk_buff *skb, int hdr_len, void *priv)
{
	struct rtllib_ccmp_data *key = priv;
	int data_len, i;
	u8 *pos;
	struct rtllib_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;

	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 rtllib_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 = skb_put(skb, CCMP_MIC_LEN);

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

		blocks = (data_len + AES_BLOCK_LEN - 1) / 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);
			rtllib_ccmp_aes_encrypt(key->tfm, b, b);
			/* Encryption, with counter */
			b0[14] = (i >> 8) & 0xff;
			b0[15] = i & 0xff;
			rtllib_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 rtllib_ccmp_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
{
	struct rtllib_ccmp_data *key = priv;
	u8 keyidx, *pos;
	struct rtllib_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 rtllib_hdr_4addr *) skb->data;
	pos = skb->data + hdr_len;
	keyidx = pos[3];
	if (!(keyidx & (1 << 5))) {
		if (net_ratelimit()) {
			pr_debug("CCMP: received packet without ExtIV flag from %pM\n",
				 hdr->addr2);
		}
		key->dot11RSNAStatsCCMPFormatErrors++;
		return -2;
	}
	keyidx >>= 6;
	if (key->key_idx != keyidx) {
		pr_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()) {
			pr_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) {
		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 = (data_len + AES_BLOCK_LEN - 1) / 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;
			rtllib_ccmp_aes_encrypt(key->tfm, b0, b);
			xor_block(pos, b, len);
			/* Authentication */
			xor_block(a, pos, len);
			rtllib_ccmp_aes_encrypt(key->tfm, a, a);
			pos += len;
		}

		if (memcmp(mic, a, CCMP_MIC_LEN) != 0) {
			if (net_ratelimit()) {
				pr_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 rtllib_ccmp_set_key(void *key, int len, u8 *seq, void *priv)
{
	struct rtllib_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 rtllib_ccmp_get_key(void *key, int len, u8 *seq, void *priv)
{
	struct rtllib_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 void rtllib_ccmp_print_stats(struct seq_file *m, void *priv)
{
	struct rtllib_ccmp_data *ccmp = priv;

	seq_printf(m,
		   "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);
}

static struct lib80211_crypto_ops rtllib_crypt_ccmp = {
	.name			= "R-CCMP",
	.init			= rtllib_ccmp_init,
	.deinit			= rtllib_ccmp_deinit,
	.encrypt_mpdu		= rtllib_ccmp_encrypt,
	.decrypt_mpdu		= rtllib_ccmp_decrypt,
	.encrypt_msdu		= NULL,
	.decrypt_msdu		= NULL,
	.set_key		= rtllib_ccmp_set_key,
	.get_key		= rtllib_ccmp_get_key,
	.print_stats		= rtllib_ccmp_print_stats,
	.extra_mpdu_prefix_len = CCMP_HDR_LEN,
	.extra_mpdu_postfix_len = CCMP_MIC_LEN,
	.owner			= THIS_MODULE,
};


static int __init rtllib_crypto_ccmp_init(void)
{
	return lib80211_register_crypto_ops(&rtllib_crypt_ccmp);
}


static void __exit rtllib_crypto_ccmp_exit(void)
{
	lib80211_unregister_crypto_ops(&rtllib_crypt_ccmp);
}

module_init(rtllib_crypto_ccmp_init);
module_exit(rtllib_crypto_ccmp_exit);

MODULE_LICENSE("GPL");
Commit	Line	Data
ecdfa446 GKH	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
ecdfa446 GKH	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>
a44325f9	20	#include <linux/string.h>
ecdfa446	21	#include <linux/wireless.h>
94a79942	22	#include "rtllib.h"
ecdfa446	23
ecdfa446	24	#include <linux/crypto.h>
ecdfa446	25
cb762154	26	#include <linux/scatterlist.h>
ecdfa446 GKH	27
	28	#define AES_BLOCK_LEN 16
	29	#define CCMP_HDR_LEN 8
	30	#define CCMP_MIC_LEN 8
	31	#define CCMP_TK_LEN 16
	32	#define CCMP_PN_LEN 6
	33
94a79942	34	struct rtllib_ccmp_data {
ecdfa446 GKH	35	u8 key[CCMP_TK_LEN];
	36	int key_set;
	37
	38	u8 tx_pn[CCMP_PN_LEN];
	39	u8 rx_pn[CCMP_PN_LEN];
	40
	41	u32 dot11RSNAStatsCCMPFormatErrors;
	42	u32 dot11RSNAStatsCCMPReplays;
	43	u32 dot11RSNAStatsCCMPDecryptErrors;
	44
	45	int key_idx;
	46
	47	struct crypto_tfm *tfm;
	48
	49	/* scratch buffers for virt_to_page() (crypto API) */
	50	u8 tx_b0[AES_BLOCK_LEN], tx_b[AES_BLOCK_LEN],
	51	tx_e[AES_BLOCK_LEN], tx_s0[AES_BLOCK_LEN];
	52	u8 rx_b0[AES_BLOCK_LEN], rx_b[AES_BLOCK_LEN], rx_a[AES_BLOCK_LEN];
	53	};
	54
ec0dc6be	55	static void rtllib_ccmp_aes_encrypt(struct crypto_tfm *tfm,
ecdfa446 GKH	56	const u8 pt[16], u8 ct[16])
ecdfa446 GKH	57	{
a44325f9	58	crypto_cipher_encrypt_one((void *)tfm, ct, pt);
ecdfa446 GKH	59	}
ecdfa446 GKH	60
a44325f9	61	static void *rtllib_ccmp_init(int key_idx)
ecdfa446	62	{
94a79942	63	struct rtllib_ccmp_data *priv;
ecdfa446	64
929fa2a4	65	priv = kzalloc(sizeof(*priv), GFP_ATOMIC);
ecdfa446 GKH	66	if (priv == NULL)
ecdfa446 GKH	67	goto fail;
ecdfa446 GKH	68	priv->key_idx = key_idx;
ecdfa446 GKH	69
a44325f9	70	priv->tfm = (void *)crypto_alloc_cipher("aes", 0, CRYPTO_ALG_ASYNC);
ecdfa446	71	if (IS_ERR(priv->tfm)) {
7bdfaa0a	72	pr_debug("Could not allocate crypto API aes\n");
ecdfa446 GKH	73	priv->tfm = NULL;
	74	goto fail;
	75	}
ecdfa446 GKH	76	return priv;
	77
	78	fail:
	79	if (priv) {
	80	if (priv->tfm)
a44325f9	81	crypto_free_cipher((void *)priv->tfm);
ecdfa446 GKH	82	kfree(priv);
	83	}
	84
	85	return NULL;
	86	}
	87
	88
94a79942	89	static void rtllib_ccmp_deinit(void *priv)
ecdfa446	90	{
94a79942	91	struct rtllib_ccmp_data *_priv = priv;
3a6b70c3	92
ecdfa446	93	if (_priv && _priv->tfm)
a44325f9	94	crypto_free_cipher((void *)_priv->tfm);
ecdfa446 GKH	95	kfree(priv);
	96	}
	97
	98
	99	static inline void xor_block(u8 b, u8 a, size_t len)
	100	{
	101	int i;
3a6b70c3	102
ecdfa446 GKH	103	for (i = 0; i < len; i++)
	104	b[i] ^= a[i];
	105	}
	106
	107
	108
	109	static void ccmp_init_blocks(struct crypto_tfm *tfm,
94a79942	110	struct rtllib_hdr_4addr *hdr,
ecdfa446 GKH	111	u8 pn, size_t dlen, u8 b0, u8 *auth,
	112	u8 *s0)
	113	{
	114	u8 *pos, qc = 0;
	115	size_t aad_len;
	116	u16 fc;
	117	int a4_included, qc_included;
	118	u8 aad[2 * AES_BLOCK_LEN];
	119
	120	fc = le16_to_cpu(hdr->frame_ctl);
94a79942 LF	121	a4_included = ((fc & (RTLLIB_FCTL_TODS \| RTLLIB_FCTL_FROMDS)) ==
94a79942 LF	122	(RTLLIB_FCTL_TODS \| RTLLIB_FCTL_FROMDS));
8852112e	123
94a79942	124	qc_included = ((WLAN_FC_GET_TYPE(fc) == RTLLIB_FTYPE_DATA) &&
ecdfa446 GKH	125	(WLAN_FC_GET_STYPE(fc) & 0x80));
	126	aad_len = 22;
	127	if (a4_included)
	128	aad_len += 6;
	129	if (qc_included) {
	130	pos = (u8 *) &hdr->addr4;
	131	if (a4_included)
	132	pos += 6;
	133	qc = *pos & 0x0f;
	134	aad_len += 2;
	135	}
	136	/* CCM Initial Block:
	137	* Flag (Include authentication header, M=3 (8-octet MIC),
	138	* L=1 (2-octet Dlen))
	139	* Nonce: 0x00 \| A2 \| PN
14b40d92 MK	140	* Dlen
14b40d92 MK	141	*/
ecdfa446 GKH	142	b0[0] = 0x59;
	143	b0[1] = qc;
	144	memcpy(b0 + 2, hdr->addr2, ETH_ALEN);
	145	memcpy(b0 + 8, pn, CCMP_PN_LEN);
	146	b0[14] = (dlen >> 8) & 0xff;
	147	b0[15] = dlen & 0xff;
	148
	149	/* AAD:
	150	* FC with bits 4..6 and 11..13 masked to zero; 14 is always one
	151	* A1 \| A2 \| A3
	152	* SC with bits 4..15 (seq#) masked to zero
	153	* A4 (if present)
	154	* QC (if present)
	155	*/
	156	pos = (u8 *) hdr;
	157	aad[0] = 0; /* aad_len >> 8 */
	158	aad[1] = aad_len & 0xff;
	159	aad[2] = pos[0] & 0x8f;
	160	aad[3] = pos[1] & 0xc7;
	161	memcpy(aad + 4, hdr->addr1, 3 * ETH_ALEN);
	162	pos = (u8 *) &hdr->seq_ctl;
	163	aad[22] = pos[0] & 0x0f;
	164	aad[23] = 0; /* all bits masked */
	165	memset(aad + 24, 0, 8);
	166	if (a4_included)
	167	memcpy(aad + 24, hdr->addr4, ETH_ALEN);
	168	if (qc_included) {
	169	aad[a4_included ? 30 : 24] = qc;
	170	/* rest of QC masked */
	171	}
	172
	173	/* Start with the first block and AAD */
94a79942	174	rtllib_ccmp_aes_encrypt(tfm, b0, auth);
ecdfa446	175	xor_block(auth, aad, AES_BLOCK_LEN);
94a79942	176	rtllib_ccmp_aes_encrypt(tfm, auth, auth);
ecdfa446	177	xor_block(auth, &aad[AES_BLOCK_LEN], AES_BLOCK_LEN);
94a79942	178	rtllib_ccmp_aes_encrypt(tfm, auth, auth);
ecdfa446 GKH	179	b0[0] &= 0x07;
ecdfa446 GKH	180	b0[14] = b0[15] = 0;
94a79942	181	rtllib_ccmp_aes_encrypt(tfm, b0, s0);
ecdfa446 GKH	182	}
	183
	184
	185
94a79942	186	static int rtllib_ccmp_encrypt(struct sk_buff skb, int hdr_len, void priv)
ecdfa446	187	{
94a79942	188	struct rtllib_ccmp_data *key = priv;
ecdfa446 GKH	189	int data_len, i;
ecdfa446 GKH	190	u8 *pos;
94a79942	191	struct rtllib_hdr_4addr *hdr;
a44325f9 LF	192	struct cb_desc tcb_desc = (struct cb_desc )(skb->cb +
a44325f9 LF	193	MAX_DEV_ADDR_SIZE);
ecdfa446 GKH	194	if (skb_headroom(skb) < CCMP_HDR_LEN \|\|
	195	skb_tailroom(skb) < CCMP_MIC_LEN \|\|
	196	skb->len < hdr_len)
	197	return -1;
	198
	199	data_len = skb->len - hdr_len;
	200	pos = skb_push(skb, CCMP_HDR_LEN);
	201	memmove(pos, pos + CCMP_HDR_LEN, hdr_len);
	202	pos += hdr_len;
ecdfa446 GKH	203
	204	i = CCMP_PN_LEN - 1;
	205	while (i >= 0) {
	206	key->tx_pn[i]++;
	207	if (key->tx_pn[i] != 0)
	208	break;
	209	i--;
	210	}
	211
	212	*pos++ = key->tx_pn[5];
	213	*pos++ = key->tx_pn[4];
	214	*pos++ = 0;
	215	pos++ = (key->key_idx << 6) \| (1 << 5) / Ext IV included */;
	216	*pos++ = key->tx_pn[3];
	217	*pos++ = key->tx_pn[2];
	218	*pos++ = key->tx_pn[1];
	219	*pos++ = key->tx_pn[0];
	220
	221
94a79942 LF	222	hdr = (struct rtllib_hdr_4addr *) skb->data;
94a79942 LF	223	if (!tcb_desc->bHwSec) {
ecdfa446 GKH	224	int blocks, last, len;
	225	u8 *mic;
	226	u8 *b0 = key->tx_b0;
	227	u8 *b = key->tx_b;
	228	u8 *e = key->tx_e;
	229	u8 *s0 = key->tx_s0;
	230
ecdfa446 GKH	231	mic = skb_put(skb, CCMP_MIC_LEN);
ecdfa446 GKH	232
a44325f9 LF	233	ccmp_init_blocks(key->tfm, hdr, key->tx_pn, data_len,
a44325f9 LF	234	b0, b, s0);
ecdfa446 GKH	235
	236	blocks = (data_len + AES_BLOCK_LEN - 1) / AES_BLOCK_LEN;
	237	last = data_len % AES_BLOCK_LEN;
	238
	239	for (i = 1; i <= blocks; i++) {
	240	len = (i == blocks && last) ? last : AES_BLOCK_LEN;
	241	/* Authentication */
	242	xor_block(b, pos, len);
94a79942	243	rtllib_ccmp_aes_encrypt(key->tfm, b, b);
ecdfa446 GKH	244	/* Encryption, with counter */
	245	b0[14] = (i >> 8) & 0xff;
	246	b0[15] = i & 0xff;
94a79942	247	rtllib_ccmp_aes_encrypt(key->tfm, b0, e);
ecdfa446 GKH	248	xor_block(pos, e, len);
	249	pos += len;
	250	}
	251
	252	for (i = 0; i < CCMP_MIC_LEN; i++)
	253	mic[i] = b[i] ^ s0[i];
	254	}
	255	return 0;
	256	}
	257
	258
94a79942	259	static int rtllib_ccmp_decrypt(struct sk_buff skb, int hdr_len, void priv)
ecdfa446	260	{
94a79942	261	struct rtllib_ccmp_data *key = priv;
ecdfa446	262	u8 keyidx, *pos;
94a79942	263	struct rtllib_hdr_4addr *hdr;
a44325f9 LF	264	struct cb_desc tcb_desc = (struct cb_desc )(skb->cb +
a44325f9 LF	265	MAX_DEV_ADDR_SIZE);
ecdfa446 GKH	266	u8 pn[6];
	267
	268	if (skb->len < hdr_len + CCMP_HDR_LEN + CCMP_MIC_LEN) {
	269	key->dot11RSNAStatsCCMPFormatErrors++;
	270	return -1;
	271	}
	272
94a79942	273	hdr = (struct rtllib_hdr_4addr *) skb->data;
ecdfa446 GKH	274	pos = skb->data + hdr_len;
	275	keyidx = pos[3];
	276	if (!(keyidx & (1 << 5))) {
	277	if (net_ratelimit()) {
0822339b MK	278	pr_debug("CCMP: received packet without ExtIV flag from %pM\n",
0822339b MK	279	hdr->addr2);
ecdfa446 GKH	280	}
	281	key->dot11RSNAStatsCCMPFormatErrors++;
	282	return -2;
	283	}
	284	keyidx >>= 6;
	285	if (key->key_idx != keyidx) {
0822339b MK	286	pr_debug("CCMP: RX tkey->key_idx=%d frame keyidx=%d priv=%p\n",
0822339b MK	287	key->key_idx, keyidx, priv);
ecdfa446 GKH	288	return -6;
	289	}
	290	if (!key->key_set) {
	291	if (net_ratelimit()) {
0822339b MK	292	pr_debug("CCMP: received packet from %pM with keyid=%d that does not have a configured key\n",
0822339b MK	293	hdr->addr2, keyidx);
ecdfa446 GKH	294	}
	295	return -3;
	296	}
	297
	298	pn[0] = pos[7];
	299	pn[1] = pos[6];
	300	pn[2] = pos[5];
	301	pn[3] = pos[4];
	302	pn[4] = pos[1];
	303	pn[5] = pos[0];
	304	pos += 8;
ecdfa446	305	if (memcmp(pn, key->rx_pn, CCMP_PN_LEN) <= 0) {
ecdfa446 GKH	306	key->dot11RSNAStatsCCMPReplays++;
	307	return -4;
	308	}
94a79942	309	if (!tcb_desc->bHwSec) {
a44325f9 LF	310	size_t data_len = skb->len - hdr_len - CCMP_HDR_LEN -
a44325f9 LF	311	CCMP_MIC_LEN;
ecdfa446 GKH	312	u8 *mic = skb->data + skb->len - CCMP_MIC_LEN;
	313	u8 *b0 = key->rx_b0;
	314	u8 *b = key->rx_b;
	315	u8 *a = key->rx_a;
	316	int i, blocks, last, len;
	317
	318
	319	ccmp_init_blocks(key->tfm, hdr, pn, data_len, b0, a, b);
	320	xor_block(mic, b, CCMP_MIC_LEN);
	321
	322	blocks = (data_len + AES_BLOCK_LEN - 1) / AES_BLOCK_LEN;
	323	last = data_len % AES_BLOCK_LEN;
	324
	325	for (i = 1; i <= blocks; i++) {
	326	len = (i == blocks && last) ? last : AES_BLOCK_LEN;
	327	/* Decrypt, with counter */
	328	b0[14] = (i >> 8) & 0xff;
	329	b0[15] = i & 0xff;
94a79942	330	rtllib_ccmp_aes_encrypt(key->tfm, b0, b);
ecdfa446 GKH	331	xor_block(pos, b, len);
	332	/* Authentication */
	333	xor_block(a, pos, len);
94a79942	334	rtllib_ccmp_aes_encrypt(key->tfm, a, a);
ecdfa446 GKH	335	pos += len;
	336	}
	337
	338	if (memcmp(mic, a, CCMP_MIC_LEN) != 0) {
	339	if (net_ratelimit()) {
0822339b MK	340	pr_debug("CCMP: decrypt failed: STA= %pM\n",
0822339b MK	341	hdr->addr2);
ecdfa446 GKH	342	}
	343	key->dot11RSNAStatsCCMPDecryptErrors++;
	344	return -5;
	345	}
	346
	347	memcpy(key->rx_pn, pn, CCMP_PN_LEN);
	348	}
	349	/* Remove hdr and MIC */
	350	memmove(skb->data + CCMP_HDR_LEN, skb->data, hdr_len);
	351	skb_pull(skb, CCMP_HDR_LEN);
	352	skb_trim(skb, skb->len - CCMP_MIC_LEN);
	353
	354	return keyidx;
	355	}
	356
	357
94a79942	358	static int rtllib_ccmp_set_key(void key, int len, u8 seq, void *priv)
ecdfa446	359	{
94a79942	360	struct rtllib_ccmp_data *data = priv;
ecdfa446 GKH	361	int keyidx;
	362	struct crypto_tfm *tfm = data->tfm;
	363
	364	keyidx = data->key_idx;
	365	memset(data, 0, sizeof(*data));
	366	data->key_idx = keyidx;
	367	data->tfm = tfm;
	368	if (len == CCMP_TK_LEN) {
	369	memcpy(data->key, key, CCMP_TK_LEN);
	370	data->key_set = 1;
	371	if (seq) {
	372	data->rx_pn[0] = seq[5];
	373	data->rx_pn[1] = seq[4];
	374	data->rx_pn[2] = seq[3];
	375	data->rx_pn[3] = seq[2];
	376	data->rx_pn[4] = seq[1];
	377	data->rx_pn[5] = seq[0];
	378	}
a44325f9	379	crypto_cipher_setkey((void *)data->tfm, data->key, CCMP_TK_LEN);
ecdfa446 GKH	380	} else if (len == 0)
	381	data->key_set = 0;
	382	else
	383	return -1;
	384
	385	return 0;
	386	}
	387
	388
94a79942	389	static int rtllib_ccmp_get_key(void key, int len, u8 seq, void *priv)
ecdfa446	390	{
94a79942	391	struct rtllib_ccmp_data *data = priv;
ecdfa446 GKH	392
	393	if (len < CCMP_TK_LEN)
	394	return -1;
	395
	396	if (!data->key_set)
	397	return 0;
	398	memcpy(key, data->key, CCMP_TK_LEN);
	399
	400	if (seq) {
	401	seq[0] = data->tx_pn[5];
	402	seq[1] = data->tx_pn[4];
	403	seq[2] = data->tx_pn[3];
	404	seq[3] = data->tx_pn[2];
	405	seq[4] = data->tx_pn[1];
	406	seq[5] = data->tx_pn[0];
	407	}
	408
	409	return CCMP_TK_LEN;
	410	}
	411
	412
6bbefe86	413	static void rtllib_ccmp_print_stats(struct seq_file m, void priv)
ecdfa446	414	{
94a79942	415	struct rtllib_ccmp_data *ccmp = priv;
3a6b70c3	416
6bbefe86	417	seq_printf(m,
0822339b	418	"key[%d] alg=CCMP key_set=%d tx_pn=%pM rx_pn=%pM format_errors=%d replays=%d decrypt_errors=%d\n",
6bbefe86 DH	419	ccmp->key_idx, ccmp->key_set,
	420	ccmp->tx_pn, ccmp->rx_pn,
	421	ccmp->dot11RSNAStatsCCMPFormatErrors,
	422	ccmp->dot11RSNAStatsCCMPReplays,
	423	ccmp->dot11RSNAStatsCCMPDecryptErrors);
ecdfa446 GKH	424	}
ecdfa446 GKH	425
32c44cb5	426	static struct lib80211_crypto_ops rtllib_crypt_ccmp = {
3b148be0	427	.name = "R-CCMP",
94a79942 LF	428	.init = rtllib_ccmp_init,
	429	.deinit = rtllib_ccmp_deinit,
	430	.encrypt_mpdu = rtllib_ccmp_encrypt,
	431	.decrypt_mpdu = rtllib_ccmp_decrypt,
ecdfa446 GKH	432	.encrypt_msdu = NULL,
ecdfa446 GKH	433	.decrypt_msdu = NULL,
94a79942 LF	434	.set_key = rtllib_ccmp_set_key,
	435	.get_key = rtllib_ccmp_get_key,
	436	.print_stats = rtllib_ccmp_print_stats,
32c44cb5 SM	437	.extra_mpdu_prefix_len = CCMP_HDR_LEN,
32c44cb5 SM	438	.extra_mpdu_postfix_len = CCMP_MIC_LEN,
ecdfa446 GKH	439	.owner = THIS_MODULE,
	440	};
	441
	442
327ca222	443	static int __init rtllib_crypto_ccmp_init(void)
ecdfa446	444	{
3b148be0	445	return lib80211_register_crypto_ops(&rtllib_crypt_ccmp);
ecdfa446 GKH	446	}
	447
	448
327ca222	449	static void __exit rtllib_crypto_ccmp_exit(void)
ecdfa446	450	{
3b148be0	451	lib80211_unregister_crypto_ops(&rtllib_crypt_ccmp);
ecdfa446	452	}
d37e0208 SM	453
	454	module_init(rtllib_crypto_ccmp_init);
	455	module_exit(rtllib_crypto_ccmp_exit);
	456
	457	MODULE_LICENSE("GPL");