hostap_crypt_ccmp.c

IEEE 802.11a/b/g linux2.4/2.6 驱动程序源代码

	kfree(priv);
#ifdef NEW_MODULE_CODE
	module_put(THIS_MODULE);
#else
	MOD_DEC_USE_COUNT;
#endif
}


static inline void xor_block(u8 *b, u8 *a, size_t len)
{
	int i;
	for (i = 0; i < len; i++)
		b[i] ^= a[i];
}


#ifdef HOSTAP_USE_CRYPTO_API
static void ccmp_init_blocks(struct crypto_tfm *tfm,
#else /* HOSTAP_USE_CRYPTO_API */
static void ccmp_init_blocks(u32 tfm[],
#endif /* HOSTAP_USE_CRYPTO_API */
			     struct hostap_ieee80211_hdr *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_control);
	a4_included = ((fc & (WLAN_FC_TODS | WLAN_FC_FROMDS)) ==
		       (WLAN_FC_TODS | WLAN_FC_FROMDS));
	qc_included = ((WLAN_FC_GET_TYPE(fc) == WLAN_FC_TYPE_DATA) &&
		       (WLAN_FC_GET_STYPE(fc) & 0x08));
	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_ctrl;
	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 */
	hostap_ccmp_aes_encrypt(tfm, b0, auth);
	xor_block(auth, aad, AES_BLOCK_LEN);
	hostap_ccmp_aes_encrypt(tfm, auth, auth);
	xor_block(auth, &aad[AES_BLOCK_LEN], AES_BLOCK_LEN);
	hostap_ccmp_aes_encrypt(tfm, auth, auth);
	b0[0] &= 0x07;
	b0[14] = b0[15] = 0;
	hostap_ccmp_aes_encrypt(tfm, b0, s0);
}


static int hostap_ccmp_encrypt(struct sk_buff *skb, int hdr_len, void *priv)
{
	struct hostap_ccmp_data *key = priv;
	int data_len, i, blocks, last, len;
	u8 *pos, *mic;
	struct hostap_ieee80211_hdr *hdr;
	u8 *b0 = key->tx_b0;
	u8 *b = key->tx_b;
	u8 *e = key->tx_e;
	u8 *s0 = key->tx_s0;

	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 hostap_ieee80211_hdr *) skb->data;
	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);
		hostap_ccmp_aes_encrypt(key->tfm, b, b);
		/* Encryption, with counter */
		b0[14] = (i >> 8) & 0xff;
		b0[15] = i & 0xff;
		hostap_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 hostap_ccmp_decrypt(struct sk_buff *skb, int hdr_len, void *priv)
{
	struct hostap_ccmp_data *key = priv;
	u8 keyidx, *pos;
	struct hostap_ieee80211_hdr *hdr;
	u8 *b0 = key->rx_b0;
	u8 *b = key->rx_b;
	u8 *a = key->rx_a;
	u8 pn[6];
	int i, blocks, last, len;
	size_t data_len = skb->len - hdr_len - CCMP_HDR_LEN - CCMP_MIC_LEN;
	u8 *mic = skb->data + skb->len - CCMP_MIC_LEN;

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

	hdr = (struct hostap_ieee80211_hdr *) 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 " MACSTR "\n", MAC2STR(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 " MACSTR
			       " with keyid=%d that does not have a configured"
			       " key\n", MAC2STR(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=" MACSTR
			       " previous PN %02x%02x%02x%02x%02x%02x "
			       "received PN %02x%02x%02x%02x%02x%02x\n",
			       MAC2STR(hdr->addr2), MAC2STR(key->rx_pn),
			       MAC2STR(pn));
		}
		key->dot11RSNAStatsCCMPReplays++;
		return -4;
	}

	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;
		hostap_ccmp_aes_encrypt(key->tfm, b0, b);
		xor_block(pos, b, len);
		/* Authentication */
		xor_block(a, pos, len);
		hostap_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="
			       MACSTR "\n", MAC2STR(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 hostap_ccmp_set_key(void *key, int len, u8 *seq, void *priv)
{
	struct hostap_ccmp_data *data = priv;
	int keyidx;
#ifdef HOSTAP_USE_CRYPTO_API
	struct crypto_tfm *tfm = data->tfm;
#endif /* HOSTAP_USE_CRYPTO_API */

	keyidx = data->key_idx;
	memset(data, 0, sizeof(*data));
	data->key_idx = keyidx;
#ifdef HOSTAP_USE_CRYPTO_API
	data->tfm = tfm;
#endif /* HOSTAP_USE_CRYPTO_API */
	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(data->tfm, data->key, CCMP_TK_LEN);
	} else if (len == 0) {
		data->key_set = 0;
	} else
		return -1;

	return 0;
}


static int hostap_ccmp_get_key(void *key, int len, u8 *seq, void *priv)
{
	struct hostap_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 * hostap_ccmp_print_stats(char *p, void *priv)
{
	struct hostap_ccmp_data *ccmp = priv;
	p += sprintf(p, "key[%d] alg=CCMP key_set=%d "
		     "tx_pn=%02x%02x%02x%02x%02x%02x "
		     "rx_pn=%02x%02x%02x%02x%02x%02x "
		     "format_errors=%d replays=%d decrypt_errors=%d\n",
		     ccmp->key_idx, ccmp->key_set,
		     MAC2STR(ccmp->tx_pn), MAC2STR(ccmp->rx_pn),
		     ccmp->dot11RSNAStatsCCMPFormatErrors,
		     ccmp->dot11RSNAStatsCCMPReplays,
		     ccmp->dot11RSNAStatsCCMPDecryptErrors);

	return p;
}


static struct hostap_crypto_ops hostap_crypt_ccmp = {
	.name			= "CCMP",
	.init			= hostap_ccmp_init,
	.deinit			= hostap_ccmp_deinit,
	.encrypt_mpdu		= hostap_ccmp_encrypt,
	.decrypt_mpdu		= hostap_ccmp_decrypt,
	.encrypt_msdu		= NULL,
	.decrypt_msdu		= NULL,
	.set_key		= hostap_ccmp_set_key,
	.get_key		= hostap_ccmp_get_key,
	.print_stats		= hostap_ccmp_print_stats,
	.extra_prefix_len	= CCMP_HDR_LEN,
	.extra_postfix_len	= CCMP_MIC_LEN
};


static int __init hostap_crypto_ccmp_init(void)
{
	if (hostap_register_crypto_ops(&hostap_crypt_ccmp) < 0)
		return -1;

	return 0;
}


static void __exit hostap_crypto_ccmp_exit(void)
{
	hostap_unregister_crypto_ops(&hostap_crypt_ccmp);
}


module_init(hostap_crypto_ccmp_init);
module_exit(hostap_crypto_ccmp_exit);