51f94a7b1f
Signed-off-by: Bartlomiej Zolnierkiewicz <bzolnier@gmail.com> Signed-off-by: Greg Kroah-Hartman <gregkh@suse.de>
1587 lines
38 KiB
C
1587 lines
38 KiB
C
/*
|
|
*************************************************************************
|
|
* Ralink Tech Inc.
|
|
* 5F., No.36, Taiyuan St., Jhubei City,
|
|
* Hsinchu County 302,
|
|
* Taiwan, R.O.C.
|
|
*
|
|
* (c) Copyright 2002-2007, Ralink Technology, Inc.
|
|
*
|
|
* This program is free software; you can redistribute it and/or modify *
|
|
* it under the terms of the GNU General Public License as published by *
|
|
* the Free Software Foundation; either version 2 of the License, or *
|
|
* (at your option) any later version. *
|
|
* *
|
|
* This program is distributed in the hope that it will be useful, *
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
|
|
* GNU General Public License for more details. *
|
|
* *
|
|
* You should have received a copy of the GNU General Public License *
|
|
* along with this program; if not, write to the *
|
|
* Free Software Foundation, Inc., *
|
|
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
|
|
* *
|
|
*************************************************************************
|
|
|
|
Module Name:
|
|
rtmp_tkip.c
|
|
|
|
Abstract:
|
|
|
|
Revision History:
|
|
Who When What
|
|
-------- ---------- ----------------------------------------------
|
|
Paul Wu 02-25-02 Initial
|
|
*/
|
|
|
|
#include "../rt_config.h"
|
|
|
|
// Rotation functions on 32 bit values
|
|
#define ROL32( A, n ) \
|
|
( ((A) << (n)) | ( ((A)>>(32-(n))) & ( (1UL << (n)) - 1 ) ) )
|
|
#define ROR32( A, n ) ROL32( (A), 32-(n) )
|
|
|
|
UINT Tkip_Sbox_Lower[256] =
|
|
{
|
|
0xA5,0x84,0x99,0x8D,0x0D,0xBD,0xB1,0x54,
|
|
0x50,0x03,0xA9,0x7D,0x19,0x62,0xE6,0x9A,
|
|
0x45,0x9D,0x40,0x87,0x15,0xEB,0xC9,0x0B,
|
|
0xEC,0x67,0xFD,0xEA,0xBF,0xF7,0x96,0x5B,
|
|
0xC2,0x1C,0xAE,0x6A,0x5A,0x41,0x02,0x4F,
|
|
0x5C,0xF4,0x34,0x08,0x93,0x73,0x53,0x3F,
|
|
0x0C,0x52,0x65,0x5E,0x28,0xA1,0x0F,0xB5,
|
|
0x09,0x36,0x9B,0x3D,0x26,0x69,0xCD,0x9F,
|
|
0x1B,0x9E,0x74,0x2E,0x2D,0xB2,0xEE,0xFB,
|
|
0xF6,0x4D,0x61,0xCE,0x7B,0x3E,0x71,0x97,
|
|
0xF5,0x68,0x00,0x2C,0x60,0x1F,0xC8,0xED,
|
|
0xBE,0x46,0xD9,0x4B,0xDE,0xD4,0xE8,0x4A,
|
|
0x6B,0x2A,0xE5,0x16,0xC5,0xD7,0x55,0x94,
|
|
0xCF,0x10,0x06,0x81,0xF0,0x44,0xBA,0xE3,
|
|
0xF3,0xFE,0xC0,0x8A,0xAD,0xBC,0x48,0x04,
|
|
0xDF,0xC1,0x75,0x63,0x30,0x1A,0x0E,0x6D,
|
|
0x4C,0x14,0x35,0x2F,0xE1,0xA2,0xCC,0x39,
|
|
0x57,0xF2,0x82,0x47,0xAC,0xE7,0x2B,0x95,
|
|
0xA0,0x98,0xD1,0x7F,0x66,0x7E,0xAB,0x83,
|
|
0xCA,0x29,0xD3,0x3C,0x79,0xE2,0x1D,0x76,
|
|
0x3B,0x56,0x4E,0x1E,0xDB,0x0A,0x6C,0xE4,
|
|
0x5D,0x6E,0xEF,0xA6,0xA8,0xA4,0x37,0x8B,
|
|
0x32,0x43,0x59,0xB7,0x8C,0x64,0xD2,0xE0,
|
|
0xB4,0xFA,0x07,0x25,0xAF,0x8E,0xE9,0x18,
|
|
0xD5,0x88,0x6F,0x72,0x24,0xF1,0xC7,0x51,
|
|
0x23,0x7C,0x9C,0x21,0xDD,0xDC,0x86,0x85,
|
|
0x90,0x42,0xC4,0xAA,0xD8,0x05,0x01,0x12,
|
|
0xA3,0x5F,0xF9,0xD0,0x91,0x58,0x27,0xB9,
|
|
0x38,0x13,0xB3,0x33,0xBB,0x70,0x89,0xA7,
|
|
0xB6,0x22,0x92,0x20,0x49,0xFF,0x78,0x7A,
|
|
0x8F,0xF8,0x80,0x17,0xDA,0x31,0xC6,0xB8,
|
|
0xC3,0xB0,0x77,0x11,0xCB,0xFC,0xD6,0x3A
|
|
};
|
|
|
|
UINT Tkip_Sbox_Upper[256] =
|
|
{
|
|
0xC6,0xF8,0xEE,0xF6,0xFF,0xD6,0xDE,0x91,
|
|
0x60,0x02,0xCE,0x56,0xE7,0xB5,0x4D,0xEC,
|
|
0x8F,0x1F,0x89,0xFA,0xEF,0xB2,0x8E,0xFB,
|
|
0x41,0xB3,0x5F,0x45,0x23,0x53,0xE4,0x9B,
|
|
0x75,0xE1,0x3D,0x4C,0x6C,0x7E,0xF5,0x83,
|
|
0x68,0x51,0xD1,0xF9,0xE2,0xAB,0x62,0x2A,
|
|
0x08,0x95,0x46,0x9D,0x30,0x37,0x0A,0x2F,
|
|
0x0E,0x24,0x1B,0xDF,0xCD,0x4E,0x7F,0xEA,
|
|
0x12,0x1D,0x58,0x34,0x36,0xDC,0xB4,0x5B,
|
|
0xA4,0x76,0xB7,0x7D,0x52,0xDD,0x5E,0x13,
|
|
0xA6,0xB9,0x00,0xC1,0x40,0xE3,0x79,0xB6,
|
|
0xD4,0x8D,0x67,0x72,0x94,0x98,0xB0,0x85,
|
|
0xBB,0xC5,0x4F,0xED,0x86,0x9A,0x66,0x11,
|
|
0x8A,0xE9,0x04,0xFE,0xA0,0x78,0x25,0x4B,
|
|
0xA2,0x5D,0x80,0x05,0x3F,0x21,0x70,0xF1,
|
|
0x63,0x77,0xAF,0x42,0x20,0xE5,0xFD,0xBF,
|
|
0x81,0x18,0x26,0xC3,0xBE,0x35,0x88,0x2E,
|
|
0x93,0x55,0xFC,0x7A,0xC8,0xBA,0x32,0xE6,
|
|
0xC0,0x19,0x9E,0xA3,0x44,0x54,0x3B,0x0B,
|
|
0x8C,0xC7,0x6B,0x28,0xA7,0xBC,0x16,0xAD,
|
|
0xDB,0x64,0x74,0x14,0x92,0x0C,0x48,0xB8,
|
|
0x9F,0xBD,0x43,0xC4,0x39,0x31,0xD3,0xF2,
|
|
0xD5,0x8B,0x6E,0xDA,0x01,0xB1,0x9C,0x49,
|
|
0xD8,0xAC,0xF3,0xCF,0xCA,0xF4,0x47,0x10,
|
|
0x6F,0xF0,0x4A,0x5C,0x38,0x57,0x73,0x97,
|
|
0xCB,0xA1,0xE8,0x3E,0x96,0x61,0x0D,0x0F,
|
|
0xE0,0x7C,0x71,0xCC,0x90,0x06,0xF7,0x1C,
|
|
0xC2,0x6A,0xAE,0x69,0x17,0x99,0x3A,0x27,
|
|
0xD9,0xEB,0x2B,0x22,0xD2,0xA9,0x07,0x33,
|
|
0x2D,0x3C,0x15,0xC9,0x87,0xAA,0x50,0xA5,
|
|
0x03,0x59,0x09,0x1A,0x65,0xD7,0x84,0xD0,
|
|
0x82,0x29,0x5A,0x1E,0x7B,0xA8,0x6D,0x2C
|
|
};
|
|
|
|
/*****************************/
|
|
/******** SBOX Table *********/
|
|
/*****************************/
|
|
|
|
UCHAR SboxTable[256] =
|
|
{
|
|
0x63, 0x7c, 0x77, 0x7b, 0xf2, 0x6b, 0x6f, 0xc5,
|
|
0x30, 0x01, 0x67, 0x2b, 0xfe, 0xd7, 0xab, 0x76,
|
|
0xca, 0x82, 0xc9, 0x7d, 0xfa, 0x59, 0x47, 0xf0,
|
|
0xad, 0xd4, 0xa2, 0xaf, 0x9c, 0xa4, 0x72, 0xc0,
|
|
0xb7, 0xfd, 0x93, 0x26, 0x36, 0x3f, 0xf7, 0xcc,
|
|
0x34, 0xa5, 0xe5, 0xf1, 0x71, 0xd8, 0x31, 0x15,
|
|
0x04, 0xc7, 0x23, 0xc3, 0x18, 0x96, 0x05, 0x9a,
|
|
0x07, 0x12, 0x80, 0xe2, 0xeb, 0x27, 0xb2, 0x75,
|
|
0x09, 0x83, 0x2c, 0x1a, 0x1b, 0x6e, 0x5a, 0xa0,
|
|
0x52, 0x3b, 0xd6, 0xb3, 0x29, 0xe3, 0x2f, 0x84,
|
|
0x53, 0xd1, 0x00, 0xed, 0x20, 0xfc, 0xb1, 0x5b,
|
|
0x6a, 0xcb, 0xbe, 0x39, 0x4a, 0x4c, 0x58, 0xcf,
|
|
0xd0, 0xef, 0xaa, 0xfb, 0x43, 0x4d, 0x33, 0x85,
|
|
0x45, 0xf9, 0x02, 0x7f, 0x50, 0x3c, 0x9f, 0xa8,
|
|
0x51, 0xa3, 0x40, 0x8f, 0x92, 0x9d, 0x38, 0xf5,
|
|
0xbc, 0xb6, 0xda, 0x21, 0x10, 0xff, 0xf3, 0xd2,
|
|
0xcd, 0x0c, 0x13, 0xec, 0x5f, 0x97, 0x44, 0x17,
|
|
0xc4, 0xa7, 0x7e, 0x3d, 0x64, 0x5d, 0x19, 0x73,
|
|
0x60, 0x81, 0x4f, 0xdc, 0x22, 0x2a, 0x90, 0x88,
|
|
0x46, 0xee, 0xb8, 0x14, 0xde, 0x5e, 0x0b, 0xdb,
|
|
0xe0, 0x32, 0x3a, 0x0a, 0x49, 0x06, 0x24, 0x5c,
|
|
0xc2, 0xd3, 0xac, 0x62, 0x91, 0x95, 0xe4, 0x79,
|
|
0xe7, 0xc8, 0x37, 0x6d, 0x8d, 0xd5, 0x4e, 0xa9,
|
|
0x6c, 0x56, 0xf4, 0xea, 0x65, 0x7a, 0xae, 0x08,
|
|
0xba, 0x78, 0x25, 0x2e, 0x1c, 0xa6, 0xb4, 0xc6,
|
|
0xe8, 0xdd, 0x74, 0x1f, 0x4b, 0xbd, 0x8b, 0x8a,
|
|
0x70, 0x3e, 0xb5, 0x66, 0x48, 0x03, 0xf6, 0x0e,
|
|
0x61, 0x35, 0x57, 0xb9, 0x86, 0xc1, 0x1d, 0x9e,
|
|
0xe1, 0xf8, 0x98, 0x11, 0x69, 0xd9, 0x8e, 0x94,
|
|
0x9b, 0x1e, 0x87, 0xe9, 0xce, 0x55, 0x28, 0xdf,
|
|
0x8c, 0xa1, 0x89, 0x0d, 0xbf, 0xe6, 0x42, 0x68,
|
|
0x41, 0x99, 0x2d, 0x0f, 0xb0, 0x54, 0xbb, 0x16
|
|
};
|
|
|
|
VOID xor_32(
|
|
IN PUCHAR a,
|
|
IN PUCHAR b,
|
|
OUT PUCHAR out);
|
|
|
|
VOID xor_128(
|
|
IN PUCHAR a,
|
|
IN PUCHAR b,
|
|
OUT PUCHAR out);
|
|
|
|
VOID next_key(
|
|
IN PUCHAR key,
|
|
IN INT round);
|
|
|
|
VOID byte_sub(
|
|
IN PUCHAR in,
|
|
OUT PUCHAR out);
|
|
|
|
VOID shift_row(
|
|
IN PUCHAR in,
|
|
OUT PUCHAR out);
|
|
|
|
VOID mix_column(
|
|
IN PUCHAR in,
|
|
OUT PUCHAR out);
|
|
|
|
UCHAR RTMPCkipSbox(
|
|
IN UCHAR a);
|
|
//
|
|
// Expanded IV for TKIP function.
|
|
//
|
|
typedef struct PACKED _IV_CONTROL_
|
|
{
|
|
union PACKED
|
|
{
|
|
struct PACKED
|
|
{
|
|
UCHAR rc0;
|
|
UCHAR rc1;
|
|
UCHAR rc2;
|
|
|
|
union PACKED
|
|
{
|
|
struct PACKED
|
|
{
|
|
UCHAR Rsvd:5;
|
|
UCHAR ExtIV:1;
|
|
UCHAR KeyID:2;
|
|
} field;
|
|
UCHAR Byte;
|
|
} CONTROL;
|
|
} field;
|
|
|
|
ULONG word;
|
|
} IV16;
|
|
|
|
ULONG IV32;
|
|
} TKIP_IV, *PTKIP_IV;
|
|
|
|
|
|
/*
|
|
========================================================================
|
|
|
|
Routine Description:
|
|
Convert from UCHAR[] to ULONG in a portable way
|
|
|
|
Arguments:
|
|
pMICKey pointer to MIC Key
|
|
|
|
Return Value:
|
|
None
|
|
|
|
Note:
|
|
|
|
========================================================================
|
|
*/
|
|
ULONG RTMPTkipGetUInt32(
|
|
IN PUCHAR pMICKey)
|
|
{
|
|
ULONG res = 0;
|
|
INT i;
|
|
|
|
for (i = 0; i < 4; i++)
|
|
{
|
|
res |= (*pMICKey++) << (8 * i);
|
|
}
|
|
|
|
return res;
|
|
}
|
|
|
|
/*
|
|
========================================================================
|
|
|
|
Routine Description:
|
|
Convert from ULONG to UCHAR[] in a portable way
|
|
|
|
Arguments:
|
|
pDst pointer to destination for convert ULONG to UCHAR[]
|
|
val the value for convert
|
|
|
|
Return Value:
|
|
None
|
|
|
|
IRQL = DISPATCH_LEVEL
|
|
|
|
Note:
|
|
|
|
========================================================================
|
|
*/
|
|
VOID RTMPTkipPutUInt32(
|
|
IN OUT PUCHAR pDst,
|
|
IN ULONG val)
|
|
{
|
|
INT i;
|
|
|
|
for(i = 0; i < 4; i++)
|
|
{
|
|
*pDst++ = (UCHAR) (val & 0xff);
|
|
val >>= 8;
|
|
}
|
|
}
|
|
|
|
/*
|
|
========================================================================
|
|
|
|
Routine Description:
|
|
Set the MIC Key.
|
|
|
|
Arguments:
|
|
pAd Pointer to our adapter
|
|
pMICKey pointer to MIC Key
|
|
|
|
Return Value:
|
|
None
|
|
|
|
IRQL = DISPATCH_LEVEL
|
|
|
|
Note:
|
|
|
|
========================================================================
|
|
*/
|
|
VOID RTMPTkipSetMICKey(
|
|
IN PTKIP_KEY_INFO pTkip,
|
|
IN PUCHAR pMICKey)
|
|
{
|
|
// Set the key
|
|
pTkip->K0 = RTMPTkipGetUInt32(pMICKey);
|
|
pTkip->K1 = RTMPTkipGetUInt32(pMICKey + 4);
|
|
// and reset the message
|
|
pTkip->L = pTkip->K0;
|
|
pTkip->R = pTkip->K1;
|
|
pTkip->nBytesInM = 0;
|
|
pTkip->M = 0;
|
|
}
|
|
|
|
/*
|
|
========================================================================
|
|
|
|
Routine Description:
|
|
Calculate the MIC Value.
|
|
|
|
Arguments:
|
|
pAd Pointer to our adapter
|
|
uChar Append this uChar
|
|
|
|
Return Value:
|
|
None
|
|
|
|
IRQL = DISPATCH_LEVEL
|
|
|
|
Note:
|
|
|
|
========================================================================
|
|
*/
|
|
VOID RTMPTkipAppendByte(
|
|
IN PTKIP_KEY_INFO pTkip,
|
|
IN UCHAR uChar)
|
|
{
|
|
// Append the byte to our word-sized buffer
|
|
pTkip->M |= (uChar << (8* pTkip->nBytesInM));
|
|
pTkip->nBytesInM++;
|
|
// Process the word if it is full.
|
|
if( pTkip->nBytesInM >= 4 )
|
|
{
|
|
pTkip->L ^= pTkip->M;
|
|
pTkip->R ^= ROL32( pTkip->L, 17 );
|
|
pTkip->L += pTkip->R;
|
|
pTkip->R ^= ((pTkip->L & 0xff00ff00) >> 8) | ((pTkip->L & 0x00ff00ff) << 8);
|
|
pTkip->L += pTkip->R;
|
|
pTkip->R ^= ROL32( pTkip->L, 3 );
|
|
pTkip->L += pTkip->R;
|
|
pTkip->R ^= ROR32( pTkip->L, 2 );
|
|
pTkip->L += pTkip->R;
|
|
// Clear the buffer
|
|
pTkip->M = 0;
|
|
pTkip->nBytesInM = 0;
|
|
}
|
|
}
|
|
|
|
/*
|
|
========================================================================
|
|
|
|
Routine Description:
|
|
Calculate the MIC Value.
|
|
|
|
Arguments:
|
|
pAd Pointer to our adapter
|
|
pSrc Pointer to source data for Calculate MIC Value
|
|
Len Indicate the length of the source data
|
|
|
|
Return Value:
|
|
None
|
|
|
|
IRQL = DISPATCH_LEVEL
|
|
|
|
Note:
|
|
|
|
========================================================================
|
|
*/
|
|
VOID RTMPTkipAppend(
|
|
IN PTKIP_KEY_INFO pTkip,
|
|
IN PUCHAR pSrc,
|
|
IN UINT nBytes)
|
|
{
|
|
// This is simple
|
|
while(nBytes > 0)
|
|
{
|
|
RTMPTkipAppendByte(pTkip, *pSrc++);
|
|
nBytes--;
|
|
}
|
|
}
|
|
|
|
/*
|
|
========================================================================
|
|
|
|
Routine Description:
|
|
Get the MIC Value.
|
|
|
|
Arguments:
|
|
pAd Pointer to our adapter
|
|
|
|
Return Value:
|
|
None
|
|
|
|
IRQL = DISPATCH_LEVEL
|
|
|
|
Note:
|
|
the MIC Value is store in pAd->PrivateInfo.MIC
|
|
========================================================================
|
|
*/
|
|
VOID RTMPTkipGetMIC(
|
|
IN PTKIP_KEY_INFO pTkip)
|
|
{
|
|
// Append the minimum padding
|
|
RTMPTkipAppendByte(pTkip, 0x5a );
|
|
RTMPTkipAppendByte(pTkip, 0 );
|
|
RTMPTkipAppendByte(pTkip, 0 );
|
|
RTMPTkipAppendByte(pTkip, 0 );
|
|
RTMPTkipAppendByte(pTkip, 0 );
|
|
// and then zeroes until the length is a multiple of 4
|
|
while( pTkip->nBytesInM != 0 )
|
|
{
|
|
RTMPTkipAppendByte(pTkip, 0 );
|
|
}
|
|
// The appendByte function has already computed the result.
|
|
RTMPTkipPutUInt32(pTkip->MIC, pTkip->L);
|
|
RTMPTkipPutUInt32(pTkip->MIC + 4, pTkip->R);
|
|
}
|
|
|
|
/*
|
|
========================================================================
|
|
|
|
Routine Description:
|
|
Init Tkip function.
|
|
|
|
Arguments:
|
|
pAd Pointer to our adapter
|
|
pTKey Pointer to the Temporal Key (TK), TK shall be 128bits.
|
|
KeyId TK Key ID
|
|
pTA Pointer to transmitter address
|
|
pMICKey pointer to MIC Key
|
|
|
|
Return Value:
|
|
None
|
|
|
|
IRQL = DISPATCH_LEVEL
|
|
|
|
Note:
|
|
|
|
========================================================================
|
|
*/
|
|
VOID RTMPInitTkipEngine(
|
|
IN PRTMP_ADAPTER pAd,
|
|
IN PUCHAR pKey,
|
|
IN UCHAR KeyId,
|
|
IN PUCHAR pTA,
|
|
IN PUCHAR pMICKey,
|
|
IN PUCHAR pTSC,
|
|
OUT PULONG pIV16,
|
|
OUT PULONG pIV32)
|
|
{
|
|
TKIP_IV tkipIv;
|
|
|
|
// Prepare 8 bytes TKIP encapsulation for MPDU
|
|
NdisZeroMemory(&tkipIv, sizeof(TKIP_IV));
|
|
tkipIv.IV16.field.rc0 = *(pTSC + 1);
|
|
tkipIv.IV16.field.rc1 = (tkipIv.IV16.field.rc0 | 0x20) & 0x7f;
|
|
tkipIv.IV16.field.rc2 = *pTSC;
|
|
tkipIv.IV16.field.CONTROL.field.ExtIV = 1; // 0: non-extended IV, 1: an extended IV
|
|
tkipIv.IV16.field.CONTROL.field.KeyID = KeyId;
|
|
NdisMoveMemory(&tkipIv.IV32, (pTSC + 2), 4); // Copy IV
|
|
|
|
*pIV16 = tkipIv.IV16.word;
|
|
*pIV32 = tkipIv.IV32;
|
|
}
|
|
|
|
/*
|
|
========================================================================
|
|
|
|
Routine Description:
|
|
Init MIC Value calculation function which include set MIC key &
|
|
calculate first 16 bytes (DA + SA + priority + 0)
|
|
|
|
Arguments:
|
|
pAd Pointer to our adapter
|
|
pTKey Pointer to the Temporal Key (TK), TK shall be 128bits.
|
|
pDA Pointer to DA address
|
|
pSA Pointer to SA address
|
|
pMICKey pointer to MIC Key
|
|
|
|
Return Value:
|
|
None
|
|
|
|
Note:
|
|
|
|
========================================================================
|
|
*/
|
|
VOID RTMPInitMICEngine(
|
|
IN PRTMP_ADAPTER pAd,
|
|
IN PUCHAR pKey,
|
|
IN PUCHAR pDA,
|
|
IN PUCHAR pSA,
|
|
IN UCHAR UserPriority,
|
|
IN PUCHAR pMICKey)
|
|
{
|
|
ULONG Priority = UserPriority;
|
|
|
|
// Init MIC value calculation
|
|
RTMPTkipSetMICKey(&pAd->PrivateInfo.Tx, pMICKey);
|
|
// DA
|
|
RTMPTkipAppend(&pAd->PrivateInfo.Tx, pDA, MAC_ADDR_LEN);
|
|
// SA
|
|
RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSA, MAC_ADDR_LEN);
|
|
// Priority + 3 bytes of 0
|
|
RTMPTkipAppend(&pAd->PrivateInfo.Tx, (PUCHAR)&Priority, 4);
|
|
}
|
|
|
|
/*
|
|
========================================================================
|
|
|
|
Routine Description:
|
|
Compare MIC value of received MSDU
|
|
|
|
Arguments:
|
|
pAd Pointer to our adapter
|
|
pSrc Pointer to the received Plain text data
|
|
pDA Pointer to DA address
|
|
pSA Pointer to SA address
|
|
pMICKey pointer to MIC Key
|
|
Len the length of the received plain text data exclude MIC value
|
|
|
|
Return Value:
|
|
TRUE MIC value matched
|
|
FALSE MIC value mismatched
|
|
|
|
IRQL = DISPATCH_LEVEL
|
|
|
|
Note:
|
|
|
|
========================================================================
|
|
*/
|
|
BOOLEAN RTMPTkipCompareMICValue(
|
|
IN PRTMP_ADAPTER pAd,
|
|
IN PUCHAR pSrc,
|
|
IN PUCHAR pDA,
|
|
IN PUCHAR pSA,
|
|
IN PUCHAR pMICKey,
|
|
IN UCHAR UserPriority,
|
|
IN UINT Len)
|
|
{
|
|
UCHAR OldMic[8];
|
|
ULONG Priority = UserPriority;
|
|
|
|
// Init MIC value calculation
|
|
RTMPTkipSetMICKey(&pAd->PrivateInfo.Rx, pMICKey);
|
|
// DA
|
|
RTMPTkipAppend(&pAd->PrivateInfo.Rx, pDA, MAC_ADDR_LEN);
|
|
// SA
|
|
RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSA, MAC_ADDR_LEN);
|
|
// Priority + 3 bytes of 0
|
|
RTMPTkipAppend(&pAd->PrivateInfo.Rx, (PUCHAR)&Priority, 4);
|
|
|
|
// Calculate MIC value from plain text data
|
|
RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSrc, Len);
|
|
|
|
// Get MIC valude from received frame
|
|
NdisMoveMemory(OldMic, pSrc + Len, 8);
|
|
|
|
// Get MIC value from decrypted plain data
|
|
RTMPTkipGetMIC(&pAd->PrivateInfo.Rx);
|
|
|
|
// Move MIC value from MSDU, this steps should move to data path.
|
|
// Since the MIC value might cross MPDUs.
|
|
if(!NdisEqualMemory(pAd->PrivateInfo.Rx.MIC, OldMic, 8))
|
|
{
|
|
DBGPRINT_RAW(RT_DEBUG_ERROR, ("RTMPTkipCompareMICValue(): TKIP MIC Error !\n")); //MIC error.
|
|
|
|
|
|
return (FALSE);
|
|
}
|
|
return (TRUE);
|
|
}
|
|
|
|
/*
|
|
========================================================================
|
|
|
|
Routine Description:
|
|
Compare MIC value of received MSDU
|
|
|
|
Arguments:
|
|
pAd Pointer to our adapter
|
|
pLLC LLC header
|
|
pSrc Pointer to the received Plain text data
|
|
pDA Pointer to DA address
|
|
pSA Pointer to SA address
|
|
pMICKey pointer to MIC Key
|
|
Len the length of the received plain text data exclude MIC value
|
|
|
|
Return Value:
|
|
TRUE MIC value matched
|
|
FALSE MIC value mismatched
|
|
|
|
IRQL = DISPATCH_LEVEL
|
|
|
|
Note:
|
|
|
|
========================================================================
|
|
*/
|
|
BOOLEAN RTMPTkipCompareMICValueWithLLC(
|
|
IN PRTMP_ADAPTER pAd,
|
|
IN PUCHAR pLLC,
|
|
IN PUCHAR pSrc,
|
|
IN PUCHAR pDA,
|
|
IN PUCHAR pSA,
|
|
IN PUCHAR pMICKey,
|
|
IN UINT Len)
|
|
{
|
|
UCHAR OldMic[8];
|
|
ULONG Priority = 0;
|
|
|
|
// Init MIC value calculation
|
|
RTMPTkipSetMICKey(&pAd->PrivateInfo.Rx, pMICKey);
|
|
// DA
|
|
RTMPTkipAppend(&pAd->PrivateInfo.Rx, pDA, MAC_ADDR_LEN);
|
|
// SA
|
|
RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSA, MAC_ADDR_LEN);
|
|
// Priority + 3 bytes of 0
|
|
RTMPTkipAppend(&pAd->PrivateInfo.Rx, (PUCHAR)&Priority, 4);
|
|
|
|
// Start with LLC header
|
|
RTMPTkipAppend(&pAd->PrivateInfo.Rx, pLLC, 8);
|
|
|
|
// Calculate MIC value from plain text data
|
|
RTMPTkipAppend(&pAd->PrivateInfo.Rx, pSrc, Len);
|
|
|
|
// Get MIC valude from received frame
|
|
NdisMoveMemory(OldMic, pSrc + Len, 8);
|
|
|
|
// Get MIC value from decrypted plain data
|
|
RTMPTkipGetMIC(&pAd->PrivateInfo.Rx);
|
|
|
|
// Move MIC value from MSDU, this steps should move to data path.
|
|
// Since the MIC value might cross MPDUs.
|
|
if(!NdisEqualMemory(pAd->PrivateInfo.Rx.MIC, OldMic, 8))
|
|
{
|
|
DBGPRINT_RAW(RT_DEBUG_ERROR, ("RTMPTkipCompareMICValueWithLLC(): TKIP MIC Error !\n")); //MIC error.
|
|
|
|
|
|
return (FALSE);
|
|
}
|
|
return (TRUE);
|
|
}
|
|
/*
|
|
========================================================================
|
|
|
|
Routine Description:
|
|
Copy frame from waiting queue into relative ring buffer and set
|
|
appropriate ASIC register to kick hardware transmit function
|
|
|
|
Arguments:
|
|
pAd Pointer to our adapter
|
|
PNDIS_PACKET Pointer to Ndis Packet for MIC calculation
|
|
pEncap Pointer to LLC encap data
|
|
LenEncap Total encap length, might be 0 which indicates no encap
|
|
|
|
Return Value:
|
|
None
|
|
|
|
IRQL = DISPATCH_LEVEL
|
|
|
|
Note:
|
|
|
|
========================================================================
|
|
*/
|
|
VOID RTMPCalculateMICValue(
|
|
IN PRTMP_ADAPTER pAd,
|
|
IN PNDIS_PACKET pPacket,
|
|
IN PUCHAR pEncap,
|
|
IN PCIPHER_KEY pKey,
|
|
IN UCHAR apidx)
|
|
{
|
|
PACKET_INFO PacketInfo;
|
|
PUCHAR pSrcBufVA;
|
|
UINT SrcBufLen;
|
|
PUCHAR pSrc;
|
|
UCHAR UserPriority;
|
|
UCHAR vlan_offset = 0;
|
|
|
|
RTMP_QueryPacketInfo(pPacket, &PacketInfo, &pSrcBufVA, &SrcBufLen);
|
|
|
|
UserPriority = RTMP_GET_PACKET_UP(pPacket);
|
|
pSrc = pSrcBufVA;
|
|
|
|
// determine if this is a vlan packet
|
|
if (((*(pSrc + 12) << 8) + *(pSrc + 13)) == 0x8100)
|
|
vlan_offset = 4;
|
|
|
|
{
|
|
RTMPInitMICEngine(
|
|
pAd,
|
|
pKey->Key,
|
|
pSrc,
|
|
pSrc + 6,
|
|
UserPriority,
|
|
pKey->TxMic);
|
|
}
|
|
|
|
|
|
if (pEncap != NULL)
|
|
{
|
|
// LLC encapsulation
|
|
RTMPTkipAppend(&pAd->PrivateInfo.Tx, pEncap, 6);
|
|
// Protocol Type
|
|
RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSrc + 12 + vlan_offset, 2);
|
|
}
|
|
SrcBufLen -= (14 + vlan_offset);
|
|
pSrc += (14 + vlan_offset);
|
|
do
|
|
{
|
|
if (SrcBufLen > 0)
|
|
{
|
|
RTMPTkipAppend(&pAd->PrivateInfo.Tx, pSrc, SrcBufLen);
|
|
}
|
|
|
|
break; // No need handle next packet
|
|
|
|
} while (TRUE); // End of copying payload
|
|
|
|
// Compute the final MIC Value
|
|
RTMPTkipGetMIC(&pAd->PrivateInfo.Tx);
|
|
}
|
|
|
|
|
|
/************************************************************/
|
|
/* tkip_sbox() */
|
|
/* Returns a 16 bit value from a 64K entry table. The Table */
|
|
/* is synthesized from two 256 entry byte wide tables. */
|
|
/************************************************************/
|
|
|
|
UINT tkip_sbox(UINT index)
|
|
{
|
|
UINT index_low;
|
|
UINT index_high;
|
|
UINT left, right;
|
|
|
|
index_low = (index % 256);
|
|
index_high = ((index >> 8) % 256);
|
|
|
|
left = Tkip_Sbox_Lower[index_low] + (Tkip_Sbox_Upper[index_low] * 256);
|
|
right = Tkip_Sbox_Upper[index_high] + (Tkip_Sbox_Lower[index_high] * 256);
|
|
|
|
return (left ^ right);
|
|
}
|
|
|
|
UINT rotr1(UINT a)
|
|
{
|
|
unsigned int b;
|
|
|
|
if ((a & 0x01) == 0x01)
|
|
{
|
|
b = (a >> 1) | 0x8000;
|
|
}
|
|
else
|
|
{
|
|
b = (a >> 1) & 0x7fff;
|
|
}
|
|
b = b % 65536;
|
|
return b;
|
|
}
|
|
|
|
VOID RTMPTkipMixKey(
|
|
UCHAR *key,
|
|
UCHAR *ta,
|
|
ULONG pnl, /* Least significant 16 bits of PN */
|
|
ULONG pnh, /* Most significant 32 bits of PN */
|
|
UCHAR *rc4key,
|
|
UINT *p1k)
|
|
{
|
|
|
|
UINT tsc0;
|
|
UINT tsc1;
|
|
UINT tsc2;
|
|
|
|
UINT ppk0;
|
|
UINT ppk1;
|
|
UINT ppk2;
|
|
UINT ppk3;
|
|
UINT ppk4;
|
|
UINT ppk5;
|
|
|
|
INT i;
|
|
INT j;
|
|
|
|
tsc0 = (unsigned int)((pnh >> 16) % 65536); /* msb */
|
|
tsc1 = (unsigned int)(pnh % 65536);
|
|
tsc2 = (unsigned int)(pnl % 65536); /* lsb */
|
|
|
|
/* Phase 1, step 1 */
|
|
p1k[0] = tsc1;
|
|
p1k[1] = tsc0;
|
|
p1k[2] = (UINT)(ta[0] + (ta[1]*256));
|
|
p1k[3] = (UINT)(ta[2] + (ta[3]*256));
|
|
p1k[4] = (UINT)(ta[4] + (ta[5]*256));
|
|
|
|
/* Phase 1, step 2 */
|
|
for (i=0; i<8; i++)
|
|
{
|
|
j = 2*(i & 1);
|
|
p1k[0] = (p1k[0] + tkip_sbox( (p1k[4] ^ ((256*key[1+j]) + key[j])) % 65536 )) % 65536;
|
|
p1k[1] = (p1k[1] + tkip_sbox( (p1k[0] ^ ((256*key[5+j]) + key[4+j])) % 65536 )) % 65536;
|
|
p1k[2] = (p1k[2] + tkip_sbox( (p1k[1] ^ ((256*key[9+j]) + key[8+j])) % 65536 )) % 65536;
|
|
p1k[3] = (p1k[3] + tkip_sbox( (p1k[2] ^ ((256*key[13+j]) + key[12+j])) % 65536 )) % 65536;
|
|
p1k[4] = (p1k[4] + tkip_sbox( (p1k[3] ^ (((256*key[1+j]) + key[j]))) % 65536 )) % 65536;
|
|
p1k[4] = (p1k[4] + i) % 65536;
|
|
}
|
|
|
|
/* Phase 2, Step 1 */
|
|
ppk0 = p1k[0];
|
|
ppk1 = p1k[1];
|
|
ppk2 = p1k[2];
|
|
ppk3 = p1k[3];
|
|
ppk4 = p1k[4];
|
|
ppk5 = (p1k[4] + tsc2) % 65536;
|
|
|
|
/* Phase2, Step 2 */
|
|
ppk0 = ppk0 + tkip_sbox( (ppk5 ^ ((256*key[1]) + key[0])) % 65536);
|
|
ppk1 = ppk1 + tkip_sbox( (ppk0 ^ ((256*key[3]) + key[2])) % 65536);
|
|
ppk2 = ppk2 + tkip_sbox( (ppk1 ^ ((256*key[5]) + key[4])) % 65536);
|
|
ppk3 = ppk3 + tkip_sbox( (ppk2 ^ ((256*key[7]) + key[6])) % 65536);
|
|
ppk4 = ppk4 + tkip_sbox( (ppk3 ^ ((256*key[9]) + key[8])) % 65536);
|
|
ppk5 = ppk5 + tkip_sbox( (ppk4 ^ ((256*key[11]) + key[10])) % 65536);
|
|
|
|
ppk0 = ppk0 + rotr1(ppk5 ^ ((256*key[13]) + key[12]));
|
|
ppk1 = ppk1 + rotr1(ppk0 ^ ((256*key[15]) + key[14]));
|
|
ppk2 = ppk2 + rotr1(ppk1);
|
|
ppk3 = ppk3 + rotr1(ppk2);
|
|
ppk4 = ppk4 + rotr1(ppk3);
|
|
ppk5 = ppk5 + rotr1(ppk4);
|
|
|
|
/* Phase 2, Step 3 */
|
|
/* Phase 2, Step 3 */
|
|
|
|
tsc0 = (unsigned int)((pnh >> 16) % 65536); /* msb */
|
|
tsc1 = (unsigned int)(pnh % 65536);
|
|
tsc2 = (unsigned int)(pnl % 65536); /* lsb */
|
|
|
|
rc4key[0] = (tsc2 >> 8) % 256;
|
|
rc4key[1] = (((tsc2 >> 8) % 256) | 0x20) & 0x7f;
|
|
rc4key[2] = tsc2 % 256;
|
|
rc4key[3] = ((ppk5 ^ ((256*key[1]) + key[0])) >> 1) % 256;
|
|
|
|
rc4key[4] = ppk0 % 256;
|
|
rc4key[5] = (ppk0 >> 8) % 256;
|
|
|
|
rc4key[6] = ppk1 % 256;
|
|
rc4key[7] = (ppk1 >> 8) % 256;
|
|
|
|
rc4key[8] = ppk2 % 256;
|
|
rc4key[9] = (ppk2 >> 8) % 256;
|
|
|
|
rc4key[10] = ppk3 % 256;
|
|
rc4key[11] = (ppk3 >> 8) % 256;
|
|
|
|
rc4key[12] = ppk4 % 256;
|
|
rc4key[13] = (ppk4 >> 8) % 256;
|
|
|
|
rc4key[14] = ppk5 % 256;
|
|
rc4key[15] = (ppk5 >> 8) % 256;
|
|
}
|
|
|
|
|
|
/************************************************/
|
|
/* construct_mic_header1() */
|
|
/* Builds the first MIC header block from */
|
|
/* header fields. */
|
|
/************************************************/
|
|
|
|
void construct_mic_header1(
|
|
unsigned char *mic_header1,
|
|
int header_length,
|
|
unsigned char *mpdu)
|
|
{
|
|
mic_header1[0] = (unsigned char)((header_length - 2) / 256);
|
|
mic_header1[1] = (unsigned char)((header_length - 2) % 256);
|
|
mic_header1[2] = mpdu[0] & 0xcf; /* Mute CF poll & CF ack bits */
|
|
mic_header1[3] = mpdu[1] & 0xc7; /* Mute retry, more data and pwr mgt bits */
|
|
mic_header1[4] = mpdu[4]; /* A1 */
|
|
mic_header1[5] = mpdu[5];
|
|
mic_header1[6] = mpdu[6];
|
|
mic_header1[7] = mpdu[7];
|
|
mic_header1[8] = mpdu[8];
|
|
mic_header1[9] = mpdu[9];
|
|
mic_header1[10] = mpdu[10]; /* A2 */
|
|
mic_header1[11] = mpdu[11];
|
|
mic_header1[12] = mpdu[12];
|
|
mic_header1[13] = mpdu[13];
|
|
mic_header1[14] = mpdu[14];
|
|
mic_header1[15] = mpdu[15];
|
|
}
|
|
|
|
/************************************************/
|
|
/* construct_mic_header2() */
|
|
/* Builds the last MIC header block from */
|
|
/* header fields. */
|
|
/************************************************/
|
|
|
|
void construct_mic_header2(
|
|
unsigned char *mic_header2,
|
|
unsigned char *mpdu,
|
|
int a4_exists,
|
|
int qc_exists)
|
|
{
|
|
int i;
|
|
|
|
for (i = 0; i<16; i++) mic_header2[i]=0x00;
|
|
|
|
mic_header2[0] = mpdu[16]; /* A3 */
|
|
mic_header2[1] = mpdu[17];
|
|
mic_header2[2] = mpdu[18];
|
|
mic_header2[3] = mpdu[19];
|
|
mic_header2[4] = mpdu[20];
|
|
mic_header2[5] = mpdu[21];
|
|
|
|
// In Sequence Control field, mute sequence numer bits (12-bit)
|
|
mic_header2[6] = mpdu[22] & 0x0f; /* SC */
|
|
mic_header2[7] = 0x00; /* mpdu[23]; */
|
|
|
|
if ((!qc_exists) & a4_exists)
|
|
{
|
|
for (i=0;i<6;i++) mic_header2[8+i] = mpdu[24+i]; /* A4 */
|
|
|
|
}
|
|
|
|
if (qc_exists && (!a4_exists))
|
|
{
|
|
mic_header2[8] = mpdu[24] & 0x0f; /* mute bits 15 - 4 */
|
|
mic_header2[9] = mpdu[25] & 0x00;
|
|
}
|
|
|
|
if (qc_exists && a4_exists)
|
|
{
|
|
for (i=0;i<6;i++) mic_header2[8+i] = mpdu[24+i]; /* A4 */
|
|
|
|
mic_header2[14] = mpdu[30] & 0x0f;
|
|
mic_header2[15] = mpdu[31] & 0x00;
|
|
}
|
|
}
|
|
|
|
|
|
/************************************************/
|
|
/* construct_mic_iv() */
|
|
/* Builds the MIC IV from header fields and PN */
|
|
/************************************************/
|
|
|
|
void construct_mic_iv(
|
|
unsigned char *mic_iv,
|
|
int qc_exists,
|
|
int a4_exists,
|
|
unsigned char *mpdu,
|
|
unsigned int payload_length,
|
|
unsigned char *pn_vector)
|
|
{
|
|
int i;
|
|
|
|
mic_iv[0] = 0x59;
|
|
if (qc_exists && a4_exists)
|
|
mic_iv[1] = mpdu[30] & 0x0f; /* QoS_TC */
|
|
if (qc_exists && !a4_exists)
|
|
mic_iv[1] = mpdu[24] & 0x0f; /* mute bits 7-4 */
|
|
if (!qc_exists)
|
|
mic_iv[1] = 0x00;
|
|
for (i = 2; i < 8; i++)
|
|
mic_iv[i] = mpdu[i + 8]; /* mic_iv[2:7] = A2[0:5] = mpdu[10:15] */
|
|
#ifdef CONSISTENT_PN_ORDER
|
|
for (i = 8; i < 14; i++)
|
|
mic_iv[i] = pn_vector[i - 8]; /* mic_iv[8:13] = PN[0:5] */
|
|
#else
|
|
for (i = 8; i < 14; i++)
|
|
mic_iv[i] = pn_vector[13 - i]; /* mic_iv[8:13] = PN[5:0] */
|
|
#endif
|
|
i = (payload_length / 256);
|
|
i = (payload_length % 256);
|
|
mic_iv[14] = (unsigned char) (payload_length / 256);
|
|
mic_iv[15] = (unsigned char) (payload_length % 256);
|
|
|
|
}
|
|
|
|
|
|
|
|
/************************************/
|
|
/* bitwise_xor() */
|
|
/* A 128 bit, bitwise exclusive or */
|
|
/************************************/
|
|
|
|
void bitwise_xor(unsigned char *ina, unsigned char *inb, unsigned char *out)
|
|
{
|
|
int i;
|
|
for (i=0; i<16; i++)
|
|
{
|
|
out[i] = ina[i] ^ inb[i];
|
|
}
|
|
}
|
|
|
|
|
|
void aes128k128d(unsigned char *key, unsigned char *data, unsigned char *ciphertext)
|
|
{
|
|
int round;
|
|
int i;
|
|
unsigned char intermediatea[16];
|
|
unsigned char intermediateb[16];
|
|
unsigned char round_key[16];
|
|
|
|
for(i=0; i<16; i++) round_key[i] = key[i];
|
|
|
|
for (round = 0; round < 11; round++)
|
|
{
|
|
if (round == 0)
|
|
{
|
|
xor_128(round_key, data, ciphertext);
|
|
next_key(round_key, round);
|
|
}
|
|
else if (round == 10)
|
|
{
|
|
byte_sub(ciphertext, intermediatea);
|
|
shift_row(intermediatea, intermediateb);
|
|
xor_128(intermediateb, round_key, ciphertext);
|
|
}
|
|
else /* 1 - 9 */
|
|
{
|
|
byte_sub(ciphertext, intermediatea);
|
|
shift_row(intermediatea, intermediateb);
|
|
mix_column(&intermediateb[0], &intermediatea[0]);
|
|
mix_column(&intermediateb[4], &intermediatea[4]);
|
|
mix_column(&intermediateb[8], &intermediatea[8]);
|
|
mix_column(&intermediateb[12], &intermediatea[12]);
|
|
xor_128(intermediatea, round_key, ciphertext);
|
|
next_key(round_key, round);
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
void construct_ctr_preload(
|
|
unsigned char *ctr_preload,
|
|
int a4_exists,
|
|
int qc_exists,
|
|
unsigned char *mpdu,
|
|
unsigned char *pn_vector,
|
|
int c)
|
|
{
|
|
|
|
int i = 0;
|
|
for (i=0; i<16; i++) ctr_preload[i] = 0x00;
|
|
i = 0;
|
|
|
|
ctr_preload[0] = 0x01; /* flag */
|
|
if (qc_exists && a4_exists) ctr_preload[1] = mpdu[30] & 0x0f; /* QoC_Control */
|
|
if (qc_exists && !a4_exists) ctr_preload[1] = mpdu[24] & 0x0f;
|
|
|
|
for (i = 2; i < 8; i++)
|
|
ctr_preload[i] = mpdu[i + 8]; /* ctr_preload[2:7] = A2[0:5] = mpdu[10:15] */
|
|
#ifdef CONSISTENT_PN_ORDER
|
|
for (i = 8; i < 14; i++)
|
|
ctr_preload[i] = pn_vector[i - 8]; /* ctr_preload[8:13] = PN[0:5] */
|
|
#else
|
|
for (i = 8; i < 14; i++)
|
|
ctr_preload[i] = pn_vector[13 - i]; /* ctr_preload[8:13] = PN[5:0] */
|
|
#endif
|
|
ctr_preload[14] = (unsigned char) (c / 256); // Ctr
|
|
ctr_preload[15] = (unsigned char) (c % 256);
|
|
|
|
}
|
|
|
|
|
|
//
|
|
// TRUE: Success!
|
|
// FALSE: Decrypt Error!
|
|
//
|
|
BOOLEAN RTMPSoftDecryptTKIP(
|
|
IN PRTMP_ADAPTER pAd,
|
|
IN PUCHAR pData,
|
|
IN ULONG DataByteCnt,
|
|
IN UCHAR UserPriority,
|
|
IN PCIPHER_KEY pWpaKey)
|
|
{
|
|
UCHAR KeyID;
|
|
UINT HeaderLen;
|
|
UCHAR fc0;
|
|
UCHAR fc1;
|
|
USHORT fc;
|
|
UINT frame_type;
|
|
UINT frame_subtype;
|
|
UINT from_ds;
|
|
UINT to_ds;
|
|
INT a4_exists;
|
|
INT qc_exists;
|
|
USHORT duration;
|
|
USHORT seq_control;
|
|
USHORT qos_control;
|
|
UCHAR TA[MAC_ADDR_LEN];
|
|
UCHAR DA[MAC_ADDR_LEN];
|
|
UCHAR SA[MAC_ADDR_LEN];
|
|
UCHAR RC4Key[16];
|
|
UINT p1k[5]; //for mix_key;
|
|
ULONG pnl;/* Least significant 16 bits of PN */
|
|
ULONG pnh;/* Most significant 32 bits of PN */
|
|
UINT num_blocks;
|
|
UINT payload_remainder;
|
|
ARCFOURCONTEXT ArcFourContext;
|
|
UINT crc32 = 0;
|
|
UINT trailfcs = 0;
|
|
UCHAR MIC[8];
|
|
UCHAR TrailMIC[8];
|
|
|
|
fc0 = *pData;
|
|
fc1 = *(pData + 1);
|
|
|
|
fc = *((PUSHORT)pData);
|
|
|
|
frame_type = ((fc0 >> 2) & 0x03);
|
|
frame_subtype = ((fc0 >> 4) & 0x0f);
|
|
|
|
from_ds = (fc1 & 0x2) >> 1;
|
|
to_ds = (fc1 & 0x1);
|
|
|
|
a4_exists = (from_ds & to_ds);
|
|
qc_exists = ((frame_subtype == 0x08) || /* Assumed QoS subtypes */
|
|
(frame_subtype == 0x09) || /* Likely to change. */
|
|
(frame_subtype == 0x0a) ||
|
|
(frame_subtype == 0x0b)
|
|
);
|
|
|
|
HeaderLen = 24;
|
|
if (a4_exists)
|
|
HeaderLen += 6;
|
|
|
|
KeyID = *((PUCHAR)(pData+ HeaderLen + 3));
|
|
KeyID = KeyID >> 6;
|
|
|
|
if (pWpaKey[KeyID].KeyLen == 0)
|
|
{
|
|
DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptTKIP failed!(KeyID[%d] Length can not be 0)\n", KeyID));
|
|
return FALSE;
|
|
}
|
|
|
|
duration = *((PUSHORT)(pData+2));
|
|
|
|
seq_control = *((PUSHORT)(pData+22));
|
|
|
|
if (qc_exists)
|
|
{
|
|
if (a4_exists)
|
|
{
|
|
qos_control = *((PUSHORT)(pData+30));
|
|
}
|
|
else
|
|
{
|
|
qos_control = *((PUSHORT)(pData+24));
|
|
}
|
|
}
|
|
|
|
if (to_ds == 0 && from_ds == 1)
|
|
{
|
|
NdisMoveMemory(DA, pData+4, MAC_ADDR_LEN);
|
|
NdisMoveMemory(SA, pData+16, MAC_ADDR_LEN);
|
|
NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN); //BSSID
|
|
}
|
|
else if (to_ds == 0 && from_ds == 0 )
|
|
{
|
|
NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
|
|
NdisMoveMemory(DA, pData+4, MAC_ADDR_LEN);
|
|
NdisMoveMemory(SA, pData+10, MAC_ADDR_LEN);
|
|
}
|
|
else if (to_ds == 1 && from_ds == 0)
|
|
{
|
|
NdisMoveMemory(SA, pData+10, MAC_ADDR_LEN);
|
|
NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
|
|
NdisMoveMemory(DA, pData+16, MAC_ADDR_LEN);
|
|
}
|
|
else if (to_ds == 1 && from_ds == 1)
|
|
{
|
|
NdisMoveMemory(TA, pData+10, MAC_ADDR_LEN);
|
|
NdisMoveMemory(DA, pData+16, MAC_ADDR_LEN);
|
|
NdisMoveMemory(SA, pData+22, MAC_ADDR_LEN);
|
|
}
|
|
|
|
num_blocks = (DataByteCnt - 16) / 16;
|
|
payload_remainder = (DataByteCnt - 16) % 16;
|
|
|
|
pnl = (*(pData + HeaderLen)) * 256 + *(pData + HeaderLen + 2);
|
|
pnh = *((PULONG)(pData + HeaderLen + 4));
|
|
pnh = cpu2le32(pnh);
|
|
RTMPTkipMixKey(pWpaKey[KeyID].Key, TA, pnl, pnh, RC4Key, p1k);
|
|
|
|
ARCFOUR_INIT(&ArcFourContext, RC4Key, 16);
|
|
|
|
ARCFOUR_DECRYPT(&ArcFourContext, pData + HeaderLen, pData + HeaderLen + 8, DataByteCnt - HeaderLen - 8);
|
|
NdisMoveMemory(&trailfcs, pData + DataByteCnt - 8 - 4, 4);
|
|
crc32 = RTMP_CALC_FCS32(PPPINITFCS32, pData + HeaderLen, DataByteCnt - HeaderLen - 8 - 4); //Skip IV+EIV 8 bytes & Skip last 4 bytes(FCS).
|
|
crc32 ^= 0xffffffff; /* complement */
|
|
|
|
if(crc32 != cpu2le32(trailfcs))
|
|
{
|
|
DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptTKIP, WEP Data ICV Error !\n")); //ICV error.
|
|
|
|
return (FALSE);
|
|
}
|
|
|
|
NdisMoveMemory(TrailMIC, pData + DataByteCnt - 8 - 8 - 4, 8);
|
|
RTMPInitMICEngine(pAd, pWpaKey[KeyID].Key, DA, SA, UserPriority, pWpaKey[KeyID].RxMic);
|
|
RTMPTkipAppend(&pAd->PrivateInfo.Tx, pData + HeaderLen, DataByteCnt - HeaderLen - 8 - 12);
|
|
RTMPTkipGetMIC(&pAd->PrivateInfo.Tx);
|
|
NdisMoveMemory(MIC, pAd->PrivateInfo.Tx.MIC, 8);
|
|
|
|
if (!NdisEqualMemory(MIC, TrailMIC, 8))
|
|
{
|
|
DBGPRINT(RT_DEBUG_ERROR, ("RTMPSoftDecryptTKIP, WEP Data MIC Error !\n")); //MIC error.
|
|
return (FALSE);
|
|
}
|
|
|
|
return TRUE;
|
|
}
|
|
|
|
|
|
|
|
|
|
BOOLEAN RTMPSoftDecryptAES(
|
|
IN PRTMP_ADAPTER pAd,
|
|
IN PUCHAR pData,
|
|
IN ULONG DataByteCnt,
|
|
IN PCIPHER_KEY pWpaKey)
|
|
{
|
|
UCHAR KeyID;
|
|
UINT HeaderLen;
|
|
UCHAR PN[6];
|
|
UINT payload_len;
|
|
UINT num_blocks;
|
|
UINT payload_remainder;
|
|
USHORT fc;
|
|
UCHAR fc0;
|
|
UCHAR fc1;
|
|
UINT frame_type;
|
|
UINT frame_subtype;
|
|
UINT from_ds;
|
|
UINT to_ds;
|
|
INT a4_exists;
|
|
INT qc_exists;
|
|
UCHAR aes_out[16];
|
|
int payload_index;
|
|
UINT i;
|
|
UCHAR ctr_preload[16];
|
|
UCHAR chain_buffer[16];
|
|
UCHAR padded_buffer[16];
|
|
UCHAR mic_iv[16];
|
|
UCHAR mic_header1[16];
|
|
UCHAR mic_header2[16];
|
|
UCHAR MIC[8];
|
|
UCHAR TrailMIC[8];
|
|
|
|
fc0 = *pData;
|
|
fc1 = *(pData + 1);
|
|
|
|
fc = *((PUSHORT)pData);
|
|
|
|
frame_type = ((fc0 >> 2) & 0x03);
|
|
frame_subtype = ((fc0 >> 4) & 0x0f);
|
|
|
|
from_ds = (fc1 & 0x2) >> 1;
|
|
to_ds = (fc1 & 0x1);
|
|
|
|
a4_exists = (from_ds & to_ds);
|
|
qc_exists = ((frame_subtype == 0x08) || /* Assumed QoS subtypes */
|
|
(frame_subtype == 0x09) || /* Likely to change. */
|
|
(frame_subtype == 0x0a) ||
|
|
(frame_subtype == 0x0b)
|
|
);
|
|
|
|
HeaderLen = 24;
|
|
if (a4_exists)
|
|
HeaderLen += 6;
|
|
|
|
KeyID = *((PUCHAR)(pData+ HeaderLen + 3));
|
|
KeyID = KeyID >> 6;
|
|
|
|
if (pWpaKey[KeyID].KeyLen == 0)
|
|
{
|
|
DBGPRINT(RT_DEBUG_TRACE, ("RTMPSoftDecryptAES failed!(KeyID[%d] Length can not be 0)\n", KeyID));
|
|
return FALSE;
|
|
}
|
|
|
|
PN[0] = *(pData+ HeaderLen);
|
|
PN[1] = *(pData+ HeaderLen + 1);
|
|
PN[2] = *(pData+ HeaderLen + 4);
|
|
PN[3] = *(pData+ HeaderLen + 5);
|
|
PN[4] = *(pData+ HeaderLen + 6);
|
|
PN[5] = *(pData+ HeaderLen + 7);
|
|
|
|
payload_len = DataByteCnt - HeaderLen - 8 - 8; // 8 bytes for CCMP header , 8 bytes for MIC
|
|
payload_remainder = (payload_len) % 16;
|
|
num_blocks = (payload_len) / 16;
|
|
|
|
|
|
|
|
// Find start of payload
|
|
payload_index = HeaderLen + 8; //IV+EIV
|
|
|
|
for (i=0; i< num_blocks; i++)
|
|
{
|
|
construct_ctr_preload(ctr_preload,
|
|
a4_exists,
|
|
qc_exists,
|
|
pData,
|
|
PN,
|
|
i+1 );
|
|
|
|
aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);
|
|
|
|
bitwise_xor(aes_out, pData + payload_index, chain_buffer);
|
|
NdisMoveMemory(pData + payload_index - 8, chain_buffer, 16);
|
|
payload_index += 16;
|
|
}
|
|
|
|
//
|
|
// If there is a short final block, then pad it
|
|
// encrypt it and copy the unpadded part back
|
|
//
|
|
if (payload_remainder > 0)
|
|
{
|
|
construct_ctr_preload(ctr_preload,
|
|
a4_exists,
|
|
qc_exists,
|
|
pData,
|
|
PN,
|
|
num_blocks + 1);
|
|
|
|
NdisZeroMemory(padded_buffer, 16);
|
|
NdisMoveMemory(padded_buffer, pData + payload_index, payload_remainder);
|
|
|
|
aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);
|
|
|
|
bitwise_xor(aes_out, padded_buffer, chain_buffer);
|
|
NdisMoveMemory(pData + payload_index - 8, chain_buffer, payload_remainder);
|
|
payload_index += payload_remainder;
|
|
}
|
|
|
|
//
|
|
// Descrypt the MIC
|
|
//
|
|
construct_ctr_preload(ctr_preload,
|
|
a4_exists,
|
|
qc_exists,
|
|
pData,
|
|
PN,
|
|
0);
|
|
NdisZeroMemory(padded_buffer, 16);
|
|
NdisMoveMemory(padded_buffer, pData + payload_index, 8);
|
|
|
|
aes128k128d(pWpaKey[KeyID].Key, ctr_preload, aes_out);
|
|
|
|
bitwise_xor(aes_out, padded_buffer, chain_buffer);
|
|
|
|
NdisMoveMemory(TrailMIC, chain_buffer, 8);
|
|
|
|
//
|
|
// Calculate MIC
|
|
//
|
|
|
|
//Force the protected frame bit on
|
|
*(pData + 1) = *(pData + 1) | 0x40;
|
|
|
|
// Find start of payload
|
|
// Because the CCMP header has been removed
|
|
payload_index = HeaderLen;
|
|
|
|
construct_mic_iv(
|
|
mic_iv,
|
|
qc_exists,
|
|
a4_exists,
|
|
pData,
|
|
payload_len,
|
|
PN);
|
|
|
|
construct_mic_header1(
|
|
mic_header1,
|
|
HeaderLen,
|
|
pData);
|
|
|
|
construct_mic_header2(
|
|
mic_header2,
|
|
pData,
|
|
a4_exists,
|
|
qc_exists);
|
|
|
|
aes128k128d(pWpaKey[KeyID].Key, mic_iv, aes_out);
|
|
bitwise_xor(aes_out, mic_header1, chain_buffer);
|
|
aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
|
|
bitwise_xor(aes_out, mic_header2, chain_buffer);
|
|
aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
|
|
|
|
// iterate through each 16 byte payload block
|
|
for (i = 0; i < num_blocks; i++)
|
|
{
|
|
bitwise_xor(aes_out, pData + payload_index, chain_buffer);
|
|
payload_index += 16;
|
|
aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
|
|
}
|
|
|
|
// Add on the final payload block if it needs padding
|
|
if (payload_remainder > 0)
|
|
{
|
|
NdisZeroMemory(padded_buffer, 16);
|
|
NdisMoveMemory(padded_buffer, pData + payload_index, payload_remainder);
|
|
|
|
bitwise_xor(aes_out, padded_buffer, chain_buffer);
|
|
aes128k128d(pWpaKey[KeyID].Key, chain_buffer, aes_out);
|
|
}
|
|
// aes_out contains padded mic, discard most significant
|
|
// 8 bytes to generate 64 bit MIC
|
|
for (i = 0 ; i < 8; i++) MIC[i] = aes_out[i];
|
|
|
|
if (!NdisEqualMemory(MIC, TrailMIC, 8))
|
|
{
|
|
DBGPRINT(RT_DEBUG_ERROR, ("RTMPSoftDecryptAES, MIC Error !\n")); //MIC error.
|
|
return FALSE;
|
|
}
|
|
|
|
return TRUE;
|
|
}
|
|
|
|
/****************************************/
|
|
/* aes128k128d() */
|
|
/* Performs a 128 bit AES encrypt with */
|
|
/* 128 bit data. */
|
|
/****************************************/
|
|
VOID xor_128(
|
|
IN PUCHAR a,
|
|
IN PUCHAR b,
|
|
OUT PUCHAR out)
|
|
{
|
|
INT i;
|
|
|
|
for (i=0;i<16; i++)
|
|
{
|
|
out[i] = a[i] ^ b[i];
|
|
}
|
|
}
|
|
|
|
VOID next_key(
|
|
IN PUCHAR key,
|
|
IN INT round)
|
|
{
|
|
UCHAR rcon;
|
|
UCHAR sbox_key[4];
|
|
UCHAR rcon_table[12] =
|
|
{
|
|
0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80,
|
|
0x1b, 0x36, 0x36, 0x36
|
|
};
|
|
|
|
sbox_key[0] = RTMPCkipSbox(key[13]);
|
|
sbox_key[1] = RTMPCkipSbox(key[14]);
|
|
sbox_key[2] = RTMPCkipSbox(key[15]);
|
|
sbox_key[3] = RTMPCkipSbox(key[12]);
|
|
|
|
rcon = rcon_table[round];
|
|
|
|
xor_32(&key[0], sbox_key, &key[0]);
|
|
key[0] = key[0] ^ rcon;
|
|
|
|
xor_32(&key[4], &key[0], &key[4]);
|
|
xor_32(&key[8], &key[4], &key[8]);
|
|
xor_32(&key[12], &key[8], &key[12]);
|
|
}
|
|
|
|
VOID xor_32(
|
|
IN PUCHAR a,
|
|
IN PUCHAR b,
|
|
OUT PUCHAR out)
|
|
{
|
|
INT i;
|
|
|
|
for (i=0;i<4; i++)
|
|
{
|
|
out[i] = a[i] ^ b[i];
|
|
}
|
|
}
|
|
|
|
VOID byte_sub(
|
|
IN PUCHAR in,
|
|
OUT PUCHAR out)
|
|
{
|
|
INT i;
|
|
|
|
for (i=0; i< 16; i++)
|
|
{
|
|
out[i] = RTMPCkipSbox(in[i]);
|
|
}
|
|
}
|
|
|
|
UCHAR RTMPCkipSbox(
|
|
IN UCHAR a)
|
|
{
|
|
return SboxTable[(int)a];
|
|
}
|
|
|
|
VOID shift_row(
|
|
IN PUCHAR in,
|
|
OUT PUCHAR out)
|
|
{
|
|
out[0] = in[0];
|
|
out[1] = in[5];
|
|
out[2] = in[10];
|
|
out[3] = in[15];
|
|
out[4] = in[4];
|
|
out[5] = in[9];
|
|
out[6] = in[14];
|
|
out[7] = in[3];
|
|
out[8] = in[8];
|
|
out[9] = in[13];
|
|
out[10] = in[2];
|
|
out[11] = in[7];
|
|
out[12] = in[12];
|
|
out[13] = in[1];
|
|
out[14] = in[6];
|
|
out[15] = in[11];
|
|
}
|
|
|
|
VOID mix_column(
|
|
IN PUCHAR in,
|
|
OUT PUCHAR out)
|
|
{
|
|
INT i;
|
|
UCHAR add1b[4];
|
|
UCHAR add1bf7[4];
|
|
UCHAR rotl[4];
|
|
UCHAR swap_halfs[4];
|
|
UCHAR andf7[4];
|
|
UCHAR rotr[4];
|
|
UCHAR temp[4];
|
|
UCHAR tempb[4];
|
|
|
|
for (i=0 ; i<4; i++)
|
|
{
|
|
if ((in[i] & 0x80)== 0x80)
|
|
add1b[i] = 0x1b;
|
|
else
|
|
add1b[i] = 0x00;
|
|
}
|
|
|
|
swap_halfs[0] = in[2]; /* Swap halfs */
|
|
swap_halfs[1] = in[3];
|
|
swap_halfs[2] = in[0];
|
|
swap_halfs[3] = in[1];
|
|
|
|
rotl[0] = in[3]; /* Rotate left 8 bits */
|
|
rotl[1] = in[0];
|
|
rotl[2] = in[1];
|
|
rotl[3] = in[2];
|
|
|
|
andf7[0] = in[0] & 0x7f;
|
|
andf7[1] = in[1] & 0x7f;
|
|
andf7[2] = in[2] & 0x7f;
|
|
andf7[3] = in[3] & 0x7f;
|
|
|
|
for (i = 3; i>0; i--) /* logical shift left 1 bit */
|
|
{
|
|
andf7[i] = andf7[i] << 1;
|
|
if ((andf7[i-1] & 0x80) == 0x80)
|
|
{
|
|
andf7[i] = (andf7[i] | 0x01);
|
|
}
|
|
}
|
|
andf7[0] = andf7[0] << 1;
|
|
andf7[0] = andf7[0] & 0xfe;
|
|
|
|
xor_32(add1b, andf7, add1bf7);
|
|
|
|
xor_32(in, add1bf7, rotr);
|
|
|
|
temp[0] = rotr[0]; /* Rotate right 8 bits */
|
|
rotr[0] = rotr[1];
|
|
rotr[1] = rotr[2];
|
|
rotr[2] = rotr[3];
|
|
rotr[3] = temp[0];
|
|
|
|
xor_32(add1bf7, rotr, temp);
|
|
xor_32(swap_halfs, rotl,tempb);
|
|
xor_32(temp, tempb, out);
|
|
}
|
|
|