📄 rtmp_tkip.c
字号:
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));
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 */
#ifndef BIG_ENDIAN
if(crc32 != trailfcs)
#else
if(crc32 != SWAP32(trailfcs))
#endif
{
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.
//RTMPReportMicError(pAd, &pWpaKey[KeyID]); // marked by AlbertY @ 20060630
return (FALSE);
}
#ifdef BIG_ENDIAN
RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
#endif
//DBGPRINT(RT_DEBUG_TRACE, "RTMPSoftDecryptTKIP Decript done!!\n");
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];
#ifdef BIG_ENDIAN
RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
#endif
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;
DBGPRINT(RT_DEBUG_INFO, "SoftDecryptAES: payload = %d, num_blocks = %d, payload_remainder = %d\n", payload_len, num_blocks, payload_remainder);
// 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);
#if 0
{
int j;
printk("chain_buffer[%d]: ", i+1);
for (j = 0; j < 16; j++)
{
printk("%02x ", chain_buffer[j]);
}
printk("\n");
}
#endif
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);
#if 0
{
int j;
printk("chain_buffer[%d]: ", num_blocks + 1);
for (j = 0; j < payload_remainder; j++)
{
printk("%02x ", chain_buffer[j]);
}
printk("\n");
}
#endif
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);
#if 0
{
printk("chain_buffer: ");
for (i = 0; i < 16; i++)
{
printk("%02x ", chain_buffer[i]);
}
printk("\n");
}
#endif
//
// 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);
}
#if 0
{
printk("aes_out: ");
for (i = 0; i < 16; i++)
{
printk("%02x ", aes_out[i]);
}
printk("\n");
}
#endif
// 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;
}
#ifdef BIG_ENDIAN
RTMPFrameEndianChange(pAd, (PUCHAR)pData, DIR_READ, FALSE);
#endif
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);
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -