[net-next-2.6.git] / drivers / staging / rtl8192e / 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/config.h>
#include <linux/version.h>
#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 <asm/string.h>
#include <linux/wireless.h>

#include "ieee80211.h"

#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0))
#include "rtl_crypto.h"
#else
#include <linux/crypto.h>
#endif

#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
    #include <asm/scatterlist.h>
#else
    #include <linux/scatterlist.h>
#endif
//#include <asm/scatterlist.h>

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

#ifndef OPENSUSE_SLED
#define OPENSUSE_SLED 0
#endif

#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];
};

void ieee80211_ccmp_aes_encrypt(struct crypto_tfm *tfm,
			     const u8 pt[16], u8 ct[16])
{
#if((LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21)) && (!OPENSUSE_SLED))
	struct scatterlist src, dst;

	src.page = virt_to_page(pt);
	src.offset = offset_in_page(pt);
	src.length = AES_BLOCK_LEN;

	dst.page = virt_to_page(ct);
	dst.offset = offset_in_page(ct);
	dst.length = AES_BLOCK_LEN;

	crypto_cipher_encrypt(tfm, &dst, &src, AES_BLOCK_LEN);
#else
	crypto_cipher_encrypt_one((void*)tfm, ct, pt);
#endif
}

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

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

#if((LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21)) && (!OPENSUSE_SLED))
	priv->tfm = crypto_alloc_tfm("aes", 0);
	if (priv->tfm == NULL) {
		printk(KERN_DEBUG "ieee80211_crypt_ccmp: could not allocate "
		       "crypto API aes\n");
		goto fail;
	}
       #else
       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;
	}
	#endif
	return priv;

fail:
	if (priv) {
		if (priv->tfm)
			#if(LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))
			crypto_free_tfm(priv->tfm);
                    #else
			crypto_free_cipher((void*)priv->tfm);
		      #endif
		kfree(priv);
	}

	return NULL;
}


static void ieee80211_ccmp_deinit(void *priv)
{
	struct ieee80211_ccmp_data *_priv = priv;
	if (_priv && _priv->tfm)
#if(LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))
		crypto_free_tfm(_priv->tfm);
#else
		crypto_free_cipher((void*)_priv->tfm);
#endif
	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 ieee80211_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] = 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 ieee80211_hdr_4addr *hdr;
	cb_desc *tcb_desc = (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 ieee80211_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 = (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);
			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 ieee80211_hdr_4addr *hdr;
	cb_desc *tcb_desc = (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 ieee80211_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 = (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;
			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;
	int i;

	p += sprintf(p, "key[%d] alg=CCMP key_set=%d tx_pn=",
		     ccmp->key_idx, ccmp->key_set);

	for (i = 0; i < ARRAY_SIZE(ccmp->tx_pn); i++)
		p += sprintf(p, "%02x", ccmp->tx_pn[i]);

	sprintf(p, " rx_pn=");
	for (i = 0; i < ARRAY_SIZE(ccmp->rx_pn); i++)
		p += sprintf(p, "%02x", ccmp->tx_pn[i]);

	p += sprintf(p, " format_errors=%d replays=%d decrypt_errors=%d\n",
		     ccmp->dot11RSNAStatsCCMPFormatErrors,
		     ccmp->dot11RSNAStatsCCMPReplays,
		     ccmp->dot11RSNAStatsCCMPDecryptErrors);

	return p;
}

void ieee80211_ccmp_null(void)
{
//    printk("============>%s()\n", __FUNCTION__);
	return;
}

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 ieee80211_crypto_ccmp_exit(void)
{
	ieee80211_unregister_crypto_ops(&ieee80211_crypt_ccmp);
}

#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,5,0))
//EXPORT_SYMBOL(ieee80211_ccmp_null);
#else
EXPORT_SYMBOL_NOVERS(ieee80211_ccmp_null);
#endif

//module_init(ieee80211_crypto_ccmp_init);
//module_exit(ieee80211_crypto_ccmp_exit);
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
	12	//#include <linux/config.h>
	13	#include <linux/version.h>
	14	#include <linux/module.h>
	15	#include <linux/init.h>
	16	#include <linux/slab.h>
	17	#include <linux/random.h>
	18	#include <linux/skbuff.h>
	19	#include <linux/netdevice.h>
	20	#include <linux/if_ether.h>
	21	#include <linux/if_arp.h>
	22	#include <asm/string.h>
	23	#include <linux/wireless.h>
	24
	25	#include "ieee80211.h"
	26
	27	#if (LINUX_VERSION_CODE < KERNEL_VERSION(2,5,0))
	28	#include "rtl_crypto.h"
	29	#else
	30	#include <linux/crypto.h>
	31	#endif
	32
	33	#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,20)
	34	#include <asm/scatterlist.h>
	35	#else
	36	#include <linux/scatterlist.h>
	37	#endif
	38	//#include <asm/scatterlist.h>
	39
	40	MODULE_AUTHOR("Jouni Malinen");
	41	MODULE_DESCRIPTION("Host AP crypt: CCMP");
	42	MODULE_LICENSE("GPL");
	43
	44	#ifndef OPENSUSE_SLED
	45	#define OPENSUSE_SLED 0
	46	#endif
	47
	48	#define AES_BLOCK_LEN 16
	49	#define CCMP_HDR_LEN 8
	50	#define CCMP_MIC_LEN 8
	51	#define CCMP_TK_LEN 16
	52	#define CCMP_PN_LEN 6
	53
	54	struct ieee80211_ccmp_data {
	55	u8 key[CCMP_TK_LEN];
	56	int key_set;
	57
	58	u8 tx_pn[CCMP_PN_LEN];
	59	u8 rx_pn[CCMP_PN_LEN];
	60
	61	u32 dot11RSNAStatsCCMPFormatErrors;
	62	u32 dot11RSNAStatsCCMPReplays;
	63	u32 dot11RSNAStatsCCMPDecryptErrors;
	64
65	int key_idx;
66
67	struct crypto_tfm *tfm;
68
69	/* scratch buffers for virt_to_page() (crypto API) */
70	u8 tx_b0[AES_BLOCK_LEN], tx_b[AES_BLOCK_LEN],
71	tx_e[AES_BLOCK_LEN], tx_s0[AES_BLOCK_LEN];
72	u8 rx_b0[AES_BLOCK_LEN], rx_b[AES_BLOCK_LEN], rx_a[AES_BLOCK_LEN];
73	};
74
75	void ieee80211_ccmp_aes_encrypt(struct crypto_tfm *tfm,
76	const u8 pt[16], u8 ct[16])
77	{
78	#if((LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21)) && (!OPENSUSE_SLED))
79	struct scatterlist src, dst;
80
81	src.page = virt_to_page(pt);
82	src.offset = offset_in_page(pt);
83	src.length = AES_BLOCK_LEN;
84
85	dst.page = virt_to_page(ct);
86	dst.offset = offset_in_page(ct);
87	dst.length = AES_BLOCK_LEN;
88
89	crypto_cipher_encrypt(tfm, &dst, &src, AES_BLOCK_LEN);
90	#else
91	crypto_cipher_encrypt_one((void*)tfm, ct, pt);
92	#endif
93	}
94
95	static void * ieee80211_ccmp_init(int key_idx)
96	{
97	struct ieee80211_ccmp_data *priv;
98
99	priv = kmalloc(sizeof(*priv), GFP_ATOMIC);
100	if (priv == NULL)
101	goto fail;
102	memset(priv, 0, sizeof(*priv));
103	priv->key_idx = key_idx;
104
105	#if((LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21)) && (!OPENSUSE_SLED))
106	priv->tfm = crypto_alloc_tfm("aes", 0);
107	if (priv->tfm == NULL) {
108	printk(KERN_DEBUG "ieee80211_crypt_ccmp: could not allocate "
109	"crypto API aes\n");
110	goto fail;
111	}
112	#else
113	priv->tfm = (void*)crypto_alloc_cipher("aes", 0, CRYPTO_ALG_ASYNC);
114	if (IS_ERR(priv->tfm)) {
115	printk(KERN_DEBUG "ieee80211_crypt_ccmp: could not allocate "
116	"crypto API aes\n");
117	priv->tfm = NULL;
118	goto fail;
119	}
120	#endif
121	return priv;
122
123	fail:
124	if (priv) {
125	if (priv->tfm)
126	#if(LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))
127	crypto_free_tfm(priv->tfm);
128	#else
129	crypto_free_cipher((void*)priv->tfm);
130	#endif
131	kfree(priv);
132	}
133
134	return NULL;
135	}
136
137
138	static void ieee80211_ccmp_deinit(void *priv)
139	{
140	struct ieee80211_ccmp_data *_priv = priv;
141	if (_priv && _priv->tfm)
142	#if(LINUX_VERSION_CODE < KERNEL_VERSION(2,6,21))
143	crypto_free_tfm(_priv->tfm);
144	#else
145	crypto_free_cipher((void*)_priv->tfm);
146	#endif
147	kfree(priv);
148	}
149
150
151	static inline void xor_block(u8 b, u8 a, size_t len)
152	{
153	int i;
154	for (i = 0; i < len; i++)
155	b[i] ^= a[i];
156	}
157
158
159
160	static void ccmp_init_blocks(struct crypto_tfm *tfm,
161	struct ieee80211_hdr_4addr *hdr,
162	u8 pn, size_t dlen, u8 b0, u8 *auth,
163	u8 *s0)
164	{
165	u8 *pos, qc = 0;
166	size_t aad_len;
167	u16 fc;
168	int a4_included, qc_included;
169	u8 aad[2 * AES_BLOCK_LEN];
170
171	fc = le16_to_cpu(hdr->frame_ctl);
172	a4_included = ((fc & (IEEE80211_FCTL_TODS \| IEEE80211_FCTL_FROMDS)) ==
173	(IEEE80211_FCTL_TODS \| IEEE80211_FCTL_FROMDS));
174	/*
175	qc_included = ((WLAN_FC_GET_TYPE(fc) == IEEE80211_FTYPE_DATA) &&
176	(WLAN_FC_GET_STYPE(fc) & 0x08));
177	*/
178	// fixed by David :2006.9.6
179	qc_included = ((WLAN_FC_GET_TYPE(fc) == IEEE80211_FTYPE_DATA) &&
180	(WLAN_FC_GET_STYPE(fc) & 0x80));
181	aad_len = 22;
182	if (a4_included)
183	aad_len += 6;
184	if (qc_included) {
185	pos = (u8 *) &hdr->addr4;
186	if (a4_included)
187	pos += 6;
188	qc = *pos & 0x0f;
189	aad_len += 2;
190	}
191	/* CCM Initial Block:
192	* Flag (Include authentication header, M=3 (8-octet MIC),
193	* L=1 (2-octet Dlen))
194	* Nonce: 0x00 \| A2 \| PN
195	* Dlen */
196	b0[0] = 0x59;
197	b0[1] = qc;
198	memcpy(b0 + 2, hdr->addr2, ETH_ALEN);
199	memcpy(b0 + 8, pn, CCMP_PN_LEN);
200	b0[14] = (dlen >> 8) & 0xff;
201	b0[15] = dlen & 0xff;
202
203	/* AAD:
204	* FC with bits 4..6 and 11..13 masked to zero; 14 is always one
205	* A1 \| A2 \| A3
206	* SC with bits 4..15 (seq#) masked to zero
207	* A4 (if present)
208	* QC (if present)
209	*/
210	pos = (u8 *) hdr;
211	aad[0] = 0; /* aad_len >> 8 */
212	aad[1] = aad_len & 0xff;
213	aad[2] = pos[0] & 0x8f;
214	aad[3] = pos[1] & 0xc7;
215	memcpy(aad + 4, hdr->addr1, 3 * ETH_ALEN);
216	pos = (u8 *) &hdr->seq_ctl;
217	aad[22] = pos[0] & 0x0f;
218	aad[23] = 0; /* all bits masked */
219	memset(aad + 24, 0, 8);
220	if (a4_included)
221	memcpy(aad + 24, hdr->addr4, ETH_ALEN);
222	if (qc_included) {
223	aad[a4_included ? 30 : 24] = qc;
224	/* rest of QC masked */
225	}
226
227	/* Start with the first block and AAD */
228	ieee80211_ccmp_aes_encrypt(tfm, b0, auth);
229	xor_block(auth, aad, AES_BLOCK_LEN);
230	ieee80211_ccmp_aes_encrypt(tfm, auth, auth);
231	xor_block(auth, &aad[AES_BLOCK_LEN], AES_BLOCK_LEN);
232	ieee80211_ccmp_aes_encrypt(tfm, auth, auth);
233	b0[0] &= 0x07;
234	b0[14] = b0[15] = 0;
235	ieee80211_ccmp_aes_encrypt(tfm, b0, s0);
236	}
237
238
239
240	static int ieee80211_ccmp_encrypt(struct sk_buff skb, int hdr_len, void priv)
241	{
242	struct ieee80211_ccmp_data *key = priv;
243	int data_len, i;
244	u8 *pos;
245	struct ieee80211_hdr_4addr *hdr;
246	cb_desc tcb_desc = (cb_desc )(skb->cb + MAX_DEV_ADDR_SIZE);
247
248	if (skb_headroom(skb) < CCMP_HDR_LEN \|\|
249	skb_tailroom(skb) < CCMP_MIC_LEN \|\|
250	skb->len < hdr_len)
251	return -1;
252
253	data_len = skb->len - hdr_len;
254	pos = skb_push(skb, CCMP_HDR_LEN);
255	memmove(pos, pos + CCMP_HDR_LEN, hdr_len);
256	pos += hdr_len;
257	// mic = skb_put(skb, CCMP_MIC_LEN);
258
259	i = CCMP_PN_LEN - 1;
260	while (i >= 0) {
261	key->tx_pn[i]++;
262	if (key->tx_pn[i] != 0)
263	break;
264	i--;
265	}
266
267	*pos++ = key->tx_pn[5];
268	*pos++ = key->tx_pn[4];
269	*pos++ = 0;
270	pos++ = (key->key_idx << 6) \| (1 << 5) / Ext IV included */;
271	*pos++ = key->tx_pn[3];
272	*pos++ = key->tx_pn[2];
273	*pos++ = key->tx_pn[1];
274	*pos++ = key->tx_pn[0];
275
276
277	hdr = (struct ieee80211_hdr_4addr *) skb->data;
278	if (!tcb_desc->bHwSec)
279	{
280	int blocks, last, len;
281	u8 *mic;
282	u8 *b0 = key->tx_b0;
283	u8 *b = key->tx_b;
284	u8 *e = key->tx_e;
285	u8 *s0 = key->tx_s0;
286
287	//mic is moved to here by john
288	mic = skb_put(skb, CCMP_MIC_LEN);
289
290	ccmp_init_blocks(key->tfm, hdr, key->tx_pn, data_len, b0, b, s0);
291
292	blocks = (data_len + AES_BLOCK_LEN - 1) / AES_BLOCK_LEN;
293	last = data_len % AES_BLOCK_LEN;
294
295	for (i = 1; i <= blocks; i++) {
296	len = (i == blocks && last) ? last : AES_BLOCK_LEN;
297	/* Authentication */
298	xor_block(b, pos, len);
299	ieee80211_ccmp_aes_encrypt(key->tfm, b, b);
300	/* Encryption, with counter */
301	b0[14] = (i >> 8) & 0xff;
302	b0[15] = i & 0xff;
303	ieee80211_ccmp_aes_encrypt(key->tfm, b0, e);
304	xor_block(pos, e, len);
305	pos += len;
306	}
307
308	for (i = 0; i < CCMP_MIC_LEN; i++)
309	mic[i] = b[i] ^ s0[i];
310	}
311	return 0;
312	}
313
314
315	static int ieee80211_ccmp_decrypt(struct sk_buff skb, int hdr_len, void priv)
316	{
317	struct ieee80211_ccmp_data *key = priv;
318	u8 keyidx, *pos;
319	struct ieee80211_hdr_4addr *hdr;
320	cb_desc tcb_desc = (cb_desc )(skb->cb + MAX_DEV_ADDR_SIZE);
321	u8 pn[6];
322
323	if (skb->len < hdr_len + CCMP_HDR_LEN + CCMP_MIC_LEN) {
324	key->dot11RSNAStatsCCMPFormatErrors++;
325	return -1;
326	}
327
328	hdr = (struct ieee80211_hdr_4addr *) skb->data;
329	pos = skb->data + hdr_len;
330	keyidx = pos[3];
331	if (!(keyidx & (1 << 5))) {
332	if (net_ratelimit()) {
333	printk(KERN_DEBUG "CCMP: received packet without ExtIV"
0ee9f67c	334	" flag from %pM\n", hdr->addr2);
ecdfa446 GKH	335	}
	336	key->dot11RSNAStatsCCMPFormatErrors++;
	337	return -2;
	338	}
	339	keyidx >>= 6;
	340	if (key->key_idx != keyidx) {
	341	printk(KERN_DEBUG "CCMP: RX tkey->key_idx=%d frame "
	342	"keyidx=%d priv=%p\n", key->key_idx, keyidx, priv);
	343	return -6;
	344	}
	345	if (!key->key_set) {
	346	if (net_ratelimit()) {
0ee9f67c	347	printk(KERN_DEBUG "CCMP: received packet from %pM"
ecdfa446	348	" with keyid=%d that does not have a configured"
0ee9f67c	349	" key\n", hdr->addr2, keyidx);
ecdfa446 GKH	350	}
	351	return -3;
	352	}
	353
	354	pn[0] = pos[7];
	355	pn[1] = pos[6];
	356	pn[2] = pos[5];
	357	pn[3] = pos[4];
	358	pn[4] = pos[1];
	359	pn[5] = pos[0];
	360	pos += 8;
	361
	362	if (memcmp(pn, key->rx_pn, CCMP_PN_LEN) <= 0) {
	363	if (net_ratelimit()) {
65a43784	364	//printk(KERN_DEBUG "CCMP: replay detected: STA=%pM"
	365	// " previous PN %pm received PN %pm\n",
	366	// hdr->addr2, key->rx_pn, pn);
ecdfa446 GKH	367	}
	368	key->dot11RSNAStatsCCMPReplays++;
	369	return -4;
	370	}
	371	if (!tcb_desc->bHwSec)
	372	{
	373	size_t data_len = skb->len - hdr_len - CCMP_HDR_LEN - CCMP_MIC_LEN;
	374	u8 *mic = skb->data + skb->len - CCMP_MIC_LEN;
	375	u8 *b0 = key->rx_b0;
	376	u8 *b = key->rx_b;
	377	u8 *a = key->rx_a;
	378	int i, blocks, last, len;
	379
	380
	381	ccmp_init_blocks(key->tfm, hdr, pn, data_len, b0, a, b);
	382	xor_block(mic, b, CCMP_MIC_LEN);
	383
	384	blocks = (data_len + AES_BLOCK_LEN - 1) / AES_BLOCK_LEN;
	385	last = data_len % AES_BLOCK_LEN;
	386
	387	for (i = 1; i <= blocks; i++) {
	388	len = (i == blocks && last) ? last : AES_BLOCK_LEN;
	389	/* Decrypt, with counter */
	390	b0[14] = (i >> 8) & 0xff;
	391	b0[15] = i & 0xff;
	392	ieee80211_ccmp_aes_encrypt(key->tfm, b0, b);
	393	xor_block(pos, b, len);
	394	/* Authentication */
	395	xor_block(a, pos, len);
	396	ieee80211_ccmp_aes_encrypt(key->tfm, a, a);
	397	pos += len;
	398	}
	399
	400	if (memcmp(mic, a, CCMP_MIC_LEN) != 0) {
	401	if (net_ratelimit()) {
	402	printk(KERN_DEBUG "CCMP: decrypt failed: STA="
0ee9f67c	403	"%pM\n", hdr->addr2);
ecdfa446 GKH	404	}
	405	key->dot11RSNAStatsCCMPDecryptErrors++;
	406	return -5;
	407	}
	408
	409	memcpy(key->rx_pn, pn, CCMP_PN_LEN);
	410	}
	411	/* Remove hdr and MIC */
	412	memmove(skb->data + CCMP_HDR_LEN, skb->data, hdr_len);
	413	skb_pull(skb, CCMP_HDR_LEN);
	414	skb_trim(skb, skb->len - CCMP_MIC_LEN);
	415
	416	return keyidx;
	417	}
	418
	419
	420	static int ieee80211_ccmp_set_key(void key, int len, u8 seq, void *priv)
	421	{
	422	struct ieee80211_ccmp_data *data = priv;
	423	int keyidx;
	424	struct crypto_tfm *tfm = data->tfm;
	425
	426	keyidx = data->key_idx;
	427	memset(data, 0, sizeof(*data));
	428	data->key_idx = keyidx;
	429	data->tfm = tfm;
	430	if (len == CCMP_TK_LEN) {
	431	memcpy(data->key, key, CCMP_TK_LEN);
	432	data->key_set = 1;
	433	if (seq) {
	434	data->rx_pn[0] = seq[5];
	435	data->rx_pn[1] = seq[4];
	436	data->rx_pn[2] = seq[3];
	437	data->rx_pn[3] = seq[2];
	438	data->rx_pn[4] = seq[1];
	439	data->rx_pn[5] = seq[0];
	440	}
	441	crypto_cipher_setkey((void*)data->tfm, data->key, CCMP_TK_LEN);
	442	} else if (len == 0)
	443	data->key_set = 0;
	444	else
	445	return -1;
	446
	447	return 0;
	448	}
	449
	450
	451	static int ieee80211_ccmp_get_key(void key, int len, u8 seq, void *priv)
	452	{
	453	struct ieee80211_ccmp_data *data = priv;
	454
	455	if (len < CCMP_TK_LEN)
	456	return -1;
	457
	458	if (!data->key_set)
	459	return 0;
	460	memcpy(key, data->key, CCMP_TK_LEN);
	461
	462	if (seq) {
	463	seq[0] = data->tx_pn[5];
	464	seq[1] = data->tx_pn[4];
	465	seq[2] = data->tx_pn[3];
	466	seq[3] = data->tx_pn[2];
	467	seq[4] = data->tx_pn[1];
468	seq[5] = data->tx_pn[0];
469	}
470
471	return CCMP_TK_LEN;
472	}
473
474
475	static char * ieee80211_ccmp_print_stats(char p, void priv)
476	{
477	struct ieee80211_ccmp_data *ccmp = priv;
9bfafe82 SH	478	int i;
	479
	480	p += sprintf(p, "key[%d] alg=CCMP key_set=%d tx_pn=",
	481	ccmp->key_idx, ccmp->key_set);
	482
	483	for (i = 0; i < ARRAY_SIZE(ccmp->tx_pn); i++)
	484	p += sprintf(p, "%02x", ccmp->tx_pn[i]);
	485
	486	sprintf(p, " rx_pn=");
	487	for (i = 0; i < ARRAY_SIZE(ccmp->rx_pn); i++)
	488	p += sprintf(p, "%02x", ccmp->tx_pn[i]);
	489
	490	p += sprintf(p, " format_errors=%d replays=%d decrypt_errors=%d\n",
ecdfa446 GKH	491	ccmp->dot11RSNAStatsCCMPFormatErrors,
	492	ccmp->dot11RSNAStatsCCMPReplays,
	493	ccmp->dot11RSNAStatsCCMPDecryptErrors);
	494
	495	return p;
	496	}
	497
	498	void ieee80211_ccmp_null(void)
	499	{
	500	// printk("============>%s()\n", __FUNCTION__);
	501	return;
	502	}
	503
	504	static struct ieee80211_crypto_ops ieee80211_crypt_ccmp = {
	505	.name = "CCMP",
	506	.init = ieee80211_ccmp_init,
	507	.deinit = ieee80211_ccmp_deinit,
	508	.encrypt_mpdu = ieee80211_ccmp_encrypt,
	509	.decrypt_mpdu = ieee80211_ccmp_decrypt,
	510	.encrypt_msdu = NULL,
	511	.decrypt_msdu = NULL,
	512	.set_key = ieee80211_ccmp_set_key,
	513	.get_key = ieee80211_ccmp_get_key,
	514	.print_stats = ieee80211_ccmp_print_stats,
	515	.extra_prefix_len = CCMP_HDR_LEN,
	516	.extra_postfix_len = CCMP_MIC_LEN,
	517	.owner = THIS_MODULE,
	518	};
	519
	520
	521	int __init ieee80211_crypto_ccmp_init(void)
	522	{
	523	return ieee80211_register_crypto_ops(&ieee80211_crypt_ccmp);
	524	}
	525
	526
27b93f80	527	void ieee80211_crypto_ccmp_exit(void)
ecdfa446 GKH	528	{
	529	ieee80211_unregister_crypto_ops(&ieee80211_crypt_ccmp);
	530	}
	531
	532	#if (LINUX_VERSION_CODE > KERNEL_VERSION(2,5,0))
	533	//EXPORT_SYMBOL(ieee80211_ccmp_null);
	534	#else
	535	EXPORT_SYMBOL_NOVERS(ieee80211_ccmp_null);
	536	#endif
	537
	538	//module_init(ieee80211_crypto_ccmp_init);
	539	//module_exit(ieee80211_crypto_ccmp_exit);