ieee80211_crypto_tkip.c

Linux下wifi实现

static __inline u16
_S_(u16 v)
{
	u16 t = Sbox[Hi8(v)];
	return Sbox[Lo8(v)] ^ ((t << 8) | (t >> 8));
}

#define PHASE1_LOOP_COUNT 8

static void
tkip_mixing_phase1(u16 *TTAK, const u8 *TK, const u8 *TA, u32 IV32)
{
	int i, j;

	/* Initialize the 80-bit TTAK from TSC (IV32) and TA[0..5] */
	TTAK[0] = Lo16(IV32);
	TTAK[1] = Hi16(IV32);
	TTAK[2] = Mk16(TA[1], TA[0]);
	TTAK[3] = Mk16(TA[3], TA[2]);
	TTAK[4] = Mk16(TA[5], TA[4]);

	for (i = 0; i < PHASE1_LOOP_COUNT; i++) {
		j = 2 * (i & 1);
		TTAK[0] += _S_(TTAK[4] ^ Mk16(TK[1 + j], TK[0 + j]));
		TTAK[1] += _S_(TTAK[0] ^ Mk16(TK[5 + j], TK[4 + j]));
		TTAK[2] += _S_(TTAK[1] ^ Mk16(TK[9 + j], TK[8 + j]));
		TTAK[3] += _S_(TTAK[2] ^ Mk16(TK[13 + j], TK[12 + j]));
		TTAK[4] += _S_(TTAK[3] ^ Mk16(TK[1 + j], TK[0 + j])) + i;
	}
}

#ifndef _BYTE_ORDER
#error "Don't know native byte order"
#endif

static void
tkip_mixing_phase2(u8 *WEPSeed, const u8 *TK, const u16 *TTAK, u16 IV16)
{
	/* Make temporary area overlap WEP seed so that the final copy can be
	 * avoided on little endian hosts. */
	u16 *PPK = (u16 *) &WEPSeed[4];

	/* Step 1 - make copy of TTAK and bring in TSC */
	PPK[0] = TTAK[0];
	PPK[1] = TTAK[1];
	PPK[2] = TTAK[2];
	PPK[3] = TTAK[3];
	PPK[4] = TTAK[4];
	PPK[5] = TTAK[4] + IV16;

	/* Step 2 - 96-bit bijective mixing using S-box */
	PPK[0] += _S_(PPK[5] ^ Mk16_le((const u16 *) &TK[0]));
	PPK[1] += _S_(PPK[0] ^ Mk16_le((const u16 *) &TK[2]));
	PPK[2] += _S_(PPK[1] ^ Mk16_le((const u16 *) &TK[4]));
	PPK[3] += _S_(PPK[2] ^ Mk16_le((const u16 *) &TK[6]));
	PPK[4] += _S_(PPK[3] ^ Mk16_le((const u16 *) &TK[8]));
	PPK[5] += _S_(PPK[4] ^ Mk16_le((const u16 *) &TK[10]));

	PPK[0] += RotR1(PPK[5] ^ Mk16_le((const u16 *) &TK[12]));
	PPK[1] += RotR1(PPK[0] ^ Mk16_le((const u16 *) &TK[14]));
	PPK[2] += RotR1(PPK[1]);
	PPK[3] += RotR1(PPK[2]);
	PPK[4] += RotR1(PPK[3]);
	PPK[5] += RotR1(PPK[4]);

	/* Step 3 - bring in last of TK bits, assign 24-bit WEP IV value
	 * WEPSeed[0..2] is transmitted as WEP IV */
	WEPSeed[0] = Hi8(IV16);
	WEPSeed[1] = (Hi8(IV16) | 0x20) & 0x7F;
	WEPSeed[2] = Lo8(IV16);
	WEPSeed[3] = Lo8((PPK[5] ^ Mk16_le((const u16 *) &TK[0])) >> 1);

#if _BYTE_ORDER == _BIG_ENDIAN
	{
		int i;
		for (i = 0; i < 6; i++)
			PPK[i] = (PPK[i] << 8) | (PPK[i] >> 8);
	}
#endif
}

static void
wep_encrypt(u8 *key, struct sk_buff *skb0, u_int off, size_t data_len)
{
#define S_SWAP(a,b) do { uint8_t t = S[a]; S[a] = S[b]; S[b] = t; } while(0)
	struct sk_buff *skb = skb0;
	uint32_t i, j, k, crc;
	size_t buflen;
	uint8_t S[256];
	uint8_t *pos, *icv;

	/* Setup RC4 state */
	for (i = 0; i < 256; i++)
		S[i] = i;
	j = 0;
	for (i = 0; i < 256; i++) {
		j = (j + S[i] + key[i & 0x0f]) & 0xff;
		S_SWAP(i, j);
	}

	/* Compute CRC32 over unencrypted data and apply RC4 to data */
	crc = ~0;
	i = j = 0;
	pos = skb->data + off;
	buflen = skb->len - off;
	for (;;) {
		if (buflen > data_len)
			buflen = data_len;
		data_len -= buflen;
		for (k = 0; k < buflen; k++) {
			crc = crc32_table[(crc ^ *pos) & 0xff] ^ (crc >> 8);
			i = (i + 1) & 0xff;
			j = (j + S[i]) & 0xff;
			S_SWAP(i, j);
			*pos++ ^= S[(S[i] + S[j]) & 0xff];
		}
		if (skb->next == NULL) {
			KASSERT(data_len == 0,
				("missing data, data_len %u", (int)data_len));
			break;
		}
		skb = skb->next;
		pos = skb->data;
		buflen = skb->len;
	}
	crc = ~crc;

	icv = skb_put(skb, tkip.ic_trailer);
	/* Append little-endian CRC32 and encrypt it to produce ICV */
	icv[0] = crc;
	icv[1] = crc >> 8;
	icv[2] = crc >> 16;
	icv[3] = crc >> 24;
	for (k = 0; k < IEEE80211_WEP_CRCLEN; k++) {
		i = (i + 1) & 0xff;
		j = (j + S[i]) & 0xff;
		S_SWAP(i, j);
		icv[k] ^= S[(S[i] + S[j]) & 0xff];
	}
#undef S_SWAP
}

static int
wep_decrypt(u8 *key, struct sk_buff *skb, u_int off, size_t data_len)
{
#define S_SWAP(a,b) do { uint8_t t = S[a]; S[a] = S[b]; S[b] = t; } while(0)
	u32 i, j, k, crc;
	u8 S[256];
	u8 *pos, icv[4];
	size_t buflen;

	/* Setup RC4 state */
	for (i = 0; i < 256; i++)
		S[i] = i;
	j = 0;
	for (i = 0; i < 256; i++) {
		j = (j + S[i] + key[i & 0x0f]) & 0xff;
		S_SWAP(i, j);
	}

	/* Apply RC4 to data and compute CRC32 over decrypted data */
	crc = ~0;
	i = j = 0;
	pos = skb->data + off;
	buflen = skb->len - off;
	for (;;) {
		if (buflen > data_len)
			buflen = data_len;
		data_len -= buflen;
		for (k = 0; k < buflen; k++) {
			i = (i + 1) & 0xff;
			j = (j + S[i]) & 0xff;
			S_SWAP(i, j);
			*pos ^= S[(S[i] + S[j]) & 0xff];
			crc = crc32_table[(crc ^ *pos) & 0xff] ^ (crc >> 8);
			pos++;
		}
		if (skb->next == NULL) {
			if (data_len != 0) {
				/* XXX msg? stat? cannot happen? */
				return -1;
			}
			break;
		}
		skb = skb->next;
		pos = skb->data;
		buflen = skb->len;
	}
	crc = ~crc;

	/* Encrypt little-endian CRC32 and verify that it matches with the
	 * received ICV */
	icv[0] = crc;
	icv[1] = crc >> 8;
	icv[2] = crc >> 16;
	icv[3] = crc >> 24;
	for (k = 0; k < 4; k++) {
		i = (i + 1) & 0xff;
		j = (j + S[i]) & 0xff;
		S_SWAP(i, j);
		if ((icv[k] ^ S[(S[i] + S[j]) & 0xff]) != *pos++) {
			/* ICV mismatch - drop frame */
			return -1;
		}
	}
	return 0;
#undef S_SWAP
}


static __inline u32
rotl(u32 val, int bits)
{
	return (val << bits) | (val >> (32 - bits));
}


static __inline u32
rotr(u32 val, int bits)
{
	return (val >> bits) | (val << (32 - bits));
}


static __inline u32
xswap(u32 val)
{
	return ((val & 0x00ff00ff) << 8) | ((val & 0xff00ff00) >> 8);
}


#define michael_block(l, r)	\
do {				\
	r ^= rotl(l, 17);	\
	l += r;			\
	r ^= xswap(l);		\
	l += r;			\
	r ^= rotl(l, 3);	\
	l += r;			\
	r ^= rotr(l, 2);	\
	l += r;			\
} while (0)


static __inline u32
get_le32_split(u8 b0, u8 b1, u8 b2, u8 b3)
{
	return b0 | (b1 << 8) | (b2 << 16) | (b3 << 24);
}

static __inline u32
get_le32(const u8 *p)
{
	return get_le32_split(p[0], p[1], p[2], p[3]);
}


static __inline void
put_le32(u8 *p, u32 v)
{
	p[0] = v;
	p[1] = v >> 8;
	p[2] = v >> 16;
	p[3] = v >> 24;
}

/*
 * Craft pseudo header used to calculate the MIC.
 */
static void
michael_mic_hdr(const struct ieee80211_frame *wh0, u8 hdr[16])
{
	const struct ieee80211_frame_addr4 *wh =
		(const struct ieee80211_frame_addr4 *) wh0;

	switch (wh->i_fc[1] & IEEE80211_FC1_DIR_MASK) {
	case IEEE80211_FC1_DIR_NODS:
		IEEE80211_ADDR_COPY(hdr, wh->i_addr1); /* DA */
		IEEE80211_ADDR_COPY(hdr + IEEE80211_ADDR_LEN, wh->i_addr2);
		break;
	case IEEE80211_FC1_DIR_TODS:
		IEEE80211_ADDR_COPY(hdr, wh->i_addr3); /* DA */
		IEEE80211_ADDR_COPY(hdr + IEEE80211_ADDR_LEN, wh->i_addr2);
		break;
	case IEEE80211_FC1_DIR_FROMDS:
		IEEE80211_ADDR_COPY(hdr, wh->i_addr1); /* DA */
		IEEE80211_ADDR_COPY(hdr + IEEE80211_ADDR_LEN, wh->i_addr3);
		break;
	case IEEE80211_FC1_DIR_DSTODS:
		IEEE80211_ADDR_COPY(hdr, wh->i_addr3); /* DA */
		IEEE80211_ADDR_COPY(hdr + IEEE80211_ADDR_LEN, wh->i_addr4);
		break;
	}

	if (wh->i_fc[0] & IEEE80211_FC0_SUBTYPE_QOS) {
		const struct ieee80211_qosframe *qwh =
			(const struct ieee80211_qosframe *) wh;
		hdr[12] = qwh->i_qos[0] & IEEE80211_QOS_TID;
	} else
		hdr[12] = 0;
	hdr[13] = hdr[14] = hdr[15] = 0; /* reserved */
}

static void
michael_mic(struct tkip_ctx *ctx, const u8 *key,
	struct sk_buff *skb, u_int off, size_t data_len,
	u8 mic[IEEE80211_WEP_MICLEN])
{
	uint8_t hdr[16];
	u32 l, r;
	const uint8_t *data;
	u_int space;

	michael_mic_hdr((struct ieee80211_frame *) skb->data, hdr);

	l = get_le32(key);
	r = get_le32(key + 4);

	/* Michael MIC pseudo header: DA, SA, 3 x 0, Priority */
	l ^= get_le32(hdr);
	michael_block(l, r);
	l ^= get_le32(&hdr[4]);
	michael_block(l, r);
	l ^= get_le32(&hdr[8]);
	michael_block(l, r);
	l ^= get_le32(&hdr[12]);
	michael_block(l, r);

	/* first buffer has special handling */
	data = skb->data + off;
	space = skb->len - off;
	for (;;) {
		if (space > data_len)
			space = data_len;
		/* collect 32-bit blocks from current buffer */
		while (space >= sizeof(uint32_t)) {
			l ^= get_le32(data);
			michael_block(l, r);
			data += sizeof(uint32_t), space -= sizeof(uint32_t);
			data_len -= sizeof(uint32_t);
		}
		if (data_len < sizeof(uint32_t))
			break;
		skb = skb->next;
		if (skb == NULL) {
			KASSERT(0, ("out of data, data_len %lu\n",
				    (unsigned long)data_len));
			break;
		}
		if (space != 0) {
			const uint8_t *data_next;
			/*
			 * Block straddles buffers, split references.
			 */
			data_next = skb->data;
			KASSERT(skb->len >= sizeof(uint32_t) - space,
				("not enough data in following buffer, "
				"skb len %u need %u\n", skb->len,
				(int)sizeof(uint32_t) - space));
			switch (space) {
			case 1:
				l ^= get_le32_split(data[0], data_next[0],
					data_next[1], data_next[2]);
				data = data_next + 3;
				space = skb->len - 3;
				break;
			case 2:
				l ^= get_le32_split(data[0], data[1],
					data_next[0], data_next[1]);
				data = data_next + 2;
				space = skb->len - 2;
				break;
			case 3:
				l ^= get_le32_split(data[0], data[1],
					data[2], data_next[0]);
				data = data_next + 1;
				space = skb->len - 1;
				break;
			}
			michael_block(l, r);
			data_len -= sizeof(uint32_t);
		} else {
			/*
			 * Setup for next buffer.
			 */
			data = skb->data;
			space = skb->len;
		}
	}
	/* Last block and padding (0x5a, 4..7 x 0) */
	switch (data_len) {
	case 0:
		l ^= get_le32_split(0x5a, 0, 0, 0);
		break;
	case 1:
		l ^= get_le32_split(data[0], 0x5a, 0, 0);
		break;
	case 2:
		l ^= get_le32_split(data[0], data[1], 0x5a, 0);
		break;
	case 3:
		l ^= get_le32_split(data[0], data[1], data[2], 0x5a);
		break;
	}
	michael_block(l, r);
	/* l ^= 0; */
	michael_block(l, r);

	put_le32(mic, l);
	put_le32(mic + 4, r);
}

static int
tkip_encrypt(struct tkip_ctx *ctx, struct ieee80211_key *key,
	struct sk_buff *skb0, int hdrlen)
{
	struct ieee80211_frame *wh = (struct ieee80211_frame *) skb0->data;
	struct ieee80211vap *vap = ctx->tc_vap;
	struct sk_buff *skb;
	size_t pktlen;

	vap->iv_stats.is_crypto_tkip++;

	skb = skb0;
	pktlen = skb->len;
	while (skb->next != NULL) {
		skb = skb->next;
		pktlen += skb->len;
	}
	if (skb_tailroom(skb) < tkip.ic_trailer) {
		/* NB: should not happen */
		IEEE80211_NOTE_MAC(vap, IEEE80211_MSG_CRYPTO,
			wh->i_addr1, "No room for TKIP CRC, tailroom %u",
			skb_tailroom(skb));
		/* XXX statistic */
		return 0;
	}

	if (!ctx->tx_phase1_done) {
		tkip_mixing_phase1(ctx->tx_ttak, key->wk_key, wh->i_addr2,
			(u32)(key->wk_keytsc >> 16));
		ctx->tx_phase1_done = 1;
	}
	tkip_mixing_phase2(ctx->tx_rc4key, key->wk_key, ctx->tx_ttak,
		(u16) key->wk_keytsc);

	wep_encrypt(ctx->tx_rc4key,
		skb0, hdrlen + tkip.ic_header,
		pktlen - (hdrlen + tkip.ic_header));

	key->wk_keytsc++;
	if ((u16)(key->wk_keytsc) == 0)
		ctx->tx_phase1_done = 0;
	return 1;
}

static int
tkip_decrypt(struct tkip_ctx *ctx, struct ieee80211_key *key,
	struct sk_buff *skb0, int hdrlen)
{
	struct ieee80211_frame *wh = (struct ieee80211_frame *) skb0->data;
	struct ieee80211vap *vap = ctx->tc_vap;
	struct sk_buff *skb;
	size_t pktlen;
	u32 iv32;
	u16 iv16;
	u_int8_t tid;

	vap->iv_stats.is_crypto_tkip++;

	skb = skb0;
	pktlen = skb->len;
	while (skb->next != NULL) {
		skb = skb->next;
		pktlen += skb->len;
	}
	/* NB: tkip_decap already verified header and left seq in rx_rsc */
	iv16 = (u16) ctx->rx_rsc;
	iv32 = (u32) (ctx->rx_rsc >> 16);

	wh = (struct ieee80211_frame *) skb0->data;
	tid = 0;
	if (IEEE80211_QOS_HAS_SEQ(wh)) 
		tid = ((struct ieee80211_qosframe *)wh)->i_qos[0] & IEEE80211_QOS_TID;
	if (iv32 != (u32)(key->wk_keyrsc[tid] >> 16) || !ctx->rx_phase1_done) {
		tkip_mixing_phase1(ctx->rx_ttak, key->wk_key,
			wh->i_addr2, iv32);
		ctx->rx_phase1_done = 1;
	}
	tkip_mixing_phase2(ctx->rx_rc4key, key->wk_key, ctx->rx_ttak, iv16);

	/* NB: skb is unstripped; deduct headers + ICV to get payload */
	if (wep_decrypt(ctx->rx_rc4key,
	    skb0, hdrlen + tkip.ic_header,
	    pktlen - (hdrlen + tkip.ic_header + tkip.ic_trailer))) {
		if (iv32 != (u32)(key->wk_keyrsc[tid] >> 16)) {
			/* Previously cached Phase1 result was already lost, so
			 * it needs to be recalculated for the next packet. */
			ctx->rx_phase1_done = 0;
		}
		IEEE80211_DPRINTF(vap, IEEE80211_MSG_CRYPTO,
			"[%s] TKIP ICV mismatch on decrypt (keyix %d, rsc %llu)\n",
			ether_sprintf(wh->i_addr2), key->wk_keyix, ctx->rx_rsc);
		vap->iv_stats.is_rx_tkipicv++;
		return 0;
	}
	return 1;
}

/*
 * Module glue.
 */
MODULE_AUTHOR("Errno Consulting, Sam Leffler");
MODULE_DESCRIPTION("802.11 wireless support: TKIP cipher");
#ifdef MODULE_LICENSE
MODULE_LICENSE("Dual BSD/GPL");
#endif

static int __init
init_crypto_tkip(void)
{
	ieee80211_crypto_register(&tkip);
	return 0;
}
module_init(init_crypto_tkip);

static void __exit
exit_crypto_tkip(void)
{
	ieee80211_crypto_unregister(&tkip);
}
module_exit(exit_crypto_tkip);